GeomPropagators/src/IterativeHelixExtrapolatorToLine.cc

0001 #include "TrackingTools/GeomPropagators/interface/IterativeHelixExtrapolatorToLine.h"
0002 #include <iostream>
0003
0004 IterativeHelixExtrapolatorToLine::IterativeHelixExtrapolatorToLine(const PositionType& point,
0005                                                                    const DirectionType& direction,
0006                                                                    const float curvature,
0007                                                                    const PropagationDirection propDir)
0008     : theX0(point.x()),
0009       theY0(point.y()),
0010       theZ0(point.z()),
0011       theRho(curvature),
0012       theQuadraticSolutionFromStart(point, direction, curvature, propDir),
0013       thePropDir(propDir),
0014       theCachedS(0),
0015       theCachedDPhi(0.),
0016       theCachedSDPhi(0.),
0017       theCachedCDPhi(1.) {
0018   //
0019   // Components of direction vector (with correct normalisation)
0020   //
0021   double px = direction.x();
0022   double py = direction.y();
0023   double pz = direction.z();
0024   double pt = px * px + py * py;
0025   double p = sqrt(pt + pz * pz);
0026   pt = sqrt(pt);
0027   theCosPhi0 = px / pt;
0028   theSinPhi0 = py / pt;
0029   theCosTheta = pz / p;
0030   theSinTheta = pt / p;
0031 }
0032
0033 /** Propagation status (true if valid) and (signed) path length
0034    *  along the helix from the starting point to the closest approach.
0035    *  to the point. The starting point is given in the constructor.
0036    */
0037 std::pair<bool, double> IterativeHelixExtrapolatorToLine::pathLength(const GlobalPoint& point) const {
0038   return genericPathLength(point);
0039 }
0040
0041 /** Propagation status (true if valid) and (signed) path length
0042    *  along the helix from the starting point to the closest approach
0043    *  to the line. The starting point is given in the constructor.
0044    */
0045 std::pair<bool, double> IterativeHelixExtrapolatorToLine::pathLength(const Line& line) const {
0046   return genericPathLength(line);
0047 }
0048
0049 //
0050 // Propagation status and path length to intersection
0051 //
0052 template <class T>
0053 std::pair<bool, double> IterativeHelixExtrapolatorToLine::genericPathLength(const T& object) const {
0054   //
0055   // Constants used for control of convergence
0056   //
0057   const int maxIterations(100);
0058   //
0059   // Prepare internal value of the propagation direction and position / direction vectors for iteration
0060   //
0061   PropagationDirection propDir = thePropDir;
0062   PositionTypeDouble xnew(theX0, theY0, theZ0);
0063   DirectionTypeDouble pnew(theCosPhi0, theSinPhi0, theCosTheta / theSinTheta);
0064   //
0065   // Prepare iterations: count and total pathlength
0066   //
0067   unsigned int iteration(maxIterations + 1);
0068   double dSTotal(0.);
0069   //
0070   // Convergence criterion: maximal lateral displacement in a step < 1um
0071   //
0072   double maxDeltaS2 = 2 * 1.e-4 / theSinTheta / theSinTheta / fabs(theRho);
0073   //
0074   bool first(true);
0075   while (true) {
0076     //
0077     // return empty solution vector if no convergence after maxIterations iterations
0078     //
0079     if (--iteration == 0) {
0080       return std::pair<bool, double>(false, 0);
0081     }
0082     //
0083     // Use existing second order object at first pass, create temporary object
0084     // for subsequent passes.
0085     //
0086     std::pair<bool, double> deltaS1;
0087     if (first) {
0088       first = false;
0089       deltaS1 = theQuadraticSolutionFromStart.pathLength(object);
0090     } else {
0091       HelixExtrapolatorToLine2Order linearCrossing(
0092           xnew.x(), xnew.y(), xnew.z(), pnew.x(), pnew.y(), theCosTheta, theSinTheta, theRho, anyDirection);
0093       deltaS1 = linearCrossing.pathLength(object);
0094     }
0095     if (!deltaS1.first)
0096       return deltaS1;
0097     //
0098     // Calculate total pathlength
0099     //
0100     dSTotal += deltaS1.second;
0101     PropagationDirection newDir = dSTotal >= 0 ? alongMomentum : oppositeToMomentum;
0102     if (propDir == anyDirection) {
0103       propDir = newDir;
0104     } else {
0105       if (newDir != propDir)
0106         return std::pair<bool, double>(false, 0);
0107     }
0108     //
0109     // Check convergence
0110     //
0111     if (deltaS1.second * deltaS1.second < maxDeltaS2)
0112       break;
0113     //
0114     // Step forward by dSTotal.
0115     //
0116     xnew = positionInDouble(dSTotal);
0117     pnew = directionInDouble(dSTotal);
0118   }
0119   //
0120   // Return result
0121   //
0122   return std::pair<bool, double>(true, dSTotal);
0123 }
0124
0125 //
0126 // Position on helix after a step of path length s.
0127 //
0128 HelixLineExtrapolation::PositionType IterativeHelixExtrapolatorToLine::position(double s) const {
0129   // use result in double precision
0130   return PositionType(positionInDouble(s));
0131 }
0132
0133 //
0134 // Position on helix after a step of path length s in double precision.
0135 //
0136 HelixLineExtrapolation::PositionTypeDouble IterativeHelixExtrapolatorToLine::positionInDouble(double s) const {
0137   //
0138   // Calculate delta phi (if not already available)
0139   //
0140   if (s != theCachedS) {
0141     theCachedS = s;
0142     theCachedDPhi = theCachedS * theRho * theSinTheta;
0143     theCachedSDPhi = sin(theCachedDPhi);
0144     theCachedCDPhi = cos(theCachedDPhi);
0145   }
0146   //
0147   // Calculate with appropriate formulation of full helix formula or with
0148   //   1st order approximation.
0149   //
0150   //    if ( fabs(theCachedDPhi)>1.e-1 ) {
0151   if (fabs(theCachedDPhi) > 1.e-4) {
0152     // "standard" helix formula
0153     return PositionTypeDouble(theX0 + (-theSinPhi0 * (1. - theCachedCDPhi) + theCosPhi0 * theCachedSDPhi) / theRho,
0154                               theY0 + (theCosPhi0 * (1. - theCachedCDPhi) + theSinPhi0 * theCachedSDPhi) / theRho,
0155                               theZ0 + theCachedS * theCosTheta);
0156   }
0157   //    else if ( fabs(theCachedDPhi)>theNumericalPrecision ) {
0158   //      // full helix formula, but avoiding (1-cos(deltaPhi)) for small angles
0159   //      return PositionTypeDouble(theX0+(-theSinPhi0*theCachedSDPhi*theCachedSDPhi/(1.+theCachedCDPhi)+
0160   //                     theCosPhi0*theCachedSDPhi)/theRho,
0161   //                  theY0+(theCosPhi0*theCachedSDPhi*theCachedSDPhi/(1.+theCachedCDPhi)+
0162   //                     theSinPhi0*theCachedSDPhi)/theRho,
0163   //                  theZ0+theCachedS*theCosTheta);
0164   //    }
0165   else {
0166     // Use 1st order.
0167     return theQuadraticSolutionFromStart.positionInDouble(theCachedS);
0168   }
0169 }
0170
0171 //
0172 // Direction vector on helix after a step of path length s.
0173 //
0174 HelixLineExtrapolation::DirectionType IterativeHelixExtrapolatorToLine::direction(double s) const {
0175   // use result in double precision
0176   //   DirectionTypeDouble dir = directionInDouble(s);
0177   //   return DirectionType(dir.x(),dir.y(),dir.z());
0178   return DirectionType(directionInDouble(s));
0179 }
0180
0181 //
0182 // Direction vector on helix after a step of path length s in double precision.
0183 //
0184 HelixLineExtrapolation::DirectionTypeDouble IterativeHelixExtrapolatorToLine::directionInDouble(double s) const {
0185   //
0186   // Calculate delta phi (if not already available)
0187   //
0188   if (s != theCachedS) {
0189     theCachedS = s;
0190     theCachedDPhi = theCachedS * theRho * theSinTheta;
0191     theCachedSDPhi = sin(theCachedDPhi);
0192     theCachedCDPhi = cos(theCachedDPhi);
0193   }
0194
0195   if (fabs(theCachedDPhi) > 1.e-4) {
0196     // full helix formula
0197     return DirectionTypeDouble(theCosPhi0 * theCachedCDPhi - theSinPhi0 * theCachedSDPhi,
0198                                theSinPhi0 * theCachedCDPhi + theCosPhi0 * theCachedSDPhi,
0199                                theCosTheta / theSinTheta);
0200   } else {
0201     // 1st order
0202     return theQuadraticSolutionFromStart.directionInDouble(theCachedS);
0203   }
0204 }