GeomPropagators/src/HelixArbitraryPlaneCrossing2Order.cc

0001 #include "TrackingTools/GeomPropagators/interface/HelixArbitraryPlaneCrossing2Order.h"
0002
0003 #include "DataFormats/GeometrySurface/interface/Plane.h"
0004 #include "DataFormats/GeometryVector/interface/GlobalPoint.h"
0005
0006 #include <cmath>
0007 #include <cfloat>
0008 #include "FWCore/Utilities/interface/Likely.h"
0009 #include "FWCore/Utilities/interface/isFinite.h"
0010
0011 HelixArbitraryPlaneCrossing2Order::HelixArbitraryPlaneCrossing2Order(const PositionType& point,
0012                                                                      const DirectionType& direction,
0013                                                                      const float curvature,
0014                                                                      const PropagationDirection propDir)
0015     : theX0(point.x()), theY0(point.y()), theZ0(point.z()), theRho(curvature), thePropDir(propDir) {
0016   //
0017   // Components of direction vector (with correct normalisation)
0018   //
0019   double px = direction.x();
0020   double py = direction.y();
0021   double pz = direction.z();
0022   double pt2 = px * px + py * py;
0023   double p2 = pt2 + pz * pz;
0024   double pI = 1. / sqrt(p2);
0025   double ptI = 1. / sqrt(pt2);
0026   theCosPhi0 = px * ptI;
0027   theSinPhi0 = py * ptI;
0028   theCosTheta = pz * pI;
0029   theSinThetaI = p2 * pI * ptI;  //  (1/(pt/p)) = p/pt = p*ptI and p = p2/p = p2*pI
0030 }
0031
0032 //
0033 // Propagation status and path length to intersection
0034 //
0035 std::pair<bool, double> HelixArbitraryPlaneCrossing2Order::pathLength(const Plane& plane) {
0036   //
0037   // get local z-vector in global co-ordinates and
0038   // distance to starting point
0039   //
0040   GlobalVector normalToPlane = plane.normalVector();
0041   double nPx = normalToPlane.x();
0042   double nPy = normalToPlane.y();
0043   double nPz = normalToPlane.z();
0044   double cP = plane.localZ(GlobalPoint(theX0, theY0, theZ0));
0045   //
0046   // coefficients of 2nd order equation to obtain intersection point
0047   // in approximation (without curvature-related factors).
0048   //
0049   double ceq1 = theRho * (nPx * theSinPhi0 - nPy * theCosPhi0);
0050   double ceq2 = nPx * theCosPhi0 + nPy * theSinPhi0 + nPz * theCosTheta * theSinThetaI;
0051   double ceq3 = cP;
0052   //
0053   // Check for degeneration to linear equation (zero
0054   //   curvature, forward plane or direction perp. to plane)
0055   //
0056   double dS1, dS2;
0057   if LIKELY (std::abs(ceq1) > FLT_MIN) {
0058     double deq1 = ceq2 * ceq2;
0059     double deq2 = ceq1 * ceq3;
0060     if (std::abs(deq1) < FLT_MIN || std::abs(deq2 / deq1) > 1.e-6) {
0061       //
0062       // Standard solution for quadratic equations
0063       //
0064       double deq = deq1 + 2 * deq2;
0065       if UNLIKELY (deq < 0.)
0066         return std::pair<bool, double>(false, 0);
0067       double ceq = ceq2 + std::copysign(std::sqrt(deq), ceq2);
0068       dS1 = (ceq / ceq1) * theSinThetaI;
0069       dS2 = -2. * (ceq3 / ceq) * theSinThetaI;
0070     } else {
0071       //
0072       // Solution by expansion of sqrt(1+deq)
0073       //
0074       double ceq = (ceq2 / ceq1) * theSinThetaI;
0075       double deq = deq2 / deq1;
0076       deq *= (1 - 0.5 * deq);
0077       dS1 = -ceq * deq;
0078       dS2 = ceq * (2 + deq);
0079     }
0080   } else {
0081     //
0082     // Special case: linear equation
0083     //
0084     dS1 = dS2 = -(ceq3 / ceq2) * theSinThetaI;
0085   }
0086   //
0087   // Choose and return solution
0088   //
0089   return solutionByDirection(dS1, dS2);
0090 }
0091
0092 //
0093 // Position after a step of path length s (2nd order)
0094 //
0095 HelixPlaneCrossing::PositionType HelixArbitraryPlaneCrossing2Order::position(double s) const {
0096   // use double precision result
0097   PositionTypeDouble pos = positionInDouble(s);
0098   return PositionType(pos.x(), pos.y(), pos.z());
0099 }
0100 //
0101 // Position after a step of path length s (2nd order) (in double precision)
0102 //
0103 HelixArbitraryPlaneCrossing2Order::PositionTypeDouble HelixArbitraryPlaneCrossing2Order::positionInDouble(
0104     double s) const {
0105   // based on path length in the transverse plane
0106   double st = s / theSinThetaI;
0107   return PositionTypeDouble(theX0 + (theCosPhi0 - (st * 0.5 * theRho) * theSinPhi0) * st,
0108                             theY0 + (theSinPhi0 + (st * 0.5 * theRho) * theCosPhi0) * st,
0109                             theZ0 + st * theCosTheta * theSinThetaI);
0110 }
0111 //
0112 // Direction after a step of path length 2 (2nd order) (in double precision)
0113 //
0114 HelixPlaneCrossing::DirectionType HelixArbitraryPlaneCrossing2Order::direction(double s) const {
0115   // use double precision result
0116   DirectionTypeDouble dir = directionInDouble(s);
0117   return DirectionType(dir.x(), dir.y(), dir.z());
0118 }
0119 //
0120 // Direction after a step of path length 2 (2nd order)
0121 //
0122 HelixArbitraryPlaneCrossing2Order::DirectionTypeDouble HelixArbitraryPlaneCrossing2Order::directionInDouble(
0123     double s) const {
0124   // based on delta phi
0125   double dph = s * theRho / theSinThetaI;
0126   return DirectionTypeDouble(theCosPhi0 - (theSinPhi0 + 0.5 * dph * theCosPhi0) * dph,
0127                              theSinPhi0 + (theCosPhi0 - 0.5 * dph * theSinPhi0) * dph,
0128                              theCosTheta * theSinThetaI);
0129 }
0130 //
0131 // Choice of solution according to propagation direction
0132 //
0133 std::pair<bool, double> HelixArbitraryPlaneCrossing2Order::solutionByDirection(const double dS1,
0134                                                                                const double dS2) const {
0135   bool valid = false;
0136   double path = 0;
0137   if (thePropDir == anyDirection) {
0138     valid = true;
0139     path = smallestPathLength(dS1, dS2);
0140   } else {
0141     // use same logic for both directions (invert if necessary)
0142     double propSign = thePropDir == alongMomentum ? 1 : -1;
0143     double s1(propSign * dS1);
0144     double s2(propSign * dS2);
0145     // sort
0146     if (s1 > s2)
0147       std::swap(s1, s2);
0148     // choose solution (if any with positive sign)
0149     if ((s1 < 0) & (s2 >= 0)) {
0150       // First solution in backward direction: choose second one.
0151       valid = true;
0152       path = propSign * s2;
0153     } else if (s1 >= 0) {
0154       // First solution in forward direction: choose it (s2 is further away!).
0155       valid = true;
0156       path = propSign * s1;
0157     }
0158   }
0159   if (edm::isNotFinite(path))
0160     valid = false;
0161   return std::pair<bool, double>(valid, path);
0162 }