GeomPropagators/src/HelixBarrelPlaneCrossingByCircle.cc

0001 #include "TrackingTools/GeomPropagators/interface/HelixBarrelPlaneCrossingByCircle.h"
0002 #include "TrackingTools/GeomPropagators/src/RealQuadEquation.h"
0003 #include "TrackingTools/GeomPropagators/interface/StraightLinePlaneCrossing.h"
0004 #include "DataFormats/GeometrySurface/interface/Plane.h"
0005 #include "DataFormats/Math/interface/SIMDVec.h"
0006
0007 #include <algorithm>
0008 #include <cfloat>
0009
0010 HelixBarrelPlaneCrossingByCircle::HelixBarrelPlaneCrossingByCircle(const PositionType& pos,
0011                                                                    const DirectionType& dir,
0012                                                                    double rho,
0013                                                                    PropagationDirection propDir)
0014     : theStartingPos(pos), theStartingDir(dir), theRho(rho), thePropDir(propDir) {
0015   init();
0016 }
0017
0018 HelixBarrelPlaneCrossingByCircle::HelixBarrelPlaneCrossingByCircle(const GlobalPoint& pos,
0019                                                                    const GlobalVector& dir,
0020                                                                    double rho,
0021                                                                    PropagationDirection propDir)
0022     : theStartingPos(pos.basicVector()), theStartingDir(dir.basicVector()), theRho(rho), thePropDir(propDir) {
0023   init();
0024 }
0025
0026 void HelixBarrelPlaneCrossingByCircle::init() {
0027   double pabsI = 1. / theStartingDir.mag();
0028   double pt = theStartingDir.perp();
0029   theCosTheta = theStartingDir.z() * pabsI;
0030   theSinTheta = pt * pabsI;
0031
0032   // protect for zero curvature case
0033   const double sraightLineCutoff = 1.e-7;
0034   if (fabs(theRho) < sraightLineCutoff && fabs(theRho) * theStartingPos.perp() < sraightLineCutoff) {
0035     useStraightLine = true;
0036   } else {
0037     // circle parameters
0038     // position of center of curvature is on a line perpendicular
0039     // to startingDir and at a distance 1/curvature.
0040     double o = 1. / (pt * theRho);
0041     theXCenter = theStartingPos.x() - theStartingDir.y() * o;
0042     theYCenter = theStartingPos.y() + theStartingDir.x() * o;
0043     useStraightLine = false;
0044   }
0045 }
0046
0047 std::pair<bool, double> HelixBarrelPlaneCrossingByCircle::pathLength(const Plane& plane) {
0048   typedef std::pair<bool, double> ResultType;
0049
0050   if (useStraightLine) {
0051     // switch to straight line case
0052     StraightLinePlaneCrossing slc(theStartingPos, theStartingDir, thePropDir);
0053     return slc.pathLength(plane);
0054   }
0055
0056   // plane parameters
0057   GlobalVector n = plane.normalVector();
0058   double distToPlane = -plane.localZ(GlobalPoint(theStartingPos));
0059   //    double distToPlane = (plane.position().x()-theStartingPos.x()) * n.x() +
0060   //                         (plane.position().y()-theStartingPos.y()) * n.y();
0061   double nx = n.x();  // convert to double
0062   double ny = n.y();  // convert to double
0063   double distCx = theStartingPos.x() - theXCenter;
0064   double distCy = theStartingPos.y() - theYCenter;
0065
0066   double nfac, dfac;
0067   double A, B, C;
0068   bool solveForX;
0069   if (fabs(nx) > fabs(ny)) {
0070     solveForX = false;
0071     nfac = ny / nx;
0072     dfac = distToPlane / nx;
0073     B = distCy - nfac * distCx;  // only part of B, may have large cancelation
0074     C = (2. * distCx + dfac) * dfac;
0075   } else {
0076     solveForX = true;
0077     nfac = nx / ny;
0078     dfac = distToPlane / ny;
0079     B = distCx - nfac * distCy;  // only part of B, may have large cancelation
0080     C = (2. * distCy + dfac) * dfac;
0081   }
0082   B -= nfac * dfac;
0083   B *= 2;  // the rest of B (normally small)
0084   A = 1. + nfac * nfac;
0085
0086   double dx1, dx2, dy1, dy2;
0087   RealQuadEquation equation(A, B, C);
0088   if (!equation.hasSolution)
0089     return ResultType(false, theS = 0.);
0090   else {
0091     if (solveForX) {
0092       dx1 = equation.first;
0093       dx2 = equation.second;
0094       dy1 = dfac - nfac * dx1;
0095       dy2 = dfac - nfac * dx2;
0096     } else {
0097       dy1 = equation.first;
0098       dy2 = equation.second;
0099       dx1 = dfac - nfac * dy1;
0100       dx2 = dfac - nfac * dy2;
0101     }
0102   }
0103   bool solved = chooseSolution(Vector2D(dx1, dy1), Vector2D(dx2, dy2));
0104   if (solved) {
0105     theDmag = theD.mag();
0106     // protect asin
0107     double sinAlpha = 0.5 * theDmag * theRho;
0108     if (std::abs(sinAlpha) > 1.)
0109       sinAlpha = std::copysign(1., sinAlpha);
0110     theS = theActualDir * 2. / (theRho * theSinTheta) * asin(sinAlpha);
0111     return ResultType(true, theS);
0112   } else
0113     return ResultType(false, theS = 0.);
0114 }
0115
0116 bool HelixBarrelPlaneCrossingByCircle::chooseSolution(const Vector2D& d1, const Vector2D& d2) {
0117   bool solved;
0118   double momProj1 = theStartingDir.x() * d1.x() + theStartingDir.y() * d1.y();
0119   double momProj2 = theStartingDir.x() * d2.x() + theStartingDir.y() * d2.y();
0120
0121   if (thePropDir == anyDirection) {
0122     solved = true;
0123     if (d1.mag2() < d2.mag2()) {
0124       theD = d1;
0125       theActualDir = (momProj1 > 0) ? 1. : -1.;
0126     } else {
0127       theD = d2;
0128       theActualDir = (momProj2 > 0) ? 1. : -1.;
0129     }
0130
0131   } else {
0132     double propSign = thePropDir == alongMomentum ? 1 : -1;
0133     if (!mathSSE::samesign(momProj1, momProj2)) {
0134       // if different signs return the positive one
0135       solved = true;
0136       theD = mathSSE::samesign(momProj1, propSign) ? d1 : d2;
0137       theActualDir = propSign;
0138     } else if (mathSSE::samesign(momProj1, propSign)) {
0139       // if both positive, return the shortest
0140       solved = true;
0141       theD = (d1.mag2() < d2.mag2()) ? d1 : d2;
0142       theActualDir = propSign;
0143     } else
0144       solved = false;
0145   }
0146   return solved;
0147 }
0148
0149 HelixPlaneCrossing::PositionType HelixBarrelPlaneCrossingByCircle::position(double s) const {
0150   if (useStraightLine) {
0151     return PositionType(theStartingPos + s * theStartingDir.unit());
0152   } else {
0153     if (s == theS) {
0154       return PositionType(
0155           theStartingPos.x() + theD.x(), theStartingPos.y() + theD.y(), theStartingPos.z() + s * theCosTheta);
0156     } else {
0157       double phi = s * theSinTheta * theRho;
0158       double x1Shift = theStartingPos.x() - theXCenter;
0159       double y1Shift = theStartingPos.y() - theYCenter;
0160
0161       return PositionType(x1Shift * cos(phi) - y1Shift * sin(phi) + theXCenter,
0162                           x1Shift * sin(phi) + y1Shift * cos(phi) + theYCenter,
0163                           theStartingPos.z() + s * theCosTheta);
0164     }
0165   }
0166 }
0167
0168 HelixPlaneCrossing::DirectionType HelixBarrelPlaneCrossingByCircle::direction(double s) const {
0169   if (useStraightLine) {
0170     return theStartingDir;
0171   } else {
0172     double sinPhi, cosPhi;
0173     if (s == theS) {
0174       double tmp = 0.5 * theDmag * theRho;
0175       if (s < 0)
0176         tmp = -tmp;
0177       // protect sqrt
0178       sinPhi = 1. - (tmp * tmp);
0179       if (sinPhi < 0)
0180         sinPhi = 0.;
0181       sinPhi = 2. * tmp * sqrt(sinPhi);
0182       cosPhi = 1. - 2. * (tmp * tmp);
0183     } else {
0184       double phi = s * theSinTheta * theRho;
0185       sinPhi = sin(phi);
0186       cosPhi = cos(phi);
0187     }
0188     return DirectionType(theStartingDir.x() * cosPhi - theStartingDir.y() * sinPhi,
0189                          theStartingDir.x() * sinPhi + theStartingDir.y() * cosPhi,
0190                          theStartingDir.z());
0191   }
0192 }