Back to home page

Project CMSSW displayed by LXR
 
 

    

File indexing completed on 2024-10-17 22:59:07 

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