CbmRoot
CbmKFFieldMath.cxx
Go to the documentation of this file.
1 #include "CbmKFFieldMath.h"
2 
3 #include "CbmKFMath.h"
4 
5 #include "FairField.h"
6 
7 #include "math.h"
8 
9 //using std::sqrt;
10 //using std::finite;
11 
12 ClassImp(CbmKFFieldMath)
13 
14  void CbmKFFieldMath::ExtrapolateLine(const Double_t T_in[],
15  const Double_t C_in[],
16  Double_t T_out[],
17  Double_t C_out[],
18  Double_t z_out) {
19  if (!T_in) return;
20 
21  Double_t dz = z_out - T_in[5];
22 
23  if (T_out) {
24  T_out[0] = T_in[0] + dz * T_in[2];
25  T_out[1] = T_in[1] + dz * T_in[3];
26  T_out[2] = T_in[2];
27  T_out[3] = T_in[3];
28  T_out[4] = T_in[4];
29  T_out[5] = z_out;
30  }
31 
32  if (C_in && C_out) {
33  const Double_t dzC_in8 = dz * C_in[8];
34 
35  C_out[4] = C_in[4] + dzC_in8;
36  C_out[1] = C_in[1] + dz * (C_out[4] + C_in[6]);
37 
38  const Double_t C_in3 = C_in[3];
39 
40  C_out[3] = C_in3 + dz * C_in[5];
41  C_out[0] = C_in[0] + dz * (C_out[3] + C_in3);
42 
43  const Double_t C_in7 = C_in[7];
44 
45  C_out[7] = C_in7 + dz * C_in[9];
46  C_out[2] = C_in[2] + dz * (C_out[7] + C_in7);
47  C_out[5] = C_in[5];
48  C_out[6] = C_in[6] + dzC_in8;
49  C_out[8] = C_in[8];
50  C_out[9] = C_in[9];
51 
52  C_out[10] = C_in[10];
53  C_out[11] = C_in[11];
54  C_out[12] = C_in[12];
55  C_out[13] = C_in[13];
56  C_out[14] = C_in[14];
57  }
58 }
59 
60 
61 Int_t CbmKFFieldMath::ExtrapolateRK4(
62  const Double_t T_in[], // input track parameters (x,y,tx,ty,Q/p)
63  const Double_t C_in[], // input covariance matrix
64  Double_t T_out[], // output track parameters
65  Double_t C_out[], // output covariance matrix
66  Double_t z_out, // extrapolate to this z position
67  Double_t qp0, // use Q/p linearisation at this value
68  FairField* MF // magnetic field
69 ) {
70  //
71  // Forth-order Runge-Kutta method for solution of the equation
72  // of motion of a particle with parameter qp = Q /P
73  // in the magnetic field B()
74  //
75  // | x | tx
76  // | y | ty
77  // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) )
78  // | ty| ft * (-tx * ( B(3)+ty+b(2) - (1+ty**2)*B(1) ) ,
79  //
80  // where ft = c_light*qp*sqrt ( 1 + tx**2 + ty**2 ) .
81  //
82  // In the following for RK step :
83  //
84  // x=x[0], y=x[1], tx=x[3], ty=x[4].
85  //
86  //========================================================================
87  //
88  // NIM A395 (1997) 169-184; NIM A426 (1999) 268-282.
89  //
90  // the routine is based on LHC(b) utility code
91  //
92  //========================================================================
93 
94  const Double_t c_light = 0.000299792458;
95 
96  static Double_t a[4] = {0.0, 0.5, 0.5, 1.0};
97  static Double_t c[4] = {1.0 / 6.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 6.0};
98  static Double_t b[4] = {0.0, 0.5, 0.5, 1.0};
99 
100  Int_t step4;
101  Double_t k[16], x0[4], x[4], k1[16];
102  Double_t Ax[4], Ay[4], Ax_tx[4], Ay_tx[4], Ax_ty[4], Ay_ty[4];
103 
104  //----------------------------------------------------------------
105 
106  Double_t qp_in = T_in[4];
107  Double_t z_in = T_in[5];
108  Double_t h = z_out - z_in;
109  Double_t hC = h * c_light;
110  x0[0] = T_in[0];
111  x0[1] = T_in[1];
112  x0[2] = T_in[2];
113  x0[3] = T_in[3];
114  //
115  // Runge-Kutta step
116  //
117 
118  Int_t step;
119  Int_t i;
120 
121  for (step = 0; step < 4; ++step) {
122  for (i = 0; i < 4; ++i) {
123  if (step == 0) {
124  x[i] = x0[i];
125  } else {
126  x[i] = x0[i] + b[step] * k[step * 4 - 4 + i];
127  }
128  }
129 
130  Double_t point[3] = {x[0], x[1], z_in + a[step] * h};
131  Double_t B[3];
132  if (MF)
133  MF->GetFieldValue(point, B);
134  else {
135  B[0] = B[1] = B[2] = 0.;
136  }
137 
138  Double_t tx = x[2];
139  Double_t ty = x[3];
140  Double_t tx2 = tx * tx;
141  Double_t ty2 = ty * ty;
142  Double_t txty = tx * ty;
143  Double_t tx2ty21 = 1.0 + tx2 + ty2;
144  if (tx2ty21 > 1.e4) return 1;
145  Double_t I_tx2ty21 = 1.0 / tx2ty21 * qp0;
146  Double_t tx2ty2 = sqrt(tx2ty21);
147  // Double_t I_tx2ty2 = qp0 * hC / tx2ty2 ; unsused ???
148  tx2ty2 *= hC;
149  Double_t tx2ty2qp = tx2ty2 * qp0;
150  Ax[step] = (txty * B[0] + ty * B[2] - (1.0 + tx2) * B[1]) * tx2ty2;
151  Ay[step] = (-txty * B[1] - tx * B[2] + (1.0 + ty2) * B[0]) * tx2ty2;
152 
153  Ax_tx[step] =
154  Ax[step] * tx * I_tx2ty21 + (ty * B[0] - 2.0 * tx * B[1]) * tx2ty2qp;
155  Ax_ty[step] = Ax[step] * ty * I_tx2ty21 + (tx * B[0] + B[2]) * tx2ty2qp;
156  Ay_tx[step] = Ay[step] * tx * I_tx2ty21 + (-ty * B[1] - B[2]) * tx2ty2qp;
157  Ay_ty[step] =
158  Ay[step] * ty * I_tx2ty21 + (-tx * B[1] + 2.0 * ty * B[0]) * tx2ty2qp;
159 
160  step4 = step * 4;
161  k[step4] = tx * h;
162  k[step4 + 1] = ty * h;
163  k[step4 + 2] = Ax[step] * qp0;
164  k[step4 + 3] = Ay[step] * qp0;
165 
166  } // end of Runge-Kutta steps
167 
168  for (i = 0; i < 4; ++i) {
169  T_out[i] = x0[i] + c[0] * k[i] + c[1] * k[4 + i] + c[2] * k[8 + i]
170  + c[3] * k[12 + i];
171  }
172  T_out[4] = T_in[4];
173  T_out[5] = z_out;
174  //
175  // Derivatives dx/dqp
176  //
177 
178  x0[0] = 0.0;
179  x0[1] = 0.0;
180  x0[2] = 0.0;
181  x0[3] = 0.0;
182 
183  //
184  // Runge-Kutta step for derivatives dx/dqp
185 
186  for (step = 0; step < 4; ++step) {
187  for (i = 0; i < 4; ++i) {
188  if (step == 0) {
189  x[i] = x0[i];
190  } else {
191  x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
192  }
193  }
194  step4 = step * 4;
195  k1[step4] = x[2] * h;
196  k1[step4 + 1] = x[3] * h;
197  k1[step4 + 2] = Ax[step] + Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
198  k1[step4 + 3] = Ay[step] + Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
199 
200  } // end of Runge-Kutta steps for derivatives dx/dqp
201 
202  Double_t J[25];
203 
204  for (i = 0; i < 4; ++i) {
205  J[20 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i]
206  + c[3] * k1[12 + i];
207  }
208  J[24] = 1.;
209  //
210  // end of derivatives dx/dqp
211  //
212 
213  // Derivatives dx/tx
214  //
215 
216  x0[0] = 0.0;
217  x0[1] = 0.0;
218  x0[2] = 1.0;
219  x0[3] = 0.0;
220 
221  //
222  // Runge-Kutta step for derivatives dx/dtx
223  //
224 
225  for (step = 0; step < 4; ++step) {
226  for (i = 0; i < 4; ++i) {
227  if (step == 0) {
228  x[i] = x0[i];
229  } else if (i != 2) {
230  x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
231  }
232  }
233  step4 = step * 4;
234  k1[step4] = x[2] * h;
235  k1[step4 + 1] = x[3] * h;
236  // k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
237  k1[step4 + 3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
238 
239  } // end of Runge-Kutta steps for derivatives dx/dtx
240 
241  for (i = 0; i < 4; ++i) {
242  if (i != 2) {
243  J[10 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i]
244  + c[3] * k1[12 + i];
245  }
246  }
247  // end of derivatives dx/dtx
248  J[12] = 1.0;
249  J[14] = 0.0;
250 
251  // Derivatives dx/ty
252  //
253 
254  x0[0] = 0.0;
255  x0[1] = 0.0;
256  x0[2] = 0.0;
257  x0[3] = 1.0;
258 
259  //
260  // Runge-Kutta step for derivatives dx/dty
261  //
262 
263  for (step = 0; step < 4; ++step) {
264  for (i = 0; i < 4; ++i) {
265  if (step == 0) {
266  x[i] = x0[i]; // ty fixed
267  } else if (i != 3) {
268  x[i] = x0[i] + b[step] * k1[step * 4 - 4 + i];
269  }
270  }
271  step4 = step * 4;
272  k1[step4] = x[2] * h;
273  k1[step4 + 1] = x[3] * h;
274  k1[step4 + 2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3];
275  // k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3];
276 
277  } // end of Runge-Kutta steps for derivatives dx/dty
278 
279  for (i = 0; i < 3; ++i) {
280  J[15 + i] = x0[i] + c[0] * k1[i] + c[1] * k1[4 + i] + c[2] * k1[8 + i]
281  + c[3] * k1[12 + i];
282  }
283  // end of derivatives dx/dty
284  J[18] = 1.;
285  J[19] = 0.;
286 
287  //
288  // derivatives dx/dx and dx/dy
289 
290  for (i = 0; i < 10; ++i) {
291  J[i] = 0.;
292  }
293  J[0] = 1.;
294  J[6] = 1.;
295 
296  // extrapolate inverse momentum
297 
298  T_out[4] = qp_in;
299 
300  Double_t dqp = qp_in - qp0;
301 
302  {
303  for (Int_t ip = 0; ip < 4; ip++) {
304  T_out[ip] += J[5 * 4 + ip] * dqp;
305  }
306  }
307 
308  // covariance matrix transport
309 
310  if (C_in && C_out) CbmKFMath::multQtSQ(5, J, C_in, C_out);
311  return 0;
312 } // end of RK4
313 
314 #ifdef XXX
315 /****************************************************************
316  *
317  * Field Integration
318  *
319  ****************************************************************/
320 
321 void CbmKFFieldMath::IntegrateField(CbmMagField* MF,
322  Double_t r0[],
323  Double_t r1[],
324  Double_t r2[],
325  Double_t si[3],
326  Double_t Si[3],
327  Double_t sii[3][3],
328  Double_t Sii[3][3],
329  Double_t siii[3][3][3],
330  Double_t Siii[3][3][3]) {
331  Double_t dz = r2[2] - r0[2];
332 
333  Double_t B[3][3];
334 
335  if (MF) {
336  MF->GetFieldValue(r0, B[0]);
337  MF->GetFieldValue(r1, B[1]);
338  MF->GetFieldValue(r2, B[2]);
339  } else
340  B[0][0] = B[0][1] = B[0][2] = B[1][0] = B[1][1] = B[1][2] = B[2][0] =
341  B[2][1] = B[2][2] = 0;
342 
343  // coefficients
344 
345  Double_t c1[3] = {1, 4, 1} // /=6.
346  ,
347  c2[3][3] = {{5, -4, -1}, {44, 80, -4}, {11, 44, 5}} // /=360.
348  ,
349  c3[3][3][3] = {{{35, 20, -1}, {-124, -256, 20}, {-19, -52, -1}},
350  {{524, 176, -52}, {1760, 2240, -256}, {-52, 176, 20}},
351  {{125, -52, -19},
352  {1028, 1760, -124},
353  {125, 524, 35}}}; // /=45360.
354 
355  Double_t C1[3] = {1, 2, 0} // /=6.
356  ,
357  C2[3][3] = {{38, 8, -4}, {148, 208, -20}, {3, 36, 3}} // /=2520.
358  ,
359  C3[3][3][3] = {
360  {{85, 28, -5}, {4, -80, 4}, {-17, -20, 1}},
361  {{494, 200, -46}, {1256, 1376, -184}, {-94, 8, 14}},
362  {{15, -12, -3}, {252, 432, -36}, {21, 84, 3}}}; // /=90720.
363 
364  // integrate field
365 
366  for (Int_t x = 0; x < 3; x++) {
367  if (si) {
368  si[x] = 0;
369  for (Int_t n = 0; n < 3; n++)
370  si[x] += c1[n] * B[n][x];
371  si[x] *= dz / 6.;
372  }
373 
374  if (Si) {
375  Si[x] = 0;
376  for (Int_t n = 0; n < 3; n++)
377  Si[x] += C1[n] * B[n][x];
378  Si[x] *= dz * dz / 6.;
379  }
380 
381  for (Int_t y = 0; y < 3; y++) {
382  if (sii) {
383  sii[x][y] = 0;
384  for (Int_t n = 0; n < 3; n++)
385  for (Int_t m = 0; m < 3; m++)
386  sii[x][y] += c2[n][m] * B[n][x] * B[m][y];
387  sii[x][y] *= dz * dz / 360.;
388  }
389 
390  if (Sii) {
391  Sii[x][y] = 0;
392  for (Int_t n = 0; n < 3; n++)
393  for (Int_t m = 0; m < 3; m++)
394  Sii[x][y] += C2[n][m] * B[n][x] * B[m][y];
395  Sii[x][y] *= dz * dz * dz / 2520.;
396  }
397 
398  for (Int_t z = 0; z < 3; z++) {
399  if (siii) {
400  siii[x][y][z] = 0;
401  for (Int_t n = 0; n < 3; n++)
402  for (Int_t m = 0; m < 3; m++)
403  for (Int_t k = 0; k < 3; k++)
404  siii[x][y][z] += c3[n][m][k] * B[n][x] * B[m][y] * B[k][z];
405  siii[x][y][z] *= dz * dz * dz / 45360.;
406  }
407 
408  if (Siii) {
409  Siii[x][y][z] = 0;
410  for (Int_t n = 0; n < 3; n++)
411  for (Int_t m = 0; m < 3; m++)
412  for (Int_t k = 0; k < 3; k++)
413  Siii[x][y][z] += C3[n][m][k] * B[n][x] * B[m][y] * B[k][z];
414  Siii[x][y][z] *= dz * dz * dz * dz / 90720.;
415  }
416  }
417  }
418  }
419 }
420 
421 
422 /****************************************************************
423  *
424  * Calculate extrapolation coefficients
425  *
426  ****************************************************************/
427 
428 void CbmKFFieldMath::GetCoefficients(const Double_t x,
429  const Double_t y,
430  Double_t Xi[3][3],
431  Double_t Yi[3][3],
432  Double_t Xii[3][3][3],
433  Double_t Yii[3][3][3],
434  Double_t Xiii[3][3][3][3],
435  Double_t Yiii[3][3][3][3]) {
436  Double_t
437 
438  xx = x * x,
439  xy = x * y, yy = y * y,
440 
441  x2 = x * 2, x4 = x * 4, xx31 = xx * 3 + 1, xx33 = xx * 3 + 3,
442  xx82 = xx * 8 + 2, xx86 = xx * 8 + 6, xx153 = xx * 15 + 3,
443  xx159 = xx * 15 + 9,
444 
445  y2 = y * 2, y4 = y * 4, yy31 = yy * 3 + 1, yy33 = yy * 3 + 3,
446  yy82 = yy * 8 + 2, yy86 = yy * 8 + 6, yy153 = yy * 15 + 3,
447  yy159 = yy * 15 + 9, xxyy2 = 2 * (xx - yy),
448 
449  xx1053 = y * (30 * xx + xx159), yy1053 = x * (30 * yy + yy159);
450 
451 
452  Double_t X1[3] = {xy, -xx - 1, y}
453 
454  ,
455  X2[3][3] = {{x * yy31, -y * xx31, 2 * yy + 1},
456  {-y * xx31, x * xx33, -2 * xy},
457  {yy - xx, -2 * xy, -x}}
458 
459  ,
460  X3[3][3][3] = {
461  {{xy * yy159, -xx * yy153 - yy31, y * yy86},
462  {-xx * yy153 - yy31, xy * xx159, -x * yy82},
463  {y2 * (-xxyy2 + 1), -x * yy82, -3 * xy}},
464  {{-xx * yy153 - yy31, xy * xx159, -x * yy82},
465  {xy * xx159, -3 * (5 * xx * xx + 6 * xx + 1), y * xx82},
466  {x2 * (xxyy2 + 1), y * xx82, xx31}},
467  {{y * (-6 * xx + yy31), x * (xx31 - 6 * yy), -4 * xy},
468  {x * (3 * xx - 6 * yy + 1), 3 * y * xx31, xxyy2},
469  {-4 * xy, xxyy2, -y}}};
470  Double_t X1x[3] = {y, -x2, 0}
471 
472  ,
473  X2x[3][3] = {{yy31, -6 * xy, 0},
474  {-6 * xy, 3 * xx31, -y2},
475  {-x2, -y2, -1}}
476 
477  ,
478  X3x[3][3][3] = {{{y * yy159, -x2 * yy153, 0},
479  {-x2 * yy153, xx1053, -yy82},
480  {-8 * xy, -yy82, -3 * y}},
481  {{-x2 * yy153, xx1053, -yy82},
482  {xx1053, -x4 * xx159, 16 * xy},
483  {2 * (6 * xx - 2 * yy + 1), 16 * xy, 6 * x}},
484  {{-12 * xy, 9 * xx - 6 * yy + 1, -y4},
485  {9 * xx - 6 * yy + 1, 18 * xy, x4},
486  {-y4, x4, 0}}};
487 
488  Double_t X1y[3] = {x, 0, 1}
489 
490  ,
491  X2y[3][3] = {{6 * xy, -xx31, y4}, {-xx31, 0, -x2}, {y2, -x2, 0}}
492 
493  ,
494  X3y[3][3][3] = {{{yy1053, -y2 * xx153, 16 * yy + yy86},
495  {-y2 * xx153, x * xx159, -16 * xy},
496  {yy82 - 2 * xxyy2, -16 * xy, -3 * x}},
497  {{-y2 * xx153, x * xx159, -16 * xy},
498  {x * xx159, 0, xx82},
499  {-8 * xy, xx82, 0}},
500  {{-6 * xx + 9 * yy + 1, -12 * xy, -x4},
501  {-12 * xy, 3 * xx31, -y4},
502  {-x4, -y4, -1}}};
503 
504 
505  Double_t Y1[3] = {yy + 1, -xy, -x}
506 
507  ,
508  Y2[3][3] = {{y * yy33, -x * yy31, -2 * xy},
509  {-x * yy31, y * xx31, 2 * xx + 1},
510  {-2 * xy, xx - yy, -y}}
511 
512  ,
513  Y3[3][3][3] = {
514  {{3 * (5 * yy * yy + 6 * yy + 1), -xy * yy159, -x * yy82},
515  {-xy * yy159, xx * yy153 + yy31, y * xx82},
516  {-x * yy82, -y2 * (-xxyy2 + 1), -yy31}},
517  {{-xy * yy159, xx * yy153 + yy31, y * xx82},
518  {xx * yy153 + yy31, -xy * xx159, -x * xx86},
519  {y * xx82, -x2 * (xxyy2 + 1), 3 * xy}},
520  {{-3 * x * yy31, y * (6 * xx - yy31), xxyy2},
521  {y * (6 * xx - yy31), x * (6 * yy - xx31), 4 * xy},
522  {xxyy2, 4 * xy, x}}};
523  Double_t Y1x[3] = {0, -y, -1}
524 
525  ,
526  Y2x[3][3] = {{0, -yy31, -y2}, {-yy31, 6 * xy, x4}, {-y2, x2, 0}}
527 
528  ,
529  Y3x[3][3][3] = {{{0, -y * yy159, -yy82},
530  {-y * yy159, x2 * yy153, 16 * xy},
531  {-yy82, 8 * xy, 0}},
532  {{-y * yy159, x2 * yy153, 16 * xy},
533  {x2 * yy153, -xx1053, -16 * xx - xx86},
534  {16 * xy, -2 * (6 * xx - 2 * yy + 1), 3 * y}},
535  {{-3 * yy31, 12 * xy, x4},
536  {12 * xy, -9 * xx + 6 * yy - 1, y4},
537  {x4, y4, 1}}};
538  Double_t Y1y[3] = {y2, -x, 0}
539 
540  ,
541  Y2y[3][3] = {{3 * yy31, -6 * xy, -x2},
542  {-6 * xy, xx31, 0},
543  {-x2, -y2, -1}}
544 
545  ,
546  Y3y[3][3][3] = {{{y4 * yy159, -yy1053, -16 * xy},
547  {-yy1053, y2 * xx153, xx82},
548  {-16 * xy, 4 * xx - 12 * yy - 2, -6 * y}},
549  {{-yy1053, y2 * xx153, xx82},
550  {y2 * xx153, -x * xx159, 0},
551  {xx82, 8 * xy, 3 * x}},
552  {{-18 * xy, 6 * xx - 9 * yy - 1, -y4},
553  {6 * xx - 9 * yy - 1, 12 * xy, x4},
554  {-y4, x4, 0}}};
555 
556  if (Xi) {
557  for (Int_t i = 0; i < 3; i++) {
558  Xi[0][i] = X1[i];
559  Xi[1][i] = X1x[i];
560  Xi[2][i] = X1y[i];
561  }
562  }
563  if (Yi) {
564  for (Int_t i = 0; i < 3; i++) {
565  Yi[0][i] = Y1[i];
566  Yi[1][i] = Y1x[i];
567  Yi[2][i] = Y1y[i];
568  }
569  }
570  if (Xii) {
571  for (Int_t i = 0; i < 3; i++)
572  for (Int_t j = 0; j < 3; j++) {
573  Xii[0][i][j] = X2[i][j];
574  Xii[1][i][j] = X2x[i][j];
575  Xii[2][i][j] = X2y[i][j];
576  }
577  }
578  if (Yii) {
579  for (Int_t i = 0; i < 3; i++)
580  for (Int_t j = 0; j < 3; j++) {
581  Yii[0][i][j] = Y2[i][j];
582  Yii[1][i][j] = Y2x[i][j];
583  Yii[2][i][j] = Y2y[i][j];
584  }
585  }
586  if (Xiii) {
587  for (Int_t i = 0; i < 3; i++)
588  for (Int_t j = 0; j < 3; j++)
589  for (Int_t k = 0; k < 3; k++) {
590  Xiii[0][i][j][k] = X3[i][j][k];
591  Xiii[1][i][j][k] = X3x[i][j][k];
592  Xiii[2][i][j][k] = X3y[i][j][k];
593  }
594  }
595  if (Yiii) {
596  for (Int_t i = 0; i < 3; i++)
597  for (Int_t j = 0; j < 3; j++)
598  for (Int_t k = 0; k < 3; k++) {
599  Yiii[0][i][j][k] = Y3[i][j][k];
600  Yiii[1][i][j][k] = Y3x[i][j][k];
601  Yiii[2][i][j][k] = Y3y[i][j][k];
602  }
603  }
604 }
605 
606 
607 /****************************************************************
608  *
609  * ANALYTIC 1-3
610  *
611  ****************************************************************/
612 
613 void CbmKFFieldMath::ExtrapolateAnalytic(
614  const Double_t T_in[], // input track parameters (x,y,tx,ty,Q/p)
615  const Double_t C_in[], // input covariance matrix
616  Double_t T_out[], // output track parameters
617  Double_t C_out[], // output covariance matrix
618  Double_t z_out, // extrapolate to this z position
619  Double_t qp0, // use Q/p linearisation at this value
620  FairField* MF, // magnetic field
621  Int_t order) {
622  //
623  // Part of the exact extrapolation formula with error (c_light*B*dz)^4/4!
624  //
625  // Under investigation, finally supposed to be fast.
626  //
627 
628  const Double_t c_light = 0.000299792458;
629 
630  Double_t qp_in = T_in[4];
631  Double_t z_in = T_in[5];
632  Double_t dz = z_out - z_in;
633 
634  // construct coefficients
635 
636  Double_t tx = T_in[2], ty = T_in[3],
637 
638  A1[3][3], B1[3][3], A2[3][3][3], B2[3][3][3], A3[3][3][3][3],
639  B3[3][3][3][3];
640 
641  GetCoefficients(tx, ty, A1, B1, A2, B2, A3, B3);
642 
643  Double_t t2 = 1. + tx * tx + ty * ty, t = sqrt(t2), h = qp0 * c_light,
644  ht = h * t;
645 
646  Double_t s1[3], s2[3][3], s3[3][3][3], S1[3], S2[3][3], S3[3][3][3];
647 
648  {
649  Double_t r0[3], r1[3], r2[3];
650 
651  r0[0] = T_in[0];
652  r0[1] = T_in[1];
653  r0[2] = T_in[5];
654 
655  r2[0] = T_in[0] + T_in[2] * dz;
656  r2[1] = T_in[1] + T_in[3] * dz;
657  r2[2] = z_out;
658 
659  r1[0] = 0.5 * (r0[0] + r2[0]);
660  r1[1] = 0.5 * (r0[1] + r2[1]);
661  r1[2] = 0.5 * (r0[2] + r2[2]);
662 
663 
664  Double_t B[3][3];
665 
666  if (MF) {
667  MF->GetFieldValue(r0, B[0]);
668  MF->GetFieldValue(r1, B[1]);
669  MF->GetFieldValue(r2, B[2]);
670  } else
671  B[0][0] = B[0][1] = B[0][2] = B[1][0] = B[1][1] = B[1][2] = B[2][0] =
672  B[2][1] = B[2][2] = 0;
673 
674  Double_t Sy = (7 * B[0][1] + 6 * B[1][1] - B[2][1]) * dz * dz / 96.;
675  r1[0] = T_in[0] + tx * dz / 2 + ht * Sy * A1[0][1];
676  r1[1] = T_in[1] + ty * dz / 2 + ht * Sy * B1[0][1];
677 
678  Sy = (B[0][1] + 2 * B[1][1]) * dz * dz / 6.;
679  r2[0] = T_in[0] + tx * dz + ht * Sy * A1[0][1];
680  r2[1] = T_in[1] + ty * dz + ht * Sy * B1[0][1];
681 
682  IntegrateField(MF, r0, r1, r2, s1, S1, s2, S2, s3, S3);
683  }
684 
685  Double_t sA1 = 0, sA2 = 0, sA3 = 0, sB1 = 0, sB2 = 0, sB3 = 0;
686  Double_t sA1x = 0, sA2x = 0, sA3x = 0, sB1x = 0, sB2x = 0, sB3x = 0;
687  Double_t sA1y = 0, sA2y = 0, sA3y = 0, sB1y = 0, sB2y = 0, sB3y = 0;
688 
689  Double_t SA1 = 0, SA2 = 0, SA3 = 0, SB1 = 0, SB2 = 0, SB3 = 0;
690  Double_t SA1x = 0, SA2x = 0, SA3x = 0, SB1x = 0, SB2x = 0, SB3x = 0;
691  Double_t SA1y = 0, SA2y = 0, SA3y = 0, SB1y = 0, SB2y = 0, SB3y = 0;
692 
693  {
694  for (Int_t n = 0; n < 3; n++) {
695  sA1 += s1[n] * A1[0][n];
696  sA1x += s1[n] * A1[1][n];
697  sA1y += s1[n] * A1[2][n];
698  sB1 += s1[n] * B1[0][n];
699  sB1x += s1[n] * B1[1][n];
700  sB1y += s1[n] * B1[2][n];
701 
702  SA1 += S1[n] * A1[0][n];
703  SA1x += S1[n] * A1[1][n];
704  SA1y += S1[n] * A1[2][n];
705  SB1 += S1[n] * B1[0][n];
706  SB1x += S1[n] * B1[1][n];
707  SB1y += S1[n] * B1[2][n];
708 
709  if (order > 1)
710  for (Int_t m = 0; m < 3; m++) {
711  sA2 += s2[n][m] * A2[0][n][m];
712  sA2x += s2[n][m] * A2[1][n][m];
713  sA2y += s2[n][m] * A2[2][n][m];
714  sB2 += s2[n][m] * B2[0][n][m];
715  sB2x += s2[n][m] * B2[1][n][m];
716  sB2y += s2[n][m] * B2[2][n][m];
717 
718  SA2 += S2[n][m] * A2[0][n][m];
719  SA2x += S2[n][m] * A2[1][n][m];
720  SA2y += S2[n][m] * A2[2][n][m];
721  SB2 += S2[n][m] * B2[0][n][m];
722  SB2x += S2[n][m] * B2[1][n][m];
723  SB2y += S2[n][m] * B2[2][n][m];
724 
725  if (order > 2)
726  for (Int_t k = 0; k < 3; k++) {
727  sA3 += s3[n][m][k] * A3[0][n][m][k];
728  sA3x += s3[n][m][k] * A3[1][n][m][k];
729  sA3y += s3[n][m][k] * A3[2][n][m][k];
730  sB3 += s3[n][m][k] * B3[0][n][m][k];
731  sB3x += s3[n][m][k] * B3[1][n][m][k];
732  sB3y += s3[n][m][k] * B3[2][n][m][k];
733 
734  SA3 += S3[n][m][k] * A3[0][n][m][k];
735  SA3x += S3[n][m][k] * A3[1][n][m][k];
736  SA3y += S3[n][m][k] * A3[2][n][m][k];
737  SB3 += S3[n][m][k] * B3[0][n][m][k];
738  SB3x += S3[n][m][k] * B3[1][n][m][k];
739  SB3y += S3[n][m][k] * B3[2][n][m][k];
740  }
741  }
742  }
743  }
744 
745 
746  T_out[0] = T_in[0] + tx * dz + ht * (SA1 + ht * (SA2 + ht * SA3));
747  T_out[1] = T_in[1] + ty * dz + ht * (SB1 + ht * (SB2 + ht * SB3));
748  T_out[2] = T_in[2] + ht * (sA1 + ht * (sA2 + ht * sA3));
749  T_out[3] = T_in[3] + ht * (sB1 + ht * (sB2 + ht * sB3));
750  T_out[4] = T_in[4];
751  T_out[5] = z_out;
752 
753  Double_t J[25];
754 
755  // derivatives '_x
756 
757  J[0] = 1;
758  J[1] = 0;
759  J[2] = 0;
760  J[3] = 0;
761  J[4] = 0;
762 
763  // derivatives '_y
764 
765  J[5] = 0;
766  J[6] = 1;
767  J[7] = 0;
768  J[8] = 0;
769  J[9] = 0;
770 
771 
772  // derivatives '_tx
773 
774  J[10] = dz + h * tx * (1. / t * SA1 + h * (2 * SA2 + 3 * ht * SA3))
775  + ht * (SA1x + ht * (SA2x + ht * SA3x));
776  J[11] = h * tx * (1. / t * SB1 + h * (2 * SB2 + 3 * ht * SB3))
777  + ht * (SB1x + ht * (SB2x + ht * SB3x));
778  J[12] = 1 + h * tx * (1. / t * sA1 + h * (2 * sA2 + 3 * ht * sA3))
779  + ht * (sA1x + ht * (sA2x + ht * sA3x));
780  J[13] = h * tx * (1. / t * sB1 + h * (2 * sB2 + 3 * ht * sB3))
781  + ht * (sB1x + ht * (sB2x + ht * sB3x));
782  J[14] = 0;
783 
784 
785  // derivatives '_ty
786 
787  J[15] = h * ty * (1. / t * SA1 + h * (2 * SA2 + 3 * ht * SA3))
788  + ht * (SA1y + ht * (SA2y + ht * SA3y));
789  J[16] = dz + h * ty * (1. / t * SB1 + h * (2 * SB2 + 3 * ht * SB3))
790  + ht * (SB1y + ht * (SB2y + ht * SB3y));
791  J[17] = h * ty * (1. / t * sA1 + h * (2 * sA2 + 3 * ht * sA3))
792  + ht * (sA1y + ht * (sA2y + ht * sA3y));
793  J[18] = 1 + h * ty * (1. / t * sB1 + h * (2 * sB2 + 3 * ht * sB3))
794  + ht * (sB1y + ht * (sB2y + ht * sB3y));
795  J[19] = 0;
796 
797 
798  // derivatives '_qp
799 
800  J[20] = c_light * t * (SA1 + ht * (2 * SA2 + 3 * ht * SA3));
801  J[21] = c_light * t * (SB1 + ht * (2 * SB2 + 3 * ht * SB3));
802  J[22] = c_light * t * (sA1 + ht * (2 * sA2 + 3 * ht * sA3));
803  J[23] = c_light * t * (sB1 + ht * (2 * sB2 + 3 * ht * sB3));
804  J[24] = 1;
805 
806  // extrapolate inverse momentum
807 
808  T_out[4] = qp_in;
809 
810  Double_t dqp = qp_in - qp0;
811 
812  {
813  for (Int_t i = 0; i < 4; i++) {
814  T_out[i] += J[5 * 4 + i] * dqp;
815  }
816  }
817 
818  // covariance matrix transport
819 
820  if (C_in && C_out) CbmKFMath::multQtSQ(5, J, C_in, C_out);
821 }
822 
823 
824 /****************************************************************
825  *
826  * Analytic Central
827  *
828  ****************************************************************/
829 
830 
831 void CbmKFFieldMath::ExtrapolateACentral(
832  const Double_t T_in[], // input track parameters (x,y,tx,ty,Q/p,z)
833  const Double_t C_in[], // input covariance matrix
834  Double_t T_out[], // output track parameters
835  Double_t C_out[], // output covariance matrix
836  Double_t z_out, // extrapolate to this z position
837  Double_t qp0, // use Q/p linearisation at this value
838  CbmMagField* MF // magnetic field
839 ) {
840  //
841  // Part of the exact extrapolation formula with error (c_light*B*dz)^?/?!
842  //
843  // Under investigation, finally supposed to be fast.
844  //
845 
846  const Double_t c_light = 0.000299792458;
847 
848  Double_t qp_in = T_in[4];
849  Double_t z_in = T_in[5];
850  Double_t dz = z_out - z_in;
851 
852  Double_t i1[3], I1[3];
853 
854  { // get integrated field
855 
856  // get field value at 3 points
857 
858  Double_t r[3][3];
859 
860  r[0][0] = 0;
861  r[0][1] = 0;
862  r[0][2] = T_in[5];
863 
864  r[2][0] = 0;
865  r[2][1] = 0;
866  r[2][2] = z_out;
867 
868  r[1][0] = 0;
869  r[1][1] = 0;
870  r[1][2] = 0.5 * (r[0][2] + r[2][2]);
871 
872  IntegrateField(MF, r[0], r[1], r[2], i1, I1, 0, 0, 0, 0);
873 
874  } // get integrated field
875 
876  Double_t x = T_in[2], // tx !!
877  y = T_in[3], // ty !!
878 
879  xx = x * x, xy = x * y, yy = y * y,
880 
881  x2 = x * 2, y2 = y * 2;
882 
883 
884  Double_t X1[3] = {xy, -xx - 1, y};
885  Double_t X1x[3] = {y, -x2, 0};
886  Double_t X1y[3] = {x, 0, 1};
887 
888 
889  Double_t Y1[3] = {yy + 1, -xy, -x};
890  Double_t Y1x[3] = {0, -y, -1};
891  Double_t Y1y[3] = {y2, -x, 0};
892 
893 
894  Double_t iX1 = 0, iY1 = 0;
895  Double_t iX1x = 0, iY1x = 0;
896  Double_t iX1y = 0, iY1y = 0;
897 
898  Double_t IX1 = 0, IY1 = 0;
899  Double_t IX1x = 0, IY1x = 0;
900  Double_t IX1y = 0, IY1y = 0;
901 
902  {
903  for (Int_t n = 0; n < 3; n++) {
904  if (n != 1) continue;
905  iX1 += i1[n] * X1[n];
906  iX1x += i1[n] * X1x[n];
907  iX1y += i1[n] * X1y[n];
908  iY1 += i1[n] * Y1[n];
909  iY1x += i1[n] * Y1x[n];
910  iY1y += i1[n] * Y1y[n];
911 
912  IX1 += I1[n] * X1[n];
913  IX1x += I1[n] * X1x[n];
914  IX1y += I1[n] * X1y[n];
915  IY1 += I1[n] * Y1[n];
916  IY1x += I1[n] * Y1x[n];
917  IY1y += I1[n] * Y1y[n];
918  }
919  }
920 
921  Double_t t2 = 1. + xx + yy, t = sqrt(t2), h = qp0 * c_light, ht = h * t;
922 
923  T_out[0] = T_in[0] + x * dz + ht * IX1;
924  T_out[1] = T_in[1] + y * dz + ht * IY1;
925  T_out[2] = T_in[2] + ht * iX1;
926  T_out[3] = T_in[3] + ht * iY1;
927  T_out[4] = T_in[4];
928  T_out[5] = z_out;
929 
930  Double_t J[25];
931 
932  // derivatives '_x
933 
934  J[0] = 1;
935  J[1] = 0;
936  J[2] = 0;
937  J[3] = 0;
938  J[4] = 0;
939 
940  // derivatives '_y
941 
942  J[5] = 0;
943  J[6] = 1;
944  J[7] = 0;
945  J[8] = 0;
946  J[9] = 0;
947 
948 
949  // derivatives '_tx
950 
951  J[10] = dz + h * x / t * IX1 + ht * IX1x;
952  J[11] = h * x / t * IY1 + ht * IY1x;
953  J[12] = 1 + h * x / t * iX1 + ht * iX1x;
954  J[13] = h * x / t * iY1 + ht * iY1x;
955  J[14] = 0;
956 
957 
958  // derivatives '_ty
959 
960  J[15] = h * y / t * IX1 + ht * IX1y;
961  J[16] = dz + h * y / t * IY1 + ht * IY1y;
962  J[17] = h * y / t * iX1 + ht * iX1y;
963  J[18] = 1 + h * y / t * iY1 + ht * iY1y;
964  J[19] = 0;
965 
966 
967  // derivatives '_qp
968 
969  J[20] = c_light * t * IX1;
970  J[21] = c_light * t * IY1;
971  J[22] = c_light * t * iX1;
972  J[23] = c_light * t * iY1;
973  J[24] = 1;
974 
975  // extrapolate inverse momentum
976 
977  T_out[4] = qp_in;
978 
979  Double_t dqp = qp_in - qp0;
980 
981  {
982  for (Int_t i = 0; i < 4; i++) {
983  T_out[i] += J[5 * 4 + i] * dqp;
984  }
985  }
986 
987  // covariance matrix transport
988 
989  if (C_in && C_out) CbmKFMath::multQtSQ(5, J, C_in, C_out);
990 }
991 
992 #endif
993 
994 /****************************************************************
995  *
996  * Analytic Light
997  *
998  ****************************************************************/
999 
1000 Int_t CbmKFFieldMath::ExtrapolateALight(
1001  const Double_t T_in[], // input track parameters (x,y,tx,ty,Q/p)
1002  const Double_t C_in[], // input covariance matrix
1003  Double_t T_out[], // output track parameters
1004  Double_t C_out[], // output covariance matrix
1005  Double_t z_out, // extrapolate to this z position
1006  Double_t qp0, // use Q/p linearisation at this value
1007  FairField* MF // magnetic field
1008 ) {
1009  //
1010  // Part of the analytic extrapolation formula with error (c_light*B*dz)^4/4!
1011  //
1012  {
1013  bool ok = 1;
1014  for (int i = 0; i < 6; i++)
1015  ok = ok && finite(T_in[i]) && (T_in[i] < 1.e5);
1016  if (C_in)
1017  for (int i = 0; i < 15; i++)
1018  ok = ok && finite(C_in[i]);
1019  if (!ok) {
1020  for (int i = 0; i < 6; i++)
1021  T_out[i] = 0;
1022  if (C_out) {
1023  for (int i = 0; i < 15; i++)
1024  C_out[i] = 0;
1025  C_out[0] = C_out[2] = C_out[5] = C_out[9] = C_out[14] = 100.;
1026  }
1027  return 1;
1028  }
1029  }
1030 
1031  const Double_t c_light = 0.000299792458;
1032 
1033  Double_t qp_in = T_in[4];
1034  Double_t z_in = T_in[5];
1035  Double_t dz = z_out - z_in;
1036 
1037  // construct coefficients
1038 
1039  Double_t x = T_in[2], // tx !!
1040  y = T_in[3], // ty !!
1041 
1042  xx = x * x, xy = x * y, yy = y * y, x2 = x * 2, x4 = x * 4,
1043  xx31 = xx * 3 + 1, xx159 = xx * 15 + 9, y2 = y * 2;
1044 
1045  Double_t Ax = xy, Ay = -xx - 1, Az = y, Ayy = x * (xx * 3 + 3), Ayz = -2 * xy,
1046  Ayyy = -(15 * xx * xx + 18 * xx + 3),
1047 
1048  Ax_x = y, Ay_x = -x2, Az_x = 0, Ayy_x = 3 * xx31, Ayz_x = -y2,
1049  Ayyy_x = -x4 * xx159,
1050 
1051  Ax_y = x, Ay_y = 0, Az_y = 1, Ayy_y = 0, Ayz_y = -x2, Ayyy_y = 0,
1052 
1053  Bx = yy + 1, By = -xy, Bz = -x, Byy = y * xx31, Byz = 2 * xx + 1,
1054  Byyy = -xy * xx159,
1055 
1056  Bx_x = 0, By_x = -y, Bz_x = -1, Byy_x = 6 * xy, Byz_x = x4,
1057  Byyy_x = -y * (45 * xx + 9),
1058 
1059  Bx_y = y2, By_y = -x, Bz_y = 0, Byy_y = xx31, Byz_y = 0,
1060  Byyy_y = -x * xx159;
1061 
1062  // end of coefficients calculation
1063 
1064 
1065  Double_t t2 = 1. + xx + yy;
1066  if (t2 > 1.e4) return 1;
1067  Double_t t = sqrt(t2), h = qp0 * c_light, ht = h * t;
1068 
1069  Double_t sx = 0, sy = 0, sz = 0, syy = 0, syz = 0, syyy = 0, Sx = 0, Sy = 0,
1070  Sz = 0, Syy = 0, Syz = 0, Syyy = 0;
1071 
1072  { // get field integrals
1073 
1074  Double_t B[3][3];
1075  Double_t r0[3], r1[3], r2[3];
1076 
1077  // first order track approximation
1078 
1079  r0[0] = T_in[0];
1080  r0[1] = T_in[1];
1081  r0[2] = T_in[5];
1082 
1083  r2[0] = T_in[0] + T_in[2] * dz;
1084  r2[1] = T_in[1] + T_in[3] * dz;
1085  r2[2] = z_out;
1086 
1087  r1[0] = 0.5 * (r0[0] + r2[0]);
1088  r1[1] = 0.5 * (r0[1] + r2[1]);
1089  r1[2] = 0.5 * (r0[2] + r2[2]);
1090 
1091  MF->GetFieldValue(r0, B[0]);
1092  MF->GetFieldValue(r1, B[1]);
1093  MF->GetFieldValue(r2, B[2]);
1094 
1095  Sy = (7 * B[0][1] + 6 * B[1][1] - B[2][1]) * dz * dz / 96.;
1096  r1[0] = T_in[0] + x * dz / 2 + ht * Sy * Ay;
1097  r1[1] = T_in[1] + y * dz / 2 + ht * Sy * By;
1098 
1099  Sy = (B[0][1] + 2 * B[1][1]) * dz * dz / 6.;
1100  r2[0] = T_in[0] + x * dz + ht * Sy * Ay;
1101  r2[1] = T_in[1] + y * dz + ht * Sy * By;
1102 
1103  Sy = 0;
1104 
1105  // integrals
1106 
1107  MF->GetFieldValue(r0, B[0]);
1108  MF->GetFieldValue(r1, B[1]);
1109  MF->GetFieldValue(r2, B[2]);
1110 
1111  sx = (B[0][0] + 4 * B[1][0] + B[2][0]) * dz / 6.;
1112  sy = (B[0][1] + 4 * B[1][1] + B[2][1]) * dz / 6.;
1113  sz = (B[0][2] + 4 * B[1][2] + B[2][2]) * dz / 6.;
1114 
1115  Sx = (B[0][0] + 2 * B[1][0]) * dz * dz / 6.;
1116  Sy = (B[0][1] + 2 * B[1][1]) * dz * dz / 6.;
1117  Sz = (B[0][2] + 2 * B[1][2]) * dz * dz / 6.;
1118 
1119  Double_t c2[3][3] = {{5, -4, -1}, {44, 80, -4}, {11, 44, 5}}; // /=360.
1120  Double_t C2[3][3] = {{38, 8, -4}, {148, 208, -20}, {3, 36, 3}}; // /=2520.
1121  for (Int_t n = 0; n < 3; n++)
1122  for (Int_t m = 0; m < 3; m++) {
1123  syz += c2[n][m] * B[n][1] * B[m][2];
1124  Syz += C2[n][m] * B[n][1] * B[m][2];
1125  }
1126 
1127  syz *= dz * dz / 360.;
1128  Syz *= dz * dz * dz / 2520.;
1129 
1130  syy = (B[0][1] + 4 * B[1][1] + B[2][1]) * dz;
1131  syyy = syy * syy * syy / 1296;
1132  syy = syy * syy / 72;
1133 
1134  Syy = (B[0][1] * (38 * B[0][1] + 156 * B[1][1] - B[2][1])
1135  + B[1][1] * (208 * B[1][1] + 16 * B[2][1]) + B[2][1] * (3 * B[2][1]))
1136  * dz * dz * dz / 2520.;
1137  Syyy = (B[0][1]
1138  * (B[0][1] * (85 * B[0][1] + 526 * B[1][1] - 7 * B[2][1])
1139  + B[1][1] * (1376 * B[1][1] + 84 * B[2][1])
1140  + B[2][1] * (19 * B[2][1]))
1141  + B[1][1]
1142  * (B[1][1] * (1376 * B[1][1] + 256 * B[2][1])
1143  + B[2][1] * (62 * B[2][1]))
1144  + B[2][1] * B[2][1] * (3 * B[2][1]))
1145  * dz * dz * dz * dz / 90720.;
1146  }
1147 
1148 
1149  Double_t
1150 
1151  sA1 = sx * Ax + sy * Ay + sz * Az,
1152  sA1_x = sx * Ax_x + sy * Ay_x + sz * Az_x,
1153  sA1_y = sx * Ax_y + sy * Ay_y + sz * Az_y,
1154 
1155  sB1 = sx * Bx + sy * By + sz * Bz,
1156  sB1_x = sx * Bx_x + sy * By_x + sz * Bz_x,
1157  sB1_y = sx * Bx_y + sy * By_y + sz * Bz_y,
1158 
1159  SA1 = Sx * Ax + Sy * Ay + Sz * Az,
1160  SA1_x = Sx * Ax_x + Sy * Ay_x + Sz * Az_x,
1161  SA1_y = Sx * Ax_y + Sy * Ay_y + Sz * Az_y,
1162 
1163  SB1 = Sx * Bx + Sy * By + Sz * Bz,
1164  SB1_x = Sx * Bx_x + Sy * By_x + Sz * Bz_x,
1165  SB1_y = Sx * Bx_y + Sy * By_y + Sz * Bz_y,
1166 
1167 
1168  sA2 = syy * Ayy + syz * Ayz, sA2_x = syy * Ayy_x + syz * Ayz_x,
1169  sA2_y = syy * Ayy_y + syz * Ayz_y, sB2 = syy * Byy + syz * Byz,
1170  sB2_x = syy * Byy_x + syz * Byz_x, sB2_y = syy * Byy_y + syz * Byz_y,
1171 
1172  SA2 = Syy * Ayy + Syz * Ayz, SA2_x = Syy * Ayy_x + Syz * Ayz_x,
1173  SA2_y = Syy * Ayy_y + Syz * Ayz_y, SB2 = Syy * Byy + Syz * Byz,
1174  SB2_x = Syy * Byy_x + Syz * Byz_x, SB2_y = Syy * Byy_y + Syz * Byz_y,
1175 
1176 
1177  sA3 = syyy * Ayyy, sA3_x = syyy * Ayyy_x, sA3_y = syyy * Ayyy_y,
1178  sB3 = syyy * Byyy, sB3_x = syyy * Byyy_x, sB3_y = syyy * Byyy_y,
1179 
1180  SA3 = Syyy * Ayyy, SA3_x = Syyy * Ayyy_x, SA3_y = Syyy * Ayyy_y,
1181  SB3 = Syyy * Byyy, SB3_x = Syyy * Byyy_x, SB3_y = Syyy * Byyy_y;
1182 
1183 
1184  T_out[0] = T_in[0] + x * dz + ht * (SA1 + ht * (SA2 + ht * SA3));
1185  T_out[1] = T_in[1] + y * dz + ht * (SB1 + ht * (SB2 + ht * SB3));
1186  T_out[2] = T_in[2] + ht * (sA1 + ht * (sA2 + ht * sA3));
1187  T_out[3] = T_in[3] + ht * (sB1 + ht * (sB2 + ht * sB3));
1188  T_out[4] = T_in[4];
1189  T_out[5] = z_out;
1190 
1191  Double_t J[25];
1192 
1193  // derivatives '_x
1194 
1195  J[0] = 1;
1196  J[1] = 0;
1197  J[2] = 0;
1198  J[3] = 0;
1199  J[4] = 0;
1200 
1201  // derivatives '_y
1202 
1203  J[5] = 0;
1204  J[6] = 1;
1205  J[7] = 0;
1206  J[8] = 0;
1207  J[9] = 0;
1208 
1209 
1210  // derivatives '_tx
1211 
1212  J[10] = dz + h * x * (1. / t * SA1 + h * (2 * SA2 + 3 * ht * SA3))
1213  + ht * (SA1_x + ht * (SA2_x + ht * SA3_x));
1214  J[11] = h * x * (1. / t * SB1 + h * (2 * SB2 + 3 * ht * SB3))
1215  + ht * (SB1_x + ht * (SB2_x + ht * SB3_x));
1216  J[12] = 1 + h * x * (1. / t * sA1 + h * (2 * sA2 + 3 * ht * sA3))
1217  + ht * (sA1_x + ht * (sA2_x + ht * sA3_x));
1218  J[13] = h * x * (1. / t * sB1 + h * (2 * sB2 + 3 * ht * sB3))
1219  + ht * (sB1_x + ht * (sB2_x + ht * sB3_x));
1220  J[14] = 0;
1221 
1222 
1223  // derivatives '_ty
1224 
1225  J[15] = h * y * (1. / t * SA1 + h * (2 * SA2 + 3 * ht * SA3))
1226  + ht * (SA1_y + ht * (SA2_y + ht * SA3_y));
1227  J[16] = dz + h * y * (1. / t * SB1 + h * (2 * SB2 + 3 * ht * SB3))
1228  + ht * (SB1_y + ht * (SB2_y + ht * SB3_y));
1229  J[17] = h * y * (1. / t * sA1 + h * (2 * sA2 + 3 * ht * sA3))
1230  + ht * (sA1_y + ht * (sA2_y + ht * sA3_y));
1231  J[18] = 1 + h * y * (1. / t * sB1 + h * (2 * sB2 + 3 * ht * sB3))
1232  + ht * (sB1_y + ht * (sB2_y + ht * sB3_y));
1233  J[19] = 0;
1234 
1235 
1236  // derivatives '_qp
1237 
1238  J[20] = c_light * t * (SA1 + ht * (2 * SA2 + 3 * ht * SA3));
1239  J[21] = c_light * t * (SB1 + ht * (2 * SB2 + 3 * ht * SB3));
1240  J[22] = c_light * t * (sA1 + ht * (2 * sA2 + 3 * ht * sA3));
1241  J[23] = c_light * t * (sB1 + ht * (2 * sB2 + 3 * ht * sB3));
1242  J[24] = 1;
1243 
1244  // extrapolate inverse momentum
1245 
1246  T_out[4] = qp_in;
1247 
1248  Double_t dqp = qp_in - qp0;
1249 
1250  {
1251  for (Int_t i = 0; i < 4; i++) {
1252  T_out[i] += J[5 * 4 + i] * dqp;
1253  }
1254  }
1255 
1256  // covariance matrix transport
1257 
1258  if (C_in && C_out) CbmKFMath::multQtSQ(5, J, C_in, C_out);
1259  return 0;
1260 }
CbmKFMath::multQtSQ
static void multQtSQ(Int_t N, const Double_t Q[], const Double_t S[], Double_t S_out[])
Definition: CbmKFMath.cxx:70
CbmKFFieldMath::ExtrapolateRK4
static Int_t ExtrapolateRK4(const Double_t T_in[], const Double_t C_in[], Double_t T_out[], Double_t C_out[], Double_t z_out, Double_t qp0, FairField *MF)
Definition: CbmKFFieldMath.cxx:61
sqrt
friend F32vec4 sqrt(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:41
CbmKFFieldMath::ExtrapolateALight
static Int_t ExtrapolateALight(const Double_t T_in[], const Double_t C_in[], Double_t T_out[], Double_t C_out[], Double_t z_out, Double_t qp0, FairField *MF)
Definition: CbmKFFieldMath.cxx:1000
i
int i
Definition: L1/vectors/P4_F32vec4.h:25
CbmKFFieldMath
Definition: CbmKFFieldMath.h:19
CbmKFFieldMath.h
CbmKFFieldMath::ExtrapolateLine
static void ExtrapolateLine(const Double_t T_in[], const Double_t C_in[], Double_t T_out[], Double_t C_out[], Double_t z_out)
ClassImp
ClassImp(CbmKFFieldMath) void CbmKFFieldMath
Definition: CbmKFFieldMath.cxx:12
CbmKFMath.h
finite
T finite(T x)
Definition: CbmL1Def.h:21
h
Data class with information on a STS local track.
x
Double_t x
Definition: CbmMvdSensorDigiToHitTask.cxx:68
m
__m128 m
Definition: L1/vectors/P4_F32vec4.h:26
y
Double_t y
Definition: CbmMvdSensorDigiToHitTask.cxx:68
NS_L1TrackFitter::c_light
const fvec c_light
Definition: L1TrackFitter.cxx:31