Analysis Software
Documentation for sPHENIX simulation software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
KFParticleSIMD.cxx
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file KFParticleSIMD.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  *
8  * KFParticle is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 3 of the License, or
11  * (at your option) any later version.
12  *
13  * KFParticle is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program. If not, see <https://www.gnu.org/licenses/>.
20  */
21 
22 
23 
24 #include "KFParticleSIMD.h"
25 #include "KFParticle.h"
26 #include "KFParticleDatabase.h"
27 
28 #ifdef HomogeneousField
29 float_v KFParticleSIMD::fgBz = -5.f; //* Bz compoment of the magnetic field
30 #endif
31 
32 KFParticleSIMD::KFParticleSIMD( const KFParticleSIMD &d1, const KFParticleSIMD &d2 ): KFParticleBaseSIMD()
33 #ifdef NonhomogeneousField
34 , fField()
35 #endif
36 {
42  KFParticleSIMD mother;
43  mother+= d1;
44  mother+= d2;
45  *this = mother;
46 }
47 
48 void KFParticleSIMD::Create( const float_v Param[], const float_v Cov[], int_v Charge, float_v mass /*Int_t PID*/ )
49 {
66  float_v C[21];
67  for( int i=0; i<21; i++ ) C[i] = Cov[i];
68 
69  KFParticleBaseSIMD::Initialize( Param, C, Charge, mass );
70 }
71 
72 KFParticleSIMD::KFParticleSIMD( const KFPTrack *track, Int_t PID ): KFParticleBaseSIMD()
73 #ifdef NonhomogeneousField
74 , fField()
75 #endif
76 {
82  Double_t r[3];
83  Double_t C[21];
84 
85  for(Int_t iPart = 0; iPart<float_vLen; iPart++)
86  {
87  track[iPart].XvYvZv(r);
88  for(Int_t i=0; i<3; i++)
89  fP[i][iPart] = r[i];
90  track[iPart].PxPyPz(r);
91  for(Int_t i=0; i<3; i++)
92  fP[i+3][iPart] = r[i];
93  fQ[iPart] = track[iPart].Charge();
94  track[iPart].GetCovarianceXYZPxPyPz( C );
95  for(Int_t i=0; i<21; i++)
96  fC[i][iPart] = C[i];
97  }
98 
99  float_v mass = KFParticleDatabase::Instance()->GetMass(PID);
100  Create(fP,fC,fQ,mass);
101 
102  for(Int_t iPart = 0; iPart<float_vLen; iPart++)
103  {
104  fChi2[iPart] = track[iPart].GetChi2();
105  fNDF[iPart] = track[iPart].GetNDF();
106  }
107 }
108 
109 KFParticleSIMD::KFParticleSIMD(KFPTrack &Track, const Int_t *pdg): KFParticleBaseSIMD()
110 #ifdef NonhomogeneousField
111 , fField()
112 #endif
113 {
119  Double_t r[3];
120  Double_t C[21];
121 
122  Track.XvYvZv(r);
123  for(Int_t i=0; i<3; i++)
124  fP[i] = r[i];
125  Track.PxPyPz(r);
126  for(Int_t i=0; i<3; i++)
127  fP[i+3] = r[i];
128  fQ = Track.Charge();
129  Track.GetCovarianceXYZPxPyPz( C );
130  for(Int_t i=0; i<21; i++)
131  fC[i] = C[i];
132 
133  float_v mass = KFParticleDatabase::Instance()->GetMass(*pdg);
134  Create(fP,fC,fQ,mass);
135 
136  fChi2 = Track.GetChi2();
137  fNDF = Track.GetNDF();
138 }
139 
140 KFParticleSIMD::KFParticleSIMD(KFPTrackVector &track, int n, const Int_t *pdg): KFParticleBaseSIMD()
141 #ifdef NonhomogeneousField
142 , fField()
143 #endif
144 {
152  for(int i=0; i<6; i++)
153  fP[i] = track.Parameter(i)[n];
154  for(int i=0; i<21; i++)
155  fC[i] = track.Covariance(i)[n];
156 #ifdef NonhomogeneousField
157  for(int i=0; i<10; i++)
158  fField.fField[i] = track.FieldCoefficient(i)[n];
159 #endif
160  fQ = track.Q()[n];
161 
162  float_v mass = KFParticleDatabase::Instance()->GetMass(*pdg);
163  Create(fP,fC,fQ,mass);
164 }
165 
166 
167 KFParticleSIMD::KFParticleSIMD(KFPTrack* Track[], int NTracks, const Int_t *pdg): KFParticleBaseSIMD()
168 #ifdef NonhomogeneousField
169 , fField()
170 #endif
171 {
177  Create(Track, NTracks, pdg);
178 }
179 
180 void KFParticleSIMD::Create(KFPTrack* Track[], int NTracks, const Int_t *pdg)
181 {
188  Double_t r[3];
189  Double_t C[21];
190 
191  for(Int_t iPart = 0; iPart<float_vLen; iPart++)
192  {
193  Int_t iEntry = (iPart < NTracks) ? iPart : 0;
194  Track[iEntry]->XvYvZv(r);
195  for(Int_t i=0; i<3; i++)
196  fP[i][iEntry] = r[i];
197  Track[iEntry]->PxPyPz(r);
198  for(Int_t i=0; i<3; i++)
199  fP[i+3][iEntry] = r[i];
200  fQ[iEntry] = Track[iEntry]->Charge();
201  Track[iEntry]->GetCovarianceXYZPxPyPz( C );
202  for(Int_t i=0; i<21; i++)
203  fC[i][iEntry] = C[i];
204  }
205 
206  float_v mass = KFParticleDatabase::Instance()->GetMass(*pdg);
207  Create(fP,fC,fQ,mass);
208 
209  for(Int_t iPart = 0; iPart<float_vLen; iPart++)
210  {
211  Int_t iEntry = (iPart < NTracks) ? iPart : 0;
212  fChi2[iEntry] = Track[iEntry]->GetChi2();
213  fNDF[iEntry] = Track[iEntry]->GetNDF();
214  }
215 }
216 
217 KFParticleSIMD::KFParticleSIMD(KFPTrackVector &track, uint_v& index, const int_v& pdg): KFParticleBaseSIMD()
218 #ifdef NonhomogeneousField
219 , fField()
220 #endif
221 {
228  Create(track, index, pdg);
229 }
230 
231 void KFParticleSIMD::Create(KFPTrackVector &track, uint_v& index, const int_v& pdg)
232 {
241  for(int i=0; i<6; i++)
242  fP[i].gather(&(track.Parameter(i)[0]), index);
243  for(int i=0; i<21; i++)
244  fC[i].gather(&(track.Covariance(i)[0]), index);
245 #ifdef NonhomogeneousField
246  for(int i=0; i<10; i++)
247  fField.fField[i].gather(&(track.FieldCoefficient(i)[0]), index);
248 #endif
249 
250  // fPDG.gather(&(track.PDG()[0]), index);
251  fQ.gather(&(track.Q()[0]), index);
252 
253  float_v mass = KFParticleDatabase::Instance()->GetMass(pdg);
254  Create(fP,fC,fQ,mass);
255 }
256 
257 void KFParticleSIMD::Load(KFPTrackVector &track, int index, const int_v& pdg)
258 {
266  for(int i=0; i<6; i++)
267  fP[i] = reinterpret_cast<const float_v&>(track.Parameter(i)[index]);
268  for(int i=0; i<21; i++)
269  fC[i] = reinterpret_cast<const float_v&>(track.Covariance(i)[index]);
270 #ifdef NonhomogeneousField
271  for(int i=0; i<10; i++)
272  fField.fField[i] = reinterpret_cast<const float_v&>(track.FieldCoefficient(i)[index]);
273 #endif
274 
275  // fPDG.gather(&(track.PDG()[0]), index);
276  fQ = reinterpret_cast<const int_v&>(track.Q()[index]);
277 
278  float_v mass = KFParticleDatabase::Instance()->GetMass(pdg);
279  Create(fP,fC,fQ,mass);
280 }
281 
282 void KFParticleSIMD::Rotate()
283 {
286  for(int i=0; i<7; i++)
287  fP[i] = fP[i].rotated(1);
288  for(int i=0; i<27; i++)
289  fC[i] = fC[i].rotated(1);
290 #ifdef NonhomogeneousField
291  for(int i=0; i<10; i++)
292  fField.fField[i] = fField.fField[i].rotated(1);
293 #endif
294  fQ = fQ.rotated(1);
295  fId = fId.rotated(1);
296 }
297 
298 KFParticleSIMD::KFParticleSIMD(KFPEmcCluster &track, uint_v& index, const KFParticleSIMD& vertexGuess): KFParticleBaseSIMD()
299 #ifdef NonhomogeneousField
300 , fField()
301 #endif
302 {
310  Create(track, index, vertexGuess);
311 }
312 
313 void KFParticleSIMD::Create(KFPEmcCluster &track, uint_v& index, const KFParticleSIMD& vertexGuess)
314 {
322  for(int i=0; i<3; i++)
323  fP[i].gather(&(track.Parameter(i)[0]), index);
324  fP[6].gather(&(track.Parameter(3)[0]), index);
325 
326  const float_v& dx = fP[0] - vertexGuess.fP[0];
327  const float_v& dy = fP[1] - vertexGuess.fP[1];
328  const float_v& dz = fP[2] - vertexGuess.fP[2];
329  const float_v& dl2 = dx*dx + dy*dy + dz*dz;
330  const float_v& dl = sqrt(dl2);
331 
332  fP[0] = vertexGuess.fP[0];
333  fP[1] = vertexGuess.fP[1];
334  fP[2] = vertexGuess.fP[2];
335 
336  fP[3] = dx/dl * fP[6];
337  fP[4] = dy/dl * fP[6];
338  fP[5] = dz/dl * fP[6];
339 
340  float_v V[10];
341  for(int i=0; i<10; i++)
342  V[i].gather(&(track.Covariance(i)[0]), index);
343 
344  float_v J[7][4];
345  for(int i=0; i<7; i++)
346  for(int j=0; j<4; j++)
347  J[i][j] = 0.f;
348  J[0][0] = 1.f; J[1][1] = 1.f; J[2][2] = 1.f; J[6][3] = 1.f;
349  J[3][0] = fP[6]/dl * (1.f - dx*dx/dl2); J[3][1] = -dx*dy*fP[6]/(dl*dl2); J[3][2] = -dx*dz*fP[6]/(dl*dl2); J[3][3] = dx/dl;
350  J[4][0] = -dx*dy*fP[6]/(dl*dl2); J[4][1] = fP[6]/dl * (1.f - dy*dy/dl2); J[4][2] = -dy*dz*fP[6]/(dl*dl2); J[4][3] = dy/dl;
351  J[5][0] = -dx*dz*fP[6]/(dl*dl2); J[5][1] = -dy*dz*fP[6]/(dl*dl2); J[5][2] = fP[6]/dl * (1.f - dz*dz/dl2); J[5][3] = dz/dl;
352 
353  float_v VJT[4][7]; // V*J^T
354  for(Int_t i=0; i<4; i++)
355  {
356  for(Int_t j=0; j<7; j++)
357  {
358  VJT[i][j] = 0.f;
359  for(Int_t k=0; k<4; k++)
360  VJT[i][j] += V[IJ(i,k)]*J[j][k];
361  }
362  }
363  //Calculate the covariance matrix of the particle fC
364  for( Int_t i=0; i<36; i++ ) fC[i] = 0.f;
365 
366  for(Int_t i=0; i<7; ++i)
367  for(Int_t j=0; j<=i; ++j)
368  for(Int_t l=0; l<4; l++)
369  fC[IJ(i,j)]+= J[i][l]*VJT[l][j];
370  fC[35] = 1.f;
371 
372  fQ = int_v(Vc::Zero);
373  fNDF = 0;
374  fChi2 = 0;
375 
376  SumDaughterMass = float_v(Vc::Zero);
377  fMassHypo = float_v(Vc::Zero);
378 }
379 
380 KFParticleSIMD::KFParticleSIMD(KFPEmcCluster &track, int index, const KFParticleSIMD& vertexGuess): KFParticleBaseSIMD()
381 #ifdef NonhomogeneousField
382 , fField()
383 #endif
384 {
392  Load(track, index, vertexGuess);
393 }
394 
395 void KFParticleSIMD::Load(KFPEmcCluster &track, int index, const KFParticleSIMD& vertexGuess)
396 {
404  for(int i=0; i<3; i++)
405  fP[i] = reinterpret_cast<const float_v&>(track.Parameter(i)[index]);
406  fP[6] = reinterpret_cast<const float_v&>(track.Parameter(3)[index]);
407  const float_v& dx = fP[0] - vertexGuess.fP[0];
408  const float_v& dy = fP[1] - vertexGuess.fP[1];
409  const float_v& dz = fP[2] - vertexGuess.fP[2];
410  const float_v& dl2 = dx*dx + dy*dy + dz*dz;
411  const float_v& dl = sqrt(dl2);
412 
413  fP[0] = vertexGuess.fP[0];
414  fP[1] = vertexGuess.fP[1];
415  fP[2] = vertexGuess.fP[2];
416 
417  fP[3] = dx/dl * fP[6];
418  fP[4] = dy/dl * fP[6];
419  fP[5] = dz/dl * fP[6];
420 
421  float_v V[10];
422  for(int i=0; i<10; i++)
423  V[i] = reinterpret_cast<const float_v&>(track.Covariance(i)[index]);
424 
425  float_v J[7][4];
426  for(int i=0; i<7; i++)
427  for(int j=0; j<4; j++)
428  J[i][j] = 0.f;
429  J[0][0] = 1.f; J[1][1] = 1.f; J[2][2] = 1.f; J[6][3] = 1.f;
430  J[3][0] = fP[6]/dl * (1.f - dx*dx/dl2); J[3][1] = -dx*dy*fP[6]/(dl*dl2); J[3][2] = -dx*dz*fP[6]/(dl*dl2); J[3][3] = dx/dl;
431  J[4][0] = -dx*dy*fP[6]/(dl*dl2); J[4][1] = fP[6]/dl * (1.f - dy*dy/dl2); J[4][2] = -dy*dz*fP[6]/(dl*dl2); J[4][3] = dy/dl;
432  J[5][0] = -dx*dz*fP[6]/(dl*dl2); J[5][1] = -dy*dz*fP[6]/(dl*dl2); J[5][2] = fP[6]/dl * (1.f - dz*dz/dl2); J[5][3] = dz/dl;
433 
434  float_v VJT[4][7]; // V*J^T
435  for(Int_t i=0; i<4; i++)
436  {
437  for(Int_t j=0; j<7; j++)
438  {
439  VJT[i][j] = 0.f;
440  for(Int_t k=0; k<4; k++)
441  VJT[i][j] += V[IJ(i,k)]*J[j][k];
442  }
443  }
444  //Calculate the covariance matrix of the particle fC
445  for( Int_t i=0; i<36; i++ ) fC[i] = 0.f;
446 
447  for(Int_t i=0; i<7; ++i)
448  for(Int_t j=0; j<=i; ++j)
449  for(Int_t l=0; l<4; l++)
450  fC[IJ(i,j)]+= J[i][l]*VJT[l][j];
451  fC[35] = 1.f;
452 
453  fQ = int_v(Vc::Zero);
454  fNDF = 0;
455  fChi2 = 0;
456 
457  SumDaughterMass = float_v(Vc::Zero);
458  fMassHypo = float_v(Vc::Zero);
459 }
460 
461 KFParticleSIMD::KFParticleSIMD( const KFPVertex &vertex ): KFParticleBaseSIMD()
462 #ifdef NonhomogeneousField
463 , fField()
464 #endif
465 {
470  Double_t r[3];
471  Double_t C[21];
472 
473  vertex.GetXYZ( r );
474  for(Int_t i=0; i<3; i++)
475  fP[i] = r[i];
476  vertex.GetCovarianceMatrix( C );
477  for(Int_t i=0; i<21; i++)
478  fC[i] = C[i];
479  fChi2 = vertex.GetChi2();
480  fNDF = 2*vertex.GetNContributors() - 3;
481  fQ = int_v(Vc::Zero);
482  fAtProductionVertex = 0;
483  fSFromDecay = 0;
484 }
485 
486 void KFParticleSIMD::SetOneEntry(int iEntry, KFParticleSIMD& part, int iEntryPart)
487 {
494  for( int i = 0; i < 7; ++i )
495  fP[i][iEntry] = part.Parameters()[i][iEntryPart];
496  for( int i = 0; i < 36; ++i )
497  fC[i][iEntry] = part.CovarianceMatrix()[i][iEntryPart];
498 
499  fQ[iEntry] = part.Q()[iEntryPart];
500  fNDF[iEntry] = part.NDF()[iEntryPart];
501  fChi2[iEntry] = part.Chi2()[iEntryPart];
502 
503 // fSFromDecay[iEntry] = part.fSFromDecay[iEntryPart];
504 // SumDaughterMass[iEntry] = part.SumDaughterMass[iEntryPart];
505 // fMassHypo[iEntry] = part.fMassHypo[iEntryPart];
506 
507  fId[iEntry] = part.Id()[iEntryPart];
508 
509  fPDG[iEntry] = part.GetPDG()[iEntryPart];
510 
511  if(iEntry==0)
512  fDaughterIds.resize( part.NDaughters(), int_v(-1) );
513 
514  for(int iD=0; iD<part.NDaughters(); iD++)
515  fDaughterIds[iD][iEntry] = part.fDaughterIds[iD][iEntryPart];
516 
517 #ifdef NonhomogeneousField
518  fField.SetOneEntry( iEntry, part.fField, iEntryPart ); //CHECKME
519 #endif
520 }
521 
522 KFParticleSIMD::KFParticleSIMD(KFParticle* parts[], const int nPart): KFParticleBaseSIMD()
523 #ifdef NonhomogeneousField
524 , fField()
525 #endif
526 {
532  { // check
533  bool ok = 1;
534 
535  const int nD = (parts[0])->NDaughters();
536  for ( int ie = 1; ie < nPart; ie++ ) {
537  const KFParticle &part = *(parts[ie]);
538  ok &= part.NDaughters() == nD;
539  }
540  if (!ok) {
541  std::cout << " void CbmKFParticle_simd::Create(CbmKFParticle *parts[], int N) " << std::endl;
542  exit(1);
543  }
544  }
545  fDaughterIds.resize( (parts[0])->NDaughters(), int_v(-1) );
546 
547  for ( int iPart = 0; iPart < float_vLen; iPart++ ) {
548  Int_t iEntry = (iPart < nPart) ? iPart : 0;
549  KFParticle &part = *(parts[iEntry]);
550 
551  fId[iEntry] = part.Id();
552  for(int iD=0; iD<part.NDaughters(); iD++)
553  fDaughterIds[iD][iEntry] = part.DaughterIds()[iD];
554 
555  fPDG[iEntry] = part.GetPDG();
556 
557  for( int i = 0; i < 8; ++i )
558  fP[i][iEntry] = part.Parameters()[i];
559  for( int i = 0; i < 36; ++i )
560  fC[i][iEntry] = part.CovarianceMatrix()[i];
561 
562  fNDF[iEntry] = part.GetNDF();
563  fChi2[iEntry] = part.GetChi2();
564  fQ[iEntry] = part.GetQ();
565  fAtProductionVertex = part.GetAtProductionVertex(); // CHECKME
566 #ifdef NonhomogeneousField
567  fField.SetOneEntry( part.GetFieldCoeff(), iEntry);
568 #endif
569  }
570 }
571 
572 KFParticleSIMD::KFParticleSIMD( KFParticle &part): KFParticleBaseSIMD()
573 #ifdef NonhomogeneousField
574 , fField()
575 #endif
576 {
581  fId = part.Id();
582  fNDF = part.GetNDF();
583  fChi2 = part.GetChi2();
584  fQ = part.GetQ();
585  fPDG = part.GetPDG();
586  fAtProductionVertex = part.GetAtProductionVertex();
587 
588  SetNDaughters(part.NDaughters());
589  for( int i = 0; i < part.NDaughters(); ++i ) {
590  fDaughterIds.push_back( part.DaughterIds()[i] );
591  }
592 
593  for( int i = 0; i < 8; ++i )
594  fP[i] = part.Parameters()[i];
595  for( int i = 0; i < 36; ++i )
596  fC[i] = part.CovarianceMatrix()[i];
597 
598 #ifdef NonhomogeneousField
599  fField = KFParticleFieldRegion(part.GetFieldCoeff());
600 #endif
601 }
602 
603 float_m KFParticleSIMD::GetDistanceFromVertexXY( const float_v vtx[], const float_v Cv[], float_v &val, float_v &err ) const
604 {
614  float_v mP[8];
615  float_v mC[36];
616 
617  float_v dsdr[6] = {0.f,0.f,0.f,0.f,0.f,0.f};
618  const float_v dS = GetDStoPoint(vtx, dsdr);
619  Transport( dS, dsdr, mP, mC );
620 
621  float_v dx = mP[0] - vtx[0];
622  float_v dy = mP[1] - vtx[1];
623  float_v px = mP[3];
624  float_v py = mP[4];
625  float_v pt = sqrt(px*px + py*py);
626  float_v ex(Vc::Zero), ey(Vc::Zero);
627  float_m mask = ( pt < float_v(1.e-4) );
628 
629  pt(mask) = float_v(1.f);
630  ex(!mask) = (px/pt);
631  ey(!mask) = (py/pt);
632  val(mask) = float_v(1.e4);
633  val(!mask)= dy*ex - dx*ey;
634 
635  float_v h0 = -ey;
636  float_v h1 = ex;
637  float_v h3 = (dy*ey + dx*ex)*ey/pt;
638  float_v h4 = -(dy*ey + dx*ex)*ex/pt;
639 
640  err =
641  h0*(h0*GetCovariance(0,0) + h1*GetCovariance(0,1) + h3*GetCovariance(0,3) + h4*GetCovariance(0,4) ) +
642  h1*(h0*GetCovariance(1,0) + h1*GetCovariance(1,1) + h3*GetCovariance(1,3) + h4*GetCovariance(1,4) ) +
643  h3*(h0*GetCovariance(3,0) + h1*GetCovariance(3,1) + h3*GetCovariance(3,3) + h4*GetCovariance(3,4) ) +
644  h4*(h0*GetCovariance(4,0) + h1*GetCovariance(4,1) + h3*GetCovariance(4,3) + h4*GetCovariance(4,4) );
645 
646  if( Cv ){
647  err+= h0*(h0*Cv[0] + h1*Cv[1] ) + h1*(h0*Cv[1] + h1*Cv[2] );
648  }
649 
650  err = sqrt(abs(err));
651 
652  return mask;
653 }
654 
655 float_m KFParticleSIMD::GetDistanceFromVertexXY( const float_v vtx[], float_v &val, float_v &err ) const
656 {
663  return GetDistanceFromVertexXY( vtx, 0, val, err );
664 }
665 
666 
667 float_m KFParticleSIMD::GetDistanceFromVertexXY( const KFParticleSIMD &Vtx, float_v &val, float_v &err ) const
668 {
676  return GetDistanceFromVertexXY( Vtx.fP, Vtx.fC, val, err );
677 }
678 
679 #ifdef HomogeneousField
680 float_m KFParticleSIMD::GetDistanceFromVertexXY( const KFPVertex &Vtx, float_v &val, float_v &err ) const
681 {
689  return GetDistanceFromVertexXY( KFParticleSIMD(Vtx), val, err );
690 }
691 #endif
692 
693 float_v KFParticleSIMD::GetDistanceFromVertexXY( const float_v vtx[] ) const
694 {
699  float_v val, err;
700  GetDistanceFromVertexXY( vtx, 0, val, err );
701  return val;
702 }
703 
704 float_v KFParticleSIMD::GetDistanceFromVertexXY( const KFParticleSIMD &Vtx ) const
705 {
710  return GetDistanceFromVertexXY( Vtx.fP );
711 }
712 
713 #ifdef HomogeneousField
714 float_v KFParticleSIMD::GetDistanceFromVertexXY( const KFPVertex &Vtx ) const
715 {
720  return GetDistanceFromVertexXY( KFParticleSIMD(Vtx).fP );
721 }
722 #endif
723 
724 float_v KFParticleSIMD::GetDistanceFromParticleXY( const KFParticleSIMD &p ) const
725 {
730  float_v dS[2];
731  float_v dsdr[4][6];
732  GetDStoParticle( p, dS, dsdr );
733  float_v mP[8], mC[36], mP1[8], mC1[36];
734  Transport( dS[0], dsdr[0], mP, mC );
735  p.Transport( dS[1], dsdr[3], mP1, mC1 );
736  float_v dx = mP[0]-mP1[0];
737  float_v dy = mP[1]-mP1[1];
738  return sqrt(dx*dx+dy*dy);
739 }
740 
741 float_v KFParticleSIMD::GetDeviationFromParticleXY( const KFParticleSIMD &p ) const
742 {
747  float_v ds[2] = {0.f,0.f};
748  float_v dsdr[4][6];
749  float_v F1[36], F2[36], F3[36], F4[36];
750  for(int i1=0; i1<36; i1++)
751  {
752  F1[i1] = 0;
753  F2[i1] = 0;
754  F3[i1] = 0;
755  F4[i1] = 0;
756  }
757  GetDStoParticle( p, ds, dsdr );
758 
759  float_v V0Tmp[36] ;
760  float_v V1Tmp[36] ;
761 
762 
763  float_v mP1[8], mC1[36];
764  float_v mP2[8], mC2[36];
765 
766  Transport(ds[0], dsdr[0], mP1, mC1, dsdr[1], F1, F2);
767  p.Transport(ds[1], dsdr[3], mP2, mC2, dsdr[2], F4, F3);
768 
769  MultQSQt(F2, p.fC, V0Tmp, 6);
770  MultQSQt(F3, fC, V1Tmp, 6);
771 
772  for(int iC=0; iC<3; iC++)
773  mC1[iC] += V0Tmp[iC] + mC2[iC] + V1Tmp[iC];
774 
775  float_v d[3]={ mP2[0]-mP1[0], mP2[1]-mP1[1], mP2[2]-mP1[2]};
776 
777  return ( ( mC1[0]*d[0] + mC1[1]*d[1])*d[0]
778  +(mC1[1]*d[0] + mC1[2]*d[1])*d[1] );
779 }
780 
781 
782 float_v KFParticleSIMD::GetDeviationFromVertexXY( const float_v vtx[], const float_v Cv[] ) const
783 {
789  float_v mP[8];
790  float_v mC[36];
791  float_v dsdr[6] = {0.f,0.f,0.f,0.f,0.f,0.f};
792  const float_v dS = GetDStoPoint(vtx, dsdr);
793  float_v dsdp[6] = {-dsdr[0], -dsdr[1], -dsdr[2], 0.f, 0.f, 0.f};
794  float_v F[36], F1[36];
795  for(int i2=0; i2<36; i2++)
796  {
797  F[i2] = 0.f;
798  F1[i2] = 0.f;
799  }
800  Transport( dS, dsdr, mP, mC, dsdp, F, F1 );
801 
802  if(Cv)
803  {
804  float_v VFT[3][6];
805  for(int i=0; i<3; i++)
806  for(int j=0; j<6; j++)
807  {
808  VFT[i][j] = 0;
809  for(int k=0; k<3; k++)
810  {
811  VFT[i][j] += Cv[IJ(i,k)] * F1[j*6+k];
812  }
813  }
814 
815  float_v FVFT[6][6];
816  for(int i=0; i<6; i++)
817  for(int j=0; j<6; j++)
818  {
819  FVFT[i][j] = 0;
820  for(int k=0; k<3; k++)
821  {
822  FVFT[i][j] += F1[i*6+k] * VFT[k][j];
823  }
824  }
825  mC[0] += FVFT[0][0] + Cv[0];
826  mC[1] += FVFT[1][0] + Cv[1];
827  mC[2] += FVFT[1][1] + Cv[2];
828  mC[3] += FVFT[2][0] + Cv[3];
829  mC[4] += FVFT[2][1] + Cv[4];
830  mC[5] += FVFT[2][2] + Cv[5];
831  }
832 
833  InvertCholetsky3(mC);
834 
835  float_v d[3]={ vtx[0]-mP[0], vtx[1]-mP[1], vtx[2]-mP[2]};
836 
837  return ( ( mC[0]*d[0] + mC[1]*d[1] )*d[0]
838  +(mC[1]*d[0] + mC[2]*d[1] )*d[1] );
839 }
840 
841 
842 float_v KFParticleSIMD::GetDeviationFromVertexXY( const KFParticleSIMD &Vtx ) const
843 {
848  return GetDeviationFromVertexXY( Vtx.fP, Vtx.fC );
849 }
850 
851 #ifdef HomogeneousField
852 float_v KFParticleSIMD::GetDeviationFromVertexXY( const KFPVertex &Vtx ) const
853 {
858  KFParticleSIMD v(Vtx);
859  return GetDeviationFromVertexXY( v.fP, v.fC );
860 }
861 #endif
862 
863 float_v KFParticleSIMD::GetAngle ( const KFParticleSIMD &p ) const
864 {
869  float_v ds[2] = {0.f,0.f};
870  float_v dsdr[4][6];
871  GetDStoParticle( p, ds, dsdr );
872  float_v mP[8], mC[36], mP1[8], mC1[36];
873  Transport( ds[0], dsdr[0], mP, mC );
874  p.Transport( ds[1], dsdr[3], mP1, mC1 );
875  float_v n = sqrt( mP[3]*mP[3] + mP[4]*mP[4] + mP[5]*mP[5] );
876  float_v n1= sqrt( mP1[3]*mP1[3] + mP1[4]*mP1[4] + mP1[5]*mP1[5] );
877  n*=n1;
878  float_v a(Vc::Zero);
879  float_m mask = (n>(1.e-8f));
880  a(mask) = ( mP[3]*mP1[3] + mP[4]*mP1[4] + mP[5]*mP1[5] )/n;
881  mask = ( abs(a) < float_v(1.f));
882  float_m aPos = (a>=float_v(Vc::Zero));
883  a(mask) = KFPMath::ACos(a);
884  a((!mask) && aPos) = float_v(Vc::Zero);
885  a((!mask) && (!aPos)) = 3.1415926535f;
886  return a;
887 }
888 
889 float_v KFParticleSIMD::GetAngleXY( const KFParticleSIMD &p ) const
890 {
895  float_v ds[2] = {0.f,0.f};
896  float_v dsdr[4][6];
897  GetDStoParticle( p, ds, dsdr );
898  float_v mP[8], mC[36], mP1[8], mC1[36];
899  Transport( ds[0], dsdr[0], mP, mC );
900  p.Transport( ds[1], dsdr[3], mP1, mC1 );
901  float_v n = sqrt( mP[3]*mP[3] + mP[4]*mP[4] );
902  float_v n1= sqrt( mP1[3]*mP1[3] + mP1[4]*mP1[4] );
903  n*=n1;
904  float_v a(Vc::Zero);
905  float_m mask = (n>(1.e-8f));
906  a = ( mP[3]*mP1[3] + mP[4]*mP1[4] )/n;
907  a(!mask) = 0.f;
908  mask = ( abs(a) < float_v(1.f));
909  float_m aPos = (a>=float_v(Vc::Zero));
910  a(mask) = KFPMath::ACos(a);
911  a((!mask) && aPos) = float_v(Vc::Zero);
912  a((!mask) && (!aPos)) = 3.1415926535f;
913  return a;
914 }
915 
916 float_v KFParticleSIMD::GetAngleRZ( const KFParticleSIMD &p ) const
917 {
922  float_v ds[2] = {0.f,0.f};
923  float_v dsdr[4][6];
924  GetDStoParticle( p, ds, dsdr );
925  float_v mP[8], mC[36], mP1[8], mC1[36];
926  Transport( ds[0], dsdr[0], mP, mC );
927  p.Transport( ds[1], dsdr[3], mP1, mC1 );
928  float_v nr = sqrt( mP[3]*mP[3] + mP[4]*mP[4] );
929  float_v n1r= sqrt( mP1[3]*mP1[3] + mP1[4]*mP1[4] );
930  float_v n = sqrt( nr*nr + mP[5]*mP[5] );
931  float_v n1= sqrt( n1r*n1r + mP1[5]*mP1[5] );
932  n*=n1;
933  float_v a(Vc::Zero);
934  float_m mask = (n>(1.e-8f));
935  a(mask) = ( nr*n1r +mP[5]*mP1[5])/n;
936  mask = ( abs(a) < float_v(Vc::Zero));
937  float_m aPos = (a>=float_v(Vc::Zero));
938  a(mask) = KFPMath::ACos(a);
939  a((!mask) && aPos) = float_v(Vc::Zero);
940  a((!mask) && (!aPos)) = 3.1415926535f;
941  return a;
942 }
943 
944 float_v KFParticleSIMD::GetPseudoProperDecayTime( const KFParticleSIMD &pV, const float_v& mass, float_v* timeErr2 ) const
945 {
953  const float_v ipt2 = 1/( Px()*Px() + Py()*Py() );
954  const float_v mipt2 = mass*ipt2;
955  const float_v dx = X() - pV.X();
956  const float_v dy = Y() - pV.Y();
957 
958  if ( timeErr2 ) {
959  // -- calculate error = sigma(f(r)) = f'Cf'
960  // r = {x,y,px,py,x_pV,y_pV}
961  // df/dr = { px*m/pt^2,
962  // py*m/pt^2,
963  // ( x - x_pV )*m*(1/pt^2 - 2(px/pt^2)^2),
964  // ( y - y_pV )*m*(1/pt^2 - 2(py/pt^2)^2),
965  // -px*m/pt^2,
966  // -py*m/pt^2 }
967  const float_v f0 = Px()*mipt2;
968  const float_v f1 = Py()*mipt2;
969  const float_v mipt2derivative = mipt2*(1-2*Px()*Px()*ipt2);
970  const float_v f2 = dx*mipt2derivative;
971  const float_v f3 = -dy*mipt2derivative;
972  const float_v f4 = -f0;
973  const float_v f5 = -f1;
974 
975  const float_v& mC00 = GetCovariance(0,0);
976  const float_v& mC10 = GetCovariance(0,1);
977  const float_v& mC11 = GetCovariance(1,1);
978  const float_v& mC20 = GetCovariance(3,0);
979  const float_v& mC21 = GetCovariance(3,1);
980  const float_v& mC22 = GetCovariance(3,3);
981  const float_v& mC30 = GetCovariance(4,0);
982  const float_v& mC31 = GetCovariance(4,1);
983  const float_v& mC32 = GetCovariance(4,3);
984  const float_v& mC33 = GetCovariance(4,4);
985  const float_v& mC44 = pV.GetCovariance(0,0);
986  const float_v& mC54 = pV.GetCovariance(1,0);
987  const float_v& mC55 = pV.GetCovariance(1,1);
988 
989  *timeErr2 =
990  f5*mC55*f5 +
991  f5*mC54*f4 +
992  f4*mC44*f4 +
993  f3*mC33*f3 +
994  f3*mC32*f2 +
995  f3*mC31*f1 +
996  f3*mC30*f0 +
997  f2*mC22*f2 +
998  f2*mC21*f1 +
999  f2*mC20*f0 +
1000  f1*mC11*f1 +
1001  f1*mC10*f0 +
1002  f0*mC00*f0;
1003  }
1004  return ( dx*Px() + dy*Py() )*mipt2;
1005 }
1006 
1007 void KFParticleSIMD::GetKFParticle(KFParticle &Part, int iPart)
1008 {
1014  Part.SetId(static_cast<int>(Id()[iPart]));
1015 
1016  Part.CleanDaughtersId();
1017  for( unsigned int i = 0; i < DaughterIds().size(); i++ )
1018  Part.AddDaughterId(static_cast<int>(DaughterIds()[i][iPart]));
1019 
1020  Part.SetPDG( static_cast<int>(GetPDG()[iPart]) );
1021 
1022  for(int iP=0; iP<8; iP++)
1023  Part.Parameters()[iP] = Parameters()[iP][iPart];
1024  for(int iC=0; iC<36; iC++)
1025  Part.CovarianceMatrix()[iC] = CovarianceMatrix()[iC][iPart];
1026 
1027  Part.NDF() = static_cast<int>(GetNDF()[iPart]);
1028  Part.Chi2() = GetChi2()[iPart];
1029  Part.Q() = GetQ()[iPart];
1030  Part.SetAtProductionVertex(fAtProductionVertex);
1031 #ifdef NonhomogeneousField
1032  for(int iF=0; iF<10; iF++)
1033  Part.SetFieldCoeff(fField.fField[iF][iPart], iF);
1034 #endif
1035 }
1036 
1037 void KFParticleSIMD::GetKFParticle(KFParticle* Part, int nPart)
1038 {
1044  for(int i=0; i<nPart; i++)
1045  GetKFParticle(Part[i],i);
1046 }