CMS 3D CMS Logo

PropagationMPlex.cc
Go to the documentation of this file.
5 
6 #include "PropagationMPlex.h"
7 
8 //#define DEBUG
9 #include "Debug.h"
10 
11 //==============================================================================
12 // propagateLineToRMPlex
13 //==============================================================================
14 
15 //using namespace Matriplex;
16 
17 namespace mkfit {
18 
19  void propagateLineToRMPlex(const MPlexLS& psErr,
20  const MPlexLV& psPar,
21  const MPlexHS& msErr,
22  const MPlexHV& msPar,
23  MPlexLS& outErr,
24  MPlexLV& outPar,
25  const int N_proc) {
26  // XXX Regenerate parts below with a script.
27 
28  const Matriplex::idx_t N = NN;
29 
30 #pragma omp simd
31  for (int n = 0; n < NN; ++n) {
32  const float cosA = (psPar[0 * N + n] * psPar[3 * N + n] + psPar[1 * N + n] * psPar[4 * N + n]) /
33  (std::sqrt((psPar[0 * N + n] * psPar[0 * N + n] + psPar[1 * N + n] * psPar[1 * N + n]) *
34  (psPar[3 * N + n] * psPar[3 * N + n] + psPar[4 * N + n] * psPar[4 * N + n])));
35  const float dr = (hipo(msPar[0 * N + n], msPar[1 * N + n]) - hipo(psPar[0 * N + n], psPar[1 * N + n])) / cosA;
36 
37  dprint_np(n, "propagateLineToRMPlex dr=" << dr);
38 
39  const float pt = hipo(psPar[3 * N + n], psPar[4 * N + n]);
40  const float p = dr / pt; // path
41  const float psq = p * p;
42 
43  outPar[0 * N + n] = psPar[0 * N + n] + p * psPar[3 * N + n];
44  outPar[1 * N + n] = psPar[1 * N + n] + p * psPar[4 * N + n];
45  outPar[2 * N + n] = psPar[2 * N + n] + p * psPar[5 * N + n];
46  outPar[3 * N + n] = psPar[3 * N + n];
47  outPar[4 * N + n] = psPar[4 * N + n];
48  outPar[5 * N + n] = psPar[5 * N + n];
49 
50  {
51  const MPlexLS& A = psErr;
52  MPlexLS& B = outErr;
53 
54  B.fArray[0 * N + n] = A.fArray[0 * N + n];
55  B.fArray[1 * N + n] = A.fArray[1 * N + n];
56  B.fArray[2 * N + n] = A.fArray[2 * N + n];
57  B.fArray[3 * N + n] = A.fArray[3 * N + n];
58  B.fArray[4 * N + n] = A.fArray[4 * N + n];
59  B.fArray[5 * N + n] = A.fArray[5 * N + n];
60  B.fArray[6 * N + n] = A.fArray[6 * N + n] + p * A.fArray[0 * N + n];
61  B.fArray[7 * N + n] = A.fArray[7 * N + n] + p * A.fArray[1 * N + n];
62  B.fArray[8 * N + n] = A.fArray[8 * N + n] + p * A.fArray[3 * N + n];
63  B.fArray[9 * N + n] =
64  A.fArray[9 * N + n] + p * (A.fArray[6 * N + n] + A.fArray[6 * N + n]) + psq * A.fArray[0 * N + n];
65  B.fArray[10 * N + n] = A.fArray[10 * N + n] + p * A.fArray[1 * N + n];
66  B.fArray[11 * N + n] = A.fArray[11 * N + n] + p * A.fArray[2 * N + n];
67  B.fArray[12 * N + n] = A.fArray[12 * N + n] + p * A.fArray[4 * N + n];
68  B.fArray[13 * N + n] =
69  A.fArray[13 * N + n] + p * (A.fArray[7 * N + n] + A.fArray[10 * N + n]) + psq * A.fArray[1 * N + n];
70  B.fArray[14 * N + n] =
71  A.fArray[14 * N + n] + p * (A.fArray[11 * N + n] + A.fArray[11 * N + n]) + psq * A.fArray[2 * N + n];
72  B.fArray[15 * N + n] = A.fArray[15 * N + n] + p * A.fArray[3 * N + n];
73  B.fArray[16 * N + n] = A.fArray[16 * N + n] + p * A.fArray[4 * N + n];
74  B.fArray[17 * N + n] = A.fArray[17 * N + n] + p * A.fArray[5 * N + n];
75  B.fArray[18 * N + n] =
76  A.fArray[18 * N + n] + p * (A.fArray[8 * N + n] + A.fArray[15 * N + n]) + psq * A.fArray[3 * N + n];
77  B.fArray[19 * N + n] =
78  A.fArray[19 * N + n] + p * (A.fArray[12 * N + n] + A.fArray[16 * N + n]) + psq * A.fArray[4 * N + n];
79  B.fArray[20 * N + n] =
80  A.fArray[20 * N + n] + p * (A.fArray[17 * N + n] + A.fArray[17 * N + n]) + psq * A.fArray[5 * N + n];
81  }
82 
83  dprint_np(n, "propagateLineToRMPlex arrive at r=" << hipo(outPar[0 * N + n], outPar[1 * N + n]));
84  }
85  }
86 
87 } // end namespace mkfit
88 
89 //==============================================================================
90 // propagateHelixToRMPlex
91 //==============================================================================
92 
93 namespace {
94  using namespace mkfit;
95 
96  void MultHelixProp(const MPlexLL& A, const MPlexLS& B, MPlexLL& C) {
97  // C = A * B
98 
99  typedef float T;
100  const Matriplex::idx_t N = NN;
101 
102  const T* a = A.fArray;
103  ASSUME_ALIGNED(a, 64);
104  const T* b = B.fArray;
105  ASSUME_ALIGNED(b, 64);
106  T* c = C.fArray;
107  ASSUME_ALIGNED(c, 64);
108 
109 #include "MultHelixProp.ah"
110  }
111 
112  void MultHelixPropTransp(const MPlexLL& A, const MPlexLL& B, MPlexLS& C) {
113  // C = B * AT;
114 
115  typedef float T;
116  const Matriplex::idx_t N = NN;
117 
118  const T* a = A.fArray;
119  ASSUME_ALIGNED(a, 64);
120  const T* b = B.fArray;
121  ASSUME_ALIGNED(b, 64);
122  T* c = C.fArray;
123  ASSUME_ALIGNED(c, 64);
124 
125 #include "MultHelixPropTransp.ah"
126  }
127 
128  void MultHelixPropEndcap(const MPlexLL& A, const MPlexLS& B, MPlexLL& C) {
129  // C = A * B
130 
131  typedef float T;
132  const Matriplex::idx_t N = NN;
133 
134  const T* a = A.fArray;
135  ASSUME_ALIGNED(a, 64);
136  const T* b = B.fArray;
137  ASSUME_ALIGNED(b, 64);
138  T* c = C.fArray;
139  ASSUME_ALIGNED(c, 64);
140 
141 #include "MultHelixPropEndcap.ah"
142  }
143 
144  void MultHelixPropTranspEndcap(const MPlexLL& A, const MPlexLL& B, MPlexLS& C) {
145  // C = B * AT;
146 
147  typedef float T;
148  const Matriplex::idx_t N = NN;
149 
150  const T* a = A.fArray;
151  ASSUME_ALIGNED(a, 64);
152  const T* b = B.fArray;
153  ASSUME_ALIGNED(b, 64);
154  T* c = C.fArray;
155  ASSUME_ALIGNED(c, 64);
156 
157 #include "MultHelixPropTranspEndcap.ah"
158  }
159 
160  inline void MultHelixPropTemp(const MPlexLL& A, const MPlexLL& B, MPlexLL& C, int n) {
161  // C = A * B
162 
163  typedef float T;
164  const Matriplex::idx_t N = NN;
165 
166  const T* a = A.fArray;
167  ASSUME_ALIGNED(a, 64);
168  const T* b = B.fArray;
169  ASSUME_ALIGNED(b, 64);
170  T* c = C.fArray;
171  ASSUME_ALIGNED(c, 64);
172 
173  c[0 * N + n] = a[0 * N + n] * b[0 * N + n] + a[1 * N + n] * b[6 * N + n] + a[2 * N + n] * b[12 * N + n] +
174  a[4 * N + n] * b[24 * N + n];
175  c[1 * N + n] = a[0 * N + n] * b[1 * N + n] + a[1 * N + n] * b[7 * N + n] + a[2 * N + n] * b[13 * N + n] +
176  a[4 * N + n] * b[25 * N + n];
177  c[2 * N + n] = a[2 * N + n];
178  c[3 * N + n] = a[0 * N + n] * b[3 * N + n] + a[1 * N + n] * b[9 * N + n] + a[2 * N + n] * b[15 * N + n] +
179  a[3 * N + n] + a[4 * N + n] * b[27 * N + n];
180  c[4 * N + n] = a[0 * N + n] * b[4 * N + n] + a[1 * N + n] * b[10 * N + n] + a[4 * N + n];
181  c[5 * N + n] = a[2 * N + n] * b[17 * N + n] + a[5 * N + n];
182  c[6 * N + n] = a[6 * N + n] * b[0 * N + n] + a[7 * N + n] * b[6 * N + n] + a[8 * N + n] * b[12 * N + n] +
183  a[10 * N + n] * b[24 * N + n];
184  c[7 * N + n] = a[6 * N + n] * b[1 * N + n] + a[7 * N + n] * b[7 * N + n] + a[8 * N + n] * b[13 * N + n] +
185  a[10 * N + n] * b[25 * N + n];
186  c[8 * N + n] = a[8 * N + n];
187  c[9 * N + n] = a[6 * N + n] * b[3 * N + n] + a[7 * N + n] * b[9 * N + n] + a[8 * N + n] * b[15 * N + n] +
188  a[9 * N + n] + a[10 * N + n] * b[27 * N + n];
189  c[10 * N + n] = a[6 * N + n] * b[4 * N + n] + a[7 * N + n] * b[10 * N + n] + a[10 * N + n];
190  c[11 * N + n] = a[8 * N + n] * b[17 * N + n] + a[11 * N + n];
191  c[12 * N + n] = a[12 * N + n] * b[0 * N + n] + a[13 * N + n] * b[6 * N + n] + a[14 * N + n] * b[12 * N + n] +
192  a[16 * N + n] * b[24 * N + n];
193  c[13 * N + n] = a[12 * N + n] * b[1 * N + n] + a[13 * N + n] * b[7 * N + n] + a[14 * N + n] * b[13 * N + n] +
194  a[16 * N + n] * b[25 * N + n];
195  c[14 * N + n] = a[14 * N + n];
196  c[15 * N + n] = a[12 * N + n] * b[3 * N + n] + a[13 * N + n] * b[9 * N + n] + a[14 * N + n] * b[15 * N + n] +
197  a[15 * N + n] + a[16 * N + n] * b[27 * N + n];
198  c[16 * N + n] = a[12 * N + n] * b[4 * N + n] + a[13 * N + n] * b[10 * N + n] + a[16 * N + n];
199  c[17 * N + n] = a[14 * N + n] * b[17 * N + n] + a[17 * N + n];
200  c[18 * N + n] = a[18 * N + n] * b[0 * N + n] + a[19 * N + n] * b[6 * N + n] + a[20 * N + n] * b[12 * N + n] +
201  a[22 * N + n] * b[24 * N + n];
202  c[19 * N + n] = a[18 * N + n] * b[1 * N + n] + a[19 * N + n] * b[7 * N + n] + a[20 * N + n] * b[13 * N + n] +
203  a[22 * N + n] * b[25 * N + n];
204  c[20 * N + n] = a[20 * N + n];
205  c[21 * N + n] = a[18 * N + n] * b[3 * N + n] + a[19 * N + n] * b[9 * N + n] + a[20 * N + n] * b[15 * N + n] +
206  a[21 * N + n] + a[22 * N + n] * b[27 * N + n];
207  c[22 * N + n] = a[18 * N + n] * b[4 * N + n] + a[19 * N + n] * b[10 * N + n] + a[22 * N + n];
208  c[23 * N + n] = a[20 * N + n] * b[17 * N + n] + a[23 * N + n];
209  c[24 * N + n] = a[24 * N + n] * b[0 * N + n] + a[25 * N + n] * b[6 * N + n] + a[26 * N + n] * b[12 * N + n] +
210  a[28 * N + n] * b[24 * N + n];
211  c[25 * N + n] = a[24 * N + n] * b[1 * N + n] + a[25 * N + n] * b[7 * N + n] + a[26 * N + n] * b[13 * N + n] +
212  a[28 * N + n] * b[25 * N + n];
213  c[26 * N + n] = a[26 * N + n];
214  c[27 * N + n] = a[24 * N + n] * b[3 * N + n] + a[25 * N + n] * b[9 * N + n] + a[26 * N + n] * b[15 * N + n] +
215  a[27 * N + n] + a[28 * N + n] * b[27 * N + n];
216  c[28 * N + n] = a[24 * N + n] * b[4 * N + n] + a[25 * N + n] * b[10 * N + n] + a[28 * N + n];
217  c[29 * N + n] = a[26 * N + n] * b[17 * N + n] + a[29 * N + n];
218  c[30 * N + n] = a[30 * N + n] * b[0 * N + n] + a[31 * N + n] * b[6 * N + n] + a[32 * N + n] * b[12 * N + n] +
219  a[34 * N + n] * b[24 * N + n];
220  c[31 * N + n] = a[30 * N + n] * b[1 * N + n] + a[31 * N + n] * b[7 * N + n] + a[32 * N + n] * b[13 * N + n] +
221  a[34 * N + n] * b[25 * N + n];
222  c[32 * N + n] = a[32 * N + n];
223  c[33 * N + n] = a[30 * N + n] * b[3 * N + n] + a[31 * N + n] * b[9 * N + n] + a[32 * N + n] * b[15 * N + n] +
224  a[33 * N + n] + a[34 * N + n] * b[27 * N + n];
225  c[34 * N + n] = a[30 * N + n] * b[4 * N + n] + a[31 * N + n] * b[10 * N + n] + a[34 * N + n];
226  c[35 * N + n] = a[32 * N + n] * b[17 * N + n] + a[35 * N + n];
227  }
228 
229  // this version does not assume to know which elements are 0 or 1, so it does the full multiplication
230  void MultHelixPropFull(const MPlexLL& A, const MPlexLS& B, MPlexLL& C) {
231 #pragma omp simd
232  for (int n = 0; n < NN; ++n) {
233  for (int i = 0; i < 6; ++i) {
234  for (int j = 0; j < 6; ++j) {
235  C(n, i, j) = 0.;
236  for (int k = 0; k < 6; ++k)
237  C(n, i, j) += A.constAt(n, i, k) * B.constAt(n, k, j);
238  }
239  }
240  }
241  }
242 
243  // this version does not assume to know which elements are 0 or 1, so it does the full mupltiplication
244  void MultHelixPropTranspFull(const MPlexLL& A, const MPlexLL& B, MPlexLS& C) {
245 #pragma omp simd
246  for (int n = 0; n < NN; ++n) {
247  for (int i = 0; i < 6; ++i) {
248  for (int j = 0; j < 6; ++j) {
249  C(n, i, j) = 0.;
250  for (int k = 0; k < 6; ++k)
251  C(n, i, j) += B.constAt(n, i, k) * A.constAt(n, j, k);
252  }
253  }
254  }
255  }
256 
257 #ifdef UNUSED
258  // this version does not assume to know which elements are 0 or 1, so it does the full multiplication
259  void MultHelixPropFull(const MPlexLL& A, const MPlexLL& B, MPlexLL& C) {
260 #pragma omp simd
261  for (int n = 0; n < NN; ++n) {
262  for (int i = 0; i < 6; ++i) {
263  for (int j = 0; j < 6; ++j) {
264  C(n, i, j) = 0.;
265  for (int k = 0; k < 6; ++k)
266  C(n, i, j) += A.constAt(n, i, k) * B.constAt(n, k, j);
267  }
268  }
269  }
270  }
271 
272  // this version does not assume to know which elements are 0 or 1, so it does the full mupltiplication
273  void MultHelixPropTranspFull(const MPlexLL& A, const MPlexLL& B, MPlexLL& C) {
274 #pragma omp simd
275  for (int n = 0; n < NN; ++n) {
276  for (int i = 0; i < 6; ++i) {
277  for (int j = 0; j < 6; ++j) {
278  C(n, i, j) = 0.;
279  for (int k = 0; k < 6; ++k)
280  C(n, i, j) += B.constAt(n, i, k) * A.constAt(n, j, k);
281  }
282  }
283  }
284  }
285 #endif
286 } // end unnamed namespace
287 
288 //==============================================================================
289 
290 namespace mkfit {
291 
293  const MPlexQI& inChg,
294  const MPlexQF& msRad,
295  MPlexLV& outPar,
296  MPlexLL& errorProp,
297  const int N_proc) {
298  errorProp.setVal(0.f);
299  MPlexLL errorPropTmp(0.f); //initialize to zero
300  MPlexLL errorPropSwap(0.f); //initialize to zero
301 
302  // loop does not vectorize with llvm16, and it issues a warning
303  // that apparently can't be suppressed with a pragma. Needs to
304  // be rechecked if future llvm versions improve vectorization.
305 #if !defined(__clang__)
306 #pragma omp simd
307 #endif
308  for (int n = 0; n < NN; ++n) {
309  //initialize erroProp to identity matrix
310  errorProp(n, 0, 0) = 1.f;
311  errorProp(n, 1, 1) = 1.f;
312  errorProp(n, 2, 2) = 1.f;
313  errorProp(n, 3, 3) = 1.f;
314  errorProp(n, 4, 4) = 1.f;
315  errorProp(n, 5, 5) = 1.f;
316 
317  const float k = inChg.constAt(n, 0, 0) * 100.f / (-Const::sol * Config::Bfield);
318  const float r = msRad.constAt(n, 0, 0);
319  float r0 = hipo(inPar.constAt(n, 0, 0), inPar.constAt(n, 1, 0));
320 
321  if (std::abs(r - r0) < 0.0001f) {
322  dprint_np(n, "distance less than 1mum, skip");
323  continue;
324  }
325 
326  const float ipt = inPar.constAt(n, 3, 0);
327  const float phiin = inPar.constAt(n, 4, 0);
328  const float theta = inPar.constAt(n, 5, 0);
329 
330  //set those that are 1. before iterations
331  errorPropTmp(n, 2, 2) = 1.f;
332  errorPropTmp(n, 3, 3) = 1.f;
333  errorPropTmp(n, 4, 4) = 1.f;
334  errorPropTmp(n, 5, 5) = 1.f;
335 
336  float cosah = 0., sinah = 0.;
337  //no trig approx here, phi and theta can be large
338  float cosP = std::cos(phiin), sinP = std::sin(phiin);
339  const float cosT = std::cos(theta), sinT = std::sin(theta);
340  float pxin = cosP / ipt;
341  float pyin = sinP / ipt;
342 
344  for (int i = 0; i < Config::Niter; ++i) {
345  dprint_np(n,
346  std::endl
347  << "attempt propagation from r=" << r0 << " to r=" << r << std::endl
348  << "x=" << outPar.At(n, 0, 0) << " y=" << outPar.At(n, 1, 0) << " z=" << outPar.At(n, 2, 0)
349  << " px=" << std::cos(phiin) / ipt << " py=" << std::sin(phiin) / ipt
350  << " pz=" << 1.f / (ipt * tan(theta)) << " q=" << inChg.constAt(n, 0, 0) << std::endl);
351 
352  r0 = hipo(outPar.constAt(n, 0, 0), outPar.constAt(n, 1, 0));
353  const float ialpha = (r - r0) * ipt / k;
354  //alpha+=ialpha;
355 
356  if constexpr (Config::useTrigApprox) {
357  sincos4(ialpha * 0.5f, sinah, cosah);
358  } else {
359  cosah = std::cos(ialpha * 0.5f);
360  sinah = std::sin(ialpha * 0.5f);
361  }
362  const float cosa = 1.f - 2.f * sinah * sinah;
363  const float sina = 2.f * sinah * cosah;
364 
365  //derivatives of alpha
366  const float dadx = -outPar.At(n, 0, 0) * ipt / (k * r0);
367  const float dady = -outPar.At(n, 1, 0) * ipt / (k * r0);
368  const float dadipt = (r - r0) / k;
369 
370  outPar.At(n, 0, 0) = outPar.constAt(n, 0, 0) + 2.f * k * sinah * (pxin * cosah - pyin * sinah);
371  outPar.At(n, 1, 0) = outPar.constAt(n, 1, 0) + 2.f * k * sinah * (pyin * cosah + pxin * sinah);
372  const float pxinold = pxin; //copy before overwriting
373  pxin = pxin * cosa - pyin * sina;
374  pyin = pyin * cosa + pxinold * sina;
375 
376  //need phi at origin, so this goes before redefining phi
377  //no trig approx here, phi can be large
378  cosP = std::cos(outPar.At(n, 4, 0));
379  sinP = std::sin(outPar.At(n, 4, 0));
380 
381  outPar.At(n, 2, 0) = outPar.constAt(n, 2, 0) + k * ialpha * cosT / (ipt * sinT);
382  outPar.At(n, 3, 0) = ipt;
383  outPar.At(n, 4, 0) = outPar.constAt(n, 4, 0) + ialpha;
384  outPar.At(n, 5, 0) = theta;
385 
386  errorPropTmp(n, 0, 0) = 1.f + k * (cosP * dadx * cosa - sinP * dadx * sina) / ipt;
387  errorPropTmp(n, 0, 1) = k * (cosP * dady * cosa - sinP * dady * sina) / ipt;
388  errorPropTmp(n, 0, 3) =
389  k * (cosP * (ipt * dadipt * cosa - sina) + sinP * ((1.f - cosa) - ipt * dadipt * sina)) / (ipt * ipt);
390  errorPropTmp(n, 0, 4) = -k * (sinP * sina + cosP * (1.f - cosa)) / ipt;
391 
392  errorPropTmp(n, 1, 0) = k * (sinP * dadx * cosa + cosP * dadx * sina) / ipt;
393  errorPropTmp(n, 1, 1) = 1.f + k * (sinP * dady * cosa + cosP * dady * sina) / ipt;
394  errorPropTmp(n, 1, 3) =
395  k * (sinP * (ipt * dadipt * cosa - sina) + cosP * (ipt * dadipt * sina - (1.f - cosa))) / (ipt * ipt);
396  errorPropTmp(n, 1, 4) = k * (cosP * sina - sinP * (1.f - cosa)) / ipt;
397 
398  errorPropTmp(n, 2, 0) = k * cosT * dadx / (ipt * sinT);
399  errorPropTmp(n, 2, 1) = k * cosT * dady / (ipt * sinT);
400  errorPropTmp(n, 2, 3) = k * cosT * (ipt * dadipt - ialpha) / (ipt * ipt * sinT);
401  errorPropTmp(n, 2, 5) = -k * ialpha / (ipt * sinT * sinT);
402 
403  errorPropTmp(n, 4, 0) = dadx;
404  errorPropTmp(n, 4, 1) = dady;
405  errorPropTmp(n, 4, 3) = dadipt;
406 
407  MultHelixPropTemp(errorProp, errorPropTmp, errorPropSwap, n);
408  errorProp = errorPropSwap;
409  }
410 
411  dprint_np(
412  n,
413  "propagation end, dump parameters"
414  << std::endl
415  << "pos = " << outPar.At(n, 0, 0) << " " << outPar.At(n, 1, 0) << " " << outPar.At(n, 2, 0) << std::endl
416  << "mom = " << std::cos(outPar.At(n, 4, 0)) / outPar.At(n, 3, 0) << " "
417  << std::sin(outPar.At(n, 4, 0)) / outPar.At(n, 3, 0) << " "
418  << 1. / (outPar.At(n, 3, 0) * tan(outPar.At(n, 5, 0)))
419  << " r=" << std::sqrt(outPar.At(n, 0, 0) * outPar.At(n, 0, 0) + outPar.At(n, 1, 0) * outPar.At(n, 1, 0))
420  << " pT=" << 1. / std::abs(outPar.At(n, 3, 0)) << std::endl);
421 
422 #ifdef DEBUG
423  if (debug && g_debug && n < N_proc) {
424  dmutex_guard;
425  std::cout << n << " jacobian" << std::endl;
426  printf("%5f %5f %5f %5f %5f %5f\n",
427  errorProp(n, 0, 0),
428  errorProp(n, 0, 1),
429  errorProp(n, 0, 2),
430  errorProp(n, 0, 3),
431  errorProp(n, 0, 4),
432  errorProp(n, 0, 5));
433  printf("%5f %5f %5f %5f %5f %5f\n",
434  errorProp(n, 1, 0),
435  errorProp(n, 1, 1),
436  errorProp(n, 1, 2),
437  errorProp(n, 1, 3),
438  errorProp(n, 1, 4),
439  errorProp(n, 1, 5));
440  printf("%5f %5f %5f %5f %5f %5f\n",
441  errorProp(n, 2, 0),
442  errorProp(n, 2, 1),
443  errorProp(n, 2, 2),
444  errorProp(n, 2, 3),
445  errorProp(n, 2, 4),
446  errorProp(n, 2, 5));
447  printf("%5f %5f %5f %5f %5f %5f\n",
448  errorProp(n, 3, 0),
449  errorProp(n, 3, 1),
450  errorProp(n, 3, 2),
451  errorProp(n, 3, 3),
452  errorProp(n, 3, 4),
453  errorProp(n, 3, 5));
454  printf("%5f %5f %5f %5f %5f %5f\n",
455  errorProp(n, 4, 0),
456  errorProp(n, 4, 1),
457  errorProp(n, 4, 2),
458  errorProp(n, 4, 3),
459  errorProp(n, 4, 4),
460  errorProp(n, 4, 5));
461  printf("%5f %5f %5f %5f %5f %5f\n",
462  errorProp(n, 5, 0),
463  errorProp(n, 5, 1),
464  errorProp(n, 5, 2),
465  errorProp(n, 5, 3),
466  errorProp(n, 5, 4),
467  errorProp(n, 5, 5));
468  }
469 #endif
470  }
471  }
472 
473 } // end namespace mkfit
474 
475 //#pragma omp declare simd simdlen(NN) notinbranch linear(n)
476 #include "PropagationMPlex.icc"
477 
478 namespace mkfit {
479 
481  const MPlexQI& inChg,
482  const MPlexQF& msRad,
483  MPlexLV& outPar,
484  MPlexLL& errorProp,
485  MPlexQI& outFailFlag,
486  const int N_proc,
487  const PropagationFlags& pflags) {
488  errorProp.setVal(0.f);
489  outFailFlag.setVal(0.f);
490 
491  helixAtRFromIterativeCCS_impl(inPar, inChg, msRad, outPar, errorProp, outFailFlag, 0, NN, N_proc, pflags);
492  }
493 
494  void propagateHelixToRMPlex(const MPlexLS& inErr,
495  const MPlexLV& inPar,
496  const MPlexQI& inChg,
497  const MPlexQF& msRad,
498  MPlexLS& outErr,
499  MPlexLV& outPar,
500  MPlexQI& outFailFlag,
501  const int N_proc,
502  const PropagationFlags& pflags,
503  const MPlexQI* noMatEffPtr) {
504  // bool debug = true;
505 
506  // This is used further down when calculating similarity with errorProp (and before in DEBUG).
507  // MT: I don't think this really needed if we use inErr where required.
508  outErr = inErr;
509  // This requirement for helixAtRFromIterativeCCS_impl() and for helixAtRFromIterativeCCSFullJac().
510  // MT: This should be properly handled in both functions (expecting input in output parameters sucks).
511  outPar = inPar;
512 
513  MPlexLL errorProp;
514 
515  helixAtRFromIterativeCCS(inPar, inChg, msRad, outPar, errorProp, outFailFlag, N_proc, pflags);
516 
517 #ifdef DEBUG
518  if (debug && g_debug) {
519  for (int kk = 0; kk < N_proc; ++kk) {
520  dprintf("outErr before prop %d\n", kk);
521  for (int i = 0; i < 6; ++i) {
522  for (int j = 0; j < 6; ++j)
523  dprintf("%8f ", outErr.At(kk, i, j));
524  dprintf("\n");
525  }
526  dprintf("\n");
527 
528  dprintf("errorProp %d\n", kk);
529  for (int i = 0; i < 6; ++i) {
530  for (int j = 0; j < 6; ++j)
531  dprintf("%8f ", errorProp.At(kk, i, j));
532  dprintf("\n");
533  }
534  dprintf("\n");
535  }
536  }
537 #endif
538 
539  // MultHelixProp can be optimized for CCS coordinates, see GenMPlexOps.pl
540  MPlexLL temp;
541  MultHelixProp(errorProp, outErr, temp);
542  MultHelixPropTransp(errorProp, temp, outErr);
543  // can replace with: MultHelixPropFull(errorProp, outErr, temp); MultHelixPropTranspFull(errorProp, temp, outErr);
544 
545  if (pflags.apply_material) {
546  MPlexQF hitsRl;
547  MPlexQF hitsXi;
548  MPlexQF propSign;
549 
550  const TrackerInfo& tinfo = *pflags.tracker_info;
551 
552 #pragma omp simd
553  for (int n = 0; n < NN; ++n) {
554  if (n >= N_proc || (outFailFlag(n, 0, 0) || (noMatEffPtr && noMatEffPtr->constAt(n, 0, 0)))) {
555  hitsRl(n, 0, 0) = 0.f;
556  hitsXi(n, 0, 0) = 0.f;
557  } else {
558  auto mat = tinfo.material_checked(std::abs(outPar(n, 2, 0)), msRad(n, 0, 0));
559  hitsRl(n, 0, 0) = mat.radl;
560  hitsXi(n, 0, 0) = mat.bbxi;
561  }
562  const float r0 = hipo(inPar(n, 0, 0), inPar(n, 1, 0));
563  const float r = msRad(n, 0, 0);
564  propSign(n, 0, 0) = (r > r0 ? 1. : -1.);
565  }
566  MPlexHV plNrm;
567 #pragma omp simd
568  for (int n = 0; n < NN; ++n) {
569  plNrm(n, 0, 0) = std::cos(outPar.constAt(n, 4, 0));
570  plNrm(n, 1, 0) = std::sin(outPar.constAt(n, 4, 0));
571  plNrm(n, 2, 0) = 0.f;
572  }
573  applyMaterialEffects(hitsRl, hitsXi, propSign, plNrm, outErr, outPar, N_proc);
574  }
575 
576  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
577 
578  // Matriplex version of:
579  // result.errors = ROOT::Math::Similarity(errorProp, outErr);
580 
581  /*
582  // To be used with: MPT_DIM = 1
583  if (fabs(sqrt(outPar[0]*outPar[0]+outPar[1]*outPar[1]) - msRad[0]) > 0.0001)
584  {
585  std::cout << "DID NOT GET TO R, FailFlag=" << failFlag[0]
586  << " dR=" << msRad[0] - std::hypot(outPar[0],outPar[1])
587  << " r=" << msRad[0] << " rin=" << std::hypot(inPar[0],inPar[1]) << " rout=" << std::hypot(outPar[0],outPar[1])
588  << std::endl;
589  // std::cout << " pt=" << pt << " pz=" << inPar.At(n, 2) << std::endl;
590  }
591  */
592 
593  // PROP-FAIL-ENABLE To keep physics changes minimal, we always restore the
594  // state to input when propagation fails -- as was the default before.
595  // if (pflags.copy_input_state_on_fail) {
596  for (int i = 0; i < N_proc; ++i) {
597  if (outFailFlag(i, 0, 0)) {
598  outPar.copySlot(i, inPar);
599  outErr.copySlot(i, inErr);
600  }
601  }
602  // }
603  }
604 
605  //==============================================================================
606 
607  void propagateHelixToZMPlex(const MPlexLS& inErr,
608  const MPlexLV& inPar,
609  const MPlexQI& inChg,
610  const MPlexQF& msZ,
611  MPlexLS& outErr,
612  MPlexLV& outPar,
613  MPlexQI& outFailFlag,
614  const int N_proc,
615  const PropagationFlags& pflags,
616  const MPlexQI* noMatEffPtr) {
617  // debug = true;
618 
619  outErr = inErr;
620  outPar = inPar;
621 
622  MPlexLL errorProp;
623 
624  //helixAtZ_new(inPar, inChg, msZ, outPar, errorProp, outFailFlag, N_proc, pflags);
625  helixAtZ(inPar, inChg, msZ, outPar, errorProp, outFailFlag, N_proc, pflags);
626 
627 #ifdef DEBUG
628  if (debug && g_debug) {
629  for (int kk = 0; kk < N_proc; ++kk) {
630  dprintf("inPar %d\n", kk);
631  for (int i = 0; i < 6; ++i) {
632  dprintf("%8f ", inPar.constAt(kk, i, 0));
633  }
634  dprintf("\n");
635 
636  dprintf("inErr %d\n", kk);
637  for (int i = 0; i < 6; ++i) {
638  for (int j = 0; j < 6; ++j)
639  dprintf("%8f ", inErr.constAt(kk, i, j));
640  dprintf("\n");
641  }
642  dprintf("\n");
643 
644  dprintf("errorProp %d\n", kk);
645  for (int i = 0; i < 6; ++i) {
646  for (int j = 0; j < 6; ++j)
647  dprintf("%8f ", errorProp.At(kk, i, j));
648  dprintf("\n");
649  }
650  dprintf("\n");
651  }
652  }
653 #endif
654 
655 #ifdef DEBUG
656  if (debug && g_debug) {
657  for (int kk = 0; kk < N_proc; ++kk) {
658  dprintf("outErr %d\n", kk);
659  for (int i = 0; i < 6; ++i) {
660  for (int j = 0; j < 6; ++j)
661  dprintf("%8f ", outErr.constAt(kk, i, j));
662  dprintf("\n");
663  }
664  dprintf("\n");
665  }
666  }
667 #endif
668 
669  // Matriplex version of: result.errors = ROOT::Math::Similarity(errorProp, outErr);
670  MPlexLL temp;
671  MultHelixPropEndcap(errorProp, outErr, temp);
672  MultHelixPropTranspEndcap(errorProp, temp, outErr);
673  // can replace with: MultHelixPropFull(errorProp, outErr, temp); MultHelixPropTranspFull(errorProp, temp, outErr);
674 
675  if (pflags.apply_material) {
676  MPlexQF hitsRl;
677  MPlexQF hitsXi;
678  MPlexQF propSign;
679 
680  const TrackerInfo& tinfo = *pflags.tracker_info;
681 
682 #pragma omp simd
683  for (int n = 0; n < NN; ++n) {
684  if (n >= N_proc || (noMatEffPtr && noMatEffPtr->constAt(n, 0, 0))) {
685  hitsRl(n, 0, 0) = 0.f;
686  hitsXi(n, 0, 0) = 0.f;
687  } else {
688  const float hypo = std::hypot(outPar(n, 0, 0), outPar(n, 1, 0));
689  auto mat = tinfo.material_checked(std::abs(msZ(n, 0, 0)), hypo);
690  hitsRl(n, 0, 0) = mat.radl;
691  hitsXi(n, 0, 0) = mat.bbxi;
692  }
693  const float zout = msZ.constAt(n, 0, 0);
694  const float zin = inPar.constAt(n, 2, 0);
695  propSign(n, 0, 0) = (std::abs(zout) > std::abs(zin) ? 1.f : -1.f);
696  }
697  MPlexHV plNrm;
698 #pragma omp simd
699  for (int n = 0; n < NN; ++n) {
700  plNrm(n, 0, 0) = 0.f;
701  plNrm(n, 1, 0) = 0.f;
702  plNrm(n, 2, 0) = 1.f;
703  }
704  applyMaterialEffects(hitsRl, hitsXi, propSign, plNrm, outErr, outPar, N_proc);
705 #ifdef DEBUG
706  if (debug && g_debug) {
707  for (int kk = 0; kk < N_proc; ++kk) {
708  dprintf("propSign %d\n", kk);
709  for (int i = 0; i < 1; ++i) {
710  dprintf("%8f ", propSign.constAt(kk, i, 0));
711  }
712  dprintf("\n");
713  dprintf("plNrm %d\n", kk);
714  for (int i = 0; i < 3; ++i) {
715  dprintf("%8f ", plNrm.constAt(kk, i, 0));
716  }
717  dprintf("\n");
718  dprintf("outErr(after material) %d\n", kk);
719  for (int i = 0; i < 6; ++i) {
720  for (int j = 0; j < 6; ++j)
721  dprintf("%8f ", outErr.constAt(kk, i, j));
722  dprintf("\n");
723  }
724  dprintf("\n");
725  }
726  }
727 #endif
728  }
729 
730  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
731 
732  // PROP-FAIL-ENABLE To keep physics changes minimal, we always restore the
733  // state to input when propagation fails -- as was the default before.
734  // if (pflags.copy_input_state_on_fail) {
735  for (int i = 0; i < N_proc; ++i) {
736  if (outFailFlag(i, 0, 0)) {
737  outPar.copySlot(i, inPar);
738  outErr.copySlot(i, inErr);
739  }
740  }
741  // }
742  }
743 
744  void helixAtZ(const MPlexLV& inPar,
745  const MPlexQI& inChg,
746  const MPlexQF& msZ,
747  MPlexLV& outPar,
748  MPlexLL& errorProp,
749  MPlexQI& outFailFlag,
750  const int N_proc,
751  const PropagationFlags& pflags) {
752  errorProp.setVal(0.f);
753  outFailFlag.setVal(0.f);
754 
755  // debug = true;
756 #pragma omp simd
757  for (int n = 0; n < NN; ++n) {
758  //initialize erroProp to identity matrix, except element 2,2 which is zero
759  errorProp(n, 0, 0) = 1.f;
760  errorProp(n, 1, 1) = 1.f;
761  errorProp(n, 3, 3) = 1.f;
762  errorProp(n, 4, 4) = 1.f;
763  errorProp(n, 5, 5) = 1.f;
764  }
765  float zout[NN];
766  float zin[NN];
767  float ipt[NN];
768  float phiin[NN];
769  float theta[NN];
770 #pragma omp simd
771  for (int n = 0; n < NN; ++n) {
772  //initialize erroProp to identity matrix, except element 2,2 which is zero
773  zout[n] = msZ.constAt(n, 0, 0);
774  zin[n] = inPar.constAt(n, 2, 0);
775  ipt[n] = inPar.constAt(n, 3, 0);
776  phiin[n] = inPar.constAt(n, 4, 0);
777  theta[n] = inPar.constAt(n, 5, 0);
778  }
779 
780  float k[NN];
781  if (pflags.use_param_b_field) {
782 #pragma omp simd
783  for (int n = 0; n < NN; ++n) {
784  k[n] = inChg.constAt(n, 0, 0) * 100.f /
785  (-Const::sol * Config::bFieldFromZR(zin[n], hipo(inPar.constAt(n, 0, 0), inPar.constAt(n, 1, 0))));
786  }
787  } else {
788 #pragma omp simd
789  for (int n = 0; n < NN; ++n) {
790  k[n] = inChg.constAt(n, 0, 0) * 100.f / (-Const::sol * Config::Bfield);
791  }
792  }
793 
794  float kinv[NN];
795 #pragma omp simd
796  for (int n = 0; n < NN; ++n) {
797  kinv[n] = 1.f / k[n];
798  }
799 
800 #pragma omp simd
801  for (int n = 0; n < NN; ++n) {
802  dprint_np(n,
803  std::endl
804  << "input parameters"
805  << " inPar.constAt(n, 0, 0)=" << std::setprecision(9) << inPar.constAt(n, 0, 0)
806  << " inPar.constAt(n, 1, 0)=" << std::setprecision(9) << inPar.constAt(n, 1, 0)
807  << " inPar.constAt(n, 2, 0)=" << std::setprecision(9) << inPar.constAt(n, 2, 0)
808  << " inPar.constAt(n, 3, 0)=" << std::setprecision(9) << inPar.constAt(n, 3, 0)
809  << " inPar.constAt(n, 4, 0)=" << std::setprecision(9) << inPar.constAt(n, 4, 0)
810  << " inPar.constAt(n, 5, 0)=" << std::setprecision(9) << inPar.constAt(n, 5, 0)
811  << " inChg.constAt(n, 0, 0)=" << std::setprecision(9) << inChg.constAt(n, 0, 0));
812  }
813 #pragma omp simd
814  for (int n = 0; n < NN; ++n) {
815  dprint_np(n,
816  "propagation start, dump parameters"
817  << std::endl
818  << "pos = " << inPar.constAt(n, 0, 0) << " " << inPar.constAt(n, 1, 0) << " "
819  << inPar.constAt(n, 2, 0) << std::endl
820  << "mom (cart) = " << std::cos(inPar.constAt(n, 4, 0)) / inPar.constAt(n, 3, 0) << " "
821  << std::sin(inPar.constAt(n, 4, 0)) / inPar.constAt(n, 3, 0) << " "
822  << 1. / (inPar.constAt(n, 3, 0) * tan(inPar.constAt(n, 5, 0))) << " r="
823  << std::sqrt(inPar.constAt(n, 0, 0) * inPar.constAt(n, 0, 0) +
824  inPar.constAt(n, 1, 0) * inPar.constAt(n, 1, 0))
825  << " pT=" << 1. / std::abs(inPar.constAt(n, 3, 0)) << " q=" << inChg.constAt(n, 0, 0)
826  << " targetZ=" << msZ.constAt(n, 0, 0) << std::endl);
827  }
828 
829  float pt[NN];
830 #pragma omp simd
831  for (int n = 0; n < NN; ++n) {
832  pt[n] = 1.f / ipt[n];
833  }
834 
835  //no trig approx here, phi can be large
836  float cosP[NN];
837  float sinP[NN];
838 #pragma omp simd
839  for (int n = 0; n < NN; ++n) {
840  cosP[n] = std::cos(phiin[n]);
841  }
842 
843 #pragma omp simd
844  for (int n = 0; n < NN; ++n) {
845  sinP[n] = std::sin(phiin[n]);
846  }
847 
848  float cosT[NN];
849  float sinT[NN];
850 #pragma omp simd
851  for (int n = 0; n < NN; ++n) {
852  cosT[n] = std::cos(theta[n]);
853  }
854 
855 #pragma omp simd
856  for (int n = 0; n < NN; ++n) {
857  sinT[n] = std::sin(theta[n]);
858  }
859 
860  float tanT[NN];
861  float icos2T[NN];
862  float pxin[NN];
863  float pyin[NN];
864 #pragma omp simd
865  for (int n = 0; n < NN; ++n) {
866  tanT[n] = sinT[n] / cosT[n];
867  icos2T[n] = 1.f / (cosT[n] * cosT[n]);
868  pxin[n] = cosP[n] * pt[n];
869  pyin[n] = sinP[n] * pt[n];
870  }
871 
872  float deltaZ[NN];
873  float alpha[NN];
874 #pragma omp simd
875  for (int n = 0; n < NN; ++n) {
876  deltaZ[n] = zout[n] - zin[n];
877  alpha[n] = deltaZ[n] * tanT[n] * ipt[n] * kinv[n];
878  }
879 
880  float cosahTmp[NN];
881  float sinahTmp[NN];
882  if constexpr (Config::useTrigApprox) {
883 #if !defined(__INTEL_COMPILER)
884 #pragma omp simd
885 #endif
886  for (int n = 0; n < NN; ++n) {
887  sincos4(alpha[n] * 0.5f, sinahTmp[n], cosahTmp[n]);
888  }
889  } else {
890 #if !defined(__INTEL_COMPILER)
891 #pragma omp simd
892 #endif
893  for (int n = 0; n < NN; ++n) {
894  cosahTmp[n] = std::cos(alpha[n] * 0.5f);
895  }
896 #if !defined(__INTEL_COMPILER)
897 #pragma omp simd
898 #endif
899  for (int n = 0; n < NN; ++n) {
900  sinahTmp[n] = std::sin(alpha[n] * 0.5f);
901  }
902  }
903 
904  float cosah[NN];
905  float sinah[NN];
906  float cosa[NN];
907  float sina[NN];
908 #pragma omp simd
909  for (int n = 0; n < NN; ++n) {
910  cosah[n] = cosahTmp[n];
911  sinah[n] = sinahTmp[n];
912  cosa[n] = 1.f - 2.f * sinah[n] * sinah[n];
913  sina[n] = 2.f * sinah[n] * cosah[n];
914  }
915 
916 //update parameters
917 #pragma omp simd
918  for (int n = 0; n < NN; ++n) {
919  outPar.At(n, 0, 0) = outPar.At(n, 0, 0) + 2.f * k[n] * sinah[n] * (pxin[n] * cosah[n] - pyin[n] * sinah[n]);
920  outPar.At(n, 1, 0) = outPar.At(n, 1, 0) + 2.f * k[n] * sinah[n] * (pyin[n] * cosah[n] + pxin[n] * sinah[n]);
921  outPar.At(n, 2, 0) = zout[n];
922  outPar.At(n, 4, 0) = phiin[n] + alpha[n];
923  }
924 
925 #pragma omp simd
926  for (int n = 0; n < NN; ++n) {
927  dprint_np(n,
928  "propagation to Z end (OLD), dump parameters\n"
929  << " pos = " << outPar(n, 0, 0) << " " << outPar(n, 1, 0) << " " << outPar(n, 2, 0) << "\t\t r="
930  << std::sqrt(outPar(n, 0, 0) * outPar(n, 0, 0) + outPar(n, 1, 0) * outPar(n, 1, 0)) << std::endl
931  << " mom = " << outPar(n, 3, 0) << " " << outPar(n, 4, 0) << " " << outPar(n, 5, 0) << std::endl
932  << " cart= " << std::cos(outPar(n, 4, 0)) / outPar(n, 3, 0) << " "
933  << std::sin(outPar(n, 4, 0)) / outPar(n, 3, 0) << " "
934  << 1. / (outPar(n, 3, 0) * tan(outPar(n, 5, 0))) << "\t\tpT=" << 1. / std::abs(outPar(n, 3, 0))
935  << std::endl);
936  }
937 
938  float pxcaMpysa[NN];
939 #pragma omp simd
940  for (int n = 0; n < NN; ++n) {
941  pxcaMpysa[n] = pxin[n] * cosa[n] - pyin[n] * sina[n];
942  }
943 
944 #pragma omp simd
945  for (int n = 0; n < NN; ++n) {
946  errorProp(n, 0, 2) = -tanT[n] * ipt[n] * pxcaMpysa[n];
947  errorProp(n, 0, 3) =
948  k[n] * pt[n] * pt[n] *
949  (cosP[n] * (alpha[n] * cosa[n] - sina[n]) + sinP[n] * 2.f * sinah[n] * (sinah[n] - alpha[n] * cosah[n]));
950  errorProp(n, 0, 4) = -2.f * k[n] * pt[n] * sinah[n] * (sinP[n] * cosah[n] + cosP[n] * sinah[n]);
951  errorProp(n, 0, 5) = deltaZ[n] * ipt[n] * pxcaMpysa[n] * icos2T[n];
952  }
953 
954  float pycaPpxsa[NN];
955 #pragma omp simd
956  for (int n = 0; n < NN; ++n) {
957  pycaPpxsa[n] = pyin[n] * cosa[n] + pxin[n] * sina[n];
958  }
959 
960 #pragma omp simd
961  for (int n = 0; n < NN; ++n) {
962  errorProp(n, 1, 2) = -tanT[n] * ipt[n] * pycaPpxsa[n];
963  errorProp(n, 1, 3) =
964  k[n] * pt[n] * pt[n] *
965  (sinP[n] * (alpha[n] * cosa[n] - sina[n]) - cosP[n] * 2.f * sinah[n] * (sinah[n] - alpha[n] * cosah[n]));
966  errorProp(n, 1, 4) = 2.f * k[n] * pt[n] * sinah[n] * (cosP[n] * cosah[n] - sinP[n] * sinah[n]);
967  errorProp(n, 1, 5) = deltaZ[n] * ipt[n] * pycaPpxsa[n] * icos2T[n];
968  }
969 
970 #pragma omp simd
971  for (int n = 0; n < NN; ++n) {
972  errorProp(n, 4, 2) = -ipt[n] * tanT[n] * kinv[n];
973  errorProp(n, 4, 3) = tanT[n] * deltaZ[n] * kinv[n];
974  errorProp(n, 4, 5) = ipt[n] * deltaZ[n] * kinv[n] * icos2T[n];
975  }
976 
977 #pragma omp simd
978  for (int n = 0; n < NN; ++n) {
979  dprint_np(
980  n,
981  "propagation end, dump parameters"
982  << std::endl
983  << "pos = " << outPar.At(n, 0, 0) << " " << outPar.At(n, 1, 0) << " " << outPar.At(n, 2, 0) << std::endl
984  << "mom (cart) = " << std::cos(outPar.At(n, 4, 0)) / outPar.At(n, 3, 0) << " "
985  << std::sin(outPar.At(n, 4, 0)) / outPar.At(n, 3, 0) << " "
986  << 1. / (outPar.At(n, 3, 0) * tan(outPar.At(n, 5, 0)))
987  << " r=" << std::sqrt(outPar.At(n, 0, 0) * outPar.At(n, 0, 0) + outPar.At(n, 1, 0) * outPar.At(n, 1, 0))
988  << " pT=" << 1. / std::abs(outPar.At(n, 3, 0)) << std::endl);
989  }
990 
991  // PROP-FAIL-ENABLE Disabled to keep physics changes minimal.
992  // To be reviewed, enabled and processed accordingly elsewhere.
993  /*
994  // Check for errors, set fail-flag.
995  for (int n = 0; n < NN; ++n) {
996  // We propagate for alpha: mark fail when prop angle more than pi/2
997  if (std::abs(alpha[n]) > 1.57) {
998  dprintf("helixAtZ: more than quarter turn, alpha = %f\n", alpha[n]);
999  outFailFlag[n] = 1;
1000  } else {
1001  // Have we reached desired z? We can't know, we copy desired z to actual z.
1002  // Are we close to apex? Same condition as in propToR, 12.5 deg, cos(78.5deg) = 0.2
1003  float dotp = (outPar.At(n, 0, 0) * std::cos(outPar.At(n, 4, 0)) +
1004  outPar.At(n, 1, 0) * std::sin(outPar.At(n, 4, 0))) /
1005  std::hypot(outPar.At(n, 0, 0), outPar.At(n, 1, 0));
1006  if (dotp < 0.2 || dotp < 0) {
1007  dprintf("helixAtZ: dot product bad, dotp = %f\n", dotp);
1008  outFailFlag[n] = 1;
1009  }
1010  }
1011  }
1012  */
1013 
1014 #ifdef DEBUG
1015  if (debug && g_debug) {
1016  for (int n = 0; n < N_proc; ++n) {
1017  dmutex_guard;
1018  std::cout << n << ": jacobian" << std::endl;
1019  printf("%5f %5f %5f %5f %5f %5f\n",
1020  errorProp(n, 0, 0),
1021  errorProp(n, 0, 1),
1022  errorProp(n, 0, 2),
1023  errorProp(n, 0, 3),
1024  errorProp(n, 0, 4),
1025  errorProp(n, 0, 5));
1026  printf("%5f %5f %5f %5f %5f %5f\n",
1027  errorProp(n, 1, 0),
1028  errorProp(n, 1, 1),
1029  errorProp(n, 1, 2),
1030  errorProp(n, 1, 3),
1031  errorProp(n, 1, 4),
1032  errorProp(n, 1, 5));
1033  printf("%5f %5f %5f %5f %5f %5f\n",
1034  errorProp(n, 2, 0),
1035  errorProp(n, 2, 1),
1036  errorProp(n, 2, 2),
1037  errorProp(n, 2, 3),
1038  errorProp(n, 2, 4),
1039  errorProp(n, 2, 5));
1040  printf("%5f %5f %5f %5f %5f %5f\n",
1041  errorProp(n, 3, 0),
1042  errorProp(n, 3, 1),
1043  errorProp(n, 3, 2),
1044  errorProp(n, 3, 3),
1045  errorProp(n, 3, 4),
1046  errorProp(n, 3, 5));
1047  printf("%5f %5f %5f %5f %5f %5f\n",
1048  errorProp(n, 4, 0),
1049  errorProp(n, 4, 1),
1050  errorProp(n, 4, 2),
1051  errorProp(n, 4, 3),
1052  errorProp(n, 4, 4),
1053  errorProp(n, 4, 5));
1054  printf("%5f %5f %5f %5f %5f %5f\n",
1055  errorProp(n, 5, 0),
1056  errorProp(n, 5, 1),
1057  errorProp(n, 5, 2),
1058  errorProp(n, 5, 3),
1059  errorProp(n, 5, 4),
1060  errorProp(n, 5, 5));
1061  }
1062  }
1063 #endif
1064  }
1065 
1066  void helixAtPlane(const MPlexLV& inPar,
1067  const MPlexQI& inChg,
1068  const MPlexHV& plPnt,
1069  const MPlexHV& plNrm,
1070  MPlexQF& pathL,
1071  MPlexLV& outPar,
1072  MPlexLL& errorProp,
1073  MPlexQI& outFailFlag,
1074  const int N_proc,
1075  const PropagationFlags& pflags) {
1076  errorProp.setVal(0.f);
1077  outFailFlag.setVal(0.f);
1078 
1079  helixAtPlane_impl(inPar, inChg, plPnt, plNrm, pathL, outPar, errorProp, outFailFlag, 0, NN, N_proc, pflags);
1080  }
1081 
1083  const MPlexLV& inPar,
1084  const MPlexQI& inChg,
1085  const MPlexHV& plPnt,
1086  const MPlexHV& plNrm,
1087  MPlexLS& outErr,
1088  MPlexLV& outPar,
1089  MPlexQI& outFailFlag,
1090  const int N_proc,
1091  const PropagationFlags& pflags,
1092  const MPlexQI* noMatEffPtr) {
1093  // debug = true;
1094 
1095  outErr = inErr;
1096  outPar = inPar;
1097 
1098  MPlexQF pathL;
1099  MPlexLL errorProp;
1100 
1101  helixAtPlane(inPar, inChg, plPnt, plNrm, pathL, outPar, errorProp, outFailFlag, N_proc, pflags);
1102 
1103  for (int n = 0; n < NN; ++n) {
1104  dprint_np(
1105  n,
1106  "propagation to plane end, dump parameters\n"
1107  //<< " D = " << s[n] << " alpha = " << s[n] * std::sin(inPar(n, 5, 0)) * inPar(n, 3, 0) * kinv[n] << " kinv = " << kinv[n] << std::endl
1108  << " pos = " << outPar(n, 0, 0) << " " << outPar(n, 1, 0) << " " << outPar(n, 2, 0) << "\t\t r="
1109  << std::sqrt(outPar(n, 0, 0) * outPar(n, 0, 0) + outPar(n, 1, 0) * outPar(n, 1, 0)) << std::endl
1110  << " mom = " << outPar(n, 3, 0) << " " << outPar(n, 4, 0) << " " << outPar(n, 5, 0) << std::endl
1111  << " cart= " << std::cos(outPar(n, 4, 0)) / outPar(n, 3, 0) << " "
1112  << std::sin(outPar(n, 4, 0)) / outPar(n, 3, 0) << " " << 1. / (outPar(n, 3, 0) * tan(outPar(n, 5, 0)))
1113  << "\t\tpT=" << 1. / std::abs(outPar(n, 3, 0)) << std::endl);
1114  }
1115 
1116 #ifdef DEBUG
1117  if (debug && g_debug) {
1118  for (int kk = 0; kk < N_proc; ++kk) {
1119  dprintf("inPar %d\n", kk);
1120  for (int i = 0; i < 6; ++i) {
1121  dprintf("%8f ", inPar.constAt(kk, i, 0));
1122  }
1123  dprintf("\n");
1124  dprintf("inErr %d\n", kk);
1125  for (int i = 0; i < 6; ++i) {
1126  for (int j = 0; j < 6; ++j)
1127  dprintf("%8f ", inErr.constAt(kk, i, j));
1128  dprintf("\n");
1129  }
1130  dprintf("\n");
1131 
1132  for (int kk = 0; kk < N_proc; ++kk) {
1133  dprintf("plNrm %d\n", kk);
1134  for (int j = 0; j < 3; ++j)
1135  dprintf("%8f ", plNrm.constAt(kk, 0, j));
1136  }
1137  dprintf("\n");
1138 
1139  for (int kk = 0; kk < N_proc; ++kk) {
1140  dprintf("pathL %d\n", kk);
1141  for (int j = 0; j < 1; ++j)
1142  dprintf("%8f ", pathL.constAt(kk, 0, j));
1143  }
1144  dprintf("\n");
1145 
1146  dprintf("errorProp %d\n", kk);
1147  for (int i = 0; i < 6; ++i) {
1148  for (int j = 0; j < 6; ++j)
1149  dprintf("%8f ", errorProp.At(kk, i, j));
1150  dprintf("\n");
1151  }
1152  dprintf("\n");
1153  }
1154  }
1155 #endif
1156 
1157  // Matriplex version of:
1158  // result.errors = ROOT::Math::Similarity(errorProp, outErr);
1159  MPlexLL temp;
1160  MultHelixPropFull(errorProp, outErr, temp);
1161  MultHelixPropTranspFull(errorProp, temp, outErr);
1162 
1163 #ifdef DEBUG
1164  if (debug && g_debug) {
1165  for (int kk = 0; kk < N_proc; ++kk) {
1166  dprintf("outErr %d\n", kk);
1167  for (int i = 0; i < 6; ++i) {
1168  for (int j = 0; j < 6; ++j)
1169  dprintf("%8f ", outErr.constAt(kk, i, j));
1170  dprintf("\n");
1171  }
1172  dprintf("\n");
1173  }
1174  }
1175 #endif
1176 
1177  if (pflags.apply_material) {
1178  MPlexQF hitsRl;
1179  MPlexQF hitsXi;
1180  MPlexQF propSign;
1181 
1182  const TrackerInfo& tinfo = *pflags.tracker_info;
1183 
1184 #pragma omp simd
1185  for (int n = 0; n < NN; ++n) {
1186  if (n >= N_proc || (noMatEffPtr && noMatEffPtr->constAt(n, 0, 0))) {
1187  hitsRl(n, 0, 0) = 0.f;
1188  hitsXi(n, 0, 0) = 0.f;
1189  } else {
1190  const float hypo = std::hypot(outPar(n, 0, 0), outPar(n, 1, 0));
1191  auto mat = tinfo.material_checked(std::abs(outPar(n, 2, 0)), hypo);
1192  hitsRl(n, 0, 0) = mat.radl;
1193  hitsXi(n, 0, 0) = mat.bbxi;
1194  }
1195  propSign(n, 0, 0) = (pathL(n, 0, 0) > 0.f ? 1.f : -1.f);
1196  }
1197  applyMaterialEffects(hitsRl, hitsXi, propSign, plNrm, outErr, outPar, N_proc);
1198 #ifdef DEBUG
1199  if (debug && g_debug) {
1200  for (int kk = 0; kk < N_proc; ++kk) {
1201  dprintf("propSign %d\n", kk);
1202  for (int i = 0; i < 1; ++i) {
1203  dprintf("%8f ", propSign.constAt(kk, i, 0));
1204  }
1205  dprintf("\n");
1206  dprintf("plNrm %d\n", kk);
1207  for (int i = 0; i < 3; ++i) {
1208  dprintf("%8f ", plNrm.constAt(kk, i, 0));
1209  }
1210  dprintf("\n");
1211  dprintf("outErr(after material) %d\n", kk);
1212  for (int i = 0; i < 6; ++i) {
1213  for (int j = 0; j < 6; ++j)
1214  dprintf("%8f ", outErr.constAt(kk, i, j));
1215  dprintf("\n");
1216  }
1217  dprintf("\n");
1218  }
1219  }
1220 #endif
1221  }
1222 
1223  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
1224 
1225  // PROP-FAIL-ENABLE To keep physics changes minimal, we always restore the
1226  // state to input when propagation fails -- as was the default before.
1227  // if (pflags.copy_input_state_on_fail) {
1228  for (int i = 0; i < N_proc; ++i) {
1229  if (outFailFlag(i, 0, 0)) {
1230  outPar.copySlot(i, inPar);
1231  outErr.copySlot(i, inErr);
1232  }
1233  }
1234  // }
1235  }
1236 
1237  //==============================================================================
1238 
1239  void applyMaterialEffects(const MPlexQF& hitsRl,
1240  const MPlexQF& hitsXi,
1241  const MPlexQF& propSign,
1242  const MPlexHV& plNrm,
1243  MPlexLS& outErr,
1244  MPlexLV& outPar,
1245  const int N_proc) {
1246 #pragma omp simd
1247  for (int n = 0; n < NN; ++n) {
1248  float radL = hitsRl.constAt(n, 0, 0);
1249  if (radL < 1e-13f)
1250  continue; //ugly, please fixme
1251  const float theta = outPar.constAt(n, 5, 0);
1252  // const float pt = 1.f / outPar.constAt(n, 3, 0); //fixme, make sure it is positive?
1253  const float ipt = outPar.constAt(n, 3, 0);
1254  const float pt = 1.f / ipt; //fixme, make sure it is positive?
1255  const float ipt2 = ipt * ipt;
1256  const float p = pt / std::sin(theta);
1257  const float pz = p * std::cos(theta);
1258  const float p2 = p * p;
1259  constexpr float mpi = 0.140; // m=140 MeV, pion
1260  constexpr float mpi2 = mpi * mpi; // m=140 MeV, pion
1261  const float beta2 = p2 / (p2 + mpi2);
1262  const float beta = std::sqrt(beta2);
1263  //radiation lenght, corrected for the crossing angle (cos alpha from dot product of radius vector and momentum)
1264  const float invCos =
1265  p / std::abs(pt * std::cos(outPar.constAt(n, 4, 0)) * plNrm.constAt(n, 0, 0) +
1266  pt * std::sin(outPar.constAt(n, 4, 0)) * plNrm.constAt(n, 1, 0) + pz * plNrm.constAt(n, 2, 0));
1267  radL = radL * invCos; //fixme works only for barrel geom
1268  // multiple scattering
1269  //vary independently phi and theta by the rms of the planar multiple scattering angle
1270  // XXX-KMD radL < 0, see your fixme above! Repeating bailout
1271  if (radL < 1e-13f)
1272  continue;
1273  // const float thetaMSC = 0.0136f*std::sqrt(radL)*(1.f+0.038f*std::log(radL))/(beta*p);// eq 32.15
1274  // const float thetaMSC2 = thetaMSC*thetaMSC;
1275  const float thetaMSC = 0.0136f * (1.f + 0.038f * std::log(radL)) / (beta * p); // eq 32.15
1276  const float thetaMSC2 = thetaMSC * thetaMSC * radL;
1277  if (Config::usePtMultScat) {
1278  outErr.At(n, 3, 3) += thetaMSC2 * pz * pz * ipt2 * ipt2;
1279  outErr.At(n, 3, 5) -= thetaMSC2 * pz * ipt2;
1280  outErr.At(n, 4, 4) += thetaMSC2 * p2 * ipt2;
1281  outErr.At(n, 5, 5) += thetaMSC2;
1282  } else {
1283  outErr.At(n, 4, 4) += thetaMSC2;
1284  outErr.At(n, 5, 5) += thetaMSC2;
1285  }
1286  //std::cout << "beta=" << beta << " p=" << p << std::endl;
1287  //std::cout << "multiple scattering thetaMSC=" << thetaMSC << " thetaMSC2=" << thetaMSC2 << " radL=" << radL << std::endl;
1288  // energy loss
1289  // XXX-KMD beta2 = 1 => 1 / sqrt(0)
1290  // const float gamma = 1.f/std::sqrt(1.f - std::min(beta2, 0.999999f));
1291  // const float gamma2 = gamma*gamma;
1292  const float gamma2 = (p2 + mpi2) / mpi2;
1293  const float gamma = std::sqrt(gamma2); //1.f/std::sqrt(1.f - std::min(beta2, 0.999999f));
1294  constexpr float me = 0.0005; // m=0.5 MeV, electron
1295  const float wmax = 2.f * me * beta2 * gamma2 / (1.f + 2.f * gamma * me / mpi + me * me / (mpi * mpi));
1296  constexpr float I = 16.0e-9 * 10.75;
1297  const float deltahalf = std::log(28.816e-9f * std::sqrt(2.33f * 0.498f) / I) + std::log(beta * gamma) - 0.5f;
1298  const float dEdx =
1299  beta < 1.f
1300  ? (2.f * (hitsXi.constAt(n, 0, 0) * invCos *
1301  (0.5f * std::log(2.f * me * beta2 * gamma2 * wmax / (I * I)) - beta2 - deltahalf) / beta2))
1302  : 0.f; //protect against infs and nans
1303  // dEdx = dEdx*2.;//xi in cmssw is defined with an extra factor 0.5 with respect to formula 27.1 in pdg
1304  //std::cout << "dEdx=" << dEdx << " delta=" << deltahalf << " wmax=" << wmax << " Xi=" << hitsXi.constAt(n,0,0) << std::endl;
1305  const float dP = propSign.constAt(n, 0, 0) * dEdx / beta;
1306  outPar.At(n, 3, 0) = p / (std::max(p + dP, 0.001f) * pt); //stay above 1MeV
1307  //assume 100% uncertainty
1308  outErr.At(n, 3, 3) += dP * dP / (p2 * pt * pt);
1309  }
1310  }
1311 
1312 } // end namespace mkfit
void propagateHelixToPlaneMPlex(const MPlexLS &inErr, const MPlexLV &inPar, const MPlexQI &inChg, const MPlexHV &plPnt, const MPlexHV &plNrm, MPlexLS &outErr, MPlexLV &outPar, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags &pflags, const MPlexQI *noMatEffPtr)
Matriplex::Matriplex< float, LL, LL, NN > MPlexLL
Definition: Matrix.h:48
MPlex< T, D1, D2, N > hypot(const MPlex< T, D1, D2, N > &a, const MPlex< T, D1, D2, N > &b)
Definition: Matriplex.h:436
void propagateHelixToRMPlex(const MPlexLS &inErr, const MPlexLV &inPar, const MPlexQI &inChg, const MPlexQF &msRad, MPlexLS &outErr, MPlexLV &outPar, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags &pflags, const MPlexQI *noMatEffPtr)
Definition: APVGainStruct.h:7
#define dprint_np(n, x)
Definition: Debug.h:96
Matriplex::Matriplex< float, HH, 1, NN > MPlexHV
Definition: Matrix.h:53
Sin< T >::type sin(const T &t)
Definition: Sin.h:22
constexpr bool usePtMultScat
Definition: Config.h:52
#define CMS_UNROLL_LOOP_COUNT(N)
Definition: CMSUnrollLoop.h:48
void propagateLineToRMPlex(const MPlexLS &psErr, const MPlexLV &psPar, const MPlexHS &msErr, const MPlexHV &msPar, MPlexLS &outErr, MPlexLV &outPar, const int N_proc)
const TrackerInfo * tracker_info
void applyMaterialEffects(const MPlexQF &hitsRl, const MPlexQF &hitsXi, const MPlexQF &propSign, const MPlexHV &plNrm, MPlexLS &outErr, MPlexLV &outPar, const int N_proc)
Matriplex::Matriplex< float, LL, 1, NN > MPlexLV
Definition: Matrix.h:49
void squashPhiMPlex(MPlexLV &par, const int N_proc)
bool g_debug
Definition: Debug.cc:2
void helixAtZ(const MPlexLV &inPar, const MPlexQI &inChg, const MPlexQF &msZ, MPlexLV &outPar, MPlexLL &errorProp, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags &pflags)
T sqrt(T t)
Definition: SSEVec.h:19
constexpr Matriplex::idx_t NN
Definition: Matrix.h:43
Cos< T >::type cos(const T &t)
Definition: Cos.h:22
constexpr float Bfield
Definition: Config.h:55
void helixAtRFromIterativeCCSFullJac(const MPlexLV &inPar, const MPlexQI &inChg, const MPlexQF &msRad, MPlexLV &outPar, MPlexLL &errorProp, const int N_proc)
Tan< T >::type tan(const T &t)
Definition: Tan.h:22
Abs< T >::type abs(const T &t)
Definition: Abs.h:22
const std::complex< double > I
Definition: I.h:8
double f[11][100]
constexpr bool useTrigApprox
Definition: Config.h:50
Matriplex::Matriplex< int, 1, 1, NN > MPlexQI
Definition: Matrix.h:72
#define debug
Definition: HDRShower.cc:19
constexpr float sol
Definition: Config.h:13
Matriplex::Matriplex< float, 1, 1, NN > MPlexQF
Definition: Matrix.h:71
#define N
Definition: blowfish.cc:9
float hipo(float x, float y)
Definition: Matrix.h:9
float bFieldFromZR(const float z, const float r)
Definition: Config.h:126
double b
Definition: hdecay.h:120
void helixAtPlane(const MPlexLV &inPar, const MPlexQI &inChg, const MPlexHV &plPnt, const MPlexHV &plNrm, MPlexQF &pathL, MPlexLV &outPar, MPlexLL &errorProp, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags &pflags)
Matriplex::MatriplexSym< float, HH, NN > MPlexHS
Definition: Matrix.h:54
double a
Definition: hdecay.h:121
void propagateHelixToZMPlex(const MPlexLS &inErr, const MPlexLV &inPar, const MPlexQI &inChg, const MPlexQF &msZ, MPlexLS &outErr, MPlexLV &outPar, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags &pflags, const MPlexQI *noMatEffPtr)
Matriplex::MatriplexSym< float, LL, NN > MPlexLS
Definition: Matrix.h:50
Material material_checked(float z, float r) const
Definition: TrackerInfo.h:237
Definition: APVGainStruct.h:7
void helixAtRFromIterativeCCS(const MPlexLV &inPar, const MPlexQI &inChg, const MPlexQF &msRad, MPlexLV &outPar, MPlexLL &errorProp, MPlexQI &outFailFlag, const int N_proc, const PropagationFlags &pflags)
long double T
void sincos4(const float x, float &sin, float &cos)
Definition: Matrix.h:13
Geom::Theta< T > theta() const
#define ASSUME_ALIGNED(a, b)
#define dprintf(...)
Definition: Debug.h:98
constexpr int Niter
Definition: Config.h:49
__host__ __device__ V V wmax