TrackingTools/TrajectoryState/src/PerigeeConversions.cc - master - githubmirror/cmssw

TrackingTools/TrajectoryState/src/PerigeeConversions.cc
266 строк10 KB
Shahzad Malik Muzaffar
[RECONSTRUCTION] [LLVM21] Apply clang-tidy changes
26 ноя 2025, 17:10
26 ноя 2025, 17:107863ba0
#include "TrackingTools/TrajectoryState/interface/PerigeeConversions.h"
#include "TrackingTools/TrajectoryState/interface/TrajectoryStateClosestToPoint.h"
#include "MagneticField/Engine/interface/MagneticField.h"
#include <cmath>
#include <vdt/vdtMath.h>

std::optional<PerigeeTrajectoryParameters> PerigeeConversions::ftsToPerigeeParameters(const FTS& originalFTS,
                                                                                      const GlobalPoint& referencePoint,
                                                                                      double& pt)

{
  GlobalVector impactDistance = originalFTS.position() - referencePoint;

  pt = originalFTS.momentum().perp();
  if (pt == 0.)
    return std::nullopt;

  double theta = originalFTS.momentum().theta();
  double phi = originalFTS.momentum().phi();
  double field = originalFTS.parameters().magneticFieldInInverseGeV().z();
  //   if (field==0.) throw cms::Exception("PerigeeConversions", "Field is 0") << " at " << originalFTS.position() << "\n" ;

  double positiveMomentumPhi = ((phi > 0) ? phi : (2 * M_PI + phi));
  double positionPhi = impactDistance.phi();
  double positivePositionPhi = ((positionPhi > 0) ? positionPhi : (2 * M_PI + positionPhi));
  double phiDiff = positiveMomentumPhi - positivePositionPhi;
  if (phiDiff < 0.0)
    phiDiff += (2 * M_PI);
  double signEpsilon = ((phiDiff > M_PI) ? -1.0 : 1.0);

  double epsilon =
      signEpsilon * sqrt(impactDistance.x() * impactDistance.x() + impactDistance.y() * impactDistance.y());

  // The track parameters:
  AlgebraicVector5 theTrackParameters;

  double signTC = -originalFTS.charge();
  bool isCharged = (signTC != 0) && (std::abs(field) > 1.e-10);
  if (isCharged) {
    theTrackParameters[0] = field / pt * signTC;
  } else {
    theTrackParameters[0] = 1 / pt;
  }
  theTrackParameters[1] = theta;
  theTrackParameters[2] = phi;
  theTrackParameters[3] = epsilon;
  theTrackParameters[4] = impactDistance.z();
  return PerigeeTrajectoryParameters(theTrackParameters, isCharged);
}

PerigeeTrajectoryError PerigeeConversions::ftsToPerigeeError(const FTS& originalFTS) {
  AlgebraicSymMatrix55 errorMatrix = originalFTS.curvilinearError().matrix();
  AlgebraicMatrix55 curv2perigee = jacobianCurvilinear2Perigee(originalFTS);
  return PerigeeTrajectoryError(ROOT::Math::Similarity(curv2perigee, errorMatrix));
}

CurvilinearTrajectoryError PerigeeConversions::curvilinearError(const PerigeeTrajectoryError& perigeeError,
                                                                const GlobalTrajectoryParameters& gtp) {
  AlgebraicMatrix55 perigee2curv = jacobianPerigee2Curvilinear(gtp);
  return CurvilinearTrajectoryError(ROOT::Math::Similarity(perigee2curv, perigeeError.covarianceMatrix()));
}

GlobalPoint PerigeeConversions::positionFromPerigee(const PerigeeTrajectoryParameters& parameters,
                                                    const GlobalPoint& referencePoint) {
  const AlgebraicVector5& theVector = parameters.vector();
  return GlobalPoint(theVector[3] * vdt::fast_sin(theVector[2]) + referencePoint.x(),
                     -theVector[3] * vdt::fast_cos(theVector[2]) + referencePoint.y(),
                     theVector[4] + referencePoint.z());
}

GlobalVector PerigeeConversions::momentumFromPerigee(const PerigeeTrajectoryParameters& parameters,
                                                     double pt,
                                                     const GlobalPoint& referencePoint) {
  return GlobalVector(vdt::fast_cos(parameters.phi()) * pt,
                      vdt::fast_sin(parameters.phi()) * pt,
                      pt / vdt::fast_tan(parameters.theta()));
}

GlobalVector PerigeeConversions::momentumFromPerigee(const AlgebraicVector3& momentum,
                                                     const TrackCharge& charge,
                                                     const GlobalPoint& referencePoint,
                                                     const MagneticField* field) {
  double pt;

  double bz = fabs(field->inInverseGeV(referencePoint).z());
  if (charge != 0 && std::abs(bz) > 1.e-10) {
    pt = std::abs(bz / momentum[0]);
    // if (pt<1.e-10) throw cms::Exception("PerigeeConversions", "pt is 0");
  } else {
    pt = 1 / momentum[0];
  }

  return GlobalVector(
      vdt::fast_cos(momentum[2]) * pt, vdt::fast_sin(momentum[2]) * pt, pt / vdt::fast_tan(momentum[1]));
}

TrajectoryStateClosestToPoint PerigeeConversions::trajectoryStateClosestToPoint(
    const AlgebraicVector3& momentum,
    const GlobalPoint& referencePoint,
    const TrackCharge& charge,
    const AlgebraicSymMatrix66& theCovarianceMatrix,
    const MagneticField* field) {
  AlgebraicMatrix66 param2cart = jacobianParameters2Cartesian(momentum, referencePoint, charge, field);
  CartesianTrajectoryError cartesianTrajErr(ROOT::Math::Similarity(param2cart, theCovarianceMatrix));

  FTS theFTS(GlobalTrajectoryParameters(
                 referencePoint, momentumFromPerigee(momentum, charge, referencePoint, field), charge, field),
             cartesianTrajErr);

  return TrajectoryStateClosestToPoint(theFTS, referencePoint);
}

AlgebraicMatrix66 PerigeeConversions::jacobianParameters2Cartesian(const AlgebraicVector3& momentum,
                                                                   const GlobalPoint& position,
                                                                   const TrackCharge& charge,
                                                                   const MagneticField* field) {
  float factor = -1.;
  float bField = field->inInverseGeV(position).z();
  if (charge != 0 && std::abs(bField) > 1.e-10f)
    factor = bField * charge;

  float s1, c1;
  vdt::fast_sincosf(momentum[1], s1, c1);
  float s2, c2;
  vdt::fast_sincosf(momentum[2], s2, c2);
  float f1 = factor / (momentum[0] * momentum[0]);
  float f2 = factor / momentum[0];

  AlgebraicMatrix66 frameTransJ;
  frameTransJ(0, 0) = 1;
  frameTransJ(1, 1) = 1;
  frameTransJ(2, 2) = 1;
  frameTransJ(3, 3) = (f1 * c2);
  frameTransJ(4, 3) = (f1 * s2);
  frameTransJ(5, 3) = (f1 * c1 / s1);

  frameTransJ(3, 5) = (f2 * s2);
  frameTransJ(4, 5) = -(f2 * c2);
  frameTransJ(5, 4) = (f2 / (s1 * s1));

  return frameTransJ;
}

AlgebraicMatrix55 PerigeeConversions::jacobianCurvilinear2Perigee(const FreeTrajectoryState& fts) {
  GlobalVector p = fts.momentum();

  GlobalVector Z = GlobalVector(0., 0., 1.);
  GlobalVector T = p.unit();
  GlobalVector U = Z.cross(T).unit();
  ;
  GlobalVector V = T.cross(U);

  GlobalVector I = GlobalVector(-p.x(), -p.y(), 0.);  //opposite to track dir.
  I = I.unit();
  GlobalVector J(-I.y(), I.x(), 0.);  //counterclockwise rotation
  const GlobalVector& K(Z);
  GlobalVector B = fts.parameters().magneticFieldInInverseGeV();
  GlobalVector H = B.unit();
  GlobalVector HxT = H.cross(T);
  GlobalVector N = HxT.unit();
  double alpha = HxT.mag();
  double qbp = fts.signedInverseMomentum();
  double Q = -B.mag() * qbp;
  double alphaQ = alpha * Q;

  double lambda = 0.5 * M_PI - p.theta();
  double sinlambda, coslambda;
  vdt::fast_sincos(lambda, sinlambda, coslambda);
  double seclambda = 1. / coslambda;

  double ITI = 1. / T.dot(I);
  double NU = N.dot(U);
  double NV = N.dot(V);
  double UI = U.dot(I);
  double VI = V.dot(I);
  double UJ = U.dot(J);
  double VJ = V.dot(J);
  double UK = U.dot(K);
  double VK = V.dot(K);

  AlgebraicMatrix55 jac;

  if (fabs(fts.transverseCurvature()) < 1.e-10) {
    jac(0, 0) = seclambda;
    jac(0, 1) = sinlambda * seclambda * seclambda * std::abs(qbp);
  } else {
    double Bz = B.z();
    jac(0, 0) = -Bz * seclambda;
    jac(0, 1) = -Bz * sinlambda * seclambda * seclambda * qbp;
    jac(1, 3) = alphaQ * NV * UI * ITI;
    jac(1, 4) = alphaQ * NV * VI * ITI;
    jac(0, 3) = -jac(0, 1) * jac(1, 3);
    jac(0, 4) = -jac(0, 1) * jac(1, 4);
    jac(2, 3) = -alphaQ * seclambda * NU * UI * ITI;
    jac(2, 4) = -alphaQ * seclambda * NU * VI * ITI;
  }
  jac(1, 1) = -1.;
  jac(2, 2) = 1.;
  jac(3, 3) = VK * ITI;
  jac(3, 4) = -UK * ITI;
  jac(4, 3) = -VJ * ITI;
  jac(4, 4) = UJ * ITI;

  return jac;
}

AlgebraicMatrix55 PerigeeConversions::jacobianPerigee2Curvilinear(const GlobalTrajectoryParameters& gtp) {
  GlobalVector p = gtp.momentum();

  GlobalVector Z = GlobalVector(0.f, 0.f, 1.f);
  GlobalVector T = p.unit();
  GlobalVector U = Z.cross(T).unit();
  ;
  GlobalVector V = T.cross(U);

  GlobalVector I = GlobalVector(-p.x(), -p.y(), 0.f);  //opposite to track dir.
  I = I.unit();
  GlobalVector J(-I.y(), I.x(), 0.f);  //counterclockwise rotation
  const GlobalVector& K(Z);
  GlobalVector B = gtp.magneticFieldInInverseGeV();
  GlobalVector H = B.unit();
  GlobalVector HxT = H.cross(T);
  GlobalVector N = HxT.unit();
  double alpha = HxT.mag();
  double qbp = gtp.signedInverseMomentum();
  double Q = -B.mag() * qbp;
  double alphaQ = alpha * Q;

  double lambda = 0.5 * M_PI - p.theta();
  double sinlambda, coslambda;
  vdt::fast_sincos(lambda, sinlambda, coslambda);
  double seclambda = 1. / coslambda;

  double mqbpt = -1. / coslambda * qbp;

  double TJ = T.dot(J);
  double TK = T.dot(K);
  double NU = N.dot(U);
  double NV = N.dot(V);
  double UJ = U.dot(J);
  double VJ = V.dot(J);
  double UK = U.dot(K);
  double VK = V.dot(K);

  AlgebraicMatrix55 jac;

  if (fabs(gtp.transverseCurvature()) < 1.e-10f) {
    jac(0, 0) = coslambda;
    jac(0, 1) = sinlambda / coslambda / gtp.momentum().mag();
  } else {
    jac(0, 0) = -coslambda / B.z();
    jac(0, 1) = -sinlambda * mqbpt;
    jac(1, 3) = -alphaQ * NV * TJ;
    jac(1, 4) = -alphaQ * NV * TK;
    jac(2, 3) = -alphaQ * seclambda * NU * TJ;
    jac(2, 4) = -alphaQ * seclambda * NU * TK;
  }
  jac(1, 1) = -1.;
  jac(2, 2) = 1.;
  jac(3, 3) = UJ;
  jac(3, 4) = UK;
  jac(4, 3) = VJ;
  jac(4, 4) = VK;

  return jac;
}