00001 #include "TrackingTools/GeomPropagators/interface/HelixArbitraryPlaneCrossing2Order.h"
00002
00003 #include "DataFormats/GeometrySurface/interface/Plane.h"
00004 #include "DataFormats/GeometryVector/interface/GlobalPoint.h"
00005
00006 #include <cmath>
00007 #include <cfloat>
00008
00009 HelixArbitraryPlaneCrossing2Order::HelixArbitraryPlaneCrossing2Order(const PositionType& point,
00010 const DirectionType& direction,
00011 const float curvature,
00012 const PropagationDirection propDir) :
00013 theX0(point.x()),
00014 theY0(point.y()),
00015 theZ0(point.z()),
00016 theRho(curvature),
00017 thePropDir(propDir)
00018 {
00019
00020
00021
00022 double px = direction.x();
00023 double py = direction.y();
00024 double pz = direction.z();
00025 double pt = px*px+py*py;
00026 double p = sqrt(pt+pz*pz);
00027 pt = sqrt(pt);
00028 theCosPhi0 = px/pt;
00029 theSinPhi0 = py/pt;
00030 theCosTheta = pz/p;
00031 theSinTheta = pt/p;
00032 }
00033
00034
00035
00036
00037 std::pair<bool,double>
00038 HelixArbitraryPlaneCrossing2Order::pathLength(const Plane& plane) {
00039
00040
00041
00042
00043 GlobalVector normalToPlane = plane.normalVector();
00044 double nPx = normalToPlane.x();
00045 double nPy = normalToPlane.y();
00046 double nPz = normalToPlane.z();
00047 double cP = plane.localZ(GlobalPoint(theX0,theY0,theZ0));
00048
00049
00050
00051
00052 double ceq1 = theRho*(nPx*theSinPhi0-nPy*theCosPhi0);
00053 double ceq2 = nPx*theCosPhi0 + nPy*theSinPhi0 + nPz*theCosTheta/theSinTheta;
00054 double ceq3 = cP;
00055
00056
00057
00058
00059 double dS1,dS2;
00060 if ( fabs(ceq1)>FLT_MIN ) {
00061 double deq1 = ceq2*ceq2;
00062 double deq2 = ceq1*ceq3;
00063 if ( fabs(deq1)<FLT_MIN || fabs(deq2/deq1)>1.e-6 ) {
00064
00065
00066
00067 double deq = deq1+2*deq2;
00068 if ( deq<0. ) return std::pair<bool,double>(false,0);
00069 double ceq = -0.5*(ceq2+(ceq2>0?1:-1)*sqrt(deq));
00070 dS1 = -2*ceq/ceq1/theSinTheta;
00071 dS2 = ceq3/ceq/theSinTheta;
00072 }
00073 else {
00074
00075
00076
00077 double ceq = ceq2/ceq1/theSinTheta;
00078 double deq = deq2/deq1;
00079 deq *= (1-deq/2);
00080 dS1 = -ceq*deq;
00081 dS2 = ceq*(2+deq);
00082 }
00083 }
00084 else {
00085
00086
00087
00088 dS1 = dS2 = -ceq3/ceq2*(1/theSinTheta);
00089 }
00090
00091
00092
00093 return solutionByDirection(dS1,dS2);
00094 }
00095
00096
00097
00098 HelixPlaneCrossing::PositionType
00099 HelixArbitraryPlaneCrossing2Order::position (double s) const {
00100
00101 PositionTypeDouble pos = positionInDouble(s);
00102 return PositionType(pos.x(),pos.y(),pos.z());
00103 }
00104
00105
00106
00107 HelixArbitraryPlaneCrossing2Order::PositionTypeDouble
00108 HelixArbitraryPlaneCrossing2Order::positionInDouble (double s) const {
00109
00110 double st = s*theSinTheta;
00111 return PositionTypeDouble(theX0+(theCosPhi0-st*0.5*theRho*theSinPhi0)*st,
00112 theY0+(theSinPhi0+st*0.5*theRho*theCosPhi0)*st,
00113 theZ0+st*theCosTheta/theSinTheta);
00114 }
00115
00116
00117
00118 HelixPlaneCrossing::DirectionType
00119 HelixArbitraryPlaneCrossing2Order::direction (double s) const {
00120
00121 DirectionTypeDouble dir = directionInDouble(s);
00122 return DirectionType(dir.x(),dir.y(),dir.z());
00123 }
00124
00125
00126
00127 HelixArbitraryPlaneCrossing2Order::DirectionTypeDouble
00128 HelixArbitraryPlaneCrossing2Order::directionInDouble (double s) const {
00129
00130 double dph = s*theRho*theSinTheta;
00131 return DirectionTypeDouble(theCosPhi0-(theSinPhi0+0.5*theCosPhi0*dph)*dph,
00132 theSinPhi0+(theCosPhi0-0.5*theSinPhi0*dph)*dph,
00133 theCosTheta/theSinTheta);
00134 }
00135
00136
00137
00138 std::pair<bool,double>
00139 HelixArbitraryPlaneCrossing2Order::solutionByDirection(const double dS1,
00140 const double dS2) const {
00141 bool valid = false;
00142 double path = 0;
00143 if ( thePropDir == anyDirection ) {
00144 valid = true;
00145 path = smallestPathLength(dS1,dS2);
00146 }
00147 else {
00148
00149 int propSign = thePropDir==alongMomentum ? 1 : -1;
00150 double s1(propSign*dS1);
00151 double s2(propSign*dS2);
00152
00153 if ( s1 > s2 ) {
00154 double aux = s1;
00155 s1 = s2;
00156 s2 = aux;
00157 }
00158
00159 if ( s1<0 && s2>=0 ) {
00160
00161 valid = true;
00162 path = propSign*s2;
00163 }
00164 else if ( s1>=0 ) {
00165
00166 valid = true;
00167 path = propSign*s1;
00168 }
00169 }
00170 return std::pair<bool,double>(valid,path);
00171 }