14 int runnumber=(loop->GetJEvent()).GetRunNumber();
16 JCalibration *jcalib = dapp->GetJCalibration(runnumber);
20 vector<const DTrackFinder *> finders;
35 map<string,double>drift_res_parms;
36 jcalib->Get(
"FDC/drift_resolution_parms",drift_res_parms);
42 map<string,double>drift_func_parms;
43 jcalib->Get(
"FDC/drift_function_parms",drift_func_parms);
50 map<string,double>drift_func_ext;
51 if (jcalib->Get(
"FDC/drift_function_ext",drift_func_ext)==
false){
57 gPARMS->SetDefaultParameter(
"TRKFIT:PLANE_TO_SKIP",
PLANE_TO_SKIP);
63 gPARMS->SetDefaultParameter(
"TRKFIT:RING_TO_SKIP",
RING_TO_SKIP);
65 map<string, double> cdc_res_parms;
66 jcalib->Get(
"CDC/cdc_resolution_parms_v2", cdc_res_parms);
71 vector< map<string, double> > tvals;
73 if (jcalib->Get(
"CDC/cdc_drift_table", tvals)==
false){
74 for(
unsigned int i=0; i<tvals.size(); i++){
75 map<string, double> &row = tvals[i];
80 jerr <<
" CDC time-to-distance table not available... bailing..." << endl;
84 if (jcalib->Get(
"CDC/drift_parameters", tvals)==
false){
85 map<string, double> &row = tvals[0];
111 unsigned int numstraws[28]={42,42,54,54,66,66,80,80,93,93,106,106,123,123,
112 135,135,146,146,158,158,170,170,182,182,197,197,209,209};
113 unsigned int straw_count=0,ring_count=0;
114 if (jcalib->Get(
"CDC/sag_parameters", tvals)==
false){
115 vector<double>
temp,temp2;
116 for(
unsigned int i=0; i<tvals.size(); i++){
117 map<string, double> &row = tvals[i];
119 temp.push_back(row[
"offset"]);
120 temp2.push_back(row[
"phi"]);
123 if (straw_count==numstraws[ring_count]){
135 vector<double>cdc_origin;
136 vector<double>cdc_center;
137 vector<double>cdc_upstream_endplate_pos;
138 vector<double>cdc_endplate_dim;
139 geom->
Get(
"//posXYZ[@volume='CentralDC'/@X_Y_Z",cdc_origin);
140 geom->
Get(
"//posXYZ[@volume='centralDC']/@X_Y_Z",cdc_center);
141 geom->
Get(
"//posXYZ[@volume='CDPU']/@X_Y_Z",cdc_upstream_endplate_pos);
142 geom->
Get(
"//tubs[@name='CDPU']/@Rio_Z",cdc_endplate_dim);
143 cdc_origin[2]+=cdc_center[2]+cdc_upstream_endplate_pos[2]
144 +0.5*cdc_endplate_dim[2];
147 double endplate_dz=0.;
155 vector<double>tof_face;
156 geom->
Get(
"//section/composition/posXYZ[@volume='ForwardTOF']/@X_Y_Z",
158 vector<double>tof_plane;
159 geom->
Get(
"//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='0']", tof_plane);
160 dTOFz=tof_face[2]+tof_plane[2];
161 geom->
Get(
"//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='1']", tof_plane);
162 dTOFz+=tof_face[2]+tof_plane[2];
168 for (
int i=0;i<30;i++){
169 vector<DVector3>
temp;
170 for (
unsigned int j=0;j<
sc_pos[i].size()-1;j++){
174 temp.push_back(
DVector3(dx/dz,dy/dz,1.));
188 gPARMS->SetDefaultParameter(
"TRKFIT:VERBOSE",
VERBOSE);
190 gPARMS->SetDefaultParameter(
"TRKFIND:COSMICS",
COSMICS);
192 gPARMS->SetDefaultParameter(
"TRKFIT:CHI2CUT",
CHI2CUT);
194 gPARMS->SetDefaultParameter(
"TRKFIT:DO_PRUNING",
DO_PRUNING);
222 if (a==NULL || b==NULL){
223 cout <<
"Null pointer in CDC hit list??" << endl;
271 double sqrt_t=
sqrt(my_t);
272 double t3=my_t*my_t*my_t;
273 double delta_mag=fabs(delta);
274 f_delta=(a1+a2*delta_mag)*sqrt_t+(b1+b2*delta_mag)*my_t
275 +(c1+c2*delta_mag+c3*delta*delta)*t3;
276 f_0=a1*sqrt_t+b1*my_t+c1*t3;
280 double sqrt_t=
sqrt(my_t);
281 double delta_mag=fabs(delta);
291 double delta_sq=delta*delta;
292 f_delta= (a1+a2*delta_mag+a3*delta_sq)*sqrt_t
293 +(b1+b2*delta_mag+b3*delta_sq)*my_t;
294 f_0=a1*sqrt_t+b1*my_t;
306 unsigned int index=0;
309 double frac=(t-cdc_drift_table[
index])/dt;
310 double d_0=0.01*(double(index)+frac);
317 d=f_delta*(d_0/f_0*P+1.-P);
324 vector<int>&used_hits,
325 vector<cdc_update_t>&updates,
334 double V=1.15*(0.78*0.78/12.);
336 const double d_EPS=1
e-8;
339 for (
unsigned int i=0;i<used_hits.size();i++) used_hits[i]=0;
352 unsigned int cdc_index=
cdchits.size()-1;
356 double z0=origin.z();
357 double vz=wire->
udir.z();
358 if (
VERBOSE) jout <<
" Starting in Ring " << wire->
ring << endl;
365 double old_doca2=dx*dx+dy*dy;
370 for (
unsigned int k=1;k<
trajectory.size();k++){
371 if (!C.
IsPosDef())
return UNRECOVERABLE_ERROR;
394 if (doca2>old_doca2 && more_hits){
403 double dx0=diff.x(),dy0=diff.y();
404 double wdir_dot_diff=diff.Dot(wdir);
405 double tdir_dot_diff=diff.Dot(tdir);
406 double tdir_dot_wdir=tdir.Dot(wdir);
407 double tdir2=tdir.Mag2();
408 double wdir2=wdir.Mag2();
409 double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir;
410 double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff;
411 double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff;
416 double d=diff.Mag()+d_EPS;
425 double phi_d=diff.Phi();
426 double dphi=phi_d-origin.Phi();
427 while (dphi>M_PI) dphi-=2*M_PI;
428 while (dphi<-M_PI) dphi+=2*M_PI;
430 int ring_index=
cdchits[cdc_index]->wire->ring-1;
431 int straw_index=
cdchits[cdc_index]->wire->straw-1;
432 double dz=t*wdir.z();
433 delta=
max_sag[ring_index][straw_index]*(1.-dz*dz/5625.)
440 if (
VERBOSE>5) jout <<
" Residual " << res << endl;
444 double one_over_d=1./d;
445 double diffx=diff.x(),diffy=diff.y(),diffz=diff.z();
446 double wx=wdir.x(),wy=wdir.y();
448 double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0;
449 double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx;
450 double dtdtx=scale*(dN1dtx-t*dDdtx);
452 double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0;
453 double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy;
454 double dtdty=scale*(dN1dty-t*dDdty);
456 double dNdtx=wx*wdir_dot_diff-wdir2*dx0;
457 double dsdtx=scale*(dNdtx-s*dDdtx);
459 double dNdty=wy*wdir_dot_diff-wdir2*dy0;
460 double dsdty=scale*(dNdty-s*dDdty);
463 =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx)
464 +diffz*(dsdtx-dtdtx));
466 =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty)
467 +diffz*(dsdty-dtdty));
469 double dsdx=scale*(tdir_dot_wdir*wx-wdir2*
tx);
470 double dtdx=scale*(tdir2*wx-tdir_dot_wdir*
tx);
471 double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty);
472 double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty);
475 =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy)
478 =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy)
481 double InvV=1./(V+H*C*H_T);
484 double chi2check=res*res*InvV;
486 if (
VERBOSE>5) jout <<
"Hit Added to track " << endl;
500 if (!Ctest.IsPosDef())
return VALUE_OUT_OF_RANGE;
513 double fit_V=V-H*C*H_T;
518 updates[cdc_index].V=fit_V;
521 updates[cdc_index].V=V;
525 used_hits[cdc_index]=1;
528 updates[cdc_index].resi=res;
529 updates[cdc_index].d=d;
530 updates[cdc_index].delta=delta;
531 updates[cdc_index].S=
S;
532 updates[cdc_index].C=C;
533 updates[cdc_index].tdrift=tdrift;
534 updates[cdc_index].ddrift=dmeas;
545 jout <<
" Next Wire ring " << wire->
ring <<
" straw " << wire->
straw <<
" origin udir" << endl;
551 wdir=(1./vz)*wire->
udir;
560 else more_hits=
false;
565 if (ndof<=4)
return VALUE_OUT_OF_RANGE;
584 const double d_EPS=1
e-8;
586 for (
unsigned int m=max-1;m>0;m--){
589 A=cdc_updates[id].C*JT*C.
Invert();
590 Ss=cdc_updates[id].S+A*(Ss-
S);
593 Cs=cdc_updates[id].C+dC;
595 jout <<
" In Smoothing Step Using ID " <<
id <<
"/" << cdc_updates.size() <<
" for ring " <<
cdchits[id]->wire->ring << endl;
596 jout <<
" A cdc_updates[id].C Ss Cs " << endl;
600 if (
VERBOSE) jout <<
"Cs is not PosDef!" << endl;
601 return VALUE_OUT_OF_RANGE;
606 double z0=origin.z();
607 double vz=wire->
udir.z();
621 double dx0=diff.x(),dy0=diff.y();
622 double wdir_dot_diff=diff.Dot(wdir);
623 double tdir_dot_diff=diff.Dot(tdir);
624 double tdir_dot_wdir=tdir.Dot(wdir);
625 double tdir2=tdir.Mag2();
626 double wdir2=wdir.Mag2();
627 double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir;
628 double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff;
629 double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff;
634 double d=diff.Mag()+d_EPS;
635 double ddrift = cdc_updates[id].ddrift;
637 double resi = ddrift - d;
641 double one_over_d=1./d;
642 double diffx=diff.x(),diffy=diff.y(),diffz=diff.z();
643 double wx=wdir.x(),wy=wdir.y();
645 double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0;
646 double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx;
647 double dtdtx=scale*(dN1dtx-t*dDdtx);
649 double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0;
650 double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy;
651 double dtdty=scale*(dN1dty-t*dDdty);
653 double dNdtx=wx*wdir_dot_diff-wdir2*dx0;
654 double dsdtx=scale*(dNdtx-s*dDdtx);
656 double dNdty=wy*wdir_dot_diff-wdir2*dy0;
657 double dsdty=scale*(dNdty-s*dDdty);
660 =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx)
661 +diffz*(dsdtx-dtdtx));
663 =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty)
664 +diffz*(dsdty-dtdty));
666 double dsdx=scale*(tdir_dot_wdir*wx-wdir2*
tx);
667 double dtdx=scale*(tdir2*wx-tdir_dot_wdir*
tx);
668 double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty);
669 double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty);
672 =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy)
675 =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy)
678 double V=cdc_updates[id].V;
680 if (
VERBOSE > 10) jout <<
" d " << d <<
" H*S " << H*S << endl;
687 if (V<0)
return VALUE_OUT_OF_RANGE;
690 double myscale=1./
sqrt(1.+tx*tx+ty*ty);
691 double cosThetaRel=wire->
udir.Dot(
DVector3(myscale*tx,myscale*ty,myscale));
694 cdc_updates[
id].tdrift,
698 cdc_updates[id].tdrift);
701 double wtx=wire->
udir.X(), wty=wire->
udir.Y(), wtz=wire->
udir.Z();
705 double tx2=tx*
tx, ty2=ty*ty;
706 double wtx2=wtx*wtx, wty2=wty*wty, wtz2=wtz*wtz;
707 double denom=(1 + ty2)*wtx2 + (1 + tx2)*wty2 - 2*ty*wty*wtz + (tx2 + ty2)*wtz2 - 2*tx*wtx*(ty*wty + wtz) +d_EPS;
708 double denom2=denom*denom;
709 double c1=-(wtx - tx*wtz)*(wy - y);
710 double c2=wty*(wx - tx*wz - x + tx*z);
711 double c3=ty*(-(wtz*wx) + wtx*wz + wtz*x - wtx*z);
712 double dscale=0.5*(1./d);
714 vector<double> derivatives(11);
723 (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) +
724 wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy +
y) + wtz*(-wz + z))))/denom2;
727 (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) +
728 wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy +
y) + wtz*(-wz + z))))/denom2;
731 (-(tx*(ty*wty + wtz)*(wx - x)) + tx2*(wty*(wy - y) + wtz*(wz - z)) + (wty - ty*wtz)*(wy - y + ty*(-wz + z)) +
732 wtx*((1 + ty2)*wx - (1 + ty2)*x + tx*(-(ty*wy) - wz + ty*y + z))))/denom2;
739 (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z +
740 tx*(wty2*(wx - x) + wtx*wty*(-wy +
y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) +
741 ty*(wtx*wty*(-wx + x) + wtx2*(wy -
y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2;
744 (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z +
745 tx*(wty2*(wx - x) + wtx*wty*(-wy +
y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) +
746 ty*(wtx*wty*(-wx + x) + wtx2*(wy -
y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2;
748 thisPull.AddTrackDerivatives(derivatives);
749 pulls.push_back(thisPull);
770 DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.);
777 double last_r2=last_cdc->
wire->
origin.Perp2();
780 if (fabs(dir.Theta() - M_PI_2) < 0.2) ds = 0.1;
786 if (
COSMICS) jout <<
" Swimming Reference Trajectory last CDC y " << last_y <<
" dz "<< dz << endl;
787 else jout <<
" Swimming Reference Trajectory last CDC r2 " << last_r2 <<
" dz "<< dz << endl;
788 jout <<
" S" << endl; S.
Print(); jout <<
"z= "<< z << endl;
789 jout <<
" Last CDC ring " << last_cdc->
wire->
ring <<
" straw " << last_cdc->
wire->
straw << endl;
791 unsigned int numsteps=0;
806 if (z < upstreamEndplate && dz > 0)
continue;
812 done |= (r2>last_r2);
814 }
while (!done && numsteps<MAX_STEPS);
820 for (
unsigned int i=0;i<
trajectory.size();i++){
826 printf(
"%i step trajectory Begin/End:\n", numsteps);
832 if (
trajectory.size()<2)
return UNRECOVERABLE_ERROR;
844 double pz=input_mom.z();
845 DMatrix4x1 S(input_pos.x(),input_pos.y(),input_mom.x()/pz,input_mom.y()/pz);
859 double z=input_pos.z();
882 double dzsign=(pz>0)?1.:-1.;
889 double phi=atan2(ty,tx);
890 double tanl=1./
sqrt(tx*tx+ty*ty);
894 if (phi_diff<-M_PI) phi_diff+=2.*M_PI;
895 if (phi_diff> M_PI) phi_diff-=2.*M_PI;
896 if (fabs(phi_diff)<M_PI_2){
901 double pt=5.0*cos(atan(tanl));
905 TMatrixFSym errMatrix(5);
906 for(
unsigned int loc_i = 0; loc_i < 4; ++loc_i){
907 for(
unsigned int loc_j = 0; loc_j < 4; ++loc_j){
908 errMatrix(loc_i, loc_j) = C(loc_i, loc_j);
914 double sign=(mom.Theta()>M_PI_2)?-1.:1.;
925 DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.);
942 locTrackingCovarianceMatrix->ResizeTo(5, 5);
943 locTrackingCovarianceMatrix->Zero();
944 for(
unsigned int loc_i = 0; loc_i < 4; ++loc_i){
945 for(
unsigned int loc_j = 0; loc_j < 4; ++loc_j){
946 (*locTrackingCovarianceMatrix)(loc_i, loc_j) = C(loc_i, loc_j);
949 (*locTrackingCovarianceMatrix)(4,4)=1.;
963 if (x==xx[0])
return 0;
964 else if (x==xx[n-1])
return n-2;
968 int ascnd=(xx[n-1]>=xx[0]);
971 if ( (x>=xx[jm])==ascnd)
990 double chi2=chi2_best;
993 unsigned int numhits=
cdchits.size();
994 unsigned int maxindex=numhits-1;
997 vector<int> used_cdc_hits(numhits);
998 vector<int> used_cdc_hits_best_fit(numhits);
1001 vector<cdc_update_t>updates(numhits);
1002 vector<cdc_update_t>best_updates(numhits);
1009 for(iter=0;iter<20;iter++){
1010 if (
VERBOSE) jout <<
" Performing Pass iter " << iter << endl;
1016 if (
VERBOSE) jout <<
" Reference Trajectory Set " << endl;
1018 if (
KalmanFilter(S,C,used_cdc_hits,updates,chi2,ndof,iter)!=NOERROR)
break;
1019 if (
VERBOSE) jout <<
" Filter returns NOERROR" << endl;
1020 if (iter>0 && (fabs(chi2_best-chi2)<0.1 || chi2>chi2_best))
break;
1028 used_cdc_hits_best_fit.assign(used_cdc_hits.begin(),used_cdc_hits.end());
1029 best_updates.assign(updates.begin(),updates.end());
1044 for (
unsigned int m=0;m<used_cdc_hits_best_fit.size();m++){
1045 if (used_cdc_hits_best_fit[m]){
1060 if (time<0.)
return 0.;
1062 double tsq=time*time;
1070 double t_high_sq=t_high*t_high;
1098 unsigned int numfdchits=
fdchits.size();
1099 vector<int> used_fdc_hits(numfdchits);
1100 vector<int> used_fdc_hits_best_fit(numfdchits);
1102 unsigned int numcdchits=
cdchits.size();
1103 vector<int> used_cdc_hits(numcdchits);
1104 vector<int> used_cdc_hits_best_fit(numcdchits);
1107 vector<fdc_update_t>updates(numfdchits);
1108 vector<fdc_update_t>best_updates(numfdchits);
1109 vector<cdc_update_t>cdc_updates(numcdchits);
1110 vector<cdc_update_t>best_cdc_updates(numcdchits);
1112 vector<const DCDCTrackHit *> matchedCDCHits;
1115 double chi2=chi2_best;
1123 for(iter=0;iter<20;iter++){
1128 if (
KalmanFilter(S,C,used_fdc_hits,used_cdc_hits,updates,cdc_updates,
1129 chi2,ndof)!=NOERROR)
break;
1132 if (iter>0 && (chi2>chi2_best || fabs(chi2_best-chi2)<0.1))
break;
1140 used_cdc_hits_best_fit.assign(used_cdc_hits.begin(),used_cdc_hits.end());
1141 used_fdc_hits_best_fit.assign(used_fdc_hits.begin(),used_fdc_hits.end());
1142 best_updates.assign(updates.begin(),updates.end());
1143 best_cdc_updates.assign(cdc_updates.begin(),cdc_updates.end());
1158 for (
unsigned int m=0;m<used_cdc_hits_best_fit.size();m++){
1159 if (used_cdc_hits_best_fit[m]){
1164 for (
unsigned int m=0;m<used_fdc_hits_best_fit.size();m++){
1165 if (used_fdc_hits_best_fit[m]){
1179 const double EPS=1
e-3;
1182 DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.);
1190 for (
unsigned int i=0;i<
fdchits.size();i++){
1191 zhit=
fdchits[i]->wire->origin.z();
1194 if (fabs(zhit-old_zhit)<EPS && i>0){
1243 for (
unsigned int i=0;i<
trajectory.size();i++){
1255 vector<int>&used_fdc_hits,
1256 vector<int>&used_cdc_hits,
1257 vector<fdc_update_t>&updates,
1258 vector<cdc_update_t>&cdc_updates,
1259 double &chi2,
int &ndof){
1275 const double d_EPS=1
e-8;
1278 for (
unsigned int i=0;i<used_fdc_hits.size();i++) used_fdc_hits[i]=0;
1279 for (
unsigned int i=0;i<used_cdc_hits.size();i++) used_cdc_hits[i]=0;
1294 bool more_hits =
cdchits.size() == 0 ?
false:
true;
1295 bool firstCDCStep=
true;
1296 unsigned int cdc_index=0;
1299 double doca2=0.0, old_doca2=0.0;
1302 wire=
cdchits[cdc_index]->wire;
1304 double vz=wire->
udir.z();
1305 if (
VERBOSE) jout <<
" Additional CDC Hits in FDC track Starting in Ring " << wire->
ring << endl;
1306 wdir=(1./vz)*wire->
udir;
1309 for (
unsigned int k=1;k<
trajectory.size();k++){
1310 if (C(0,0)<=0. || C(1,1)<=0. || C(2,2)<=0. || C(3,3)<=0.)
1311 return UNRECOVERABLE_ERROR;
1329 double cospsi=cos(
fdchits[
id]->wire->angle);
1339 if (std::isnan(x) || std::isnan(y))
return UNRECOVERABLE_ERROR;
1346 double vpred_wire_plane=y*cospsi+x*sinpsi;
1347 double upred_wire_plane=x*cospsi-y*sinpsi;
1348 double tu=tx*cospsi-ty*sinpsi;
1349 double tv=tx*sinpsi+ty*cospsi;
1353 double alpha=atan(tu);
1354 double cosalpha=cos(alpha);
1355 double cos2_alpha=cosalpha*cosalpha;
1356 double sinalpha=
sin(alpha);
1357 double sin2_alpha=sinalpha*sinalpha;
1358 double cos2_alpha_minus_sin2_alpha=cos2_alpha-sin2_alpha;
1361 for (
int m=
trajectory[k].numhits-1;m>=0;m--){
1362 unsigned int my_id=
id+m;
1363 double uwire=
fdchits[my_id]->w;
1365 double du=upred_wire_plane-uwire;
1366 double doca=du*cosalpha;
1369 double vpred=vpred_wire_plane;
1382 Mdiff(0)=drift-doca;
1386 H_T(
state_x,0)=cospsi*cosalpha;
1387 H_T(
state_y,0)=-sinpsi*cosalpha;
1388 double temp=-du*sinalpha*cos2_alpha;
1391 double temp2=cosalpha*sinalpha*tv;
1392 H_T(
state_x,1)=sinpsi-temp2*cospsi;
1393 H_T(
state_y,1)=cospsi+temp2*sinpsi;
1394 double temp4=sinalpha*doca;
1395 double temp5=tv*cos2_alpha*du*cos2_alpha_minus_sin2_alpha;
1396 H_T(
state_tx,1)=-sinpsi*temp4-cospsi*temp5;
1397 H_T(
state_ty,1)=-cospsi*temp4+sinpsi*temp5;
1410 InvV=(V+H*C*H_T).Invert();
1436 updates[my_id].V=RC;
1442 used_fdc_hits[my_id]=1;
1445 updates[my_id].d=doca;
1455 double z0=origin.Z();
1464 if (doca2>old_doca2 && more_hits && !firstCDCStep){
1473 double dx0=diff.x(),dy0=diff.y();
1474 double wdir_dot_diff=diff.Dot(wdir);
1475 double tdir_dot_diff=diff.Dot(tdir);
1476 double tdir_dot_wdir=tdir.Dot(wdir);
1477 double tdir2=tdir.Mag2();
1478 double wdir2=wdir.Mag2();
1479 double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir;
1480 double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff;
1481 double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff;
1485 diff+=s*tdir-t*wdir;
1486 double d=diff.Mag()+d_EPS;
1492 double phi_d=diff.Phi();
1493 double dphi=phi_d-origin.Phi();
1494 while (dphi>M_PI) dphi-=2*M_PI;
1495 while (dphi<-M_PI) dphi+=2*M_PI;
1497 int ring_index=
cdchits[cdc_index]->wire->ring-1;
1498 int straw_index=
cdchits[cdc_index]->wire->straw-1;
1499 double dz=t*wdir.z();
1500 double delta=
max_sag[ring_index][straw_index]*(1.-dz*dz/5625.)
1506 if (
VERBOSE>5) jout <<
" Residual " << res << endl;
1508 double one_over_d=1./d;
1509 double diffx=diff.x(),diffy=diff.y(),diffz=diff.z();
1510 double wx=wdir.x(),wy=wdir.y();
1512 double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0;
1513 double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx;
1514 double dtdtx=scale*(dN1dtx-t*dDdtx);
1516 double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0;
1517 double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy;
1518 double dtdty=scale*(dN1dty-t*dDdty);
1520 double dNdtx=wx*wdir_dot_diff-wdir2*dx0;
1521 double dsdtx=scale*(dNdtx-s*dDdtx);
1523 double dNdty=wy*wdir_dot_diff-wdir2*dy0;
1524 double dsdty=scale*(dNdty-s*dDdty);
1527 =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx)
1528 +diffz*(dsdtx-dtdtx));
1530 =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty)
1531 +diffz*(dsdty-dtdty));
1533 double dsdx=scale*(tdir_dot_wdir*wx-wdir2*
tx);
1534 double dtdx=scale*(tdir2*wx-tdir_dot_wdir*
tx);
1535 double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty);
1536 double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty);
1539 =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy)
1540 +diffz*(dsdx-dtdx));
1542 =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy)
1543 +diffz*(dsdy-dtdy));
1545 double InvV=1./(V_CDC+H_CDC*C*H_T_CDC);
1548 double chi2check=res*res*InvV;
1550 if (
VERBOSE) jout <<
"CDC Hit Added to FDC track " << endl;
1554 K_CDC=InvV*(C*H_T_CDC);
1563 if (!Ctest.IsPosDef())
return VALUE_OUT_OF_RANGE;
1573 res=res-H_CDC*K_CDC*res;
1576 double fit_V=V_CDC-H_CDC*C*H_T_CDC;
1577 chi2+=res*res/fit_V;
1581 cdc_updates[cdc_index].V=fit_V;
1584 cdc_updates[cdc_index].V=V_CDC;
1588 cdc_updates[cdc_index].resi=res;
1589 cdc_updates[cdc_index].d=d;
1590 cdc_updates[cdc_index].delta=delta;
1591 cdc_updates[cdc_index].S=
S;
1592 cdc_updates[cdc_index].C=C;
1593 cdc_updates[cdc_index].tdrift=tdrift;
1594 cdc_updates[cdc_index].ddrift=dmeas;
1595 cdc_updates[cdc_index].s=29.98*
trajectory[k].t;
1598 used_cdc_hits[cdc_index]=1;
1605 wire=
cdchits[cdc_index]->wire;
1606 if (
VERBOSE>5) jout <<
" Next Wire ring " << wire->
ring << endl;
1608 double vz=wire->
udir.z();
1609 wdir=(1./vz)*wire->
udir;
1618 else more_hits=
false;
1638 vector<cdc_update_t>&cdc_updates){
1647 const double d_EPS=1
e-8;
1649 for (
unsigned int m=max-1;m>0;m--){
1652 A=fdc_updates[id].C*JT*C.
Invert();
1653 Ss=fdc_updates[id].S+A*(Ss-
S);
1656 Cs=fdc_updates[id].C+dC;
1658 double cosa=cos(
fdchits[
id]->wire->angle);
1659 double cos2a=cos(2*
fdchits[
id]->wire->angle);
1672 double vpred=x*sina+y*cosa;
1675 double upred=x*cosa-y*sina;
1678 double tu=tx*cosa-ty*sina;
1679 double alpha=atan(tu);
1680 double cosalpha=cos(alpha);
1682 double sinalpha=
sin(alpha);
1686 double doca=du*cosalpha;
1688 double tv=tx*sina+ty*cosa;
1689 double resi_c=v-vpred;
1694 double drift_time=fdc_updates[id].tdrift;
1697 double resi_a=drift-doca;
1703 double temp2=-tv*sinalpha;
1704 H_T(
state_x,1)=sina+cosa*cosalpha*temp2;
1705 H_T(
state_y,1)=cosa-sina*cosalpha*temp2;
1707 double cos2_minus_sin2=cosalpha*cosalpha-sinalpha*sinalpha;
1708 double doca_cosalpha=doca*cosalpha;
1709 H_T(
state_tx,1)=-doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2);
1710 H_T(
state_ty,1)=-doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2);
1713 H_T(
state_y,0)=-sina*cosalpha;
1714 double one_plus_tu2=1.+tu*tu;
1715 double factor=du*tu/
sqrt(one_plus_tu2)/one_plus_tu2;
1745 double scale=1./
sqrt(1.+tx*tx+ty*ty);
1746 double cosThetaRel=
fdchits[id]->wire->udir.Dot(
DVector3(scale*tx,scale*ty,scale));
1749 fdc_updates[
id].tdrift,
1755 resi_c,
sqrt(V(1,1))
1759 vector<double> derivatives;
1776 cosa*(-(tx*ty*x) + y + pow(tx,2)*y + (pow(tx,2) - pow(ty,2))*u*sina))/
1777 pow(1 + pow(tx*cosa - ty*sina,2),1.5);
1786 derivatives[
FDCTrackD::dDOCAW_dtx] = -((cosa*(tx*cosa - ty*sina)*(-u + x*cosa - y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5));
1789 derivatives[
FDCTrackD::dDOCAW_dty] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5);
1821 pulls.push_back(thisPull);
1825 A=cdc_updates[id].C*JT*C.
Invert();
1826 Ss=cdc_updates[id].S+A*(Ss-
S);
1829 Cs=cdc_updates[id].C+dC;
1831 jout <<
" In Smoothing Step Using ID " <<
id <<
"/" << cdc_updates.size() <<
" for ring " <<
cdchits[id]->wire->ring << endl;
1832 jout <<
" A cdc_updates[id].C Ss Cs " << endl;
1836 if (
VERBOSE) jout <<
"Cs is not PosDef!" << endl;
1837 return VALUE_OUT_OF_RANGE;
1842 double z0=origin.z();
1843 double vz=wire->
udir.z();
1857 double dx0=diff.x(),dy0=diff.y();
1858 double wdir_dot_diff=diff.Dot(wdir);
1859 double tdir_dot_diff=diff.Dot(tdir);
1860 double tdir_dot_wdir=tdir.Dot(wdir);
1861 double tdir2=tdir.Mag2();
1862 double wdir2=wdir.Mag2();
1863 double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir;
1864 double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff;
1865 double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff;
1869 diff+=s*tdir-t*wdir;
1870 double d=diff.Mag()+d_EPS;
1871 double ddrift = cdc_updates[id].ddrift;
1873 double resi = ddrift - d;
1879 double one_over_d=1./d;
1880 double diffx=diff.x(),diffy=diff.y(),diffz=diff.z();
1881 double wx=wdir.x(),wy=wdir.y();
1883 double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0;
1884 double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx;
1885 double dtdtx=scale*(dN1dtx-t*dDdtx);
1887 double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0;
1888 double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy;
1889 double dtdty=scale*(dN1dty-t*dDdty);
1891 double dNdtx=wx*wdir_dot_diff-wdir2*dx0;
1892 double dsdtx=scale*(dNdtx-s*dDdtx);
1894 double dNdty=wy*wdir_dot_diff-wdir2*dy0;
1895 double dsdty=scale*(dNdty-s*dDdty);
1898 =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx)
1899 +diffz*(dsdtx-dtdtx));
1901 =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty)
1902 +diffz*(dsdty-dtdty));
1904 double dsdx=scale*(tdir_dot_wdir*wx-wdir2*
tx);
1905 double dtdx=scale*(tdir2*wx-tdir_dot_wdir*
tx);
1906 double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty);
1907 double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty);
1910 =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy)
1911 +diffz*(dsdx-dtdx));
1913 =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy)
1914 +diffz*(dsdy-dtdy));
1916 double V=cdc_updates[id].V;
1918 if (
VERBOSE > 10) jout <<
" d " << d <<
" H*S " << H*S << endl;
1925 if (V<0)
return VALUE_OUT_OF_RANGE;
1928 double myscale=1./
sqrt(1.+tx*tx+ty*ty);
1929 double cosThetaRel=wire->
udir.Dot(
DVector3(myscale*tx,myscale*ty,myscale));
1932 cdc_updates[
id].tdrift,
1936 cdc_updates[id].tdrift);
1939 double wtx=wire->
udir.X(), wty=wire->
udir.Y(), wtz=wire->
udir.Z();
1943 double tx2=tx*
tx, ty2=ty*ty;
1944 double wtx2=wtx*wtx, wty2=wty*wty, wtz2=wtz*wtz;
1945 double denom=(1 + ty2)*wtx2 + (1 + tx2)*wty2 - 2*ty*wty*wtz + (tx2 + ty2)*wtz2 - 2*tx*wtx*(ty*wty + wtz)+d_EPS;
1946 double denom2=denom*denom;
1947 double c1=-(wtx - tx*wtz)*(wy - y);
1948 double c2=wty*(wx - tx*wz - x + tx*z);
1949 double c3=ty*(-(wtz*wx) + wtx*wz + wtz*x - wtx*z);
1950 double dscale=0.5*(1/d);
1952 vector<double> derivatives(11);
1961 (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) +
1962 wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy +
y) + wtz*(-wz + z))))/denom2;
1965 (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) +
1966 wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy +
y) + wtz*(-wz + z))))/denom2;
1969 (-(tx*(ty*wty + wtz)*(wx - x)) + tx2*(wty*(wy - y) + wtz*(wz - z)) + (wty - ty*wtz)*(wy - y + ty*(-wz + z)) +
1970 wtx*((1 + ty2)*wx - (1 + ty2)*x + tx*(-(ty*wy) - wz + ty*y + z))))/denom2;
1977 (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z +
1978 tx*(wty2*(wx - x) + wtx*wty*(-wy +
y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) +
1979 ty*(wtx*wty*(-wx + x) + wtx2*(wy -
y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2;
1982 (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z +
1983 tx*(wty2*(wx - x) + wtx*wty*(-wy +
y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) +
1984 ty*(wtx*wty*(-wx + x) + wtx2*(wy -
y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2;
1986 thisPull.AddTrackDerivatives(derivatives);
1988 pulls.push_back(thisPull);
2004 shared_ptr<TMatrixFSym>
2007 C7x7->ResizeTo(7, 7);
2015 double tanl=sign/
sqrt(tx_*tx_+ty_*ty_);
2016 double tanl2=tanl*tanl;
2017 double lambda=atan(tanl);
2018 double sinl=
sin(lambda);
2019 double sinl3=sinl*sinl*sinl;
2032 double diff=tx_*tx_-ty_*ty_;
2033 double frac=tanl2*tanl2;
2038 *C7x7=C.Similarity(J);
2053 double s=diff.Mag();
2078 double R=pos0.Perp();
2081 while (R<89.0 && z>17. && z<410.){
2082 diff+=(1./dir.z())*dir;
2091 double d_old=1000.,d=1000.;
2092 unsigned int index=0;
2093 for (
unsigned int m=0;m<12;m++){
2095 if (dphi<0) dphi+=2.*M_PI;
2096 index=int(floor(dphi/(2.*M_PI/30.)));
2097 if (index>29) index=0;
2104 while (fabs(d)>0.05 && count<20){
2107 if (dphi<0) dphi+=2.*M_PI;
2108 index=int(floor(dphi/(2.*M_PI/30.)));
deque< trajectory_t > best_trajectory
void setMomentum(const DVector3 &aMomentum)
shared_ptr< TMatrixFSym > Get7x7ErrorMatrix(TMatrixFSym C, DMatrix4x1 &S, double sign)
DTrackFitter::fit_status_t FitCentralTrack(double &z0, double t0, double dzsign, DMatrix4x1 &Sbest, DMatrix4x4 &Cbest, double &chi2_best, int &ndof_best)
The DTrackFitter class is a base class for different charged track fitting algorithms. It does not actually fit the track itself, but provides the interface and some common support features most algorthims will need to implement.
vector< vector< DVector3 > > sc_dir
void ClearExtrapolations(void)
double fdc_drift_variance(double t) const
unsigned int Locate(const vector< double > &xx, double x) const
shared_ptr< DResourcePool< TMatrixFSym > > dResourcePool_TMatrixFSym
bool Get(string xpath, string &sval) const
const DVector3 & position(void) const
void setTrackingErrorMatrix(const shared_ptr< const TMatrixFSym > &aMatrix)
const DFDCWire * wire
DFDCWire for this wire.
void setForwardParmFlag(bool aFlag)
bool DTrackFitterStraightTrack_fdc_hit_cmp(const DFDCPseudo *a, const DFDCPseudo *b)
class DFDCPseudo: definition for a reconstructed point in the FDC
map< DetectorSystem_t, vector< Extrapolation_t > > extrapolations
vector< const DFDCPseudo * > fdchits_used_in_fit
bool GetFCALZ(double &z_fcal) const
z-location of front face of CCAL in cm
static char index(char c)
~DTrackFitterStraightTrack()
double short_drift_func[3][3]
jerror_t Smooth(vector< cdc_update_t > &cdc_updates)
double CDCDriftDistance(double dphi, double delta, double t) const
vector< vector< double > > max_sag
vector< vector< double > > sag_phi_offset
vector< double > cdc_drift_table
void setErrorMatrix(const shared_ptr< const TMatrixFSym > &aMatrix)
DTrackFitter::fit_status_t FitForwardTrack(double t0, double &start_z, DMatrix4x1 &Sbest, DMatrix4x4 &Cbest, double &chi2_best, int &ndof_best)
double DRIFT_RES_PARMS[3]
vector< const DCDCTrackHit * > cdchits
DTrackFitterStraightTrack(JEventLoop *loop)
const map< DetectorSystem_t, vector< Extrapolation_t > > & GetExtrapolations(void) const
jerror_t SetReferenceTrajectory(double t0, double z, DMatrix4x1 &S, const DCDCTrackHit *last_cdc, double &dzsign)
fit_status_t FitTrack(void)
vector< vector< DVector3 > > sc_pos
DGeometry * GetDGeometry(unsigned int run_number)
Double_t sigma[NCHANNELS]
vector< const DCDCTrackHit * > cdchits_used_in_fit
bool DTrackFitterStraightTrack_cdc_hit_reverse_cmp(const DCDCTrackHit *a, const DCDCTrackHit *b)
bool DTrackFitterStraightTrack_cdc_hit_radius_cmp(const DCDCTrackHit *a, const DCDCTrackHit *b)
vector< const DFDCPseudo * > fdchits
vector< vector< DVector3 > > sc_norm
deque< trajectory_t > trajectory
void AddTrackDerivatives(vector< double > d)
const DVector3 & momentum(void) const
double fdc_drift_distance(double time) const
DTrackingData input_params
double long_drift_func[3][3]
bool GetCDCEndplate(double &z, double &dz, double &rmin, double &rmax) const
bool GetDIRCZ(double &z_dirc) const
z-location of DIRC in cm
void setPosition(const DVector3 &aPosition)
jerror_t KalmanFilter(DMatrix4x1 &S, DMatrix4x4 &C, vector< int > &used_hits, vector< cdc_update_t > &updates, double &chi2, int &ndof, unsigned int iter)
double CDCDriftVariance(double t) const
double FindDoca(double z, const DMatrix4x1 &S, const DVector3 &wdir, const DVector3 &origin, DVector3 *poca=NULL) const
printf("string=%s", string)
double downstreamEndplate
double DRIFT_FUNC_PARMS[6]
bool DTrackFitterStraightTrack_cdc_hit_cmp(const DCDCTrackHit *a, const DCDCTrackHit *b)
bool GetStartCounterGeom(vector< vector< DVector3 > > &pos, vector< vector< DVector3 > > &norm) const