GeomPropagators/src/HelixForwardPlaneCrossing.cc

0001 #include "TrackingTools/GeomPropagators/interface/HelixForwardPlaneCrossing.h"
0002
0003 #include <cmath>
0004 #include <vdt/vdtMath.h>
0005
0006 HelixForwardPlaneCrossing::HelixForwardPlaneCrossing(const PositionType& point,
0007                                                      const DirectionType& direction,
0008                                                      const float curvature,
0009                                                      const PropagationDirection propDir)
0010     : theX0(point.x()),
0011       theY0(point.y()),
0012       theZ0(point.z()),
0013       theRho(curvature),
0014       thePropDir(propDir),
0015       theCachedS(0),
0016       theCachedDPhi(0.),
0017       theCachedSDPhi(0.),
0018       theCachedCDPhi(1.) {
0019   //
0020   // Components of direction vector (with correct normalisation)
0021   //
0022   double px = direction.x();
0023   double py = direction.y();
0024   double pz = direction.z();
0025   double pt2 = px * px + py * py;
0026   double p2 = pt2 + pz * pz;
0027   double pI = 1. / sqrt(p2);
0028   double ptI = 1. / sqrt(pt2);
0029   theCosPhi0 = px * ptI;
0030   theSinPhi0 = py * ptI;
0031   theCosTheta = pz * pI;
0032   theSinTheta = pt2 * ptI * pI;
0033 }
0034
0035 //
0036 // Position on helix after a step of path length s.
0037 //
0038 HelixPlaneCrossing::PositionType HelixForwardPlaneCrossing::position(double s) const {
0039   //
0040   // Calculate delta phi (if not already available)
0041   //
0042   if (s != theCachedS) {
0043     theCachedS = s;
0044     theCachedDPhi = theCachedS * theRho * theSinTheta;
0045     vdt::fast_sincos(theCachedDPhi, theCachedSDPhi, theCachedCDPhi);
0046   }
0047   //
0048   // Calculate with appropriate formulation of full helix formula or with
0049   //   2nd order approximation.
0050   //
0051   if (std::abs(theCachedDPhi) > 1.e-4) {
0052     // "standard" helix formula
0053     double o = 1. / theRho;
0054     return PositionTypeDouble(theX0 + (-theSinPhi0 * (1. - theCachedCDPhi) + theCosPhi0 * theCachedSDPhi) * o,
0055                               theY0 + (theCosPhi0 * (1. - theCachedCDPhi) + theSinPhi0 * theCachedSDPhi) * o,
0056                               theZ0 + theCachedS * theCosTheta);
0057   } else {
0058     // 2nd order
0059     double st = theCachedS * theSinTheta;
0060     return PositionType(theX0 + (theCosPhi0 - st * 0.5 * theRho * theSinPhi0) * st,
0061                         theY0 + (theSinPhi0 + st * 0.5 * theRho * theCosPhi0) * st,
0062                         theZ0 + st * theCosTheta / theSinTheta);
0063   }
0064 }
0065 //
0066 // Direction vector on helix after a step of path length s.
0067 //
0068 HelixPlaneCrossing::DirectionType HelixForwardPlaneCrossing::direction(double s) const {
0069   //
0070   // Calculate delta phi (if not already available)
0071   //
0072   if (s != theCachedS) {
0073     theCachedS = s;
0074     theCachedDPhi = theCachedS * theRho * theSinTheta;
0075     vdt::fast_sincos(theCachedDPhi, theCachedSDPhi, theCachedCDPhi);
0076   }
0077
0078   if (fabs(theCachedDPhi) > 1.e-4) {
0079     // full helix formula
0080     return DirectionType(theCosPhi0 * theCachedCDPhi - theSinPhi0 * theCachedSDPhi,
0081                          theSinPhi0 * theCachedCDPhi + theCosPhi0 * theCachedSDPhi,
0082                          theCosTheta / theSinTheta);
0083   } else {
0084     // 2nd order
0085     double dph = theCachedS * theRho * theSinTheta;
0086     return DirectionType(theCosPhi0 - (theSinPhi0 + 0.5 * theCosPhi0 * dph) * dph,
0087                          theSinPhi0 + (theCosPhi0 - 0.5 * theSinPhi0 * dph) * dph,
0088                          theCosTheta / theSinTheta);
0089   }
0090 }
0091
0092 const float HelixForwardPlaneCrossing::theNumericalPrecision = 5.e-7;