File indexing completed on 2023-03-17 11:26:22
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
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;
0030 }
0031
0032
0033
0034
0035 std::pair<bool, double> HelixArbitraryPlaneCrossing2Order::pathLength(const Plane& plane) {
0036
0037
0038
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
0047
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
0054
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
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
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
0083
0084 dS1 = dS2 = -(ceq3 / ceq2) * theSinThetaI;
0085 }
0086
0087
0088
0089 return solutionByDirection(dS1, dS2);
0090 }
0091
0092
0093
0094
0095 HelixPlaneCrossing::PositionType HelixArbitraryPlaneCrossing2Order::position(double s) const {
0096
0097 PositionTypeDouble pos = positionInDouble(s);
0098 return PositionType(pos.x(), pos.y(), pos.z());
0099 }
0100
0101
0102
0103 HelixArbitraryPlaneCrossing2Order::PositionTypeDouble HelixArbitraryPlaneCrossing2Order::positionInDouble(
0104 double s) const {
0105
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
0113
0114 HelixPlaneCrossing::DirectionType HelixArbitraryPlaneCrossing2Order::direction(double s) const {
0115
0116 DirectionTypeDouble dir = directionInDouble(s);
0117 return DirectionType(dir.x(), dir.y(), dir.z());
0118 }
0119
0120
0121
0122 HelixArbitraryPlaneCrossing2Order::DirectionTypeDouble HelixArbitraryPlaneCrossing2Order::directionInDouble(
0123 double s) const {
0124
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
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
0142 double propSign = thePropDir == alongMomentum ? 1 : -1;
0143 double s1(propSign * dS1);
0144 double s2(propSign * dS2);
0145
0146 if (s1 > s2)
0147 std::swap(s1, s2);
0148
0149 if ((s1 < 0) & (s2 >= 0)) {
0150
0151 valid = true;
0152 path = propSign * s2;
0153 } else if (s1 >= 0) {
0154
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 }