TrajectoryState/src/PerigeeConversions.cc

0001 #include "TrackingTools/TrajectoryState/interface/PerigeeConversions.h"
0002 #include "TrackingTools/TrajectoryState/interface/TrajectoryStateClosestToPoint.h"
0003 #include "MagneticField/Engine/interface/MagneticField.h"
0004 #include <cmath>
0005 #include <vdt/vdtMath.h>
0006
0007 std::optional<PerigeeTrajectoryParameters> PerigeeConversions::ftsToPerigeeParameters(const FTS& originalFTS,
0008                                                                                       const GlobalPoint& referencePoint,
0009                                                                                       double& pt)
0010
0011 {
0012   GlobalVector impactDistance = originalFTS.position() - referencePoint;
0013
0014   pt = originalFTS.momentum().perp();
0015   if (pt == 0.)
0016     return std::nullopt;
0017
0018   double theta = originalFTS.momentum().theta();
0019   double phi = originalFTS.momentum().phi();
0020   double field = originalFTS.parameters().magneticFieldInInverseGeV().z();
0021   //   if (field==0.) throw cms::Exception("PerigeeConversions", "Field is 0") << " at " << originalFTS.position() << "\n" ;
0022
0023   double positiveMomentumPhi = ((phi > 0) ? phi : (2 * M_PI + phi));
0024   double positionPhi = impactDistance.phi();
0025   double positivePositionPhi = ((positionPhi > 0) ? positionPhi : (2 * M_PI + positionPhi));
0026   double phiDiff = positiveMomentumPhi - positivePositionPhi;
0027   if (phiDiff < 0.0)
0028     phiDiff += (2 * M_PI);
0029   double signEpsilon = ((phiDiff > M_PI) ? -1.0 : 1.0);
0030
0031   double epsilon =
0032       signEpsilon * sqrt(impactDistance.x() * impactDistance.x() + impactDistance.y() * impactDistance.y());
0033
0034   // The track parameters:
0035   AlgebraicVector5 theTrackParameters;
0036
0037   double signTC = -originalFTS.charge();
0038   bool isCharged = (signTC != 0) && (std::abs(field) > 1.e-10);
0039   if (isCharged) {
0040     theTrackParameters[0] = field / pt * signTC;
0041   } else {
0042     theTrackParameters[0] = 1 / pt;
0043   }
0044   theTrackParameters[1] = theta;
0045   theTrackParameters[2] = phi;
0046   theTrackParameters[3] = epsilon;
0047   theTrackParameters[4] = impactDistance.z();
0048   return PerigeeTrajectoryParameters(theTrackParameters, isCharged);
0049 }
0050
0051 PerigeeTrajectoryError PerigeeConversions::ftsToPerigeeError(const FTS& originalFTS) {
0052   AlgebraicSymMatrix55 errorMatrix = originalFTS.curvilinearError().matrix();
0053   AlgebraicMatrix55 curv2perigee = jacobianCurvilinear2Perigee(originalFTS);
0054   return PerigeeTrajectoryError(ROOT::Math::Similarity(curv2perigee, errorMatrix));
0055 }
0056
0057 CurvilinearTrajectoryError PerigeeConversions::curvilinearError(const PerigeeTrajectoryError& perigeeError,
0058                                                                 const GlobalTrajectoryParameters& gtp) {
0059   AlgebraicMatrix55 perigee2curv = jacobianPerigee2Curvilinear(gtp);
0060   return CurvilinearTrajectoryError(ROOT::Math::Similarity(perigee2curv, perigeeError.covarianceMatrix()));
0061 }
0062
0063 GlobalPoint PerigeeConversions::positionFromPerigee(const PerigeeTrajectoryParameters& parameters,
0064                                                     const GlobalPoint& referencePoint) {
0065   AlgebraicVector5 theVector = parameters.vector();
0066   return GlobalPoint(theVector[3] * vdt::fast_sin(theVector[2]) + referencePoint.x(),
0067                      -theVector[3] * vdt::fast_cos(theVector[2]) + referencePoint.y(),
0068                      theVector[4] + referencePoint.z());
0069 }
0070
0071 GlobalVector PerigeeConversions::momentumFromPerigee(const PerigeeTrajectoryParameters& parameters,
0072                                                      double pt,
0073                                                      const GlobalPoint& referencePoint) {
0074   return GlobalVector(vdt::fast_cos(parameters.phi()) * pt,
0075                       vdt::fast_sin(parameters.phi()) * pt,
0076                       pt / vdt::fast_tan(parameters.theta()));
0077 }
0078
0079 GlobalVector PerigeeConversions::momentumFromPerigee(const AlgebraicVector3& momentum,
0080                                                      const TrackCharge& charge,
0081                                                      const GlobalPoint& referencePoint,
0082                                                      const MagneticField* field) {
0083   double pt;
0084
0085   double bz = fabs(field->inInverseGeV(referencePoint).z());
0086   if (charge != 0 && std::abs(bz) > 1.e-10) {
0087     pt = std::abs(bz / momentum[0]);
0088     // if (pt<1.e-10) throw cms::Exception("PerigeeConversions", "pt is 0");
0089   } else {
0090     pt = 1 / momentum[0];
0091   }
0092
0093   return GlobalVector(
0094       vdt::fast_cos(momentum[2]) * pt, vdt::fast_sin(momentum[2]) * pt, pt / vdt::fast_tan(momentum[1]));
0095 }
0096
0097 TrajectoryStateClosestToPoint PerigeeConversions::trajectoryStateClosestToPoint(
0098     const AlgebraicVector3& momentum,
0099     const GlobalPoint& referencePoint,
0100     const TrackCharge& charge,
0101     const AlgebraicSymMatrix66& theCovarianceMatrix,
0102     const MagneticField* field) {
0103   AlgebraicMatrix66 param2cart = jacobianParameters2Cartesian(momentum, referencePoint, charge, field);
0104   CartesianTrajectoryError cartesianTrajErr(ROOT::Math::Similarity(param2cart, theCovarianceMatrix));
0105
0106   FTS theFTS(GlobalTrajectoryParameters(
0107                  referencePoint, momentumFromPerigee(momentum, charge, referencePoint, field), charge, field),
0108              cartesianTrajErr);
0109
0110   return TrajectoryStateClosestToPoint(theFTS, referencePoint);
0111 }
0112
0113 AlgebraicMatrix66 PerigeeConversions::jacobianParameters2Cartesian(const AlgebraicVector3& momentum,
0114                                                                    const GlobalPoint& position,
0115                                                                    const TrackCharge& charge,
0116                                                                    const MagneticField* field) {
0117   float factor = -1.;
0118   float bField = field->inInverseGeV(position).z();
0119   if (charge != 0 && std::abs(bField) > 1.e-10f)
0120     factor = bField * charge;
0121
0122   float s1, c1;
0123   vdt::fast_sincosf(momentum[1], s1, c1);
0124   float s2, c2;
0125   vdt::fast_sincosf(momentum[2], s2, c2);
0126   float f1 = factor / (momentum[0] * momentum[0]);
0127   float f2 = factor / momentum[0];
0128
0129   AlgebraicMatrix66 frameTransJ;
0130   frameTransJ(0, 0) = 1;
0131   frameTransJ(1, 1) = 1;
0132   frameTransJ(2, 2) = 1;
0133   frameTransJ(3, 3) = (f1 * c2);
0134   frameTransJ(4, 3) = (f1 * s2);
0135   frameTransJ(5, 3) = (f1 * c1 / s1);
0136
0137   frameTransJ(3, 5) = (f2 * s2);
0138   frameTransJ(4, 5) = -(f2 * c2);
0139   frameTransJ(5, 4) = (f2 / (s1 * s1));
0140
0141   return frameTransJ;
0142 }
0143
0144 AlgebraicMatrix55 PerigeeConversions::jacobianCurvilinear2Perigee(const FreeTrajectoryState& fts) {
0145   GlobalVector p = fts.momentum();
0146
0147   GlobalVector Z = GlobalVector(0., 0., 1.);
0148   GlobalVector T = p.unit();
0149   GlobalVector U = Z.cross(T).unit();
0150   ;
0151   GlobalVector V = T.cross(U);
0152
0153   GlobalVector I = GlobalVector(-p.x(), -p.y(), 0.);  //opposite to track dir.
0154   I = I.unit();
0155   GlobalVector J(-I.y(), I.x(), 0.);  //counterclockwise rotation
0156   const GlobalVector& K(Z);
0157   GlobalVector B = fts.parameters().magneticFieldInInverseGeV();
0158   GlobalVector H = B.unit();
0159   GlobalVector HxT = H.cross(T);
0160   GlobalVector N = HxT.unit();
0161   double alpha = HxT.mag();
0162   double qbp = fts.signedInverseMomentum();
0163   double Q = -B.mag() * qbp;
0164   double alphaQ = alpha * Q;
0165
0166   double lambda = 0.5 * M_PI - p.theta();
0167   double sinlambda, coslambda;
0168   vdt::fast_sincos(lambda, sinlambda, coslambda);
0169   double seclambda = 1. / coslambda;
0170
0171   double ITI = 1. / T.dot(I);
0172   double NU = N.dot(U);
0173   double NV = N.dot(V);
0174   double UI = U.dot(I);
0175   double VI = V.dot(I);
0176   double UJ = U.dot(J);
0177   double VJ = V.dot(J);
0178   double UK = U.dot(K);
0179   double VK = V.dot(K);
0180
0181   AlgebraicMatrix55 jac;
0182
0183   if (fabs(fts.transverseCurvature()) < 1.e-10) {
0184     jac(0, 0) = seclambda;
0185     jac(0, 1) = sinlambda * seclambda * seclambda * std::abs(qbp);
0186   } else {
0187     double Bz = B.z();
0188     jac(0, 0) = -Bz * seclambda;
0189     jac(0, 1) = -Bz * sinlambda * seclambda * seclambda * qbp;
0190     jac(1, 3) = alphaQ * NV * UI * ITI;
0191     jac(1, 4) = alphaQ * NV * VI * ITI;
0192     jac(0, 3) = -jac(0, 1) * jac(1, 3);
0193     jac(0, 4) = -jac(0, 1) * jac(1, 4);
0194     jac(2, 3) = -alphaQ * seclambda * NU * UI * ITI;
0195     jac(2, 4) = -alphaQ * seclambda * NU * VI * ITI;
0196   }
0197   jac(1, 1) = -1.;
0198   jac(2, 2) = 1.;
0199   jac(3, 3) = VK * ITI;
0200   jac(3, 4) = -UK * ITI;
0201   jac(4, 3) = -VJ * ITI;
0202   jac(4, 4) = UJ * ITI;
0203
0204   return jac;
0205 }
0206
0207 AlgebraicMatrix55 PerigeeConversions::jacobianPerigee2Curvilinear(const GlobalTrajectoryParameters& gtp) {
0208   GlobalVector p = gtp.momentum();
0209
0210   GlobalVector Z = GlobalVector(0.f, 0.f, 1.f);
0211   GlobalVector T = p.unit();
0212   GlobalVector U = Z.cross(T).unit();
0213   ;
0214   GlobalVector V = T.cross(U);
0215
0216   GlobalVector I = GlobalVector(-p.x(), -p.y(), 0.f);  //opposite to track dir.
0217   I = I.unit();
0218   GlobalVector J(-I.y(), I.x(), 0.f);  //counterclockwise rotation
0219   const GlobalVector& K(Z);
0220   GlobalVector B = gtp.magneticFieldInInverseGeV();
0221   GlobalVector H = B.unit();
0222   GlobalVector HxT = H.cross(T);
0223   GlobalVector N = HxT.unit();
0224   double alpha = HxT.mag();
0225   double qbp = gtp.signedInverseMomentum();
0226   double Q = -B.mag() * qbp;
0227   double alphaQ = alpha * Q;
0228
0229   double lambda = 0.5 * M_PI - p.theta();
0230   double sinlambda, coslambda;
0231   vdt::fast_sincos(lambda, sinlambda, coslambda);
0232   double seclambda = 1. / coslambda;
0233
0234   double mqbpt = -1. / coslambda * qbp;
0235
0236   double TJ = T.dot(J);
0237   double TK = T.dot(K);
0238   double NU = N.dot(U);
0239   double NV = N.dot(V);
0240   double UJ = U.dot(J);
0241   double VJ = V.dot(J);
0242   double UK = U.dot(K);
0243   double VK = V.dot(K);
0244
0245   AlgebraicMatrix55 jac;
0246
0247   if (fabs(gtp.transverseCurvature()) < 1.e-10f) {
0248     jac(0, 0) = coslambda;
0249     jac(0, 1) = sinlambda / coslambda / gtp.momentum().mag();
0250   } else {
0251     jac(0, 0) = -coslambda / B.z();
0252     jac(0, 1) = -sinlambda * mqbpt;
0253     jac(1, 3) = -alphaQ * NV * TJ;
0254     jac(1, 4) = -alphaQ * NV * TK;
0255     jac(2, 3) = -alphaQ * seclambda * NU * TJ;
0256     jac(2, 4) = -alphaQ * seclambda * NU * TK;
0257   }
0258   jac(1, 1) = -1.;
0259   jac(2, 2) = 1.;
0260   jac(3, 3) = UJ;
0261   jac(3, 4) = UK;
0262   jac(4, 3) = VJ;
0263   jac(4, 4) = VK;
0264
0265   return jac;
0266 }