CbmRoot
CbmKFMath.cxx
Go to the documentation of this file.
1 #include "CbmKFMath.h"
2 
3 #include "FairField.h"
4 #include "FairTrackParam.h"
5 
6 #include <cmath>
7 
8 using std::exp;
9 using std::fabs;
10 using std::log;
11 using std::sqrt;
12 
13 ClassImp(CbmKFMath)
14 
15  Bool_t CbmKFMath::intersectCone(Double_t zCone,
16  Double_t ZCone,
17  Double_t rCone,
18  Double_t RCone,
19  const Double_t x[],
20  Double_t* z1,
21  Double_t* z2) {
22  Double_t t = (RCone - rCone) / (ZCone - zCone);
23  Double_t A = (RCone * zCone - rCone * ZCone) / (ZCone - zCone);
24  Double_t x0 = x[0] - x[5] * x[2];
25  Double_t y0 = x[1] - x[5] * x[3];
26  Double_t tx = x[2];
27  Double_t ty = x[3];
28  Double_t a = tx * tx + ty * ty - t * t;
29  Double_t b = 2 * (tx * x0 + ty * y0 + t * A);
30  Double_t c = x0 * x0 + y0 * y0 - A * A;
31 
32  if (fabs(a) < 1.E-4) return 1;
33  Double_t D = b * b - 4 * a * c;
34  if (D < 0.) return 1;
35  D = sqrt(D);
36  *z1 = (-b + D) / (2 * a);
37  *z2 = (-b - D) / (2 * a);
38  return 0;
39 }
40 
41 
42 void CbmKFMath::multQSQt(Int_t N,
43  const Double_t Q[],
44  const Double_t S[],
45  Double_t S_out[]) {
46  Double_t A[N * N];
47 
48  {
49  for (Int_t i = 0, n = 0; i < N; i++) {
50  for (Int_t j = 0; j < N; j++, ++n) {
51  A[n] = 0;
52  for (Int_t k = 0; k < N; ++k)
53  A[n] += S[indexS(i, k)] * Q[j * N + k];
54  }
55  }
56  }
57 
58  {
59  for (Int_t i = 0; i < N; i++) {
60  for (Int_t j = 0; j <= i; j++) {
61  Int_t n = indexS(i, j);
62  S_out[n] = 0;
63  for (Int_t k = 0; k < N; k++)
64  S_out[n] += Q[i * N + k] * A[k * N + j];
65  }
66  }
67  }
68 }
69 
70 void CbmKFMath::multQtSQ(Int_t N,
71  const Double_t Q[],
72  const Double_t S[],
73  Double_t S_out[]) {
74  Double_t A[N * N];
75 
76  for (Int_t i = 0, n = 0; i < N; i++) {
77  for (Int_t j = 0; j < N; j++, ++n) {
78  A[n] = 0;
79  for (Int_t k = 0; k < N; ++k)
80  A[n] += S[indexS(i, k)] * Q[k * N + j];
81  }
82  }
83 
84  for (Int_t i = 0; i < N; i++) {
85  for (Int_t j = 0; j <= i; j++) {
86  Int_t n = indexS(i, j);
87  S_out[n] = 0;
88  for (Int_t k = 0; k < N; k++)
89  S_out[n] += Q[k * N + i] * A[k * N + j];
90  }
91  }
92 }
93 
94 void CbmKFMath::multSSQ(const Double_t* A,
95  const Double_t* B,
96  Double_t* C,
97  Int_t n) {
98  for (Int_t i = 0; i < n; ++i) {
99  for (Int_t j = 0; j < n; ++j) {
100  Int_t ind = i * n + j;
101  C[ind] = 0;
102  for (Int_t k = 0; k < n; ++k)
103  C[ind] += A[indexS(i, k)] * B[indexS(k, j)];
104  }
105  }
106 }
107 
108 void CbmKFMath::four_dim_inv(Double_t a[4][4]) {
109  /**** Gaussian algorithm for 4x4 matrix inversion ****/
110  Int_t i, j, k, l;
111  Double_t factor;
112  Double_t temp[4];
113  Double_t b[4][4];
114 
115  // Set b to I
116  for (i = 0; i < 4; i++)
117  for (j = 0; j < 4; j++)
118  if (i == j)
119  b[i][j] = 1.0;
120  else
121  b[i][j] = 0.0;
122 
123  for (i = 0; i < 4; i++) {
124  for (j = i + 1; j < 4; j++)
125  if (fabs(a[i][i]) < fabs(a[j][i])) {
126  for (l = 0; l < 4; l++)
127  temp[l] = a[i][l];
128  for (l = 0; l < 4; l++)
129  a[i][l] = a[j][l];
130  for (l = 0; l < 4; l++)
131  a[j][l] = temp[l];
132  for (l = 0; l < 4; l++)
133  temp[l] = b[i][l];
134  for (l = 0; l < 4; l++)
135  b[i][l] = b[j][l];
136  for (l = 0; l < 4; l++)
137  b[j][l] = temp[l];
138  }
139  factor = a[i][i];
140  for (j = 4 - 1; j > -1; j--) {
141  b[i][j] /= factor;
142  a[i][j] /= factor;
143  }
144  for (j = i + 1; j < 4; j++) {
145  factor = -a[j][i];
146  for (k = 0; k < 4; k++) {
147  a[j][k] += a[i][k] * factor;
148  b[j][k] += b[i][k] * factor;
149  }
150  }
151  }
152  for (i = 3; i > 0; i--) {
153  for (j = i - 1; j > -1; j--) {
154  factor = -a[j][i];
155  for (k = 0; k < 4; k++) {
156  a[j][k] += a[i][k] * factor;
157  b[j][k] += b[i][k] * factor;
158  }
159  }
160  }
161 
162  // copy b to a and return
163  for (i = 0; i < 4; i++)
164  for (j = 0; j < 4; j++)
165  a[i][j] = b[i][j];
166 }
167 
168 void CbmKFMath::five_dim_inv(Double_t a[5][5]) {
169  /**** Gaussian algorithm for 5x5 matrix inversion ****/
170  Int_t i, j, k, l;
171  Double_t factor;
172  Double_t temp[5];
173  Double_t b[5][5];
174 
175  // Set b to I
176  for (i = 0; i < 5; i++) {
177  for (j = 0; j < 5; j++) {
178  if (i == j)
179  b[i][j] = 1.0;
180  else
181  b[i][j] = 0.0;
182  }
183  }
184 
185 
186  for (i = 0; i < 5; i++) {
187  for (j = i + 1; j < 5; j++)
188  if (fabs(a[i][i]) < fabs(a[j][i])) {
189  for (l = 0; l < 5; l++)
190  temp[l] = a[i][l];
191  for (l = 0; l < 5; l++)
192  a[i][l] = a[j][l];
193  for (l = 0; l < 5; l++)
194  a[j][l] = temp[l];
195  for (l = 0; l < 5; l++)
196  temp[l] = b[i][l];
197  for (l = 0; l < 5; l++)
198  b[i][l] = b[j][l];
199  for (l = 0; l < 5; l++)
200  b[j][l] = temp[l];
201  }
202  factor = a[i][i];
203  // cout<<"Highest element "<<a[i][i]<<endl;
204  for (j = 5 - 1; j > -1; j--) {
205  b[i][j] /= factor;
206  a[i][j] /= factor;
207  }
208  for (j = i + 1; j < 5; j++) {
209  factor = -a[j][i];
210  for (k = 0; k < 5; k++) {
211  a[j][k] += a[i][k] * factor;
212  b[j][k] += b[i][k] * factor;
213  }
214  }
215  }
216  for (i = 4; i > 0; i--) {
217  for (j = i - 1; j > -1; j--) {
218  factor = -a[j][i];
219  for (k = 0; k < 5; k++) {
220  a[j][k] += a[i][k] * factor;
221  b[j][k] += b[i][k] * factor;
222  }
223  }
224  }
225 
226  // copy b to a and return
227  for (i = 0; i < 5; i++)
228  for (j = 0; j < 5; j++)
229  a[i][j] = b[i][j];
230 }
231 
232 Bool_t CbmKFMath::invS(Double_t A[], Int_t N) {
233 
234  Bool_t ret = 0;
235 
236  const Double_t ZERO = 1.E-20;
237 
238  // input: simmetric > 0 NxN matrix A = {a11,a21,a22,a31..a33,..}
239  // output: inverse A, in case of problems fill zero and return 1
240 
241  // A->low triangular Anew : A = Anew x Anew^T
242  // method:
243  // for(j=1,N) for(i=j,N) Aij=(Aii-sum_{k=1}^{j-1}Aik*Ajk )/Ajj
244  //
245 
246  {
247  Double_t *j1 = A, *jj = A;
248  for (Int_t j = 1; j <= N; j1 += j++, jj += j) {
249  Double_t *ik = j1, x = 0;
250  while (ik != jj) {
251  x -= (*ik) * (*ik);
252  ik++;
253  }
254  x += *ik;
255  if (x > ZERO) {
256  x = sqrt(x);
257  *ik = x;
258  ik++;
259  x = 1 / x;
260  for (Int_t step = 1; step <= N - j; ik += ++step) { // ik==Ai1
261  Double_t sum = 0;
262  for (Double_t* jk = j1; jk != jj; sum += *(jk++) * *(ik++))
263  ;
264  *ik = (*ik - sum) * x; // ik == Aij
265  }
266  } else {
267  Double_t* ji = jj;
268  for (Int_t i = j; i < N; i++)
269  *(ji += i) = 0.;
270  ret = 1;
271  }
272  }
273  }
274 
275  // A -> Ainv
276  // method :
277  // for(i=1,N){
278  // Aii = 1/Aii;
279  // for(j=1,i-1) Aij=-(sum_{k=j}^{i-1} Aik * Akj) / Aii ;
280  // }
281 
282  {
283  Double_t *ii = A, *ij = A;
284  for (Int_t i = 1; i <= N; ij = ii + 1, ii += ++i) {
285  if (*ii > ZERO) {
286  Double_t x = -(*ii = 1. / *ii);
287  {
288  Double_t* jj = A;
289  for (Int_t j = 1; j < i; jj += ++j, ij++) {
290  Double_t *ik = ij, *kj = jj, sum = 0.;
291  for (Int_t k = j; ik != ii; kj += k++, ik++) {
292  sum += *ik * *kj;
293  }
294  *kj = sum * x;
295  }
296  }
297  } else {
298  for (Double_t* ik = ij; ik != ii + 1; ik++) {
299  *ik = 0.;
300  }
301  ret = 1;
302  }
303  }
304  }
305 
306  // A -> A^T x A
307  // method:
308  // Aij = sum_{k=i}^N Aki * Akj
309 
310  {
311  Double_t *ii = A, *ij = A;
312  for (Int_t i = 1; i <= N; ii += ++i) {
313  do {
314  Double_t *ki = ii, *kj = ij, sum = 0.;
315  for (Int_t k = i; k <= N; ki += k, kj += k++)
316  sum += (*ki) * (*kj);
317  *ij = sum;
318  } while ((ij++) != ii);
319  }
320  }
321  return ret;
322 }
323 
324 Double_t CbmKFMath::getDeviation(Double_t x,
325  Double_t y,
326  Double_t C[],
327  Double_t vx,
328  Double_t vy,
329  Double_t Cv[]) {
330  Double_t dx = x - vx;
331  Double_t dy = y - vy;
332  Double_t c[3] = {0, 0, 0};
333  if (C) {
334  c[0] = C[0];
335  c[1] = C[1];
336  c[2] = C[2];
337  }
338  if (Cv) {
339  c[0] += Cv[0];
340  c[1] += Cv[1];
341  c[2] += Cv[2];
342  }
343  Double_t d = c[0] * c[2] - c[1] * c[1];
344  if (fabs(d) < 1.e-20) return 0;
345  return sqrt(
346  fabs(0.5 * (dx * dx * c[0] - 2 * dx * dy * c[1] + dy * dy * c[2]) / d));
347 }
348 
349 Double_t
350 CbmKFMath::AnalyticQP(const Double_t T[], // track parameters (x,y,tx,ty,Q/p,z)
351  const Double_t V[], // vertex parameters (x,y,z)
352  FairField* MagneticField // magnetic field
353 ) {
354 
355  const Double_t c_light = 0.000299792458;
356 
357  Double_t x = T[0], y = T[1], tx = T[2], ty = T[3], qp0 = T[4], z = T[5],
358 
359  vx = V[0],
360  //vy = V[1],
361  vz = V[2],
362 
363  txtx = tx * tx, tyty = ty * ty, txty = tx * ty;
364 
365  Double_t Ax = txty, Ay = -txtx - 1, Az = ty, Ayy = tx * (txtx * 3 + 3),
366  Ayz = -2 * txty, Azy = Ayz,
367  Ayyy = -(15 * txtx * txtx + 18 * txtx + 3),
368 
369  Bx = tyty + 1, By = -txty, Bz = -tx, Byy = ty * (txtx * 3 + 1),
370  Byz = 2 * txtx + 1, Bzy = txtx - tyty,
371  Byyy = -txty * (txtx * 15 + 9);
372 
373  Double_t t = c_light * sqrt(1. + txtx + tyty), h = t * qp0;
374 
375  // integrals
376 
377  Double_t Sx = 0, Sy = 0, Sz = 0, Syy = 0, Syz = 0, Szy = 0, Syyy = 0;
378 
379  {
380  Double_t sx = 0, sy = 0, sz = 0, syy = 0, syz = 0, szy = 0, syyy = 0;
381 
382  Double_t r[3] = {x, y, z};
383  Double_t B[3][3];
384 
385  Int_t n = int(fabs(vz - z) / 5.);
386  if (n < 2) n = 2;
387  Double_t dz = (vz - z) / n;
388 
389  {
390  MagneticField->GetFieldValue(r, B[0]);
391  r[0] += tx * dz;
392  r[1] += ty * dz;
393  r[2] += dz;
394  MagneticField->GetFieldValue(r, B[1]);
395  r[0] += tx * dz;
396  r[1] += ty * dz;
397  r[2] += dz;
398  MagneticField->GetFieldValue(r, B[2]);
399 
400  sx = (B[0][0] + 4 * B[1][0] + B[2][0]) * dz * 2 / 6.;
401  sy = (B[0][1] + 4 * B[1][1] + B[2][1]) * dz * 2 / 6.;
402  sz = (B[0][2] + 4 * B[1][2] + B[2][2]) * dz * 2 / 6.;
403 
404  Sx = (B[0][0] + 2 * B[1][0]) * dz * dz * 4 / 6.;
405  Sy = (B[0][1] + 2 * B[1][1]) * dz * dz * 4 / 6.;
406  Sz = (B[0][2] + 2 * B[1][2]) * dz * dz * 4 / 6.;
407 
408  Double_t c2[3][3] = {{5, -4, -1}, {44, 80, -4}, {11, 44, 5}}; // /=360.
409  Double_t C2[3][3] = {
410  {38, 8, -4}, {148, 208, -20}, {3, 36, 3}}; // /=2520.
411  for (Int_t k = 0; k < 3; k++)
412  for (Int_t m = 0; m < 3; m++) {
413  syz += c2[k][m] * B[k][1] * B[m][2];
414  Syz += C2[k][m] * B[k][1] * B[m][2];
415  szy += c2[k][m] * B[k][2] * B[m][1];
416  Szy += C2[k][m] * B[k][2] * B[m][1];
417  }
418 
419  syz *= dz * dz * 4 / 360.;
420  Syz *= dz * dz * dz * 8 / 2520.;
421 
422  szy *= dz * dz * 4 / 360.;
423  Szy *= dz * dz * dz * 8 / 2520.;
424 
425  syy = (B[0][1] + 4 * B[1][1] + B[2][1]) * dz * 2;
426  syyy = syy * syy * syy / 1296;
427  syy = syy * syy / 72;
428 
429  Syy =
430  (B[0][1] * (38 * B[0][1] + 156 * B[1][1] - B[2][1])
431  + B[1][1] * (208 * B[1][1] + 16 * B[2][1]) + B[2][1] * (3 * B[2][1]))
432  * dz * dz * dz * 8 / 2520.;
433  Syyy = (B[0][1]
434  * (B[0][1] * (85 * B[0][1] + 526 * B[1][1] - 7 * B[2][1])
435  + B[1][1] * (1376 * B[1][1] + 84 * B[2][1])
436  + B[2][1] * (19 * B[2][1]))
437  + B[1][1]
438  * (B[1][1] * (1376 * B[1][1] + 256 * B[2][1])
439  + B[2][1] * (62 * B[2][1]))
440  + B[2][1] * B[2][1] * (3 * B[2][1]))
441  * dz * dz * dz * dz * 16 / 90720.;
442 
443  x += tx * 2 * dz + h * (Sx * Ax + Sy * Ay + Sz * Az)
444  + h * h * (Syy * Ayy + Syz * Ayz + Szy * Azy)
445  + h * h * h * Syyy * Ayyy;
446  y += ty * 2 * dz + h * (Sx * Bx + Sy * By + Sz * Bz)
447  + h * h * (Syy * Byy + Syz * Byz + Szy * Bzy)
448  + h * h * h * Syyy * Byyy;
449  tx += h * (sx * Ax + sy * Ay + sz * Az)
450  + h * h * (syy * Ayy + syz * Ayz + szy * Azy)
451  + h * h * h * syyy * Ayyy;
452  ty += h * (sx * Bx + sy * By + sz * Bz)
453  + h * h * (syy * Byy + syz * Byz + szy * Bzy)
454  + h * h * h * syyy * Byyy;
455  z += 2 * dz;
456  txtx = tx * tx;
457  tyty = ty * ty;
458  txty = tx * ty;
459 
460  Ax = txty;
461  Ay = -txtx - 1;
462  Az = ty;
463  Ayy = tx * (txtx * 3 + 3);
464  Ayz = -2 * txty;
465  Azy = Ayz;
466  Ayyy = -(15 * txtx * txtx + 18 * txtx + 3);
467 
468  Bx = tyty + 1;
469  By = -txty;
470  Bz = -tx;
471  Byy = ty * (txtx * 3 + 1);
472  Byz = 2 * txtx + 1;
473  Bzy = txtx - tyty;
474  Byyy = -txty * (txtx * 15 + 9);
475 
476  t = c_light * sqrt(1. + txtx + tyty);
477  h = t * qp0;
478  }
479 
480  for (Int_t i = 2; i < n; i++) {
481 
482  Double_t B0[3] = {B[0][0], B[0][1], B[0][2]};
483 
484  B[0][0] = B[1][0];
485  B[0][1] = B[1][1];
486  B[0][2] = B[1][2];
487 
488  B[1][0] = B[2][0];
489  B[1][1] = B[2][1];
490  B[1][2] = B[2][2];
491 
492  // estimate B[2] at +dz
493 
494  B[2][0] = B0[0] - 3 * B[0][0] + 3 * B[1][0];
495  B[2][1] = B0[1] - 3 * B[0][1] + 3 * B[1][1];
496  B[2][2] = B0[2] - 3 * B[0][2] + 3 * B[1][2];
497 
498  Double_t Sx_, Sy_, Sz_, Syy_, Syz_, Szy_, Syyy_;
499 
500  Sx_ = (-B[0][0] + 10 * B[1][0] + 3 * B[2][0]) * dz * dz * 4 / 96.;
501  Sy_ = (-B[0][1] + 10 * B[1][1] + 3 * B[2][1]) * dz * dz * 4 / 96.;
502  Sz_ = (-B[0][2] + 10 * B[1][2] + 3 * B[2][2]) * dz * dz * 4 / 96.;
503 
504  Syz_ = Szy_ = 0;
505 
506  Double_t c2[3][3] = {
507  {5, -52, -13}, {-28, 320, 68}, {-37, 332, 125}}; // /=5760
508  Double_t C2[3][3] = {
509  {13, -152, -29}, {-82, 1088, 170}, {-57, 576, 153}}; // /=80640
510  {
511  for (Int_t k = 0; k < 3; k++)
512  for (Int_t m = 0; m < 3; m++) {
513  Syz_ += C2[k][m] * B[k][1] * B[m][2];
514  Szy_ += C2[k][m] * B[k][2] * B[m][1];
515  }
516  }
517 
518  Syz_ *= dz * dz * dz * 8 / 80640.;
519  Szy_ *= dz * dz * dz * 8 / 80640.;
520 
521  Syy_ = (B[0][1] * (13 * B[0][1] - 234 * B[1][1] - 86 * B[2][1])
522  + B[1][1] * (1088 * B[1][1] + 746 * B[2][1])
523  + B[2][1] * (153 * B[2][1]))
524  * dz * dz * dz * 8 / 80640.;
525 
526  Syyy_ = (B[0][1]
527  * (B[0][1] * (-43 * B[0][1] + 1118 * B[1][1] + 451 * B[2][1])
528  + B[1][1] * (-9824 * B[1][1] - 7644 * B[2][1])
529  + B[2][1] * (-1669 * B[2][1]))
530  + B[1][1]
531  * (B[1][1] * (29344 * B[1][1] + 32672 * B[2][1])
532  + B[2][1] * (13918 * B[2][1]))
533  + B[2][1] * B[2][1] * (2157 * B[2][1]))
534  * dz * dz * dz * dz * 16 / 23224320.;
535 
536  r[2] += dz;
537  r[0] = x + tx * dz + h * (Sx_ * Ax + Sy_ * Ay + Sz_ * Az)
538  + h * h * (Syy_ * Ayy + Syz_ * Ayz + Szy_ * Azy)
539  + h * h * h * Syyy_ * Ayyy;
540  r[1] = y + ty * dz + h * (Sx_ * Bx + Sy_ * By + Sz_ * Bz)
541  + h * h * (Syy_ * Byy + Syz_ * Byz + Szy_ * Bzy)
542  + h * h * h * Syyy_ * Byyy;
543  /*
544  Syyy_+= dz*syyy+Sy_*syy+Syy_*sy+Syyy;
545  Syy_ += dz*syy + sy*Sy_ + Syy;
546  Syz_ += dz*syz + sz*Sy_ + Syz;
547  Szy_ += dz*szy + sy*Sz_ + Szy;
548  Sx_ += dz*sx + Sx;
549  Sy_ += dz*sy + Sy;
550  Sz_ += dz*sz + Sz;
551 
552  r[2] += dz;
553  r[0] =
554  x + tx*(r[2]-z)
555  + h*( Sx_*Ax + Sy_*Ay + Sz_*Az )
556  + h*h*( Syy_*Ayy + Syz_*Ayz + Szy_*Azy )
557  + h*h*h*Syyy_*Ayyy;
558  r[1] = y + ty*(r[2]-z)
559  + h*( Sx_*Bx + Sy_*By + Sz_*Bz )
560  + h*h*( Syy_*Byy + Syz_*Byz + Szy_*Bzy )
561  + h*h*h*Syyy_*Byyy;
562  */
563  MagneticField->GetFieldValue(r, B[2]);
564 
565  Double_t sx_, sy_, sz_, syy_, syz_, szy_, syyy_;
566 
567  sx_ = (-B[0][0] + 8 * B[1][0] + 5 * B[2][0]) * dz * 2 / 24.;
568  sy_ = (-B[0][1] + 8 * B[1][1] + 5 * B[2][1]) * dz * 2 / 24.;
569  sz_ = (-B[0][2] + 8 * B[1][2] + 5 * B[2][2]) * dz * 2 / 24.;
570 
571  Sx_ = (-B[0][0] + 10 * B[1][0] + 3 * B[2][0]) * dz * dz * 4 / 96.;
572  Sy_ = (-B[0][1] + 10 * B[1][1] + 3 * B[2][1]) * dz * dz * 4 / 96.;
573  Sz_ = (-B[0][2] + 10 * B[1][2] + 3 * B[2][2]) * dz * dz * 4 / 96.;
574 
575  syz_ = Syz_ = szy_ = Szy_ = 0;
576 
577  {
578  for (Int_t k = 0; k < 3; k++)
579  for (Int_t m = 0; m < 3; m++) {
580  syz_ += c2[k][m] * B[k][1] * B[m][2];
581  Syz_ += C2[k][m] * B[k][1] * B[m][2];
582  szy_ += c2[k][m] * B[k][2] * B[m][1];
583  Szy_ += C2[k][m] * B[k][2] * B[m][1];
584  }
585  }
586  syz_ *= dz * dz * 4 / 5760.;
587  Syz_ *= dz * dz * dz * 8 / 80640.;
588 
589  szy_ *= dz * dz * 4 / 5760.;
590  Szy_ *= dz * dz * dz * 8 / 80640.;
591 
592  syy_ = (B[0][1] - 8 * B[1][1] - 5 * B[2][1]) * dz * 2;
593  syyy_ = -syy_ * syy_ * syy_ / 82944;
594  syy_ = syy_ * syy_ / 1152;
595 
596  Syy_ = (B[0][1] * (13 * B[0][1] - 234 * B[1][1] - 86 * B[2][1])
597  + B[1][1] * (1088 * B[1][1] + 746 * B[2][1])
598  + B[2][1] * (153 * B[2][1]))
599  * dz * dz * dz * 8 / 80640.;
600 
601  Syyy_ = (B[0][1]
602  * (B[0][1] * (-43 * B[0][1] + 1118 * B[1][1] + 451 * B[2][1])
603  + B[1][1] * (-9824 * B[1][1] - 7644 * B[2][1])
604  + B[2][1] * (-1669 * B[2][1]))
605  + B[1][1]
606  * (B[1][1] * (29344 * B[1][1] + 32672 * B[2][1])
607  + B[2][1] * (13918 * B[2][1]))
608  + B[2][1] * B[2][1] * (2157 * B[2][1]))
609  * dz * dz * dz * dz * 16 / 23224320.;
610 
611  x += tx * dz + h * (Sx_ * Ax + Sy_ * Ay + Sz_ * Az)
612  + h * h * (Syy_ * Ayy + Syz_ * Ayz + Szy_ * Azy)
613  + h * h * h * Syyy_ * Ayyy;
614  y += ty * dz + h * (Sx_ * Bx + Sy_ * By + Sz_ * Bz)
615  + h * h * (Syy_ * Byy + Syz_ * Byz + Szy_ * Bzy)
616  + h * h * h * Syyy_ * Byyy;
617  tx += h * (sx_ * Ax + sy_ * Ay + sz_ * Az)
618  + h * h * (syy_ * Ayy + syz_ * Ayz + szy_ * Azy)
619  + h * h * h * syyy_ * Ayyy;
620  ty += h * (sx_ * Bx + sy_ * By + sz_ * Bz)
621  + h * h * (syy_ * Byy + syz_ * Byz + szy_ * Bzy)
622  + h * h * h * syyy_ * Byyy;
623  z += dz;
624  txtx = tx * tx;
625  tyty = ty * ty;
626  txty = tx * ty;
627 
628  Ax = txty;
629  Ay = -txtx - 1;
630  Az = ty;
631  Ayy = tx * (txtx * 3 + 3);
632  Ayz = -2 * txty;
633  Azy = Ayz;
634  Ayyy = -(15 * txtx * txtx + 18 * txtx + 3);
635 
636  Bx = tyty + 1;
637  By = -txty;
638  Bz = -tx;
639  Byy = ty * (txtx * 3 + 1);
640  Byz = 2 * txtx + 1;
641  Bzy = txtx - tyty;
642  Byyy = -txty * (txtx * 15 + 9);
643 
644  t = c_light * sqrt(1. + txtx + tyty);
645  h = t * qp0;
646 
647 
648  Syyy += dz * syyy + Sy_ * syy + Syy_ * sy + Syyy_;
649  Syy += dz * syy + sy * Sy_ + Syy_;
650  Syz += dz * syz + sz * Sy_ + Syz_;
651  Szy += dz * szy + sy * Sz_ + Szy_;
652  Sx += dz * sx + Sx_;
653  Sy += dz * sy + Sy_;
654  Sz += dz * sz + Sz_;
655 
656  syyy += sy_ * syy + syy_ * sy + syyy_;
657  syy += sy * sy_ + syy_;
658  syz += sz * sy_ + syz_;
659  szy += sz * sz_ + szy_;
660  sx += sx_;
661  sy += sy_;
662  sz += sz_;
663  }
664  //cout<<x-vx<<" "<<y-vy<<" "<<z-vz<<" "<<1./qp0<<endl;
665  }
666 
667  x = T[0];
668  y = T[1];
669  tx = T[2];
670  ty = T[3];
671  z = T[5];
672  txtx = tx * tx;
673  tyty = ty * ty;
674  txty = tx * ty;
675 
676  Ax = txty;
677  Ay = -txtx - 1;
678  Az = ty;
679  Ayy = tx * (txtx * 3 + 3);
680  Ayz = -2 * txty;
681  Azy = Ayz;
682  Ayyy = -(15 * txtx * txtx + 18 * txtx + 3);
683 
684  Bx = tyty + 1;
685  By = -txty;
686  Bz = -tx;
687  Byy = ty * (txtx * 3 + 1);
688  Byz = 2 * txtx + 1;
689  Bzy = txtx - tyty;
690  Byyy = -txty * (txtx * 15 + 9);
691 
692  t = c_light * sqrt(1. + txtx + tyty);
693  h = t * qp0;
694 
695  Double_t
696 
697  c = (x - vx) + tx * (vz - z),
698  b = t * (Sx * Ax + Sy * Ay + Sz * Az),
699  a = t * t * (Syy * Ayy + Syz * Ayz + Szy * Azy),
700  d = t * t * t * (Syyy * Ayyy);
701 
702  Double_t C = d * qp0 * qp0 * qp0 + a * qp0 * qp0 + b * qp0 + c,
703  B = 3 * d * qp0 * qp0 + 2 * a * qp0 + b, A = 3 * d + a;
704 
705  Double_t D = B * B - 4 * A * C;
706  if (D < 0) D = 0;
707  D = sqrt(D);
708 
709  Double_t dqp1 = (-B + D) / 2 / A;
710  Double_t dqp2 = (-B - D) / 2 / A;
711  Double_t dqp = (fabs(dqp1) < fabs(dqp2)) ? dqp1 : dqp2;
712 
713  return qp0 + dqp;
714 }
715 
716 
717 Bool_t CbmKFMath::GetThickness(Double_t z1,
718  Double_t z2,
719  Double_t mz,
720  Double_t mthick,
721  Double_t* mz_out,
722  Double_t* mthick_out) {
723  Double_t z, Z;
724 
725  if (z1 < z2) {
726  z = z1;
727  Z = z2;
728  } else {
729  Z = z1;
730  z = z2;
731  }
732 
733  Double_t tmin = mz - mthick / 2, tmax = mz + mthick / 2;
734  if (tmin >= Z || z >= tmax) return 1;
735  if (z <= tmin && tmax <= Z) // case z |**| Z
736  {
737  *mz_out = mz;
738  *mthick_out = mthick;
739  } else if (z <= tmin && tmin < Z && Z <= tmax) // case z |*Z*|
740  {
741  *mz_out = (tmin + Z) / 2;
742  *mthick_out = Z - tmin;
743  } else if (tmax <= Z && tmin <= z && z < tmax) // case |*z*| Z
744  {
745  *mz_out = (tmax + z) / 2;
746  *mthick_out = tmax - z;
747  } else if (tmin <= z && Z <= tmax) // case |*zZ*|
748  {
749  *mz_out = (Z + z) / 2;
750  *mthick_out = Z - z;
751  }
752  return 0;
753 }
754 
755 
756 Int_t CbmKFMath::GetNoise(Double_t Lrl,
757  Double_t F,
758  Double_t Fe,
759  Double_t tx,
760  Double_t ty,
761  Double_t qp,
762  Double_t mass,
763  Bool_t is_electron,
764  Bool_t downstream_direction,
765  Double_t* Q5,
766  Double_t* Q8,
767  Double_t* Q9,
768  Double_t* Ecor) {
769  *Q5 = 0;
770  *Q8 = 0;
771  *Q9 = 0;
772  *Ecor = 1.;
773  Double_t t = sqrt(1. + tx * tx + ty * ty);
774  if (!finite(t) || t > 1.e4) return 1;
775  t = sqrt(t);
776  Double_t l = t * Lrl;
777  if (l > 1.) l = 1.; // protect against l too big
778  Double_t s0 =
779  (l > exp(-1. / 0.038)) ? F * .0136 * (1 + 0.038 * log(l)) * qp : 0.;
780  Double_t a = (1. + mass * mass * qp * qp) * s0 * s0 * t * t * l;
781 
782  *Q5 = a * (1. + tx * tx);
783  *Q8 = a * tx * ty;
784  *Q9 = a * (1. + ty * ty);
785 
786  // Apply correction for dE/dx energy loss
787 
788  Double_t L = downstream_direction ? l : -l;
789 
790  if (0 && is_electron) // Bremsstrahlung effect for e+,e-
791  {
792  *Ecor = exp(L);
793  }
794 
795  if (1) // ionisation energy loss
796  {
797  Double_t m_energyLoss = Fe;
798  // Double_t m_energyLoss = 0.02145;//0.060;
799  Double_t corr = (1. - fabs(qp) * L * m_energyLoss);
800  if (corr > 0.001 * fabs(qp))
801  *Ecor *= 1. / corr;
802  else
803  *Ecor = 1000. / fabs(qp);
804  }
805  return 0;
806 }
807 
808 
809 void CbmKFMath::CopyTC2TrackParam(FairTrackParam* par,
810  Double_t T[],
811  Double_t C[]) {
812  if (T) {
813  par->SetX(T[0]);
814  par->SetY(T[1]);
815  par->SetZ(T[5]);
816  par->SetTx(T[2]);
817  par->SetTy(T[3]);
818  par->SetQp(T[4]);
819  }
820  if (C) {
821  for (Int_t i = 0, iCov = 0; i < 5; i++)
822  for (Int_t j = 0; j <= i; j++, iCov++)
823  par->SetCovariance(i, j, C[iCov]);
824  }
825 }
826 
827 void CbmKFMath::CopyTrackParam2TC(const FairTrackParam* par,
828  Double_t T[],
829  Double_t C[]) {
830  if (T) {
831  T[0] = par->GetX();
832  T[1] = par->GetY();
833  T[2] = par->GetTx();
834  T[3] = par->GetTy();
835  T[4] = par->GetQp();
836  T[5] = par->GetZ();
837  }
838  if (C) {
839  for (Int_t i = 0, iCov = 0; i < 5; i++)
840  for (Int_t j = 0; j <= i; j++, iCov++)
841  C[iCov] = par->GetCovariance(i, j);
842  }
843 }
exp
friend F32vec4 exp(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:134
CbmKFMath::intersectCone
static Bool_t intersectCone(Double_t zCone, Double_t ZCone, Double_t rCone, Double_t RCone, const Double_t x[], Double_t *z1, Double_t *z2)
CbmKFMath::multQtSQ
static void multQtSQ(Int_t N, const Double_t Q[], const Double_t S[], Double_t S_out[])
Definition: CbmKFMath.cxx:70
CbmKFMath::four_dim_inv
static void four_dim_inv(Double_t a[4][4])
Definition: CbmKFMath.cxx:108
sqrt
friend F32vec4 sqrt(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:41
CbmKFMath::indexS
static Int_t indexS(Int_t i, Int_t j)
Definition: CbmKFMath.h:39
CbmKFMath::multSSQ
static void multSSQ(const Double_t *A, const Double_t *B, Double_t *C, Int_t n)
Definition: CbmKFMath.cxx:94
CbmKFMath::CopyTrackParam2TC
static void CopyTrackParam2TC(const FairTrackParam *par, Double_t T[], Double_t C[])
Definition: CbmKFMath.cxx:827
i
int i
Definition: L1/vectors/P4_F32vec4.h:25
CbmKFMath.h
CbmKFMath::getDeviation
static Double_t getDeviation(Double_t x, Double_t y, Double_t C[], Double_t vx, Double_t vy, Double_t Cv[]=0)
Definition: CbmKFMath.cxx:324
finite
T finite(T x)
Definition: CbmL1Def.h:21
z2
Double_t z2[nSects2]
Definition: pipe_v16a_mvdsts100.h:11
CbmKFMath::multQSQt
static void multQSQt(Int_t N, const Double_t Q[], const Double_t S[], Double_t S_out[])
Definition: CbmKFMath.cxx:42
h
Data class with information on a STS local track.
CbmKFMath::invS
static Bool_t invS(Double_t A[], Int_t N)
Definition: CbmKFMath.cxx:232
d
double d
Definition: P4_F64vec2.h:24
log
friend F32vec4 log(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:135
CbmKFMath::five_dim_inv
static void five_dim_inv(Double_t a[5][5])
Definition: CbmKFMath.cxx:168
x
Double_t x
Definition: CbmMvdSensorDigiToHitTask.cxx:68
fabs
friend F32vec4 fabs(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:60
CbmKFMath
Definition: CbmKFMath.h:19
CbmKFMath::CopyTC2TrackParam
static void CopyTC2TrackParam(FairTrackParam *par, Double_t T[], Double_t C[])
Definition: CbmKFMath.cxx:809
m
__m128 m
Definition: L1/vectors/P4_F32vec4.h:26
y
Double_t y
Definition: CbmMvdSensorDigiToHitTask.cxx:68
CbmKFMath::GetNoise
static Int_t GetNoise(Double_t Lrl, Double_t F, Double_t Fe, Double_t tx, Double_t ty, Double_t qp, Double_t mass, Bool_t is_electron, Bool_t downstream_direction, Double_t *Q5, Double_t *Q8, Double_t *Q9, Double_t *Ecor)
Definition: CbmKFMath.cxx:756
z1
Double_t z1[nSects1]
Definition: pipe_v16a_mvdsts100.h:6
NS_L1TrackFitter::ZERO
const fvec ZERO
Definition: L1TrackFitter.cxx:32
CbmKFMath::GetThickness
static Bool_t GetThickness(Double_t z1, Double_t z2, Double_t mz, Double_t mthick, Double_t *mz_out, Double_t *mthick_out)
Definition: CbmKFMath.cxx:717
CbmKFMath::AnalyticQP
static Double_t AnalyticQP(const Double_t T[], const Double_t V[], FairField *MagneticField)
Definition: CbmKFMath.cxx:350
ClassImp
ClassImp(CbmKFMath) Bool_t CbmKFMath
Definition: CbmKFMath.cxx:13
NS_L1TrackFitter::c_light
const fvec c_light
Definition: L1TrackFitter.cxx:31