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