1 #include "../inc/trackstate.h"
3 #include "../util/inc/helixrep.h"
4 #include "../util/inc/vector3.h"
5 #include "../util/inc/matrix.h"
6 #include "../inc/track.h"
9 #define min(a,b) (((a)<(b))?(a):(b))
10 #define max(a,b) (((a)>(b))?(a):(b))
15 const double TrackState::_swimprecision = 0.0001;
17 TrackState::TrackState(
Track* TTrack)
27 std::cerr <<
"Warning: Trackstate.cpp:28 Null Track Pointer" << std::endl;
31 : _Charge(cha),_Init(init),_InitCovarMatrix(cov),_PosValid(0),_MomValid(0),_PositionCovarValid(0),_InversePositionCovarValid(0),_ParentTrack(tra)
42 _InversePositionCovarValid = 0;
43 _PositionCovarValid = 0;
58 double u = (((P3.x()-P1.x())*(P2.x()-P1.x())) + ((P3.y()-P1.y())*(P2.y()-P1.y())) + ((P3.z()-P1.z())*(P2.z()-P1.z()))) / (P2.subtract(P1)).mag2();
67 if (this->
distanceTo(Point)<_swimprecision)
return;
73 double s0 = _DistanceSwum;
78 double invrval = -_Init.invR();
79 double c = (-2*(-(invrval*_Init.tanLambda()*(_Init.z0() - P.z() + s0*_Init.tanLambda())) + invrval*P.x()*cos(_Init.phi() + invrval*s0) + (-1.0 + invrval*-_Init.d0())*sin(invrval*s0) + invrval*P.y()*sin(_Init.phi() + invrval*s0)))/invrval;
80 double b = 2*(pow(_Init.tanLambda(),2) + (1.0 - invrval*-_Init.d0())*cos(invrval*s0) - invrval*P.y()*cos(_Init.phi() + invrval*s0) + invrval*P.x()*sin(_Init.phi() + invrval*s0));
81 double a = invrval*(invrval*P.x()*cos(_Init.phi() + invrval*s0) + (-1.0 + invrval*-_Init.d0())*sin(invrval*s0) + invrval*P.y()*sin(_Init.phi() + invrval*s0));
86 if (fabs(a)>std::numeric_limits<double>::epsilon())
88 double root = sqrt((b*b)-(4*a*c));
89 s1 = (-b + root)/(2*a);
90 s2 = (-b - root)/(2*a);
98 double s1ToPoint,s2ToPoint;
101 if (_finite(s1) && fabs(s1) < 10000)
103 if (std::isfinite(s1) && fabs(s1) < 10000)
112 #ifdef WIN32 //this template version could probably be used for both, but need to check first
113 s1ToPoint = std::numeric_limits<double>::infinity();
115 s1ToPoint = INFINITY;
120 if (_finite(s2) && fabs(s2) < 10000)
122 if (std::isfinite(s2) && fabs(s2) < 10000)
131 #ifdef WIN32 //this template version could probably be used for both, but need to check first
132 s2ToPoint = std::numeric_limits<double>::infinity();
134 s2ToPoint = INFINITY;
138 if (std::isfinite(s1ToPoint) && std::isfinite(s2ToPoint))
140 if (s1ToPoint < s2ToPoint)
147 if (std::isfinite(s1ToPoint))
151 if (std::isfinite(s2ToPoint))
160 done = (this->
distanceTo(initPos) < _swimprecision);
163 }
while (!done && loopcount < 100);
164 if (loopcount == 100)
166 std::cerr <<
"Warning: TrackState.cpp:167:swimToStateNearest Max Iterations reached" << std::endl;
167 std::cerr <<
"Point " << Point <<
" Track:" << *
this << std::endl;
190 double d1343,d4321,d1321,d4343,d2121;
193 p13.x() = p1.x() - p3.x();
194 p13.y() = p1.y() - p3.y();
195 p13.z() = p1.z() - p3.z();
196 p43.x() = p4.x() - p3.x();
197 p43.y() = p4.y() - p3.y();
198 p43.z() = p4.z() - p3.z();
200 if (fabs(p43.x()) < std::numeric_limits<double>::epsilon() && fabs(p43.y()) < std::numeric_limits<double>::epsilon() && fabs(p43.z()) < std::numeric_limits<double>::epsilon())
202 p21.x() = p2.x() - p1.x();
203 p21.y() = p2.y() - p1.y();
204 p21.z() = p2.z() - p1.z();
205 if (fabs(p21.x()) < std::numeric_limits<double>::epsilon() && fabs(p21.y()) < std::numeric_limits<double>::epsilon() && fabs(p21.z()) < std::numeric_limits<double>::epsilon())
208 d1343 = p13.x() * p43.x() + p13.y() * p43.y() + p13.z() * p43.z();
209 d4321 = p43.x() * p21.x() + p43.y() * p21.y() + p43.z() * p21.z();
210 d1321 = p13.x() * p21.x() + p13.y() * p21.y() + p13.z() * p21.z();
211 d4343 = p43.x() * p43.x() + p43.y() * p43.y() + p43.z() * p43.z();
212 d2121 = p21.x() * p21.x() + p21.y() * p21.y() + p21.z() * p21.z();
214 denom = d2121 * d4343 - d4321 * d4321;
215 if (fabs(denom) < std::numeric_limits<double>::epsilon())
217 numer = d1343 * d4321 - d1321 * d4343;
230 double currentDelta=10.0;
232 long int iterations=0;
241 double delta = (fabs(h2track)+1.0)*0.00001;
247 changeRate = (f2track-h2track)/delta;
248 changeRate = changeRate/fabs(changeRate);
249 double currentStep = -changeRate*currentDelta;
264 }
while (0!=changeRate && iterations < maxIters) ;
265 if (iterations == maxIters)
267 std::cerr <<
"Warning TrackState.cpp:278:_swimToStateNearest - Too many iterations" <<std::endl;
269 std::cerr << *
this << std::endl;
271 std::cerr << *TrackToSwimTo << std::endl;
276 }
while( currentDelta>_swimprecision );
293 double u = (((P3.x()-P1.x())*(P2.x()-P1.x())) + ((P3.y()-P1.y())*(P2.y()-P1.y()))) / (((P2.x()-P1.x())*(P2.x()-P1.x()))+((P2.y()-P1.y())*(P2.y()-P1.y())));
315 double distanceRoundCircle = atan(P2.x()/P2.y())/_Init.invR();
318 distanceRoundCircle = distanceRoundCircle + (_Init.phi()/_Init.invR());
330 if (dist1 < dist2) this->
swimDistance(-_Init.halfCircum());
335 this->
swimDistance(-_Init.circum() * round(this->
position().subtract(Point).z() / _Init.zLength()));
344 boost::numeric::ublas::bounded_vector<double,2> Residual;
355 if (fabs(this->
distanceTo(Point)-Residual(0)) < _swimprecision)
363 std::cerr <<
"Warning trackstate.cpp:364:chi2 2D Distance to track was longer than 3D - swimming problem?" << std::endl;
369 Residual(1) = sqrt(this->
distanceTo2(Point)-pow(Residual(0),2))*sqrt(pow(_Init.tanLambda(),2)+1.0);
374 if (std::isnan(chi2))
376 std::cerr <<
"Warning trackstate.cpp:377 Chi2 is NAN, are your cov matrices ok?" << std::endl;
377 std::cerr <<
"3D: " << this->
distanceTo(Point) << std::endl ;
378 std::cerr <<
"2D: " << Residual(0) << std::endl ;
379 std::cerr <<
"Res: " << Residual << std::endl ;
380 std::cerr <<
"Q: " << this->
isCharged() << std::endl ;
382 std::cerr <<
"Chi2: " << prec_inner_prod(trans(Residual),prec_prod(this->
inversePositionCovarMatrix(), Residual))<< std::endl<< std::endl;
390 boost::numeric::ublas::bounded_vector<double,2> Residual;
403 Residual(1) = sqrt(this->
distanceTo2(Point)-pow(Residual(0),2))*sqrt(pow(_Init.tanLambda(),2)+1.0);
446 double phival = _Init.phi();
447 _Position.x() = -_Init.d0()*sin(phival) + (sin(phival)-sin(phival-_Init.invR()*_DistanceSwum))/_Init.invR();
449 _Position.y() = _Init.d0()*cos(phival) + (-cos(phival)+cos(phival-_Init.invR()*_DistanceSwum))/_Init.invR();
453 _Position.z() = (_DistanceSwum*_Init.tanLambda()) + _Init.z0();
462 _Position.x() = (_Init.d0()*sin(_Init.phi())) + (_DistanceSwum*cos(_Init.phi()));
463 _Position.y() = (-_Init.d0()*cos(_Init.phi())) + (_DistanceSwum*sin(_Init.phi()));
464 _Position.z() = (_DistanceSwum*_Init.tanLambda()) + _Init.z0();
483 double STHE = 1.0/sqrt(1.0+pow(_Init.tanLambda(),2));
484 double CTHE = STHE*_Init.tanLambda();
504 double DXDD = -sin(_Init.phi());
505 double DXDZ = -cos(_Init.phi())*STHE*CTHE;
506 double DYDD = cos(_Init.phi());
507 double DYDZ = -sin(_Init.phi())*STHE*CTHE;
508 double DZDZ = pow(STHE,2);
530 ROTAT(0,0)= CTHE*cos(_Init.phi());
531 ROTAT(0,1)= CTHE*sin(_Init.phi());
533 ROTAT(1,0)= -sin(_Init.phi());
534 ROTAT(1,1)= cos(_Init.phi());
536 ROTAT(2,0)= STHE*cos(_Init.phi());
537 ROTAT(2,1)= STHE*sin(_Init.phi());
546 for (
short i =0 ;i<6;++i)
548 for(
short IIP = 1;IIP<3;++IIP)
551 for(
short JJP = 1;JJP<IIP+1;++JJP)
554 short JLD = IIP*(IIP-1)/2 + JJP;
555 for(
short KKP = 1;KKP<4;++KKP)
558 for(
short LLP = 1;LLP<4;++LLP)
561 short MINLD=min(KKP,LLP);
562 short MAXLD=max(KKP,LLP);
563 short ILD = MAXLD*(MAXLD-1)/2 + MINLD;
564 EUV[JLD-1]=EUV[JLD-1]+ROTAT(IIP-1,KKP-1)*EXYZ[ILD-1]*ROTAT(JJP-1,LLP-1);
569 double DETEUV = 1.0/(EUV[0]*EUV[2]-EUV[1]*EUV[1]);
598 GG(0,0)=GG(0,0)+DETEUV*(ROTAT(0,0)*ROTAT(0,0)*EUV[2]-1.0*ROTAT(0,0)*ROTAT(1,0)*EUV[1]+ROTAT(1,0)*ROTAT(1,0)*EUV[0]);
599 GG(1,0)=GG(1,0)+DETEUV*(ROTAT(0,0)*ROTAT(0,1)*EUV[2]-ROTAT(0,0)*ROTAT(1,1)*EUV[1]-ROTAT(1,0)*ROTAT(0,1)*EUV[1]+ROTAT(1,0)*ROTAT(1,1)*EUV[0]);
600 GG(1,1)=GG(1,1)+DETEUV*(ROTAT(0,1)*ROTAT(0,1)*EUV[2]-1.0*ROTAT(0,1)*ROTAT(1,1)*EUV[1]+ROTAT(1,1)*ROTAT(1,1)*EUV[0]);
601 GG(2,0)=GG(2,0)+DETEUV*(ROTAT(0,0)*ROTAT(0,2)*EUV[2]-ROTAT(0,0)*ROTAT(1,2)*EUV[1]-ROTAT(1,0)*ROTAT(0,2)*EUV[1]+ROTAT(1,0)*ROTAT(1,2)*EUV[0]);
602 GG(2,1)=GG(2,1)+DETEUV*(ROTAT(0,1)*ROTAT(0,2)*EUV[2]-ROTAT(0,1)*ROTAT(1,2)*EUV[1]-ROTAT(1,1)*ROTAT(0,2)*EUV[1]+ROTAT(1,1)*ROTAT(1,2)*EUV[0]);
603 GG(2,2)=GG(2,2)+DETEUV*(ROTAT(0,2)*ROTAT(0,2)*EUV[2]-1.0*ROTAT(0,2)*ROTAT(1,2)*EUV[1]+ROTAT(1,2)*ROTAT(1,2)*EUV[0]);
609 if(!_PositionCovarValid)
628 _PositionCovarMatrix(0,0) = _InitCovarMatrix(0,0);
629 _PositionCovarMatrix(0,1) = _InitCovarMatrix(0,3);
630 _PositionCovarMatrix(1,1) = _InitCovarMatrix(3,3);
631 _PositionCovarValid = 1;
633 return _PositionCovarMatrix;
638 if(!_InversePositionCovarValid)
642 double factor = pow(_PositionCovarMatrix(0,1),2) - _PositionCovarMatrix(0,0)*_PositionCovarMatrix(1,1);
643 _InversePositionCovarMatrix(0,0) = _PositionCovarMatrix(1,1)/-factor;
644 _InversePositionCovarMatrix(0,1) = _PositionCovarMatrix(0,1)/factor;
645 _InversePositionCovarMatrix(1,1) = _PositionCovarMatrix(0,0)/-factor;
646 _InversePositionCovarValid = 1;
648 return _InversePositionCovarMatrix;
693 std::cout <<
"Pos:" << this->
position().x() <<
"," << this->
position().y() <<
"," << this->
position().z() << std::endl;
695 std::cout <<
"Cha:" << this->
charge() << std::endl;
696 std::cout <<
"Par:" << this->
parentTrack() << std::endl;
697 std::cout <<
"Sws:" << _DistanceSwum << std::endl;
705 _Charge=_ParentTrack->
charge();
710 _InversePositionCovarValid = 0;
711 _PositionCovarValid = 0;
double charge() const
Charge.
void resetToRef()
Reset the track to distance swum = 0.
Track * parentTrack() const
The Track that this TrackState belongs to, (if any)
const SymMatrix2x2 & positionCovarMatrix() const
Current position covariance matrix of the trackstate (d0,z0)
const SymMatrix5x5 & covarianceMatrix() const
Covariance Matrix.
void swimToStateNearestXY(const Vector3 &Point)
Swim to the point of closest approach in the XY plane to Point.
double distanceTo(const Vector3 &Point) const
Distance from the TrackStates current position to Point.
const SymMatrix2x2 & inversePositionCovarMatrix() const
Current inverse position covariance matrix of the trackstate (d0,z0)
double xyDistanceTo(const Vector3 &Point) const
Distance in the XY plane from the TrackStates current position to Point.
Spatial point on a Track.
double chi2_nomove(const Vector3 &Point)
Calculate this tracks chi squared to Point at the TrackStates current position.
double distanceTo2(const Vector3 &Point) const
Distance squared from the TrackStates current position to Point.
bool isNeutral() const
Is this track neutral.
double chi2(const Vector3 &Point)
Calculate this tracks minimum chi squared to Point.
Unique Track representation.
void swimDistance(const double s)
Swim the track a fixed distance.
const HelixRep & helixRep() const
Helix represenation of this track.
void debugOut()
Print some info to std::cout.
double charge() const
Track charge.
bool isCharged() const
Is this track charged.
const Matrix3x3 vertexErrorContribution(Vector3 point) const
The error contribution of this trackstate to a vertex at point.
const Vector3 & position() const
Current position of the trackstate.
void swimToStateNearest(const Vector3 &Point)
Swim to the point of closest approach to Point.