277 assert(
dynamic_cast<State*
> (&istate)!=
nullptr);
279 CascadeState& cstate = *state.m_cascadeState;
282 std::vector< Vect3DF > cVertices;
283 std::vector< std::vector<double> > covVertices;
284 std::vector< std::vector< VectMOM> > fittedParticles;
285 std::vector< std::vector<double> > fittedCovariance;
286 std::vector<double> fitFullCovariance;
287 std::vector<double> particleChi2;
291 std::vector<const TrackParameters*> baseInpTrk;
293 std::vector<const xAOD::TrackParticle*>::const_iterator i_ntrk;
295 unsigned int indexFMP;
296 for (i_ntrk = cstate.m_partListForCascade.begin(); i_ntrk < cstate.m_partListForCascade.end(); ++i_ntrk) {
298 ATH_MSG_DEBUG(
"FirstMeasuredPoint on track is discovered. Use it.");
301 ATH_MSG_DEBUG(
"No FirstMeasuredPoint on track in CascadeFitter. Stop fit");
314 std::vector< std::vector<int> > vertexDefinition;
315 std::vector< std::vector<int> > cascadeDefinition;
318 double * partMass=
new double[ntrk];
319 for(
int i=0;
i<ntrk;
i++) partMass[
i] = cstate.m_partMassForCascade[
i];
320 int IERR =
makeCascade(state.m_vkalFitControl, ntrk, state.m_ich, partMass, &state.m_apar[0][0], &state.m_awgt[0][0],
324 CascadeEvent & refCascadeEvent=*(state.m_vkalFitControl.getCascadeEvent());
328 std::vector<int> indexT,indexV,indexTT,indexVV,tmpInd;
329 for (
const PartialMassConstraint&
c : cstate.m_partMassCnstForCascade) {
334 for (
int idx :
c.pseudoInVrt)
353 double vertex[3] = {primVrt->position().x()-state.m_refFrameX, primVrt->position().y()-state.m_refFrameY,primVrt->position().z()-state.m_refFrameZ};
354 const RecVertex* primVrtRec=
dynamic_cast< const RecVertex*
> (primVrt);
356 double covari[6] = {primVrtRec->covariancePosition()(0,0),primVrtRec->covariancePosition()(0,1),
357 primVrtRec->covariancePosition()(1,1),primVrtRec->covariancePosition()(0,2),
358 primVrtRec->covariancePosition()(1,2),primVrtRec->covariancePosition()(2,2)};
368 getFittedCascade(refCascadeEvent, cVertices, covVertices, fittedParticles, fittedCovariance, particleChi2, fitFullCovariance );
381 int ip,ivFrom=0,ivTo;
383 for( ivTo=0; ivTo<(
int)vertexDefinition.size(); ivTo++){
384 if(cascadeDefinition[ivTo].
empty())
continue;
386 ivFrom=cascadeDefinition[ivTo][
ip];
389 px += fittedParticles[ivFrom][
it].Px;
390 py += fittedParticles[ivFrom][
it].Py;
391 pz += fittedParticles[ivFrom][
it].Pz;
393 Sign= (cVertices[ivFrom].X-cVertices[ivTo].X)*
px
394 +(cVertices[ivFrom].
Y-cVertices[ivTo].
Y)*
py
395 +(cVertices[ivFrom].Z-cVertices[ivTo].Z)*
pz;
406 std::vector< std::vector<int> > new_vertexDefinition;
407 std::vector< std::vector<int> > new_cascadeDefinition;
408 cstate.m_cascadeVList[ivFrom].mergedTO=cstate.m_cascadeVList[ivTo].vID;
409 cstate.m_cascadeVList[ivTo].mergedIN.push_back(ivFrom);
416 state.m_vkalFitControl.renewCascadeEvent(
new CascadeEvent());
417 partMass=
new double[ntrk];
418 for(
int i=0;
i<ntrk;
i++) partMass[
i] = cstate.m_partMassForCascade[
i];
419 int IERR =
makeCascade(state.m_vkalFitControl, ntrk, state.m_ich, partMass, &state.m_apar[0][0], &state.m_awgt[0][0],
420 new_vertexDefinition,
421 new_cascadeDefinition);
delete[] partMass;
if(IERR){
CLEANCASCADE();
return nullptr;}
423 for (
const PartialMassConstraint&
c : cstate.m_partMassCnstForCascade) {
424 indexT.clear(); indexV.clear();
428 int icv=
indexInV(inV, cstate);
if(icv<0)
break;
429 if(cstate.m_cascadeVList[icv].mergedTO ==
c.VRT){
430 IERR =
findPositions(cstate.m_cascadeVList[icv].trkInVrt, new_vertexDefinition[
index], indexTT);
432 indexT.insert (indexT.end(), indexTT.begin(), indexTT.end());
434 std::vector<int> tmpI(1); tmpI[0]=inV;
437 indexV.insert (indexV.end(), indexVV.begin(), indexVV.end());
444 ATH_MSG_DEBUG(
"Setting compressed mass constraints ierr="<<IERR);
450 double vertex[3] = {primVrt->position().x()-state.m_refFrameX, primVrt->position().y()-state.m_refFrameY,primVrt->position().z()-state.m_refFrameZ};
451 const RecVertex* primVrtRec=
dynamic_cast< const RecVertex*
> (primVrt);
453 double covari[6] = {primVrtRec->covariancePosition()(0,0),primVrtRec->covariancePosition()(0,1),
454 primVrtRec->covariancePosition()(1,1),primVrtRec->covariancePosition()(0,2),
455 primVrtRec->covariancePosition()(1,2),primVrtRec->covariancePosition()(2,2)};
461 IERR =
processCascade(*(state.m_vkalFitControl.getCascadeEvent()));
468 std::vector< Vect3DF > t_cVertices;
469 std::vector< std::vector<double> > t_covVertices;
470 std::vector< std::vector< VectMOM> > t_fittedParticles;
471 std::vector< std::vector<double> > t_fittedCovariance;
472 std::vector<double> t_fitFullCovariance;
473 getFittedCascade(*(state.m_vkalFitControl.getCascadeEvent()), t_cVertices, t_covVertices, t_fittedParticles,
474 t_fittedCovariance, particleChi2, t_fitFullCovariance);
475 cVertices.clear(); covVertices.clear();
481 for(
int kv=0; kv<(
int)fittedParticles.size(); kv++){
484 " Px="<<fittedParticles[kv][
kt].Px<<
" Py="<<fittedParticles[kv][
kt].Py<<
";";
488 for(
int kv=0; kv<(
int)t_fittedParticles.size(); kv++){
491 " Px="<<t_fittedParticles[kv][
kt].Px<<
" Py="<<t_fittedParticles[kv][
kt].Py<<
";";
495 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
497 cVertices.push_back(t_cVertices[
index]);
498 covVertices.push_back(t_covVertices[
index]);
499 for(
it=0;
it<(
int)cstate.m_cascadeVList[iv].trkInVrt.size();
it++){
500 int numTrk=cstate.m_cascadeVList[iv].trkInVrt[
it];
501 for(itmp=0; itmp<(
int)new_vertexDefinition[
index].
size(); itmp++)
if(numTrk==new_vertexDefinition[
index][itmp])
break;
502 fittedParticles[iv][
it]=t_fittedParticles[
index][itmp];
521 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
523 int NTrkInVrt=cstate.m_cascadeVList[iv].trkInVrt.size();
524 for(
ip=0;
ip<(
int)cstate.m_cascadeVList[iv].inPointingV.size();
ip++){
525 int tmpIndexV=
indexInV( cstate.m_cascadeVList[iv].inPointingV[
ip], cstate);
526 if(cstate.m_cascadeVList[tmpIndexV].mergedTO){
527 tmpMom.Px=tmpMom.Py=tmpMom.Pz=tmpMom.E=0.;
528 for(
it=0;
it<(
int)(cstate.m_cascadeVList[tmpIndexV].trkInVrt.size()+
529 cstate.m_cascadeVList[tmpIndexV].inPointingV.size());
it++){
530 tmpMom.Px += fittedParticles[tmpIndexV][
it].Px; tmpMom.Py += fittedParticles[tmpIndexV][
it].Py;
531 tmpMom.Pz += fittedParticles[tmpIndexV][
it].Pz; tmpMom.E += fittedParticles[tmpIndexV][
it].E;
533 fittedParticles[iv][
ip+NTrkInVrt]=tmpMom;
536 for(itmp=0; itmp<(
int)new_cascadeDefinition[
index].
size(); itmp++)
if(indexS==new_cascadeDefinition[
index][itmp])
break;
537 fittedParticles[iv][
ip+NTrkInVrt]=t_fittedParticles[
index][itmp+new_vertexDefinition[
index].size()];
543 for(
int kv=0; kv<(
int)fittedParticles.size(); kv++){
546 " Px="<<fittedParticles[kv][
kt].Px<<
" Py="<<fittedParticles[kv][
kt].Py<<
";";
553 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
555 if(cstate.m_cascadeVList[iv].mergedTO)
isMerged=
true;
557 for(
ip=0;
ip<(
int)cstate.m_cascadeVList[iv].inPointingV.size();
ip++){
558 int tmpIndexV=
indexInV( cstate.m_cascadeVList[iv].inPointingV[
ip], cstate);
559 if(cstate.m_cascadeVList[tmpIndexV].mergedTO)
isMerged=
true;
562 fittedCovariance[iv]=t_fittedCovariance[
index];
572 std::vector<xAOD::Vertex*> xaodVrtList(0);
575 int NDOF=
getCascadeNDoF(cstate);
if(NDOFsqueezed) NDOF=NDOFsqueezed;
576 if(primVrt){
if(FirstDecayAtPV){ NDOF+=3; }
else{ NDOF+=2; } }
578 for(iv=0; iv<(
int)cVertices.size(); iv++){
579 Amg::Vector3D FitVertex(cVertices[iv].
X+state.m_refFrameX,cVertices[iv].Y+state.m_refFrameY,cVertices[iv].Z+state.m_refFrameZ);
580 VrtCovMtx(0,0) = covVertices[iv][0]; VrtCovMtx(0,1) = covVertices[iv][1];
581 VrtCovMtx(1,1) = covVertices[iv][2]; VrtCovMtx(0,2) = covVertices[iv][3];
582 VrtCovMtx(1,2) = covVertices[iv][4]; VrtCovMtx(2,2) = covVertices[iv][5];
583 VrtCovMtx(1,0) = VrtCovMtx(0,1);
584 VrtCovMtx(2,0) = VrtCovMtx(0,2);
585 VrtCovMtx(2,1) = VrtCovMtx(1,2);
587 for(
it=0;
it<(
int)vertexDefinition[iv].
size();
it++) { Chi2 += particleChi2[vertexDefinition[iv][
it]];};
595 std::vector<VxTrackAtVertex> & tmpVTAV=tmpXAODVertex->
vxTrackAtVertex();
598 int NRealT=vertexDefinition[iv].size();
600 for(
it=0;
it<NRealT*3+3;
it++){
601 for( jt=0; jt<=
it; jt++){
602 genCOV(
it,jt) = genCOV(jt,
it) = fittedCovariance[iv][
it*(
it+1)/2+jt];
608 fullDeriv(0,0)=fullDeriv(1,1)=fullDeriv(2,2)=1.;
610 for(
it=0;
it<NRealT;
it++) {
611 mom= sqrt( fittedParticles[iv][
it].Pz*fittedParticles[iv][
it].Pz
612 +fittedParticles[iv][
it].Py*fittedParticles[iv][
it].Py
613 +fittedParticles[iv][
it].Px*fittedParticles[iv][
it].Px);
614 double Px=fittedParticles[iv][
it].Px;
615 double Py=fittedParticles[iv][
it].Py;
616 double Pz=fittedParticles[iv][
it].Pz;
617 double Pt= sqrt(Px*Px + Py*Py) ;
620 invP = - state.m_ich[vertexDefinition[iv][
it]] /
mom;
624 tmpDeriv(0,1) = -
sin(
phi);
629 tmpDeriv(2+0,3*
it+3+0) = -Py/
Pt/
Pt;
630 tmpDeriv(2+0,3*
it+3+1) = Px/
Pt/
Pt;
631 tmpDeriv(2+0,3*
it+3+2) = 0;
635 tmpDeriv(2+2,3*
it+3+0) = -Px/(
mom*
mom) * invP;
636 tmpDeriv(2+2,3*
it+3+1) = -Py/(
mom*
mom) * invP;
637 tmpDeriv(2+2,3*
it+3+2) = -Pz/(
mom*
mom) * invP;
643 measPerigee =
new Perigee( 0.,0.,
phi,
theta, invP, PerigeeSurface(FitVertex), std::move(tmpCovMtx) );
644 tmpVTAV.emplace_back( particleChi2[vertexDefinition[iv][
it]] , measPerigee ) ;
646 std::
vector<
float> floatErrMtx;
649 tmpCovMtx=genCOV.similarity(fullDeriv);
650 floatErrMtx.resize((NRealT*3+3)*(NRealT*3+3+1)/2);
652 for(
int i=0;
i<NRealT*3+3;
i++){
653 for(
int j=0;j<=
i;j++){
654 floatErrMtx.at(ivk++)=tmpCovMtx(
i,j);
658 floatErrMtx.resize(6);
659 for(
int i=0;
i<6;
i++) floatErrMtx[
i]=covVertices[iv][
i];
662 for(
int itvk=0; itvk<NRealT; itvk++) {
664 if(itvk < (
int)cstate.m_cascadeVList[iv].trkInVrt.size()){
665 TEL.
setElement( cstate.m_partListForCascade[ cstate.m_cascadeVList[iv].trkInVrt[itvk] ] );
671 xaodVrtList.push_back(tmpXAODVertex);
677 std::vector<TLorentzVector> tmpMoms;
678 std::vector<std::vector<TLorentzVector> > particleMoms;
679 std::vector<Amg::MatrixX> particleCovs;
681 for(iv=0; iv<(
int)cVertices.size(); iv++){
683 int NTrkF=fittedParticles[iv].size();
684 for(
it=0;
it< NTrkF;
it++) {
685 tmpMoms.emplace_back( fittedParticles[iv][
it].Px, fittedParticles[iv][
it].Py,
686 fittedParticles[iv][
it].Pz, fittedParticles[iv][
it].
E );
690 for(
it=0;
it<NTrkF*3+3;
it++){
691 for( jt=0; jt<=
it; jt++){
692 COV(
it,jt) = COV(jt,
it) = fittedCovariance[iv][
it*(
it+1)/2+jt];
694 particleMoms.push_back( std::move(tmpMoms) );
695 particleCovs.push_back( std::move(COV) );
699 int NAPAR=(allFitPrt+cVertices.size())*3;
703 for(
it=0;
it<NAPAR;
it++){
704 for( jt=0; jt<=
it; jt++){
710 for(iv=0; iv<(
int)cstate.m_cascadeVList.size(); iv++){
712 FULL.block(mcount,mcount,particleCovs[iv].
rows(),particleCovs[iv].cols())=particleCovs[iv];
713 mcount += particleCovs[iv].rows();
719 VxCascadeInfo * recCascade=
new VxCascadeInfo(std::move(xaodVrtList),std::move(particleMoms),std::move(particleCovs), NDOF ,fullChi2);
720 recCascade->setFullCascadeCovariance(
FULL);