22 #define ONE_THIRD 0.33333333333333333
23 #define TWO_THIRD 0.66666666666666667
25 #define QuietNaN std::numeric_limits<double>::quiet_NaN()
40 this->step_size = step_size;
41 this->bfield = bfield;
42 this->Nswim_steps = 0;
43 this->dist_to_rt_depth = 0;
45 this->mass_sq=this->mass*this->mass;
46 this->hit_cdc_endplate =
false;
49 this->ploss_direction = kForward;
50 this->check_material_boundaries =
true;
51 this->zmin_track_boundary = -100.0;
52 this->zmax_track_boundary = 670.0;
53 this->Rsqmax_interior = 65.0*65.0;
54 this->Rsqmax_exterior = 88.0*88.0;
57 this->last_swim_step = NULL;
58 this->last_dist_along_wire = 0.0;
59 this->last_dz_dphi = 0.0;
61 this->debug_level = 0;
64 BOUNDARY_STEP_FRACTION = 0.80;
67 int MAX_SWIM_STEPS = 2500;
69 gPARMS->SetDefaultParameter(
"TRK:BOUNDARY_STEP_FRACTION" , BOUNDARY_STEP_FRACTION,
"Fraction of estimated distance to boundary to use as step size");
70 gPARMS->SetDefaultParameter(
"TRK:MIN_STEP_SIZE" ,
MIN_STEP_SIZE,
"Minimum step size in cm to take when swimming a track with adaptive step sizes");
71 gPARMS->SetDefaultParameter(
"TRK:MAX_STEP_SIZE" , MAX_STEP_SIZE,
"Maximum step size in cm to take when swimming a track with adaptive step sizes");
72 gPARMS->SetDefaultParameter(
"TRK:MAX_SWIM_STEPS" , MAX_SWIM_STEPS,
"Number of swim steps for DReferenceTrajectory to allocate memory for (when not using external buffer)");
73 gPARMS->SetDefaultParameter(
"TRK:DEBUG_LEVEL" , this->debug_level);
81 own_swim_steps =
true;
82 this->max_swim_steps = MAX_SWIM_STEPS;
83 this->swim_steps =
new swim_step_t[this->max_swim_steps];
85 own_swim_steps =
false;
86 this->max_swim_steps = max_swim_steps;
87 this->swim_steps = swim_steps;
103 this->own_swim_steps =
true;
110 this->geom = rt.
geom;
111 this->dist_to_rt_depth = 0;
113 this->mass_sq=this->mass*this->mass;
120 this->zmin_track_boundary = -100.0;
121 this->zmax_track_boundary = 670.0;
122 this->Rsqmax_interior = 65.0*65.0;
123 this->Rsqmax_exterior = 88.0*88.0;
126 this->swim_steps =
new swim_step_t[this->max_swim_steps];
127 this->last_swim_step = NULL;
128 for(
int i=0; i<Nswim_steps; i++)
132 this->last_swim_step = &(swim_steps[i]);
150 if(&rt ==
this)
return *
this;
153 if(own_swim_steps==
true && max_swim_steps<rt.
Nswim_steps){
166 this->own_swim_steps =
true;
173 this->geom = rt.
geom;
176 this->mass_sq=this->mass*this->mass;
184 if(swim_steps==NULL)this->swim_steps =
new swim_step_t[this->max_swim_steps];
187 this->last_swim_step = NULL;
188 for(
int i=0; i<Nswim_steps; i++)
192 this->last_swim_step = &(swim_steps[i]);
218 for(
int i=0; i<Nswim_steps; i++)swim_steps[i].origin += shift;
227 this->Nswim_steps = 0;
228 this->ploss_direction = kForward;
229 this->mass = 0.13957;
230 this->mass_sq=this->mass*this->mass;
231 this->hit_cdc_endplate =
false;
232 this->last_phi = 0.0;
233 this->last_swim_step = NULL;
234 this->last_dist_along_wire = 0.0;
235 this->last_dz_dphi = 0.0;
236 this->dist_to_rt_depth = 0;
237 this->check_material_boundaries =
true;
247 double q,
double smax,
251 FastSwim(pos,mom,last_pos,last_mom,q,wire_origin,wire_dir,smax);
264 double s=0,doca=1000.,old_doca=1000.,dP_dx=0.;
272 double KrhoZ_overA=0.0;
273 double rhoZ_overA=0.0;
276 if (geom->FindMatALT1(mypos,mymom,KrhoZ_overA,rhoZ_overA,LogI,X0)
279 dP_dx = dPdx(mymom.Mag(), KrhoZ_overA, rhoZ_overA,LogI);
280 double my_step_size = 0.0001/fabs(dP_dx);
282 if(my_step_size>MAX_STEP_SIZE)my_step_size=MAX_STEP_SIZE;
285 if (ploss_direction==kBackward) my_step_size*=-1.;
290 double ds=stepper.
Step(NULL);
295 double ptot=mymom.Mag();
305 if (fabs(dir.z())>0.){
306 linepos+=((mypos.z()-origin.z())/dir.z())*dir;
308 doca=(linepos-mypos).Mag();
309 if (doca>old_doca)
break;
327 double itheta02 = 0.0;
328 double itheta02s = 0.0;
329 double itheta02s2 = 0.0;
333 for(
double s=0; fabs(s)<1000.; Nswim_steps++, swim_step++){
335 if(Nswim_steps>=this->max_swim_steps){
337 jerr<<__FILE__<<
":"<<__LINE__<<
" Too many steps in trajectory. Truncating..."<<endl;
345 swim_step->
Ro = stepper.
GetRo();
349 double Rsq=swim_step->
origin.Perp2();
350 double z=swim_step->
origin.Z();
351 if (z>345. || z<0. || Rsq>60.*60.){
352 Nswim_steps++;
break;
356 double p_sq=swim_step->
mom.Mag2();
357 double one_over_beta_sq=1.+mass_sq/p_sq;
363 double KrhoZ_overA=0.0;
364 double rhoZ_overA=0.0;
367 jerror_t err = geom->FindMatALT1(swim_step->
origin, swim_step->
mom, KrhoZ_overA, rhoZ_overA,LogI, X0);
372 if(last_step)delta_s -= last_step->
s;
373 double radlen = delta_s/
X0;
379 double factor=1.0+0.038*log(radlen);
380 double theta02=1.8496e-4*factor*factor*radlen*one_over_beta_sq/p_sq;
383 itheta02s += s*theta02;
384 itheta02s2 += s*s*theta02;
389 dP_dx = dPdx(p, KrhoZ_overA, rhoZ_overA,LogI);
392 last_step = swim_step;
397 swim_step->
invX0=Nswim_steps/X0sum;
401 double my_step_size = 0.0001/fabs(dP_dx);
402 if(my_step_size>MAX_STEP_SIZE)my_step_size=MAX_STEP_SIZE;
418 double ptot = mom.Mag() - dP;
419 if (ptot<0.005) {Nswim_steps++;
break;}
450 double itheta02 = 0.0;
451 double itheta02s = 0.0;
452 double itheta02s2 = 0.0;
455 double old_radius_sq=1e6;
462 bool hit_bcal=
false,hit_fcal=
false,hit_tof=
false;
464 for(
double s=0; fabs(s)<smax; Nswim_steps++, swim_step++){
466 if(Nswim_steps>=this->max_swim_steps){
468 jerr<<__FILE__<<
":"<<__LINE__<<
" Too many steps in trajectory. Truncating..."<<endl;
476 swim_step->
Ro = stepper.
GetRo();
481 double p_sq=swim_step->
mom.Mag2();
482 double one_over_beta_sq=1.+mass_sq/p_sq;
488 if(RootGeom || geom){
489 double KrhoZ_overA=0.0;
490 double rhoZ_overA=0.0;
495 double rhoZ_overA,rhoZ_overA_logI;
496 err = RootGeom->FindMatLL(swim_step->
origin,
500 KrhoZ_overA=0.1535e-3*rhoZ_overA;
501 LogI=rhoZ_overA_logI/rhoZ_overA;
503 err = geom->FindMatALT1(swim_step->
origin, swim_step->
mom, KrhoZ_overA, rhoZ_overA,LogI, X0);
509 if(last_step)delta_s -= last_step->
s;
510 double radlen = delta_s/
X0;
516 double factor=1.0+0.038*log(radlen);
517 double theta02=1.8496e-4*factor*factor*radlen*one_over_beta_sq/p_sq;
520 itheta02s += s*theta02;
521 itheta02s2 += s*s*theta02;
526 dP_dx = dPdx(p, KrhoZ_overA, rhoZ_overA,LogI);
529 last_step = swim_step;
534 swim_step->
invX0=Nswim_steps/X0sum;
539 double my_step_size = 0.0001/fabs(dP_dx);
540 if(my_step_size>MAX_STEP_SIZE)my_step_size=MAX_STEP_SIZE;
556 double ptot = mom.Mag() - dP;
557 if (ptot<0.005) {Nswim_steps++;
break;}
568 double Rsq=swim_step->
origin.Perp2();
569 double z=swim_step->
origin.Z();
570 if (hit_bcal==
false && Rsq>Rsqmax_interior && z<407 &&z>0){
571 index_at_bcal=Nswim_steps-1;
574 if (hit_tof==
false && z>606.){
575 index_at_tof=Nswim_steps-1;
578 if (hit_fcal==
false && z>625.){
579 index_at_fcal=Nswim_steps-1;
586 if (Rsq<old_radius_sq && Rsq>Rsqmax_interior && z<407.0 && z>-100.0){
587 Nswim_steps++;
break;
591 if (z>zmax){Nswim_steps++;
break;}
592 if(Rsq>Rsqmax_exterior && z<407.0){Nswim_steps++;
break;}
593 if (fabs(swim_step->
origin.X())>160.0
594 || fabs(swim_step->
origin.Y())>160.0)
595 {Nswim_steps++;
break;}
596 if(z>670.0){Nswim_steps++;
break;}
597 if(z<zmin){Nswim_steps++;
break;}
632 double itheta02 = 0.0;
633 double itheta02s = 0.0;
634 double itheta02s2 = 0.0;
637 double old_radius_sq=1e6;
639 TMatrixFSym mycov(7);
671 double Bz_old = B.z();
679 bool hit_bcal=
false,hit_fcal=
false,hit_tof=
false;
681 for(
double s=0; fabs(s)<smax; Nswim_steps++, swim_step++){
683 if(Nswim_steps>=this->max_swim_steps){
685 jerr<<__FILE__<<
":"<<__LINE__<<
" Too many steps in trajectory. Truncating..."<<endl;
692 swim_step->
Ro = stepper.
GetRo();
697 double p_sq=swim_step->
mom.Mag2();
698 double one_over_beta_sq=1.+mass_sq/p_sq;
704 double s_to_boundary=1.0E6;
705 if(RootGeom || geom){
706 double KrhoZ_overA=0.0;
707 double rhoZ_overA=0.0;
712 double rhoZ_overA,rhoZ_overA_logI;
713 err = RootGeom->FindMatLL(swim_step->
origin,
717 KrhoZ_overA=0.1535e-3*rhoZ_overA;
718 LogI=rhoZ_overA_logI/rhoZ_overA;
720 if(check_material_boundaries){
721 err = geom->FindMatALT1(swim_step->
origin, swim_step->
mom, KrhoZ_overA, rhoZ_overA,LogI, X0, &s_to_boundary);
723 err = geom->FindMatALT1(swim_step->
origin, swim_step->
mom, KrhoZ_overA, rhoZ_overA,LogI, X0);
740 if(last_step)delta_s -= last_step->
s;
741 double radlen = delta_s/
X0;
747 double factor=1.0+0.038*log(radlen);
748 double theta02=1.8496e-4*factor*factor*radlen*one_over_beta_sq/p_sq;
751 itheta02s += s*theta02;
752 itheta02s2 += s*s*theta02;
761 dP_dx = dPdx(p, KrhoZ_overA, rhoZ_overA,LogI);
764 last_step = swim_step;
769 swim_step->
invX0=Nswim_steps/X0sum;
775 double my_step_size = 0.0001/fabs(dP_dx);
781 if (fabs(Bz-Bz_old)>
EPS){
782 double my_step_size_B=0.01*my_step_size
783 *fabs(Bz/(Bz_old-Bz));
784 if (my_step_size_B<my_step_size)
785 my_step_size=my_step_size_B;
799 if(my_step_size>MAX_STEP_SIZE)my_step_size=MAX_STEP_SIZE;
806 double ds=stepper.
Step(NULL,&swim_step->
B);
808 PropagateCovariance(ds,q,mass_sq,mom,pos,swim_step->
B,mycov);
822 double ptot = mom.Mag() - dP;
823 bool ranged_out =
false;
830 if(ptot<0.005)ranged_out=
true;
831 if(dP<0.0 && ploss_direction==kForward)ranged_out=
true;
832 if(dP>0.0 && ploss_direction==kBackward)ranged_out=
true;
848 double Rsq=swim_step->
origin.Perp2();
849 double z=swim_step->
origin.Z();
850 if (hit_bcal==
false && Rsq>Rsqmax_interior && z<407 &&z>0){
851 index_at_bcal=Nswim_steps-1;
854 if (hit_tof==
false && z>618.){
855 index_at_tof=Nswim_steps-1;
858 if (hit_fcal==
false && z>625.){
859 index_at_fcal=Nswim_steps-1;
865 if (Rsq<old_radius_sq && Rsq>Rsqmax_interior && z<407.0 && z>-100.0){
866 Nswim_steps++;
break;
871 if(Rsq>Rsqmax_exterior && z<407.0){Nswim_steps++;
break;}
872 if (fabs(swim_step->
origin.X())>160.0
873 || fabs(swim_step->
origin.Y())>160.0)
874 {Nswim_steps++;
break;}
875 if(z>zmax_track_boundary){Nswim_steps++;
break;}
876 if(z<zmin_track_boundary){Nswim_steps++;
break;}
877 if(wire && Nswim_steps>0){
878 swim_step_t *closest_step = FindClosestSwimStep(wire);
879 if(++closest_step!=swim_step){Nswim_steps++;
break;}
899 if(p_at_intersection)
903 _DBG_<<
"No swim steps! You must \"Swim\" the track before calling GetIntersectionWithRadius(...)"<<endl;
906 double outer_radius=swim_steps[Nswim_steps-1].origin.Perp();
910 return VALUE_OUT_OF_RANGE;
914 double inner_radius=swim_steps[0].origin.Perp();
918 return VALUE_OUT_OF_RANGE;
927 for(
int i=0; i<Nswim_steps; i++, swim_step++){
928 if (swim_step->
origin.Perp()>R){
932 if (swim_step->
origin.Z()>407.0)
return VALUE_OUT_OF_RANGE;
935 if (step==NULL||last_step==NULL)
return VALUE_OUT_OF_RANGE;
936 if (p_at_intersection!=NULL){
937 *p_at_intersection=last_step->
mom;
951 double A = dx.Mod2();
952 double B = 2.0*(x1.X()*dx.X() + x1.Y()*dx.Y());
953 double C = x1.Mod2() - R*R;
955 double sqrt_D=
sqrt(B*B-4.0*A*C);
956 double one_over_denom=0.5/A;
957 double alpha1 = (-B + sqrt_D)*one_over_denom;
958 double alpha2 = (-B - sqrt_D)*one_over_denom;
959 double alpha = alpha1;
960 if(alpha1<0.0 || alpha1>1.0)alpha=alpha2;
961 if(!isfinite(alpha))
return VALUE_OUT_OF_RANGE;
964 mypos = last_step->
origin + alpha*delta;
969 if (s) *s = step->
s-(1.0-
alpha)*delta.Mag();
973 double p_sq=step->
mom.Mag2();
974 double one_over_beta=
sqrt(1.+mass_sq/p_sq);
986 return GetIntersectionWithPlane(origin,norm,pos,dummy,s,t,var_t,detector);
1007 p_at_intersection.SetXYZ(0,0,0);
1011 if (origin.z()>swim_steps[Nswim_steps-1].origin.z()){
1012 return VALUE_OUT_OF_RANGE;
1019 if (index_at_fcal<0)
return VALUE_OUT_OF_RANGE;
1020 first_i=index_at_fcal;
1023 if (index_at_tof<0)
return VALUE_OUT_OF_RANGE;
1024 first_i=index_at_tof;
1029 swim_step_t *step=FindPlaneCrossing(origin,norm,first_i,detector);
1031 _DBG_<<
"Could not find closest swim step!"<<endl;
1032 return RESOURCE_UNAVAILABLE;
1075 double &Ro = step->
Ro;
1090 double nx = norm.Dot(step->
sdir);
1091 double ny = norm.Dot(step->
tdir);
1092 double nz = norm.Dot(step->
udir);
1094 double delta_z = step->
mom.Dot(step->
udir);
1095 double delta_phi = step->
mom.Dot(step->
tdir)/Ro;
1096 double dz_dphi = delta_z/delta_phi;
1098 double A = -Ro*nx/2.0;
1099 double B = Ro*ny + dz_dphi*nz;
1100 double C = norm.Dot(step->
origin-origin);
1101 double sqroot=
sqrt(B*B-4.0*A*C);
1104 double phi_1 = (-B + sqroot)/(twoA);
1105 double phi_2 = (-B - sqroot)/(twoA);
1107 double phi = fabs(phi_1)<fabs(phi_2) ? phi_1:phi_2;
1108 if(!isfinite(phi_1))phi = phi_2;
1109 if(!isfinite(phi_2))phi = phi_1;
1112 double my_s = -Ro/2.0 * phi*phi;
1113 double my_t = Ro * phi;
1114 double my_u = dz_dphi * phi;
1117 p_at_intersection = step->
mom;
1119 double delta_s =
sqrt(my_t*my_t + my_u*my_u);
1120 *s = step->
s + (phi>0 ? +delta_s:-delta_s);
1124 double delta_s =
sqrt(my_t*my_t + my_u*my_u);
1125 double ds=(phi>0 ? +delta_s:-delta_s);
1126 double p_sq=step->
mom.Mag2();
1127 double one_over_beta=
sqrt(1.+mass_sq/p_sq);
1140 double p_sq=step->
mom.Mag2();
1141 double dz_over_pz=(origin.z()-step->
origin.z())/step->
mom.z();
1142 double ds=
sqrt(p_sq)*dz_over_pz;
1143 pos.SetXYZ(step->
origin.x()+dz_over_pz*step->
mom.x(),
1144 step->
origin.y()+dz_over_pz*step->
mom.y(),
1146 p_at_intersection = step->
mom;
1153 double one_over_beta=
sqrt(1.+mass_sq/p_sq);
1176 if(!start_step)
return -1;
1193 auto_ptr<StepStruct> steps_aptr(
new StepStruct);
1197 rt.
Swim(pos, mom, my_q,NULL,fabs(delta_s));
1212 double s1 = start_step->
s;
1213 double s2 = start_step->
s + (double)direction*sdiff;
1214 double smin = s1<s2 ? s1:s2;
1215 double smax = s1<s2 ? s2:s1;
1216 int istep_start = 0;
1218 for(
int i=0; i<Nswim_steps; i++){
1219 if(swim_steps[i].s < smin)istep_start = i;
1220 if(swim_steps[i].s <= smax)istep_end = i+1;
1227 int steps_to_overwrite = istep_end - istep_start - 1;
1228 int steps_to_shift = rt.
Nswim_steps - steps_to_overwrite;
1231 for(
int i=Nswim_steps-1; i>=istep_end; i--)swim_steps[i+steps_to_shift] = swim_steps[i];
1234 double s_0 = start_step->
s;
1235 double itheta02_0 = start_step->
itheta02;
1236 double itheta02s_0 = start_step->
itheta02s;
1237 double itheta02s2_0 = start_step->
itheta02s2;
1239 int index = direction>0 ? (istep_start+1+i):(istep_start+1+rt.
Nswim_steps-1-i);
1241 swim_steps[
index].
s = s_0 + (double)direction*swim_steps[index].s;
1242 swim_steps[
index].itheta02 = itheta02_0 + (double)direction*swim_steps[index].itheta02;
1243 swim_steps[
index].itheta02s = itheta02s_0 + (double)direction*swim_steps[index].itheta02s;
1244 swim_steps[
index].itheta02s2 = itheta02s2_0 + (double)direction*swim_steps[index].itheta02s2;
1246 swim_steps[
index].sdir *= -1.0;
1247 swim_steps[
index].tdir *= -1.0;
1265 double dist=DistToRT(hit,s,detector);
1266 if (s!=NULL && t!=NULL)
1268 if(last_swim_step==NULL)
1278 double p_sq=last_swim_step->mom.Mag2();
1279 double one_over_beta=
sqrt(1.+mass_sq/p_sq);
1280 *t=last_swim_step->t+(*s-last_swim_step->s)*one_over_beta/
SPEED_OF_LIGHT;
1282 *var_t=last_swim_step->cov_t_t;
1295 last_swim_step=NULL;
1301 if (index_at_bcal<0)
return numeric_limits<double>::quiet_NaN();
1302 start_index=index_at_bcal;
1305 if (index_at_fcal<0)
return numeric_limits<double>::quiet_NaN();
1306 start_index=index_at_fcal;
1309 if (index_at_tof<0)
return numeric_limits<double>::quiet_NaN();
1310 start_index=index_at_tof;
1317 swim_step_t *swim_step = &swim_steps[start_index];
1321 double old_delta2=10.e6,delta2=1.0e6;
1325 int last_index=Nswim_steps-1;
1326 double forward_delta2=(swim_step->
origin - hit).Mag2();
1327 double backward_delta2=(swim_steps[last_index].origin-hit).Mag2();
1329 if (forward_delta2<backward_delta2){
1330 for(
int i=start_index; i<Nswim_steps; i++, swim_step++){
1333 delta2 = pos_diff.Mag2();
1335 if (delta2>old_delta2){
1348 for(
int i=last_index; i>=start_index; i--){
1349 swim_step=&swim_steps[i];
1351 delta2 = pos_diff.Mag2();
1352 if (delta2>old_delta2)
break;
1371 _DBG_<<
"\"hit\" passed to DistToRT(DVector3) out of range!"<<endl;
1372 _DBG_<<
"hit x,y,z = "<<hit.x()<<
", "<<hit.y()<<
", "<<hit.z()<<
" Nswim_steps="<<Nswim_steps<<
" min_delta2="<<delta2<<endl;
1378 last_swim_step=step;
1440 double x0 = hit.Dot(step->
sdir);
1441 double y0 = hit.Dot(step->
tdir);
1442 double z0 = hit.Dot(step->
udir);
1443 double &Ro = step->
Ro;
1445 double delta_z = step->
mom.Dot(step->
udir);
1446 double delta_phi = step->
mom.Dot(step->
tdir)/Ro;
1447 double dz_dphi = delta_z/delta_phi;
1450 double alpha=x0*Ro + Ro2 +dz_dphi*dz_dphi;
1452 double beta = -2.0*(y0*Ro + z0*dz_dphi);
1456 double c = 0.5*(beta/Ro2);
1458 double d2=b*b*b+c*
c;
1459 double phi=0.,dist2=1e8;
1468 double phisq=phi*phi;
1470 dist2 = 0.25*Ro2*phisq*phisq + alpha*phisq + beta*phi
1471 + x0*x0 + y0*y0 + z0*z0;
1474 return numeric_limits<double>::quiet_NaN();
1484 double sum_over_2=temp*cos(theta1);
1485 double diff_over_2=-temp*
sin(theta1);
1487 double phi0=2.*sum_over_2;
1488 double phi0sq=phi0*phi0;
1489 double phi1=-sum_over_2+
sqrt(3)*diff_over_2;
1490 double phi1sq=phi1*phi1;
1491 double phi2=-sum_over_2-
sqrt(3)*diff_over_2;
1492 double phi2sq=phi2*phi2;
1493 double d2_2 = 0.25*Ro2*phi2sq*phi2sq + alpha*phi2sq + beta*phi2 + x0*x0 + y0*y0 + z0*z0;
1494 double d2_1 = 0.25*Ro2*phi1sq*phi1sq + alpha*phi1sq + beta*phi1 + x0*x0 + y0*y0 + z0*z0;
1495 double d2_0 = 0.25*Ro2*phi0sq*phi0sq + alpha*phi0sq + beta*phi0 + x0*x0 + y0*y0 + z0*z0;
1497 if (d2_0<d2_1 && d2_0<d2_2){
1501 else if (d2_1<d2_0 && d2_1<d2_2){
1510 return numeric_limits<double>::quiet_NaN();
1517 double dz = dz_dphi*phi;
1518 double Rodphi = Ro*phi;
1519 double ds =
sqrt(dz*dz + Rodphi*Rodphi);
1520 *s = step->
s + (phi>0.0 ? ds:-ds);
1523 this->last_phi = phi;
1524 this->last_swim_step = step;
1525 this->last_dz_dphi = dz_dphi;
1540 if(istep_ptr)*istep_ptr=-1;
1543 _DBG_<<
"No swim steps! You must \"Swim\" the track before calling FindClosestSwimStep(...)"<<endl;
1547 if(!wire)
return NULL;
1553 double old_delta2=1.0e6;
1554 double L_over_2 = wire->
L/2.0;
1569 ux = wire->
udir.X();
1570 uy = wire->
udir.Y();
1571 uz = wire->
udir.Z();
1574 for(i=0; i<Nswim_steps; i++, swim_step++){
1580 dx = swim_step->
origin.X() - wx;
1581 dy = swim_step->
origin.Y() - wy;
1582 dz = swim_step->
origin.Z() - wz;
1585 double u = ux * dx + uy * dy + uz * dz;
1589 double delta2 = dx*dx + dy*dy + dz*dz - u*
u;
1593 if( fabs(u)>L_over_2){
1595 double u_minus_L_over_2=fabs(u)-L_over_2;
1596 delta2 += ( u_minus_L_over_2*u_minus_L_over_2 );
1600 if(debug_level>3)
_DBG_<<
"delta2="<<delta2<<
" old_delta2="<<old_delta2<<endl;
1601 if (delta2>old_delta2)
break;
1613 if(istep_ptr)*istep_ptr=istep;
1615 if(debug_level>3)
_DBG_<<
"found closest step at i="<<i<<
" istep_ptr="<<istep_ptr<<endl;
1628 if(istep_ptr)*istep_ptr=-1;
1631 _DBG_<<
"No swim steps! You must \"Swim\" the track before calling FindClosestSwimStep(...)"<<endl;
1641 double old_dist=1.0e6;
1644 for(
int i=0; i<Nswim_steps; i++, swim_step++){
1648 double dist = fabs(norm.Dot(swim_step->
origin-origin));
1650 if (dist>old_dist)
break;
1666 if(istep_ptr)*istep_ptr=istep;
1683 _DBG_<<
"No swim steps! You must \"Swim\" the track before calling FindPlaneCrossing(...)"<<endl;
1693 double old_dist=1.0e6;
1697 int last_index=Nswim_steps-1;
1698 double forward_dist= norm.Dot(swim_step->
origin-origin);
1699 if( forward_dist == 0.0 )
return swim_step;
1700 double backward_dist= norm.Dot(swim_steps[last_index].origin-origin);
1701 if( backward_dist ==0.0 )
return &swim_steps[last_index];
1702 if (detector==
SYS_START || fabs(forward_dist)<fabs(backward_dist)){
1703 for(
int i=first_i; i<Nswim_steps; i++, swim_step++){
1708 double dist = norm.Dot(swim_step->
origin-origin);
1711 if (dist*old_dist<0 && i>0) {
1712 if (fabs(dist)<fabs(old_dist)){
1722 for(
int i=last_index; i>=0; i--){
1723 swim_step=&swim_steps[i];
1724 double dist = norm.Dot(swim_step->
origin-origin);
1726 if (dist*old_dist<0 && i<last_index) {
1727 if (fabs(dist)<fabs(old_dist)){
1755 return (step && step->
s>0) ? DistToRT(wire, step, s):std::numeric_limits<double>::quiet_NaN();
1769 return step ? DistToRTBruteForce(wire, step, s):std::numeric_limits<double>::quiet_NaN();
1885 double A = sdir.Dot(xdir);
1886 double B = sdir.Dot(ydir);
1887 double C = sdir.Dot(zdir);
1888 double D = sdir.Dot(pos_diff);
1890 double E = tdir.Dot(xdir);
1891 double F = tdir.Dot(ydir);
1892 double G = tdir.Dot(zdir);
1893 double H = tdir.Dot(pos_diff);
1909 double Ro = step->
Ro;
1911 double delta_z = step->
mom.Dot(step->
udir);
1912 double delta_phi = step->
mom.Dot(step->
tdir)/Ro;
1913 double dz_dphi = delta_z/delta_phi;
1914 double dz_dphi2=dz_dphi*dz_dphi;
1915 double Ro_dz_dphi=Ro*dz_dphi;
1918 double Q=0.25*Ro2*(A*A+E*E);
1920 double R = -((A*B+E*
F)*Ro2 + (A*C+E*G)*Ro_dz_dphi);
1923 double S= (B*B+F*
F)*Ro2+(C*C+G*G)*dz_dphi2+2.0*(B*C+F*
G)*Ro_dz_dphi
1926 double T = 2.0*((B*D+F*
H)*Ro + (C*D+G*H)*dz_dphi);
1927 double U = D*D + H*
H;
1988 double one_over_fourQ=0.25/Q;
1989 double a2=3.0*R*one_over_fourQ;
1990 double a1=2.0*S*one_over_fourQ;
1991 double a0=T*one_over_fourQ;
1998 double c=0.5*(a0-
ONE_THIRD*a1*a2)+a2*a2sq/27.0;
1999 double my_d2=b*b*b+c*
c;
2002 double d=
sqrt(my_d2);
2015 double d=
sqrt(-my_d2);
2019 double sum_over_2=temp*cos(theta1);
2020 double diff_over_2=-temp*
sin(theta1);
2022 double phi0=-a2/3+2.*sum_over_2;
2023 double phi1=-a2/3-sum_over_2+
sqrt(3)*diff_over_2;
2024 double phi2=-a2/3-sum_over_2-
sqrt(3)*diff_over_2;
2026 double d2_0 = U + phi0*(T + phi0*(S + phi0*(R + phi0*Q)));
2027 double d2_1 = U + phi1*(T + phi1*(S + phi1*(R + phi1*Q)));
2028 double d2_2 = U + phi2*(T + phi2*(S + phi2*(R + phi2*Q)));
2030 if (d2_0<d2_1 && d2_0<d2_2){
2033 else if (d2_1<d2_0 && d2_1<d2_2){
2042 if(fabs(Q)<=1.0E-6 || !isfinite(phi)){
2046 phi = (-b +
sqrt(b*b - 4.0*a*c))/(2.0*a);
2059 if(isfinite(phi) && fabs(phi)>2.0E-4){
2060 if(dist_to_rt_depth>=3){
2061 _DBG_<<
"3 or more recursive calls to DistToRT(). Something is wrong! bailing ..."<<endl;
2067 return std::numeric_limits<double>::quiet_NaN();
2069 double scale_step = 1.0;
2070 double s_range = 1.0*scale_step;
2071 double step_size = 0.02*scale_step;
2072 int err = InsertSteps(step, phi>0.0 ? +s_range:-s_range, step_size);
2074 step=FindClosestSwimStep(wire);
2075 if(!step)
return std::numeric_limits<double>::quiet_NaN();
2077 double doca = DistToRT(wire, step, s);
2081 if(err<0)
return std::numeric_limits<double>::quiet_NaN();
2097 double x = -0.5*Ro*phi*phi;
2099 double z = dz_dphi*phi;
2100 DVector3 h = pos_diff + x*xdir + y*ydir + z*zdir;
2101 double u = h.Dot(udir);
2102 if(fabs(u) > wire->
L/2.0){
2105 double L_over_2 = u>0.0 ? wire->
L/2.0:-wire->
L/2.0;
2106 double a = -0.5*Ro*udir.Dot(xdir);
2107 double b = Ro*udir.Dot(ydir) + dz_dphi*udir.Dot(zdir);
2108 double c = udir.Dot(pos_diff) - L_over_2;
2110 double sqroot=
sqrt(b*b-4.0*a*c);
2111 double phi1 = (-b + sqroot)/(twoa);
2112 double phi2 = (-b - sqroot)/(twoa);
2113 phi = fabs(phi1)<fabs(phi2) ? phi1:phi2;
2116 this->last_dist_along_wire =
u;
2119 double d2 = U + phi*(T + phi*(S + phi*(R + phi*Q)));
2120 double d =
sqrt(d2);
2123 double dz = dz_dphi*phi;
2124 double Rodphi = Ro*phi;
2125 double ds =
sqrt(dz*dz + Rodphi*Rodphi);
2126 if(s)*s=step->
s + (phi>0.0 ? ds:-ds);
2128 _DBG_<<
"distance to rt: "<< step->
s + (phi>0.0 ? ds:-ds) <<
" from step at "<<step->
s<<
" with ds="<<ds<<
" d="<<d<<
" dz="<<dz<<
" Rodphi="<<Rodphi<<endl;
2129 _DBG_<<
"phi="<<phi<<
" U="<<U<<
" u="<<u<<endl;
2133 this->last_phi = phi;
2134 this->last_swim_step = step;
2135 this->last_dz_dphi = dz_dphi;
2163 double Ro = step->
Ro;
2164 double delta_z = step->
mom.Dot(step->
udir);
2165 double delta_phi = step->
mom.Dot(step->
tdir)/Ro;
2166 double dz_dphi = delta_z/delta_phi;
2169 double min_d2 = 1.0E6;
2171 for(
int i=-2000; i<2000; i++){
2172 double myphi=(double)i*0.000005;
2173 DVector3 d = Ro*(cos(myphi)-1.0)*xdir
2174 + Ro*
sin(myphi)*ydir
2175 + dz_dphi*myphi*zdir
2178 double d2 = pow(d.Dot(sdir),2.0) + pow(d.Dot(tdir),2.0);
2182 this->last_phi = myphi;
2186 double d =
sqrt(d2);
2187 this->last_phi = phi;
2188 this->last_swim_step = step;
2189 this->last_dz_dphi = dz_dphi;
2192 double dz = dz_dphi*phi;
2193 double Rodphi = Ro*phi;
2194 double ds =
sqrt(dz*dz + Rodphi*Rodphi);
2195 if(s)*s=step->
s + (phi>0.0 ? ds:-ds);
2217 double doca = DistToRT(wire, &s);
2222 if(doca>=radius)
return 0.0;
2226 GetLastDOCAPoint(pos, momdir);
2227 if(momdir.Mag()!=0.0)momdir.SetMag(1.0);
2241 if(B.Mag()<1.0E-10)
return std::numeric_limits<double>::quiet_NaN();
2244 double b = A.Dot(B);
2245 double c = A.Mag() - radius;
2246 double d =
sqrt(b*b - 4.0*a*c);
2249 double alpha1 = (-b + d)/(2.0*a);
2250 double alpha2 = (-b - d)/(2.0*a);
2252 DVector3 int1 = pos + alpha1*momdir;
2253 DVector3 int2 = pos + alpha2*momdir;
2256 double q = udir.Dot(int1 - wire->
origin);
2257 if(fabs(q) > wire->
L/2.0){
2258 double gamma = udir.Dot(wire->
origin - pos) + (q>0.0 ? +1.0:-1.0)*wire->
L/2.0;
2259 gamma /= momdir.Dot(udir);
2260 int1 = pos + gamma*momdir;
2264 q = udir.Dot(int2 - wire->
origin);
2265 if(fabs(q) > wire->
L/2.0){
2266 double gamma = udir.Dot(wire->
origin - pos) + (q>0.0 ? +1.0:-1.0)*wire->
L/2.0;
2267 gamma /= momdir.Dot(udir);
2268 int2 = pos + gamma*momdir;
2286 if(last_swim_step==NULL){
2288 last_swim_step = &swim_steps[0];
2298 if(!isfinite(last_phi))last_phi = 0.0;
2300 const DVector3 &xdir = last_swim_step->sdir;
2301 const DVector3 &ydir = last_swim_step->tdir;
2302 const DVector3 &zdir = last_swim_step->udir;
2304 double x = -(last_swim_step->Ro/2.0)*last_phi*last_phi;
2305 double y = last_swim_step->Ro*last_phi;
2306 double z = last_dz_dphi*last_phi;
2308 pos = last_swim_step->origin + x*xdir + y*ydir + z*zdir;
2309 mom = last_swim_step->mom;
2311 mom.Rotate(-last_phi, zdir);
2322 if(last_swim_step==NULL){
2324 last_swim_step = &swim_steps[0];
2330 const DVector3 &xdir = last_swim_step->sdir;
2331 const DVector3 &ydir = last_swim_step->tdir;
2332 const DVector3 &zdir = last_swim_step->udir;
2333 double Ro = last_swim_step->Ro;
2334 double delta_z = last_swim_step->mom.Dot(zdir);
2335 double delta_phi = last_swim_step->mom.Dot(ydir)/Ro;
2336 double dz_dphi = delta_z/delta_phi;
2338 double x = -(Ro/2.0)*last_phi*last_phi;
2339 double y = Ro*last_phi;
2340 double z = dz_dphi*last_phi;
2342 return last_swim_step->origin + x*xdir + y*ydir + z*zdir;
2350 double I = (Z*12.0 + 7.0)*1.0E-9;
2351 if (Z>=13) I=(9.76*Z+58.8*pow(Z,-0.19))*1.0
e-9;
2352 double rhoZ_overA=density*Z/A;
2353 double KrhoZ_overA = 0.1535e-3*rhoZ_overA;
2355 return dPdx(ptot, KrhoZ_overA,rhoZ_overA,log(I));
2362 double rhoZ_overA,
double LogI)
const
2367 if(mass==0.0)
return 0.0;
2369 double gammabeta = ptot/mass;
2370 double gammabeta2=gammabeta*gammabeta;
2371 double gamma =
sqrt(gammabeta2+1.);
2372 double beta = gammabeta/gamma;
2373 double beta2=beta*beta;
2374 double me = 0.511E-3;
2375 double m_ratio=me/mass;
2376 double two_me_gammabeta2=2.*me*gammabeta2;
2378 double Tmax = two_me_gammabeta2/(1.0+2.0*gamma*m_ratio+m_ratio*m_ratio);
2382 double X=log10(gammabeta);
2384 double Cbar=2.*(LogI-log(28.816
e-9*
sqrt(rhoZ_overA)))+1.;
2385 if (rhoZ_overA>0.01){
2387 if (Cbar<=3.681) X0=0.2;
2388 else X0=0.326*Cbar-1.;
2392 if (Cbar<=5.215) X0=0.2;
2393 else X0=0.326*Cbar-1.5;
2399 if (Cbar<=9.5) X0=1.6;
2400 else if (Cbar>9.5 && Cbar<=10.) X0=1.7;
2401 else if (Cbar>10 && Cbar<=10.5) X0=1.8;
2402 else if (Cbar>10.5 && Cbar<=11.) X0=1.9;
2403 else if (Cbar>11.0 && Cbar<=12.25) X0=2.;
2404 else if (Cbar>12.25 && Cbar<=13.804){
2414 delta=4.606*X-Cbar+(Cbar-4.606*
X0)*pow((X1-X)/(X1-
X0),3.);
2416 delta= 4.606*X-Cbar;
2418 double dEdx = KrhoZ_overA/beta2*(log(two_me_gammabeta2*Tmax)
2419 -2.*LogI - 2.0*beta2 -delta);
2421 double dP_dx = dEdx/beta;
2427 if(ploss_direction==kBackward)dP_dx = -dP_dx;
2438 for(
int i=0; i<Nswim_steps; i++, step++){
2439 vector<pair<string,string> > item;
2442 double z = step->
origin.Z();
2443 if(z<zmin || z>zmax)
continue;
2445 double px = step->
mom.X();
2446 double py = step->
mom.Y();
2447 double pz = step->
mom.Z();
2450 cout<<
"(x,y,z)=("<<x<<
","<<y<<
","<<z<<
") ";
2451 cout<<
"(px,py,pz)=("<<px<<
","<<py<<
","<<pz<<
") ";
2452 cout<<
"(Ro,s,t)=("<<step->
Ro<<
","<<step->
s<<
","<<step->
t<<
") ";
2464 TMatrixFSym &C)
const{
2467 double one_over_p_sq=1./mom.Mag2();
2468 double one_over_p=
sqrt(one_over_p_sq);
2472 double Bx=B.x(),By=B.y(),Bz=B.z();
2474 double ds_over_p=ds*one_over_p;
2475 double factor=0.003*q*ds_over_p;
2476 double temp=(Bz*py-Bx*pz)*one_over_p_sq;
2477 J(0,0)=1-factor*px*
temp;
2478 J(0,1)=factor*(Bz-py*
temp);
2479 J(0,2)=-factor*(By+pz*
temp);
2481 temp=(Bx*pz-Bz*px)*one_over_p_sq;
2482 J(1,0)=-factor*(Bz+px*
temp);
2483 J(1,1)=1-factor*py*
temp;
2484 J(1,2)=factor*(Bx-pz*
temp);
2486 temp=(By*px-Bx*
py)*one_over_p_sq;
2487 J(2,0)=factor*(By-px*
temp);
2488 J(2,1)=-factor*(Bx+py*
temp);
2489 J(2,2)=1-factor*pz*
temp;
2492 double ds_over_p3=one_over_p_sq*ds_over_p;
2493 J(3,0)=ds_over_p*(1-px*px*one_over_p_sq);
2494 J(3,1)=-px*py*ds_over_p3;
2495 J(3,2)=-px*pz*ds_over_p3;
2499 J(4,1)=ds_over_p*(1-py*py*one_over_p_sq);
2500 J(4,2)=-py*pz*ds_over_p3;
2505 J(5,2)=ds_over_p*(1-pz*pz*one_over_p_sq);
2509 double fac2=(-ds/
SPEED_OF_LIGHT)*mass_sq*one_over_p_sq*one_over_p_sq
2510 /
sqrt(1.+mass_sq*one_over_p_sq);
2529 DVector3 &commonpos,
double &doca,
double &var_doca)
const{
2532 shared_ptr<TMatrixFSym> cov = (track_kd!=NULL) ? dResourcePool_TMatrixFSym->Get_SharedResource() :
nullptr;
2535 cov->ResizeTo(7, 7);
2540 double mass_sq=this->mass_sq;
2542 double step_size=1.0,s=-step_size;
2547 double pscale=dir.Mag();
2550 bool move_along_line=(pscale>0)?
true:
false;
2554 for (
int i=0;i<this->Nswim_steps-1; i++, swim_step++){
2557 double new_doca=diff.Mag();
2562 swim_step=this->swim_steps;
2573 double ds=stepper.
Step(&pos,&B,-0.5);
2576 new_doca=diff.Mag();
2578 if(new_doca > doca)
break;
2582 this->PropagateCovariance(ds,q,mass_sq,mom,oldpos,B,*cov);
2589 double one_over_p_sq=1./mom.Mag2();
2599 if (move_along_line){
2600 point-=(step_size/pscale)*dir;
2623 var_doca=(dx*dx*((*cov)(kX,kX))+dy*dy*((*cov)(kY,kY))
2624 +dz*dz*((*cov)(kZ,kZ))+2.*dx*dy*((*cov)(kX,kY))
2625 +2.*dx*dz*((*cov)(kX,kZ))+2.*dy*dz*((*cov)(kY,kZ)))
2630 if (move_along_line){
2633 cov2(kX,kX)+=two_s*cov2(kPx,kX)+s_sq*cov2(kPx,kPx);
2634 cov2(kY,kY)+=two_s*cov2(kPy,kY)+s_sq*cov2(kPy,kPy);
2635 cov2(kZ,kZ)+=two_s*cov2(kPz,kZ)+s_sq*cov2(kPz,kPz);
2637 var_doca=(dx*dx*((*cov)(kX,kX)+cov2(kX,kX))
2638 +dy*dy*((*cov)(kY,kY)+cov2(kY,kY))
2639 +dz*dz*((*cov)(kZ,kZ)+cov2(kZ,kZ))
2640 +2.*dx*dy*((*cov)(kX,kY)+cov2(kX,kY))
2641 +2.*dx*dz*((*cov)(kX,kZ)+cov2(kX,kZ))
2642 +2.*dy*dz*((*cov)(kY,kZ)+cov2(kY,kZ)))
2648 if (move_along_line){
2649 point+=(step_size/pscale)*dir;
2655 this->PropagateCovariance(this->swim_steps[i+1].s-swim_step->
s,q,mass_sq,swim_step->
mom,swim_step->
origin,swim_step->
B,*cov);
2659 oldmom=swim_step->
mom;
2660 tflight=swim_step->
t;
2666 commonpos = 0.5*(oldpos + point);
2679 double &doca,
double &var_doca)
const{
2680 if (track_kd==NULL)
return RESOURCE_UNAVAILABLE;
2683 return FindPOCAtoLine(point,dir,covpoint,track_kd,commonpos,doca,var_doca);
2693 TMatrixFSym cov1(7), cov2(7);
2694 shared_ptr<TMatrixFSym> locCovarianceMatrix1 = (track1_kd != NULL) ? dResourcePool_TMatrixFSym->Get_SharedResource() :
nullptr;
2695 shared_ptr<TMatrixFSym> locCovarianceMatrix2 = (track2_kd != NULL) ? dResourcePool_TMatrixFSym->Get_SharedResource() :
nullptr;
2697 if((track1_kd != NULL) && (track2_kd != NULL)){
2698 locCovarianceMatrix1->ResizeTo(7, 7);
2699 locCovarianceMatrix2->ResizeTo(7, 7);
2706 double mass_sq1=this->mass_sq;
2712 double tflight1=0.,tflight2=0.;
2713 for (
int i=0;i<this->Nswim_steps-1&&i<rt2->
Nswim_steps-1; i++, swim_step1++, swim_step2++){
2717 double new_doca=diff.Mag();
2723 pos1=this->swim_steps[prev_i].origin;
2724 DVector3 mom1=this->swim_steps[prev_i].mom;
2731 tflight1=tflight2=0.;
2732 if((track1_kd != NULL) && (track2_kd != NULL)){
2745 if (pos1.z()<0. || pos2.z()<0. || pos1.z()>400. || pos2.z()>400.
2746 || pos1.Perp()>65. || pos2.Perp()>65.){
2750 double ds1=stepper1.
Step(&pos1,&B1,-0.5);
2751 double ds2=stepper2.
Step(&pos2,&B2,-0.5);
2755 new_doca=diff.Mag();
2757 if(new_doca > doca){
2764 if((track1_kd != NULL) && (track2_kd != NULL)){
2765 this->PropagateCovariance(ds1,q1,mass_sq1,mom1,oldpos1,B1,cov1);
2774 double one_over_p1_sq=1./mom1.Mag2();
2777 double one_over_p2_sq=1./mom2.Mag2();
2789 BrentsAlgorithm(pos1,mom1,pos2,mom2,ds,q2,doca);
2793 pos=0.5*(pos1+pos2);
2795 if((track1_kd != NULL) && (track2_kd != NULL)){
2799 FitVertex(pos1,mom1,pos2,mom2,cov1,cov2,pos,vertex_chi2);
2803 if (DoFitVertex) doca=diff.Mag();
2807 var_doca=(dx*dx*(cov1(kX,kX)+cov2(kX,kX))
2808 +dy*dy*(cov1(kY,kY)+cov2(kY,kY))
2809 +dz*dz*(cov1(kZ,kZ)+cov2(kZ,kZ))
2810 +2.*dx*dy*(cov1(kX,kY)+cov2(kX,kY))
2811 +2.*dx*dz*(cov1(kX,kZ)+cov2(kX,kZ))
2812 +2.*dy*dz*(cov1(kY,kZ)+cov2(kY,kZ)))
2817 double one_over_p1_sq=1./mom1.Mag2();
2820 double one_over_p2_sq=1./mom2.Mag2();
2823 *locCovarianceMatrix1 = cov1;
2829 *locCovarianceMatrix2 = cov2;
2839 if((track1_kd != NULL) && (track2_kd != NULL)){
2840 this->PropagateCovariance(this->swim_steps[i+1].s-swim_step1->s,q1,mass_sq1,swim_step1->mom,swim_step1->origin,swim_step1->B,cov1);
2845 tflight1=swim_step1->t;
2846 tflight2=swim_step2->
t;
2857 #define CGOLD 0.3819660
2859 #define ZEPS 1.0e-10
2860 #define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
2861 #define SIGN(a,b) ((b)>=0.0?fabs(a):-fabs(a))
2864 double ds,
double q2,
2865 double &doca)
const{
2872 double a=(ax<cx?ax:cx);
2873 double b=(ax>cx?ax:cx);
2874 double x=bx,w=bx,v=bx;
2885 for (
unsigned int iter=1;iter<=
ITMAX;iter++){
2886 double xm=0.5*(a+b);
2888 double tol2=2.0*tol1;
2889 if (fabs(x-xm)<=(tol2-0.5*(b-a))){
2890 doca=(pos1-pos2).Mag();
2900 double x_minus_w=x-w;
2901 double x_minus_v=x-v;
2902 double r=x_minus_w*(fx-fv);
2903 double q=x_minus_v*(fx-fw);
2904 double p=x_minus_v*q-x_minus_w*r;
2910 if (fabs(p)>=fabs(0.5*q*etemp) || p<=q*(a-x) || p>=q*(b-x))
2912 d=
CGOLD*(e=(x>=xm?a-x:b-
x));
2917 if (
u-a<tol2 || b-
u <tol2)
2921 d=
CGOLD*(e=(x>=xm?a-x:b-
x));
2923 u=(fabs(d)>=tol1 ? x+d: x+
SIGN(tol1,d));
2927 stepper1.
Step(&pos1,NULL,du);
2928 stepper2.
Step(&pos2,NULL,du);
2930 double fu=diff.Mag();
2934 if (u>=x) a=
x;
else b=
x;
2939 if (u<x) a=
u;
else b=
u;
2940 if (fu<=fw || w==x){
2946 else if (fu<=fv || v==x || v==w){
2954 doca=(pos1-pos2).Mag();
2969 const TMatrixFSym &cov1,
2970 const TMatrixFSym &cov2,
2971 DVector3 &pos,
double &vertex_chi2,
double q1,
double q2)
const{
2974 TMatrixD eta0(12,1),eta(12,1);
2976 TMatrixD xi(3,1),xi_old(3,1);
2978 TMatrixD F_eta(4,12);
2987 double Bz=fabs(bfield->GetBz(xi(0,0),xi(1,0),xi(2,0)));
2988 bool got_field=
false;
2997 eta0(kX,0)=pos1.x();
2998 eta0(kY,0)=pos1.y();
2999 eta0(kZ,0)=pos1.z();
3000 eta0(kPx,0)=mom1.x();
3001 eta0(kPy,0)=mom1.y();
3002 eta0(kPz,0)=mom1.z();
3003 eta0(kX+6,0)=pos2.x();
3004 eta0(kY+6,0)=pos2.y();
3005 eta0(kZ+6,0)=pos2.z();
3006 eta0(kPx+6,0)=mom2.x();
3007 eta0(kPy+6,0)=mom2.y();
3008 eta0(kPz+6,0)=mom2.z();
3012 for (
int m=0;m<6;m++){
3013 for (
int n=0;n<6;n++){
3015 V(m+6,n+6)=cov2(n,m);
3023 TMatrixD LagMul(4,1);
3028 cout <<
"Measured quantities:" <<endl;
3030 cout <<
"Covariance matrix for measured quantities:"<<endl;
3032 cout <<
"Initial guess for common vertex position:"<<endl;
3036 double chi2=1.e6,chi2_old=1e6;
3037 for (
int iter=0;iter<4;iter++){
3040 double dx1=xi(0,0)-eta(kX,0);
3041 double dy1=xi(1,0)-eta(kY,0);
3042 double dz1=xi(2,0)-eta(kZ,0);
3043 double dx2=xi(0,0)-eta(kX+6,0);
3044 double dy2=xi(1,0)-eta(kY+6,0);
3045 double dz2=xi(2,0)-eta(kZ+6,0);
3046 double px1=eta(kPx,0);
3047 double py1=eta(kPy,0);
3048 double pz1=eta(kPz,0);
3049 double px2=eta(kPx+6,0);
3050 double py2=eta(kPy+6,0);
3051 double pz2=eta(kPz+6,0);
3054 if (got_field==
false){
3056 f(0,0)=pz1*dx1-px1*dz1;
3057 f(1,0)=pz1*dy1-py1*dz1;
3058 f(2,0)=pz2*dx2-px2*dz2;
3059 f(3,0)=pz2*dy2-py2*dz2;
3063 if (fabs(pz1)>1
e-8) twoks1=a1*dz1/pz1;
3064 double one_minus_cos_2ks1=1.-cos(twoks1);
3065 double sin_2ks1=
sin(twoks1);
3066 f(0,0)=a1*dx1-px1*sin_2ks1+py1*one_minus_cos_2ks1;
3067 f(1,0)=a1*dy1-py1*sin_2ks1-px1*one_minus_cos_2ks1;
3070 if (fabs(pz2)>1
e-8) twoks2=a2*dz2/pz2;
3071 double one_minus_cos_2ks2=1.-cos(twoks2);
3072 double sin_2ks2=
sin(twoks2);
3073 f(2,0)=a2*dx2-px2*sin_2ks2+py2*one_minus_cos_2ks2;
3074 f(3,0)=a2*dy2-py2*sin_2ks2-px2*one_minus_cos_2ks2;
3079 TMatrixD LagMul_T(TMatrixD::kTransposed,LagMul);
3080 chi2=(LagMul_T*S*LagMul)(0,0);
3082 chi2+=(LagMul_T*
f)(0,0);
3085 cout <<
"iter " << iter <<
" : chi2=" << chi2 << endl;
3087 cout <<
"Constraint equations:" << endl;
3089 cout <<
"Unmeasured quantities:" << endl;
3091 cout <<
"Measured quantities:" << endl;
3099 if (got_field==
false){
3111 F_eta(2,kPx+6)=-dz2;
3112 F_eta(2,kPz+6)=+dx2;
3115 F_eta(3,kPy+6)=-dz2;
3116 F_eta(3,kPz+6)=+dy2;
3131 if (fabs(pz1)>1
e-8) twoks1=a1*dz1/pz1;
3132 double cos_2ks1=cos(twoks1);
3133 double one_minus_cos_2ks1=1.-cos_2ks1;
3134 double sin_2ks1=
sin(twoks1);
3136 F_eta(0,kZ)=(-a1/pz1)*(py1*sin_2ks1-px1*cos_2ks1);
3137 F_eta(0,kPx)=-sin_2ks1;
3138 F_eta(0,kPy)=one_minus_cos_2ks1;
3139 F_eta(0,kPz)=(a1*dz1/(pz1*pz1))*(px1*cos_2ks1-py1*sin_2ks1);
3141 F_eta(1,kZ)=(a1/pz1)*(py1*cos_2ks1+px1*sin_2ks1);
3142 F_eta(1,kPy)=-sin_2ks1;
3143 F_eta(1,kPx)=-one_minus_cos_2ks1;
3144 F_eta(1,kPz)=(a1*dz1/(pz1*pz1))*(px1*cos_2ks1-py1*sin_2ks1);
3146 if (fabs(pz2)>1
e-8) twoks2=a2*dz2/pz2;
3147 double cos_2ks2=cos(twoks2);
3148 double one_minus_cos_2ks2=1.-cos_2ks2;
3149 double sin_2ks2=
sin(twoks2);
3151 F_eta(2,kZ+6)=(-a2/pz2)*(py2*sin_2ks2-px2*cos_2ks2);
3152 F_eta(2,kPx+6)=-sin_2ks2;
3153 F_eta(2,kPy+6)=one_minus_cos_2ks2;
3154 F_eta(2,kPz+6)=(a2*dz2/(pz2*pz2))*(px2*cos_2ks2-py2*sin_2ks2);
3156 F_eta(3,kZ+6)=(a2/pz2)*(py2*cos_2ks2+px2*sin_2ks2);
3157 F_eta(3,kPy+6)=-sin_2ks2;
3158 F_eta(3,kPx+6)=-one_minus_cos_2ks2;
3159 F_eta(3,kPz+6)=(a2*dz2/(pz2*pz2))*(px2*cos_2ks2-py2*sin_2ks2);
3162 F_xi(0,0)=-F_eta(0,kX);
3163 F_xi(0,2)=-F_eta(0,kZ);
3164 F_xi(1,1)=-F_eta(1,kY);
3165 F_xi(1,2)=-F_eta(1,kZ);
3166 F_xi(2,0)=-F_eta(2,kX+6);
3167 F_xi(2,2)=-F_eta(2,kZ+6);
3168 F_xi(3,1)=-F_eta(3,kY+6);
3169 F_xi(3,2)=-F_eta(3,kZ+6);
3174 cout <<
"Jacobian matrices for eta and xi" <<endl;
3179 TMatrixD F_eta_T(TMatrixD::kTransposed,F_eta);
3180 TMatrixD F_xi_T(TMatrixD::kTransposed,F_xi);
3185 TMatrixD Sinv(TMatrixD::kInverted,S);
3186 r=f+F_eta*(eta0-eta);
3187 TMatrixD Mtemp(TMatrixD::kInverted,F_xi_T*Sinv*F_xi);
3188 xi=xi_old-Mtemp*F_xi_T*Sinv*r;
3189 LagMul=Sinv*(r+F_xi*(xi-xi_old));
3190 eta=eta0-V*F_eta_T*LagMul;
3192 pos.SetXYZ(xi_old(0,0),xi_old(1,0),xi_old(2,0));
3193 vertex_chi2=chi2_old;
void GetBField(DVector3 &B)
void setMomentum(const DVector3 &aMomentum)
void setTime(double locTime)
int InsertSteps(const swim_step_t *start_step, double delta_s, double step_size=0.02)
jerror_t BrentsAlgorithm(DVector3 &pos1, DVector3 &mom1, DVector3 &pos2, DVector3 &mom2, double ds, double q2, double &doca) const
double Straw_dx(const DCoordinateSystem *wire, double radius) const
bool GetCheckMaterialBoundaries(void) const
double GetBoundaryStepFraction(void) const
void FastSwim(const DVector3 &pos, const DVector3 &mom, double q, double smax=2000.0, double zmin=-100., double zmax=1000.0)
void CopyWithShift(const DReferenceTrajectory *rt, DVector3 shift)
const DMagneticFieldMap * bfield
DVector3 GetLastDOCAPoint(void) const
swim_step_t * FindPlaneCrossing(const DVector3 &origin, DVector3 norm, int first_i=0, DetectorSystem_t detector=SYS_NULL) const
void Swim(const DVector3 &pos, const DVector3 &mom, double q=-1000.0, const TMatrixFSym *cov=NULL, double smax=2000.0, const DCoordinateSystem *wire=NULL)
double dPdx(double ptot, double KrhoZ_overA, double rhoZ_overA, double LogI) const
void GetPosMom(DVector3 &pos, DVector3 &mom)
static char index(char c)
jerror_t IntersectTracks(const DReferenceTrajectory *rt2, DKinematicData *track1_kd, DKinematicData *track2_kd, DVector3 &pos, double &doca, double &var_doca, double &vertex_chi2, bool DoFitVertex=false) const
DMagneticFieldStepper class.
swim_step_t * FindClosestSwimStep(const DCoordinateSystem *wire, int *istep_ptr=NULL) const
double GetMaxStepSize(void) const
jerror_t SetStartingParams(double q, const DVector3 *x, const DVector3 *p)
void setErrorMatrix(const shared_ptr< const TMatrixFSym > &aMatrix)
direction_t ploss_direction
void FastSwimForHitSelection(const DVector3 &pos, const DVector3 &mom, double q)
double DistToRT(double x, double y, double z) const
double DistToRTBruteForce(const DCoordinateSystem *wire, double *s=NULL) const
double DistToRTwithTime(DVector3 hit, double *s=NULL, double *t=NULL, double *var_t=NULL, DetectorSystem_t detector=SYS_NULL) const
virtual ~DReferenceTrajectory()
jerror_t FindPOCAtoPoint(const DVector3 &point, const DMatrixDSym *covpoint, DKinematicData *track_kd, double &doca, double &var_doca) const
void GetMomentum(DVector3 &mom)
void Dump(double zmin=-1000.0, double zmax=1000.0)
DReferenceTrajectory::swim_step_t steps[256]
double GetMass(void) const
double FastStep(double stepsize=0.0)
double dPdx_from_A_Z_rho(double ptot, double A, double Z, double density) const
void FitVertex(const DVector3 &pos1, const DVector3 &mom1, const DVector3 &pos2, const DVector3 &mom2, const TMatrixFSym &cov1, const TMatrixFSym &cov2, DVector3 &pos, double &vertex_chi2, double q1=1., double q2=1.) const
jerror_t GetIntersectionWithRadius(double R, DVector3 &mypos, double *s=NULL, double *t=NULL, DVector3 *dir=NULL) const
DReferenceTrajectory & operator=(const DReferenceTrajectory &rt)
const swim_step_t * last_swim_step
last swim step used in DistToRT
double last_phi
last phi found in DistToRT
double last_dist_along_wire
double GetMinStepSize(void) const
jerror_t FindPOCAtoLine(const DVector3 &origin, const DVector3 &dir, const DMatrixDSym *covpoint, DKinematicData *track_kd, DVector3 &commonpos, double &doca, double &var_doca) const
double Step(DVector3 *newpos=NULL, DVector3 *B=NULL, double stepsize=0.0)
shared_ptr< const TMatrixFSym > errorMatrix(void) const
void setPosition(const DVector3 &aPosition)
const DRootGeom * RootGeom
jerror_t SetStepSize(double step)
void SetStepSize(double step_size)
jerror_t GetIntersectionWithPlane(const DVector3 &origin, const DVector3 &norm, DVector3 &pos, double *s=NULL, double *t=NULL, double *var_t=NULL, DetectorSystem_t detector=SYS_NULL) const
void GetDirs(DVector3 &xdir, DVector3 &ydir, DVector3 &zdir)
jerror_t PropagateCovariance(double ds, double q, double mass_sq, const DVector3 &mom, const DVector3 &pos, const DVector3 &B, TMatrixFSym &C) const