CbmRoot
Loading...
Searching...
No Matches
LitExtrapolation.h
Go to the documentation of this file.
1/* Copyright (C) 2009-2012 GSI/JINR-LIT, Darmstadt/Dubna
2 SPDX-License-Identifier: GPL-3.0-only
3 Authors: Andrey Lebedev [committer] */
4
22#ifndef LITEXTRAPOLATION_H_
23#define LITEXTRAPOLATION_H_
24
25#include "LitFieldGrid.h"
26#include "LitFieldRegion.h"
27#include "LitMath.h"
28#include "LitTrackParam.h"
29
30namespace lit
31{
32 namespace parallel
33 {
34
42 template<class T>
43 inline void LitLineExtrapolation(LitTrackParam<T>& par, T zOut)
44 {
45 T dz = zOut - par.Z;
46
47 // transport state vector F*X*F.T()
48 par.X += dz * par.Tx;
49 par.Y += dz * par.Ty;
50
51 // transport covariance matrix F*C*F.T()
52 T t3 = par.C2 + dz * par.C9;
53 T t7 = dz * par.C10;
54 T t8 = par.C3 + t7;
55 T t19 = par.C7 + dz * par.C12;
56 par.C0 += dz * par.C2 + t3 * dz;
57 par.C1 += dz * par.C6 + t8 * dz;
58 par.C2 = t3;
59 par.C3 = t8;
60 par.C4 += dz * par.C11;
61 par.C5 += dz * par.C7 + t19 * dz;
62 par.C6 += t7;
63 par.C7 = t19;
64 par.C8 += dz * par.C13;
65
66 par.Z = zOut;
67 }
68
79 template<class T>
80 inline void LitRK4Extrapolation(LitTrackParam<T>& par, T zOut, const LitFieldGrid& field1,
81 const LitFieldGrid& field2, const LitFieldGrid& field3)
82 {
83 static const T fC = 0.000299792458;
84 static const T ZERO = 0., ONE = 1., TWO = 2., C1_3 = 1. / 3., C1_6 = 1. / 6.;
85
86 T coef[4] = {0.0, 0.5, 0.5, 1.0};
87
88 T Ax[4], Ay[4];
89 T dAx_dtx[4], dAy_dtx[4], dAx_dty[4], dAy_dty[4];
90 T kx[4];
91 T ky[4];
92 T ktx[4];
93 T kty[4];
94
95 T zIn = par.Z;
96 T h = zOut - zIn;
97 T hC = h * fC;
98 T hCqp = h * fC * par.Qp;
99 T x0[4];
100
101 T x[4] = {par.X, par.Y, par.Tx, par.Ty};
102
103 T F[25]; // derivatives, transport matrix
104
105 // Field values for each step
106 LitFieldValue<T> Bfield[4];
107 // Field grid for each step
108 const LitFieldGrid* Bgrid[4] = {&field1, &field2, &field2, &field3};
109 // field.GetFieldValue(zIn + coef[0] * h, B[0]);
110 // field.GetFieldValue(zIn + coef[1] * h, B[1]);
111 // B[2] = B[1];
112 // field.GetFieldValue(zIn + coef[3] * h, B[3]);
113 // B[0] = field1;
114 // B[1] = field2;
115 // B[2] = B[1];
116 // B[3] = field3;
117
118 // Calculation for zero step
119 {
120 Bgrid[0]->GetFieldValue(x[0], x[1], Bfield[0]);
121
122 T Bx = Bfield[0].Bx;
123 T By = Bfield[0].By;
124 T Bz = Bfield[0].Bz;
125
126 T tx = x[2];
127 T ty = x[3];
128 T txtx = tx * tx;
129 T tyty = ty * ty;
130 T txty = tx * ty;
131 T txtxtyty1 = ONE + txtx + tyty;
132 T t1 = sqrt(txtxtyty1);
133 T t2 = rcp(txtxtyty1); //1.0 / txtxtyty1;
134
135 Ax[0] = (txty * Bx + ty * Bz - (ONE + txtx) * By) * t1;
136 Ay[0] = (-txty * By - tx * Bz + (ONE + tyty) * Bx) * t1;
137
138 dAx_dtx[0] = Ax[0] * tx * t2 + (ty * Bx - TWO * tx * By) * t1;
139 dAx_dty[0] = Ax[0] * ty * t2 + (tx * Bx + Bz) * t1;
140 dAy_dtx[0] = Ay[0] * tx * t2 + (-ty * By - Bz) * t1;
141 dAy_dty[0] = Ay[0] * ty * t2 + (-tx * By + TWO * ty * Bx) * t1;
142
143 kx[0] = tx * h;
144 ky[0] = ty * h;
145 ktx[0] = Ax[0] * hCqp;
146 kty[0] = Ay[0] * hCqp;
147 }
148 // end calculation for zero step
149
150 // Calculate for steps starting from 1
151 for (unsigned int iStep = 1; iStep < 4; iStep++) { // 1
152 x[0] = par.X + coef[iStep] * kx[iStep - 1];
153 x[1] = par.Y + coef[iStep] * ky[iStep - 1];
154 x[2] = par.Tx + coef[iStep] * ktx[iStep - 1];
155 x[3] = par.Ty + coef[iStep] * kty[iStep - 1];
156
157 Bgrid[iStep]->GetFieldValue(x[0], x[1], Bfield[iStep]);
158
159 T Bx = Bfield[iStep].Bx;
160 T By = Bfield[iStep].By;
161 T Bz = Bfield[iStep].Bz;
162
163 T tx = x[2];
164 T ty = x[3];
165 T txtx = tx * tx;
166 T tyty = ty * ty;
167 T txty = tx * ty;
168 T txtxtyty1 = ONE + txtx + tyty;
169 T t1 = sqrt(txtxtyty1);
170 T t2 = rcp(txtxtyty1); //1.0 / txtxtyty1;
171
172 Ax[iStep] = (txty * Bx + ty * Bz - (ONE + txtx) * By) * t1;
173 Ay[iStep] = (-txty * By - tx * Bz + (ONE + tyty) * Bx) * t1;
174
175 dAx_dtx[iStep] = Ax[iStep] * tx * t2 + (ty * Bx - TWO * tx * By) * t1;
176 dAx_dty[iStep] = Ax[iStep] * ty * t2 + (tx * Bx + Bz) * t1;
177 dAy_dtx[iStep] = Ay[iStep] * tx * t2 + (-ty * By - Bz) * t1;
178 dAy_dty[iStep] = Ay[iStep] * ty * t2 + (-tx * By + TWO * ty * Bx) * t1;
179
180 kx[iStep] = tx * h;
181 ky[iStep] = ty * h;
182 ktx[iStep] = Ax[iStep] * hCqp;
183 kty[iStep] = Ay[iStep] * hCqp;
184
185 } // 1
186
187 // Calculate output state vector
188 // for (unsigned int i = 0; i < 4; i++) xOut[i] = CalcOut(xIn[i], k[i]);
189 par.X += kx[0] * C1_6 + kx[1] * C1_3 + kx[2] * C1_3 + kx[3] * C1_6;
190 par.Y += ky[0] * C1_6 + ky[1] * C1_3 + ky[2] * C1_3 + ky[3] * C1_6;
191 par.Tx += ktx[0] * C1_6 + ktx[1] * C1_3 + ktx[2] * C1_3 + ktx[3] * C1_6;
192 par.Ty += kty[0] * C1_6 + kty[1] * C1_3 + kty[2] * C1_3 + kty[3] * C1_6;
193 // xOut[4] = xIn[4];
194
195
196 // Calculation of the derivatives
197
198 // derivatives dx/dx and dx/dy
199 // dx/dx
200 F[0] = ONE;
201 F[5] = ZERO;
202 F[10] = ZERO;
203 F[15] = ZERO;
204 F[20] = ZERO;
205 // dx/dy
206 F[1] = ZERO;
207 F[6] = ONE;
208 F[11] = ZERO;
209 F[16] = ZERO;
210 F[21] = ZERO;
211 // end of derivatives dx/dx and dx/dy
212
213 // Derivatives dx/tx
214 x[0] = x0[0] = ZERO;
215 x[1] = x0[1] = ZERO;
216 x[2] = x0[2] = ONE;
217 x[3] = x0[3] = ZERO;
218
219 //Calculate for zero step
220 kx[0] = x[2] * h;
221 ky[0] = x[3] * h;
222 //ktx[0] = (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]) * hCqp;
223 kty[0] = (dAy_dtx[0] * x[2] + dAy_dty[0] * x[3]) * hCqp;
224 // Calculate for steps starting from 1
225 for (unsigned int iStep = 1; iStep < 4; iStep++) { // 2
226 x[0] = x0[0] + coef[iStep] * kx[iStep - 1];
227 x[1] = x0[1] + coef[iStep] * ky[iStep - 1];
228 // x[2] = x0[2] + coef[iStep] * ktx[iStep - 1];
229 x[3] = x0[3] + coef[iStep] * kty[iStep - 1];
230
231 kx[iStep] = x[2] * h;
232 ky[iStep] = x[3] * h;
233 //ktx[iStep] = (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]) * hCqp;
234 kty[iStep] = (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]) * hCqp;
235 } // 2
236
237 F[2] = x0[0] + kx[0] * C1_6 + kx[1] * C1_3 + kx[2] * C1_3 + kx[3] * C1_6;
238 F[7] = x0[1] + ky[0] * C1_6 + ky[1] * C1_3 + ky[2] * C1_3 + ky[3] * C1_6;
239 F[12] = ONE;
240 F[17] = x0[3] + kty[0] * C1_6 + kty[1] * C1_3 + kty[2] * C1_3 + kty[3] * C1_6;
241 F[22] = ZERO;
242 // end of derivatives dx/dtx
243
244 // Derivatives dx/ty
245 x[0] = x0[0] = ZERO;
246 x[1] = x0[1] = ZERO;
247 x[2] = x0[2] = ZERO;
248 x[3] = x0[3] = ONE;
249
250 //Calculate for zero step
251 kx[0] = x[2] * h;
252 ky[0] = x[3] * h;
253 ktx[0] = (dAx_dtx[0] * x[2] + dAx_dty[0] * x[3]) * hCqp;
254 //kty[0] = (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]) * hCqp;
255 //Calculate for steps starting from 1
256 for (unsigned int iStep = 1; iStep < 4; iStep++) { // 4
257 x[0] = x0[0] + coef[iStep] * kx[iStep - 1];
258 x[1] = x0[1] + coef[iStep] * ky[iStep - 1];
259 x[2] = x0[2] + coef[iStep] * ktx[iStep - 1];
260 // x[3] = x0[0] + coef[iStep] * kty[iStep - 1];
261
262 kx[iStep] = x[2] * h;
263 ky[iStep] = x[3] * h;
264 ktx[iStep] = (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]) * hCqp;
265 //kty[iStep] = (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]) * hCqp;
266 } // 4
267
268 F[3] = x0[0] + kx[0] * C1_6 + kx[1] * C1_3 + kx[2] * C1_3 + kx[3] * C1_6; // CalcOut(x0[0], k[0]);
269 F[8] = x0[1] + ky[0] * C1_6 + ky[1] * C1_3 + ky[2] * C1_3 + ky[3] * C1_6; //CalcOut(x0[1], k[1]);
270 F[13] = x0[2] + ktx[0] * C1_6 + ktx[1] * C1_3 + ktx[2] * C1_3 + ktx[3] * C1_6; //CalcOut(x0[2], k[2]);
271 F[18] = ONE;
272 F[23] = ZERO;
273 // end of derivatives dx/dty
274
275 // Derivatives dx/dqp
276 x[0] = x0[0] = ZERO;
277 x[1] = x0[1] = ZERO;
278 x[2] = x0[2] = ZERO;
279 x[3] = x0[3] = ZERO;
280
281 //Calculate for zero step
282 kx[0] = x[2] * h;
283 ky[0] = x[3] * h;
284 ktx[0] = Ax[0] * hC + hCqp * (dAx_dtx[0] * x[2] + dAx_dty[0] * x[3]);
285 kty[0] = Ay[0] * hC + hCqp * (dAy_dtx[0] * x[2] + dAy_dty[0] * x[3]);
286 //Calculate for steps starting from 1
287 for (unsigned int iStep = 1; iStep < 4; iStep++) { // 4
288 x[0] = x0[0] + coef[iStep] * kx[iStep - 1];
289 x[1] = x0[1] + coef[iStep] * ky[iStep - 1];
290 x[2] = x0[2] + coef[iStep] * ktx[iStep - 1];
291 x[3] = x0[3] + coef[iStep] * kty[iStep - 1];
292
293 kx[iStep] = x[2] * h;
294 ky[iStep] = x[3] * h;
295 ktx[iStep] = Ax[iStep] * hC + hCqp * (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]);
296 kty[iStep] = Ay[iStep] * hC + hCqp * (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]);
297 } // 4
298
299 F[4] = x0[0] + kx[0] * C1_6 + kx[1] * C1_3 + kx[2] * C1_3 + kx[3] * C1_6;
300 F[9] = x0[1] + ky[0] * C1_6 + ky[1] * C1_3 + ky[2] * C1_3 + ky[3] * C1_6;
301 F[14] = x0[2] + ktx[0] * C1_6 + ktx[1] * C1_3 + ktx[2] * C1_3 + ktx[3] * C1_6;
302 F[19] = x0[3] + kty[0] * C1_6 + kty[1] * C1_3 + kty[2] * C1_3 + kty[3] * C1_6;
303 F[24] = 1.;
304 // end of derivatives dx/dqp
305
306 // end calculation of the derivatives
307
308
309 // Transport C matrix
310 {
311 T cIn[15] = {par.C0, par.C1, par.C2, par.C3, par.C4, par.C5, par.C6, par.C7,
312 par.C8, par.C9, par.C10, par.C11, par.C12, par.C13, par.C14};
313 // F*C*Ft
314 T A = cIn[2] + F[2] * cIn[9] + F[3] * cIn[10] + F[4] * cIn[11];
315 T B = cIn[3] + F[2] * cIn[10] + F[3] * cIn[12] + F[4] * cIn[13];
316 T C = cIn[4] + F[2] * cIn[11] + F[3] * cIn[13] + F[4] * cIn[14];
317
318 T D = cIn[6] + F[7] * cIn[9] + F[8] * cIn[10] + F[9] * cIn[11];
319 T E = cIn[7] + F[7] * cIn[10] + F[8] * cIn[12] + F[9] * cIn[13];
320 T G = cIn[8] + F[7] * cIn[11] + F[8] * cIn[13] + F[9] * cIn[14];
321
322 T H = cIn[9] + F[13] * cIn[10] + F[14] * cIn[11];
323 T I = cIn[10] + F[13] * cIn[12] + F[14] * cIn[13];
324 T J = cIn[11] + F[13] * cIn[13] + F[14] * cIn[14];
325
326 T K = cIn[13] + F[17] * cIn[11] + F[19] * cIn[14];
327
328 par.C0 = cIn[0] + F[2] * cIn[2] + F[3] * cIn[3] + F[4] * cIn[4] + A * F[2] + B * F[3] + C * F[4];
329 par.C1 = cIn[1] + F[2] * cIn[6] + F[3] * cIn[7] + F[4] * cIn[8] + A * F[7] + B * F[8] + C * F[9];
330 par.C2 = A + B * F[13] + C * F[14];
331 par.C3 = B + A * F[17] + C * F[19];
332 par.C4 = C;
333
334 par.C5 = cIn[5] + F[7] * cIn[6] + F[8] * cIn[7] + F[9] * cIn[8] + D * F[7] + E * F[8] + G * F[9];
335 par.C6 = D + E * F[13] + G * F[14];
336 par.C7 = E + D * F[17] + G * F[19];
337 par.C8 = G;
338
339 par.C9 = H + I * F[13] + J * F[14];
340 par.C10 = I + H * F[17] + J * F[19];
341 par.C11 = J;
342
343 par.C12 = cIn[12] + F[17] * cIn[10] + F[19] * cIn[13] + (F[17] * cIn[9] + cIn[10] + F[19] * cIn[11]) * F[17]
344 + K * F[19];
345 par.C13 = K;
346
347 par.C14 = cIn[14];
348 }
349 //end transport C matrix
350
351 par.Z = zOut;
352 }
353
354
360 template<class T>
361 inline void LitRK4Extrapolation(LitTrackParam<T>& par, T zOut, const LitFieldValue<T>& field1,
362 const LitFieldValue<T>& field2, const LitFieldValue<T>& field3)
363 {
364 static const T fC = 0.000299792458;
365 static const T ZERO = 0., ONE = 1., TWO = 2., C1_3 = 1. / 3., C1_6 = 1. / 6.;
366
367 T coef[4] = {0.0, 0.5, 0.5, 1.0};
368
369 T Ax[4], Ay[4];
370 T dAx_dtx[4], dAy_dtx[4], dAx_dty[4], dAy_dty[4];
371 T kx[4];
372 T ky[4];
373 T ktx[4];
374 T kty[4];
375
376 T zIn = par.Z;
377 T h = zOut - zIn;
378 T hC = h * fC;
379 T hCqp = h * fC * par.Qp;
380 T x0[4];
381
382 T x[4] = {par.X, par.Y, par.Tx, par.Ty};
383
384 T F[25]; // derivatives, transport matrix
385
386 // Field values for each step
387 LitFieldValue<T> Bfield[4];
388 // Field grid for each step
389 //const LitFieldGrid* Bgrid[4] = {&field1, &field2, &field2, &field3};
390 // field.GetFieldValue(zIn + coef[0] * h, B[0]);
391 // field.GetFieldValue(zIn + coef[1] * h, B[1]);
392 // B[2] = B[1];
393 // field.GetFieldValue(zIn + coef[3] * h, B[3]);
394 Bfield[0] = field1;
395 Bfield[1] = field2;
396 Bfield[2] = Bfield[1];
397 Bfield[3] = field3;
398
399 // Calculation for zero step
400 {
401 // Bgrid[0]->GetFieldValue(x[0], x[1], B[0]);
402
403 T Bx = Bfield[0].Bx;
404 T By = Bfield[0].By;
405 T Bz = Bfield[0].Bz;
406
407 T tx = x[2];
408 T ty = x[3];
409 T txtx = tx * tx;
410 T tyty = ty * ty;
411 T txty = tx * ty;
412 T txtxtyty1 = ONE + txtx + tyty;
413 T t1 = sqrt(txtxtyty1);
414 T t2 = rcp(txtxtyty1); //1.0 / txtxtyty1;
415
416 Ax[0] = (txty * Bx + ty * Bz - (ONE + txtx) * By) * t1;
417 Ay[0] = (-txty * By - tx * Bz + (ONE + tyty) * Bx) * t1;
418
419 dAx_dtx[0] = Ax[0] * tx * t2 + (ty * Bx - TWO * tx * By) * t1;
420 dAx_dty[0] = Ax[0] * ty * t2 + (tx * Bx + Bz) * t1;
421 dAy_dtx[0] = Ay[0] * tx * t2 + (-ty * By - Bz) * t1;
422 dAy_dty[0] = Ay[0] * ty * t2 + (-tx * By + TWO * ty * Bx) * t1;
423
424 kx[0] = tx * h;
425 ky[0] = ty * h;
426 ktx[0] = Ax[0] * hCqp;
427 kty[0] = Ay[0] * hCqp;
428 }
429 // end calculation for zero step
430
431 // Calculate for steps starting from 1
432 for (unsigned int iStep = 1; iStep < 4; iStep++) { // 1
433 x[0] = par.X + coef[iStep] * kx[iStep - 1];
434 x[1] = par.Y + coef[iStep] * ky[iStep - 1];
435 x[2] = par.Tx + coef[iStep] * ktx[iStep - 1];
436 x[3] = par.Ty + coef[iStep] * kty[iStep - 1];
437
438 //Bgrid[iStep]->GetFieldValue(x[0], x[1], B[iStep]);
439
440 T Bx = Bfield[iStep].Bx;
441 T By = Bfield[iStep].By;
442 T Bz = Bfield[iStep].Bz;
443
444 T tx = x[2];
445 T ty = x[3];
446 T txtx = tx * tx;
447 T tyty = ty * ty;
448 T txty = tx * ty;
449 T txtxtyty1 = ONE + txtx + tyty;
450 T t1 = sqrt(txtxtyty1);
451 T t2 = rcp(txtxtyty1); //1.0 / txtxtyty1;
452
453 Ax[iStep] = (txty * Bx + ty * Bz - (ONE + txtx) * By) * t1;
454 Ay[iStep] = (-txty * By - tx * Bz + (ONE + tyty) * Bx) * t1;
455
456 dAx_dtx[iStep] = Ax[iStep] * tx * t2 + (ty * Bx - TWO * tx * By) * t1;
457 dAx_dty[iStep] = Ax[iStep] * ty * t2 + (tx * Bx + Bz) * t1;
458 dAy_dtx[iStep] = Ay[iStep] * tx * t2 + (-ty * By - Bz) * t1;
459 dAy_dty[iStep] = Ay[iStep] * ty * t2 + (-tx * By + TWO * ty * Bx) * t1;
460
461 kx[iStep] = tx * h;
462 ky[iStep] = ty * h;
463 ktx[iStep] = Ax[iStep] * hCqp;
464 kty[iStep] = Ay[iStep] * hCqp;
465
466 } // 1
467
468 // Calculate output state vector
469 // for (unsigned int i = 0; i < 4; i++) xOut[i] = CalcOut(xIn[i], k[i]);
470 par.X += kx[0] * C1_6 + kx[1] * C1_3 + kx[2] * C1_3 + kx[3] * C1_6;
471 par.Y += ky[0] * C1_6 + ky[1] * C1_3 + ky[2] * C1_3 + ky[3] * C1_6;
472 par.Tx += ktx[0] * C1_6 + ktx[1] * C1_3 + ktx[2] * C1_3 + ktx[3] * C1_6;
473 par.Ty += kty[0] * C1_6 + kty[1] * C1_3 + kty[2] * C1_3 + kty[3] * C1_6;
474 // xOut[4] = xIn[4];
475
476
477 // Calculation of the derivatives
478
479 // derivatives dx/dx and dx/dy
480 // dx/dx
481 F[0] = ONE;
482 F[5] = ZERO;
483 F[10] = ZERO;
484 F[15] = ZERO;
485 F[20] = ZERO;
486 // dx/dy
487 F[1] = ZERO;
488 F[6] = ONE;
489 F[11] = ZERO;
490 F[16] = ZERO;
491 F[21] = ZERO;
492 // end of derivatives dx/dx and dx/dy
493
494 // Derivatives dx/tx
495 x[0] = x0[0] = ZERO;
496 x[1] = x0[1] = ZERO;
497 x[2] = x0[2] = ONE;
498 x[3] = x0[3] = ZERO;
499
500 //Calculate for zero step
501 kx[0] = x[2] * h;
502 ky[0] = x[3] * h;
503 //ktx[0] = (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]) * hCqp;
504 kty[0] = (dAy_dtx[0] * x[2] + dAy_dty[0] * x[3]) * hCqp;
505 // Calculate for steps starting from 1
506 for (unsigned int iStep = 1; iStep < 4; iStep++) { // 2
507 x[0] = x0[0] + coef[iStep] * kx[iStep - 1];
508 x[1] = x0[1] + coef[iStep] * ky[iStep - 1];
509 // x[2] = x0[2] + coef[iStep] * ktx[iStep - 1];
510 x[3] = x0[3] + coef[iStep] * kty[iStep - 1];
511
512 kx[iStep] = x[2] * h;
513 ky[iStep] = x[3] * h;
514 //ktx[iStep] = (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]) * hCqp;
515 kty[iStep] = (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]) * hCqp;
516 } // 2
517
518 F[2] = x0[0] + kx[0] * C1_6 + kx[1] * C1_3 + kx[2] * C1_3 + kx[3] * C1_6;
519 F[7] = x0[1] + ky[0] * C1_6 + ky[1] * C1_3 + ky[2] * C1_3 + ky[3] * C1_6;
520 F[12] = ONE;
521 F[17] = x0[3] + kty[0] * C1_6 + kty[1] * C1_3 + kty[2] * C1_3 + kty[3] * C1_6;
522 F[22] = ZERO;
523 // end of derivatives dx/dtx
524
525 // Derivatives dx/ty
526 x[0] = x0[0] = ZERO;
527 x[1] = x0[1] = ZERO;
528 x[2] = x0[2] = ZERO;
529 x[3] = x0[3] = ONE;
530
531 //Calculate for zero step
532 kx[0] = x[2] * h;
533 ky[0] = x[3] * h;
534 ktx[0] = (dAx_dtx[0] * x[2] + dAx_dty[0] * x[3]) * hCqp;
535 //kty[0] = (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]) * hCqp;
536 //Calculate for steps starting from 1
537 for (unsigned int iStep = 1; iStep < 4; iStep++) { // 4
538 x[0] = x0[0] + coef[iStep] * kx[iStep - 1];
539 x[1] = x0[1] + coef[iStep] * ky[iStep - 1];
540 x[2] = x0[2] + coef[iStep] * ktx[iStep - 1];
541 // x[3] = x0[0] + coef[iStep] * kty[iStep - 1];
542
543 kx[iStep] = x[2] * h;
544 ky[iStep] = x[3] * h;
545 ktx[iStep] = (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]) * hCqp;
546 //kty[iStep] = (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]) * hCqp;
547 } // 4
548
549 F[3] = x0[0] + kx[0] * C1_6 + kx[1] * C1_3 + kx[2] * C1_3 + kx[3] * C1_6; // CalcOut(x0[0], k[0]);
550 F[8] = x0[1] + ky[0] * C1_6 + ky[1] * C1_3 + ky[2] * C1_3 + ky[3] * C1_6; //CalcOut(x0[1], k[1]);
551 F[13] = x0[2] + ktx[0] * C1_6 + ktx[1] * C1_3 + ktx[2] * C1_3 + ktx[3] * C1_6; //CalcOut(x0[2], k[2]);
552 F[18] = ONE;
553 F[23] = ZERO;
554 // end of derivatives dx/dty
555
556 // Derivatives dx/dqp
557 x[0] = x0[0] = ZERO;
558 x[1] = x0[1] = ZERO;
559 x[2] = x0[2] = ZERO;
560 x[3] = x0[3] = ZERO;
561
562 //Calculate for zero step
563 kx[0] = x[2] * h;
564 ky[0] = x[3] * h;
565 ktx[0] = Ax[0] * hC + hCqp * (dAx_dtx[0] * x[2] + dAx_dty[0] * x[3]);
566 kty[0] = Ay[0] * hC + hCqp * (dAy_dtx[0] * x[2] + dAy_dty[0] * x[3]);
567 //Calculate for steps starting from 1
568 for (unsigned int iStep = 1; iStep < 4; iStep++) { // 4
569 x[0] = x0[0] + coef[iStep] * kx[iStep - 1];
570 x[1] = x0[1] + coef[iStep] * ky[iStep - 1];
571 x[2] = x0[2] + coef[iStep] * ktx[iStep - 1];
572 x[3] = x0[3] + coef[iStep] * kty[iStep - 1];
573
574 kx[iStep] = x[2] * h;
575 ky[iStep] = x[3] * h;
576 ktx[iStep] = Ax[iStep] * hC + hCqp * (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]);
577 kty[iStep] = Ay[iStep] * hC + hCqp * (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]);
578 } // 4
579
580 F[4] = x0[0] + kx[0] * C1_6 + kx[1] * C1_3 + kx[2] * C1_3 + kx[3] * C1_6;
581 F[9] = x0[1] + ky[0] * C1_6 + ky[1] * C1_3 + ky[2] * C1_3 + ky[3] * C1_6;
582 F[14] = x0[2] + ktx[0] * C1_6 + ktx[1] * C1_3 + ktx[2] * C1_3 + ktx[3] * C1_6;
583 F[19] = x0[3] + kty[0] * C1_6 + kty[1] * C1_3 + kty[2] * C1_3 + kty[3] * C1_6;
584 F[24] = 1.;
585 // end of derivatives dx/dqp
586
587 // end calculation of the derivatives
588
589
590 // Transport C matrix
591 {
592 T cIn[15] = {par.C0, par.C1, par.C2, par.C3, par.C4, par.C5, par.C6, par.C7,
593 par.C8, par.C9, par.C10, par.C11, par.C12, par.C13, par.C14};
594 // F*C*Ft
595 T A = cIn[2] + F[2] * cIn[9] + F[3] * cIn[10] + F[4] * cIn[11];
596 T B = cIn[3] + F[2] * cIn[10] + F[3] * cIn[12] + F[4] * cIn[13];
597 T C = cIn[4] + F[2] * cIn[11] + F[3] * cIn[13] + F[4] * cIn[14];
598
599 T D = cIn[6] + F[7] * cIn[9] + F[8] * cIn[10] + F[9] * cIn[11];
600 T E = cIn[7] + F[7] * cIn[10] + F[8] * cIn[12] + F[9] * cIn[13];
601 T G = cIn[8] + F[7] * cIn[11] + F[8] * cIn[13] + F[9] * cIn[14];
602
603 T H = cIn[9] + F[13] * cIn[10] + F[14] * cIn[11];
604 T I = cIn[10] + F[13] * cIn[12] + F[14] * cIn[13];
605 T J = cIn[11] + F[13] * cIn[13] + F[14] * cIn[14];
606
607 T K = cIn[13] + F[17] * cIn[11] + F[19] * cIn[14];
608
609 par.C0 = cIn[0] + F[2] * cIn[2] + F[3] * cIn[3] + F[4] * cIn[4] + A * F[2] + B * F[3] + C * F[4];
610 par.C1 = cIn[1] + F[2] * cIn[6] + F[3] * cIn[7] + F[4] * cIn[8] + A * F[7] + B * F[8] + C * F[9];
611 par.C2 = A + B * F[13] + C * F[14];
612 par.C3 = B + A * F[17] + C * F[19];
613 par.C4 = C;
614
615 par.C5 = cIn[5] + F[7] * cIn[6] + F[8] * cIn[7] + F[9] * cIn[8] + D * F[7] + E * F[8] + G * F[9];
616 par.C6 = D + E * F[13] + G * F[14];
617 par.C7 = E + D * F[17] + G * F[19];
618 par.C8 = G;
619
620 par.C9 = H + I * F[13] + J * F[14];
621 par.C10 = I + H * F[17] + J * F[19];
622 par.C11 = J;
623
624 par.C12 = cIn[12] + F[17] * cIn[10] + F[19] * cIn[13] + (F[17] * cIn[9] + cIn[10] + F[19] * cIn[11]) * F[17]
625 + K * F[19];
626 par.C13 = K;
627
628 par.C14 = cIn[14];
629 }
630 //end transport C matrix
631
632 par.Z = zOut;
633 }
634
635
644 template<class T>
645 inline void LitRK4Extrapolation(LitTrackParam<T>& par, T zOut, const LitFieldRegion<T>& field)
646 {
647 static const T C1_2 = 0.5;
648 T zIn = par.Z;
649 T h = zOut - zIn;
650 LitFieldValue<T> v1, v2, v3;
651 field.GetFieldValue(zIn, v1);
652 field.GetFieldValue(zIn + C1_2 * h, v2);
653 field.GetFieldValue(zIn + h, v3);
654
655 LitRK4Extrapolation(par, zOut, v1, v2, v3);
656 }
657
658 } // namespace parallel
659} // namespace lit
660#endif /* LITEXTRAPOLATION_H_ */
sqrt
friend fvec sqrt(const fvec &a)
Definition KfSimdPseudo.h:118
LitFieldGrid.h
Class stores a grid of magnetic field values in XY slice at Z position.
LitMath.h
Useful math functions.
LitTrackParam.h
Track parameters data class.
h
Data class with information on a STS local track.
lit::parallel::LitFieldGrid
Class stores a grid of magnetic field values in XY slice at Z position.
Definition LitFieldGrid.h:51
lit::parallel::LitFieldGrid::GetFieldValue
void GetFieldValue(fscal x, fscal y, LitFieldValue< fscal > &B) const
Return field value for (X, Y) position (scalar version).
Definition LitFieldGrid.h:118
lit::parallel::LitFieldRegion
Storage for field approximation along Z.
Definition LitFieldRegion.h:32
lit::parallel::LitFieldRegion::GetFieldValue
void GetFieldValue(const T &z, LitFieldValue< T > &B) const
Returns field value at a certain Z position.
Definition LitFieldRegion.h:123
lit::parallel::LitFieldValue
Magnetic field value at a certain point in the space.
Definition LitFieldValue.h:36
lit::parallel::LitTrackParam
Track parameters data class.
Definition LitTrackParam.h:40
lit::parallel::rcp
fscal rcp(const fscal &a)
Returns reciprocal.
Definition LitMath.h:32
lit::parallel::LitRK4Extrapolation
void LitRK4Extrapolation(LitTrackParam< T > &par, T zOut, const LitFieldGrid &field1, const LitFieldGrid &field2, const LitFieldGrid &field3)
Definition LitExtrapolation.h:80
lit::parallel::LitLineExtrapolation
void LitLineExtrapolation(LitTrackParam< T > &par, T zOut)
Line track extrapolation for the field free regions.
Definition LitExtrapolation.h:43