30 #include <TDecompLU.h>
35 #define MINSTEP 0.001 // minimum step [cm] for Runge Kutta and iteration to POCA
39 const bool useInvertFast =
false;
47 lastStartState_(this),
62 lastStartState_(this),
83 bool calcJacobianNoise)
const {
86 debugOut <<
"RKTrackRep::extrapolateToPlane()\n";
92 debugOut <<
"state is already defined at plane. Do nothing! \n";
100 M1x7 state7 = {{0, 0, 0, 0, 0, 0, 0}};
103 TMatrixDSym* covPtr(NULL);
104 bool fillExtrapSteps(
false);
105 if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
107 fillExtrapSteps =
true;
109 else if (calcJacobianNoise)
110 fillExtrapSteps =
true;
113 bool isAtBoundary(
false);
114 double flightTime( 0. );
115 double coveredDistance(
Extrap(*(state.
getPlane()), *plane,
getCharge(state),
getMass(state), isAtBoundary, state7, flightTime, fillExtrapSteps, covPtr,
false, stopAtBoundary) );
117 if (stopAtBoundary && isAtBoundary) {
119 TVector3(state7[3], state7[4], state7[5]))));
131 return coveredDistance;
136 const TVector3& linePoint,
137 const TVector3& lineDirection,
139 bool calcJacobianNoise)
const {
142 debugOut <<
"RKTrackRep::extrapolateToLine()\n";
147 static const unsigned int maxIt(1000);
153 bool fillExtrapSteps(
false);
154 if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
155 fillExtrapSteps =
true;
157 else if (calcJacobianNoise)
158 fillExtrapSteps =
true;
160 double step(0.), lastStep(0.), maxStep(1.E99), angle(0), distToPoca(0), tracklength(0);
163 double flightTime = 0;
164 TVector3 dir(state7[3], state7[4], state7[5]);
165 TVector3 lastDir(0,0,0);
166 TVector3 poca, poca_onwire;
167 bool isAtBoundary(
false);
171 unsigned int iterations(0);
174 if(++iterations == maxIt) {
175 Exception exc(
"RKTrackRep::extrapolateToLine ==> extrapolation to line failed, maximum number of iterations reached",__LINE__,__FILE__);
183 step = this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false, NULL,
true, stopAtBoundary, maxStep);
186 dir.SetXYZ(state7[3], state7[4], state7[5]);
187 poca.SetXYZ(state7[0], state7[1], state7[2]);
188 poca_onwire =
pocaOnLine(linePoint, lineDirection, poca);
191 if (stopAtBoundary && isAtBoundary) {
192 plane->setON(dir, poca);
196 angle = fabs(dir.Angle((poca_onwire-poca))-TMath::PiOver2());
197 distToPoca = (poca_onwire-poca).Mag();
198 if (angle*distToPoca < 0.1*
MINSTEP)
break;
202 if (lastStep*step < 0){
204 maxStep = 0.5*fabs(lastStep);
208 plane->
setU(dir.Cross(lineDirection));
211 if (fillExtrapSteps) {
226 debugOut <<
"RKTrackRep::extrapolateToLine(): Reached POCA after " << iterations+1 <<
" iterations. Distance: " << (poca_onwire-poca).Mag() <<
" cm. Angle deviation: " << dir.Angle((poca_onwire-poca))-TMath::PiOver2() <<
" rad \n";
236 const TVector3& point,
237 const TMatrixDSym* G,
239 bool calcJacobianNoise)
const {
242 debugOut <<
"RKTrackRep::extrapolateToPoint()\n";
247 static const unsigned int maxIt(1000);
253 bool fillExtrapSteps(
false);
254 if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
255 fillExtrapSteps =
true;
257 else if (calcJacobianNoise)
258 fillExtrapSteps =
true;
260 double step(0.), lastStep(0.), maxStep(1.E99), angle(0), distToPoca(0), tracklength(0);
261 TVector3 dir(state7[3], state7[4], state7[5]);
263 if(G->GetNrows() != 3) {
264 Exception exc(
"RKTrackRep::extrapolateToLine ==> G is not 3x3",__LINE__,__FILE__);
268 dir = TMatrix(*G) * dir;
270 TVector3 lastDir(0,0,0);
273 bool isAtBoundary(
false);
277 unsigned int iterations(0);
280 double flightTime = 0;
283 if(++iterations == maxIt) {
284 Exception exc(
"RKTrackRep::extrapolateToPoint ==> extrapolation to point failed, maximum number of iterations reached",__LINE__,__FILE__);
292 step = this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false, NULL,
true, stopAtBoundary, maxStep);
295 dir.SetXYZ(state7[3], state7[4], state7[5]);
297 dir = TMatrix(*G) * dir;
299 poca.SetXYZ(state7[0], state7[1], state7[2]);
302 if (stopAtBoundary && isAtBoundary) {
303 plane->setON(dir, poca);
307 angle = fabs(dir.Angle((point-poca))-TMath::PiOver2());
308 distToPoca = (point-poca).Mag();
309 if (angle*distToPoca < 0.1*
MINSTEP)
break;
313 if (lastStep*step < 0){
319 maxStep = 0.5*fabs(lastStep);
326 if (fillExtrapSteps) {
342 debugOut <<
"RKTrackRep::extrapolateToPoint(): Reached POCA after " << iterations+1 <<
" iterations. Distance: " << (point-poca).Mag() <<
" cm. Angle deviation: " << dir.Angle((point-poca))-TMath::PiOver2() <<
" rad \n";
353 const TVector3& linePoint,
354 const TVector3& lineDirection,
356 bool calcJacobianNoise)
const {
359 debugOut <<
"RKTrackRep::extrapolateToCylinder()\n";
364 static const unsigned int maxIt(1000);
370 bool fillExtrapSteps(
false);
371 if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
372 fillExtrapSteps =
true;
374 else if (calcJacobianNoise)
375 fillExtrapSteps =
true;
377 double tracklength(0.), maxStep(1.E99);
379 TVector3 dest, pos, dir;
381 bool isAtBoundary(
false);
385 unsigned int iterations(0);
388 double flightTime = 0;
391 if(++iterations == maxIt) {
392 Exception exc(
"RKTrackRep::extrapolateToCylinder ==> maximum number of iterations reached",__LINE__,__FILE__);
397 pos.SetXYZ(state7[0], state7[1], state7[2]);
398 dir.SetXYZ(state7[3], state7[4], state7[5]);
401 TVector3 AO = (pos - linePoint);
402 TVector3 AOxAB = (AO.Cross(lineDirection));
403 TVector3 VxAB = (dir.Cross(lineDirection));
404 float ab2 = (lineDirection * lineDirection);
405 float a = (VxAB * VxAB);
406 float b = 2 * (VxAB * AOxAB);
407 float c = (AOxAB * AOxAB) - (radius*radius * ab2);
408 double arg = b*b - 4.*a*c;
410 Exception exc(
"RKTrackRep::extrapolateToCylinder ==> cannot solve",__LINE__,__FILE__);
414 double term = sqrt(arg);
417 k1 = (-b + term)/(2.*a);
418 k2 = 2.*c/(-b + term);
421 k1 = 2.*c/(-b - term);
422 k2 = (-b - term)/(2.*a);
427 if (fabs(k2)<fabs(k))
431 debugOut <<
"RKTrackRep::extrapolateToCylinder(); k = " << k <<
"\n";
434 dest = pos + k * dir;
437 plane->setUV((dest-linePoint).Cross(lineDirection), lineDirection);
439 tracklength += this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false, NULL,
true, stopAtBoundary, maxStep);
442 if (stopAtBoundary && isAtBoundary) {
443 pos.SetXYZ(state7[0], state7[1], state7[2]);
444 dir.SetXYZ(state7[3], state7[4], state7[5]);
446 plane->setUV((pos-linePoint).Cross(lineDirection), lineDirection);
456 if (fillExtrapSteps) {
478 const TVector3& conePoint,
479 const TVector3& coneDirection,
481 bool calcJacobianNoise)
const {
484 debugOut <<
"RKTrackRep::extrapolateToCone()\n";
489 static const unsigned int maxIt(1000);
495 bool fillExtrapSteps(
false);
496 if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
497 fillExtrapSteps =
true;
499 else if (calcJacobianNoise)
500 fillExtrapSteps =
true;
502 double tracklength(0.), maxStep(1.E99);
504 TVector3 dest, pos, dir;
506 bool isAtBoundary(
false);
510 unsigned int iterations(0);
513 double flightTime = 0;
516 if(++iterations == maxIt) {
517 Exception exc(
"RKTrackRep::extrapolateToCone ==> maximum number of iterations reached",__LINE__,__FILE__);
522 pos.SetXYZ(state7[0], state7[1], state7[2]);
523 dir.SetXYZ(state7[3], state7[4], state7[5]);
530 TVector3 cDirection = coneDirection.Unit();
531 TVector3 Delta = (pos - conePoint);
532 double DirDelta = cDirection * Delta;
533 double Delta2 = Delta*Delta;
534 double UDir = dir * cDirection;
535 double UDelta = dir * Delta;
536 double U2 = dir * dir;
537 double cosAngle2 = cos(openingAngle)*cos(openingAngle);
538 double a = UDir*UDir - cosAngle2*U2;
539 double b = UDir*DirDelta - cosAngle2*UDelta;
540 double c = DirDelta*DirDelta - cosAngle2*Delta2;
542 double arg = b*b - a*c;
544 Exception exc(
"RKTrackRep::extrapolateToCone ==> cannot solve",__LINE__,__FILE__);
551 double term = sqrt(arg);
553 k1 = (-b + term) / a;
554 k2 = (-b - term) / a;
558 if(fabs(k2) < fabs(k)) {
563 debugOut <<
"RKTrackRep::extrapolateToCone(); k = " << k <<
"\n";
566 dest = pos + k * dir;
571 plane->setUV((dest-conePoint).Cross(coneDirection), dest-conePoint);
573 tracklength += this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false, NULL,
true, stopAtBoundary, maxStep);
576 if (stopAtBoundary && isAtBoundary) {
577 pos.SetXYZ(state7[0], state7[1], state7[2]);
578 dir.SetXYZ(state7[3], state7[4], state7[5]);
580 plane->setUV((pos-conePoint).Cross(coneDirection), pos-conePoint);
590 if (fillExtrapSteps) {
612 const TVector3& point,
614 bool calcJacobianNoise)
const {
617 debugOut <<
"RKTrackRep::extrapolateToSphere()\n";
622 static const unsigned int maxIt(1000);
628 bool fillExtrapSteps(
false);
629 if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
630 fillExtrapSteps =
true;
632 else if (calcJacobianNoise)
633 fillExtrapSteps =
true;
635 double tracklength(0.), maxStep(1.E99);
637 TVector3 dest, pos, dir;
639 bool isAtBoundary(
false);
643 unsigned int iterations(0);
646 double flightTime = 0;
649 if(++iterations == maxIt) {
650 Exception exc(
"RKTrackRep::extrapolateToSphere ==> maximum number of iterations reached",__LINE__,__FILE__);
655 pos.SetXYZ(state7[0], state7[1], state7[2]);
656 dir.SetXYZ(state7[3], state7[4], state7[5]);
659 TVector3 AO = (pos - point);
660 double dirAO = dir * AO;
661 double arg = dirAO*dirAO - AO*AO + radius*radius;
663 Exception exc(
"RKTrackRep::extrapolateToSphere ==> cannot solve",__LINE__,__FILE__);
667 double term = sqrt(arg);
674 if (fabs(k2)<fabs(k))
678 debugOut <<
"RKTrackRep::extrapolateToSphere(); k = " << k <<
"\n";
681 dest = pos + k * dir;
683 plane->setON(dest, dest-point);
685 tracklength += this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false, NULL,
true, stopAtBoundary, maxStep);
688 if (stopAtBoundary && isAtBoundary) {
689 pos.SetXYZ(state7[0], state7[1], state7[2]);
690 dir.SetXYZ(state7[3], state7[4], state7[5]);
691 plane->setON(pos, pos-point);
701 if (fillExtrapSteps) {
724 bool calcJacobianNoise)
const {
727 debugOut <<
"RKTrackRep::extrapolateBy()\n";
732 static const unsigned int maxIt(1000);
738 bool fillExtrapSteps(
false);
739 if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
740 fillExtrapSteps =
true;
742 else if (calcJacobianNoise)
743 fillExtrapSteps =
true;
745 double tracklength(0.);
747 TVector3 dest, pos, dir;
749 bool isAtBoundary(
false);
753 unsigned int iterations(0);
756 double flightTime = 0;
759 if(++iterations == maxIt) {
760 Exception exc(
"RKTrackRep::extrapolateBy ==> maximum number of iterations reached",__LINE__,__FILE__);
765 pos.SetXYZ(state7[0], state7[1], state7[2]);
766 dir.SetXYZ(state7[3], state7[4], state7[5]);
768 dest = pos + 1.5*(step-tracklength) * dir;
770 plane->setON(dest, dir);
772 tracklength += this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false, NULL,
true, stopAtBoundary, (step-tracklength));
775 if (stopAtBoundary && isAtBoundary) {
776 pos.SetXYZ(state7[0], state7[1], state7[2]);
777 dir.SetXYZ(state7[3], state7[4], state7[5]);
778 plane->setON(pos, dir);
782 if (fabs(tracklength-step) <
MINSTEP) {
784 debugOut <<
"RKTrackRep::extrapolateBy(): reached after " << iterations <<
" iterations. \n";
786 pos.SetXYZ(state7[0], state7[1], state7[2]);
787 dir.SetXYZ(state7[3], state7[4], state7[5]);
788 plane->setON(pos, dir);
796 if (fillExtrapSteps) {
820 return TVector3(state7[0], state7[1], state7[2]);
825 M1x7 state7 = {{0, 0, 0, 0, 0, 0, 0}};
828 TVector3 mom(state7[3], state7[4], state7[5]);
835 M1x7 state7 = {{0, 0, 0, 0, 0, 0, 0}};
838 pos.SetXYZ(state7[0], state7[1], state7[2]);
839 mom.SetXYZ(state7[3], state7[4], state7[5]);
861 if (dynamic_cast<const MeasurementOnPlane*>(&state) != NULL) {
862 Exception exc(
"RKTrackRep::getCharge - cannot get charge from MeasurementOnPlane",__LINE__,__FILE__);
870 if (state.
getState()(0) * pdgCharge < 0)
887 if (dynamic_cast<const MeasurementOnPlane*>(&state) != NULL) {
888 Exception exc(
"RKTrackRep::getMomVar - cannot get momVar from MeasurementOnPlane",__LINE__,__FILE__);
907 if (dynamic_cast<const MeasurementOnPlane*>(&state) != NULL) {
908 Exception exc(
"RKTrackRep::getSpu - cannot get spu from MeasurementOnPlane",__LINE__,__FILE__);
914 if (auxInfo.GetNrows() == 2
915 || auxInfo.GetNrows() == 1)
924 if (auxInfo.GetNrows() == 2)
932 const M1x7& destState7,
const DetPlane& destPlane)
const {
935 debugOut <<
"RKTrackRep::calcForwardJacobianAndNoise " << std::endl;
939 Exception exc(
"RKTrackRep::calcForwardJacobianAndNoise ==> cache is empty. Extrapolation must run with a MeasuredStateOnPlane.",__LINE__,__FILE__);
944 TMatrixD jac(TMatrixD::kTransposed, TMatrixD(7, 7,
ExtrapSteps_.back().jac7_.begin()));
945 TMatrixDSym noise(7,
ExtrapSteps_.back().noise7_.begin());
947 noise += TMatrixDSym(7,
ExtrapSteps_[i].noise7_.begin()).Similarity(jac);
948 jac *= TMatrixD(TMatrixD::kTransposed, TMatrixD(7, 7,
ExtrapSteps_[i].jac7_.begin()));
952 M1x3 pTilde = {{startState7[3], startState7[4], startState7[5]}};
953 const TVector3& normal = startPlane.
getNormal();
954 double pTildeW = pTilde[0] * normal.X() + pTilde[1] * normal.Y() + pTilde[2] * normal.Z();
955 double spu = pTildeW > 0 ? 1 : -1;
956 for (
unsigned int i=0; i<3; ++i) {
957 pTilde[i] *= spu/pTildeW;
979 jacobian.ResizeTo(5,5);
986 deltaState.ResizeTo(5);
990 deltaState *= jacobian;
996 debugOut <<
"delta state : "; deltaState.Print();
1004 debugOut <<
"RKTrackRep::getBackwardJacobianAndNoise " << std::endl;
1008 Exception exc(
"RKTrackRep::getBackwardJacobianAndNoise ==> cache is empty. Extrapolation must run with a MeasuredStateOnPlane.",__LINE__,__FILE__);
1012 jacobian.ResizeTo(5,5);
1014 if (!useInvertFast) {
1015 bool status = TDecompLU::InvertLU(jacobian, 0.0);
1017 Exception e(
"cannot invert matrix, status = 0", __LINE__,__FILE__);
1023 jacobian.InvertFast(&det);
1025 Exception e(
"cannot invert matrix, status = 0", __LINE__,__FILE__);
1031 noise.ResizeTo(5,5);
1033 noise.Similarity(jacobian);
1036 deltaState.ResizeTo(5);
1046 Exception exc(
"RKTrackRep::getSteps ==> cache is empty.",__LINE__,__FILE__);
1050 std::vector<MatStep> retVal;
1053 for (
unsigned int i = 0; i<
RKSteps_.size(); ++i) {
1054 retVal.push_back(
RKSteps_[i].matStep_);
1066 Exception exc(
"RKTrackRep::getRadiationLenght ==> cache is empty.",__LINE__,__FILE__);
1072 for (
unsigned int i = 0; i<
RKSteps_.size(); ++i) {
1073 radLen +=
RKSteps_.at(i).matStep_.stepSize_ /
RKSteps_.at(i).matStep_.materialProperties_.getRadLen();
1083 if (state.
getRep() !=
this){
1084 Exception exc(
"RKTrackRep::setPosMom ==> state is defined wrt. another TrackRep",__LINE__,__FILE__);
1088 if (dynamic_cast<MeasurementOnPlane*>(&state) != NULL) {
1089 Exception exc(
"RKTrackRep::setPosMom - cannot set pos/mom of a MeasurementOnPlane",__LINE__,__FILE__);
1094 if (mom.Mag2() == 0) {
1095 Exception exc(
"RKTrackRep::setPosMom - momentum is 0",__LINE__,__FILE__);
1102 if (auxInfo.GetNrows() != 2) {
1103 bool alreadySet = auxInfo.GetNrows() == 1;
1104 auxInfo.ResizeTo(2);
1113 state7[0] = pos.X();
1114 state7[1] = pos.Y();
1115 state7[2] = pos.Z();
1117 state7[3] = mom.X();
1118 state7[4] = mom.Y();
1119 state7[5] = mom.Z();
1122 double norm = 1. / sqrt(state7[3]*state7[3] + state7[4]*state7[4] + state7[5]*state7[5]);
1123 for (
unsigned int i=3; i<6; ++i)
1137 TVectorD& state5(state.
getState());
1152 if (state6.GetNrows()!=6){
1153 Exception exc(
"RKTrackRep::setPosMom ==> state has to be 6d (x, y, z, px, py, pz)",__LINE__,__FILE__);
1156 setPosMom(state, TVector3(state6(0), state6(1), state6(2)), TVector3(state6(3), state6(4), state6(5)));
1166 const TVector3& U(state.
getPlane()->getU());
1167 const TVector3& V(state.
getPlane()->getV());
1168 TVector3 W(state.
getPlane()->getNormal());
1170 double pw = mom * W;
1171 double pu = mom * U;
1172 double pv = mom * V;
1174 TMatrixDSym& cov(state.
getCov());
1176 cov(0,0) = pow(
getCharge(state), 2) / pow(mom.Mag(), 6) *
1177 (mom.X()*mom.X() * momErr.X()*momErr.X()+
1178 mom.Y()*mom.Y() * momErr.Y()*momErr.Y()+
1179 mom.Z()*mom.Z() * momErr.Z()*momErr.Z());
1181 cov(1,1) = pow((U.X()/pw - W.X()*pu/(pw*pw)),2.) * momErr.X()*momErr.X() +
1182 pow((U.Y()/pw - W.Y()*pu/(pw*pw)),2.) * momErr.Y()*momErr.Y() +
1183 pow((U.Z()/pw - W.Z()*pu/(pw*pw)),2.) * momErr.Z()*momErr.Z();
1185 cov(2,2) = pow((V.X()/pw - W.X()*pv/(pw*pw)),2.) * momErr.X()*momErr.X() +
1186 pow((V.Y()/pw - W.Y()*pv/(pw*pw)),2.) * momErr.Y()*momErr.Y() +
1187 pow((V.Z()/pw - W.Z()*pv/(pw*pw)),2.) * momErr.Z()*momErr.Z();
1189 cov(3,3) = posErr.X()*posErr.X() * U.X()*U.X() +
1190 posErr.Y()*posErr.Y() * U.Y()*U.Y() +
1191 posErr.Z()*posErr.Z() * U.Z()*U.Z();
1193 cov(4,4) = posErr.X()*posErr.X() * V.X()*V.X() +
1194 posErr.Y()*posErr.Y() * V.Y()*V.Y() +
1195 posErr.Z()*posErr.Z() * V.Z()*V.Z();
1204 if (cov6x6.GetNcols()!=6 || cov6x6.GetNrows()!=6){
1205 Exception exc(
"RKTrackRep::setPosMomCov ==> cov has to be 6x6 (x, y, z, px, py, pz)",__LINE__,__FILE__);
1214 const M6x6& cov6x6_( *((
M6x6*) cov6x6.GetMatrixArray()) );
1222 if (state6.GetNrows()!=6){
1223 Exception exc(
"RKTrackRep::setPosMomCov ==> state has to be 6d (x, y, z, px, py, pz)",__LINE__,__FILE__);
1227 if (cov6x6.GetNcols()!=6 || cov6x6.GetNrows()!=6){
1228 Exception exc(
"RKTrackRep::setPosMomCov ==> cov has to be 6x6 (x, y, z, px, py, pz)",__LINE__,__FILE__);
1232 TVector3 pos(state6(0), state6(1), state6(2));
1233 TVector3 mom(state6(3), state6(4), state6(5));
1239 const M6x6& cov6x6_( *((
M6x6*) cov6x6.GetMatrixArray()) );
1248 if (dynamic_cast<MeasurementOnPlane*>(&state) != NULL) {
1249 Exception exc(
"RKTrackRep::setChargeSign - cannot set charge of a MeasurementOnPlane",__LINE__,__FILE__);
1254 if (state.
getState()(0) * charge < 0) {
1277 bool calcOnlyLastRowOfJ)
const {
1292 static const double EC ( 0.000149896229 );
1293 static const double P3 ( 1./3. );
1294 static const double DLT ( .0002 );
1298 double S3(0), S4(0), PS2(0);
1299 M1x3 H0 = {{0.,0.,0.}}, H1 = {{0.,0.,0.}}, H2 = {{0.,0.,0.}};
1300 M1x3 r = {{0.,0.,0.}};
1302 double A0(0), A1(0), A2(0), A3(0), A4(0), A5(0), A6(0);
1303 double B0(0), B1(0), B2(0), B3(0), B4(0), B5(0), B6(0);
1304 double C0(0), C1(0), C2(0), C3(0), C4(0), C5(0), C6(0);
1311 PS2 = state7[6]*EC * S;
1314 r[0] = R[0]; r[1] = R[1]; r[2]=R[2];
1316 H0[0] *= PS2; H0[1] *= PS2; H0[2] *= PS2;
1317 A0 = A[1]*H0[2]-A[2]*H0[1]; B0 = A[2]*H0[0]-A[0]*H0[2]; C0 = A[0]*H0[1]-A[1]*H0[0];
1318 A2 = A[0]+A0 ; B2 = A[1]+B0 ; C2 = A[2]+C0 ;
1319 A1 = A2+A[0] ; B1 = B2+A[1] ; C1 = C2+A[2] ;
1323 r[0] += A1*S4; r[1] += B1*S4; r[2] += C1*S4;
1325 H1[0] *= PS2; H1[1] *= PS2; H1[2] *= PS2;
1328 A3 = B2*H1[2]-C2*H1[1]+A[0]; B3 = C2*H1[0]-A2*H1[2]+A[1]; C3 = A2*H1[1]-B2*H1[0]+A[2];
1329 A4 = B3*H1[2]-C3*H1[1]+A[0]; B4 = C3*H1[0]-A3*H1[2]+A[1]; C4 = A3*H1[1]-B3*H1[0]+A[2];
1330 A5 = A4-A[0]+A4 ; B5 = B4-A[1]+B4 ; C5 = C4-A[2]+C4 ;
1334 r[0]=R[0]+S*A4; r[1]=R[1]+S*B4; r[2]=R[2]+S*C4;
1336 H2[0] *= PS2; H2[1] *= PS2; H2[2] *= PS2;
1339 A6 = B5*H2[2]-C5*H2[1]; B6 = C5*H2[0]-A5*H2[2]; C6 = A5*H2[1]-B5*H2[0];
1345 if(jacobianT != NULL){
1355 M7x7& J = *jacobianT;
1357 double dA0(0), dA2(0), dA3(0), dA4(0), dA5(0), dA6(0);
1358 double dB0(0), dB2(0), dB3(0), dB4(0), dB5(0), dB6(0);
1359 double dC0(0), dC2(0), dC3(0), dC4(0), dC5(0), dC6(0);
1363 if (!calcOnlyLastRowOfJ) {
1367 J(0, 0) = 1; J(1, 1) = 1; J(2, 2) = 1;
1373 for(
int i=start; i<6; ++i) {
1376 dA0 = H0[2]*J(i, 4)-H0[1]*J(i, 5);
1377 dB0 = H0[0]*J(i, 5)-H0[2]*J(i, 3);
1378 dC0 = H0[1]*J(i, 3)-H0[0]*J(i, 4);
1385 dA3 = J(i, 3)+dB2*H1[2]-dC2*H1[1];
1386 dB3 = J(i, 4)+dC2*H1[0]-dA2*H1[2];
1387 dC3 = J(i, 5)+dA2*H1[1]-dB2*H1[0];
1389 dA4 = J(i, 3)+dB3*H1[2]-dC3*H1[1];
1390 dB4 = J(i, 4)+dC3*H1[0]-dA3*H1[2];
1391 dC4 = J(i, 5)+dA3*H1[1]-dB3*H1[0];
1394 dA5 = dA4+dA4-J(i, 3);
1395 dB5 = dB4+dB4-J(i, 4);
1396 dC5 = dC4+dC4-J(i, 5);
1398 dA6 = dB5*H2[2]-dC5*H2[1];
1399 dB6 = dC5*H2[0]-dA5*H2[2];
1400 dC6 = dA5*H2[1]-dB5*H2[0];
1403 J(i, 0) += (dA2+dA3+dA4)*S3; J(i, 3) = ((dA0+2.*dA3)+(dA5+dA6))*P3;
1404 J(i, 1) += (dB2+dB3+dB4)*S3; J(i, 4) = ((dB0+2.*dB3)+(dB5+dB6))*P3;
1405 J(i, 2) += (dC2+dC3+dC4)*S3; J(i, 5) = ((dC0+2.*dC3)+(dC5+dC6))*P3;
1410 J(6, 3) *= state7[6]; J(6, 4) *= state7[6]; J(6, 5) *= state7[6];
1413 dA0 = H0[2]*J(6, 4)-H0[1]*J(6, 5) + A0;
1414 dB0 = H0[0]*J(6, 5)-H0[2]*J(6, 3) + B0;
1415 dC0 = H0[1]*J(6, 3)-H0[0]*J(6, 4) + C0;
1422 dA3 = J(6, 3)+dB2*H1[2]-dC2*H1[1] + (A3-A[0]);
1423 dB3 = J(6, 4)+dC2*H1[0]-dA2*H1[2] + (B3-A[1]);
1424 dC3 = J(6, 5)+dA2*H1[1]-dB2*H1[0] + (C3-A[2]);
1426 dA4 = J(6, 3)+dB3*H1[2]-dC3*H1[1] + (A4-A[0]);
1427 dB4 = J(6, 4)+dC3*H1[0]-dA3*H1[2] + (B4-A[1]);
1428 dC4 = J(6, 5)+dA3*H1[1]-dB3*H1[0] + (C4-A[2]);
1431 dA5 = dA4+dA4-J(6, 3);
1432 dB5 = dB4+dB4-J(6, 4);
1433 dC5 = dC4+dC4-J(6, 5);
1435 dA6 = dB5*H2[2]-dC5*H2[1] + A6;
1436 dB6 = dC5*H2[0]-dA5*H2[2] + B6;
1437 dC6 = dA5*H2[1]-dB5*H2[0] + C6;
1440 J(6, 0) += (dA2+dA3+dA4)*S3/state7[6]; J(6, 3) = ((dA0+2.*dA3)+(dA5+dA6))*P3/state7[6];
1441 J(6, 1) += (dB2+dB3+dB4)*S3/state7[6]; J(6, 4) = ((dB0+2.*dB3)+(dB5+dB6))*P3/state7[6];
1442 J(6, 2) += (dC2+dC3+dC4)*S3/state7[6]; J(6, 5) = ((dC0+2.*dC3)+(dC5+dC6))*P3/state7[6];
1449 R[0] += (A2+A3+A4)*S3; A[0] += (SA[0]=((A0+2.*A3)+(A5+A6))*P3-A[0]);
1450 R[1] += (B2+B3+B4)*S3; A[1] += (SA[1]=((B0+2.*B3)+(B5+B6))*P3-A[1]);
1451 R[2] += (C2+C3+C4)*S3; A[2] += (SA[2]=((C0+2.*C3)+(C5+C6))*P3-A[2]);
1454 double CBA ( 1./sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]) );
1455 A[0] *= CBA; A[1] *= CBA; A[2] *= CBA;
1459 double EST ( fabs((A1+A6)-(A3+A4)) +
1460 fabs((B1+B6)-(B3+B4)) +
1461 fabs((C1+C6)-(C3+C4)) );
1463 debugOut <<
" RKTrackRep::RKPropagate. Step = "<< S <<
"; quality EST = " << EST <<
" \n";
1473 return pow(DLT/EST, 1./5.);
1481 for (
unsigned int i=0; i<7; ++i)
1499 if (dynamic_cast<const MeasurementOnPlane*>(&state) != NULL) {
1500 Exception exc(
"RKTrackRep::getState7 - cannot get pos or mom from a MeasurementOnPlane",__LINE__,__FILE__);
1505 const TVector3& U(state.
getPlane()->getU());
1506 const TVector3& V(state.
getPlane()->getV());
1507 const TVector3& O(state.
getPlane()->getO());
1508 const TVector3& W(state.
getPlane()->getNormal());
1510 assert(state.
getState().GetNrows() == 5);
1511 const double* state5 = state.
getState().GetMatrixArray();
1513 double spu =
getSpu(state);
1515 state7[0] = O.X() + state5[3]*U.X() + state5[4]*V.X();
1516 state7[1] = O.Y() + state5[3]*U.Y() + state5[4]*V.Y();
1517 state7[2] = O.Z() + state5[3]*U.Z() + state5[4]*V.Z();
1519 state7[3] = spu * (W.X() + state5[1]*U.X() + state5[2]*V.X());
1520 state7[4] = spu * (W.Y() + state5[1]*U.Y() + state5[2]*V.Y());
1521 state7[5] = spu * (W.Z() + state5[1]*U.Z() + state5[2]*V.Z());
1524 double norm = 1. / sqrt(state7[3]*state7[3] + state7[4]*state7[4] + state7[5]*state7[5]);
1525 for (
unsigned int i=3; i<6; ++i) state7[i] *= norm;
1527 state7[6] = state5[0];
1537 const TVector3& O(state.
getPlane()->getO());
1538 const TVector3& U(state.
getPlane()->getU());
1539 const TVector3& V(state.
getPlane()->getV());
1540 const TVector3& W(state.
getPlane()->getNormal());
1543 double AtW( state7[3]*W.X() + state7[4]*W.Y() + state7[5]*W.Z() );
1550 double* state5 = state.
getState().GetMatrixArray();
1552 state5[0] = state7[6];
1553 state5[1] = (state7[3]*U.X() + state7[4]*U.Y() + state7[5]*U.Z()) / AtW;
1554 state5[2] = (state7[3]*V.X() + state7[4]*V.Y() + state7[5]*V.Z()) / AtW;
1555 state5[3] = ((state7[0]-O.X())*U.X() +
1556 (state7[1]-O.Y())*U.Y() +
1557 (state7[2]-O.Z())*U.Z());
1558 state5[4] = ((state7[0]-O.X())*V.X() +
1559 (state7[1]-O.Y())*V.Y() +
1560 (state7[2]-O.Z())*V.Z());
1569 M7x7& out7x7)
const {
1572 const TVector3& U(state.
getPlane()->getU());
1573 const TVector3& V(state.
getPlane()->getV());
1574 const TVector3& W(state.
getPlane()->getNormal());
1576 const TVectorD& state5(state.
getState());
1577 double spu(
getSpu(state));
1580 pTilde[0] = spu * (W.X() + state5(1)*U.X() + state5(2)*V.X());
1581 pTilde[1] = spu * (W.Y() + state5(1)*U.Y() + state5(2)*V.Y());
1582 pTilde[2] = spu * (W.Z() + state5(1)*U.Z() + state5(2)*V.Z());
1604 std::fill(J_pM.
begin(), J_pM.
end(), 0);
1606 const double pTildeMag = sqrt(pTilde[0]*pTilde[0] + pTilde[1]*pTilde[1] + pTilde[2]*pTilde[2]);
1607 const double pTildeMag2 = pTildeMag*pTildeMag;
1609 const double utpTildeOverpTildeMag2 = (U.X()*pTilde[0] + U.Y()*pTilde[1] + U.Z()*pTilde[2]) / pTildeMag2;
1610 const double vtpTildeOverpTildeMag2 = (V.X()*pTilde[0] + V.Y()*pTilde[1] + V.Z()*pTilde[2]) / pTildeMag2;
1625 double fact = spu / pTildeMag;
1626 J_pM[10] = fact * ( U.X() - pTilde[0]*utpTildeOverpTildeMag2 );
1627 J_pM[11] = fact * ( U.Y() - pTilde[1]*utpTildeOverpTildeMag2 );
1628 J_pM[12] = fact * ( U.Z() - pTilde[2]*utpTildeOverpTildeMag2 );
1630 J_pM[17] = fact * ( V.X() - pTilde[0]*vtpTildeOverpTildeMag2 );
1631 J_pM[18] = fact * ( V.Y() - pTilde[1]*vtpTildeOverpTildeMag2 );
1632 J_pM[19] = fact * ( V.Z() - pTilde[2]*vtpTildeOverpTildeMag2 );
1641 M6x6& out6x6)
const {
1644 const TVector3& U(state.
getPlane()->getU());
1645 const TVector3& V(state.
getPlane()->getV());
1646 const TVector3& W(state.
getPlane()->getNormal());
1648 const TVectorD& state5(state.
getState());
1649 double spu(
getSpu(state));
1652 pTilde[0] = spu * (W.X() + state5(1)*U.X() + state5(2)*V.X());
1653 pTilde[1] = spu * (W.Y() + state5(1)*U.Y() + state5(2)*V.Y());
1654 pTilde[2] = spu * (W.Z() + state5(1)*U.Z() + state5(2)*V.Z());
1656 const double pTildeMag = sqrt(pTilde[0]*pTilde[0] + pTilde[1]*pTilde[1] + pTilde[2]*pTilde[2]);
1657 const double pTildeMag2 = pTildeMag*pTildeMag;
1659 const double utpTildeOverpTildeMag2 = (U.X()*pTilde[0] + U.Y()*pTilde[1] + U.Z()*pTilde[2]) / pTildeMag2;
1660 const double vtpTildeOverpTildeMag2 = (V.X()*pTilde[0] + V.Y()*pTilde[1] + V.Z()*pTilde[2]) / pTildeMag2;
1664 const double qop = state5(0);
1668 std::fill(J_pM_5x6.
begin(), J_pM_5x6.
end(), 0);
1671 double fact = -1. * p / (pTildeMag * qop);
1672 J_pM_5x6[3] = fact * pTilde[0];
1673 J_pM_5x6[4] = fact * pTilde[1];
1674 J_pM_5x6[5] = fact * pTilde[2];
1676 fact = p * spu / pTildeMag;
1677 J_pM_5x6[9] = fact * ( U.X() - pTilde[0]*utpTildeOverpTildeMag2 );
1678 J_pM_5x6[10] = fact * ( U.Y() - pTilde[1]*utpTildeOverpTildeMag2 );
1679 J_pM_5x6[11] = fact * ( U.Z() - pTilde[2]*utpTildeOverpTildeMag2 );
1681 J_pM_5x6[15] = fact * ( V.X() - pTilde[0]*vtpTildeOverpTildeMag2 );
1682 J_pM_5x6[16] = fact * ( V.Y() - pTilde[1]*vtpTildeOverpTildeMag2 );
1683 J_pM_5x6[17] = fact * ( V.Z() - pTilde[2]*vtpTildeOverpTildeMag2 );
1685 J_pM_5x6[18] = U.X();
1686 J_pM_5x6[19] = U.Y();
1687 J_pM_5x6[20] = U.Z();
1689 J_pM_5x6[24] = V.X();
1690 J_pM_5x6[25] = V.Y();
1691 J_pM_5x6[26] = V.Z();
1707 const TVector3& U(state.
getPlane()->getU());
1708 const TVector3& V(state.
getPlane()->getV());
1709 const TVector3& W(state.
getPlane()->getNormal());
1735 std::fill(J_Mp.
begin(), J_Mp.
end(), 7*5);
1737 const double AtU = A[0]*U.X() + A[1]*U.Y() + A[2]*U.Z();
1738 const double AtV = A[0]*V.X() + A[1]*V.Y() + A[2]*V.Z();
1739 const double AtW = A[0]*W.X() + A[1]*W.Y() + A[2]*W.Z();
1744 double fact = 1./(AtW*AtW);
1745 J_Mp[16] = fact * (U.X()*AtW - W.X()*AtU);
1746 J_Mp[21] = fact * (U.Y()*AtW - W.Y()*AtU);
1747 J_Mp[26] = fact * (U.Z()*AtW - W.Z()*AtU);
1749 J_Mp[17] = fact * (V.X()*AtW - W.X()*AtV);
1750 J_Mp[22] = fact * (V.Y()*AtW - W.Y()*AtV);
1751 J_Mp[27] = fact * (V.Z()*AtW - W.Z()*AtV);
1775 const TVector3& U(state.
getPlane()->getU());
1776 const TVector3& V(state.
getPlane()->getV());
1777 const TVector3& W(state.
getPlane()->getNormal());
1779 const double AtU = state7[3]*U.X() + state7[4]*U.Y() + state7[5]*U.Z();
1780 const double AtV = state7[3]*V.X() + state7[4]*V.Y() + state7[5]*V.Z();
1781 const double AtW = state7[3]*W.X() + state7[4]*W.Y() + state7[5]*W.Z();
1785 const double qop = state7[6];
1789 std::fill(J_Mp_6x5.
begin(), J_Mp_6x5.
end(), 0);
1792 J_Mp_6x5[3] = U.X();
1793 J_Mp_6x5[8] = U.Y();
1794 J_Mp_6x5[13] = U.Z();
1796 J_Mp_6x5[4] = V.X();
1797 J_Mp_6x5[9] = V.Y();
1798 J_Mp_6x5[14] = V.Z();
1800 double fact = (-1.) * qop / p;
1801 J_Mp_6x5[15] = fact * state7[3];
1802 J_Mp_6x5[20] = fact * state7[4];
1803 J_Mp_6x5[25] = fact * state7[5];
1805 fact = 1./(p*AtW*AtW);
1806 J_Mp_6x5[16] = fact * (U.X()*AtW - W.X()*AtU);
1807 J_Mp_6x5[21] = fact * (U.Y()*AtW - W.Y()*AtU);
1808 J_Mp_6x5[26] = fact * (U.Z()*AtW - W.Z()*AtU);
1810 J_Mp_6x5[17] = fact * (V.X()*AtW - W.X()*AtV);
1811 J_Mp_6x5[22] = fact * (V.Y()*AtW - W.Y()*AtV);
1812 J_Mp_6x5[27] = fact * (V.Z()*AtW - W.Z()*AtV);
1858 M1x7* J_MMT_unprojected_lastRow,
1859 double& coveredDistance,
1862 M7x7& noiseProjection,
1865 bool calcOnlyLastRowOfJ)
const {
1868 static const double Wmax ( 3000. );
1869 static const double AngleMax ( 6.3 );
1870 static const double Pmin ( 4.E-3 );
1871 static const unsigned int maxNumIt ( 1000 );
1875 M1x3 SA = {{0.,0.,0.}};
1877 double momentum ( fabs(charge/state7[6]) );
1878 double relMomLoss ( 0 );
1879 double deltaAngle ( 0. );
1880 double An(0), S(0), Sl(0), CBA(0);
1884 debugOut <<
"RKTrackRep::RKutta \n";
1885 debugOut <<
"position: "; TVector3(R[0], R[1], R[2]).Print();
1886 debugOut <<
"direction: "; TVector3(A[0], A[1], A[2]).Print();
1887 debugOut <<
"momentum: " << momentum <<
" GeV\n";
1891 checkJacProj =
false;
1894 if(momentum < Pmin){
1895 std::ostringstream sstream;
1896 sstream <<
"RKTrackRep::RKutta ==> momentum too low: " << momentum*1000. <<
" MeV";
1897 Exception exc(sstream.str(),__LINE__,__FILE__);
1902 unsigned int counter(0);
1905 S =
estimateStep(state7, SU, plane, charge, relMomLoss, limits);
1910 while (fabs(S) >=
MINSTEP || counter == 0) {
1912 if(++counter > maxNumIt){
1913 Exception exc(
"RKTrackRep::RKutta ==> maximum number of iterations exceeded",__LINE__,__FILE__);
1919 debugOut <<
"------ RKutta main loop nr. " << counter-1 <<
" ------\n";
1922 M1x3 ABefore = {{ A[0], A[1], A[2] }};
1923 RKPropagate(state7, jacobianT, SA, S,
true, calcOnlyLastRowOfJ);
1926 coveredDistance += S;
1929 double beta = 1/hypot(1, mass*state7[6]/charge);
1930 flightTime += S / beta / 29.9792458;
1934 std::ostringstream sstream;
1935 sstream <<
"RKTrackRep::RKutta ==> Total extrapolation length is longer than length limit : " << Way <<
" cm !";
1936 Exception exc(sstream.str(),__LINE__,__FILE__);
1941 if (onlyOneStep)
return(
true);
1946 debugOut<<
" momLossExceeded -> return(true); \n";
1954 debugOut<<
" at boundary -> return(true); \n";
1966 S =
estimateStep(state7, SU, plane, charge, relMomLoss, limits);
1971 debugOut<<
" (at Plane && fabs(S) < MINSTEP) -> break and do linear extrapolation \n";
1978 debugOut<<
" (momLossExceeded && fabs(S) < MINSTEP) -> return(true), no linear extrapolation; \n";
1986 double arg = ABefore[0]*A[0] + ABefore[1]*A[1] + ABefore[2]*A[2];
1987 arg = arg > 1 ? 1 : arg;
1988 arg = arg < -1 ? -1 : arg;
1989 deltaAngle += acos(arg);
1990 if (fabs(deltaAngle) > AngleMax){
1991 std::ostringstream sstream;
1992 sstream <<
"RKTrackRep::RKutta ==> Do not get to an active plane! Already extrapolated " << deltaAngle * 180 / TMath::Pi() <<
"°.";
1993 Exception exc(sstream.str(),__LINE__,__FILE__);
2000 if (S *
RKSteps_.at(counter-1).matStep_.stepSize_ < 0 &&
2001 RKSteps_.at(counter-1).matStep_.stepSize_*
RKSteps_.at(counter-2).matStep_.stepSize_ < 0 &&
2002 RKSteps_.at(counter-2).matStep_.stepSize_*
RKSteps_.at(counter-3).matStep_.stepSize_ < 0){
2003 Exception exc(
"RKTrackRep::RKutta ==> Do not get closer to plane!",__LINE__,__FILE__);
2017 if (fabs(Sl) > 0.001*
MINSTEP){
2019 debugOut <<
" RKutta - linear extrapolation to surface\n";
2024 SA[0]*=Sl; SA[1]*=Sl; SA[2]*=Sl;
2032 CBA = 1./sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]);
2033 A[0] *= CBA; A[1] *= CBA; A[2] *= CBA;
2035 R[0] += S*(A[0]-0.5*S*SA[0]);
2036 R[1] += S*(A[1]-0.5*S*SA[1]);
2037 R[2] += S*(A[2]-0.5*S*SA[2]);
2040 coveredDistance += S;
2043 double beta = 1/hypot(1, mass*state7[6]/charge);
2044 flightTime += S / beta / 29.9792458;
2047 debugOut <<
" RKutta - last stepsize too small -> can't do linear extrapolation! \n";
2053 if (jacobianT != NULL) {
2064 if (checkJacProj &&
RKSteps_.size()>0){
2065 Exception exc(
"RKTrackRep::Extrap ==> covariance is projected onto destination plane again",__LINE__,__FILE__);
2072 debugOut <<
" Project Jacobian of extrapolation onto destination plane\n";
2074 An = A[0]*SU[0] + A[1]*SU[1] + A[2]*SU[2];
2075 An = (fabs(An) > 1.E-7 ? 1./An : 0);
2078 if (calcOnlyLastRowOfJ)
2081 double* jacPtr = jacobianT->
begin();
2083 for(
unsigned int j=42; j<49; j+=7) {
2084 (*J_MMT_unprojected_lastRow)[j-42] = jacPtr[j];
2088 norm = (jacPtr[i]*SU[0] + jacPtr[i+1]*SU[1] + jacPtr[i+2]*SU[2]) * An;
2089 jacPtr[i] -= norm*A [0]; jacPtr[i+1] -= norm*A [1]; jacPtr[i+2] -= norm*A [2];
2090 jacPtr[i+3] -= norm*SA[0]; jacPtr[i+4] -= norm*SA[1]; jacPtr[i+5] -= norm*SA[2];
2092 checkJacProj =
true;
2100 if (!calcOnlyLastRowOfJ) {
2101 for (
int iRow = 0; iRow < 3; ++iRow) {
2102 for (
int iCol = 0; iCol < 3; ++iCol) {
2103 noiseProjection[iRow*7 + iCol] = (iRow == iCol) - An * SU[iCol] * A[iRow];
2104 noiseProjection[(iRow + 3)*7 + iCol] = - An * SU[iCol] * SA[iRow];
2129 const double& charge,
2158 debugOut <<
" RKTrackRep::estimateStep \n";
2159 debugOut <<
" position: "; TVector3(state7[0], state7[1], state7[2]).Print();
2160 debugOut <<
" direction: "; TVector3(state7[3], state7[4], state7[5]).Print();
2164 double Dist ( SU[3] - (state7[0]*SU[0] +
2167 double An ( state7[3]*SU[0] +
2172 if (fabs(An) > 1.E-10)
2175 SLDist = Dist*1.E10;
2176 if (An<0) SLDist *= -1.;
2183 debugOut <<
" Distance to plane: " << Dist <<
"\n";
2184 debugOut <<
" SL distance to plane: " << SLDist <<
"\n";
2186 debugOut <<
" Direction is pointing towards surface.\n";
2188 debugOut <<
" Direction is pointing away from surface.\n";
2197 std::pair<double, double> distVsStep (9.E99, 9.E99);
2199 static const unsigned int maxNumIt = 10;
2200 unsigned int counter(0);
2202 while (fabs(fieldCurvLimit) >
MINSTEP) {
2204 if(++counter > maxNumIt){
2208 fieldCurvLimit *= 0.5;
2212 M1x7 state7_temp = state7;
2213 M1x3 SA = {{0, 0, 0}};
2215 double q (
RKPropagate(state7_temp, NULL, SA, fieldCurvLimit,
true) );
2217 debugOut <<
" maxStepArg = " << fieldCurvLimit <<
"; q = " << q <<
" \n";
2222 Dist = SU[3] - (state7_temp[0] * SU[0] +
2223 state7_temp[1] * SU[1] +
2224 state7_temp[2] * SU[2]);
2226 An = state7_temp[3] * SU[0] +
2227 state7_temp[4] * SU[1] +
2228 state7_temp[5] * SU[2];
2230 if (fabs(Dist/An) < fabs(distVsStep.first)) {
2231 distVsStep.first = Dist/An;
2232 distVsStep.second = fieldCurvLimit;
2239 fieldCurvLimit *= 2;
2243 fieldCurvLimit *= q * 0.95;
2245 if (fabs(q-1) < 0.25 ||
2249 if (fabs(fieldCurvLimit) <
MINSTEP)
2255 if (fabs(distVsStep.first) < 8.E99) {
2256 stepToPlane = distVsStep.first + distVsStep.second;
2267 debugOut <<
" auto select direction";
2275 debugOut <<
" straight line approximation is fine.\n";
2279 if( plane.
isInActive(state7[0], state7[1], state7[2], state7[3], state7[4], state7[5]) ) {
2281 debugOut <<
" direction is pointing to active part of surface. \n";
2289 debugOut <<
" we are near the plane, but not pointing to the active area. make a big step! \n";
2299 debugOut <<
" invert Step according to propDir_ and make a big step. \n";
2306 static const RKStep defaultRKStep;
2307 RKSteps_.push_back( defaultRKStep );
2308 std::vector<RKStep>::iterator lastStep =
RKSteps_.end() - 1;
2309 lastStep->state7_ = state7;
2313 M1x7 state7_temp = {{ state7[0], state7[1], state7[2], state7[3], state7[4], state7[5], state7[6] }};
2320 lastStep->matStep_.materialProperties_,
2325 lastStep->matStep_.materialProperties_ = (lastStep - 1)->matStep_.materialProperties_;
2336 lastStep->matStep_.stepSize_ = finalStep;
2337 lastStep->limits_ = limits;
2340 debugOut <<
" --> Step to be used: " << finalStep <<
"\n";
2350 TVector3 retVal(lineDirection);
2352 double t = 1./(retVal.Mag2()) * ((point*retVal) - (linePoint*retVal));
2354 retVal += linePoint;
2367 bool fillExtrapSteps,
2370 bool stopAtBoundary,
2371 double maxStep)
const
2374 static const unsigned int maxNumIt(500);
2375 unsigned int numIt(0);
2377 double coveredDistance(0.);
2380 const TVector3 W(destPlane.
getNormal());
2381 M1x4 SU = {{W.X(), W.Y(), W.Z(), destPlane.
distance(0., 0., 0.)}};
2384 if (W*destPlane.
getO() < 0) {
2391 M1x7 startState7 = state7;
2396 debugOut <<
"\n============ RKTrackRep::Extrap loop nr. " << numIt <<
" ============\n";
2398 debugOut <<
"fillExtrapSteps " << fillExtrapSteps <<
"\n";
2401 if(++numIt > maxNumIt){
2402 Exception exc(
"RKTrackRep::Extrap ==> maximum number of iterations exceeded",__LINE__,__FILE__);
2408 for(
int i = 0; i < 7*7; ++i)
J_MMT_[i] = 0;
2409 for(
int i=0; i<7; ++i)
J_MMT_[8*i] = 1.;
2412 isAtBoundary =
false;
2415 bool checkJacProj =
false;
2419 M1x7 J_MMT_unprojected_lastRow = {{0, 0, 0, 0, 0, 0, 1}};
2421 if( !
RKutta(SU, destPlane, charge, mass, state7, &
J_MMT_, &J_MMT_unprojected_lastRow,
2423 limits_, onlyOneStep, !fillExtrapSteps) ) {
2424 Exception exc(
"RKTrackRep::Extrap ==> Runge Kutta propagation failed",__LINE__,__FILE__);
2431 isAtBoundary =
true;
2436 for (std::vector<RKStep>::iterator it =
RKSteps_.begin(); it !=
RKSteps_.end(); ++it){
2437 debugOut <<
"stepSize = " << it->matStep_.stepSize_ <<
"\t";
2438 it->matStep_.materialProperties_.Print();
2446 if(fillExtrapSteps) {
2457 fabs(charge/state7[6]),
2464 debugOut <<
"momLoss: " << momLoss <<
" GeV; relative: " << momLoss/fabs(charge/state7[6])
2465 <<
"; coveredDistance = " << coveredDistance <<
"\n";
2473 if(fabs(state7[6])>1.E-10) {
2476 debugOut <<
"correct state7 with dx/dqop, dy/dqop ...\n";
2479 dqop = charge/(fabs(charge/state7[6])-momLoss) - state7[6];
2486 if (checkJacProj && fabs(coveredDistance) >
MINSTEP) {
2487 M1x3 state7_correction_unprojected = {{0, 0, 0}};
2488 for (
unsigned int i=0; i<3; ++i) {
2489 state7_correction_unprojected[i] = 0.5 * dqop * J_MMT_unprojected_lastRow[i];
2494 M1x3 state7_correction_projected = {{0, 0, 0}};
2495 for (
unsigned int i=0; i<3; ++i) {
2496 state7_correction_projected[i] = 0.5 * dqop *
J_MMT_[6*7 + i];
2502 M1x3 delta_state = {{0, 0, 0}};
2503 for (
unsigned int i=0; i<3; ++i) {
2504 delta_state[i] = state7_correction_unprojected[i] - state7_correction_projected[i];
2507 double Dist( sqrt(delta_state[0]*delta_state[0]
2508 + delta_state[1]*delta_state[1]
2509 + delta_state[2]*delta_state[2] ) );
2512 if (delta_state[0]*state7[3] + delta_state[1]*state7[4] + delta_state[2]*state7[5] > 0)
2515 double correctionFactor( 1. + Dist / coveredDistance );
2516 flightTime *= correctionFactor;
2517 momLoss *= correctionFactor;
2518 coveredDistance = coveredDistance + Dist;
2521 debugOut <<
"correctionFactor-1 = " << correctionFactor-1. <<
"; Dist = " << Dist <<
"\n";
2522 debugOut <<
"corrected momLoss: " << momLoss <<
" GeV; relative: " << momLoss/fabs(charge/state7[6])
2523 <<
"; corrected coveredDistance = " << coveredDistance <<
"\n";
2528 double qop( charge/(fabs(charge/state7[6])-momLoss) );
2529 dqop = qop - state7[6];
2532 for (
unsigned int i=0; i<6; ++i) {
2533 state7[i] += 0.5 * dqop *
J_MMT_[6*7 + i];
2536 double norm( 1. / sqrt(state7[3]*state7[3] + state7[4]*state7[4] + state7[5]*state7[5]) );
2537 for (
unsigned int i=3; i<6; ++i)
2545 if (fillExtrapSteps) {
2548 std::vector<ExtrapStep>::iterator lastStep =
ExtrapSteps_.end() - 1;
2551 lastStep->jac7_ =
J_MMT_;
2553 if( checkJacProj ==
true ){
2558 debugOut <<
"7D noise projected onto plane: \n";
2579 if (stopAtBoundary) {
2581 debugOut <<
"stopAtBoundary -> break; \n ";
2588 debugOut <<
"onlyOneStep -> break; \n ";
2596 debugOut <<
"arrived at destPlane with a distance of " << destPlane.
distance(state7[0], state7[1], state7[2]) <<
" cm left. ";
2597 if (destPlane.
isInActive(state7[0], state7[1], state7[2], state7[3], state7[4], state7[5]))
2598 debugOut <<
"In active area of destPlane. \n";
2600 debugOut <<
"NOT in active area of plane. \n";
2602 debugOut <<
" position: "; TVector3(state7[0], state7[1], state7[2]).Print();
2603 debugOut <<
" direction: "; TVector3(state7[3], state7[4], state7[5]).Print();
2610 if (fillExtrapSteps) {
2621 debugOut <<
"final covariance matrix after Extrap: "; cov->Print();
2626 return coveredDistance;
2632 if (state.
getRep() !=
this){
2633 Exception exc(
"RKTrackRep::checkCache ==> state is defined wrt. another TrackRep",__LINE__,__FILE__);
2638 if (dynamic_cast<const MeasurementOnPlane*>(&state) != NULL) {
2639 Exception exc(
"RKTrackRep::checkCache - cannot extrapolate MeasurementOnPlane",__LINE__,__FILE__);
2660 double firstStep(0);
2661 for (
unsigned int i=0; i<
RKSteps_.size(); ++i) {
2663 firstStep =
RKSteps_.at(0).matStep_.stepSize_;
2666 if (
RKSteps_.at(i).matStep_.stepSize_ * firstStep < 0) {
2667 if (
RKSteps_.at(i-1).matStep_.materialProperties_ ==
RKSteps_.at(i).matStep_.materialProperties_) {
2675 debugOut <<
"RKTrackRep::checkCache: use cached material and step values.\n";
2681 debugOut <<
"RKTrackRep::checkCache: can NOT use cached material and step values.\n";
2683 if (plane != NULL) {
2685 debugOut <<
"state.getPlane() != lastStartState_.getPlane()\n";
2689 debugOut <<
"state.getState() != lastStartState_.getState()\n";
2708 return fabs(1/state7[6]);
2713 if (dynamic_cast<const RKTrackRep*>(other) == NULL)
2728 void RKTrackRep::Streamer(TBuffer &R__b)
2733 typedef ::genfit::RKTrackRep thisClass;
2735 if (R__b.IsReading()) {
2736 ::genfit::AbsTrackRep::Streamer(R__b);
2737 Version_t R__v = R__b.ReadVersion(&R__s, &R__c);
if (R__v) { }
2738 R__b.CheckByteCount(R__s, R__c, thisClass::IsA());
2742 ::genfit::AbsTrackRep::Streamer(R__b);
2743 R__c = R__b.WriteVersion(thisClass::IsA(), kTRUE);
2744 R__b.SetByteCount(R__c, kTRUE);
void transformPM7(const MeasuredStateOnPlane &state, M7x7 &out7x7) const
void calcForwardJacobianAndNoise(const M1x7 &startState7, const DetPlane &startPlane, const M1x7 &destState7, const DetPlane &destPlane) const
Helper to store different limits on the stepsize for the RKTRackRep.
const AbsTrackRep * getRep() const
void stepper(const RKTrackRep *rep, M1x7 &state7, const double &mom, double &relMomLoss, const int &pdg, MaterialProperties ¤tMaterial, StepLimits &limits, bool varField=true)
Returns maximum length so that a specified momentum loss will not be exceeded.
virtual TVector3 getPos(const StateOnPlane &state) const
Get the cartesian position of a state.
void setNormal(const TVector3 &n)
M7x7 noiseProjection_
noise matrix of the last extrapolation
char propDir_
propagation direction (-1, 0, 1) -> (backward, auto, forward)
void setStatePlane(const TVectorD &state, const SharedPlanePtr &plane)
TVector3 pocaOnLine(const TVector3 &linePoint, const TVector3 &lineDirection, const TVector3 &point) const
boost::shared_ptr< genfit::DetPlane > SharedPlanePtr
Shared Pointer to a DetPlane.
double getLowestLimitSignedVal(double margin=1.E-3) const
Get the numerical value of the lowest limit, signed with stepSign_.
virtual void getPosMomCov(const MeasuredStateOnPlane &state, TVector3 &pos, TVector3 &mom, TMatrixDSym &cov) const
Translates MeasuredStateOnPlane into 3D position, momentum and 6x6 covariance.
double momMag(const M1x7 &state7) const
virtual double extrapolateToPlane(StateOnPlane &state, const SharedPlanePtr &plane, bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state to plane, and returns the extrapolation length and, via reference, the extrapolated state.
double RKPropagate(M1x7 &state7, M7x7 *jacobian, M1x3 &SA, double S, bool varField=true, bool calcOnlyLastRowOfJ=false) const
The actual Runge Kutta propagation.
bool RKutta(const M1x4 &SU, const DetPlane &plane, double charge, double mass, M1x7 &state7, M7x7 *jacobianT, M1x7 *J_MMT_unprojected_lastRow, double &coveredDistance, double &flightTime, bool &checkJacProj, M7x7 &noiseProjection, StepLimits &limits, bool onlyOneStep=false, bool calcOnlyLastRowOfJ=false) const
Propagates the particle through the magnetic field.
virtual double extrapolateBy(StateOnPlane &state, double step, bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state by step (cm) and returns the extrapolation length and, via reference...
double getMass(const StateOnPlane &state) const
Get tha particle mass in GeV/c^2.
void checkCache(const StateOnPlane &state, const SharedPlanePtr *plane) const
const TVectorD & getState() const
virtual double extrapolateToSphere(StateOnPlane &state, double radius, const TVector3 &point=TVector3(0., 0., 0.), bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state to the sphere surface, and returns the extrapolation length and...
virtual void setPosMom(StateOnPlane &state, const TVector3 &pos, const TVector3 &mom) const
Set position and momentum of state.
double distance(const TVector3 &point) const
absolute distance from a point to the plane
void transformM6P(const M6x6 &in6x6, const M1x7 &state7, MeasuredStateOnPlane &state) const
const TVectorD & getAuxInfo() const
void calcJ_pM_5x7(M5x7 &J_pM, const TVector3 &U, const TVector3 &V, const M1x3 &pTilde, double spu) const
int RKStepsFXStart_
RungeKutta steps made in the last extrapolation.
void setPlane(const SharedPlanePtr &plane)
virtual double getRadiationLenght() const
Get the accumulated X/X0 (path / radiation length) of the material crossed in the last extrapolation...
TMatrixD fJacobian_
steps made in Extrap during last extrapolation
virtual void setPosMomErr(MeasuredStateOnPlane &state, const TVector3 &pos, const TVector3 &mom, const TVector3 &posErr, const TVector3 &momErr) const
Set position and momentum and error of state.
double getLimitSigned(StepLimitType type) const
TVector3 getFieldVal(const TVector3 &position)
This does NOT use the cache!
void setSpu(StateOnPlane &state, double spu) const
const TVector3 & getV() const
void setLimit(StepLimitType type, double value)
absolute of value will be taken! If limit is already lower, it will be set to value anyway...
void Print(const Option_t *="") const
void setRep(const AbsTrackRep *rep)
std::pair< StepLimitType, double > getLowestLimit(double margin=1.E-3) const
Get the lowest limit.
void setTime(StateOnPlane &state, double time) const
Set time at which the state was defined.
StateOnPlane lastEndState_
state where the last extrapolation has started
virtual double extrapolateToLine(StateOnPlane &state, const TVector3 &linePoint, const TVector3 &lineDirection, bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state to the POCA to a line, and returns the extrapolation length and...
virtual bool isSameType(const AbsTrackRep *other)
check if other is of same type (e.g. RKTrackRep).
static FieldManager * getInstance()
Get singleton instance.
StateOnPlane with additional covariance matrix.
virtual double getCharge(const StateOnPlane &state) const
Get the (fitted) charge of a state. This is not always equal the pdg charge (e.g. if the charge sign ...
virtual TVector3 getMom(const StateOnPlane &state) const
Get the cartesian momentum vector of a state.
virtual TMatrixDSym get6DCov(const MeasuredStateOnPlane &state) const
Get the 6D covariance.
unsigned int cachePos_
use cached RKSteps_ for extrapolation
void setFatal(bool b=true)
Set fatal flag.
virtual void getPosMom(const StateOnPlane &state, TVector3 &pos, TVector3 &mom) const
Get cartesian position and momentum vector of a state.
virtual void getBackwardJacobianAndNoise(TMatrixD &jacobian, TMatrixDSym &noise, TVectorD &deltaState) const
Get the jacobian and noise matrix of the last extrapolation if it would have been done in opposite di...
Abstract base class for a track representation.
virtual void setPosMomCov(MeasuredStateOnPlane &state, const TVector3 &pos, const TVector3 &mom, const TMatrixDSym &cov6x6) const
Set position, momentum and covariance of state.
double estimateStep(const M1x7 &state7, const M1x4 &SU, const DetPlane &plane, const double &charge, double &relMomLoss, StepLimits &limits) const
double getSpu(const StateOnPlane &state) const
void setU(const TVector3 &u)
virtual double extrapolateToCone(StateOnPlane &state, double radius, const TVector3 &linePoint=TVector3(0., 0., 0.), const TVector3 &lineDirection=TVector3(0., 0., 1.), bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state to the cone surface, and returns the extrapolation length and, via reference, the extrapolated state.
void getState7(const StateOnPlane &state, M1x7 &state7) const
double getTime(const StateOnPlane &state) const
Get the time corresponding to the StateOnPlane. Extrapolation.
Exception class for error handling in GENFIT (provides storage for diagnostic information) ...
void calcJ_Mp_7x5(M7x5 &J_Mp, const TVector3 &U, const TVector3 &V, const TVector3 &W, const M1x3 &A) const
void removeLimit(StepLimitType type)
std::vector< genfit::MatStep > getSteps() const
Get stepsizes and material properties of crossed materials of the last extrapolation.
StateOnPlane lastStartState_
void getState5(StateOnPlane &state, const M1x7 &state7) const
int pdgCode_
Particle code.
double getPDGCharge() const
Get the charge of the particle of the pdg code.
const TVector3 & getU() const
double Extrap(const DetPlane &startPlane, const DetPlane &destPlane, double charge, double mass, bool &isAtBoundary, M1x7 &state7, double &flightTime, bool fillExtrapSteps, TMatrixDSym *cov=nullptr, bool onlyOneStep=false, bool stopAtBoundary=false, double maxStep=1.E99) const
Handles propagation and material effects.
virtual double getMomVar(const MeasuredStateOnPlane &state) const
get the variance of the absolute value of the momentum .
virtual bool isSame(const AbsTrackRep *other)
check if other is of same type (e.g. RKTrackRep) and has same pdg code.
const SharedPlanePtr & getPlane() const
A state with arbitrary dimension defined in a DetPlane.
virtual double getMomMag(const StateOnPlane &state) const
get the magnitude of the momentum in GeV.
std::vector< RKStep > RKSteps_
state where the last extrapolation has ended
void setStepSign(char signedVal)
sets stepSign_ to sign of signedVal
virtual void getForwardJacobianAndNoise(TMatrixD &jacobian, TMatrixDSym &noise, TVectorD &deltaState) const
Get the jacobian and noise matrix of the last extrapolation.
virtual double extrapToPoint(StateOnPlane &state, const TVector3 &point, const TMatrixDSym *G=NULL, bool stopAtBoundary=false, bool calcJacobianNoise=false) const
const TVector3 & getO() const
bool isInActive(const TVector3 &point, const TVector3 &dir) const
intersect in the active area? C.f. AbsFinitePlane
static MaterialEffects * getInstance()
virtual double extrapolateToCylinder(StateOnPlane &state, double radius, const TVector3 &linePoint=TVector3(0., 0., 0.), const TVector3 &lineDirection=TVector3(0., 0., 1.), bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state to the cylinder surface, and returns the extrapolation length and...
double getLimit(StepLimitType type) const
Get limit of type. If that limit has not yet been set, return max double value.
const TMatrixDSym & getCov() const
TVector3 getNormal() const
void transformPM6(const MeasuredStateOnPlane &state, M6x6 &out6x6) const
double getLowestLimitVal(double margin=1.E-3) const
Get the unsigned numerical value of the lowest limit.
int getPDG() const
Get the pdg code.
std::vector< ExtrapStep > ExtrapSteps_
void transformM7P(const M7x7 &in7x7, const M1x7 &state7, MeasuredStateOnPlane &state) const
virtual void setChargeSign(StateOnPlane &state, double charge) const
Set the sign of the charge according to charge.
double effects(const std::vector< RKStep > &steps, int materialsFXStart, int materialsFXStop, const double &mom, const int &pdg, M7x7 *noise=nullptr)
Calculates energy loss in the traveled path, optional calculation of noise matrix.
Defines for I/O streams used for error and debug printing.