CbmRoot
CbmLitRK4TrackExtrapolator.cxx
Go to the documentation of this file.
1 
6 #include "propagation/CbmLitRK4TrackExtrapolator.h"
7 
8 #include "interface/CbmLitField.h"
9 
10 #include <cmath>
11 #include <complex>
12 
13 CbmLitRK4TrackExtrapolator::CbmLitRK4TrackExtrapolator(
14  std::shared_ptr<CbmLitField> field)
15  : fField(field) {}
16 
17 CbmLitRK4TrackExtrapolator::~CbmLitRK4TrackExtrapolator() {}
18 
19 LitStatus CbmLitRK4TrackExtrapolator::Extrapolate(const CbmLitTrackParam* parIn,
20  CbmLitTrackParam* parOut,
21  litfloat zOut,
22  std::vector<litfloat>* F) {
23  *parOut = *parIn;
24  return Extrapolate(parOut, zOut, F);
25 }
26 
27 LitStatus CbmLitRK4TrackExtrapolator::Extrapolate(CbmLitTrackParam* par,
28  litfloat zOut,
29  std::vector<litfloat>* F) {
30  litfloat zIn = par->GetZ();
31 
32  std::vector<litfloat> xIn = par->GetStateVector();
33  std::vector<litfloat> xOut(6, 0.);
34  std::vector<litfloat> F1(36, 0.);
35 
36  RK4Order(xIn, zIn, xOut, zOut, F1);
37 
38  std::vector<litfloat> cIn = par->GetCovMatrix();
39  std::vector<litfloat> cOut(21);
40  TransportC(cIn, F1, cOut);
41 
42  par->SetStateVector(xOut);
43  par->SetCovMatrix(cOut);
44  par->SetZ(zOut);
45 
46  if (F != NULL) { F->assign(F1.begin(), F1.end()); }
47 
48  return kLITSUCCESS;
49 }
50 
51 void CbmLitRK4TrackExtrapolator::RK4Order(const std::vector<litfloat>& xIn,
52  litfloat zIn,
53  std::vector<litfloat>& xOut,
54  litfloat zOut,
55  std::vector<litfloat>& derivs) const {
56  const litfloat fC = 0.000299792458;
57 
58  litfloat coef[4] = {0.0, 0.5, 0.5, 1.0};
59 
60  litfloat Ax[4], Ay[4];
61  litfloat dAx_dtx[4], dAy_dtx[4], dAx_dty[4], dAy_dty[4];
62  litfloat k[5][4];
63  litfloat fTx[4];
64  litfloat fTy[4];
65 
66  litfloat h = zOut - zIn;
67  litfloat hC = h * fC;
68  litfloat hCqp = h * fC * xIn[4];
69  litfloat x0[5];
70 
71  litfloat x[5] = {xIn[0], xIn[1], xIn[2], xIn[3], xIn[5]};
72 
73  for (unsigned int iStep = 0; iStep < 4; iStep++) { // 1
74  if (iStep > 0) {
75  for (unsigned int i = 0; i < 5; i++) {
76  x[i] = xIn[4 == i ? 5 : i] + coef[iStep] * k[i][iStep - 1];
77  }
78  }
79 
80  litfloat Bx, By, Bz;
81  fField->GetFieldValue(x[0], x[1], zIn + coef[iStep] * h, Bx, By, Bz);
82 
83  litfloat tx = x[2];
84  fTx[iStep] = tx;
85  litfloat ty = x[3];
86  fTy[iStep] = ty;
87  litfloat txtx = tx * tx;
88  litfloat tyty = ty * ty;
89  litfloat txty = tx * ty;
90  litfloat txtxtyty1 = 1.0 + txtx + tyty;
91  litfloat t1 = std::sqrt(txtxtyty1);
92  litfloat t2 = 1.0 / txtxtyty1;
93 
94  Ax[iStep] = (txty * Bx + ty * Bz - (1.0 + txtx) * By) * t1;
95  Ay[iStep] = (-txty * By - tx * Bz + (1.0 + tyty) * Bx) * t1;
96 
97  dAx_dtx[iStep] = Ax[iStep] * tx * t2 + (ty * Bx - 2.0 * tx * By) * t1;
98  dAx_dty[iStep] = Ax[iStep] * ty * t2 + (tx * Bx + Bz) * t1;
99  dAy_dtx[iStep] = Ay[iStep] * tx * t2 + (-ty * By - Bz) * t1;
100  dAy_dty[iStep] = Ay[iStep] * ty * t2 + (-tx * By + 2.0 * ty * Bx) * t1;
101 
102  k[0][iStep] = tx * h;
103  k[1][iStep] = ty * h;
104  k[2][iStep] = Ax[iStep] * hCqp;
105  k[3][iStep] = Ay[iStep] * hCqp;
106  k[4][iStep] = t1 * h / CbmLitTrackParam::fSpeedOfLight;
107  } // 1
108 
109  for (unsigned int i = 0; i < 4; i++) {
110  xOut[i] = CalcOut(xIn[i], k[i]);
111  }
112  xOut[4] = xIn[4];
113  xOut[5] = CalcOut(xIn[5], k[4]);
114 
115  // Calculation of the derivatives
116 
117  // derivatives dx/dx, dx/dy and dx/dt
118  // dx/dx
119  derivs[0] = 1.;
120  derivs[6] = 0.;
121  derivs[12] = 0.;
122  derivs[18] = 0.;
123  derivs[24] = 0.;
124  derivs[30] = 0.;
125  // dx/dy
126  derivs[1] = 0.;
127  derivs[7] = 1.;
128  derivs[13] = 0.;
129  derivs[19] = 0.;
130  derivs[25] = 0.;
131  derivs[31] = 0.;
132  // dx/dt
133  derivs[5] = 0.;
134  derivs[11] = 0.;
135  derivs[17] = 0.;
136  derivs[23] = 0.;
137  derivs[29] = 0.;
138  derivs[35] = 1.;
139  // end of derivatives dx/dx, dx/dy and dx/dt
140 
141  litfloat fT1 = std::sqrt(1 + xIn[2] * xIn[2] + xIn[3] * xIn[3]);
142 
143  // Derivatives dx/tx
144  x[0] = x0[0] = 0.0;
145  x[1] = x0[1] = 0.0;
146  x[2] = x0[2] = 1.0;
147  x[3] = x0[3] = 0.0;
148  x[4] = x0[4] = 0.0;
149  for (unsigned int iStep = 0; iStep < 4; iStep++) { // 2
150  if (iStep > 0) {
151  for (unsigned int i = 0; i < 5; i++) {
152  if (i != 2) { x[i] = x0[i] + coef[iStep] * k[i][iStep - 1]; }
153  }
154  }
155 
156  k[0][iStep] = x[2] * h;
157  k[1][iStep] = x[3] * h;
158  //k[2][iStep] = (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]) * hCqp;
159  k[3][iStep] = (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]) * hCqp;
160  k[4][iStep] = (fTx[iStep] * x[2] + fTy[iStep] * x[3]) * h
161  / CbmLitTrackParam::fSpeedOfLight / fT1;
162  } // 2
163 
164  derivs[2] = CalcOut(x0[0], k[0]);
165  derivs[8] = CalcOut(x0[1], k[1]);
166  derivs[14] = 1.0;
167  derivs[20] = CalcOut(x0[3], k[3]);
168  derivs[26] = 0.0;
169  derivs[32] = CalcOut(x0[4], k[4]);
170  // end of derivatives dx/dtx
171 
172  // Derivatives dx/ty
173  x[0] = x0[0] = 0.0;
174  x[1] = x0[1] = 0.0;
175  x[2] = x0[2] = 0.0;
176  x[3] = x0[3] = 1.0;
177  x[4] = x0[4] = 0.0;
178  for (unsigned int iStep = 0; iStep < 4; iStep++) { // 4
179  if (iStep > 0) {
180  for (unsigned int i = 0; i < 5; i++) {
181  if (i != 3) { x[i] = x0[i] + coef[iStep] * k[i][iStep - 1]; }
182  }
183  }
184 
185  k[0][iStep] = x[2] * h;
186  k[1][iStep] = x[3] * h;
187  k[2][iStep] = (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]) * hCqp;
188  //k[3][iStep] = (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]) * hCqp;
189  k[4][iStep] = (fTx[iStep] * x[2] + fTy[iStep] * x[3]) * h
190  / CbmLitTrackParam::fSpeedOfLight / fT1;
191  } // 4
192 
193  derivs[3] = CalcOut(x0[0], k[0]);
194  derivs[9] = CalcOut(x0[1], k[1]);
195  derivs[15] = CalcOut(x0[2], k[2]);
196  derivs[21] = 1.;
197  derivs[27] = 0.;
198  derivs[33] = CalcOut(x0[4], k[4]);
199  // end of derivatives dx/dty
200 
201  // Derivatives dx/dqp
202  x[0] = x0[0] = 0.0;
203  x[1] = x0[1] = 0.0;
204  x[2] = x0[2] = 0.0;
205  x[3] = x0[3] = 0.0;
206  x[4] = x0[4] = 0.0;
207  for (unsigned int iStep = 0; iStep < 4; iStep++) { // 4
208  if (iStep > 0) {
209  for (unsigned int i = 0; i < 5; i++) {
210  x[i] = x0[i] + coef[iStep] * k[i][iStep - 1];
211  }
212  }
213 
214  k[0][iStep] = x[2] * h;
215  k[1][iStep] = x[3] * h;
216  k[2][iStep] =
217  Ax[iStep] * hC + hCqp * (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]);
218  k[3][iStep] =
219  Ay[iStep] * hC + hCqp * (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]);
220  k[4][iStep] = (fTx[iStep] * x[2] + fTy[iStep] * x[3]) * h
221  / CbmLitTrackParam::fSpeedOfLight / fT1;
222  } // 4
223 
224  derivs[4] = CalcOut(x0[0], k[0]);
225  derivs[10] = CalcOut(x0[1], k[1]);
226  derivs[16] = CalcOut(x0[2], k[2]);
227  derivs[22] = CalcOut(x0[3], k[3]);
228  derivs[28] = 1.;
229  derivs[34] = CalcOut(x0[4], k[4]);
230  // end of derivatives dx/dqp
231 
232  // end calculation of the derivatives
233 }
234 
235 litfloat CbmLitRK4TrackExtrapolator::CalcOut(litfloat in,
236  const litfloat k[4]) const {
237  return in + k[0] / 6. + k[1] / 3. + k[2] / 3. + k[3] / 6.;
238 }
239 
240 static void MultiplyMatrices(const std::vector<litfloat>& lm,
241  const std::vector<litfloat>& rm,
242  std::vector<litfloat>& res) {
243  for (int i = 0; i < 6; ++i) {
244  for (int j = 0; j < 6; ++j) {
245  litfloat& r = res[6 * i + j];
246  r = 0;
247 
248  for (int k = 0; k < 6; ++k)
249  r += lm[6 * i + k] * rm[j + 6 * k];
250  }
251  }
252 }
253 
254 static void TransposeMatrix(const std::vector<litfloat>& m,
255  std::vector<litfloat>& res) {
256  for (int i = 0; i < 6; ++i) {
257  for (int j = 0; j < 6; ++j)
258  res[i + 6 * j] = m[6 * i + j];
259  }
260 }
261 
262 void CbmLitRK4TrackExtrapolator::TransportC(const std::vector<litfloat>& cIn,
263  const std::vector<litfloat>& F,
264  std::vector<litfloat>& cOut) const {
265  // F*C*Ft
266  /*litfloat A = cIn[2] + F[2] * cIn[9] + F[3] * cIn[10] + F[4] * cIn[11];
267  litfloat B = cIn[3] + F[2] * cIn[10] + F[3] * cIn[12] + F[4] * cIn[13];
268  litfloat C = cIn[4] + F[2] * cIn[11] + F[3] * cIn[13] + F[4] * cIn[14];
269 
270  litfloat D = cIn[6] + F[7] * cIn[9] + F[8] * cIn[10] + F[9] * cIn[11];
271  litfloat E = cIn[7] + F[7] * cIn[10] + F[8] * cIn[12] + F[9] * cIn[13];
272  litfloat G = cIn[8] + F[7] * cIn[11] + F[8] * cIn[13] + F[9] * cIn[14];
273 
274  litfloat H = cIn[9] + F[13] * cIn[10] + F[14] * cIn[11];
275  litfloat I = cIn[10] + F[13] * cIn[12] + F[14] * cIn[13];
276  litfloat J = cIn[11] + F[13] * cIn[13] + F[14] * cIn[14];
277 
278  litfloat K = cIn[13] + F[17] * cIn[11] + F[19] * cIn[14];
279 
280  cOut[0] = cIn[0] + F[2] * cIn[2] + F[3] * cIn[3] + F[4] * cIn[4] + A * F[2] + B * F[3] + C * F[4];
281  cOut[1] = cIn[1] + F[2] * cIn[6] + F[3] * cIn[7] + F[4] * cIn[8] + A * F[7] + B * F[8] + C * F[9];
282  cOut[2] = A + B * F[13] + C * F[14];
283  cOut[3] = B + A * F[17] + C * F[19];
284  cOut[4] = C;
285 
286  cOut[5] = cIn[5] + F[7] * cIn[6] + F[8] * cIn[7] + F[9] * cIn[8] + D * F[7] + E * F[8] + G * F[9];
287  cOut[6] = D + E * F[13] + G * F[14];
288  cOut[7] = E + D * F[17] + G * F[19];
289  cOut[8] = G;
290 
291  cOut[9] = H + I * F[13] + J * F[14];
292  cOut[10] = I + H * F[17] + J * F[19];
293  cOut[11] = J;
294 
295  cOut[12] = cIn[12] + F[17] * cIn[10] + F[19] * cIn[13] + (F[17] * cIn[9] + cIn[10] + F[19] * cIn[11]) * F[17] + K * F[19];
296  cOut[13] = K;
297 
298  cOut[14] = cIn[14];*/
299  std::vector<litfloat> C(36);
300  int k = 0;
301 
302  for (int i = 0; i < 6; ++i) {
303  for (int j = i; j < 6; ++j) {
304  C[6 * i + j] = cIn[k++];
305 
306  if (i < j) C[i + 6 * j] = C[6 * i + j];
307  }
308  }
309 
310  std::vector<litfloat> tmp(36);
311  MultiplyMatrices(F, C, tmp);
312  std::vector<litfloat> FT(36);
313  TransposeMatrix(F, FT);
314  std::vector<litfloat> tmp2(36);
315  MultiplyMatrices(tmp, FT, tmp2);
316  k = 0;
317 
318  for (int i = 0; i < 6; ++i) {
319  for (int j = i; j < 6; ++j)
320  cOut[k++] = tmp2[6 * i + j];
321  }
322 }
CbmLitRK4TrackExtrapolator.h
litfloat
double litfloat
Definition: CbmLitFloat.h:15
sqrt
friend F32vec4 sqrt(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:41
CbmLitTrackParam
Data class for track parameters.
Definition: CbmLitTrackParam.h:29
CbmLitRK4TrackExtrapolator::fField
std::shared_ptr< CbmLitField > fField
Definition: CbmLitRK4TrackExtrapolator.h:73
i
int i
Definition: L1/vectors/P4_F32vec4.h:25
TransposeMatrix
static void TransposeMatrix(const std::vector< litfloat > &m, std::vector< litfloat > &res)
Definition: CbmLitRK4TrackExtrapolator.cxx:254
CbmLitTrackParam::SetZ
void SetZ(litfloat z)
Definition: CbmLitTrackParam.h:66
CbmLitField.h
Interface for accessing the magnetic field.
CbmLitTrackParam::GetStateVector
vector< litfloat > GetStateVector() const
Return state vector as vector.
Definition: CbmLitTrackParam.h:97
kLITSUCCESS
@ kLITSUCCESS
Definition: CbmLitEnums.h:24
h
Data class with information on a STS local track.
CbmLitTrackParam::GetZ
litfloat GetZ() const
Definition: CbmLitTrackParam.h:55
CbmLitRK4TrackExtrapolator::~CbmLitRK4TrackExtrapolator
virtual ~CbmLitRK4TrackExtrapolator()
Definition: CbmLitRK4TrackExtrapolator.cxx:17
CbmLitRK4TrackExtrapolator::TransportC
void TransportC(const std::vector< litfloat > &cIn, const std::vector< litfloat > &F, std::vector< litfloat > &cOut) const
Definition: CbmLitRK4TrackExtrapolator.cxx:262
MultiplyMatrices
static void MultiplyMatrices(const std::vector< litfloat > &lm, const std::vector< litfloat > &rm, std::vector< litfloat > &res)
Definition: CbmLitRK4TrackExtrapolator.cxx:240
CbmLitTrackParam::SetStateVector
void SetStateVector(const vector< litfloat > &x)
Set parameters from vector.
Definition: CbmLitTrackParam.h:112
CbmLitTrackParam::SetCovMatrix
void SetCovMatrix(const vector< litfloat > &C)
Definition: CbmLitTrackParam.h:71
x
Double_t x
Definition: CbmMvdSensorDigiToHitTask.cxx:68
m
__m128 m
Definition: L1/vectors/P4_F32vec4.h:26
CbmLitRK4TrackExtrapolator::CbmLitRK4TrackExtrapolator
CbmLitRK4TrackExtrapolator(std::shared_ptr< CbmLitField > field)
Definition: CbmLitRK4TrackExtrapolator.cxx:13
CbmLitTrackParam::GetCovMatrix
const vector< litfloat > & GetCovMatrix() const
Definition: CbmLitTrackParam.h:61
CbmLitRK4TrackExtrapolator::CalcOut
litfloat CalcOut(litfloat in, const litfloat k[4]) const
Definition: CbmLitRK4TrackExtrapolator.cxx:235
LitStatus
LitStatus
Definition: CbmLitEnums.h:23
CbmLitRK4TrackExtrapolator::RK4Order
void RK4Order(const std::vector< litfloat > &xIn, litfloat zIn, std::vector< litfloat > &xOut, litfloat zOut, std::vector< litfloat > &derivs) const
Definition: CbmLitRK4TrackExtrapolator.cxx:51
CbmLitRK4TrackExtrapolator::Extrapolate
virtual LitStatus Extrapolate(const CbmLitTrackParam *parIn, CbmLitTrackParam *parOut, litfloat zOut, std::vector< litfloat > *F)
Track parameters extrapolation with calculation of transport matrix.
Definition: CbmLitRK4TrackExtrapolator.cxx:19
CbmLitTrackParam::fSpeedOfLight
static litfloat fSpeedOfLight
Definition: CbmLitTrackParam.h:31