Analysis Software
Documentation for sPHENIX simulation software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
KFParticleBase.cxx
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file KFParticleBase.cxx
1 /*
2  * This file is part of KFParticle package
3  * Copyright (C) 2007-2019 FIAS Frankfurt Institute for Advanced Studies
4  * 2007-2019 Goethe University of Frankfurt
5  * 2007-2019 Ivan Kisel <I.Kisel@compeng.uni-frankfurt.de>
6  * 2007-2019 Maksym Zyzak
7  * 2007-2019 Sergey Gorbunov
8  *
9  * KFParticle is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * KFParticle is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program. If not, see <https://www.gnu.org/licenses/>.
21  */
22 
23 
24 #include "KFParticleBase.h"
25 #include <cmath>
26 
27 #ifndef KFParticleStandalone
28 ClassImp(KFParticleBase)
29 #endif
30 
31 #ifdef __ROOT__
32 #include "TClass.h"
33 #include "TRSymMatrix.h"
34 #include "TRVector.h"
35 #include <iostream>
36 
37 KFParticleBase::KFParticleBase() :fChi2(0), fSFromDecay(0), SumDaughterMass(0), fMassHypo(-1), fNDF(-3),
38  fId(-1), fParentID(0), fIdTruth(0), fQuality(0), fIdParentMcVx(0), fAtProductionVertex(0),
39  fQ(0), fConstructMethod(0), fPDG(0), fDaughtersIds()
40 {
41  static Bool_t first = kTRUE;
42  if (first) {
43  first = kFALSE;
44  KFParticleBase::Class()->IgnoreTObjectStreamer();
45  }
46  //* Constructor
47  Clear();
48 }
49 
50 void KFParticleBase::Clear(Option_t *option) {
51  Initialize();
52  fIdTruth = 0;
53  fQuality = 0;
54  fIdParentMcVx = 0;
55  fParentID = 0;
56 }
57 
58 void KFParticleBase::Print(Option_t *opt) const {
59  std::cout << *this << std::endl;
60  if (opt && (opt[0] == 'a' || opt[0] == 'A')) {
61  TRVector P(8,fP); std::cout << "par. " << P << std::endl;
62  TRSymMatrix C(8,fC); std::cout << "cov. " << C << std::endl;
63 
64  }
65 }
66 
67 std::ostream& operator<<(std::ostream& os, const KFParticleBase& particle) {
68  static const Char_t *vn[14] = {"x","y","z","px","py","pz","E","S","M","t","p","Q","Chi2","NDF"};
69  os << Form("p(%4i,%4i,%4i)",particle.Id(),particle.GetParentID(),particle.IdParentMcVx());
70  for (Int_t i = 0; i < 8; i++) {
71  if (i == 6) continue; // E
72  if (i == 7 && particle.GetParameter(i) <= 0.0) continue; // S
73  if (particle.GetParameter(i) == 0. && particle.GetCovariance(i,i) == 0) continue;
74  if (particle.GetCovariance(i,i) > 0)
75  os << Form(" %s:%8.3f+/-%6.3f", vn[i], particle.GetParameter(i), TMath::Sqrt(particle.GetCovariance(i,i)));
76  else
77  os << Form(" %s:%8.3f", vn[i], particle.GetParameter(i));
78  }
79  float Mtp[3] = {0.f, 0.f, 0.f}, MtpErr[3] = {0.f, 0.f, 0.f};
80  particle.GetMass(Mtp[0], MtpErr[0]); if (MtpErr[0] < 1e-7 || MtpErr[0] > 1e10) MtpErr[0] = -13;
81  particle.GetLifeTime(Mtp[1], MtpErr[1]); if (MtpErr[1] <= 0 || MtpErr[1] > 1e10) MtpErr[1] = -13;
82  particle.GetMomentum(Mtp[2], MtpErr[2]); if (MtpErr[2] <= 0 || MtpErr[2] > 1e10) MtpErr[2] = -13;
83  for (Int_t i = 8; i < 11; i++) {
84  if (i == 9 && Mtp[i-8] <= 0.0) continue; // t
85  if (MtpErr[i-8] > 0 && MtpErr[i-8] < 9e2) os << Form(" %s:%8.3f+/-%7.3f", vn[i],Mtp[i-8],MtpErr[i-8]);
86  else os << Form(" %s:%8.3f", vn[i],Mtp[i-8]);
87  }
88  os << Form(" pdg:%5i Q:%2i chi2/NDF :%8.2f/%2i",particle.GetPDG(),particle.GetQ(),particle.GetChi2(),particle.GetNDF());
89  if (particle.IdTruth()) os << Form(" IdT:%4i/%3i",particle.IdTruth(),particle.QaTruth());
90  int nd = particle.NDaughters();
91  if (nd > 1) {
92  os << " ND: " << nd << ":";
93  if (nd > 3) nd = 3;
94  for (int d = 0; d < nd; d++) {
95  os << particle.DaughterIds()[d];
96  if (d < nd-1) os << ",";
97  }
98  }
99  return os;
100 }
101 #endif
102 
103 #ifndef __ROOT__
104 KFParticleBase::KFParticleBase() : fChi2(0), fSFromDecay(0),
105  SumDaughterMass(0), fMassHypo(-1), fNDF(-3), fId(-1), fAtProductionVertex(0), fQ(0), fConstructMethod(0), fPDG(0), fDaughtersIds()
106 {
114  Initialize();
115 }
116 #endif
117 
118 void KFParticleBase::Initialize( const float Param[], const float Cov[], Int_t Charge, float Mass )
119 {
137  for( Int_t i=0; i<6 ; i++ ) fP[i] = Param[i];
138  for( Int_t i=0; i<21; i++ ) fC[i] = Cov[i];
139 
140  float energy = sqrt( Mass*Mass + fP[3]*fP[3] + fP[4]*fP[4] + fP[5]*fP[5]);
141  fP[6] = energy;
142  fP[7] = 0;
143  fQ = Charge;
144  fNDF = 0;
145  fChi2 = 0;
146  fAtProductionVertex = 0;
147  fSFromDecay = 0;
148 
149  float energyInv = 1./energy;
150  float
151  h0 = fP[3]*energyInv,
152  h1 = fP[4]*energyInv,
153  h2 = fP[5]*energyInv;
154 
155  fC[21] = h0*fC[ 6] + h1*fC[10] + h2*fC[15];
156  fC[22] = h0*fC[ 7] + h1*fC[11] + h2*fC[16];
157  fC[23] = h0*fC[ 8] + h1*fC[12] + h2*fC[17];
158  fC[24] = h0*fC[ 9] + h1*fC[13] + h2*fC[18];
159  fC[25] = h0*fC[13] + h1*fC[14] + h2*fC[19];
160  fC[26] = h0*fC[18] + h1*fC[19] + h2*fC[20];
161  fC[27] = ( h0*h0*fC[ 9] + h1*h1*fC[14] + h2*h2*fC[20]
162  + 2*(h0*h1*fC[13] + h0*h2*fC[18] + h1*h2*fC[19] ) );
163  for( Int_t i=28; i<36; i++ ) fC[i] = 0;
164  fC[35] = 1.;
165 
166  SumDaughterMass = Mass;
167  fMassHypo = Mass;
168 }
169 
170 void KFParticleBase::Initialize()
171 {
180  for( Int_t i=0; i<8; i++) fP[i] = 0;
181  for(Int_t i=0;i<36;++i) fC[i]=0.;
182  fC[0] = fC[2] = fC[5] = 100.;
183  fC[35] = 1.;
184  fNDF = -3;
185  fChi2 = 0.;
186  fQ = 0;
187  fSFromDecay = 0;
188  fAtProductionVertex = 0;
189  SumDaughterMass = 0;
190  fMassHypo = -1;
191 }
192 
193 Int_t KFParticleBase::GetMomentum( float &p, float &error ) const
194 {
200  float x = fP[3];
201  float y = fP[4];
202  float z = fP[5];
203  float x2 = x*x;
204  float y2 = y*y;
205  float z2 = z*z;
206  float p2 = x2+y2+z2;
207  p = sqrt(p2);
208  error = (x2*fC[9]+y2*fC[14]+z2*fC[20] + 2*(x*y*fC[13]+x*z*fC[18]+y*z*fC[19]) );
209  if( error>1.e-16 && p>1.e-4 ){
210  error = sqrt(error)/p;
211  return 0;
212  }
213  error = 1.e8;
214  return 1;
215 }
216 
217 Int_t KFParticleBase::GetPt( float &pt, float &error ) const
218 {
224  float px = fP[3];
225  float py = fP[4];
226  float px2 = px*px;
227  float py2 = py*py;
228  float pt2 = px2+py2;
229  pt = sqrt(pt2);
230  error = (px2*fC[9] + py2*fC[14] + 2*px*py*fC[13] );
231  if( error>0 && pt>1.e-4 ){
232  error = sqrt(error)/pt;
233  return 0;
234  }
235  error = 1.e10;
236  return 1;
237 }
238 
239 Int_t KFParticleBase::GetEta( float &eta, float &error ) const
240 {
246  float px = fP[3];
247  float py = fP[4];
248  float pz = fP[5];
249  float pt2 = px*px + py*py;
250  float p2 = pt2 + pz*pz;
251  float p = sqrt(p2);
252  float a = p + pz;
253  float b = p - pz;
254  eta = 1.e10;
255  if( b > 1.e-8 ){
256  float c = a/b;
257  if( c>1.e-8 ) eta = 0.5*log(c);
258  }
259  float h3 = -px*pz;
260  float h4 = -py*pz;
261  float pt4 = pt2*pt2;
262  float p2pt4 = p2*pt4;
263  error = (h3*h3*fC[9] + h4*h4*fC[14] + pt4*fC[20] + 2*( h3*(h4*fC[13] + fC[18]*pt2) + pt2*h4*fC[19] ) );
264 
265  if( error>0 && p2pt4>1.e-10 ){
266  error = sqrt(error/p2pt4);
267  return 0;
268  }
269 
270  error = 1.e10;
271  return 1;
272 }
273 
274 Int_t KFParticleBase::GetPhi( float &phi, float &error ) const
275 {
281  float px = fP[3];
282  float py = fP[4];
283  float px2 = px*px;
284  float py2 = py*py;
285  float pt2 = px2 + py2;
286  phi = atan2(py,px);
287  error = (py2*fC[9] + px2*fC[14] - 2*px*py*fC[13] );
288  if( error>0 && pt2>1.e-4 ){
289  error = sqrt(error)/pt2;
290  return 0;
291  }
292  error = 1.e10;
293  return 1;
294 }
295 
296 Int_t KFParticleBase::GetR( float &r, float &error ) const
297 {
303  float x = fP[0];
304  float y = fP[1];
305  float x2 = x*x;
306  float y2 = y*y;
307  r = sqrt(x2 + y2);
308  error = (x2*fC[0] + y2*fC[2] - 2*x*y*fC[1] );
309  if( error>0 && r>1.e-4 ){
310  error = sqrt(error)/r;
311  return 0;
312  }
313  error = 1.e10;
314  return 1;
315 }
316 
317 Int_t KFParticleBase::GetMass( float &m, float &error ) const
318 {
324  // s = sigma^2 of m2/2
325 
326  float s = ( fP[3]*fP[3]*fC[9] + fP[4]*fP[4]*fC[14] + fP[5]*fP[5]*fC[20]
327  + fP[6]*fP[6]*fC[27]
328  + 2*( + fP[3]*fP[4]*fC[13] + fP[5]*(fP[3]*fC[18] + fP[4]*fC[19])
329  - fP[6]*( fP[3]*fC[24] + fP[4]*fC[25] + fP[5]*fC[26] ) )
330  );
331 
332  float m2 = (fP[6]*fP[6] - fP[3]*fP[3] - fP[4]*fP[4] - fP[5]*fP[5]);
333 
334  if(m2<0.)
335  {
336  error = 1.e3;
337  m = -sqrt(-m2);
338  return 1;
339  }
340 
341  m = sqrt(m2);
342  if( m>1.e-6 ){
343  if( s >= 0 ) {
344  error = sqrt(s)/m;
345  return 0;
346  }
347  }
348  else {
349  error = 0.;
350  return 0;
351  }
352  error = 1.e3;
353 
354  return 1;
355 }
356 
357 
358 Int_t KFParticleBase::GetDecayLength( float &l, float &error ) const
359 {
366  float x = fP[3];
367  float y = fP[4];
368  float z = fP[5];
369  float t = fP[7];
370  float x2 = x*x;
371  float y2 = y*y;
372  float z2 = z*z;
373  float p2 = x2+y2+z2;
374  l = t*sqrt(p2);
375  if( p2>1.e-4){
376  error = p2*fC[35] + t*t/p2*(x2*fC[9]+y2*fC[14]+z2*fC[20]
377  + 2*(x*y*fC[13]+x*z*fC[18]+y*z*fC[19]) )
378  + 2*t*(x*fC[31]+y*fC[32]+z*fC[33]);
379  error = sqrt(fabs(error));
380  return 0;
381  }
382  error = 1.e20;
383  return 1;
384 }
385 
386 Int_t KFParticleBase::GetDecayLengthXY( float &l, float &error ) const
387 {
395  float x = fP[3];
396  float y = fP[4];
397  float t = fP[7];
398  float x2 = x*x;
399  float y2 = y*y;
400  float pt2 = x2+y2;
401  l = t*sqrt(pt2);
402  if( pt2>1.e-4){
403  error = pt2*fC[35] + t*t/pt2*(x2*fC[9]+y2*fC[14] + 2*x*y*fC[13] )
404  + 2*t*(x*fC[31]+y*fC[32]);
405  error = sqrt(fabs(error));
406  return 0;
407  }
408  error = 1.e20;
409  return 1;
410 }
411 
412 
413 Int_t KFParticleBase::GetLifeTime( float &ctau, float &error ) const
414 {
422  float m, dm;
423  GetMass( m, dm );
424  float cTM = (-fP[3]*fC[31] - fP[4]*fC[32] - fP[5]*fC[33] + fP[6]*fC[34]);
425  ctau = fP[7]*m;
426  error = m*m*fC[35] + 2*fP[7]*cTM + fP[7]*fP[7]*dm*dm;
427  if( error > 0 ){
428  error = sqrt( error );
429  return 0;
430  }
431  error = 1.e20;
432  return 1;
433 }
434 
435 
436 void KFParticleBase::operator +=( const KFParticleBase &Daughter )
437 {
442  AddDaughter( Daughter );
443 }
444 
445 bool KFParticleBase::GetMeasurement( const KFParticleBase& daughter, float m[], float V[], float D[3][3] )
446 {
460  if(fNDF == -1)
461  {
462  float ds[2] = {0.f,0.f};
463  float dsdr[4][6];
464  float F1[36], F2[36], F3[36], F4[36];
465  for(int i1=0; i1<36; i1++)
466  {
467  F1[i1] = 0;
468  F2[i1] = 0;
469  F3[i1] = 0;
470  F4[i1] = 0;
471  }
472  GetDStoParticle( daughter, ds, dsdr );
473 
474  if( fabs(ds[0]*fP[5]) > 1000.f || fabs(ds[1]*daughter.fP[5]) > 1000.f)
475  return 0;
476 
477  float V0Tmp[36] = {0.};
478  float V1Tmp[36] = {0.};
479 
480  float C[36];
481  for(int iC=0; iC<36; iC++)
482  C[iC] = fC[iC];
483 
484  Transport(ds[0], dsdr[0], fP, fC, dsdr[1], F1, F2);
485  daughter.Transport(ds[1], dsdr[3], m, V, dsdr[2], F4, F3);
486 
487  MultQSQt(F2, daughter.fC, V0Tmp, 6);
488  MultQSQt(F3, C, V1Tmp, 6);
489 
490  for(int iC=0; iC<21; iC++)
491  {
492  fC[iC] += V0Tmp[iC];
493  V[iC] += V1Tmp[iC];
494  }
495 
496  float C1F1T[6][6];
497  for(int i=0; i<6; i++)
498  for(int j=0; j<6; j++)
499  {
500  C1F1T[i][j] = 0;
501  for(int k=0; k<6; k++)
502  {
503  C1F1T[i][j] += C[IJ(i,k)] * F1[j*6+k];
504  }
505  }
506  float F3C1F1T[6][6];
507  for(int i=0; i<6; i++)
508  for(int j=0; j<6; j++)
509  {
510  F3C1F1T[i][j] = 0;
511  for(int k=0; k<6; k++)
512  {
513  F3C1F1T[i][j] += F3[i*6+k] * C1F1T[k][j];
514  }
515  }
516  float C2F2T[6][6];
517  for(int i=0; i<6; i++)
518  for(int j=0; j<6; j++)
519  {
520  C2F2T[i][j] = 0;
521  for(int k=0; k<6; k++)
522  {
523  C2F2T[i][j] += daughter.fC[IJ(i,k)] * F2[j*6+k];
524  }
525  }
526  for(int i=0; i<3; i++)
527  for(int j=0; j<3; j++)
528  {
529  D[i][j] = F3C1F1T[i][j];
530  for(int k=0; k<6; k++)
531  {
532  D[i][j] += F4[i*6+k] * C2F2T[k][j];
533  }
534  }
535  }
536  else
537  {
538  float dsdr[6];
539  float dS = daughter.GetDStoPoint(fP, dsdr);
540 
541  float dsdp[6] = {-dsdr[0], -dsdr[1], -dsdr[2], 0, 0, 0};
542 
543  float F[36], F1[36];
544  for(int i2=0; i2<36; i2++)
545  {
546  F[i2] = 0;
547  F1[i2] = 0;
548  }
549  daughter.Transport(dS, dsdr, m, V, dsdp, F, F1);
550 
551 // float V1Tmp[36] = {0.};
552 // MultQSQt(F1, fC, V1Tmp, 6);
553 
554 // for(int iC=0; iC<21; iC++)
555 // V[iC] += V1Tmp[iC];
556 
557  float VFT[3][6];
558  for(int i=0; i<3; i++)
559  for(int j=0; j<6; j++)
560  {
561  VFT[i][j] = 0;
562  for(int k=0; k<3; k++)
563  {
564  VFT[i][j] += fC[IJ(i,k)] * F1[j*6+k];
565  }
566  }
567 
568  float FVFT[6][6];
569  for(int i=0; i<6; i++)
570  for(int j=0; j<6; j++)
571  {
572  FVFT[i][j] = 0;
573  for(int k=0; k<3; k++)
574  {
575  FVFT[i][j] += F1[i*6+k] * VFT[k][j];
576  }
577  }
578 
579  for(int i=0; i<3; i++)
580  for(int j=0; j<3; j++)
581  {
582  D[i][j] = 0;
583  for(int k=0; k<3; k++)
584  {
585  D[i][j] += fC[IJ(j,k)] * F1[i*6+k];
586  }
587  }
588 
589  V[0] += FVFT[0][0];
590  V[1] += FVFT[1][0];
591  V[2] += FVFT[1][1];
592  V[3] += FVFT[2][0];
593  V[4] += FVFT[2][1];
594  V[5] += FVFT[2][2];
595 
596 // if(fNDF > 100)
597 // {
598 // float dx = fP[0] - m[0];
599 // float dy = fP[1] - m[1];
600 // float dz = fP[2] - m[2];
601 // float sigmaS = 3.f*sqrt( (dx*dx + dy*dy + dz*dz) / (m[3]*m[3] + m[4]*m[4] + m[5]*m[5]) );
602 //
603 // float h[3] = { m[3]*sigmaS, m[4]*sigmaS, m[5]*sigmaS };
604 // V[0]+= h[0]*h[0];
605 // V[1]+= h[1]*h[0];
606 // V[2]+= h[1]*h[1];
607 // V[3]+= h[2]*h[0];
608 // V[4]+= h[2]*h[1];
609 // V[5]+= h[2]*h[2];
610 // }
611  }
612 
613  return 1;
614 }
615 
616 void KFParticleBase::AddDaughter( const KFParticleBase &Daughter )
617 {
631  if( fNDF<-1 ){ // first daughter -> just copy
632  fNDF = -1;
633  fQ = Daughter.GetQ();
634  for( Int_t i=0; i<7; i++) fP[i] = Daughter.fP[i];
635  for( Int_t i=0; i<28; i++) fC[i] = Daughter.fC[i];
636  fSFromDecay = 0;
637  fMassHypo = Daughter.fMassHypo;
638  SumDaughterMass = Daughter.SumDaughterMass;
639  return;
640  }
641 
642  if(static_cast<int>(fConstructMethod) == 0)
643  AddDaughterWithEnergyFit(Daughter);
644  else if(static_cast<int>(fConstructMethod) == 2)
645  AddDaughterWithEnergyFitMC(Daughter);
646 
647  SumDaughterMass += Daughter.SumDaughterMass;
648  fMassHypo = -1;
649 }
650 
651 void KFParticleBase::AddDaughterWithEnergyFit( const KFParticleBase &Daughter )
652 {
661  Int_t maxIter = 1;
662 
663  for( Int_t iter=0; iter<maxIter; iter++ ){
664 
665  float m[8], mV[36];
666 
667  float D[3][3];
668  if(! GetMeasurement(Daughter, m, mV, D) )
669  return;
670 
671  float mS[6]= { fC[0]+mV[0],
672  fC[1]+mV[1], fC[2]+mV[2],
673  fC[3]+mV[3], fC[4]+mV[4], fC[5]+mV[5] };
674 
675  InvertCholetsky3(mS);
676 
677  //* Residual (measured - estimated)
678 
679  float zeta[3] = { m[0]-fP[0], m[1]-fP[1], m[2]-fP[2] };
680 
681  float dChi2 = (mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2])*zeta[0]
682  + (mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2])*zeta[1]
683  + (mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2])*zeta[2];
684  if (dChi2 > 1e9) return;
685 // if(fNDF > 100 && dChi2 > 9) return;
686 
687  float K[3][3];
688  for(int i=0; i<3; i++)
689  for(int j=0; j<3; j++)
690  {
691  K[i][j] = 0;
692  for(int k=0; k<3; k++)
693  K[i][j] += fC[IJ(i,k)] * mS[IJ(k,j)];
694  }
695 
696  //* CHt = CH' - D'
697  float mCHt0[7], mCHt1[7], mCHt2[7];
698 
699  mCHt0[0]=fC[ 0] ; mCHt1[0]=fC[ 1] ; mCHt2[0]=fC[ 3] ;
700  mCHt0[1]=fC[ 1] ; mCHt1[1]=fC[ 2] ; mCHt2[1]=fC[ 4] ;
701  mCHt0[2]=fC[ 3] ; mCHt1[2]=fC[ 4] ; mCHt2[2]=fC[ 5] ;
702  mCHt0[3]=fC[ 6]-mV[ 6]; mCHt1[3]=fC[ 7]-mV[ 7]; mCHt2[3]=fC[ 8]-mV[ 8];
703  mCHt0[4]=fC[10]-mV[10]; mCHt1[4]=fC[11]-mV[11]; mCHt2[4]=fC[12]-mV[12];
704  mCHt0[5]=fC[15]-mV[15]; mCHt1[5]=fC[16]-mV[16]; mCHt2[5]=fC[17]-mV[17];
705  mCHt0[6]=fC[21]-mV[21]; mCHt1[6]=fC[22]-mV[22]; mCHt2[6]=fC[23]-mV[23];
706 
707  //* Kalman gain K = mCH'*S
708 
709  float k0[7], k1[7], k2[7];
710 
711  for(Int_t i=0;i<7;++i){
712  k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
713  k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
714  k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
715  }
716 
717  //* Add the daughter momentum to the particle momentum
718 
719  fP[ 3] += m[ 3];
720  fP[ 4] += m[ 4];
721  fP[ 5] += m[ 5];
722  fP[ 6] += m[ 6];
723 
724  fC[ 9] += mV[ 9];
725  fC[13] += mV[13];
726  fC[14] += mV[14];
727  fC[18] += mV[18];
728  fC[19] += mV[19];
729  fC[20] += mV[20];
730  fC[24] += mV[24];
731  fC[25] += mV[25];
732  fC[26] += mV[26];
733  fC[27] += mV[27];
734 
735 
736  //* New estimation of the vertex position r += K*zeta
737 
738  for(Int_t i=0;i<7;++i)
739  fP[i] = fP[i] + k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2];
740 
741  //* New covariance matrix C -= K*(mCH')'
742 
743  for(Int_t i=0, k=0;i<7;++i){
744  for(Int_t j=0;j<=i;++j,++k){
745  fC[k] = fC[k] - (k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j] );
746  }
747  }
748 
749  float K2[3][3];
750  for(int i=0; i<3; i++)
751  {
752  for(int j=0; j<3; j++)
753  K2[i][j] = -K[j][i];
754  K2[i][i] += 1;
755  }
756 
757  float A[3][3];
758  for(int i=0; i<3; i++)
759  for(int j=0; j<3; j++)
760  {
761  A[i][j] = 0;
762  for(int k=0; k<3; k++)
763  {
764  A[i][j] += D[i][k] * K2[k][j];
765  }
766  }
767 
768  double M[3][3];
769  for(int i=0; i<3; i++)
770  for(int j=0; j<3; j++)
771  {
772  M[i][j] = 0;
773  for(int k=0; k<3; k++)
774  {
775  M[i][j] += K[i][k] * A[k][j];
776  }
777  }
778 
779  fC[0] += 2*M[0][0];
780  fC[1] += M[0][1] + M[1][0];
781  fC[2] += 2*M[1][1];
782  fC[3] += M[0][2] + M[2][0];
783  fC[4] += M[1][2] + M[2][1];
784  fC[5] += 2*M[2][2];
785 
786  //* Calculate Chi^2
787 
788  fNDF += 2;
789  fQ += Daughter.GetQ();
790  fSFromDecay = 0;
791  fChi2 += dChi2;
792 
793  }
794 }
795 
796 void KFParticleBase::SubtractDaughter( const KFParticleBase &Daughter )
797 {
803  float m[8], mV[36];
804 
805  float D[3][3];
806  if(! GetMeasurement(Daughter, m, mV, D) )
807  return;
808 
809  float mS[6]= { fC[0]+mV[0],
810  fC[1]+mV[1], fC[2]+mV[2],
811  fC[3]+mV[3], fC[4]+mV[4], fC[5]+mV[5] };
812 
813  InvertCholetsky3(mS);
814 
815  //* Residual (measured - estimated)
816 
817  float zeta[3] = { m[0]-fP[0], m[1]-fP[1], m[2]-fP[2] };
818 
819  float dChi2 = (mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2])*zeta[0]
820  + (mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2])*zeta[1]
821  + (mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2])*zeta[2];
822 
823  float K[3][3];
824  for(int i=0; i<3; i++)
825  for(int j=0; j<3; j++)
826  {
827  K[i][j] = 0;
828  for(int k=0; k<3; k++)
829  K[i][j] += fC[IJ(i,k)] * mS[IJ(k,j)];
830  }
831 
832  //* CHt = CH' - D'
833  float mCHt0[7], mCHt1[7], mCHt2[7];
834 
835  mCHt0[0]=fC[ 0] ; mCHt1[0]=fC[ 1] ; mCHt2[0]=fC[ 3] ;
836  mCHt0[1]=fC[ 1] ; mCHt1[1]=fC[ 2] ; mCHt2[1]=fC[ 4] ;
837  mCHt0[2]=fC[ 3] ; mCHt1[2]=fC[ 4] ; mCHt2[2]=fC[ 5] ;
838  mCHt0[3]=fC[ 6]+mV[ 6]; mCHt1[3]=fC[ 7]+mV[ 7]; mCHt2[3]=fC[ 8]+mV[ 8];
839  mCHt0[4]=fC[10]+mV[10]; mCHt1[4]=fC[11]+mV[11]; mCHt2[4]=fC[12]+mV[12];
840  mCHt0[5]=fC[15]+mV[15]; mCHt1[5]=fC[16]+mV[16]; mCHt2[5]=fC[17]+mV[17];
841  mCHt0[6]=fC[21]+mV[21]; mCHt1[6]=fC[22]+mV[22]; mCHt2[6]=fC[23]+mV[23];
842 
843  //* Kalman gain K = mCH'*S
844 
845  float k0[7], k1[7], k2[7];
846 
847  for(Int_t i=0;i<7;++i){
848  k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
849  k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
850  k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
851  }
852 
853  //* Add the daughter momentum to the particle momentum
854 
855  fP[ 3] -= m[ 3];
856  fP[ 4] -= m[ 4];
857  fP[ 5] -= m[ 5];
858  fP[ 6] -= m[ 6];
859 
860  fC[ 9] += mV[ 9];
861  fC[13] += mV[13];
862  fC[14] += mV[14];
863  fC[18] += mV[18];
864  fC[19] += mV[19];
865  fC[20] += mV[20];
866  fC[24] += mV[24];
867  fC[25] += mV[25];
868  fC[26] += mV[26];
869  fC[27] += mV[27];
870 
871 
872  //* New estimation of the vertex position r += K*zeta
873 
874  for(Int_t i=0;i<7;++i)
875  fP[i] = fP[i] + k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2];
876 
877  //* New covariance matrix C -= K*(mCH')'
878 
879  for(Int_t i=0, k=0;i<7;++i){
880  for(Int_t j=0;j<=i;++j,++k){
881  fC[k] = fC[k] - (k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j] );
882  }
883  }
884 
885  float K2[3][3];
886  for(int i=0; i<3; i++)
887  {
888  for(int j=0; j<3; j++)
889  K2[i][j] = -K[j][i];
890  K2[i][i] += 1;
891  }
892 
893  float A[3][3];
894  for(int i=0; i<3; i++)
895  for(int j=0; j<3; j++)
896  {
897  A[i][j] = 0;
898  for(int k=0; k<3; k++)
899  {
900  A[i][j] += D[i][k] * K2[k][j];
901  }
902  }
903 
904  double M[3][3];
905  for(int i=0; i<3; i++)
906  for(int j=0; j<3; j++)
907  {
908  M[i][j] = 0;
909  for(int k=0; k<3; k++)
910  {
911  M[i][j] += K[i][k] * A[k][j];
912  }
913  }
914 
915  fC[0] += 2*M[0][0];
916  fC[1] += M[0][1] + M[1][0];
917  fC[2] += 2*M[1][1];
918  fC[3] += M[0][2] + M[2][0];
919  fC[4] += M[1][2] + M[2][1];
920  fC[5] += 2*M[2][2];
921 
922  //* Calculate Chi^2
923 
924  fNDF += 2;
925  fQ += Daughter.GetQ();
926  fSFromDecay = 0;
927  fChi2 += dChi2;
928 }
929 
930 
931 void KFParticleBase::AddDaughterWithEnergyFitMC( const KFParticleBase &Daughter )
932 {
939  Int_t maxIter = 1;
940 
941  for( Int_t iter=0; iter<maxIter; iter++ ){
942 
943  float m[8], mV[36];
944 
945  float D[3][3];
946  GetMeasurement(Daughter, m, mV, D);
947 
948  float mS[6]= { fC[0]+mV[0],
949  fC[1]+mV[1], fC[2]+mV[2],
950  fC[3]+mV[3], fC[4]+mV[4], fC[5]+mV[5] };
951  InvertCholetsky3(mS);
952  //* Residual (measured - estimated)
953 
954  float zeta[3] = { m[0]-fP[0], m[1]-fP[1], m[2]-fP[2] };
955 
956  float K[3][6];
957  for(int i=0; i<3; i++)
958  for(int j=0; j<3; j++)
959  {
960  K[i][j] = 0;
961  for(int k=0; k<3; k++)
962  K[i][j] += fC[IJ(i,k)] * mS[IJ(k,j)];
963  }
964 
965 
966  //* CHt = CH'
967 
968  float mCHt0[7], mCHt1[7], mCHt2[7];
969 
970  mCHt0[0]=fC[ 0] ; mCHt1[0]=fC[ 1] ; mCHt2[0]=fC[ 3] ;
971  mCHt0[1]=fC[ 1] ; mCHt1[1]=fC[ 2] ; mCHt2[1]=fC[ 4] ;
972  mCHt0[2]=fC[ 3] ; mCHt1[2]=fC[ 4] ; mCHt2[2]=fC[ 5] ;
973  mCHt0[3]=fC[ 6] ; mCHt1[3]=fC[ 7] ; mCHt2[3]=fC[ 8] ;
974  mCHt0[4]=fC[10] ; mCHt1[4]=fC[11] ; mCHt2[4]=fC[12] ;
975  mCHt0[5]=fC[15] ; mCHt1[5]=fC[16] ; mCHt2[5]=fC[17] ;
976  mCHt0[6]=fC[21] ; mCHt1[6]=fC[22] ; mCHt2[6]=fC[23] ;
977 
978  //* Kalman gain K = mCH'*S
979 
980  float k0[7], k1[7], k2[7];
981 
982  for(Int_t i=0;i<7;++i){
983  k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
984  k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
985  k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
986  }
987 
988  // last itearation -> update the particle
989 
990  //* VHt = VH'
991 
992  float mVHt0[7], mVHt1[7], mVHt2[7];
993 
994  mVHt0[0]=mV[ 0] ; mVHt1[0]=mV[ 1] ; mVHt2[0]=mV[ 3] ;
995  mVHt0[1]=mV[ 1] ; mVHt1[1]=mV[ 2] ; mVHt2[1]=mV[ 4] ;
996  mVHt0[2]=mV[ 3] ; mVHt1[2]=mV[ 4] ; mVHt2[2]=mV[ 5] ;
997  mVHt0[3]=mV[ 6] ; mVHt1[3]=mV[ 7] ; mVHt2[3]=mV[ 8] ;
998  mVHt0[4]=mV[10] ; mVHt1[4]=mV[11] ; mVHt2[4]=mV[12] ;
999  mVHt0[5]=mV[15] ; mVHt1[5]=mV[16] ; mVHt2[5]=mV[17] ;
1000  mVHt0[6]=mV[21] ; mVHt1[6]=mV[22] ; mVHt2[6]=mV[23] ;
1001 
1002  //* Kalman gain Km = mCH'*S
1003 
1004  float km0[7], km1[7], km2[7];
1005 
1006  for(Int_t i=0;i<7;++i){
1007  km0[i] = mVHt0[i]*mS[0] + mVHt1[i]*mS[1] + mVHt2[i]*mS[3];
1008  km1[i] = mVHt0[i]*mS[1] + mVHt1[i]*mS[2] + mVHt2[i]*mS[4];
1009  km2[i] = mVHt0[i]*mS[3] + mVHt1[i]*mS[4] + mVHt2[i]*mS[5];
1010  }
1011 
1012  for(Int_t i=0;i<7;++i)
1013  fP[i] = fP[i] + k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2];
1014 
1015  for(Int_t i=0;i<7;++i)
1016  m[i] = m[i] - km0[i]*zeta[0] - km1[i]*zeta[1] - km2[i]*zeta[2];
1017 
1018  for(Int_t i=0, k=0;i<7;++i){
1019  for(Int_t j=0;j<=i;++j,++k){
1020  fC[k] = fC[k] - (k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j] );
1021  }
1022  }
1023 
1024  for(Int_t i=0, k=0;i<7;++i){
1025  for(Int_t j=0;j<=i;++j,++k){
1026  mV[k] = mV[k] - (km0[i]*mVHt0[j] + km1[i]*mVHt1[j] + km2[i]*mVHt2[j] );
1027  }
1028  }
1029 
1030  float mDf[7][7];
1031 
1032  for(Int_t i=0;i<7;++i){
1033  for(Int_t j=0;j<7;++j){
1034  mDf[i][j] = (km0[i]*mCHt0[j] + km1[i]*mCHt1[j] + km2[i]*mCHt2[j] );
1035  }
1036  }
1037 
1038  float mJ1[7][7], mJ2[7][7];
1039  for(Int_t iPar1=0; iPar1<7; iPar1++)
1040  {
1041  for(Int_t iPar2=0; iPar2<7; iPar2++)
1042  {
1043  mJ1[iPar1][iPar2] = 0;
1044  mJ2[iPar1][iPar2] = 0;
1045  }
1046  }
1047 
1048  float mMassParticle = fP[6]*fP[6] - (fP[3]*fP[3] + fP[4]*fP[4] + fP[5]*fP[5]);
1049  float mMassDaughter = m[6]*m[6] - (m[3]*m[3] + m[4]*m[4] + m[5]*m[5]);
1050  if(mMassParticle > 0) mMassParticle = sqrt(mMassParticle);
1051  if(mMassDaughter > 0) mMassDaughter = sqrt(mMassDaughter);
1052 
1053  if( fMassHypo > -0.5)
1054  SetMassConstraint(fP,fC,mJ1,fMassHypo);
1055  else if((mMassParticle < SumDaughterMass) || (fP[6]<0) )
1056  SetMassConstraint(fP,fC,mJ1,SumDaughterMass);
1057 
1058  if(Daughter.fMassHypo > -0.5)
1059  SetMassConstraint(m,mV,mJ2,Daughter.fMassHypo);
1060  else if((mMassDaughter < Daughter.SumDaughterMass) || (m[6] < 0) )
1061  SetMassConstraint(m,mV,mJ2,Daughter.SumDaughterMass);
1062 
1063  float mDJ[7][7];
1064 
1065  for(Int_t i=0; i<7; i++) {
1066  for(Int_t j=0; j<7; j++) {
1067  mDJ[i][j] = 0;
1068  for(Int_t k=0; k<7; k++) {
1069  mDJ[i][j] += mDf[i][k]*mJ1[j][k];
1070  }
1071  }
1072  }
1073 
1074  for(Int_t i=0; i<7; ++i){
1075  for(Int_t j=0; j<7; ++j){
1076  mDf[i][j]=0;
1077  for(Int_t l=0; l<7; l++){
1078  mDf[i][j] += mJ2[i][l]*mDJ[l][j];
1079  }
1080  }
1081  }
1082 
1083  //* Add the daughter momentum to the particle momentum
1084 
1085  fP[ 3] += m[ 3];
1086  fP[ 4] += m[ 4];
1087  fP[ 5] += m[ 5];
1088  fP[ 6] += m[ 6];
1089 
1090  fC[ 9] += mV[ 9];
1091  fC[13] += mV[13];
1092  fC[14] += mV[14];
1093  fC[18] += mV[18];
1094  fC[19] += mV[19];
1095  fC[20] += mV[20];
1096  fC[24] += mV[24];
1097  fC[25] += mV[25];
1098  fC[26] += mV[26];
1099  fC[27] += mV[27];
1100 
1101  fC[6 ] += mDf[3][0]; fC[7 ] += mDf[3][1]; fC[8 ] += mDf[3][2];
1102  fC[10] += mDf[4][0]; fC[11] += mDf[4][1]; fC[12] += mDf[4][2];
1103  fC[15] += mDf[5][0]; fC[16] += mDf[5][1]; fC[17] += mDf[5][2];
1104  fC[21] += mDf[6][0]; fC[22] += mDf[6][1]; fC[23] += mDf[6][2];
1105 
1106  fC[9 ] += mDf[3][3] + mDf[3][3];
1107  fC[13] += mDf[4][3] + mDf[3][4]; fC[14] += mDf[4][4] + mDf[4][4];
1108  fC[18] += mDf[5][3] + mDf[3][5]; fC[19] += mDf[5][4] + mDf[4][5]; fC[20] += mDf[5][5] + mDf[5][5];
1109  fC[24] += mDf[6][3] + mDf[3][6]; fC[25] += mDf[6][4] + mDf[4][6]; fC[26] += mDf[6][5] + mDf[5][6]; fC[27] += mDf[6][6] + mDf[6][6];
1110 
1111 
1112  float K2[3][3];
1113  for(int i=0; i<3; i++)
1114  {
1115  for(int j=0; j<3; j++)
1116  K2[i][j] = -K[j][i];
1117  K2[i][i] += 1;
1118  }
1119 
1120  float A[3][3];
1121  for(int i=0; i<3; i++)
1122  for(int j=0; j<3; j++)
1123  {
1124  A[i][j] = 0;
1125  for(int k=0; k<3; k++)
1126  {
1127  A[i][j] += D[i][k] * K2[k][j];
1128  }
1129  }
1130 
1131  double M[3][3];
1132  for(int i=0; i<3; i++)
1133  for(int j=0; j<3; j++)
1134  {
1135  M[i][j] = 0;
1136  for(int k=0; k<3; k++)
1137  {
1138  M[i][j] += K[i][k] * A[k][j];
1139  }
1140  }
1141 
1142  fC[0] += 2*M[0][0];
1143  fC[1] += M[0][1] + M[1][0];
1144  fC[2] += 2*M[1][1];
1145  fC[3] += M[0][2] + M[2][0];
1146  fC[4] += M[1][2] + M[2][1];
1147  fC[5] += 2*M[2][2];
1148 
1149  //* Calculate Chi^2
1150 
1151  fNDF += 2;
1152  fQ += Daughter.GetQ();
1153  fSFromDecay = 0;
1154  fChi2 += (mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2])*zeta[0]
1155  + (mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2])*zeta[1]
1156  + (mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2])*zeta[2];
1157  }
1158 }
1159 
1160 void KFParticleBase::SetProductionVertex( const KFParticleBase &Vtx )
1161 {
1170  const float *m = Vtx.fP, *mV = Vtx.fC;
1171 
1172  float decayPoint[3] = {fP[0], fP[1], fP[2]};
1173  float decayPointCov[6] = { fC[0], fC[1], fC[2], fC[3], fC[4], fC[5] };
1174 
1175  float D[6][6];
1176  for(int iD1=0; iD1<6; iD1++)
1177  for(int iD2=0; iD2<6; iD2++)
1178  D[iD1][iD2] = 0.f;
1179 
1180  Bool_t noS = ( fC[35]<=0 ); // no decay length allowed
1181 
1182  if( noS ){
1183  TransportToDecayVertex();
1184  fP[7] = 0;
1185  fC[28] = fC[29] = fC[30] = fC[31] = fC[32] = fC[33] = fC[34] = fC[35] = 0;
1186  }
1187  else
1188  {
1189  float dsdr[6] = {0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
1190  float dS = GetDStoPoint(Vtx.fP, dsdr);
1191 
1192  float dsdp[6] = {-dsdr[0], -dsdr[1], -dsdr[2], 0, 0, 0};
1193 
1194  float F[36], F1[36];
1195  for(int i2=0; i2<36; i2++)
1196  {
1197  F[i2] = 0;
1198  F1[i2] = 0;
1199  }
1200  Transport( dS, dsdr, fP, fC, dsdp, F, F1 );
1201 
1202  float CTmp[36] = {0.};
1203  MultQSQt(F1, mV, CTmp, 6);
1204 
1205  for(int iC=0; iC<6; iC++)
1206  fC[iC] += CTmp[iC];
1207 
1208  for(int i=0; i<6; i++)
1209  for(int j=0; j<3; j++)
1210  {
1211  D[i][j] = 0;
1212  for(int k=0; k<3; k++)
1213  {
1214  D[i][j] += mV[IJ(j,k)] * F1[i*6+k];
1215  }
1216  }
1217  }
1218 
1219  float mS[6] = { fC[0] + mV[0],
1220  fC[1] + mV[1], fC[2] + mV[2],
1221  fC[3] + mV[3], fC[4] + mV[4], fC[5] + mV[5] };
1222  InvertCholetsky3(mS);
1223 
1224  float res[3] = { m[0] - X(), m[1] - Y(), m[2] - Z() };
1225 
1226  float K[3][6];
1227  for(int i=0; i<3; i++)
1228  for(int j=0; j<3; j++)
1229  {
1230  K[i][j] = 0;
1231  for(int k=0; k<3; k++)
1232  K[i][j] += fC[IJ(i,k)] * mS[IJ(k,j)];
1233  }
1234 
1235  float mCHt0[7], mCHt1[7], mCHt2[7];
1236  mCHt0[0]=fC[ 0]; mCHt1[0]=fC[ 1]; mCHt2[0]=fC[ 3];
1237  mCHt0[1]=fC[ 1]; mCHt1[1]=fC[ 2]; mCHt2[1]=fC[ 4];
1238  mCHt0[2]=fC[ 3]; mCHt1[2]=fC[ 4]; mCHt2[2]=fC[ 5];
1239  mCHt0[3]=fC[ 6]; mCHt1[3]=fC[ 7]; mCHt2[3]=fC[ 8];
1240  mCHt0[4]=fC[10]; mCHt1[4]=fC[11]; mCHt2[4]=fC[12];
1241  mCHt0[5]=fC[15]; mCHt1[5]=fC[16]; mCHt2[5]=fC[17];
1242  mCHt0[6]=fC[21]; mCHt1[6]=fC[22]; mCHt2[6]=fC[23];
1243 
1244  float k0[7], k1[7], k2[7];
1245  for(Int_t i=0;i<7;++i){
1246  k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
1247  k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
1248  k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
1249  }
1250 
1251  for(Int_t i=0;i<7;++i)
1252  fP[i] = fP[i] + k0[i]*res[0] + k1[i]*res[1] + k2[i]*res[2];
1253 
1254  for(Int_t i=0, k=0;i<7;++i){
1255  for(Int_t j=0;j<=i;++j,++k){
1256  fC[k] = fC[k] - (k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j] );
1257  }
1258  }
1259 
1260  float K2[3][3];
1261  for(int i=0; i<3; i++)
1262  {
1263  for(int j=0; j<3; j++)
1264  K2[i][j] = -K[j][i];
1265  K2[i][i] += 1;
1266  }
1267 
1268  float A[3][3];
1269  for(int i=0; i<3; i++)
1270  for(int j=0; j<3; j++)
1271  {
1272  A[i][j] = 0;
1273  for(int k=0; k<3; k++)
1274  {
1275  A[i][j] += D[k][i] * K2[k][j];
1276  }
1277  }
1278 
1279  double M[3][3];
1280  for(int i=0; i<3; i++)
1281  for(int j=0; j<3; j++)
1282  {
1283  M[i][j] = 0;
1284  for(int k=0; k<3; k++)
1285  {
1286  M[i][j] += K[i][k] * A[k][j];
1287  }
1288  }
1289 
1290  fC[0] += 2*M[0][0];
1291  fC[1] += M[0][1] + M[1][0];
1292  fC[2] += 2*M[1][1];
1293  fC[3] += M[0][2] + M[2][0];
1294  fC[4] += M[1][2] + M[2][1];
1295  fC[5] += 2*M[2][2];
1296 
1297  fChi2 += (mS[0]*res[0] + mS[1]*res[1] + mS[3]*res[2])*res[0]
1298  + (mS[1]*res[0] + mS[2]*res[1] + mS[4]*res[2])*res[1]
1299  + (mS[3]*res[0] + mS[4]*res[1] + mS[5]*res[2])*res[2];
1300  fNDF += 2;
1301 
1302  if( noS ){
1303  fP[7] = 0;
1304  fC[28] = fC[29] = fC[30] = fC[31] = fC[32] = fC[33] = fC[34] = fC[35] = 0;
1305  fSFromDecay = 0;
1306  }
1307  else
1308  {
1309  float dsdr[6] = {0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
1310  fP[7] = GetDStoPoint(decayPoint, dsdr);
1311 
1312  float dsdp[6] = {-dsdr[0], -dsdr[1], -dsdr[2], 0, 0, 0};
1313 
1314  float F[36], F1[36];
1315  for(int i2=0; i2<36; i2++)
1316  {
1317  F[i2] = 0;
1318  F1[i2] = 0;
1319  }
1320  float tmpP[8], tmpC[36];
1321  Transport( fP[7], dsdr, tmpP, tmpC, dsdp, F, F1 );
1322 
1323  fC[35] = 0;
1324  for(int iDsDr=0; iDsDr<6; iDsDr++)
1325  {
1326  float dsdrC = 0, dsdpV = 0;
1327 
1328  for(int k=0; k<6; k++)
1329  dsdrC += dsdr[k] * fC[IJ(k,iDsDr)]; // (-dsdr[k])*fC[k,j]
1330 
1331  fC[iDsDr+28] = dsdrC;
1332  fC[35] += dsdrC*dsdr[iDsDr] ;
1333  if(iDsDr < 3)
1334  {
1335  for(int k=0; k<3; k++)
1336  dsdpV -= dsdr[k] * decayPointCov[IJ(k,iDsDr)]; //
1337  fC[35] -= dsdpV*dsdr[iDsDr];
1338  }
1339  }
1340  fSFromDecay = -fP[7];
1341  }
1342 
1343  fAtProductionVertex = 1;
1344 }
1345 
1346 void KFParticleBase::SetMassConstraint( float *mP, float *mC, float mJ[7][7], float mass )
1347 {
1355  //* Set nonlinear mass constraint (Mass) on the state vector mP with a covariance matrix mC.
1356 
1357  const float energy2 = mP[6]*mP[6], p2 = mP[3]*mP[3]+mP[4]*mP[4]+mP[5]*mP[5], mass2 = mass*mass;
1358 
1359  const float a = energy2 - p2 + 2.*mass2;
1360  const float b = -2.*(energy2 + p2);
1361  const float c = energy2 - p2 - mass2;
1362 
1363  float lambda = 0;
1364  if(fabs(b) > 1.e-10) lambda = -c / b;
1365 
1366  float d = 4.*energy2*p2 - mass2*(energy2-p2-2.*mass2);
1367  if(d>=0 && fabs(a) > 1.e-10) lambda = (energy2 + p2 - sqrt(d))/a;
1368 
1369  if(mP[6] < 0) //If energy < 0 we need a lambda < 0
1370  lambda = -1000000.; //Empirical, a better solution should be found
1371 
1372  Int_t iIter=0;
1373  for(iIter=0; iIter<100; iIter++)
1374  {
1375  float lambda2 = lambda*lambda;
1376  float lambda4 = lambda2*lambda2;
1377 
1378  float lambda0 = lambda;
1379 
1380  float f = -mass2 * lambda4 + a*lambda2 + b*lambda + c;
1381  float df = -4.*mass2 * lambda2*lambda + 2.*a*lambda + b;
1382  if(fabs(df) < 1.e-10) break;
1383  lambda -= f/df;
1384  if(fabs(lambda0 - lambda) < 1.e-8) break;
1385  }
1386 
1387  const float lpi = 1./(1. + lambda);
1388  const float lmi = 1./(1. - lambda);
1389  const float lp2i = lpi*lpi;
1390  const float lm2i = lmi*lmi;
1391 
1392  float lambda2 = lambda*lambda;
1393 
1394  float dfl = -4.*mass2 * lambda2*lambda + 2.*a*lambda + b;
1395  float dfx[7] = {0};//,0,0,0};
1396  dfx[0] = -2.*(1. + lambda)*(1. + lambda)*mP[3];
1397  dfx[1] = -2.*(1. + lambda)*(1. + lambda)*mP[4];
1398  dfx[2] = -2.*(1. + lambda)*(1. + lambda)*mP[5];
1399  dfx[3] = 2.*(1. - lambda)*(1. - lambda)*mP[6];
1400  float dlx[4] = {1,1,1,1};
1401  if(fabs(dfl) > 1.e-10 )
1402  {
1403  for(int i=0; i<4; i++)
1404  dlx[i] = -dfx[i] / dfl;
1405  }
1406 
1407  float dxx[4] = {mP[3]*lm2i, mP[4]*lm2i, mP[5]*lm2i, -mP[6]*lp2i};
1408 
1409  for(Int_t i=0; i<7; i++)
1410  for(Int_t j=0; j<7; j++)
1411  mJ[i][j]=0;
1412  mJ[0][0] = 1.;
1413  mJ[1][1] = 1.;
1414  mJ[2][2] = 1.;
1415 
1416  for(Int_t i=3; i<7; i++)
1417  for(Int_t j=3; j<7; j++)
1418  mJ[i][j] = dlx[j-3]*dxx[i-3];
1419 
1420  for(Int_t i=3; i<6; i++)
1421  mJ[i][i] += lmi;
1422  mJ[6][6] += lpi;
1423 
1424  float mCJ[7][7];
1425 
1426  for(Int_t i=0; i<7; i++) {
1427  for(Int_t j=0; j<7; j++) {
1428  mCJ[i][j] = 0;
1429  for(Int_t k=0; k<7; k++) {
1430  mCJ[i][j] += mC[IJ(i,k)]*mJ[j][k];
1431  }
1432  }
1433  }
1434 
1435  for(Int_t i=0; i<7; ++i){
1436  for(Int_t j=0; j<=i; ++j){
1437  mC[IJ(i,j)]=0;
1438  for(Int_t l=0; l<7; l++){
1439  mC[IJ(i,j)] += mJ[i][l]*mCJ[l][j];
1440  }
1441  }
1442  }
1443 
1444  mP[3] *= lmi;
1445  mP[4] *= lmi;
1446  mP[5] *= lmi;
1447  mP[6] *= lpi;
1448 }
1449 
1450 void KFParticleBase::SetNonlinearMassConstraint( float mass )
1451 {
1456  const float& px = fP[3];
1457  const float& py = fP[4];
1458  const float& pz = fP[5];
1459  const float& energy = fP[6];
1460 
1461  const float residual = (energy*energy - px*px - py*py - pz*pz) - mass*mass;
1462  const float dm2 = float(4.f) * ( px*px*fC[9] + py*py*fC[14] + pz*pz*fC[20] + energy*energy*fC[27] +
1463  float(2.f) * ( px*py*fC[13] + pz*(px*fC[18]+py*fC[19]) - energy*(px*fC[24]+py*fC[25]+pz*fC[26]) ) );
1464  const float dChi2 = residual*residual / dm2;
1465  fChi2 += dChi2;
1466  fNDF += 1;
1467 
1468  float mJ[7][7];
1469  SetMassConstraint( fP, fC, mJ, mass );
1470  fMassHypo = mass;
1471  SumDaughterMass = mass;
1472 }
1473 
1474 void KFParticleBase::SetMassConstraint( float Mass, float SigmaMass )
1475 {
1482  fMassHypo = Mass;
1483  SumDaughterMass = Mass;
1484 
1485  float m2 = Mass*Mass; // measurement, weighted by Mass
1486  float s2 = m2*SigmaMass*SigmaMass; // sigma^2
1487 
1488  float p2 = fP[3]*fP[3] + fP[4]*fP[4] + fP[5]*fP[5];
1489  float e0 = sqrt(m2+p2);
1490 
1491  float mH[8];
1492  mH[0] = mH[1] = mH[2] = 0.;
1493  mH[3] = -2*fP[3];
1494  mH[4] = -2*fP[4];
1495  mH[5] = -2*fP[5];
1496  mH[6] = 2*fP[6];//e0;
1497  mH[7] = 0;
1498 
1499  float zeta = e0*e0 - e0*fP[6];
1500  zeta = m2 - (fP[6]*fP[6]-p2);
1501 
1502  float mCHt[8], s2_est=0;
1503  for( Int_t i=0; i<8; ++i ){
1504  mCHt[i] = 0.0;
1505  for (Int_t j=0;j<8;++j) mCHt[i] += Cij(i,j)*mH[j];
1506  s2_est += mH[i]*mCHt[i];
1507  }
1508 
1509  if( s2_est<1.e-20 ) return; // calculated mass error is already 0,
1510  // the particle can not be constrained on mass
1511 
1512  float w2 = 1./( s2 + s2_est );
1513  fChi2 += zeta*zeta*w2;
1514  fNDF += 1;
1515  for( Int_t i=0, ii=0; i<8; ++i ){
1516  float ki = mCHt[i]*w2;
1517  fP[i]+= ki*zeta;
1518  for(Int_t j=0;j<=i;++j) fC[ii++] -= ki*mCHt[j];
1519  }
1520 }
1521 
1522 
1523 void KFParticleBase::SetNoDecayLength()
1524 {
1529  TransportToDecayVertex();
1530 
1531  float h[8];
1532  h[0] = h[1] = h[2] = h[3] = h[4] = h[5] = h[6] = 0;
1533  h[7] = 1;
1534 
1535  float zeta = 0 - fP[7];
1536  for(Int_t i=0;i<8;++i) zeta -= h[i]*(fP[i]-fP[i]);
1537 
1538  float s = fC[35];
1539  if( s>1.e-20 ){
1540  s = 1./s;
1541  fChi2 += zeta*zeta*s;
1542  fNDF += 1;
1543  for( Int_t i=0, ii=0; i<7; ++i ){
1544  float ki = fC[28+i]*s;
1545  fP[i]+= ki*zeta;
1546  for(Int_t j=0;j<=i;++j) fC[ii++] -= ki*fC[28+j];
1547  }
1548  }
1549  fP[7] = 0;
1550  fC[28] = fC[29] = fC[30] = fC[31] = fC[32] = fC[33] = fC[34] = fC[35] = 0;
1551 }
1552 
1553 
1554 void KFParticleBase::Construct( const KFParticleBase* vDaughters[], Int_t nDaughters,
1555  const KFParticleBase *Parent, float Mass )
1556 {
1568  const int maxIter = 1;
1569  for( Int_t iter=0; iter<maxIter; iter++ ){
1570  fAtProductionVertex = 0;
1571  fSFromDecay = 0;
1572  SumDaughterMass = 0;
1573 
1574  for(Int_t i=0;i<36;++i) fC[i]=0.;
1575  fC[35] = 1.;
1576 
1577  fNDF = -3;
1578  fChi2 = 0.;
1579  fQ = 0;
1580 
1581  for( Int_t itr =0; itr<nDaughters; itr++ ){
1582  AddDaughter( *vDaughters[itr] );
1583  }
1584  }
1585 
1586  if( Mass>=0 ) SetMassConstraint( Mass );
1587  if( Parent ) SetProductionVertex( *Parent );
1588 }
1589 
1590 void KFParticleBase::TransportToDecayVertex()
1591 {
1593  float dsdr[6] = {0.f};
1594  if( fSFromDecay != 0 ) TransportToDS( -fSFromDecay, dsdr );
1595  fAtProductionVertex = 0;
1596 }
1597 
1598 void KFParticleBase::TransportToProductionVertex()
1599 {
1601  float dsdr[6] = {0.f};
1602  if( fSFromDecay != -fP[7] ) TransportToDS( -fSFromDecay-fP[7], dsdr );
1603  fAtProductionVertex = 1;
1604 }
1605 
1606 
1607 void KFParticleBase::TransportToDS( float dS, const float* dsdr )
1608 {
1616  Transport( dS, dsdr, fP, fC );
1617  fSFromDecay+= dS;
1618 }
1619 
1620 
1621 float KFParticleBase::GetDStoPointLine( const float xyz[3], float dsdr[6] ) const
1622 {
1632  float p2 = fP[3]*fP[3] + fP[4]*fP[4] + fP[5]*fP[5];
1633  if( p2<1.e-4 ) p2 = 1;
1634 
1635  const float& a = fP[3]*(xyz[0]-fP[0]) + fP[4]*(xyz[1]-fP[1]) + fP[5]*(xyz[2]-fP[2]);
1636  dsdr[0] = -fP[3]/p2;
1637  dsdr[1] = -fP[4]/p2;
1638  dsdr[2] = -fP[5]/p2;
1639  dsdr[3] = ((xyz[0]-fP[0])*p2 - 2.f* fP[3]*a)/(p2*p2);
1640  dsdr[4] = ((xyz[1]-fP[1])*p2 - 2.f* fP[4]*a)/(p2*p2);
1641  dsdr[5] = ((xyz[2]-fP[2])*p2 - 2.f* fP[5]*a)/(p2*p2);
1642 
1643  return a/p2;
1644 }
1645 
1646 
1647 float KFParticleBase::GetDStoPointBz( float B, const float xyz[3], float dsdr[6], const float* param) const
1648 {
1661  if(!param)
1662  param = fP;
1663 
1664  const float& x = param[0];
1665  const float& y = param[1];
1666  const float& z = param[2];
1667  const float& px = param[3];
1668  const float& py = param[4];
1669  const float& pz = param[5];
1670 
1671  const float kCLight = 0.000299792458f;
1672  float bq = B*fQ*kCLight;
1673  float pt2 = px*px + py*py;
1674  float p2 = pt2 + pz*pz;
1675 
1676  float dx = xyz[0] - x;
1677  float dy = xyz[1] - y;
1678  float dz = xyz[2] - z;
1679  float a = dx*px+dy*py;
1680  float dS(0.f);
1681 
1682  float abq = bq*a;
1683 
1684  const float LocalSmall = 1.e-8f;
1685  bool mask = ( fabs(bq)<LocalSmall );
1686  if(mask && p2>1.e-4f)
1687  {
1688  dS = (a + dz*pz)/p2;
1689 
1690  dsdr[0] = -px/p2;
1691  dsdr[1] = -py/p2;
1692  dsdr[2] = -pz/p2;
1693  dsdr[3] = (dx*p2 - 2.f* px *(a + dz *pz))/(p2*p2);
1694  dsdr[4] = (dy*p2 - 2.f* py *(a + dz *pz))/(p2*p2);
1695  dsdr[5] = (dz*p2 - 2.f* pz *(a + dz *pz))/(p2*p2);
1696  }
1697  if(mask)
1698  {
1699  return dS;
1700  }
1701 
1702  dS = atan2( abq, pt2 + bq*(dy*px -dx*py) )/bq;
1703 
1704  float bs= bq*dS;
1705 
1706  float s = sin(bs), c = cos(bs);
1707 
1708  if(fabs(bq) < LocalSmall)
1709  bq = LocalSmall;
1710  float bbq = bq*(dx*py - dy*px) - pt2;
1711 
1712  dsdr[0] = (px*bbq - py*abq)/(abq*abq + bbq*bbq);
1713  dsdr[1] = (px*abq + py*bbq)/(abq*abq + bbq*bbq);
1714  dsdr[2] = 0;
1715  dsdr[3] = -(dx*bbq + dy*abq + 2.f*px*a)/(abq*abq + bbq*bbq);
1716  dsdr[4] = (dx*abq - dy*bbq - 2.f*py*a)/(abq*abq + bbq*bbq);
1717  dsdr[5] = 0;
1718 
1719  float sz(0.f);
1720  float cCoeff = (bbq*c - abq*s) - pz*pz ;
1721  if(fabs(cCoeff) > 1.e-8f)
1722  sz = (dS*pz - dz)*pz / cCoeff;
1723 
1724  float dcdr[6] = {0.f};
1725  dcdr[0] = -bq*py*c - bbq*s*bq*dsdr[0] + px*bq*s - abq*c*bq*dsdr[0];
1726  dcdr[1] = bq*px*c - bbq*s*bq*dsdr[1] + py*bq*s - abq*c*bq*dsdr[1];
1727  dcdr[3] = (-bq*dy-2*px)*c - bbq*s*bq*dsdr[3] - dx*bq*s - abq*c*bq*dsdr[3];
1728  dcdr[4] = ( bq*dx-2*py)*c - bbq*s*bq*dsdr[4] - dy*bq*s - abq*c*bq*dsdr[4];
1729  dcdr[5] = -2*pz;
1730 
1731  for(int iP=0; iP<6; iP++)
1732  dsdr[iP] += pz*pz/cCoeff*dsdr[iP] - sz/cCoeff*dcdr[iP];
1733  dsdr[2] += pz/cCoeff;
1734  dsdr[5] += (2.f*pz*dS - dz)/cCoeff;
1735 
1736  dS += sz;
1737 
1738  bs= bq*dS;
1739  s = sin(bs), c = cos(bs);
1740 
1741  float sB, cB;
1742  const float kOvSqr6 = 1.f/sqrt(float(6.f));
1743 
1744  if(LocalSmall < fabs(bs))
1745  {
1746  sB = s/bq;
1747  cB = (1.f-c)/bq;
1748  }
1749  else
1750  {
1751  sB = (1.f-bs*kOvSqr6)*(1.f+bs*kOvSqr6)*dS;
1752  cB = .5f*sB*bs;
1753  }
1754 
1755  float p[5];
1756  p[0] = x + sB*px + cB*py;
1757  p[1] = y - cB*px + sB*py;
1758  p[2] = z + dS*pz;
1759  p[3] = c*px + s*py;
1760  p[4] = -s*px + c*py;
1761 
1762  dx = xyz[0] - p[0];
1763  dy = xyz[1] - p[1];
1764  dz = xyz[2] - p[2];
1765  a = dx*p[3]+dy*p[4] + dz*pz;
1766  abq = bq*a;
1767 
1768  dS += atan2( abq, p2 + bq*(dy*p[3] -dx*p[4]) )/bq;
1769 
1770  return dS;
1771 }
1772 
1773 float KFParticleBase::GetDStoPointBy( float By, const float xyz[3], float dsdr[6] ) const
1774 {
1788  const float param[6] = { fP[0], -fP[2], fP[1], fP[3], -fP[5], fP[4] };
1789  const float point[3] = { xyz[0], -xyz[2], xyz[1] };
1790 
1791  float dsdrBz[6] = {0.f};
1792 
1793  const float dS = GetDStoPointBz(By, point, dsdrBz, param);
1794  dsdr[0] = dsdrBz[0];
1795  dsdr[1] = dsdrBz[2];
1796  dsdr[2] = -dsdrBz[1];
1797  dsdr[3] = dsdrBz[3];
1798  dsdr[4] = dsdrBz[5];
1799  dsdr[5] = -dsdrBz[4];
1800 
1801  return dS;
1802 }
1803 
1804 float KFParticleBase::GetDStoPointB( const float* B, const float xyz[3], float dsdr[6] ) const
1805 {
1819  const float& Bx = B[0];
1820  const float& By = B[1];
1821  const float& Bz = B[2];
1822 
1823  const float& Bxz = sqrt(Bx*Bx + Bz*Bz);
1824  const float& Br = sqrt(Bx*Bx + By*By + Bz*Bz);
1825 
1826  float cosA = 1;
1827  float sinA = 0;
1828  if(fabs(Bxz) > 1.e-8f)
1829  {
1830  cosA = Bz/Bxz;
1831  sinA = Bx/Bxz;
1832  }
1833 
1834  const float& sinP = By/Br;
1835  const float& cosP = Bxz/Br;
1836 
1837  const float param[6] = { cosA*fP[0] - sinA*fP[2],
1838  -sinA*sinP*fP[0] + cosP*fP[1] - cosA*sinP*fP[2],
1839  cosP*sinA*fP[0] + sinP*fP[1] + cosA*cosP*fP[2],
1840  cosA*fP[3] - sinA*fP[5],
1841  -sinA*sinP*fP[3] + cosP*fP[4] - cosA*sinP*fP[5],
1842  cosP*sinA*fP[3] + sinP*fP[4] + cosA*cosP*fP[5]};
1843  const float point[3] = { cosA*xyz[0] - sinA*xyz[2],
1844  -sinA*sinP*xyz[0] + cosP*xyz[1] - cosA*sinP*xyz[2],
1845  cosP*sinA*xyz[0] + sinP*xyz[1] + cosA*cosP*xyz[2] };
1846 
1847  float dsdrBz[6] = {0.f};
1848 
1849  const float dS = GetDStoPointBz(Br, point, dsdrBz, param);
1850  dsdr[0] = dsdrBz[0]*cosA - dsdrBz[1]*sinA*sinP + dsdrBz[2]*sinA*cosP;
1851  dsdr[1] = dsdrBz[1]*cosP + dsdrBz[2]*sinP;
1852  dsdr[2] = -dsdrBz[0]*sinA - dsdrBz[1]*cosA*sinP + dsdrBz[2]*cosA*cosP;
1853  dsdr[3] = dsdrBz[3]*cosA - dsdrBz[4]*sinA*sinP + dsdrBz[5]*sinA*cosP;
1854  dsdr[4] = dsdrBz[4]*cosP + dsdrBz[5]*sinP;
1855  dsdr[5] = -dsdrBz[3]*sinA - dsdrBz[4]*cosA*sinP + dsdrBz[5]*cosA*cosP;
1856 
1857  return dS;
1858 }
1859 
1860 float KFParticleBase::GetDStoPointCBM( const float xyz[3], float dsdr[6] ) const
1861 {
1873  float dS = 0;
1874  float fld[3];
1875  GetFieldValue( fP, fld );
1876  dS = GetDStoPointBy( fld[1], xyz, dsdr );
1877 
1878  return dS;
1879 }
1880 
1881 void KFParticleBase::GetDStoParticleBz( float Bz, const KFParticleBase &p, float dS[2], float dsdr[4][6], const float* param1, const float* param2 ) const
1882 {
1906  if(!param1)
1907  {
1908  param1 = fP;
1909  param2 = p.fP;
1910  }
1911 
1912  //* Get dS to another particle for Bz field
1913  const float kOvSqr6 = 1.f/sqrt(float(6.f));
1914  const float kCLight = 0.000299792458f;
1915 
1916  //in XY plane
1917  //first root
1918  const float& bq1 = Bz*fQ*kCLight;
1919  const float& bq2 = Bz*p.fQ*kCLight;
1920 
1921  const bool& isStraight1 = fabs(bq1) < 1.e-8f;
1922  const bool& isStraight2 = fabs(bq2) < 1.e-8f;
1923 
1924  if( isStraight1 && isStraight2 )
1925  {
1926  GetDStoParticleLine(p, dS, dsdr);
1927  return;
1928  }
1929 
1930  const float& px1 = param1[3];
1931  const float& py1 = param1[4];
1932  const float& pz1 = param1[5];
1933 
1934  const float& px2 = param2[3];
1935  const float& py2 = param2[4];
1936  const float& pz2 = param2[5];
1937 
1938  const float& pt12 = px1*px1 + py1*py1;
1939  const float& pt22 = px2*px2 + py2*py2;
1940 
1941  const float& x01 = param1[0];
1942  const float& y01 = param1[1];
1943  const float& z01 = param1[2];
1944 
1945  const float& x02 = param2[0];
1946  const float& y02 = param2[1];
1947  const float& z02 = param2[2];
1948 
1949  float dS1[2] = {0.f}, dS2[2]={0.f};
1950 
1951  const float& dx0 = (x01 - x02);
1952  const float& dy0 = (y01 - y02);
1953  const float& dr02 = dx0*dx0 + dy0*dy0;
1954  const float& drp1 = dx0*px1 + dy0*py1;
1955  const float& dxyp1 = dx0*py1 - dy0*px1;
1956  const float& drp2 = dx0*px2 + dy0*py2;
1957  const float& dxyp2 = dx0*py2 - dy0*px2;
1958  const float& p1p2 = px1*px2 + py1*py2;
1959  const float& dp1p2 = px1*py2 - px2*py1;
1960 
1961  const float& k11 = (bq2*drp1 - dp1p2);
1962  const float& k21 = (bq1*(bq2*dxyp1 - p1p2) + bq2*pt12);
1963  const float& k12 = ((bq1*drp2 - dp1p2));
1964  const float& k22 = (bq2*(bq1*dxyp2 + p1p2) - bq1*pt22);
1965 
1966  const float& kp = (dxyp1*bq2 - dxyp2*bq1 - p1p2);
1967  const float& kd = dr02/2.f*bq1*bq2 + kp;
1968  const float& c1 = -(bq1*kd + pt12*bq2);
1969  const float& c2 = bq2*kd + pt22*bq1;
1970 
1971  float d1 = pt12*pt22 - kd*kd;
1972  if(d1<0.f)
1973  d1 = float(0.f);
1974  d1 = sqrt( d1 );
1975  float d2 = pt12*pt22 - kd*kd;
1976  if(d2<0.f)
1977  d2 = float(0.f);
1978  d2 = sqrt( d2 );
1979 
1980  // find two points of closest approach in XY plane
1981 
1982  float dS1dR1[2][6];
1983  float dS2dR2[2][6];
1984 
1985  float dS1dR2[2][6];
1986  float dS2dR1[2][6];
1987 
1988  float dk11dr1[6] = {bq2*px1, bq2*py1, 0, bq2*dx0 - py2, bq2*dy0 + px2, 0};
1989  float dk11dr2[6] = {-bq2*px1, -bq2*py1, 0, py1, -px1, 0};
1990  float dk12dr1[6] = {bq1*px2, bq1*py2, 0, -py2, px2, 0};
1991  float dk12dr2[6] = {-bq1*px2, -bq1*py2, 0, bq1*dx0 + py1, bq1*dy0 - px1, 0};
1992  float dk21dr1[6] = {bq1*bq2*py1, -bq1*bq2*px1, 0, 2*bq2*px1 + bq1*(-(bq2*dy0) - px2), 2*bq2*py1 + bq1*(bq2*dx0 - py2), 0};
1993  float dk21dr2[6] = {-(bq1*bq2*py1), bq1*bq2*px1, 0, -(bq1*px1), -(bq1*py1), 0};
1994  float dk22dr1[6] = {bq1*bq2*py2, -(bq1*bq2*px2), 0, bq2*px2, bq2*py2, 0};
1995  float dk22dr2[6] = {-(bq1*bq2*py2), bq1*bq2*px2, 0, bq2*(-(bq1*dy0) + px1) - 2*bq1*px2, bq2*(bq1*dx0 + py1) - 2*bq1*py2, 0};
1996 
1997  float dkddr1[6] = {bq1*bq2*dx0 + bq2*py1 - bq1*py2, bq1*bq2*dy0 - bq2*px1 + bq1*px2, 0, -bq2*dy0 - px2, bq2*dx0 - py2, 0};
1998  float dkddr2[6] = {-bq1*bq2*dx0 - bq2*py1 + bq1*py2, -bq1*bq2*dy0 + bq2*px1 - bq1*px2, 0, bq1*dy0 - px1, -bq1*dx0 - py1, 0};
1999 
2000  float dc1dr1[6] = {-(bq1*(bq1*bq2*dx0 + bq2*py1 - bq1*py2)), -(bq1*(bq1*bq2*dy0 - bq2*px1 + bq1*px2)), 0, -2*bq2*px1 - bq1*(-(bq2*dy0) - px2), -2*bq2*py1 - bq1*(bq2*dx0 - py2), 0};
2001  float dc1dr2[6] = {-(bq1*(-(bq1*bq2*dx0) - bq2*py1 + bq1*py2)), -(bq1*(-(bq1*bq2*dy0) + bq2*px1 - bq1*px2)), 0, -(bq1*(bq1*dy0 - px1)), -(bq1*(-(bq1*dx0) - py1)), 0};
2002 
2003  float dc2dr1[6] = {bq2*(bq1*bq2*dx0 + bq2*py1 - bq1*py2), bq2*(bq1*bq2*dy0 - bq2*px1 + bq1*px2), 0, bq2*(-(bq2*dy0) - px2), bq2*(bq2*dx0 - py2), 0};
2004  float dc2dr2[6] = {bq2*(-(bq1*bq2*dx0) - bq2*py1 + bq1*py2), bq2*(-(bq1*bq2*dy0) + bq2*px1 - bq1*px2), 0, bq2*(bq1*dy0 - px1) + 2*bq1*px2, bq2*(-(bq1*dx0) - py1) + 2*bq1*py2, 0};
2005 
2006  float dd1dr1[6] = {0,0,0,0,0,0};
2007  float dd1dr2[6] = {0,0,0,0,0,0};
2008  if(d1>0)
2009  {
2010  for(int i=0; i<6; i++)
2011  {
2012  dd1dr1[i] = -kd/d1*dkddr1[i];
2013  dd1dr2[i] = -kd/d1*dkddr2[i];
2014  }
2015  dd1dr1[3] += px1/d1*pt22; dd1dr1[4] += py1/d1*pt22;
2016  dd1dr2[3] += px2/d1*pt12; dd1dr2[4] += py2/d1*pt12;
2017  }
2018 
2019  if(!isStraight1)
2020  {
2021  dS1[0] = atan2( bq1*(k11*c1 + k21*d1), (bq1*k11*d1*bq1 - k21*c1) )/bq1;
2022  dS1[1] = atan2( bq1*(k11*c1 - k21*d1), (-bq1*k11*d1*bq1 - k21*c1) )/bq1;
2023 
2024  float a = bq1*(k11*c1 + k21*d1);
2025  float b = bq1*k11*d1*bq1 - k21*c1;
2026  for(int iP=0; iP<6; iP++)
2027  {
2028  if(( b*b + a*a ) > 0)
2029  {
2030  const float dadr1 = bq1*( dk11dr1[iP]*c1 + k11*dc1dr1[iP] + dk21dr1[iP]*d1 + k21*dd1dr1[iP] );
2031  const float dadr2 = bq1*( dk11dr2[iP]*c1 + k11*dc1dr2[iP] + dk21dr2[iP]*d1 + k21*dd1dr2[iP] );
2032  const float dbdr1 = bq1*bq1*( dk11dr1[iP]*d1 + k11*dd1dr1[iP] ) - ( dk21dr1[iP]*c1 + k21*dc1dr1[iP] );
2033  const float dbdr2 = bq1*bq1*( dk11dr2[iP]*d1 + k11*dd1dr2[iP] ) - ( dk21dr2[iP]*c1 + k21*dc1dr2[iP] );
2034 
2035  dS1dR1[0][iP] = 1/bq1 * 1/( b*b + a*a ) * ( dadr1*b - dbdr1*a );
2036  dS1dR2[0][iP] = 1/bq1 * 1/( b*b + a*a ) * ( dadr2*b - dbdr2*a );
2037  }
2038  else
2039  {
2040  dS1dR1[0][iP] = 0;
2041  dS1dR2[0][iP] = 0;
2042  }
2043  }
2044 
2045  a = bq1*(k11*c1 - k21*d1);
2046  b = -bq1*k11*d1*bq1 - k21*c1;
2047  for(int iP=0; iP<6; iP++)
2048  {
2049  if(( b*b + a*a ) > 0)
2050  {
2051  const float dadr1 = bq1*( dk11dr1[iP]*c1 + k11*dc1dr1[iP] - (dk21dr1[iP]*d1 + k21*dd1dr1[iP]) );
2052  const float dadr2 = bq1*( dk11dr2[iP]*c1 + k11*dc1dr2[iP] - (dk21dr2[iP]*d1 + k21*dd1dr2[iP]) );
2053  const float dbdr1 = -bq1*bq1*( dk11dr1[iP]*d1 + k11*dd1dr1[iP] ) - ( dk21dr1[iP]*c1 + k21*dc1dr1[iP] );
2054  const float dbdr2 = -bq1*bq1*( dk11dr2[iP]*d1 + k11*dd1dr2[iP] ) - ( dk21dr2[iP]*c1 + k21*dc1dr2[iP] );
2055 
2056  dS1dR1[1][iP] = 1/bq1 * 1/( b*b + a*a ) * ( dadr1*b - dbdr1*a );
2057  dS1dR2[1][iP] = 1/bq1 * 1/( b*b + a*a ) * ( dadr2*b - dbdr2*a );
2058  }
2059  else
2060  {
2061  dS1dR1[1][iP] = 0;
2062  dS1dR2[1][iP] = 0;
2063  }
2064  }
2065  }
2066  if(!isStraight2)
2067  {
2068  dS2[0] = atan2( (bq2*k12*c2 + k22*d2*bq2), (bq2*k12*d2*bq2 - k22*c2) )/bq2;
2069  dS2[1] = atan2( (bq2*k12*c2 - k22*d2*bq2), (-bq2*k12*d2*bq2 - k22*c2) )/bq2;
2070 
2071  float a = bq2*(k12*c2 + k22*d2);
2072  float b = bq2*k12*d2*bq2 - k22*c2;
2073  for(int iP=0; iP<6; iP++)
2074  {
2075  if(( b*b + a*a ) > 0)
2076  {
2077  const float dadr1 = bq2*( dk12dr1[iP]*c2 + k12*dc2dr1[iP] + dk22dr1[iP]*d1 + k22*dd1dr1[iP] );
2078  const float dadr2 = bq2*( dk12dr2[iP]*c2 + k12*dc2dr2[iP] + dk22dr2[iP]*d1 + k22*dd1dr2[iP] );
2079  const float dbdr1 = bq2*bq2*( dk12dr1[iP]*d1 + k12*dd1dr1[iP] ) - (dk22dr1[iP]*c2 + k22*dc2dr1[iP]);
2080  const float dbdr2 = bq2*bq2*( dk12dr2[iP]*d1 + k12*dd1dr2[iP] ) - (dk22dr2[iP]*c2 + k22*dc2dr2[iP]);
2081 
2082  dS2dR1[0][iP] = 1/bq2 * 1/( b*b + a*a ) * ( dadr1*b - dbdr1*a );
2083  dS2dR2[0][iP] = 1/bq2 * 1/( b*b + a*a ) * ( dadr2*b - dbdr2*a );
2084  }
2085  else
2086  {
2087  dS2dR1[0][iP] = 0;
2088  dS2dR2[0][iP] = 0;
2089  }
2090  }
2091 
2092  a = bq2*(k12*c2 - k22*d2);
2093  b = -bq2*k12*d2*bq2 - k22*c2;
2094  for(int iP=0; iP<6; iP++)
2095  {
2096  if(( b*b + a*a ) > 0)
2097  {
2098  const float dadr1 = bq2*( dk12dr1[iP]*c2 + k12*dc2dr1[iP] - (dk22dr1[iP]*d1 + k22*dd1dr1[iP]) );
2099  const float dadr2 = bq2*( dk12dr2[iP]*c2 + k12*dc2dr2[iP] - (dk22dr2[iP]*d1 + k22*dd1dr2[iP]) );
2100  const float dbdr1 = -bq2*bq2*( dk12dr1[iP]*d1 + k12*dd1dr1[iP] ) - (dk22dr1[iP]*c2 + k22*dc2dr1[iP]);
2101  const float dbdr2 = -bq2*bq2*( dk12dr2[iP]*d1 + k12*dd1dr2[iP] ) - (dk22dr2[iP]*c2 + k22*dc2dr2[iP]);
2102 
2103  dS2dR1[1][iP] = 1/bq2 * 1/( b*b + a*a ) * ( dadr1*b - dbdr1*a );
2104  dS2dR2[1][iP] = 1/bq2 * 1/( b*b + a*a ) * ( dadr2*b - dbdr2*a );
2105  }
2106  else
2107  {
2108  dS2dR1[1][iP] = 0;
2109  dS2dR2[1][iP] = 0;
2110  }
2111  }
2112  }
2113  if(isStraight1 && (pt12>0.f) )
2114  {
2115  dS1[0] = (k11*c1 + k21*d1)/(- k21*c1);
2116  dS1[1] = (k11*c1 - k21*d1)/(- k21*c1);
2117 
2118  float a = k11*c1 + k21*d1;
2119  float b = -k21*c1;
2120 
2121  for(int iP=0; iP<6; iP++)
2122  {
2123  if(b*b > 0)
2124  {
2125  const float dadr1 = ( dk11dr1[iP]*c1 + k11*dc1dr1[iP] + dk21dr1[iP]*d1 + k21*dd1dr1[iP] );
2126  const float dadr2 = ( dk11dr2[iP]*c1 + k11*dc1dr2[iP] + dk21dr2[iP]*d1 + k21*dd1dr2[iP] );
2127  const float dbdr1 = -( dk21dr1[iP]*c1 + k21*dc1dr1[iP] );
2128  const float dbdr2 = -( dk21dr2[iP]*c1 + k21*dc1dr2[iP] );
2129 
2130  dS1dR1[0][iP] = dadr1/b - dbdr1*a/(b*b) ;
2131  dS1dR2[0][iP] = dadr2/b - dbdr2*a/(b*b) ;
2132  }
2133  else
2134  {
2135  dS1dR1[0][iP] = 0;
2136  dS1dR2[0][iP] = 0;
2137  }
2138  }
2139 
2140  a = k11*c1 - k21*d1;
2141  for(int iP=0; iP<6; iP++)
2142  {
2143  if(b*b > 0)
2144  {
2145  const float dadr1 = ( dk11dr1[iP]*c1 + k11*dc1dr1[iP] - dk21dr1[iP]*d1 - k21*dd1dr1[iP] );
2146  const float dadr2 = ( dk11dr2[iP]*c1 + k11*dc1dr2[iP] - dk21dr2[iP]*d1 - k21*dd1dr2[iP] );
2147  const float dbdr1 = -( dk21dr1[iP]*c1 + k21*dc1dr1[iP] );
2148  const float dbdr2 = -( dk21dr2[iP]*c1 + k21*dc1dr2[iP] );
2149 
2150  dS1dR1[1][iP] = dadr1/b - dbdr1*a/(b*b) ;
2151  dS1dR2[1][iP] = dadr2/b - dbdr2*a/(b*b) ;
2152  }
2153  else
2154  {
2155  dS1dR1[1][iP] = 0;
2156  dS1dR2[1][iP] = 0;
2157  }
2158  }
2159  }
2160  if(isStraight2 && (pt22>0.f) )
2161  {
2162  dS2[0] = (k12*c2 + k22*d2)/(- k22*c2);
2163  dS2[1] = (k12*c2 - k22*d2)/(- k22*c2);
2164 
2165  float a = k12*c2 + k22*d1;
2166  float b = -k22*c2;
2167 
2168  for(int iP=0; iP<6; iP++)
2169  {
2170  if(b*b > 0)
2171  {
2172  const float dadr1 = ( dk12dr1[iP]*c2 + k12*dc2dr1[iP] + dk22dr1[iP]*d1 + k22*dd1dr1[iP] );
2173  const float dadr2 = ( dk12dr2[iP]*c2 + k12*dc2dr2[iP] + dk22dr2[iP]*d1 + k22*dd1dr2[iP] );
2174  const float dbdr1 = -( dk22dr1[iP]*c2 + k22*dc2dr1[iP] );
2175  const float dbdr2 = -( dk22dr2[iP]*c2 + k22*dc2dr2[iP] );
2176 
2177  dS2dR1[0][iP] = dadr1/b - dbdr1*a/(b*b) ;
2178  dS2dR2[0][iP] = dadr2/b - dbdr2*a/(b*b) ;
2179  }
2180  else
2181  {
2182  dS2dR1[0][iP] = 0;
2183  dS2dR2[0][iP] = 0;
2184  }
2185  }
2186 
2187  a = k12*c2 - k22*d1;
2188  for(int iP=0; iP<6; iP++)
2189  {
2190  if(b*b > 0)
2191  {
2192  const float dadr1 = ( dk12dr1[iP]*c2 + k12*dc2dr1[iP] - dk22dr1[iP]*d1 - k22*dd1dr1[iP] );
2193  const float dadr2 = ( dk12dr2[iP]*c2 + k12*dc2dr2[iP] - dk22dr2[iP]*d1 - k22*dd1dr2[iP] );
2194  const float dbdr1 = -( dk22dr1[iP]*c2 + k22*dc2dr1[iP] );
2195  const float dbdr2 = -( dk22dr2[iP]*c2 + k22*dc2dr2[iP] );
2196 
2197  dS2dR1[1][iP] = dadr1/b - dbdr1*a/(b*b) ;
2198  dS2dR2[1][iP] = dadr2/b - dbdr2*a/(b*b) ;
2199  }
2200  else
2201  {
2202  dS2dR1[1][iP] = 0;
2203  dS2dR2[1][iP] = 0;
2204  }
2205  }
2206  }
2207 
2208  //select a point which is close to the primary vertex (with the smallest r)
2209 
2210  float dr2[2];
2211  for(int iP = 0; iP<2; iP++)
2212  {
2213  const float& bs1 = bq1*dS1[iP];
2214  const float& bs2 = bq2*dS2[iP];
2215  float sss = sin(bs1), ccc = cos(bs1);
2216 
2217  const bool& bs1Big = fabs(bs1) > 1.e-8f;
2218  const bool& bs2Big = fabs(bs2) > 1.e-8f;
2219 
2220  float sB(0.f), cB(0.f);
2221  if(bs1Big)
2222  {
2223  sB = sss/bq1;
2224  cB = (1.f-ccc)/bq1;
2225  }
2226  else
2227  {
2228  sB = ((1.f-bs1*kOvSqr6)*(1.f+bs1*kOvSqr6)*dS1[iP]);
2229  cB = .5f*sB*bs1;
2230  }
2231 
2232  const float& x1 = param1[0] + sB*px1 + cB*py1;
2233  const float& y1 = param1[1] - cB*px1 + sB*py1;
2234  const float& z1 = param1[2] + dS1[iP]*param1[5];
2235 
2236  sss = sin(bs2), ccc = cos(bs2);
2237 
2238  if(bs2Big)
2239  {
2240  sB = sss/bq2;
2241  cB = (1.f-ccc)/bq2;
2242  }
2243  else
2244  {
2245  sB = ((1.f-bs2*kOvSqr6)*(1.f+bs2*kOvSqr6)*dS2[iP]);
2246  cB = .5f*sB*bs2;
2247  }
2248 
2249  const float& x2 = param2[0] + sB*px2 + cB*py2;
2250  const float& y2 = param2[1] - cB*px2 + sB*py2;
2251  const float& z2 = param2[2] + dS2[iP]*param2[5];
2252 
2253  float dx = (x1-x2);
2254  float dy = (y1-y2);
2255  float dz = (z1-z2);
2256 
2257  dr2[iP] = dx*dx + dy*dy + dz*dz;
2258  }
2259 
2260  const bool isFirstRoot = dr2[0] < dr2[1];
2261  if(isFirstRoot)
2262  {
2263  dS[0] = dS1[0];
2264  dS[1] = dS2[0];
2265 
2266  for(int iP=0; iP<6; iP++)
2267  {
2268  dsdr[0][iP] = dS1dR1[0][iP];
2269  dsdr[1][iP] = dS1dR2[0][iP];
2270  dsdr[2][iP] = dS2dR1[0][iP];
2271  dsdr[3][iP] = dS2dR2[0][iP];
2272  }
2273  }
2274  else
2275  {
2276  dS[0] = dS1[1];
2277  dS[1] = dS2[1];
2278 
2279  for(int iP=0; iP<6; iP++)
2280  {
2281  dsdr[0][iP] = dS1dR1[1][iP];
2282  dsdr[1][iP] = dS1dR2[1][iP];
2283  dsdr[2][iP] = dS2dR1[1][iP];
2284  dsdr[3][iP] = dS2dR2[1][iP];
2285  }
2286  }
2287 
2288  //find correct parts of helices
2289 // int n1(0);
2290 // int n2(0);
2291 // float dzMin = fabs( (z01-z02) + dS[0]*pz1 - dS[1]*pz2 );
2292 // const float pi2(6.283185307f);
2293 
2294  //TODO optimise for loops for neutral particles
2295 // const float& i1Float = -bq1/pi2*(z01/pz1+dS[0]);
2296 // for(int di1=-1; di1<=1; di1++)
2297 // {
2298 // int i1(0);
2299 // if(!isStraight1)
2300 // i1 = int(i1Float) + di1;
2301 //
2302 // const float& i2Float = ( ((z01-z02) + (dS[0]+pi2*i1/bq1)*pz1)/pz2 - dS[1]) * bq2/pi2;
2303 // for(int di2 = -1; di2<=1; di2++)
2304 // {
2305 // int i2(0);
2306 // if(!isStraight2)
2307 // i2 = int(i2Float) + di2;
2308 //
2309 // const float& z1 = z01 + (dS[0]+pi2*i1/bq1)*pz1;
2310 // const float& z2 = z02 + (dS[1]+pi2*i2/bq2)*pz2;
2311 // const float& dz = fabs( z1-z2 );
2312 //
2313 // if(dz < dzMin)
2314 // {
2315 // n1 = i1;
2316 // n2 = i2;
2317 // dzMin = dz;
2318 // }
2319 // }
2320 // }
2321 //
2322 // if(!isStraight1)
2323 // dS[0] += float(n1)*pi2/bq1;
2324 // if(!isStraight2)
2325 // dS[1] += float(n2)*pi2/bq2;
2326 
2327  //Line correction
2328  {
2329  const float& bs1 = bq1*dS[0];
2330  const float& bs2 = bq2*dS[1];
2331  float sss = sin(bs1), ccc = cos(bs1);
2332 
2333  const bool& bs1Big = fabs(bs1) > 1.e-8f;
2334  const bool& bs2Big = fabs(bs2) > 1.e-8f;
2335 
2336  float sB(0.f), cB(0.f);
2337  if(bs1Big)
2338  {
2339  sB = sss/bq1;
2340  cB = (1.f-ccc)/bq1;
2341  }
2342  else
2343  {
2344  sB = ((1.f-bs1*kOvSqr6)*(1.f+bs1*kOvSqr6)*dS[0]);
2345  cB = .5f*sB*bs1;
2346  }
2347 
2348  const float& x1 = x01 + sB*px1 + cB*py1;
2349  const float& y1 = y01 - cB*px1 + sB*py1;
2350  const float& z1 = z01 + dS[0]*pz1;
2351  const float& ppx1 = ccc*px1 + sss*py1;
2352  const float& ppy1 = -sss*px1 + ccc*py1;
2353  const float& ppz1 = pz1;
2354 
2355  float sss1 = sin(bs2), ccc1 = cos(bs2);
2356 
2357  float sB1(0.f), cB1(0.f);
2358  if(bs2Big)
2359  {
2360  sB1 = sss1/bq2;
2361  cB1 = (1.f-ccc1)/bq2;
2362  }
2363  else
2364  {
2365  sB1 = ((1.f-bs2*kOvSqr6)*(1.f+bs2*kOvSqr6)*dS[1]);
2366  cB1 = .5f*sB1*bs2;
2367  }
2368 
2369  const float& x2 = x02 + sB1*px2 + cB1*py2;
2370  const float& y2 = y02 - cB1*px2 + sB1*py2;
2371  const float& z2 = z02 + dS[1]*pz2;
2372  const float& ppx2 = ccc1*px2 + sss1*py2;
2373  const float& ppy2 = -sss1*px2 + ccc1*py2;
2374  const float& ppz2 = pz2;
2375 
2376  const float& p12 = ppx1*ppx1 + ppy1*ppy1 + ppz1*ppz1;
2377  const float& p22 = ppx2*ppx2 + ppy2*ppy2 + ppz2*ppz2;
2378  const float& lp1p2 = ppx1*ppx2 + ppy1*ppy2 + ppz1*ppz2;
2379 
2380  const float& dx = (x2 - x1);
2381  const float& dy = (y2 - y1);
2382  const float& dz = (z2 - z1);
2383 
2384  const float& ldrp1 = ppx1*dx + ppy1*dy + ppz1*dz;
2385  const float& ldrp2 = ppx2*dx + ppy2*dy + ppz2*dz;
2386 
2387  float detp = lp1p2*lp1p2 - p12*p22;
2388  if( fabs(detp)<1.e-4 ) detp = 1; //TODO correct!!!
2389 
2390  //dsdr calculation
2391  const float a1 = ldrp2*lp1p2 - ldrp1*p22;
2392  const float a2 = ldrp2*p12 - ldrp1*lp1p2;
2393  const float lp1p2_ds0 = bq1*( ppx2*ppy1 - ppy2*ppx1);
2394  const float lp1p2_ds1 = bq2*( ppx1*ppy2 - ppy1*ppx2);
2395  const float ldrp1_ds0 = -p12 + bq1*(ppy1*dx - ppx1*dy);
2396  const float ldrp1_ds1 = lp1p2;
2397  const float ldrp2_ds0 = -lp1p2;
2398  const float ldrp2_ds1 = p22 + bq2*(ppy2*dx - ppx2*dy);
2399  const float detp_ds0 = 2*lp1p2*lp1p2_ds0;
2400  const float detp_ds1 = 2*lp1p2*lp1p2_ds1;
2401  const float a1_ds0 = ldrp2_ds0*lp1p2 + ldrp2*lp1p2_ds0 - ldrp1_ds0*p22;
2402  const float a1_ds1 = ldrp2_ds1*lp1p2 + ldrp2*lp1p2_ds1 - ldrp1_ds1*p22;
2403  const float a2_ds0 = ldrp2_ds0*p12 - ldrp1_ds0*lp1p2 - ldrp1*lp1p2_ds0;
2404  const float a2_ds1 = ldrp2_ds1*p12 - ldrp1_ds1*lp1p2 - ldrp1*lp1p2_ds1;
2405 
2406  const float dsl1ds0 = a1_ds0/detp - a1*detp_ds0/(detp*detp);
2407  const float dsl1ds1 = a1_ds1/detp - a1*detp_ds1/(detp*detp);
2408  const float dsl2ds0 = a2_ds0/detp - a2*detp_ds0/(detp*detp);
2409  const float dsl2ds1 = a2_ds1/detp - a2*detp_ds1/(detp*detp);
2410 
2411  float dsldr[4][6];
2412  for(int iP=0; iP<6; iP++)
2413  {
2414  dsldr[0][iP] = dsl1ds0*dsdr[0][iP] + dsl1ds1*dsdr[2][iP];
2415  dsldr[1][iP] = dsl1ds0*dsdr[1][iP] + dsl1ds1*dsdr[3][iP];
2416  dsldr[2][iP] = dsl2ds0*dsdr[0][iP] + dsl2ds1*dsdr[2][iP];
2417  dsldr[3][iP] = dsl2ds0*dsdr[1][iP] + dsl2ds1*dsdr[3][iP];
2418  }
2419 
2420  for(int iDS=0; iDS<4; iDS++)
2421  for(int iP=0; iP<6; iP++)
2422  dsdr[iDS][iP] += dsldr[iDS][iP];
2423 
2424  const float lp1p2_dr0[6] = {0, 0, 0, ccc*ppx2 - ppy2*sss, ccc*ppy2 + ppx2*sss, pz2};
2425  const float lp1p2_dr1[6] = {0, 0, 0, ccc1*ppx1 - ppy1*sss1, ccc1*ppy1 + ppx1*sss1, pz1};
2426  const float ldrp1_dr0[6] = {-ppx1, -ppy1, -pz1, cB*ppy1 - ppx1*sB + ccc*dx - sss*dy, -cB*ppx1-ppy1*sB + sss*dx + ccc*dy, -dS[0]*pz1 + dz};
2427  const float ldrp1_dr1[6] = { ppx1, ppy1, pz1, -cB1*ppy1 + ppx1*sB1, cB1*ppx1 + ppy1*sB1, dS[1]*pz1};
2428  const float ldrp2_dr0[6] = {-ppx2, -ppy2, -pz2, cB*ppy2 - ppx2*sB, -cB*ppx2-ppy2*sB, -dS[0]*pz2};
2429  const float ldrp2_dr1[6] = {ppx2, ppy2, pz2, -cB1*ppy2 + ppx2*sB1 + ccc1*dx- sss1*dy, cB1*ppx2 + ppy2*sB1 + sss1*dx + ccc1*dy, dz + dS[1]*pz2};
2430  const float p12_dr0[6] = {0, 0, 0, 2*px1, 2*py1, 2*pz1};
2431  const float p22_dr1[6] = {0, 0, 0, 2*px2, 2*py2, 2*pz2};
2432  float a1_dr0[6], a1_dr1[6], a2_dr0[6], a2_dr1[6], detp_dr0[6], detp_dr1[6];
2433  for(int iP=0; iP<6; iP++)
2434  {
2435  a1_dr0[iP] = ldrp2_dr0[iP]*lp1p2 + ldrp2*lp1p2_dr0[iP] - ldrp1_dr0[iP]*p22;
2436  a1_dr1[iP] = ldrp2_dr1[iP]*lp1p2 + ldrp2*lp1p2_dr1[iP] - ldrp1_dr1[iP]*p22 - ldrp1*p22_dr1[iP];
2437  a2_dr0[iP] = ldrp2_dr0[iP]*p12 + ldrp2*p12_dr0[iP] - ldrp1_dr0[iP]*lp1p2 - ldrp1*lp1p2_dr0[iP];
2438  a2_dr1[iP] = ldrp2_dr1[iP]*p12 - ldrp1_dr1[iP]*lp1p2 - ldrp1*lp1p2_dr1[iP];
2439  detp_dr0[iP] = 2*lp1p2*lp1p2_dr0[iP] - p12_dr0[iP]*p22;
2440  detp_dr1[iP] = 2*lp1p2*lp1p2_dr1[iP] - p12*p22_dr1[iP];
2441 
2442  dsdr[0][iP] += a1_dr0[iP]/detp - a1*detp_dr0[iP]/(detp*detp);
2443  dsdr[1][iP] += a1_dr1[iP]/detp - a1*detp_dr1[iP]/(detp*detp);
2444  dsdr[2][iP] += a2_dr0[iP]/detp - a2*detp_dr0[iP]/(detp*detp);
2445  dsdr[3][iP] += a2_dr1[iP]/detp - a2*detp_dr1[iP]/(detp*detp);
2446  }
2447 
2448  dS[0] += (ldrp2*lp1p2 - ldrp1*p22) /detp;
2449  dS[1] += (ldrp2*p12 - ldrp1*lp1p2)/detp;
2450  }
2451 }
2452 
2453 void KFParticleBase::GetDStoParticleBy( float B, const KFParticleBase &p, float dS[2], float dsdr[4][6] ) const
2454 {
2476  const float param1[6] = { fP[0], -fP[2], fP[1], fP[3], -fP[5], fP[4] };
2477  const float param2[6] = { p.fP[0], -p.fP[2], p.fP[1], p.fP[3], -p.fP[5], p.fP[4] };
2478 
2479  float dsdrBz[4][6];
2480  for(int i1=0; i1<4; i1++)
2481  for(int i2=0; i2<6; i2++)
2482  dsdrBz[i1][i2] = 0;
2483 
2484  GetDStoParticleBz(B, p, dS, dsdrBz, param1, param2);
2485 
2486  for(int iDs=0; iDs<4; iDs++)
2487  {
2488  dsdr[iDs][0] = dsdrBz[iDs][0];
2489  dsdr[iDs][1] = dsdrBz[iDs][2];
2490  dsdr[iDs][2] = -dsdrBz[iDs][1];
2491  dsdr[iDs][3] = dsdrBz[iDs][3];
2492  dsdr[iDs][4] = dsdrBz[iDs][5];
2493  dsdr[iDs][5] = -dsdrBz[iDs][4];
2494  }
2495 }
2496 
2497 void KFParticleBase::GetDStoParticleLine( const KFParticleBase &p, float dS[2], float dsdr[4][6] ) const
2498 {
2516  float p12 = fP[3]*fP[3] + fP[4]*fP[4] + fP[5]*fP[5];
2517  float p22 = p.fP[3]*p.fP[3] + p.fP[4]*p.fP[4] + p.fP[5]*p.fP[5];
2518  float p1p2 = fP[3]*p.fP[3] + fP[4]*p.fP[4] + fP[5]*p.fP[5];
2519 
2520  float drp1 = fP[3]*(p.fP[0]-fP[0]) + fP[4]*(p.fP[1]-fP[1]) + fP[5]*(p.fP[2]-fP[2]);
2521  float drp2 = p.fP[3]*(p.fP[0]-fP[0]) + p.fP[4]*(p.fP[1]-fP[1]) + p.fP[5]*(p.fP[2]-fP[2]);
2522 
2523  float detp = p1p2*p1p2 - p12*p22;
2524  if( fabs(detp)<1.e-4 ) detp = 1; //TODO correct!!!
2525 
2526  dS[0] = (drp2*p1p2 - drp1*p22) /detp;
2527  dS[1] = (drp2*p12 - drp1*p1p2)/detp;
2528 
2529  const float x01 = fP[0];
2530  const float y01 = fP[1];
2531  const float z01 = fP[2];
2532  const float px1 = fP[3];
2533  const float py1 = fP[4];
2534  const float pz1 = fP[5];
2535 
2536  const float x02 = p.fP[0];
2537  const float y02 = p.fP[1];
2538  const float z02 = p.fP[2];
2539  const float px2 = p.fP[3];
2540  const float py2 = p.fP[4];
2541  const float pz2 = p.fP[5];
2542 
2543  const float drp1_dr1[6] = {-px1, -py1, -pz1, -x01 + x02, -y01 + y02, -z01 + z02};
2544  const float drp1_dr2[6] = {px1, py1, pz1, 0, 0, 0};
2545  const float drp2_dr1[6] = {-px2, -py2, -pz2, 0, 0, 0};
2546  const float drp2_dr2[6] = {px2, py2, pz2, -x01 + x02, -y01 + y02, -z01 + z02};
2547  const float dp1p2_dr1[6] = {0, 0, 0, px2, py2, pz2};
2548  const float dp1p2_dr2[6] = {0, 0, 0, px1, py1, pz1};
2549  const float dp12_dr1[6] = {0, 0, 0, 2*px1, 2*py1, 2*pz1};
2550  const float dp12_dr2[6] = {0, 0, 0, 0, 0, 0};
2551  const float dp22_dr1[6] = {0, 0, 0, 0, 0, 0};
2552  const float dp22_dr2[6] = {0, 0, 0, 2*px2, 2*py2, 2*pz2};
2553  const float ddetp_dr1[6] = {0, 0, 0, -2*p22*px1 + 2*p1p2*px2, -2*p22*py1 + 2*p1p2*py2, -2*p22*pz1 + 2*p1p2*pz2};
2554  const float ddetp_dr2[6] = {0, 0, 0, 2*p1p2*px1 - 2*p12*px2, 2*p1p2*py1 - 2*p12*py2, 2*p1p2*pz1 - 2*p12*pz2};
2555 
2556 
2557  float da1_dr1[6], da1_dr2[6], da2_dr1[6], da2_dr2[6];
2558 
2559  const float a1 = drp2*p1p2 - drp1*p22;
2560  const float a2 = drp2*p12 - drp1*p1p2;
2561  for(int i=0; i<6; i++)
2562  {
2563  da1_dr1[i] = drp2_dr1[i]*p1p2 + drp2*dp1p2_dr1[i] - drp1_dr1[i]*p22 - drp1*dp22_dr1[i];
2564  da1_dr2[i] = drp2_dr2[i]*p1p2 + drp2*dp1p2_dr2[i] - drp1_dr2[i]*p22 - drp1*dp22_dr2[i];
2565 
2566  da2_dr1[i] = drp2_dr1[i]*p12 + drp2*dp12_dr1[i] - drp1_dr1[i]*p1p2 - drp1*dp1p2_dr1[i];
2567  da2_dr2[i] = drp2_dr2[i]*p12 + drp2*dp12_dr2[i] - drp1_dr2[i]*p1p2 - drp1*dp1p2_dr2[i];
2568 
2569  dsdr[0][i] = da1_dr1[i]/detp - a1/(detp*detp)*ddetp_dr1[i];
2570  dsdr[1][i] = da1_dr2[i]/detp - a1/(detp*detp)*ddetp_dr2[i];
2571 
2572  dsdr[2][i] = da2_dr1[i]/detp - a2/(detp*detp)*ddetp_dr1[i];
2573  dsdr[3][i] = da2_dr2[i]/detp - a2/(detp*detp)*ddetp_dr2[i];
2574  }
2575 }
2576 
2577 void KFParticleBase::GetDStoParticleCBM( const KFParticleBase &p, float dS[2], float dsdr[4][6] ) const
2578 {
2600  float fld[3];
2601  GetFieldValue( fP, fld );
2602 
2603  const float& bq1 = fld[1]*fQ;
2604  const float& bq2 = fld[1]*p.fQ;
2605  const bool& isStraight1 = fabs(bq1) < 1.e-8f;
2606  const bool& isStraight2 = fabs(bq2) < 1.e-8f;
2607 
2608  if( isStraight1 && isStraight2 )
2609  GetDStoParticleLine(p, dS, dsdr);
2610  else
2611  GetDStoParticleBy(fld[1], p, dS, dsdr);
2612 }
2613 
2614 void KFParticleBase::TransportCBM( float dS, const float* dsdr, float P[], float C[], float* dsdr1, float* F, float* F1) const
2615 {
2637  if( fQ==0 ){
2638  TransportLine( dS, dsdr, P, C, dsdr1, F, F1 );
2639  return;
2640  }
2641 
2642  if( fabs(dS*fP[5]) > 1000.f ) dS = 0;
2643 
2644  const float kCLight = 0.000299792458;
2645 
2646  float c = fQ*kCLight;
2647 
2648  // construct coefficients
2649 
2650  float
2651  px = fP[3],
2652  py = fP[4],
2653  pz = fP[5];
2654 
2655  float sx=0, sy=0, sz=0, syy=0, syz=0, syyy=0, ssx=0, ssy=0, ssz=0, ssyy=0, ssyz=0, ssyyy=0;
2656 
2657  { // get field integrals
2658 
2659  float fld[3][3];
2660  float p0[3], p1[3], p2[3];
2661 
2662  // line track approximation
2663 
2664  p0[0] = fP[0];
2665  p0[1] = fP[1];
2666  p0[2] = fP[2];
2667 
2668  p2[0] = fP[0] + px*dS;
2669  p2[1] = fP[1] + py*dS;
2670  p2[2] = fP[2] + pz*dS;
2671 
2672  p1[0] = 0.5*(p0[0]+p2[0]);
2673  p1[1] = 0.5*(p0[1]+p2[1]);
2674  p1[2] = 0.5*(p0[2]+p2[2]);
2675 
2676  // first order track approximation
2677  {
2678  GetFieldValue( p0, fld[0] );
2679  GetFieldValue( p1, fld[1] );
2680  GetFieldValue( p2, fld[2] );
2681 
2682  float ssy1 = ( 7*fld[0][1] + 6*fld[1][1]-fld[2][1] )*c*dS*dS/96.;
2683  float ssy2 = ( fld[0][1] + 2*fld[1][1] )*c*dS*dS/6.;
2684 
2685  p1[0] -= ssy1*pz;
2686  p1[2] += ssy1*px;
2687  p2[0] -= ssy2*pz;
2688  p2[2] += ssy2*px;
2689  }
2690 
2691  GetFieldValue( p0, fld[0] );
2692  GetFieldValue( p1, fld[1] );
2693  GetFieldValue( p2, fld[2] );
2694 
2695  sx = c*( fld[0][0] + 4*fld[1][0] + fld[2][0] )*dS/6.;
2696  sy = c*( fld[0][1] + 4*fld[1][1] + fld[2][1] )*dS/6.;
2697  sz = c*( fld[0][2] + 4*fld[1][2] + fld[2][2] )*dS/6.;
2698 
2699  ssx = c*( fld[0][0] + 2*fld[1][0])*dS*dS/6.;
2700  ssy = c*( fld[0][1] + 2*fld[1][1])*dS*dS/6.;
2701  ssz = c*( fld[0][2] + 2*fld[1][2])*dS*dS/6.;
2702 
2703  float c2[3][3] = { { 5, -4, -1},{ 44, 80, -4},{ 11, 44, 5} }; // /=360.
2704  float cc2[3][3] = { { 38, 8, -4},{ 148, 208, -20},{ 3, 36, 3} }; // /=2520.
2705  for(Int_t n=0; n<3; n++)
2706  for(Int_t m=0; m<3; m++)
2707  {
2708  syz += c2[n][m]*fld[n][1]*fld[m][2];
2709  ssyz += cc2[n][m]*fld[n][1]*fld[m][2];
2710  }
2711 
2712  syz *= c*c*dS*dS/360.;
2713  ssyz *= c*c*dS*dS*dS/2520.;
2714 
2715  syy = c*( fld[0][1] + 4*fld[1][1] + fld[2][1] )*dS;
2716  syyy = syy*syy*syy / 1296;
2717  syy = syy*syy/72;
2718 
2719  ssyy = ( fld[0][1]*( 38*fld[0][1] + 156*fld[1][1] - fld[2][1] )+
2720  fld[1][1]*( 208*fld[1][1] +16*fld[2][1] )+
2721  fld[2][1]*( 3*fld[2][1] )
2722  )*dS*dS*dS*c*c/2520.;
2723  ssyyy =
2724  (
2725  fld[0][1]*( fld[0][1]*( 85*fld[0][1] + 526*fld[1][1] - 7*fld[2][1] )+
2726  fld[1][1]*( 1376*fld[1][1] +84*fld[2][1] )+
2727  fld[2][1]*( 19*fld[2][1] ) )+
2728  fld[1][1]*( fld[1][1]*( 1376*fld[1][1] +256*fld[2][1] )+
2729  fld[2][1]*( 62*fld[2][1] ) )+
2730  fld[2][1]*fld[2][1] *( 3*fld[2][1] )
2731  )*dS*dS*dS*dS*c*c*c/90720.;
2732 
2733  }
2734 
2735  float mJ[8][8];
2736  for( Int_t i=0; i<8; i++ ) for( Int_t j=0; j<8; j++) mJ[i][j]=0;
2737 
2738  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;
2739  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;
2740  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;
2741 
2742  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;
2743  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;
2744  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;
2745  mJ[6][6] = mJ[7][7] = 1;
2746 
2747  P[0] = fP[0] + mJ[0][3]*px + mJ[0][4]*py + mJ[0][5]*pz;
2748  P[1] = fP[1] + mJ[1][3]*px + mJ[1][4]*py + mJ[1][5]*pz;
2749  P[2] = fP[2] + mJ[2][3]*px + mJ[2][4]*py + mJ[2][5]*pz;
2750  P[3] = mJ[3][3]*px + mJ[3][4]*py + mJ[3][5]*pz;
2751  P[4] = mJ[4][3]*px + mJ[4][4]*py + mJ[4][5]*pz;
2752  P[5] = mJ[5][3]*px + mJ[5][4]*py + mJ[5][5]*pz;
2753  P[6] = fP[6];
2754  P[7] = fP[7];
2755 
2756  float mJds[6][6];
2757  for( Int_t i=0; i<6; i++ ) for( Int_t j=0; j<6; j++) mJds[i][j]=0;
2758 
2759  if(fabs(dS)>0)
2760  {
2761  mJds[0][3]= 1 - 3*ssyy/dS; mJds[0][4]= 2*ssx/dS; mJds[0][5]= (4.f*ssyyy-2*ssy)/dS;
2762  mJds[1][3]= -2.f*ssz/dS; mJds[1][4]= 1; mJds[1][5]= (2.f*ssx + 3.*ssyz)/dS;
2763  mJds[2][3]= (2.f*ssy-4.f*ssyyy)/dS; mJds[2][4]=-2*ssx/dS; mJds[2][5]= 1 - 3.f*ssyy/dS;
2764 
2765  mJds[3][3]= -2.f*syy/dS; mJds[3][4]= sx/dS; mJds[3][5]= 3.f*syyy/dS - sy/dS;
2766  mJds[4][3]= -sz/dS; mJds[4][4]=0; mJds[4][5] = sx/dS + 2.f*syz/dS;
2767  mJds[5][3]= sy/dS - 3.f*syyy/dS; mJds[5][4]=-sx/dS; mJds[5][5]= -2.f*syy/dS;
2768  }
2769 
2770  for(int i1=0; i1<6; i1++)
2771  for(int i2=0; i2<6; i2++)
2772  mJ[i1][i2] += mJds[i1][3]*px*dsdr[i2] + mJds[i1][4]*py*dsdr[i2] + mJds[i1][5]*pz*dsdr[i2];
2773 
2774  MultQSQt( mJ[0], fC, C, 8);
2775 
2776  if(F)
2777  {
2778  for(int i=0; i<6; i++)
2779  for(int j=0; j<6; j++)
2780  F[i*6+j] = mJ[i][j];
2781 
2782  for(int i1=0; i1<6; i1++)
2783  for(int i2=0; i2<6; i2++)
2784  F1[i1*6 + i2] = mJds[i1][3]*px*dsdr1[i2] + mJds[i1][4]*py*dsdr1[i2] + mJds[i1][5]*pz*dsdr1[i2];
2785  }
2786 }
2787 
2788 
2789 void KFParticleBase::TransportBz( float Bz, float dS, const float* dsdr, float P[], float C[], float* dsdr1, float* F, float* F1 ) const
2790 {
2813  const float kCLight = 0.000299792458;
2814  Bz = Bz*fQ*kCLight;
2815  float bs= Bz*dS;
2816  float s = sin(bs), c = cos(bs);
2817  float sB, cB;
2818  if( fabs(bs)>1.e-10){
2819  sB= s/Bz;
2820  cB= (1-c)/Bz;
2821  }else{
2822  const float kOvSqr6 = 1./sqrt(6.);
2823  sB = (1.-bs*kOvSqr6)*(1.+bs*kOvSqr6)*dS;
2824  cB = .5*sB*bs;
2825  }
2826 
2827  float px = fP[3];
2828  float py = fP[4];
2829  float pz = fP[5];
2830 
2831  P[0] = fP[0] + sB*px + cB*py;
2832  P[1] = fP[1] - cB*px + sB*py;
2833  P[2] = fP[2] + dS*pz;
2834  P[3] = c*px + s*py;
2835  P[4] = -s*px + c*py;
2836  P[5] = fP[5];
2837  P[6] = fP[6];
2838  P[7] = fP[7];
2839 
2840  float mJ[8][8];
2841  for( Int_t i=0; i<8; i++ ) for( Int_t j=0; j<8; j++) mJ[i][j]=0;
2842 
2843  for(int i=0; i<8; i++) mJ[i][i]=1;
2844  mJ[0][3] = sB; mJ[0][4] = cB;
2845  mJ[1][3] = -cB; mJ[1][4] = sB;
2846  mJ[2][5] = dS;
2847  mJ[3][3] = c; mJ[3][4] = s;
2848  mJ[4][3] = -s; mJ[4][4] = c;
2849 
2850 
2851  float mJds[6][6];
2852  for( Int_t i=0; i<6; i++ ) for( Int_t j=0; j<6; j++) mJds[i][j]=0;
2853  mJds[0][3] = c; mJds[0][4] = s;
2854  mJds[1][3] = -s; mJds[1][4] = c;
2855  mJds[2][5] = 1;
2856  mJds[3][3] = -Bz*s; mJds[3][4] = Bz*c;
2857  mJds[4][3] = -Bz*c; mJds[4][4] = -Bz*s;
2858 
2859  for(int i1=0; i1<6; i1++)
2860  for(int i2=0; i2<6; i2++)
2861  mJ[i1][i2] += mJds[i1][3]*px*dsdr[i2] + mJds[i1][4]*py*dsdr[i2] + mJds[i1][5]*pz*dsdr[i2];
2862 
2863  MultQSQt( mJ[0], fC, C, 8);
2864 
2865  if(F)
2866  {
2867  for(int i=0; i<6; i++)
2868  for(int j=0; j<6; j++)
2869  F[i*6+j] = mJ[i][j];
2870 
2871  for(int i1=0; i1<6; i1++)
2872  for(int i2=0; i2<6; i2++)
2873  F1[i1*6 + i2] = mJds[i1][3]*px*dsdr1[i2] + mJds[i1][4]*py*dsdr1[i2] + mJds[i1][5]*pz*dsdr1[i2];
2874  }
2875 }
2876 
2877 
2878 float KFParticleBase::GetDistanceFromVertex( const KFParticleBase &Vtx ) const
2879 {
2884  return GetDistanceFromVertex( Vtx.fP );
2885 }
2886 
2887 float KFParticleBase::GetDistanceFromVertex( const float vtx[] ) const
2888 {
2893  float mP[8], mC[36];
2894 
2895  float dsdr[6] = {0.f};
2896  const float dS = GetDStoPoint(vtx, dsdr);
2897 
2898  Transport( dS, dsdr, mP, mC );
2899  float d[3]={ vtx[0]-mP[0], vtx[1]-mP[1], vtx[2]-mP[2]};
2900  return sqrt( d[0]*d[0]+d[1]*d[1]+d[2]*d[2] );
2901 }
2902 
2903 float KFParticleBase::GetDistanceFromParticle( const KFParticleBase &p )
2904  const
2905 {
2910  float dsdr[4][6];
2911  float dS[2];
2912  GetDStoParticle( p, dS, dsdr );
2913  float mP[8], mC[36], mP1[8], mC1[36];
2914  Transport( dS[0], dsdr[0], mP, mC );
2915  p.Transport( dS[1], dsdr[3], mP1, mC1 );
2916  float dx = mP[0]-mP1[0];
2917  float dy = mP[1]-mP1[1];
2918  float dz = mP[2]-mP1[2];
2919  return sqrt(dx*dx+dy*dy+dz*dz);
2920 }
2921 
2922 float KFParticleBase::GetDeviationFromVertex( const KFParticleBase &Vtx ) const
2923 {
2928  return GetDeviationFromVertex( Vtx.fP, Vtx.fC );
2929 }
2930 
2931 
2932 float KFParticleBase::GetDeviationFromVertex( const float v[], const float Cv[] ) const
2933 {
2939  float mP[8];
2940  float mC[36];
2941  float dsdr[6] = {0.f,0.f,0.f,0.f,0.f,0.f};
2942  const float dS = GetDStoPoint(v, dsdr);
2943  float dsdp[6] = {-dsdr[0], -dsdr[1], -dsdr[2], 0, 0, 0};
2944  float F[36], F1[36];
2945  for(int i2=0; i2<36; i2++)
2946  {
2947  F[i2] = 0;
2948  F1[i2] = 0;
2949  }
2950  Transport( dS, dsdr, mP, mC, dsdp, F, F1 );
2951 
2952  if(Cv)
2953  {
2954  float VFT[3][6];
2955  for(int i=0; i<3; i++)
2956  for(int j=0; j<6; j++)
2957  {
2958  VFT[i][j] = 0;
2959  for(int k=0; k<3; k++)
2960  {
2961  VFT[i][j] += Cv[IJ(i,k)] * F1[j*6+k];
2962  }
2963  }
2964 
2965  float FVFT[6][6];
2966  for(int i=0; i<6; i++)
2967  for(int j=0; j<6; j++)
2968  {
2969  FVFT[i][j] = 0;
2970  for(int k=0; k<3; k++)
2971  {
2972  FVFT[i][j] += F1[i*6+k] * VFT[k][j];
2973  }
2974  }
2975  mC[0] += FVFT[0][0] + Cv[0];
2976  mC[1] += FVFT[1][0] + Cv[1];
2977  mC[2] += FVFT[1][1] + Cv[2];
2978  mC[3] += FVFT[2][0] + Cv[3];
2979  mC[4] += FVFT[2][1] + Cv[4];
2980  mC[5] += FVFT[2][2] + Cv[5];
2981  }
2982 
2983  InvertCholetsky3(mC);
2984 
2985  float d[3]={ v[0]-mP[0], v[1]-mP[1], v[2]-mP[2]};
2986 
2987  return ( ( mC[0]*d[0] + mC[1]*d[1] + mC[3]*d[2])*d[0]
2988  +(mC[1]*d[0] + mC[2]*d[1] + mC[4]*d[2])*d[1]
2989  +(mC[3]*d[0] + mC[4]*d[1] + mC[5]*d[2])*d[2] );
2990 }
2991 
2992 
2993 float KFParticleBase::GetDeviationFromParticle( const KFParticleBase &p ) const
2994 {
2999  float ds[2] = {0.f,0.f};
3000  float dsdr[4][6];
3001  float F1[36], F2[36], F3[36], F4[36];
3002  for(int i1=0; i1<36; i1++)
3003  {
3004  F1[i1] = 0;
3005  F2[i1] = 0;
3006  F3[i1] = 0;
3007  F4[i1] = 0;
3008  }
3009  GetDStoParticle( p, ds, dsdr );
3010 
3011  float V0Tmp[36] = {0.};
3012  float V1Tmp[36] = {0.};
3013 
3014 
3015  float mP1[8], mC1[36];
3016  float mP2[8], mC2[36];
3017 
3018  Transport(ds[0], dsdr[0], mP1, mC1, dsdr[1], F1, F2);
3019  p.Transport(ds[1], dsdr[3], mP2, mC2, dsdr[2], F4, F3);
3020 
3021  MultQSQt(F2, p.fC, V0Tmp, 6);
3022  MultQSQt(F3, fC, V1Tmp, 6);
3023 
3024  for(int iC=0; iC<6; iC++)
3025  mC1[iC] += V0Tmp[iC] + mC2[iC] + V1Tmp[iC];
3026 
3027  float d[3]={ mP2[0]-mP1[0], mP2[1]-mP1[1], mP2[2]-mP1[2]};
3028 
3029  return ( ( mC1[0]*d[0] + mC1[1]*d[1] + mC1[3]*d[2])*d[0]
3030  +(mC1[1]*d[0] + mC1[2]*d[1] + mC1[4]*d[2])*d[1]
3031  +(mC1[3]*d[0] + mC1[4]*d[1] + mC1[5]*d[2])*d[2] );
3032 }
3033 
3034 
3035 
3036 void KFParticleBase::SubtractFromVertex( KFParticleBase &Vtx ) const
3037 {
3042  float m[8];
3043  float mCm[36];
3044  float D[3][3];
3045  Vtx.GetMeasurement( *this, m, mCm, D );
3046  //*
3047 
3048  float mS[6] = { mCm[0] - Vtx.fC[0] + (D[0][0] + D[0][0]),
3049  mCm[1] - Vtx.fC[1] + (D[1][0] + D[0][1]), mCm[2] - Vtx.fC[2] + (D[1][1] + D[1][1]),
3050  mCm[3] - Vtx.fC[3] + (D[2][0] + D[0][2]), mCm[4] - Vtx.fC[4] + (D[1][2] + D[2][1]), mCm[5] - Vtx.fC[5] + (D[2][2] + D[2][2]) };
3051  InvertCholetsky3(mS);
3052 
3053  //* Residual (measured - estimated)
3054 
3055  float zeta[3] = { m[0]-Vtx.fP[0], m[1]-Vtx.fP[1], m[2]-Vtx.fP[2] };
3056 
3057  //* mCHt = mCH' - D'
3058 
3059  float mCHt0[3], mCHt1[3], mCHt2[3];
3060 
3061  mCHt0[0]=Vtx.fC[ 0] ; mCHt1[0]=Vtx.fC[ 1] ; mCHt2[0]=Vtx.fC[ 3] ;
3062  mCHt0[1]=Vtx.fC[ 1] ; mCHt1[1]=Vtx.fC[ 2] ; mCHt2[1]=Vtx.fC[ 4] ;
3063  mCHt0[2]=Vtx.fC[ 3] ; mCHt1[2]=Vtx.fC[ 4] ; mCHt2[2]=Vtx.fC[ 5] ;
3064 
3065  //* Kalman gain K = mCH'*S
3066 
3067  float k0[3], k1[3], k2[3];
3068 
3069  for(Int_t i=0;i<3;++i){
3070  k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
3071  k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
3072  k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
3073  }
3074 
3075  //* New estimation of the vertex position r += K*zeta
3076 
3077  float dChi2 = ((mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2])*zeta[0]
3078  + (mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2])*zeta[1]
3079  + (mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2])*zeta[2]);
3080 
3081  for(Int_t i=0;i<3;++i)
3082  Vtx.fP[i] -= k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2];
3083 
3084  //* New covariance matrix C -= K*(mCH')'
3085 
3086  for(Int_t i=0, k=0;i<3;++i){
3087  for(Int_t j=0;j<=i;++j,++k)
3088  Vtx.fC[k] += k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j];
3089  }
3090 
3091  //* Calculate Chi^2
3092 
3093  Vtx.fNDF -= 2;
3094  Vtx.fChi2 -= dChi2;
3095 }
3096 
3097 void KFParticleBase::SubtractFromParticle( KFParticleBase &Vtx ) const
3098 {
3104  float m[8];
3105  float mV[36];
3106 
3107  float D[3][3];
3108  Vtx.GetMeasurement( *this, m, mV, D );
3109 
3110  float mS[6] = { mV[0] - Vtx.fC[0] + (D[0][0] + D[0][0]),
3111  mV[1] - Vtx.fC[1] + (D[1][0] + D[0][1]), mV[2] - Vtx.fC[2] + (D[1][1] + D[1][1]),
3112  mV[3] - Vtx.fC[3] + (D[2][0] + D[0][2]), mV[4] - Vtx.fC[4] + (D[1][2] + D[2][1]), mV[5] - Vtx.fC[5] + (D[2][2] + D[2][2]) };
3113  InvertCholetsky3(mS);
3114 
3115  //* Residual (measured - estimated)
3116 
3117  float zeta[3] = { m[0]-Vtx.fP[0], m[1]-Vtx.fP[1], m[2]-Vtx.fP[2] };
3118 
3119  //* CHt = CH' - D'
3120 
3121  float mCHt0[7], mCHt1[7], mCHt2[7];
3122 
3123  mCHt0[0]=mV[ 0] ; mCHt1[0]=mV[ 1] ; mCHt2[0]=mV[ 3] ;
3124  mCHt0[1]=mV[ 1] ; mCHt1[1]=mV[ 2] ; mCHt2[1]=mV[ 4] ;
3125  mCHt0[2]=mV[ 3] ; mCHt1[2]=mV[ 4] ; mCHt2[2]=mV[ 5] ;
3126  mCHt0[3]=Vtx.fC[ 6]-mV[ 6]; mCHt1[3]=Vtx.fC[ 7]-mV[ 7]; mCHt2[3]=Vtx.fC[ 8]-mV[ 8];
3127  mCHt0[4]=Vtx.fC[10]-mV[10]; mCHt1[4]=Vtx.fC[11]-mV[11]; mCHt2[4]=Vtx.fC[12]-mV[12];
3128  mCHt0[5]=Vtx.fC[15]-mV[15]; mCHt1[5]=Vtx.fC[16]-mV[16]; mCHt2[5]=Vtx.fC[17]-mV[17];
3129  mCHt0[6]=Vtx.fC[21]-mV[21]; mCHt1[6]=Vtx.fC[22]-mV[22]; mCHt2[6]=Vtx.fC[23]-mV[23];
3130 
3131  //* Kalman gain K = mCH'*S
3132 
3133  float k0[7], k1[7], k2[7];
3134 
3135  for(Int_t i=0;i<7;++i){
3136  k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
3137  k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
3138  k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
3139  }
3140 
3141  //* Add the daughter momentum to the particle momentum
3142 
3143  Vtx.fP[ 3] -= m[ 3];
3144  Vtx.fP[ 4] -= m[ 4];
3145  Vtx.fP[ 5] -= m[ 5];
3146  Vtx.fP[ 6] -= m[ 6];
3147 
3148  Vtx.fC[ 9] -= mV[ 9];
3149  Vtx.fC[13] -= mV[13];
3150  Vtx.fC[14] -= mV[14];
3151  Vtx.fC[18] -= mV[18];
3152  Vtx.fC[19] -= mV[19];
3153  Vtx.fC[20] -= mV[20];
3154  Vtx.fC[24] -= mV[24];
3155  Vtx.fC[25] -= mV[25];
3156  Vtx.fC[26] -= mV[26];
3157  Vtx.fC[27] -= mV[27];
3158 
3159  //* New estimation of the vertex position r += K*zeta
3160 
3161  for(Int_t i=0;i<3;++i)
3162  Vtx.fP[i] = m[i] - (k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2]);
3163  for(Int_t i=3;i<7;++i)
3164  Vtx.fP[i] = Vtx.fP[i] - (k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2]);
3165 
3166  //* New covariance matrix C -= K*(mCH')'
3167 
3168  float ffC[28] = {-mV[ 0],
3169  -mV[ 1], -mV[ 2],
3170  -mV[ 3], -mV[ 4], -mV[ 5],
3171  mV[ 6], mV[ 7], mV[ 8], Vtx.fC[ 9],
3172  mV[10], mV[11], mV[12], Vtx.fC[13], Vtx.fC[14],
3173  mV[15], mV[16], mV[17], Vtx.fC[18], Vtx.fC[19], Vtx.fC[20],
3174  mV[21], mV[22], mV[23], Vtx.fC[24], Vtx.fC[25], Vtx.fC[26], Vtx.fC[27] };
3175 
3176  for(Int_t i=0, k=0;i<7;++i){
3177  for(Int_t j=0;j<=i;++j,++k){
3178  Vtx.fC[k] = ffC[k] + (k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j] );
3179  }
3180  }
3181 
3182  //* Calculate Chi^2
3183  Vtx.fNDF -= 2;
3184  Vtx.fQ -= GetQ();
3185  Vtx.fSFromDecay = 0;
3186  Vtx.fChi2 -= ((mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2])*zeta[0]
3187  + (mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2])*zeta[1]
3188  + (mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2])*zeta[2]);
3189 }
3190 
3191 void KFParticleBase::TransportLine( float dS, const float* dsdr, float P[], float C[], float* dsdr1, float* F, float* F1 ) const
3192 {
3214  float mJ[8][8];
3215  for( Int_t i=0; i<8; i++ ) for( Int_t j=0; j<8; j++) mJ[i][j]=0;
3216 
3217  mJ[0][0]=1; mJ[0][1]=0; mJ[0][2]=0; mJ[0][3]=dS; mJ[0][4]=0; mJ[0][5]=0;
3218  mJ[1][0]=0; mJ[1][1]=1; mJ[1][2]=0; mJ[1][3]=0; mJ[1][4]=dS; mJ[1][5]=0;
3219  mJ[2][0]=0; mJ[2][1]=0; mJ[2][2]=1; mJ[2][3]=0; mJ[2][4]=0; mJ[2][5]=dS;
3220 
3221  mJ[3][0]=0; mJ[3][1]=0; mJ[3][2]=0; mJ[3][3]=1; mJ[3][4]=0; mJ[3][5]=0;
3222  mJ[4][0]=0; mJ[4][1]=0; mJ[4][2]=0; mJ[4][3]=0; mJ[4][4]=1; mJ[4][5]=0;
3223  mJ[5][0]=0; mJ[5][1]=0; mJ[5][2]=0; mJ[5][3]=0; mJ[5][4]=0; mJ[5][5]=1;
3224  mJ[6][6] = mJ[7][7] = 1;
3225 
3226  float px = fP[3], py = fP[4], pz = fP[5];
3227 
3228  P[0] = fP[0] + dS*fP[3];
3229  P[1] = fP[1] + dS*fP[4];
3230  P[2] = fP[2] + dS*fP[5];
3231  P[3] = fP[3];
3232  P[4] = fP[4];
3233  P[5] = fP[5];
3234  P[6] = fP[6];
3235  P[7] = fP[7];
3236 
3237  float mJds[6][6];
3238  for( Int_t i=0; i<6; i++ ) for( Int_t j=0; j<6; j++) mJds[i][j]=0;
3239 
3240  mJds[0][3]= 1;
3241  mJds[1][4]= 1;
3242  mJds[2][5]= 1;
3243 
3244  for(int i1=0; i1<6; i1++)
3245  for(int i2=0; i2<6; i2++)
3246  mJ[i1][i2] += mJds[i1][3]*px*dsdr[i2] + mJds[i1][4]*py*dsdr[i2] + mJds[i1][5]*pz*dsdr[i2];
3247  MultQSQt( mJ[0], fC, C, 8);
3248 
3249  if(F)
3250  {
3251  for(int i=0; i<6; i++)
3252  for(int j=0; j<6; j++)
3253  F[i*6+j] = mJ[i][j];
3254 
3255  for(int i1=0; i1<6; i1++)
3256  for(int i2=0; i2<6; i2++)
3257  F1[i1*6 + i2] = mJds[i1][3]*px*dsdr1[i2] + mJds[i1][4]*py*dsdr1[i2] + mJds[i1][5]*pz*dsdr1[i2];
3258  }
3259 }
3260 
3261 void KFParticleBase::GetArmenterosPodolanski(KFParticleBase& positive, KFParticleBase& negative, float QtAlfa[2] )
3262 {
3280  float alpha = 0., qt = 0.;
3281  float spx = positive.GetPx() + negative.GetPx();
3282  float spy = positive.GetPy() + negative.GetPy();
3283  float spz = positive.GetPz() + negative.GetPz();
3284  float sp = sqrt(spx*spx + spy*spy + spz*spz);
3285  if( sp == 0.0) return;
3286  float pn, pln, plp;
3287 
3288  pn = sqrt(negative.GetPx()*negative.GetPx() + negative.GetPy()*negative.GetPy() + negative.GetPz()*negative.GetPz());
3289 // pp = sqrt(positive.GetPx()*positive.GetPx() + positive.GetPy()*positive.GetPy() + positive.GetPz()*positive.GetPz());
3290  pln = (negative.GetPx()*spx+negative.GetPy()*spy+negative.GetPz()*spz)/sp;
3291  plp = (positive.GetPx()*spx+positive.GetPy()*spy+positive.GetPz()*spz)/sp;
3292 
3293  if( pn == 0.0) return;
3294  float ptm = (1.-((pln/pn)*(pln/pn)));
3295  qt= (ptm>=0.)? pn*sqrt(ptm) :0;
3296  alpha = (plp-pln)/(plp+pln);
3297 
3298  QtAlfa[0] = qt;
3299  QtAlfa[1] = alpha;
3300 }
3301 
3302 void KFParticleBase::RotateXY(float angle, float Vtx[3])
3303 {
3309  // Before rotation the center of the coordinat system should be moved to the vertex position; move back after rotation
3310  X() = X() - Vtx[0];
3311  Y() = Y() - Vtx[1];
3312  Z() = Z() - Vtx[2];
3313 
3314  // Rotate the kf particle
3315  float c = cos(angle);
3316  float s = sin(angle);
3317 
3318  float mA[8][ 8];
3319  for( Int_t i=0; i<8; i++ ){
3320  for( Int_t j=0; j<8; j++){
3321  mA[i][j] = 0;
3322  }
3323  }
3324  for( int i=0; i<8; i++ ){
3325  mA[i][i] = 1;
3326  }
3327  mA[0][0] = c; mA[0][1] = s;
3328  mA[1][0] = -s; mA[1][1] = c;
3329  mA[3][3] = c; mA[3][4] = s;
3330  mA[4][3] = -s; mA[4][4] = c;
3331 
3332  float mAC[8][8];
3333  float mAp[8];
3334 
3335  for( Int_t i=0; i<8; i++ ){
3336  mAp[i] = 0;
3337  for( Int_t k=0; k<8; k++){
3338  mAp[i]+=mA[i][k] * fP[k];
3339  }
3340  }
3341 
3342  for( Int_t i=0; i<8; i++){
3343  fP[i] = mAp[i];
3344  }
3345 
3346  for( Int_t i=0; i<8; i++ ){
3347  for( Int_t j=0; j<8; j++ ){
3348  mAC[i][j] = 0;
3349  for( Int_t k=0; k<8; k++ ){
3350  mAC[i][j]+= mA[i][k] * GetCovariance(k,j);
3351  }
3352  }
3353  }
3354 
3355  for( Int_t i=0; i<8; i++ ){
3356  for( Int_t j=0; j<=i; j++ ){
3357  float xx = 0;
3358  for( Int_t k=0; k<8; k++){
3359  xx+= mAC[i][k]*mA[j][k];
3360  }
3361  Covariance(i,j) = xx;
3362  }
3363  }
3364 
3365  X() = GetX() + Vtx[0];
3366  Y() = GetY() + Vtx[1];
3367  Z() = GetZ() + Vtx[2];
3368 }
3369 
3370 void KFParticleBase::InvertCholetsky3(float a[6])
3371 {
3376  float d[3], uud, u[3][3];
3377  for(int i=0; i<3; i++)
3378  {
3379  d[i]=0;
3380  for(int j=0; j<3; j++)
3381  u[i][j]=0;
3382  }
3383 
3384  for(int i=0; i<3 ; i++)
3385  {
3386  uud=0;
3387  for(int j=0; j<i; j++)
3388  uud += u[j][i]*u[j][i]*d[j];
3389  uud = a[i*(i+3)/2] - uud;
3390 
3391  if(fabs(uud)<1.e-12f) uud = 1.e-12f;
3392 
3393  d[i] = uud/fabs(uud);
3394  u[i][i] = sqrt(fabs(uud));
3395 
3396  for(int j=i+1; j<3; j++)
3397  {
3398  uud = 0;
3399  for(int k=0; k<i; k++)
3400  uud += u[k][i]*u[k][j]*d[k];
3401  uud = a[j*(j+1)/2+i] - uud;
3402  u[i][j] = d[i]/u[i][i]*uud;
3403  }
3404  }
3405 
3406  float u1[3];
3407 
3408  for(int i=0; i<3; i++)
3409  {
3410  u1[i] = u[i][i];
3411  u[i][i] = 1/u[i][i];
3412  }
3413  for(int i=0; i<2; i++)
3414  {
3415  u[i][i+1] = - u[i][i+1]*u[i][i]*u[i+1][i+1];
3416  }
3417  for(int i=0; i<1; i++)
3418  {
3419  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];
3420  }
3421 
3422  for(int i=0; i<3; i++)
3423  a[i+3] = u[i][2]*u[2][2]*d[2];
3424  for(int i=0; i<2; i++)
3425  a[i+1] = u[i][1]*u[1][1]*d[1] + u[i][2]*u[1][2]*d[2];
3426  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];
3427 }
3428 
3429 void KFParticleBase::MultQSQt( const float Q[], const float S[], float SOut[], const int kN )
3430 {
3438  float* mA = new float[kN*kN];
3439 
3440  for( Int_t i=0, ij=0; i<kN; i++ ){
3441  for( Int_t j=0; j<kN; j++, ++ij ){
3442  mA[ij] = 0 ;
3443  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];
3444  }
3445  }
3446 
3447  for( Int_t i=0; i<kN; i++ ){
3448  for( Int_t j=0; j<=i; j++ ){
3449  Int_t ij = ( j<=i ) ? i*(i+1)/2+j :j*(j+1)/2+i;
3450  SOut[ij] = 0 ;
3451  for( Int_t k=0; k<kN; k++ ) SOut[ij] += Q[ i*kN+k ] * mA[ k*kN+j ];
3452  }
3453  }
3454 
3455  if(mA) delete [] mA;
3456 }
3457 
3458 // 72-charachters line to define the printer border
3459 //3456789012345678901234567890123456789012345678901234567890123456789012
3460