Hall-D Software  alpha
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
DKinFitter.cc
Go to the documentation of this file.
1 #include "DKinFitter.h"
2 #include "DKinFitUtils.h"
3 
4 //MAGNETIC FIELD
5  //If the particle is charged, the magnetic field is taken into account if a common vertex is simultaneously kinematically fit
6  //(magnetic field assumed to be constant over the propagation range)
7  //If a common vertex is not defined, the momentum is assumed to be constant
8  //Energy loss between the common vertex and the particle position is ignored
9 
10 //NEUTRAL SHOWERS
11  //For input neutral showers, the momentum is defined-by and updated-with the common vertex (see "Common Vertex Notes")
12  //if a common vertex is not simultaneously kinematically fit for a neutral shower, the fit will fail
13  //To include a neutral shower in a p4 fit without a vertex fit, create a neutral particle from it
14  //with a full 7x7 covaraince matrix and input it instead of the neutral shower
15  //Massive neutral showers (e.g. neutron) cannot be used in vertex constraints: only spacetime constraints. However, photons can.
16  //This is because their momentum is defined by the vertex time
17 
18 //P4 CONSTRAINTS:
19  //Don't do if studying an inclusive channel.
20  //Pick the step of the decay chain that contains the missing or open-ended-decaying particle, to make sure it is constrained
21  //If no missing or open-ended decaying particle, pick the initial step, to make sure the beam information is used
22 
23 //MASS CONSTRAINTS:
24  //Cannot use to constrain the mass of a missing particle: use a p4 constraint instead
25  //Mass of decaying particle constrained by either missing mass or invariant mass
26  //If no missing particles, can constrain either way
27  //By default, prefer using the invariant mass. This is to ensure consistency when combining with a p4 constraint.
28  //Failure to do so could lead to uninvertible matrices, and a failed kinematic fit.
29  //If this particle has a missing decay product (inclusive or otherwise): Constrain by missing mass
30  //If computation by missing mass would have a missing particle (inclusive or otherwise): Constrain by invariant mass
31  //However, the way it is calculated is not defined in these constraints; it is defined when creating the decaying DKinFitParticle
32  //So be sure to create them correctly!
33 
34 //VERTEX CONSTRAINTS:
35  //You can include the beam particle in the common vertex fit, but only do so if it has a non-zero error matrix.
36  //Neutral and missing particles included in the constraint will not be used to constrain the vertex, but will be set with the fit common vertex
37  //This is necessary if you want the neutral particle momentum (from an input shower) to change with the reconstructed vertex
38  //decaying particles should only be used to constrain a fit if the position is defined in another vertex constraint
39  //Massive neutral showers (e.g. neutron) cannot be used in vertex constraints: only spacetime constraints. However, photons can.
40  //This is because their momentum is defined by the vertex time, which is not defined
41 
42 //SPACETIME CONSTRAINTS:
43  //THESE ARE CURRENTLY DISABLED
44  //It is not possible to fit a common time without simultaneously fitting a common vertex.
45  //Requiring a common time at a non-common vertex has no meaning, and the fitter is not setup to input a pre-fit common vertex with uncertainties.
46  //You can include the RF time in the fit by calling the Make_BeamParticle function with the RF information
47  //Missing particles included in the constraint will not be used to constrain the spacetime, but will be set with the fit common spacetime
48  //decaying particles should only be used to constrain a fit if the spacetime is defined in another vertex constraint
49 
50 //OBJECTS & MEMORY:
51  //DKinFitParticle and DKinFitConstraint objects have private constructors: User cannot create them directly
52  //Instead, they are managed by DKinFitUtils
53  //User creates DKinFitParticles, adds them to the constraints
54  //When the fit starts, INTERNALLY ONLY, the particles are cloned and the constraints are cloned to contain the clones
55  //User can resuse particles & constraints objects
56 
57 /**************************************************************** CONSTRUCTOR AND RESET ****************************************************************/
58 
59 DKinFitter::DKinFitter(DKinFitUtils* locKinFitUtils) : dKinFitUtils(locKinFitUtils)
60 {
61  dDebugLevel = 0;
62 
63  dMaxNumIterations = 20;
64  dConvergenceChiSqDiff = 0.001;
66 
67  dKinFitUtils->dKinFitter = this;
69 }
70 
72 {
74  Reset_NewFit();
75 }
76 
78 {
80 
82 
83  dKinFitConstraints.clear();
84  dKinFitParticles.clear();
85  dParticleConstraintMap.clear();
87 
88  dNumXi = 0;
89  dNumEta = 0;
90  dNumF = 0;
91 
92  dChiSq = 0.0;
93  dNDF = 0;
94  dConfidenceLevel = 0.0;
95  dPulls.clear();
96 }
97 
99 {
100  //Cannot be inlined: DKinFitUtils is a forward-declared class
103 }
104 
105 /********************************************************************** UTILITIES **********************************************************************/
106 
107 void DKinFitter::Set_DebugLevel(int locDebugLevel)
108 {
109  dDebugLevel = locDebugLevel;
111 }
112 
113 bool DKinFitter::Get_IsConstrainingVertex(const shared_ptr<DKinFitParticle>& locKinFitParticle) const
114 {
115  auto locConstraints = Get_Constraints<DKinFitConstraint_Vertex>(locKinFitParticle);
116  auto locSetIterator = locConstraints.begin();
117  for(; locSetIterator != locConstraints.end(); ++locSetIterator)
118  {
119  auto locConstrainingParticles = (*locSetIterator)->Get_AllConstrainingParticles();
120  if(locConstrainingParticles.find(locKinFitParticle) != locConstrainingParticles.end())
121  return true;
122  }
123  return false;
124 }
125 
126 bool DKinFitter::Get_IsTimeConstrained(const shared_ptr<DKinFitParticle>& locKinFitParticle) const
127 {
128  auto locConstraints = Get_Constraints<DKinFitConstraint_Spacetime>(locKinFitParticle);
129  auto locSetIterator = locConstraints.begin();
130  for(; locSetIterator != locConstraints.end(); ++locSetIterator)
131  {
132  auto locConstrainedParticles = (*locSetIterator)->Get_AllConstrainingParticles();
133  if(locConstrainedParticles.find(locKinFitParticle) != locConstrainedParticles.end())
134  return true;
135  }
136  return false;
137 }
138 
139 /******************************************************************* INITIALIZATION ********************************************************************/
140 
142 {
143  //Print constraint info
144  if(dDebugLevel > 5)
145  {
146  cout << "DKinFitter: Pre-fit constraint info:" << endl;
147  auto locConstraintIterator = dKinFitConstraints.begin();
148  for(; locConstraintIterator != dKinFitConstraints.end(); ++locConstraintIterator)
149  (*locConstraintIterator)->Print_ConstraintInfo();
150  }
151 
152  //clone constraints & particles
153  //This way the fitter doesn't modify the original, input objects: they can be reused in more fits
154  //Different objects will have to be retrieved to get the output
156 
157  //Print cloned constraint info
158  if(dDebugLevel > 10)
159  {
160  cout << "DKinFitter: Cloned (Fit) constraint info:" << endl;
161  auto locConstraintIterator = dKinFitConstraints.begin();
162  for(; locConstraintIterator != dKinFitConstraints.end(); ++locConstraintIterator)
163  (*locConstraintIterator)->Print_ConstraintInfo();
164  }
165 
166  //Build dKinFitParticles, dParticleConstraintMap & _Direct
167  auto locConstraintIterator = dKinFitConstraints.begin();
168  for(; locConstraintIterator != dKinFitConstraints.end(); ++locConstraintIterator)
169  {
170  auto locKinFitParticles = (*locConstraintIterator)->Get_AllParticles();
171  dKinFitParticles.insert(locKinFitParticles.begin(), locKinFitParticles.end());
172 
173  //loop over the particles
174  auto locParticleIterator = locKinFitParticles.begin();
175  for(; locParticleIterator != locKinFitParticles.end(); ++locParticleIterator)
176  {
177  dParticleConstraintMap[*locParticleIterator].insert(*locConstraintIterator);
178  dParticleConstraintMap_Direct[*locParticleIterator].insert(*locConstraintIterator);
179 
180  if((*locParticleIterator)->Get_KinFitParticleType() != d_DecayingParticle)
181  continue;
182 
183  //now, check for particles that may not directly be used in this constraint, but ARE used to define this decaying particle
184 
185  //this will not be the case if the particle is in a vertex constraint, but only as a no-constrain particle
186  auto locVertexConstraint = std::dynamic_pointer_cast<DKinFitConstraint_Vertex>(*locConstraintIterator);
187  if(locVertexConstraint != NULL)
188  {
189  auto locNoConstrainParticles = locVertexConstraint->Get_NoConstrainParticles();
190  if(locNoConstrainParticles.find(*locParticleIterator) != locNoConstrainParticles.end())
191  continue; //is not used to constrain, so neither is it's p4-defining particles (or at least, not here)
192  }
193 
194  //this decaying particle is directly used in the constraint. all p4-defining particles are then also used (but indirectly)
195  auto locFromAllParticles = (*locParticleIterator)->Get_FromAllParticles();
196  dKinFitParticles.insert(locFromAllParticles.begin(), locFromAllParticles.end());
197 
198  //dParticleConstraintMap
199  auto locDerivingParticleIterator = locFromAllParticles.begin();
200  for(; locDerivingParticleIterator != locFromAllParticles.end(); ++locDerivingParticleIterator)
201  dParticleConstraintMap[*locDerivingParticleIterator].insert(*locConstraintIterator);
202  }
203  }
204 
205  //Set dVertexP4AtProductionVertex on decaying particles
206  //Loop over vertex constraints, searching for decaying particles where their positions are defined
207  auto locVertexConstraints = Get_Constraints<DKinFitConstraint_Vertex>();
208  auto locVertexIterator = locVertexConstraints.begin();
209  for(; locVertexIterator != locVertexConstraints.end(); ++locVertexIterator)
210  {
211  auto locAllConstraintParticles = (*locVertexIterator)->Get_AllParticles();
212  auto locNoConstrainParticles = (*locVertexIterator)->Get_NoConstrainParticles();
213  auto locParticleIterator = locNoConstrainParticles.begin();
214  for(; locParticleIterator != locNoConstrainParticles.end(); ++locParticleIterator)
215  {
216  auto locKinFitParticle = *locParticleIterator;
217  if(locKinFitParticle->Get_KinFitParticleType() != d_DecayingParticle)
218  continue;
219 
220  //We have the decaying particle where it's position is defined (no-constrain)
221  //Now we must determine whether this is the production or decay vertex
222 
223  //See which from-final-state particles are found in the vertex constraint
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()));
228 
229  //Set vertex flag based on whether any particles found or not
230  bool locIsProductionVertex = false;
231  if(dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle.get())) //p4 defined by invariant mass (decay products)
232  locIsProductionVertex = locFinalStateParticlesAtVertex.empty() ? true : false; //if no overlap: not at decay vertex: production
233  else //p4 defined by missing mass
234  locIsProductionVertex = locFinalStateParticlesAtVertex.empty() ? false : true; //if no overlap: not at production vertex: decay
235  locKinFitParticle->Set_VertexP4AtProductionVertex(locIsProductionVertex);
236  }
237  }
238 
239  //Initialize un-set particle data
240 
241  //Common vertices
242  for(locVertexIterator = locVertexConstraints.begin(); locVertexIterator != locVertexConstraints.end(); ++locVertexIterator)
243  (*locVertexIterator)->Set_CommonVertex((*locVertexIterator)->Get_InitVertexGuess());
244 
245  //Common times (init vertex guess already set above)
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());
250 
251  //neutral shower p3
252  auto locParticleIterator = dKinFitParticles.begin();
253  for(; locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
254  {
255  auto locParticle = *locParticleIterator;
256  if(!locParticle->Get_IsNeutralShowerFlag())
257  continue; //only do for neutral showers
258 
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);
265  }
266 
267  //missing/open-ended-decaying p3
268  auto locP4Constraints = Get_Constraints<DKinFitConstraint_P4>();
269  if(!locP4Constraints.empty())
270  {
271  auto locP4Constraint = *(locP4Constraints.begin());
272  auto locKinFitParticle = locP4Constraint->Get_DefinedParticle();
273  if(locKinFitParticle != NULL)
274  locKinFitParticle->Set_Momentum(locP4Constraint->Get_InitP3Guess());
275  }
276 
277  //decaying: p3
278  for(locParticleIterator = dKinFitParticles.begin(); locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
279  {
280  auto locKinFitParticle = *locParticleIterator;
281  if(locKinFitParticle->Get_KinFitParticleType() == d_DecayingParticle)
282  locKinFitParticle->Set_Momentum(dKinFitUtils->Calc_DecayingP4_ByPosition(locKinFitParticle.get(), true).Vect());
283  }
284 
285  //set vertex constraint flags: used if not accelerating
286  for(locVertexIterator = locVertexConstraints.begin(); locVertexIterator != locVertexConstraints.end(); ++locVertexIterator)
287  {
288  auto locFullConstrainParticles = (*locVertexIterator)->Get_FullConstrainParticles();
289  for(locParticleIterator = locFullConstrainParticles.begin(); locParticleIterator != locFullConstrainParticles.end(); ++locParticleIterator)
290  {
291  auto locParticle = *locParticleIterator;
292  TVector3 locMomentum = locParticle->Get_Momentum();
293 
294  unsigned short int locVertexConstraintFlag = 0;
295  if(fabs(locMomentum.Pz()) > fabs(locMomentum.Px()))
296  locVertexConstraintFlag = (fabs(locMomentum.Pz()) > fabs(locMomentum.Py())) ? 1 : 2;
297  else
298  locVertexConstraintFlag = (fabs(locMomentum.Px()) > fabs(locMomentum.Py())) ? 3 : 2;
299  locParticle->Set_VertexConstraintFlag(locVertexConstraintFlag);
300  }
301  }
302 }
303 
305 {
306  //set matrix sizes
307  dNumXi = 0; //num unknowns
308  dNumEta = 0; //num measurables
309  dNumF = 0; //num constraint eqs
310 
311  //Calculate dNumEta
312  auto locParticleIterator = dKinFitParticles.begin();
313  for(; locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
314  {
315  auto locKinFitParticle = *locParticleIterator;
316  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
317  if((locKinFitParticleType == d_MissingParticle) || (locKinFitParticleType == d_DecayingParticle) || (locKinFitParticleType == d_TargetParticle))
318  continue;
319 
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);
324  bool locChargedBFieldFlag = (locKinFitParticle->Get_Charge() != 0) && dKinFitUtils->Get_IsBFieldNearBeamline();
325 
326  if(dDebugLevel >= 20)
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())
329  {
330  if(locIsInP4Constraint || locIsInMassConstraint || Get_IsConstrainingVertex(locKinFitParticle) || locIsIndirectlyInVertexConstraint)
331  dNumEta += 3; //p3
332  if(Get_IsConstrainingVertex(locKinFitParticle) || (locIsIndirectlyInVertexConstraint && locChargedBFieldFlag))
333  dNumEta += 3; //v3 //directly (first condition) or indirectly AND accelerating (second condition)
334  }
335  else //neutral shower
336  {
337  if((locIsInP4Constraint || locIsInMassConstraint) && locIsInVertexConstraint)
338  dNumEta += 4; //E + v3 (p4 fit needs p3, which is derived from v3 + kinfit vertex)
339  }
340  if(Get_IsTimeConstrained(locKinFitParticle))
341  ++dNumEta; //t //directly (first condition) or indirectly AND accelerating (second condition)
342  }
343 
344  //Calculate dNumXi and dNumF
345  auto locConstraintIterator = dKinFitConstraints.begin();
346  for(; locConstraintIterator != dKinFitConstraints.end(); ++locConstraintIterator)
347  {
348  auto locKinFitConstraint_P4 = std::dynamic_pointer_cast<DKinFitConstraint_P4>(*locConstraintIterator);
349  if(locKinFitConstraint_P4 != NULL)
350  {
351  dNumF += 4;
352  if(locKinFitConstraint_P4->Get_DefinedParticle() != NULL)
353  dNumXi += 3; //p3 //missing or open-ended decaying particles
354  continue;
355  }
356 
357  auto locKinFitConstraint_Mass = std::dynamic_pointer_cast<DKinFitConstraint_Mass>(*locConstraintIterator);
358  if(locKinFitConstraint_Mass != NULL)
359  {
360  dNumF += 1;
361  continue;
362  }
363 
364  auto locKinFitConstraint_Vertex = std::dynamic_pointer_cast<DKinFitConstraint_Vertex>(*locConstraintIterator);
365  if(locKinFitConstraint_Vertex != NULL)
366  {
367  dNumXi += 3; //v3
368  dNumF += 2*locKinFitConstraint_Vertex->Get_FullConstrainParticles().size();
369  continue;
370  }
371 
372  auto locKinFitConstraint_Spacetime = std::dynamic_pointer_cast<DKinFitConstraint_Spacetime>(*locConstraintIterator);
373  if(locKinFitConstraint_Spacetime != NULL)
374  {
375  dNumXi += 4; //v3, t
376  dNumF += 3*locKinFitConstraint_Spacetime->Get_FullConstrainParticles().size();
377  dNumF += locKinFitConstraint_Spacetime->Get_OnlyConstrainTimeParticles().size(); //for each neutral shower
378  }
379  }
380 
381  if(dDebugLevel > 10)
382  cout << "DKinFitter: Num measurables, unknowns, constraints = " << dNumEta << ", " << dNumXi << ", " << dNumF << endl;
383 }
384 
386 {
387  if(dF.GetNrows() != static_cast<int>(dNumF))
388  {
389  dF.ResizeTo(dNumF, 1);
390  dS.ResizeTo(dNumF, dNumF);
391  dS_Inverse.ResizeTo(dNumF, dNumF);
392  dLambda.ResizeTo(dNumF, 1);
393  dLambda_T.ResizeTo(1, dNumF);
394  dF_dEta.ResizeTo(dNumF, dNumEta);
395  dF_dEta_T.ResizeTo(dNumEta, dNumF);
396  dF_dXi.ResizeTo(dNumF, dNumXi);
397  dF_dXi_T.ResizeTo(dNumXi, dNumF);
398  }
399  else
400  {
401  if(dF_dEta.GetNcols() != static_cast<int>(dNumEta))
402  {
403  dF_dEta.ResizeTo(dNumF, dNumEta);
404  dF_dEta_T.ResizeTo(dNumEta, dNumF);
405  }
406  if(dF_dXi.GetNcols() != static_cast<int>(dNumXi))
407  {
408  dF_dXi.ResizeTo(dNumF, dNumXi);
409  dF_dXi_T.ResizeTo(dNumXi, dNumF);
410  }
411  }
412 
413  if(dY.GetNrows() != static_cast<int>(dNumEta))
414  {
415  dY.ResizeTo(dNumEta, 1);
416  dEta.ResizeTo(dNumEta, 1);
417  dEpsilon.ResizeTo(dNumEta, 1);
418  dVY.ResizeTo(dNumEta, dNumEta);
419  dVEta.ResizeTo(dNumEta, dNumEta);
420  }
421 
422  if(dXi.GetNrows() != static_cast<int>(dNumXi))
423  {
424  dXi.ResizeTo(dNumXi, 1);
425  dU.ResizeTo(dNumXi, dNumXi);
426  dU_Inverse.ResizeTo(dNumXi, dNumXi);
427  dVXi.ResizeTo(dNumXi, dNumXi);
428  }
429 
430  if(dV.GetNrows() != static_cast<int>(dNumEta + dNumXi))
431  dV.ResizeTo(dNumEta + dNumXi, dNumEta + dNumXi);
432 
433  Zero_Matrices(); //zeroes all class matrices
434 }
435 
437 {
438  dXi.Zero();
439  dEta.Zero();
440  dY.Zero();
441  dVY.Zero();
442  dF.Zero();
443  dEpsilon.Zero();
444 
445  dLambda.Zero();
446  dLambda_T.Zero();
447  dF_dEta.Zero();
448  dF_dEta_T.Zero();
449  dF_dXi.Zero();
450  dF_dXi_T.Zero();
451 
452  dS.Zero();
453  dS_Inverse.Zero();
454  dU.Zero();
455  dU_Inverse.Zero();
456 
457  dVXi.Zero();
458  dVEta.Zero();
459  dV.Zero();
460 }
461 
463 {
464  //fill dY, dEta, dVY, dXi
465 
466  //SETUP dY
467  int locParamIndex = 0;
468  auto locParticleIterator = dKinFitParticles.begin();
469  for(; locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
470  {
471  auto locKinFitParticle = *locParticleIterator;
472  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
473 
474  if((locKinFitParticleType == d_MissingParticle) || (locKinFitParticleType == d_DecayingParticle) || (locKinFitParticleType == d_TargetParticle))
475  continue;
476 
477  TVector3 locMomentum = locKinFitParticle->Get_Momentum();
478  TVector3 locPosition = locKinFitParticle->Get_Position();
479 
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);
484  bool locChargedBFieldFlag = (locKinFitParticle->Get_Charge() != 0) && dKinFitUtils->Get_IsBFieldNearBeamline();
485 
486  if(!locKinFitParticle->Get_IsNeutralShowerFlag()) //non-neutral-shower
487  {
488  if(locIsInP4Constraint || locIsInMassConstraint || Get_IsConstrainingVertex(locKinFitParticle) || locIsIndirectlyInVertexConstraint)
489  {
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();
494  locParamIndex += 3;
495  }
496  if(Get_IsConstrainingVertex(locKinFitParticle) || (locIsIndirectlyInVertexConstraint && locChargedBFieldFlag))
497  {
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();
502  locParamIndex += 3;
503  }
504  }
505  else //neutral shower
506  {
507  if((locIsInP4Constraint || locIsInMassConstraint) && locIsInVertexConstraint)
508  {
509  //E + v3 (p4 fit needs p3, which is derived from v3 + kinfit vertex)
510  locKinFitParticle->Set_EParamIndex(locParamIndex);
511  dY(locParamIndex, 0) = locKinFitParticle->Get_ShowerEnergy();
512  ++locParamIndex;
513 
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();
518  locParamIndex += 3;
519  }
520  }
521 
522  if(Get_IsTimeConstrained(locKinFitParticle))
523  {
524  locKinFitParticle->Set_TParamIndex(locParamIndex);
525  dY(locParamIndex, 0) = locKinFitParticle->Get_Time();
526  ++locParamIndex;
527  }
528  }
529 
530  //SETUP dEta
531  dEta = dY; //use measurements as first guess
532 
533  //SETUP dXi (with initial guesses) and constraint equation indices
534  locParamIndex = 0;
535  int locConstraintIndex = 0;
536  auto locConstraintIterator = dKinFitConstraints.begin();
537  for(; locConstraintIterator != dKinFitConstraints.end(); ++locConstraintIterator)
538  {
539  auto locKinFitConstraint_P4 = std::dynamic_pointer_cast<DKinFitConstraint_P4>(*locConstraintIterator);
540  if(locKinFitConstraint_P4 != NULL)
541  {
542  locKinFitConstraint_P4->Set_FIndex(locConstraintIndex);
543  locConstraintIndex += 4; //p4
544 
545  auto locDefinedParticle = locKinFitConstraint_P4->Get_DefinedParticle();
546  if(locDefinedParticle == NULL)
547  continue; //no missing particles
548 
549  //set initial p3 guess
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);
555  locParamIndex += 3;
556  continue;
557  }
558 
559  auto locKinFitConstraint_Mass = std::dynamic_pointer_cast<DKinFitConstraint_Mass>(*locConstraintIterator);
560  if(locKinFitConstraint_Mass != NULL)
561  {
562  locKinFitConstraint_Mass->Set_FIndex(locConstraintIndex);
563  locConstraintIndex += 1; //mass
564  continue;
565  }
566 
567  auto locKinFitConstraint_Vertex = std::dynamic_pointer_cast<DKinFitConstraint_Vertex>(*locConstraintIterator);
568  auto locKinFitConstraint_Spacetime = std::dynamic_pointer_cast<DKinFitConstraint_Spacetime>(*locConstraintIterator);
569  if((locKinFitConstraint_Vertex != NULL) && (locKinFitConstraint_Spacetime == NULL))
570  {
571  TVector3 locPosition = locKinFitConstraint_Vertex->Get_InitVertexGuess();
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);
576  locParamIndex += 3;
577 
578  auto locFullConstrainParticles = locKinFitConstraint_Vertex->Get_FullConstrainParticles();
579  locParticleIterator = locFullConstrainParticles.begin();
580  for(; locParticleIterator != locFullConstrainParticles.end(); ++locParticleIterator)
581  {
582  locKinFitConstraint_Vertex->Set_FIndex(*locParticleIterator, locConstraintIndex);
583  locConstraintIndex += 2;
584  }
585  continue;
586  }
587 
588  if(locKinFitConstraint_Spacetime != NULL)
589  {
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);
597  locParamIndex += 4;
598 
599  auto locFullConstrainParticles = locKinFitConstraint_Spacetime->Get_FullConstrainParticles();
600  locParticleIterator = locFullConstrainParticles.begin();
601  for(; locParticleIterator != locFullConstrainParticles.end(); ++locParticleIterator)
602  {
603  locKinFitConstraint_Spacetime->Set_FIndex(*locParticleIterator, locConstraintIndex);
604  locConstraintIndex += 3;
605  }
606 
607  auto locOnlyConstrainTimeParticles = locKinFitConstraint_Spacetime->dOnlyConstrainTimeParticles;
608  locParticleIterator = locOnlyConstrainTimeParticles.begin();
609  for(; locParticleIterator != locOnlyConstrainTimeParticles.end(); ++locParticleIterator)
610  {
611  locKinFitConstraint_Spacetime->Set_FIndex(*locParticleIterator, locConstraintIndex);
612  ++locConstraintIndex;
613  }
614 
615  continue;
616  }
617  }
618 
619  //SETUP dVY
620  locParticleIterator = dKinFitParticles.begin();
621  for(; locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
622  {
623  auto locKinFitParticle = *locParticleIterator;
624  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
625 
626  if((locKinFitParticleType == d_DecayingParticle) || (locKinFitParticleType == d_MissingParticle) || (locKinFitParticleType == d_TargetParticle))
627  continue; //uncertainties of target particle momentum are zero
628 
629  TVector3 locMomentum = locKinFitParticle->Get_Momentum();
630  TVector3 locPosition = locKinFitParticle->Get_Position();
631  const TMatrixFSym& locCovarianceMatrix = *(locKinFitParticle->Get_CovarianceMatrix().get());
632 
633  int locPxParamIndex = locKinFitParticle->Get_PxParamIndex();
634  int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
635  int locTParamIndex = locKinFitParticle->Get_TParamIndex();
636  int locEParamIndex = locKinFitParticle->Get_EParamIndex();
637 
638  int locCovMatrixEParamIndex = locKinFitParticle->Get_CovMatrixEParamIndex();
639  int locCovMatrixPxParamIndex = locKinFitParticle->Get_CovMatrixPxParamIndex();
640  int locCovMatrixVxParamIndex = locKinFitParticle->Get_CovMatrixVxParamIndex();
641  int locCovMatrixTParamIndex = locKinFitParticle->Get_CovMatrixTParamIndex();
642 
643  //localized terms (E, p, v, t)
644  if(locEParamIndex >= 0)
645  dVY(locEParamIndex, locEParamIndex) = locCovarianceMatrix(locCovMatrixEParamIndex, locCovMatrixEParamIndex);
646  if(locPxParamIndex >= 0)
647  {
648  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
649  {
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);
652  }
653  }
654  if(locVxParamIndex >= 0)
655  {
656  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
657  {
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);
660  }
661  }
662  if(locTParamIndex >= 0)
663  dVY(locTParamIndex, locTParamIndex) = locCovarianceMatrix(locCovMatrixTParamIndex, locCovMatrixTParamIndex);
664 
665  //cross terms
666  if((locEParamIndex >= 0) && (locVxParamIndex >= 0))
667  {
668  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
669  {
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);
672  }
673  }
674  if((locEParamIndex >= 0) && (locTParamIndex >= 0))
675  {
676  dVY(locEParamIndex, locTParamIndex) = locCovarianceMatrix(locCovMatrixEParamIndex, locCovMatrixTParamIndex);
677  dVY(locTParamIndex, locEParamIndex) = locCovarianceMatrix(locCovMatrixTParamIndex, locCovMatrixEParamIndex);
678  }
679  if((locPxParamIndex >= 0) && (locVxParamIndex >= 0))
680  {
681  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
682  {
683  for(unsigned int loc_k = 0; loc_k < 3; ++loc_k)
684  {
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);
687  }
688  }
689  }
690  if((locPxParamIndex >= 0) && (locTParamIndex >= 0))
691  {
692  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
693  {
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);
696  }
697  }
698  if((locVxParamIndex >= 0) && (locTParamIndex >= 0))
699  {
700  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
701  {
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);
704  }
705  }
706  }
707 
708  if(dDebugLevel > 20)
709  {
710  cout << "DKinFitter: dEta: " << endl;
712  cout << "DKinFitter: dVY: " << endl;
714  cout << "DKinFitter: dXi: " << endl;
716  for(locParticleIterator = dKinFitParticles.begin(); locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
717  (*locParticleIterator)->Print_ParticleParams();
718  }
719 }
720 
721 /********************************************************************* PERFORM FIT *********************************************************************/
722 
724 {
725  //Initialize
728  {
730  return false;
731  }
732  Set_MatrixSizes();
733  Resize_Matrices();
735 
736  //Do the fit
737  if(!Iterate())
738  return false;
739 
740  // Calculate final covariance matrices
741  if(dNumXi > 0)
742  dVXi = dU;
743  Calc_dVdEta();
744 
745  // Calculate fit NDF, Confidence level
746  dNDF = dNumF - dNumXi;
747  dConfidenceLevel = TMath::Prob(dChiSq, dNDF);
748 
749  // Calculate pulls
750  dEpsilon = dY - dEta;
751  Calc_Pulls();
752 
753  // Set final track info
754  Set_FinalTrackInfo(); //Update_ParticleParams() already called: at the end of the last iteration
755 
756  if(dDebugLevel > 5)
757  cout << "DKinFitter: Final dChiSq, dNDF, dConfidenceLevel = " << dChiSq << ", " << dNDF << ", " << dConfidenceLevel << endl;
758 
760  return true;
761 }
762 
764 {
765  dChiSq = 9.9E99;
766  double locPreviousChiSq = 0.0;
767  unsigned int locNumIterations = 0;
768  while((fabs(dChiSq - locPreviousChiSq) > dConvergenceChiSqDiff) || (dChiSq < 0.0))
769  {
770  if(locNumIterations >= dMaxNumIterations)
771  {
772  //sometimes the chisq will walk (very slightly) forever, without any real meaningful change in the variables
773  if(dDebugLevel > 10)
774  cout << "DKinFitter: At maximum number of iterations, this chisq, last chisq, last resort cutoff = " << dChiSq << ", " << locPreviousChiSq << ", " << dConvergenceChiSqDiff_LastResort << endl;
775  if((fabs(dChiSq - locPreviousChiSq) <= dConvergenceChiSqDiff_LastResort) && (dChiSq >= 0.0))
776  break; //close enough
777  if(dDebugLevel > 10)
778  cout << "DKinFitter: Exceeded maximum number of iterations. Returning false." << endl;
780  return false; //diverging!
781  }
782 
783  locPreviousChiSq = dChiSq;
784  if(dDebugLevel > 20)
785  cout << "DKinFitter: Begin iteration" << endl;
786 
787  Calc_dF();
788  if(dDebugLevel > 20)
789  {
790  cout << "DKinFitter: dF: " << endl;
792  cout << "DKinFitter: dF_dXi: " << endl;
794  cout << "DKinFitter: dF_dEta: " << endl;
796  }
797 
798  TMatrixD locR(dF + dF_dEta*(dY - dEta)); //dimensions are dNumF, 1
799 
800  if(!Calc_dS())
801  {
803  return false; // matrix is not invertible
804  }
805 
806  if(dNumXi > 0)
807  {
808  if(!Calc_dU())
809  {
811  return false; // matrix is not invertible
812  }
813 
814  TMatrixD locDeltaXi(-1.0*dU*dF_dXi_T*dS_Inverse*locR); //dimensions are dNumXi, 1
815 
816  dXi += locDeltaXi;
817  if(dDebugLevel > 20)
818  {
819  cout << "DKinFitter: locDeltaXi: " << endl;
820  dKinFitUtils->Print_Matrix(locDeltaXi);
821  cout << "DKinFitter: dXi: " << endl;
823  }
824 
825  dLambda = dS_Inverse*(locR + dF_dXi*locDeltaXi);
826  }
827  else
828  dLambda = dS_Inverse*locR;
829 
830  dLambda_T.Transpose(dLambda);
832 
833  TMatrixDSym locTempMatrix = dS; //similarity (below) destroys the matrix: use a temp to preserve dS
834  dChiSq = (locTempMatrix.SimilarityT(dLambda) + 2.0*dLambda_T*dF)(0, 0);
835 
836  Update_ParticleParams(); //input eta & xi info into particle objects
837 
838  if(dDebugLevel > 20)
839  {
840  cout << "DKinFitter: dLambda: " << endl;
841  dKinFitUtils->Print_Matrix(dLambda);
842  cout << "DKinFitter: dEta: " << endl;
844  cout << "DKinFitter: dChiSq = " << dChiSq << endl;
845  auto locParticleIterator = dKinFitParticles.begin();
846  for(; locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
847  (*locParticleIterator)->Print_ParticleParams();
848  }
849 
850  ++locNumIterations;
851  }
852 
853  return true;
854 }
855 
856 /****************************************************************** CALCULATE MATRICES *****************************************************************/
857 
859 {
860  TMatrixDSym locTempMatrix(dVY);
861  locTempMatrix.Similarity(dF_dEta);
862  dS = locTempMatrix;
863  if(dDebugLevel > 20)
864  {
865  cout << "DKinFitter: dS: " << endl;
867  cout << "determinant magnitude = " << fabs(dS.Determinant()) << endl;
868  }
869 
870  TDecompLU locDecompLU_S(dS);
871  //debugging step: lowering Tol by iterations to pass.
872  if( locDecompLU_S.Decompose() && fabs(dS.Determinant()==0.0) )
873  {
874  if(dDebugLevel > 10) cout << "trying to lower Tol = "<< dS.GetTol() << "\n" << endl;
875  TMatrixD matrixLU_dS = locDecompLU_S.GetLU();
876 
877  //Search for smallest diagonal term in matrixLU_dS
878  Double_t minDiagElement = fabs(matrixLU_dS(0,0));
879  for(int i=1;i<matrixLU_dS.GetNrows();i++)
880  {
881  if(fabs(matrixLU_dS(i,i))<minDiagElement)
882  {
883  minDiagElement = fabs(matrixLU_dS(i,i));
884  }
885  }
886  //Set the new tolerance
887  if(dS.GetTol()>minDiagElement && minDiagElement>1.0E-26)
888  {
889  //cout << "Old Tol = "<< dS.GetTol() << "\n" << endl;
890  dS.SetTol(minDiagElement/2);
891  if(dDebugLevel > 10) cout << "New Tol is set at " << minDiagElement/2 << "\n" << endl;
892  }
893 
894  }
895 
896  //check to make sure that the matrix is decomposable and has a non-zero determinant
897  if((!locDecompLU_S.Decompose()) || (fabs(dS.Determinant()) < 1.0E-300))
898  {
899  if(dDebugLevel > 10)
900  cout << "DKinFitter: dS not invertible. Returning false." << endl;
901  return false; // matrix is not invertible
902  }
903  dS_Inverse = dS;
904  dS_Inverse.Invert();
905  if(dDebugLevel > 20)
906  {
907  cout << "DKinFitter: dS_Inverse: " << endl;
909  }
910 
911  //set dS and dS_Inverse back to standard value
912  dS.SetTol(2.22044604925031308e-16);
913  dS_Inverse.SetTol(2.22044604925031308e-16);
914 
915  return true;
916 }
917 
919 {
920  TMatrixDSym locTempMatrix(dS_Inverse);
921  locTempMatrix.SimilarityT(dF_dXi);
922  dU_Inverse = locTempMatrix;
923  if(dDebugLevel > 20)
924  {
925  cout << "DKinFitter: dU_Inverse: " << endl;
927  cout << "determinant magnitude = " << fabs(dU_Inverse.Determinant()) << endl;
928  }
929  TDecompLU locDecompLU_VXiInv(dU_Inverse);
930  //check to make sure that the matrix is decomposable and has a non-zero determinant
931  if((!locDecompLU_VXiInv.Decompose()) || (fabs(dU_Inverse.Determinant()) < 1.0E-300))
932  {
933  if(dDebugLevel > 10)
934  cout << "DKinFitter: dU_Inverse not invertible. Returning false." << endl;
935  return false; // matrix is not invertible
936  }
937  dU = dU_Inverse;
938  dU.Invert();
939  if(dDebugLevel > 20)
940  {
941  cout << "DKinFitter: dU: " << endl;
943  }
944  return true;
945 }
946 
948 {
949  TMatrixDSym locG = dS_Inverse;
950  locG.SimilarityT(dF_dEta);
951  if(dNumXi == 0)
952  {
953  dVEta = dVY - locG.Similarity(dVY); //destroys locG, but it's not needed anymore
954  dV = dVEta;
955  if(dDebugLevel > 20)
956  {
957  cout << "DKinFitter: dVEta: " << endl;
959  cout << "DKinFitter: dV: " << endl;
961  }
962  return;
963  }
964 
965  TMatrixD locH = dF_dEta_T*dS_Inverse*dF_dXi;
966  TMatrixDSym locTempMatrix11 = dVXi;
967  dVEta = dVY - (locG - locTempMatrix11.Similarity(locH)).Similarity(dVY);
968 
969  //dV:
970  TMatrixD locEtaXiCovariance = -1.0*dVY*locH*dU;
971  for(unsigned int loc_i = 0; loc_i < dNumEta; ++loc_i)
972  {
973  for(unsigned int loc_j = 0; loc_j < dNumEta; ++loc_j)
974  dV(loc_i, loc_j) = dVEta(loc_i, loc_j);
975  }
976  for(unsigned int loc_i = 0; loc_i < dNumXi; ++loc_i)
977  {
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);
980  }
981  for(unsigned int loc_i = 0; loc_i < dNumEta; ++loc_i)
982  {
983  for(unsigned int loc_j = 0; loc_j < dNumXi; ++loc_j)
984  {
985  dV(loc_i, loc_j + dNumEta) = locEtaXiCovariance(loc_i, loc_j);
986  dV(loc_j + dNumEta, loc_i) = locEtaXiCovariance(loc_i, loc_j);
987  }
988  }
989 
990  if(dDebugLevel > 20)
991  {
992  cout << "DKinFitter: dVEta: " << endl;
994  cout << "DKinFitter: dV: " << endl;
996  }
997 }
998 
1000 {
1001  dF.Zero();
1002  dF_dXi.Zero();
1003  dF_dEta.Zero();
1004 
1005  size_t locFIndex = 0;
1006  auto locConstraintIterator = dKinFitConstraints.begin();
1007  for(; locConstraintIterator != dKinFitConstraints.end(); ++locConstraintIterator)
1008  {
1009  auto locKinFitConstraint_P4 = std::dynamic_pointer_cast<DKinFitConstraint_P4>(*locConstraintIterator);
1010  if(locKinFitConstraint_P4 != NULL)
1011  {
1012  int locFIndex = locKinFitConstraint_P4->Get_FIndex();
1013  if(dDebugLevel > 10)
1014  cout << "DKinFitter: P4 Constraint, F index = " << locFIndex << endl;
1015 
1016  //initial state
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);
1021 
1022  //final state
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);
1026 
1027  continue;
1028  }
1029 
1030  auto locKinFitConstraint_Mass = std::dynamic_pointer_cast<DKinFitConstraint_Mass>(*locConstraintIterator);
1031  if(locKinFitConstraint_Mass != NULL)
1032  {
1033  int locFIndex = locKinFitConstraint_Mass->Get_FIndex();
1034  if(dDebugLevel > 10)
1035  cout << "DKinFitter: Mass Constraint, F index = " << locFIndex << endl;
1036 
1037  auto locDecayingParticle = locKinFitConstraint_Mass->Get_DecayingParticle();
1038  double locTargetedMass = locDecayingParticle->Get_Mass();
1039 
1040  TLorentzVector locXP4 = dKinFitUtils->Calc_DecayingP4_ByP3Derived(locDecayingParticle.get(), true);
1041  if(dDebugLevel > 30)
1042  cout << "Final decaying pxyzE is: " << locXP4.Px() << ", " << locXP4.Py() << ", " << locXP4.Pz() << ", " << locXP4.E() << endl;
1043  dF(locFIndex, 0) += locXP4.M2() - locTargetedMass*locTargetedMass;
1044 
1045  Calc_dF_MassDerivs(locFIndex, locDecayingParticle.get(), locXP4, 1.0, true);
1046  continue;
1047  }
1048 
1049  auto locKinFitConstraint_Vertex = std::dynamic_pointer_cast<DKinFitConstraint_Vertex>(*locConstraintIterator);
1050  auto locKinFitConstraint_Spacetime = std::dynamic_pointer_cast<DKinFitConstraint_Spacetime>(*locConstraintIterator);
1051  if((locKinFitConstraint_Vertex != NULL) && (locKinFitConstraint_Spacetime == NULL))
1052  {
1053  auto locFullConstrainParticles = locKinFitConstraint_Vertex->Get_FullConstrainParticles();
1054  auto locParticleIterator = locFullConstrainParticles.begin();
1055  for(; locParticleIterator != locFullConstrainParticles.end(); ++locParticleIterator)
1056  {
1057  auto locKinFitParticle = *locParticleIterator;
1058  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
1059 
1060  bool locIsDecayingFlag = (locKinFitParticleType == d_DecayingParticle);
1061  locFIndex = locKinFitConstraint_Vertex->Get_FIndex(locKinFitParticle);
1062  if(dDebugLevel > 10)
1063  cout << "DKinFitter: F index, locIsDecayingFlag = " << locFIndex << ", " << locIsDecayingFlag << endl;
1064 
1065  Calc_dF_Vertex(locFIndex, locKinFitParticle.get(), NULL, 1.0);
1066  }
1067 
1068  continue;
1069  }
1070 
1071  if(locKinFitConstraint_Spacetime != NULL)
1072  {
1073  //HANDLE HERE WHEN READY!
1074  }
1075  }
1076 
1077  dF_dEta_T.Transpose(dF_dEta);
1078  dF_dXi_T.Transpose(dF_dXi);
1079 }
1080 
1081 /**************************************************************** CALCULATE P4 MATRICES ****************************************************************/
1082 
1083 void DKinFitter::Calc_dF_P4(int locFIndex, const DKinFitParticle* locKinFitParticle, double locStateSignMultiplier)
1084 {
1085  //E, px, py, pz
1086  int locCharge = locKinFitParticle->Get_Charge();
1087  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
1088 
1089  TLorentzVector locP4 = locKinFitParticle->Get_P4();
1090  TVector3 locPosition = locKinFitParticle->Get_Position();
1091  TVector3 locBField = dKinFitUtils->Get_IsBFieldNearBeamline() ? dKinFitUtils->Get_BField(locPosition) : TVector3(0.0, 0.0, 0.0);
1092  TVector3 locCommonVertex = locKinFitParticle->Get_CommonVertex();
1093 
1094  //This section is calculated assuming that the p4 is NEEDED at the COMMON vertex
1095  //if not, need a factor of -1 on delta-x, and on the derivatives wrst the vertices
1096  //e.g. for detected particles, needed p4 is at the common vertex, but it is defined at some other point on its trajectory
1097  //HOWEVER, for decaying particles, this MAY NOT be true: it MAY be defined at the position where it is needed
1098  //Decaying particles technically have two common vertices: where it is produced and where it decays
1099  //So here, the DEFINED vertex is where its position is defined by the other tracks,
1100  //and its COMMON vertex is the vertex where it is used to constrain another vertex
1101  //e.g. g, p -> K+, K+, Xi- Xi- -> pi-, Lambda Lambda -> p, pi-
1102  //Assuming standard constraint setup: p4 constraint is initial step, mass constraints by invariant mass, Xi- vertex defined by K's
1103  //For Xi-, the p3 is defined at its decay vertex (by decay products)
1104  //And the Xi- DEFINED vertex is at its production vertex (from kaons)
1105  //But the p3 is NEEDED at the production vertex, which is where it's DEFINED
1106  //Thus we need a factor of -1
1107  bool locNeedP4AtProductionVertex = dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle); //true if defined by decay products; else by missing mass
1108  double locVertexSignMultiplier = (locNeedP4AtProductionVertex == locKinFitParticle->Get_VertexP4AtProductionVertex()) ? -1.0 : 1.0;
1109  TVector3 locDeltaX = locVertexSignMultiplier*(locCommonVertex - locPosition); //vector points in the OPPOSITE direction of the momentum
1110 
1111  TVector3 locH = locBField.Unit();
1112  double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
1113 
1114  bool locCommonVertexFitFlag = locKinFitParticle->Get_FitCommonVertexFlag();
1115  bool locChargedBFieldFlag = (locCharge != 0) && dKinFitUtils->Get_IsBFieldNearBeamline();
1116  bool locNeutralShowerFlag = locKinFitParticle->Get_IsNeutralShowerFlag();
1117 
1118  int locPxParamIndex = locKinFitParticle->Get_PxParamIndex();
1119  int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
1120  int locEParamIndex = locKinFitParticle->Get_EParamIndex();
1121  int locCommonVxParamIndex = locKinFitParticle->Get_CommonVxParamIndex();
1122 
1123  if((locKinFitParticleType != d_DecayingParticle) || (locPxParamIndex >= 0))
1124  {
1125  //not an enclosed decaying particle. for decaying particles, will instead get p4 from the deriving particles
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();
1130  }
1131 
1132  if(dDebugLevel > 30)
1133  cout << "PID, sign, pxyzE = " << locKinFitParticle->Get_PID() << ", " << locStateSignMultiplier << ", " << locP4.Px() << ", " << locP4.Py() << ", " << locP4.Pz() << ", " << locP4.E() << endl;
1134 
1135  if(locCommonVertexFitFlag && locChargedBFieldFlag && (locKinFitParticleType != d_TargetParticle) && (locKinFitParticleType != d_MissingParticle))
1136  {
1137  //fitting vertex of charged track in magnetic field: momentum changes as function of vertex
1138  //decaying particles: p4 not directly used, deriving particles are: so must propagate if charged
1139  //if initial particle is a "detected" particle (actually a decaying particle treated as detected): still propagate vertex (assume p3/v3 defined at production vertex)
1140 
1141  TVector3 locDeltaXCrossH = locDeltaX.Cross(locH);
1142  if(dDebugLevel > 30)
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();
1147  }
1148 
1149  if(locKinFitParticleType == d_TargetParticle)
1150  {
1151  if(dDebugLevel > 30)
1152  cout << "DKinFitter: Calc_dF_P4() Section 1; PID = " << locKinFitParticle->Get_PID() << endl;
1153  return; //target params are fixed: no partial derivatives
1154  }
1155  else if(locNeutralShowerFlag)
1156  {
1157  if(dDebugLevel > 30)
1158  cout << "DKinFitter: Calc_dF_P4() Section 3; PID = " << locKinFitParticle->Get_PID() << endl;
1159 
1160  dF_dEta(locFIndex, locEParamIndex) = locStateSignMultiplier;
1161 
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();
1166 
1167  TVector3 locDeltaXOverMagDeltaXSq = locDeltaX*(1.0/locDeltaX.Mag2());
1168 
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());
1172 
1173  dF_dEta(locFIndex + 1, locVxParamIndex + 1) = locStateSignMultiplier*locP4.Px()*locDeltaXOverMagDeltaXSq.Y();
1174  dF_dEta(locFIndex + 1, locVxParamIndex + 2) = locStateSignMultiplier*locP4.Px()*locDeltaXOverMagDeltaXSq.Z();
1175 
1176  dF_dEta(locFIndex + 2, locVxParamIndex) = locStateSignMultiplier*locP4.Py()*locDeltaXOverMagDeltaXSq.X();
1177  dF_dEta(locFIndex + 2, locVxParamIndex + 2) = locStateSignMultiplier*locP4.Py()*locDeltaXOverMagDeltaXSq.Z();
1178 
1179  dF_dEta(locFIndex + 3, locVxParamIndex) = locStateSignMultiplier*locP4.Pz()*locDeltaXOverMagDeltaXSq.X();
1180  dF_dEta(locFIndex + 3, locVxParamIndex + 1) = locStateSignMultiplier*locP4.Pz()*locDeltaXOverMagDeltaXSq.Y();
1181 
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);
1185 
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);
1188 
1189  dF_dXi(locFIndex + 2, locCommonVxParamIndex) -= dF_dEta(locFIndex + 2, locVxParamIndex);
1190  dF_dXi(locFIndex + 2, locCommonVxParamIndex + 2) -= dF_dEta(locFIndex + 2, locVxParamIndex + 2);
1191 
1192  dF_dXi(locFIndex + 3, locCommonVxParamIndex) -= dF_dEta(locFIndex + 3, locVxParamIndex);
1193  dF_dXi(locFIndex + 3, locCommonVxParamIndex + 1) -= dF_dEta(locFIndex + 3, locVxParamIndex + 1);
1194  }
1195  else if((locKinFitParticleType == d_MissingParticle) || ((locKinFitParticleType == d_DecayingParticle) && (locPxParamIndex >= 0)))
1196  {
1197  //missing particle or open-ended decaying particle: p3 is unknown
1198  if(dDebugLevel > 30)
1199  cout << "DKinFitter: Calc_dF_P4() Section 4; PID = " << locKinFitParticle->Get_PID() << endl;
1200 
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();
1204 
1205  dF_dXi(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier;
1206  dF_dXi(locFIndex + 2, locPxParamIndex + 1) = locStateSignMultiplier;
1207  dF_dXi(locFIndex + 3, locPxParamIndex + 2) = locStateSignMultiplier;
1208  }
1209  else if(locKinFitParticleType == d_DecayingParticle)
1210  {
1211  //enclosed decaying particle
1212  if(dDebugLevel > 30)
1213  cout << "DKinFitter: Calc_dF_P4() Section 5; PID = " << locKinFitParticle->Get_PID() << endl;
1214  if(locChargedBFieldFlag && locCommonVertexFitFlag)
1215  {
1216  if(dDebugLevel > 30)
1217  cout << "DKinFitter: Calc_dF_P4() Section 5a" << endl;
1218  //vertex factors
1219  //locVertexSignMultiplier is needed because the signs for Vx & CommonVx might be switched
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();
1222 
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();
1225 
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();
1228 
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);
1231 
1232  dF_dXi(locFIndex + 2, locCommonVxParamIndex) -= dF_dXi(locFIndex + 2, locVxParamIndex);
1233  dF_dXi(locFIndex + 2, locCommonVxParamIndex + 2) -= dF_dXi(locFIndex + 2, locVxParamIndex + 2);
1234 
1235  dF_dXi(locFIndex + 3, locCommonVxParamIndex) -= dF_dXi(locFIndex + 3, locVxParamIndex);
1236  dF_dXi(locFIndex + 3, locCommonVxParamIndex + 1) -= dF_dXi(locFIndex + 3, locVxParamIndex + 1);
1237  }
1238 
1239  //replace the decaying particle with the particles it's momentum is derived from
1240  //initial state
1241  auto locFromInitialState = locKinFitParticle->Get_FromInitialState();
1242  auto locParticleIterator = locFromInitialState.begin();
1243  for(; locParticleIterator != locFromInitialState.end(); ++locParticleIterator)
1244  {
1245  if(dDebugLevel > 30)
1246  cout << "decaying, partially replace with init-state PID = " << (*locParticleIterator)->Get_PID() << endl;
1247  auto locNextStateSignMultiplier = dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle) ? -1.0*locStateSignMultiplier : locStateSignMultiplier;
1248  Calc_dF_P4(locFIndex, (*locParticleIterator).get(), locNextStateSignMultiplier);
1249  }
1250 
1251  //final state
1252  auto locFromFinalState = locKinFitParticle->Get_FromFinalState();
1253  for(locParticleIterator = locFromFinalState.begin(); locParticleIterator != locFromFinalState.end(); ++locParticleIterator)
1254  {
1255  if(dDebugLevel > 30)
1256  cout << "decaying, partially replace with final-state PID = " << (*locParticleIterator)->Get_PID() << endl;
1257  //If defined by invariant mass: add p4s of final state particles
1258  //If defined by missing mass: add p4s of init state, subtract final state
1259  auto locNextStateSignMultiplier = dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle) ? locStateSignMultiplier : -1.0*locStateSignMultiplier;
1260  Calc_dF_P4(locFIndex, (*locParticleIterator).get(), locNextStateSignMultiplier);
1261  }
1262  }
1263  else if(locChargedBFieldFlag && locCommonVertexFitFlag && (locKinFitParticleType != d_DecayingParticle) && (locKinFitParticleType != d_MissingParticle))
1264  {
1265  //detected charged particle in b-field (can be beam particle) & in vertex fit
1266  if(dDebugLevel > 30)
1267  cout << "DKinFitter: Calc_dF_P4() Section 2; PID = " << locKinFitParticle->Get_PID() << endl;
1268 
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();
1272 
1273  dF_dEta(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier;
1274  dF_dEta(locFIndex + 2, locPxParamIndex + 1) = locStateSignMultiplier;
1275  dF_dEta(locFIndex + 3, locPxParamIndex + 2) = locStateSignMultiplier;
1276 
1277  dF_dEta(locFIndex + 1, locVxParamIndex + 1) = locStateSignMultiplier*locA*locH.Z();
1278  dF_dEta(locFIndex + 1, locVxParamIndex + 2) = -1.0*locStateSignMultiplier*locA*locH.Y();
1279 
1280  dF_dEta(locFIndex + 2, locVxParamIndex) = -1.0*locStateSignMultiplier*locA*locH.Z();
1281  dF_dEta(locFIndex + 2, locVxParamIndex + 2) = locStateSignMultiplier*locA*locH.X();
1282 
1283  dF_dEta(locFIndex + 3, locVxParamIndex) = locStateSignMultiplier*locA*locH.Y();
1284  dF_dEta(locFIndex + 3, locVxParamIndex + 1) = -1.0*locStateSignMultiplier*locA*locH.X();
1285 
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);
1288 
1289  dF_dXi(locFIndex + 2, locCommonVxParamIndex) -= dF_dEta(locFIndex + 2, locVxParamIndex);
1290  dF_dXi(locFIndex + 2, locCommonVxParamIndex + 2) -= dF_dEta(locFIndex + 2, locVxParamIndex + 2);
1291 
1292  dF_dXi(locFIndex + 3, locCommonVxParamIndex) -= dF_dEta(locFIndex + 3, locVxParamIndex);
1293  dF_dXi(locFIndex + 3, locCommonVxParamIndex + 1) -= dF_dEta(locFIndex + 3, locVxParamIndex + 1);
1294  }
1295  else
1296  {
1297  // either no common vertex constraint, charged and detected but b-field = 0, or neutral particle with pre-ordained vertex (e.g. beam particle)
1298  if(dDebugLevel > 30)
1299  cout << "DKinFitter: Calc_dF_P4() Section 6; PID = " << locKinFitParticle->Get_PID() << endl;
1300 
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();
1304 
1305  dF_dEta(locFIndex + 1, locPxParamIndex) = locStateSignMultiplier;
1306  dF_dEta(locFIndex + 2, locPxParamIndex + 1) = locStateSignMultiplier;
1307  dF_dEta(locFIndex + 3, locPxParamIndex + 2) = locStateSignMultiplier;
1308  }
1309 }
1310 
1311 /*************************************************************** CALCULATE MASS MATRICES ***************************************************************/
1312 
1313 void DKinFitter::Calc_dF_MassDerivs(size_t locFIndex, const DKinFitParticle* locKinFitParticle, TLorentzVector locXP4, double locStateSignMultiplier, bool locIsConstrainedParticle)
1314 {
1315  //E, px, py, pz
1316  int locCharge = locKinFitParticle->Get_Charge();
1317  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
1318 
1319  TLorentzVector locP4 = locKinFitParticle->Get_P4();
1320  TVector3 locPosition = locKinFitParticle->Get_Position();
1321  TVector3 locBField = dKinFitUtils->Get_IsBFieldNearBeamline() ? dKinFitUtils->Get_BField(locPosition) : TVector3(0.0, 0.0, 0.0);
1322  TVector3 locCommonVertex = locKinFitParticle->Get_CommonVertex();
1323 
1324  //This section is calculated assuming that the p4 is NEEDED at the COMMON vertex
1325  //if not, need a factor of -1 on delta-x, and on the derivatives wrst the vertices
1326  //e.g. for detected particles, needed p4 is at the common vertex, but it is defined at some other point on its trajectory
1327  //HOWEVER, for decaying particles, this MAY NOT be true: it MAY be defined at the position where it is needed
1328  //Decaying particles technically have two common vertices: where it is produced and where it decays
1329  //So here, the DEFINED vertex is where its position is defined by the other tracks,
1330  //and its COMMON vertex is the vertex where it is used to constrain another vertex
1331  //e.g. g, p -> K+, K+, Xi- Xi- -> pi-, Lambda Lambda -> p, pi-
1332  //Assuming standard constraint setup: p4 constraint is initial step, mass constraints by invariant mass, Xi- vertex defined by K's
1333  //For Xi-, the p3 is defined at its decay vertex (by decay products)
1334  //And the Xi- DEFINED vertex is at its production vertex (from kaons)
1335  //But the p3 is NEEDED at the production vertex, which is where it's DEFINED
1336  //Thus we need a factor of -1 for the Xi-
1337  bool locNeedP4AtProductionVertex = dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle); //true if defined by decay products; else by missing mass
1338  double locVertexSignMultiplier = (locNeedP4AtProductionVertex == locKinFitParticle->Get_VertexP4AtProductionVertex()) ? -1.0 : 1.0;
1339  TVector3 locDeltaX = locVertexSignMultiplier*(locCommonVertex - locPosition); //vector points in the OPPOSITE direction of the momentum
1340 
1341  TVector3 locH = locBField.Unit();
1342  double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
1343 
1344  bool locCommonVertexFitFlag = locKinFitParticle->Get_FitCommonVertexFlag();
1345  bool locChargedBFieldFlag = (locCharge != 0) && dKinFitUtils->Get_IsBFieldNearBeamline();
1346  bool locNeutralShowerFlag = locKinFitParticle->Get_IsNeutralShowerFlag();
1347 
1348  int locPxParamIndex = locKinFitParticle->Get_PxParamIndex();
1349  int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
1350  int locEParamIndex = locKinFitParticle->Get_EParamIndex();
1351  int locCommonVxParamIndex = locKinFitParticle->Get_CommonVxParamIndex();
1352 
1353  TVector3 locPXCrossH = locXP4.Vect().Cross(locH);
1354 
1355  if(dDebugLevel > 30)
1356  cout << "PID, sign, pxyzE = " << locKinFitParticle->Get_PID() << ", " << locStateSignMultiplier << ", " << locP4.Px() << ", " << locP4.Py() << ", " << locP4.Pz() << ", " << locP4.E() << endl;
1357 
1358  if(locKinFitParticleType == d_TargetParticle)
1359  {
1360  if(dDebugLevel > 30)
1361  cout << "DKinFitter: Calc_dF_MassDerivs() Section 1; PID = " << locKinFitParticle->Get_PID() << endl;
1362  return; //target params are fixed: no partial derivatives
1363  }
1364  else if(locNeutralShowerFlag)
1365  {
1366  if(dDebugLevel > 30)
1367  cout << "DKinFitter: Calc_dF_MassDerivs() Section 3; PID = " << locKinFitParticle->Get_PID() << endl;
1368 
1369  double locEOverPSq = locP4.E()/locP4.Vect().Mag2();
1370  dF_dEta(locFIndex, locEParamIndex) = 2.0*locStateSignMultiplier*(locXP4.E() - locEOverPSq*locXP4.Vect().Dot(locP4.Vect()));
1371 
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);
1376 
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);
1380  }
1381  else if(locKinFitParticleType == d_MissingParticle)
1382  {
1383  //missing or open-ended-decaying particle: p3 is unknown (not derivable)
1384  if(dDebugLevel > 30)
1385  cout << "DKinFitter: Calc_dF_MassDerivs() Section 4; PID = " << locKinFitParticle->Get_PID() << endl;
1386 
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());
1390  }
1391  else if(locKinFitParticleType == d_DecayingParticle)
1392  {
1393  //enclosed decaying particle
1394  if(dDebugLevel > 30)
1395  cout << "DKinFitter: Calc_dF_MassDerivs() Section 5; PID = " << locKinFitParticle->Get_PID() << endl;
1396  if(locChargedBFieldFlag && locCommonVertexFitFlag && !locIsConstrainedParticle)
1397  {
1398  //if locKinFitSubConstraint_P4 is NULL is first particle; don't include: replace it
1399  if(dDebugLevel > 30)
1400  cout << "DKinFitter: Calc_dF_MassDerivs() Section 5a" << endl;
1401 
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();
1405 
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);
1409  }
1410 
1411  //replace the decaying particle with the particles it's momentum is derived from
1412  //initial state
1413  auto locFromInitialState = locKinFitParticle->Get_FromInitialState();
1414  auto locParticleIterator = locFromInitialState.begin();
1415  for(; locParticleIterator != locFromInitialState.end(); ++locParticleIterator)
1416  {
1417  if(dDebugLevel > 30)
1418  cout << "decaying, partially replace with init-state PID = " << (*locParticleIterator)->Get_PID() << endl;
1419  auto locNextStateSignMultiplier = dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle) ? -1.0*locStateSignMultiplier : locStateSignMultiplier;
1420  Calc_dF_MassDerivs(locFIndex, (*locParticleIterator).get(), locXP4, locNextStateSignMultiplier, false);
1421  }
1422 
1423  //final state
1424  auto locFromFinalState = locKinFitParticle->Get_FromFinalState();
1425  for(locParticleIterator = locFromFinalState.begin(); locParticleIterator != locFromFinalState.end(); ++locParticleIterator)
1426  {
1427  if(dDebugLevel > 30)
1428  cout << "decaying, partially replace with final-state PID = " << (*locParticleIterator)->Get_PID() << endl;
1429  //If defined by invariant mass: add p4s of final state particles
1430  //If defined by missing mass: add p4s of init state, subtract final state
1431  auto locNextStateSignMultiplier = dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle) ? locStateSignMultiplier : -1.0*locStateSignMultiplier;
1432  Calc_dF_MassDerivs(locFIndex, (*locParticleIterator).get(), locXP4, locNextStateSignMultiplier, false);
1433  }
1434  }
1435  else if(locChargedBFieldFlag && locCommonVertexFitFlag && (locKinFitParticleType != d_DecayingParticle) && (locKinFitParticleType != d_MissingParticle))
1436  {
1437  //detected charged particle in b-field (can be beam particle) & in vertex fit
1438  if(dDebugLevel > 30)
1439  cout << "DKinFitter: Calc_dF_MassDerivs() Section 2; PID = " << locKinFitParticle->Get_PID() << endl;
1440 
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());
1444 
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();
1448 
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);
1452  }
1453  else
1454  {
1455  // either no common vertex constraint, charged and detected but b-field = 0, or neutral particle with pre-ordained vertex (e.g. beam particle)
1456  if(dDebugLevel > 30)
1457  cout << "DKinFitter: Calc_dF_MassDerivs() Section 6; PID = " << locKinFitParticle->Get_PID() << endl;
1458 
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());
1462  }
1463 }
1464 
1465 /************************************************************** CALCULATE VERTEX MATRICES **************************************************************/
1466 
1467 void DKinFitter::Calc_dF_Vertex(size_t locFIndex, const DKinFitParticle* locKinFitParticle, const DKinFitParticle* locKinFitParticle_DecayingSource, double locStateSignMultiplier)
1468 {
1469  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
1470 
1471  if((locKinFitParticleType == d_TargetParticle) || (locKinFitParticleType == d_MissingParticle))
1472  {
1473  if(dDebugLevel > 30)
1474  cout << "DKinFitter: Calc_dF_Vertex() Section 1; PID = " << locKinFitParticle->Get_PID() << endl;
1475  return; //no partial derivatives
1476  }
1477 
1478  if(locKinFitParticle_DecayingSource == NULL) //this particle is directly at the vertex
1479  Calc_dF_Vertex_NotDecaying(locFIndex, locKinFitParticle);
1480  else //this particle is used to define the momentum of a decaying particle
1481  {
1482  int locCharge = locKinFitParticle_DecayingSource->Get_Charge();
1483  if((locCharge != 0) && dKinFitUtils->Get_IsBFieldNearBeamline())
1484  Calc_dF_Vertex_Decaying_Accel(locFIndex, locKinFitParticle, locKinFitParticle_DecayingSource, locStateSignMultiplier);
1485  else
1486  Calc_dF_Vertex_Decaying_NonAccel(locFIndex, locKinFitParticle, locKinFitParticle_DecayingSource, locStateSignMultiplier);
1487  }
1488 
1489  if(locKinFitParticleType != d_DecayingParticle)
1490  return;
1491 
1492  //replace the decaying particle with the particles it's momentum is derived from
1493  //initial state
1494  auto locFromInitialState = locKinFitParticle->Get_FromInitialState();
1495  auto locParticleIterator = locFromInitialState.begin();
1496  for(; locParticleIterator != locFromInitialState.end(); ++locParticleIterator)
1497  {
1498  auto locNextStateSignMultiplier = dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle) ? -1.0*locStateSignMultiplier : locStateSignMultiplier;
1499  if(dDebugLevel > 30)
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);
1502  }
1503 
1504  //final state
1505  auto locFromFinalState = locKinFitParticle->Get_FromFinalState();
1506  for(locParticleIterator = locFromFinalState.begin(); locParticleIterator != locFromFinalState.end(); ++locParticleIterator)
1507  {
1508  //If defined by invariant mass: add p4s of final state particles
1509  //If defined by missing mass: add p4s of init state, subtract final state
1510  auto locNextStateSignMultiplier = dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle) ? locStateSignMultiplier : -1.0*locStateSignMultiplier;
1511  if(dDebugLevel > 30)
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);
1514  }
1515 }
1516 
1517 void DKinFitter::Calc_dF_Vertex_NotDecaying(size_t locFIndex, const DKinFitParticle* locKinFitParticle)
1518 {
1519  //The input particle is directly at the vertex being constrained
1520  //The input particle ITSELF may be decaying
1521  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
1522  int locCharge = locKinFitParticle->Get_Charge();
1523 
1524  TVector3 locPosition = locKinFitParticle->Get_Position();
1525  TVector3 locCommonVertex = locKinFitParticle->Get_CommonVertex();
1526  TVector3 locDeltaX = locCommonVertex - locPosition;
1527  TVector3 locMomentum = locKinFitParticle->Get_Momentum();
1528 
1529  int locPxParamIndex = locKinFitParticle->Get_PxParamIndex();
1530  int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
1531  int locCommonVxParamIndex = locKinFitParticle->Get_CommonVxParamIndex();
1532  TVector3 locBField = dKinFitUtils->Get_BField(locPosition);
1533  bool IsNonZeroBField = (locBField.Mag()>1e-4)?true:false;
1534 
1535  if((locCharge != 0) && IsNonZeroBField && ((locKinFitParticleType == d_DetectedParticle) || (locKinFitParticleType == d_BeamParticle))){
1536  //detected charged particle in b-field (can be beam particle)
1537  if(dDebugLevel > 30)
1538  cout << "DKinFitter: Calc_dF_Vertex() Section 2; PID = " << locKinFitParticle->Get_PID() << endl;
1539 
1540  TVector3 locH = locBField.Unit();
1541  TVector3 locPCrossH = locMomentum.Cross(locH);
1542  TVector3 locPCrossDeltaX = locMomentum.Cross(locDeltaX);
1543  TVector3 locDeltaXCrossH = locDeltaX.Cross(locH);
1544 
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;
1554 
1555  // Guard against division by zero errors
1556  if (fabs(locPDotH)<1e-15){
1557  locPDotH+=(locPDotH>0.)?1e-15:-1e-15;
1558  }
1559 
1560  dF(locFIndex, 0) = locDeltaXDotH*locPDotH - locDeltaX.Dot(locMomentum)
1561  + PsqMinusPDotHsq*sinRhoS/locA;
1562  dF(locFIndex, 1) = -locPCrossDeltaX.Dot(locH)
1563  + PsqMinusPDotHsq*OneMinusCosRhoS/locA;
1564 
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();
1570 
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();
1577 
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();
1583 
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();
1590 
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);
1594 
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);
1598  }
1599  else if((locCharge != 0) && IsNonZeroBField && (locKinFitParticleType == d_DecayingParticle)){
1600  //constraining this decaying charged particle in b-field //one-time contributions from decaying particle, does not include the particles it is replaced by (elsewhere)
1601  if(dDebugLevel > 30)
1602  cout << "DKinFitter: Calc_dF_Vertex() Section 3; PID = " << locKinFitParticle->Get_PID() << endl;
1603 
1604  TVector3 locH = locBField.Unit();
1605  double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
1606 
1607  TVector3 locPCrossH = locMomentum.Cross(locH);
1608  TVector3 locPCrossDeltaX = locMomentum.Cross(locDeltaX);
1609  double locDeltaXDotH = locDeltaX.Dot(locH);
1610  double locPDotH = locMomentum.Dot(locH);
1611 
1612  //true if the object's p3, & x4 are defined at its production vertex (& common x4 is at decay vertex).
1613  bool locVertexP4AtProductionVertexFlag = locKinFitParticle->Get_VertexP4AtProductionVertex();
1614 
1615  //true if defined by decay products
1616  bool locP4DefinedByInvariantMassFlag = dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle);
1617 
1618  //Tricky when: P4-define step = common-vertex step
1619  //In other words: P4 calculated by particles at a different vertex where
1620  //the position is defined
1621  //Main case: The Sigma+ in: g, p -> K0, Sigma+ K0 -> pi+, pi- Sigma+ -> p, pi0
1622  bool locP4DerivedAtCommonVertexFlag = (locP4DefinedByInvariantMassFlag == locVertexP4AtProductionVertexFlag);
1623 
1624  if(locP4DerivedAtCommonVertexFlag) //Tricky case
1625  {
1626  TVector3 locQ, locM, locD;
1627  double locJ;
1628  Calc_Vertex_Params(locKinFitParticle, locJ, locQ, locM, locD);
1629 
1630  dF(locFIndex, 0) = locPCrossDeltaX.Dot(locH) - 0.5*locA*(locDeltaX.Mag2() - locDeltaXDotH*locDeltaXDotH);
1631  dF(locFIndex + 1, 0) = locDeltaXDotH - (locPDotH/locA)*asin(locJ);
1632 
1633  TVector3 locR = Calc_VertexParams_P4DerivedAtCommonVertex(locKinFitParticle);
1634 
1635  dF_dXi(locFIndex, locVxParamIndex) += locPCrossH.X();
1636  dF_dXi(locFIndex, locVxParamIndex + 1) += locPCrossH.Y();
1637  dF_dXi(locFIndex, locVxParamIndex + 2) += locPCrossH.Z();
1638 
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();
1642  }
1643  else{
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;
1650 
1651  // Guard against division by zero errors
1652  if (fabs(locPDotH)<1e-15){
1653  locPDotH+=(locPDotH>0.)?1e-15:-1e-15;
1654  }
1655 
1656  dF(locFIndex, 0) = locDeltaXDotH*locPDotH - locDeltaX.Dot(locMomentum)
1657  + PsqMinusPDotHsq*sinRhoS/locA;
1658  dF(locFIndex, 1) = -locPCrossDeltaX.Dot(locH)
1659  + PsqMinusPDotHsq*OneMinusCosRhoS/locA;
1660 
1661  TVector3 dF0_dEta_VxVec = locMomentum
1662  - (locPDotH + PsqMinusPDotHsq*cosRhoS/locPDotH)*locH;
1663  TVector3 dF1_dEta_VxVec = -locPCrossH
1664  - PsqMinusPDotHsq*sinRhoS/locPDotH*locH;
1665 
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();
1669 
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();
1673  }
1674 
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);
1678 
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);
1682  }
1683  else if(locKinFitParticleType == d_DecayingParticle) //non-accel decaying particle
1684  {
1685  //constraining this decaying non-accel particle //one-time contributions from decaying particle, does not include the particles it is replaced by (elsewhere)
1686 
1687  unsigned short int locVertexConstraintFlag = locKinFitParticle->Get_VertexConstraintFlag();
1688  if(locVertexConstraintFlag == 1) //1 & 2 //pz is largest
1689  {
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();
1692 
1693  if(dDebugLevel > 30)
1694  cout << "DKinFitter: Calc_dF_Vertex() Section 4a; PID = " << locKinFitParticle->Get_PID() << endl;
1695 
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();
1700 
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);
1705  }
1706  else if(locVertexConstraintFlag == 2) //1 & 3 //py is largest
1707  {
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();
1710 
1711  if(dDebugLevel > 30)
1712  cout << "DKinFitter: Calc_dF_Vertex() Section 4b; PID = " << locKinFitParticle->Get_PID() << endl;
1713 
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();
1718 
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);
1723  }
1724  else //2 & 3 //px is largest
1725  {
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();
1728 
1729  if(dDebugLevel > 30)
1730  cout << "DKinFitter: Calc_dF_Vertex() Section 4c; PID = " << locKinFitParticle->Get_PID() << endl;
1731 
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();
1736 
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);
1741  }
1742  }
1743  else //neutral detected or beam particle, or no magnetic field
1744  {
1745  unsigned short int locVertexConstraintFlag = locKinFitParticle->Get_VertexConstraintFlag();
1746  if(locVertexConstraintFlag == 1) //1 & 2 //pz is largest
1747  {
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();
1750 
1751  if(dDebugLevel > 30)
1752  cout << "DKinFitter: Calc_dF_Vertex() Section 5a; PID = " << locKinFitParticle->Get_PID() << endl;
1753 
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();
1758 
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();
1763 
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);
1768  }
1769  else if(locVertexConstraintFlag == 2) //1 & 3 //py is largest
1770  {
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();
1773 
1774  if(dDebugLevel > 30)
1775  cout << "DKinFitter: Calc_dF_Vertex() Section 5b; PID = " << locKinFitParticle->Get_PID() << endl;
1776 
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();
1781 
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();
1786 
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);
1791  }
1792  else //2 & 3 //px is largest
1793  {
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();
1796 
1797  if(dDebugLevel > 30)
1798  cout << "DKinFitter: Calc_dF_Vertex() Section 5c; PID = " << locKinFitParticle->Get_PID() << endl;
1799 
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();
1804 
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();
1809 
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);
1814  }
1815  }
1816 }
1817 
1818 void DKinFitter::Calc_dF_Vertex_Decaying_Accel(size_t locFIndex, const DKinFitParticle* locKinFitParticle, const DKinFitParticle* locKinFitParticle_DecayingSource, double locStateSignMultiplier)
1819 {
1820  //The input particle is used to (perhaps indirectly) define a decaying particle at the vertex being constrained
1821  //The decaying particle that this particle directly defines is accelerating: charged and in a magnetic field
1822  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
1823  int locCharge = locKinFitParticle->Get_Charge();
1824 
1825  double locEnergy = locKinFitParticle->Get_P4().E();
1826  TVector3 locPosition = locKinFitParticle->Get_Position();
1827  TVector3 locCommonVertex = locKinFitParticle->Get_CommonVertex();
1828  TVector3 locDeltaX = locCommonVertex - locPosition;
1829  TVector3 locMomentum = locKinFitParticle->Get_Momentum();
1830 
1831  int locEParamIndex = locKinFitParticle->Get_EParamIndex();
1832  int locPxParamIndex = locKinFitParticle->Get_PxParamIndex();
1833  int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
1834  int locCommonVxParamIndex = locKinFitParticle->Get_CommonVxParamIndex();
1835 
1836  if((locCharge != 0) && dKinFitUtils->Get_IsBFieldNearBeamline() && ((locKinFitParticleType == d_DetectedParticle) || (locKinFitParticleType == d_BeamParticle)))
1837  {
1838  //detected charged particle in b-field (can be beam particle)
1839  if(dDebugLevel > 30)
1840  cout << "DKinFitter: Calc_dF_Vertex() Section 6; PID = " << locKinFitParticle->Get_PID() << endl;
1841 
1842  TVector3 locQ, locM, locD;
1843  double locJ;
1844  Calc_Vertex_Params(locKinFitParticle_DecayingSource, locJ, locQ, locM, locD);
1845 
1846  TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->Get_Position();
1847  TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->Get_CommonVertex();
1848  TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
1849 
1850  TVector3 locBField_DecayingSource = dKinFitUtils->Get_BField(locPosition_DecayingSource);
1851  TVector3 locH_DecayingSource = locBField_DecayingSource.Unit();
1852  TVector3 locBField = dKinFitUtils->Get_BField(locPosition);
1853  TVector3 locH = locBField.Unit();
1854  double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
1855 
1856  TVector3 locDeltaXCrossH_DecayingSource = locDeltaX_DecayingSource.Cross(locH_DecayingSource);
1857  TVector3 locDeltaXCrossH_DecayingSource_CrossH = locDeltaXCrossH_DecayingSource.Cross(locH);
1858  TVector3 locQCrossH = locQ.Cross(locH);
1859 
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();
1863 
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();
1867 
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();
1871 
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();
1875 
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);
1879 
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);
1883  }
1884  else if(locKinFitParticle->Get_IsNeutralShowerFlag() && (locKinFitParticle->Get_Mass() < 0.00001))
1885  {
1886  if(dDebugLevel > 30)
1887  cout << "DKinFitter: Calc_dF_Vertex() Section 7; PID = " << locKinFitParticle->Get_PID() << endl;
1888 
1889  TVector3 locQ, locM, locD;
1890  double locJ;
1891  Calc_Vertex_Params(locKinFitParticle, locJ, locQ, locM, locD);
1892 
1893  TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->Get_Position();
1894  TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->Get_CommonVertex();
1895  TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
1896 
1897  TVector3 locBField_DecayingSource = dKinFitUtils->Get_BField(locPosition_DecayingSource);
1898  TVector3 locH_DecayingSource = locBField_DecayingSource.Unit();
1899 
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());
1903 
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);
1908 
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);
1913 
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);
1917 
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);
1921  }
1922  else if(locKinFitParticle->Get_IsNeutralShowerFlag() && (locKinFitParticle->Get_Mass() >= 0.00001))
1923  {
1924  return;
1925  }
1926  else if((locKinFitParticleType == d_DetectedParticle) || (locKinFitParticleType == d_BeamParticle))
1927  {
1928  if(dDebugLevel > 30)
1929  cout << "DKinFitter: Calc_dF_Vertex() Section 8; PID = " << locKinFitParticle->Get_PID() << endl;
1930 
1931  //detected/beam, non-accelerating particle
1932  TVector3 locQ, locM, locD;
1933  double locJ;
1934  Calc_Vertex_Params(locKinFitParticle_DecayingSource, locJ, locQ, locM, locD);
1935 
1936  TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->Get_Position();
1937  TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->Get_CommonVertex();
1938  TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
1939 
1940  TVector3 locBField_DecayingSource = dKinFitUtils->Get_BField(locPosition_DecayingSource);
1941  TVector3 locH_DecayingSource = locBField_DecayingSource.Unit();
1942 
1943  TVector3 locDeltaXCrossH_DecayingSource = locDeltaX_DecayingSource.Cross(locH_DecayingSource);
1944 
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();
1948 
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();
1952  }
1953  else if((locKinFitParticleType == d_MissingParticle) || ((locKinFitParticleType == d_DecayingParticle) && (locKinFitParticle->Get_PxParamIndex() >= 0)))
1954  {
1955  //missing, or open-ended decaying particle
1956  if(dDebugLevel > 30)
1957  cout << "DKinFitter: Calc_dF_Vertex() Section 9; PID = " << locKinFitParticle->Get_PID() << endl;
1958 
1959  TVector3 locQ, locM, locD;
1960  double locJ;
1961  Calc_Vertex_Params(locKinFitParticle_DecayingSource, locJ, locQ, locM, locD);
1962 
1963  TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->Get_Position();
1964  TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->Get_CommonVertex();
1965  TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
1966 
1967  TVector3 locBField_DecayingSource = dKinFitUtils->Get_BField(locPosition_DecayingSource);
1968  TVector3 locH_DecayingSource = locBField_DecayingSource.Unit();
1969 
1970  TVector3 locDeltaXCrossH_DecayingSource = locDeltaX_DecayingSource.Cross(locH_DecayingSource);
1971 
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();
1975 
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();
1979  }
1980  else if((locCharge != 0) && dKinFitUtils->Get_IsBFieldNearBeamline() && (locKinFitParticleType == d_DecayingParticle))
1981  {
1982  //decaying charged particle in b-field (doesn't include contributions from the particles it is replaced by here)
1983  if(dDebugLevel > 30)
1984  cout << "DKinFitter: Calc_dF_Vertex() Section 10; PID = " << locKinFitParticle->Get_PID() << endl;
1985 
1986  TVector3 locQ, locM, locD;
1987  double locJ;
1988  Calc_Vertex_Params(locKinFitParticle_DecayingSource, locJ, locQ, locM, locD);
1989 
1990  TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->Get_Position();
1991  TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->Get_CommonVertex();
1992  TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
1993 
1994  TVector3 locBField_DecayingSource = dKinFitUtils->Get_BField(locPosition_DecayingSource);
1995  TVector3 locH_DecayingSource = locBField_DecayingSource.Unit();
1996  TVector3 locBField = dKinFitUtils->Get_BField(locPosition);
1997  TVector3 locH = locBField.Unit();
1998  double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
1999 
2000  TVector3 locDeltaXCrossH_DecayingSource = locDeltaX_DecayingSource.Cross(locH_DecayingSource);
2001  TVector3 locDeltaXCrossH_DecayingSource_CrossH = locDeltaXCrossH_DecayingSource.Cross(locH);
2002  TVector3 locQCrossH = locQ.Cross(locH);
2003 
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();
2007 
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();
2011 
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);
2015 
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);
2019  }
2020 }
2021 
2022 void DKinFitter::Calc_dF_Vertex_Decaying_NonAccel(size_t locFIndex, const DKinFitParticle* locKinFitParticle, const DKinFitParticle* locKinFitParticle_DecayingSource, double locStateSignMultiplier)
2023 {
2024  //The input particle is used to (perhaps indirectly) define a decaying particle at the vertex being constrained
2025  //The decaying particle that this particle directly defines is not accelerating: either neutral or not in a magnetic field
2026  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
2027  int locCharge = locKinFitParticle->Get_Charge();
2028 
2029  double locEnergy = locKinFitParticle->Get_P4().E();
2030  TVector3 locPosition = locKinFitParticle->Get_Position();
2031  TVector3 locCommonVertex = locKinFitParticle->Get_CommonVertex();
2032  TVector3 locDeltaX = locCommonVertex - locPosition;
2033  TVector3 locMomentum = locKinFitParticle->Get_Momentum();
2034 
2035  int locEParamIndex = locKinFitParticle->Get_EParamIndex();
2036  int locPxParamIndex = locKinFitParticle->Get_PxParamIndex();
2037  int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
2038  int locCommonVxParamIndex = locKinFitParticle->Get_CommonVxParamIndex();
2039 
2040  if((locCharge != 0) && dKinFitUtils->Get_IsBFieldNearBeamline() && ((locKinFitParticleType == d_DetectedParticle) || (locKinFitParticleType == d_BeamParticle)))
2041  {
2042  //detected charged particle in b-field (can be beam particle)
2043  TVector3 locBField = dKinFitUtils->Get_BField(locPosition);
2044  double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
2045  TVector3 locH = locBField.Unit();
2046 
2047  TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->Get_Position();
2048  TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->Get_CommonVertex();
2049  TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
2050 
2051  unsigned short int locVertexConstraintFlag = locKinFitParticle->Get_VertexConstraintFlag();
2052  if(locVertexConstraintFlag == 1) //1 & 2 //pz is largest
2053  {
2054  if(dDebugLevel > 30)
2055  cout << "DKinFitter: Calc_dF_Vertex() Section 11a; PID = " << locKinFitParticle->Get_PID() << endl;
2056 
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();
2061 
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();
2068  }
2069  else if(locVertexConstraintFlag == 2) //1 & 3 //py is largest
2070  {
2071  if(dDebugLevel > 30)
2072  cout << "DKinFitter: Calc_dF_Vertex() Section 11b; PID = " << locKinFitParticle->Get_PID() << endl;
2073 
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();
2078 
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());
2085  }
2086  else //2 & 3 //px is largest
2087  {
2088  if(dDebugLevel > 30)
2089  cout << "DKinFitter: Calc_dF_Vertex() Section 11c; PID = " << locKinFitParticle->Get_PID() << endl;
2090 
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();
2095 
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());
2102  }
2103 
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);
2107 
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);
2111  }
2112  else if(locKinFitParticle->Get_IsNeutralShowerFlag() && (locKinFitParticle->Get_Mass() < 0.00001))
2113  {
2114  TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->Get_Position();
2115  TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->Get_CommonVertex();
2116  TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
2117 
2118  TVector3 locPiCrossDeltaXX = locMomentum.Cross(locDeltaX_DecayingSource);
2119  TVector3 locDeltaXCrossDeltaXX = locDeltaX.Cross(locDeltaX_DecayingSource);
2120  double locDeltaXiMagSq = locDeltaX.Mag2();
2121 
2122  unsigned short int locVertexConstraintFlag = locKinFitParticle->Get_VertexConstraintFlag();
2123  if(locVertexConstraintFlag == 1) //1 & 2 //pz is largest
2124  {
2125  if(dDebugLevel > 30)
2126  cout << "DKinFitter: Calc_dF_Vertex() Section 12a; PID = " << locKinFitParticle->Get_PID() << endl;
2127 
2128  dF_dEta(locFIndex, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.X()/locMomentum.Mag2();
2129  dF_dEta(locFIndex + 1, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.Y()/locMomentum.Mag2();
2130 
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();
2137  }
2138  else if(locVertexConstraintFlag == 2) //1 & 3 //py is largest
2139  {
2140  if(dDebugLevel > 30)
2141  cout << "DKinFitter: Calc_dF_Vertex() Section 12b; PID = " << locKinFitParticle->Get_PID() << endl;
2142 
2143  dF_dEta(locFIndex, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.X()/locMomentum.Mag2();
2144  dF_dEta(locFIndex + 1, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.Z()/locMomentum.Mag2();
2145 
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;
2152  }
2153  else //2 & 3 //px is largest
2154  {
2155  if(dDebugLevel > 30)
2156  cout << "DKinFitter: Calc_dF_Vertex() Section 12c; PID = " << locKinFitParticle->Get_PID() << endl;
2157 
2158  dF_dEta(locFIndex, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.Y()/locMomentum.Mag2();
2159  dF_dEta(locFIndex + 1, locEParamIndex) = locStateSignMultiplier*locEnergy*locPiCrossDeltaXX.Z()/locMomentum.Mag2();
2160 
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;
2167  }
2168 
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);
2172 
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);
2176  }
2177  else if(locKinFitParticle->Get_IsNeutralShowerFlag() && (locKinFitParticle->Get_Mass() >= 0.00001))
2178  {
2179  return;
2180  }
2181  else if((locKinFitParticleType == d_DetectedParticle) || (locKinFitParticleType == d_BeamParticle))
2182  {
2183  //detected/beam, non-accelerating particle
2184  TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->Get_Position();
2185  TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->Get_CommonVertex();
2186  TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
2187 
2188  unsigned short int locVertexConstraintFlag = locKinFitParticle->Get_VertexConstraintFlag();
2189  if(locVertexConstraintFlag == 1) //1 & 2 //pz is largest
2190  {
2191  if(dDebugLevel > 30)
2192  cout << "DKinFitter: Calc_dF_Vertex() Section 13a; PID = " << locKinFitParticle->Get_PID() << endl;
2193 
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();
2198  }
2199  else if(locVertexConstraintFlag == 2) //1 & 3 //py is largest
2200  {
2201  if(dDebugLevel > 30)
2202  cout << "DKinFitter: Calc_dF_Vertex() Section 13b; PID = " << locKinFitParticle->Get_PID() << endl;
2203 
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();
2208  }
2209  else //2 & 3 //px is largest
2210  {
2211  if(dDebugLevel > 30)
2212  cout << "DKinFitter: Calc_dF_Vertex() Section 13c; PID = " << locKinFitParticle->Get_PID() << endl;
2213 
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();
2218  }
2219  }
2220  else if((locKinFitParticleType == d_MissingParticle) || ((locKinFitParticleType == d_DecayingParticle) && (locKinFitParticle->Get_PxParamIndex() >= 0)))
2221  {
2222  //missing, or open-ended decaying particle
2223  TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->Get_Position();
2224  TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->Get_CommonVertex();
2225  TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
2226 
2227  unsigned short int locVertexConstraintFlag = locKinFitParticle->Get_VertexConstraintFlag();
2228  if(locVertexConstraintFlag == 1) //1 & 2 //pz is largest
2229  {
2230  if(dDebugLevel > 30)
2231  cout << "DKinFitter: Calc_dF_Vertex() Section 14a; PID = " << locKinFitParticle->Get_PID() << endl;
2232 
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();
2237  }
2238  else if(locVertexConstraintFlag == 2) //1 & 3 //py is largest
2239  {
2240  if(dDebugLevel > 30)
2241  cout << "DKinFitter: Calc_dF_Vertex() Section 14b; PID = " << locKinFitParticle->Get_PID() << endl;
2242 
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();
2247  }
2248  else //2 & 3 //px is largest
2249  {
2250  if(dDebugLevel > 30)
2251  cout << "DKinFitter: Calc_dF_Vertex() Section 14c; PID = " << locKinFitParticle->Get_PID() << endl;
2252 
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();
2257  }
2258  }
2259  else if((locCharge != 0) && dKinFitUtils->Get_IsBFieldNearBeamline() && (locKinFitParticleType == d_DecayingParticle))
2260  {
2261  //decaying charged particle in b-field (doesn't include contributions from the particles it is replaced by here)
2262  TVector3 locBField = dKinFitUtils->Get_BField(locPosition);
2263  double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
2264  TVector3 locH = locBField.Unit();
2265 
2266  TVector3 locPosition_DecayingSource = locKinFitParticle_DecayingSource->Get_Position();
2267  TVector3 locCommonVertex_DecayingSource = locKinFitParticle_DecayingSource->Get_CommonVertex();
2268  TVector3 locDeltaX_DecayingSource = locCommonVertex_DecayingSource - locPosition_DecayingSource;
2269 
2270  unsigned short int locVertexConstraintFlag = locKinFitParticle->Get_VertexConstraintFlag();
2271  if(locVertexConstraintFlag == 1) //1 & 2 //pz is largest
2272  {
2273  if(dDebugLevel > 30)
2274  cout << "DKinFitter: Calc_dF_Vertex() Section 15a; PID = " << locKinFitParticle->Get_PID() << endl;
2275 
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();
2282  }
2283  else if(locVertexConstraintFlag == 2) //1 & 3 //py is largest
2284  {
2285  if(dDebugLevel > 30)
2286  cout << "DKinFitter: Calc_dF_Vertex() Section 15b; PID = " << locKinFitParticle->Get_PID() << endl;
2287 
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());
2294  }
2295  else //2 & 3 //px is largest
2296  {
2297  if(dDebugLevel > 30)
2298  cout << "DKinFitter: Calc_dF_Vertex() Section 15c; PID = " << locKinFitParticle->Get_PID() << endl;
2299 
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());
2306  }
2307 
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);
2311 
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);
2315  }
2316 }
2317 
2318 void DKinFitter::Calc_Vertex_Params(const DKinFitParticle* locKinFitParticle, double& locJ, TVector3& locQ, TVector3& locM, TVector3& locD)
2319 {
2320  int locCharge = locKinFitParticle->Get_Charge();
2321  TVector3 locPosition = locKinFitParticle->Get_Position();
2322  TVector3 locCommonVertex = locKinFitParticle->Get_CommonVertex();
2323  TVector3 locDeltaX = locCommonVertex - locPosition;
2324  TVector3 locMomentum = locKinFitParticle->Get_Momentum();
2325 
2326  TVector3 locBField = dKinFitUtils->Get_BField(locPosition);
2327  TVector3 locH = locBField.Unit();
2328 
2329  TVector3 locPCrossH = locMomentum.Cross(locH);
2330  TVector3 locPCrossDeltaX = locMomentum.Cross(locDeltaX);
2331  double locPCrossHMagSq = locPCrossH.Mag2();
2332 
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);
2337 
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);
2343 
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);
2347 }
2348 
2350 {
2351  int locCharge = locKinFitParticle->Get_Charge();
2352  TVector3 locPosition = locKinFitParticle->Get_Position(); //x_0X
2353  TVector3 locCommonVertex = locKinFitParticle->Get_CommonVertex(); //x_X
2354  TVector3 locDeltaX = locCommonVertex - locPosition;
2355  TVector3 locP0X = locKinFitParticle->Get_Momentum(); //at x_0X
2356 
2357  TVector3 locBField = dKinFitUtils->Get_BField(locPosition);
2358  TVector3 locH = locBField.Unit();
2359  double locA = -0.00299792458*(double(locCharge))*locBField.Mag();
2360 
2361  TVector3 locDeltaXCrossH = locDeltaX.Cross(locH);
2362  TVector3 locPX = locP0X - locA*locDeltaXCrossH; //at common vertex x_X
2363 
2364  TVector3 locP0XCrossH = locP0X.Cross(locH);
2365  double locPXDotH = locPX.Dot(locH);
2366  double locP0XCrossHMagSq = locP0XCrossH.Mag2();
2367 
2368  double locDeltaXDotH = locDeltaX.Dot(locH);
2369  double locPXDotDeltaX = locPX.Dot(locDeltaX);
2370 
2371  double locK = locPXDotDeltaX - locPXDotH*locDeltaXDotH;
2372  double locJ = locA*locK/locP0XCrossHMagSq;
2373 
2374  double locC = locPXDotH/(locP0XCrossHMagSq*sqrt(1.0 - locJ*locJ));
2375  TVector3 locD = locC*(locPX - locPXDotH*locH) - locH;
2376 
2377  TVector3 locR = 2.0*locJ*locC*locP0XCrossH + locD;
2378  return locR;
2379 }
2380 
2381 /************************************************************************ UPDATE ***********************************************************************/
2382 
2384 {
2385  // update common vertices
2386  auto locVertexConstraints = Get_Constraints<DKinFitConstraint_Vertex>();
2387  auto locVertexIterator = locVertexConstraints.begin();
2388  for(; locVertexIterator != locVertexConstraints.end(); ++locVertexIterator)
2389  {
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);
2394  }
2395 
2396  // update common times
2397  auto locSpacetimeConstraints = Get_Constraints<DKinFitConstraint_Spacetime>();
2398  auto locSpacetimeIterator = locSpacetimeConstraints.begin();
2399  for(; locSpacetimeIterator != locSpacetimeConstraints.end(); ++locSpacetimeIterator)
2400  {
2401  auto locConstraint = *locSpacetimeIterator;
2402  int locParamIndex = locConstraint->Get_CommonTParamIndex();
2403  locConstraint->Set_CommonTime(dXi(locParamIndex, 0));
2404  }
2405 
2406  // update particle information
2407  auto locParticleIterator = dKinFitParticles.begin();
2408  for(; locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
2409  {
2410  auto locKinFitParticle = *locParticleIterator;
2411  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
2412  if(locKinFitParticleType == d_TargetParticle)
2413  continue; //nothing to update
2414 
2415  bool locIsUnknownParticleFlag = ((locKinFitParticleType == d_DecayingParticle) || (locKinFitParticleType == d_MissingParticle));
2416  TMatrixD& locSourceMatrix = locIsUnknownParticleFlag ? dXi : dEta;
2417 
2418  TLorentzVector locP4 = locKinFitParticle->Get_P4();
2419  TLorentzVector locPreviousP4 = locP4;
2420 
2421  int locParamIndex = locKinFitParticle->Get_PxParamIndex();
2422  if(locParamIndex >= 0)
2423  {
2424  TVector3 locMomentum(locSourceMatrix(locParamIndex, 0), locSourceMatrix(locParamIndex + 1, 0), locSourceMatrix(locParamIndex + 2, 0));
2425  locKinFitParticle->Set_Momentum(locMomentum);
2426  locP4 = locKinFitParticle->Get_P4();
2427  }
2428 
2429  //vertex
2430  locParamIndex = locKinFitParticle->Get_VxParamIndex();
2431  if(locParamIndex >= 0)
2432  {
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);
2438  }
2439 
2440  //neutral shower energy and time
2441  locParamIndex = locKinFitParticle->Get_EParamIndex();
2442  if(locParamIndex >= 0) //neutral shower: set momentum also //must be after Vx & common vertex are set
2443  {
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);
2451  }
2452 
2453  //time
2454  locParamIndex = locKinFitParticle->Get_TParamIndex();
2455  if(locParamIndex >= 0)
2456  {
2457  double locTime = locSourceMatrix(locParamIndex, 0);
2458  locKinFitParticle->Set_Time(locTime);
2459  if(locKinFitParticle->Get_CommonTParamIndex() < 0)
2460  locKinFitParticle->Set_CommonTime(locTime);
2461  }
2462  else if((locKinFitParticle->Get_PxParamIndex() >= 0) && !locIsUnknownParticleFlag && (locKinFitParticle->Get_EParamIndex() < 0))
2463  {
2464  //momentum has changed (and not a neutral shower): update time
2465  //note: not dependent on position change: trajectory moved, but doesn't change path length (if does, is unknown & small)
2466 
2467  //note: for charged, is losing energy along the trajectory
2468  //however, this change in beta is very small, except for slow protons (which are easy to identify anyway)
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);
2474  }
2475  }
2476 
2477  // calc non-unknown decaying particle momentum (derived from other particles)
2478  //must do last because assumes all other particle p3's are updated
2479  for(locParticleIterator = dKinFitParticles.begin(); locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
2480  {
2481  auto locKinFitParticle = *locParticleIterator;
2482  if(locKinFitParticle->Get_KinFitParticleType() != d_DecayingParticle)
2483  continue;
2484  locKinFitParticle->Set_Momentum(dKinFitUtils->Calc_DecayingP4_ByPosition(locKinFitParticle.get(), true).Vect());
2485  }
2486 }
2487 
2488 /************************************************************************ FINAL ************************************************************************/
2489 
2491 {
2492  auto locParticleIterator = dKinFitParticles.begin();
2493  for(; locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
2494  {
2495  auto locKinFitParticle = *locParticleIterator;
2496  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
2497 
2498  if((locKinFitParticleType == d_DecayingParticle) || (locKinFitParticleType == d_MissingParticle) || (locKinFitParticleType == d_TargetParticle))
2499  continue;
2500 
2501  map<DKinFitPullType, double> locParticlePulls;
2502  if(dDebugLevel >= 50)
2503  {
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;
2506  }
2507 
2508  int locParamIndex = locKinFitParticle->Get_EParamIndex();
2509  if(locParamIndex >= 0) //E
2510  {
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();
2513  }
2514 
2515  locParamIndex = locKinFitParticle->Get_PxParamIndex();
2516  if(locParamIndex >= 0) //px, py, pz
2517  {
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();
2520  ++locParamIndex;
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();
2523  ++locParamIndex;
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();
2526  }
2527 
2528  locParamIndex = locKinFitParticle->Get_VxParamIndex();
2529  if(locParamIndex >= 0) //vx, vy, vz
2530  {
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();
2533  ++locParamIndex;
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();
2536  ++locParamIndex;
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();
2539  }
2540 
2541  locParamIndex = locKinFitParticle->Get_TParamIndex();
2542  if(locParamIndex >= 0) //T
2543  {
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();
2546  }
2547 
2548  if(!locParticlePulls.empty())
2549  dPulls[locKinFitParticle] = locParticlePulls;
2550  }
2551 
2552  if(dDebugLevel > 20)
2553  {
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)
2561  {
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)
2567  {
2568  if(locParticlePulls.find((DKinFitPullType)loc_i) != locParticlePulls.end())
2569  cout << locParticlePulls[(DKinFitPullType)loc_i] << ", ";
2570  }
2571  cout << endl;
2572  }
2573  }
2574 }
2575 
2577 {
2578  // first update the covariance matrices of each particle with the fit results (prior to any propagation)
2579  Update_CovarianceMatrices(!dKinFitUtils->Get_UpdateCovarianceMatricesFlag()); //if false, then only update decaying particle matrices
2580 
2581  // propagate the track parameters
2582  auto locParticleIterator = dKinFitParticles.begin();
2583  for(; locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
2584  {
2585  auto locKinFitParticle = *locParticleIterator;
2586  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
2587  if((locKinFitParticleType == d_TargetParticle) || (locKinFitParticleType == d_MissingParticle) || (locKinFitParticleType == d_DecayingParticle))
2588  continue; // particle properties already defined at the fit vertex
2589 
2590  if(!locKinFitParticle->Get_FitCommonVertexFlag())
2591  continue; // no distance over which to propagate
2592 
2593  //updating the covariance matrix: a unique cov matrix object was cloned on fit start, so can safely update it directly
2594  //unless DKinFitUtils::Get_UpdateCovarianceMatricesFlag() is false. In which case, this matrix is not propagated (is unchanged)
2595  TMatrixFSym* locCovarianceMatrix = const_cast<TMatrixFSym*>(locKinFitParticle->Get_CovarianceMatrix().get());
2596 
2597  TVector3 locMomentum;
2598  TLorentzVector locSpacetimeVertex;
2599  pair<double, double> locPathLengthPair, locRestFrameLifetimePair;
2600  if(!dKinFitUtils->Propagate_TrackInfoToCommonVertex(locKinFitParticle.get(), &dVXi, locMomentum, locSpacetimeVertex, locPathLengthPair, locRestFrameLifetimePair, locCovarianceMatrix))
2601  continue; // info not propagated
2602 
2603  if(dDebugLevel >= 50)
2604  {
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;
2610  }
2611 
2612  //no need to set the covariance matrix: already updated
2613  locKinFitParticle->Set_Momentum(locMomentum);
2614  if(locKinFitParticle->Get_IsNeutralShowerFlag())
2615  locKinFitParticle->Set_CommonSpacetimeVertex(locSpacetimeVertex);
2616  else
2617  locKinFitParticle->Set_SpacetimeVertex(locSpacetimeVertex);
2618  locKinFitParticle->Set_PathLength(locPathLengthPair.first);
2619  locKinFitParticle->Set_PathLengthUncertainty(locPathLengthPair.second);
2620  }
2621 
2622  //calculate the path length of decaying particles involved in 2 vertex fits
2623  for(locParticleIterator = dKinFitParticles.begin(); locParticleIterator != dKinFitParticles.end(); ++locParticleIterator)
2624  {
2625  auto locKinFitParticle = *locParticleIterator;
2626  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
2627 
2628  if((locKinFitParticleType != d_DecayingParticle) || !locKinFitParticle->Get_FitCommonVertexFlag())
2629  continue;
2630 
2631  pair<double, double> locPathLengthPair, locRestFrameLifetimePair;
2632  if(dKinFitUtils->Calc_PathLength(locKinFitParticle.get(), &dVXi, locKinFitParticle->Get_CovarianceMatrix().get(), locPathLengthPair, locRestFrameLifetimePair))
2633  {
2634  locKinFitParticle->Set_PathLength(locPathLengthPair.first);
2635  locKinFitParticle->Set_PathLengthUncertainty(locPathLengthPair.second);
2636  locKinFitParticle->Set_RestFrameLifetime(locRestFrameLifetimePair.first);
2637  locKinFitParticle->Set_RestFrameLifetimeUncertainty(locRestFrameLifetimePair.second);
2638  }
2639  }
2640 }
2641 
2642 void DKinFitter::Update_CovarianceMatrices(bool locDecayingParticlesOnlyFlag)
2643 {
2644  //decaying particles first
2645  for(auto locKinFitParticle : dKinFitParticles)
2646  {
2647  DKinFitParticleType locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
2648  if(locKinFitParticleType != d_DecayingParticle)
2649  continue;
2650 
2651  //decaying particle: the momentum is derived from the other particles
2652  //if the decaying particle is not a constraining particle (e.g. vertex no-constrain):
2653  //look for particles that define the momentum that were NOT in the fit. Their uncertainties are needed, but are not in the input matrices
2654 
2655  auto locIncludeCommonSpacetimeVertex = locKinFitParticle->Get_FitCommonVertexFlag();
2656 
2657  //Particle had no cov matrix: Make a new one
2658  auto locMatrixSize = locIncludeCommonSpacetimeVertex ? 11 : 7;
2659  auto locCovarianceMatrix = dKinFitUtils->Get_SymMatrixResource(locMatrixSize);
2660  locCovarianceMatrix->Zero();
2661  locKinFitParticle->Set_CovarianceMatrix(locCovarianceMatrix);
2662 
2663  auto locDerivedFromParticles = locKinFitParticle->Get_FromAllParticles();
2664  auto locFromIterator = locDerivedFromParticles.begin();
2665  map<const DKinFitParticle*, int> locAdditionalPxParamIndices;
2666  for(; locFromIterator != locDerivedFromParticles.end(); ++locFromIterator)
2667  {
2668  DKinFitParticleType locKinFitParticleType = (*locFromIterator)->Get_KinFitParticleType();
2669  if((locKinFitParticleType == d_DecayingParticle) || (locKinFitParticleType == d_TargetParticle) || (locKinFitParticleType == d_MissingParticle))
2670  continue;
2671  if((*locFromIterator)->Get_PxParamIndex() >= 0)
2672  continue; //uncertainties are already in matrices: ignore
2673  int locNewPxParamIndex = dNumEta + dNumXi + 3*locAdditionalPxParamIndices.size();
2674  locAdditionalPxParamIndices[(*locFromIterator).get()] = locNewPxParamIndex;
2675  }
2676 
2677  TMatrixD locJacobian(locMatrixSize, dNumEta + dNumXi + 3*locAdditionalPxParamIndices.size());
2678 
2679  //set p3 terms
2680  bool locP3DerivedAtProductionVertexFlag = !dKinFitUtils->Get_IsDecayingParticleDefinedByProducts(locKinFitParticle.get()); //else decay vertex
2681  bool locP3DerivedAtPositionFlag = (locP3DerivedAtProductionVertexFlag == locKinFitParticle->Get_VertexP4AtProductionVertex());
2682  dKinFitUtils->Calc_DecayingParticleJacobian(locKinFitParticle.get(), locP3DerivedAtPositionFlag, 1.0, dNumEta, locAdditionalPxParamIndices, locJacobian);
2683 
2684  //set vertex & time terms
2685  int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
2686  if(locVxParamIndex >= 0)
2687  {
2688  locJacobian(3, locVxParamIndex + dNumEta) = 1.0;
2689  locJacobian(4, locVxParamIndex + dNumEta + 1) = 1.0;
2690  locJacobian(5, locVxParamIndex + dNumEta + 2) = 1.0;
2691  }
2692  int locTParamIndex = locKinFitParticle->Get_TParamIndex();
2693  if(locTParamIndex >= 0)
2694  locJacobian(6, locTParamIndex + dNumEta) = 1.0;
2695 
2696  //set common vertex & time terms
2697  int locCommonVxParamIndex = locKinFitParticle->Get_CommonVxParamIndex();
2698  if(locCommonVxParamIndex >= 0)
2699  {
2700  locJacobian(7, locCommonVxParamIndex + dNumEta) = 1.0;
2701  locJacobian(8, locCommonVxParamIndex + dNumEta + 1) = 1.0;
2702  locJacobian(9, locCommonVxParamIndex + dNumEta + 2) = 1.0;
2703  }
2704  int locCommonTParamIndex = locKinFitParticle->Get_CommonTParamIndex();
2705  if(locCommonTParamIndex >= 0)
2706  locJacobian(10, locCommonTParamIndex + dNumEta) = 1.0;
2707 
2708  if(dDebugLevel >= 50)
2709  {
2710  cout << "JACOBIAN MATRIX:" << endl;
2711  dKinFitUtils->Print_Matrix(locJacobian);
2712  }
2713 
2714  //build full covariance matrix (all particles), transform errors to those for decaying particle
2715  if(locAdditionalPxParamIndices.empty())
2716  {
2717  TMatrixDSym locTempCovarianceMatrix = dV; //already computed
2718  *locCovarianceMatrix = locTempCovarianceMatrix.Similarity(locJacobian);
2719  }
2720  else //need to expand error matrix
2721  {
2722  TMatrixDSym locTempCovarianceMatrix(dNumEta + dNumXi + 3*locAdditionalPxParamIndices.size());
2723  locTempCovarianceMatrix.SetSub(0, dV); //insert dV
2724 
2725  //insert p3 covariance matrices of additional particles (they are uncorrelated with the vertices since they were not used in the fit)
2726  auto locPxParamIterator = locAdditionalPxParamIndices.begin();
2727  for(; locPxParamIterator != locAdditionalPxParamIndices.end(); ++locPxParamIterator)
2728  {
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)
2733  {
2734  for(int loc_r = 0; loc_r < 3; ++loc_r)
2735  locTempCovarianceMatrix(loc_q + locPxParamIndex, loc_r + locPxParamIndex) = locAdditionalCovMatrix(loc_q, loc_r);
2736  }
2737  }
2738 
2739  if(dDebugLevel >= 50)
2740  {
2741  cout << "FULL ERROR MATRIX (for calculated cov for enclosed decaying particle):" << endl;
2742  dKinFitUtils->Print_Matrix(locTempCovarianceMatrix);
2743  }
2744 
2745  //transform errors and set
2746  locTempCovarianceMatrix.Similarity(locJacobian);
2747  for(int loc_q = 0; loc_q < 7; ++loc_q)
2748  {
2749  for(int loc_r = 0; loc_r < 7; ++loc_r)
2750  (*locCovarianceMatrix)(loc_q, loc_r) = locTempCovarianceMatrix(loc_q, loc_r);
2751  }
2752  }
2753 
2754  if(dDebugLevel >= 50)
2755  {
2756  cout << "FINAL COV MATRIX (enclosed decaying particle):" << endl;
2757  dKinFitUtils->Print_Matrix(*locCovarianceMatrix);
2758  }
2759  }
2760 
2761  if(locDecayingParticlesOnlyFlag)
2762  return;
2763 
2764  //measured, beam, and missing particles
2765  //correlations between fit and unfit measured parameters:
2766  //assume that the correlations remain unchanged: the changes in the parameters and their uncertainties should be small wrst their values
2767  for(auto locKinFitParticle : dKinFitParticles)
2768  {
2769  auto locKinFitParticleType = locKinFitParticle->Get_KinFitParticleType();
2770  if((locKinFitParticleType == d_TargetParticle) || (locKinFitParticleType == d_DecayingParticle))
2771  continue;
2772 
2773  //Get covariance matrix
2774  bool locReconstructedParticleFlag = (locKinFitParticleType == d_MissingParticle);
2775  TMatrixDSym& locKinFitMatrix = locReconstructedParticleFlag ? dVXi : dVEta;
2776  if(locReconstructedParticleFlag) //Brand new particle: Set the covariance matrix from scratch
2777  {
2778  //Particle had none: Make a new one
2779  auto locCovarianceMatrix = dKinFitUtils->Get_SymMatrixResource(7);
2780  locCovarianceMatrix->Zero();
2781  locKinFitParticle->Set_CovarianceMatrix(locCovarianceMatrix);
2782  }
2783  //updating the covariance matrix: a unique cov matrix object was cloned on fit start, so can safely update it directly
2784  TMatrixFSym& locCovarianceMatrix = *(const_cast<TMatrixFSym*>(locKinFitParticle->Get_CovarianceMatrix().get()));
2785 
2786  int locPxParamIndex = locKinFitParticle->Get_PxParamIndex();
2787  int locVxParamIndex = locKinFitParticle->Get_VxParamIndex();
2788  int locTParamIndex = locKinFitParticle->Get_TParamIndex();
2789  int locEParamIndex = locKinFitParticle->Get_EParamIndex();
2790 
2791  int locCovMatrixEParamIndex = locKinFitParticle->Get_CovMatrixEParamIndex();
2792  int locCovMatrixPxParamIndex = locKinFitParticle->Get_CovMatrixPxParamIndex();
2793  int locCovMatrixVxParamIndex = locKinFitParticle->Get_CovMatrixVxParamIndex();
2794  int locCovMatrixTParamIndex = locKinFitParticle->Get_CovMatrixTParamIndex();
2795 
2796  if(dDebugLevel >= 50)
2797  {
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;
2802  }
2803 
2804  //set localized terms first
2805  if(locEParamIndex >= 0)
2806  locCovarianceMatrix(locCovMatrixEParamIndex, locCovMatrixEParamIndex) = locKinFitMatrix(locEParamIndex, locEParamIndex);
2807  if(locPxParamIndex >= 0)
2808  {
2809  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2810  {
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);
2813  }
2814  }
2815  if(locVxParamIndex >= 0)
2816  {
2817  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2818  {
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);
2821  }
2822  }
2823  if(locTParamIndex >= 0)
2824  locCovarianceMatrix(locCovMatrixTParamIndex, locCovMatrixTParamIndex) = locKinFitMatrix(locTParamIndex, locTParamIndex);
2825 
2826  //cross terms: E & V (neutral shower)
2827  if((locEParamIndex >= 0) && (locVxParamIndex >= 0)) //both included in the fit
2828  {
2829  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2830  {
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);
2833  }
2834  }
2835  else if(!locReconstructedParticleFlag && (locEParamIndex >= 0) && (locVxParamIndex < 0)) //only E included in the fit
2836  {
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)
2840  {
2841  locCovarianceMatrix(locCovMatrixEParamIndex + 0, locCovMatrixVxParamIndex + loc_j) *= locUncertaintyRatio;
2842  locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixEParamIndex + 0) *= locUncertaintyRatio;
2843  }
2844  }
2845  else if(!locReconstructedParticleFlag && (locEParamIndex < 0) && (locVxParamIndex >= 0) && (locCovMatrixEParamIndex >= 0)) //only V included in the fit //E may not be in the covariance matrix!!
2846  {
2847  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2848  {
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;
2853  }
2854  }
2855 
2856  //cross terms: E & T (neutral shower)
2857  if((locEParamIndex >= 0) && (locTParamIndex >= 0)) //both included in the fit
2858  {
2859  locCovarianceMatrix(locCovMatrixEParamIndex, locCovMatrixTParamIndex) = locKinFitMatrix(locEParamIndex, locTParamIndex);
2860  locCovarianceMatrix(locCovMatrixTParamIndex, locCovMatrixEParamIndex) = locKinFitMatrix(locTParamIndex, locEParamIndex);
2861  }
2862  else if(!locReconstructedParticleFlag && (locEParamIndex >= 0) && (locTParamIndex < 0)) //only E included in the fit
2863  {
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;
2868  }
2869  else if(!locReconstructedParticleFlag && (locEParamIndex < 0) && (locTParamIndex >= 0) && (locCovMatrixEParamIndex >= 0)) //only T included in the fit //E may not be in the covariance matrix!!
2870  {
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;
2875  }
2876 
2877  //cross terms: P & V
2878  if((locPxParamIndex >= 0) && (locVxParamIndex >= 0)) //both included in the fit
2879  {
2880  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2881  {
2882  for(unsigned int loc_k = 0; loc_k < 3; ++loc_k)
2883  {
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);
2886  }
2887  }
2888  }
2889  else if(!locReconstructedParticleFlag && (locPxParamIndex >= 0) && (locVxParamIndex < 0)) //only P included in the fit
2890  {
2891  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2892  {
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)
2896  {
2897  locCovarianceMatrix(locCovMatrixPxParamIndex + loc_j, locCovMatrixVxParamIndex + loc_k) *= locUncertaintyRatio;
2898  locCovarianceMatrix(locCovMatrixVxParamIndex + loc_k, locCovMatrixPxParamIndex + loc_j) *= locUncertaintyRatio;
2899  }
2900  }
2901  }
2902  else if(!locReconstructedParticleFlag && (locPxParamIndex < 0) && (locVxParamIndex >= 0)) //only V included in the fit
2903  {
2904  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2905  {
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)
2909  {
2910  locCovarianceMatrix(locCovMatrixPxParamIndex + loc_k, locCovMatrixVxParamIndex + loc_j) *= locUncertaintyRatio;
2911  locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixPxParamIndex + loc_k) *= locUncertaintyRatio;
2912  }
2913  }
2914  }
2915 
2916  //cross terms: P & T
2917  if((locPxParamIndex >= 0) && (locTParamIndex >= 0)) //both included in the fit
2918  {
2919  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2920  {
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);
2923  }
2924  }
2925  else if(!locReconstructedParticleFlag && (locPxParamIndex >= 0) && (locTParamIndex < 0)) //only P included in the fit
2926  {
2927  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2928  {
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;
2933  }
2934  }
2935  else if(!locReconstructedParticleFlag && (locPxParamIndex < 0) && (locTParamIndex >= 0)) //only T included in the fit
2936  {
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)
2940  {
2941  locCovarianceMatrix(locCovMatrixPxParamIndex + loc_j, locCovMatrixTParamIndex + 0) *= locUncertaintyRatio;
2942  locCovarianceMatrix(locCovMatrixTParamIndex + 0, locCovMatrixPxParamIndex + loc_j) *= locUncertaintyRatio;
2943  }
2944  }
2945 
2946  //cross terms: V & T
2947  if((locVxParamIndex >= 0) && (locTParamIndex >= 0)) //both included in the fit
2948  {
2949  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2950  {
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);
2953  }
2954  }
2955  else if(!locReconstructedParticleFlag && (locVxParamIndex >= 0) && (locTParamIndex < 0)) //only V included in the fit
2956  {
2957  for(unsigned int loc_j = 0; loc_j < 3; ++loc_j)
2958  {
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;
2963  }
2964  }
2965  else if(!locReconstructedParticleFlag && (locVxParamIndex < 0) && (locTParamIndex >= 0)) //only T included in the fit
2966  {
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)
2970  {
2971  locCovarianceMatrix(locCovMatrixVxParamIndex + loc_j, locCovMatrixTParamIndex + 0) *= locUncertaintyRatio;
2972  locCovarianceMatrix(locCovMatrixTParamIndex + 0, locCovMatrixVxParamIndex + loc_j) *= locUncertaintyRatio;
2973  }
2974  }
2975 
2976  if(dDebugLevel >= 50)
2977  {
2978  cout << "FINAL COV MATRIX:" << endl;
2979  dKinFitUtils->Print_Matrix(locCovarianceMatrix);
2980  }
2981  } //end set cov matrix loop
2982 }
2983 
int Get_PID(void) const
void Reset_NewEvent(void)
Definition: DKinFitter.cc:71
bool Get_FitCommonVertexFlag(void) const
char Get_Charge(void) const
bool Fit_Reaction(void)
Definition: DKinFitter.cc:723
void Calc_dF(void)
Definition: DKinFitter.cc:999
char Get_CommonVxParamIndex(void) const
DKinFitPullType
TMatrixDSym dS_Inverse
Definition: DKinFitter.h:190
TMatrixD dXi
Definition: DKinFitter.h:184
bool Calc_dS(void)
Definition: DKinFitter.cc:858
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)
Definition: DKinFitter.cc:1818
set< shared_ptr< DKinFitParticle > > Get_FromInitialState(void) const
void Calc_dVdEta(void)
Definition: DKinFitter.cc:947
TVector3 Calc_VertexParams_P4DerivedAtCommonVertex(const DKinFitParticle *locKinFitParticle)
Definition: DKinFitter.cc:2349
TMatrixD dEta
Definition: DKinFitter.h:185
TMatrixD dF
Definition: DKinFitter.h:194
map< shared_ptr< DKinFitParticle >, set< shared_ptr< DKinFitConstraint > > > dParticleConstraintMap
Definition: DKinFitter.h:174
char Get_FIndex(void) const
unsigned int dMaxNumIterations
Definition: DKinFitter.h:165
double dConvergenceChiSqDiff_LastResort
Definition: DKinFitter.h:168
double dConvergenceChiSqDiff
Definition: DKinFitter.h:167
unsigned int dDebugLevel
Definition: DKinFitter.h:163
DKinFitParticleType
double dChiSq
Definition: DKinFitter.h:210
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
TMatrixDSym dVXi
Definition: DKinFitter.h:204
unsigned int dNumF
Definition: DKinFitter.h:182
unsigned int dNumXi
Definition: DKinFitter.h:180
TMatrixDSym dS
Definition: DKinFitter.h:189
void Calc_dF_MassDerivs(size_t locFIndex, const DKinFitParticle *locKinFitParticle, TLorentzVector locXP4, double locStateSignMultiplier, bool locIsConstrainedParticle)
Definition: DKinFitter.cc:1313
bool Get_IsConstrainingVertex(const shared_ptr< DKinFitParticle > &locKinFitParticle) const
Definition: DKinFitter.cc:113
void Update_CovarianceMatrices(bool locDecayingParticlesOnlyFlag)
Definition: DKinFitter.cc:2642
void Recycle_LastFitMemory(void)
Definition: DKinFitter.cc:98
TMatrixD dLambda_T
Definition: DKinFitter.h:197
void Prepare_ConstraintsAndParticles(void)
Definition: DKinFitter.cc:141
unsigned char Get_VertexConstraintFlag(void) const
unsigned int dNDF
Definition: DKinFitter.h:211
TMatrixDSym dU_Inverse
Definition: DKinFitter.h:192
TMatrixD dY
Definition: DKinFitter.h:186
TMatrixD dLambda
Definition: DKinFitter.h:196
bool Calc_dU(void)
Definition: DKinFitter.cc:918
TMatrixDSym dVEta
Definition: DKinFitter.h:205
virtual TVector3 Get_BField(const TVector3 &locPosition) const =0
set< shared_ptr< DKinFitParticle > > Get_FromFinalState(void) const
virtual void Reset_NewEvent(void)
Definition: DKinFitUtils.cc:37
TEllipse * e
char Get_PxParamIndex(void) const
DKinFitter(void)
Definition: DKinFitter.h:107
double dConfidenceLevel
Definition: DKinFitter.h:212
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)
Definition: DKinFitUtils.cc:47
void Set_FIndex(char locFIndex)
void Resize_Matrices(void)
Definition: DKinFitter.cc:385
TMatrixD dF_dEta
Definition: DKinFitter.h:199
bool Iterate(void)
Definition: DKinFitter.cc:763
map< shared_ptr< DKinFitParticle >, map< DKinFitPullType, double > > dPulls
Definition: DKinFitter.h:215
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
void Zero_Matrices(void)
Definition: DKinFitter.cc:436
set< shared_ptr< DKinFitConstraint > > Clone_ParticlesAndConstraints(const set< shared_ptr< DKinFitConstraint >> &locInputConstraints)
void Set_DebugLevel(int locDebugLevel)
Definition: DKinFitter.cc:107
virtual void Reset_NewFit(void)
Definition: DKinFitUtils.h:40
DKinFitter * dKinFitter
Definition: DKinFitUtils.h:139
TMatrixDSym dVY
Definition: DKinFitter.h:187
bool Get_UpdateCovarianceMatricesFlag(void) const
Definition: DKinFitUtils.h:47
void Reset_NewFit(void)
Definition: DKinFitter.cc:77
DKinFitUtils * dKinFitUtils
Definition: DKinFitter.h:160
TMatrixD dEpsilon
Definition: DKinFitter.h:195
TMatrixD dF_dXi
Definition: DKinFitter.h:201
double sqrt(double)
char Get_EParamIndex(void) const
char Get_VxParamIndex(void) const
double sin(double)
void Calc_dF_Vertex_NotDecaying(size_t locFIndex, const DKinFitParticle *locKinFitParticle)
Definition: DKinFitter.cc:1517
void Set_FinalTrackInfo(void)
Definition: DKinFitter.cc:2576
TMatrixDSym dU
Definition: DKinFitter.h:191
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)
Definition: DKinFitter.cc:1467
void Calc_dF_Vertex_Decaying_NonAccel(size_t locFIndex, const DKinFitParticle *locKinFitParticle, const DKinFitParticle *locKinFitParticle_DecayingSource, double locStateSignMultiplier)
Definition: DKinFitter.cc:2022
TMatrixD dF_dXi_T
Definition: DKinFitter.h:202
bool Get_IsTimeConstrained(const shared_ptr< DKinFitParticle > &locKinFitParticle) const
Definition: DKinFitter.cc:126
bool Get_IsDecayingParticleDefinedByProducts(const DKinFitParticle *locKinFitParticle) const
DKinFitParticleType Get_KinFitParticleType(void) const
void Calc_Pulls(void)
Definition: DKinFitter.cc:2490
set< shared_ptr< DKinFitConstraint > > dKinFitConstraints
Definition: DKinFitter.h:172
void Calc_dF_P4(int locFIndex, const DKinFitParticle *locKinFitParticle, double locStateSignMultiplier)
Definition: DKinFitter.cc:1083
TVector3 Get_Position(void) const
TLorentzVector Calc_DecayingP4_ByPosition(const DKinFitParticle *locKinFitParticle, bool locAtPositionFlag, bool locDontPropagateAtAllFlag=false) const
set< shared_ptr< DKinFitParticle > > dKinFitParticles
Definition: DKinFitter.h:173
DKinFitStatus dKinFitStatus
Definition: DKinFitter.h:162
unsigned int dNumEta
Definition: DKinFitter.h:181
void Print_Matrix(const TMatrixD &locMatrix) const
void Update_ParticleParams(void)
Definition: DKinFitter.cc:2383
TMatrixD dF_dEta_T
Definition: DKinFitter.h:200
TVector3 Get_CommonVertex(void) const
void Set_MatrixSizes(void)
Definition: DKinFitter.cc:304
map< shared_ptr< DKinFitParticle >, set< shared_ptr< DKinFitConstraint > > > dParticleConstraintMap_Direct
Definition: DKinFitter.h:175
TVector3 Get_InitVertexGuess(void) const
TMatrixDSym dV
Definition: DKinFitter.h:206
TLorentzVector Get_P4(void) const
void Set_DebugLevel(int locDebugLevel)
Definition: DKinFitUtils.h:51
void Fill_InputMatrices(void)
Definition: DKinFitter.cc:462
void Set_FIndex(char locFIndex)
void Calc_Vertex_Params(const DKinFitParticle *locKinFitParticle, double &locJ, TVector3 &locQ, TVector3 &locM, TVector3 &locD)
Definition: DKinFitter.cc:2318
TVector3 Get_Momentum(void) const
bool Get_VertexP4AtProductionVertex(void) const