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 
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) {
555  if (outFailFlag(n, 0, 0) || (noMatEffPtr && noMatEffPtr->constAt(n, 0, 0))) {
556  hitsRl(n, 0, 0) = 0.f;
557  hitsXi(n, 0, 0) = 0.f;
558  } else {
559  auto mat = tinfo.material_checked(std::abs(outPar(n, 2, 0)), msRad(n, 0, 0));
560  hitsRl(n, 0, 0) = mat.radl;
561  hitsXi(n, 0, 0) = mat.bbxi;
562  }
563  const float r0 = hipo(inPar(n, 0, 0), inPar(n, 1, 0));
564  const float r = msRad(n, 0, 0);
565  propSign(n, 0, 0) = (r > r0 ? 1. : -1.);
566  }
567  }
568  MPlexHV plNrm;
569 #pragma omp simd
570  for (int n = 0; n < NN; ++n) {
571  plNrm(n, 0, 0) = std::cos(outPar.constAt(n, 4, 0));
572  plNrm(n, 1, 0) = std::sin(outPar.constAt(n, 4, 0));
573  plNrm(n, 2, 0) = 0.f;
574  }
575  applyMaterialEffects(hitsRl, hitsXi, propSign, plNrm, outErr, outPar, N_proc);
576  }
577 
578  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
579 
580  // Matriplex version of:
581  // result.errors = ROOT::Math::Similarity(errorProp, outErr);
582 
583  /*
584  // To be used with: MPT_DIM = 1
585  if (fabs(sqrt(outPar[0]*outPar[0]+outPar[1]*outPar[1]) - msRad[0]) > 0.0001)
586  {
587  std::cout << "DID NOT GET TO R, FailFlag=" << failFlag[0]
588  << " dR=" << msRad[0] - std::hypot(outPar[0],outPar[1])
589  << " r=" << msRad[0] << " rin=" << std::hypot(inPar[0],inPar[1]) << " rout=" << std::hypot(outPar[0],outPar[1])
590  << std::endl;
591  // std::cout << " pt=" << pt << " pz=" << inPar.At(n, 2) << std::endl;
592  }
593  */
594 
595  // PROP-FAIL-ENABLE To keep physics changes minimal, we always restore the
596  // state to input when propagation fails -- as was the default before.
597  // if (pflags.copy_input_state_on_fail) {
598  for (int i = 0; i < N_proc; ++i) {
599  if (outFailFlag(i, 0, 0)) {
600  outPar.copySlot(i, inPar);
601  outErr.copySlot(i, inErr);
602  }
603  }
604  // }
605  }
606 
607  //==============================================================================
608 
609  void propagateHelixToZMPlex(const MPlexLS& inErr,
610  const MPlexLV& inPar,
611  const MPlexQI& inChg,
612  const MPlexQF& msZ,
613  MPlexLS& outErr,
614  MPlexLV& outPar,
615  MPlexQI& outFailFlag,
616  const int N_proc,
617  const PropagationFlags& pflags,
618  const MPlexQI* noMatEffPtr) {
619  // debug = true;
620 
621  outErr = inErr;
622  outPar = inPar;
623 
624  MPlexLL errorProp;
625 
626  //helixAtZ_new(inPar, inChg, msZ, outPar, errorProp, outFailFlag, N_proc, pflags);
627  helixAtZ(inPar, inChg, msZ, outPar, errorProp, outFailFlag, N_proc, pflags);
628 
629 #ifdef DEBUG
630  if (debug && g_debug) {
631  for (int kk = 0; kk < N_proc; ++kk) {
632  dprintf("inPar %d\n", kk);
633  for (int i = 0; i < 6; ++i) {
634  dprintf("%8f ", inPar.constAt(kk, i, 0));
635  }
636  dprintf("\n");
637 
638  dprintf("inErr %d\n", kk);
639  for (int i = 0; i < 6; ++i) {
640  for (int j = 0; j < 6; ++j)
641  dprintf("%8f ", inErr.constAt(kk, i, j));
642  dprintf("\n");
643  }
644  dprintf("\n");
645 
646  dprintf("errorProp %d\n", kk);
647  for (int i = 0; i < 6; ++i) {
648  for (int j = 0; j < 6; ++j)
649  dprintf("%8f ", errorProp.At(kk, i, j));
650  dprintf("\n");
651  }
652  dprintf("\n");
653  }
654  }
655 #endif
656 
657 #ifdef DEBUG
658  if (debug && g_debug) {
659  for (int kk = 0; kk < N_proc; ++kk) {
660  dprintf("outErr %d\n", kk);
661  for (int i = 0; i < 6; ++i) {
662  for (int j = 0; j < 6; ++j)
663  dprintf("%8f ", outErr.constAt(kk, i, j));
664  dprintf("\n");
665  }
666  dprintf("\n");
667  }
668  }
669 #endif
670 
671  // Matriplex version of: result.errors = ROOT::Math::Similarity(errorProp, outErr);
672  MPlexLL temp;
673  MultHelixPropEndcap(errorProp, outErr, temp);
674  MultHelixPropTranspEndcap(errorProp, temp, outErr);
675  // can replace with: MultHelixPropFull(errorProp, outErr, temp); MultHelixPropTranspFull(errorProp, temp, outErr);
676 
677  if (pflags.apply_material) {
678  MPlexQF hitsRl;
679  MPlexQF hitsXi;
680  MPlexQF propSign;
681 
682  const TrackerInfo& tinfo = *pflags.tracker_info;
683 
684 #pragma omp simd
685  for (int n = 0; n < NN; ++n) {
686  if (n >= N_proc || (noMatEffPtr && noMatEffPtr->constAt(n, 0, 0))) {
687  hitsRl(n, 0, 0) = 0.f;
688  hitsXi(n, 0, 0) = 0.f;
689  } else {
690  const float hypo = std::hypot(outPar(n, 0, 0), outPar(n, 1, 0));
691  auto mat = tinfo.material_checked(std::abs(msZ(n, 0, 0)), hypo);
692  hitsRl(n, 0, 0) = mat.radl;
693  hitsXi(n, 0, 0) = mat.bbxi;
694  }
695  if (n < N_proc) {
696  const float zout = msZ.constAt(n, 0, 0);
697  const float zin = inPar.constAt(n, 2, 0);
698  propSign(n, 0, 0) = (std::abs(zout) > std::abs(zin) ? 1.f : -1.f);
699  }
700  }
701  MPlexHV plNrm;
702 #pragma omp simd
703  for (int n = 0; n < NN; ++n) {
704  plNrm(n, 0, 0) = 0.f;
705  plNrm(n, 1, 0) = 0.f;
706  plNrm(n, 2, 0) = 1.f;
707  }
708  applyMaterialEffects(hitsRl, hitsXi, propSign, plNrm, outErr, outPar, N_proc);
709 #ifdef DEBUG
710  if (debug && g_debug) {
711  for (int kk = 0; kk < N_proc; ++kk) {
712  dprintf("propSign %d\n", kk);
713  for (int i = 0; i < 1; ++i) {
714  dprintf("%8f ", propSign.constAt(kk, i, 0));
715  }
716  dprintf("\n");
717  dprintf("plNrm %d\n", kk);
718  for (int i = 0; i < 3; ++i) {
719  dprintf("%8f ", plNrm.constAt(kk, i, 0));
720  }
721  dprintf("\n");
722  dprintf("outErr(after material) %d\n", kk);
723  for (int i = 0; i < 6; ++i) {
724  for (int j = 0; j < 6; ++j)
725  dprintf("%8f ", outErr.constAt(kk, i, j));
726  dprintf("\n");
727  }
728  dprintf("\n");
729  }
730  }
731 #endif
732  }
733 
734  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
735 
736  // PROP-FAIL-ENABLE To keep physics changes minimal, we always restore the
737  // state to input when propagation fails -- as was the default before.
738  // if (pflags.copy_input_state_on_fail) {
739  for (int i = 0; i < N_proc; ++i) {
740  if (outFailFlag(i, 0, 0)) {
741  outPar.copySlot(i, inPar);
742  outErr.copySlot(i, inErr);
743  }
744  }
745  // }
746  }
747 
748  void helixAtZ(const MPlexLV& inPar,
749  const MPlexQI& inChg,
750  const MPlexQF& msZ,
751  MPlexLV& outPar,
752  MPlexLL& errorProp,
753  MPlexQI& outFailFlag,
754  const int N_proc,
755  const PropagationFlags& pflags) {
756  errorProp.setVal(0.f);
757  outFailFlag.setVal(0.f);
758 
759  // debug = true;
760 #pragma omp simd
761  for (int n = 0; n < NN; ++n) {
762  //initialize erroProp to identity matrix, except element 2,2 which is zero
763  errorProp(n, 0, 0) = 1.f;
764  errorProp(n, 1, 1) = 1.f;
765  errorProp(n, 3, 3) = 1.f;
766  errorProp(n, 4, 4) = 1.f;
767  errorProp(n, 5, 5) = 1.f;
768  }
769  float zout[NN];
770  float zin[NN];
771  float ipt[NN];
772  float phiin[NN];
773  float theta[NN];
774 #pragma omp simd
775  for (int n = 0; n < NN; ++n) {
776  //initialize erroProp to identity matrix, except element 2,2 which is zero
777  zout[n] = msZ.constAt(n, 0, 0);
778  zin[n] = inPar.constAt(n, 2, 0);
779  ipt[n] = inPar.constAt(n, 3, 0);
780  phiin[n] = inPar.constAt(n, 4, 0);
781  theta[n] = inPar.constAt(n, 5, 0);
782  }
783 
784  float k[NN];
785  if (pflags.use_param_b_field) {
786 #pragma omp simd
787  for (int n = 0; n < NN; ++n) {
788  k[n] = inChg.constAt(n, 0, 0) * 100.f /
789  (-Const::sol * Config::bFieldFromZR(zin[n], hipo(inPar.constAt(n, 0, 0), inPar.constAt(n, 1, 0))));
790  }
791  } else {
792 #pragma omp simd
793  for (int n = 0; n < NN; ++n) {
794  k[n] = inChg.constAt(n, 0, 0) * 100.f / (-Const::sol * Config::Bfield);
795  }
796  }
797 
798  float kinv[NN];
799 #pragma omp simd
800  for (int n = 0; n < NN; ++n) {
801  kinv[n] = 1.f / k[n];
802  }
803 
804 #pragma omp simd
805  for (int n = 0; n < NN; ++n) {
806  dprint_np(n,
807  std::endl
808  << "input parameters"
809  << " inPar.constAt(n, 0, 0)=" << std::setprecision(9) << inPar.constAt(n, 0, 0)
810  << " inPar.constAt(n, 1, 0)=" << std::setprecision(9) << inPar.constAt(n, 1, 0)
811  << " inPar.constAt(n, 2, 0)=" << std::setprecision(9) << inPar.constAt(n, 2, 0)
812  << " inPar.constAt(n, 3, 0)=" << std::setprecision(9) << inPar.constAt(n, 3, 0)
813  << " inPar.constAt(n, 4, 0)=" << std::setprecision(9) << inPar.constAt(n, 4, 0)
814  << " inPar.constAt(n, 5, 0)=" << std::setprecision(9) << inPar.constAt(n, 5, 0)
815  << " inChg.constAt(n, 0, 0)=" << std::setprecision(9) << inChg.constAt(n, 0, 0));
816  }
817 #pragma omp simd
818  for (int n = 0; n < NN; ++n) {
819  dprint_np(n,
820  "propagation start, dump parameters"
821  << std::endl
822  << "pos = " << inPar.constAt(n, 0, 0) << " " << inPar.constAt(n, 1, 0) << " "
823  << inPar.constAt(n, 2, 0) << std::endl
824  << "mom (cart) = " << std::cos(inPar.constAt(n, 4, 0)) / inPar.constAt(n, 3, 0) << " "
825  << std::sin(inPar.constAt(n, 4, 0)) / inPar.constAt(n, 3, 0) << " "
826  << 1. / (inPar.constAt(n, 3, 0) * tan(inPar.constAt(n, 5, 0))) << " r="
827  << std::sqrt(inPar.constAt(n, 0, 0) * inPar.constAt(n, 0, 0) +
828  inPar.constAt(n, 1, 0) * inPar.constAt(n, 1, 0))
829  << " pT=" << 1. / std::abs(inPar.constAt(n, 3, 0)) << " q=" << inChg.constAt(n, 0, 0)
830  << " targetZ=" << msZ.constAt(n, 0, 0) << std::endl);
831  }
832 
833  float pt[NN];
834 #pragma omp simd
835  for (int n = 0; n < NN; ++n) {
836  pt[n] = 1.f / ipt[n];
837  }
838 
839  //no trig approx here, phi can be large
840  float cosP[NN];
841  float sinP[NN];
842 #pragma omp simd
843  for (int n = 0; n < NN; ++n) {
844  cosP[n] = std::cos(phiin[n]);
845  }
846 
847 #pragma omp simd
848  for (int n = 0; n < NN; ++n) {
849  sinP[n] = std::sin(phiin[n]);
850  }
851 
852  float cosT[NN];
853  float sinT[NN];
854 #pragma omp simd
855  for (int n = 0; n < NN; ++n) {
856  cosT[n] = std::cos(theta[n]);
857  }
858 
859 #pragma omp simd
860  for (int n = 0; n < NN; ++n) {
861  sinT[n] = std::sin(theta[n]);
862  }
863 
864  float tanT[NN];
865  float icos2T[NN];
866  float pxin[NN];
867  float pyin[NN];
868 #pragma omp simd
869  for (int n = 0; n < NN; ++n) {
870  tanT[n] = sinT[n] / cosT[n];
871  icos2T[n] = 1.f / (cosT[n] * cosT[n]);
872  pxin[n] = cosP[n] * pt[n];
873  pyin[n] = sinP[n] * pt[n];
874  }
875 
876  float deltaZ[NN];
877  float alpha[NN];
878 #pragma omp simd
879  for (int n = 0; n < NN; ++n) {
880  deltaZ[n] = zout[n] - zin[n];
881  alpha[n] = deltaZ[n] * tanT[n] * ipt[n] * kinv[n];
882  }
883 
884  float cosahTmp[NN];
885  float sinahTmp[NN];
887 #if !defined(__INTEL_COMPILER)
888 #pragma omp simd
889 #endif
890  for (int n = 0; n < NN; ++n) {
891  sincos4(alpha[n] * 0.5f, sinahTmp[n], cosahTmp[n]);
892  }
893  } else {
894 #if !defined(__INTEL_COMPILER)
895 #pragma omp simd
896 #endif
897  for (int n = 0; n < NN; ++n) {
898  cosahTmp[n] = std::cos(alpha[n] * 0.5f);
899  }
900 #if !defined(__INTEL_COMPILER)
901 #pragma omp simd
902 #endif
903  for (int n = 0; n < NN; ++n) {
904  sinahTmp[n] = std::sin(alpha[n] * 0.5f);
905  }
906  }
907 
908  float cosah[NN];
909  float sinah[NN];
910  float cosa[NN];
911  float sina[NN];
912 #pragma omp simd
913  for (int n = 0; n < NN; ++n) {
914  cosah[n] = cosahTmp[n];
915  sinah[n] = sinahTmp[n];
916  cosa[n] = 1.f - 2.f * sinah[n] * sinah[n];
917  sina[n] = 2.f * sinah[n] * cosah[n];
918  }
919 
920 //update parameters
921 #pragma omp simd
922  for (int n = 0; n < NN; ++n) {
923  outPar.At(n, 0, 0) = outPar.At(n, 0, 0) + 2.f * k[n] * sinah[n] * (pxin[n] * cosah[n] - pyin[n] * sinah[n]);
924  outPar.At(n, 1, 0) = outPar.At(n, 1, 0) + 2.f * k[n] * sinah[n] * (pyin[n] * cosah[n] + pxin[n] * sinah[n]);
925  outPar.At(n, 2, 0) = zout[n];
926  outPar.At(n, 4, 0) = phiin[n] + alpha[n];
927  }
928 
929 #pragma omp simd
930  for (int n = 0; n < NN; ++n) {
931  dprint_np(n,
932  "propagation to Z end (OLD), dump parameters\n"
933  << " pos = " << outPar(n, 0, 0) << " " << outPar(n, 1, 0) << " " << outPar(n, 2, 0) << "\t\t r="
934  << std::sqrt(outPar(n, 0, 0) * outPar(n, 0, 0) + outPar(n, 1, 0) * outPar(n, 1, 0)) << std::endl
935  << " mom = " << outPar(n, 3, 0) << " " << outPar(n, 4, 0) << " " << outPar(n, 5, 0) << std::endl
936  << " cart= " << std::cos(outPar(n, 4, 0)) / outPar(n, 3, 0) << " "
937  << std::sin(outPar(n, 4, 0)) / outPar(n, 3, 0) << " "
938  << 1. / (outPar(n, 3, 0) * tan(outPar(n, 5, 0))) << "\t\tpT=" << 1. / std::abs(outPar(n, 3, 0))
939  << std::endl);
940  }
941 
942  float pxcaMpysa[NN];
943 #pragma omp simd
944  for (int n = 0; n < NN; ++n) {
945  pxcaMpysa[n] = pxin[n] * cosa[n] - pyin[n] * sina[n];
946  }
947 
948 #pragma omp simd
949  for (int n = 0; n < NN; ++n) {
950  errorProp(n, 0, 2) = -tanT[n] * ipt[n] * pxcaMpysa[n];
951  errorProp(n, 0, 3) =
952  k[n] * pt[n] * pt[n] *
953  (cosP[n] * (alpha[n] * cosa[n] - sina[n]) + sinP[n] * 2.f * sinah[n] * (sinah[n] - alpha[n] * cosah[n]));
954  errorProp(n, 0, 4) = -2.f * k[n] * pt[n] * sinah[n] * (sinP[n] * cosah[n] + cosP[n] * sinah[n]);
955  errorProp(n, 0, 5) = deltaZ[n] * ipt[n] * pxcaMpysa[n] * icos2T[n];
956  }
957 
958  float pycaPpxsa[NN];
959 #pragma omp simd
960  for (int n = 0; n < NN; ++n) {
961  pycaPpxsa[n] = pyin[n] * cosa[n] + pxin[n] * sina[n];
962  }
963 
964 #pragma omp simd
965  for (int n = 0; n < NN; ++n) {
966  errorProp(n, 1, 2) = -tanT[n] * ipt[n] * pycaPpxsa[n];
967  errorProp(n, 1, 3) =
968  k[n] * pt[n] * pt[n] *
969  (sinP[n] * (alpha[n] * cosa[n] - sina[n]) - cosP[n] * 2.f * sinah[n] * (sinah[n] - alpha[n] * cosah[n]));
970  errorProp(n, 1, 4) = 2.f * k[n] * pt[n] * sinah[n] * (cosP[n] * cosah[n] - sinP[n] * sinah[n]);
971  errorProp(n, 1, 5) = deltaZ[n] * ipt[n] * pycaPpxsa[n] * icos2T[n];
972  }
973 
974 #pragma omp simd
975  for (int n = 0; n < NN; ++n) {
976  errorProp(n, 4, 2) = -ipt[n] * tanT[n] * kinv[n];
977  errorProp(n, 4, 3) = tanT[n] * deltaZ[n] * kinv[n];
978  errorProp(n, 4, 5) = ipt[n] * deltaZ[n] * kinv[n] * icos2T[n];
979  }
980 
981 #pragma omp simd
982  for (int n = 0; n < NN; ++n) {
983  dprint_np(
984  n,
985  "propagation end, dump parameters"
986  << std::endl
987  << "pos = " << outPar.At(n, 0, 0) << " " << outPar.At(n, 1, 0) << " " << outPar.At(n, 2, 0) << std::endl
988  << "mom (cart) = " << std::cos(outPar.At(n, 4, 0)) / outPar.At(n, 3, 0) << " "
989  << std::sin(outPar.At(n, 4, 0)) / outPar.At(n, 3, 0) << " "
990  << 1. / (outPar.At(n, 3, 0) * tan(outPar.At(n, 5, 0)))
991  << " r=" << std::sqrt(outPar.At(n, 0, 0) * outPar.At(n, 0, 0) + outPar.At(n, 1, 0) * outPar.At(n, 1, 0))
992  << " pT=" << 1. / std::abs(outPar.At(n, 3, 0)) << std::endl);
993  }
994 
995  // PROP-FAIL-ENABLE Disabled to keep physics changes minimal.
996  // To be reviewed, enabled and processed accordingly elsewhere.
997  /*
998  // Check for errors, set fail-flag.
999  for (int n = 0; n < NN; ++n) {
1000  // We propagate for alpha: mark fail when prop angle more than pi/2
1001  if (std::abs(alpha[n]) > 1.57) {
1002  dprintf("helixAtZ: more than quarter turn, alpha = %f\n", alpha[n]);
1003  outFailFlag[n] = 1;
1004  } else {
1005  // Have we reached desired z? We can't know, we copy desired z to actual z.
1006  // Are we close to apex? Same condition as in propToR, 12.5 deg, cos(78.5deg) = 0.2
1007  float dotp = (outPar.At(n, 0, 0) * std::cos(outPar.At(n, 4, 0)) +
1008  outPar.At(n, 1, 0) * std::sin(outPar.At(n, 4, 0))) /
1009  std::hypot(outPar.At(n, 0, 0), outPar.At(n, 1, 0));
1010  if (dotp < 0.2 || dotp < 0) {
1011  dprintf("helixAtZ: dot product bad, dotp = %f\n", dotp);
1012  outFailFlag[n] = 1;
1013  }
1014  }
1015  }
1016  */
1017 
1018 #ifdef DEBUG
1019  if (debug && g_debug) {
1020  for (int n = 0; n < N_proc; ++n) {
1021  dmutex_guard;
1022  std::cout << n << ": jacobian" << std::endl;
1023  printf("%5f %5f %5f %5f %5f %5f\n",
1024  errorProp(n, 0, 0),
1025  errorProp(n, 0, 1),
1026  errorProp(n, 0, 2),
1027  errorProp(n, 0, 3),
1028  errorProp(n, 0, 4),
1029  errorProp(n, 0, 5));
1030  printf("%5f %5f %5f %5f %5f %5f\n",
1031  errorProp(n, 1, 0),
1032  errorProp(n, 1, 1),
1033  errorProp(n, 1, 2),
1034  errorProp(n, 1, 3),
1035  errorProp(n, 1, 4),
1036  errorProp(n, 1, 5));
1037  printf("%5f %5f %5f %5f %5f %5f\n",
1038  errorProp(n, 2, 0),
1039  errorProp(n, 2, 1),
1040  errorProp(n, 2, 2),
1041  errorProp(n, 2, 3),
1042  errorProp(n, 2, 4),
1043  errorProp(n, 2, 5));
1044  printf("%5f %5f %5f %5f %5f %5f\n",
1045  errorProp(n, 3, 0),
1046  errorProp(n, 3, 1),
1047  errorProp(n, 3, 2),
1048  errorProp(n, 3, 3),
1049  errorProp(n, 3, 4),
1050  errorProp(n, 3, 5));
1051  printf("%5f %5f %5f %5f %5f %5f\n",
1052  errorProp(n, 4, 0),
1053  errorProp(n, 4, 1),
1054  errorProp(n, 4, 2),
1055  errorProp(n, 4, 3),
1056  errorProp(n, 4, 4),
1057  errorProp(n, 4, 5));
1058  printf("%5f %5f %5f %5f %5f %5f\n",
1059  errorProp(n, 5, 0),
1060  errorProp(n, 5, 1),
1061  errorProp(n, 5, 2),
1062  errorProp(n, 5, 3),
1063  errorProp(n, 5, 4),
1064  errorProp(n, 5, 5));
1065  }
1066  }
1067 #endif
1068  }
1069 
1070  void helixAtPlane(const MPlexLV& inPar,
1071  const MPlexQI& inChg,
1072  const MPlexHV& plPnt,
1073  const MPlexHV& plNrm,
1074  MPlexQF& pathL,
1075  MPlexLV& outPar,
1076  MPlexLL& errorProp,
1077  MPlexQI& outFailFlag,
1078  const int N_proc,
1079  const PropagationFlags& pflags) {
1080  errorProp.setVal(0.f);
1081  outFailFlag.setVal(0.f);
1082 
1083  helixAtPlane_impl(inPar, inChg, plPnt, plNrm, pathL, outPar, errorProp, outFailFlag, 0, NN, N_proc, pflags);
1084  }
1085 
1087  const MPlexLV& inPar,
1088  const MPlexQI& inChg,
1089  const MPlexHV& plPnt,
1090  const MPlexHV& plNrm,
1091  MPlexLS& outErr,
1092  MPlexLV& outPar,
1093  MPlexQI& outFailFlag,
1094  const int N_proc,
1095  const PropagationFlags& pflags,
1096  const MPlexQI* noMatEffPtr) {
1097  // debug = true;
1098 
1099  outErr = inErr;
1100  outPar = inPar;
1101 
1102  MPlexQF pathL;
1103  MPlexLL errorProp;
1104 
1105  helixAtPlane(inPar, inChg, plPnt, plNrm, pathL, outPar, errorProp, outFailFlag, N_proc, pflags);
1106 
1107  for (int n = 0; n < NN; ++n) {
1108  dprint_np(
1109  n,
1110  "propagation to plane end, dump parameters\n"
1111  //<< " D = " << s[n] << " alpha = " << s[n] * std::sin(inPar(n, 5, 0)) * inPar(n, 3, 0) * kinv[n] << " kinv = " << kinv[n] << std::endl
1112  << " pos = " << outPar(n, 0, 0) << " " << outPar(n, 1, 0) << " " << outPar(n, 2, 0) << "\t\t r="
1113  << std::sqrt(outPar(n, 0, 0) * outPar(n, 0, 0) + outPar(n, 1, 0) * outPar(n, 1, 0)) << std::endl
1114  << " mom = " << outPar(n, 3, 0) << " " << outPar(n, 4, 0) << " " << outPar(n, 5, 0) << std::endl
1115  << " cart= " << std::cos(outPar(n, 4, 0)) / outPar(n, 3, 0) << " "
1116  << std::sin(outPar(n, 4, 0)) / outPar(n, 3, 0) << " " << 1. / (outPar(n, 3, 0) * tan(outPar(n, 5, 0)))
1117  << "\t\tpT=" << 1. / std::abs(outPar(n, 3, 0)) << std::endl);
1118  }
1119 
1120 #ifdef DEBUG
1121  if (debug && g_debug) {
1122  for (int kk = 0; kk < N_proc; ++kk) {
1123  dprintf("inPar %d\n", kk);
1124  for (int i = 0; i < 6; ++i) {
1125  dprintf("%8f ", inPar.constAt(kk, i, 0));
1126  }
1127  dprintf("\n");
1128  dprintf("inErr %d\n", kk);
1129  for (int i = 0; i < 6; ++i) {
1130  for (int j = 0; j < 6; ++j)
1131  dprintf("%8f ", inErr.constAt(kk, i, j));
1132  dprintf("\n");
1133  }
1134  dprintf("\n");
1135 
1136  for (int kk = 0; kk < N_proc; ++kk) {
1137  dprintf("plNrm %d\n", kk);
1138  for (int j = 0; j < 3; ++j)
1139  dprintf("%8f ", plNrm.constAt(kk, 0, j));
1140  }
1141  dprintf("\n");
1142 
1143  for (int kk = 0; kk < N_proc; ++kk) {
1144  dprintf("pathL %d\n", kk);
1145  for (int j = 0; j < 1; ++j)
1146  dprintf("%8f ", pathL.constAt(kk, 0, j));
1147  }
1148  dprintf("\n");
1149 
1150  dprintf("errorProp %d\n", kk);
1151  for (int i = 0; i < 6; ++i) {
1152  for (int j = 0; j < 6; ++j)
1153  dprintf("%8f ", errorProp.At(kk, i, j));
1154  dprintf("\n");
1155  }
1156  dprintf("\n");
1157  }
1158  }
1159 #endif
1160 
1161  // Matriplex version of:
1162  // result.errors = ROOT::Math::Similarity(errorProp, outErr);
1163  MPlexLL temp;
1164  MultHelixPropFull(errorProp, outErr, temp);
1165  MultHelixPropTranspFull(errorProp, temp, outErr);
1166 
1167 #ifdef DEBUG
1168  if (debug && g_debug) {
1169  for (int kk = 0; kk < N_proc; ++kk) {
1170  dprintf("outErr %d\n", kk);
1171  for (int i = 0; i < 6; ++i) {
1172  for (int j = 0; j < 6; ++j)
1173  dprintf("%8f ", outErr.constAt(kk, i, j));
1174  dprintf("\n");
1175  }
1176  dprintf("\n");
1177  }
1178  }
1179 #endif
1180 
1181  if (pflags.apply_material) {
1182  MPlexQF hitsRl;
1183  MPlexQF hitsXi;
1184  MPlexQF propSign;
1185 
1186  const TrackerInfo& tinfo = *pflags.tracker_info;
1187 
1188 #pragma omp simd
1189  for (int n = 0; n < NN; ++n) {
1190  if (n >= N_proc || (noMatEffPtr && noMatEffPtr->constAt(n, 0, 0))) {
1191  hitsRl(n, 0, 0) = 0.f;
1192  hitsXi(n, 0, 0) = 0.f;
1193  } else {
1194  const float hypo = std::hypot(outPar(n, 0, 0), outPar(n, 1, 0));
1195  auto mat = tinfo.material_checked(std::abs(outPar(n, 2, 0)), hypo);
1196  hitsRl(n, 0, 0) = mat.radl;
1197  hitsXi(n, 0, 0) = mat.bbxi;
1198  }
1199  propSign(n, 0, 0) = (pathL(n, 0, 0) > 0.f ? 1.f : -1.f);
1200  }
1201  applyMaterialEffects(hitsRl, hitsXi, propSign, plNrm, outErr, outPar, N_proc);
1202 #ifdef DEBUG
1203  if (debug && g_debug) {
1204  for (int kk = 0; kk < N_proc; ++kk) {
1205  dprintf("propSign %d\n", kk);
1206  for (int i = 0; i < 1; ++i) {
1207  dprintf("%8f ", propSign.constAt(kk, i, 0));
1208  }
1209  dprintf("\n");
1210  dprintf("plNrm %d\n", kk);
1211  for (int i = 0; i < 3; ++i) {
1212  dprintf("%8f ", plNrm.constAt(kk, i, 0));
1213  }
1214  dprintf("\n");
1215  dprintf("outErr(after material) %d\n", kk);
1216  for (int i = 0; i < 6; ++i) {
1217  for (int j = 0; j < 6; ++j)
1218  dprintf("%8f ", outErr.constAt(kk, i, j));
1219  dprintf("\n");
1220  }
1221  dprintf("\n");
1222  }
1223  }
1224 #endif
1225  }
1226 
1227  squashPhiMPlex(outPar, N_proc); // ensure phi is between |pi|
1228 
1229  // PROP-FAIL-ENABLE To keep physics changes minimal, we always restore the
1230  // state to input when propagation fails -- as was the default before.
1231  // if (pflags.copy_input_state_on_fail) {
1232  for (int i = 0; i < N_proc; ++i) {
1233  if (outFailFlag(i, 0, 0)) {
1234  outPar.copySlot(i, inPar);
1235  outErr.copySlot(i, inErr);
1236  }
1237  }
1238  // }
1239  }
1240 
1241  //==============================================================================
1242 
1243  void applyMaterialEffects(const MPlexQF& hitsRl,
1244  const MPlexQF& hitsXi,
1245  const MPlexQF& propSign,
1246  const MPlexHV& plNrm,
1247  MPlexLS& outErr,
1248  MPlexLV& outPar,
1249  const int N_proc) {
1250 #pragma omp simd
1251  for (int n = 0; n < NN; ++n) {
1252  if (n >= N_proc)
1253  continue;
1254  float radL = hitsRl.constAt(n, 0, 0);
1255  if (radL < 1e-13f)
1256  continue; //ugly, please fixme
1257  const float theta = outPar.constAt(n, 5, 0);
1258  // const float pt = 1.f / outPar.constAt(n, 3, 0); //fixme, make sure it is positive?
1259  const float ipt = outPar.constAt(n, 3, 0);
1260  const float pt = 1.f / ipt; //fixme, make sure it is positive?
1261  const float ipt2 = ipt * ipt;
1262  const float p = pt / std::sin(theta);
1263  const float pz = p * std::cos(theta);
1264  const float p2 = p * p;
1265  constexpr float mpi = 0.140; // m=140 MeV, pion
1266  constexpr float mpi2 = mpi * mpi; // m=140 MeV, pion
1267  const float beta2 = p2 / (p2 + mpi2);
1268  const float beta = std::sqrt(beta2);
1269  //radiation lenght, corrected for the crossing angle (cos alpha from dot product of radius vector and momentum)
1270  const float invCos =
1271  p / std::abs(pt * std::cos(outPar.constAt(n, 4, 0)) * plNrm.constAt(n, 0, 0) +
1272  pt * std::sin(outPar.constAt(n, 4, 0)) * plNrm.constAt(n, 1, 0) + pz * plNrm.constAt(n, 2, 0));
1273  radL = radL * invCos; //fixme works only for barrel geom
1274  // multiple scattering
1275  //vary independently phi and theta by the rms of the planar multiple scattering angle
1276  // XXX-KMD radL < 0, see your fixme above! Repeating bailout
1277  if (radL < 1e-13f)
1278  continue;
1279  // const float thetaMSC = 0.0136f*std::sqrt(radL)*(1.f+0.038f*std::log(radL))/(beta*p);// eq 32.15
1280  // const float thetaMSC2 = thetaMSC*thetaMSC;
1281  const float thetaMSC = 0.0136f * (1.f + 0.038f * std::log(radL)) / (beta * p); // eq 32.15
1282  const float thetaMSC2 = thetaMSC * thetaMSC * radL;
1283  if (Config::usePtMultScat) {
1284  outErr.At(n, 3, 3) += thetaMSC2 * pz * pz * ipt2 * ipt2;
1285  outErr.At(n, 3, 5) -= thetaMSC2 * pz * ipt2;
1286  outErr.At(n, 4, 4) += thetaMSC2 * p2 * ipt2;
1287  outErr.At(n, 5, 5) += thetaMSC2;
1288  } else {
1289  outErr.At(n, 4, 4) += thetaMSC2;
1290  outErr.At(n, 5, 5) += thetaMSC2;
1291  }
1292  //std::cout << "beta=" << beta << " p=" << p << std::endl;
1293  //std::cout << "multiple scattering thetaMSC=" << thetaMSC << " thetaMSC2=" << thetaMSC2 << " radL=" << radL << std::endl;
1294  // energy loss
1295  // XXX-KMD beta2 = 1 => 1 / sqrt(0)
1296  // const float gamma = 1.f/std::sqrt(1.f - std::min(beta2, 0.999999f));
1297  // const float gamma2 = gamma*gamma;
1298  const float gamma2 = (p2 + mpi2) / mpi2;
1299  const float gamma = std::sqrt(gamma2); //1.f/std::sqrt(1.f - std::min(beta2, 0.999999f));
1300  constexpr float me = 0.0005; // m=0.5 MeV, electron
1301  const float wmax = 2.f * me * beta2 * gamma2 / (1.f + 2.f * gamma * me / mpi + me * me / (mpi * mpi));
1302  constexpr float I = 16.0e-9 * 10.75;
1303  const float deltahalf = std::log(28.816e-9f * std::sqrt(2.33f * 0.498f) / I) + std::log(beta * gamma) - 0.5f;
1304  const float dEdx =
1305  beta < 1.f
1306  ? (2.f * (hitsXi.constAt(n, 0, 0) * invCos *
1307  (0.5f * std::log(2.f * me * beta2 * gamma2 * wmax / (I * I)) - beta2 - deltahalf) / beta2))
1308  : 0.f; //protect against infs and nans
1309  // dEdx = dEdx*2.;//xi in cmssw is defined with an extra factor 0.5 with respect to formula 27.1 in pdg
1310  //std::cout << "dEdx=" << dEdx << " delta=" << deltahalf << " wmax=" << wmax << " Xi=" << hitsXi.constAt(n,0,0) << std::endl;
1311  const float dP = propSign.constAt(n, 0, 0) * dEdx / beta;
1312  outPar.At(n, 3, 0) = p / (std::max(p + dP, 0.001f) * pt); //stay above 1MeV
1313  //assume 100% uncertainty
1314  outErr.At(n, 3, 3) += dP * dP / (p2 * pt * pt);
1315  }
1316  }
1317 
1318 } // 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