MkFitCore/src/Track.cc

0001 #include "RecoTracker/MkFitCore/interface/cms_common_macros.h"
0002 #include "RecoTracker/MkFitCore/interface/Track.h"
0003 #include "Matrix.h"
0004
0005 //#define DEBUG
0006 #include "Debug.h"
0007
0008 namespace mkfit {
0009
0010   //==============================================================================
0011   // TrackState
0012   //==============================================================================
0013
0014   void TrackState::convertFromCartesianToCCS() {
0015     //assume we are currently in cartesian coordinates and want to move to ccs
0016     const float px = parameters.At(3);
0017     const float py = parameters.At(4);
0018     const float pz = parameters.At(5);
0019     const float pt = std::sqrt(px * px + py * py);
0020     const float phi = getPhi(px, py);
0021     const float theta = getTheta(pt, pz);
0022     parameters.At(3) = 1.f / pt;
0023     parameters.At(4) = phi;
0024     parameters.At(5) = theta;
0025     SMatrix66 jac = jacobianCartesianToCCS(px, py, pz);
0026     errors = ROOT::Math::Similarity(jac, errors);
0027   }
0028
0029   void TrackState::convertFromCCSToCartesian() {
0030     //assume we are currently in ccs coordinates and want to move to cartesian
0031     const float invpt = parameters.At(3);
0032     const float phi = parameters.At(4);
0033     const float theta = parameters.At(5);
0034     const float pt = 1.f / invpt;
0035     float cosP = std::cos(phi);
0036     float sinP = std::sin(phi);
0037     float cosT = std::cos(theta);
0038     float sinT = std::sin(theta);
0039     parameters.At(3) = cosP * pt;
0040     parameters.At(4) = sinP * pt;
0041     parameters.At(5) = cosT * pt / sinT;
0042     SMatrix66 jac = jacobianCCSToCartesian(invpt, phi, theta);
0043     errors = ROOT::Math::Similarity(jac, errors);
0044   }
0045
0046   SMatrix66 TrackState::jacobianCCSToCartesian(float invpt, float phi, float theta) const {
0047     //arguments are passed so that the function can be used both starting from ccs and from cartesian
0048     SMatrix66 jac = ROOT::Math::SMatrixIdentity();
0049     float cosP = std::cos(phi);
0050     float sinP = std::sin(phi);
0051     float cosT = std::cos(theta);
0052     float sinT = std::sin(theta);
0053     const float pt = 1.f / invpt;
0054     jac(3, 3) = -cosP * pt * pt;
0055     jac(3, 4) = -sinP * pt;
0056     jac(4, 3) = -sinP * pt * pt;
0057     jac(4, 4) = cosP * pt;
0058     jac(5, 3) = -cosT * pt * pt / sinT;
0059     jac(5, 5) = -pt / (sinT * sinT);
0060     return jac;
0061   }
0062
0063   SMatrix66 TrackState::jacobianCartesianToCCS(float px, float py, float pz) const {
0064     //arguments are passed so that the function can be used both starting from ccs and from cartesian
0065     SMatrix66 jac = ROOT::Math::SMatrixIdentity();
0066     const float pt = std::sqrt(px * px + py * py);
0067     const float p2 = px * px + py * py + pz * pz;
0068     jac(3, 3) = -px / (pt * pt * pt);
0069     jac(3, 4) = -py / (pt * pt * pt);
0070     jac(4, 3) = -py / (pt * pt);
0071     jac(4, 4) = px / (pt * pt);
0072     jac(5, 3) = px * pz / (pt * p2);
0073     jac(5, 4) = py * pz / (pt * p2);
0074     jac(5, 5) = -pt / p2;
0075     return jac;
0076   }
0077
0078   void TrackState::convertFromGlbCurvilinearToCCS() {
0079     //assume we are currently in global state with curvilinear error and want to move to ccs
0080     const float px = parameters.At(3);
0081     const float py = parameters.At(4);
0082     const float pz = parameters.At(5);
0083     const float pt = std::sqrt(px * px + py * py);
0084     const float phi = getPhi(px, py);
0085     const float theta = getTheta(pt, pz);
0086     parameters.At(3) = 1.f / pt;
0087     parameters.At(4) = phi;
0088     parameters.At(5) = theta;
0089     SMatrix66 jac = jacobianCurvilinearToCCS(px, py, pz, charge);
0090     errors = ROOT::Math::Similarity(jac, errors);
0091   }
0092
0093   void TrackState::convertFromCCSToGlbCurvilinear() {
0094     //assume we are currently in ccs coordinates and want to move to global state with cartesian error
0095     const float invpt = parameters.At(3);
0096     const float phi = parameters.At(4);
0097     const float theta = parameters.At(5);
0098     const float pt = 1.f / invpt;
0099     float cosP = std::cos(phi);
0100     float sinP = std::sin(phi);
0101     float cosT = std::cos(theta);
0102     float sinT = std::sin(theta);
0103     parameters.At(3) = cosP * pt;
0104     parameters.At(4) = sinP * pt;
0105     parameters.At(5) = cosT * pt / sinT;
0106     SMatrix66 jac = jacobianCCSToCurvilinear(invpt, cosP, sinP, cosT, sinT, charge);
0107     errors = ROOT::Math::Similarity(jac, errors);
0108   }
0109
0110   SMatrix66 TrackState::jacobianCCSToCurvilinear(
0111       float invpt, float cosP, float sinP, float cosT, float sinT, short charge) const {
0112     SMatrix66 jac;
0113     jac(3, 0) = -sinP;
0114     jac(4, 0) = -cosP * cosT;
0115     jac(3, 1) = cosP;
0116     jac(4, 1) = -sinP * cosT;
0117     jac(4, 2) = sinT;
0118     jac(0, 3) = charge * sinT;
0119     jac(0, 5) = charge * cosT * invpt;
0120     jac(1, 5) = -1.f;
0121     jac(2, 4) = 1.f;
0122
0123     return jac;
0124   }
0125
0126   SMatrix66 TrackState::jacobianCurvilinearToCCS(float px, float py, float pz, short charge) const {
0127     const float pt2 = px * px + py * py;
0128     const float pt = sqrt(pt2);
0129     const float invpt2 = 1.f / pt2;
0130     const float invpt = 1.f / pt;
0131     const float invp = 1.f / sqrt(pt2 + pz * pz);
0132     const float sinPhi = py * invpt;
0133     const float cosPhi = px * invpt;
0134     const float sinLam = pz * invp;
0135     const float cosLam = pt * invp;
0136
0137     SMatrix66 jac;
0138     jac(0, 3) = -sinPhi;
0139     jac(0, 4) = -sinLam * cosPhi;
0140     jac(1, 3) = cosPhi;
0141     jac(1, 4) = -sinLam * sinPhi;
0142     jac(2, 4) = cosLam;
0143     jac(3, 0) = charge / cosLam;  //assumes |charge|==1 ; else 1.f/charge here
0144     jac(3, 1) = pz * invpt2;
0145     jac(4, 2) = 1.f;
0146     jac(5, 1) = -1.f;
0147
0148     return jac;
0149   }
0150
0151   //==============================================================================
0152   // TrackBase
0153   //==============================================================================
0154
0155   bool TrackBase::hasSillyValues(bool dump, bool fix, const char* pref) {
0156     bool is_silly = false;
0157     for (int i = 0; i < LL; ++i) {
0158       for (int j = 0; j <= i; ++j) {
0159         if ((i == j && state_.errors.At(i, j) < 0) || !isFinite(state_.errors.At(i, j))) {
0160           if (!is_silly) {
0161             is_silly = true;
0162             if (dump)
0163               printf("%s (label=%d, pT=%f):", pref, label(), pT());
0164           }
0165           if (dump)
0166             printf(" (%d,%d)=%e", i, j, state_.errors.At(i, j));
0167           if (fix)
0168             state_.errors.At(i, j) = 0.00001;
0169         }
0170       }
0171     }
0172     if (is_silly && dump)
0173       printf("\n");
0174     return is_silly;
0175   }
0176
0177   bool TrackBase::hasNanNSillyValues() const {
0178     bool is_silly = false;
0179     for (int i = 0; i < LL; ++i) {
0180       for (int j = 0; j <= i; ++j) {
0181         if ((i == j && state_.errors.At(i, j) < 0) || !isFinite(state_.errors.At(i, j))) {
0182           is_silly = true;
0183           return is_silly;
0184         }
0185       }
0186     }
0187     return is_silly;
0188   }
0189
0190   // If linearize=true, use linear estimate of d0: suitable at pT>~10 GeV (--> 10 micron error)
0191   float TrackBase::d0BeamSpot(const float x_bs, const float y_bs, bool linearize) const {
0192     if (linearize) {
0193       return std::abs(std::cos(momPhi()) * (y() - y_bs) - std::sin(momPhi()) * (x() - x_bs));
0194     } else {
0195       const float k = ((charge() < 0) ? 100.0f : -100.0f) / (Const::sol * Config::Bfield);
0196       const float abs_ooc_half = std::abs(k * pT());
0197       // center of helix in x,y plane
0198       const float x_center = x() - k * py();
0199       const float y_center = y() + k * px();
0200       return std::hypot(x_center - x_bs, y_center - y_bs) - abs_ooc_half;
0201     }
0202   }
0203   float TrackBase::swimPhiToR(const float x0, const float y0) const {
0204     const float dR = getHypot(x() - x0, y() - y0);
0205     // XXX-ASSUMPTION-ERROR can not always reach R, should see what callers expect.
0206     // For now return PI to signal apex on the ohter side of the helix.
0207     const float v = dR / 176.f / pT() * charge();
0208     const float dPhi = std::abs(v) <= 1.0f ? 2.f * std::asin(v) : Const::PI;
0209     ;
0210     return squashPhiGeneral(momPhi() - dPhi);
0211   }
0212
0213   bool TrackBase::canReachRadius(float R) const {
0214     const float k = ((charge() < 0) ? 100.0f : -100.0f) / (Const::sol * Config::Bfield);
0215     const float ooc = 2.0f * k * pT();
0216     return std::abs(ooc) > R - std::hypot(x(), y());
0217   }
0218
0219   float TrackBase::maxReachRadius() const {
0220     const float k = ((charge() < 0) ? 100.0f : -100.0f) / (Const::sol * Config::Bfield);
0221     const float abs_ooc_half = std::abs(k * pT());
0222     // center of helix in x,y plane
0223     const float x_center = x() - k * py();
0224     const float y_center = y() + k * px();
0225     return std::hypot(x_center, y_center) + abs_ooc_half;
0226   }
0227
0228   float TrackBase::zAtR(float R, float* r_reached) const {
0229     float xc = x();
0230     float yc = y();
0231     float pxc = px();
0232     float pyc = py();
0233
0234     const float ipt = invpT();
0235     const float kinv = ((charge() < 0) ? 0.01f : -0.01f) * Const::sol * Config::Bfield;
0236     const float k = 1.0f / kinv;
0237
0238     const float c = 0.5f * kinv * ipt;
0239     const float ooc = 1.0f / c;  // 2 * radius of curvature
0240     const float lambda = pz() * ipt;
0241
0242     //printf("Track::zAtR to R=%f: k=%e, ipt=%e, c=%e, ooc=%e  -- can hit = %f (if > 1 can)\n",
0243     //       R, k, ipt, c, ooc, ooc / (R - std::hypot(xc,yc)));
0244
0245     float D = 0;
0246
0247     for (int i = 0; i < Config::Niter; ++i) {
0248       // compute tangental and ideal distance for the current iteration.
0249       // 3-rd order asin for symmetric incidence (shortest arc lenght).
0250       float r0 = std::hypot(xc, yc);
0251       float td = (R - r0) * c;
0252       float id = ooc * td * (1.0f + 0.16666666f * td * td);
0253       // This would be for line approximation:
0254       // float id = R - r0;
0255       D += id;
0256
0257       //printf("%-3d r0=%f R-r0=%f td=%f id=%f id_line=%f delta_id=%g\n",
0258       //       i, r0, R-r0, td, id, R - r0, id - (R-r0));
0259
0260       float cosa = std::cos(id * ipt * kinv);
0261       float sina = std::sin(id * ipt * kinv);
0262
0263       //update parameters
0264       xc += k * (pxc * sina - pyc * (1.0f - cosa));
0265       yc += k * (pyc * sina + pxc * (1.0f - cosa));
0266
0267       const float pxo = pxc;  //copy before overwriting
0268       pxc = pxc * cosa - pyc * sina;
0269       pyc = pyc * cosa + pxo * sina;
0270     }
0271
0272     if (r_reached)
0273       *r_reached = std::hypot(xc, yc);
0274
0275     return z() + lambda * D;
0276
0277     // ----------------------------------------------------------------
0278     // Exact solution from Avery's notes ... loses precision somewhere
0279     // {
0280     //   const float a = kinv;
0281     //   float pT      = S.pT();
0282
0283     //   float ax2y2  = a*(x*x + y*y);
0284     //   float T      = std::sqrt(pT*pT - 2.0f*a*(x*py - y*px) + a*ax2y2);
0285     //   float D0     = (T - pT) / a;
0286     //   float D      = (-2.0f * (x*py - y*px) + a * (x*x + y*y)) / (T + pT);
0287
0288     //   float B      = c * std::sqrt((R*R - D*D) / (1.0f + 2.0f*c*D));
0289     //   float s1     = std::asin(B) / c;
0290     //   float s2     = (Const::PI - std::asin(B)) / c;
0291
0292     //   printf("pt %f, invpT %f\n", pT, S.invpT());
0293     //   printf("lambda %f, a %f, c %f, T %f, D0 %f, D %f, B %f, s1 %f, s2 %f\n",
0294     //          lambda, a, c, T, D0, D, B, s1, s2);
0295     //   printf("%f = %f / %f\n", (R*R - D*D) / (1.0f + 2.0f*c*D), (R*R - D*D), (1.0f + 2.0f*c*D));
0296
0297     //   z1 = S.z() + lambda * s1;
0298     //   z2 = S.z() + lambda * s2;
0299
0300     //   printf("z1=%f z2=%f\n", z1, z2);
0301     // }
0302     // ----------------------------------------------------------------
0303   }
0304
0305   float TrackBase::rAtZ(float Z) const {
0306     float xc = x();
0307     float yc = y();
0308     float pxc = px();
0309     float pyc = py();
0310
0311     const float ipt = invpT();
0312     const float kinv = ((charge() < 0) ? 0.01f : -0.01f) * Const::sol * Config::Bfield;
0313     const float k = 1.0f / kinv;
0314
0315     const float dz = Z - z();
0316     const float alpha = dz * ipt * kinv * std::tan(theta());
0317
0318     const float cosa = std::cos(alpha);
0319     const float sina = std::sin(alpha);
0320
0321     xc += k * (pxc * sina - pyc * (1.0f - cosa));
0322     yc += k * (pyc * sina + pxc * (1.0f - cosa));
0323
0324     // const float pxo = pxc;//copy before overwriting
0325     // pxc = pxc * cosa  -  pyc * sina;
0326     // pyc = pyc * cosa  +  pxo * sina;
0327
0328     return std::hypot(xc, yc);
0329   }
0330
0331   const char* TrackBase::algoint_to_cstr(int algo) {
0332     static const char* const names[] = {"undefAlgorithm",
0333                                         "ctf",
0334                                         "duplicateMerge",
0335                                         "cosmics",
0336                                         "initialStep",
0337                                         "lowPtTripletStep",
0338                                         "pixelPairStep",
0339                                         "detachedTripletStep",
0340                                         "mixedTripletStep",
0341                                         "pixelLessStep",
0342                                         "tobTecStep",
0343                                         "jetCoreRegionalStep",
0344                                         "conversionStep",
0345                                         "muonSeededStepInOut",
0346                                         "muonSeededStepOutIn",
0347                                         "outInEcalSeededConv",
0348                                         "inOutEcalSeededConv",
0349                                         "nuclInter",
0350                                         "standAloneMuon",
0351                                         "globalMuon",
0352                                         "cosmicStandAloneMuon",
0353                                         "cosmicGlobalMuon",
0354                                         "highPtTripletStep",
0355                                         "lowPtQuadStep",
0356                                         "detachedQuadStep",
0357                                         "reservedForUpgrades1",
0358                                         "reservedForUpgrades2",
0359                                         "bTagGhostTracks",
0360                                         "beamhalo",
0361                                         "gsf",
0362                                         "hltPixel",
0363                                         "hltIter0",
0364                                         "hltIter1",
0365                                         "hltIter2",
0366                                         "hltIter3",
0367                                         "hltIter4",
0368                                         "hltIterX",
0369                                         "hiRegitMuInitialStep",
0370                                         "hiRegitMuLowPtTripletStep",
0371                                         "hiRegitMuPixelPairStep",
0372                                         "hiRegitMuDetachedTripletStep",
0373                                         "hiRegitMuMixedTripletStep",
0374                                         "hiRegitMuPixelLessStep",
0375                                         "hiRegitMuTobTecStep",
0376                                         "hiRegitMuMuonSeededStepInOut",
0377                                         "hiRegitMuMuonSeededStepOutIn",
0378                                         "algoSize"};
0379
0380     if (algo < 0 || algo >= (int)TrackAlgorithm::algoSize)
0381       return names[0];
0382     return names[algo];
0383   }
0384
0385   //==============================================================================
0386   // Track
0387   //==============================================================================
0388
0389   void Track::resizeHitsForInput() {
0390     bzero(&hitsOnTrk_, sizeof(hitsOnTrk_));
0391     hitsOnTrk_.resize(lastHitIdx_ + 1);
0392   }
0393
0394   void Track::sortHitsByLayer() {
0395     std::stable_sort(&hitsOnTrk_[0], &hitsOnTrk_[lastHitIdx_ + 1], [](const auto& h1, const auto& h2) {
0396       return h1.layer < h2.layer;
0397     });
0398   }
0399
0400   //==============================================================================
0401
0402   void print(const TrackState& s) {
0403     std::cout << " x:  " << s.parameters[0] << " y:  " << s.parameters[1] << " z:  " << s.parameters[2] << std::endl
0404               << " px: " << s.parameters[3] << " py: " << s.parameters[4] << " pz: " << s.parameters[5] << std::endl
0405               << "valid: " << s.valid << " errors: " << std::endl;
0406     dumpMatrix(s.errors);
0407     std::cout << std::endl;
0408   }
0409
0410   void print(std::string pfx, int itrack, const Track& trk, bool print_hits) {
0411     std::cout << std::endl
0412               << pfx << ": " << itrack << " hits: " << trk.nFoundHits() << " label: " << trk.label() << " State"
0413               << std::endl;
0414     print(trk.state());
0415     if (print_hits) {
0416       for (int i = 0; i < trk.nTotalHits(); ++i)
0417         printf("  %2d: lyr %2d idx %d\n", i, trk.getHitLyr(i), trk.getHitIdx(i));
0418     }
0419   }
0420
0421   void print(std::string pfx, const TrackState& s) {
0422     std::cout << pfx << std::endl;
0423     print(s);
0424   }
0425
0426 }  // end namespace mkfit