15# if defined( __EXTENSIONS__ )
18# define __EXTENSIONS__
22#elif defined( __GNUC__ )
23# if defined( _XOPEN_SOURCE )
36#include "CLHEP/Matrix/DiagMatrix.h"
37#include "CLHEP/Matrix/Matrix.h"
38#include "CLHEP/Matrix/SymMatrix.h"
39#include "CLHEP/Matrix/Vector.h"
40#include "MdcTables/MdcTables.h"
41#include "TrkReco/THelixFitter.h"
42#include "TrkReco/TTrack.h"
44#include "MdcCalibFunSvc/IMdcCalibFunSvc.h"
47#include "GaudiKernel/Bootstrap.h"
48#include "GaudiKernel/ISvcLocator.h"
50#include "GaudiKernel/Bootstrap.h"
51#include "GaudiKernel/IDataProviderSvc.h"
52#include "GaudiKernel/IInterface.h"
53#include "GaudiKernel/IMessageSvc.h"
54#include "GaudiKernel/ISvcLocator.h"
55#include "GaudiKernel/Kernel.h"
56#include "GaudiKernel/MsgStream.h"
57#include "GaudiKernel/Service.h"
58#include "GaudiKernel/SmartDataPtr.h"
59#include "GaudiKernel/StatusCode.h"
61#include "EvTimeEvent/RecEsTime.h"
62#include "EventModel/EventModel.h"
64using CLHEP::HepDiagMatrix;
65using CLHEP::HepMatrix;
66using CLHEP::HepSymMatrix;
67using CLHEP::HepVector;
100#define Convergence 1.0e-5
123 , _propagation( false )
128 , m_pmgnIMF( nullptr ) {}
134int THelixFitter::main(
TTrackBase& b,
float t0Offset,
double* pre_chi2,
135 double* fitted_chi2 )
const {
136 std::cout << std::fixed << std::setprecision( 6 );
221 std::cout << std::fixed << std::setprecision( 6 );
223 _pre_chi2 = _fitted_chi2 = 0.;
224 if ( pre_chi2 ) *pre_chi2 = _pre_chi2;
225 if ( fitted_chi2 ) *fitted_chi2 = _fitted_chi2;
237 cout <<
" cores in helix = " << cores.length() << endl;
239 if ( _fit2D ) cores =
AxialHits( cores );
240 unsigned nCores = cores.length();
244 bool fitBy2D = _fit2D;
245 if ( !fitBy2D ) fitBy2D = ( !nStereoCores );
250 if ( ( nStereoCores < 2 ) || ( nCores - nStereoCores < 3 ) )
return TFitErrorFewHits;
270 bool allAxial =
true;
279 AList<TMLink> initBadWires;
280 unsigned nInitBadWires = 0;
281 Vector initBadDchi2da( 5 );
283 for (
unsigned j = 0; j < 5; ++j ) initBadDchi2da[j] = 0.;
284 double initBadChi2 = 0.;
293 for (
unsigned j = 0; j < 5; j++ ) dchi2da[j] = 0.;
296 std::cout << std::fixed << std::setprecision( 6 );
299 cout <<
"helix fitter a5 " <<
t.helix().a()[0] <<
"\n"
300 <<
t.helix().a()[1] <<
"\n"
301 <<
t.helix().a()[2] <<
"\n"
302 <<
t.helix().a()[3] <<
"\n"
303 <<
t.helix().a()[4] <<
"\n"
304 <<
" cores.length " << cores.length() << endl;
308 while ( TMLink* l = cores[i++] )
310 const TMDCWireHit& h = *l->hit();
313 cout <<
"trial " << nTrial <<
" wire name " << l->wire()->name() <<
" L "
318 t.approach( *l, _sag );
319 double dPhi = l->dPhi();
320 const HepPoint3D& onTrack = l->positionOnTrack();
321 const HepPoint3D& onWire = l->positionOnWire();
327 drift(
t, *l, t0Offset, distance, eDistance );
328 l->drift( distance, 0 );
329 l->drift( distance, 1 );
330 l->dDrift( eDistance, 0 );
331 l->dDrift( eDistance, 1 );
333 double inv_eDistance2 = 1. / ( eDistance * eDistance );
337 double vmag =
v.mag();
338 std::cout << std::fixed << std::setprecision( 6 );
340 std::cout <<
"THelixFitter::eDistance-----" << eDistance << endl;
341 cout <<
" vmag = " << vmag <<
" distance = " << distance
342 <<
" eDistance = " << eDistance << endl;
344 double dDistance = vmag - distance;
347 this->dxda( *l,
t.helix(), dPhi, dxda, dyda, dzda );
354 double vwxy = vw[0] * vw[1];
355 double vwyz = vw[1] * vw[2];
356 double vwzx = vw[2] * vw[0];
359 ? ( (
v.x() * ( 1. - vw[0] * vw[0] ) -
v.y() * vwxy -
v.z() * vwzx ) * dxda +
360 (
v.y() * ( 1. - vw[1] * vw[1] ) -
v.z() * vwyz -
v.x() * vwxy ) * dyda +
361 (
v.z() * ( 1. - vw[2] * vw[2] ) -
v.x() * vwzx -
v.y() * vwyz ) * dzda ) /
366 std::cout <<
" in fit " << onTrack <<
", " << onWire;
369 double pChi2 = dDistance * dDistance * inv_eDistance2;
370 std::cout << std::fixed << std::setprecision( 3 );
372 std::cout <<
"THelixFitter::dDistance" << dDistance <<
" inv_eDistance2 "
373 << inv_eDistance2 << endl;
374 cout <<
"Liuqg check ... .. pChi2 = " << pChi2 << endl;
376 std::cout << std::fixed << std::setprecision( 6 );
379 if ( flagBad && nTrial == 0 )
383 initBadWires.append( l );
384 initBadDchi2da += ( dDistance * inv_eDistance2 ) * dDda;
385 initBadD2chi2d2a += vT_times_v( dDda ) * inv_eDistance2;
386 initBadChi2 += pChi2;
391 dchi2da += ( dDistance * inv_eDistance2 ) * dDda;
392 d2chi2d2a += vT_times_v( dDda ) * inv_eDistance2;
396 l->update( onTrack, onWire, leftRight, pChi2 );
420 if ( flagBad && nTrial == 0 )
422 if ( ( initBadWires.length() == 1 || initBadWires.length() == 2 ) && nCores >= 20 &&
423 chi2 / (
double)( nCores - initBadWires.length() ) < 10. )
425 cores.remove( initBadWires );
426 nInitBadWires = initBadWires.length();
428 else if ( initBadWires.length() != 0 )
430 dchi2da += initBadDchi2da;
431 d2chi2d2a += initBadD2chi2d2a;
442 else { _fitted_chi2 =
chi2; }
445 double change = chi2Old -
chi2;
446# ifdef TRKRECO_DEBUG_DETAIL
447 std::cout <<
" chi2 change " << change <<
" convergence " << convergence <<
" break? "
448 << ( fabs( change ) < convergence == true ) << std::endl;
450 if ( fabs( change ) < convergence )
break;
463 f = dchi2da.sub( 1, 3 );
464 e = d2chi2d2a.sub( 1, 3 );
472 else { da = solve( d2chi2d2a, dchi2da ); }
481 if ( fabs( a[3] ) > 1000. )
488# ifdef TRKRECO_DEBUG_DETAIL
489 std::cout <<
" fit " << nTrial - 1 <<
" : " <<
chi2 <<
" : " << change
524 Eb = d2chi2d2a.sub( 1, 3 );
525 Ec = Eb.inverse( err );
536 Ea = d2chi2d2a.inverse( err );
548 t._charge = copysign( 1., a[2] );
549 t._ndf = nCores - dim - nInitBadWires;
555 for (
unsigned i = 0; i < nInitBadWires; i++ )
557 TMLink* l = initBadWires[i];
558 t.approach( *l, _sag );
559 double dPhi = l->
dPhi();
563 double vmag =
v.mag();
567 drift(
t, *l, t0Offset, distance, eDistance );
568 l->
drift( distance, 0 );
569 l->
drift( distance, 1 );
570 l->
dDrift( eDistance, 0 );
571 l->
dDrift( eDistance, 1 );
573 double inv_eDistance2 = 1. / ( eDistance * eDistance );
574 double dDistance = vmag - distance;
575 double pChi2 = dDistance * dDistance * inv_eDistance2;
576 l->
update( onTrack, onWire, leftRight, pChi2 );
578# ifdef TRKRECO_DEBUG_DETAIL
579 std::cout <<
" HelixFitter : # of rejected hits=" << nInitBadWires << endl;
583 if ( pre_chi2 ) *pre_chi2 = _pre_chi2;
584 if ( fitted_chi2 ) *fitted_chi2 = _fitted_chi2;
590int THelixFitter::main(
TTrackBase& b,
float t0Offset,
double* pre_chi2,
591 double* fitted_chi2 )
const {
592 std::cout << std::fixed << std::setprecision( 6 );
593# ifdef TRKRECO_DEBUG_DETAIL
610 _pre_chi2 = _fitted_chi2 = 0.;
611 if ( pre_chi2 ) *pre_chi2 = _pre_chi2;
612 if ( fitted_chi2 ) *fitted_chi2 = _fitted_chi2;
616 TTrack&
t = (TTrack&)b;
622 AList<TMLink> cores =
t.cores();
623 if ( _fit2D ) cores =
AxialHits( cores );
624 unsigned nCores = cores.length();
628 bool fitBy2D = _fit2D;
629 if ( !fitBy2D ) fitBy2D = ( !nStereoCores );
634 if ( ( nStereoCores < 2 ) || ( nCores - nStereoCores < 3 ) )
return TFitErrorFewHits;
654 bool allAxial =
true;
661 AList<TMLink> initBadWires;
662 unsigned nInitBadWires = 0;
663 Vector initBadDchi2da( 5 );
665 for (
unsigned j = 0; j < 5; ++j ) initBadDchi2da[j] = 0.;
666 double initBadChi2 = 0.;
675 for (
unsigned j = 0; j < 5; j++ ) dchi2da[j] = 0.;
680 while ( TMLink* l = cores[i++] )
682 const TMDCWireHit& h = *l->
hit();
685 t.approach( *l, _sag );
686 double dPhi = l->
dPhi();
690 if ( onWire.cross( onTrack ).z() < 0. ) leftRight =
WireHitLeft;
695 drift(
t, *l, t0Offset, distance, eDistance );
697 double eDistance2 = eDistance * eDistance;
701 double vmag =
v.mag();
702 double dDistance = vmag - distance;
705 this->dxda( *l,
t.helix(), dPhi, dxda, dyda, dzda );
710 dDda = ( vmag > 0. ) ? ( (
v.x() * ( 1. - vw.x() * vw.x() ) -
v.y() * vw.x() * vw.y() -
711 v.z() * vw.x() * vw.z() ) *
713 (
v.y() * ( 1. - vw.y() * vw.y() ) -
v.z() * vw.y() * vw.z() -
714 v.x() * vw.y() * vw.x() ) *
716 (
v.z() * ( 1. - vw.z() * vw.z() ) -
v.x() * vw.z() * vw.x() -
717 v.y() * vw.z() * vw.y() ) *
723 std::cout <<
" in fit " << onTrack <<
", " << onWire;
726 double pChi2 = dDistance * dDistance / eDistance2;
729 if ( nTrial == 0 && pChi2 > 1500. )
731 initBadWires.append( l );
732 initBadDchi2da += ( dDistance / eDistance2 ) * dDda;
733 initBadD2chi2d2a += vT_times_v( dDda ) / eDistance2;
734 initBadChi2 += pChi2;
738 dchi2da += ( dDistance / eDistance2 ) * dDda;
739 d2chi2d2a += vT_times_v( dDda ) / eDistance2;
742 l->
update( onTrack, onWire, leftRight, pChi2 );
747 dchi2da += ( dDistance / eDistance2 ) * dDda;
748 d2chi2d2a += vT_times_v( dDda ) / eDistance2;
751 l->
update( onTrack, onWire, leftRight, pChi2 );
757 if ( nTrial == 0 && ( initBadWires.length() == 1 || initBadWires.length() == 2 ) &&
758 nCores >= 20 &&
chi2 / (
double)( nCores - initBadWires.length() ) < 10. )
760 cores.remove( initBadWires );
761 nInitBadWires = initBadWires.length();
763 else if ( nTrial == 0 && initBadWires.length() != 0 )
765 dchi2da += initBadDchi2da;
766 d2chi2d2a += initBadD2chi2d2a;
777 else _fitted_chi2 =
chi2;
780 double change = chi2Old -
chi2;
781 if ( fabs( change ) < convergence )
break;
794 f = dchi2da.sub( 1, 3 );
795 e = d2chi2d2a.sub( 1, 3 );
803 else { da = solve( d2chi2d2a, dchi2da ); }
812 if ( fabs( a[3] ) > 1000. )
819# ifdef TRKRECO_DEBUG_DETAIL
820 std::cout <<
" fit " << nTrial - 1 <<
" : " <<
chi2 <<
" : " << change
825# ifdef TRKRECO_DEBUG_DETAIL
840 Eb = d2chi2d2a.sub( 1, 3 );
841 Ec = Eb.inverse( err );
852 Ea = d2chi2d2a.inverse( err );
864 t._charge = copysign( 1., a[2] );
865 t._ndf = nCores - dim - nInitBadWires;
868 if ( pre_chi2 ) *pre_chi2 = _pre_chi2;
869 if ( fitted_chi2 ) *fitted_chi2 = _fitted_chi2;
879int THelixFitter::dxda(
const TMLink& link,
const Helix& h,
double dPhi,
Vector& dxda,
885 std::cout <<
" ERROR: THelixFitter::dxda 1st m_pmgnIMF == NULL " << std::endl;
889 const TMDCWire&
w = *link.
wire();
891 const double dRho = a[0];
892 const double phi0 = a[1];
893 const double kappa = a[2];
897 const double Bz = -1000 * m_pmgnIMF->getReferField();
898 const double rho = 333.564095 / ( kappa * Bz );
899 const double tanLambda = a[4];
901 const double sinPhi0 =
sin( phi0 );
902 const double cosPhi0 =
cos( phi0 );
905 const double sinPhi0dPhi =
sin( phi0 + dPhi );
906 const double cosPhi0dPhi =
cos( phi0 + dPhi );
912 HepPoint3D wireBackwardPosition =
w.backwardPosition();
914 if ( _sag )
w.wirePosition( link.
positionOnTrack().z(), xw, wireBackwardPosition,
v );
956 const double v_dot_wireBackwardPosition =
v.x() * wireBackwardPosition.x() +
957 v.y() * wireBackwardPosition.y() +
958 v.z() * wireBackwardPosition.z();
959 const double c[3] = {
w.backwardPosition().x() - v_dot_wireBackwardPosition *
v.x(),
960 w.backwardPosition().y() - v_dot_wireBackwardPosition *
v.y(),
961 w.backwardPosition().z() - v_dot_wireBackwardPosition *
v.z() };
965 const double x_minus_c[3] = {
x[0] - c[0],
x[1] - c[1],
x[2] - c[2] };
968 const double dxdphi[3] = { rho * sinPhi0dPhi, -rho * cosPhi0dPhi, -rho * tanLambda };
971 const double d2xdphi2[3] = { -dxdphi[1], dxdphi[0], 0. };
973 double dxdphi_dot_v = ( dxdphi[0] *
v.x() + dxdphi[1] *
v.y() + dxdphi[2] *
v.z() );
974 double x_dot_v =
x[0] *
v.x() +
x[1] *
v.y() +
x[2] *
v.z();
975 double inv_dfdphi = -1. / ( ( dxdphi[0] - dxdphi_dot_v *
v.x() ) * dxdphi[0] +
976 ( dxdphi[1] - dxdphi_dot_v *
v.y() ) * dxdphi[1] +
977 ( dxdphi[2] - dxdphi_dot_v *
v.z() ) * dxdphi[2] +
978 ( x_minus_c[0] - x_dot_v *
v.x() ) * d2xdphi2[0] +
979 ( x_minus_c[1] - x_dot_v *
v.y() ) * d2xdphi2[1] );
982 const double rho_per_kappa = rho / kappa;
983 const double dRho_plus_rho = dRho + rho;
984 const double& rhoSinPhi0dPhi = dxdphi[0];
985 const double rhoCosPhi0dPhi = -dxdphi[1];
986 const double rhoTanLambda = -dxdphi[2];
991 dxda_phi[0] = cosPhi0;
992 dxda_phi[1] = -dRho_plus_rho * sinPhi0 + rhoSinPhi0dPhi;
993 dxda_phi[2] = -rho_per_kappa * ( cosPhi0 - cosPhi0dPhi );
999 dyda_phi[0] = sinPhi0;
1000 dyda_phi[1] = dRho_plus_rho * cosPhi0 - rhoCosPhi0dPhi;
1001 dyda_phi[2] = -rho_per_kappa * ( sinPhi0 - sinPhi0dPhi );
1009 dzda_phi[2] = rho_per_kappa * tanLambda * dPhi;
1011 dzda_phi[4] = -rho * dPhi;
1014 double d2xdphida[5];
1016 d2xdphida[1] = rhoCosPhi0dPhi;
1017 d2xdphida[2] = -rho_per_kappa * sinPhi0dPhi;
1022 double d2ydphida[5];
1024 d2ydphida[1] = rhoSinPhi0dPhi;
1025 d2ydphida[2] = rho_per_kappa * cosPhi0dPhi;
1030 double d2zdphida[5];
1033 d2zdphida[2] = rho_per_kappa * tanLambda;
1035 d2zdphida[4] = -rho;
1039 for (
int i = 0; i < 5; ++i )
1041 double d_dot_v = (
v.x() * dxda_phi[i] +
v.y() * dyda_phi[i] +
v.z() * dzda_phi[i] );
1042 dfda[i] = ( -( dxda_phi[i] - d_dot_v *
v.x() ) * dxdphi[0] -
1043 ( dyda_phi[i] - d_dot_v *
v.y() ) * dxdphi[1] -
1044 ( dzda_phi[i] - d_dot_v *
v.z() ) * dxdphi[2] -
1045 ( x_minus_c[0] - x_dot_v *
v.x() ) * d2xdphida[i] -
1046 ( x_minus_c[1] - x_dot_v *
v.y() ) * d2ydphida[i] -
1047 ( x_minus_c[2] - x_dot_v *
v.z() ) * d2zdphida[i] );
1048 dphida[i] = -dfda[i] * inv_dfdphi;
1051 dxda[0] = cosPhi0 + rhoSinPhi0dPhi * dphida[0];
1052 dxda[1] = -dRho_plus_rho * sinPhi0 + rhoSinPhi0dPhi * ( 1. + dphida[1] );
1053 dxda[2] = -rho_per_kappa * ( cosPhi0 - cosPhi0dPhi ) + rhoSinPhi0dPhi * dphida[2];
1054 dxda[3] = rhoSinPhi0dPhi * dphida[3];
1055 dxda[4] = rhoSinPhi0dPhi * dphida[4];
1057 dyda[0] = sinPhi0 - rhoCosPhi0dPhi * dphida[0];
1058 dyda[1] = dRho_plus_rho * cosPhi0 - rhoCosPhi0dPhi * ( 1. + dphida[1] );
1059 dyda[2] = -rho_per_kappa * ( sinPhi0 - sinPhi0dPhi ) - rhoCosPhi0dPhi * dphida[2];
1060 dyda[3] = -rhoCosPhi0dPhi * dphida[3];
1061 dyda[4] = -rhoCosPhi0dPhi * dphida[4];
1063 dzda[0] = -rhoTanLambda * dphida[0];
1064 dzda[1] = -rhoTanLambda * dphida[1];
1065 dzda[2] = rho_per_kappa * tanLambda * dPhi - rhoTanLambda * dphida[2];
1066 dzda[3] = 1. - rhoTanLambda * dphida[3];
1067 dzda[4] = -rho * dPhi - rhoTanLambda * dphida[4];
1077int THelixFitter::dxda(
const TMLink& link,
const Helix& h,
double dPhi,
Vector& dxda,
1082 const TMDCWire&
w = *link.
wire();
1086 double kappa = a[2];
1093 std::cout <<
" ERROR: THelixFitter::dxda 1st m_pmgnIMF == NULL " << std::endl;
1096 double rho = 333.564095 / ( -1000 * ( m_pmgnIMF->getReferField() ) ) / kappa;
1098 double tanLambda = a[4];
1100 double sinPhi0 =
sin( phi0 );
1101 double cosPhi0 =
cos( phi0 );
1104 double sinPhi0dPhi =
sin( phi0 + dPhi );
1105 double cosPhi0dPhi =
cos( phi0 + dPhi );
1110 HepPoint3D wireBackwardPosition =
w.backwardPosition();
1112 if ( _sag )
w.wirePosition( link.
positionOnTrack().z(), xw, wireBackwardPosition,
v );
1138 c =
HepPoint3D(
w.backwardPosition() - (
v * wireBackwardPosition ) *
v );
1144 dxdphi[0] = rho * sinPhi0dPhi;
1145 dxdphi[1] = -rho * cosPhi0dPhi;
1146 dxdphi[2] = -rho * tanLambda;
1149 d2xdphi2[0] = rho * cosPhi0dPhi;
1150 d2xdphi2[1] = rho * sinPhi0dPhi;
1153 double dxdphi_dot_v = ( dxdphi[0] *
v.x() + dxdphi[1] *
v.y() + dxdphi[2] *
v.z() );
1154 double x_dot_v =
x[0] *
v.x() +
x[1] *
v.y() +
x[2] *
v.z();
1155 double dfdphi = -( dxdphi[0] - dxdphi_dot_v *
v.x() ) * dxdphi[0] -
1156 ( dxdphi[1] - dxdphi_dot_v *
v.y() ) * dxdphi[1] -
1157 ( dxdphi[2] - dxdphi_dot_v *
v.z() ) * dxdphi[2] -
1158 (
x[0] - c[0] - x_dot_v *
v.x() ) * d2xdphi2[0] -
1159 (
x[1] - c[1] - x_dot_v *
v.y() ) * d2xdphi2[1];
1164 dxda_phi[0] = cosPhi0;
1165 dxda_phi[1] = -( dRho + rho ) * sinPhi0 + rho * sinPhi0dPhi;
1166 dxda_phi[2] = -( rho / kappa ) * ( cosPhi0 - cosPhi0dPhi );
1171 dyda_phi[0] = sinPhi0;
1172 dyda_phi[1] = ( dRho + rho ) * cosPhi0 - rho * cosPhi0dPhi;
1173 dyda_phi[2] = -( rho / kappa ) * ( sinPhi0 - sinPhi0dPhi );
1180 dzda_phi[2] = ( rho / kappa ) * tanLambda * dPhi;
1182 dzda_phi[4] = -rho * dPhi;
1186 d2xdphida[1] = rho * cosPhi0dPhi;
1187 d2xdphida[2] = -( rho / kappa ) * sinPhi0dPhi;
1193 d2ydphida[1] = rho * sinPhi0dPhi;
1194 d2ydphida[2] = ( rho / kappa ) * cosPhi0dPhi;
1201 d2zdphida[2] = ( rho / kappa ) * tanLambda;
1203 d2zdphida[4] = -rho;
1206 for (
int i = 0; i < 5; i++ )
1208 double d_dot_v = (
v.x() * dxda_phi[i] +
v.y() * dyda_phi[i] +
v.z() * dzda_phi[i] );
1209 dfda[i] = ( -( dxda_phi[i] - d_dot_v *
v.x() ) * dxdphi[0] -
1210 ( dyda_phi[i] - d_dot_v *
v.y() ) * dxdphi[1] -
1211 ( dzda_phi[i] - d_dot_v *
v.z() ) * dxdphi[2] -
1212 (
x[0] - c[0] - x_dot_v *
v.x() ) * d2xdphida[i] -
1213 (
x[1] - c[1] - x_dot_v *
v.y() ) * d2ydphida[i] -
1214 (
x[2] - c[2] - x_dot_v *
v.z() ) * d2zdphida[i] );
1215 dphida[i] = -dfda[i] / dfdphi;
1219 dxda[0] = cosPhi0 + rho * sinPhi0dPhi * dphida[0];
1220 dxda[1] = -( dRho + rho ) * sinPhi0 + rho * sinPhi0dPhi * ( 1. + dphida[1] );
1221 dxda[2] = -rho / kappa * ( cosPhi0 - cosPhi0dPhi ) + rho * sinPhi0dPhi * dphida[2];
1222 dxda[3] = rho * sinPhi0dPhi * dphida[3];
1223 dxda[4] = rho * sinPhi0dPhi * dphida[4];
1225 dyda[0] = sinPhi0 - rho * cosPhi0dPhi * dphida[0];
1226 dyda[1] = ( dRho + rho ) * cosPhi0 - rho * cosPhi0dPhi * ( 1. + dphida[1] );
1227 dyda[2] = -rho / kappa * ( sinPhi0 - sinPhi0dPhi ) - rho * cosPhi0dPhi * dphida[2];
1228 dyda[3] = -rho * cosPhi0dPhi * dphida[3];
1229 dyda[4] = -rho * cosPhi0dPhi * dphida[4];
1231 dzda[0] = -rho * tanLambda * dphida[0];
1232 dzda[1] = -rho * tanLambda * dphida[1];
1233 dzda[2] = rho / kappa * tanLambda * dPhi - rho * tanLambda * dphida[2];
1234 dzda[3] = 1. - rho * tanLambda * dphida[3];
1235 dzda[4] = -rho * dPhi - rho * tanLambda * dphida[4];
1245void THelixFitter::drift(
const TTrack&
t,
TMLink& l,
float t0Offset,
double& distance,
1246 double& err )
const {
1247 std::cout << std::fixed << std::setprecision( 6 );
1249 const TMDCWireHit& h = *l.
hit();
1254 if ( onWire.cross( onTrack ).z() < 0. ) leftRight =
WireHitLeft;
1255 int charge = (
t.helix().a()[2] > 0 ) ? 1 : -1;
1257 if ( NULL == m_pmgnIMF )
1259 Gaudi::svcLocator()->service(
"MagneticFieldSvc", m_pmgnIMF );
1260 if ( NULL == m_pmgnIMF )
1262 std::cout << __FILE__ <<
" " << __LINE__ <<
" Unable to open Magnetic field service"
1266 const double Bz = -1000 * m_pmgnIMF->getReferField();
1268 std::cout <<
" orignal ambig " << leftRight <<
" charge " << charge <<
" Bz " << Bz
1279 std::cout <<
" new ambig " << leftRight << std::endl;
1284 if ( ( t0Offset == 0. ) && ( !_propagation ) && ( !_tof ) )
1286 distance = h.
drift( leftRight );
1287 err = h.
dDrift( leftRight );
1299 float tl =
t.helix().a()[4];
1300 float f = sqrt( 1. + tl * tl );
1301 float s = fabs(
t.helix().curv() ) * fabs( l.
dPhi() ) *
f;
1302 float p =
f / fabs(
t.helix().a()[2] );
1304 float Mass[5] = { 0.000511, 0.105, 0.140, 0.493677, 0.98327 };
1305 tof =
s / 29.98 * sqrt( 1.0 + ( Mass[imass - 1] / p ) * ( Mass[imass - 1] / p ) );
1323 int side = leftRight;
1333 int prop = _propagation;
1335 const double x0 =
t.helix().pivot().x();
1336 const double y0 =
t.helix().pivot().y();
1337 Hep3Vector pivot_helix( x0, y0, 0 );
1338 const double dr =
t.helix().a()[0];
1339 const double phi0 =
t.helix().a()[1];
1340 const double kappa =
t.helix().a()[2];
1341 const double dz =
t.helix().a()[3];
1342 const double tanl =
t.helix().a()[4];
1347 const double alpha = 333.564095 / ( -1000 * ( m_pmgnIMF->getReferField() ) );
1350 const double cox = x0 + dr *
cos( phi0 ) +
alpha *
cos( phi0 ) / kappa;
1351 const double coy = y0 + dr *
sin( phi0 ) +
alpha *
sin( phi0 ) / kappa;
1353 unsigned nHits =
t.links().length();
1354 unsigned nStereos = 0;
1355 unsigned firstLyr = 44;
1356 unsigned lastLyr = 0;
1360 HepPoint3D dir( ontrack.y() - coy, cox - ontrack.x(), 0 );
1361 double pos_phi = onwire.phi();
1362 double dir_phi = dir.phi();
1363 while ( pos_phi >
pi ) { pos_phi -=
pi; }
1364 while ( pos_phi < 0 ) { pos_phi +=
pi; }
1365 while ( dir_phi >
pi ) { dir_phi -=
pi; }
1366 while ( dir_phi < 0 ) { dir_phi +=
pi; }
1367 double entrangle = dir_phi - pos_phi;
1368 while ( entrangle >
pi / 2 ) entrangle -=
pi;
1369 while ( entrangle < ( -1 ) *
pi / 2 ) entrangle +=
pi;
1371 double zhit = onWire.z();
1374 std::cout <<
" onWire: " << onWire << std::endl;
1375 std::cout <<
" zhit: " << zhit << std::endl;
1378 const double vinner = 220.0e8;
1379 const double vouter = 240.0e8;
1380 double vprop = 300.0e9;
1383 if ( layerId < 8 ) { vprop = vinner; }
1384 else { vprop = vouter; }
1388 Gaudi::svcLocator()->service(
"MessageSvc",
msgSvc );
1389 MsgStream log(
msgSvc,
"TCosmicFitter" );
1391 IDataProviderSvc* eventSvc = NULL;
1392 Gaudi::svcLocator()->service(
"EventDataSvc", eventSvc );
1394 SmartDataPtr<RecEsTimeCol> aevtimeCol( eventSvc,
"/Event/Recon/RecEsTimeCol" );
1397 RecEsTimeCol::iterator iter_evt = aevtimeCol->begin();
1398 EsT0 = ( *iter_evt )->getTest();
1400 else { log << MSG::WARNING <<
"Could not find RecEsTimeCol" << endmsg; }
1402 double rawTime = 0.;
1410 IMdcCalibFunSvc* l_mdcCalFunSvc;
1411 Gaudi::svcLocator()->service(
"MdcCalibFunSvc", l_mdcCalFunSvc );
1413 double tprop = l_mdcCalFunSvc->
getTprop( layerId, zhit * 10. );
1415 double timeWalk = l_mdcCalFunSvc->
getTimeWalk( layerId, rawadc );
1417 double T0 = l_mdcCalFunSvc->
getT0( layerId, wire );
1418 double drifttime = rawTime -
tof - tprop - timeWalk - T0;
1422 std::cout <<
"timewalk is : " << timeWalk << std::endl;
1423 std::cout <<
"T0 is : " << T0 << std::endl;
1424 std::cout <<
"EsT0 is : " << EsT0 << std::endl;
1425 std::cout <<
"tprop is : " << tprop << std::endl;
1426 std::cout <<
"tof is : " <<
tof << std::endl;
1427 std::cout <<
"rawTime is : " << rawTime << std::endl;
1428 std::cout <<
"driftTime is : " << drifttime << std::endl;
1432 dist = l_mdcCalFunSvc->
driftTimeToDist( drifttime, layerId, wire, side,
1434 edist = l_mdcCalFunSvc->
getSigma( layerId, side, dist, entrangle,
tanl, zhit * 10, rawadc );
1436 edist = edist / 10.0;
1453 std::cout << std::defaultfloat;
1459int THelixFitter::main(
TTrackBase& b,
float& tev,
float& tev_err,
double* pre_chi2,
1460 double* fitted_chi2 )
const {
1463 _pre_chi2 = _fitted_chi2 = 0.;
1464 if ( pre_chi2 ) *pre_chi2 = _pre_chi2;
1465 if ( fitted_chi2 ) *fitted_chi2 = _fitted_chi2;
1469 TTrack&
t = (TTrack&)b;
1475 AList<TMLink> cores =
t.cores();
1476 if ( _fit2D ) cores =
AxialHits( cores );
1477 unsigned nCores = cores.length();
1481 bool fitBy2D = _fit2D;
1482 if ( !fitBy2D ) fitBy2D = ( !nStereoCores );
1487 if ( ( nStereoCores < 2 ) || ( nCores - nStereoCores < 3 ) )
return TFitErrorFewHits;
1497 for (
unsigned j = 0; j < 5; j++ ) a[j] =
t.helix().a()[j];
1510 const double convergence = 1.0e-4;
1511 bool allAxial =
true;
1515 double factor = 1.0;
1518 unsigned nTrial = 0;
1524 for (
unsigned j = 0; j < 6; j++ ) dchi2da[j] = 0.;
1529 while ( TMLink* l = cores[i++] )
1531 const TMDCWireHit& h = *l->
hit();
1534 t.approach( *l, _sag );
1535 double dPhi = l->
dPhi();
1544 drift(
t, *l, tev, distance, eDistance, dddt );
1550 double inv_eDistance2 = 1. / ( eDistance * eDistance );
1554 double vmag =
v.mag();
1555 double dDistance = vmag - distance;
1558 this->dxda( *l,
t.helix(), dPhi, dxda, dyda, dzda );
1564 double vwxy = vw[0] * vw[1];
1565 double vwyz = vw[1] * vw[2];
1566 double vwzx = vw[2] * vw[0];
1569 ? ( (
v.x() * ( 1. - vw[0] * vw[0] ) -
v.y() * vwxy -
v.z() * vwzx ) * dxda +
1570 (
v.y() * ( 1. - vw[1] * vw[1] ) -
v.z() * vwyz -
v.x() * vwxy ) * dyda +
1571 (
v.z() * ( 1. - vw[2] * vw[2] ) -
v.x() * vwzx -
v.y() * vwyz ) * dzda ) /
1576 std::cout <<
" in fit " << onTrack <<
", " << onWire;
1580 dDda[0] = dDda_5dim[0];
1581 dDda[1] = dDda_5dim[1];
1582 dDda[2] = dDda_5dim[2];
1583 dDda[3] = dDda_5dim[3];
1584 dDda[4] = dDda_5dim[4];
1587 dchi2da += ( dDistance * inv_eDistance2 ) * dDda;
1588 d2chi2d2a += vT_times_v( dDda ) * inv_eDistance2;
1589 double pChi2 = dDistance * dDistance * inv_eDistance2;
1593 l->
update( onTrack, onWire, leftRight, pChi2 );
1600 _fitted_chi2 =
chi2;
1602 else _fitted_chi2 =
chi2;
1605 double change = chi2Old -
chi2;
1606 if ( fabs( change ) < convergence )
break;
1623 f = dchi2da.sub( 1, 4 );
1624 e = d2chi2d2a.sub( 1, 4 );
1633 else { da = solve( d2chi2d2a, dchi2da ); }
1643 t._helix->a( a_5dim );
1650# ifdef TRKRECO_DEBUG
1651 std::cout <<
"fit " << nTrial - 1 <<
" : " <<
chi2 <<
" : " << change << std::endl;
1662 Eb = d2chi2d2a.sub( 1, 4 );
1663 Ec = Eb.inverse( err );
1664 Ea[0][0] = Ec[0][0];
1665 Ea[0][1] = Ec[0][1];
1666 Ea[0][2] = Ec[0][2];
1667 Ea[0][3] = Ec[0][3];
1668 Ea[1][1] = Ec[1][1];
1669 Ea[1][2] = Ec[1][2];
1670 Ea[1][3] = Ec[1][3];
1671 Ea[2][2] = Ec[2][2];
1672 Ea[2][3] = Ec[2][3];
1673 Ea[3][3] = Ec[3][3];
1678 Ea = d2chi2d2a.inverse( err );
1685 for (
unsigned j = 0; j < 5; j++ ) a_5dim[j] = a[j];
1687 Ea_5dim = Ea.sub( 1, 5 );
1688 t._helix->a( a_5dim );
1689 t._helix->Ea( Ea_5dim );
1691 tev_err = sqrt( Ea[5][5] );
1703 t._ndf = nCores - dim;
1706 if ( pre_chi2 ) *pre_chi2 = _pre_chi2;
1707 if ( fitted_chi2 ) *fitted_chi2 = _fitted_chi2;
1713int THelixFitter::main(
TTrackBase& b,
float& tev,
float& tev_err,
double* pre_chi2,
1714 double* fitted_chi2 )
const {
1717 _pre_chi2 = _fitted_chi2 = 0.;
1718 if ( pre_chi2 ) *pre_chi2 = _pre_chi2;
1719 if ( fitted_chi2 ) *fitted_chi2 = _fitted_chi2;
1723 TTrack&
t = (TTrack&)b;
1729 AList<TMLink> cores =
t.cores();
1730 if ( _fit2D ) cores =
AxialHits( cores );
1731 unsigned nCores = cores.length();
1735 bool fitBy2D = _fit2D;
1736 if ( !fitBy2D ) fitBy2D = ( !nStereoCores );
1741 if ( ( nStereoCores < 2 ) || ( nCores - nStereoCores < 3 ) )
return TFitErrorFewHits;
1751 for (
unsigned j = 0; j < 5; j++ ) a[j] =
t.helix().a()[j];
1764 const double convergence = 1.0e-4;
1765 bool allAxial =
true;
1769 double factor = 1.0;
1772 unsigned nTrial = 0;
1778 for (
unsigned j = 0; j < 6; j++ ) dchi2da[j] = 0.;
1783 while ( TMLink* l = cores[i++] )
1785 const TMDCWireHit& h = *l->
hit();
1788 t.approach( *l, _sag );
1789 double dPhi = l->
dPhi();
1793 if ( onWire.cross( onTrack ).z() < 0. ) leftRight =
WireHitLeft;
1799 drift(
t, *l, tev, distance, eDistance, dddt );
1801 double eDistance2 = eDistance * eDistance;
1805 double vmag =
v.mag();
1806 double dDistance = vmag - distance;
1809 this->dxda( *l,
t.helix(), dPhi, dxda, dyda, dzda );
1814 dDda_5dim = ( vmag > 0. ) ? ( (
v.x() * ( 1. - vw.x() * vw.x() ) -
1815 v.y() * vw.x() * vw.y() -
v.z() * vw.x() * vw.z() ) *
1817 (
v.y() * ( 1. - vw.y() * vw.y() ) -
1818 v.z() * vw.y() * vw.z() -
v.x() * vw.y() * vw.x() ) *
1820 (
v.z() * ( 1. - vw.z() * vw.z() ) -
1821 v.x() * vw.z() * vw.x() -
v.y() * vw.z() * vw.y() ) *
1827 std::cout <<
" in fit " << onTrack <<
", " << onWire;
1831 dDda[0] = dDda_5dim[0];
1832 dDda[1] = dDda_5dim[1];
1833 dDda[2] = dDda_5dim[2];
1834 dDda[3] = dDda_5dim[3];
1835 dDda[4] = dDda_5dim[4];
1838 dchi2da += ( dDistance / eDistance2 ) * dDda;
1839 d2chi2d2a += vT_times_v( dDda ) / eDistance2;
1840 double pChi2 = dDistance * dDistance / eDistance2;
1844 l->
update( onTrack, onWire, leftRight, pChi2 );
1851 _fitted_chi2 =
chi2;
1853 else _fitted_chi2 =
chi2;
1856 double change = chi2Old -
chi2;
1857 if ( fabs( change ) < convergence )
break;
1873 f = dchi2da.sub( 1, 4 );
1874 e = d2chi2d2a.sub( 1, 4 );
1883 else { da = solve( d2chi2d2a, dchi2da ); }
1893 t._helix->a( a_5dim );
1900# ifdef TRKRECO_DEBUG
1901 std::cout <<
"fit " << nTrial - 1 <<
" : " <<
chi2 <<
" : " << change << std::endl;
1912 Eb = d2chi2d2a.sub( 1, 4 );
1913 Ec = Eb.inverse( err );
1914 Ea[0][0] = Ec[0][0];
1915 Ea[0][1] = Ec[0][1];
1916 Ea[0][2] = Ec[0][2];
1917 Ea[0][3] = Ec[0][3];
1918 Ea[1][1] = Ec[1][1];
1919 Ea[1][2] = Ec[1][2];
1920 Ea[1][3] = Ec[1][3];
1921 Ea[2][2] = Ec[2][2];
1922 Ea[2][3] = Ec[2][3];
1923 Ea[3][3] = Ec[3][3];
1928 Ea = d2chi2d2a.inverse( err );
1935 for (
unsigned j = 0; j < 5; j++ ) a_5dim[j] = a[j];
1937 Ea_5dim = Ea.sub( 1, 5 );
1938 t._helix->a( a_5dim );
1939 t._helix->Ea( Ea_5dim );
1941 tev_err = sqrt( Ea[5][5] );
1952 t._ndf = nCores - dim;
1955 if ( pre_chi2 ) *pre_chi2 = _pre_chi2;
1956 if ( fitted_chi2 ) *fitted_chi2 = _fitted_chi2;
1963void THelixFitter::drift(
const TTrack&
t,
TMLink& l,
float tev,
double& distance,
double& err,
1964 double& dddt )
const {
1967 const TMDCWireHit& h = *l.
hit();
1971 if ( onWire.cross( onTrack ).z() < 0. ) leftRight =
WireHitLeft;
1974 if ( ( tev == 0. ) && ( !_propagation ) && ( !_tof ) )
1976 distance = h.
drift( leftRight );
1977 err = h.
dDrift( leftRight );
1986 float tl =
t.helix().a()[4];
1987 float f = sqrt( 1. + tl * tl );
1988 float s = fabs(
t.helix().curv() ) * fabs( l.
dPhi() ) *
f;
1989 float p =
f / fabs(
t.helix().a()[2] );
1991 float Mass[5] = { 0.000511, 0.105, 0.140, 0.493677, 0.98327 };
1992 tof =
s / 29.98 * sqrt( 1.0 + ( Mass[imass - 1] / p ) * ( Mass[imass - 1] / p ) );
2000 int side = leftRight;
2003 const double zhit = onWire.z();
2004 const double vinner = 220.0e8;
2005 const double vouter = 240.0e8;
2008 const double vprop = ( layerId < 8 ) ? vinner : vouter;
2009 if ( 0 == layerId % 2 )
2034 Gaudi::svcLocator()->service(
"MessageSvc",
msgSvc );
2035 MsgStream log(
msgSvc,
"TCosmicFitter" );
2037 IDataProviderSvc* eventSvc = NULL;
2038 Gaudi::svcLocator()->service(
"EventDataSvc", eventSvc );
2040 SmartDataPtr<RecEsTimeCol> aevtimeCol( eventSvc,
"/Event/Recon/RecEsTimeCol" );
2043 RecEsTimeCol::iterator iter_evt = aevtimeCol->begin();
2044 EsT0 = ( *iter_evt )->getTest();
2046 else { log << MSG::WARNING <<
"Could not find RecEsTimeCol" << endmsg; }
2048 double rawTime = 0.;
2056 IMdcCalibFunSvc* l_mdcCalFunSvc;
2057 Gaudi::svcLocator()->service(
"MdcCalibFunSvc", l_mdcCalFunSvc );
2059 double timeWalk = l_mdcCalFunSvc->
getTimeWalk( layerId, rawadc );
2060 double drifttime = rawTime -
tof - 1.0e9 * tprop - EsT0 - timeWalk;
2066 int prop = _propagation;
2097 float time_tmp =
time;
2101 Gaudi::svcLocator()->service(
"MdcCalibFunSvc", l_mdcCalFunSvc );
2103 dist = l_mdcCalFunSvc->
driftTimeToDist( time_tmp, layerId, wire, side,
2105 edist = l_mdcCalFunSvc->
getSigma( layerId, side, dist, 0.0 );
2107 edist = edist / 10.0;
2114 float time_p =
time - 0.1;
2115 float time_m =
time + 0.1;
HepGeom::Vector3D< double > HepVector3D
HepGeom::Point3D< double > HepPoint3D
TFile f("ana_bhabha660a_dqa_mcPat_zy_old.root")
double sin(const BesAngle a)
double cos(const BesAngle a)
CLHEP::HepSymMatrix SymMatrix
#define WireHitPatternLeft
#define WireHitPatternRight
#define TFitAlreadyFitted
AList< TMLink > AxialHits(const AList< TMLink > &links)
returns axial hits.
unsigned NStereoHits(const AList< TMLink > &links)
returns # of stereo hits.
**********Class see also m_nmax DOUBLE PRECISION m_amel DOUBLE PRECISION m_x2 DOUBLE PRECISION m_alfinv DOUBLE PRECISION m_Xenph INTEGER m_KeyWtm INTEGER m_idyfs DOUBLE PRECISION m_zini DOUBLE PRECISION m_q2 DOUBLE PRECISION m_Wt_KF DOUBLE PRECISION m_WtCut INTEGER m_KFfin *COMMON c_KarLud $ !Input CMS energy[GeV] $ !CMS energy after beam spread beam strahlung[GeV] $ !Beam energy spread[GeV] $ !z boost due to beam spread $ !electron beam mass *ff pair spectrum $ !minimum v
float TrkRecoHelixFitterChisqMax
const HepVector & a(void) const
returns helix parameters.
virtual double driftTimeToDist(double drifttime, int layid, int cellid, int lr, double entrance=0.0) const =0
virtual double getSigma(int layid, int lr, double dist, double entrance=0.0, double tanlam=0.0, double z=0.0, double Q=1000.0) const =0
virtual double getTprop(int lay, double z) const =0
virtual double getT0(int layid, int cellid) const =0
virtual double getTimeWalk(int layid, double Q) const =0
double chi2(void) const
returns sum of chi2 aftter fit.
virtual ~THelixFitter()
Destructor.
bool tof(void) const
sets/returns propagation-delay correction flag.
THelixFitter(const std::string &name)
Constructor.
bool tanl(void) const
sets/returns tanLambda correction flag.
struct MdcRec_wirhit * reccdc(void) const
returns a pointer to RECMDC_WIRHIT.
void dump(const std::string &message=std::string(""), const std::string &prefix=std::string("")) const
dumps debug information.
unsigned state(void) const
returns state.
float dDrift(unsigned) const
returns drift distance error.
float drift(unsigned) const
returns drift distance.
const TMDCWire *const wire(void) const
returns a pointer to a TMDCWire.
unsigned localId(void) const
returns local id in a wire layer.
unsigned layerId(void) const
returns layer id.
const HepVector3D & direction(void) const
returns direction vector of the wire.
const HepPoint3D & forwardPosition(void) const
returns position in forward endplate.
const HepPoint3D & backwardPosition(void) const
returns position in backward endplate.
const std::string & name(void) const
returns name.
TMFitter(const std::string &name)
Constructor.
A class to relate TMDCWireHit and TTrack objects.
void setDriftTime(double)
add by jialk returns timeDrift after prop correction
const HepPoint3D & positionOnWire(void) const
returns the closest point on wire to a track.
const HepPoint3D & positionOnTrack(void) const
returns the closest point on track to wire.
const TMDCWireHit * hit(void) const
returns a pointer to a hit.
void update(const HepPoint3D &onTrack, const HepPoint3D &onWire, unsigned leftRight, double pull)
sets results of fitting.
float dDrift(void) const
returns/sets drift distance error.
double dPhi(void) const
returns dPhi to the closest point.
float drift(void) const
returns/sets drift distance.
const TMDCWire *const wire(void) const
returns a pointer to a wire.
A virtual class for a track class in tracking.
virtual unsigned objectType(void) const
returns object type.
A class to represent a track in tracking.