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