ATLAS Offline Software
RungeKuttaUtils.cxx
Go to the documentation of this file.
1 /*
2  Copyright (C) 2002-2022 CERN for the benefit of the ATLAS collaboration
3 */
4 
6 // RungeKuttaUtils.cxx , (c) ATLAS Detector software
7 // author Igor Gavrilenko
9 
10 #include "TrkExUtils/RungeKuttaUtils.h"
11 #include "TrkPatternParameters/PatternTrackParameters.h"
12 #include "TrkSurfaces/ConeSurface.h"
13 #include "TrkSurfaces/CylinderSurface.h"
14 #include "TrkSurfaces/DiscSurface.h"
15 #include "TrkSurfaces/DistanceSolution.h"
16 #include "TrkSurfaces/PerigeeSurface.h"
17 #include "TrkSurfaces/PlaneSurface.h"
18 #include "TrkSurfaces/StraightLineSurface.h"
19 
20 #include "CxxUtils/vec.h"
21 #include "CxxUtils/inline_hints.h"
22 
23 namespace {
24 /*
25  * Hide all internal implementation methods
26  * inside an anonymous namespace
27  */
28 
29 /*The notation of this package
30 for array P[42].
31  /dL0 /dL1 /dPhi /dThe /dCM
32 X ->P[0] dX / P[ 7] P[14] P[21] P[28] P[35]
33 Y ->P[1] dY / P[ 8] P[15] P[22] P[29] P[36]
34 Z ->P[2] dZ / P[ 9] P[16] P[23] P[30] P[37]
35 Ax ->P[3] dAx/ P[10] P[17] P[24] P[31] P[38]
36 Ay ->P[4] dAy/ P[11] P[18] P[25] P[32] P[39]
37 Az ->P[5] dAz/ P[12] P[19] P[26] P[33] P[40]
38 CM ->P[6] dCM/ P[13] P[20] P[27] P[34] P[41]
39 */
40 
41 inline void
42 globalToLocalVecHelper(double* ATH_RESTRICT P,
43  const double s0,
44  const double s1,
45  const double s2,
46  const double s3,
47  const double s4)
48 {
49  using namespace CxxUtils;
50  using vec2 = CxxUtils::vec<double, 2>;
51  /* The calculation (original form)
52  P[ 7]-=(s0*P[ 3]); P[ 8]-=(s0*P[ 4]); P[ 9]-=(s0*P[ 5]);
53  P[10]-=(s0*P[42]); P[11]-=(s0*P[43]); P[12]-=(s0*P[44]);
54  P[14]-=(s1*P[ 3]); P[15]-=(s1*P[ 4]); P[16]-=(s1*P[ 5]);
55  P[17]-=(s1*P[42]); P[18]-=(s1*P[43]); P[19]-=(s1*P[44]);
56  P[21]-=(s2*P[ 3]); P[22]-=(s2*P[ 4]); P[23]-=(s2*P[ 5]);
57  P[24]-=(s2*P[42]); P[25]-=(s2*P[43]); P[26]-=(s2*P[44]);
58  P[28]-=(s3*P[ 3]); P[29]-=(s3*P[ 4]); P[30]-=(s3*P[ 5]);
59  P[31]-=(s3*P[42]); P[32]-=(s3*P[43]); P[33]-=(s3*P[44]);
60  P[35]-=(s4*P[ 3]); P[36]-=(s4*P[ 4]); P[37]-=(s4*P[ 5]);
61  P[38]-=(s4*P[42]); P[39]-=(s4*P[43]); P[40]-=(s4*P[44]);
62  */
63  /*
64  * The naming convention we follow is
65  * A_B -->SIMD vector of
66  * of size 2 containing
67  * {A,B}
68  * For example :
69  * dZdTheta_dAxdTheta --> {dZ/dTheta, dAx/dTheta}
70  * --> {P[30],P[31]}
71  */
72  vec2 Pmult1 = { P[3], P[4] };
73  vec2 Pmult2 = { P[5], P[42] };
74  vec2 Pmult3 = { P[43], P[44] };
75 
76  vec2 dXdL0_dYdL0;
77  vload(dXdL0_dYdL0, &P[7]);
78  vec2 dZdL0_dAxdL0;
79  vload(dZdL0_dAxdL0, &P[9]);
80  vec2 dAydL0_dAzdL0;
81  vload(dAydL0_dAzdL0, &P[11]);
82  dXdL0_dYdL0 -= s0 * Pmult1;
83  dZdL0_dAxdL0 -= s0 * Pmult2;
84  dAydL0_dAzdL0 -= s0 * Pmult3;
85  vstore(&P[7], dXdL0_dYdL0);
86  vstore(&P[9], dZdL0_dAxdL0);
87  vstore(&P[11], dAydL0_dAzdL0);
88 
89  vec2 dXdL1_dYdL1;
90  vload(dXdL1_dYdL1, &P[14]);
91  vec2 dZdL1_dAxdL1;
92  vload(dZdL1_dAxdL1, &P[16]);
93  vec2 dAydL1_dAzdL1;
94  vload(dAydL1_dAzdL1, &P[18]);
95  dXdL1_dYdL1 -= s1 * Pmult1;
96  dZdL1_dAxdL1 -= s1 * Pmult2;
97  dAydL1_dAzdL1 -= s1 * Pmult3;
98  vstore(&P[14], dXdL1_dYdL1);
99  vstore(&P[16], dZdL1_dAxdL1);
100  vstore(&P[18], dAydL1_dAzdL1);
101 
102  vec2 dXdPhi_dYdPhi;
103  vload(dXdPhi_dYdPhi, &P[21]);
104  vec2 dZdPhi_dAxdPhi;
105  vload(dZdPhi_dAxdPhi, &P[23]);
106  vec2 dAydPhi_dAzdPhi;
107  vload(dAydPhi_dAzdPhi, &P[25]);
108  dXdPhi_dYdPhi -= s2 * Pmult1;
109  dZdPhi_dAxdPhi -= s2 * Pmult2;
110  dAydPhi_dAzdPhi -= s2 * Pmult3;
111  vstore(&P[21], dXdPhi_dYdPhi);
112  vstore(&P[23], dZdPhi_dAxdPhi);
113  vstore(&P[25], dAydPhi_dAzdPhi);
114 
115  vec2 dXdTheta_dYdTheta;
116  vload(dXdTheta_dYdTheta, &P[28]);
117  vec2 dZdTheta_dAxdTheta;
118  vload(dZdTheta_dAxdTheta, &P[30]);
119  vec2 dAydTheta_dAzdTheta;
120  vload(dAydTheta_dAzdTheta, &P[32]);
121  dXdTheta_dYdTheta -= s3 * Pmult1;
122  dZdTheta_dAxdTheta -= s3 * Pmult2;
123  dAydTheta_dAzdTheta -= s3 * Pmult3;
124  vstore(&P[28], dXdTheta_dYdTheta);
125  vstore(&P[30], dZdTheta_dAxdTheta);
126  vstore(&P[32], dAydTheta_dAzdTheta);
127 
128  vec2 dXdCM_dYdCM;
129  vload(dXdCM_dYdCM, &P[35]);
130  vec2 dZdCM_dAxdCM;
131  vload(dZdCM_dAxdCM, &P[37]);
132  vec2 AydCM_dAzdCM;
133  vload(AydCM_dAzdCM, &P[39]);
134  dXdCM_dYdCM -= s4 * Pmult1;
135  dZdCM_dAxdCM -= s4 * Pmult2;
136  AydCM_dAzdCM -= s4 * Pmult3;
137  vstore(&P[35], dXdCM_dYdCM);
138  vstore(&P[37], dZdCM_dAxdCM);
139  vstore(&P[39], AydCM_dAzdCM);
140 }
141 
142 /* Helper to replace repeated calculation of
143  * 5x1 = 5x3 * 3X1
144  * for the Jacobian
145  *
146  * E.g a calculation like :
147  * Jac[ 0] = Ax[0]*P[ 7]+Ax[1]*P[ 8]+Ax[2]*P[ 9]; // dL0/dL0
148  * Jac[ 1] = Ax[0]*P[14]+Ax[1]*P[15]+Ax[2]*P[16]; // dL0/dL1
149  * Jac[ 2] = Ax[0]*P[21]+Ax[1]*P[22]+Ax[2]*P[23]; // dL0/dPhi
150  * Jac[ 3] = Ax[0]*P[28]+Ax[1]*P[29]+Ax[2]*P[30]; // dL0/dThe
151  * Jac[ 4] = Ax[0]*P[35]+Ax[1]*P[36]+Ax[2]*P[37]; // dL0/dCM
152  * Jac[ 5] = Ay[0]*P[ 7]+Ay[1]*P[ 8]+Ay[2]*P[ 9]; // dL1/dL0
153  * Jac[ 6] = Ay[0]*P[14]+Ay[1]*P[15]+Ay[2]*P[16]; // dL1/dL1
154  * Jac[ 7] = Ay[0]*P[21]+Ay[1]*P[22]+Ay[2]*P[23]; // dL1/dPhi
155  * Jac[ 8] = Ay[0]*P[28]+Ay[1]*P[29]+Ay[2]*P[30]; // dL1/dThe
156  * Jac[ 9] = Ay[0]*P[35]+Ay[1]*P[36]+Ay[2]*P[37]; // dL1/dCM
157  * is replaces with
158  * mult3x5Helper(&Jac[0],Ax,&P[7]);
159  * mult3x5Helper(&Jac[5],Ay,&P[7]);
160  */
161 inline void
162 mult3x5Helper(double* ATH_RESTRICT Jac,
163  const double* ATH_RESTRICT V,
164  const double* ATH_RESTRICT P)
165 {
166  /*
167  * |Jac[0] |= |V[0]| * |P[0]| + |V[1]| * |P[1] | + |V[2]| * |P[2] |
168  * |Jac[1] |= |V[0]| * |P[7]| + |V[1]| * |P[8] | + |V[2]| * |P[9] |
169  * |Jac[2] |= |V[0]| * |P[14]| + |V[1]| * |P[15]| + |V[2]| * |P[16]|
170  * |Jac[3] |= |V[0]| * |P[21]| + |V[1]| * |P[22]| + |V[2]| * |P[23]|
171  *
172  * Jac[4] = V[0] * P[28] + V[1] * P[29] + V[2] * P[30];
173  *
174  * The first 4 we can do in vertical SIMD fashion
175  * {Jac[0] | Jac[1] Jac[2] | Jac[3] } =
176  * V[0] * {P[0] | P[7] | P[14] | P[21]} +
177  * V[1] * {P[1] | P[8] | P[15] | P[22]} +
178  * V[2] * {P[2] | P[9] | P[16] | P[23]}
179  * Where {} is a SIMD size 4 vector
180  *
181  * The remaining odd (5th) element is done at the end
182  * Jac[4] = V[0] * P[28] + V[1] * P[29] + V[2] * P[30];
183  */
184 
185  using vec4 = CxxUtils::vec<double, 4>;
186  // 1st 4 elements
187  vec4 P0 = { P[0], P[7], P[14], P[21] };
188  vec4 res = V[0] * P0;
189 
190  vec4 P1 = { P[1], P[8], P[15], P[22] };
191  res += V[1] * P1;
192 
193  vec4 P2 = { P[2], P[9], P[16], P[23] };
194  res += V[2] * P2;
195 
196  CxxUtils::vstore(&Jac[0], res);
197 
198  // The 5th element
199  Jac[4] = V[0] * P[28] + V[1] * P[29] + V[2] * P[30];
200 }
201 
202 void
203 transformGlobalToPlane(const Amg::Transform3D& T,
204  bool useJac,
205  double* ATH_RESTRICT P,
206  double* ATH_RESTRICT par,
207  double* ATH_RESTRICT Jac)
208 {
209  const double* Ax = T.matrix().col(0).data(); // Oth column
210  const double* Ay = T.matrix().col(1).data(); // 1st column
211 
212  const double d[3] = { P[0] - T(0, 3), P[1] - T(1, 3), P[2] - T(2, 3) };
213 
214  par[0] = d[0] * Ax[0] + d[1] * Ax[1] + d[2] * Ax[2];
215  par[1] = d[0] * Ay[0] + d[1] * Ay[1] + d[2] * Ay[2];
216 
217  if (!useJac)
218  return;
219 
220  // Condition trajectory on surface
221  //
222  double S[3] = { T(0, 2), T(1, 2), T(2, 2) };
223 
224  double A = P[3] * S[0] + P[4] * S[1] + P[5] * S[2];
225  if (A != 0.)
226  A = 1. / A;
227  S[0] *= A;
228  S[1] *= A;
229  S[2] *= A;
230 
231  double s[5] = {};
232  mult3x5Helper(s, S, &P[7]);
233  globalToLocalVecHelper(P, s[0], s[1], s[2], s[3], s[4]);
234  // Jacobian production
235  mult3x5Helper(&Jac[0], Ax, &P[7]);
236  mult3x5Helper(&Jac[5], Ay, &P[7]);
237 }
238 
240 // Global position transformation to local Disc system coordinate
242 
243 void
244 transformGlobalToDisc(const Amg::Transform3D& T,
245  bool useJac,
246  double* ATH_RESTRICT P,
247  double* ATH_RESTRICT par,
248  double* ATH_RESTRICT Jac)
249 {
250  const double* Ax = T.matrix().col(0).data(); // Oth column
251  const double* Ay = T.matrix().col(1).data(); // 1st column
252 
253  const double d[3] = { P[0] - T(0, 3), P[1] - T(1, 3), P[2] - T(2, 3) };
254 
255  const double RC = d[0] * Ax[0] + d[1] * Ax[1] + d[2] * Ax[2];
256  const double RS = d[0] * Ay[0] + d[1] * Ay[1] + d[2] * Ay[2];
257  const double R2 = RC * RC + RS * RS;
258  par[0] = std::sqrt(R2);
259  par[1] = atan2(RS, RC);
260 
261  if (!useJac)
262  return;
263 
264  // Condition trajectory on surface
265  //
266  double S[3] = { T(0, 2), T(1, 2), T(2, 2) };
267 
268  double A = P[3] * S[0] + P[4] * S[1] + P[5] * S[2];
269  if (A != 0.)
270  A = 1. / A;
271  S[0] *= A;
272  S[1] *= A;
273  S[2] *= A;
274 
275  double s[5] = {};
276  mult3x5Helper(s, S, &P[7]);
277  globalToLocalVecHelper(P, s[0], s[1], s[2], s[3], s[4]);
278  // Jacobian production
279  //
280  double Ri = 1. / par[0];
281 
282  const double Av[3] = { (RC * Ax[0] + RS * Ay[0]) * Ri,
283  (RC * Ax[1] + RS * Ay[1]) * Ri,
284  (RC * Ax[2] + RS * Ay[2]) * Ri };
285  const double Bv[3] = { (RC * Ay[0] - RS * Ax[0]) * (Ri = 1. / R2),
286  (RC * Ay[1] - RS * Ax[1]) * Ri,
287  (RC * Ay[2] - RS * Ax[2]) * Ri };
288 
289  mult3x5Helper(&Jac[0], Av, &P[7]);
290  mult3x5Helper(&Jac[5], Bv, &P[7]);
291 }
293 // Global position transformation to local Cylinder system coordinate
295 void
296 transformGlobalToCylinder(const Amg::Transform3D& T,
297  double R,
298  bool useJac,
299  double* ATH_RESTRICT P,
300  double* ATH_RESTRICT par,
301  double* ATH_RESTRICT Jac)
302 {
303 
304  const double* Ax = T.matrix().col(0).data(); // Oth column
305  const double* Ay = T.matrix().col(1).data(); // 1st column
306  const double* Az = T.matrix().col(2).data(); // 2nd column
307 
308  double x = P[0] - T(0, 3);
309  double y = P[1] - T(1, 3);
310  double z = P[2] - T(2, 3);
311  double RC = x * Ax[0] + y * Ax[1] + z * Ax[2];
312  double RS = x * Ay[0] + y * Ay[1] + z * Ay[2];
313  par[0] = atan2(RS, RC) * R;
314  par[1] = x * Az[0] + y * Az[1] + z * Az[2];
315 
316  if (!useJac)
317  return;
318 
319  // Condition trajectory on surface
320  //
321  double B = par[1];
322  double C = P[3] * Az[0] + P[4] * Az[1] + P[5] * Az[2];
323  const double ax = P[3] - Az[0] * C;
324  x -= (B * Az[0]);
325  const double ay = P[4] - Az[1] * C;
326  y -= (B * Az[1]);
327  const double az = P[5] - Az[2] * C;
328  z -= (B * Az[2]);
329  double A = (ax * x + ay * y + az * z);
330  if (A != 0.)
331  A = 1. / A;
332  x *= A;
333  y *= A;
334  z *= A;
335 
336  const double S[3] = { x, y, z };
337  double s[5] = {};
338  mult3x5Helper(s, S, &P[7]);
339  globalToLocalVecHelper(P, s[0], s[1], s[2], s[3], s[4]);
340  // Jacobian production
341  //
342  const double Av[3] = { (RC * Ay[0] - RS * Ax[0]) * (R = 1. / R),
343  (RC * Ay[1] - RS * Ax[1]) * R,
344  (RC * Ay[2] - RS * Ax[2]) * R };
345 
346  mult3x5Helper(&Jac[0], Av, &P[7]);
347  mult3x5Helper(&Jac[5], Az, &P[7]);
348 }
349 
351 // Global position transformation to local Straight line system coordinate
353 
354 void
355 transformGlobalToLine(const Amg::Transform3D& T,
356  bool useJac,
357  double* ATH_RESTRICT P,
358  double* ATH_RESTRICT par,
359  double* ATH_RESTRICT Jac)
360 {
361  const double* Az = T.matrix().col(2).data(); // 2nd column
362  double Bx = Az[1] * P[5] - Az[2] * P[4];
363  double By = Az[2] * P[3] - Az[0] * P[5];
364  double Bz = Az[0] * P[4] - Az[1] * P[3];
365  const double B2 = Bx * Bx + By * By + Bz * Bz;
366  const double x = P[0] - T(0, 3);
367  const double y = P[1] - T(1, 3);
368  const double z = P[2] - T(2, 3);
369  if (B2 > 1e-14) {
370  const double Bn = 1. / std::sqrt(B2);
371  Bx *= Bn;
372  By *= Bn;
373  Bz *= Bn;
374  par[0] = x * Bx + y * By + z * Bz;
375  } else {
376  // surface and trajectory are parallel, calculate the distance
377  // of closest approach differently.
378  par[0] = std::sqrt(
379  ( std::pow(Az[1] * z - Az[2] * y, 2.)
380  + std::pow(Az[2] * x - Az[0] * z, 2.)
381  + std::pow(Az[0] * y - Az[1] * x, 2.))
382  / (Az[0] * Az[0] + Az[1] * Az[1] + Az[2] * Az[2]));
383  }
384  par[1] = x * Az[0] + y * Az[1] + z * Az[2];
385 
386  if (!useJac) {
387  return;
388  }
389 
390  // Condition trajectory on surface
391  //
392  const double d = P[3] * Az[0] + P[4] * Az[1] + P[5] * Az[2];
393  double a = (1. - d) * (1. + d);
394  if (a != 0.) {
395  a = 1. / a;
396  }
397  const double X = d * Az[0] - P[3];
398  const double Y = d * Az[1] - P[4];
399  const double Z = d * Az[2] - P[5];
400 
401  double D[5] = {};
402  mult3x5Helper(D, Az, &P[10]);
403  const double s0 =
404  (((P[7] * X + P[8] * Y + P[9] * Z) + x * (D[0] * Az[0] - P[10])) +
405  (y * (D[0] * Az[1] - P[11]) + z * (D[0] * Az[2] - P[12]))) *
406  a;
407  const double s1 =
408  (((P[14] * X + P[15] * Y + P[16] * Z) + x * (D[1] * Az[0] - P[17])) +
409  (y * (D[1] * Az[1] - P[18]) + z * (D[1] * Az[2] - P[19]))) *
410  a;
411  const double s2 =
412  (((P[21] * X + P[22] * Y + P[23] * Z) + x * (D[2] * Az[0] - P[24])) +
413  (y * (D[2] * Az[1] - P[25]) + z * (D[2] * Az[2] - P[26]))) *
414  a;
415  const double s3 =
416  (((P[28] * X + P[29] * Y + P[30] * Z) + x * (D[3] * Az[0] - P[31])) +
417  (y * (D[3] * Az[1] - P[32]) + z * (D[3] * Az[2] - P[33]))) *
418  a;
419  const double s4 =
420  (((P[35] * X + P[36] * Y + P[37] * Z) + x * (D[4] * Az[0] - P[38])) +
421  (y * (D[4] * Az[1] - P[39]) + z * (D[4] * Az[2] - P[40]))) *
422  a;
423 
424  // pass -1 (As we want do add rather subtract in the helper)
425  globalToLocalVecHelper(P, -1. * s0, -1. * s1, -1. * s2, -1. * s3, -1. * s4);
426  // Jacobian production
427  const double B[3] = { Bx, By, Bz };
428  mult3x5Helper(&Jac[0], B, &P[7]);
429  mult3x5Helper(&Jac[5], Az, &P[7]);
430 }
431 
433 // Global position transformation to local Cone system coordinate
435 
436 void
437 transformGlobalToCone(const Amg::Transform3D& T,
438  double tA,
439  bool useJac,
440  const double* ATH_RESTRICT P,
441  double* ATH_RESTRICT par,
442  double* ATH_RESTRICT Jac)
443 {
444  const double* Ax = T.matrix().col(0).data(); // Oth column
445  const double* Ay = T.matrix().col(1).data(); // 1st column
446  const double* Az = T.matrix().col(2).data(); // 2nd column
447 
448  const double x = P[0] - T(0, 3);
449  const double y = P[1] - T(1, 3);
450  const double z = P[2] - T(2, 3);
451  const double RC = x * Ax[0] + y * Ax[1] + z * Ax[2];
452  const double RS = x * Ay[0] + y * Ay[1] + z * Ay[2];
453  par[1] = x * Az[0] + y * Az[1] + z * Az[2];
454  par[0] = atan2(RS, RC) * (par[1] * tA);
455 
456  if (!useJac)
457  return;
458 
459  Jac[0] = 0.; // dL0/dL0
460  Jac[1] = 0.; // dL0/dL1
461  Jac[2] = 0.; // dL0/dPhi
462  Jac[3] = 0.; // dL0/dThe
463  Jac[4] = 0.; // dL0/dCM
464  Jac[5] = 0.; // dL1/dL0
465  Jac[6] = 0.; // dL1/dL1
466  Jac[7] = 0.; // dL1/dPhi
467  Jac[8] = 0.; // dL1/dThe
468  Jac[9] = 0.; // dL1/dCM
469 }
470 
472 // Plane local position transformation to global system coordinate
474 
475 void
476 transformPlaneToGlobal(bool useJac,
477  const Amg::Transform3D& T,
478  const AmgVector(5) & ATH_RESTRICT p,
479  double* ATH_RESTRICT P)
480 {
481  const double* Ax = T.matrix().col(0).data(); // Oth column
482  const double* Ay = T.matrix().col(1).data(); // 1st column
483 
484  P[0] = p[0] * Ax[0] + p[1] * Ay[0] + T(0, 3); // X
485  P[1] = p[0] * Ax[1] + p[1] * Ay[1] + T(1, 3); // Y
486  P[2] = p[0] * Ax[2] + p[1] * Ay[2] + T(2, 3); // Z
487 
488  if (!useJac)
489  return;
490 
491  // /dL1 | /dL2 | /dPhi | /dThe |
492  P[7] = Ax[0];
493  P[14] = Ay[0];
494  P[21] = 0.;
495  P[28] = 0.; // dX/
496  P[8] = Ax[1];
497  P[15] = Ay[1];
498  P[22] = 0.;
499  P[29] = 0.; // dY/
500  P[9] = Ax[2];
501  P[16] = Ay[2];
502  P[23] = 0.;
503  P[30] = 0.; // dZ/
504 }
505 
507 // Disc local position transformation to global system coordinate
509 
510 void
511 transformDiscToGlobal(bool useJac,
512  const Amg::Transform3D& T,
513  const AmgVector(5) & ATH_RESTRICT p,
514  double* ATH_RESTRICT P)
515 {
516  const double* Ax = T.matrix().col(0).data(); // Oth column
517  const double* Ay = T.matrix().col(1).data(); // 1st column
518 
519  double Sf, Cf;
520  sincos(p[1], &Sf, &Cf);
521 
522  const double d0 = Cf * Ax[0] + Sf * Ay[0];
523  const double d1 = Cf * Ax[1] + Sf * Ay[1];
524  const double d2 = Cf * Ax[2] + Sf * Ay[2];
525  P[0] = p[0] * d0 + T(0, 3); // X
526  P[1] = p[0] * d1 + T(1, 3); // Y
527  P[2] = p[0] * d2 + T(2, 3); // Z
528 
529  if (!useJac)
530  return;
531 
532  // /dL1 | /dL2 | /dPhi | /dThe |
533  P[7] = d0;
534  P[14] = p[0] * (Cf * Ay[0] - Sf * Ax[0]);
535  P[21] = 0.;
536  P[28] = 0.; // dX/
537  P[8] = d1;
538  P[15] = p[0] * (Cf * Ay[1] - Sf * Ax[1]);
539  P[22] = 0.;
540  P[29] = 0.; // dY/
541  P[9] = d2;
542  P[16] = p[0] * (Cf * Ay[2] - Sf * Ax[2]);
543  P[23] = 0.;
544  P[30] = 0.; // dZ/
545 }
546 
548 // Cylinder local position transformation to global system coordinate
550 
551 void
552 transformCylinderToGlobal(bool useJac,
553  const Amg::Transform3D& T,
554  double R,
555  const AmgVector(5) & ATH_RESTRICT p,
556  double* ATH_RESTRICT P)
557 {
558  const double* Ax = T.matrix().col(0).data(); // Oth column
559  const double* Ay = T.matrix().col(1).data(); // 1st column
560  const double* Az = T.matrix().col(2).data(); // 2nd column
561 
562  const double fr = p[0] / R;
563  double Sf, Cf;
564  sincos(fr, &Sf, &Cf);
565 
566  P[0] = R * (Cf * Ax[0] + Sf * Ay[0]) + p[1] * Az[0] + T(0, 3); // X
567  P[1] = R * (Cf * Ax[1] + Sf * Ay[1]) + p[1] * Az[1] + T(1, 3); // Y
568  P[2] = R * (Cf * Ax[2] + Sf * Ay[2]) + p[1] * Az[2] + T(2, 3); // Z
569 
570  if (!useJac)
571  return;
572 
573  // /dL1 | /dL2 | /dPhi | /dThe |
574  P[7] = Cf * Ay[0] - Sf * Ax[0];
575  P[14] = Az[0];
576  P[21] = 0.;
577  P[28] = 0.; // dX/
578  P[8] = Cf * Ay[1] - Sf * Ax[1];
579  P[15] = Az[1];
580  P[22] = 0.;
581  P[29] = 0.; // dY/
582  P[9] = Cf * Ay[2] - Sf * Ax[2];
583  P[16] = Az[2];
584  P[23] = 0.;
585  P[30] = 0.; // dZ/
586 }
587 
589 // Straight line local position transformation to global system coordinate
591 
592 void
593 transformLineToGlobal(bool useJac,
594  const Amg::Transform3D& T,
595  const AmgVector(5) & ATH_RESTRICT p,
596  double* ATH_RESTRICT P)
597 {
598  const double* Az = T.matrix().col(2).data(); // 2nd column
599 
600  double Bx = Az[1] * P[5] - Az[2] * P[4];
601  double By = Az[2] * P[3] - Az[0] * P[5];
602  double Bz = Az[0] * P[4] - Az[1] * P[3];
603  const double tmp_B2 = Bx * Bx + By * By + Bz * Bz;
604  constexpr double epsilon2 = 1e-14;
605  // assume that B := Bx, By, Boa is null vector if its norm is very small wrt. P:=P[3],P[4],P[5]:
606  // || B || < epsilon * || P|| => || B || = 0
607  // P seems to be always normalised, therefor it is assumed its norm is 1.
608  const double Bn = ( tmp_B2 > epsilon2 ? 1. / std::sqrt(tmp_B2) : 0. );
609  Bx *= Bn;
610  By *= Bn;
611  Bz *= Bn;
612  P[0] = p[1] * Az[0] + Bx * p[0] + T(0, 3); // X
613  P[1] = p[1] * Az[1] + By * p[0] + T(1, 3); // Y
614  P[2] = p[1] * Az[2] + Bz * p[0] + T(2, 3); // Z
615 
616  if (!useJac)
617  return;
618 
619  double Bx2 = -Az[2] * P[25];
620  double Bx3 = Az[1] * P[33] - Az[2] * P[32];
621  double By2 = Az[2] * P[24];
622  double By3 = Az[2] * P[31] - Az[0] * P[33];
623  double Bz2 = Az[0] * P[25] - Az[1] * P[24];
624  double Bz3 = Az[0] * P[32] - Az[1] * P[31];
625  const double B2 = Bx * Bx2 + By * By2 + Bz * Bz2;
626  const double B3 = Bx * Bx3 + By * By3 + Bz * Bz3;
627  Bx2 = (Bx2 - Bx * B2) * Bn;
628  Bx3 = (Bx3 - Bx * B3) * Bn;
629  By2 = (By2 - By * B2) * Bn;
630  By3 = (By3 - By * B3) * Bn;
631  Bz2 = (Bz2 - Bz * B2) * Bn;
632  Bz3 = (Bz3 - Bz * B3) * Bn;
633 
634  // /dL1 | /dL2 | /dPhi | /dThe |
635  P[7] = Bx;
636  P[14] = Az[0];
637  P[21] = Bx2 * p[0];
638  P[28] = Bx3 * p[0]; // dX/
639  P[8] = By;
640  P[15] = Az[1];
641  P[22] = By2 * p[0];
642  P[29] = By3 * p[0]; // dY/
643  P[9] = Bz;
644  P[16] = Az[2];
645  P[23] = Bz2 * p[0];
646  P[30] = Bz3 * p[0]; // dZ/
647 }
648 
650 // Jacobian of transformations from curvilinear to Plane system coordinates
652 
653 void
654 jacobianTransformCurvilinearToPlane(double* ATH_RESTRICT P,
655  double* ATH_RESTRICT Jac)
656 {
657  const double* At = &P[4];
658  double* Au = &P[7];
659  double* Av = &P[10];
660  const double* Ax = &P[13];
661  const double* Ay = &P[16];
662  double* S = &P[19];
663 
664  double A = At[0] * S[0] + At[1] * S[1] + At[2] * S[2];
665  if (A != 0.)
666  A = 1. / A;
667  S[0] *= A;
668  S[1] *= A;
669  S[2] *= A;
670 
671  const double s1 = Au[0] * S[0] + Au[1] * S[1];
672  const double s2 = Av[0] * S[0] + Av[1] * S[1] + Av[2] * S[2];
673 
674  Au[0] -= (s1 * At[0]);
675  Au[1] -= (s1 * At[1]);
676  Au[2] -= (s1 * At[2]);
677  Av[0] -= (s2 * At[0]);
678  Av[1] -= (s2 * At[1]);
679  Av[2] -= (s2 * At[2]);
680 
681  Jac[0] = Ax[0] * Au[0] + Ax[1] * Au[1] + Ax[2] * Au[2]; // dL0/dL0
682  Jac[1] = Ax[0] * Av[0] + Ax[1] * Av[1] + Ax[2] * Av[2]; // dL0/dL1
683  Jac[2] = 0.; // dL0/dPhi
684  Jac[3] = 0.; // dL0/dThe
685  Jac[5] = Ay[0] * Au[0] + Ay[1] * Au[1] + Ay[2] * Au[2]; // dL1/dL0
686  Jac[6] = Ay[0] * Av[0] + Ay[1] * Av[1] + Ay[2] * Av[2]; // dL1/dL1
687  Jac[7] = 0.; // dL1/dPhi
688  Jac[8] = 0.; // dL1/dThe
689 }
690 
692 // Jacobian of transformations from curvilinear to Disc system coordinates
694 
695 void
696 jacobianTransformCurvilinearToDisc(double* ATH_RESTRICT P,
697  double* ATH_RESTRICT Jac)
698 {
699  const double* p = &P[0];
700  const double* At = &P[4];
701  double* Au = &P[7];
702  double* Av = &P[10];
703  const double* Ax = &P[13];
704  const double* Ay = &P[16];
705  double* S = &P[19];
706 
707  // Condition trajectory on surface
708  //
709  double A = At[0] * S[0] + At[1] * S[1] + At[2] * S[2];
710  if (A != 0.)
711  A = 1. / A;
712  S[0] *= A;
713  S[1] *= A;
714  S[2] *= A;
715 
716  const double s1 = Au[0] * S[0] + Au[1] * S[1];
717  const double s2 = Av[0] * S[0] + Av[1] * S[1] + Av[2] * S[2];
718 
719  Au[0] -= (s1 * At[0]);
720  Au[1] -= (s1 * At[1]);
721  Au[2] -= (s1 * At[2]);
722  Av[0] -= (s2 * At[0]);
723  Av[1] -= (s2 * At[1]);
724  Av[2] -= (s2 * At[2]);
725 
726  // Jacobian production
727  //
728  double Sf, Cf;
729  sincos(p[1], &Sf, &Cf);
730 
731  const double Ri = 1. / p[0];
732  const double A0 = (Cf * Ax[0] + Sf * Ay[0]);
733  const double A1 = (Cf * Ax[1] + Sf * Ay[1]);
734  const double A2 = (Cf * Ax[2] + Sf * Ay[2]);
735  const double B0 = (Cf * Ay[0] - Sf * Ax[0]) * Ri;
736  const double B1 = (Cf * Ay[1] - Sf * Ax[1]) * Ri;
737  const double B2 = (Cf * Ay[2] - Sf * Ax[2]) * Ri;
738 
739  Jac[0] = A0 * Au[0] + A1 * Au[1] + A2 * Au[2]; // dL0/dL0
740  Jac[1] = A0 * Av[0] + A1 * Av[1] + A2 * Av[2]; // dL0/dL1
741  Jac[2] = 0.; // dL0/dPhi
742  Jac[3] = 0.; // dL0/dThe
743  Jac[5] = B0 * Au[0] + B1 * Au[1] + B2 * Au[2]; // dL1/dL0
744  Jac[6] = B0 * Av[0] + B1 * Av[1] + B2 * Av[2]; // dL1/dL1
745  Jac[7] = 0.; // dL1/dPhi
746  Jac[8] = 0.; // dL1/dThe
747 }
748 
750 // Jacobian of transformations from curvilinear to Cylinder system coordinates
752 
753 void
754 jacobianTransformCurvilinearToCylinder(double* ATH_RESTRICT P,
755  double* ATH_RESTRICT Jac)
756 {
757  const double* p = &P[0];
758  const double* At = &P[4];
759  double* Au = &P[7];
760  double* Av = &P[10];
761  const double* Ax = &P[13];
762  const double* Ay = &P[16];
763  const double* Az = &P[19];
764  const double R = P[22];
765 
766  const double fr = p[0] / R;
767  double Sf, Cf;
768  sincos(fr, &Sf, &Cf);
769 
770  const double x = Cf * Ax[0] + Sf * Ay[0];
771  const double y = Cf * Ax[1] + Sf * Ay[1];
772  const double z = Cf * Ax[2] + Sf * Ay[2];
773 
774  // Condition trajectory on surface
775  //
776  const double C = At[0] * Az[0] + At[1] * Az[1] + At[2] * Az[2];
777  const double ax = At[0] - Az[0] * C;
778  const double ay = At[1] - Az[1] * C;
779  const double az = At[2] - Az[2] * C;
780  double A = (ax * x + ay * y + az * z);
781  if (A != 0.)
782  A = 1. / A;
783 
784  const double s1 = (Au[0] * x + Au[1] * y) * A;
785  const double s2 = (Av[0] * x + Av[1] * y + Av[2] * z) * A;
786 
787  Au[0] -= (s1 * At[0]);
788  Au[1] -= (s1 * At[1]);
789  Au[2] -= (s1 * At[2]);
790  Av[0] -= (s2 * At[0]);
791  Av[1] -= (s2 * At[1]);
792  Av[2] -= (s2 * At[2]);
793 
794  // Jacobian production
795  //
796  const double A0 = (Cf * Ay[0] - Sf * Ax[0]);
797  const double A1 = (Cf * Ay[1] - Sf * Ax[1]);
798  const double A2 = (Cf * Ay[2] - Sf * Ax[2]);
799 
800  Jac[0] = A0 * Au[0] + A1 * Au[1] + A2 * Au[2]; // dL0/dL0
801  Jac[1] = A0 * Av[0] + A1 * Av[1] + A2 * Av[2]; // dL0/dL1
802  Jac[2] = 0.; // dL0/dPhi
803  Jac[3] = 0.; // dL0/dThe
804  Jac[5] = Az[0] * Au[0] + Az[1] * Au[1] + Az[2] * Au[2]; // dL1/dL0
805  Jac[6] = Az[0] * Av[0] + Az[1] * Av[1] + Az[2] * Av[2]; // dL1/dL1
806  Jac[7] = 0.; // dL1/dPhi
807  Jac[8] = 0.; // dL1/dThe
808 }
809 
811 // Jacobian of transformations from curvilinear to Straight Line system
812 // coordinates
814 
815 void
816 jacobianTransformCurvilinearToStraightLine(const double* ATH_RESTRICT P,
817  double* ATH_RESTRICT Jac)
818 {
819  const double* p = &P[0];
820  const double* At = &P[4];
821  const double* Au = &P[7];
822  const double* Av = &P[10];
823  const double* A = &P[19];
824 
825  double Bx = A[1] * At[2] - A[2] * At[1];
826  double By = A[2] * At[0] - A[0] * At[2];
827  double Bz = A[0] * At[1] - A[1] * At[0];
828  const double Bn = 1. / std::sqrt(Bx * Bx + By * By + Bz * Bz);
829  Bx *= Bn;
830  By *= Bn;
831  Bz *= Bn;
832 
833  // Condition trajectory on surface
834  //
835  const double d = At[0] * A[0] + At[1] * A[1] + At[2] * A[2];
836  double a = (1. - d) * (1. + d);
837  if (a != 0.)
838  a = 1. / a;
839  const double X = d * A[0] - At[0], Y = d * A[1] - At[1], Z = d * A[2] - At[2];
840 
841  const double s1 = (Au[0] * X + Au[1] * Y) * a;
842  const double s2 = (Av[0] * X + Av[1] * Y + Av[2] * Z) * a;
843  const double s3 = p[0] * (Bx * At[1] - By * At[0]) * a;
844  const double s4 = p[0] * (Bx * Av[0] + By * Av[1] + Bz * Av[2]) * a;
845 
846  // Jacobian production
847  //
848  Jac[0] = Bx * Au[0] + By * Au[1]; // dL0/dL0
849  Jac[1] = Bx * Av[0] + By * Av[1] + Bz * Av[2]; // dL0/dL1
850  Jac[2] = 0.; // dL0/dPhi
851  Jac[3] = 0.; // dL0/dThe
852  Jac[5] = A[0] * Au[0] + A[1] * Au[1] + s1 * d; // dL1/dL0
853  Jac[6] = (A[0] * Av[0] + A[1] * Av[1] + A[2] * Av[2]) + s2 * d; // dL1/dL1
854  Jac[7] = s3 * d; // dL1/dPhi
855  Jac[8] = s4 * d; // dL1/dThe
856 }
857 
859 // Step estimation to Plane
861 double
862 stepEstimatorToPlane(const double* ATH_RESTRICT S,
863  const double* ATH_RESTRICT P,
864  bool& Q)
865 {
866  const double* r = &P[0]; // Start coordinate
867  const double* a = &P[3]; // Start direction
868 
869  const double A = a[0] * S[0] + a[1] * S[1] + a[2] * S[2];
870  if (A == 0.) {
871  Q = false;
872  return 1000000.;
873  }
874  const double D = (S[3] - r[0] * S[0]) - (r[1] * S[1] + r[2] * S[2]);
875  Q = true;
876  return (D / A);
877 }
878 
880 // Step estimation to Cylinder
882 double
883 stepEstimatorToCylinder(double* ATH_RESTRICT S,
884  const double* ATH_RESTRICT P,
885  bool& Q)
886 {
887  const double* r = &P[0]; // Start coordinate
888  const double* a = &P[3]; // Start direction
889 
890  double dx = r[0] - S[0];
891  double dy = r[1] - S[1];
892  double dz = r[2] - S[2];
893  double B = dx * S[3] + dy * S[4] + dz * S[5];
894  double C = a[0] * S[3] + a[1] * S[4] + a[2] * S[5];
895  const double ax = a[0] - S[3] * C;
896  dx -= (B * S[3]);
897  const double ay = a[1] - S[4] * C;
898  dy -= (B * S[4]);
899  const double az = a[2] - S[5] * C;
900  dz -= (B * S[5]);
901  double A = 2. * (ax * ax + ay * ay + az * az);
902  if (A == 0.) {
903  Q = false;
904  return 0.;
905  }
906  B = 2. * (ax * dx + ay * dy + az * dz);
907  double g = dx + dy + dz;
908  C = (g - S[6]) * (g + S[6]) - 2. * (dx * (dy + dz) + dy * dz);
909  double Sq = B * B - 2. * A * C;
910 
911  double Smin = -B / A, Smax = Smin;
912 
913  if (Sq > 0.) {
914 
915  Sq = std::sqrt(Sq) / A;
916  if (B > 0.) {
917  Smin += Sq;
918  Smax -= Sq;
919  } else {
920  Smin -= Sq;
921  Smax += Sq;
922  }
923  } else {
924  if (std::fabs(Smax) < .1) {
925  Q = false;
926  return 0.;
927  }
928  }
929 
930  Q = true;
931  const double inside = Smin * Smax;
932 
933  if (S[8] != 0.) {
934 
935  if (inside > 0. || S[8] > 0.)
936  return Smin;
937  if (S[7] >= 0.) {
938  if (Smin >= 0.)
939  return Smin;
940  return Smax;
941  }
942  if (Smin <= 0.)
943  return Smin;
944  return Smax;
945  }
946 
947  if (inside < 0.) {
948 
949  S[8] = -1.;
950  if (S[7] >= 0.) {
951  if (Smin >= 0.)
952  return Smin;
953  return Smax;
954  }
955  if (Smin <= 0.)
956  return Smin;
957  return Smax;
958  }
959 
960  // if(std::fabs(Smin) < .001) {S[8]=-1.; return Smax;}
961 
962  S[8] = 1.;
963  return Smin;
964 }
965 
967 // Step estimation to Straight Line
969 double
970 stepEstimatorToStraightLine(const double* ATH_RESTRICT S,
971  const double* ATH_RESTRICT P,
972  bool& Q)
973 {
974  const double* r = &P[0]; // Start coordinate
975  const double* a = &P[3]; // Start direction
976 
977  double D = a[0] * S[3] + a[1] * S[4] + a[2] * S[5];
978  const double A = (1. - D) * (1. + D);
979  if (A == 0.) {
980  Q = true;
981  return 1000000.;
982  }
983  D = (r[0] - S[0]) * (D * S[3] - a[0]) + (r[1] - S[1]) * (D * S[4] - a[1]) +
984  (r[2] - S[2]) * (D * S[5] - a[2]);
985  Q = true;
986  return (D / A);
987 }
988 
990 // Step estimation to Cone
992 double
993 stepEstimatorToCone(double* ATH_RESTRICT S,
994  const double* ATH_RESTRICT P,
995  bool& Q)
996 {
997  const double* r = &P[0]; // Start coordinate
998  const double* a = &P[3]; // Start direction
999 
1000  const double dx = r[0] - S[0];
1001  const double dy = r[1] - S[1];
1002  const double dz = r[2] - S[2];
1003  const double A = dx * S[3] + dy * S[4] + dz * S[5];
1004  const double B = a[0] * S[3] + a[1] * S[4] + a[2] * S[5];
1005  const double C = a[0] * dx + a[1] * dy + a[2] * dz;
1006  const double k = S[6];
1007 
1008  const double KB = 1. - k * B * B;
1009  const double KABC = k * A * B - C;
1010  double Smin, Smax;
1011 
1012  if (KB != 0.) {
1013 
1014  Smin = KABC / KB;
1015  Smax = Smin;
1016  double Sq = KABC * KABC + (k * A * A - dx * dx - dy * dy - dz * dz) * KB;
1017  if (Sq >= 0.) {
1018  Sq = std::sqrt(Sq) / KB;
1019  if (KABC > 0.) {
1020  Smin -= Sq;
1021  Smax += Sq;
1022  } else {
1023  Smin += Sq;
1024  Smax -= Sq;
1025  }
1026  int n = 2;
1027  if (A + B * Smin < 0.) {
1028  --n;
1029  Smin = Smax;
1030  }
1031  if (A + B * Smax < 0.) {
1032  --n;
1033  Smax = Smin;
1034  }
1035  if (!n) {
1036  Q = false;
1037  return 0.;
1038  }
1039 
1040  } else {
1041  Q = false;
1042  return 0.;
1043  }
1044  } else {
1045  Smin = (dx * dx + dy * dy + dz * dz - k * A * A) / (2. * KABC);
1046  Smax = Smin;
1047  if (A + B * Smin < 0.) {
1048  Q = false;
1049  return 0.;
1050  }
1051  }
1052 
1053  Q = true;
1054  const double inside = Smin * Smax;
1055 
1056  if (S[8] != 0.) {
1057 
1058  if (inside > 0. || S[8] > 0.)
1059  return Smin;
1060  if (S[7] >= 0.) {
1061  if (Smin >= 0.)
1062  return Smin;
1063  return Smax;
1064  }
1065  if (Smin <= 0.)
1066  return Smin;
1067  return Smax;
1068  }
1069 
1070  if (inside < 0.) {
1071 
1072  S[8] = -1.;
1073  if (S[7] >= 0.) {
1074  if (Smin >= 0.)
1075  return Smin;
1076  return Smax;
1077  }
1078  if (Smin <= 0.)
1079  return Smin;
1080  return Smax;
1081  }
1082 
1083  S[8] = 1.;
1084  return Smin;
1085 }
1086 
1087 
1088 } //end of anonymous namespace
1089 
1090 
1092 // Common transformation from local to global system coordinates for all
1093 // surfaces for charged track parameters
1095 
1096 bool
1097 Trk::RungeKuttaUtils::transformLocalToGlobal(bool useJac,
1098  const Trk::TrackParameters& Tp,
1099  double* ATH_RESTRICT P)
1100 {
1101  return transformLocalToGlobal(
1102  useJac, &Tp.associatedSurface(), Tp.parameters(), P);
1103 }
1104 
1106 // Common transformation from local to global system coordinates for all
1107 // surfaces for neutral track parameters
1109 
1110 bool
1111 Trk::RungeKuttaUtils::transformLocalToGlobal(bool useJac,
1112  const Trk::NeutralParameters& Tp,
1113  double* ATH_RESTRICT P)
1114 {
1115  return transformLocalToGlobal(
1116  useJac, &Tp.associatedSurface(), Tp.parameters(), P);
1117 }
1118 
1120 // Common transformation from local to global system coordinates for all
1121 // surfaces for pattern parameters
1123 
1124 bool
1125 Trk::RungeKuttaUtils::transformLocalToGlobal(
1126  bool useJac,
1127  const Trk::PatternTrackParameters& Tp,
1128  double* P)
1129 {
1130  return transformLocalToGlobal(
1131  useJac, &Tp.associatedSurface(), Tp.parameters(), P);
1132 }
1133 
1135 // Common transformation from global to local system coordinates for all
1136 // surfaces
1138 
1139 void
1140 Trk::RungeKuttaUtils::transformGlobalToLocal(double* ATH_RESTRICT P,
1141  double* ATH_RESTRICT par)
1142 {
1143  par[2] = atan2(P[4], P[3]);
1144  par[3] = acos(P[5]);
1145  par[4] = P[6];
1146 }
1147 
1148 void
1149 Trk::RungeKuttaUtils::transformGlobalToLocal(const Trk::Surface* su,
1150  bool useJac,
1151  double* ATH_RESTRICT P,
1152  double* ATH_RESTRICT par,
1153  double* ATH_RESTRICT Jac)
1154 {
1155  par[2] = atan2(P[4], P[3]);
1156  par[3] = acos(P[5]);
1157  par[4] = P[6];
1158 
1159  const Trk::SurfaceType ty = su->type();
1160  const Amg::Transform3D& T = su->transform();
1161  switch (ty) {
1162  case Trk::SurfaceType::Plane: {
1163  transformGlobalToPlane(T, useJac, P, par, Jac);
1164  break;
1165  }
1166  // Perigee call the same as line
1167  case Trk::SurfaceType::Line:
1168  case Trk::SurfaceType::Perigee: {
1169  transformGlobalToLine(T, useJac, P, par, Jac);
1170  break;
1171  }
1172  case Trk::SurfaceType::Cylinder: {
1173  transformGlobalToCylinder(
1174  T,
1175  static_cast<const Trk::CylinderSurface*>(su)->bounds().r(),
1176  useJac,
1177  P,
1178  par,
1179  Jac);
1180  break;
1181  }
1182  case Trk::SurfaceType::Disc: {
1183  transformGlobalToDisc(T, useJac, P, par, Jac);
1184  break;
1185  }
1186  case Trk::SurfaceType::Cone: {
1187  transformGlobalToCone(
1188  T,
1189  static_cast<const Trk::ConeSurface*>(su)->bounds().tanAlpha(),
1190  useJac,
1191  P,
1192  par,
1193  Jac);
1194  break;
1195  }
1196  default: {
1197  break;
1198  }
1199  }
1200 
1201  if (!useJac)
1202  return;
1203 
1204  double P3, P4, C = P[3] * P[3] + P[4] * P[4];
1205  if (C > 1.e-20) {
1206  C = 1. / C;
1207  P3 = P[3] * C;
1208  P4 = P[4] * C;
1209  C = -std::sqrt(C);
1210  } else {
1211  C = -1.e10;
1212  P3 = 1.;
1213  P4 = 0.;
1214  }
1215 
1216  Jac[10] = P3 * P[11] - P4 * P[10]; // dPhi/dL0
1217  Jac[11] = P3 * P[18] - P4 * P[17]; // dPhi/dL1
1218  Jac[12] = P3 * P[25] - P4 * P[24]; // dPhi/dPhi
1219  Jac[13] = P3 * P[32] - P4 * P[31]; // dPhi/dThe
1220  Jac[14] = P3 * P[39] - P4 * P[38]; // dPhi/dCM
1221  Jac[15] = C * P[12]; // dThe/dL0
1222  Jac[16] = C * P[19]; // dThe/dL1
1223  Jac[17] = C * P[26]; // dThe/dPhi
1224  Jac[18] = C * P[33]; // dThe/dThe
1225  Jac[19] = C * P[40]; // dThe/dCM
1226  Jac[20] = P[41]; // dCM /dCM
1227 }
1228 
1230 // step estimation to surfaces
1232 double
1233 Trk::RungeKuttaUtils::stepEstimator(
1234  int kind,
1235  double* ATH_RESTRICT Su,
1236  const double* ATH_RESTRICT P,
1237  bool& Q)
1238 {
1239  switch (kind) {
1240  case 0: {
1241  return stepEstimatorToStraightLine(Su, P, Q);
1242  }
1243  case 1: {
1244  return stepEstimatorToPlane(Su, P, Q);
1245  }
1246  case 2: {
1247  return stepEstimatorToCylinder(Su, P, Q);
1248  }
1249  case 3: {
1250  return stepEstimatorToCone(Su, P, Q);
1251  }
1252  default: {
1253  return 1000000.;
1254  }
1255  }
1256 }
1257 double
1258 Trk::RungeKuttaUtils::stepEstimator(
1259  Trk::SurfaceType surfaceType,
1260  double* ATH_RESTRICT Su,
1261  const double* ATH_RESTRICT P,
1262  bool& Q)
1263 {
1264  if (surfaceType == Trk::SurfaceType::Plane ||
1265  surfaceType == Trk::SurfaceType::Disc) {
1266  return stepEstimatorToPlane(Su, P, Q);
1267  }
1268  if (surfaceType == Trk::SurfaceType::Cylinder) {
1269  return stepEstimatorToCylinder(Su, P, Q);
1270  }
1271 
1272  if (surfaceType == Trk::SurfaceType::Line ||
1273  surfaceType == Trk::SurfaceType::Perigee) {
1274  return stepEstimatorToStraightLine(Su, P, Q);
1275  }
1276 
1277  if (surfaceType == Trk::SurfaceType::Cone) {
1278  return stepEstimatorToCone(Su, P, Q);
1279  }
1280  // presumably curvilinear
1281  return stepEstimatorToPlane(Su, P, Q);
1282 }
1283 
1285 // Step estimation to surfaces
1286 //
1287 // Input iformation
1288 //
1289 // SU - input vector surfaces and conditions for boundary check
1290 // DN - is work map
1291 // Pinp - array of track parameters in global system coordinates before some
1292 // step Pout - array of track parameters in global system coordinates before
1293 // some step W - trajectory total pass for Pinp position So - max. step
1294 // for straight line propagation model Nv - surface number in vector SU which
1295 // we don't need to use
1296 //
1297 // Output information
1298 //
1299 // pair(step to surface,surface number from vector SU)
1300 // next - condition to use Pout for next step
1302 
1303 std::pair<double, int>
1304 Trk::RungeKuttaUtils::stepEstimator(
1305  std::vector<std::pair<const Trk::Surface*, Trk::BoundaryCheck>>& SU,
1306  std::multimap<double, int>& DN,
1307  const double* ATH_RESTRICT Pinp,
1308  const double* ATH_RESTRICT Pout,
1309  double W,
1310  double So,
1311  int Nv,
1312  bool& next)
1313 {
1314  W = std::fabs(W);
1315  next = false;
1316  int N = -1;
1317  const double D[3] = { Pout[0] - Pinp[0], Pout[1] - Pinp[1], Pout[2] - Pinp[2] };
1318  double Smax = std::sqrt(D[0] * D[0] + D[1] * D[1] + D[2] * D[2]);
1319  double Sign = D[0] * Pinp[3] + D[1] * Pinp[4] + D[2] * Pinp[5];
1320  // The magnitude of the vector is essentially 0. No
1322  if (Smax < DBL_EPSILON) {
1323  next = true;
1324  return std::make_pair(0., -1);
1325  }
1326  Eigen::Map<const Amg::Vector3D> pos(&Pinp[0],3,1);
1327  const Amg::Vector3D dir(D[0] / Smax, D[1] / Smax, D[2] / Smax);
1328 
1329  double Smin = 2. * Smax;
1330 
1331  std::vector<std::pair<double, int>> LD;
1332  std::multimap<double, int>::iterator i = DN.begin(), ie = DN.end();
1333 
1334  for (; i != ie; ++i) {
1335 
1336  if ((*i).first > W + Smin)
1337  break;
1338 
1339  int j = (*i).second;
1340  Trk::DistanceSolution ds =
1341  SU[j].first->straightLineDistanceEstimate(pos, dir, SU[j].second);
1342  LD.emplace_back(ds.currentDistance(false) + W, j);
1343 
1344  const int n = ds.numberOfSolutions();
1345  if (!n)
1346  continue;
1347 
1348  for (int i = 0; i != n; ++i) {
1349  double s;
1350  i == 0 ? s = ds.first() : s = ds.second();
1351  if (s < 0. || s > Smin || (j == Nv && s < So))
1352  continue;
1353  Smin = s;
1354  N = j;
1355  }
1356  }
1357 
1358  if (!LD.empty()) {
1359 
1360  DN.erase(DN.begin(), i);
1361  std::vector<std::pair<double, int>>::iterator l = LD.begin(), le = LD.end();
1362  for (; l != le; ++l)
1363  DN.insert((*l));
1364  }
1365 
1366  if (N < 0) {
1367  next = true;
1368  return std::make_pair(0., -1);
1369  }
1370 
1371  double Sm = Smin;
1372 
1373  if (Sign < 0.)
1374  Sm = -Sm;
1375  if (Smin < So || (Smax - Smin) > 2. * So)
1376  return std::make_pair(Sm, N);
1377 
1378  Eigen::Map<const Amg::Vector3D> posn(&Pout[0], 3, 1);
1379  Eigen::Map<const Amg::Vector3D> dirn(&Pout[3], 3, 1);
1380 
1381  Trk::DistanceSolution ds =
1382  SU[N].first->straightLineDistanceEstimate(posn, dirn, SU[N].second);
1383  const int n = ds.numberOfSolutions();
1384  if (!n)
1385  return std::make_pair(Sm, N);
1386  Sm = 10000.;
1387 
1388  for (int i = 0; i != n; ++i) {
1389 
1390  double sa, s;
1391  i == 0 ? s = ds.first() : s = ds.second();
1392  sa = std::fabs(s);
1393 
1394  if (s * Sign < 0.) {
1395  // if(sa < So ) {next = true; return std::make_pair(s,N);}
1396  next = true;
1397  return std::make_pair(s, N);
1398  }
1399  if (sa < Sm) {
1400  Sm = s;
1401  next = true;
1402  }
1403  }
1404  return std::make_pair(Sm, N);
1405 }
1406 
1408 // New covariance matrix calculation from old matrix and jacobian
1409 // We use Eigen so we use flatten to avoid out of line calls
1411 ATH_FLATTEN
1412 AmgSymMatrix(5) Trk::RungeKuttaUtils::newCovarianceMatrix(
1413  const double* ATH_RESTRICT J,
1414  const AmgSymMatrix(5) & ATH_RESTRICT M)
1415 {
1416  AmgSymMatrix(5) nM;
1417  AmgSymMatrix(5)& m = nM;
1418 
1419  Eigen::Map<const AmgVector(5)> JacMap0(&J[0], 5, 1);
1420  const AmgVector(5) a1 = M * JacMap0; //(5x5 * 5x1)
1421  m(0, 0) = a1.dot(JacMap0); // dot product
1422 
1423  Eigen::Map<const AmgVector(5)> JacMap5(&J[5]);
1424  const AmgVector(5) a2 = M * JacMap5;
1425  m(1, 0) = a2.dot(JacMap0);
1426  m(1, 1) = a2.dot(JacMap5);
1427  m(0, 1) = m(1, 0);
1428 
1429  Eigen::Map<const AmgVector(5)> JacMap10(&J[10], 5, 1);
1430  const AmgVector(5) a3 = M * JacMap10;
1431  m(2, 0) = a3.dot(JacMap0);
1432  m(2, 1) = a3.dot(JacMap5);
1433  m(2, 2) = a3.dot(JacMap10);
1434  m(0, 2) = m(2, 0);
1435  m(1, 2) = m(2, 1);
1436 
1437  Eigen::Map<const AmgVector(5)> JacMap15(&J[15], 5, 1);
1438  const AmgVector(5) a4 = M * JacMap15;
1439  m(3, 0) = a4.dot(JacMap0);
1440  m(3, 1) = a4.dot(JacMap5);
1441  m(3, 2) = a4.dot(JacMap10);
1442  m(3, 3) = a4.dot(JacMap15);
1443  m(0, 3) = m(3, 0);
1444  m(1, 3) = m(3, 1);
1445  m(2, 3) = m(3, 2);
1446 
1447  const AmgVector(5) a5 = M.row(4) * J[20];
1448  m(4, 0) = a5.dot(JacMap0);
1449  m(4, 1) = a5.dot(JacMap5);
1450  m(4, 2) = a5.dot(JacMap10);
1451  m(4, 3) = a5.dot(JacMap15);
1452  m(4, 4) = a5[4] * J[20];
1453  m(0, 4) = m(4, 0);
1454  m(1, 4) = m(4, 1);
1455  m(2, 4) = m(4, 2);
1456  m(3, 4) = m(4, 3);
1457 
1458  return nM;
1459 }
1460 
1462 // Tramsform from local to global for all track parameters
1464 
1465 bool
1466 Trk::RungeKuttaUtils::transformLocalToGlobal(bool useJac,
1467  const Trk::Surface* Su,
1468  const AmgVector(5) & ATH_RESTRICT p,
1469  double* ATH_RESTRICT P)
1470 {
1471  if (!Su)
1472  return false;
1473 
1474  double Sf, Cf, Ce, Se;
1475  sincos(p[2], &Sf, &Cf);
1476  sincos(p[3], &Se, &Ce);
1477 
1478  P[3] = Cf * Se; // Ax
1479  P[4] = Sf * Se; // Ay
1480  P[5] = Ce; // Az
1481  P[6] = p[4]; // CM
1482  if (std::fabs(P[6]) < .000000000000001) {
1483  P[6] < 0. ? P[6] = -.000000000000001 : P[6] = .000000000000001;
1484  }
1485 
1486  if (useJac) {
1487 
1488  // /dL1 | /dL2 | /dPhi | /dThe | /dCM |
1489  P[35] = 0.; // dX /
1490  P[36] = 0.; // dY /
1491  P[37] = 0.; // dZ /
1492  P[10] = 0.;
1493  P[17] = 0.;
1494  P[24] = -P[4];
1495  P[31] = Cf * Ce;
1496  P[38] = 0.; // dAx/
1497  P[11] = 0.;
1498  P[18] = 0.;
1499  P[25] = P[3];
1500  P[32] = Sf * Ce;
1501  P[39] = 0.; // dAy/
1502  P[12] = 0.;
1503  P[19] = 0.;
1504  P[26] = 0.;
1505  P[33] = -Se;
1506  P[40] = 0.; // dAz/
1507  P[13] = 0.;
1508  P[20] = 0.;
1509  P[27] = 0.;
1510  P[34] = 0.;
1511  P[41] = 1.; // dCM/
1512  P[42] = 0.;
1513  P[43] = 0.;
1514  P[44] = 0.; // d(Ax,Ay,Az)/ds
1515  }
1516 
1517  const Trk::SurfaceType ty = Su->type();
1518  const Amg::Transform3D& T = Su->transform();
1519  switch (ty) {
1520  case Trk::SurfaceType::Plane: {
1521  transformPlaneToGlobal(useJac, T, p, P);
1522  return true;
1523  }
1524  case Trk::SurfaceType::Line:
1525  case Trk::SurfaceType::Perigee: {
1526  transformLineToGlobal(useJac, T, p, P);
1527  return true;
1528  }
1529  case Trk::SurfaceType::Cylinder: {
1530  transformCylinderToGlobal(
1531  useJac,
1532  T,
1533  static_cast<const Trk::CylinderSurface*>(Su)->bounds().r(),
1534  p,
1535  P);
1536  return true;
1537  }
1538  case Trk::SurfaceType::Disc: {
1539  transformDiscToGlobal(useJac, T, p, P);
1540  return true;
1541  }
1542  default: {
1543  return false;
1544  }
1545  }
1546 }
1547 
1549 // Global position transformation to curvilinear system coordinate
1551 
1552 void
1553 Trk::RungeKuttaUtils::transformGlobalToCurvilinear(bool useJac,
1554  double* ATH_RESTRICT P,
1555  double* ATH_RESTRICT par,
1556  double* ATH_RESTRICT Jac)
1557 {
1558  par[0] = 0.;
1559  par[1] = 0.;
1560 
1561  if (!useJac)
1562  return;
1563 
1564  const double An = std::sqrt(P[3] * P[3] + P[4] * P[4]);
1565  double Ax[3];
1566  if (An != 0.) {
1567  Ax[0] = -P[4] / An;
1568  Ax[1] = P[3] / An;
1569  Ax[2] = 0.;
1570  } else {
1571  Ax[0] = 1.;
1572  Ax[1] = 0.;
1573  Ax[2] = 0.;
1574  }
1575 
1576  const double Ay[3] = { -Ax[1] * P[5], Ax[0] * P[5], An };
1577  double S[3] = { P[3], P[4], P[5] };
1578 
1579  double A = P[3] * S[0] + P[4] * S[1] + P[5] * S[2];
1580  if (A != 0.)
1581  A = 1. / A;
1582  S[0] *= A;
1583  S[1] *= A;
1584  S[2] *= A;
1585 
1586  double s[5] = {};
1587  mult3x5Helper(s, S, &P[7]);
1588  globalToLocalVecHelper(P, s[0], s[1], s[2], s[3], s[4]);
1589 
1590  double P3, P4, C = P[3] * P[3] + P[4] * P[4];
1591  if (C > 1.e-20) {
1592  C = 1. / C;
1593  P3 = P[3] * C;
1594  P4 = P[4] * C;
1595  C = -std::sqrt(C);
1596  } else {
1597  C = -1.e10;
1598  P3 = 1.;
1599  P4 = 0.;
1600  }
1601 
1602  // Jacobian production
1603  //
1604  Jac[0] = Ax[0] * P[7] + Ax[1] * P[8]; // dL0/dL0
1605  Jac[1] = Ax[0] * P[14] + Ax[1] * P[15]; // dL0/dL1
1606  Jac[2] = Ax[0] * P[21] + Ax[1] * P[22]; // dL0/dPhi
1607  Jac[3] = Ax[0] * P[28] + Ax[1] * P[29]; // dL0/dThe
1608  Jac[4] = Ax[0] * P[35] + Ax[1] * P[36]; // dL0/dCM
1609  Jac[5] = Ay[0] * P[7] + Ay[1] * P[8] + Ay[2] * P[9]; // dL1/dL0
1610  Jac[6] = Ay[0] * P[14] + Ay[1] * P[15] + Ay[2] * P[16]; // dL1/dL1
1611  Jac[7] = Ay[0] * P[21] + Ay[1] * P[22] + Ay[2] * P[23]; // dL1/dPhi
1612  Jac[8] = Ay[0] * P[28] + Ay[1] * P[29] + Ay[2] * P[30]; // dL1/dThe
1613  Jac[9] = Ay[0] * P[35] + Ay[1] * P[36] + Ay[2] * P[37]; // dL1/dCM
1614  Jac[10] = P3 * P[11] - P4 * P[10]; // dPhi/dL0
1615  Jac[11] = P3 * P[18] - P4 * P[17]; // dPhi/dL1
1616  Jac[12] = P3 * P[25] - P4 * P[24]; // dPhi/dPhi
1617  Jac[13] = P3 * P[32] - P4 * P[31]; // dPhi/dThe
1618  Jac[14] = P3 * P[39] - P4 * P[38]; // dPhi/dCM
1619  Jac[15] = C * P[12]; // dThe/dL0
1620  Jac[16] = C * P[19]; // dThe/dL1
1621  Jac[17] = C * P[26]; // dThe/dPhi
1622  Jac[18] = C * P[33]; // dThe/dThe
1623  Jac[19] = C * P[40]; // dThe/dCM
1624  Jac[20] = P[41]; // dCM /dCM
1625 }
1626 
1628 // Curvilinear covariance transformation to global system coordinate
1630 
1631 void
1632 Trk::RungeKuttaUtils::transformCurvilinearToGlobal(double* ATH_RESTRICT p,
1633  double* ATH_RESTRICT P)
1634 {
1635  double Sf, Cf, Ce, Se;
1636  sincos(p[2], &Sf, &Cf);
1637  sincos(p[3], &Se, &Ce);
1638 
1639  const double Ax[3] = { -Sf, Cf, 0. };
1640  const double Ay[3] = { -Cf * Ce, -Sf * Ce, Se };
1641 
1642  // /dL1 | /dL2 | /dPhi | /dThe | /dCM |
1643  //
1644  P[7] = Ax[0];
1645  P[14] = Ay[0];
1646  P[21] = 0.;
1647  P[28] = 0.;
1648  P[35] = 0.; // dX /
1649  P[8] = Ax[1];
1650  P[15] = Ay[1];
1651  P[22] = 0.;
1652  P[29] = 0.;
1653  P[36] = 0.; // dY /
1654  P[9] = Ax[2];
1655  P[16] = Ay[2];
1656  P[23] = 0.;
1657  P[30] = 0.;
1658  P[37] = 0.; // dZ /
1659  P[10] = 0.;
1660  P[17] = 0.;
1661  P[24] = -Sf * Se;
1662  P[31] = -Ay[0];
1663  P[38] = 0.; // dAx/
1664  P[11] = 0.;
1665  P[18] = 0.;
1666  P[25] = Cf * Se;
1667  P[32] = -Ay[1];
1668  P[39] = 0.; // dAy/
1669  P[12] = 0.;
1670  P[19] = 0.;
1671  P[26] = 0.;
1672  P[33] = -Ay[2];
1673  P[40] = 0.; // dAz/
1674  P[13] = 0.;
1675  P[20] = 0.;
1676  P[27] = 0.;
1677  P[34] = 0.;
1678  P[41] = 1.; // dCM/
1679  P[42] = 0.;
1680  P[43] = 0.;
1681  P[44] = 0.;
1682 }
1683 
1685 // Jacobian of transformations from curvilinear to local system coordinates
1686 // for Trk::TrackParameters
1688 
1689 void
1690 Trk::RungeKuttaUtils::jacobianTransformCurvilinearToLocal(
1691  const Trk::TrackParameters& Tp,
1692  double* Jac)
1693 {
1694  const AmgVector(5)& Vp = Tp.parameters();
1695  double P[23];
1696  P[0] = Vp[0];
1697  P[1] = Vp[1];
1698  P[2] = Vp[2];
1699  P[3] = Vp[3];
1700  jacobianTransformCurvilinearToLocal(P, &Tp.associatedSurface(), Jac);
1701 }
1702 
1704 // Jacobian of transformations from curvilinear to local system coordinates
1705 // for Trk::PatternTrackParameters
1707 
1708 void
1709 Trk::RungeKuttaUtils::jacobianTransformCurvilinearToLocal(
1710  const Trk::PatternTrackParameters& Tp,
1711  double* Jac)
1712 {
1713  double P[23];
1714  const AmgVector(5)& p = Tp.parameters();
1715  P[0] = p[0];
1716  P[1] = p[1];
1717  P[2] = p[2];
1718  P[3] = p[3];
1719  jacobianTransformCurvilinearToLocal(P, &Tp.associatedSurface(), Jac);
1720 }
1721 
1723 // Jacobian of transformations from curvilinear to local system coordinates
1725 
1726 void
1727 Trk::RungeKuttaUtils::jacobianTransformCurvilinearToLocal(
1728  double* ATH_RESTRICT P,
1729  const Trk::Surface* su,
1730  double* ATH_RESTRICT Jac)
1731 {
1732  // Common for all surfaces terms of jacobian
1733  //
1734  Jac[4] = 0.; // dL0/dCM
1735  Jac[9] = 0.; // dL1/dCM
1736  Jac[10] = 0.; // dPhi/dL0
1737  Jac[11] = 0.; // dPhi/dL1
1738  Jac[12] = 1.; // dPhi/dPhi
1739  Jac[13] = 0.; // dPhi/dThe
1740  Jac[14] = 0.; // dPhi/dCM
1741  Jac[15] = 0.; // dThe/dL0
1742  Jac[16] = 0.; // dThe/dL1
1743  Jac[17] = 0.; // dThe/dPhi
1744  Jac[18] = 1.; // dThe/dThe
1745  Jac[19] = 0.; // dThe/dCM
1746  Jac[20] = 1.; // dCM /dCM
1747 
1748  const Amg::Transform3D& T = su->transform();
1749 
1750  double Sf, Cf, Ce, Se;
1751  sincos(P[2], &Sf, &Cf);
1752  sincos(P[3], &Se, &Ce);
1753 
1754  P[4] = Cf * Se;
1755  P[5] = Sf * Se;
1756  P[6] = Ce; // At
1757  P[7] = -Sf;
1758  P[8] = Cf;
1759  P[9] = 0.; // Au
1760  P[10] = -Cf * Ce;
1761  P[11] = -Sf * Ce;
1762  P[12] = Se; // Av
1763  P[13] = T(0, 0);
1764  P[14] = T(1, 0);
1765  P[15] = T(2, 0); // Ax
1766  P[16] = T(0, 1);
1767  P[17] = T(1, 1);
1768  P[18] = T(2, 1); // Ay
1769  P[19] = T(0, 2);
1770  P[20] = T(1, 2);
1771  P[21] = T(2, 2); // Az
1772 
1773  const Trk::SurfaceType ty = su->type();
1774 
1775  switch (ty) {
1776  case Trk::SurfaceType::Plane: {
1777  jacobianTransformCurvilinearToPlane(P, Jac);
1778  return;
1779  }
1780  case Trk::SurfaceType::Line:
1781  case Trk::SurfaceType::Perigee: {
1782  jacobianTransformCurvilinearToStraightLine(P, Jac);
1783  return;
1784  }
1785  case Trk::SurfaceType::Cylinder: {
1786  P[22] = static_cast<const Trk::CylinderSurface*>(su)->bounds().r();
1787  jacobianTransformCurvilinearToCylinder(P, Jac);
1788  return;
1789  }
1790  case Trk::SurfaceType::Disc: {
1791  jacobianTransformCurvilinearToDisc(P, Jac);
1792  ;
1793  return;
1794  }
1795  default: {
1796  return;
1797  }
1798  }
1799 
1800  Jac[0] = Jac[3] = 1.;
1801  Jac[1] = Jac[2] = 0.;
1802 }
1803 
1805 // Fill distances map (initial map<distance,N of SU> preparation
1806 // Output
1807 // Step[0] - min negative step
1808 // Step[1] - min positive step
1809 // Step[2] - max distance
1810 // int - vector SU element number with initial surface
1812 
1813 int
1814 Trk::RungeKuttaUtils::fillDistancesMap(
1815  std::vector<std::pair<const Trk::Surface*, Trk::BoundaryCheck>>& SU,
1816  std::multimap<double, int>& DN,
1817  const double* ATH_RESTRICT Pinp,
1818  double W,
1819  const Trk::Surface* So,
1820  double* ATH_RESTRICT Step)
1821 {
1822  int Ns = -1;
1823  DN.erase(DN.begin(), DN.end());
1824 
1825  Eigen::Map<const Amg::Vector3D> pos(&Pinp[0], 3, 1);
1826  Eigen::Map<const Amg::Vector3D> dir(&Pinp[3], 3, 1);
1827 
1828  Step[0] = -1.e+20;
1829  Step[1] = 1.e+20;
1830  Step[2] = 0.;
1831 
1832  int N = SU.size();
1833  W = std::fabs(W);
1834 
1835  for (int i = 0; i != N; ++i) {
1836 
1837  const Trk::Surface* s = SU[i].first;
1838  if (!s)
1839  continue;
1840  if (s == So)
1841  Ns = i;
1842  Trk::DistanceSolution ds =
1843  s->straightLineDistanceEstimate(pos, dir, SU[i].second);
1844  double dist = ds.currentDistance(false);
1845  DN.insert(std::make_pair(dist + W, i));
1846 
1847  if (dist > Step[2])
1848  Step[2] = dist;
1849 
1850  int n = ds.numberOfSolutions();
1851  if (!n)
1852  continue;
1853  for (int i = 0; i != n; ++i) {
1854 
1855  double st;
1856  i == 0 ? st = ds.first() : st = ds.second();
1857 
1858  if (s == So && std::fabs(st) <= .001)
1859  continue;
1860 
1861  if (st < 0.) {
1862  if (st > Step[0])
1863  Step[0] = st;
1864  } else {
1865  if (st < Step[1])
1866  Step[1] = st;
1867  }
1868  }
1869  }
1870  return Ns;
1871 }
xAOD::iterator
JetConstituentVector::iterator iterator
Definition: JetConstituentVector.cxx:68
Trk::RungeKuttaUtils::jacobianTransformCurvilinearToLocal
void jacobianTransformCurvilinearToLocal(const Trk::TrackParameters &, double *)
Definition: RungeKuttaUtils.cxx:1690
beamspotman.r
def r
Definition: beamspotman.py:676
python.SystemOfUnits.second
int second
Definition: SystemOfUnits.py:120
ReadCellNoiseFromCoolCompare.s1
s1
Definition: ReadCellNoiseFromCoolCompare.py:378
D3PDMakerTestInstan::vec2
std::vector< D3PDTest::MyVec2 > vec2
Definition: D3PDMakerTestDict.h:14
checkxAOD.ds
ds
Definition: Tools/PyUtils/bin/checkxAOD.py:257
python.SystemOfUnits.s
int s
Definition: SystemOfUnits.py:131
inline_hints.h
python.SystemOfUnits.m
int m
Definition: SystemOfUnits.py:91
python.PerfMonSerializer.p
def p
Definition: PerfMonSerializer.py:743
Trk::RungeKuttaUtils::transformGlobalToCurvilinear
void transformGlobalToCurvilinear(bool, double *ATH_RESTRICT, double *ATH_RESTRICT, double *ATH_RESTRICT)
StraightLineSurface.h
fitman.ax
ax
Definition: fitman.py:522
PerigeeSurface.h
IDTPM::R
float R(const U &p)
Definition: TrackParametersHelper.h:101
DistanceSolution.h
Trk::PatternTrackParameters::associatedSurface
virtual const Surface & associatedSurface() const override final
Trk::DistanceSolution
Definition: DistanceSolution.h:25
JetTiledMap::W
@ W
Definition: TiledEtaPhiMap.h:44
Trk::ParametersBase::associatedSurface
virtual const Surface & associatedSurface() const override=0
Access to the Surface associated to the Parameters.
Monitored::Z
@ Z
Definition: HistogramFillerUtils.h:24
hist_file_dump.d
d
Definition: hist_file_dump.py:137
conifer::pow
constexpr int pow(int x)
Definition: conifer.h:20
DMTest::C
C_v1 C
Definition: C.h:26
MCP::ScaleSmearParam::s0
@ s0
dq_defect_virtual_defect_validation.d1
d1
Definition: dq_defect_virtual_defect_validation.py:79
Trk::SurfaceType
SurfaceType
Definition: SurfaceTypes.h:17
UploadAMITag.l
list l
Definition: UploadAMITag.larcaf.py:158
JetTiledMap::N
@ N
Definition: TiledEtaPhiMap.h:44
ReadCellNoiseFromCoolCompare.s4
s4
Definition: ReadCellNoiseFromCoolCompare.py:381
CxxUtils::vstore
ATH_ALWAYS_INLINE void vstore(vec_type_t< VEC > *dst, const VEC &src)
Definition: vec.h:290
const
bool const RAWDATA *ch2 const
Definition: LArRodBlockPhysicsV0.cxx:562
Trk::RungeKuttaUtils::transformLocalToGlobal
bool transformLocalToGlobal(bool, const Trk::TrackParameters &, double *ATH_RESTRICT)
x
#define x
JetTiledMap::S
@ S
Definition: TiledEtaPhiMap.h:44
Trk::RungeKuttaUtils::transformGlobalToLocal
void transformGlobalToLocal(double *ATH_RESTRICT, double *ATH_RESTRICT)
Monitored::X
@ X
Definition: HistogramFillerUtils.h:24
ATH_RESTRICT
#define ATH_RESTRICT
Definition: restrict.h:31
PlotCalibFromCool.ie
ie
Definition: PlotCalibFromCool.py:420
dqt_zlumi_alleff_HIST.A
A
Definition: dqt_zlumi_alleff_HIST.py:110
CxxUtils::vec
typename vecDetail::vec_typedef< T, N >::type vec
Define a nice alias for the vectorized type.
Definition: vec.h:207
ConeSurface.h
fillPileUpNoiseLumi.next
next
Definition: fillPileUpNoiseLumi.py:52
Trk::RungeKuttaUtils::transformCurvilinearToGlobal
void transformCurvilinearToGlobal(double *ATH_RESTRICT, double *ATH_RESTRICT)
lumiFormat.i
int i
Definition: lumiFormat.py:92
z
#define z
ATH_FLATTEN
#define ATH_FLATTEN
Definition: inline_hints.h:52
beamspotman.n
n
Definition: beamspotman.py:731
python.CaloCondTools.g
g
Definition: CaloCondTools.py:15
Trk::CylinderSurface
Definition: CylinderSurface.h:55
AmgVector
AmgVector(4) T2BSTrackFilterTool
Definition: T2BSTrackFilterTool.cxx:114
Amg::Transform3D
Eigen::Affine3d Transform3D
Definition: GeoPrimitives.h:46
CxxUtils
Definition: aligned_vector.h:29
res
std::pair< std::vector< unsigned int >, bool > res
Definition: JetGroupProductTest.cxx:14
CylinderSurface.h
TRT::Track::d0
@ d0
Definition: InnerDetector/InDetCalibEvent/TRT_CalibData/TRT_CalibData/TrackInfo.h:62
Trk::ParametersBase
Definition: ParametersBase.h:55
Trk::SurfaceType::Cone
@ Cone
RungeKuttaUtils
Definition: RungeKuttaUtils.h:30
beamspotman.dir
string dir
Definition: beamspotman.py:623
Monitored::Y
@ Y
Definition: HistogramFillerUtils.h:24
Trk
Ensure that the ATLAS eigen extensions are properly loaded.
Definition: FakeTrackBuilder.h:9
Trk::SurfaceType::Perigee
@ Perigee
ReadCellNoiseFromCoolCompare.s3
s3
Definition: ReadCellNoiseFromCoolCompare.py:380
CxxUtils::vload
ATH_ALWAYS_INLINE void vload(VEC &dst, vec_type_t< VEC > const *src)
Definition: vec.h:272
createCoolChannelIdFile.par
par
Definition: createCoolChannelIdFile.py:29
dqt_zlumi_alleff_HIST.B
B
Definition: dqt_zlumi_alleff_HIST.py:110
Amg::Vector3D
Eigen::Matrix< double, 3, 1 > Vector3D
Definition: GeoPrimitives.h:47
Trk::RungeKuttaUtils::fillDistancesMap
int fillDistancesMap(std::vector< std::pair< const Trk::Surface *, Trk::BoundaryCheck >> &, std::multimap< double, int > &, const double *ATH_RESTRICT, double, const Trk::Surface *, double *ATH_RESTRICT)
python.LumiBlobConversion.pos
pos
Definition: LumiBlobConversion.py:18
Trk::inside
@ inside
Definition: PropDirection.h:29
makeTRTBarrelCans.dy
tuple dy
Definition: makeTRTBarrelCans.py:21
DiTauMassTools::MaxHistStrategyV2::e
e
Definition: PhysicsAnalysis/TauID/DiTauMassTools/DiTauMassTools/HelperFunctions.h:26
a
TList * a
Definition: liststreamerinfos.cxx:10
y
#define y
vec.h
Vectorization helpers.
Trk::PatternTrackParameters
Definition: PatternTrackParameters.h:38
RungeKuttaUtils.h
PlaneSurface.h
dq_defect_virtual_defect_validation.d2
d2
Definition: dq_defect_virtual_defect_validation.py:81
ReadCellNoiseFromCoolCompare.s2
s2
Definition: ReadCellNoiseFromCoolCompare.py:379
makeTRTBarrelCans.dx
tuple dx
Definition: makeTRTBarrelCans.py:20
Trk::SurfaceType::Disc
@ Disc
Trk::RungeKuttaUtils::stepEstimator
double stepEstimator(int kind, double *ATH_RESTRICT Su, const double *ATH_RESTRICT P, bool &Q)
Definition: RungeKuttaUtils.cxx:1233
Trk::SurfaceType::Cylinder
@ Cylinder
CxxUtils::sincos
Helper to simultaneously calculate sin and cos of the same angle.
Definition: sincos.h:76
DiscSurface.h
Trk::ConeSurface
Definition: ConeSurface.h:51
PatternTrackParameters.h
Trk::SurfaceType::Plane
@ Plane
Trk::SurfaceType::Line
@ Line
Trk::Surface
Definition: Tracking/TrkDetDescr/TrkSurfaces/TrkSurfaces/Surface.h:75
grepfile.fr
fr
Definition: grepfile.py:32
Trk::Surface::transform
const Amg::Transform3D & transform() const
Returns HepGeom::Transform3D by reference.
Trk::Surface::type
constexpr virtual SurfaceType type() const =0
Returns the Surface type to avoid dynamic casts.
TSU::T
unsigned long long T
Definition: L1TopoDataTypes.h:35
fitman.k
k
Definition: fitman.py:528
AmgSymMatrix
ATH_FLATTEN AmgSymMatrix(5) Trk
Definition: RungeKuttaUtils.cxx:1412
fitman.ay
ay
Definition: fitman.py:525
Generated on Tue Jul 2 2024 21:25:54 for ATLAS Offline Software by   doxygen 1.8.18