MkFitCore/src/PropagationMPlex.cc

0001 #include "FWCore/Utilities/interface/CMSUnrollLoop.h"
0002
0003 #include "MaterialEffects.h"
0004 #include "PropagationMPlex.h"
0005
0006 //#define DEBUG
0007 #include "Debug.h"
0008
0009 //==============================================================================
0010 // propagateLineToRMPlex
0011 //==============================================================================
0012
0013 using namespace Matriplex;
0014
0015 namespace mkfit {
0016
0017   void propagateLineToRMPlex(const MPlexLS& psErr,
0018                              const MPlexLV& psPar,
0019                              const MPlexHS& msErr,
0020                              const MPlexHV& msPar,
0021                              MPlexLS& outErr,
0022                              MPlexLV& outPar,
0023                              const int N_proc) {
0024     // XXX Regenerate parts below with a script.
0025
0026     const idx_t N = NN;
0027
0028 #pragma omp simd
0029     for (int n = 0; n < NN; ++n) {
0030       const float cosA = (psPar[0 * N + n] * psPar[3 * N + n] + psPar[1 * N + n] * psPar[4 * N + n]) /
0031                          (std::sqrt((psPar[0 * N + n] * psPar[0 * N + n] + psPar[1 * N + n] * psPar[1 * N + n]) *
0032                                     (psPar[3 * N + n] * psPar[3 * N + n] + psPar[4 * N + n] * psPar[4 * N + n])));
0033       const float dr = (hipo(msPar[0 * N + n], msPar[1 * N + n]) - hipo(psPar[0 * N + n], psPar[1 * N + n])) / cosA;
0034
0035       dprint_np(n, "propagateLineToRMPlex dr=" << dr);
0036
0037       const float pt = hipo(psPar[3 * N + n], psPar[4 * N + n]);
0038       const float p = dr / pt;  // path
0039       const float psq = p * p;
0040
0041       outPar[0 * N + n] = psPar[0 * N + n] + p * psPar[3 * N + n];
0042       outPar[1 * N + n] = psPar[1 * N + n] + p * psPar[4 * N + n];
0043       outPar[2 * N + n] = psPar[2 * N + n] + p * psPar[5 * N + n];
0044       outPar[3 * N + n] = psPar[3 * N + n];
0045       outPar[4 * N + n] = psPar[4 * N + n];
0046       outPar[5 * N + n] = psPar[5 * N + n];
0047
0048       {
0049         const MPlexLS& A = psErr;
0050         MPlexLS& B = outErr;
0051
0052         B.fArray[0 * N + n] = A.fArray[0 * N + n];
0053         B.fArray[1 * N + n] = A.fArray[1 * N + n];
0054         B.fArray[2 * N + n] = A.fArray[2 * N + n];
0055         B.fArray[3 * N + n] = A.fArray[3 * N + n];
0056         B.fArray[4 * N + n] = A.fArray[4 * N + n];
0057         B.fArray[5 * N + n] = A.fArray[5 * N + n];
0058         B.fArray[6 * N + n] = A.fArray[6 * N + n] + p * A.fArray[0 * N + n];
0059         B.fArray[7 * N + n] = A.fArray[7 * N + n] + p * A.fArray[1 * N + n];
0060         B.fArray[8 * N + n] = A.fArray[8 * N + n] + p * A.fArray[3 * N + n];
0061         B.fArray[9 * N + n] =
0062             A.fArray[9 * N + n] + p * (A.fArray[6 * N + n] + A.fArray[6 * N + n]) + psq * A.fArray[0 * N + n];
0063         B.fArray[10 * N + n] = A.fArray[10 * N + n] + p * A.fArray[1 * N + n];
0064         B.fArray[11 * N + n] = A.fArray[11 * N + n] + p * A.fArray[2 * N + n];
0065         B.fArray[12 * N + n] = A.fArray[12 * N + n] + p * A.fArray[4 * N + n];
0066         B.fArray[13 * N + n] =
0067             A.fArray[13 * N + n] + p * (A.fArray[7 * N + n] + A.fArray[10 * N + n]) + psq * A.fArray[1 * N + n];
0068         B.fArray[14 * N + n] =
0069             A.fArray[14 * N + n] + p * (A.fArray[11 * N + n] + A.fArray[11 * N + n]) + psq * A.fArray[2 * N + n];
0070         B.fArray[15 * N + n] = A.fArray[15 * N + n] + p * A.fArray[3 * N + n];
0071         B.fArray[16 * N + n] = A.fArray[16 * N + n] + p * A.fArray[4 * N + n];
0072         B.fArray[17 * N + n] = A.fArray[17 * N + n] + p * A.fArray[5 * N + n];
0073         B.fArray[18 * N + n] =
0074             A.fArray[18 * N + n] + p * (A.fArray[8 * N + n] + A.fArray[15 * N + n]) + psq * A.fArray[3 * N + n];
0075         B.fArray[19 * N + n] =
0076             A.fArray[19 * N + n] + p * (A.fArray[12 * N + n] + A.fArray[16 * N + n]) + psq * A.fArray[4 * N + n];
0077         B.fArray[20 * N + n] =
0078             A.fArray[20 * N + n] + p * (A.fArray[17 * N + n] + A.fArray[17 * N + n]) + psq * A.fArray[5 * N + n];
0079       }
0080
0081       dprint_np(n, "propagateLineToRMPlex arrive at r=" << hipo(outPar[0 * N + n], outPar[1 * N + n]));
0082     }
0083   }
0084
0085 }  // end namespace mkfit
0086
0087 //==============================================================================
0088 // propagateHelixToRMPlex
0089 //==============================================================================
0090
0091 namespace {
0092   using namespace mkfit;
0093
0094   void MultHelixProp(const MPlexLL& A, const MPlexLS& B, MPlexLL& C) {
0095     // C = A * B
0096
0097     typedef float T;
0098     const idx_t N = NN;
0099
0100     const T* a = A.fArray;
0101     ASSUME_ALIGNED(a, 64);
0102     const T* b = B.fArray;
0103     ASSUME_ALIGNED(b, 64);
0104     T* c = C.fArray;
0105     ASSUME_ALIGNED(c, 64);
0106
0107 #include "MultHelixProp.ah"
0108   }
0109
0110   void MultHelixPropTransp(const MPlexLL& A, const MPlexLL& B, MPlexLS& C) {
0111     // C = B * AT;
0112
0113     typedef float T;
0114     const idx_t N = NN;
0115
0116     const T* a = A.fArray;
0117     ASSUME_ALIGNED(a, 64);
0118     const T* b = B.fArray;
0119     ASSUME_ALIGNED(b, 64);
0120     T* c = C.fArray;
0121     ASSUME_ALIGNED(c, 64);
0122
0123 #include "MultHelixPropTransp.ah"
0124   }
0125
0126   void MultHelixPropEndcap(const MPlexLL& A, const MPlexLS& B, MPlexLL& C) {
0127     // C = A * B
0128
0129     typedef float T;
0130     const idx_t N = NN;
0131
0132     const T* a = A.fArray;
0133     ASSUME_ALIGNED(a, 64);
0134     const T* b = B.fArray;
0135     ASSUME_ALIGNED(b, 64);
0136     T* c = C.fArray;
0137     ASSUME_ALIGNED(c, 64);
0138
0139 #include "MultHelixPropEndcap.ah"
0140   }
0141
0142   void MultHelixPropTranspEndcap(const MPlexLL& A, const MPlexLL& B, MPlexLS& C) {
0143     // C = B * AT;
0144
0145     typedef float T;
0146     const idx_t N = NN;
0147
0148     const T* a = A.fArray;
0149     ASSUME_ALIGNED(a, 64);
0150     const T* b = B.fArray;
0151     ASSUME_ALIGNED(b, 64);
0152     T* c = C.fArray;
0153     ASSUME_ALIGNED(c, 64);
0154
0155 #include "MultHelixPropTranspEndcap.ah"
0156   }
0157
0158   inline void MultHelixPropTemp(const MPlexLL& A, const MPlexLL& B, MPlexLL& C, int n) {
0159     // C = A * B
0160
0161     typedef float T;
0162     const idx_t N = NN;
0163
0164     const T* a = A.fArray;
0165     ASSUME_ALIGNED(a, 64);
0166     const T* b = B.fArray;
0167     ASSUME_ALIGNED(b, 64);
0168     T* c = C.fArray;
0169     ASSUME_ALIGNED(c, 64);
0170
0171     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] +
0172                    a[4 * N + n] * b[24 * N + n];
0173     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] +
0174                    a[4 * N + n] * b[25 * N + n];
0175     c[2 * N + n] = a[2 * N + n];
0176     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] +
0177                    a[3 * N + n] + a[4 * N + n] * b[27 * N + n];
0178     c[4 * N + n] = a[0 * N + n] * b[4 * N + n] + a[1 * N + n] * b[10 * N + n] + a[4 * N + n];
0179     c[5 * N + n] = a[2 * N + n] * b[17 * N + n] + a[5 * N + n];
0180     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] +
0181                    a[10 * N + n] * b[24 * N + n];
0182     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] +
0183                    a[10 * N + n] * b[25 * N + n];
0184     c[8 * N + n] = a[8 * N + n];
0185     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] +
0186                    a[9 * N + n] + a[10 * N + n] * b[27 * N + n];
0187     c[10 * N + n] = a[6 * N + n] * b[4 * N + n] + a[7 * N + n] * b[10 * N + n] + a[10 * N + n];
0188     c[11 * N + n] = a[8 * N + n] * b[17 * N + n] + a[11 * N + n];
0189     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] +
0190                     a[16 * N + n] * b[24 * N + n];
0191     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] +
0192                     a[16 * N + n] * b[25 * N + n];
0193     c[14 * N + n] = a[14 * N + n];
0194     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] +
0195                     a[15 * N + n] + a[16 * N + n] * b[27 * N + n];
0196     c[16 * N + n] = a[12 * N + n] * b[4 * N + n] + a[13 * N + n] * b[10 * N + n] + a[16 * N + n];
0197     c[17 * N + n] = a[14 * N + n] * b[17 * N + n] + a[17 * N + n];
0198     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] +
0199                     a[22 * N + n] * b[24 * N + n];
0200     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] +
0201                     a[22 * N + n] * b[25 * N + n];
0202     c[20 * N + n] = a[20 * N + n];
0203     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] +
0204                     a[21 * N + n] + a[22 * N + n] * b[27 * N + n];
0205     c[22 * N + n] = a[18 * N + n] * b[4 * N + n] + a[19 * N + n] * b[10 * N + n] + a[22 * N + n];
0206     c[23 * N + n] = a[20 * N + n] * b[17 * N + n] + a[23 * N + n];
0207     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] +
0208                     a[28 * N + n] * b[24 * N + n];
0209     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] +
0210                     a[28 * N + n] * b[25 * N + n];
0211     c[26 * N + n] = a[26 * N + n];
0212     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] +
0213                     a[27 * N + n] + a[28 * N + n] * b[27 * N + n];
0214     c[28 * N + n] = a[24 * N + n] * b[4 * N + n] + a[25 * N + n] * b[10 * N + n] + a[28 * N + n];
0215     c[29 * N + n] = a[26 * N + n] * b[17 * N + n] + a[29 * N + n];
0216     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] +
0217                     a[34 * N + n] * b[24 * N + n];
0218     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] +
0219                     a[34 * N + n] * b[25 * N + n];
0220     c[32 * N + n] = a[32 * N + n];
0221     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] +
0222                     a[33 * N + n] + a[34 * N + n] * b[27 * N + n];
0223     c[34 * N + n] = a[30 * N + n] * b[4 * N + n] + a[31 * N + n] * b[10 * N + n] + a[34 * N + n];
0224     c[35 * N + n] = a[32 * N + n] * b[17 * N + n] + a[35 * N + n];
0225   }
0226
0227 #ifdef UNUSED
0228   // this version does not assume to know which elements are 0 or 1, so it does the full multiplication
0229   void MultHelixPropFull(const MPlexLL& A, const MPlexLS& B, MPlexLL& C) {
0230 #pragma omp simd
0231     for (int n = 0; n < NN; ++n) {
0232       for (int i = 0; i < 6; ++i) {
0233         for (int j = 0; j < 6; ++j) {
0234           C(n, i, j) = 0.;
0235           for (int k = 0; k < 6; ++k)
0236             C(n, i, j) += A.constAt(n, i, k) * B.constAt(n, k, j);
0237         }
0238       }
0239     }
0240   }
0241
0242   // this version does not assume to know which elements are 0 or 1, so it does the full multiplication
0243   void MultHelixPropFull(const MPlexLL& A, const MPlexLL& B, MPlexLL& C) {
0244 #pragma omp simd
0245     for (int n = 0; n < NN; ++n) {
0246       for (int i = 0; i < 6; ++i) {
0247         for (int j = 0; j < 6; ++j) {
0248           C(n, i, j) = 0.;
0249           for (int k = 0; k < 6; ++k)
0250             C(n, i, j) += A.constAt(n, i, k) * B.constAt(n, k, j);
0251         }
0252       }
0253     }
0254   }
0255
0256   // this version does not assume to know which elements are 0 or 1, so it does the full mupltiplication
0257   void MultHelixPropTranspFull(const MPlexLL& A, const MPlexLL& B, MPlexLS& C) {
0258 #pragma omp simd
0259     for (int n = 0; n < NN; ++n) {
0260       for (int i = 0; i < 6; ++i) {
0261         for (int j = 0; j < 6; ++j) {
0262           C(n, i, j) = 0.;
0263           for (int k = 0; k < 6; ++k)
0264             C(n, i, j) += B.constAt(n, i, k) * A.constAt(n, j, k);
0265         }
0266       }
0267     }
0268   }
0269
0270   // this version does not assume to know which elements are 0 or 1, so it does the full mupltiplication
0271   void MultHelixPropTranspFull(const MPlexLL& A, const MPlexLL& B, MPlexLL& C) {
0272 #pragma omp simd
0273     for (int n = 0; n < NN; ++n) {
0274       for (int i = 0; i < 6; ++i) {
0275         for (int j = 0; j < 6; ++j) {
0276           C(n, i, j) = 0.;
0277           for (int k = 0; k < 6; ++k)
0278             C(n, i, j) += B.constAt(n, i, k) * A.constAt(n, j, k);
0279         }
0280       }
0281     }
0282   }
0283 #endif
0284 }  // end unnamed namespace
0285
0286 //==============================================================================
0287
0288 namespace mkfit {
0289
0290   void helixAtRFromIterativeCCSFullJac(const MPlexLV& inPar,
0291                                        const MPlexQI& inChg,
0292                                        const MPlexQF& msRad,
0293                                        MPlexLV& outPar,
0294                                        MPlexLL& errorProp,
0295                                        const int N_proc) {
0296     errorProp.setVal(0.f);
0297     MPlexLL errorPropTmp(0.f);   //initialize to zero
0298     MPlexLL errorPropSwap(0.f);  //initialize to zero
0299
0300 #pragma omp simd
0301     for (int n = 0; n < NN; ++n) {
0302       //initialize erroProp to identity matrix
0303       errorProp(n, 0, 0) = 1.f;
0304       errorProp(n, 1, 1) = 1.f;
0305       errorProp(n, 2, 2) = 1.f;
0306       errorProp(n, 3, 3) = 1.f;
0307       errorProp(n, 4, 4) = 1.f;
0308       errorProp(n, 5, 5) = 1.f;
0309
0310       const float k = inChg.constAt(n, 0, 0) * 100.f / (-Const::sol * Config::Bfield);
0311       const float r = msRad.constAt(n, 0, 0);
0312       float r0 = hipo(inPar.constAt(n, 0, 0), inPar.constAt(n, 1, 0));
0313
0314       if (std::abs(r - r0) < 0.0001f) {
0315         dprint_np(n, "distance less than 1mum, skip");
0316         continue;
0317       }
0318
0319       const float ipt = inPar.constAt(n, 3, 0);
0320       const float phiin = inPar.constAt(n, 4, 0);
0321       const float theta = inPar.constAt(n, 5, 0);
0322
0323       //set those that are 1. before iterations
0324       errorPropTmp(n, 2, 2) = 1.f;
0325       errorPropTmp(n, 3, 3) = 1.f;
0326       errorPropTmp(n, 4, 4) = 1.f;
0327       errorPropTmp(n, 5, 5) = 1.f;
0328
0329       float cosah = 0., sinah = 0.;
0330       //no trig approx here, phi and theta can be large
0331       float cosP = std::cos(phiin), sinP = std::sin(phiin);
0332       const float cosT = std::cos(theta), sinT = std::sin(theta);
0333       float pxin = cosP / ipt;
0334       float pyin = sinP / ipt;
0335
0336       CMS_UNROLL_LOOP_COUNT(Config::Niter)
0337       for (int i = 0; i < Config::Niter; ++i) {
0338         dprint_np(n,
0339                   std::endl
0340                       << "attempt propagation from r=" << r0 << " to r=" << r << std::endl
0341                       << "x=" << outPar.At(n, 0, 0) << " y=" << outPar.At(n, 1, 0) << " z=" << outPar.At(n, 2, 0)
0342                       << " px=" << std::cos(phiin) / ipt << " py=" << std::sin(phiin) / ipt
0343                       << " pz=" << 1.f / (ipt * tan(theta)) << " q=" << inChg.constAt(n, 0, 0) << std::endl);
0344
0345         r0 = hipo(outPar.constAt(n, 0, 0), outPar.constAt(n, 1, 0));
0346         const float ialpha = (r - r0) * ipt / k;
0347         //alpha+=ialpha;
0348
0349         if constexpr (Config::useTrigApprox) {
0350           sincos4(ialpha * 0.5f, sinah, cosah);
0351         } else {
0352           cosah = std::cos(ialpha * 0.5f);
0353           sinah = std::sin(ialpha * 0.5f);
0354         }
0355         const float cosa = 1.f - 2.f * sinah * sinah;
0356         const float sina = 2.f * sinah * cosah;
0357
0358         //derivatives of alpha
0359         const float dadx = -outPar.At(n, 0, 0) * ipt / (k * r0);
0360         const float dady = -outPar.At(n, 1, 0) * ipt / (k * r0);
0361         const float dadipt = (r - r0) / k;
0362
0363         outPar.At(n, 0, 0) = outPar.constAt(n, 0, 0) + 2.f * k * sinah * (pxin * cosah - pyin * sinah);
0364         outPar.At(n, 1, 0) = outPar.constAt(n, 1, 0) + 2.f * k * sinah * (pyin * cosah + pxin * sinah);
0365         const float pxinold = pxin;  //copy before overwriting
0366         pxin = pxin * cosa - pyin * sina;
0367         pyin = pyin * cosa + pxinold * sina;
0368
0369         //need phi at origin, so this goes before redefining phi
0370         //no trig approx here, phi can be large
0371         cosP = std::cos(outPar.At(n, 4, 0));
0372         sinP = std::sin(outPar.At(n, 4, 0));
0373
0374         outPar.At(n, 2, 0) = outPar.constAt(n, 2, 0) + k * ialpha * cosT / (ipt * sinT);
0375         outPar.At(n, 3, 0) = ipt;
0376         outPar.At(n, 4, 0) = outPar.constAt(n, 4, 0) + ialpha;
0377         outPar.At(n, 5, 0) = theta;
0378
0379         errorPropTmp(n, 0, 0) = 1.f + k * (cosP * dadx * cosa - sinP * dadx * sina) / ipt;
0380         errorPropTmp(n, 0, 1) = k * (cosP * dady * cosa - sinP * dady * sina) / ipt;
0381         errorPropTmp(n, 0, 3) =
0382             k * (cosP * (ipt * dadipt * cosa - sina) + sinP * ((1.f - cosa) - ipt * dadipt * sina)) / (ipt * ipt);
0383         errorPropTmp(n, 0, 4) = -k * (sinP * sina + cosP * (1.f - cosa)) / ipt;
0384
0385         errorPropTmp(n, 1, 0) = k * (sinP * dadx * cosa + cosP * dadx * sina) / ipt;
0386         errorPropTmp(n, 1, 1) = 1.f + k * (sinP * dady * cosa + cosP * dady * sina) / ipt;
0387         errorPropTmp(n, 1, 3) =
0388             k * (sinP * (ipt * dadipt * cosa - sina) + cosP * (ipt * dadipt * sina - (1.f - cosa))) / (ipt * ipt);
0389         errorPropTmp(n, 1, 4) = k * (cosP * sina - sinP * (1.f - cosa)) / ipt;
0390
0391         errorPropTmp(n, 2, 0) = k * cosT * dadx / (ipt * sinT);
0392         errorPropTmp(n, 2, 1) = k * cosT * dady / (ipt * sinT);
0393         errorPropTmp(n, 2, 3) = k * cosT * (ipt * dadipt - ialpha) / (ipt * ipt * sinT);
0394         errorPropTmp(n, 2, 5) = -k * ialpha / (ipt * sinT * sinT);
0395
0396         errorPropTmp(n, 4, 0) = dadx;
0397         errorPropTmp(n, 4, 1) = dady;
0398         errorPropTmp(n, 4, 3) = dadipt;
0399
0400         MultHelixPropTemp(errorProp, errorPropTmp, errorPropSwap, n);
0401         errorProp = errorPropSwap;
0402       }
0403
0404       dprint_np(
0405           n,
0406           "propagation end, dump parameters"
0407               << std::endl
0408               << "pos = " << outPar.At(n, 0, 0) << " " << outPar.At(n, 1, 0) << " " << outPar.At(n, 2, 0) << std::endl
0409               << "mom = " << std::cos(outPar.At(n, 4, 0)) / outPar.At(n, 3, 0) << " "
0410               << std::sin(outPar.At(n, 4, 0)) / outPar.At(n, 3, 0) << " "
0411               << 1. / (outPar.At(n, 3, 0) * tan(outPar.At(n, 5, 0)))
0412               << " r=" << std::sqrt(outPar.At(n, 0, 0) * outPar.At(n, 0, 0) + outPar.At(n, 1, 0) * outPar.At(n, 1, 0))
0413               << " pT=" << 1. / std::abs(outPar.At(n, 3, 0)) << std::endl);
0414
0415 #ifdef DEBUG
0416       if (n < N_proc) {
0417         dmutex_guard;
0418         std::cout << n << " jacobian" << std::endl;
0419         printf("%5f %5f %5f %5f %5f %5f\n",
0420                errorProp(n, 0, 0),
0421                errorProp(n, 0, 1),
0422                errorProp(n, 0, 2),
0423                errorProp(n, 0, 3),
0424                errorProp(n, 0, 4),
0425                errorProp(n, 0, 5));
0426         printf("%5f %5f %5f %5f %5f %5f\n",
0427                errorProp(n, 1, 0),
0428                errorProp(n, 1, 1),
0429                errorProp(n, 1, 2),
0430                errorProp(n, 1, 3),
0431                errorProp(n, 1, 4),
0432                errorProp(n, 1, 5));
0433         printf("%5f %5f %5f %5f %5f %5f\n",
0434                errorProp(n, 2, 0),
0435                errorProp(n, 2, 1),
0436                errorProp(n, 2, 2),
0437                errorProp(n, 2, 3),
0438                errorProp(n, 2, 4),
0439                errorProp(n, 2, 5));
0440         printf("%5f %5f %5f %5f %5f %5f\n",
0441                errorProp(n, 3, 0),
0442                errorProp(n, 3, 1),
0443                errorProp(n, 3, 2),
0444                errorProp(n, 3, 3),
0445                errorProp(n, 3, 4),
0446                errorProp(n, 3, 5));
0447         printf("%5f %5f %5f %5f %5f %5f\n",
0448                errorProp(n, 4, 0),
0449                errorProp(n, 4, 1),
0450                errorProp(n, 4, 2),
0451                errorProp(n, 4, 3),
0452                errorProp(n, 4, 4),
0453                errorProp(n, 4, 5));
0454         printf("%5f %5f %5f %5f %5f %5f\n",
0455                errorProp(n, 5, 0),
0456                errorProp(n, 5, 1),
0457                errorProp(n, 5, 2),
0458                errorProp(n, 5, 3),
0459                errorProp(n, 5, 4),
0460                errorProp(n, 5, 5));
0461       }
0462 #endif
0463     }
0464   }
0465
0466 }  // end namespace mkfit
0467
0468 //#pragma omp declare simd simdlen(NN) notinbranch linear(n)
0469 #include "PropagationMPlex.icc"
0470
0471 namespace mkfit {
0472
0473   void helixAtRFromIterativeCCS(const MPlexLV& inPar,
0474                                 const MPlexQI& inChg,
0475                                 const MPlexQF& msRad,
0476                                 MPlexLV& outPar,
0477                                 MPlexLL& errorProp,
0478                                 MPlexQI& outFailFlag,
0479                                 const int N_proc,
0480                                 const PropagationFlags pflags) {
0481     errorProp.setVal(0.f);
0482     outFailFlag.setVal(0.f);
0483
0484     helixAtRFromIterativeCCS_impl(inPar, inChg, msRad, outPar, errorProp, outFailFlag, 0, NN, N_proc, pflags);
0485   }
0486
0487   void propagateHelixToRMPlex(const MPlexLS& inErr,
0488                               const MPlexLV& inPar,
0489                               const MPlexQI& inChg,
0490                               const MPlexQF& msRad,
0491                               MPlexLS& outErr,
0492                               MPlexLV& outPar,
0493                               const int N_proc,
0494                               const PropagationFlags pflags,
0495                               const MPlexQI* noMatEffPtr) {
0496     // bool debug = true;
0497
0498     // This is used further down when calculating similarity with errorProp (and before in DEBUG).
0499     // MT: I don't think this really needed if we use inErr where required.
0500     outErr = inErr;
0501     // This requirement for helixAtRFromIterativeCCS_impl() and for helixAtRFromIterativeCCSFullJac().
0502     // MT: This should be properly handled in both functions (expecting input in output parameters sucks).
0503     outPar = inPar;
0504
0505     MPlexLL errorProp;
0506     MPlexQI failFlag;
0507
0508     helixAtRFromIterativeCCS(inPar, inChg, msRad, outPar, errorProp, failFlag, N_proc, pflags);
0509
0510 #ifdef DEBUG
0511     {
0512       for (int kk = 0; kk < N_proc; ++kk) {
0513         dprintf("outErr before prop %d\n", kk);
0514         for (int i = 0; i < 6; ++i) {
0515           for (int j = 0; j < 6; ++j)
0516             dprintf("%8f ", outErr.At(kk, i, j));
0517           printf("\n");
0518         }
0519         dprintf("\n");
0520
0521         dprintf("errorProp %d\n", kk);
0522         for (int i = 0; i < 6; ++i) {
0523           for (int j = 0; j < 6; ++j)
0524             dprintf("%8f ", errorProp.At(kk, i, j));
0525           printf("\n");
0526         }
0527         dprintf("\n");
0528       }
0529     }
0530 #endif
0531
0532     if (pflags.apply_material) {
0533       MPlexQF hitsRl;
0534       MPlexQF hitsXi;
0535       MPlexQF propSign;
0536 #pragma omp simd
0537       for (int n = 0; n < N_proc; ++n) {
0538         if (failFlag(n, 0, 0) || (noMatEffPtr && noMatEffPtr->constAt(n, 0, 0))) {
0539           hitsRl(n, 0, 0) = 0.f;
0540           hitsXi(n, 0, 0) = 0.f;
0541         } else {
0542           const int zbin = Config::materialEff.getZbin(outPar(n, 2, 0));
0543           const int rbin = Config::materialEff.getRbin(msRad(n, 0, 0));
0544           hitsRl(n, 0, 0) = (zbin >= 0 && zbin < Config::nBinsZME && rbin >= 0 && rbin < Config::nBinsRME)
0545                                 ? Config::materialEff.getRlVal(zbin, rbin)
0546                                 : 0.f;  // protect against crazy propagations
0547           hitsXi(n, 0, 0) = (zbin >= 0 && zbin < Config::nBinsZME && rbin >= 0 && rbin < Config::nBinsRME)
0548                                 ? Config::materialEff.getXiVal(zbin, rbin)
0549                                 : 0.f;  // protect against crazy propagations
0550         }
0551         const float r0 = hipo(inPar(n, 0, 0), inPar(n, 1, 0));
0552         const float r = msRad(n, 0, 0);
0553         propSign(n, 0, 0) = (r > r0 ? 1. : -1.);
0554       }
0555       applyMaterialEffects(hitsRl, hitsXi, propSign, outErr, outPar, N_proc, true);
0556     }
0557
0558     squashPhiMPlex(outPar, N_proc);  // ensure phi is between |pi|
0559
0560     // Matriplex version of:
0561     // result.errors = ROOT::Math::Similarity(errorProp, outErr);
0562
0563     // MultHelixProp can be optimized for CCS coordinates, see GenMPlexOps.pl
0564     MPlexLL temp;
0565     MultHelixProp(errorProp, outErr, temp);
0566     MultHelixPropTransp(errorProp, temp, outErr);
0567
0568     /*
0569      // To be used with: MPT_DIM = 1
0570      if (fabs(sqrt(outPar[0]*outPar[0]+outPar[1]*outPar[1]) - msRad[0]) > 0.0001)
0571      {
0572        std::cout << "DID NOT GET TO R, FailFlag=" << failFlag[0]
0573                  << " dR=" << msRad[0] - std::hypot(outPar[0],outPar[1])
0574                  << " r="  << msRad[0] << " rin=" << std::hypot(inPar[0],inPar[1]) << " rout=" << std::hypot(outPar[0],outPar[1])
0575                  << std::endl;
0576        // std::cout << "    pt=" << pt << " pz=" << inPar.At(n, 2) << std::endl;
0577      }
0578    */
0579
0580     // FIXUP BOTCHED (low pT) propagations.
0581     // For now let's enforce reseting output to input for failed cases. But:
0582     // - perhaps this should be optional;
0583     // - alternatively, it could also be an extra output parameter;
0584     // - if we pass fail outwards, we might *not* need to also reset botched output.
0585     for (int i = 0; i < N_proc; ++i) {
0586       if (failFlag(i, 0, 0)) {
0587         outPar.copySlot(i, inPar);
0588         outErr.copySlot(i, inErr);
0589       }
0590     }
0591   }
0592
0593   //==============================================================================
0594
0595   void propagateHelixToZMPlex(const MPlexLS& inErr,
0596                               const MPlexLV& inPar,
0597                               const MPlexQI& inChg,
0598                               const MPlexQF& msZ,
0599                               MPlexLS& outErr,
0600                               MPlexLV& outPar,
0601                               const int N_proc,
0602                               const PropagationFlags pflags,
0603                               const MPlexQI* noMatEffPtr) {
0604     // debug = true;
0605
0606     outErr = inErr;
0607     outPar = inPar;
0608
0609     MPlexLL errorProp;
0610
0611     helixAtZ(inPar, inChg, msZ, outPar, errorProp, N_proc, pflags);
0612
0613 #ifdef DEBUG
0614     {
0615       for (int kk = 0; kk < N_proc; ++kk) {
0616         dprintf("inErr %d\n", kk);
0617         for (int i = 0; i < 6; ++i) {
0618           for (int j = 0; j < 6; ++j)
0619             dprintf("%8f ", inErr.constAt(kk, i, j));
0620           printf("\n");
0621         }
0622         dprintf("\n");
0623
0624         dprintf("errorProp %d\n", kk);
0625         for (int i = 0; i < 6; ++i) {
0626           for (int j = 0; j < 6; ++j)
0627             dprintf("%8f ", errorProp.At(kk, i, j));
0628           printf("\n");
0629         }
0630         dprintf("\n");
0631       }
0632     }
0633 #endif
0634
0635     if (pflags.apply_material) {
0636       MPlexQF hitsRl;
0637       MPlexQF hitsXi;
0638       MPlexQF propSign;
0639 #pragma omp simd
0640       for (int n = 0; n < N_proc; ++n) {
0641         if (noMatEffPtr && noMatEffPtr->constAt(n, 0, 0)) {
0642           hitsRl(n, 0, 0) = 0.f;
0643           hitsXi(n, 0, 0) = 0.f;
0644         } else {
0645           const int zbin = Config::materialEff.getZbin(msZ(n, 0, 0));
0646           const int rbin = Config::materialEff.getRbin(std::hypot(outPar(n, 0, 0), outPar(n, 1, 0)));
0647           hitsRl(n, 0, 0) = (zbin >= 0 && zbin < Config::nBinsZME && rbin >= 0 && rbin < Config::nBinsRME)
0648                                 ? Config::materialEff.getRlVal(zbin, rbin)
0649                                 : 0.f;  // protect against crazy propagations
0650           hitsXi(n, 0, 0) = (zbin >= 0 && zbin < Config::nBinsZME && rbin >= 0 && rbin < Config::nBinsRME)
0651                                 ? Config::materialEff.getXiVal(zbin, rbin)
0652                                 : 0.f;  // protect against crazy propagations
0653         }
0654         const float zout = msZ.constAt(n, 0, 0);
0655         const float zin = inPar.constAt(n, 2, 0);
0656         propSign(n, 0, 0) = (std::abs(zout) > std::abs(zin) ? 1. : -1.);
0657       }
0658       applyMaterialEffects(hitsRl, hitsXi, propSign, outErr, outPar, N_proc, false);
0659     }
0660
0661     squashPhiMPlex(outPar, N_proc);  // ensure phi is between |pi|
0662
0663     // Matriplex version of:
0664     // result.errors = ROOT::Math::Similarity(errorProp, outErr);
0665     MPlexLL temp;
0666     MultHelixPropEndcap(errorProp, outErr, temp);
0667     MultHelixPropTranspEndcap(errorProp, temp, outErr);
0668
0669     // This dump is now out of its place as similarity is done with matriplex ops.
0670     /*
0671 #ifdef DEBUG
0672    {
0673      dmutex_guard;
0674      for (int kk = 0; kk < N_proc; ++kk)
0675      {
0676        dprintf("outErr %d\n", kk);
0677        for (int i = 0; i < 6; ++i) { for (int j = 0; j < 6; ++j)
0678            dprintf("%8f ", outErr.At(kk,i,j)); printf("\n");
0679        } dprintf("\n");
0680
0681        dprintf("outPar %d\n", kk);
0682        for (int i = 0; i < 6; ++i) {
0683            dprintf("%8f ", outPar.At(kk,i,0)); printf("\n");
0684        } dprintf("\n");
0685        if (std::abs(outPar.At(kk,2,0) - msZ.constAt(kk, 0, 0)) > 0.0001) {
0686          float pt = 1.0f / inPar.constAt(kk,3,0);
0687      dprint_np(kk, "DID NOT GET TO Z, dZ=" << std::abs(outPar.At(kk,2,0) - msZ.constAt(kk, 0, 0))
0688            << " z=" << msZ.constAt(kk, 0, 0) << " zin=" << inPar.constAt(kk,2,0) << " zout=" << outPar.At(kk,2,0) << std::endl
0689            << "pt=" << pt << " pz=" << pt/std::tan(inPar.constAt(kk,5,0)));
0690        }
0691      }
0692    }
0693 #endif
0694    */
0695   }
0696
0697   void helixAtZ(const MPlexLV& inPar,
0698                 const MPlexQI& inChg,
0699                 const MPlexQF& msZ,
0700                 MPlexLV& outPar,
0701                 MPlexLL& errorProp,
0702                 const int N_proc,
0703                 const PropagationFlags pflags) {
0704     errorProp.setVal(0.f);
0705
0706 #pragma omp simd
0707     for (int n = 0; n < NN; ++n) {
0708       //initialize erroProp to identity matrix, except element 2,2 which is zero
0709       errorProp(n, 0, 0) = 1.f;
0710       errorProp(n, 1, 1) = 1.f;
0711       errorProp(n, 3, 3) = 1.f;
0712       errorProp(n, 4, 4) = 1.f;
0713       errorProp(n, 5, 5) = 1.f;
0714     }
0715     float zout[NN];
0716     float zin[NN];
0717     float ipt[NN];
0718     float phiin[NN];
0719     float theta[NN];
0720 #pragma omp simd
0721     for (int n = 0; n < NN; ++n) {
0722       //initialize erroProp to identity matrix, except element 2,2 which is zero
0723       zout[n] = msZ.constAt(n, 0, 0);
0724       zin[n] = inPar.constAt(n, 2, 0);
0725       ipt[n] = inPar.constAt(n, 3, 0);
0726       phiin[n] = inPar.constAt(n, 4, 0);
0727       theta[n] = inPar.constAt(n, 5, 0);
0728     }
0729
0730     float k[NN];
0731     if (pflags.use_param_b_field) {
0732 #pragma omp simd
0733       for (int n = 0; n < NN; ++n) {
0734         k[n] = inChg.constAt(n, 0, 0) * 100.f /
0735                (-Const::sol * Config::bFieldFromZR(zin[n], hipo(inPar.constAt(n, 0, 0), inPar.constAt(n, 1, 0))));
0736       }
0737     } else {
0738 #pragma omp simd
0739       for (int n = 0; n < NN; ++n) {
0740         k[n] = inChg.constAt(n, 0, 0) * 100.f / (-Const::sol * Config::Bfield);
0741       }
0742     }
0743
0744     float kinv[NN];
0745 #pragma omp simd
0746     for (int n = 0; n < NN; ++n) {
0747       kinv[n] = 1.f / k[n];
0748     }
0749
0750 #pragma omp simd
0751     for (int n = 0; n < NN; ++n) {
0752       dprint_np(n,
0753                 std::endl
0754                     << "input parameters"
0755                     << " inPar.constAt(n, 0, 0)=" << std::setprecision(9) << inPar.constAt(n, 0, 0)
0756                     << " inPar.constAt(n, 1, 0)=" << std::setprecision(9) << inPar.constAt(n, 1, 0)
0757                     << " inPar.constAt(n, 2, 0)=" << std::setprecision(9) << inPar.constAt(n, 2, 0)
0758                     << " inPar.constAt(n, 3, 0)=" << std::setprecision(9) << inPar.constAt(n, 3, 0)
0759                     << " inPar.constAt(n, 4, 0)=" << std::setprecision(9) << inPar.constAt(n, 4, 0)
0760                     << " inPar.constAt(n, 5, 0)=" << std::setprecision(9) << inPar.constAt(n, 5, 0));
0761     }
0762
0763     float pt[NN];
0764 #pragma omp simd
0765     for (int n = 0; n < NN; ++n) {
0766       pt[n] = 1.f / ipt[n];
0767     }
0768
0769     //no trig approx here, phi can be large
0770     float cosP[NN];
0771     float sinP[NN];
0772 #pragma omp simd
0773     for (int n = 0; n < NN; ++n) {
0774       cosP[n] = std::cos(phiin[n]);
0775     }
0776
0777 #pragma omp simd
0778     for (int n = 0; n < NN; ++n) {
0779       sinP[n] = std::sin(phiin[n]);
0780     }
0781
0782     float cosT[NN];
0783     float sinT[NN];
0784 #pragma omp simd
0785     for (int n = 0; n < NN; ++n) {
0786       cosT[n] = std::cos(theta[n]);
0787     }
0788
0789 #pragma omp simd
0790     for (int n = 0; n < NN; ++n) {
0791       sinT[n] = std::sin(theta[n]);
0792     }
0793
0794     float tanT[NN];
0795     float icos2T[NN];
0796     float pxin[NN];
0797     float pyin[NN];
0798 #pragma omp simd
0799     for (int n = 0; n < NN; ++n) {
0800       tanT[n] = sinT[n] / cosT[n];
0801       icos2T[n] = 1.f / (cosT[n] * cosT[n]);
0802       pxin[n] = cosP[n] * pt[n];
0803       pyin[n] = sinP[n] * pt[n];
0804     }
0805 #pragma omp simd
0806     for (int n = 0; n < NN; ++n) {
0807       //fixme, make this printout useful for propagation to z
0808       dprint_np(n,
0809                 std::endl
0810                     << "k=" << std::setprecision(9) << k[n] << " pxin=" << std::setprecision(9) << pxin[n]
0811                     << " pyin=" << std::setprecision(9) << pyin[n] << " cosP=" << std::setprecision(9) << cosP[n]
0812                     << " sinP=" << std::setprecision(9) << sinP[n] << " pt=" << std::setprecision(9) << pt[n]);
0813     }
0814     float deltaZ[NN];
0815     float alpha[NN];
0816 #pragma omp simd
0817     for (int n = 0; n < NN; ++n) {
0818       deltaZ[n] = zout[n] - zin[n];
0819       alpha[n] = deltaZ[n] * tanT[n] * ipt[n] * kinv[n];
0820     }
0821
0822     float cosahTmp[NN];
0823     float sinahTmp[NN];
0824     if constexpr (Config::useTrigApprox) {
0825 #if !defined(__INTEL_COMPILER)
0826 #pragma omp simd
0827 #endif
0828       for (int n = 0; n < NN; ++n) {
0829         sincos4(alpha[n] * 0.5f, sinahTmp[n], cosahTmp[n]);
0830       }
0831     } else {
0832 #if !defined(__INTEL_COMPILER)
0833 #pragma omp simd
0834 #endif
0835       for (int n = 0; n < NN; ++n) {
0836         cosahTmp[n] = std::cos(alpha[n] * 0.5f);
0837       }
0838 #if !defined(__INTEL_COMPILER)
0839 #pragma omp simd
0840 #endif
0841       for (int n = 0; n < NN; ++n) {
0842         sinahTmp[n] = std::sin(alpha[n] * 0.5f);
0843       }
0844     }
0845
0846     float cosah[NN];
0847     float sinah[NN];
0848     float cosa[NN];
0849     float sina[NN];
0850 #pragma omp simd
0851     for (int n = 0; n < NN; ++n) {
0852       cosah[n] = cosahTmp[n];
0853       sinah[n] = sinahTmp[n];
0854       cosa[n] = 1.f - 2.f * sinah[n] * sinah[n];
0855       sina[n] = 2.f * sinah[n] * cosah[n];
0856     }
0857 //update parameters
0858 #pragma omp simd
0859     for (int n = 0; n < NN; ++n) {
0860       outPar.At(n, 0, 0) = outPar.At(n, 0, 0) + 2.f * k[n] * sinah[n] * (pxin[n] * cosah[n] - pyin[n] * sinah[n]);
0861       outPar.At(n, 1, 0) = outPar.At(n, 1, 0) + 2.f * k[n] * sinah[n] * (pyin[n] * cosah[n] + pxin[n] * sinah[n]);
0862       outPar.At(n, 2, 0) = zout[n];
0863       outPar.At(n, 4, 0) = phiin[n] + alpha[n];
0864     }
0865
0866 #pragma omp simd
0867     for (int n = 0; n < NN; ++n) {
0868       dprint_np(n,
0869                 std::endl
0870                     << "outPar.At(n, 0, 0)=" << outPar.At(n, 0, 0) << " outPar.At(n, 1, 0)=" << outPar.At(n, 1, 0)
0871                     << " pxin=" << pxin[n] << " pyin=" << pyin[n]);
0872     }
0873
0874     float pxcaMpysa[NN];
0875 #pragma omp simd
0876     for (int n = 0; n < NN; ++n) {
0877       pxcaMpysa[n] = pxin[n] * cosa[n] - pyin[n] * sina[n];
0878     }
0879
0880 #pragma omp simd
0881     for (int n = 0; n < NN; ++n) {
0882       errorProp(n, 0, 2) = -tanT[n] * ipt[n] * pxcaMpysa[n];
0883       errorProp(n, 0, 3) =
0884           k[n] * pt[n] * pt[n] *
0885           (cosP[n] * (alpha[n] * cosa[n] - sina[n]) + sinP[n] * 2.f * sinah[n] * (sinah[n] - alpha[n] * cosah[n]));
0886       errorProp(n, 0, 4) = -2.f * k[n] * pt[n] * sinah[n] * (sinP[n] * cosah[n] + cosP[n] * sinah[n]);
0887       errorProp(n, 0, 5) = deltaZ[n] * ipt[n] * pxcaMpysa[n] * icos2T[n];
0888     }
0889
0890     float pycaPpxsa[NN];
0891 #pragma omp simd
0892     for (int n = 0; n < NN; ++n) {
0893       pycaPpxsa[n] = pyin[n] * cosa[n] + pxin[n] * sina[n];
0894     }
0895
0896 #pragma omp simd
0897     for (int n = 0; n < NN; ++n) {
0898       errorProp(n, 1, 2) = -tanT[n] * ipt[n] * pycaPpxsa[n];
0899       errorProp(n, 1, 3) =
0900           k[n] * pt[n] * pt[n] *
0901           (sinP[n] * (alpha[n] * cosa[n] - sina[n]) - cosP[n] * 2.f * sinah[n] * (sinah[n] - alpha[n] * cosah[n]));
0902       errorProp(n, 1, 4) = 2.f * k[n] * pt[n] * sinah[n] * (cosP[n] * cosah[n] - sinP[n] * sinah[n]);
0903       errorProp(n, 1, 5) = deltaZ[n] * ipt[n] * pycaPpxsa[n] * icos2T[n];
0904     }
0905
0906 #pragma omp simd
0907     for (int n = 0; n < NN; ++n) {
0908       errorProp(n, 4, 2) = -ipt[n] * tanT[n] * kinv[n];
0909       errorProp(n, 4, 3) = tanT[n] * deltaZ[n] * kinv[n];
0910       errorProp(n, 4, 5) = ipt[n] * deltaZ[n] * kinv[n] * icos2T[n];
0911     }
0912
0913 #pragma omp simd
0914     for (int n = 0; n < NN; ++n) {
0915       dprint_np(
0916           n,
0917           "propagation end, dump parameters"
0918               << std::endl
0919               << "pos = " << outPar.At(n, 0, 0) << " " << outPar.At(n, 1, 0) << " " << outPar.At(n, 2, 0) << std::endl
0920               << "mom = " << std::cos(outPar.At(n, 4, 0)) / outPar.At(n, 3, 0) << " "
0921               << std::sin(outPar.At(n, 4, 0)) / outPar.At(n, 3, 0) << " "
0922               << 1. / (outPar.At(n, 3, 0) * tan(outPar.At(n, 5, 0)))
0923               << " r=" << std::sqrt(outPar.At(n, 0, 0) * outPar.At(n, 0, 0) + outPar.At(n, 1, 0) * outPar.At(n, 1, 0))
0924               << " pT=" << 1. / std::abs(outPar.At(n, 3, 0)) << std::endl);
0925     }
0926
0927 #ifdef DEBUG
0928 #pragma omp simd
0929     for (int n = 0; n < NN; ++n) {
0930       if (n < N_proc) {
0931         dmutex_guard;
0932         std::cout << n << ": jacobian" << std::endl;
0933         printf("%5f %5f %5f %5f %5f %5f\n",
0934                errorProp(n, 0, 0),
0935                errorProp(n, 0, 1),
0936                errorProp(n, 0, 2),
0937                errorProp(n, 0, 3),
0938                errorProp(n, 0, 4),
0939                errorProp(n, 0, 5));
0940         printf("%5f %5f %5f %5f %5f %5f\n",
0941                errorProp(n, 1, 0),
0942                errorProp(n, 1, 1),
0943                errorProp(n, 1, 2),
0944                errorProp(n, 1, 3),
0945                errorProp(n, 1, 4),
0946                errorProp(n, 1, 5));
0947         printf("%5f %5f %5f %5f %5f %5f\n",
0948                errorProp(n, 2, 0),
0949                errorProp(n, 2, 1),
0950                errorProp(n, 2, 2),
0951                errorProp(n, 2, 3),
0952                errorProp(n, 2, 4),
0953                errorProp(n, 2, 5));
0954         printf("%5f %5f %5f %5f %5f %5f\n",
0955                errorProp(n, 3, 0),
0956                errorProp(n, 3, 1),
0957                errorProp(n, 3, 2),
0958                errorProp(n, 3, 3),
0959                errorProp(n, 3, 4),
0960                errorProp(n, 3, 5));
0961         printf("%5f %5f %5f %5f %5f %5f\n",
0962                errorProp(n, 4, 0),
0963                errorProp(n, 4, 1),
0964                errorProp(n, 4, 2),
0965                errorProp(n, 4, 3),
0966                errorProp(n, 4, 4),
0967                errorProp(n, 4, 5));
0968         printf("%5f %5f %5f %5f %5f %5f\n",
0969                errorProp(n, 5, 0),
0970                errorProp(n, 5, 1),
0971                errorProp(n, 5, 2),
0972                errorProp(n, 5, 3),
0973                errorProp(n, 5, 4),
0974                errorProp(n, 5, 5));
0975       }
0976     }
0977 #endif
0978   }
0979
0980   //==============================================================================
0981
0982   void applyMaterialEffects(const MPlexQF& hitsRl,
0983                             const MPlexQF& hitsXi,
0984                             const MPlexQF& propSign,
0985                             MPlexLS& outErr,
0986                             MPlexLV& outPar,
0987                             const int N_proc,
0988                             const bool isBarrel) {
0989 #pragma omp simd
0990     for (int n = 0; n < NN; ++n) {
0991       float radL = hitsRl.constAt(n, 0, 0);
0992       if (radL < 1e-13f)
0993         continue;  //ugly, please fixme
0994       const float theta = outPar.constAt(n, 5, 0);
0995       const float pt = 1.f / outPar.constAt(n, 3, 0);  //fixme, make sure it is positive?
0996       const float p = pt / std::sin(theta);
0997       const float p2 = p * p;
0998       constexpr float mpi = 0.140;       // m=140 MeV, pion
0999       constexpr float mpi2 = mpi * mpi;  // m=140 MeV, pion
1000       const float beta2 = p2 / (p2 + mpi2);
1001       const float beta = std::sqrt(beta2);
1002       //radiation lenght, corrected for the crossing angle (cos alpha from dot product of radius vector and momentum)
1003       const float invCos = (isBarrel ? p / pt : 1.f / std::abs(std::cos(theta)));
1004       radL = radL * invCos;  //fixme works only for barrel geom
1005       // multiple scattering
1006       //vary independently phi and theta by the rms of the planar multiple scattering angle
1007       // XXX-KMD radL < 0, see your fixme above! Repeating bailout
1008       if (radL < 1e-13f)
1009         continue;
1010       // const float thetaMSC = 0.0136f*std::sqrt(radL)*(1.f+0.038f*std::log(radL))/(beta*p);// eq 32.15
1011       // const float thetaMSC2 = thetaMSC*thetaMSC;
1012       const float thetaMSC = 0.0136f * (1.f + 0.038f * std::log(radL)) / (beta * p);  // eq 32.15
1013       const float thetaMSC2 = thetaMSC * thetaMSC * radL;
1014       outErr.At(n, 4, 4) += thetaMSC2;
1015       // outErr.At(n, 4, 5) += thetaMSC2;
1016       outErr.At(n, 5, 5) += thetaMSC2;
1017       //std::cout << "beta=" << beta << " p=" << p << std::endl;
1018       //std::cout << "multiple scattering thetaMSC=" << thetaMSC << " thetaMSC2=" << thetaMSC2 << " radL=" << radL << std::endl;
1019       // energy loss
1020       // XXX-KMD beta2 = 1 => 1 / sqrt(0)
1021       // const float gamma = 1.f/std::sqrt(1.f - std::min(beta2, 0.999999f));
1022       // const float gamma2 = gamma*gamma;
1023       const float gamma2 = (p2 + mpi2) / mpi2;
1024       const float gamma = std::sqrt(gamma2);  //1.f/std::sqrt(1.f - std::min(beta2, 0.999999f));
1025       constexpr float me = 0.0005;            // m=0.5 MeV, electron
1026       const float wmax = 2.f * me * beta2 * gamma2 / (1.f + 2.f * gamma * me / mpi + me * me / (mpi * mpi));
1027       constexpr float I = 16.0e-9 * 10.75;
1028       const float deltahalf = std::log(28.816e-9f * std::sqrt(2.33f * 0.498f) / I) + std::log(beta * gamma) - 0.5f;
1029       const float dEdx =
1030           beta < 1.f
1031               ? (2.f * (hitsXi.constAt(n, 0, 0) * invCos *
1032                         (0.5f * std::log(2.f * me * beta2 * gamma2 * wmax / (I * I)) - beta2 - deltahalf) / beta2))
1033               : 0.f;  //protect against infs and nans
1034       // dEdx = dEdx*2.;//xi in cmssw is defined with an extra factor 0.5 with respect to formula 27.1 in pdg
1035       //std::cout << "dEdx=" << dEdx << " delta=" << deltahalf << " wmax=" << wmax << " Xi=" << hitsXi.constAt(n,0,0) << std::endl;
1036       const float dP = propSign.constAt(n, 0, 0) * dEdx / beta;
1037       outPar.At(n, 3, 0) = p / (std::max(p + dP, 0.001f) * pt);  //stay above 1MeV
1038       //assume 100% uncertainty
1039       outErr.At(n, 3, 3) += dP * dP / (p2 * pt * pt);
1040     }
1041   }
1042
1043 }  // end namespace mkfit