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