TrackingTools/GeomPropagators/src/HelixExtrapolatorToLine2Order.cc - master - githubmirror/cmssw

TrackingTools/GeomPropagators/src/HelixExtrapolatorToLine2Order.cc
223 строки8 KB
Cms Build
Clang-Format
27 май 2019, 05:59
27 май 2019, 05:594ed5991
#include "TrackingTools/GeomPropagators/interface/HelixExtrapolatorToLine2Order.h"
#include "DataFormats/GeometrySurface/interface/Line.h"
#include "DataFormats/GeometryVector/interface/GlobalPoint.h"

#include <cfloat>

HelixExtrapolatorToLine2Order::HelixExtrapolatorToLine2Order(const PositionType& point,
                                                             const DirectionType& direction,
                                                             const float curvature,
                                                             const PropagationDirection propDir)
    : thePosition(point), theRho(curvature), thePropDir(propDir) {
  //
  // Components of direction vector (with correct normalisation)
  //
  double px = direction.x();
  double py = direction.y();
  double pz = direction.z();
  double pt = px * px + py * py;
  double p = sqrt(pt + pz * pz);
  pt = sqrt(pt);
  theDirection = DirectionTypeDouble(px / pt, py / pt, pz / pt);
  theSinTheta = pt / p;
}

//
// Propagation status and path length to closest approach with point
//
std::pair<bool, double> HelixExtrapolatorToLine2Order::pathLength(const GlobalPoint& point) const {
  //
  PositionTypeDouble position(point);
  DirectionTypeDouble helix2(-0.5 * theRho * theDirection.y(), 0.5 * theRho * theDirection.x(), 0.);
  DirectionTypeDouble deltaPos(thePosition - position);
  //
  // coefficients of 3rd order equation
  //
  double ceq[4];
  ceq[3] = 2 * helix2.mag2();
  // ceq[2] = 3*theDirection.dot(helix2) = 0 since they are orthogonal
  ceq[2] = 0.;
  ceq[1] = theDirection.mag2() + 2 * deltaPos.dot(helix2);
  ceq[0] = deltaPos.dot(theDirection);
  //
  return pathLengthFromCoefficients(ceq);
}

//
// Propagation status and path length to closest approach with line
//
std::pair<bool, double> HelixExtrapolatorToLine2Order::pathLength(const Line& line) const {
  //
  // Auxiliary vectors. Assumes that line.direction().mag()=1 !
  //
  PositionTypeDouble linePosition(line.position());
  DirectionTypeDouble lineDirection(line.direction());
  DirectionTypeDouble helix2(-0.5 * theRho * theDirection.y(), 0.5 * theRho * theDirection.x(), 0.);
  DirectionTypeDouble deltaPos(thePosition - linePosition);
  DirectionTypeDouble helix1p(theDirection - lineDirection * theDirection.dot(lineDirection));
  DirectionTypeDouble helix2p(helix2 - lineDirection * helix2.dot(lineDirection));
  //
  // coefficients of 3rd order equation
  //
  double ceq[4];
  ceq[3] = 2 * helix2.dot(helix2p);
  // ceq[2] = 3*helix1.dot(helix1p);
  // since theDirection.dot(helix2)==0 equivalent to
  ceq[2] = 3 * theDirection.dot(lineDirection) * helix2.dot(lineDirection);
  ceq[1] = theDirection.dot(helix1p) + 2 * deltaPos.dot(helix2p);
  ceq[0] = deltaPos.dot(helix1p);
  //
  return pathLengthFromCoefficients(ceq);
}

//
// Propagation status and path length to intersection
//
std::pair<bool, double> HelixExtrapolatorToLine2Order::pathLengthFromCoefficients(const double ceq[4]) const {
  //
  // Solution of 3rd order equation
  //
  double solutions[3];
  unsigned int nRaw = solve3rdOrder(ceq, solutions);
  //
  // check compatibility with propagation direction
  //
  unsigned int nDir(0);
  for (unsigned int i = 0; i < nRaw; i++) {
    if (thePropDir == anyDirection || (solutions[i] >= 0 && thePropDir == alongMomentum) ||
        (solutions[i] <= 0 && thePropDir == oppositeToMomentum))
      solutions[nDir++] = solutions[i];
  }
  if (nDir == 0)
    return std::make_pair(false, 0.);
  //
  // check 2nd derivative
  //
  unsigned int nMin(0);
  for (unsigned int i = 0; i < nDir; i++) {
    double st = solutions[i];
    double deri2 = (3 * ceq[3] * st + 2 * ceq[2]) * st + ceq[1];
    if (deri2 > 0.)
      solutions[nMin++] = st;
  }
  if (nMin == 0)
    return std::make_pair(false, 0.);
  //
  // choose smallest path length
  //
  double dSt = solutions[0];
  for (unsigned int i = 1; i < nMin; i++) {
    if (fabs(solutions[i]) < fabs(dSt))
      dSt = solutions[i];
  }

  return std::make_pair(true, dSt / theSinTheta);
}

int HelixExtrapolatorToLine2Order::solve3rdOrder(const double ceq[], double solutions[]) const {
  //
  // Real 3rd order equation? Follow numerical recipes ..
  //
  if (fabs(ceq[3]) > FLT_MIN) {
    int result(0);
    double q = (ceq[2] * ceq[2] - 3 * ceq[3] * ceq[1]) / (ceq[3] * ceq[3]) / 9.;
    double r = (2 * ceq[2] * ceq[2] * ceq[2] - 9 * ceq[3] * ceq[2] * ceq[1] + 27 * ceq[3] * ceq[3] * ceq[0]) /
               (ceq[3] * ceq[3] * ceq[3]) / 54.;
    double q3 = q * q * q;
    if (r * r < q3) {
      double phi = acos(r / sqrt(q3)) / 3.;
      double rootq = sqrt(q);
      for (int i = 0; i < 3; i++) {
        solutions[i] = -2 * rootq * cos(phi) - ceq[2] / ceq[3] / 3.;
        phi += 2. / 3. * M_PI;
      }
      result = 3;
    } else {
      double a = pow(fabs(r) + sqrt(r * r - q3), 1. / 3.);
      if (r > 0.)
        a *= -1;
      double b = fabs(a) > FLT_MIN ? q / a : 0.;
      solutions[0] = a + b - ceq[2] / ceq[3] / 3.;
      result = 1;
    }
    return result;
  }
  //
  // Second order equation
  //
  else if (fabs(ceq[2]) > FLT_MIN) {
    return solve2ndOrder(ceq, solutions);
  } else {
    //
    // Special case: linear equation
    //
    solutions[0] = -ceq[0] / ceq[1];
    return 1;
  }
}

int HelixExtrapolatorToLine2Order::solve2ndOrder(const double coeff[], double solutions[]) const {
  //
  double deq1 = coeff[1] * coeff[1];
  double deq2 = coeff[2] * coeff[0];
  if (fabs(deq1) < FLT_MIN || fabs(deq2 / deq1) > 1.e-6) {
    //
    // Standard solution for quadratic equations
    //
    double deq = deq1 + 2 * deq2;
    if (deq < 0.)
      return 0;
    double ceq = -0.5 * (coeff[1] + (coeff[1] > 0 ? 1 : -1) * sqrt(deq));
    solutions[0] = -2 * ceq / coeff[2];
    solutions[1] = coeff[0] / ceq;
    return 2;
  } else {
    //
    // Solution by expansion of sqrt(1+deq)
    //
    double ceq = coeff[1] / coeff[2];
    double deq = deq2 / deq1;
    deq *= (1 - deq / 2);
    solutions[0] = -ceq * deq;
    solutions[1] = ceq * (2 + deq);
    return 2;
  }
}
//
// Position after a step of path length s (2nd order)
//
HelixLineExtrapolation::PositionType HelixExtrapolatorToLine2Order::position(double s) const {
  // use double precision result
  //   PositionTypeDouble pos = positionInDouble(s);
  //   return PositionType(pos.x(),pos.y(),pos.z());
  return PositionType(positionInDouble(s));
}
//
// Position after a step of path length s (2nd order) (in double precision)
//
HelixExtrapolatorToLine2Order::PositionTypeDouble HelixExtrapolatorToLine2Order::positionInDouble(double s) const {
  // based on path length in the transverse plane
  double st = s * theSinTheta;
  return PositionTypeDouble(thePosition.x() + (theDirection.x() - st * 0.5 * theRho * theDirection.y()) * st,
                            thePosition.y() + (theDirection.y() + st * 0.5 * theRho * theDirection.x()) * st,
                            thePosition.z() + st * theDirection.z());
}
//
// Direction after a step of path length 2 (2nd order) (in double precision)
//
HelixLineExtrapolation::DirectionType HelixExtrapolatorToLine2Order::direction(double s) const {
  // use double precision result
  //   DirectionTypeDouble dir = directionInDouble(s);
  //   return DirectionType(dir.x(),dir.y(),dir.z());
  return DirectionType(directionInDouble(s));
}
//
// Direction after a step of path length 2 (2nd order)
//
HelixExtrapolatorToLine2Order::DirectionTypeDouble HelixExtrapolatorToLine2Order::directionInDouble(double s) const {
  // based on delta phi
  double dph = s * theRho * theSinTheta;
  return DirectionTypeDouble(theDirection.x() - (theDirection.y() + 0.5 * theDirection.x() * dph) * dph,
                             theDirection.y() + (theDirection.x() - 0.5 * theDirection.y() * dph) * dph,
                             theDirection.z());
}