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