Back to home page

Project CMSSW displayed by LXR
 
 

    

File indexing completed on 2025-01-18 03:42:21 

0001 #include "FWCore/Utilities/interface/CMSUnrollLoop.h"
0002 #include "RecoTracker/MkFitCore/interface/PropagationConfig.h"
0003 #include "RecoTracker/MkFitCore/interface/Config.h"
0004 #include "RecoTracker/MkFitCore/interface/TrackerInfo.h"
0005 #include "RecoTracker/MkFitCore/interface/cms_common_macros.h"
0006 
0007 #include "PropagationMPlex.h"
0008 
0009 //#define DEBUG
0010 #include "Debug.h"
0011 
0012 namespace {
0013   using namespace mkfit;
0014 
0015   void MultHelixPlaneProp(const MPlexLL& A, const MPlexLS& B, MPlexLL& C) {
0016     // C = A * B
0017 
0018     typedef float T;
0019     const Matriplex::idx_t N = NN;
0020 
0021     const T* a = A.fArray;
0022     ASSUME_ALIGNED(a, 64);
0023     const T* b = B.fArray;
0024     ASSUME_ALIGNED(b, 64);
0025     T* c = C.fArray;
0026     ASSUME_ALIGNED(c, 64);
0027 
0028 #include "MultHelixPlaneProp.ah"
0029   }
0030 
0031   void MultHelixPlanePropTransp(const MPlexLL& A, const MPlexLL& B, MPlexLS& C) {
0032     // C = B * AT;
0033 
0034     typedef float T;
0035     const Matriplex::idx_t N = NN;
0036 
0037     const T* a = A.fArray;
0038     ASSUME_ALIGNED(a, 64);
0039     const T* b = B.fArray;
0040     ASSUME_ALIGNED(b, 64);
0041     T* c = C.fArray;
0042     ASSUME_ALIGNED(c, 64);
0043 
0044 #include "MultHelixPlanePropTransp.ah"
0045   }
0046 
0047 }  // namespace
0048 
0049 // ============================================================================
0050 // BEGIN STUFF FROM PropagationMPlex.icc
0051 namespace {
0052 
0053   using MPF = MPlexQF;
0054 
0055   MPF getBFieldFromZXY(const MPF& z, const MPF& x, const MPF& y) {
0056     MPF b;
0057     for (int n = 0; n < NN; ++n)
0058       b[n] = Config::bFieldFromZR(z[n], hipo(x[n], y[n]));
0059     return b;
0060   }
0061 
0062   void JacErrPropCurv1(const MPlex65& A, const MPlex55& B, MPlex65& C) {
0063     // C = A * B
0064     typedef float T;
0065     const Matriplex::idx_t N = NN;
0066 
0067     const T* a = A.fArray;
0068     ASSUME_ALIGNED(a, 64);
0069     const T* b = B.fArray;
0070     ASSUME_ALIGNED(b, 64);
0071     T* c = C.fArray;
0072     ASSUME_ALIGNED(c, 64);
0073 
0074 #include "JacErrPropCurv1.ah"
0075   }
0076 
0077   void JacErrPropCurv2(const MPlex65& A, const MPlex56& B, MPlexLL& __restrict__ C) {
0078     // C = A * B
0079     typedef float T;
0080     const Matriplex::idx_t N = NN;
0081 
0082     const T* a = A.fArray;
0083     ASSUME_ALIGNED(a, 64);
0084     const T* b = B.fArray;
0085     ASSUME_ALIGNED(b, 64);
0086     T* c = C.fArray;
0087     ASSUME_ALIGNED(c, 64);
0088 
0089 #include "JacErrPropCurv2.ah"
0090   }
0091 
0092   void parsFromPathL_impl(const MPlexLV& __restrict__ inPar,
0093                           MPlexLV& __restrict__ outPar,
0094                           const MPlexQF& __restrict__ kinv,
0095                           const MPlexQF& __restrict__ s) {
0096     namespace mpt = Matriplex;
0097     using MPF = MPlexQF;
0098 
0099     MPF alpha = s * mpt::fast_sin(inPar(5, 0)) * inPar(3, 0) * kinv;
0100 
0101     MPF sinah, cosah;
0102     if constexpr (Config::useTrigApprox) {
0103       mpt::sincos4(0.5f * alpha, sinah, cosah);
0104     } else {
0105       mpt::fast_sincos(0.5f * alpha, sinah, cosah);
0106     }
0107 
0108     MPF sin_mom_phi, cos_mom_phi;
0109     mpt::fast_sincos(inPar(4, 0), sin_mom_phi, cos_mom_phi);
0110 
0111     MPF sin_mom_tht, cos_mom_tht;
0112     mpt::fast_sincos(inPar(5, 0), sin_mom_tht, cos_mom_tht);
0113 
0114     outPar.aij(0, 0) = inPar(0, 0) + 2.f * sinah * (cos_mom_phi * cosah - sin_mom_phi * sinah) / (inPar(3, 0) * kinv);
0115     outPar.aij(1, 0) = inPar(1, 0) + 2.f * sinah * (sin_mom_phi * cosah + cos_mom_phi * sinah) / (inPar(3, 0) * kinv);
0116     outPar.aij(2, 0) = inPar(2, 0) + alpha / kinv * cos_mom_tht / (inPar(3, 0) * sin_mom_tht);
0117     outPar.aij(3, 0) = inPar(3, 0);
0118     outPar.aij(4, 0) = inPar(4, 0) + alpha;
0119     outPar.aij(5, 0) = inPar(5, 0);
0120   }
0121 
0122   //*****************************************************************************************************
0123 
0124   //should kinv and D be templated???
0125   void parsAndErrPropFromPathL_impl(const MPlexLV& __restrict__ inPar,
0126                                     const MPlexQI& __restrict__ inChg,
0127                                     MPlexLV& __restrict__ outPar,
0128                                     const MPlexQF& __restrict__ kinv,
0129                                     const MPlexQF& __restrict__ s,
0130                                     MPlexLL& __restrict__ errorProp,
0131                                     const int N_proc,
0132                                     const PropagationFlags& pf) {
0133     //iteration should return the path length s, then update parameters and compute errors
0134 
0135     namespace mpt = Matriplex;
0136     using MPF = MPlexQF;
0137 
0138     parsFromPathL_impl(inPar, outPar, kinv, s);
0139 
0140     MPF sinPin, cosPin;
0141     mpt::fast_sincos(inPar(4, 0), sinPin, cosPin);
0142     MPF sinPout, cosPout;
0143     mpt::fast_sincos(outPar(4, 0), sinPout, cosPout);
0144     MPF sinT, cosT;
0145     mpt::fast_sincos(inPar(5, 0), sinT, cosT);
0146 
0147     // use code from AnalyticalCurvilinearJacobian::computeFullJacobian for error propagation in curvilinear coordinates, then convert to CCS
0148     // main difference from the above function is that we assume that the magnetic field is purely along z (which also implies that there is no change in pz)
0149     // this simplifies significantly the code
0150 
0151     MPlex55 errorPropCurv;
0152 
0153     const MPF qbp = mpt::negate_if_ltz(sinT * inPar(3, 0), inChg);
0154     // calculate transport matrix
0155     // Origin: TRPRFN
0156     const MPF t11 = cosPin * sinT;
0157     const MPF t12 = sinPin * sinT;
0158     const MPF t21 = cosPout * sinT;
0159     const MPF t22 = sinPout * sinT;
0160     const MPF cosl1 = 1.f / sinT;
0161     // define average magnetic field and gradient
0162     // at initial point - inlike TRPRFN
0163     const MPF bF = (pf.use_param_b_field ? Const::sol_over_100 * getBFieldFromZXY(inPar(2, 0), inPar(0, 0), inPar(1, 0))
0164                                          : Const::sol_over_100 * Config::Bfield);
0165     const MPF q = -bF * qbp;
0166     const MPF theta = q * s;
0167     MPF sint, cost;
0168     mpt::fast_sincos(theta, sint, cost);
0169     const MPF dx1 = inPar(0, 0) - outPar(0, 0);
0170     const MPF dx2 = inPar(1, 0) - outPar(1, 0);
0171     const MPF dx3 = inPar(2, 0) - outPar(2, 0);
0172     MPF au = mpt::fast_isqrt(t11 * t11 + t12 * t12);
0173     const MPF u11 = -au * t12;
0174     const MPF u12 = au * t11;
0175     const MPF v11 = -cosT * u12;
0176     const MPF v12 = cosT * u11;
0177     const MPF v13 = t11 * u12 - t12 * u11;
0178     au = mpt::fast_isqrt(t21 * t21 + t22 * t22);
0179     const MPF u21 = -au * t22;
0180     const MPF u22 = au * t21;
0181     const MPF v21 = -cosT * u22;
0182     const MPF v22 = cosT * u21;
0183     const MPF v23 = t21 * u22 - t22 * u21;
0184     // now prepare the transport matrix
0185     const MPF omcost = 1.f - cost;
0186     const MPF tmsint = theta - sint;
0187     //   1/p - doesn't change since |p1| = |p2|
0188     errorPropCurv.aij(0, 0) = 1.f;
0189     for (int i = 1; i < 5; ++i)
0190       errorPropCurv.aij(0, i) = 0.f;
0191     //   lambda
0192     errorPropCurv.aij(1, 0) = 0.f;
0193     errorPropCurv.aij(1, 1) =
0194         cost * (v11 * v21 + v12 * v22 + v13 * v23) + sint * (-v12 * v21 + v11 * v22) + omcost * v13 * v23;
0195     errorPropCurv.aij(1, 2) = (cost * (u11 * v21 + u12 * v22) + sint * (-u12 * v21 + u11 * v22)) * sinT;
0196     errorPropCurv.aij(1, 3) = 0.f;
0197     errorPropCurv.aij(1, 4) = 0.f;
0198     //   phi
0199     errorPropCurv.aij(2, 0) = bF * v23 * (t21 * dx1 + t22 * dx2 + cosT * dx3) * cosl1;
0200     errorPropCurv.aij(2, 1) = (cost * (v11 * u21 + v12 * u22) + sint * (-v12 * u21 + v11 * u22) +
0201                                v23 * (-sint * (v11 * t21 + v12 * t22 + v13 * cosT) + omcost * (-v11 * t22 + v12 * t21) -
0202                                       tmsint * cosT * v13)) *
0203                               cosl1;
0204     errorPropCurv.aij(2, 2) = (cost * (u11 * u21 + u12 * u22) + sint * (-u12 * u21 + u11 * u22) +
0205                                v23 * (-sint * (u11 * t21 + u12 * t22) + omcost * (-u11 * t22 + u12 * t21))) *
0206                               cosl1 * sinT;
0207     errorPropCurv.aij(2, 3) = -q * v23 * (u11 * t21 + u12 * t22) * cosl1;
0208     errorPropCurv.aij(2, 4) = -q * v23 * (v11 * t21 + v12 * t22 + v13 * cosT) * cosl1;
0209 
0210     //   yt
0211     for (int n = 0; n < N_proc; ++n) {
0212       float cutCriterion = fabs(s[n] * sinT[n] * inPar(n, 3, 0));
0213       const float limit = 5.f;  // valid for propagations with effectively float precision
0214       if (cutCriterion > limit) {
0215         const float pp = 1.f / qbp[n];
0216         errorPropCurv(n, 3, 0) = pp * (u21[n] * dx1[n] + u22[n] * dx2[n]);
0217         errorPropCurv(n, 4, 0) = pp * (v21[n] * dx1[n] + v22[n] * dx2[n] + v23[n] * dx3[n]);
0218       } else {
0219         const float temp1 = -t12[n] * u21[n] + t11[n] * u22[n];
0220         const float s2 = s[n] * s[n];
0221         const float secondOrder41 = -0.5f * bF[n] * temp1 * s2;
0222         const float temp2 = -t11[n] * u21[n] - t12[n] * u22[n];
0223         const float s3 = s2 * s[n];
0224         const float s4 = s3 * s[n];
0225         const float h2 = bF[n] * bF[n];
0226         const float h3 = h2 * bF[n];
0227         const float qbp2 = qbp[n] * qbp[n];
0228         const float thirdOrder41 = 1.f / 3 * h2 * s3 * qbp[n] * temp2;
0229         const float fourthOrder41 = 1.f / 8 * h3 * s4 * qbp2 * temp1;
0230         errorPropCurv(n, 3, 0) = secondOrder41 + (thirdOrder41 + fourthOrder41);
0231         const float temp3 = -t12[n] * v21[n] + t11[n] * v22[n];
0232         const float secondOrder51 = -0.5f * bF[n] * temp3 * s2;
0233         const float temp4 = -t11[n] * v21[n] - t12[n] * v22[n] - cosT[n] * v23[n];
0234         const float thirdOrder51 = 1.f / 3 * h2 * s3 * qbp[n] * temp4;
0235         const float fourthOrder51 = 1.f / 8 * h3 * s4 * qbp2 * temp3;
0236         errorPropCurv(n, 4, 0) = secondOrder51 + (thirdOrder51 + fourthOrder51);
0237       }
0238     }
0239 
0240     errorPropCurv.aij(3, 1) = (sint * (v11 * u21 + v12 * u22) + omcost * (-v12 * u21 + v11 * u22)) / q;
0241     errorPropCurv.aij(3, 2) = (sint * (u11 * u21 + u12 * u22) + omcost * (-u12 * u21 + u11 * u22)) * sinT / q;
0242     errorPropCurv.aij(3, 3) = (u11 * u21 + u12 * u22);
0243     errorPropCurv.aij(3, 4) = (v11 * u21 + v12 * u22);
0244     //   zt
0245     errorPropCurv.aij(4, 1) =
0246         (sint * (v11 * v21 + v12 * v22 + v13 * v23) + omcost * (-v12 * v21 + v11 * v22) + tmsint * v23 * v13) / q;
0247     errorPropCurv.aij(4, 2) = (sint * (u11 * v21 + u12 * v22) + omcost * (-u12 * v21 + u11 * v22)) * sinT / q;
0248     errorPropCurv.aij(4, 3) = (u11 * v21 + u12 * v22);
0249     errorPropCurv.aij(4, 4) = (v11 * v21 + v12 * v22 + v13 * v23);
0250 
0251 //debug = true;
0252 #ifdef DEBUG
0253     for (int n = 0; n < NN; ++n) {
0254       if (debug && g_debug && n < N_proc) {
0255         dmutex_guard;
0256         std::cout << n << ": errorPropCurv" << std::endl;
0257         printf("%5f %5f %5f %5f %5f\n",
0258                errorPropCurv(n, 0, 0),
0259                errorPropCurv(n, 0, 1),
0260                errorPropCurv(n, 0, 2),
0261                errorPropCurv(n, 0, 3),
0262                errorPropCurv(n, 0, 4));
0263         printf("%5f %5f %5f %5f %5f\n",
0264                errorPropCurv(n, 1, 0),
0265                errorPropCurv(n, 1, 1),
0266                errorPropCurv(n, 1, 2),
0267                errorPropCurv(n, 1, 3),
0268                errorPropCurv(n, 1, 4));
0269         printf("%5f %5f %5f %5f %5f\n",
0270                errorPropCurv(n, 2, 0),
0271                errorPropCurv(n, 2, 1),
0272                errorPropCurv(n, 2, 2),
0273                errorPropCurv(n, 2, 3),
0274                errorPropCurv(n, 2, 4));
0275         printf("%5f %5f %5f %5f %5f\n",
0276                errorPropCurv(n, 3, 0),
0277                errorPropCurv(n, 3, 1),
0278                errorPropCurv(n, 3, 2),
0279                errorPropCurv(n, 3, 3),
0280                errorPropCurv(n, 3, 4));
0281         printf("%5f %5f %5f %5f %5f\n",
0282                errorPropCurv(n, 4, 0),
0283                errorPropCurv(n, 4, 1),
0284                errorPropCurv(n, 4, 2),
0285                errorPropCurv(n, 4, 3),
0286                errorPropCurv(n, 4, 4));
0287         printf("\n");
0288       }
0289     }
0290 #endif
0291 
0292     //now we need jacobians to convert to/from curvilinear and CCS
0293     // code from TrackState::jacobianCCSToCurvilinear
0294     MPlex56 jacCCS2Curv(0.0f);
0295     jacCCS2Curv.aij(0, 3) = mpt::negate_if_ltz(sinT, inChg);
0296     jacCCS2Curv.aij(0, 5) = mpt::negate_if_ltz(cosT * inPar(3, 0), inChg);
0297     jacCCS2Curv.aij(1, 5) = -1.f;
0298     jacCCS2Curv.aij(2, 4) = 1.f;
0299     jacCCS2Curv.aij(3, 0) = -sinPin;
0300     jacCCS2Curv.aij(3, 1) = cosPin;
0301     jacCCS2Curv.aij(4, 0) = -cosPin * cosT;
0302     jacCCS2Curv.aij(4, 1) = -sinPin * cosT;
0303     jacCCS2Curv.aij(4, 2) = sinT;
0304 
0305     // code from TrackState::jacobianCurvilinearToCCS
0306     MPlex65 jacCurv2CCS(0.0f);
0307     jacCurv2CCS.aij(0, 3) = -sinPout;
0308     jacCurv2CCS.aij(0, 4) = -cosT * cosPout;
0309     jacCurv2CCS.aij(1, 3) = cosPout;
0310     jacCurv2CCS.aij(1, 4) = -cosT * sinPout;
0311     jacCurv2CCS.aij(2, 4) = sinT;
0312     jacCurv2CCS.aij(3, 0) = mpt::negate_if_ltz(1.f / sinT, inChg);
0313     jacCurv2CCS.aij(3, 1) = outPar(3, 0) * cosT / sinT;
0314     jacCurv2CCS.aij(4, 2) = 1.f;
0315     jacCurv2CCS.aij(5, 1) = -1.f;
0316 
0317     //need to compute errorProp = jacCurv2CCS*errorPropCurv*jacCCS2Curv
0318     MPlex65 tmp;
0319     JacErrPropCurv1(jacCurv2CCS, errorPropCurv, tmp);
0320     JacErrPropCurv2(tmp, jacCCS2Curv, errorProp);
0321     /*
0322     Matriplex::multiplyGeneral(jacCurv2CCS, errorPropCurv, tmp);
0323     for (int kk = 0; kk < 1; ++kk) {
0324       std::cout << "jacCurv2CCS" << std::endl;
0325       for (int i = 0; i < 6; ++i) {
0326       for (int j = 0; j < 5; ++j)
0327         std::cout << jacCurv2CCS.constAt(kk, i, j) << " ";
0328       std::cout << std::endl;;
0329       }
0330       std::cout << std::endl;;
0331       std::cout << "errorPropCurv" << std::endl;
0332       for (int i = 0; i < 5; ++i) {
0333       for (int j = 0; j < 5; ++j)
0334         std::cout << errorPropCurv.constAt(kk, i, j) << " ";
0335       std::cout << std::endl;;
0336       }
0337       std::cout << std::endl;;
0338       std::cout << "tmp" << std::endl;
0339       for (int i = 0; i < 6; ++i) {
0340       for (int j = 0; j < 5; ++j)
0341         std::cout << tmp.constAt(kk, i, j) << " ";
0342       std::cout << std::endl;;
0343       }
0344       std::cout << std::endl;;
0345       std::cout << "jacCCS2Curv" << std::endl;
0346       for (int i = 0; i < 5; ++i) {
0347       for (int j = 0; j < 6; ++j)
0348         std::cout << jacCCS2Curv.constAt(kk, i, j) << " ";
0349       std::cout << std::endl;;
0350       }
0351     }
0352     Matriplex::multiplyGeneral(tmp, jacCCS2Curv, errorProp);
0353     */
0354   }
0355 
0356   // from P.Avery's notes (http://www.phys.ufl.edu/~avery/fitting/transport.pdf eq. 5)
0357   float getS(float delta0,
0358              float delta1,
0359              float delta2,
0360              float eta0,
0361              float eta1,
0362              float eta2,
0363              float sinP,
0364              float cosP,
0365              float sinT,
0366              float cosT,
0367              float pt,
0368              int q,
0369              float kinv) {
0370     float A = delta0 * eta0 + delta1 * eta1 + delta2 * eta2;
0371     float ip = sinT / pt;
0372     float p0[3] = {pt * cosP, pt * sinP, cosT / ip};
0373     float B = (p0[0] * eta0 + p0[1] * eta1 + p0[2] * eta2) * ip;
0374     float rho = kinv * ip;
0375     float C = (eta0 * p0[1] - eta1 * p0[0]) * rho * 0.5f * ip;
0376     float sqb2m4ac = std::sqrt(B * B - 4.f * A * C);
0377     float s1 = (-B + sqb2m4ac) * 0.5f / C;
0378     float s2 = (-B - sqb2m4ac) * 0.5f / C;
0379 #ifdef DEBUG
0380     if (debug)
0381       std::cout << "A=" << A << " B=" << B << " C=" << C << " s1=" << s1 << " s2=" << s2 << std::endl;
0382 #endif
0383     //take the closest
0384     return (std::abs(s1) > std::abs(s2) ? s2 : s1);
0385   }
0386 
0387   void helixAtPlane_impl(const MPlexLV& __restrict__ inPar,
0388                          const MPlexQI& __restrict__ inChg,
0389                          const MPlexHV& __restrict__ plPnt,
0390                          const MPlexHV& __restrict__ plNrm,
0391                          MPlexQF& __restrict__ s,
0392                          MPlexLV& __restrict__ outPar,
0393                          MPlexLL& __restrict__ errorProp,
0394                          MPlexQI& __restrict__ outFailFlag,  // expected to be initialized to 0
0395                          const int N_proc,
0396                          const PropagationFlags& pf) {
0397     namespace mpt = Matriplex;
0398     using MPF = MPlexQF;
0399 
0400 #ifdef DEBUG
0401     for (int n = 0; n < N_proc; ++n) {
0402       dprint_np(n,
0403                 "input parameters"
0404                     << " inPar(n, 0, 0)=" << std::setprecision(9) << inPar(n, 0, 0) << " inPar(n, 1, 0)="
0405                     << std::setprecision(9) << inPar(n, 1, 0) << " inPar(n, 2, 0)=" << std::setprecision(9)
0406                     << inPar(n, 2, 0) << " inPar(n, 3, 0)=" << std::setprecision(9) << inPar(n, 3, 0)
0407                     << " inPar(n, 4, 0)=" << std::setprecision(9) << inPar(n, 4, 0)
0408                     << " inPar(n, 5, 0)=" << std::setprecision(9) << inPar(n, 5, 0));
0409     }
0410 #endif
0411 
0412     MPF kinv = mpt::negate_if_ltz(MPF(-Const::sol_over_100), inChg);
0413     if (pf.use_param_b_field) {
0414       kinv *= getBFieldFromZXY(inPar(2, 0), inPar(0, 0), inPar(1, 0));
0415     } else {
0416       kinv *= Config::Bfield;
0417     }
0418 
0419     MPF delta0 = inPar(0, 0) - plPnt(0, 0);
0420     MPF delta1 = inPar(1, 0) - plPnt(1, 0);
0421     MPF delta2 = inPar(2, 0) - plPnt(2, 0);
0422 
0423     MPF sinP, cosP;
0424     mpt::fast_sincos(inPar(4, 0), sinP, cosP);
0425     MPF sinT, cosT;
0426     mpt::fast_sincos(inPar(5, 0), sinT, cosT);
0427 
0428     // determine solution for straight line
0429     MPF sl = -(plNrm(0, 0) * delta0 + plNrm(1, 0) * delta1 + plNrm(2, 0) * delta2) /
0430              (plNrm(0, 0) * cosP * sinT + plNrm(1, 0) * sinP * sinT + plNrm(2, 0) * cosT);
0431 
0432     //float s[nmax - nmin];
0433     //first iteration outside the loop
0434 #pragma omp simd
0435     for (int n = 0; n < N_proc; ++n) {
0436       s[n] = (std::abs(plNrm(n, 2, 0)) < 1.f ? getS(delta0[n],
0437                                                     delta1[n],
0438                                                     delta2[n],
0439                                                     plNrm(n, 0, 0),
0440                                                     plNrm(n, 1, 0),
0441                                                     plNrm(n, 2, 0),
0442                                                     sinP[n],
0443                                                     cosP[n],
0444                                                     sinT[n],
0445                                                     cosT[n],
0446                                                     inPar(n, 3, 0),
0447                                                     inChg(n, 0, 0),
0448                                                     kinv[n])
0449                                              : (plPnt.constAt(n, 2, 0) - inPar.constAt(n, 2, 0)) / cosT[n]);
0450     }
0451 
0452     MPlexLV outParTmp;
0453 
0454     CMS_UNROLL_LOOP_COUNT(Config::Niter - 1)
0455     for (int i = 0; i < Config::Niter - 1; ++i) {
0456       parsFromPathL_impl(inPar, outParTmp, kinv, s);
0457 
0458       delta0 = outParTmp(0, 0) - plPnt(0, 0);
0459       delta1 = outParTmp(1, 0) - plPnt(1, 0);
0460       delta2 = outParTmp(2, 0) - plPnt(2, 0);
0461 
0462       mpt::fast_sincos(outParTmp(4, 0), sinP, cosP);
0463       // Note, sinT/cosT not updated
0464 
0465 #pragma omp simd
0466       for (int n = 0; n < N_proc; ++n) {
0467         s[n] += (std::abs(plNrm(n, 2, 0)) < 1.f
0468                      ? getS(delta0[n],
0469                             delta1[n],
0470                             delta2[n],
0471                             plNrm(n, 0, 0),
0472                             plNrm(n, 1, 0),
0473                             plNrm(n, 2, 0),
0474                             sinP[n],
0475                             cosP[n],
0476                             sinT[n],
0477                             cosT[n],
0478                             inPar(n, 3, 0),
0479                             inChg(n, 0, 0),
0480                             kinv[n])
0481                      : (plPnt.constAt(n, 2, 0) - outParTmp.constAt(n, 2, 0)) / std::cos(outParTmp.constAt(n, 5, 0)));
0482       }
0483     }  //end Niter-1
0484 
0485     // use linear approximation if s did not converge (for very high pT tracks)
0486     for (int n = 0; n < N_proc; ++n) {
0487 #ifdef DEBUG
0488       if (debug)
0489         std::cout << "s[n]=" << s[n] << " sl[n]=" << sl[n] << " std::isnan(s[n])=" << std::isnan(s[n])
0490                   << " std::isfinite(s[n])=" << std::isfinite(s[n]) << " std::isnormal(s[n])=" << std::isnormal(s[n])
0491                   << std::endl;
0492 #endif
0493       if (mkfit::isFinite(s[n]) == false && mkfit::isFinite(sl[n]))  // replace with sl even if not fully correct
0494         s[n] = sl[n];
0495     }
0496 
0497 #ifdef DEBUG
0498     if (debug)
0499       std::cout << "s=" << s[0] << std::endl;
0500 #endif
0501     parsAndErrPropFromPathL_impl(inPar, inChg, outPar, kinv, s, errorProp, N_proc, pf);
0502   }
0503 
0504 }  // namespace
0505 // END STUFF FROM PropagationMPlex.icc
0506 // ============================================================================
0507 
0508 namespace mkfit {
0509 
0510   void helixAtPlane(const MPlexLV& inPar,
0511                     const MPlexQI& inChg,
0512                     const MPlexHV& plPnt,
0513                     const MPlexHV& plNrm,
0514                     MPlexQF& pathL,
0515                     MPlexLV& outPar,
0516                     MPlexLL& errorProp,
0517                     MPlexQI& outFailFlag,
0518                     const int N_proc,
0519                     const PropagationFlags& pflags) {
0520     errorProp.setVal(0.f);
0521     outFailFlag.setVal(0.f);
0522 
0523     helixAtPlane_impl(inPar, inChg, plPnt, plNrm, pathL, outPar, errorProp, outFailFlag, N_proc, pflags);
0524   }
0525 
0526   void propagateHelixToPlaneMPlex(const MPlexLS& inErr,
0527                                   const MPlexLV& inPar,
0528                                   const MPlexQI& inChg,
0529                                   const MPlexHV& plPnt,
0530                                   const MPlexHV& plNrm,
0531                                   MPlexLS& outErr,
0532                                   MPlexLV& outPar,
0533                                   MPlexQI& outFailFlag,
0534                                   const int N_proc,
0535                                   const PropagationFlags& pflags,
0536                                   const MPlexQI* noMatEffPtr) {
0537     // debug = true;
0538 
0539     outErr = inErr;
0540     outPar = inPar;
0541 
0542     MPlexQF pathL;
0543     MPlexLL errorProp;
0544 
0545     helixAtPlane(inPar, inChg, plPnt, plNrm, pathL, outPar, errorProp, outFailFlag, N_proc, pflags);
0546 
0547 #ifdef DEBUG
0548     for (int n = 0; n < N_proc; ++n) {
0549       dprint_np(
0550           n,
0551           "propagation to plane end, dump parameters\n"
0552               //<< "   D = " << s[n] << " alpha = " << s[n] * std::sin(inPar(n, 5, 0)) * inPar(n, 3, 0) * kinv[n] << " kinv = " << kinv[n] << std::endl
0553               << "   pos = " << outPar(n, 0, 0) << " " << outPar(n, 1, 0) << " " << outPar(n, 2, 0) << "\t\t r="
0554               << std::sqrt(outPar(n, 0, 0) * outPar(n, 0, 0) + outPar(n, 1, 0) * outPar(n, 1, 0)) << std::endl
0555               << "   mom = " << outPar(n, 3, 0) << " " << outPar(n, 4, 0) << " " << outPar(n, 5, 0) << std::endl
0556               << " charge = " << inChg(n, 0, 0) << std::endl
0557               << " cart= " << std::cos(outPar(n, 4, 0)) / outPar(n, 3, 0) << " "
0558               << std::sin(outPar(n, 4, 0)) / outPar(n, 3, 0) << " " << 1. / (outPar(n, 3, 0) * tan(outPar(n, 5, 0)))
0559               << "\t\tpT=" << 1. / std::abs(outPar(n, 3, 0)) << std::endl);
0560     }
0561 
0562     if (debug && g_debug) {
0563       for (int kk = 0; kk < N_proc; ++kk) {
0564         dprintf("inPar %d\n", kk);
0565         for (int i = 0; i < 6; ++i) {
0566           dprintf("%8f ", inPar.constAt(kk, i, 0));
0567         }
0568         dprintf("\n");
0569         dprintf("inErr %d\n", kk);
0570         for (int i = 0; i < 6; ++i) {
0571           for (int j = 0; j < 6; ++j)
0572             dprintf("%8f ", inErr.constAt(kk, i, j));
0573           dprintf("\n");
0574         }
0575         dprintf("\n");
0576 
0577         for (int kk = 0; kk < N_proc; ++kk) {
0578           dprintf("plNrm %d\n", kk);
0579           for (int j = 0; j < 3; ++j)
0580             dprintf("%8f ", plNrm.constAt(kk, 0, j));
0581         }
0582         dprintf("\n");
0583 
0584         for (int kk = 0; kk < N_proc; ++kk) {
0585           dprintf("pathL %d\n", kk);
0586           for (int j = 0; j < 1; ++j)
0587             dprintf("%8f ", pathL.constAt(kk, 0, j));
0588         }
0589         dprintf("\n");
0590 
0591         dprintf("errorProp %d\n", kk);
0592         for (int i = 0; i < 6; ++i) {
0593           for (int j = 0; j < 6; ++j)
0594             dprintf("%8f ", errorProp.At(kk, i, j));
0595           dprintf("\n");
0596         }
0597         dprintf("\n");
0598       }
0599     }
0600 #endif
0601 
0602     // Matriplex version of:
0603     // result.errors = ROOT::Math::Similarity(errorProp, outErr);
0604     MPlexLL temp;
0605     MultHelixPlaneProp(errorProp, outErr, temp);
0606     MultHelixPlanePropTransp(errorProp, temp, outErr);
0607     // MultHelixPropFull(errorProp, outErr, temp);
0608     // for (int kk = 0; kk < 1; ++kk) {
0609     //   std::cout << "errorProp" << std::endl;
0610     //   for (int i = 0; i < 6; ++i) {
0611     //  for (int j = 0; j < 6; ++j)
0612     //    std::cout << errorProp.constAt(kk, i, j) << " ";
0613     //  std::cout << std::endl;;
0614     //   }
0615     //   std::cout << std::endl;;
0616     //   std::cout << "outErr" << std::endl;
0617     //   for (int i = 0; i < 6; ++i) {
0618     //  for (int j = 0; j < 6; ++j)
0619     //    std::cout << outErr.constAt(kk, i, j) << " ";
0620     //  std::cout << std::endl;;
0621     //   }
0622     //   std::cout << std::endl;;
0623     //   std::cout << "temp" << std::endl;
0624     //   for (int i = 0; i < 6; ++i) {
0625     //  for (int j = 0; j < 6; ++j)
0626     //    std::cout << temp.constAt(kk, i, j) << " ";
0627     //  std::cout << std::endl;;
0628     //   }
0629     //   std::cout << std::endl;;
0630     // }
0631     // MultHelixPropTranspFull(errorProp, temp, outErr);
0632 
0633 #ifdef DEBUG
0634     if (debug && g_debug) {
0635       for (int kk = 0; kk < N_proc; ++kk) {
0636         dprintf("outErr %d\n", kk);
0637         for (int i = 0; i < 6; ++i) {
0638           for (int j = 0; j < 6; ++j)
0639             dprintf("%8f ", outErr.constAt(kk, i, j));
0640           dprintf("\n");
0641         }
0642         dprintf("\n");
0643       }
0644     }
0645 #endif
0646 
0647     if (pflags.apply_material) {
0648       MPlexQF hitsRl;
0649       MPlexQF hitsXi;
0650       MPlexQF propSign;
0651 
0652       const TrackerInfo& tinfo = *pflags.tracker_info;
0653 
0654 #if !defined(__clang__)
0655 #pragma omp simd
0656 #endif
0657       for (int n = 0; n < NN; ++n) {
0658         if (n >= N_proc || (noMatEffPtr && noMatEffPtr->constAt(n, 0, 0))) {
0659           hitsRl(n, 0, 0) = 0.f;
0660           hitsXi(n, 0, 0) = 0.f;
0661         } else {
0662           const float hypo = hipo(outPar(n, 0, 0), outPar(n, 1, 0));
0663           const auto mat = tinfo.material_checked(std::abs(outPar(n, 2, 0)), hypo);
0664           hitsRl(n, 0, 0) = mat.radl;
0665           hitsXi(n, 0, 0) = mat.bbxi;
0666         }
0667         propSign(n, 0, 0) = (pathL(n, 0, 0) > 0.f ? 1.f : -1.f);
0668       }
0669       applyMaterialEffects(hitsRl, hitsXi, propSign, plNrm, outErr, outPar, N_proc);
0670 #ifdef DEBUG
0671       if (debug && g_debug) {
0672         for (int kk = 0; kk < N_proc; ++kk) {
0673           dprintf("propSign %d\n", kk);
0674           for (int i = 0; i < 1; ++i) {
0675             dprintf("%8f ", propSign.constAt(kk, i, 0));
0676           }
0677           dprintf("\n");
0678           dprintf("plNrm %d\n", kk);
0679           for (int i = 0; i < 3; ++i) {
0680             dprintf("%8f ", plNrm.constAt(kk, i, 0));
0681           }
0682           dprintf("\n");
0683           dprintf("outErr(after material) %d\n", kk);
0684           for (int i = 0; i < 6; ++i) {
0685             for (int j = 0; j < 6; ++j)
0686               dprintf("%8f ", outErr.constAt(kk, i, j));
0687             dprintf("\n");
0688           }
0689           dprintf("\n");
0690         }
0691       }
0692 #endif
0693     }
0694 
0695     squashPhiMPlex(outPar, N_proc);  // ensure phi is between |pi|
0696 
0697     // PROP-FAIL-ENABLE To keep physics changes minimal, we always restore the
0698     // state to input when propagation fails -- as was the default before.
0699     // if (pflags.copy_input_state_on_fail) {
0700     for (int i = 0; i < N_proc; ++i) {
0701       if (outFailFlag(i, 0, 0)) {
0702         outPar.copySlot(i, inPar);
0703         outErr.copySlot(i, inErr);
0704       }
0705     }
0706     // }
0707   }
0708 
0709 }  // namespace mkfit
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.2.1 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!