GeomPropagators/src/HelixExtrapolatorToLine2Order.cc

0001 #include "TrackingTools/GeomPropagators/interface/HelixExtrapolatorToLine2Order.h"
0002 #include "DataFormats/GeometrySurface/interface/Line.h"
0003 #include "DataFormats/GeometryVector/interface/GlobalPoint.h"
0004
0005 #include <cfloat>
0006
0007 HelixExtrapolatorToLine2Order::HelixExtrapolatorToLine2Order(const PositionType& point,
0008                                                              const DirectionType& direction,
0009                                                              const float curvature,
0010                                                              const PropagationDirection propDir)
0011     : thePosition(point), theRho(curvature), thePropDir(propDir) {
0012   //
0013   // Components of direction vector (with correct normalisation)
0014   //
0015   double px = direction.x();
0016   double py = direction.y();
0017   double pz = direction.z();
0018   double pt = px * px + py * py;
0019   double p = sqrt(pt + pz * pz);
0020   pt = sqrt(pt);
0021   theDirection = DirectionTypeDouble(px / pt, py / pt, pz / pt);
0022   theSinTheta = pt / p;
0023 }
0024
0025 //
0026 // Propagation status and path length to closest approach with point
0027 //
0028 std::pair<bool, double> HelixExtrapolatorToLine2Order::pathLength(const GlobalPoint& point) const {
0029   //
0030   PositionTypeDouble position(point);
0031   DirectionTypeDouble helix2(-0.5 * theRho * theDirection.y(), 0.5 * theRho * theDirection.x(), 0.);
0032   DirectionTypeDouble deltaPos(thePosition - position);
0033   //
0034   // coefficients of 3rd order equation
0035   //
0036   double ceq[4];
0037   ceq[3] = 2 * helix2.mag2();
0038   // ceq[2] = 3*theDirection.dot(helix2) = 0 since they are orthogonal
0039   ceq[2] = 0.;
0040   ceq[1] = theDirection.mag2() + 2 * deltaPos.dot(helix2);
0041   ceq[0] = deltaPos.dot(theDirection);
0042   //
0043   return pathLengthFromCoefficients(ceq);
0044 }
0045
0046 //
0047 // Propagation status and path length to closest approach with line
0048 //
0049 std::pair<bool, double> HelixExtrapolatorToLine2Order::pathLength(const Line& line) const {
0050   //
0051   // Auxiliary vectors. Assumes that line.direction().mag()=1 !
0052   //
0053   PositionTypeDouble linePosition(line.position());
0054   DirectionTypeDouble lineDirection(line.direction());
0055   DirectionTypeDouble helix2(-0.5 * theRho * theDirection.y(), 0.5 * theRho * theDirection.x(), 0.);
0056   DirectionTypeDouble deltaPos(thePosition - linePosition);
0057   DirectionTypeDouble helix1p(theDirection - lineDirection * theDirection.dot(lineDirection));
0058   DirectionTypeDouble helix2p(helix2 - lineDirection * helix2.dot(lineDirection));
0059   //
0060   // coefficients of 3rd order equation
0061   //
0062   double ceq[4];
0063   ceq[3] = 2 * helix2.dot(helix2p);
0064   // ceq[2] = 3*helix1.dot(helix1p);
0065   // since theDirection.dot(helix2)==0 equivalent to
0066   ceq[2] = 3 * theDirection.dot(lineDirection) * helix2.dot(lineDirection);
0067   ceq[1] = theDirection.dot(helix1p) + 2 * deltaPos.dot(helix2p);
0068   ceq[0] = deltaPos.dot(helix1p);
0069   //
0070   return pathLengthFromCoefficients(ceq);
0071 }
0072
0073 //
0074 // Propagation status and path length to intersection
0075 //
0076 std::pair<bool, double> HelixExtrapolatorToLine2Order::pathLengthFromCoefficients(const double ceq[4]) const {
0077   //
0078   // Solution of 3rd order equation
0079   //
0080   double solutions[3];
0081   unsigned int nRaw = solve3rdOrder(ceq, solutions);
0082   //
0083   // check compatibility with propagation direction
0084   //
0085   unsigned int nDir(0);
0086   for (unsigned int i = 0; i < nRaw; i++) {
0087     if (thePropDir == anyDirection || (solutions[i] >= 0 && thePropDir == alongMomentum) ||
0088         (solutions[i] <= 0 && thePropDir == oppositeToMomentum))
0089       solutions[nDir++] = solutions[i];
0090   }
0091   if (nDir == 0)
0092     return std::make_pair(false, 0.);
0093   //
0094   // check 2nd derivative
0095   //
0096   unsigned int nMin(0);
0097   for (unsigned int i = 0; i < nDir; i++) {
0098     double st = solutions[i];
0099     double deri2 = (3 * ceq[3] * st + 2 * ceq[2]) * st + ceq[1];
0100     if (deri2 > 0.)
0101       solutions[nMin++] = st;
0102   }
0103   if (nMin == 0)
0104     return std::make_pair(false, 0.);
0105   //
0106   // choose smallest path length
0107   //
0108   double dSt = solutions[0];
0109   for (unsigned int i = 1; i < nMin; i++) {
0110     if (fabs(solutions[i]) < fabs(dSt))
0111       dSt = solutions[i];
0112   }
0113
0114   return std::make_pair(true, dSt / theSinTheta);
0115 }
0116
0117 int HelixExtrapolatorToLine2Order::solve3rdOrder(const double ceq[], double solutions[]) const {
0118   //
0119   // Real 3rd order equation? Follow numerical recipes ..
0120   //
0121   if (fabs(ceq[3]) > FLT_MIN) {
0122     int result(0);
0123     double q = (ceq[2] * ceq[2] - 3 * ceq[3] * ceq[1]) / (ceq[3] * ceq[3]) / 9.;
0124     double r = (2 * ceq[2] * ceq[2] * ceq[2] - 9 * ceq[3] * ceq[2] * ceq[1] + 27 * ceq[3] * ceq[3] * ceq[0]) /
0125                (ceq[3] * ceq[3] * ceq[3]) / 54.;
0126     double q3 = q * q * q;
0127     if (r * r < q3) {
0128       double phi = acos(r / sqrt(q3)) / 3.;
0129       double rootq = sqrt(q);
0130       for (int i = 0; i < 3; i++) {
0131         solutions[i] = -2 * rootq * cos(phi) - ceq[2] / ceq[3] / 3.;
0132         phi += 2. / 3. * M_PI;
0133       }
0134       result = 3;
0135     } else {
0136       double a = pow(fabs(r) + sqrt(r * r - q3), 1. / 3.);
0137       if (r > 0.)
0138         a *= -1;
0139       double b = fabs(a) > FLT_MIN ? q / a : 0.;
0140       solutions[0] = a + b - ceq[2] / ceq[3] / 3.;
0141       result = 1;
0142     }
0143     return result;
0144   }
0145   //
0146   // Second order equation
0147   //
0148   else if (fabs(ceq[2]) > FLT_MIN) {
0149     return solve2ndOrder(ceq, solutions);
0150   } else {
0151     //
0152     // Special case: linear equation
0153     //
0154     solutions[0] = -ceq[0] / ceq[1];
0155     return 1;
0156   }
0157 }
0158
0159 int HelixExtrapolatorToLine2Order::solve2ndOrder(const double coeff[], double solutions[]) const {
0160   //
0161   double deq1 = coeff[1] * coeff[1];
0162   double deq2 = coeff[2] * coeff[0];
0163   if (fabs(deq1) < FLT_MIN || fabs(deq2 / deq1) > 1.e-6) {
0164     //
0165     // Standard solution for quadratic equations
0166     //
0167     double deq = deq1 + 2 * deq2;
0168     if (deq < 0.)
0169       return 0;
0170     double ceq = -0.5 * (coeff[1] + (coeff[1] > 0 ? 1 : -1) * sqrt(deq));
0171     solutions[0] = -2 * ceq / coeff[2];
0172     solutions[1] = coeff[0] / ceq;
0173     return 2;
0174   } else {
0175     //
0176     // Solution by expansion of sqrt(1+deq)
0177     //
0178     double ceq = coeff[1] / coeff[2];
0179     double deq = deq2 / deq1;
0180     deq *= (1 - deq / 2);
0181     solutions[0] = -ceq * deq;
0182     solutions[1] = ceq * (2 + deq);
0183     return 2;
0184   }
0185 }
0186 //
0187 // Position after a step of path length s (2nd order)
0188 //
0189 HelixLineExtrapolation::PositionType HelixExtrapolatorToLine2Order::position(double s) const {
0190   // use double precision result
0191   //   PositionTypeDouble pos = positionInDouble(s);
0192   //   return PositionType(pos.x(),pos.y(),pos.z());
0193   return PositionType(positionInDouble(s));
0194 }
0195 //
0196 // Position after a step of path length s (2nd order) (in double precision)
0197 //
0198 HelixExtrapolatorToLine2Order::PositionTypeDouble HelixExtrapolatorToLine2Order::positionInDouble(double s) const {
0199   // based on path length in the transverse plane
0200   double st = s * theSinTheta;
0201   return PositionTypeDouble(thePosition.x() + (theDirection.x() - st * 0.5 * theRho * theDirection.y()) * st,
0202                             thePosition.y() + (theDirection.y() + st * 0.5 * theRho * theDirection.x()) * st,
0203                             thePosition.z() + st * theDirection.z());
0204 }
0205 //
0206 // Direction after a step of path length 2 (2nd order) (in double precision)
0207 //
0208 HelixLineExtrapolation::DirectionType HelixExtrapolatorToLine2Order::direction(double s) const {
0209   // use double precision result
0210   //   DirectionTypeDouble dir = directionInDouble(s);
0211   //   return DirectionType(dir.x(),dir.y(),dir.z());
0212   return DirectionType(directionInDouble(s));
0213 }
0214 //
0215 // Direction after a step of path length 2 (2nd order)
0216 //
0217 HelixExtrapolatorToLine2Order::DirectionTypeDouble HelixExtrapolatorToLine2Order::directionInDouble(double s) const {
0218   // based on delta phi
0219   double dph = s * theRho * theSinTheta;
0220   return DirectionTypeDouble(theDirection.x() - (theDirection.y() + 0.5 * theDirection.x() * dph) * dph,
0221                              theDirection.y() + (theDirection.x() - 0.5 * theDirection.y() * dph) * dph,
0222                              theDirection.z());
0223 }