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