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(
nullptr);
104 bool fillExtrapSteps(
false);
105 if (dynamic_cast<MeasuredStateOnPlane*>(&state) !=
nullptr) {
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) !=
nullptr) {
155 fillExtrapSteps =
true;
157 else if (calcJacobianNoise)
158 fillExtrapSteps =
true;
161 double step(0.), lastStep(0.), maxStep(1.E99), angle(0), distToPoca(0), tracklength(0);
164 double flightTime = 0;
165 TVector3 dir(state7[3], state7[4], state7[5]);
166 TVector3 lastDir(0,0,0);
167 TVector3 poca, poca_onwire;
168 bool isAtBoundary(
false);
175 if(++iterations == maxIt) {
176 Exception exc(
"RKTrackRep::extrapolateToLine ==> extrapolation to line failed, maximum number of iterations reached",__LINE__,__FILE__);
184 step = this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false,
nullptr,
true, stopAtBoundary, maxStep);
187 dir.SetXYZ(state7[3], state7[4], state7[5]);
188 poca.SetXYZ(state7[0], state7[1], state7[2]);
189 poca_onwire =
pocaOnLine(linePoint, lineDirection, poca);
192 if (stopAtBoundary && isAtBoundary) {
193 plane->setON(dir, poca);
197 angle = fabs(dir.Angle((poca_onwire-poca))-TMath::PiOver2());
198 distToPoca = (poca_onwire-poca).Mag();
199 if (angle*distToPoca < 0.1*
MINSTEP)
break;
203 if (lastStep*
step < 0){
205 maxStep = 0.5*fabs(lastStep);
209 plane->setU(dir.Cross(lineDirection));
212 if (fillExtrapSteps) {
227 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";
237 const TVector3&
point,
238 const TMatrixDSym* G,
240 bool calcJacobianNoise)
const {
243 debugOut <<
"RKTrackRep::extrapolateToPoint()\n";
248 static const unsigned int maxIt(1000);
254 bool fillExtrapSteps(
false);
255 if (dynamic_cast<MeasuredStateOnPlane*>(&state) !=
nullptr) {
256 fillExtrapSteps =
true;
258 else if (calcJacobianNoise)
259 fillExtrapSteps =
true;
262 double step(0.), lastStep(0.), maxStep(1.E99), angle(0), distToPoca(0), tracklength(0);
263 TVector3 dir(state7[3], state7[4], state7[5]);
265 if(G->GetNrows() != 3) {
266 Exception exc(
"RKTrackRep::extrapolateToLine ==> G is not 3x3",__LINE__,__FILE__);
270 dir = TMatrix(*G) * dir;
272 TVector3 lastDir(0,0,0);
275 bool isAtBoundary(
false);
282 double flightTime = 0;
285 if(++iterations == maxIt) {
286 Exception exc(
"RKTrackRep::extrapolateToPoint ==> extrapolation to point failed, maximum number of iterations reached",__LINE__,__FILE__);
294 step = this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false,
nullptr,
true, stopAtBoundary, maxStep);
297 dir.SetXYZ(state7[3], state7[4], state7[5]);
299 dir = TMatrix(*G) * dir;
301 poca.SetXYZ(state7[0], state7[1], state7[2]);
304 if (stopAtBoundary && isAtBoundary) {
305 plane->setON(dir, poca);
309 angle = fabs(dir.Angle((point-poca))-TMath::PiOver2());
310 distToPoca = (point-poca).Mag();
311 if (angle*distToPoca < 0.1*
MINSTEP)
break;
315 if (lastStep*
step < 0){
321 maxStep = 0.5*fabs(lastStep);
325 plane->setNormal(dir);
328 if (fillExtrapSteps) {
344 debugOut <<
"RKTrackRep::extrapolateToPoint(): Reached POCA after " << iterations+1 <<
" iterations. Distance: " << (point-poca).Mag() <<
" cm. Angle deviation: " << dir.Angle((point-poca))-TMath::PiOver2() <<
" rad \n";
355 const TVector3& linePoint,
356 const TVector3& lineDirection,
358 bool calcJacobianNoise)
const {
361 debugOut <<
"RKTrackRep::extrapolateToCylinder()\n";
366 static const unsigned int maxIt(1000);
372 bool fillExtrapSteps(
false);
373 if (dynamic_cast<MeasuredStateOnPlane*>(&state) !=
nullptr) {
374 fillExtrapSteps =
true;
376 else if (calcJacobianNoise)
377 fillExtrapSteps =
true;
379 double tracklength(0.), maxStep(1.E99);
383 bool isAtBoundary(
false);
390 double flightTime = 0;
393 if(++iterations == maxIt) {
394 Exception exc(
"RKTrackRep::extrapolateToCylinder ==> maximum number of iterations reached",__LINE__,__FILE__);
399 pos.SetXYZ(state7[0], state7[1], state7[2]);
400 dir.SetXYZ(state7[3], state7[4], state7[5]);
403 TVector3 AO = (pos - linePoint);
404 TVector3 AOxAB = (AO.Cross(lineDirection));
405 TVector3 VxAB = (dir.Cross(lineDirection));
406 float ab2 = (lineDirection * lineDirection);
407 float a = (VxAB * VxAB);
408 float b = 2 * (VxAB * AOxAB);
409 float c = (AOxAB * AOxAB) - (radius*radius * ab2);
410 double arg = b*b - 4.*a*
c;
412 Exception exc(
"RKTrackRep::extrapolateToCylinder ==> cannot solve",__LINE__,__FILE__);
416 double term = sqrt(arg);
419 k1 = (-b + term)/(2.*a);
420 k2 = 2.*c/(-b + term);
423 k1 = 2.*c/(-b - term);
424 k2 = (-b - term)/(2.*a);
429 if (fabs(k2)<fabs(k))
433 debugOut <<
"RKTrackRep::extrapolateToCylinder(); k = " << k <<
"\n";
436 dest = pos + k * dir;
439 plane->setUV((dest-linePoint).Cross(lineDirection), lineDirection);
441 tracklength += this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false,
nullptr,
true, stopAtBoundary, maxStep);
444 if (stopAtBoundary && isAtBoundary) {
445 pos.SetXYZ(state7[0], state7[1], state7[2]);
446 dir.SetXYZ(state7[3], state7[4], state7[5]);
448 plane->setUV((pos-linePoint).Cross(lineDirection), lineDirection);
458 if (fillExtrapSteps) {
480 const TVector3& conePoint,
481 const TVector3& coneDirection,
483 bool calcJacobianNoise)
const {
486 debugOut <<
"RKTrackRep::extrapolateToCone()\n";
491 static const unsigned int maxIt(1000);
497 bool fillExtrapSteps(
false);
498 if (dynamic_cast<MeasuredStateOnPlane*>(&state) !=
nullptr) {
499 fillExtrapSteps =
true;
501 else if (calcJacobianNoise)
502 fillExtrapSteps =
true;
504 double tracklength(0.), maxStep(1.E99);
508 bool isAtBoundary(
false);
515 double flightTime = 0;
518 if(++iterations == maxIt) {
519 Exception exc(
"RKTrackRep::extrapolateToCone ==> maximum number of iterations reached",__LINE__,__FILE__);
524 pos.SetXYZ(state7[0], state7[1], state7[2]);
525 dir.SetXYZ(state7[3], state7[4], state7[5]);
532 TVector3 cDirection = coneDirection.Unit();
533 TVector3 Delta = (pos - conePoint);
534 double DirDelta = cDirection * Delta;
535 double Delta2 = Delta*Delta;
536 double UDir = dir * cDirection;
537 double UDelta = dir * Delta;
538 double U2 = dir * dir;
539 double cosAngle2 = cos(openingAngle)*cos(openingAngle);
540 double a = UDir*UDir - cosAngle2*U2;
541 double b = UDir*DirDelta - cosAngle2*UDelta;
542 double c = DirDelta*DirDelta - cosAngle2*Delta2;
544 double arg = b*b - a*
c;
546 Exception exc(
"RKTrackRep::extrapolateToCone ==> cannot solve",__LINE__,__FILE__);
553 double term = sqrt(arg);
555 k1 = (-b + term) / a;
556 k2 = (-b - term) / a;
560 if(fabs(k2) < fabs(k)) {
565 debugOut <<
"RKTrackRep::extrapolateToCone(); k = " << k <<
"\n";
568 dest = pos + k * dir;
573 plane->setUV((dest-conePoint).Cross(coneDirection), dest-conePoint);
575 tracklength += this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false,
nullptr,
true, stopAtBoundary, maxStep);
578 if (stopAtBoundary && isAtBoundary) {
579 pos.SetXYZ(state7[0], state7[1], state7[2]);
580 dir.SetXYZ(state7[3], state7[4], state7[5]);
582 plane->setUV((pos-conePoint).Cross(coneDirection), pos-conePoint);
592 if (fillExtrapSteps) {
614 const TVector3&
point,
616 bool calcJacobianNoise)
const {
619 debugOut <<
"RKTrackRep::extrapolateToSphere()\n";
624 static const unsigned int maxIt(1000);
630 bool fillExtrapSteps(
false);
631 if (dynamic_cast<MeasuredStateOnPlane*>(&state) !=
nullptr) {
632 fillExtrapSteps =
true;
634 else if (calcJacobianNoise)
635 fillExtrapSteps =
true;
637 double tracklength(0.), maxStep(1.E99);
641 bool isAtBoundary(
false);
648 double flightTime = 0;
651 if(++iterations == maxIt) {
652 Exception exc(
"RKTrackRep::extrapolateToSphere ==> maximum number of iterations reached",__LINE__,__FILE__);
657 pos.SetXYZ(state7[0], state7[1], state7[2]);
658 dir.SetXYZ(state7[3], state7[4], state7[5]);
661 TVector3 AO = (pos -
point);
662 double dirAO = dir * AO;
663 double arg = dirAO*dirAO - AO*AO + radius*radius;
665 Exception exc(
"RKTrackRep::extrapolateToSphere ==> cannot solve",__LINE__,__FILE__);
669 double term = sqrt(arg);
676 if (fabs(k2)<fabs(k))
680 debugOut <<
"RKTrackRep::extrapolateToSphere(); k = " << k <<
"\n";
683 dest = pos + k * dir;
685 plane->setON(dest, dest-point);
687 tracklength += this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false,
nullptr,
true, stopAtBoundary, maxStep);
690 if (stopAtBoundary && isAtBoundary) {
691 pos.SetXYZ(state7[0], state7[1], state7[2]);
692 dir.SetXYZ(state7[3], state7[4], state7[5]);
693 plane->setON(pos, pos-point);
703 if (fillExtrapSteps) {
726 bool calcJacobianNoise)
const {
729 debugOut <<
"RKTrackRep::extrapolateBy()\n";
734 static const unsigned int maxIt(1000);
740 bool fillExtrapSteps(
false);
741 if (dynamic_cast<MeasuredStateOnPlane*>(&state) !=
nullptr) {
742 fillExtrapSteps =
true;
744 else if (calcJacobianNoise)
745 fillExtrapSteps =
true;
747 double tracklength(0.);
751 bool isAtBoundary(
false);
758 double flightTime = 0;
761 if(++iterations == maxIt) {
762 Exception exc(
"RKTrackRep::extrapolateBy ==> maximum number of iterations reached",__LINE__,__FILE__);
767 pos.SetXYZ(state7[0], state7[1], state7[2]);
768 dir.SetXYZ(state7[3], state7[4], state7[5]);
770 dest = pos + 1.5*(step-tracklength) * dir;
772 plane->setON(dest, dir);
774 tracklength += this->
Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime,
false,
nullptr,
true, stopAtBoundary, (step-tracklength));
777 if (stopAtBoundary && isAtBoundary) {
778 pos.SetXYZ(state7[0], state7[1], state7[2]);
779 dir.SetXYZ(state7[3], state7[4], state7[5]);
780 plane->setON(pos, dir);
784 if (fabs(tracklength-step) <
MINSTEP) {
786 debugOut <<
"RKTrackRep::extrapolateBy(): reached after " << iterations <<
" iterations. \n";
788 pos.SetXYZ(state7[0], state7[1], state7[2]);
789 dir.SetXYZ(state7[3], state7[4], state7[5]);
790 plane->setON(pos, dir);
798 if (fillExtrapSteps) {
822 return TVector3(state7[0], state7[1], state7[2]);
827 M1x7 state7 = {{0, 0, 0, 0, 0, 0, 0}};
830 TVector3 mom(state7[3], state7[4], state7[5]);
837 M1x7 state7 = {{0, 0, 0, 0, 0, 0, 0}};
840 pos.SetXYZ(state7[0], state7[1], state7[2]);
841 mom.SetXYZ(state7[3], state7[4], state7[5]);
863 if (dynamic_cast<const MeasurementOnPlane*>(&state) !=
nullptr) {
864 Exception exc(
"RKTrackRep::getCharge - cannot get charge from MeasurementOnPlane",__LINE__,__FILE__);
872 if (state.
getState()(0) * pdgCharge < 0)
889 if (dynamic_cast<const MeasurementOnPlane*>(&state) !=
nullptr) {
890 Exception exc(
"RKTrackRep::getMomVar - cannot get momVar from MeasurementOnPlane",__LINE__,__FILE__);
909 if (dynamic_cast<const MeasurementOnPlane*>(&state) !=
nullptr) {
910 Exception exc(
"RKTrackRep::getSpu - cannot get spu from MeasurementOnPlane",__LINE__,__FILE__);
916 if (auxInfo.GetNrows() == 2
917 || auxInfo.GetNrows() == 1)
926 if (auxInfo.GetNrows() == 2)
934 const M1x7& destState7,
const DetPlane& destPlane)
const {
937 debugOut <<
"RKTrackRep::calcForwardJacobianAndNoise " << std::endl;
941 Exception exc(
"RKTrackRep::calcForwardJacobianAndNoise ==> cache is empty. Extrapolation must run with a MeasuredStateOnPlane.",__LINE__,__FILE__);
946 TMatrixD jac(TMatrixD::kTransposed, TMatrixD(7, 7,
ExtrapSteps_.back().jac7_.begin()));
947 TMatrixDSym noise(7,
ExtrapSteps_.back().noise7_.begin());
949 noise += TMatrixDSym(7,
ExtrapSteps_[
i].noise7_.begin()).Similarity(jac);
950 jac *= TMatrixD(TMatrixD::kTransposed, TMatrixD(7, 7,
ExtrapSteps_[
i].jac7_.begin()));
954 M1x3 pTilde = {{startState7[3], startState7[4], startState7[5]}};
955 const TVector3& normal = startPlane.
getNormal();
956 double pTildeW = pTilde[0] * normal.X() + pTilde[1] * normal.Y() + pTilde[2] * normal.Z();
957 double spu = pTildeW > 0 ? 1 : -1;
958 for (
unsigned int i=0;
i<3; ++
i) {
959 pTilde[
i] *= spu/pTildeW;
981 jacobian.ResizeTo(5,5);
988 deltaState.ResizeTo(5);
992 deltaState *= jacobian;
998 debugOut <<
"delta state : "; deltaState.Print();
1006 debugOut <<
"RKTrackRep::getBackwardJacobianAndNoise " << std::endl;
1010 Exception exc(
"RKTrackRep::getBackwardJacobianAndNoise ==> cache is empty. Extrapolation must run with a MeasuredStateOnPlane.",__LINE__,__FILE__);
1014 jacobian.ResizeTo(5,5);
1016 if (!useInvertFast) {
1017 bool status = TDecompLU::InvertLU(jacobian, 0.0);
1019 Exception e(
"cannot invert matrix, status = 0", __LINE__,__FILE__);
1025 jacobian.InvertFast(&det);
1027 Exception e(
"cannot invert matrix, status = 0", __LINE__,__FILE__);
1033 noise.ResizeTo(5,5);
1035 noise.Similarity(jacobian);
1038 deltaState.ResizeTo(5);
1048 Exception exc(
"RKTrackRep::getSteps ==> cache is empty.",__LINE__,__FILE__);
1052 std::vector<MatStep> retVal;
1055 for (
unsigned int i = 0;
i<
RKSteps_.size(); ++
i) {
1068 Exception exc(
"RKTrackRep::getRadiationLenght ==> cache is empty.",__LINE__,__FILE__);
1074 for (
unsigned int i = 0;
i<
RKSteps_.size(); ++
i) {
1075 radLen +=
RKSteps_.at(
i).matStep_.stepSize_ /
RKSteps_.at(
i).matStep_.material_.radiationLength;
1085 if (state.
getRep() !=
this){
1086 Exception exc(
"RKTrackRep::setPosMom ==> state is defined wrt. another TrackRep",__LINE__,__FILE__);
1090 if (dynamic_cast<MeasurementOnPlane*>(&state) !=
nullptr) {
1091 Exception exc(
"RKTrackRep::setPosMom - cannot set pos/mom of a MeasurementOnPlane",__LINE__,__FILE__);
1096 if (mom.Mag2() == 0) {
1097 Exception exc(
"RKTrackRep::setPosMom - momentum is 0",__LINE__,__FILE__);
1104 if (auxInfo.GetNrows() != 2) {
1105 bool alreadySet = auxInfo.GetNrows() == 1;
1106 auxInfo.ResizeTo(2);
1115 state7[0] = pos.X();
1116 state7[1] = pos.Y();
1117 state7[2] = pos.Z();
1119 state7[3] = mom.X();
1120 state7[4] = mom.Y();
1121 state7[5] = mom.Z();
1124 double norm = 1. / sqrt(state7[3]*state7[3] + state7[4]*state7[4] + state7[5]*state7[5]);
1125 for (
unsigned int i=3;
i<6; ++
i)
1139 TVectorD& state5(state.
getState());
1154 if (state6.GetNrows()!=6){
1155 Exception exc(
"RKTrackRep::setPosMom ==> state has to be 6d (x, y, z, px, py, pz)",__LINE__,__FILE__);
1158 setPosMom(state, TVector3(state6(0), state6(1), state6(2)), TVector3(state6(3), state6(4), state6(5)));
1168 const TVector3& U(state.
getPlane()->getU());
1169 const TVector3& V(state.
getPlane()->getV());
1170 TVector3 W(state.
getPlane()->getNormal());
1172 double pw = mom * W;
1173 double pu = mom * U;
1174 double pv = mom * V;
1178 cov(0,0) = pow(
getCharge(state), 2) / pow(mom.Mag(), 6) *
1179 (mom.X()*mom.X() * momErr.X()*momErr.X()+
1180 mom.Y()*mom.Y() * momErr.Y()*momErr.Y()+
1181 mom.Z()*mom.Z() * momErr.Z()*momErr.Z());
1183 cov(1,1) = pow((U.X()/pw - W.X()*pu/(pw*pw)),2.) * momErr.X()*momErr.X() +
1184 pow((U.Y()/pw - W.Y()*pu/(pw*pw)),2.) * momErr.Y()*momErr.Y() +
1185 pow((U.Z()/pw - W.Z()*pu/(pw*pw)),2.) * momErr.Z()*momErr.Z();
1187 cov(2,2) = pow((V.X()/pw - W.X()*pv/(pw*pw)),2.) * momErr.X()*momErr.X() +
1188 pow((V.Y()/pw - W.Y()*pv/(pw*pw)),2.) * momErr.Y()*momErr.Y() +
1189 pow((V.Z()/pw - W.Z()*pv/(pw*pw)),2.) * momErr.Z()*momErr.Z();
1191 cov(3,3) = posErr.X()*posErr.X() * U.X()*U.X() +
1192 posErr.Y()*posErr.Y() * U.Y()*U.Y() +
1193 posErr.Z()*posErr.Z() * U.Z()*U.Z();
1195 cov(4,4) = posErr.X()*posErr.X() * V.X()*V.X() +
1196 posErr.Y()*posErr.Y() * V.Y()*V.Y() +
1197 posErr.Z()*posErr.Z() * V.Z()*V.Z();
1206 if (cov6x6.GetNcols()!=6 || cov6x6.GetNrows()!=6){
1207 Exception exc(
"RKTrackRep::setPosMomCov ==> cov has to be 6x6 (x, y, z, px, py, pz)",__LINE__,__FILE__);
1216 const M6x6& cov6x6_( *((
M6x6*) cov6x6.GetMatrixArray()) );
1224 if (state6.GetNrows()!=6){
1225 Exception exc(
"RKTrackRep::setPosMomCov ==> state has to be 6d (x, y, z, px, py, pz)",__LINE__,__FILE__);
1229 if (cov6x6.GetNcols()!=6 || cov6x6.GetNrows()!=6){
1230 Exception exc(
"RKTrackRep::setPosMomCov ==> cov has to be 6x6 (x, y, z, px, py, pz)",__LINE__,__FILE__);
1234 TVector3
pos(state6(0), state6(1), state6(2));
1235 TVector3 mom(state6(3), state6(4), state6(5));
1241 const M6x6& cov6x6_( *((
M6x6*) cov6x6.GetMatrixArray()) );
1250 if (dynamic_cast<MeasurementOnPlane*>(&state) !=
nullptr) {
1251 Exception exc(
"RKTrackRep::setChargeSign - cannot set charge of a MeasurementOnPlane",__LINE__,__FILE__);
1256 if (state.
getState()(0) * charge < 0) {
1279 bool calcOnlyLastRowOfJ)
const {
1294 static const double EC ( 0.000149896229 );
1295 static const double P3 ( 1./3. );
1296 static const double DLT ( .0002 );
1300 double S3(0), S4(0), PS2(0);
1301 M1x3 H0 = {{0.,0.,0.}}, H1 = {{0.,0.,0.}}, H2 = {{0.,0.,0.}};
1302 M1x3 r = {{0.,0.,0.}};
1304 double A0(0), A1(0), A2(0), A3(0), A4(0), A5(0), A6(0);
1305 double B0(0), B1(0), B2(0), B3(0), B4(0), B5(0), B6(0);
1306 double C0(0), C1(0), C2(0), C3(0), C4(0), C5(0), C6(0);
1313 PS2 = state7[6]*EC *
S;
1316 r[0] = R[0]; r[1] = R[1]; r[2]=R[2];
1318 H0[0] *= PS2; H0[1] *= PS2; H0[2] *= PS2;
1319 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];
1320 A2 = A[0]+A0 ; B2 = A[1]+B0 ; C2 = A[2]+C0 ;
1321 A1 = A2+A[0] ; B1 = B2+A[1] ; C1 = C2+A[2] ;
1325 r[0] += A1*S4; r[1] += B1*S4; r[2] += C1*S4;
1327 H1[0] *= PS2; H1[1] *= PS2; H1[2] *= PS2;
1330 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];
1331 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];
1332 A5 = A4-A[0]+A4 ; B5 = B4-A[1]+B4 ; C5 = C4-A[2]+C4 ;
1336 r[0]=R[0]+S*A4; r[1]=R[1]+S*B4; r[2]=R[2]+S*C4;
1338 H2[0] *= PS2; H2[1] *= PS2; H2[2] *= PS2;
1341 A6 = B5*H2[2]-C5*H2[1]; B6 = C5*H2[0]-A5*H2[2]; C6 = A5*H2[1]-B5*H2[0];
1347 if(jacobianT !=
nullptr){
1357 M7x7&
J = *jacobianT;
1359 double dA0(0), dA2(0), dA3(0), dA4(0), dA5(0), dA6(0);
1360 double dB0(0), dB2(0), dB3(0), dB4(0), dB5(0), dB6(0);
1361 double dC0(0), dC2(0), dC3(0), dC4(0), dC5(0), dC6(0);
1365 if (!calcOnlyLastRowOfJ) {
1369 J(0, 0) = 1;
J(1, 1) = 1;
J(2, 2) = 1;
1375 for(
int i=start;
i<6; ++
i) {
1378 dA0 = H0[2]*
J(
i, 4)-H0[1]*
J(
i, 5);
1379 dB0 = H0[0]*
J(
i, 5)-H0[2]*
J(
i, 3);
1380 dC0 = H0[1]*
J(
i, 3)-H0[0]*
J(
i, 4);
1387 dA3 =
J(
i, 3)+dB2*H1[2]-dC2*H1[1];
1388 dB3 =
J(
i, 4)+dC2*H1[0]-dA2*H1[2];
1389 dC3 =
J(
i, 5)+dA2*H1[1]-dB2*H1[0];
1391 dA4 =
J(
i, 3)+dB3*H1[2]-dC3*H1[1];
1392 dB4 =
J(
i, 4)+dC3*H1[0]-dA3*H1[2];
1393 dC4 =
J(
i, 5)+dA3*H1[1]-dB3*H1[0];
1396 dA5 = dA4+dA4-
J(
i, 3);
1397 dB5 = dB4+dB4-
J(
i, 4);
1398 dC5 = dC4+dC4-
J(
i, 5);
1400 dA6 = dB5*H2[2]-dC5*H2[1];
1401 dB6 = dC5*H2[0]-dA5*H2[2];
1402 dC6 = dA5*H2[1]-dB5*H2[0];
1405 J(
i, 0) += (dA2+dA3+dA4)*S3;
J(
i, 3) = ((dA0+2.*dA3)+(dA5+dA6))*P3;
1406 J(
i, 1) += (dB2+dB3+dB4)*S3;
J(
i, 4) = ((dB0+2.*dB3)+(dB5+dB6))*P3;
1407 J(
i, 2) += (dC2+dC3+dC4)*S3;
J(
i, 5) = ((dC0+2.*dC3)+(dC5+dC6))*P3;
1412 J(6, 3) *= state7[6];
J(6, 4) *= state7[6];
J(6, 5) *= state7[6];
1415 dA0 = H0[2]*
J(6, 4)-H0[1]*
J(6, 5) + A0;
1416 dB0 = H0[0]*
J(6, 5)-H0[2]*
J(6, 3) + B0;
1417 dC0 = H0[1]*
J(6, 3)-H0[0]*
J(6, 4) + C0;
1424 dA3 =
J(6, 3)+dB2*H1[2]-dC2*H1[1] + (A3-A[0]);
1425 dB3 =
J(6, 4)+dC2*H1[0]-dA2*H1[2] + (B3-A[1]);
1426 dC3 =
J(6, 5)+dA2*H1[1]-dB2*H1[0] + (C3-A[2]);
1428 dA4 =
J(6, 3)+dB3*H1[2]-dC3*H1[1] + (A4-A[0]);
1429 dB4 =
J(6, 4)+dC3*H1[0]-dA3*H1[2] + (B4-A[1]);
1430 dC4 =
J(6, 5)+dA3*H1[1]-dB3*H1[0] + (C4-A[2]);
1433 dA5 = dA4+dA4-
J(6, 3);
1434 dB5 = dB4+dB4-
J(6, 4);
1435 dC5 = dC4+dC4-
J(6, 5);
1437 dA6 = dB5*H2[2]-dC5*H2[1] + A6;
1438 dB6 = dC5*H2[0]-dA5*H2[2] + B6;
1439 dC6 = dA5*H2[1]-dB5*H2[0] + C6;
1442 J(6, 0) += (dA2+dA3+dA4)*S3/state7[6];
J(6, 3) = ((dA0+2.*dA3)+(dA5+dA6))*P3/state7[6];
1443 J(6, 1) += (dB2+dB3+dB4)*S3/state7[6];
J(6, 4) = ((dB0+2.*dB3)+(dB5+dB6))*P3/state7[6];
1444 J(6, 2) += (dC2+dC3+dC4)*S3/state7[6];
J(6, 5) = ((dC0+2.*dC3)+(dC5+dC6))*P3/state7[6];
1451 R[0] += (A2+A3+A4)*S3; A[0] += (SA[0]=((A0+2.*A3)+(A5+A6))*P3-A[0]);
1452 R[1] += (B2+B3+B4)*S3; A[1] += (SA[1]=((B0+2.*B3)+(B5+B6))*P3-A[1]);
1453 R[2] += (C2+C3+C4)*S3; A[2] += (SA[2]=((C0+2.*C3)+(C5+C6))*P3-A[2]);
1456 double CBA ( 1./sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]) );
1457 A[0] *= CBA; A[1] *= CBA; A[2] *= CBA;
1461 double EST ( fabs((A1+A6)-(A3+A4)) +
1462 fabs((B1+B6)-(B3+B4)) +
1463 fabs((C1+C6)-(C3+C4)) );
1465 debugOut <<
" RKTrackRep::RKPropagate. Step = "<< S <<
"; quality EST = " << EST <<
" \n";
1475 return pow(DLT/EST, 1./5.);
1483 for (
unsigned int i=0;
i<7; ++
i)
1501 if (dynamic_cast<const MeasurementOnPlane*>(&state) !=
nullptr) {
1502 Exception exc(
"RKTrackRep::getState7 - cannot get pos or mom from a MeasurementOnPlane",__LINE__,__FILE__);
1507 const TVector3& U(state.
getPlane()->getU());
1508 const TVector3& V(state.
getPlane()->getV());
1509 const TVector3&
O(state.
getPlane()->getO());
1510 const TVector3& W(state.
getPlane()->getNormal());
1513 const double* state5 = state.
getState().GetMatrixArray();
1515 double spu =
getSpu(state);
1517 state7[0] =
O.X() + state5[3]*U.X() + state5[4]*V.X();
1518 state7[1] =
O.Y() + state5[3]*U.Y() + state5[4]*V.Y();
1519 state7[2] =
O.Z() + state5[3]*U.Z() + state5[4]*V.Z();
1521 state7[3] = spu * (W.X() + state5[1]*U.X() + state5[2]*V.X());
1522 state7[4] = spu * (W.Y() + state5[1]*U.Y() + state5[2]*V.Y());
1523 state7[5] = spu * (W.Z() + state5[1]*U.Z() + state5[2]*V.Z());
1526 double norm = 1. / sqrt(state7[3]*state7[3] + state7[4]*state7[4] + state7[5]*state7[5]);
1527 for (
unsigned int i=3;
i<6; ++
i) state7[
i] *= norm;
1529 state7[6] = state5[0];
1539 const TVector3&
O(state.
getPlane()->getO());
1540 const TVector3& U(state.
getPlane()->getU());
1541 const TVector3& V(state.
getPlane()->getV());
1542 const TVector3& W(state.
getPlane()->getNormal());
1545 double AtW( state7[3]*W.X() + state7[4]*W.Y() + state7[5]*W.Z() );
1552 double* state5 = state.
getState().GetMatrixArray();
1554 state5[0] = state7[6];
1555 state5[1] = (state7[3]*U.X() + state7[4]*U.Y() + state7[5]*U.Z()) / AtW;
1556 state5[2] = (state7[3]*V.X() + state7[4]*V.Y() + state7[5]*V.Z()) / AtW;
1557 state5[3] = ((state7[0]-
O.X())*U.X() +
1558 (state7[1]-
O.Y())*U.Y() +
1559 (state7[2]-
O.Z())*U.Z());
1560 state5[4] = ((state7[0]-
O.X())*V.X() +
1561 (state7[1]-
O.Y())*V.Y() +
1562 (state7[2]-
O.Z())*V.Z());
1577 std::fill(J_pM.
begin(), J_pM.
end(), 0);
1579 const double pTildeMag = sqrt(pTilde[0]*pTilde[0] + pTilde[1]*pTilde[1] + pTilde[2]*pTilde[2]);
1580 const double pTildeMag2 = pTildeMag*pTildeMag;
1582 const double utpTildeOverpTildeMag2 = (U.X()*pTilde[0] + U.Y()*pTilde[1] + U.Z()*pTilde[2]) / pTildeMag2;
1583 const double vtpTildeOverpTildeMag2 = (V.X()*pTilde[0] + V.Y()*pTilde[1] + V.Z()*pTilde[2]) / pTildeMag2;
1598 double fact = spu / pTildeMag;
1599 J_pM[10] = fact * ( U.X() - pTilde[0]*utpTildeOverpTildeMag2 );
1600 J_pM[11] = fact * ( U.Y() - pTilde[1]*utpTildeOverpTildeMag2 );
1601 J_pM[12] = fact * ( U.Z() - pTilde[2]*utpTildeOverpTildeMag2 );
1603 J_pM[17] = fact * ( V.X() - pTilde[0]*vtpTildeOverpTildeMag2 );
1604 J_pM[18] = fact * ( V.Y() - pTilde[1]*vtpTildeOverpTildeMag2 );
1605 J_pM[19] = fact * ( V.Z() - pTilde[2]*vtpTildeOverpTildeMag2 );
1614 M6x6& out6x6)
const {
1617 const TVector3& U(state.
getPlane()->getU());
1618 const TVector3& V(state.
getPlane()->getV());
1619 const TVector3& W(state.
getPlane()->getNormal());
1621 const TVectorD& state5(state.
getState());
1622 double spu(
getSpu(state));
1625 pTilde[0] = spu * (W.X() + state5(1)*U.X() + state5(2)*V.X());
1626 pTilde[1] = spu * (W.Y() + state5(1)*U.Y() + state5(2)*V.Y());
1627 pTilde[2] = spu * (W.Z() + state5(1)*U.Z() + state5(2)*V.Z());
1629 const double pTildeMag = sqrt(pTilde[0]*pTilde[0] + pTilde[1]*pTilde[1] + pTilde[2]*pTilde[2]);
1630 const double pTildeMag2 = pTildeMag*pTildeMag;
1632 const double utpTildeOverpTildeMag2 = (U.X()*pTilde[0] + U.Y()*pTilde[1] + U.Z()*pTilde[2]) / pTildeMag2;
1633 const double vtpTildeOverpTildeMag2 = (V.X()*pTilde[0] + V.Y()*pTilde[1] + V.Z()*pTilde[2]) / pTildeMag2;
1637 const double qop = state5(0);
1641 std::fill(J_pM_5x6.
begin(), J_pM_5x6.
end(), 0);
1644 double fact = -1. * p / (pTildeMag * qop);
1645 J_pM_5x6[3] = fact * pTilde[0];
1646 J_pM_5x6[4] = fact * pTilde[1];
1647 J_pM_5x6[5] = fact * pTilde[2];
1649 fact = p * spu / pTildeMag;
1650 J_pM_5x6[9] = fact * ( U.X() - pTilde[0]*utpTildeOverpTildeMag2 );
1651 J_pM_5x6[10] = fact * ( U.Y() - pTilde[1]*utpTildeOverpTildeMag2 );
1652 J_pM_5x6[11] = fact * ( U.Z() - pTilde[2]*utpTildeOverpTildeMag2 );
1654 J_pM_5x6[15] = fact * ( V.X() - pTilde[0]*vtpTildeOverpTildeMag2 );
1655 J_pM_5x6[16] = fact * ( V.Y() - pTilde[1]*vtpTildeOverpTildeMag2 );
1656 J_pM_5x6[17] = fact * ( V.Z() - pTilde[2]*vtpTildeOverpTildeMag2 );
1658 J_pM_5x6[18] = U.X();
1659 J_pM_5x6[19] = U.Y();
1660 J_pM_5x6[20] = U.Z();
1662 J_pM_5x6[24] = V.X();
1663 J_pM_5x6[25] = V.Y();
1664 J_pM_5x6[26] = V.Z();
1684 std::fill(J_Mp.
begin(), J_Mp.
end(), 0);
1686 const double AtU = A[0]*U.X() + A[1]*U.Y() + A[2]*U.Z();
1687 const double AtV = A[0]*V.X() + A[1]*V.Y() + A[2]*V.Z();
1688 const double AtW = A[0]*W.X() + A[1]*W.Y() + A[2]*W.Z();
1693 double fact = 1./(AtW*AtW);
1694 J_Mp[16] = fact * (U.X()*AtW - W.X()*AtU);
1695 J_Mp[21] = fact * (U.Y()*AtW - W.Y()*AtU);
1696 J_Mp[26] = fact * (U.Z()*AtW - W.Z()*AtU);
1698 J_Mp[17] = fact * (V.X()*AtW - W.X()*AtV);
1699 J_Mp[22] = fact * (V.Y()*AtW - W.Y()*AtV);
1700 J_Mp[27] = fact * (V.Z()*AtW - W.Z()*AtV);
1724 const TVector3& U(state.
getPlane()->getU());
1725 const TVector3& V(state.
getPlane()->getV());
1726 const TVector3& W(state.
getPlane()->getNormal());
1728 const double AtU = state7[3]*U.X() + state7[4]*U.Y() + state7[5]*U.Z();
1729 const double AtV = state7[3]*V.X() + state7[4]*V.Y() + state7[5]*V.Z();
1730 const double AtW = state7[3]*W.X() + state7[4]*W.Y() + state7[5]*W.Z();
1734 const double qop = state7[6];
1738 std::fill(J_Mp_6x5.
begin(), J_Mp_6x5.
end(), 0);
1741 J_Mp_6x5[3] = U.X();
1742 J_Mp_6x5[8] = U.Y();
1743 J_Mp_6x5[13] = U.Z();
1745 J_Mp_6x5[4] = V.X();
1746 J_Mp_6x5[9] = V.Y();
1747 J_Mp_6x5[14] = V.Z();
1749 double fact = (-1.) * qop /
p;
1750 J_Mp_6x5[15] = fact * state7[3];
1751 J_Mp_6x5[20] = fact * state7[4];
1752 J_Mp_6x5[25] = fact * state7[5];
1754 fact = 1./(p*AtW*AtW);
1755 J_Mp_6x5[16] = fact * (U.X()*AtW - W.X()*AtU);
1756 J_Mp_6x5[21] = fact * (U.Y()*AtW - W.Y()*AtU);
1757 J_Mp_6x5[26] = fact * (U.Z()*AtW - W.Z()*AtU);
1759 J_Mp_6x5[17] = fact * (V.X()*AtW - W.X()*AtV);
1760 J_Mp_6x5[22] = fact * (V.Y()*AtW - W.Y()*AtV);
1761 J_Mp_6x5[27] = fact * (V.Z()*AtW - W.Z()*AtV);
1807 M1x7* J_MMT_unprojected_lastRow,
1808 double& coveredDistance,
1811 M7x7& noiseProjection,
1814 bool calcOnlyLastRowOfJ)
const {
1817 static const double Wmax ( 3000. );
1818 static const double AngleMax ( 6.3 );
1819 static const double Pmin ( 4.
E-3 );
1820 static const unsigned int maxNumIt ( 1000 );
1824 M1x3 SA = {{0.,0.,0.}};
1826 double momentum ( fabs(charge/state7[6]) );
1827 double relMomLoss ( 0 );
1828 double deltaAngle ( 0. );
1830 double An(0),
S(0), Sl(0), CBA(0);
1834 debugOut <<
"RKTrackRep::RKutta \n";
1835 debugOut <<
"position: "; TVector3(R[0], R[1], R[2]).Print();
1836 debugOut <<
"direction: "; TVector3(A[0], A[1], A[2]).Print();
1837 debugOut <<
"momentum: " << momentum <<
" GeV\n";
1841 checkJacProj =
false;
1844 if(momentum < Pmin){
1845 std::ostringstream sstream;
1846 sstream <<
"RKTrackRep::RKutta ==> momentum too low: " << momentum*1000. <<
" MeV";
1847 Exception exc(sstream.str(),__LINE__,__FILE__);
1852 unsigned int counter(0);
1855 S =
estimateStep(state7, SU, plane, charge, relMomLoss, limits);
1860 while (fabs(
S) >=
MINSTEP || counter == 0) {
1862 if(++counter > maxNumIt){
1863 Exception exc(
"RKTrackRep::RKutta ==> maximum number of iterations exceeded",__LINE__,__FILE__);
1869 debugOut <<
"------ RKutta main loop nr. " << counter-1 <<
" ------\n";
1872 M1x3 ABefore = {{ A[0], A[1], A[2] }};
1873 RKPropagate(state7, jacobianT, SA,
S,
true, calcOnlyLastRowOfJ);
1876 coveredDistance +=
S;
1879 double beta = 1/hypot(1, mass*state7[6]/charge);
1880 flightTime +=
S / beta / 29.9792458;
1884 std::ostringstream sstream;
1885 sstream <<
"RKTrackRep::RKutta ==> Total extrapolation length is longer than length limit : " << Way <<
" cm !";
1886 Exception exc(sstream.str(),__LINE__,__FILE__);
1891 if (onlyOneStep)
return(
true);
1896 debugOut<<
" momLossExceeded -> return(true); \n";
1904 debugOut<<
" at boundary -> return(true); \n";
1916 S =
estimateStep(state7, SU, plane, charge, relMomLoss, limits);
1921 debugOut<<
" (at Plane && fabs(S) < MINSTEP) -> break and do linear extrapolation \n";
1928 debugOut<<
" (momLossExceeded && fabs(S) < MINSTEP) -> return(true), no linear extrapolation; \n";
1936 double arg = ABefore[0]*A[0] + ABefore[1]*A[1] + ABefore[2]*A[2];
1937 arg = arg > 1 ? 1 : arg;
1938 arg = arg < -1 ? -1 : arg;
1939 deltaAngle += acos(arg);
1940 if (fabs(deltaAngle) > AngleMax){
1941 std::ostringstream sstream;
1942 sstream <<
"RKTrackRep::RKutta ==> Do not get to an active plane! Already extrapolated " << deltaAngle * 180 / TMath::Pi() <<
"°.";
1943 Exception exc(sstream.str(),__LINE__,__FILE__);
1950 if (
S *
RKSteps_.at(counter-1).matStep_.stepSize_ < 0 &&
1951 RKSteps_.at(counter-1).matStep_.stepSize_*
RKSteps_.at(counter-2).matStep_.stepSize_ < 0 &&
1952 RKSteps_.at(counter-2).matStep_.stepSize_*
RKSteps_.at(counter-3).matStep_.stepSize_ < 0){
1953 Exception exc(
"RKTrackRep::RKutta ==> Do not get closer to plane!",__LINE__,__FILE__);
1967 if (fabs(Sl) > 0.001*
MINSTEP){
1969 debugOut <<
" RKutta - linear extrapolation to surface\n";
1974 SA[0]*=Sl; SA[1]*=Sl; SA[2]*=Sl;
1982 CBA = 1./sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]);
1983 A[0] *= CBA; A[1] *= CBA; A[2] *= CBA;
1985 R[0] += S*(A[0]-0.5*S*SA[0]);
1986 R[1] += S*(A[1]-0.5*S*SA[1]);
1987 R[2] += S*(A[2]-0.5*S*SA[2]);
1990 coveredDistance +=
S;
1994 double beta = 1/hypot(1, mass*state7[6]/charge);
1995 flightTime += S / beta / 29.9792458;
1998 debugOut <<
" RKutta - last stepsize too small -> can't do linear extrapolation! \n";
2004 if (jacobianT !=
nullptr) {
2015 if (checkJacProj &&
RKSteps_.size()>0){
2016 Exception exc(
"RKTrackRep::Extrap ==> covariance is projected onto destination plane again",__LINE__,__FILE__);
2023 debugOut <<
" Project Jacobian of extrapolation onto destination plane\n";
2025 An = A[0]*SU[0] + A[1]*SU[1] + A[2]*SU[2];
2026 An = (fabs(
An) > 1.E-7 ? 1./
An : 0);
2029 if (calcOnlyLastRowOfJ)
2032 double* jacPtr = jacobianT->
begin();
2034 for(
unsigned int j=42;
j<49;
j+=7) {
2035 (*J_MMT_unprojected_lastRow)[
j-42] = jacPtr[
j];
2039 norm = (jacPtr[
i]*SU[0] + jacPtr[i+1]*SU[1] + jacPtr[i+2]*SU[2]) *
An;
2040 jacPtr[
i] -= norm*A [0]; jacPtr[i+1] -= norm*A [1]; jacPtr[i+2] -= norm*A [2];
2041 jacPtr[i+3] -= norm*SA[0]; jacPtr[i+4] -= norm*SA[1]; jacPtr[i+5] -= norm*SA[2];
2043 checkJacProj =
true;
2051 if (!calcOnlyLastRowOfJ) {
2052 for (
int iRow = 0; iRow < 3; ++iRow) {
2053 for (
int iCol = 0; iCol < 3; ++iCol) {
2054 noiseProjection[iRow*7 + iCol] = (iRow == iCol) -
An * SU[iCol] * A[iRow];
2055 noiseProjection[(iRow + 3)*7 + iCol] = -
An * SU[iCol] * SA[iRow];
2109 debugOut <<
" RKTrackRep::estimateStep \n";
2110 debugOut <<
" position: "; TVector3(state7[0], state7[1], state7[2]).Print();
2111 debugOut <<
" direction: "; TVector3(state7[3], state7[4], state7[5]).Print();
2115 double Dist ( SU[3] - (state7[0]*SU[0] +
2118 double An ( state7[3]*SU[0] +
2123 if (fabs(An) > 1.
E-10)
2126 SLDist = Dist*1.E10;
2127 if (An<0) SLDist *= -1.;
2134 debugOut <<
" Distance to plane: " << Dist <<
"\n";
2135 debugOut <<
" SL distance to plane: " << SLDist <<
"\n";
2137 debugOut <<
" Direction is pointing towards surface.\n";
2139 debugOut <<
" Direction is pointing away from surface.\n";
2148 std::pair<double, double> distVsStep (9.E99, 9.E99);
2150 static const unsigned int maxNumIt = 10;
2151 unsigned int counter(0);
2153 while (fabs(fieldCurvLimit) >
MINSTEP) {
2155 if(++counter > maxNumIt){
2159 fieldCurvLimit *= 0.5;
2163 M1x7 state7_temp = state7;
2164 M1x3 SA = {{0, 0, 0}};
2166 double q (
RKPropagate(state7_temp,
nullptr, SA, fieldCurvLimit,
true) );
2168 debugOut <<
" maxStepArg = " << fieldCurvLimit <<
"; q = " << q <<
" \n";
2173 Dist = SU[3] - (state7_temp[0] * SU[0] +
2174 state7_temp[1] * SU[1] +
2175 state7_temp[2] * SU[2]);
2177 An = state7_temp[3] * SU[0] +
2178 state7_temp[4] * SU[1] +
2179 state7_temp[5] * SU[2];
2181 if (fabs(Dist/An) < fabs(distVsStep.first)) {
2182 distVsStep.first = Dist/
An;
2183 distVsStep.second = fieldCurvLimit;
2190 fieldCurvLimit *= 2;
2194 fieldCurvLimit *= q * 0.95;
2196 if (fabs(q-1) < 0.25 ||
2200 if (fabs(fieldCurvLimit) <
MINSTEP)
2206 if (fabs(distVsStep.first) < 8.E99) {
2207 stepToPlane = distVsStep.first + distVsStep.second;
2218 debugOut <<
" auto select direction";
2226 debugOut <<
" straight line approximation is fine.\n";
2230 if( plane.
isInActive(state7[0], state7[1], state7[2], state7[3], state7[4], state7[5]) ) {
2232 debugOut <<
" direction is pointing to active part of surface. \n";
2240 debugOut <<
" we are near the plane, but not pointing to the active area. make a big step! \n";
2250 debugOut <<
" invert Step according to propDir_ and make a big step. \n";
2257 static const RKStep defaultRKStep;
2258 RKSteps_.push_back( defaultRKStep );
2259 std::vector<RKStep>::iterator lastStep =
RKSteps_.end() - 1;
2260 lastStep->state7_ = state7;
2264 M1x7 state7_temp = {{ state7[0], state7[1], state7[2], state7[3], state7[4], state7[5], state7[6] }};
2271 lastStep->matStep_.material_,
2276 lastStep->matStep_.material_ = (lastStep - 1)->matStep_.material_;
2287 lastStep->matStep_.stepSize_ = finalStep;
2288 lastStep->limits_ = limits;
2291 debugOut <<
" --> Step to be used: " << finalStep <<
"\n";
2301 TVector3 retVal(lineDirection);
2303 double t = 1./(retVal.Mag2()) * ((point*retVal) - (linePoint*retVal));
2305 retVal += linePoint;
2318 bool fillExtrapSteps,
2321 bool stopAtBoundary,
2322 double maxStep)
const
2325 static const unsigned int maxNumIt(500);
2326 unsigned int numIt(0);
2328 double coveredDistance(0.);
2332 const TVector3 W(destPlane.
getNormal());
2333 M1x4 SU = {{W.X(), W.Y(), W.Z(), destPlane.
distance(0., 0., 0.)}};
2336 if (W*destPlane.
getO() < 0) {
2343 M1x7 startState7 = state7;
2348 debugOut <<
"\n============ RKTrackRep::Extrap loop nr. " << numIt <<
" ============\n";
2350 debugOut <<
"fillExtrapSteps " << fillExtrapSteps <<
"\n";
2353 if(++numIt > maxNumIt){
2354 Exception exc(
"RKTrackRep::Extrap ==> maximum number of iterations exceeded",__LINE__,__FILE__);
2363 M7x7* noise =
nullptr;
2364 isAtBoundary =
false;
2367 bool checkJacProj =
false;
2371 M1x7 J_MMT_unprojected_lastRow = {{0, 0, 0, 0, 0, 0, 1}};
2373 if( !
RKutta(SU, destPlane, charge, mass, state7, &
J_MMT_, &J_MMT_unprojected_lastRow,
2375 limits_, onlyOneStep, !fillExtrapSteps) ) {
2376 Exception exc(
"RKTrackRep::Extrap ==> Runge Kutta propagation failed",__LINE__,__FILE__);
2383 isAtBoundary =
true;
2389 debugOut <<
"stepSize = " <<
it->matStep_.stepSize_ <<
"\t";
2390 it->matStep_.material_.Print();
2398 if(fillExtrapSteps) {
2409 fabs(charge/state7[6]),
2416 debugOut <<
"momLoss: " << momLoss <<
" GeV; relative: " << momLoss/fabs(charge/state7[6])
2417 <<
"; coveredDistance = " << coveredDistance <<
"\n";
2418 if (
debugLvl_ > 1 && noise !=
nullptr) {
2425 if(fabs(state7[6])>1.E-10) {
2428 debugOut <<
"correct state7 with dx/dqop, dy/dqop ...\n";
2431 dqop = charge/(fabs(charge/state7[6])-momLoss) - state7[6];
2438 if (checkJacProj && fabs(coveredDistance) >
MINSTEP) {
2439 M1x3 state7_correction_unprojected = {{0, 0, 0}};
2440 for (
unsigned int i=0;
i<3; ++
i) {
2441 state7_correction_unprojected[
i] = 0.5 * dqop * J_MMT_unprojected_lastRow[
i];
2446 M1x3 state7_correction_projected = {{0, 0, 0}};
2447 for (
unsigned int i=0;
i<3; ++
i) {
2448 state7_correction_projected[
i] = 0.5 * dqop *
J_MMT_[6*7 +
i];
2454 M1x3 delta_state = {{0, 0, 0}};
2455 for (
unsigned int i=0;
i<3; ++
i) {
2456 delta_state[
i] = state7_correction_unprojected[
i] - state7_correction_projected[
i];
2459 double Dist( sqrt(delta_state[0]*delta_state[0]
2460 + delta_state[1]*delta_state[1]
2461 + delta_state[2]*delta_state[2] ) );
2464 if (delta_state[0]*state7[3] + delta_state[1]*state7[4] + delta_state[2]*state7[5] > 0)
2467 double correctionFactor( 1. + Dist / coveredDistance );
2468 flightTime *= correctionFactor;
2469 momLoss *= correctionFactor;
2470 coveredDistance = coveredDistance + Dist;
2473 debugOut <<
"correctionFactor-1 = " << correctionFactor-1. <<
"; Dist = " << Dist <<
"\n";
2474 debugOut <<
"corrected momLoss: " << momLoss <<
" GeV; relative: " << momLoss/fabs(charge/state7[6])
2475 <<
"; corrected coveredDistance = " << coveredDistance <<
"\n";
2480 double qop( charge/(fabs(charge/state7[6])-momLoss) );
2481 dqop = qop - state7[6];
2484 for (
unsigned int i=0;
i<6; ++
i) {
2485 state7[
i] += 0.5 * dqop *
J_MMT_[6*7 +
i];
2488 double norm( 1. / sqrt(state7[3]*state7[3] + state7[4]*state7[4] + state7[5]*state7[5]) );
2489 for (
unsigned int i=3;
i<6; ++
i)
2497 if (fillExtrapSteps) {
2500 std::vector<ExtrapStep>::iterator lastStep =
ExtrapSteps_.end() - 1;
2503 lastStep->jac7_ =
J_MMT_;
2505 if( checkJacProj ==
true ){
2510 debugOut <<
"7D noise projected onto plane: \n";
2531 if (stopAtBoundary and isAtBoundary) {
2533 debugOut <<
"stopAtBoundary -> break; \n ";
2540 debugOut <<
"onlyOneStep -> break; \n ";
2548 debugOut <<
"arrived at destPlane with a distance of " << destPlane.
distance(state7[0], state7[1], state7[2]) <<
" cm left. ";
2549 if (destPlane.
isInActive(state7[0], state7[1], state7[2], state7[3], state7[4], state7[5]))
2550 debugOut <<
"In active area of destPlane. \n";
2552 debugOut <<
"NOT in active area of plane. \n";
2554 debugOut <<
" position: "; TVector3(state7[0], state7[1], state7[2]).Print();
2555 debugOut <<
" direction: "; TVector3(state7[3], state7[4], state7[5]).Print();
2562 if (fillExtrapSteps) {
2566 if (cov !=
nullptr) {
2572 if (cov !=
nullptr) {
2573 debugOut <<
"final covariance matrix after Extrap: "; cov->Print();
2578 return coveredDistance;
2584 if (state.
getRep() !=
this){
2585 Exception exc(
"RKTrackRep::checkCache ==> state is defined wrt. another TrackRep",__LINE__,__FILE__);
2590 if (dynamic_cast<const MeasurementOnPlane*>(&state) !=
nullptr) {
2591 Exception exc(
"RKTrackRep::checkCache - cannot extrapolate MeasurementOnPlane",__LINE__,__FILE__);
2612 double firstStep(0);
2615 firstStep =
RKSteps_.at(0).matStep_.stepSize_;
2618 if (
RKSteps_.at(
i).matStep_.stepSize_ * firstStep < 0) {
2627 debugOut <<
"RKTrackRep::checkCache: use cached material and step values.\n";
2633 debugOut <<
"RKTrackRep::checkCache: can NOT use cached material and step values.\n";
2635 if (plane !=
nullptr) {
2637 debugOut <<
"state.getPlane() != lastStartState_.getPlane()\n";
2641 debugOut <<
"state.getState() != lastStartState_.getState()\n";
2660 return fabs(1/state7[6]);
2665 if (dynamic_cast<const RKTrackRep*>(other) ==
nullptr)
2680 void RKTrackRep::Streamer(TBuffer &R__b)
2685 typedef ::genfit::RKTrackRep thisClass;
2687 if (R__b.IsReading()) {
2688 ::genfit::AbsTrackRep::Streamer(R__b);
2689 Version_t R__v = R__b.ReadVersion(&R__s, &R__c);
if (R__v) { }
2690 R__b.CheckByteCount(R__s, R__c, thisClass::IsA());
2694 ::genfit::AbsTrackRep::Streamer(R__b);
2695 R__c = R__b.WriteVersion(thisClass::IsA(), kTRUE);
2696 R__b.SetByteCount(R__c, kTRUE);