115 auto locConstraints = Get_Constraints<DKinFitConstraint_Vertex>(locKinFitParticle);
116 auto locSetIterator = locConstraints.begin();
117 for(; locSetIterator != locConstraints.end(); ++locSetIterator)
119 auto locConstrainingParticles = (*locSetIterator)->Get_AllConstrainingParticles();
120 if(locConstrainingParticles.find(locKinFitParticle) != locConstrainingParticles.end())
128 auto locConstraints = Get_Constraints<DKinFitConstraint_Spacetime>(locKinFitParticle);
129 auto locSetIterator = locConstraints.begin();
130 for(; locSetIterator != locConstraints.end(); ++locSetIterator)
132 auto locConstrainedParticles = (*locSetIterator)->Get_AllConstrainingParticles();
133 if(locConstrainedParticles.find(locKinFitParticle) != locConstrainedParticles.end())
146 cout <<
"DKinFitter: Pre-fit constraint info:" << endl;
149 (*locConstraintIterator)->Print_ConstraintInfo();
160 cout <<
"DKinFitter: Cloned (Fit) constraint info:" << endl;
163 (*locConstraintIterator)->Print_ConstraintInfo();
170 auto locKinFitParticles = (*locConstraintIterator)->Get_AllParticles();
171 dKinFitParticles.insert(locKinFitParticles.begin(), locKinFitParticles.end());
174 auto locParticleIterator = locKinFitParticles.begin();
175 for(; locParticleIterator != locKinFitParticles.end(); ++locParticleIterator)
187 if(locVertexConstraint != NULL)
190 if(locNoConstrainParticles.find(*locParticleIterator) != locNoConstrainParticles.end())
195 auto locFromAllParticles = (*locParticleIterator)->Get_FromAllParticles();
196 dKinFitParticles.insert(locFromAllParticles.begin(), locFromAllParticles.end());
199 auto locDerivingParticleIterator = locFromAllParticles.begin();
200 for(; locDerivingParticleIterator != locFromAllParticles.end(); ++locDerivingParticleIterator)
207 auto locVertexConstraints = Get_Constraints<DKinFitConstraint_Vertex>();
208 auto locVertexIterator = locVertexConstraints.begin();
209 for(; locVertexIterator != locVertexConstraints.end(); ++locVertexIterator)
211 auto locAllConstraintParticles = (*locVertexIterator)->Get_AllParticles();
212 auto locNoConstrainParticles = (*locVertexIterator)->Get_NoConstrainParticles();
213 auto locParticleIterator = locNoConstrainParticles.begin();
214 for(; locParticleIterator != locNoConstrainParticles.end(); ++locParticleIterator)
216 auto locKinFitParticle = *locParticleIterator;
224 auto locFromFinalState = locKinFitParticle->Get_FromFinalState();
225 set<shared_ptr<DKinFitParticle>> locFinalStateParticlesAtVertex;
226 set_intersection(locAllConstraintParticles.begin(), locAllConstraintParticles.end(), locFromFinalState.begin(),
227 locFromFinalState.end(), inserter(locFinalStateParticlesAtVertex, locFinalStateParticlesAtVertex.begin()));
230 bool locIsProductionVertex =
false;
232 locIsProductionVertex = locFinalStateParticlesAtVertex.empty() ?
true :
false;
234 locIsProductionVertex = locFinalStateParticlesAtVertex.empty() ?
false :
true;
235 locKinFitParticle->Set_VertexP4AtProductionVertex(locIsProductionVertex);
242 for(locVertexIterator = locVertexConstraints.begin(); locVertexIterator != locVertexConstraints.end(); ++locVertexIterator)
243 (*locVertexIterator)->Set_CommonVertex((*locVertexIterator)->Get_InitVertexGuess());
246 auto locSpacetimeConstraints = Get_Constraints<DKinFitConstraint_Spacetime>();
247 auto locSpacetimeIterator = locSpacetimeConstraints.begin();
248 for(; locSpacetimeIterator != locSpacetimeConstraints.end(); ++locSpacetimeIterator)
249 (*locSpacetimeIterator)->Set_CommonTime((*locSpacetimeIterator)->Get_InitTimeGuess());
255 auto locParticle = *locParticleIterator;
256 if(!locParticle->Get_IsNeutralShowerFlag())
259 double locE = locParticle->Get_ShowerEnergy();
260 double locMass = locParticle->Get_Mass();
261 double locPMag =
sqrt(locE*locE - locMass*locMass);
262 TVector3 locMomentum = locParticle->Get_Position() - locParticle->Get_CommonVertex();
263 locMomentum.SetMag(locPMag);
264 locParticle->Set_Momentum(locMomentum);
268 auto locP4Constraints = Get_Constraints<DKinFitConstraint_P4>();
269 if(!locP4Constraints.empty())
271 auto locP4Constraint = *(locP4Constraints.begin());
272 auto locKinFitParticle = locP4Constraint->Get_DefinedParticle();
273 if(locKinFitParticle != NULL)
274 locKinFitParticle->Set_Momentum(locP4Constraint->Get_InitP3Guess());
280 auto locKinFitParticle = *locParticleIterator;
286 for(locVertexIterator = locVertexConstraints.begin(); locVertexIterator != locVertexConstraints.end(); ++locVertexIterator)
288 auto locFullConstrainParticles = (*locVertexIterator)->Get_FullConstrainParticles();
289 for(locParticleIterator = locFullConstrainParticles.begin(); locParticleIterator != locFullConstrainParticles.end(); ++locParticleIterator)
291 auto locParticle = *locParticleIterator;
292 TVector3 locMomentum = locParticle->Get_Momentum();
294 unsigned short int locVertexConstraintFlag = 0;
295 if(fabs(locMomentum.Pz()) > fabs(locMomentum.Px()))
296 locVertexConstraintFlag = (fabs(locMomentum.Pz()) > fabs(locMomentum.Py())) ? 1 : 2;
298 locVertexConstraintFlag = (fabs(locMomentum.Px()) > fabs(locMomentum.Py())) ? 3 : 2;
299 locParticle->Set_VertexConstraintFlag(locVertexConstraintFlag);
315 auto locKinFitParticle = *locParticleIterator;
320 bool locIsInP4Constraint = Get_IsInConstraint<DKinFitConstraint_P4>(locKinFitParticle);
321 bool locIsInMassConstraint = Get_IsInConstraint<DKinFitConstraint_Mass>(locKinFitParticle);
322 bool locIsInVertexConstraint = Get_IsInConstraint<DKinFitConstraint_Vertex>(locKinFitParticle);
323 bool locIsIndirectlyInVertexConstraint = Get_IsIndirectlyInConstraint<DKinFitConstraint_Vertex>(locKinFitParticle);
327 cout <<
"PID, pointer, is in p4/mass/vert/indirectv, accel = " << locKinFitParticle->Get_PID() <<
", " << locKinFitParticle <<
", " << locIsInP4Constraint <<
", " << locIsInMassConstraint <<
", " << locIsInVertexConstraint <<
", " << locIsIndirectlyInVertexConstraint <<
", " << locChargedBFieldFlag << endl;
328 if(!locKinFitParticle->Get_IsNeutralShowerFlag())
330 if(locIsInP4Constraint || locIsInMassConstraint ||
Get_IsConstrainingVertex(locKinFitParticle) || locIsIndirectlyInVertexConstraint)
337 if((locIsInP4Constraint || locIsInMassConstraint) && locIsInVertexConstraint)
348 auto locKinFitConstraint_P4 = std::dynamic_pointer_cast<
DKinFitConstraint_P4>(*locConstraintIterator);
349 if(locKinFitConstraint_P4 != NULL)
352 if(locKinFitConstraint_P4->Get_DefinedParticle() != NULL)
357 auto locKinFitConstraint_Mass = std::dynamic_pointer_cast<
DKinFitConstraint_Mass>(*locConstraintIterator);
358 if(locKinFitConstraint_Mass != NULL)
365 if(locKinFitConstraint_Vertex != NULL)
368 dNumF += 2*locKinFitConstraint_Vertex->Get_FullConstrainParticles().size();
373 if(locKinFitConstraint_Spacetime != NULL)
376 dNumF += 3*locKinFitConstraint_Spacetime->Get_FullConstrainParticles().size();
377 dNumF += locKinFitConstraint_Spacetime->Get_OnlyConstrainTimeParticles().size();
382 cout <<
"DKinFitter: Num measurables, unknowns, constraints = " <<
dNumEta <<
", " <<
dNumXi <<
", " <<
dNumF << endl;
387 if(
dF.GetNrows() !=
static_cast<int>(
dNumF))
413 if(
dY.GetNrows() !=
static_cast<int>(
dNumEta))
422 if(
dXi.GetNrows() !=
static_cast<int>(
dNumXi))
467 int locParamIndex = 0;
471 auto locKinFitParticle = *locParticleIterator;
477 TVector3 locMomentum = locKinFitParticle->Get_Momentum();
478 TVector3 locPosition = locKinFitParticle->Get_Position();
480 bool locIsInP4Constraint = Get_IsInConstraint<DKinFitConstraint_P4>(locKinFitParticle);
481 bool locIsInMassConstraint = Get_IsInConstraint<DKinFitConstraint_Mass>(locKinFitParticle);
482 bool locIsInVertexConstraint = Get_IsInConstraint<DKinFitConstraint_Vertex>(locKinFitParticle);
483 bool locIsIndirectlyInVertexConstraint = Get_IsIndirectlyInConstraint<DKinFitConstraint_Vertex>(locKinFitParticle);
486 if(!locKinFitParticle->Get_IsNeutralShowerFlag())
488 if(locIsInP4Constraint || locIsInMassConstraint ||
Get_IsConstrainingVertex(locKinFitParticle) || locIsIndirectlyInVertexConstraint)
490 locKinFitParticle->Set_PxParamIndex(locParamIndex);
491 dY(locParamIndex, 0) = locMomentum.Px();
492 dY(locParamIndex + 1, 0) = locMomentum.Py();
493 dY(locParamIndex + 2, 0) = locMomentum.Pz();
498 locKinFitParticle->Set_VxParamIndex(locParamIndex);
499 dY(locParamIndex, 0) = locPosition.Px();
500 dY(locParamIndex + 1, 0) = locPosition.Py();
501 dY(locParamIndex + 2, 0) = locPosition.Pz();
507 if((locIsInP4Constraint || locIsInMassConstraint) && locIsInVertexConstraint)
510 locKinFitParticle->Set_EParamIndex(locParamIndex);
511 dY(locParamIndex, 0) = locKinFitParticle->Get_ShowerEnergy();
514 locKinFitParticle->Set_VxParamIndex(locParamIndex);
515 dY(locParamIndex, 0) = locPosition.Px();
516 dY(locParamIndex + 1, 0) = locPosition.Py();
517 dY(locParamIndex + 2, 0) = locPosition.Pz();
524 locKinFitParticle->Set_TParamIndex(locParamIndex);
525 dY(locParamIndex, 0) = locKinFitParticle->Get_Time();
535 int locConstraintIndex = 0;
539 auto locKinFitConstraint_P4 = std::dynamic_pointer_cast<
DKinFitConstraint_P4>(*locConstraintIterator);
540 if(locKinFitConstraint_P4 != NULL)
542 locKinFitConstraint_P4->
Set_FIndex(locConstraintIndex);
543 locConstraintIndex += 4;
545 auto locDefinedParticle = locKinFitConstraint_P4->Get_DefinedParticle();
546 if(locDefinedParticle == NULL)
550 TVector3 locMomentum = locKinFitConstraint_P4->Get_InitP3Guess();
551 dXi(locParamIndex, 0) = locMomentum.Px();
552 dXi(locParamIndex + 1, 0) = locMomentum.Py();
553 dXi(locParamIndex + 2, 0) = locMomentum.Pz();
554 locDefinedParticle->Set_PxParamIndex(locParamIndex);
559 auto locKinFitConstraint_Mass = std::dynamic_pointer_cast<
DKinFitConstraint_Mass>(*locConstraintIterator);
560 if(locKinFitConstraint_Mass != NULL)
562 locKinFitConstraint_Mass->
Set_FIndex(locConstraintIndex);
563 locConstraintIndex += 1;
569 if((locKinFitConstraint_Vertex != NULL) && (locKinFitConstraint_Spacetime == NULL))
572 dXi(locParamIndex, 0) = locPosition.X();
573 dXi(locParamIndex + 1, 0) = locPosition.Y();
574 dXi(locParamIndex + 2, 0) = locPosition.Z();
575 locKinFitConstraint_Vertex->Set_CommonVxParamIndex(locParamIndex);
578 auto locFullConstrainParticles = locKinFitConstraint_Vertex->Get_FullConstrainParticles();
579 locParticleIterator = locFullConstrainParticles.begin();
580 for(; locParticleIterator != locFullConstrainParticles.end(); ++locParticleIterator)
582 locKinFitConstraint_Vertex->Set_FIndex(*locParticleIterator, locConstraintIndex);
583 locConstraintIndex += 2;
588 if(locKinFitConstraint_Spacetime != NULL)
590 TVector3 locPosition = locKinFitConstraint_Spacetime->Get_InitVertexGuess();
591 dXi(locParamIndex, 0) = locPosition.X();
592 dXi(locParamIndex + 1, 0) = locPosition.Y();
593 dXi(locParamIndex + 2, 0) = locPosition.Z();
594 dXi(locParamIndex + 3, 0) = locKinFitConstraint_Spacetime->Get_InitTimeGuess();
595 locKinFitConstraint_Spacetime->Set_CommonVxParamIndex(locParamIndex);
596 locKinFitConstraint_Spacetime->Set_CommonTParamIndex(locParamIndex + 3);
599 auto locFullConstrainParticles = locKinFitConstraint_Spacetime->Get_FullConstrainParticles();
600 locParticleIterator = locFullConstrainParticles.begin();
601 for(; locParticleIterator != locFullConstrainParticles.end(); ++locParticleIterator)
603 locKinFitConstraint_Spacetime->Set_FIndex(*locParticleIterator, locConstraintIndex);
604 locConstraintIndex += 3;
607 auto locOnlyConstrainTimeParticles = locKinFitConstraint_Spacetime->dOnlyConstrainTimeParticles;
608 locParticleIterator = locOnlyConstrainTimeParticles.begin();
609 for(; locParticleIterator != locOnlyConstrainTimeParticles.end(); ++locParticleIterator)
611 locKinFitConstraint_Spacetime->Set_FIndex(*locParticleIterator, locConstraintIndex);
612 ++locConstraintIndex;
623 auto locKinFitParticle = *locParticleIterator;
629 TVector3 locMomentum = locKinFitParticle->Get_Momentum();
630 TVector3 locPosition = locKinFitParticle->Get_Position();
631 const TMatrixFSym& locCovarianceMatrix = *(locKinFitParticle->Get_CovarianceMatrix().get());
633 int locPxParamIndex = locKinFitParticle->Get_PxParamIndex();
634 int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
635 int locTParamIndex = locKinFitParticle->Get_TParamIndex();
636 int locEParamIndex = locKinFitParticle->Get_EParamIndex();
638 int locCovMatrixEParamIndex = locKinFitParticle->Get_CovMatrixEParamIndex();
639 int locCovMatrixPxParamIndex = locKinFitParticle->Get_CovMatrixPxParamIndex();
640 int locCovMatrixVxParamIndex = locKinFitParticle->Get_CovMatrixVxParamIndex();
641 int locCovMatrixTParamIndex = locKinFitParticle->Get_CovMatrixTParamIndex();
644 if(locEParamIndex >= 0)
645 dVY(locEParamIndex, locEParamIndex) = locCovarianceMatrix(locCovMatrixEParamIndex, locCovMatrixEParamIndex);
646 if(locPxParamIndex >= 0)
648 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
650 for(
unsigned int loc_k = 0; loc_k < 3; ++loc_k)
651 dVY(locPxParamIndex + loc_j, locPxParamIndex + loc_k) = locCovarianceMatrix(loc_j + locCovMatrixPxParamIndex, loc_k + locCovMatrixPxParamIndex);
654 if(locVxParamIndex >= 0)
656 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
658 for(
unsigned int loc_k = 0; loc_k < 3; ++loc_k)
659 dVY(locVxParamIndex + loc_j, locVxParamIndex + loc_k) = locCovarianceMatrix(loc_j + locCovMatrixVxParamIndex, loc_k + locCovMatrixVxParamIndex);
662 if(locTParamIndex >= 0)
663 dVY(locTParamIndex, locTParamIndex) = locCovarianceMatrix(locCovMatrixTParamIndex, locCovMatrixTParamIndex);
666 if((locEParamIndex >= 0) && (locVxParamIndex >= 0))
668 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
670 dVY(locEParamIndex + 0, locVxParamIndex + loc_j) = locCovarianceMatrix(locCovMatrixEParamIndex + 0, locCovMatrixVxParamIndex + loc_j);
671 dVY(locVxParamIndex + loc_j, locEParamIndex + 0) = locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixEParamIndex + 0);
674 if((locEParamIndex >= 0) && (locTParamIndex >= 0))
676 dVY(locEParamIndex, locTParamIndex) = locCovarianceMatrix(locCovMatrixEParamIndex, locCovMatrixTParamIndex);
677 dVY(locTParamIndex, locEParamIndex) = locCovarianceMatrix(locCovMatrixTParamIndex, locCovMatrixEParamIndex);
679 if((locPxParamIndex >= 0) && (locVxParamIndex >= 0))
681 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
683 for(
unsigned int loc_k = 0; loc_k < 3; ++loc_k)
685 dVY(locPxParamIndex + loc_j, locVxParamIndex + loc_k) = locCovarianceMatrix(locCovMatrixPxParamIndex + loc_j, locCovMatrixVxParamIndex + loc_k);
686 dVY(locVxParamIndex + loc_k, locPxParamIndex + loc_j) = locCovarianceMatrix(locCovMatrixVxParamIndex + loc_k, locCovMatrixPxParamIndex + loc_j);
690 if((locPxParamIndex >= 0) && (locTParamIndex >= 0))
692 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
694 dVY(locPxParamIndex + loc_j, locTParamIndex + 0) = locCovarianceMatrix(locCovMatrixPxParamIndex + loc_j, locCovMatrixTParamIndex + 0);
695 dVY(locTParamIndex + 0, locPxParamIndex + loc_j) = locCovarianceMatrix(locCovMatrixTParamIndex + 0, locCovMatrixPxParamIndex + loc_j);
698 if((locVxParamIndex >= 0) && (locTParamIndex >= 0))
700 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
702 dVY(locVxParamIndex + loc_j, locTParamIndex + 0) = locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixTParamIndex + 0);
703 dVY(locTParamIndex + 0, locVxParamIndex + loc_j) = locCovarianceMatrix(locCovMatrixTParamIndex + 0, locCovMatrixVxParamIndex + loc_j);
710 cout <<
"DKinFitter: dEta: " << endl;
712 cout <<
"DKinFitter: dVY: " << endl;
714 cout <<
"DKinFitter: dXi: " << endl;
717 (*locParticleIterator)->Print_ParticleParams();
766 double locPreviousChiSq = 0.0;
767 unsigned int locNumIterations = 0;
774 cout <<
"DKinFitter: At maximum number of iterations, this chisq, last chisq, last resort cutoff = " <<
dChiSq <<
", " << locPreviousChiSq <<
", " <<
dConvergenceChiSqDiff_LastResort << endl;
778 cout <<
"DKinFitter: Exceeded maximum number of iterations. Returning false." << endl;
783 locPreviousChiSq =
dChiSq;
785 cout <<
"DKinFitter: Begin iteration" << endl;
790 cout <<
"DKinFitter: dF: " << endl;
792 cout <<
"DKinFitter: dF_dXi: " << endl;
794 cout <<
"DKinFitter: dF_dEta: " << endl;
819 cout <<
"DKinFitter: locDeltaXi: " << endl;
821 cout <<
"DKinFitter: dXi: " << endl;
833 TMatrixDSym locTempMatrix =
dS;
840 cout <<
"DKinFitter: dLambda: " << endl;
842 cout <<
"DKinFitter: dEta: " << endl;
844 cout <<
"DKinFitter: dChiSq = " <<
dChiSq << endl;
847 (*locParticleIterator)->Print_ParticleParams();
860 TMatrixDSym locTempMatrix(
dVY);
861 locTempMatrix.Similarity(
dF_dEta);
865 cout <<
"DKinFitter: dS: " << endl;
867 cout <<
"determinant magnitude = " << fabs(
dS.Determinant()) << endl;
870 TDecompLU locDecompLU_S(
dS);
872 if( locDecompLU_S.Decompose() && fabs(
dS.Determinant()==0.0) )
874 if(
dDebugLevel > 10) cout <<
"trying to lower Tol = "<<
dS.GetTol() <<
"\n" << endl;
875 TMatrixD matrixLU_dS = locDecompLU_S.GetLU();
878 Double_t minDiagElement = fabs(matrixLU_dS(0,0));
879 for(
int i=1;i<matrixLU_dS.GetNrows();i++)
881 if(fabs(matrixLU_dS(i,i))<minDiagElement)
883 minDiagElement = fabs(matrixLU_dS(i,i));
887 if(
dS.GetTol()>minDiagElement && minDiagElement>1.0E-26)
890 dS.SetTol(minDiagElement/2);
891 if(
dDebugLevel > 10) cout <<
"New Tol is set at " << minDiagElement/2 <<
"\n" << endl;
897 if((!locDecompLU_S.Decompose()) || (fabs(
dS.Determinant()) < 1.0E-300))
900 cout <<
"DKinFitter: dS not invertible. Returning false." << endl;
907 cout <<
"DKinFitter: dS_Inverse: " << endl;
912 dS.SetTol(2.22044604925031308
e-16);
921 locTempMatrix.SimilarityT(
dF_dXi);
925 cout <<
"DKinFitter: dU_Inverse: " << endl;
927 cout <<
"determinant magnitude = " << fabs(
dU_Inverse.Determinant()) << endl;
931 if((!locDecompLU_VXiInv.Decompose()) || (fabs(
dU_Inverse.Determinant()) < 1.0E-300))
934 cout <<
"DKinFitter: dU_Inverse not invertible. Returning false." << endl;
941 cout <<
"DKinFitter: dU: " << endl;
957 cout <<
"DKinFitter: dVEta: " << endl;
959 cout <<
"DKinFitter: dV: " << endl;
966 TMatrixDSym locTempMatrix11 =
dVXi;
967 dVEta =
dVY - (locG - locTempMatrix11.Similarity(locH)).Similarity(
dVY);
970 TMatrixD locEtaXiCovariance = -1.0*
dVY*locH*
dU;
971 for(
unsigned int loc_i = 0; loc_i <
dNumEta; ++loc_i)
973 for(
unsigned int loc_j = 0; loc_j <
dNumEta; ++loc_j)
974 dV(loc_i, loc_j) =
dVEta(loc_i, loc_j);
976 for(
unsigned int loc_i = 0; loc_i <
dNumXi; ++loc_i)
978 for(
unsigned int loc_j = 0; loc_j <
dNumXi; ++loc_j)
979 dV(loc_i + dNumEta, loc_j + dNumEta) =
dVXi(loc_i, loc_j);
981 for(
unsigned int loc_i = 0; loc_i <
dNumEta; ++loc_i)
983 for(
unsigned int loc_j = 0; loc_j <
dNumXi; ++loc_j)
985 dV(loc_i, loc_j + dNumEta) = locEtaXiCovariance(loc_i, loc_j);
986 dV(loc_j + dNumEta, loc_i) = locEtaXiCovariance(loc_i, loc_j);
992 cout <<
"DKinFitter: dVEta: " << endl;
994 cout <<
"DKinFitter: dV: " << endl;
1005 size_t locFIndex = 0;
1009 auto locKinFitConstraint_P4 = std::dynamic_pointer_cast<
DKinFitConstraint_P4>(*locConstraintIterator);
1010 if(locKinFitConstraint_P4 != NULL)
1012 int locFIndex = locKinFitConstraint_P4->
Get_FIndex();
1014 cout <<
"DKinFitter: P4 Constraint, F index = " << locFIndex << endl;
1017 auto locInitialParticles = locKinFitConstraint_P4->Get_InitialParticles();
1018 auto locParticleIterator = locInitialParticles.begin();
1019 for(; locParticleIterator != locInitialParticles.end(); ++locParticleIterator)
1020 Calc_dF_P4(locFIndex, (*locParticleIterator).get(), 1.0);
1023 auto locFinalParticles = locKinFitConstraint_P4->Get_FinalParticles();
1024 for(locParticleIterator = locFinalParticles.begin(); locParticleIterator != locFinalParticles.end(); ++locParticleIterator)
1025 Calc_dF_P4(locFIndex, (*locParticleIterator).get(), -1.0);
1030 auto locKinFitConstraint_Mass = std::dynamic_pointer_cast<
DKinFitConstraint_Mass>(*locConstraintIterator);
1031 if(locKinFitConstraint_Mass != NULL)
1033 int locFIndex = locKinFitConstraint_Mass->
Get_FIndex();
1035 cout <<
"DKinFitter: Mass Constraint, F index = " << locFIndex << endl;
1037 auto locDecayingParticle = locKinFitConstraint_Mass->Get_DecayingParticle();
1038 double locTargetedMass = locDecayingParticle->Get_Mass();
1042 cout <<
"Final decaying pxyzE is: " << locXP4.Px() <<
", " << locXP4.Py() <<
", " << locXP4.Pz() <<
", " << locXP4.E() << endl;
1043 dF(locFIndex, 0) += locXP4.M2() - locTargetedMass*locTargetedMass;
1051 if((locKinFitConstraint_Vertex != NULL) && (locKinFitConstraint_Spacetime == NULL))
1054 auto locParticleIterator = locFullConstrainParticles.begin();
1055 for(; locParticleIterator != locFullConstrainParticles.end(); ++locParticleIterator)
1057 auto locKinFitParticle = *locParticleIterator;
1061 locFIndex = locKinFitConstraint_Vertex->Get_FIndex(locKinFitParticle);
1063 cout <<
"DKinFitter: F index, locIsDecayingFlag = " << locFIndex <<
", " << locIsDecayingFlag << endl;
1071 if(locKinFitConstraint_Spacetime != NULL)
1086 int locCharge = locKinFitParticle->
Get_Charge();
1089 TLorentzVector locP4 = locKinFitParticle->
Get_P4();
1090 TVector3 locPosition = locKinFitParticle->
Get_Position();
1109 TVector3 locDeltaX = locVertexSignMultiplier*(locCommonVertex - locPosition);
1111 TVector3 locH = locBField.Unit();
1112 double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
1126 dF(locFIndex, 0) += locStateSignMultiplier*locP4.E();
1127 dF(locFIndex + 1, 0) += locStateSignMultiplier*locP4.Px();
1128 dF(locFIndex + 2, 0) += locStateSignMultiplier*locP4.Py();
1129 dF(locFIndex + 3, 0) += locStateSignMultiplier*locP4.Pz();
1133 cout <<
"PID, sign, pxyzE = " << locKinFitParticle->
Get_PID() <<
", " << locStateSignMultiplier <<
", " << locP4.Px() <<
", " << locP4.Py() <<
", " << locP4.Pz() <<
", " << locP4.E() << endl;
1141 TVector3 locDeltaXCrossH = locDeltaX.Cross(locH);
1143 cout <<
"propagate pxyz by: " << -1.0*locStateSignMultiplier*locA*locDeltaXCrossH.X() <<
", " << -1.0*locStateSignMultiplier*locA*locDeltaXCrossH.Y() <<
", " << -1.0*locStateSignMultiplier*locA*locDeltaXCrossH.Z() << endl;
1144 dF(locFIndex + 1, 0) -= locStateSignMultiplier*locA*locDeltaXCrossH.X();
1145 dF(locFIndex + 2, 0) -= locStateSignMultiplier*locA*locDeltaXCrossH.Y();
1146 dF(locFIndex + 3, 0) -= locStateSignMultiplier*locA*locDeltaXCrossH.Z();
1152 cout <<
"DKinFitter: Calc_dF_P4() Section 1; PID = " << locKinFitParticle->
Get_PID() << endl;
1155 else if(locNeutralShowerFlag)
1158 cout <<
"DKinFitter: Calc_dF_P4() Section 3; PID = " << locKinFitParticle->
Get_PID() << endl;
1160 dF_dEta(locFIndex, locEParamIndex) = locStateSignMultiplier;
1162 double locEOverPSq = locP4.E()/locP4.Vect().Mag2();
1163 dF_dEta(locFIndex + 1, locEParamIndex) = locStateSignMultiplier*locEOverPSq*locP4.Px();
1164 dF_dEta(locFIndex + 2, locEParamIndex) = locStateSignMultiplier*locEOverPSq*locP4.Py();
1165 dF_dEta(locFIndex + 3, locEParamIndex) = locStateSignMultiplier*locEOverPSq*locP4.Pz();
1167 TVector3 locDeltaXOverMagDeltaXSq = locDeltaX*(1.0/locDeltaX.Mag2());
1169 dF_dEta(locFIndex + 1, locVxParamIndex) = locStateSignMultiplier*locP4.Px()*(locDeltaXOverMagDeltaXSq.X() - 1.0/locDeltaX.X());
1170 dF_dEta(locFIndex + 2, locVxParamIndex + 1) = locStateSignMultiplier*locP4.Py()*(locDeltaXOverMagDeltaXSq.Y() - 1.0/locDeltaX.Y());
1171 dF_dEta(locFIndex + 3, locVxParamIndex + 2) = locStateSignMultiplier*locP4.Pz()*(locDeltaXOverMagDeltaXSq.Z() - 1.0/locDeltaX.Z());
1173 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = locStateSignMultiplier*locP4.Px()*locDeltaXOverMagDeltaXSq.Y();
1174 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = locStateSignMultiplier*locP4.Px()*locDeltaXOverMagDeltaXSq.Z();
1176 dF_dEta(locFIndex + 2, locVxParamIndex) = locStateSignMultiplier*locP4.Py()*locDeltaXOverMagDeltaXSq.X();
1177 dF_dEta(locFIndex + 2, locVxParamIndex + 2) = locStateSignMultiplier*locP4.Py()*locDeltaXOverMagDeltaXSq.Z();
1179 dF_dEta(locFIndex + 3, locVxParamIndex) = locStateSignMultiplier*locP4.Pz()*locDeltaXOverMagDeltaXSq.X();
1180 dF_dEta(locFIndex + 3, locVxParamIndex + 1) = locStateSignMultiplier*locP4.Pz()*locDeltaXOverMagDeltaXSq.Y();
1182 dF_dXi(locFIndex + 1, locCommonVxParamIndex) -=
dF_dEta(locFIndex + 1, locVxParamIndex);
1183 dF_dXi(locFIndex + 2, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex + 2, locVxParamIndex + 1);
1184 dF_dXi(locFIndex + 3, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex + 3, locVxParamIndex + 2);
1186 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 1);
1187 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 2);
1189 dF_dXi(locFIndex + 2, locCommonVxParamIndex) -=
dF_dEta(locFIndex + 2, locVxParamIndex);
1190 dF_dXi(locFIndex + 2, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex + 2, locVxParamIndex + 2);
1192 dF_dXi(locFIndex + 3, locCommonVxParamIndex) -=
dF_dEta(locFIndex + 3, locVxParamIndex);
1193 dF_dXi(locFIndex + 3, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex + 3, locVxParamIndex + 1);
1199 cout <<
"DKinFitter: Calc_dF_P4() Section 4; PID = " << locKinFitParticle->
Get_PID() << endl;
1201 dF_dXi(locFIndex, locPxParamIndex) = locStateSignMultiplier*locP4.Px()/locP4.E();
1202 dF_dXi(locFIndex, locPxParamIndex + 1) = locStateSignMultiplier*locP4.Py()/locP4.E();
1203 dF_dXi(locFIndex, locPxParamIndex + 2) = locStateSignMultiplier*locP4.Pz()/locP4.E();
1205 dF_dXi(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier;
1206 dF_dXi(locFIndex + 2, locPxParamIndex + 1) = locStateSignMultiplier;
1207 dF_dXi(locFIndex + 3, locPxParamIndex + 2) = locStateSignMultiplier;
1213 cout <<
"DKinFitter: Calc_dF_P4() Section 5; PID = " << locKinFitParticle->
Get_PID() << endl;
1214 if(locChargedBFieldFlag && locCommonVertexFitFlag)
1217 cout <<
"DKinFitter: Calc_dF_P4() Section 5a" << endl;
1220 dF_dXi(locFIndex + 1, locVxParamIndex + 1) += locStateSignMultiplier*locVertexSignMultiplier*locA*locH.Z();
1221 dF_dXi(locFIndex + 1, locVxParamIndex + 2) += -1.0*locStateSignMultiplier*locVertexSignMultiplier*locA*locH.Y();
1223 dF_dXi(locFIndex + 2, locVxParamIndex) += -1.0*locStateSignMultiplier*locVertexSignMultiplier*locA*locH.Z();
1224 dF_dXi(locFIndex + 2, locVxParamIndex + 2) += locStateSignMultiplier*locVertexSignMultiplier*locA*locH.X();
1226 dF_dXi(locFIndex + 3, locVxParamIndex) += locStateSignMultiplier*locVertexSignMultiplier*locA*locH.Y();
1227 dF_dXi(locFIndex + 3, locVxParamIndex + 1) += -1.0*locStateSignMultiplier*locVertexSignMultiplier*locA*locH.X();
1229 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dXi(locFIndex + 1, locVxParamIndex + 1);
1230 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dXi(locFIndex + 1, locVxParamIndex + 2);
1232 dF_dXi(locFIndex + 2, locCommonVxParamIndex) -=
dF_dXi(locFIndex + 2, locVxParamIndex);
1233 dF_dXi(locFIndex + 2, locCommonVxParamIndex + 2) -=
dF_dXi(locFIndex + 2, locVxParamIndex + 2);
1235 dF_dXi(locFIndex + 3, locCommonVxParamIndex) -=
dF_dXi(locFIndex + 3, locVxParamIndex);
1236 dF_dXi(locFIndex + 3, locCommonVxParamIndex + 1) -=
dF_dXi(locFIndex + 3, locVxParamIndex + 1);
1242 auto locParticleIterator = locFromInitialState.begin();
1243 for(; locParticleIterator != locFromInitialState.end(); ++locParticleIterator)
1246 cout <<
"decaying, partially replace with init-state PID = " << (*locParticleIterator)->Get_PID() << endl;
1248 Calc_dF_P4(locFIndex, (*locParticleIterator).get(), locNextStateSignMultiplier);
1253 for(locParticleIterator = locFromFinalState.begin(); locParticleIterator != locFromFinalState.end(); ++locParticleIterator)
1256 cout <<
"decaying, partially replace with final-state PID = " << (*locParticleIterator)->Get_PID() << endl;
1260 Calc_dF_P4(locFIndex, (*locParticleIterator).get(), locNextStateSignMultiplier);
1267 cout <<
"DKinFitter: Calc_dF_P4() Section 2; PID = " << locKinFitParticle->
Get_PID() << endl;
1269 dF_dEta(locFIndex, locPxParamIndex) = locStateSignMultiplier*locP4.Px()/locP4.E();
1270 dF_dEta(locFIndex, locPxParamIndex + 1) = locStateSignMultiplier*locP4.Py()/locP4.E();
1271 dF_dEta(locFIndex, locPxParamIndex + 2) = locStateSignMultiplier*locP4.Pz()/locP4.E();
1273 dF_dEta(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier;
1274 dF_dEta(locFIndex + 2, locPxParamIndex + 1) = locStateSignMultiplier;
1275 dF_dEta(locFIndex + 3, locPxParamIndex + 2) = locStateSignMultiplier;
1277 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = locStateSignMultiplier*locA*locH.Z();
1278 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = -1.0*locStateSignMultiplier*locA*locH.Y();
1280 dF_dEta(locFIndex + 2, locVxParamIndex) = -1.0*locStateSignMultiplier*locA*locH.Z();
1281 dF_dEta(locFIndex + 2, locVxParamIndex + 2) = locStateSignMultiplier*locA*locH.X();
1283 dF_dEta(locFIndex + 3, locVxParamIndex) = locStateSignMultiplier*locA*locH.Y();
1284 dF_dEta(locFIndex + 3, locVxParamIndex + 1) = -1.0*locStateSignMultiplier*locA*locH.X();
1286 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 1);
1287 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 2);
1289 dF_dXi(locFIndex + 2, locCommonVxParamIndex) -=
dF_dEta(locFIndex + 2, locVxParamIndex);
1290 dF_dXi(locFIndex + 2, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex + 2, locVxParamIndex + 2);
1292 dF_dXi(locFIndex + 3, locCommonVxParamIndex) -=
dF_dEta(locFIndex + 3, locVxParamIndex);
1293 dF_dXi(locFIndex + 3, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex + 3, locVxParamIndex + 1);
1299 cout <<
"DKinFitter: Calc_dF_P4() Section 6; PID = " << locKinFitParticle->
Get_PID() << endl;
1301 dF_dEta(locFIndex, locPxParamIndex) = locStateSignMultiplier*locP4.Px()/locP4.E();
1302 dF_dEta(locFIndex, locPxParamIndex + 1) = locStateSignMultiplier*locP4.Py()/locP4.E();
1303 dF_dEta(locFIndex, locPxParamIndex + 2) = locStateSignMultiplier*locP4.Pz()/locP4.E();
1305 dF_dEta(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier;
1306 dF_dEta(locFIndex + 2, locPxParamIndex + 1) = locStateSignMultiplier;
1307 dF_dEta(locFIndex + 3, locPxParamIndex + 2) = locStateSignMultiplier;
1316 int locCharge = locKinFitParticle->
Get_Charge();
1319 TLorentzVector locP4 = locKinFitParticle->
Get_P4();
1320 TVector3 locPosition = locKinFitParticle->
Get_Position();
1339 TVector3 locDeltaX = locVertexSignMultiplier*(locCommonVertex - locPosition);
1341 TVector3 locH = locBField.Unit();
1342 double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
1353 TVector3 locPXCrossH = locXP4.Vect().Cross(locH);
1356 cout <<
"PID, sign, pxyzE = " << locKinFitParticle->
Get_PID() <<
", " << locStateSignMultiplier <<
", " << locP4.Px() <<
", " << locP4.Py() <<
", " << locP4.Pz() <<
", " << locP4.E() << endl;
1361 cout <<
"DKinFitter: Calc_dF_MassDerivs() Section 1; PID = " << locKinFitParticle->
Get_PID() << endl;
1364 else if(locNeutralShowerFlag)
1367 cout <<
"DKinFitter: Calc_dF_MassDerivs() Section 3; PID = " << locKinFitParticle->
Get_PID() << endl;
1369 double locEOverPSq = locP4.E()/locP4.Vect().Mag2();
1370 dF_dEta(locFIndex, locEParamIndex) = 2.0*locStateSignMultiplier*(locXP4.E() - locEOverPSq*locXP4.Vect().Dot(locP4.Vect()));
1372 double locDeltaXDotPXOverMagDeltaXSq = locDeltaX.Dot(locXP4.Vect())/(locDeltaX.Mag2());
1373 dF_dEta(locFIndex, locVxParamIndex) = 2.0*locStateSignMultiplier*locP4.Px()*(locXP4.Px()/locDeltaX.X() - locDeltaXDotPXOverMagDeltaXSq);
1374 dF_dEta(locFIndex, locVxParamIndex + 1) = 2.0*locStateSignMultiplier*locP4.Py()*(locXP4.Py()/locDeltaX.Y() - locDeltaXDotPXOverMagDeltaXSq);
1375 dF_dEta(locFIndex, locVxParamIndex + 2) = 2.0*locStateSignMultiplier*locP4.Pz()*(locXP4.Pz()/locDeltaX.Z() - locDeltaXDotPXOverMagDeltaXSq);
1377 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dEta(locFIndex, locVxParamIndex);
1378 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dEta(locFIndex, locVxParamIndex + 1);
1379 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dEta(locFIndex, locVxParamIndex + 2);
1385 cout <<
"DKinFitter: Calc_dF_MassDerivs() Section 4; PID = " << locKinFitParticle->
Get_PID() << endl;
1387 dF_dXi(locFIndex, locPxParamIndex) = 2.0*locStateSignMultiplier*(locP4.Px()*locXP4.E()/locP4.E() - locXP4.Px());
1388 dF_dXi(locFIndex, locPxParamIndex + 1) = 2.0*locStateSignMultiplier*(locP4.Py()*locXP4.E()/locP4.E() - locXP4.Py());
1389 dF_dXi(locFIndex, locPxParamIndex + 2) = 2.0*locStateSignMultiplier*(locP4.Pz()*locXP4.E()/locP4.E() - locXP4.Pz());
1395 cout <<
"DKinFitter: Calc_dF_MassDerivs() Section 5; PID = " << locKinFitParticle->
Get_PID() << endl;
1396 if(locChargedBFieldFlag && locCommonVertexFitFlag && !locIsConstrainedParticle)
1400 cout <<
"DKinFitter: Calc_dF_MassDerivs() Section 5a" << endl;
1402 dF_dXi(locFIndex, locVxParamIndex) += 2.0*locStateSignMultiplier*locVertexSignMultiplier*locA*locPXCrossH.X();
1403 dF_dXi(locFIndex, locVxParamIndex + 1) += 2.0*locStateSignMultiplier*locVertexSignMultiplier*locA*locPXCrossH.Y();
1404 dF_dXi(locFIndex, locVxParamIndex + 2) += 2.0*locStateSignMultiplier*locVertexSignMultiplier*locA*locPXCrossH.Z();
1406 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dXi(locFIndex, locVxParamIndex);
1407 dF_dXi(locFIndex, locCommonVxParamIndex + 1) -=
dF_dXi(locFIndex, locVxParamIndex + 1);
1408 dF_dXi(locFIndex, locCommonVxParamIndex + 2) -=
dF_dXi(locFIndex, locVxParamIndex + 2);
1414 auto locParticleIterator = locFromInitialState.begin();
1415 for(; locParticleIterator != locFromInitialState.end(); ++locParticleIterator)
1418 cout <<
"decaying, partially replace with init-state PID = " << (*locParticleIterator)->Get_PID() << endl;
1420 Calc_dF_MassDerivs(locFIndex, (*locParticleIterator).get(), locXP4, locNextStateSignMultiplier,
false);
1425 for(locParticleIterator = locFromFinalState.begin(); locParticleIterator != locFromFinalState.end(); ++locParticleIterator)
1428 cout <<
"decaying, partially replace with final-state PID = " << (*locParticleIterator)->Get_PID() << endl;
1432 Calc_dF_MassDerivs(locFIndex, (*locParticleIterator).get(), locXP4, locNextStateSignMultiplier,
false);
1439 cout <<
"DKinFitter: Calc_dF_MassDerivs() Section 2; PID = " << locKinFitParticle->
Get_PID() << endl;
1441 dF_dEta(locFIndex, locPxParamIndex) = 2.0*locStateSignMultiplier*(locP4.Px()*locXP4.E()/locP4.E() - locXP4.Px());
1442 dF_dEta(locFIndex, locPxParamIndex + 1) = 2.0*locStateSignMultiplier*(locP4.Py()*locXP4.E()/locP4.E() - locXP4.Py());
1443 dF_dEta(locFIndex, locPxParamIndex + 2) = 2.0*locStateSignMultiplier*(locP4.Pz()*locXP4.E()/locP4.E() - locXP4.Pz());
1445 dF_dEta(locFIndex, locVxParamIndex) = 2.0*locStateSignMultiplier*locA*locPXCrossH.X();
1446 dF_dEta(locFIndex, locVxParamIndex + 1) = 2.0*locStateSignMultiplier*locA*locPXCrossH.Y();
1447 dF_dEta(locFIndex, locVxParamIndex + 2) = 2.0*locStateSignMultiplier*locA*locPXCrossH.Z();
1449 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dEta(locFIndex, locVxParamIndex);
1450 dF_dXi(locFIndex, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex, locVxParamIndex + 1);
1451 dF_dXi(locFIndex, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex, locVxParamIndex + 2);
1457 cout <<
"DKinFitter: Calc_dF_MassDerivs() Section 6; PID = " << locKinFitParticle->
Get_PID() << endl;
1459 dF_dEta(locFIndex, locPxParamIndex) = 2.0*locStateSignMultiplier*(locP4.Px()*locXP4.E()/locP4.E() - locXP4.Px());
1460 dF_dEta(locFIndex, locPxParamIndex + 1) = 2.0*locStateSignMultiplier*(locP4.Py()*locXP4.E()/locP4.E() - locXP4.Py());
1461 dF_dEta(locFIndex, locPxParamIndex + 2) = 2.0*locStateSignMultiplier*(locP4.Pz()*locXP4.E()/locP4.E() - locXP4.Pz());
1474 cout <<
"DKinFitter: Calc_dF_Vertex() Section 1; PID = " << locKinFitParticle->
Get_PID() << endl;
1478 if(locKinFitParticle_DecayingSource == NULL)
1482 int locCharge = locKinFitParticle_DecayingSource->
Get_Charge();
1495 auto locParticleIterator = locFromInitialState.begin();
1496 for(; locParticleIterator != locFromInitialState.end(); ++locParticleIterator)
1500 cout <<
"decaying, partially replace with init-state PID = " << (*locParticleIterator)->Get_PID() <<
", next sign multiplier: " << locNextStateSignMultiplier << endl;
1501 Calc_dF_Vertex(locFIndex, (*locParticleIterator).get(), locKinFitParticle, locNextStateSignMultiplier);
1506 for(locParticleIterator = locFromFinalState.begin(); locParticleIterator != locFromFinalState.end(); ++locParticleIterator)
1512 cout <<
"decaying, partially replace with final-state PID = " << (*locParticleIterator)->Get_PID() <<
", next sign multiplier: " << locNextStateSignMultiplier << endl;
1513 Calc_dF_Vertex(locFIndex, (*locParticleIterator).get(), locKinFitParticle, locNextStateSignMultiplier);
1522 int locCharge = locKinFitParticle->
Get_Charge();
1524 TVector3 locPosition = locKinFitParticle->
Get_Position();
1526 TVector3 locDeltaX = locCommonVertex - locPosition;
1527 TVector3 locMomentum = locKinFitParticle->
Get_Momentum();
1533 bool IsNonZeroBField = (locBField.Mag()>1
e-4)?
true:
false;
1538 cout <<
"DKinFitter: Calc_dF_Vertex() Section 2; PID = " << locKinFitParticle->
Get_PID() << endl;
1540 TVector3 locH = locBField.Unit();
1541 TVector3 locPCrossH = locMomentum.Cross(locH);
1542 TVector3 locPCrossDeltaX = locMomentum.Cross(locDeltaX);
1543 TVector3 locDeltaXCrossH = locDeltaX.Cross(locH);
1545 double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
1546 double locDeltaXDotH = locDeltaX.Dot(locH);
1547 double locPDotH = locMomentum.Dot(locH);
1548 double locPDotHsq= locPDotH*locPDotH;
1549 double PsqMinusPDotHsq=locMomentum.Mag2()-locPDotHsq;
1550 double RhoS=locA*locDeltaXDotH/locPDotH;
1551 double sinRhoS=
sin(RhoS);
1552 double cosRhoS=cos(RhoS);
1553 double OneMinusCosRhoS=1.-cosRhoS;
1556 if (fabs(locPDotH)<1
e-15){
1557 locPDotH+=(locPDotH>0.)?1
e-15:-1
e-15;
1560 dF(locFIndex, 0) = locDeltaXDotH*locPDotH - locDeltaX.Dot(locMomentum)
1561 + PsqMinusPDotHsq*sinRhoS/locA;
1562 dF(locFIndex, 1) = -locPCrossDeltaX.Dot(locH)
1563 + PsqMinusPDotHsq*OneMinusCosRhoS/locA;
1565 TVector3 dF0_dEta_VxVec = locMomentum
1566 - (locPDotH + PsqMinusPDotHsq*cosRhoS/locPDotH)*locH;
1567 dF_dEta(locFIndex, locVxParamIndex) = dF0_dEta_VxVec.X();
1568 dF_dEta(locFIndex, locVxParamIndex + 1) = dF0_dEta_VxVec.Y();
1569 dF_dEta(locFIndex, locVxParamIndex + 2) = dF0_dEta_VxVec.Z();
1571 TVector3 dF0_dEta_PxVec = -locDeltaX
1572 + (locDeltaXDotH - PsqMinusPDotHsq*cosRhoS*locDeltaXDotH/locPDotHsq)*locH
1573 + (2./locA)*sinRhoS*(locMomentum-locPDotH*locH);
1574 dF_dEta(locFIndex, locPxParamIndex) = dF0_dEta_PxVec.X();
1575 dF_dEta(locFIndex, locPxParamIndex + 1) = dF0_dEta_PxVec.Y();
1576 dF_dEta(locFIndex, locPxParamIndex + 2) = dF0_dEta_PxVec.Z();
1578 TVector3 dF1_dEta_VxVec = -locPCrossH
1579 - PsqMinusPDotHsq*sinRhoS/locPDotH*locH;
1580 dF_dEta(locFIndex + 1, locVxParamIndex) = dF1_dEta_VxVec.X();
1581 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = dF1_dEta_VxVec.Y();
1582 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = dF1_dEta_VxVec.Z();
1584 TVector3 dF1_dEta_PxVec = -locDeltaXCrossH
1585 + (2./locA)*OneMinusCosRhoS*(locMomentum-locPDotH*locH)
1586 - PsqMinusPDotHsq*sinRhoS*locDeltaXDotH/locPDotHsq*locH;
1587 dF_dEta(locFIndex + 1, locPxParamIndex) = dF1_dEta_PxVec.X();
1588 dF_dEta(locFIndex + 1, locPxParamIndex + 1) = dF1_dEta_PxVec.Y();
1589 dF_dEta(locFIndex + 1, locPxParamIndex + 2) = dF1_dEta_PxVec.Z();
1591 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dEta(locFIndex, locVxParamIndex);
1592 dF_dXi(locFIndex, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex, locVxParamIndex + 1);
1593 dF_dXi(locFIndex, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex, locVxParamIndex + 2);
1595 dF_dXi(locFIndex + 1, locCommonVxParamIndex) -=
dF_dEta(locFIndex + 1, locVxParamIndex);
1596 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 1);
1597 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 2);
1599 else if((locCharge != 0) && IsNonZeroBField && (locKinFitParticleType ==
d_DecayingParticle)){
1602 cout <<
"DKinFitter: Calc_dF_Vertex() Section 3; PID = " << locKinFitParticle->
Get_PID() << endl;
1604 TVector3 locH = locBField.Unit();
1605 double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
1607 TVector3 locPCrossH = locMomentum.Cross(locH);
1608 TVector3 locPCrossDeltaX = locMomentum.Cross(locDeltaX);
1609 double locDeltaXDotH = locDeltaX.Dot(locH);
1610 double locPDotH = locMomentum.Dot(locH);
1622 bool locP4DerivedAtCommonVertexFlag = (locP4DefinedByInvariantMassFlag == locVertexP4AtProductionVertexFlag);
1624 if(locP4DerivedAtCommonVertexFlag)
1626 TVector3 locQ, locM, locD;
1630 dF(locFIndex, 0) = locPCrossDeltaX.Dot(locH) - 0.5*locA*(locDeltaX.Mag2() - locDeltaXDotH*locDeltaXDotH);
1631 dF(locFIndex + 1, 0) = locDeltaXDotH - (locPDotH/locA)*asin(locJ);
1635 dF_dXi(locFIndex, locVxParamIndex) += locPCrossH.X();
1636 dF_dXi(locFIndex, locVxParamIndex + 1) += locPCrossH.Y();
1637 dF_dXi(locFIndex, locVxParamIndex + 2) += locPCrossH.Z();
1639 dF_dXi(locFIndex + 1, locVxParamIndex) += locR.X();
1640 dF_dXi(locFIndex + 1, locVxParamIndex + 1) += locR.Y();
1641 dF_dXi(locFIndex + 1, locVxParamIndex + 2) += locR.Z();
1644 double locPDotHsq= locPDotH*locPDotH;
1645 double PsqMinusPDotHsq=locMomentum.Mag2()-locPDotHsq;
1646 double RhoS=locA*locDeltaXDotH/locPDotH;
1647 double sinRhoS=
sin(RhoS);
1648 double cosRhoS=cos(RhoS);
1649 double OneMinusCosRhoS=1.-cosRhoS;
1652 if (fabs(locPDotH)<1
e-15){
1653 locPDotH+=(locPDotH>0.)?1
e-15:-1
e-15;
1656 dF(locFIndex, 0) = locDeltaXDotH*locPDotH - locDeltaX.Dot(locMomentum)
1657 + PsqMinusPDotHsq*sinRhoS/locA;
1658 dF(locFIndex, 1) = -locPCrossDeltaX.Dot(locH)
1659 + PsqMinusPDotHsq*OneMinusCosRhoS/locA;
1661 TVector3 dF0_dEta_VxVec = locMomentum
1662 - (locPDotH + PsqMinusPDotHsq*cosRhoS/locPDotH)*locH;
1663 TVector3 dF1_dEta_VxVec = -locPCrossH
1664 - PsqMinusPDotHsq*sinRhoS/locPDotH*locH;
1666 dF_dXi(locFIndex, locVxParamIndex) += dF0_dEta_VxVec.X();
1667 dF_dXi(locFIndex, locVxParamIndex + 1) += dF0_dEta_VxVec.Y();
1668 dF_dXi(locFIndex, locVxParamIndex + 2) += dF0_dEta_VxVec.Z();
1670 dF_dXi(locFIndex + 1, locVxParamIndex) += dF1_dEta_VxVec.X();
1671 dF_dXi(locFIndex + 1, locVxParamIndex + 1) += dF1_dEta_VxVec.Y();
1672 dF_dXi(locFIndex + 1, locVxParamIndex + 2) += dF1_dEta_VxVec.Z();
1675 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dXi(locFIndex, locVxParamIndex);
1676 dF_dXi(locFIndex, locCommonVxParamIndex + 1) -=
dF_dXi(locFIndex, locVxParamIndex + 1);
1677 dF_dXi(locFIndex, locCommonVxParamIndex + 2) -=
dF_dXi(locFIndex, locVxParamIndex + 2);
1679 dF_dXi(locFIndex + 1, locCommonVxParamIndex) -=
dF_dXi(locFIndex + 1, locVxParamIndex);
1680 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dXi(locFIndex + 1, locVxParamIndex + 1);
1681 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dXi(locFIndex + 1, locVxParamIndex + 2);
1688 if(locVertexConstraintFlag == 1)
1690 dF(locFIndex, 0) = locMomentum.Y()*locDeltaX.Z() - locMomentum.Z()*locDeltaX.Y();
1691 dF(locFIndex + 1, 0) = locMomentum.Z()*locDeltaX.X() - locMomentum.X()*locDeltaX.Z();
1694 cout <<
"DKinFitter: Calc_dF_Vertex() Section 4a; PID = " << locKinFitParticle->
Get_PID() << endl;
1696 dF_dXi(locFIndex, locVxParamIndex + 1) += locMomentum.Z();
1697 dF_dXi(locFIndex, locVxParamIndex + 2) += -1.0*locMomentum.Y();
1698 dF_dXi(locFIndex + 1, locVxParamIndex) += -1.0*locMomentum.Z();
1699 dF_dXi(locFIndex + 1, locVxParamIndex + 2) += locMomentum.X();
1701 dF_dXi(locFIndex, locCommonVxParamIndex + 1) += -1.0*
dF_dXi(locFIndex, locVxParamIndex + 1);
1702 dF_dXi(locFIndex, locCommonVxParamIndex + 2) += -1.0*
dF_dXi(locFIndex, locVxParamIndex + 2);
1703 dF_dXi(locFIndex + 1, locCommonVxParamIndex) += -1.0*
dF_dXi(locFIndex + 1, locVxParamIndex);
1704 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) += -1.0*
dF_dXi(locFIndex + 1, locVxParamIndex + 2);
1706 else if(locVertexConstraintFlag == 2)
1708 dF(locFIndex, 0) = locMomentum.Y()*locDeltaX.Z() - locMomentum.Z()*locDeltaX.Y();
1709 dF(locFIndex + 1, 0) = locMomentum.X()*locDeltaX.Y() - locMomentum.Y()*locDeltaX.X();
1712 cout <<
"DKinFitter: Calc_dF_Vertex() Section 4b; PID = " << locKinFitParticle->
Get_PID() << endl;
1714 dF_dXi(locFIndex, locVxParamIndex + 1) += locMomentum.Z();
1715 dF_dXi(locFIndex, locVxParamIndex + 2) += -1.0*locMomentum.Y();
1716 dF_dXi(locFIndex + 1, locVxParamIndex) += locMomentum.Y();
1717 dF_dXi(locFIndex + 1, locVxParamIndex + 1) += -1.0*locMomentum.X();
1719 dF_dXi(locFIndex, locCommonVxParamIndex + 1) += -1.0*
dF_dXi(locFIndex, locVxParamIndex + 1);
1720 dF_dXi(locFIndex, locCommonVxParamIndex + 2) += -1.0*
dF_dXi(locFIndex, locVxParamIndex + 2);
1721 dF_dXi(locFIndex + 1, locCommonVxParamIndex) += -1.0*
dF_dXi(locFIndex + 1, locVxParamIndex);
1722 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) += -1.0*
dF_dXi(locFIndex + 1, locVxParamIndex + 1);
1726 dF(locFIndex, 0) = locMomentum.Z()*locDeltaX.X() - locMomentum.X()*locDeltaX.Z();
1727 dF(locFIndex + 1, 0) = locMomentum.X()*locDeltaX.Y() - locMomentum.Y()*locDeltaX.X();
1730 cout <<
"DKinFitter: Calc_dF_Vertex() Section 4c; PID = " << locKinFitParticle->
Get_PID() << endl;
1732 dF_dXi(locFIndex, locVxParamIndex) += -1.0*locMomentum.Z();
1733 dF_dXi(locFIndex, locVxParamIndex + 2) += locMomentum.X();
1734 dF_dXi(locFIndex + 1, locVxParamIndex) += locMomentum.Y();
1735 dF_dXi(locFIndex + 1, locVxParamIndex + 1) += -1.0*locMomentum.X();
1737 dF_dXi(locFIndex, locCommonVxParamIndex) += -1.0*
dF_dXi(locFIndex, locVxParamIndex);
1738 dF_dXi(locFIndex, locCommonVxParamIndex + 2) += -1.0*
dF_dXi(locFIndex, locVxParamIndex + 2);
1739 dF_dXi(locFIndex + 1, locCommonVxParamIndex) += -1.0*
dF_dXi(locFIndex + 1, locVxParamIndex);
1740 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) += -1.0*
dF_dXi(locFIndex + 1, locVxParamIndex + 1);
1746 if(locVertexConstraintFlag == 1)
1748 dF(locFIndex, 0) = locMomentum.Y()*locDeltaX.Z() - locMomentum.Z()*locDeltaX.Y();
1749 dF(locFIndex + 1, 0) = locMomentum.Z()*locDeltaX.X() - locMomentum.X()*locDeltaX.Z();
1752 cout <<
"DKinFitter: Calc_dF_Vertex() Section 5a; PID = " << locKinFitParticle->
Get_PID() << endl;
1754 dF_dEta(locFIndex, locPxParamIndex + 1) = locDeltaX.Z();
1755 dF_dEta(locFIndex, locPxParamIndex + 2) = -1.0*locDeltaX.Y();
1756 dF_dEta(locFIndex + 1, locPxParamIndex) = -1.0*locDeltaX.Z();
1757 dF_dEta(locFIndex + 1, locPxParamIndex + 2) = locDeltaX.X();
1759 dF_dEta(locFIndex, locVxParamIndex + 1) = locMomentum.Z();
1760 dF_dEta(locFIndex, locVxParamIndex + 2) = -1.0*locMomentum.Y();
1761 dF_dEta(locFIndex + 1, locVxParamIndex) = -1.0*locMomentum.Z();
1762 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = locMomentum.X();
1764 dF_dXi(locFIndex, locCommonVxParamIndex + 1) += -1.0*
dF_dEta(locFIndex, locVxParamIndex + 1);
1765 dF_dXi(locFIndex, locCommonVxParamIndex + 2) += -1.0*
dF_dEta(locFIndex, locVxParamIndex + 2);
1766 dF_dXi(locFIndex + 1, locCommonVxParamIndex) += -1.0*
dF_dEta(locFIndex + 1, locVxParamIndex);
1767 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) += -1.0*
dF_dEta(locFIndex + 1, locVxParamIndex + 2);
1769 else if(locVertexConstraintFlag == 2)
1771 dF(locFIndex, 0) = locMomentum.Y()*locDeltaX.Z() - locMomentum.Z()*locDeltaX.Y();
1772 dF(locFIndex + 1, 0) = locMomentum.X()*locDeltaX.Y() - locMomentum.Y()*locDeltaX.X();
1775 cout <<
"DKinFitter: Calc_dF_Vertex() Section 5b; PID = " << locKinFitParticle->
Get_PID() << endl;
1777 dF_dEta(locFIndex, locPxParamIndex + 1) = locDeltaX.Z();
1778 dF_dEta(locFIndex, locPxParamIndex + 2) = -1.0*locDeltaX.Y();
1779 dF_dEta(locFIndex + 1, locPxParamIndex) = locDeltaX.Y();
1780 dF_dEta(locFIndex + 1, locPxParamIndex + 1) = -1.0*locDeltaX.X();
1782 dF_dEta(locFIndex, locVxParamIndex + 1) = locMomentum.Z();
1783 dF_dEta(locFIndex, locVxParamIndex + 2) = -1.0*locMomentum.Y();
1784 dF_dEta(locFIndex + 1, locVxParamIndex) = locMomentum.Y();
1785 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = -1.0*locMomentum.X();
1787 dF_dXi(locFIndex, locCommonVxParamIndex + 1) += -1.0*
dF_dEta(locFIndex, locVxParamIndex + 1);
1788 dF_dXi(locFIndex, locCommonVxParamIndex + 2) += -1.0*
dF_dEta(locFIndex, locVxParamIndex + 2);
1789 dF_dXi(locFIndex + 1, locCommonVxParamIndex) += -1.0*
dF_dEta(locFIndex + 1, locVxParamIndex);
1790 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) += -1.0*
dF_dEta(locFIndex + 1, locVxParamIndex + 1);
1794 dF(locFIndex, 0) = locMomentum.Z()*locDeltaX.X() - locMomentum.X()*locDeltaX.Z();
1795 dF(locFIndex + 1, 0) = locMomentum.X()*locDeltaX.Y() - locMomentum.Y()*locDeltaX.X();
1798 cout <<
"DKinFitter: Calc_dF_Vertex() Section 5c; PID = " << locKinFitParticle->
Get_PID() << endl;
1800 dF_dEta(locFIndex, locPxParamIndex) = -1.0*locDeltaX.Z();
1801 dF_dEta(locFIndex, locPxParamIndex + 2) = locDeltaX.X();
1802 dF_dEta(locFIndex + 1, locPxParamIndex) = locDeltaX.Y();
1803 dF_dEta(locFIndex + 1, locPxParamIndex + 1) = -1.0*locDeltaX.X();
1805 dF_dEta(locFIndex, locVxParamIndex) = -1.0*locMomentum.Z();
1806 dF_dEta(locFIndex, locVxParamIndex + 2) = locMomentum.X();
1807 dF_dEta(locFIndex + 1, locVxParamIndex) = locMomentum.Y();
1808 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = -1.0*locMomentum.X();
1810 dF_dXi(locFIndex, locCommonVxParamIndex) += -1.0*
dF_dEta(locFIndex, locVxParamIndex);
1811 dF_dXi(locFIndex, locCommonVxParamIndex + 2) += -1.0*
dF_dEta(locFIndex, locVxParamIndex + 2);
1812 dF_dXi(locFIndex + 1, locCommonVxParamIndex) += -1.0*
dF_dEta(locFIndex + 1, locVxParamIndex);
1813 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) += -1.0*
dF_dEta(locFIndex + 1, locVxParamIndex + 1);
1823 int locCharge = locKinFitParticle->
Get_Charge();
1825 double locEnergy = locKinFitParticle->
Get_P4().E();
1826 TVector3 locPosition = locKinFitParticle->
Get_Position();
1828 TVector3 locDeltaX = locCommonVertex - locPosition;
1829 TVector3 locMomentum = locKinFitParticle->
Get_Momentum();
1840 cout <<
"DKinFitter: Calc_dF_Vertex() Section 6; PID = " << locKinFitParticle->
Get_PID() << endl;
1842 TVector3 locQ, locM, locD;
1846 TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->
Get_Position();
1847 TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->
Get_CommonVertex();
1848 TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
1851 TVector3 locH_DecayingSource = locBField_DecayingSource.Unit();
1853 TVector3 locH = locBField.Unit();
1854 double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
1856 TVector3 locDeltaXCrossH_DecayingSource = locDeltaX_DecayingSource.Cross(locH_DecayingSource);
1857 TVector3 locDeltaXCrossH_DecayingSource_CrossH = locDeltaXCrossH_DecayingSource.Cross(locH);
1858 TVector3 locQCrossH = locQ.Cross(locH);
1860 dF_dEta(locFIndex, locPxParamIndex) = locStateSignMultiplier*locDeltaXCrossH_DecayingSource.X();
1861 dF_dEta(locFIndex, locPxParamIndex + 1) = locStateSignMultiplier*locDeltaXCrossH_DecayingSource.Y();
1862 dF_dEta(locFIndex, locPxParamIndex + 2) = locStateSignMultiplier*locDeltaXCrossH_DecayingSource.Z();
1864 dF_dEta(locFIndex, locVxParamIndex) = -1.0*locStateSignMultiplier*locA*locDeltaXCrossH_DecayingSource_CrossH.X();
1865 dF_dEta(locFIndex, locVxParamIndex + 1) = -1.0*locStateSignMultiplier*locA*locDeltaXCrossH_DecayingSource_CrossH.Y();
1866 dF_dEta(locFIndex, locVxParamIndex + 2) = -1.0*locStateSignMultiplier*locA*locDeltaXCrossH_DecayingSource_CrossH.Z();
1868 dF_dEta(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier*locQ.X();
1869 dF_dEta(locFIndex + 1, locPxParamIndex + 1) = locStateSignMultiplier*locQ.Y();
1870 dF_dEta(locFIndex + 1, locPxParamIndex + 2) = locStateSignMultiplier*locQ.Z();
1872 dF_dEta(locFIndex + 1, locVxParamIndex) = -1.0*locStateSignMultiplier*locA*locQCrossH.X();
1873 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = -1.0*locStateSignMultiplier*locA*locQCrossH.Y();
1874 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = -1.0*locStateSignMultiplier*locA*locQCrossH.Z();
1876 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dEta(locFIndex, locVxParamIndex);
1877 dF_dXi(locFIndex, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex, locVxParamIndex + 1);
1878 dF_dXi(locFIndex, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex, locVxParamIndex + 2);
1880 dF_dXi(locFIndex + 1, locCommonVxParamIndex) -=
dF_dEta(locFIndex + 1, locVxParamIndex);
1881 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 1);
1882 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 2);
1887 cout <<
"DKinFitter: Calc_dF_Vertex() Section 7; PID = " << locKinFitParticle->
Get_PID() << endl;
1889 TVector3 locQ, locM, locD;
1893 TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->
Get_Position();
1894 TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->
Get_CommonVertex();
1895 TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
1898 TVector3 locH_DecayingSource = locBField_DecayingSource.Unit();
1900 TVector3 locDeltaXCrossH_DecayingSource = locDeltaX_DecayingSource.Cross(locH_DecayingSource);
1901 double locXTerm = locDeltaXCrossH_DecayingSource.Dot(locDeltaX)/(locDeltaX.Mag2());
1902 double locQTerm = locQ.Dot(locDeltaX)/(locDeltaX.Mag2());
1904 dF_dEta(locFIndex, locEParamIndex) = locStateSignMultiplier*locEnergy*(locDeltaXCrossH_DecayingSource.Dot(locMomentum))/(locMomentum.Mag2());
1905 dF_dEta(locFIndex, locVxParamIndex) = -1.0*locStateSignMultiplier*locMomentum.Px()*(locDeltaXCrossH_DecayingSource.X()/(locDeltaX.X()) - locXTerm);
1906 dF_dEta(locFIndex, locVxParamIndex + 1) = -1.0*locStateSignMultiplier*locMomentum.Py()*(locDeltaXCrossH_DecayingSource.Y()/(locDeltaX.Y()) - locXTerm);
1907 dF_dEta(locFIndex, locVxParamIndex + 2) = -1.0*locStateSignMultiplier*locMomentum.Pz()*(locDeltaXCrossH_DecayingSource.Z()/(locDeltaX.Z()) - locXTerm);
1909 dF_dEta(locFIndex + 1, locEParamIndex) = locStateSignMultiplier*locEnergy*(locMomentum.Dot(locQ))/(locMomentum.Mag2());
1910 dF_dEta(locFIndex + 1, locVxParamIndex) = -1.0*locStateSignMultiplier*locMomentum.Px()*(locQ.X()/(locDeltaX.X()) - locQTerm);
1911 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = -1.0*locStateSignMultiplier*locMomentum.Py()*(locQ.Y()/(locDeltaX.Y()) - locQTerm);
1912 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = -1.0*locStateSignMultiplier*locMomentum.Pz()*(locQ.Z()/(locDeltaX.Z()) - locQTerm);
1914 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dEta(locFIndex, locVxParamIndex);
1915 dF_dXi(locFIndex, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex, locVxParamIndex + 1);
1916 dF_dXi(locFIndex, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex, locVxParamIndex + 2);
1918 dF_dXi(locFIndex + 1, locCommonVxParamIndex) -=
dF_dEta(locFIndex + 1, locVxParamIndex);
1919 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 1);
1920 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 2);
1929 cout <<
"DKinFitter: Calc_dF_Vertex() Section 8; PID = " << locKinFitParticle->
Get_PID() << endl;
1932 TVector3 locQ, locM, locD;
1936 TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->
Get_Position();
1937 TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->
Get_CommonVertex();
1938 TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
1941 TVector3 locH_DecayingSource = locBField_DecayingSource.Unit();
1943 TVector3 locDeltaXCrossH_DecayingSource = locDeltaX_DecayingSource.Cross(locH_DecayingSource);
1945 dF_dEta(locFIndex, locPxParamIndex) = locStateSignMultiplier*locDeltaXCrossH_DecayingSource.X();
1946 dF_dEta(locFIndex, locPxParamIndex + 1) = locStateSignMultiplier*locDeltaXCrossH_DecayingSource.Y();
1947 dF_dEta(locFIndex, locPxParamIndex + 2) = locStateSignMultiplier*locDeltaXCrossH_DecayingSource.Z();
1949 dF_dEta(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier*locQ.X();
1950 dF_dEta(locFIndex + 1, locPxParamIndex + 1) = locStateSignMultiplier*locQ.Y();
1951 dF_dEta(locFIndex + 1, locPxParamIndex + 2) = locStateSignMultiplier*locQ.Z();
1957 cout <<
"DKinFitter: Calc_dF_Vertex() Section 9; PID = " << locKinFitParticle->
Get_PID() << endl;
1959 TVector3 locQ, locM, locD;
1963 TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->
Get_Position();
1964 TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->
Get_CommonVertex();
1965 TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
1968 TVector3 locH_DecayingSource = locBField_DecayingSource.Unit();
1970 TVector3 locDeltaXCrossH_DecayingSource = locDeltaX_DecayingSource.Cross(locH_DecayingSource);
1972 dF_dXi(locFIndex, locPxParamIndex) = locStateSignMultiplier*locDeltaXCrossH_DecayingSource.X();
1973 dF_dXi(locFIndex, locPxParamIndex + 1) = locStateSignMultiplier*locDeltaXCrossH_DecayingSource.Y();
1974 dF_dXi(locFIndex, locPxParamIndex + 2) = locStateSignMultiplier*locDeltaXCrossH_DecayingSource.Z();
1976 dF_dXi(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier*locQ.X();
1977 dF_dXi(locFIndex + 1, locPxParamIndex + 1) = locStateSignMultiplier*locQ.Y();
1978 dF_dXi(locFIndex + 1, locPxParamIndex + 2) = locStateSignMultiplier*locQ.Z();
1984 cout <<
"DKinFitter: Calc_dF_Vertex() Section 10; PID = " << locKinFitParticle->
Get_PID() << endl;
1986 TVector3 locQ, locM, locD;
1990 TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->
Get_Position();
1991 TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->
Get_CommonVertex();
1992 TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
1995 TVector3 locH_DecayingSource = locBField_DecayingSource.Unit();
1997 TVector3 locH = locBField.Unit();
1998 double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
2000 TVector3 locDeltaXCrossH_DecayingSource = locDeltaX_DecayingSource.Cross(locH_DecayingSource);
2001 TVector3 locDeltaXCrossH_DecayingSource_CrossH = locDeltaXCrossH_DecayingSource.Cross(locH);
2002 TVector3 locQCrossH = locQ.Cross(locH);
2004 dF_dXi(locFIndex, locVxParamIndex) -= locStateSignMultiplier*locA*locDeltaXCrossH_DecayingSource_CrossH.X();
2005 dF_dXi(locFIndex, locVxParamIndex + 1) -= locStateSignMultiplier*locA*locDeltaXCrossH_DecayingSource_CrossH.Y();
2006 dF_dXi(locFIndex, locVxParamIndex + 2) -= locStateSignMultiplier*locA*locDeltaXCrossH_DecayingSource_CrossH.Z();
2008 dF_dXi(locFIndex + 1, locVxParamIndex) -= locStateSignMultiplier*locA*locQCrossH.X();
2009 dF_dXi(locFIndex + 1, locVxParamIndex + 1) -= locStateSignMultiplier*locA*locQCrossH.Y();
2010 dF_dXi(locFIndex + 1, locVxParamIndex + 2) -= locStateSignMultiplier*locA*locQCrossH.Z();
2012 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dXi(locFIndex, locVxParamIndex);
2013 dF_dXi(locFIndex, locCommonVxParamIndex + 1) -=
dF_dXi(locFIndex, locVxParamIndex + 1);
2014 dF_dXi(locFIndex, locCommonVxParamIndex + 2) -=
dF_dXi(locFIndex, locVxParamIndex + 2);
2016 dF_dXi(locFIndex + 1, locCommonVxParamIndex) -=
dF_dXi(locFIndex + 1, locVxParamIndex);
2017 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dXi(locFIndex + 1, locVxParamIndex + 1);
2018 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dXi(locFIndex + 1, locVxParamIndex + 2);
2027 int locCharge = locKinFitParticle->
Get_Charge();
2029 double locEnergy = locKinFitParticle->
Get_P4().E();
2030 TVector3 locPosition = locKinFitParticle->
Get_Position();
2032 TVector3 locDeltaX = locCommonVertex - locPosition;
2033 TVector3 locMomentum = locKinFitParticle->
Get_Momentum();
2044 double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
2045 TVector3 locH = locBField.Unit();
2047 TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->
Get_Position();
2048 TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->
Get_CommonVertex();
2049 TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
2052 if(locVertexConstraintFlag == 1)
2055 cout <<
"DKinFitter: Calc_dF_Vertex() Section 11a; PID = " << locKinFitParticle->
Get_PID() << endl;
2057 dF_dEta(locFIndex, locPxParamIndex + 1) = locStateSignMultiplier*locDeltaX_DecayingSource.Z();
2058 dF_dEta(locFIndex, locPxParamIndex + 2) = -1.0*locStateSignMultiplier*locDeltaX_DecayingSource.Y();
2059 dF_dEta(locFIndex + 1, locPxParamIndex) = -1.0*locStateSignMultiplier*locDeltaX_DecayingSource.Z();
2060 dF_dEta(locFIndex + 1, locPxParamIndex + 2) = locStateSignMultiplier*locDeltaX_DecayingSource.X();
2062 dF_dEta(locFIndex, locVxParamIndex) = -1.0*locA*locStateSignMultiplier*(locDeltaX_DecayingSource.Y()*locH.Y() + locDeltaX_DecayingSource.Z()*locH.Z());
2063 dF_dEta(locFIndex, locVxParamIndex + 1) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.Y()*locH.X();
2064 dF_dEta(locFIndex, locVxParamIndex + 2) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.Z()*locH.X();
2065 dF_dEta(locFIndex + 1, locVxParamIndex) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.X()*locH.Y();
2066 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = -1.0*locA*locStateSignMultiplier*(locDeltaX_DecayingSource.X()*locH.X() + locDeltaX_DecayingSource.Z()*locH.Z());
2067 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.Z()*locH.Y();
2069 else if(locVertexConstraintFlag == 2)
2072 cout <<
"DKinFitter: Calc_dF_Vertex() Section 11b; PID = " << locKinFitParticle->
Get_PID() << endl;
2074 dF_dEta(locFIndex, locPxParamIndex + 1) = locStateSignMultiplier*locDeltaX_DecayingSource.Z();
2075 dF_dEta(locFIndex, locPxParamIndex + 2) = -1.0*locStateSignMultiplier*locDeltaX_DecayingSource.Y();
2076 dF_dEta(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier*locDeltaX_DecayingSource.Y();
2077 dF_dEta(locFIndex + 1, locPxParamIndex + 1) = -1.0*locStateSignMultiplier*locDeltaX_DecayingSource.X();
2079 dF_dEta(locFIndex, locVxParamIndex) = -1.0*locA*locStateSignMultiplier*(locDeltaX_DecayingSource.Y()*locH.Y() + locDeltaX_DecayingSource.Z()*locH.Z());
2080 dF_dEta(locFIndex, locVxParamIndex + 1) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.Y()*locH.X();
2081 dF_dEta(locFIndex, locVxParamIndex + 2) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.Z()*locH.X();
2082 dF_dEta(locFIndex + 1, locVxParamIndex) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.X()*locH.Z();
2083 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.Y()*locH.Z();
2084 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = -1.0*locA*locStateSignMultiplier*(locDeltaX_DecayingSource.X()*locH.X() + locDeltaX_DecayingSource.Y()*locH.Y());
2089 cout <<
"DKinFitter: Calc_dF_Vertex() Section 11c; PID = " << locKinFitParticle->
Get_PID() << endl;
2091 dF_dEta(locFIndex, locPxParamIndex) = -1.0*locStateSignMultiplier*locDeltaX_DecayingSource.Z();
2092 dF_dEta(locFIndex, locPxParamIndex + 2) = locStateSignMultiplier*locDeltaX_DecayingSource.X();
2093 dF_dEta(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier*locDeltaX_DecayingSource.Y();
2094 dF_dEta(locFIndex + 1, locPxParamIndex + 1) = -1.0*locStateSignMultiplier*locDeltaX_DecayingSource.X();
2096 dF_dEta(locFIndex, locVxParamIndex) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.X()*locH.Y();
2097 dF_dEta(locFIndex, locVxParamIndex + 1) = -1.0*locA*locStateSignMultiplier*(locDeltaX_DecayingSource.X()*locH.X() + locDeltaX_DecayingSource.Z()*locH.Z());
2098 dF_dEta(locFIndex, locVxParamIndex + 2) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.Z()*locH.Y();
2099 dF_dEta(locFIndex + 1, locVxParamIndex) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.X()*locH.Z();
2100 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = locA*locStateSignMultiplier*locDeltaX_DecayingSource.Y()*locH.Z();
2101 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = -1.0*locA*locStateSignMultiplier*(locDeltaX_DecayingSource.X()*locH.X() + locDeltaX_DecayingSource.Y()*locH.Y());
2104 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dEta(locFIndex, locVxParamIndex);
2105 dF_dXi(locFIndex, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex, locVxParamIndex + 1);
2106 dF_dXi(locFIndex, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex, locVxParamIndex + 2);
2108 dF_dXi(locFIndex + 1, locCommonVxParamIndex) -=
dF_dEta(locFIndex + 1, locVxParamIndex);
2109 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 1);
2110 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 2);
2114 TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->
Get_Position();
2115 TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->
Get_CommonVertex();
2116 TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
2118 TVector3 locPiCrossDeltaXX = locMomentum.Cross(locDeltaX_DecayingSource);
2119 TVector3 locDeltaXCrossDeltaXX = locDeltaX.Cross(locDeltaX_DecayingSource);
2120 double locDeltaXiMagSq = locDeltaX.Mag2();
2123 if(locVertexConstraintFlag == 1)
2126 cout <<
"DKinFitter: Calc_dF_Vertex() Section 12a; PID = " << locKinFitParticle->
Get_PID() << endl;
2128 dF_dEta(locFIndex, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.X()/locMomentum.Mag2();
2129 dF_dEta(locFIndex + 1, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.Y()/locMomentum.Mag2();
2131 dF_dEta(locFIndex, locVxParamIndex) = locStateSignMultiplier*locMomentum.X()*locDeltaXCrossDeltaXX.X()/locDeltaXiMagSq;
2132 dF_dEta(locFIndex, locVxParamIndex + 1) = locStateSignMultiplier*locMomentum.Y()*locDeltaXCrossDeltaXX.X()/locDeltaXiMagSq - locMomentum.Y()*locDeltaX_DecayingSource.Z()/locDeltaX.Y();
2133 dF_dEta(locFIndex, locVxParamIndex + 2) = locStateSignMultiplier*locMomentum.Z()*locDeltaXCrossDeltaXX.X()/locDeltaXiMagSq + locMomentum.Z()*locDeltaX_DecayingSource.Y()/locDeltaX.Z();
2134 dF_dEta(locFIndex + 1, locVxParamIndex) = locStateSignMultiplier*locMomentum.X()*locDeltaXCrossDeltaXX.Y()/locDeltaXiMagSq + locMomentum.X()*locDeltaX_DecayingSource.Z()/locDeltaX.X();
2135 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = locStateSignMultiplier*locMomentum.Y()*locDeltaXCrossDeltaXX.Y()/locDeltaXiMagSq;
2136 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = locStateSignMultiplier*locMomentum.Z()*locDeltaXCrossDeltaXX.Y()/locDeltaXiMagSq - locMomentum.Z()*locDeltaX_DecayingSource.X()/locDeltaX.Z();
2138 else if(locVertexConstraintFlag == 2)
2141 cout <<
"DKinFitter: Calc_dF_Vertex() Section 12b; PID = " << locKinFitParticle->
Get_PID() << endl;
2143 dF_dEta(locFIndex, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.X()/locMomentum.Mag2();
2144 dF_dEta(locFIndex + 1, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.Z()/locMomentum.Mag2();
2146 dF_dEta(locFIndex, locVxParamIndex) = locStateSignMultiplier*locMomentum.X()*locDeltaXCrossDeltaXX.X()/locDeltaXiMagSq;
2147 dF_dEta(locFIndex, locVxParamIndex + 1) = locStateSignMultiplier*locMomentum.Y()*locDeltaXCrossDeltaXX.X()/locDeltaXiMagSq - locMomentum.Y()*locDeltaX_DecayingSource.Z()/locDeltaX.Y();
2148 dF_dEta(locFIndex, locVxParamIndex + 2) = locStateSignMultiplier*locMomentum.Z()*locDeltaXCrossDeltaXX.X()/locDeltaXiMagSq + locMomentum.Z()*locDeltaX_DecayingSource.Y()/locDeltaX.Z();
2149 dF_dEta(locFIndex + 1, locVxParamIndex) = locStateSignMultiplier*locMomentum.X()*locDeltaXCrossDeltaXX.Z()/locDeltaXiMagSq - locMomentum.X()*locDeltaX_DecayingSource.Y()/locDeltaX.X();
2150 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = locStateSignMultiplier*locMomentum.Y()*locDeltaXCrossDeltaXX.Z()/locDeltaXiMagSq + locMomentum.Y()*locDeltaX_DecayingSource.X()/locDeltaX.Y();
2151 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = locStateSignMultiplier*locMomentum.Z()*locDeltaXCrossDeltaXX.Z()/locDeltaXiMagSq;
2156 cout <<
"DKinFitter: Calc_dF_Vertex() Section 12c; PID = " << locKinFitParticle->
Get_PID() << endl;
2158 dF_dEta(locFIndex, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.Y()/locMomentum.Mag2();
2159 dF_dEta(locFIndex + 1, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.Z()/locMomentum.Mag2();
2161 dF_dEta(locFIndex, locVxParamIndex) = locStateSignMultiplier*locMomentum.X()*locDeltaXCrossDeltaXX.Y()/locDeltaXiMagSq + locMomentum.X()*locDeltaX_DecayingSource.Z()/locDeltaX.X();
2162 dF_dEta(locFIndex, locVxParamIndex + 1) = locStateSignMultiplier*locMomentum.Y()*locDeltaXCrossDeltaXX.Y()/locDeltaXiMagSq;
2163 dF_dEta(locFIndex, locVxParamIndex + 2) = locStateSignMultiplier*locMomentum.Z()*locDeltaXCrossDeltaXX.Y()/locDeltaXiMagSq - locMomentum.Z()*locDeltaX_DecayingSource.X()/locDeltaX.Z();
2164 dF_dEta(locFIndex + 1, locVxParamIndex) = locStateSignMultiplier*locMomentum.X()*locDeltaXCrossDeltaXX.Z()/locDeltaXiMagSq - locMomentum.X()*locDeltaX_DecayingSource.Y()/locDeltaX.X();
2165 dF_dEta(locFIndex + 1, locVxParamIndex + 1) = locStateSignMultiplier*locMomentum.Y()*locDeltaXCrossDeltaXX.Z()/locDeltaXiMagSq + locMomentum.Y()*locDeltaX_DecayingSource.X()/locDeltaX.Y();
2166 dF_dEta(locFIndex + 1, locVxParamIndex + 2) = locStateSignMultiplier*locMomentum.Z()*locDeltaXCrossDeltaXX.Z()/locDeltaXiMagSq;
2169 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dEta(locFIndex, locVxParamIndex);
2170 dF_dXi(locFIndex, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex, locVxParamIndex + 1);
2171 dF_dXi(locFIndex, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex, locVxParamIndex + 2);
2173 dF_dXi(locFIndex + 1, locCommonVxParamIndex) -=
dF_dEta(locFIndex + 1, locVxParamIndex);
2174 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 1);
2175 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dEta(locFIndex + 1, locVxParamIndex + 2);
2184 TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->
Get_Position();
2185 TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->
Get_CommonVertex();
2186 TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
2189 if(locVertexConstraintFlag == 1)
2192 cout <<
"DKinFitter: Calc_dF_Vertex() Section 13a; PID = " << locKinFitParticle->
Get_PID() << endl;
2194 dF_dEta(locFIndex, locPxParamIndex + 1) = locDeltaX_DecayingSource.Z();
2195 dF_dEta(locFIndex, locPxParamIndex + 2) = -1.0*locDeltaX_DecayingSource.Y();
2196 dF_dEta(locFIndex + 1, locPxParamIndex) = -1.0*locDeltaX_DecayingSource.Z();
2197 dF_dEta(locFIndex + 1, locPxParamIndex + 2) = locDeltaX_DecayingSource.X();
2199 else if(locVertexConstraintFlag == 2)
2202 cout <<
"DKinFitter: Calc_dF_Vertex() Section 13b; PID = " << locKinFitParticle->
Get_PID() << endl;
2204 dF_dEta(locFIndex, locPxParamIndex + 1) = locDeltaX_DecayingSource.Z();
2205 dF_dEta(locFIndex, locPxParamIndex + 2) = -1.0*locDeltaX_DecayingSource.Y();
2206 dF_dEta(locFIndex + 1, locPxParamIndex) = locDeltaX_DecayingSource.Y();
2207 dF_dEta(locFIndex + 1, locPxParamIndex + 1) = -1.0*locDeltaX_DecayingSource.X();
2212 cout <<
"DKinFitter: Calc_dF_Vertex() Section 13c; PID = " << locKinFitParticle->
Get_PID() << endl;
2214 dF_dEta(locFIndex, locPxParamIndex) = -1.0*locDeltaX_DecayingSource.Z();
2215 dF_dEta(locFIndex, locPxParamIndex + 2) = locDeltaX_DecayingSource.X();
2216 dF_dEta(locFIndex + 1, locPxParamIndex) = locDeltaX_DecayingSource.Y();
2217 dF_dEta(locFIndex + 1, locPxParamIndex + 1) = -1.0*locDeltaX_DecayingSource.X();
2223 TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->
Get_Position();
2224 TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->
Get_CommonVertex();
2225 TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
2228 if(locVertexConstraintFlag == 1)
2231 cout <<
"DKinFitter: Calc_dF_Vertex() Section 14a; PID = " << locKinFitParticle->
Get_PID() << endl;
2233 dF_dXi(locFIndex, locPxParamIndex + 1) = locDeltaX_DecayingSource.Z();
2234 dF_dXi(locFIndex, locPxParamIndex + 2) = -1.0*locDeltaX_DecayingSource.Y();
2235 dF_dXi(locFIndex + 1, locPxParamIndex) = -1.0*locDeltaX_DecayingSource.Z();
2236 dF_dXi(locFIndex + 1, locPxParamIndex + 2) = locDeltaX_DecayingSource.X();
2238 else if(locVertexConstraintFlag == 2)
2241 cout <<
"DKinFitter: Calc_dF_Vertex() Section 14b; PID = " << locKinFitParticle->
Get_PID() << endl;
2243 dF_dXi(locFIndex, locPxParamIndex + 1) = locDeltaX_DecayingSource.Z();
2244 dF_dXi(locFIndex, locPxParamIndex + 2) = -1.0*locDeltaX_DecayingSource.Y();
2245 dF_dXi(locFIndex + 1, locPxParamIndex) = locDeltaX_DecayingSource.Y();
2246 dF_dXi(locFIndex + 1, locPxParamIndex + 1) = -1.0*locDeltaX_DecayingSource.X();
2251 cout <<
"DKinFitter: Calc_dF_Vertex() Section 14c; PID = " << locKinFitParticle->
Get_PID() << endl;
2253 dF_dXi(locFIndex, locPxParamIndex) = -1.0*locDeltaX_DecayingSource.Z();
2254 dF_dXi(locFIndex, locPxParamIndex + 2) = locDeltaX_DecayingSource.X();
2255 dF_dXi(locFIndex + 1, locPxParamIndex) = locDeltaX_DecayingSource.Y();
2256 dF_dXi(locFIndex + 1, locPxParamIndex + 1) = -1.0*locDeltaX_DecayingSource.X();
2263 double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
2264 TVector3 locH = locBField.Unit();
2266 TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->
Get_Position();
2267 TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->
Get_CommonVertex();
2268 TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
2271 if(locVertexConstraintFlag == 1)
2274 cout <<
"DKinFitter: Calc_dF_Vertex() Section 15a; PID = " << locKinFitParticle->
Get_PID() << endl;
2276 dF_dXi(locFIndex, locVxParamIndex) -= locA*locStateSignMultiplier*(locDeltaX_DecayingSource.Y()*locH.Y() + locDeltaX_DecayingSource.Z()*locH.Z());
2277 dF_dXi(locFIndex, locVxParamIndex + 1) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.Y()*locH.X();
2278 dF_dXi(locFIndex, locVxParamIndex + 2) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.Z()*locH.X();
2279 dF_dXi(locFIndex + 1, locVxParamIndex) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.X()*locH.Y();
2280 dF_dXi(locFIndex + 1, locVxParamIndex + 1) -= locA*locStateSignMultiplier*(locDeltaX_DecayingSource.X()*locH.X() + locDeltaX_DecayingSource.Z()*locH.Z());
2281 dF_dXi(locFIndex + 1, locVxParamIndex + 2) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.Z()*locH.Y();
2283 else if(locVertexConstraintFlag == 2)
2286 cout <<
"DKinFitter: Calc_dF_Vertex() Section 15b; PID = " << locKinFitParticle->
Get_PID() << endl;
2288 dF_dXi(locFIndex, locVxParamIndex) -= locA*locStateSignMultiplier*(locDeltaX_DecayingSource.Y()*locH.Y() + locDeltaX_DecayingSource.Z()*locH.Z());
2289 dF_dXi(locFIndex, locVxParamIndex + 1) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.Y()*locH.X();
2290 dF_dXi(locFIndex, locVxParamIndex + 2) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.Z()*locH.X();
2291 dF_dXi(locFIndex + 1, locVxParamIndex) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.X()*locH.Z();
2292 dF_dXi(locFIndex + 1, locVxParamIndex + 1) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.Y()*locH.Z();
2293 dF_dXi(locFIndex + 1, locVxParamIndex + 2) -= locA*locStateSignMultiplier*(locDeltaX_DecayingSource.X()*locH.X() + locDeltaX_DecayingSource.Y()*locH.Y());
2298 cout <<
"DKinFitter: Calc_dF_Vertex() Section 15c; PID = " << locKinFitParticle->
Get_PID() << endl;
2300 dF_dXi(locFIndex, locVxParamIndex) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.X()*locH.Y();
2301 dF_dXi(locFIndex, locVxParamIndex + 1) -= locA*locStateSignMultiplier*(locDeltaX_DecayingSource.X()*locH.X() + locDeltaX_DecayingSource.Z()*locH.Z());
2302 dF_dXi(locFIndex, locVxParamIndex + 2) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.Z()*locH.Y();
2303 dF_dXi(locFIndex + 1, locVxParamIndex) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.X()*locH.Z();
2304 dF_dXi(locFIndex + 1, locVxParamIndex + 1) += locA*locStateSignMultiplier*locDeltaX_DecayingSource.Y()*locH.Z();
2305 dF_dXi(locFIndex + 1, locVxParamIndex + 2) -= locA*locStateSignMultiplier*(locDeltaX_DecayingSource.X()*locH.X() + locDeltaX_DecayingSource.Y()*locH.Y());
2308 dF_dXi(locFIndex, locCommonVxParamIndex) -=
dF_dXi(locFIndex, locVxParamIndex);
2309 dF_dXi(locFIndex, locCommonVxParamIndex + 1) -=
dF_dXi(locFIndex, locVxParamIndex + 1);
2310 dF_dXi(locFIndex, locCommonVxParamIndex + 2) -=
dF_dXi(locFIndex, locVxParamIndex + 2);
2312 dF_dXi(locFIndex + 1, locCommonVxParamIndex) -=
dF_dXi(locFIndex + 1, locVxParamIndex);
2313 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 1) -=
dF_dXi(locFIndex + 1, locVxParamIndex + 1);
2314 dF_dXi(locFIndex + 1, locCommonVxParamIndex + 2) -=
dF_dXi(locFIndex + 1, locVxParamIndex + 2);
2320 int locCharge = locKinFitParticle->
Get_Charge();
2321 TVector3 locPosition = locKinFitParticle->
Get_Position();
2323 TVector3 locDeltaX = locCommonVertex - locPosition;
2324 TVector3 locMomentum = locKinFitParticle->
Get_Momentum();
2327 TVector3 locH = locBField.Unit();
2329 TVector3 locPCrossH = locMomentum.Cross(locH);
2330 TVector3 locPCrossDeltaX = locMomentum.Cross(locDeltaX);
2331 double locPCrossHMagSq = locPCrossH.Mag2();
2333 double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
2334 double locDeltaXDotH = locDeltaX.Dot(locH);
2335 double locPDotH = locMomentum.Dot(locH);
2336 double locPDotDeltaX = locMomentum.Dot(locDeltaX);
2338 TVector3 locDeltaXCrossH = locDeltaX.Cross(locH);
2339 double locK = locPDotDeltaX - locPDotH*locDeltaXDotH;
2340 locJ = locA*locK/locPCrossHMagSq;
2341 double locC = locPDotH/(locPCrossHMagSq*
sqrt(1.0 - locJ*locJ));
2342 TVector3 locPCrossHCrossH = locPCrossH.Cross(locH);
2344 locM = locDeltaX - locDeltaXDotH*locH;
2345 locD = locC*(locMomentum - locPDotH*locH) - locH;
2346 locQ = -1.0*locH*(asin(locJ)/locA) - locC*(locM + 2.0*(locK/locPCrossHMagSq)*locPCrossHCrossH);
2351 int locCharge = locKinFitParticle->
Get_Charge();
2352 TVector3 locPosition = locKinFitParticle->
Get_Position();
2354 TVector3 locDeltaX = locCommonVertex - locPosition;
2358 TVector3 locH = locBField.Unit();
2359 double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
2361 TVector3 locDeltaXCrossH = locDeltaX.Cross(locH);
2362 TVector3 locPX = locP0X - locA*locDeltaXCrossH;
2364 TVector3 locP0XCrossH = locP0X.Cross(locH);
2365 double locPXDotH = locPX.Dot(locH);
2366 double locP0XCrossHMagSq = locP0XCrossH.Mag2();
2368 double locDeltaXDotH = locDeltaX.Dot(locH);
2369 double locPXDotDeltaX = locPX.Dot(locDeltaX);
2371 double locK = locPXDotDeltaX - locPXDotH*locDeltaXDotH;
2372 double locJ = locA*locK/locP0XCrossHMagSq;
2374 double locC = locPXDotH/(locP0XCrossHMagSq*
sqrt(1.0 - locJ*locJ));
2375 TVector3 locD = locC*(locPX - locPXDotH*locH) - locH;
2377 TVector3 locR = 2.0*locJ*locC*locP0XCrossH + locD;
2386 auto locVertexConstraints = Get_Constraints<DKinFitConstraint_Vertex>();
2387 auto locVertexIterator = locVertexConstraints.begin();
2388 for(; locVertexIterator != locVertexConstraints.end(); ++locVertexIterator)
2390 auto locConstraint = *locVertexIterator;
2391 int locParamIndex = locConstraint->Get_CommonVxParamIndex();
2392 TVector3 locVertex(
dXi(locParamIndex, 0),
dXi(locParamIndex + 1, 0),
dXi(locParamIndex + 2, 0));
2393 locConstraint->Set_CommonVertex(locVertex);
2397 auto locSpacetimeConstraints = Get_Constraints<DKinFitConstraint_Spacetime>();
2398 auto locSpacetimeIterator = locSpacetimeConstraints.begin();
2399 for(; locSpacetimeIterator != locSpacetimeConstraints.end(); ++locSpacetimeIterator)
2401 auto locConstraint = *locSpacetimeIterator;
2402 int locParamIndex = locConstraint->Get_CommonTParamIndex();
2403 locConstraint->Set_CommonTime(
dXi(locParamIndex, 0));
2408 for(; locParticleIterator !=
dKinFitParticles.end(); ++locParticleIterator)
2410 auto locKinFitParticle = *locParticleIterator;
2416 TMatrixD& locSourceMatrix = locIsUnknownParticleFlag ?
dXi :
dEta;
2418 TLorentzVector locP4 = locKinFitParticle->Get_P4();
2419 TLorentzVector locPreviousP4 = locP4;
2421 int locParamIndex = locKinFitParticle->Get_PxParamIndex();
2422 if(locParamIndex >= 0)
2424 TVector3 locMomentum(locSourceMatrix(locParamIndex, 0), locSourceMatrix(locParamIndex + 1, 0), locSourceMatrix(locParamIndex + 2, 0));
2425 locKinFitParticle->Set_Momentum(locMomentum);
2426 locP4 = locKinFitParticle->Get_P4();
2430 locParamIndex = locKinFitParticle->Get_VxParamIndex();
2431 if(locParamIndex >= 0)
2433 TVector3 locPosition(locSourceMatrix(locParamIndex, 0), locSourceMatrix(locParamIndex + 1, 0), locSourceMatrix(locParamIndex + 2, 0));
2434 TVector3 locDeltaX = locPosition - locKinFitParticle->Get_Position();
2435 locKinFitParticle->Set_Position(locPosition);
2436 if(locKinFitParticle->Get_CommonVxParamIndex() < 0)
2437 locKinFitParticle->Set_CommonVertex(locPosition);
2441 locParamIndex = locKinFitParticle->Get_EParamIndex();
2442 if(locParamIndex >= 0)
2444 double locE = locSourceMatrix(locParamIndex, 0);
2445 locKinFitParticle->Set_ShowerEnergy(locE);
2446 double locPMag =
sqrt(locE*locE - locKinFitParticle->Get_Mass()*locKinFitParticle->Get_Mass());
2447 TVector3 locPath = locKinFitParticle->Get_Position() - locKinFitParticle->Get_CommonVertex();
2448 TVector3 locMomentum = locPath;
2449 locMomentum.SetMag(locPMag);
2450 locKinFitParticle->Set_Momentum(locMomentum);
2454 locParamIndex = locKinFitParticle->Get_TParamIndex();
2455 if(locParamIndex >= 0)
2457 double locTime = locSourceMatrix(locParamIndex, 0);
2458 locKinFitParticle->Set_Time(locTime);
2459 if(locKinFitParticle->Get_CommonTParamIndex() < 0)
2460 locKinFitParticle->Set_CommonTime(locTime);
2462 else if((locKinFitParticle->Get_PxParamIndex() >= 0) && !locIsUnknownParticleFlag && (locKinFitParticle->Get_EParamIndex() < 0))
2469 double locDeltaT = (locKinFitParticle->Get_PathLength()/29.9792458)*(1.0/locPreviousP4.Beta() - 1.0/locP4.Beta());
2470 double locTime = locKinFitParticle->Get_Time() + locDeltaT;
2471 locKinFitParticle->Set_Time(locTime);
2472 if(locKinFitParticle->Get_CommonTParamIndex() < 0)
2473 locKinFitParticle->Set_CommonTime(locTime);
2481 auto locKinFitParticle = *locParticleIterator;
2493 for(; locParticleIterator !=
dKinFitParticles.end(); ++locParticleIterator)
2495 auto locKinFitParticle = *locParticleIterator;
2501 map<DKinFitPullType, double> locParticlePulls;
2504 cout <<
"pulls: PID = " << locKinFitParticle->Get_PID() << endl;
2505 cout <<
"e, px, vx, t param indices = " << int(locKinFitParticle->Get_EParamIndex()) <<
", " <<
int(locKinFitParticle->Get_PxParamIndex()) <<
", " <<
int(locKinFitParticle->Get_VxParamIndex()) <<
", " <<
int(locKinFitParticle->Get_TParamIndex()) << endl;
2508 int locParamIndex = locKinFitParticle->Get_EParamIndex();
2509 if(locParamIndex >= 0)
2511 double locDenominator =
sqrt(fabs(
dVY(locParamIndex, locParamIndex) -
dVEta(locParamIndex, locParamIndex)));
2512 locParticlePulls[
d_EPull] = (locDenominator > 0.0) ?
dEpsilon(locParamIndex, 0)/locDenominator : std::numeric_limits<double>::quiet_NaN();
2515 locParamIndex = locKinFitParticle->Get_PxParamIndex();
2516 if(locParamIndex >= 0)
2518 double locDenominator =
sqrt(fabs(
dVY(locParamIndex, locParamIndex) -
dVEta(locParamIndex, locParamIndex)));
2519 locParticlePulls[
d_PxPull] = (locDenominator > 0.0) ?
dEpsilon(locParamIndex, 0)/locDenominator : std::numeric_limits<double>::quiet_NaN();
2521 locDenominator =
sqrt(fabs(
dVY(locParamIndex, locParamIndex) -
dVEta(locParamIndex, locParamIndex)));
2522 locParticlePulls[
d_PyPull] = (locDenominator > 0.0) ?
dEpsilon(locParamIndex, 0)/locDenominator : std::numeric_limits<double>::quiet_NaN();
2524 locDenominator =
sqrt(fabs(
dVY(locParamIndex, locParamIndex) -
dVEta(locParamIndex, locParamIndex)));
2525 locParticlePulls[
d_PzPull] = (locDenominator > 0.0) ?
dEpsilon(locParamIndex, 0)/locDenominator : std::numeric_limits<double>::quiet_NaN();
2528 locParamIndex = locKinFitParticle->Get_VxParamIndex();
2529 if(locParamIndex >= 0)
2531 double locDenominator =
sqrt(fabs(
dVY(locParamIndex, locParamIndex) -
dVEta(locParamIndex, locParamIndex)));
2532 locParticlePulls[
d_XxPull] = (locDenominator > 0.0) ?
dEpsilon(locParamIndex, 0)/locDenominator : std::numeric_limits<double>::quiet_NaN();
2534 locDenominator =
sqrt(fabs(
dVY(locParamIndex, locParamIndex) -
dVEta(locParamIndex, locParamIndex)));
2535 locParticlePulls[
d_XyPull] = (locDenominator > 0.0) ?
dEpsilon(locParamIndex, 0)/locDenominator : std::numeric_limits<double>::quiet_NaN();
2537 locDenominator =
sqrt(fabs(
dVY(locParamIndex, locParamIndex) -
dVEta(locParamIndex, locParamIndex)));
2538 locParticlePulls[
d_XzPull] = (locDenominator > 0.0) ?
dEpsilon(locParamIndex, 0)/locDenominator : std::numeric_limits<double>::quiet_NaN();
2541 locParamIndex = locKinFitParticle->Get_TParamIndex();
2542 if(locParamIndex >= 0)
2544 double locDenominator =
sqrt(fabs(
dVY(locParamIndex, locParamIndex) -
dVEta(locParamIndex, locParamIndex)));
2545 locParticlePulls[
d_TPull] = (locDenominator > 0.0) ?
dEpsilon(locParamIndex, 0)/locDenominator : std::numeric_limits<double>::quiet_NaN();
2548 if(!locParticlePulls.empty())
2549 dPulls[locKinFitParticle] = locParticlePulls;
2554 cout <<
"DKinFitter: dEpsilon: " << endl;
2556 cout <<
"DKinFitter: dVY: " << endl;
2558 cout <<
"DKinFitter: Pulls: " << endl;
2559 map<shared_ptr<DKinFitParticle>, map<DKinFitPullType, double> >::iterator locIterator;
2560 for(locIterator =
dPulls.begin(); locIterator !=
dPulls.end(); ++locIterator)
2562 map<DKinFitPullType, double>& locParticlePulls = locIterator->second;
2563 auto locKinFitParticle = locIterator->first;
2564 TVector3 locMomentum = locKinFitParticle->Get_Momentum();
2565 cout <<
"particle PID, p3 = " << locKinFitParticle->Get_PID() <<
", " << locMomentum.Px() <<
", " << locMomentum.Py() <<
", " << locMomentum.Pz() <<
":" << endl;
2566 for(
size_t loc_i = 0; loc_i < 8; ++loc_i)
2568 if(locParticlePulls.find((
DKinFitPullType)loc_i) != locParticlePulls.end())
2583 for(; locParticleIterator !=
dKinFitParticles.end(); ++locParticleIterator)
2585 auto locKinFitParticle = *locParticleIterator;
2590 if(!locKinFitParticle->Get_FitCommonVertexFlag())
2595 TMatrixFSym* locCovarianceMatrix =
const_cast<TMatrixFSym*
>(locKinFitParticle->Get_CovarianceMatrix().get());
2597 TVector3 locMomentum;
2598 TLorentzVector locSpacetimeVertex;
2599 pair<double, double> locPathLengthPair, locRestFrameLifetimePair;
2605 cout <<
"PROPAGATED FINAL TRACK INFO: PID = " << locKinFitParticle->Get_PID() << endl;
2606 cout <<
"p_xyz, v_xyzt = " << locMomentum.Px() <<
", " << locMomentum.Py() <<
", " << locMomentum.Pz() <<
", " << locSpacetimeVertex.X() <<
", " << locSpacetimeVertex.Y() <<
", " << locSpacetimeVertex.Z() <<
", " << locSpacetimeVertex.T() << endl;
2607 cout <<
"common v_xyzt = " << locKinFitParticle->Get_CommonVertex().X() <<
", " << locKinFitParticle->Get_CommonVertex().Y() <<
", " << locKinFitParticle->Get_CommonVertex().Z() <<
", " << locKinFitParticle->Get_CommonTime() << endl;
2608 cout <<
"path length & uncert = " << locPathLengthPair.first <<
", " << locPathLengthPair.second << endl;
2609 cout <<
"sizes = " << locCovarianceMatrix->GetNrows() <<
", " << locCovarianceMatrix->GetNcols() << endl;
2613 locKinFitParticle->Set_Momentum(locMomentum);
2614 if(locKinFitParticle->Get_IsNeutralShowerFlag())
2615 locKinFitParticle->Set_CommonSpacetimeVertex(locSpacetimeVertex);
2617 locKinFitParticle->Set_SpacetimeVertex(locSpacetimeVertex);
2618 locKinFitParticle->Set_PathLength(locPathLengthPair.first);
2619 locKinFitParticle->Set_PathLengthUncertainty(locPathLengthPair.second);
2625 auto locKinFitParticle = *locParticleIterator;
2628 if((locKinFitParticleType !=
d_DecayingParticle) || !locKinFitParticle->Get_FitCommonVertexFlag())
2631 pair<double, double> locPathLengthPair, locRestFrameLifetimePair;
2632 if(
dKinFitUtils->
Calc_PathLength(locKinFitParticle.get(), &
dVXi, locKinFitParticle->Get_CovarianceMatrix().get(), locPathLengthPair, locRestFrameLifetimePair))
2634 locKinFitParticle->Set_PathLength(locPathLengthPair.first);
2635 locKinFitParticle->Set_PathLengthUncertainty(locPathLengthPair.second);
2636 locKinFitParticle->Set_RestFrameLifetime(locRestFrameLifetimePair.first);
2637 locKinFitParticle->Set_RestFrameLifetimeUncertainty(locRestFrameLifetimePair.second);
2655 auto locIncludeCommonSpacetimeVertex = locKinFitParticle->Get_FitCommonVertexFlag();
2658 auto locMatrixSize = locIncludeCommonSpacetimeVertex ? 11 : 7;
2660 locCovarianceMatrix->Zero();
2661 locKinFitParticle->Set_CovarianceMatrix(locCovarianceMatrix);
2663 auto locDerivedFromParticles = locKinFitParticle->Get_FromAllParticles();
2664 auto locFromIterator = locDerivedFromParticles.begin();
2665 map<const DKinFitParticle*, int> locAdditionalPxParamIndices;
2666 for(; locFromIterator != locDerivedFromParticles.end(); ++locFromIterator)
2671 if((*locFromIterator)->Get_PxParamIndex() >= 0)
2673 int locNewPxParamIndex =
dNumEta +
dNumXi + 3*locAdditionalPxParamIndices.size();
2674 locAdditionalPxParamIndices[(*locFromIterator).get()] = locNewPxParamIndex;
2677 TMatrixD locJacobian(locMatrixSize,
dNumEta +
dNumXi + 3*locAdditionalPxParamIndices.size());
2681 bool locP3DerivedAtPositionFlag = (locP3DerivedAtProductionVertexFlag == locKinFitParticle->Get_VertexP4AtProductionVertex());
2685 int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
2686 if(locVxParamIndex >= 0)
2688 locJacobian(3, locVxParamIndex +
dNumEta) = 1.0;
2689 locJacobian(4, locVxParamIndex +
dNumEta + 1) = 1.0;
2690 locJacobian(5, locVxParamIndex +
dNumEta + 2) = 1.0;
2692 int locTParamIndex = locKinFitParticle->Get_TParamIndex();
2693 if(locTParamIndex >= 0)
2694 locJacobian(6, locTParamIndex +
dNumEta) = 1.0;
2697 int locCommonVxParamIndex = locKinFitParticle->Get_CommonVxParamIndex();
2698 if(locCommonVxParamIndex >= 0)
2700 locJacobian(7, locCommonVxParamIndex +
dNumEta) = 1.0;
2701 locJacobian(8, locCommonVxParamIndex +
dNumEta + 1) = 1.0;
2702 locJacobian(9, locCommonVxParamIndex +
dNumEta + 2) = 1.0;
2704 int locCommonTParamIndex = locKinFitParticle->Get_CommonTParamIndex();
2705 if(locCommonTParamIndex >= 0)
2706 locJacobian(10, locCommonTParamIndex +
dNumEta) = 1.0;
2710 cout <<
"JACOBIAN MATRIX:" << endl;
2715 if(locAdditionalPxParamIndices.empty())
2717 TMatrixDSym locTempCovarianceMatrix =
dV;
2718 *locCovarianceMatrix = locTempCovarianceMatrix.Similarity(locJacobian);
2722 TMatrixDSym locTempCovarianceMatrix(
dNumEta +
dNumXi + 3*locAdditionalPxParamIndices.size());
2723 locTempCovarianceMatrix.SetSub(0,
dV);
2726 auto locPxParamIterator = locAdditionalPxParamIndices.begin();
2727 for(; locPxParamIterator != locAdditionalPxParamIndices.end(); ++locPxParamIterator)
2729 auto locPxParamIndex = locPxParamIterator->second;
2730 TMatrixFSym locAdditionalCovMatrix(3);
2731 locAdditionalCovMatrix = locPxParamIterator->first->Get_CovarianceMatrix()->GetSub(0, 2, locAdditionalCovMatrix);
2732 for(
int loc_q = 0; loc_q < 3; ++loc_q)
2734 for(
int loc_r = 0; loc_r < 3; ++loc_r)
2735 locTempCovarianceMatrix(loc_q + locPxParamIndex, loc_r + locPxParamIndex) = locAdditionalCovMatrix(loc_q, loc_r);
2741 cout <<
"FULL ERROR MATRIX (for calculated cov for enclosed decaying particle):" << endl;
2746 locTempCovarianceMatrix.Similarity(locJacobian);
2747 for(
int loc_q = 0; loc_q < 7; ++loc_q)
2749 for(
int loc_r = 0; loc_r < 7; ++loc_r)
2750 (*locCovarianceMatrix)(loc_q, loc_r) = locTempCovarianceMatrix(loc_q, loc_r);
2756 cout <<
"FINAL COV MATRIX (enclosed decaying particle):" << endl;
2761 if(locDecayingParticlesOnlyFlag)
2767 for(
auto locKinFitParticle : dKinFitParticles)
2769 auto locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
2774 bool locReconstructedParticleFlag = (locKinFitParticleType ==
d_MissingParticle);
2775 TMatrixDSym& locKinFitMatrix = locReconstructedParticleFlag ?
dVXi :
dVEta;
2776 if(locReconstructedParticleFlag)
2780 locCovarianceMatrix->Zero();
2781 locKinFitParticle->Set_CovarianceMatrix(locCovarianceMatrix);
2784 TMatrixFSym& locCovarianceMatrix = *(
const_cast<TMatrixFSym*
>(locKinFitParticle->Get_CovarianceMatrix().get()));
2786 int locPxParamIndex = locKinFitParticle->Get_PxParamIndex();
2787 int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
2788 int locTParamIndex = locKinFitParticle->Get_TParamIndex();
2789 int locEParamIndex = locKinFitParticle->Get_EParamIndex();
2791 int locCovMatrixEParamIndex = locKinFitParticle->Get_CovMatrixEParamIndex();
2792 int locCovMatrixPxParamIndex = locKinFitParticle->Get_CovMatrixPxParamIndex();
2793 int locCovMatrixVxParamIndex = locKinFitParticle->Get_CovMatrixVxParamIndex();
2794 int locCovMatrixTParamIndex = locKinFitParticle->Get_CovMatrixTParamIndex();
2798 cout <<
"SETTING FINAL TRACK INFO: PID = " << locKinFitParticle->Get_PID() << endl;
2799 cout <<
"E, px, vx, t param indices = " << locEParamIndex <<
", " << locPxParamIndex <<
", " << locVxParamIndex <<
", " << locTParamIndex << endl;
2800 cout <<
"E, px, vx, t cov matrix indices = " << locCovMatrixEParamIndex <<
", " << locCovMatrixPxParamIndex <<
", " << locCovMatrixVxParamIndex <<
", " << locCovMatrixTParamIndex << endl;
2801 cout <<
"sizes = " << locCovarianceMatrix.GetNrows() <<
", " << locCovarianceMatrix.GetNcols() <<
", " << locKinFitMatrix.GetNrows() <<
", " << locKinFitMatrix.GetNcols() << endl;
2805 if(locEParamIndex >= 0)
2806 locCovarianceMatrix(locCovMatrixEParamIndex, locCovMatrixEParamIndex) = locKinFitMatrix(locEParamIndex, locEParamIndex);
2807 if(locPxParamIndex >= 0)
2809 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2811 for(
unsigned int loc_k = 0; loc_k < 3; ++loc_k)
2812 locCovarianceMatrix(locCovMatrixPxParamIndex + loc_j, locCovMatrixPxParamIndex + loc_k) = locKinFitMatrix(loc_j + locPxParamIndex, loc_k + locPxParamIndex);
2815 if(locVxParamIndex >= 0)
2817 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2819 for(
unsigned int loc_k = 0; loc_k < 3; ++loc_k)
2820 locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixVxParamIndex + loc_k) = locKinFitMatrix(loc_j + locVxParamIndex, loc_k + locVxParamIndex);
2823 if(locTParamIndex >= 0)
2824 locCovarianceMatrix(locCovMatrixTParamIndex, locCovMatrixTParamIndex) = locKinFitMatrix(locTParamIndex, locTParamIndex);
2827 if((locEParamIndex >= 0) && (locVxParamIndex >= 0))
2829 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2831 locCovarianceMatrix(locCovMatrixEParamIndex + 0, locCovMatrixVxParamIndex + loc_j) = locKinFitMatrix(locEParamIndex + 0, locVxParamIndex + loc_j);
2832 locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixEParamIndex + 0) = locKinFitMatrix(locVxParamIndex + loc_j, locEParamIndex + 0);
2835 else if(!locReconstructedParticleFlag && (locEParamIndex >= 0) && (locVxParamIndex < 0))
2837 double locDenominator =
sqrt(
dVY(locEParamIndex, locEParamIndex));
2838 double locUncertaintyRatio = (fabs(locDenominator) > 0.0) ?
sqrt(locKinFitMatrix(locEParamIndex, locEParamIndex))/locDenominator : 0.0;
2839 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2841 locCovarianceMatrix(locCovMatrixEParamIndex + 0, locCovMatrixVxParamIndex + loc_j) *= locUncertaintyRatio;
2842 locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixEParamIndex + 0) *= locUncertaintyRatio;
2845 else if(!locReconstructedParticleFlag && (locEParamIndex < 0) && (locVxParamIndex >= 0) && (locCovMatrixEParamIndex >= 0))
2847 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2849 double locDenominator =
sqrt(
dVY(locVxParamIndex + loc_j, locVxParamIndex + loc_j));
2850 double locUncertaintyRatio = (fabs(locDenominator) > 0.0) ?
sqrt(locKinFitMatrix(locVxParamIndex + loc_j, locVxParamIndex + loc_j))/locDenominator : 0.0;
2851 locCovarianceMatrix(locCovMatrixEParamIndex + 0, locCovMatrixVxParamIndex + loc_j) *= locUncertaintyRatio;
2852 locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixEParamIndex + 0) *= locUncertaintyRatio;
2857 if((locEParamIndex >= 0) && (locTParamIndex >= 0))
2859 locCovarianceMatrix(locCovMatrixEParamIndex, locCovMatrixTParamIndex) = locKinFitMatrix(locEParamIndex, locTParamIndex);
2860 locCovarianceMatrix(locCovMatrixTParamIndex, locCovMatrixEParamIndex) = locKinFitMatrix(locTParamIndex, locEParamIndex);
2862 else if(!locReconstructedParticleFlag && (locEParamIndex >= 0) && (locTParamIndex < 0))
2864 double locDenominator =
sqrt(
dVY(locEParamIndex, locEParamIndex));
2865 double locUncertaintyRatio = (fabs(locDenominator) > 0.0) ?
sqrt(locKinFitMatrix(locEParamIndex, locEParamIndex))/locDenominator : 0.0;
2866 locCovarianceMatrix(locCovMatrixEParamIndex, locCovMatrixTParamIndex) *= locUncertaintyRatio;
2867 locCovarianceMatrix(locCovMatrixTParamIndex, locCovMatrixEParamIndex) *= locUncertaintyRatio;
2869 else if(!locReconstructedParticleFlag && (locEParamIndex < 0) && (locTParamIndex >= 0) && (locCovMatrixEParamIndex >= 0))
2871 double locDenominator =
sqrt(
dVY(locTParamIndex, locTParamIndex));
2872 double locUncertaintyRatio = (fabs(locDenominator) > 0.0) ?
sqrt(locKinFitMatrix(locTParamIndex, locTParamIndex))/locDenominator : 0.0;
2873 locCovarianceMatrix(locCovMatrixEParamIndex, locCovMatrixTParamIndex) *= locUncertaintyRatio;
2874 locCovarianceMatrix(locCovMatrixTParamIndex, locCovMatrixEParamIndex) *= locUncertaintyRatio;
2878 if((locPxParamIndex >= 0) && (locVxParamIndex >= 0))
2880 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2882 for(
unsigned int loc_k = 0; loc_k < 3; ++loc_k)
2884 locCovarianceMatrix(locCovMatrixPxParamIndex + loc_j, locCovMatrixVxParamIndex + loc_k) = locKinFitMatrix(locPxParamIndex + loc_j, locVxParamIndex + loc_k);
2885 locCovarianceMatrix(locCovMatrixVxParamIndex + loc_k, locCovMatrixPxParamIndex + loc_j) = locKinFitMatrix(locVxParamIndex + loc_k, locPxParamIndex + loc_j);
2889 else if(!locReconstructedParticleFlag && (locPxParamIndex >= 0) && (locVxParamIndex < 0))
2891 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2893 double locDenominator =
sqrt(
dVY(locPxParamIndex + loc_j, locPxParamIndex + loc_j));
2894 double locUncertaintyRatio = (fabs(locDenominator) > 0.0) ?
sqrt(locKinFitMatrix(locPxParamIndex + loc_j, locPxParamIndex + loc_j))/locDenominator : 0.0;
2895 for(
unsigned int loc_k = 0; loc_k < 3; ++loc_k)
2897 locCovarianceMatrix(locCovMatrixPxParamIndex + loc_j, locCovMatrixVxParamIndex + loc_k) *= locUncertaintyRatio;
2898 locCovarianceMatrix(locCovMatrixVxParamIndex + loc_k, locCovMatrixPxParamIndex + loc_j) *= locUncertaintyRatio;
2902 else if(!locReconstructedParticleFlag && (locPxParamIndex < 0) && (locVxParamIndex >= 0))
2904 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2906 double locDenominator =
sqrt(
dVY(locVxParamIndex + loc_j, locVxParamIndex + loc_j));
2907 double locUncertaintyRatio = (fabs(locDenominator) > 0.0) ?
sqrt(locKinFitMatrix(locVxParamIndex + loc_j, locVxParamIndex + loc_j))/locDenominator : 0.0;
2908 for(
unsigned int loc_k = 0; loc_k < 3; ++loc_k)
2910 locCovarianceMatrix(locCovMatrixPxParamIndex + loc_k, locCovMatrixVxParamIndex + loc_j) *= locUncertaintyRatio;
2911 locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixPxParamIndex + loc_k) *= locUncertaintyRatio;
2917 if((locPxParamIndex >= 0) && (locTParamIndex >= 0))
2919 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2921 locCovarianceMatrix(locCovMatrixPxParamIndex + loc_j, locCovMatrixTParamIndex + 0) = locKinFitMatrix(locPxParamIndex + loc_j, locTParamIndex + 0);
2922 locCovarianceMatrix(locCovMatrixTParamIndex + 0, locCovMatrixPxParamIndex + loc_j) = locKinFitMatrix(locTParamIndex + 0, locPxParamIndex + loc_j);
2925 else if(!locReconstructedParticleFlag && (locPxParamIndex >= 0) && (locTParamIndex < 0))
2927 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2929 double locDenominator =
sqrt(
dVY(locPxParamIndex + loc_j, locPxParamIndex + loc_j));
2930 double locUncertaintyRatio = (fabs(locDenominator) > 0.0) ?
sqrt(locKinFitMatrix(locPxParamIndex + loc_j, locPxParamIndex + loc_j))/locDenominator : 0.0;
2931 locCovarianceMatrix(locCovMatrixPxParamIndex + loc_j, locCovMatrixTParamIndex + 0) *= locUncertaintyRatio;
2932 locCovarianceMatrix(locCovMatrixTParamIndex + 0, locCovMatrixPxParamIndex + loc_j) *= locUncertaintyRatio;
2935 else if(!locReconstructedParticleFlag && (locPxParamIndex < 0) && (locTParamIndex >= 0))
2937 double locDenominator =
sqrt(
dVY(locTParamIndex, locTParamIndex));
2938 double locUncertaintyRatio = (fabs(locDenominator) > 0.0) ?
sqrt(locKinFitMatrix(locTParamIndex, locTParamIndex))/locDenominator : 0.0;
2939 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2941 locCovarianceMatrix(locCovMatrixPxParamIndex + loc_j, locCovMatrixTParamIndex + 0) *= locUncertaintyRatio;
2942 locCovarianceMatrix(locCovMatrixTParamIndex + 0, locCovMatrixPxParamIndex + loc_j) *= locUncertaintyRatio;
2947 if((locVxParamIndex >= 0) && (locTParamIndex >= 0))
2949 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2951 locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixTParamIndex + 0) = locKinFitMatrix(locVxParamIndex + loc_j, locTParamIndex + 0);
2952 locCovarianceMatrix(locCovMatrixTParamIndex + 0, locCovMatrixVxParamIndex + loc_j) = locKinFitMatrix(locTParamIndex + 0, locVxParamIndex + loc_j);
2955 else if(!locReconstructedParticleFlag && (locVxParamIndex >= 0) && (locTParamIndex < 0))
2957 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2959 double locDenominator =
sqrt(
dVY(locVxParamIndex + loc_j, locVxParamIndex + loc_j));
2960 double locUncertaintyRatio = (fabs(locDenominator) > 0.0) ?
sqrt(locKinFitMatrix(locVxParamIndex + loc_j, locVxParamIndex + loc_j))/locDenominator : 0.0;
2961 locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixTParamIndex + 0) *= locUncertaintyRatio;
2962 locCovarianceMatrix(locCovMatrixTParamIndex + 0, locCovMatrixVxParamIndex + loc_j) *= locUncertaintyRatio;
2965 else if(!locReconstructedParticleFlag && (locVxParamIndex < 0) && (locTParamIndex >= 0))
2967 double locDenominator =
sqrt(
dVY(locTParamIndex, locTParamIndex));
2968 double locUncertaintyRatio = (fabs(locDenominator) > 0.0) ?
sqrt(locKinFitMatrix(locTParamIndex, locTParamIndex))/locDenominator : 0.0;
2969 for(
unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2971 locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixTParamIndex + 0) *= locUncertaintyRatio;
2972 locCovarianceMatrix(locCovMatrixTParamIndex + 0, locCovMatrixVxParamIndex + loc_j) *= locUncertaintyRatio;
2978 cout <<
"FINAL COV MATRIX:" << endl;
void Reset_NewEvent(void)
bool Get_FitCommonVertexFlag(void) const
char Get_Charge(void) const
char Get_CommonVxParamIndex(void) const
set< shared_ptr< DKinFitParticle > > Get_NoConstrainParticles(void) const
void Calc_dF_Vertex_Decaying_Accel(size_t locFIndex, const DKinFitParticle *locKinFitParticle, const DKinFitParticle *locKinFitParticle_DecayingSource, double locStateSignMultiplier)
set< shared_ptr< DKinFitParticle > > Get_FromInitialState(void) const
TVector3 Calc_VertexParams_P4DerivedAtCommonVertex(const DKinFitParticle *locKinFitParticle)
map< shared_ptr< DKinFitParticle >, set< shared_ptr< DKinFitConstraint > > > dParticleConstraintMap
char Get_FIndex(void) const
unsigned int dMaxNumIterations
double dConvergenceChiSqDiff_LastResort
double dConvergenceChiSqDiff
double Get_Mass(void) const
TLorentzVector Calc_DecayingP4_ByP3Derived(const DKinFitParticle *locKinFitParticle, bool locAtP3DerivedFlag, bool locDontPropagateAtAllFlag=false) const
virtual bool Get_IsBFieldNearBeamline(void) const =0
void Calc_dF_MassDerivs(size_t locFIndex, const DKinFitParticle *locKinFitParticle, TLorentzVector locXP4, double locStateSignMultiplier, bool locIsConstrainedParticle)
bool Get_IsConstrainingVertex(const shared_ptr< DKinFitParticle > &locKinFitParticle) const
void Update_CovarianceMatrices(bool locDecayingParticlesOnlyFlag)
void Recycle_LastFitMemory(void)
void Prepare_ConstraintsAndParticles(void)
unsigned char Get_VertexConstraintFlag(void) const
char Get_FIndex(void) const
virtual TVector3 Get_BField(const TVector3 &locPosition) const =0
set< shared_ptr< DKinFitParticle > > Get_FromFinalState(void) const
virtual void Reset_NewEvent(void)
char Get_PxParamIndex(void) const
void Calc_DecayingParticleJacobian(const DKinFitParticle *locKinFitParticle, bool locDontPropagateDecayingP3Flag, double locStateSignMultiplier, int locNumEta, const map< const DKinFitParticle *, int > &locAdditionalPxParamIndices, TMatrixD &locJacobian) const
shared_ptr< TMatrixFSym > Get_SymMatrixResource(unsigned int locNumMatrixRows)
void Set_FIndex(char locFIndex)
void Resize_Matrices(void)
map< shared_ptr< DKinFitParticle >, map< DKinFitPullType, double > > dPulls
void Recycle_LastFitMemory(set< shared_ptr< DKinFitConstraint >> &locKinFitConstraints)
bool Get_IsNeutralShowerFlag(void) const
virtual bool Validate_Constraints(const set< shared_ptr< DKinFitConstraint >> &locKinFitConstraints) const
bool Propagate_TrackInfoToCommonVertex(const DKinFitParticle *locKinFitParticle, const TMatrixDSym *locVXi, TVector3 &locMomentum, TLorentzVector &locSpacetimeVertex, pair< double, double > &locPathLengthPair, pair< double, double > &locRestFrameLifetimePair, TMatrixFSym *locCovarianceMatrix) const
set< shared_ptr< DKinFitConstraint > > Clone_ParticlesAndConstraints(const set< shared_ptr< DKinFitConstraint >> &locInputConstraints)
void Set_DebugLevel(int locDebugLevel)
virtual void Reset_NewFit(void)
bool Get_UpdateCovarianceMatricesFlag(void) const
DKinFitUtils * dKinFitUtils
char Get_EParamIndex(void) const
char Get_VxParamIndex(void) const
void Calc_dF_Vertex_NotDecaying(size_t locFIndex, const DKinFitParticle *locKinFitParticle)
void Set_FinalTrackInfo(void)
bool Calc_PathLength(const DKinFitParticle *locKinFitParticle, const TMatrixDSym *locVXi, const TMatrixFSym *locCovarianceMatrix, pair< double, double > &locPathLengthPair, pair< double, double > &locRestFrameLifetimePair) const
set< shared_ptr< DKinFitParticle > > Get_FullConstrainParticles(void) const
void Calc_dF_Vertex(size_t locFIndex, const DKinFitParticle *locKinFitParticle, const DKinFitParticle *locKinFitParticle_DecayingSource, double locStateSignMultiplier)
void Calc_dF_Vertex_Decaying_NonAccel(size_t locFIndex, const DKinFitParticle *locKinFitParticle, const DKinFitParticle *locKinFitParticle_DecayingSource, double locStateSignMultiplier)
bool Get_IsTimeConstrained(const shared_ptr< DKinFitParticle > &locKinFitParticle) const
bool Get_IsDecayingParticleDefinedByProducts(const DKinFitParticle *locKinFitParticle) const
DKinFitParticleType Get_KinFitParticleType(void) const
set< shared_ptr< DKinFitConstraint > > dKinFitConstraints
void Calc_dF_P4(int locFIndex, const DKinFitParticle *locKinFitParticle, double locStateSignMultiplier)
TVector3 Get_Position(void) const
TLorentzVector Calc_DecayingP4_ByPosition(const DKinFitParticle *locKinFitParticle, bool locAtPositionFlag, bool locDontPropagateAtAllFlag=false) const
set< shared_ptr< DKinFitParticle > > dKinFitParticles
DKinFitStatus dKinFitStatus
void Print_Matrix(const TMatrixD &locMatrix) const
void Update_ParticleParams(void)
TVector3 Get_CommonVertex(void) const
void Set_MatrixSizes(void)
map< shared_ptr< DKinFitParticle >, set< shared_ptr< DKinFitConstraint > > > dParticleConstraintMap_Direct
TVector3 Get_InitVertexGuess(void) const
TLorentzVector Get_P4(void) const
void Set_DebugLevel(int locDebugLevel)
void Fill_InputMatrices(void)
void Set_FIndex(char locFIndex)
void Calc_Vertex_Params(const DKinFitParticle *locKinFitParticle, double &locJ, TVector3 &locQ, TVector3 &locM, TVector3 &locD)
TVector3 Get_Momentum(void) const
bool Get_VertexP4AtProductionVertex(void) const