StRoot  1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
KFParticleBase.cxx
1 //---------------------------------------------------------------------------------
2 // Implementation of the KFParticleBase class
3 // .
4 // @author S.Gorbunov, I.Kisel
5 // @version 1.0
6 // @since 13.05.07
7 //
8 // Class to reconstruct and store the decayed particle parameters.
9 // The method is described in CBM-SOFT note 2007-003,
10 // ``Reconstruction of decayed particles based on the Kalman filter'',
11 // http://www.gsi.de/documents/DOC-2007-May-14-1.pdf
12 //
13 // This class describes general mathematics which is used by KFParticle class
14 //
15 // -= Copyright &copy ALICE HLT Group =-
16 //_________________________________________________________________________________
17 
18 #include <string.h>
19 #include "KFParticleBase.h"
20 #include "TMath.h"
21 #include "Riostream.h"
22 #include "TString.h"
23 #include "TRSymMatrix.h"
24 #include "TRVector.h"
25 ClassImp(KFParticleBase);
26 static Int_t _debug = 0;
27 
28 
29 KFParticleBase::KFParticleBase() :fID(0), fParentID(0), fQ(0), fNDF(-3), fChi2(0), fSFromDecay(0), fAtProductionVertex(0), fIsLinearized(0),
30  fIdTruth(0), fQuality(0), fIdParentMcVx(0), fPDG(0)
31 {
32  //* Constructor
33  Clear();
34  Initialize();
35 }
36 
37 void KFParticleBase::Initialize( const Double_t Param[], const Double_t Cov[], Int_t Charge, Double_t Mass, Int_t PID )
38 {
39  // Constructor from "cartesian" track, particle mass hypothesis should be provided
40  //
41  // Param[6] = { X, Y, Z, Px, Py, Pz } - position and momentum
42  // Cov [21] = lower-triangular part of the covariance matrix:
43  //
44  // ( 0 . . . . . )
45  // ( 1 2 . . . . )
46  // Cov. matrix = ( 3 4 5 . . . ) - numbering of covariance elements in Cov[]
47  // ( 6 7 8 9 . . )
48  // ( 10 11 12 13 14 . )
49  // ( 15 16 17 18 19 20 )
50  fPDG = PID;
51  for( Int_t i=0; i<6 ; i++ ) fP[i] = Param[i];
52  for( Int_t i=0; i<21; i++ ) fC[i] = Cov[i];
53 
54  Double_t energy = TMath::Sqrt( Mass*Mass + fP[3]*fP[3] + fP[4]*fP[4] + fP[5]*fP[5]);
55  fP[6] = energy;
56  fP[7] = 0;
57  fQ = Charge;
58  fNDF = 0;
59  fChi2 = 0;
60  fAtProductionVertex = 0;
61  fIsLinearized = 0;
62  fSFromDecay = 0;
63 
64  Double_t energyInv = 1./energy;
65  Double_t
66  h0 = fP[3]*energyInv,
67  h1 = fP[4]*energyInv,
68  h2 = fP[5]*energyInv;
69 
70  fC[21] = h0*fC[ 6] + h1*fC[10] + h2*fC[15];
71  fC[22] = h0*fC[ 7] + h1*fC[11] + h2*fC[16];
72  fC[23] = h0*fC[ 8] + h1*fC[12] + h2*fC[17];
73  fC[24] = h0*fC[ 9] + h1*fC[13] + h2*fC[18];
74  fC[25] = h0*fC[13] + h1*fC[14] + h2*fC[19];
75  fC[26] = h0*fC[18] + h1*fC[19] + h2*fC[20];
76  fC[27] = ( h0*h0*fC[ 9] + h1*h1*fC[14] + h2*h2*fC[20]
77  + 2*(h0*h1*fC[13] + h0*h2*fC[18] + h1*h2*fC[19] ) );
78  if (_debug) {
79  std::cout << "KFParticle::Create " << *this << std::endl;
80  }
81 }
82 
83 void KFParticleBase::Initialize()
84 {
85  //* Initialise covariance matrix and set current parameters to 0.0
86 }
87 void KFParticleBase::Clear(Option_t *option) {
88  memset(fBeg,0,fEnd-fBeg+1);
89  fC[0] = fC[2] = fC[5] = 100.;
90  fC[35] = 1.;
91  fNDF = -3;
92 }
93 void KFParticleBase::SetVtxGuess( Double_t x, Double_t y, Double_t z )
94 {
95  //* Set decay vertex parameters for linearisation
96 
97  fVtxGuess[0] = x;
98  fVtxGuess[1] = y;
99  fVtxGuess[2] = z;
100  fIsLinearized = 1;
101 }
102 
103 
104 Int_t KFParticleBase::GetMomentum( Double_t &p, Double_t &error ) const
105 {
106  //* Calculate particle momentum
107 
108  Double_t x = fP[3];
109  Double_t y = fP[4];
110  Double_t z = fP[5];
111  Double_t x2 = x*x;
112  Double_t y2 = y*y;
113  Double_t z2 = z*z;
114  Double_t p2 = x2+y2+z2;
115  p = TMath::Sqrt(p2);
116  error = (x2*fC[9]+y2*fC[14]+z2*fC[20] + 2*(x*y*fC[13]+x*z*fC[18]+y*z*fC[19]) );
117  if( error>0 && p>1.e-4 ){
118  error = TMath::Sqrt(error)/p;
119  return 0;
120  }
121  return 1;
122 }
123 
124 Int_t KFParticleBase::GetPt( Double_t &pt, Double_t &error ) const
125 {
126  //* Calculate particle transverse momentum
127 
128  Double_t px = fP[3];
129  Double_t py = fP[4];
130  Double_t px2 = px*px;
131  Double_t py2 = py*py;
132  Double_t pt2 = px2+py2;
133  pt = TMath::Sqrt(pt2);
134  error = (px2*fC[9] + py2*fC[14] + 2*px*py*fC[13] );
135  if( error>0 && pt>1.e-4 ){
136  error = TMath::Sqrt(error)/pt;
137  return 0;
138  }
139  error = 1.e10;
140  return 1;
141 }
142 
143 Int_t KFParticleBase::GetEta( Double_t &eta, Double_t &error ) const
144 {
145  //* Calculate particle pseudorapidity
146 
147  Double_t px = fP[3];
148  Double_t py = fP[4];
149  Double_t pz = fP[5];
150  Double_t pt2 = px*px + py*py;
151  Double_t p2 = pt2 + pz*pz;
152  Double_t p = TMath::Sqrt(p2);
153  Double_t a = p + pz;
154  Double_t b = p - pz;
155  eta = 1.e10;
156  if( b > 1.e-8 ){
157  Double_t c = a/b;
158  if( c>1.e-8 ) eta = 0.5*TMath::Log(c);
159  }
160  Double_t h3 = -px*pz;
161  Double_t h4 = -py*pz;
162  Double_t pt4 = pt2*pt2;
163  Double_t p2pt4 = p2*pt4;
164  error = (h3*h3*fC[9] + h4*h4*fC[14] + pt4*fC[20] + 2*( h3*(h4*fC[13] + fC[18]*pt2) + pt2*h4*fC[19] ) );
165 
166  if( error>0 && p2pt4>1.e-10 ){
167  error = TMath::Sqrt(error/p2pt4);
168  return 0;
169  }
170 
171  error = 1.e10;
172  return 1;
173 }
174 
175 Int_t KFParticleBase::GetPhi( Double_t &phi, Double_t &error ) const
176 {
177  //* Calculate particle polar angle
178 
179  Double_t px = fP[3];
180  Double_t py = fP[4];
181  Double_t px2 = px*px;
182  Double_t py2 = py*py;
183  Double_t pt2 = px2 + py2;
184  phi = TMath::ATan2(py,px);
185  error = (py2*fC[9] + px2*fC[14] - 2*px*py*fC[13] );
186  if( error>0 && pt2>1.e-4 ){
187  error = TMath::Sqrt(error)/pt2;
188  return 0;
189  }
190  error = 1.e10;
191  return 1;
192 }
193 
194 Int_t KFParticleBase::GetR( Double_t &r, Double_t &error ) const
195 {
196  //* Calculate distance to the origin
197 
198  Double_t x = fP[0];
199  Double_t y = fP[1];
200  Double_t x2 = x*x;
201  Double_t y2 = y*y;
202  r = TMath::Sqrt(x2 + y2);
203  error = (x2*fC[0] + y2*fC[2] - 2*x*y*fC[1] );
204  if( error>0 && r>1.e-4 ){
205  error = TMath::Sqrt(error)/r;
206  return 0;
207  }
208  error = 1.e10;
209  return 1;
210 }
211 
212 Int_t KFParticleBase::GetMass( Double_t &m, Double_t &error ) const
213 {
214  //* Calculate particle mass
215 
216  // s = sigma^2 of m2/2
217 
218  Double_t s = ( fP[3]*fP[3]*fC[9] + fP[4]*fP[4]*fC[14] + fP[5]*fP[5]*fC[20]
219  + fP[6]*fP[6]*fC[27]
220  +2*( + fP[3]*fP[4]*fC[13] + fP[5]*(fP[3]*fC[18] + fP[4]*fC[19])
221  - fP[6]*( fP[3]*fC[24] + fP[4]*fC[25] + fP[5]*fC[26] ) )
222  );
223  Double_t m2 = TMath::Abs(fP[6]*fP[6] - fP[3]*fP[3] - fP[4]*fP[4] - fP[5]*fP[5]);
224  m = TMath::Sqrt(m2);
225  if( m>1.e-10 ){
226  if( s>=0 ){
227  error = TMath::Sqrt(s)/m;
228  return 0;
229  }
230  }
231  error = 1.e20;
232  return 1;
233 }
234 
235 
236 Int_t KFParticleBase::GetDecayLength( Double_t &l, Double_t &error ) const
237 {
238  //* Calculate particle decay length [cm]
239 
240  Double_t x = fP[3];
241  Double_t y = fP[4];
242  Double_t z = fP[5];
243  Double_t t = fP[7];
244  Double_t x2 = x*x;
245  Double_t y2 = y*y;
246  Double_t z2 = z*z;
247  Double_t p2 = x2+y2+z2;
248  l = t*TMath::Sqrt(p2);
249  if( p2>1.e-4){
250  error = p2*fC[35] + t*t/p2*(x2*fC[9]+y2*fC[14]+z2*fC[20]
251  + 2*(x*y*fC[13]+x*z*fC[18]+y*z*fC[19]) )
252  + 2*t*(x*fC[31]+y*fC[32]+z*fC[33]);
253  error = TMath::Sqrt(TMath::Abs(error));
254  return 0;
255  }
256  error = 1.e20;
257  return 1;
258 }
259 
260 Int_t KFParticleBase::GetDecayLengthXY( Double_t &l, Double_t &error ) const
261 {
262  //* Calculate particle decay length in XY projection [cm]
263 
264  Double_t x = fP[3];
265  Double_t y = fP[4];
266  Double_t t = fP[7];
267  Double_t x2 = x*x;
268  Double_t y2 = y*y;
269  Double_t pt2 = x2+y2;
270  l = t*TMath::Sqrt(pt2);
271  if( pt2>1.e-4){
272  error = pt2*fC[35] + t*t/pt2*(x2*fC[9]+y2*fC[14] + 2*x*y*fC[13] )
273  + 2*t*(x*fC[31]+y*fC[32]);
274  error = TMath::Sqrt(TMath::Abs(error));
275  return 0;
276  }
277  error = 1.e20;
278  return 1;
279 }
280 
281 
282 Int_t KFParticleBase::GetLifeTime( Double_t &tauC, Double_t &error ) const
283 {
284  //* Calculate particle decay time [s]
285 
286  Double_t m, dm;
287  GetMass( m, dm );
288  Double_t cTM = (-fP[3]*fC[31] - fP[4]*fC[32] - fP[5]*fC[33] + fP[6]*fC[34]);
289  tauC = fP[7]*m;
290  error = m*m*fC[35] + 2*fP[7]*cTM + fP[7]*fP[7]*dm*dm;
291  if( error > 0 ){
292  error = TMath::Sqrt( error );
293  return 0;
294  }
295  error = 1.e20;
296  return 1;
297 }
298 
299 
300 void KFParticleBase::operator +=( const KFParticleBase &Daughter )
301 {
302  //* Add daughter via operator+=
303 
304  AddDaughter( Daughter );
305 }
306 
307 Double_t KFParticleBase::GetSCorrection( const Double_t Part[], const Double_t XYZ[] )
308 {
309  //* Get big enough correction for S error to let the particle Part be fitted to XYZ point
310 
311  Double_t d[3] = { XYZ[0]-Part[0], XYZ[1]-Part[1], XYZ[2]-Part[2] };
312  Double_t p2 = Part[3]*Part[3]+Part[4]*Part[4]+Part[5]*Part[5];
313  Double_t sigmaS = (p2>1.e-4) ? ( .1+3.*TMath::Sqrt( d[0]*d[0]+d[1]*d[1]+d[2]*d[2]) )/TMath::Sqrt(p2) : 1.;
314  return sigmaS;
315 }
316 
317 void KFParticleBase::GetMeasurement( const Double_t XYZ[], Double_t m[], Double_t V[] ) const
318 {
319  //* Get additional covariances V used during measurement
320 
321  Double_t b[3];
322  GetFieldValue( XYZ, b );
323  const Double_t kCLight = 0.000299792458;
324  b[0]*=kCLight; b[1]*=kCLight; b[2]*=kCLight;
325 
326  Transport( GetDStoPoint(XYZ), m, V );
327 
328  Double_t sigmaS = GetSCorrection( m, XYZ );
329 
330  Double_t h[6];
331 
332  h[0] = m[3]*sigmaS;
333  h[1] = m[4]*sigmaS;
334  h[2] = m[5]*sigmaS;
335  h[3] = ( h[1]*b[2]-h[2]*b[1] )*GetQ();
336  h[4] = ( h[2]*b[0]-h[0]*b[2] )*GetQ();
337  h[5] = ( h[0]*b[1]-h[1]*b[0] )*GetQ();
338 
339  V[ 0]+= h[0]*h[0];
340  V[ 1]+= h[1]*h[0];
341  V[ 2]+= h[1]*h[1];
342  V[ 3]+= h[2]*h[0];
343  V[ 4]+= h[2]*h[1];
344  V[ 5]+= h[2]*h[2];
345 
346  V[ 6]+= h[3]*h[0];
347  V[ 7]+= h[3]*h[1];
348  V[ 8]+= h[3]*h[2];
349  V[ 9]+= h[3]*h[3];
350 
351  V[10]+= h[4]*h[0];
352  V[11]+= h[4]*h[1];
353  V[12]+= h[4]*h[2];
354  V[13]+= h[4]*h[3];
355  V[14]+= h[4]*h[4];
356 
357  V[15]+= h[5]*h[0];
358  V[16]+= h[5]*h[1];
359  V[17]+= h[5]*h[2];
360  V[18]+= h[5]*h[3];
361  V[19]+= h[5]*h[4];
362  V[20]+= h[5]*h[5];
363 }
364 
365 
366 void KFParticleBase::AddDaughter( const KFParticleBase &Daughter )
367 {
368  //* Add daughter
369  if( fNDF<-1 ){ // first daughter -> just copy
370  fNDF = -1;
371  fQ = Daughter.GetQ();
372 
373  if( Daughter.fC[35]>0 ){ //TODO Check this: only the first daughter is used here!
374  Daughter.GetMeasurement( fVtxGuess, fP, fC );
375  } else {
376  for( Int_t i=0; i<8; i++ ) fP[i] = Daughter.fP[i];
377  for( Int_t i=0; i<36; i++ ) fC[i] = Daughter.fC[i];
378  }
379  fSFromDecay = 0;
380  return;
381  }
382 
383 // TransportToDecayVertex();
384 
385  Double_t b[3];
386  Int_t maxIter = 1;
387 
388  if( !fIsLinearized ){
389  if( fNDF==-1 ){
390  Double_t ds, ds1;
391  GetDStoParticle(Daughter, ds, ds1);
392  TransportToDS( ds );
393  Double_t m[8];
394  Double_t mCd[36];
395  Daughter.Transport( ds1, m, mCd );
396  fVtxGuess[0] = .5*( fP[0] + m[0] );
397  fVtxGuess[1] = .5*( fP[1] + m[1] );
398  fVtxGuess[2] = .5*( fP[2] + m[2] );
399  } else {
400  fVtxGuess[0] = fP[0];
401  fVtxGuess[1] = fP[1];
402  fVtxGuess[2] = fP[2];
403  }
404  maxIter = 3;
405  }
406 
407  for( Int_t iter=0; iter<maxIter; iter++ ){
408 
409  {
410  GetFieldValue( fVtxGuess, b );
411  const Double_t kCLight = 0.000299792458;
412  b[0]*=kCLight; b[1]*=kCLight; b[2]*=kCLight;
413  }
414 
415  Double_t *ffP = fP, *ffC = fC, tmpP[8], tmpC[36];
416  if( fNDF==-1 ){
417  GetMeasurement( fVtxGuess, tmpP, tmpC );
418  ffP = tmpP;
419  ffC = tmpC;
420  }
421 
422  Double_t m[8], mV[36];
423 
424  if( Daughter.fC[35]>0 ){
425  Daughter.GetMeasurement( fVtxGuess, m, mV );
426  } else {
427  for( Int_t i=0; i<8; i++ ) m[i] = Daughter.fP[i];
428  for( Int_t i=0; i<36; i++ ) mV[i] = Daughter.fC[i];
429  }
430 
431  //*
432 
433  Double_t mS[6]= { ffC[0]+mV[0],
434  ffC[1]+mV[1], ffC[2]+mV[2],
435  ffC[3]+mV[3], ffC[4]+mV[4], ffC[5]+mV[5] };
436  InvertCholetsky3(mS);
437  //* Residual (measured - estimated)
438 
439  Double_t zeta[3] = { m[0]-ffP[0], m[1]-ffP[1], m[2]-ffP[2] };
440 
441 
442  //* CHt = CH' - D'
443 
444  Double_t mCHt0[7], mCHt1[7], mCHt2[7];
445 
446  mCHt0[0]=ffC[ 0] ; mCHt1[0]=ffC[ 1] ; mCHt2[0]=ffC[ 3] ;
447  mCHt0[1]=ffC[ 1] ; mCHt1[1]=ffC[ 2] ; mCHt2[1]=ffC[ 4] ;
448  mCHt0[2]=ffC[ 3] ; mCHt1[2]=ffC[ 4] ; mCHt2[2]=ffC[ 5] ;
449  mCHt0[3]=ffC[ 6]-mV[ 6]; mCHt1[3]=ffC[ 7]-mV[ 7]; mCHt2[3]=ffC[ 8]-mV[ 8];
450  mCHt0[4]=ffC[10]-mV[10]; mCHt1[4]=ffC[11]-mV[11]; mCHt2[4]=ffC[12]-mV[12];
451  mCHt0[5]=ffC[15]-mV[15]; mCHt1[5]=ffC[16]-mV[16]; mCHt2[5]=ffC[17]-mV[17];
452  mCHt0[6]=ffC[21]-mV[21]; mCHt1[6]=ffC[22]-mV[22]; mCHt2[6]=ffC[23]-mV[23];
453 
454  //* Kalman gain K = mCH'*S
455 
456  Double_t k0[7], k1[7], k2[7];
457 
458  for(Int_t i=0;i<7;++i){
459  k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
460  k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
461  k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
462  }
463 
464  //* New estimation of the vertex position
465 
466  if( iter<maxIter-1 ){
467  for(Int_t i=0; i<3; ++i)
468  fVtxGuess[i]= ffP[i] + k0[i]*zeta[0]+k1[i]*zeta[1]+k2[i]*zeta[2];
469  continue;
470  }
471 
472  // last itearation -> update the particle
473 
474  //* Add the daughter momentum to the particle momentum
475 
476  ffP[ 3] += m[ 3];
477  ffP[ 4] += m[ 4];
478  ffP[ 5] += m[ 5];
479  ffP[ 6] += m[ 6];
480 
481  ffC[ 9] += mV[ 9];
482  ffC[13] += mV[13];
483  ffC[14] += mV[14];
484  ffC[18] += mV[18];
485  ffC[19] += mV[19];
486  ffC[20] += mV[20];
487  ffC[24] += mV[24];
488  ffC[25] += mV[25];
489  ffC[26] += mV[26];
490  ffC[27] += mV[27];
491 
492 
493  //* New estimation of the vertex position r += K*zeta
494 
495  for(Int_t i=0;i<7;++i)
496  fP[i] = ffP[i] + k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2];
497 
498  //* New covariance matrix C -= K*(mCH')'
499 
500  for(Int_t i=0, k=0;i<7;++i){
501  for(Int_t j=0;j<=i;++j,++k) {
502  fC[k] = ffC[k] - (k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j] );
503  }
504  }
505 
506  //* Calculate Chi^2
507 
508  fNDF += 2;
509  fQ += Daughter.GetQ();
510  fSFromDecay = 0;
511  fChi2 += (mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2])*zeta[0]
512  + (mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2])*zeta[1]
513  + (mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2])*zeta[2];
514  }
515 }
516 
517 
518 void KFParticleBase::SetProductionVertex( const KFParticleBase &Vtx )
519 {
520  //* Set production vertex for the particle, when the particle was not used in the vertex fit
521 
522  const Double_t *m = Vtx.fP, *mV = Vtx.fC;
523 
524  Bool_t noS = ( fC[35]<=0 ); // no decay length allowed
525 
526  if( noS ){
527  TransportToDecayVertex();
528  fP[7] = 0;
529  fC[28] = 0; fC[29] = 0; fC[30] = 0; fC[31] = 0; fC[32] = 0; fC[33] = 0; fC[35] = 0; fC[35] = 0;
530  } else {
531  TransportToDS( GetDStoPoint( m ) );
532  fP[7] = -fSFromDecay;
533  Convert(1);
534  }
535 
536  Double_t mAi[6];
537  for(int i=0; i<6; i++) mAi[i] = fC[i];
538  InvertCholetsky3(mAi);
539  //InvertCholetsky3( fC, mAi );
540 
541  Double_t mB[5][3];
542 
543  mB[0][0] = fC[ 6]*mAi[0] + fC[ 7]*mAi[1] + fC[ 8]*mAi[3];
544  mB[0][1] = fC[ 6]*mAi[1] + fC[ 7]*mAi[2] + fC[ 8]*mAi[4];
545  mB[0][2] = fC[ 6]*mAi[3] + fC[ 7]*mAi[4] + fC[ 8]*mAi[5];
546 
547  mB[1][0] = fC[10]*mAi[0] + fC[11]*mAi[1] + fC[12]*mAi[3];
548  mB[1][1] = fC[10]*mAi[1] + fC[11]*mAi[2] + fC[12]*mAi[4];
549  mB[1][2] = fC[10]*mAi[3] + fC[11]*mAi[4] + fC[12]*mAi[5];
550 
551  mB[2][0] = fC[15]*mAi[0] + fC[16]*mAi[1] + fC[17]*mAi[3];
552  mB[2][1] = fC[15]*mAi[1] + fC[16]*mAi[2] + fC[17]*mAi[4];
553  mB[2][2] = fC[15]*mAi[3] + fC[16]*mAi[4] + fC[17]*mAi[5];
554 
555  mB[3][0] = fC[21]*mAi[0] + fC[22]*mAi[1] + fC[23]*mAi[3];
556  mB[3][1] = fC[21]*mAi[1] + fC[22]*mAi[2] + fC[23]*mAi[4];
557  mB[3][2] = fC[21]*mAi[3] + fC[22]*mAi[4] + fC[23]*mAi[5];
558 
559  mB[4][0] = fC[28]*mAi[0] + fC[29]*mAi[1] + fC[30]*mAi[3];
560  mB[4][1] = fC[28]*mAi[1] + fC[29]*mAi[2] + fC[30]*mAi[4];
561  mB[4][2] = fC[28]*mAi[3] + fC[29]*mAi[4] + fC[30]*mAi[5];
562 
563  Double_t z[3] = { m[0]-fP[0], m[1]-fP[1], m[2]-fP[2] };
564 
565  {
566  Double_t mAVi[6] = { fC[0]-mV[0], fC[1]-mV[1], fC[2]-mV[2],
567  fC[3]-mV[3], fC[4]-mV[4], fC[5]-mV[5] };
568 
569  //if( !InvertCholetsky3( mAVi, mAVi ) )
570  InvertCholetsky3( mAVi);
571  {
572 
573  Double_t dChi2 = ( +(mAVi[0]*z[0] + mAVi[1]*z[1] + mAVi[3]*z[2])*z[0]
574  +(mAVi[1]*z[0] + mAVi[2]*z[1] + mAVi[4]*z[2])*z[1]
575  +(mAVi[3]*z[0] + mAVi[4]*z[1] + mAVi[5]*z[2])*z[2] );
576 
577  // Take Abs(dChi2) here. Negative value of 'det' or 'dChi2' shows that the particle
578  // was not used in the production vertex fit
579 
580  fChi2+= TMath::Abs( dChi2 );
581  }
582  fNDF += 2;
583  }
584 
585  fP[0] = m[0];
586  fP[1] = m[1];
587  fP[2] = m[2];
588  fP[3]+= mB[0][0]*z[0] + mB[0][1]*z[1] + mB[0][2]*z[2];
589  fP[4]+= mB[1][0]*z[0] + mB[1][1]*z[1] + mB[1][2]*z[2];
590  fP[5]+= mB[2][0]*z[0] + mB[2][1]*z[1] + mB[2][2]*z[2];
591  fP[6]+= mB[3][0]*z[0] + mB[3][1]*z[1] + mB[3][2]*z[2];
592  fP[7]+= mB[4][0]*z[0] + mB[4][1]*z[1] + mB[4][2]*z[2];
593 
594  Double_t d0, d1, d2;
595 
596  fC[0] = mV[0];
597  fC[1] = mV[1];
598  fC[2] = mV[2];
599  fC[3] = mV[3];
600  fC[4] = mV[4];
601  fC[5] = mV[5];
602 
603  d0= mB[0][0]*mV[0] + mB[0][1]*mV[1] + mB[0][2]*mV[3] - fC[ 6];
604  d1= mB[0][0]*mV[1] + mB[0][1]*mV[2] + mB[0][2]*mV[4] - fC[ 7];
605  d2= mB[0][0]*mV[3] + mB[0][1]*mV[4] + mB[0][2]*mV[5] - fC[ 8];
606 
607  fC[ 6]+= d0;
608  fC[ 7]+= d1;
609  fC[ 8]+= d2;
610  fC[ 9]+= d0*mB[0][0] + d1*mB[0][1] + d2*mB[0][2];
611 
612  d0= mB[1][0]*mV[0] + mB[1][1]*mV[1] + mB[1][2]*mV[3] - fC[10];
613  d1= mB[1][0]*mV[1] + mB[1][1]*mV[2] + mB[1][2]*mV[4] - fC[11];
614  d2= mB[1][0]*mV[3] + mB[1][1]*mV[4] + mB[1][2]*mV[5] - fC[12];
615 
616  fC[10]+= d0;
617  fC[11]+= d1;
618  fC[12]+= d2;
619  fC[13]+= d0*mB[0][0] + d1*mB[0][1] + d2*mB[0][2];
620  fC[14]+= d0*mB[1][0] + d1*mB[1][1] + d2*mB[1][2];
621 
622  d0= mB[2][0]*mV[0] + mB[2][1]*mV[1] + mB[2][2]*mV[3] - fC[15];
623  d1= mB[2][0]*mV[1] + mB[2][1]*mV[2] + mB[2][2]*mV[4] - fC[16];
624  d2= mB[2][0]*mV[3] + mB[2][1]*mV[4] + mB[2][2]*mV[5] - fC[17];
625 
626  fC[15]+= d0;
627  fC[16]+= d1;
628  fC[17]+= d2;
629  fC[18]+= d0*mB[0][0] + d1*mB[0][1] + d2*mB[0][2];
630  fC[19]+= d0*mB[1][0] + d1*mB[1][1] + d2*mB[1][2];
631  fC[20]+= d0*mB[2][0] + d1*mB[2][1] + d2*mB[2][2];
632 
633  d0= mB[3][0]*mV[0] + mB[3][1]*mV[1] + mB[3][2]*mV[3] - fC[21];
634  d1= mB[3][0]*mV[1] + mB[3][1]*mV[2] + mB[3][2]*mV[4] - fC[22];
635  d2= mB[3][0]*mV[3] + mB[3][1]*mV[4] + mB[3][2]*mV[5] - fC[23];
636 
637  fC[21]+= d0;
638  fC[22]+= d1;
639  fC[23]+= d2;
640  fC[24]+= d0*mB[0][0] + d1*mB[0][1] + d2*mB[0][2];
641  fC[25]+= d0*mB[1][0] + d1*mB[1][1] + d2*mB[1][2];
642  fC[26]+= d0*mB[2][0] + d1*mB[2][1] + d2*mB[2][2];
643  fC[27]+= d0*mB[3][0] + d1*mB[3][1] + d2*mB[3][2];
644 
645  d0= mB[4][0]*mV[0] + mB[4][1]*mV[1] + mB[4][2]*mV[3] - fC[28];
646  d1= mB[4][0]*mV[1] + mB[4][1]*mV[2] + mB[4][2]*mV[4] - fC[29];
647  d2= mB[4][0]*mV[3] + mB[4][1]*mV[4] + mB[4][2]*mV[5] - fC[30];
648 
649  fC[28]+= d0;
650  fC[29]+= d1;
651  fC[30]+= d2;
652  fC[31]+= d0*mB[0][0] + d1*mB[0][1] + d2*mB[0][2];
653  fC[32]+= d0*mB[1][0] + d1*mB[1][1] + d2*mB[1][2];
654  fC[33]+= d0*mB[2][0] + d1*mB[2][1] + d2*mB[2][2];
655  fC[34]+= d0*mB[3][0] + d1*mB[3][1] + d2*mB[3][2];
656  fC[35]+= d0*mB[4][0] + d1*mB[4][1] + d2*mB[4][2];
657 
658  if( noS ){
659  fP[7] = 0;
660  fC[28] = 0;fC[29] = 0;fC[30] = 0;fC[31] = 0;fC[32] = 0;fC[33] = 0;fC[35] = 0;fC[35] = 0;
661  } else {
662  TransportToDS( fP[7] );
663  Convert(0);
664  }
665 
666  fSFromDecay = 0;
667 }
668 
669 
670 
671 void KFParticleBase::SetMassConstraint( Double_t Mass, Double_t SigmaMass )
672 {
673  //* Set hard( SigmaMass=0 ) or soft (SigmaMass>0) mass constraint
674 
675  Double_t m2 = Mass*Mass; // measurement, weighted by Mass
676  Double_t s2 = m2*SigmaMass*SigmaMass; // sigma^2
677 
678  Double_t p2 = fP[3]*fP[3] + fP[4]*fP[4] + fP[5]*fP[5];
679  Double_t e0 = TMath::Sqrt(m2+p2);
680 
681  Double_t mH[8];
682  mH[0] = mH[1] = mH[2] = 0.;
683  mH[3] = -2*fP[3];
684  mH[4] = -2*fP[4];
685  mH[5] = -2*fP[5];
686  mH[6] = 2*fP[6];//e0;
687  mH[7] = 0;
688 
689  Double_t zeta = e0*e0 - e0*fP[6];
690  zeta = m2 - (fP[6]*fP[6]-p2);
691 
692  Double_t mCHt[8], s2_est=0;
693  for( Int_t i=0; i<8; ++i ){
694  mCHt[i] = 0.0;
695  for (Int_t j=0;j<8;++j) mCHt[i] += Cij(i,j)*mH[j];
696  s2_est += mH[i]*mCHt[i];
697  }
698 
699  if( s2_est<1.e-20 ) return; // calculated mass error is already 0,
700  // the particle can not be constrained on mass
701 
702  Double_t w2 = 1./( s2 + s2_est );
703  fChi2 += zeta*zeta*w2;
704  fNDF += 1;
705  for( Int_t i=0, ii=0; i<8; ++i ){
706  Double_t ki = mCHt[i]*w2;
707  fP[i]+= ki*zeta;
708  for(Int_t j=0;j<=i;++j) fC[ii++] -= ki*mCHt[j];
709  }
710 }
711 
712 
713 void KFParticleBase::SetNoDecayLength()
714 {
715  //* Set no decay length for resonances
716 
717  TransportToDecayVertex();
718 
719  Double_t h[8];
720  h[0] = h[1] = h[2] = h[3] = h[4] = h[5] = h[6] = 0;
721  h[7] = 1;
722 
723  Double_t zeta = 0 - fP[7];
724  for(Int_t i=0;i<8;++i) zeta -= h[i]*(fP[i]-fP[i]);
725 
726  Double_t s = fC[35];
727  if( s>1.e-20 ){
728  s = 1./s;
729  fChi2 += zeta*zeta*s;
730  fNDF += 1;
731  for( Int_t i=0, ii=0; i<7; ++i ){
732  Double_t ki = fC[28+i]*s;
733  fP[i]+= ki*zeta;
734  for(Int_t j=0;j<=i;++j) fC[ii++] -= ki*fC[28+j];
735  }
736  }
737  fP[7] = 0;
738  fC[28] = 0;fC[29] = 0;fC[30] = 0;fC[31] = 0;fC[32] = 0;fC[33] = 0;fC[35] = 0;fC[35] = 0;
739 }
740 
741 
742 void KFParticleBase::Construct( const KFParticleBase* vDaughters[], Int_t NDaughters,
743  const KFParticleBase *Parent, Double_t Mass, Bool_t IsConstrained )
744 {
745  //* Full reconstruction in one go
746 
747  Int_t maxIter = 1;
748  Bool_t wasLinearized = fIsLinearized;
749  if( !fIsLinearized || IsConstrained ){
750  //fVtxGuess[0] = fVtxGuess[1] = fVtxGuess[2] = 0; //!!!!
751  fVtxGuess[0] = GetX();
752  fVtxGuess[1] = GetY();
753  fVtxGuess[2] = GetZ();
754  fIsLinearized = 1;
755  maxIter = 3;
756  }
757 
758  Double_t constraintC[6];
759 
760  if( IsConstrained ){
761  for(Int_t i=0;i<6;++i) constraintC[i]=fC[i];
762  } else {
763  for(Int_t i=0;i<6;++i) constraintC[i]=0.;
764  constraintC[0] = constraintC[2] = constraintC[5] = 100.;
765  }
766 
767 
768  for( Int_t iter=0; iter<maxIter; iter++ ){
769  fAtProductionVertex = 0;
770  fSFromDecay = 0;
771  fP[0] = fVtxGuess[0];
772  fP[1] = fVtxGuess[1];
773  fP[2] = fVtxGuess[2];
774  fP[3] = 0;
775  fP[4] = 0;
776  fP[5] = 0;
777  fP[6] = 0;
778  fP[7] = 0;
779 
780  for(Int_t i=0;i<6; ++i) fC[i]=constraintC[i];
781  for(Int_t i=6;i<36;++i) fC[i]=0.;
782  fC[35] = 1.;
783 
784  fNDF = IsConstrained ?0 :-3;
785  fChi2 = 0.;
786  fQ = 0;
787 
788  for( Int_t itr =0; itr<NDaughters; itr++ ){
789  AddDaughter( *vDaughters[itr] );
790  }
791  if( iter<maxIter-1){
792  for( Int_t i=0; i<3; i++ ) fVtxGuess[i] = fP[i];
793  }
794  }
795  fIsLinearized = wasLinearized;
796 
797  if( Mass>=0 ) SetMassConstraint( Mass );
798  if( Parent ) SetProductionVertex( *Parent );
799 }
800 
801 
802 void KFParticleBase::Convert( Bool_t ToProduction )
803 {
804  //* Tricky function - convert the particle error along its trajectory to
805  //* the value which corresponds to its production/decay vertex
806  //* It is done by combination of the error of decay length with the position errors
807 
808  Double_t fld[3];
809  {
810  GetFieldValue( fP, fld );
811  const Double_t kCLight = fQ*0.000299792458;
812  fld[0]*=kCLight; fld[1]*=kCLight; fld[2]*=kCLight;
813  }
814 
815  Double_t h[6];
816 
817  h[0] = fP[3];
818  h[1] = fP[4];
819  h[2] = fP[5];
820  if( ToProduction ){ h[0]=-h[0]; h[1]=-h[1]; h[2]=-h[2]; }
821  h[3] = h[1]*fld[2]-h[2]*fld[1];
822  h[4] = h[2]*fld[0]-h[0]*fld[2];
823  h[5] = h[0]*fld[1]-h[1]*fld[0];
824 
825  Double_t c;
826 
827  c = fC[28]+h[0]*fC[35];
828  fC[ 0]+= h[0]*(c+fC[28]);
829  fC[28] = c;
830 
831  fC[ 1]+= h[1]*fC[28] + h[0]*fC[29];
832  c = fC[29]+h[1]*fC[35];
833  fC[ 2]+= h[1]*(c+fC[29]);
834  fC[29] = c;
835 
836  fC[ 3]+= h[2]*fC[28] + h[0]*fC[30];
837  fC[ 4]+= h[2]*fC[29] + h[1]*fC[30];
838  c = fC[30]+h[2]*fC[35];
839  fC[ 5]+= h[2]*(c+fC[30]);
840  fC[30] = c;
841 
842  fC[ 6]+= h[3]*fC[28] + h[0]*fC[31];
843  fC[ 7]+= h[3]*fC[29] + h[1]*fC[31];
844  fC[ 8]+= h[3]*fC[30] + h[2]*fC[31];
845  c = fC[31]+h[3]*fC[35];
846  fC[ 9]+= h[3]*(c+fC[31]);
847  fC[31] = c;
848 
849  fC[10]+= h[4]*fC[28] + h[0]*fC[32];
850  fC[11]+= h[4]*fC[29] + h[1]*fC[32];
851  fC[12]+= h[4]*fC[30] + h[2]*fC[32];
852  fC[13]+= h[4]*fC[31] + h[3]*fC[32];
853  c = fC[32]+h[4]*fC[35];
854  fC[14]+= h[4]*(c+fC[32]);
855  fC[32] = c;
856 
857  fC[15]+= h[5]*fC[28] + h[0]*fC[33];
858  fC[16]+= h[5]*fC[29] + h[1]*fC[33];
859  fC[17]+= h[5]*fC[30] + h[2]*fC[33];
860  fC[18]+= h[5]*fC[31] + h[3]*fC[33];
861  fC[19]+= h[5]*fC[32] + h[4]*fC[33];
862  c = fC[33]+h[5]*fC[35];
863  fC[20]+= h[5]*(c+fC[33]);
864  fC[33] = c;
865 
866  fC[21]+= h[0]*fC[34];
867  fC[22]+= h[1]*fC[34];
868  fC[23]+= h[2]*fC[34];
869  fC[24]+= h[3]*fC[34];
870  fC[25]+= h[4]*fC[34];
871  fC[26]+= h[5]*fC[34];
872 }
873 
874 
875 void KFParticleBase::TransportToDecayVertex()
876 {
877  //* Transport the particle to its decay vertex
878 
879  if( fSFromDecay != 0 ) TransportToDS( -fSFromDecay );
880  if( fAtProductionVertex ) Convert(0);
881  fAtProductionVertex = 0;
882 }
883 
884 void KFParticleBase::TransportToProductionVertex()
885 {
886  //* Transport the particle to its production vertex
887 
888  if( fSFromDecay != -fP[7] ) TransportToDS( -fSFromDecay-fP[7] );
889  if( !fAtProductionVertex ) Convert( 1 );
890  fAtProductionVertex = 1;
891 }
892 
893 
894 void KFParticleBase::TransportToDS( Double_t dS )
895 {
896  //* Transport the particle on dS parameter (SignedPath/Momentum)
897 
898  Transport( dS, fP, fC );
899  fSFromDecay+= dS;
900 }
901 
902 
903 Double_t KFParticleBase::GetDStoPointLine( const Double_t xyz[] ) const
904 {
905  //* Get dS to a certain space point without field
906 
907  Double_t p2 = fP[3]*fP[3] + fP[4]*fP[4] + fP[5]*fP[5];
908  if( p2<1.e-4 ) p2 = 1;
909  return ( fP[3]*(xyz[0]-fP[0]) + fP[4]*(xyz[1]-fP[1]) + fP[5]*(xyz[2]-fP[2]) )/p2;
910 }
911 
912 
913 Double_t KFParticleBase::GetDStoPointBz( Double_t B, const Double_t xyz[] )
914  const
915 {
916 
917  //* Get dS to a certain space point for Bz field
918  const Double_t kCLight = 0.000299792458;
919  Double_t bq = B*fQ*kCLight;
920  Double_t pt2 = fP[3]*fP[3] + fP[4]*fP[4];
921  if( pt2<1.e-4 ) return 0;
922  Double_t dx = xyz[0] - fP[0];
923  Double_t dy = xyz[1] - fP[1];
924  Double_t a = dx*fP[3]+dy*fP[4];
925  Double_t dS;
926 
927  if( TMath::Abs(bq)<1.e-8 ) dS = a/pt2;
928  else dS = TMath::ATan2( bq*a, pt2 + bq*(dy*fP[3] -dx*fP[4]) )/bq;
929 
930  if(0){
931 
932  Double_t px = fP[3];
933  Double_t py = fP[4];
934  Double_t pz = fP[5];
935  Double_t ss[2], g[2][5];
936 
937  ss[0] = dS;
938  ss[1] = -dS;
939  for( Int_t i=0; i<2; i++){
940  Double_t bs = bq*ss[i];
941  Double_t c = TMath::Cos(bs), s = TMath::Sin(bs);
942  Double_t cB,sB;
943  if( TMath::Abs(bq)>1.e-8){
944  cB= (1-c)/bq;
945  sB= s/bq;
946  }else{
947  const Double_t kOvSqr6 = 1./TMath::Sqrt(6.);
948  sB = (1.-bs*kOvSqr6)*(1.+bs*kOvSqr6)*ss[i];
949  cB = .5*sB*bs;
950  }
951  g[i][0] = fP[0] + sB*px + cB*py;
952  g[i][1] = fP[1] - cB*px + sB*py;
953  g[i][2] = fP[2] + ss[i]*pz;
954  g[i][3] = + c*px + s*py;
955  g[i][4] = - s*px + c*py;
956  }
957 
958  Int_t i=0;
959 
960  Double_t dMin = 1.e10;
961  for( Int_t j=0; j<2; j++){
962  Double_t xx = g[j][0]-xyz[0];
963  Double_t yy = g[j][1]-xyz[1];
964  Double_t zz = g[j][2]-xyz[2];
965  Double_t d = xx*xx + yy*yy + zz*zz;
966  if( d<dMin ){
967  dMin = d;
968  i = j;
969  }
970  }
971 
972  dS = ss[i];
973 
974  Double_t x= g[i][0], y= g[i][1], z= g[i][2], ppx= g[i][3], ppy= g[i][4];
975  Double_t ddx = x-xyz[0];
976  Double_t ddy = y-xyz[1];
977  Double_t ddz = z-xyz[2];
978  Double_t c = ddx*ppx + ddy*ppy + ddz*pz ;
979  Double_t pp2 = ppx*ppx + ppy*ppy + pz*pz;
980  if( TMath::Abs(pp2)>1.e-8 ){
981  dS+=c/pp2;
982  }
983  }
984  return dS;
985 }
986 
987 
988 void KFParticleBase::GetDStoParticleBz( Double_t B, const KFParticleBase &p,
989  Double_t &DS, Double_t &DS1 )
990  const
991 {
992  //* Get dS to another particle for Bz field
993  Double_t px = fP[3];
994  Double_t py = fP[4];
995  Double_t pz = fP[5];
996 
997  Double_t px1 = p.fP[3];
998  Double_t py1 = p.fP[4];
999  Double_t pz1 = p.fP[5];
1000 
1001  const Double_t kCLight = 0.000299792458;
1002 
1003  Double_t bq = B*fQ*kCLight;
1004  Double_t bq1 = B*p.fQ*kCLight;
1005  Double_t s=0, ds=0, s1=0, ds1=0;
1006 
1007  if( TMath::Abs(bq)>1.e-8 || TMath::Abs(bq1)>1.e-8 ){
1008 
1009  Double_t dx = (p.fP[0] - fP[0]);
1010  Double_t dy = (p.fP[1] - fP[1]);
1011  Double_t d2 = (dx*dx+dy*dy);
1012 
1013  Double_t p2 = (px *px + py *py);
1014  Double_t p21 = (px1*px1 + py1*py1);
1015 
1016  Double_t a = (px*py1 - py*px1);
1017  Double_t b = (px*px1 + py*py1);
1018 
1019  Double_t ldx = bq*bq1*dx - bq1*py + bq*py1 ;
1020  Double_t ldy = bq*bq1*dy + bq1*px - bq*px1 ;
1021  Double_t l2 = ldx*ldx + ldy*ldy;
1022 
1023  Double_t cS = bq1*p2 + bq*bq1*(dy* px - dx* py) - bq*b;
1024  Double_t cS1= bq*p21 - bq*bq1*(dy*px1 - dx*py1) - bq1*b;
1025 
1026  Double_t ca = bq*bq*bq1*d2 +2*( cS + bq*bq*(py1*dx-px1*dy)) ;
1027  Double_t ca1 = bq*bq1*bq1*d2 +2*( cS1 - bq1*bq1*(py*dx-px*dy)) ;
1028 
1029  Double_t sa = 4*l2*p2 - ca*ca;
1030  Double_t sa1 = 4*l2*p21 - ca1*ca1;
1031 
1032  if(sa<0) sa=0;
1033  if(sa1<0)sa1=0;
1034 
1035  if( TMath::Abs(bq)>1.e-8){
1036  s = TMath::ATan2( bq*( bq1*(dx*px +dy*py) + a ) , cS )/bq;
1037  ds = TMath::ATan2(TMath::Sqrt(sa),ca)/bq;
1038  } else {
1039  s = ( (dx*px + dy*py) + (py*px1-px*py1)/bq1)/p2;
1040  ds = s*s - (d2-2*(px1*dy-py1*dx)/bq1)/p2;
1041  if( ds<0 ) ds = 0;
1042  ds = TMath::Sqrt(ds);
1043  }
1044 
1045  if( TMath::Abs(bq1)>1.e-8){
1046  s1 = TMath::ATan2( -bq1*( bq*(dx*px1+dy*py1) + a), cS1 )/bq1;
1047  ds1 = TMath::ATan2(TMath::Sqrt(sa1),ca1)/bq1;
1048  } else {
1049  s1 = (-(dx*px1 + dy*py1) + (py*px1-px*py1)/bq)/p21;
1050  ds1 = s1*s1 - (d2+2*(px*dy-py*dx)/bq)/p21;
1051  if( ds1<0 ) ds1 = 0;
1052  ds1 = TMath::Sqrt(ds1);
1053  }
1054  }
1055 
1056  Double_t ss[2], ss1[2], g[2][5],g1[2][5];
1057 
1058  ss[0] = s + ds;
1059  ss[1] = s - ds;
1060  ss1[0] = s1 + ds1;
1061  ss1[1] = s1 - ds1;
1062  for( Int_t i=0; i<2; i++){
1063  Double_t bs = bq*ss[i];
1064  Double_t c = TMath::Cos(bs), sss = TMath::Sin(bs);
1065  Double_t cB,sB;
1066  if( TMath::Abs(bq)>1.e-8){
1067  cB= (1-c)/bq;
1068  sB= sss/bq;
1069  }else{
1070  const Double_t kOvSqr6 = 1./TMath::Sqrt(6.);
1071  sB = (1.-bs*kOvSqr6)*(1.+bs*kOvSqr6)*ss[i];
1072  cB = .5*sB*bs;
1073  }
1074  g[i][0] = fP[0] + sB*px + cB*py;
1075  g[i][1] = fP[1] - cB*px + sB*py;
1076  g[i][2] = fP[2] + ss[i]*pz;
1077  g[i][3] = + c*px + sss*py;
1078  g[i][4] = - sss*px + c*py;
1079 
1080  bs = bq1*ss1[i];
1081  c = TMath::Cos(bs); sss = TMath::Sin(bs);
1082  if( TMath::Abs(bq1)>1.e-8){
1083  cB= (1-c)/bq1;
1084  sB= sss/bq1;
1085  }else{
1086  const Double_t kOvSqr6 = 1./TMath::Sqrt(6.);
1087  sB = (1.-bs*kOvSqr6)*(1.+bs*kOvSqr6)*ss1[i];
1088  cB = .5*sB*bs;
1089  }
1090 
1091  g1[i][0] = p.fP[0] + sB*px1 + cB*py1;
1092  g1[i][1] = p.fP[1] - cB*px1 + sB*py1;
1093  g1[i][2] = p.fP[2] + ss[i]*pz1;
1094  g1[i][3] = + c*px1 + sss*py1;
1095  g1[i][4] = - sss*px1 + c*py1;
1096  }
1097 
1098  Int_t i=0, i1=0;
1099 
1100  Double_t dMin = 1.e10;
1101  for( Int_t j=0; j<2; j++){
1102  for( Int_t j1=0; j1<2; j1++){
1103  Double_t xx = g[j][0]-g1[j1][0];
1104  Double_t yy = g[j][1]-g1[j1][1];
1105  Double_t zz = g[j][2]-g1[j1][2];
1106  Double_t d = xx*xx + yy*yy + zz*zz;
1107  if( d<dMin ){
1108  dMin = d;
1109  i = j;
1110  i1 = j1;
1111  }
1112  }
1113  }
1114 
1115  DS = ss[i];
1116  DS1 = ss1[i1];
1117  if(0){
1118  Double_t x= g[i][0], y= g[i][1], z= g[i][2], ppx= g[i][3], ppy= g[i][4];
1119  Double_t x1=g1[i1][0], y1= g1[i1][1], z1= g1[i1][2], ppx1= g1[i1][3], ppy1= g1[i1][4];
1120  Double_t dx = x1-x;
1121  Double_t dy = y1-y;
1122  Double_t dz = z1-z;
1123  Double_t a = ppx*ppx1 + ppy*ppy1 + pz*pz1;
1124  Double_t b = dx*ppx1 + dy*ppy1 + dz*pz1;
1125  Double_t c = dx*ppx + dy*ppy + dz*pz ;
1126  Double_t pp2 = ppx*ppx + ppy*ppy + pz*pz;
1127  Double_t pp21= ppx1*ppx1 + ppy1*ppy1 + pz1*pz1;
1128  Double_t det = pp2*pp21 - a*a;
1129  if( TMath::Abs(det)>1.e-8 ){
1130  DS+=(a*b-pp21*c)/det;
1131  DS1+=(a*c-pp2*b)/det;
1132  }
1133  }
1134 }
1135 
1136 
1137 
1138 void KFParticleBase::TransportCBM( Double_t dS,
1139  Double_t P[], Double_t C[] ) const
1140 {
1141  //* Transport the particle on dS, output to P[],C[], for CBM field
1142 
1143  if( fQ==0 ){
1144  TransportLine( dS, P, C );
1145  return;
1146  }
1147 
1148  const Double_t kCLight = 0.000299792458;
1149 
1150  Double_t c = fQ*kCLight;
1151 
1152  // construct coefficients
1153 
1154  Double_t
1155  px = fP[3],
1156  py = fP[4],
1157  pz = fP[5];
1158 
1159  Double_t sx=0, sy=0, sz=0, syy=0, syz=0, syyy=0, ssx=0, ssy=0, ssz=0, ssyy=0, ssyz=0, ssyyy=0;
1160 
1161  { // get field integrals
1162 
1163  Double_t fld[3][3];
1164  Double_t p0[3], p1[3], p2[3];
1165 
1166  // line track approximation
1167 
1168  p0[0] = fP[0];
1169  p0[1] = fP[1];
1170  p0[2] = fP[2];
1171 
1172  p2[0] = fP[0] + px*dS;
1173  p2[1] = fP[1] + py*dS;
1174  p2[2] = fP[2] + pz*dS;
1175 
1176  p1[0] = 0.5*(p0[0]+p2[0]);
1177  p1[1] = 0.5*(p0[1]+p2[1]);
1178  p1[2] = 0.5*(p0[2]+p2[2]);
1179 
1180  // first order track approximation
1181  {
1182  GetFieldValue( p0, fld[0] );
1183  GetFieldValue( p1, fld[1] );
1184  GetFieldValue( p2, fld[2] );
1185 
1186  Double_t ssy1 = ( 7*fld[0][1] + 6*fld[1][1]-fld[2][1] )*c*dS*dS/96.;
1187  Double_t ssy2 = ( fld[0][1] + 2*fld[1][1] )*c*dS*dS/6.;
1188 
1189  p1[0] -= ssy1*pz;
1190  p1[2] += ssy1*px;
1191  p2[0] -= ssy2*pz;
1192  p2[2] += ssy2*px;
1193  }
1194 
1195  GetFieldValue( p0, fld[0] );
1196  GetFieldValue( p1, fld[1] );
1197  GetFieldValue( p2, fld[2] );
1198 
1199  sx = c*( fld[0][0] + 4*fld[1][0] + fld[2][0] )*dS/6.;
1200  sy = c*( fld[0][1] + 4*fld[1][1] + fld[2][1] )*dS/6.;
1201  sz = c*( fld[0][2] + 4*fld[1][2] + fld[2][2] )*dS/6.;
1202 
1203  ssx = c*( fld[0][0] + 2*fld[1][0])*dS*dS/6.;
1204  ssy = c*( fld[0][1] + 2*fld[1][1])*dS*dS/6.;
1205  ssz = c*( fld[0][2] + 2*fld[1][2])*dS*dS/6.;
1206 
1207  Double_t c2[3][3] = { { 5, -4, -1},{ 44, 80, -4},{ 11, 44, 5} }; // /=360.
1208  Double_t cc2[3][3] = { { 38, 8, -4},{ 148, 208, -20},{ 3, 36, 3} }; // /=2520.
1209  for(Int_t n=0; n<3; n++)
1210  for(Int_t m=0; m<3; m++)
1211  {
1212  syz += c2[n][m]*fld[n][1]*fld[m][2];
1213  ssyz += cc2[n][m]*fld[n][1]*fld[m][2];
1214  }
1215 
1216  syz *= c*c*dS*dS/360.;
1217  ssyz *= c*c*dS*dS*dS/2520.;
1218 
1219  syy = c*( fld[0][1] + 4*fld[1][1] + fld[2][1] )*dS;
1220  syyy = syy*syy*syy / 1296;
1221  syy = syy*syy/72;
1222 
1223  ssyy = ( fld[0][1]*( 38*fld[0][1] + 156*fld[1][1] - fld[2][1] )+
1224  fld[1][1]*( 208*fld[1][1] +16*fld[2][1] )+
1225  fld[2][1]*( 3*fld[2][1] )
1226  )*dS*dS*dS*c*c/2520.;
1227  ssyyy =
1228  (
1229  fld[0][1]*( fld[0][1]*( 85*fld[0][1] + 526*fld[1][1] - 7*fld[2][1] )+
1230  fld[1][1]*( 1376*fld[1][1] +84*fld[2][1] )+
1231  fld[2][1]*( 19*fld[2][1] ) )+
1232  fld[1][1]*( fld[1][1]*( 1376*fld[1][1] +256*fld[2][1] )+
1233  fld[2][1]*( 62*fld[2][1] ) )+
1234  fld[2][1]*fld[2][1] *( 3*fld[2][1] )
1235  )*dS*dS*dS*dS*c*c*c/90720.;
1236 
1237  }
1238 
1239  Double_t mJ[8][8];
1240  for( Int_t i=0; i<8; i++ ) for( Int_t j=0; j<8; j++) mJ[i][j]=0;
1241 
1242  mJ[0][0]=1; mJ[0][1]=0; mJ[0][2]=0; mJ[0][3]=dS-ssyy; mJ[0][4]=ssx; mJ[0][5]=ssyyy-ssy;
1243  mJ[1][0]=0; mJ[1][1]=1; mJ[1][2]=0; mJ[1][3]=-ssz; mJ[1][4]=dS; mJ[1][5]=ssx+ssyz;
1244  mJ[2][0]=0; mJ[2][1]=0; mJ[2][2]=1; mJ[2][3]=ssy-ssyyy; mJ[2][4]=-ssx; mJ[2][5]=dS-ssyy;
1245 
1246  mJ[3][0]=0; mJ[3][1]=0; mJ[3][2]=0; mJ[3][3]=1-syy; mJ[3][4]=sx; mJ[3][5]=syyy-sy;
1247  mJ[4][0]=0; mJ[4][1]=0; mJ[4][2]=0; mJ[4][3]=-sz; mJ[4][4]=1; mJ[4][5]=sx+syz;
1248  mJ[5][0]=0; mJ[5][1]=0; mJ[5][2]=0; mJ[5][3]=sy-syyy; mJ[5][4]=-sx; mJ[5][5]=1-syy;
1249  mJ[6][6] = mJ[7][7] = 1;
1250 
1251  P[0] = fP[0] + mJ[0][3]*px + mJ[0][4]*py + mJ[0][5]*pz;
1252  P[1] = fP[1] + mJ[1][3]*px + mJ[1][4]*py + mJ[1][5]*pz;
1253  P[2] = fP[2] + mJ[2][3]*px + mJ[2][4]*py + mJ[2][5]*pz;
1254  P[3] = mJ[3][3]*px + mJ[3][4]*py + mJ[3][5]*pz;
1255  P[4] = mJ[4][3]*px + mJ[4][4]*py + mJ[4][5]*pz;
1256  P[5] = mJ[5][3]*px + mJ[5][4]*py + mJ[5][5]*pz;
1257  P[6] = fP[6];
1258  P[7] = fP[7];
1259 
1260  MultQSQt( mJ[0], fC, C);
1261 
1262 }
1263 
1264 
1265 void KFParticleBase::TransportBz( Double_t b, Double_t t,
1266  Double_t p[], Double_t e[] ) const
1267 {
1268  //* Transport the particle on dS, output to P[],C[], for Bz field
1269 
1270  const Double_t kCLight = 0.000299792458;
1271  b = b*fQ*kCLight;
1272  Double_t bs= b*t;
1273  Double_t s = TMath::Sin(bs), c = TMath::Cos(bs);
1274  Double_t sB, cB;
1275  if( TMath::Abs(bs)>1.e-10){
1276  sB= s/b;
1277  cB= (1-c)/b;
1278  }else{
1279  const Double_t kOvSqr6 = 1./TMath::Sqrt(6.);
1280  sB = (1.-bs*kOvSqr6)*(1.+bs*kOvSqr6)*t;
1281  cB = .5*sB*bs;
1282  }
1283 
1284  Double_t px = fP[3];
1285  Double_t py = fP[4];
1286  Double_t pz = fP[5];
1287 
1288  p[0] = fP[0] + sB*px + cB*py;
1289  p[1] = fP[1] - cB*px + sB*py;
1290  p[2] = fP[2] + t*pz;
1291  p[3] = c*px + s*py;
1292  p[4] = -s*px + c*py;
1293  p[5] = fP[5];
1294  p[6] = fP[6];
1295  p[7] = fP[7];
1296 
1297  /*
1298  Double_t mJ[8][8] = { {1,0,0, sB, cB, 0, 0, 0 },
1299  {0,1,0, -cB, sB, 0, 0, 0 },
1300  {0,0,1, 0, 0, t, 0, 0 },
1301  {0,0,0, c, s, 0, 0, 0 },
1302  {0,0,0, -s, c, 0, 0, 0 },
1303  {0,0,0, 0, 0, 1, 0, 0 },
1304  {0,0,0, 0, 0, 0, 1, 0 },
1305  {0,0,0, 0, 0, 0, 0, 1 } };
1306  Double_t mA[8][8];
1307  for( Int_t k=0,i=0; i<8; i++)
1308  for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fC[k];
1309 
1310  Double_t mJC[8][8];
1311  for( Int_t i=0; i<8; i++ )
1312  for( Int_t j=0; j<8; j++ ){
1313  mJC[i][j]=0;
1314  for( Int_t k=0; k<8; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
1315  }
1316 
1317  for( Int_t k=0,i=0; i<8; i++)
1318  for( Int_t j=0; j<=i; j++, k++ ){
1319  e[k] = 0;
1320  for( Int_t l=0; l<8; l++ ) e[k]+=mJC[i][l]*mJ[j][l];
1321  }
1322 
1323  return;
1324  */
1325 
1326  Double_t
1327  c6=fC[6], c7=fC[7], c8=fC[8], c17=fC[17], c18=fC[18],
1328  c24 = fC[24], c31 = fC[31];
1329 
1330  Double_t
1331  cBC13 = cB*fC[13],
1332  mJC13 = c7 - cB*fC[9] + sB*fC[13],
1333  mJC14 = fC[11] - cBC13 + sB*fC[14],
1334  mJC23 = c8 + t*c18,
1335  mJC24 = fC[12] + t*fC[19],
1336  mJC33 = c*fC[9] + s*fC[13],
1337  mJC34 = c*fC[13] + s*fC[14],
1338  mJC43 = -s*fC[9] + c*fC[13],
1339  mJC44 = -s*fC[13] + c*fC[14];
1340 
1341 
1342  e[0]= fC[0] + 2*(sB*c6 + cB*fC[10]) + (sB*fC[9] + 2*cBC13)*sB + cB*cB*fC[14];
1343  e[1]= fC[1] - cB*c6 + sB*fC[10] + mJC13*sB + mJC14*cB;
1344  e[2]= fC[2] - cB*c7 + sB*fC[11] - mJC13*cB + mJC14*sB;
1345  e[3]= fC[3] + t*fC[15] + mJC23*sB + mJC24*cB;
1346  e[4]= fC[4] + t*fC[16] - mJC23*cB + mJC24*sB;
1347 
1348  e[15]= fC[15] + c18*sB + fC[19]*cB;
1349  e[16]= fC[16] - c18*cB + fC[19]*sB;
1350  e[17]= c17 + fC[20]*t;
1351  e[18]= c18*c + fC[19]*s;
1352  e[19]= -c18*s + fC[19]*c;
1353 
1354  e[5]= fC[5] + (c17 + e[17] )*t;
1355 
1356  e[6]= c*c6 + s*fC[10] + mJC33*sB + mJC34*cB;
1357  e[7]= c*c7 + s*fC[11] - mJC33*cB + mJC34*sB;
1358  e[8]= c*c8 + s*fC[12] + e[18]*t;
1359  e[9]= mJC33*c + mJC34*s;
1360  e[10]= -s*c6 + c*fC[10] + mJC43*sB + mJC44*cB;
1361 
1362 
1363  e[11]= -s*c7 + c*fC[11] - mJC43*cB + mJC44*sB;
1364  e[12]= -s*c8 + c*fC[12] + e[19]*t;
1365  e[13]= mJC43*c + mJC44*s;
1366  e[14]= -mJC43*s + mJC44*c;
1367  e[20]= fC[20];
1368  e[21]= fC[21] + fC[25]*cB + c24*sB;
1369  e[22]= fC[22] - c24*cB + fC[25]*sB;
1370  e[23]= fC[23] + fC[26]*t;
1371  e[24]= c*c24 + s*fC[25];
1372  e[25]= c*fC[25] - c24*s;
1373  e[26]= fC[26];
1374  e[27]= fC[27];
1375  e[28]= fC[28] + fC[32]*cB + c31*sB;
1376  e[29]= fC[29] - c31*cB + fC[32]*sB;
1377  e[30]= fC[30] + fC[33]*t;
1378  e[31]= c*c31 + s*fC[32];
1379  e[32]= c*fC[32] - s*c31;
1380  e[33]= fC[33];
1381  e[34]= fC[34];
1382  e[35]= fC[35];
1383 }
1384 
1385 
1386 Double_t KFParticleBase::GetDistanceFromVertex( const KFParticleBase &Vtx ) const
1387 {
1388  //* Calculate distance from vertex [cm]
1389 
1390  return GetDistanceFromVertex( Vtx.fP );
1391 }
1392 
1393 Double_t KFParticleBase::GetDistanceFromVertex( const Double_t vtx[] ) const
1394 {
1395  //* Calculate distance from vertex [cm]
1396 
1397  Double_t mP[8], mC[36];
1398  Transport( GetDStoPoint(vtx), mP, mC );
1399  Double_t d[3]={ vtx[0]-mP[0], vtx[1]-mP[1], vtx[2]-mP[2]};
1400  return TMath::Sqrt( d[0]*d[0]+d[1]*d[1]+d[2]*d[2] );
1401 }
1402 
1403 Double_t KFParticleBase::GetDistanceFromParticle( const KFParticleBase &p )
1404  const
1405 {
1406  //* Calculate distance to other particle [cm]
1407 
1408  Double_t dS, dS1;
1409  GetDStoParticle( p, dS, dS1 );
1410  Double_t mP[8], mC[36], mP1[8], mC1[36];
1411  Transport( dS, mP, mC );
1412  p.Transport( dS1, mP1, mC1 );
1413  Double_t dx = mP[0]-mP1[0];
1414  Double_t dy = mP[1]-mP1[1];
1415  Double_t dz = mP[2]-mP1[2];
1416  return TMath::Sqrt(dx*dx+dy*dy+dz*dz);
1417 }
1418 
1419 Double_t KFParticleBase::GetDeviationFromVertex( const KFParticleBase &Vtx ) const
1420 {
1421  //* Calculate sqrt(Chi2/ndf) deviation from vertex
1422 
1423  return GetDeviationFromVertex( Vtx.fP, Vtx.fC );
1424 }
1425 
1426 
1427 Double_t KFParticleBase::GetDeviationFromVertex( const Double_t v[], const Double_t Cv[] ) const
1428 {
1429  //* Calculate sqrt(Chi2/ndf) deviation from vertex
1430  //* v = [xyz], Cv=[Cxx,Cxy,Cyy,Cxz,Cyz,Czz]-covariance matrix
1431 
1432  Double_t mP[8];
1433  Double_t mC[36];
1434  Double_t dS = GetDStoPoint(v);
1435  Transport(dS , mP, mC );
1436 
1437  Double_t d[3]={ v[0]-mP[0], v[1]-mP[1], v[2]-mP[2]};
1438 
1439  Double_t sigmaS = .1+10.*TMath::Sqrt( (d[0]*d[0]+d[1]*d[1]+d[2]*d[2])/
1440  (mP[3]*mP[3]+mP[4]*mP[4]+mP[5]*mP[5]) );
1441 
1442 
1443  Double_t h[3] = { mP[3]*sigmaS, mP[4]*sigmaS, mP[5]*sigmaS };
1444 
1445  Double_t mSi[6] =
1446  { mC[0] +h[0]*h[0],
1447  mC[1] +h[1]*h[0], mC[2] +h[1]*h[1],
1448  mC[3] +h[2]*h[0], mC[4] +h[2]*h[1], mC[5] +h[2]*h[2] };
1449 
1450  if( Cv ){
1451  mSi[0]+=Cv[0];
1452  mSi[1]+=Cv[1];
1453  mSi[2]+=Cv[2];
1454  mSi[3]+=Cv[3];
1455  mSi[4]+=Cv[4];
1456  mSi[5]+=Cv[5];
1457  }
1458 
1459  Double_t mS[6];
1460 
1461  mS[0] = mSi[2]*mSi[5] - mSi[4]*mSi[4];
1462  mS[1] = mSi[3]*mSi[4] - mSi[1]*mSi[5];
1463  mS[2] = mSi[0]*mSi[5] - mSi[3]*mSi[3];
1464  mS[3] = mSi[1]*mSi[4] - mSi[2]*mSi[3];
1465  mS[4] = mSi[1]*mSi[3] - mSi[0]*mSi[4];
1466  mS[5] = mSi[0]*mSi[2] - mSi[1]*mSi[1];
1467 
1468  Double_t s = ( mSi[0]*mS[0] + mSi[1]*mS[1] + mSi[3]*mS[3] );
1469  s = ( s > 1.E-20 ) ?1./s :0;
1470 
1471  return TMath::Sqrt( TMath::Abs(s*( ( mS[0]*d[0] + mS[1]*d[1] + mS[3]*d[2])*d[0]
1472  +(mS[1]*d[0] + mS[2]*d[1] + mS[4]*d[2])*d[1]
1473  +(mS[3]*d[0] + mS[4]*d[1] + mS[5]*d[2])*d[2] ))/2);
1474 }
1475 
1476 
1477 Double_t KFParticleBase::GetDeviationFromParticle( const KFParticleBase &p )
1478  const
1479 {
1480  //* Calculate sqrt(Chi2/ndf) deviation from other particle
1481 
1482  Double_t dS, dS1;
1483  GetDStoParticle( p, dS, dS1 );
1484  Double_t mP1[8], mC1[36];
1485  p.Transport( dS1, mP1, mC1 );
1486 
1487  Double_t d[3]={ fP[0]-mP1[0], fP[1]-mP1[1], fP[2]-mP1[2]};
1488 
1489  Double_t sigmaS = .1+10.*TMath::Sqrt( (d[0]*d[0]+d[1]*d[1]+d[2]*d[2])/
1490  (mP1[3]*mP1[3]+mP1[4]*mP1[4]+mP1[5]*mP1[5]) );
1491 
1492  Double_t h[3] = { mP1[3]*sigmaS, mP1[4]*sigmaS, mP1[5]*sigmaS };
1493 
1494  mC1[0] +=h[0]*h[0];
1495  mC1[1] +=h[1]*h[0];
1496  mC1[2] +=h[1]*h[1];
1497  mC1[3] +=h[2]*h[0];
1498  mC1[4] +=h[2]*h[1];
1499  mC1[5] +=h[2]*h[2];
1500 
1501  return GetDeviationFromVertex( mP1, mC1 )*TMath::Sqrt(2./1.);
1502 }
1503 
1504 
1505 
1506 void KFParticleBase::SubtractFromVertex( KFParticleBase &Vtx ) const
1507 {
1508  //* Subtract the particle from the vertex
1509 
1510  Double_t fld[3];
1511  {
1512  GetFieldValue( Vtx.fP, fld );
1513  const Double_t kCLight = 0.000299792458;
1514  fld[0]*=kCLight; fld[1]*=kCLight; fld[2]*=kCLight;
1515  }
1516 
1517  Double_t m[8];
1518  Double_t mCm[36];
1519 
1520  if( Vtx.fIsLinearized ){
1521  GetMeasurement( Vtx.fVtxGuess, m, mCm );
1522  } else {
1523  GetMeasurement( Vtx.fP, m, mCm );
1524  }
1525 
1526  Double_t mV[6];
1527 
1528  mV[ 0] = mCm[ 0];
1529  mV[ 1] = mCm[ 1];
1530  mV[ 2] = mCm[ 2];
1531  mV[ 3] = mCm[ 3];
1532  mV[ 4] = mCm[ 4];
1533  mV[ 5] = mCm[ 5];
1534 
1535  //*
1536 
1537  Double_t mS[6] = { mV[0]-Vtx.fC[0],
1538  mV[1]-Vtx.fC[1], mV[2]-Vtx.fC[2],
1539  mV[3]-Vtx.fC[3], mV[4]-Vtx.fC[4], mV[5]-Vtx.fC[5] };
1540  InvertCholetsky3(mS);
1541  //* Residual (measured - estimated)
1542 
1543  Double_t zeta[3] = { m[0]-Vtx.fP[0], m[1]-Vtx.fP[1], m[2]-Vtx.fP[2] };
1544 
1545  //* mCHt = mCH' - D'
1546 
1547  Double_t mCHt0[3], mCHt1[3], mCHt2[3];
1548 
1549  mCHt0[0]=Vtx.fC[ 0] ; mCHt1[0]=Vtx.fC[ 1] ; mCHt2[0]=Vtx.fC[ 3] ;
1550  mCHt0[1]=Vtx.fC[ 1] ; mCHt1[1]=Vtx.fC[ 2] ; mCHt2[1]=Vtx.fC[ 4] ;
1551  mCHt0[2]=Vtx.fC[ 3] ; mCHt1[2]=Vtx.fC[ 4] ; mCHt2[2]=Vtx.fC[ 5] ;
1552 
1553  //* Kalman gain K = mCH'*S
1554 
1555  Double_t k0[3], k1[3], k2[3];
1556 
1557  for(Int_t i=0;i<3;++i){
1558  k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
1559  k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
1560  k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
1561  }
1562 
1563  //* New estimation of the vertex position r += K*zeta
1564 
1565  Double_t dChi2 = -(mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2])*zeta[0]
1566  - (mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2])*zeta[1]
1567  - (mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2])*zeta[2];
1568 
1569  if( Vtx.fChi2 - dChi2 < 0 ) return;
1570 
1571  for(Int_t i=0;i<3;++i)
1572  Vtx.fP[i] -= (k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2]);
1573 
1574  //* New covariance matrix C -= K*(mCH')'
1575 
1576  for(Int_t i=0, k=0;i<3;++i){
1577  for(Int_t j=0;j<=i;++j,++k)
1578  Vtx.fC[k] += k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j];
1579  }
1580 
1581  //* Calculate Chi^2
1582 
1583  Vtx.fNDF -= 2;
1584  Vtx.fChi2 += dChi2;
1585 }
1586 
1587 void KFParticleBase::SubtractFromParticle( KFParticleBase &Vtx ) const
1588 {
1589  //* Subtract the particle from the mother particle
1590 
1591  Double_t m[8];
1592  Double_t mV[36];
1593 
1594  if( Vtx.fIsLinearized ){
1595  GetMeasurement( Vtx.fVtxGuess, m, mV );
1596  } else {
1597  GetMeasurement( Vtx.fP, m, mV );
1598  }
1599 
1600  Double_t mS[6]= { mV[0] - Vtx.fC[0],
1601  mV[1] - Vtx.fC[1], mV[2] - Vtx.fC[2],
1602  mV[3] - Vtx.fC[3], mV[4] - Vtx.fC[4], mV[5] - Vtx.fC[5] };
1603  InvertCholetsky3(mS);
1604 
1605  //* Residual (measured - estimated)
1606 
1607  Double_t zeta[3] = { m[0]-Vtx.fP[0], m[1]-Vtx.fP[1], m[2]-Vtx.fP[2] };
1608 
1609  //* CHt = CH' - D'
1610 
1611  Double_t mCHt0[7], mCHt1[7], mCHt2[7];
1612 
1613  mCHt0[0]=mV[ 0] ; mCHt1[0]=mV[ 1] ; mCHt2[0]=mV[ 3] ;
1614  mCHt0[1]=mV[ 1] ; mCHt1[1]=mV[ 2] ; mCHt2[1]=mV[ 4] ;
1615  mCHt0[2]=mV[ 3] ; mCHt1[2]=mV[ 4] ; mCHt2[2]=mV[ 5] ;
1616  mCHt0[3]=Vtx.fC[ 6]-mV[ 6]; mCHt1[3]=Vtx.fC[ 7]-mV[ 7]; mCHt2[3]=Vtx.fC[ 8]-mV[ 8];
1617  mCHt0[4]=Vtx.fC[10]-mV[10]; mCHt1[4]=Vtx.fC[11]-mV[11]; mCHt2[4]=Vtx.fC[12]-mV[12];
1618  mCHt0[5]=Vtx.fC[15]-mV[15]; mCHt1[5]=Vtx.fC[16]-mV[16]; mCHt2[5]=Vtx.fC[17]-mV[17];
1619  mCHt0[6]=Vtx.fC[21]-mV[21]; mCHt1[6]=Vtx.fC[22]-mV[22]; mCHt2[6]=Vtx.fC[23]-mV[23];
1620 
1621  //* Kalman gain K = mCH'*S
1622 
1623  Double_t k0[7], k1[7], k2[7];
1624 
1625  for(Int_t i=0;i<7;++i){
1626  k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
1627  k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
1628  k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
1629  }
1630 
1631  //* Add the daughter momentum to the particle momentum
1632 
1633  Vtx.fP[ 3] -= m[ 3];
1634  Vtx.fP[ 4] -= m[ 4];
1635  Vtx.fP[ 5] -= m[ 5];
1636  Vtx.fP[ 6] -= m[ 6];
1637 
1638  Vtx.fC[ 9] -= mV[ 9];
1639  Vtx.fC[13] -= mV[13];
1640  Vtx.fC[14] -= mV[14];
1641  Vtx.fC[18] -= mV[18];
1642  Vtx.fC[19] -= mV[19];
1643  Vtx.fC[20] -= mV[20];
1644  Vtx.fC[24] -= mV[24];
1645  Vtx.fC[25] -= mV[25];
1646  Vtx.fC[26] -= mV[26];
1647  Vtx.fC[27] -= mV[27];
1648 
1649  //* New estimation of the vertex position r += K*zeta
1650 
1651  for(Int_t i=0;i<3;++i)
1652  Vtx.fP[i] = m[i] - (k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2]);
1653  for(Int_t i=3;i<7;++i)
1654  Vtx.fP[i] = Vtx.fP[i] - (k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2]);
1655 
1656  //* New covariance matrix C -= K*(mCH')'
1657 
1658  Double_t ffC[28] = { -mV[ 0],
1659  -mV[ 1], -mV[ 2],
1660  -mV[ 3], -mV[ 4], -mV[ 5],
1661  mV[ 6], mV[ 7], mV[ 8], Vtx.fC[ 9],
1662  mV[10], mV[11], mV[12], Vtx.fC[13], Vtx.fC[14],
1663  mV[15], mV[16], mV[17], Vtx.fC[18], Vtx.fC[19], Vtx.fC[20],
1664  mV[21], mV[22], mV[23], Vtx.fC[24], Vtx.fC[25], Vtx.fC[26], Vtx.fC[27] };
1665 
1666  for(Int_t i=0, k=0;i<7;++i){
1667  for(Int_t j=0;j<=i;++j,++k){
1668  Vtx.fC[k] = ffC[k] + (k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j] );
1669  }
1670  }
1671 
1672  //* Calculate Chi^2
1673  Vtx.fNDF -= 2;
1674  Vtx.fQ -= GetQ();
1675  Vtx.fSFromDecay = 0;
1676  Vtx.fChi2 -= ((mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2])*zeta[0]
1677  +(mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2])*zeta[1]
1678  +(mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2])*zeta[2]);
1679 }
1680 
1681 void KFParticleBase::TransportLine( Double_t dS,
1682  Double_t P[], Double_t C[] ) const
1683 {
1684  //* Transport the particle as a straight line
1685 
1686  P[0] = fP[0] + dS*fP[3];
1687  P[1] = fP[1] + dS*fP[4];
1688  P[2] = fP[2] + dS*fP[5];
1689  P[3] = fP[3];
1690  P[4] = fP[4];
1691  P[5] = fP[5];
1692  P[6] = fP[6];
1693  P[7] = fP[7];
1694 
1695  Double_t c6 = fC[ 6] + dS*fC[ 9];
1696  Double_t c11 = fC[11] + dS*fC[14];
1697  Double_t c17 = fC[17] + dS*fC[20];
1698  Double_t sc13 = dS*fC[13];
1699  Double_t sc18 = dS*fC[18];
1700  Double_t sc19 = dS*fC[19];
1701 
1702  C[ 0] = fC[ 0] + dS*( fC[ 6] + c6 );
1703  C[ 2] = fC[ 2] + dS*( fC[11] + c11 );
1704  C[ 5] = fC[ 5] + dS*( fC[17] + c17 );
1705 
1706  C[ 7] = fC[ 7] + sc13;
1707  C[ 8] = fC[ 8] + sc18;
1708  C[ 9] = fC[ 9];
1709 
1710  C[12] = fC[12] + sc19;
1711 
1712  C[ 1] = fC[ 1] + dS*( fC[10] + C[ 7] );
1713  C[ 3] = fC[ 3] + dS*( fC[15] + C[ 8] );
1714  C[ 4] = fC[ 4] + dS*( fC[16] + C[12] );
1715  C[ 6] = c6;
1716 
1717  C[10] = fC[10] + sc13;
1718  C[11] = c11;
1719 
1720  C[13] = fC[13];
1721  C[14] = fC[14];
1722  C[15] = fC[15] + sc18;
1723  C[16] = fC[16] + sc19;
1724  C[17] = c17;
1725 
1726  C[18] = fC[18];
1727  C[19] = fC[19];
1728  C[20] = fC[20];
1729  C[21] = fC[21] + dS*fC[24];
1730  C[22] = fC[22] + dS*fC[25];
1731  C[23] = fC[23] + dS*fC[26];
1732 
1733  C[24] = fC[24];
1734  C[25] = fC[25];
1735  C[26] = fC[26];
1736  C[27] = fC[27];
1737  C[28] = fC[28] + dS*fC[31];
1738  C[29] = fC[29] + dS*fC[32];
1739  C[30] = fC[30] + dS*fC[33];
1740 
1741  C[31] = fC[31];
1742  C[32] = fC[32];
1743  C[33] = fC[33];
1744  C[34] = fC[34];
1745  C[35] = fC[35];
1746 }
1747 
1748 
1749 void KFParticleBase::ConstructGammaBz( const KFParticleBase &daughter1,
1750  const KFParticleBase &daughter2, Double_t Bz )
1751 {
1752  //* Create gamma
1753 
1754  const KFParticleBase *daughters[2] = { &daughter1, &daughter2};
1755 
1756  Double_t v0[3];
1757 
1758  if( !fIsLinearized ){
1759  Double_t ds, ds1;
1760  Double_t m[8];
1761  Double_t mCd[36];
1762  daughter1.GetDStoParticle(daughter2, ds, ds1);
1763  daughter1.Transport( ds, m, mCd );
1764  v0[0] = m[0];
1765  v0[1] = m[1];
1766  v0[2] = m[2];
1767  daughter2.Transport( ds1, m, mCd );
1768  v0[0] = .5*( v0[0] + m[0] );
1769  v0[1] = .5*( v0[1] + m[1] );
1770  v0[2] = .5*( v0[2] + m[2] );
1771  } else {
1772  v0[0] = fVtxGuess[0];
1773  v0[1] = fVtxGuess[1];
1774  v0[2] = fVtxGuess[2];
1775  }
1776 
1777  fAtProductionVertex = 0;
1778  fSFromDecay = 0;
1779  fP[0] = v0[0];
1780  fP[1] = v0[1];
1781  fP[2] = v0[2];
1782  fP[3] = 0;
1783  fP[4] = 0;
1784  fP[5] = 0;
1785  fP[6] = 0;
1786  fP[7] = 0;
1787 
1788 
1789  // fit daughters to the vertex guess
1790 
1791  Double_t daughterP[2][8], daughterC[2][36];
1792  Double_t vtxMom[2][3];
1793 
1794  {
1795  for( Int_t id=0; id<2; id++ ){
1796 
1797  Double_t *p = daughterP[id];
1798  Double_t *mC = daughterC[id];
1799 
1800  daughters[id]->GetMeasurement( v0, p, mC );
1801 
1802  Double_t mAi[6];
1803  for(int i=0; i<6; i++) mAi[i] = mC[i];
1804  InvertCholetsky3(mAi);
1805  //InvertSym3(mC, mAi );
1806 
1807  Double_t mB[3][3];
1808 
1809  mB[0][0] = mC[ 6]*mAi[0] + mC[ 7]*mAi[1] + mC[ 8]*mAi[3];
1810  mB[0][1] = mC[ 6]*mAi[1] + mC[ 7]*mAi[2] + mC[ 8]*mAi[4];
1811  mB[0][2] = mC[ 6]*mAi[3] + mC[ 7]*mAi[4] + mC[ 8]*mAi[5];
1812 
1813  mB[1][0] = mC[10]*mAi[0] + mC[11]*mAi[1] + mC[12]*mAi[3];
1814  mB[1][1] = mC[10]*mAi[1] + mC[11]*mAi[2] + mC[12]*mAi[4];
1815  mB[1][2] = mC[10]*mAi[3] + mC[11]*mAi[4] + mC[12]*mAi[5];
1816 
1817  mB[2][0] = mC[15]*mAi[0] + mC[16]*mAi[1] + mC[17]*mAi[3];
1818  mB[2][1] = mC[15]*mAi[1] + mC[16]*mAi[2] + mC[17]*mAi[4];
1819  mB[2][2] = mC[15]*mAi[3] + mC[16]*mAi[4] + mC[17]*mAi[5];
1820 
1821  Double_t z[3] = { v0[0]-p[0], v0[1]-p[1], v0[2]-p[2] };
1822 
1823  vtxMom[id][0] = p[3] + mB[0][0]*z[0] + mB[0][1]*z[1] + mB[0][2]*z[2];
1824  vtxMom[id][1] = p[4] + mB[1][0]*z[0] + mB[1][1]*z[1] + mB[1][2]*z[2];
1825  vtxMom[id][2] = p[5] + mB[2][0]*z[0] + mB[2][1]*z[1] + mB[2][2]*z[2];
1826 
1827  daughters[id]->Transport( daughters[id]->GetDStoPoint(v0), p, mC );
1828 
1829  }
1830 
1831  } // fit daughters to guess
1832 
1833 
1834  // fit new vertex
1835  {
1836 
1837  Double_t mpx0 = vtxMom[0][0]+vtxMom[1][0];
1838  Double_t mpy0 = vtxMom[0][1]+vtxMom[1][1];
1839  Double_t mpt0 = TMath::Sqrt(mpx0*mpx0 + mpy0*mpy0);
1840  // Double_t a0 = TMath::ATan2(mpy0,mpx0);
1841 
1842  Double_t ca0 = mpx0/mpt0;
1843  Double_t sa0 = mpy0/mpt0;
1844  Double_t r[3] = { v0[0], v0[1], v0[2] };
1845  Double_t mC[3][3] = {{10000., 0 , 0 },
1846  {0, 10000., 0 },
1847  {0, 0, 10000. } };
1848  Double_t chi2=0;
1849 
1850  for( Int_t id=0; id<2; id++ ){
1851  const Double_t kCLight = 0.000299792458;
1852  Double_t q = Bz*daughters[id]->GetQ()*kCLight;
1853  Double_t px0 = vtxMom[id][0];
1854  Double_t py0 = vtxMom[id][1];
1855  Double_t pz0 = vtxMom[id][2];
1856  Double_t pt0 = TMath::Sqrt(px0*px0+py0*py0);
1857  Double_t mG[3][6], mB[3], mH[3][3];
1858  // r = {vx,vy,vz};
1859  // m = {x,y,z,Px,Py,Pz};
1860  // V = daughter.C
1861  // G*m + B = H*r;
1862  // q*x + Py - q*vx - sin(a)*Pt = 0
1863  // q*y - Px - q*vy + cos(a)*Pt = 0
1864  // (Px*cos(a) + Py*sin(a) ) (vz -z) - Pz( cos(a)*(vx-x) + sin(a)*(vy-y)) = 0
1865 
1866  mG[0][0] = q;
1867  mG[0][1] = 0;
1868  mG[0][2] = 0;
1869  mG[0][3] = -sa0*px0/pt0;
1870  mG[0][4] = 1 -sa0*py0/pt0;
1871  mG[0][5] = 0;
1872  mH[0][0] = q;
1873  mH[0][1] = 0;
1874  mH[0][2] = 0;
1875  mB[0] = py0 - sa0*pt0 - mG[0][3]*px0 - mG[0][4]*py0 ;
1876 
1877  // q*y - Px - q*vy + cos(a)*Pt = 0
1878 
1879  mG[1][0] = 0;
1880  mG[1][1] = q;
1881  mG[1][2] = 0;
1882  mG[1][3] = -1 + ca0*px0/pt0;
1883  mG[1][4] = + ca0*py0/pt0;
1884  mG[1][5] = 0;
1885  mH[1][0] = 0;
1886  mH[1][1] = q;
1887  mH[1][2] = 0;
1888  mB[1] = -px0 + ca0*pt0 - mG[1][3]*px0 - mG[1][4]*py0 ;
1889 
1890  // (Px*cos(a) + Py*sin(a) ) (z -vz) - Pz( cos(a)*(x-vx) + sin(a)*(y-vy)) = 0
1891 
1892  mG[2][0] = -pz0*ca0;
1893  mG[2][1] = -pz0*sa0;
1894  mG[2][2] = px0*ca0 + py0*sa0;
1895  mG[2][3] = 0;
1896  mG[2][4] = 0;
1897  mG[2][5] = 0;
1898 
1899  mH[2][0] = mG[2][0];
1900  mH[2][1] = mG[2][1];
1901  mH[2][2] = mG[2][2];
1902 
1903  mB[2] = 0;
1904 
1905  // fit the vertex
1906 
1907  // V = GVGt
1908 
1909  Double_t mGV[3][6];
1910  Double_t mV[6];
1911  Double_t m[3];
1912  for( Int_t i=0; i<3; i++ ){
1913  m[i] = mB[i];
1914  for( Int_t k=0; k<6; k++ ) m[i]+=mG[i][k]*daughterP[id][k];
1915  }
1916  for( Int_t i=0; i<3; i++ ){
1917  for( Int_t j=0; j<6; j++ ){
1918  mGV[i][j] = 0;
1919  for( Int_t k=0; k<6; k++ ) mGV[i][j]+=mG[i][k]*daughterC[id][ IJ(k,j) ];
1920  }
1921  }
1922  for( Int_t i=0, k=0; i<3; i++ ){
1923  for( Int_t j=0; j<=i; j++,k++ ){
1924  mV[k] = 0;
1925  for( Int_t l=0; l<6; l++ ) mV[k]+=mGV[i][l]*mG[j][l];
1926  }
1927  }
1928 
1929 
1930  //* CHt
1931 
1932  Double_t mCHt[3][3];
1933  Double_t mHCHt[6];
1934  Double_t mHr[3];
1935  for( Int_t i=0; i<3; i++ ){
1936  mHr[i] = 0;
1937  for( Int_t k=0; k<3; k++ ) mHr[i]+= mH[i][k]*r[k];
1938  }
1939 
1940  for( Int_t i=0; i<3; i++ ){
1941  for( Int_t j=0; j<3; j++){
1942  mCHt[i][j] = 0;
1943  for( Int_t k=0; k<3; k++ ) mCHt[i][j]+= mC[i][k]*mH[j][k];
1944  }
1945  }
1946 
1947  for( Int_t i=0, k=0; i<3; i++ ){
1948  for( Int_t j=0; j<=i; j++, k++ ){
1949  mHCHt[k] = 0;
1950  for( Int_t l=0; l<3; l++ ) mHCHt[k]+= mH[i][l]*mCHt[l][j];
1951  }
1952  }
1953 
1954  Double_t mS[6] = { mHCHt[0]+mV[0],
1955  mHCHt[1]+mV[1], mHCHt[2]+mV[2],
1956  mHCHt[3]+mV[3], mHCHt[4]+mV[4], mHCHt[5]+mV[5] };
1957 
1958 
1959  //InvertSym3(mS,mS);
1960  InvertCholetsky3(mS);
1961 
1962  //* Residual (measured - estimated)
1963 
1964  Double_t zeta[3] = { m[0]-mHr[0], m[1]-mHr[1], m[2]-mHr[2] };
1965 
1966  //* Kalman gain K = mCH'*S
1967 
1968  Double_t k[3][3];
1969 
1970  for(Int_t i=0;i<3;++i){
1971  k[i][0] = mCHt[i][0]*mS[0] + mCHt[i][1]*mS[1] + mCHt[i][2]*mS[3];
1972  k[i][1] = mCHt[i][0]*mS[1] + mCHt[i][1]*mS[2] + mCHt[i][2]*mS[4];
1973  k[i][2] = mCHt[i][0]*mS[3] + mCHt[i][1]*mS[4] + mCHt[i][2]*mS[5];
1974  }
1975 
1976  //* New estimation of the vertex position r += K*zeta
1977 
1978  for(Int_t i=0;i<3;++i)
1979  r[i] = r[i] + k[i][0]*zeta[0] + k[i][1]*zeta[1] + k[i][2]*zeta[2];
1980 
1981  //* New covariance matrix C -= K*(mCH')'
1982 
1983  for(Int_t i=0;i<3;++i){
1984  for(Int_t j=0;j<=i;++j){
1985  mC[i][j] = mC[i][j] - (k[i][0]*mCHt[j][0] + k[i][1]*mCHt[j][1] + k[i][2]*mCHt[j][2]);
1986  mC[j][i] = mC[i][j];
1987  }
1988  }
1989 
1990 
1991  //* Calculate Chi^2
1992 
1993  chi2 += ( ( mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2] )*zeta[0]
1994  +(mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2] )*zeta[1]
1995  +(mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2] )*zeta[2] );
1996  }
1997 
1998  // store vertex
1999 
2000  fNDF = 2;
2001  fChi2 = chi2;
2002  for( Int_t i=0; i<3; i++ ) fP[i] = r[i];
2003  for( Int_t i=0,k=0; i<3; i++ ){
2004  for( Int_t j=0; j<=i; j++,k++ ){
2005  fC[k] = mC[i][j];
2006  }
2007  }
2008  }
2009 
2010  // now fit daughters to the vertex
2011 
2012  fQ = 0;
2013  fSFromDecay = 0;
2014 
2015  for(Int_t i=3;i<8;++i) fP[i]=0.;
2016  for(Int_t i=6;i<36;++i) fC[i]=0.;
2017 
2018 
2019  for( Int_t id=0; id<2; id++ ){
2020 
2021  Double_t *p = daughterP[id];
2022  Double_t *mC = daughterC[id];
2023  daughters[id]->GetMeasurement( v0, p, mC );
2024 
2025  const Double_t *m = fP, *mV = fC;
2026 
2027  Double_t mAi[6];
2028  for(int i=0; i<6; i++) mAi[i] = mC[i];
2029  InvertCholetsky3(mAi);
2030  //InvertSym3(mC, mAi );
2031 
2032  Double_t mB[4][3];
2033 
2034  mB[0][0] = mC[ 6]*mAi[0] + mC[ 7]*mAi[1] + mC[ 8]*mAi[3];
2035  mB[0][1] = mC[ 6]*mAi[1] + mC[ 7]*mAi[2] + mC[ 8]*mAi[4];
2036  mB[0][2] = mC[ 6]*mAi[3] + mC[ 7]*mAi[4] + mC[ 8]*mAi[5];
2037 
2038  mB[1][0] = mC[10]*mAi[0] + mC[11]*mAi[1] + mC[12]*mAi[3];
2039  mB[1][1] = mC[10]*mAi[1] + mC[11]*mAi[2] + mC[12]*mAi[4];
2040  mB[1][2] = mC[10]*mAi[3] + mC[11]*mAi[4] + mC[12]*mAi[5];
2041 
2042  mB[2][0] = mC[15]*mAi[0] + mC[16]*mAi[1] + mC[17]*mAi[3];
2043  mB[2][1] = mC[15]*mAi[1] + mC[16]*mAi[2] + mC[17]*mAi[4];
2044  mB[2][2] = mC[15]*mAi[3] + mC[16]*mAi[4] + mC[17]*mAi[5];
2045 
2046  mB[3][0] = mC[21]*mAi[0] + mC[22]*mAi[1] + mC[23]*mAi[3];
2047  mB[3][1] = mC[21]*mAi[1] + mC[22]*mAi[2] + mC[23]*mAi[4];
2048  mB[3][2] = mC[21]*mAi[3] + mC[22]*mAi[4] + mC[23]*mAi[5];
2049 
2050 
2051  Double_t z[3] = { m[0]-p[0], m[1]-p[1], m[2]-p[2] };
2052 
2053  {
2054  Double_t mAV[6] = { mC[0]-mV[0], mC[1]-mV[1], mC[2]-mV[2],
2055  mC[3]-mV[3], mC[4]-mV[4], mC[5]-mV[5] };
2056 
2057  Double_t mAVi[6];
2058  for(int i=0; i<6; i++) mAVi[i] = mAV[i];
2059  InvertCholetsky3(mAVi);
2060 // if( !InvertSym3(mAV, mAVi) ){
2061  Double_t dChi2 = ( +(mAVi[0]*z[0] + mAVi[1]*z[1] + mAVi[3]*z[2])*z[0]
2062  +(mAVi[1]*z[0] + mAVi[2]*z[1] + mAVi[4]*z[2])*z[1]
2063  +(mAVi[3]*z[0] + mAVi[4]*z[1] + mAVi[5]*z[2])*z[2] );
2064  fChi2+= TMath::Abs( dChi2 );
2065  // }
2066  fNDF += 2;
2067  }
2068 
2069  //* Add the daughter momentum to the particle momentum
2070 
2071  fP[3]+= p[3] + mB[0][0]*z[0] + mB[0][1]*z[1] + mB[0][2]*z[2];
2072  fP[4]+= p[4] + mB[1][0]*z[0] + mB[1][1]*z[1] + mB[1][2]*z[2];
2073  fP[5]+= p[5] + mB[2][0]*z[0] + mB[2][1]*z[1] + mB[2][2]*z[2];
2074  fP[6]+= p[6] + mB[3][0]*z[0] + mB[3][1]*z[1] + mB[3][2]*z[2];
2075 
2076  Double_t d0, d1, d2;
2077 
2078  d0= mB[0][0]*mV[0] + mB[0][1]*mV[1] + mB[0][2]*mV[3] - mC[ 6];
2079  d1= mB[0][0]*mV[1] + mB[0][1]*mV[2] + mB[0][2]*mV[4] - mC[ 7];
2080  d2= mB[0][0]*mV[3] + mB[0][1]*mV[4] + mB[0][2]*mV[5] - mC[ 8];
2081 
2082  //fC[6]+= mC[ 6] + d0;
2083  //fC[7]+= mC[ 7] + d1;
2084  //fC[8]+= mC[ 8] + d2;
2085  fC[9]+= mC[ 9] + d0*mB[0][0] + d1*mB[0][1] + d2*mB[0][2];
2086 
2087  d0= mB[1][0]*mV[0] + mB[1][1]*mV[1] + mB[1][2]*mV[3] - mC[10];
2088  d1= mB[1][0]*mV[1] + mB[1][1]*mV[2] + mB[1][2]*mV[4] - mC[11];
2089  d2= mB[1][0]*mV[3] + mB[1][1]*mV[4] + mB[1][2]*mV[5] - mC[12];
2090 
2091  //fC[10]+= mC[10]+ d0;
2092  //fC[11]+= mC[11]+ d1;
2093  //fC[12]+= mC[12]+ d2;
2094  fC[13]+= mC[13]+ d0*mB[0][0] + d1*mB[0][1] + d2*mB[0][2];
2095  fC[14]+= mC[14]+ d0*mB[1][0] + d1*mB[1][1] + d2*mB[1][2];
2096 
2097  d0= mB[2][0]*mV[0] + mB[2][1]*mV[1] + mB[2][2]*mV[3] - mC[15];
2098  d1= mB[2][0]*mV[1] + mB[2][1]*mV[2] + mB[2][2]*mV[4] - mC[16];
2099  d2= mB[2][0]*mV[3] + mB[2][1]*mV[4] + mB[2][2]*mV[5] - mC[17];
2100 
2101  //fC[15]+= mC[15]+ d0;
2102  //fC[16]+= mC[16]+ d1;
2103  //fC[17]+= mC[17]+ d2;
2104  fC[18]+= mC[18]+ d0*mB[0][0] + d1*mB[0][1] + d2*mB[0][2];
2105  fC[19]+= mC[19]+ d0*mB[1][0] + d1*mB[1][1] + d2*mB[1][2];
2106  fC[20]+= mC[20]+ d0*mB[2][0] + d1*mB[2][1] + d2*mB[2][2];
2107 
2108  d0= mB[3][0]*mV[0] + mB[3][1]*mV[1] + mB[3][2]*mV[3] - mC[21];
2109  d1= mB[3][0]*mV[1] + mB[3][1]*mV[2] + mB[3][2]*mV[4] - mC[22];
2110  d2= mB[3][0]*mV[3] + mB[3][1]*mV[4] + mB[3][2]*mV[5] - mC[23];
2111 
2112  //fC[21]+= mC[21] + d0;
2113  //fC[22]+= mC[22] + d1;
2114  //fC[23]+= mC[23] + d2;
2115  fC[24]+= mC[24] + d0*mB[0][0] + d1*mB[0][1] + d2*mB[0][2];
2116  fC[25]+= mC[25] + d0*mB[1][0] + d1*mB[1][1] + d2*mB[1][2];
2117  fC[26]+= mC[26] + d0*mB[2][0] + d1*mB[2][1] + d2*mB[2][2];
2118  fC[27]+= mC[27] + d0*mB[3][0] + d1*mB[3][1] + d2*mB[3][2];
2119  }
2120 
2121  SetMassConstraint(0,0);
2122 
2123 }
2124 
2125 Bool_t KFParticleBase::InvertSym3( const Double_t A[], Double_t Ai[] )
2126 {
2127  //* Invert symmetric matric stored in low-triagonal form
2128 
2129  Bool_t ret = 0;
2130  Double_t a0 = A[0], a1 = A[1], a2 = A[2], a3 = A[3];
2131 
2132  Ai[0] = a2*A[5] - A[4]*A[4];
2133  Ai[1] = a3*A[4] - a1*A[5];
2134  Ai[3] = a1*A[4] - a2*a3;
2135  Double_t det = (a0*Ai[0] + a1*Ai[1] + a3*Ai[3]);
2136  if( det>1.e-20 ) det = 1./det;
2137  else{
2138  det = 0;
2139  ret = 1;
2140  }
2141  Ai[0] *= det;
2142  Ai[1] *= det;
2143  Ai[3] *= det;
2144  Ai[2] = ( a0*A[5] - a3*a3 )*det;
2145  Ai[4] = ( a1*a3 - a0*A[4] )*det;
2146  Ai[5] = ( a0*a2 - a1*a1 )*det;
2147  return ret;
2148 }
2149 
2150 void KFParticleBase::InvertCholetsky3(Double_t a[6])
2151 {
2152  Double_t d[3], uud, u[3][3];
2153  for(int i=0; i<3; i++)
2154  {
2155  d[i]=0;
2156  for(int j=0; j<3; j++)
2157  u[i][j]=0;
2158  }
2159 
2160  for(int i=0; i<3 ; i++)
2161  {
2162  uud=0;
2163  for(int j=0; j<i; j++)
2164  uud += u[j][i]*u[j][i]*d[j];
2165  uud = a[i*(i+3)/2] - uud;
2166 
2167  if(fabs(uud)<1.e-12f) uud = 1.e-12f;
2168 
2169  d[i] = uud/fabs(uud);
2170  u[i][i] = sqrt(fabs(uud));
2171 
2172  for(int j=i+1; j<3; j++)
2173  {
2174  uud = 0;
2175  for(int k=0; k<i; k++)
2176  uud += u[k][i]*u[k][j]*d[k];
2177  uud = a[j*(j+1)/2+i] - uud;
2178  u[i][j] = d[i]/u[i][i]*uud;
2179  }
2180  }
2181 
2182  Double_t u1[3];
2183 
2184  for(int i=0; i<3; i++)
2185  {
2186  u1[i] = u[i][i];
2187  u[i][i] = 1/u[i][i];
2188  }
2189  for(int i=0; i<2; i++)
2190  {
2191  u[i][i+1] = - u[i][i+1]*u[i][i]*u[i+1][i+1];
2192  }
2193  for(int i=0; i<1; i++)
2194  {
2195  u[i][i+2] = u[i][i+1]*u1[i+1]*u[i+1][i+2]-u[i][i+2]*u[i][i]*u[i+2][i+2];
2196  }
2197 
2198  for(int i=0; i<3; i++)
2199  a[i+3] = u[i][2]*u[2][2]*d[2];
2200  for(int i=0; i<2; i++)
2201  a[i+1] = u[i][1]*u[1][1]*d[1] + u[i][2]*u[1][2]*d[2];
2202  a[0] = u[0][0]*u[0][0]*d[0] + u[0][1]*u[0][1]*d[1] + u[0][2]*u[0][2]*d[2];
2203 }
2204 
2205 void KFParticleBase::MultQSQt( const Double_t Q[], const Double_t S[], Double_t SOut[] )
2206 {
2207  //* Matrix multiplication Q*S*Q^T, Q - square matrix, S - symmetric
2208 
2209  const Int_t kN= 8;
2210  Double_t mA[kN*kN];
2211 
2212  for( Int_t i=0, ij=0; i<kN; i++ ){
2213  for( Int_t j=0; j<kN; j++, ++ij ){
2214  mA[ij] = 0 ;
2215  for( Int_t k=0; k<kN; ++k ) mA[ij]+= S[( k<=i ) ? i*(i+1)/2+k :k*(k+1)/2+i] * Q[ j*kN+k];
2216  }
2217  }
2218 
2219  for( Int_t i=0; i<kN; i++ ){
2220  for( Int_t j=0; j<=i; j++ ){
2221  Int_t ij = ( j<=i ) ? i*(i+1)/2+j :j*(j+1)/2+i;
2222  SOut[ij] = 0 ;
2223  for( Int_t k=0; k<kN; k++ ) SOut[ij] += Q[ i*kN+k ] * mA[ k*kN+j ];
2224  }
2225  }
2226 }
2227 //________________________________________________________________________________
2228 void KFParticleBase::Print(Option_t *opt) const {
2229  std::cout << *this << std::endl;
2230  if (opt && (opt[0] == 'a' || opt[0] == 'A')) {
2231  TRVector P(8,fP); std::cout << "par. " << P << std::endl;
2232  TRSymMatrix C(8,fC); std::cout << "cov. " << C << std::endl;
2233 
2234  }
2235 }
2236 //________________________________________________________________________________
2237 std::ostream& operator<<(std::ostream& os, const KFParticleBase& particle) {
2238  static const Char_t *vn[14] = {"x","y","z","px","py","pz","E","S","M","t","p","Q","Chi2","NDF"};
2239  os << Form("p(%4i,%4i,%4i)",particle.GetID(),particle.GetParentID(),particle.IdParentMcVx());
2240  for (Int_t i = 0; i < 8; i++) {
2241  if (i == 6) continue; // E
2242  if (i == 7 && particle.GetParameter(i) <= 0.0) continue; // S
2243  if (particle.GetParameter(i) == 0. && particle.GetCovariance(i,i) == 0) continue;
2244  if (particle.GetCovariance(i,i) > 0)
2245  os << Form(" %s:%8.3f+/-%6.3f", vn[i], particle.GetParameter(i), TMath::Sqrt(particle.GetCovariance(i,i)));
2246  else
2247  os << Form(" %s:%8.3f", vn[i], particle.GetParameter(i));
2248  }
2249  Double_t Mtp[3], MtpErr[3];
2250  particle.GetMass(Mtp[0], MtpErr[0]); if (MtpErr[0] < 1e-7 || MtpErr[0] > 1e10) MtpErr[0] = -13;
2251  particle.GetLifeTime(Mtp[1], MtpErr[1]); if (MtpErr[1] <= 0 || MtpErr[1] > 1e10) MtpErr[1] = -13;
2252  particle.GetMomentum(Mtp[2], MtpErr[2]); if (MtpErr[2] <= 0 || MtpErr[2] > 1e10) MtpErr[2] = -13;
2253  for (Int_t i = 8; i < 11; i++) {
2254  if (i == 9 && Mtp[i-8] <= 0.0) continue; // t
2255  if (MtpErr[i-8] > 0 && MtpErr[i-8] < 1e10) os << Form(" %s:%8.3f+/-%7.3f", vn[i],Mtp[i-8],MtpErr[i-8]);
2256  else os << Form(" %s:%8.3f", vn[i],Mtp[i-8]);
2257  }
2258  os << Form(" pdg:%5i Q:%2i chi2/NDF :%8.2f/%2i",particle.GetPDG(),particle.GetQ(),particle.GetChi2(),particle.GetNDF());
2259  if (particle.IdTruth()) os << Form(" IdT:%4i/%3i",particle.IdTruth(),particle.QaTruth());
2260  return os;
2261 }
KFParticleBase::fVtxGuess
Double32_t fVtxGuess[3]
Flag shows that the particle error along its trajectory is taken from production vertex.
Definition: KFParticleBase.h:241
KFParticleBase::fIsLinearized
Bool_t fIsLinearized
Guess for the position of the decay vertex ( used for linearisation of equations ) ...
Definition: KFParticleBase.h:243
Generated by   doxygen 1.8.5