GeomPropagators/src/HelixArbitraryPlaneCrossing.cc

0001 #include "TrackingTools/GeomPropagators/interface/HelixArbitraryPlaneCrossing.h"
0002 #include "DataFormats/GeometrySurface/interface/Plane.h"
0003 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0004
0005 #include <cmath>
0006 #include <vdt/vdtMath.h>
0007 #include <iostream>
0008 #include "FWCore/Utilities/interface/Likely.h"
0009
0010 #ifdef VI_DEBUG
0011 #include <atomic>
0012 struct MaxIter {
0013   MaxIter() {}
0014   ~MaxIter() { std::cout << "maxiter " << v << std::endl; }
0015   void operator()(int i) const {
0016     int old = v;
0017     int t = std::min(old, i);
0018     while (not v.compare_exchange_weak(old, t)) {
0019       t = std::min(old, i);
0020     }
0021   }
0022   mutable std::atomic<int> v{100};
0023 };
0024 #else
0025 struct MaxIter {
0026   MaxIter() {}
0027   void operator()(int) const {}
0028 };
0029 #endif
0030 static const MaxIter maxiter;
0031
0032 HelixArbitraryPlaneCrossing::HelixArbitraryPlaneCrossing(const PositionType& point,
0033                                                          const DirectionType& direction,
0034                                                          const float curvature,
0035                                                          const PropagationDirection propDir)
0036     : theQuadraticCrossingFromStart(point, direction, curvature, propDir),
0037       theX0(point.x()),
0038       theY0(point.y()),
0039       theZ0(point.z()),
0040       theRho(curvature),
0041       thePropDir(propDir),
0042       theCachedS(0),
0043       theCachedDPhi(0.),
0044       theCachedSDPhi(0.),
0045       theCachedCDPhi(1.) {
0046   //
0047   // Components of direction vector (with correct normalisation)
0048   //
0049   double px = direction.x();
0050   double py = direction.y();
0051   double pz = direction.z();
0052   double pt2 = px * px + py * py;
0053   double p2 = pt2 + pz * pz;
0054   double pI = 1. / sqrt(p2);
0055   double ptI = 1. / sqrt(pt2);
0056   theCosPhi0 = px * ptI;
0057   theSinPhi0 = py * ptI;
0058   theCosTheta = pz * pI;
0059   theSinTheta = pt2 * ptI * pI;
0060 }
0061 //
0062 // Propagation status and path length to intersection
0063 //
0064 std::pair<bool, double> HelixArbitraryPlaneCrossing::pathLength(const Plane& plane) {
0065   //
0066   // Constants used for control of convergence
0067   //
0068   constexpr int maxIterations = 20;
0069   //
0070   // maximum distance to plane (taking into account numerical precision)
0071   //
0072   float maxNumDz = theNumericalPrecision * plane.position().mag();
0073   float safeMaxDist = (theMaxDistToPlane > maxNumDz ? theMaxDistToPlane : maxNumDz);
0074   //
0075   // Prepare internal value of the propagation direction and position / direction vectors for iteration
0076   //
0077
0078   float dz = plane.localZ(Surface::GlobalPoint(theX0, theY0, theZ0));
0079   if (std::abs(dz) < safeMaxDist)
0080     return std::make_pair(true, 0.);
0081
0082   bool notFail;
0083   double dSTotal;
0084   // Use existing 2nd order object at first pass
0085   std::tie(notFail, dSTotal) = theQuadraticCrossingFromStart.pathLength(plane);
0086   if UNLIKELY (!notFail)
0087     return std::make_pair(notFail, dSTotal);
0088   auto xnew = positionInDouble(dSTotal);
0089
0090   auto propDir = thePropDir;
0091   auto newDir = dSTotal >= 0 ? alongMomentum : oppositeToMomentum;
0092   if (propDir == anyDirection) {
0093     propDir = newDir;
0094   } else {
0095     if UNLIKELY (newDir != propDir)
0096       return std::pair<bool, double>(false, 0);
0097   }
0098
0099   //
0100   // Prepare iterations: count and total pathlength
0101   //
0102   auto iteration = maxIterations;
0103   while (notAtSurface(plane, xnew, safeMaxDist)) {
0104     //
0105     // return empty solution vector if no convergence after maxIterations iterations
0106     //
0107     if UNLIKELY (--iteration == 0) {
0108       LogDebug("HelixArbitraryPlaneCrossing") << "pathLength : no convergence";
0109       return std::pair<bool, double>(false, 0);
0110     }
0111
0112     //
0113     // create temporary object for subsequent passes.
0114     auto pnew = directionInDouble(dSTotal);
0115     HelixArbitraryPlaneCrossing2Order quadraticCrossing(
0116         xnew.x(), xnew.y(), xnew.z(), pnew.x(), pnew.y(), theCosTheta, theSinTheta, theRho, anyDirection);
0117
0118     auto deltaS2 = quadraticCrossing.pathLength(plane);
0119
0120     if UNLIKELY (!deltaS2.first)
0121       return deltaS2;
0122     //
0123     // Calculate and sort total pathlength (max. 2 solutions)
0124     //
0125     dSTotal += deltaS2.second;
0126     auto newDir = dSTotal >= 0 ? alongMomentum : oppositeToMomentum;
0127     if (propDir == anyDirection) {
0128       propDir = newDir;
0129     } else {
0130       if UNLIKELY (newDir != propDir)
0131         return std::pair<bool, double>(false, 0);
0132     }
0133     //
0134     // Step forward by dSTotal.
0135     //
0136     xnew = positionInDouble(dSTotal);
0137   }
0138   //
0139   // Return result
0140   //
0141   maxiter(iteration);
0142
0143   return std::make_pair(true, dSTotal);
0144 }
0145 //
0146 // Position on helix after a step of path length s.
0147 //
0148 HelixPlaneCrossing::PositionType HelixArbitraryPlaneCrossing::position(double s) const {
0149   // use result in double precision
0150   PositionTypeDouble pos = positionInDouble(s);
0151   return PositionType(pos.x(), pos.y(), pos.z());
0152 }
0153 //
0154 // Position on helix after a step of path length s in double precision.
0155 //
0156 HelixArbitraryPlaneCrossing::PositionTypeDouble HelixArbitraryPlaneCrossing::positionInDouble(double s) const {
0157   //
0158   // Calculate delta phi (if not already available)
0159   //
0160   if UNLIKELY (s != theCachedS) {
0161     theCachedS = s;
0162     theCachedDPhi = theCachedS * theRho * theSinTheta;
0163     vdt::fast_sincos(theCachedDPhi, theCachedSDPhi, theCachedCDPhi);
0164   }
0165   //
0166   // Calculate with appropriate formulation of full helix formula or with
0167   //   2nd order approximation.
0168   //
0169   //    if ( fabs(theCachedDPhi)>1.e-1 ) {
0170   if (std::abs(theCachedDPhi) > 1.e-4) {
0171     // "standard" helix formula
0172     double o = 1. / theRho;
0173     return PositionTypeDouble(theX0 + (-theSinPhi0 * (1. - theCachedCDPhi) + theCosPhi0 * theCachedSDPhi) * o,
0174                               theY0 + (theCosPhi0 * (1. - theCachedCDPhi) + theSinPhi0 * theCachedSDPhi) * o,
0175                               theZ0 + theCachedS * theCosTheta);
0176   }
0177   //    else if ( fabs(theCachedDPhi)>theNumericalPrecision ) {
0178   //      // full helix formula, but avoiding (1-cos(deltaPhi)) for small angles
0179   //      return PositionTypeDouble(theX0+(-theSinPhi0*theCachedSDPhi*theCachedSDPhi/(1.+theCachedCDPhi)+
0180   //                     theCosPhi0*theCachedSDPhi)/theRho,
0181   //                  theY0+(theCosPhi0*theCachedSDPhi*theCachedSDPhi/(1.+theCachedCDPhi)+
0182   //                     theSinPhi0*theCachedSDPhi)/theRho,
0183   //                  theZ0+theCachedS*theCosTheta);
0184   //    }
0185   else {
0186     // Use 2nd order.
0187     return theQuadraticCrossingFromStart.positionInDouble(theCachedS);
0188   }
0189 }
0190 //
0191 // Direction vector on helix after a step of path length s.
0192 //
0193 HelixPlaneCrossing::DirectionType HelixArbitraryPlaneCrossing::direction(double s) const {
0194   // use result in double precision
0195   DirectionTypeDouble dir = directionInDouble(s);
0196   return DirectionType(dir.x(), dir.y(), dir.z());
0197 }
0198 //
0199 // Direction vector on helix after a step of path length s in double precision.
0200 //
0201 HelixArbitraryPlaneCrossing::DirectionTypeDouble HelixArbitraryPlaneCrossing::directionInDouble(double s) const {
0202   //
0203   // Calculate delta phi (if not already available)
0204   //
0205   if UNLIKELY (s != theCachedS) {  // very very unlikely!
0206     theCachedS = s;
0207     theCachedDPhi = theCachedS * theRho * theSinTheta;
0208     vdt::fast_sincos(theCachedDPhi, theCachedSDPhi, theCachedCDPhi);
0209   }
0210
0211   if (std::abs(theCachedDPhi) > 1.e-4) {
0212     // full helix formula
0213     return DirectionTypeDouble(theCosPhi0 * theCachedCDPhi - theSinPhi0 * theCachedSDPhi,
0214                                theSinPhi0 * theCachedCDPhi + theCosPhi0 * theCachedSDPhi,
0215                                theCosTheta / theSinTheta);
0216   } else {
0217     // 2nd order
0218     return theQuadraticCrossingFromStart.directionInDouble(theCachedS);
0219   }
0220 }
0221 //   Iteration control: continue if distance to plane > theMaxDistToPlane. Includes
0222 //   protection for numerical precision (Surfaces work with single precision).
0223 bool HelixArbitraryPlaneCrossing::notAtSurface(const Plane& plane,
0224                                                const PositionTypeDouble& point,
0225                                                const float maxDist) const {
0226   float dz = plane.localZ(Surface::GlobalPoint(point.x(), point.y(), point.z()));
0227   return std::abs(dz) > maxDist;
0228 }
0229
0230 const float HelixArbitraryPlaneCrossing::theNumericalPrecision = 5.e-7f;
0231 const float HelixArbitraryPlaneCrossing::theMaxDistToPlane = 1.e-4f;