13 #include "GaudiKernel/IChronoStatSvc.h"
27 TLorentzVector& Momentum,
31 std::vector< std::vector<double> >& TrkAtVrt,
36 assert(
dynamic_cast<State*
> (&istate)!=
nullptr);
44 if(
sc.isFailure())
return StatusCode::FAILURE;
47 Chi2PerTrk, TrkAtVrt,Chi2, state, ifCovV0 ) ;
48 if (ierr)
return StatusCode::FAILURE;
49 return StatusCode::SUCCESS;
55 const std::vector<const xAOD::NeutralParticle*> & InpTrkN,
57 TLorentzVector& Momentum,
61 std::vector< std::vector<double> >& TrkAtVrt,
66 assert(
dynamic_cast<State*
> (&istate)!=
nullptr);
77 std::vector<const TrackParameters*> tmpInputC(0);
78 std::vector<std::unique_ptr<const TrackParameters>> TParamOwner(0);
80 double closestHitR=1.e6;
85 if(msgLvl(MSG::WARNING))
msg()<<
"No InDet extrapolator given."<<
86 "Can't use FirstMeasuredPoint with xAOD::TrackParticle!!!" <<
endmsg;
87 return StatusCode::FAILURE;
89 std::vector<const xAOD::TrackParticle*>::const_iterator i_ntrk;
90 if(msgLvl(
MSG::DEBUG))
msg()<<
"Start FirstMeasuredPoint handling"<<
'\n';
91 unsigned int indexFMP;
92 for (i_ntrk = InpTrkC.begin(); i_ntrk < InpTrkC.end(); ++i_ntrk) {
94 if(msgLvl(
MSG::DEBUG))
msg()<<
"FirstMeasuredPoint on track is discovered. Use it."<<
'\n';
96 TParamOwner.emplace_back(std::make_unique<CurvilinearParameters>(
97 (*i_ntrk)->curvilinearParameters(indexFMP)));
99 tmpInputC.push_back((TParamOwner.back()).get());
102 msg()<<
"FirstMeasuredPoint on track is absent."<<
103 "Try extrapolation from Perigee to FisrtMeasuredPoint radius"<<
endmsg;
108 tmpInputC.push_back((TParamOwner.back()).get());
109 if(tmpInputC[tmpInputC.size()-1]==
nullptr){
111 if(msgLvl(MSG::WARNING)){
112 msg()<<
"InDetExtrapolator can't etrapolate xAOD::TrackParticle Perigee "<<
113 "to FirstMeasuredPoint radius! Stop vertex fit!" <<
endmsg;
115 return StatusCode::FAILURE;
118 if( (*i_ntrk)->radiusOfFirstHit() < closestHitR ) {
119 closestHitR=(*i_ntrk)->radiusOfFirstHit();
124 return StatusCode::FAILURE;
128 if(!InpTrkC.empty()) {
132 if(
sc.isFailure())
return StatusCode::FAILURE;
133 if(!InpTrkN.empty()){
sc=
CvtNeutralParticle(InpTrkN,ntrk,state);
if(
sc.isFailure())
return StatusCode::FAILURE;}
135 int ierr =
VKalVrtFit3( ntrk,
Vertex, Momentum, Charge, ErrorMatrix, Chi2PerTrk, TrkAtVrt, Chi2, state, ifCovV0 ) ;
136 if (ierr)
return StatusCode::FAILURE;
139 state.m_planeCnstNDOF = 0;
142 if(closestHitR==1.e6){
144 for(
auto &trka : InpTrkC){
146 if(closestHitR>hitR){
151 if(closestHitR<1.e6){
153 if(perFMP) cnstRefPoint=perFMP->position();
158 if(state.m_globalFirstHit)cnstRefPoint=state.m_globalFirstHit->position();
160 if(
Vertex.perp()>closestHitR && cnstRefPoint.perp()>0.){
162 state.m_planeCnstNDOF = 1;
163 double D= unitMom.x()*(cnstRefPoint.x()-state.m_refFrameX)
164 +unitMom.y()*(cnstRefPoint.y()-state.m_refFrameY)
165 +unitMom.z()*(cnstRefPoint.z()-state.m_refFrameZ);
166 state.m_parPlaneCnst[0]=unitMom.x();
167 state.m_parPlaneCnst[1]=unitMom.y();
168 state.m_parPlaneCnst[2]=unitMom.z();
169 state.m_parPlaneCnst[3]=D;
170 state.m_cnstRadius=std::sqrt(
std::pow(cnstRefPoint.x()-state.m_refFrameX,2.)+
std::pow(cnstRefPoint.y()-state.m_refFrameY,2.));
171 state.m_cnstRadiusRef[0]=-state.m_refFrameX;
172 state.m_cnstRadiusRef[1]=-state.m_refFrameY;
173 std::vector<double> saveApproxV(3,0.); state.m_ApproximateVertex.swap(saveApproxV);
174 state.m_ApproximateVertex[0]=cnstRefPoint.x();
175 state.m_ApproximateVertex[1]=cnstRefPoint.y();
176 state.m_ApproximateVertex[2]=cnstRefPoint.z();
177 ierr =
VKalVrtFit3( ntrk,
Vertex, Momentum, Charge, ErrorMatrix, Chi2PerTrk, TrkAtVrt, Chi2, state, ifCovV0 );
178 state.m_vkalFitControl.setUsePlaneCnst(0.,0.,0.,0.);
180 ierr =
VKalVrtFit3(ntrk,
Vertex,Momentum,Charge,ErrorMatrix,Chi2PerTrk,TrkAtVrt,Chi2, state, ifCovV0);
181 state.m_planeCnstNDOF = 0;
183 state.m_ApproximateVertex.swap(saveApproxV);
187 if (ierr)
return StatusCode::FAILURE;
188 return StatusCode::SUCCESS;
193 const std::vector<const NeutralParameters*> & InpTrkN,
195 TLorentzVector& Momentum,
199 std::vector< std::vector<double> >& TrkAtVrt,
204 assert(
dynamic_cast<State*
> (&istate)!=
nullptr);
212 if(!InpTrkC.empty()){
214 if(
sc.isFailure())
return StatusCode::FAILURE;
216 if(!InpTrkN.empty()){
218 if(
sc.isFailure())
return StatusCode::FAILURE;
221 if(state.m_ApproximateVertex.empty() && state.m_globalFirstHit){
222 state.m_ApproximateVertex.reserve(3);
223 state.m_ApproximateVertex.push_back(state.m_globalFirstHit->position().x());
224 state.m_ApproximateVertex.push_back(state.m_globalFirstHit->position().y());
225 state.m_ApproximateVertex.push_back(state.m_globalFirstHit->position().z());
227 int ierr =
VKalVrtFit3( ntrk,
Vertex, Momentum, Charge, ErrorMatrix, Chi2PerTrk, TrkAtVrt,Chi2, state, ifCovV0 ) ;
228 if (ierr)
return StatusCode::FAILURE;
231 state.m_planeCnstNDOF = 0;
235 state.m_planeCnstNDOF = 1;
236 double pp[3]={Momentum.Px()/Momentum.Rho(),Momentum.Py()/Momentum.Rho(),Momentum.Pz()/Momentum.Rho()};
237 double D= pp[0]*(state.m_globalFirstHit->position().x()-state.m_refFrameX)
238 +pp[1]*(state.m_globalFirstHit->position().y()-state.m_refFrameY)
239 +pp[2]*(state.m_globalFirstHit->position().z()-state.m_refFrameZ);
240 state.m_vkalFitControl.setUsePlaneCnst( pp[0], pp[1], pp[2], D);
241 std::vector<double> saveApproxV(3,0.); state.m_ApproximateVertex.swap(saveApproxV);
242 state.m_ApproximateVertex[0]=state.m_globalFirstHit->position().x();
243 state.m_ApproximateVertex[1]=state.m_globalFirstHit->position().y();
244 state.m_ApproximateVertex[2]=state.m_globalFirstHit->position().z();
245 ierr =
VKalVrtFit3( ntrk,
Vertex, Momentum, Charge, ErrorMatrix, Chi2PerTrk, TrkAtVrt,Chi2, state, ifCovV0 ) ;
246 state.m_vkalFitControl.setUsePlaneCnst(0.,0.,0.,0.);
248 ierr =
VKalVrtFit3(ntrk,
Vertex,Momentum,Charge,ErrorMatrix,Chi2PerTrk,TrkAtVrt,Chi2, state, ifCovV0 ) ;
249 state.m_planeCnstNDOF = 0;
251 state.m_ApproximateVertex.swap(saveApproxV);
254 if (ierr)
return StatusCode::FAILURE;
255 return StatusCode::SUCCESS;
268 TLorentzVector& Momentum,
272 std::vector< std::vector<double> >& TrkAtVrt,
281 double xyz0[3],covf[21],chi2f=-10.;
283 double xyzfit[3]={0.};
307 xyz0[0]=xyz0[1]=xyz0[2]=0.;
312 Chi2PerTrk.resize (ntrk);
314 xyzfit, state.
m_parfs, ptot, covf, chi2f,
323 if(ptot[0]*ptot[0]+ptot[1]*ptot[1] == 0.)
return -5;
329 int SymCovMtxSize=(3*ntrk+3)*(3*ntrk+4)/2;
338 Momentum.SetPxPyPzE( ptot[0], ptot[1], ptot[2], ptot[3] );
347 if (
Vertex.perp() > sizeR || std::abs(
Vertex.z()) > sizeZ)
return -5;
355 double fx,fy,BMAG_CUR;
357 if(fabs(BMAG_CUR) < 0.01) BMAG_CUR=0.01;
359 Charge=0;
for(
i=0;
i<ntrk;
i++){Charge+=state.
m_ich[
i];};
363 TrkAtVrt.clear(); TrkAtVrt.reserve(ntrk);
364 for(
i=0;
i<ntrk;
i++){
365 std::vector<double> TrkPar(3);
367 TrkPar[0],TrkPar[1],TrkPar[2]);
368 TrkPar[2] = -TrkPar[2];
369 TrkAtVrt.push_back( TrkPar );
381 const TLorentzVector& Momentum,
382 const dvect& CovVrtMom,
383 const long int& Charge,
388 assert(
dynamic_cast<State*
> (&istate)!=
nullptr);
391 double Vrt[3],PMom[4],Cov0[21],Per[5],CovPer[15];
394 for(
i=0;
i<3;
i++) PMom[
i]=Momentum[
i];
395 for(ij=
i=0;
i<6;
i++){
397 Cov0[ij]=CovVrtMom[ij];
402 long int vkCharge=-Charge;
406 double fx,fy,BMAG_CUR;
408 if(fabs(BMAG_CUR) < 0.01) BMAG_CUR=0.01;
410 Trk::xyztrp( vkCharge, Vrt, PMom, Cov0, BMAG_CUR, Per, CovPer );
415 for(
i=0;
i<5;
i++)
Perigee.push_back((
double)Per[
i]);
416 for(
i=0;
i<15;
i++) CovPerigee.push_back((
double)CovPer[
i]);
418 return StatusCode::SUCCESS;
423 double& tp1,
double& tp2,
double& tp3)
const
427 tp3= vp3 *
std::sin( vp1 ) /(m_CNVMAG*curBMAG);
428 constexpr
double pi =
M_PI;
430 while ( tp1 >
pi) tp1 -= 2.*
pi;
431 while ( tp1 <-
pi) tp1 += 2.*
pi;
433 while ( tp2 >
pi) tp2 -= 2.*
pi;
434 while ( tp2 <-
pi) tp2 += 2.*
pi;
436 tp2 = fabs(tp2); tp1 +=
pi;
437 while ( tp1 >
pi) tp1 -= 2.*
pi;
454 assert(
dynamic_cast<State*
> (&istate)!=
nullptr);
457 if(NTrk<1)
return StatusCode::FAILURE;
458 if(NTrk>
NTrMaxVFit)
return StatusCode::FAILURE;
459 if(state.
m_ErrMtx.empty())
return StatusCode::FAILURE;
463 double fx,fy,BMAG_CUR;
465 if(fabs(BMAG_CUR) < 0.01) BMAG_CUR=0.01;
469 int i,j,ik,jk,
ip,iTrk;
471 std::vector<std::vector<double> > Deriv (DIM);
472 for (std::vector<double>&
v : Deriv)
v.resize (DIM);
473 std::vector<double> CovMtxOld(DIM*DIM);
476 CovVrtTrk.resize(DIM*(DIM+1)/2);
479 for(
i=0;
i<DIM;
i++) {
480 for( j=0; j<=
i; j++) {
481 CovMtxOld[
i*DIM+j]=CovMtxOld[j*DIM+
i]=state.
m_ErrMtx[
ip++];
487 for(
i=0;
i<DIM;
i++){
for(j=0;j<DIM;j++) {Deriv[
i][j]=0.;}}
494 for( iTrk=0; iTrk<NTrk; iTrk++){
502 Deriv[iSt ][iSt+1] = 1;
503 Deriv[iSt+1][iSt ] = 1;
511 Deriv[iSt ][iSt ]= 0;
512 Deriv[iSt ][iSt+1]= -
py;
513 Deriv[iSt ][iSt+2]= -
px/invR;
515 Deriv[iSt+1][iSt ]= 0;
516 Deriv[iSt+1][iSt+1]=
px;
517 Deriv[iSt+1][iSt+2]= -
py/invR;
520 Deriv[iSt+2][iSt+1]= 0;
521 Deriv[iSt+2][iSt+2]= -
pz/invR;
530 for(ik=0;ik<DIM;ik++){
531 if(Deriv[
i][ik] == 0.)
continue;
533 for(jk=DIM-1;jk>=0;jk--){
534 if(Deriv[j][jk] == 0.)
continue;
535 tmpTmp += CovMtxOld[ik*DIM+jk]*Deriv[j][jk];
537 tmp += Deriv[
i][ik]*tmpTmp;
539 CovVrtTrk[ipnt++]=
tmp;
542 return StatusCode::SUCCESS;
553 assert(
dynamic_cast<const State*
> (&istate)!=
nullptr);
554 const State& state =
static_cast<const State&
> (istate);
558 return StatusCode::SUCCESS;
565 assert(
dynamic_cast<const State*
> (&istate)!=
nullptr);
566 const State& state =
static_cast<const State&
> (istate);
574 return StatusCode::SUCCESS;