Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2022-02-21 23:14:21

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