1 | |
2 | |
3 | |
4 | |
5 | |
6 | |
7 | |
8 | #include "DTrackFitterStraightTrack.h" |
9 | |
10 | |
11 | |
12 | |
13 | DTrackFitterStraightTrack::DTrackFitterStraightTrack(JEventLoop *loop):DTrackFitter(loop){ |
14 | int runnumber=(loop->GetJEvent()).GetRunNumber(); |
15 | DApplication* dapp = dynamic_cast<DApplication*>(loop->GetJApplication()); |
16 | JCalibration *jcalib = dapp->GetJCalibration(runnumber); |
17 | const DGeometry *geom = dapp->GetDGeometry(runnumber); |
18 | |
19 | |
20 | vector<const DTrackFinder *> finders; |
21 | loop->Get(finders); |
22 | |
23 | |
24 | finder = const_cast<DTrackFinder*>(finders[0]); |
25 | |
26 | |
27 | dResourcePool_TMatrixFSym = std::make_shared<DResourcePool<TMatrixFSym>> |
28 | (); |
29 | dResourcePool_TMatrixFSym->Set_ControlParams(20, 20, 50); |
30 | |
31 | |
32 | |
33 | |
34 | |
35 | map<string,double>drift_res_parms; |
36 | jcalib->Get("FDC/drift_resolution_parms",drift_res_parms); |
37 | DRIFT_RES_PARMS[0]=drift_res_parms["p0"]; |
38 | DRIFT_RES_PARMS[1]=drift_res_parms["p1"]; |
39 | DRIFT_RES_PARMS[2]=drift_res_parms["p2"]; |
40 | |
41 | |
42 | map<string,double>drift_func_parms; |
43 | jcalib->Get("FDC/drift_function_parms",drift_func_parms); |
44 | DRIFT_FUNC_PARMS[0]=drift_func_parms["p0"]; |
45 | DRIFT_FUNC_PARMS[1]=drift_func_parms["p1"]; |
46 | DRIFT_FUNC_PARMS[2]=drift_func_parms["p2"]; |
47 | DRIFT_FUNC_PARMS[3]=drift_func_parms["p3"]; |
48 | DRIFT_FUNC_PARMS[4]=1000.; |
49 | DRIFT_FUNC_PARMS[5]=0.; |
50 | map<string,double>drift_func_ext; |
51 | if (jcalib->Get("FDC/drift_function_ext",drift_func_ext)==false){ |
52 | DRIFT_FUNC_PARMS[4]=drift_func_ext["p4"]; |
53 | DRIFT_FUNC_PARMS[5]=drift_func_ext["p5"]; |
54 | } |
55 | |
56 | PLANE_TO_SKIP=0; |
57 | gPARMS->SetDefaultParameter("TRKFIT:PLANE_TO_SKIP",PLANE_TO_SKIP); |
58 | |
59 | |
60 | |
61 | |
62 | RING_TO_SKIP=0; |
63 | gPARMS->SetDefaultParameter("TRKFIT:RING_TO_SKIP",RING_TO_SKIP); |
64 | |
65 | map<string, double> cdc_res_parms; |
66 | jcalib->Get("CDC/cdc_resolution_parms_v2", cdc_res_parms); |
67 | CDC_RES_PAR1 = cdc_res_parms["res_par1"]; |
68 | CDC_RES_PAR2 = cdc_res_parms["res_par2"]; |
69 | CDC_RES_PAR3 = cdc_res_parms["res_par3"]; |
70 | |
71 | vector< map<string, double> > tvals; |
72 | cdc_drift_table.clear(); |
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]; |
76 | cdc_drift_table.push_back(1000.*row["t"]); |
77 | } |
78 | } |
79 | else{ |
80 | jerr << " CDC time-to-distance table not available... bailing..." << endl; |
81 | exit(0); |
82 | } |
83 | |
84 | if (jcalib->Get("CDC/drift_parameters", tvals)==false){ |
85 | map<string, double> &row = tvals[0]; |
86 | long_drift_func[0][0]=row["a1"]; |
87 | long_drift_func[0][1]=row["a2"]; |
88 | long_drift_func[0][2]=row["a3"]; |
89 | long_drift_func[1][0]=row["b1"]; |
90 | long_drift_func[1][1]=row["b2"]; |
91 | long_drift_func[1][2]=row["b3"]; |
92 | long_drift_func[2][0]=row["c1"]; |
93 | long_drift_func[2][1]=row["c2"]; |
94 | long_drift_func[2][2]=row["c3"]; |
95 | |
96 | row = tvals[1]; |
97 | short_drift_func[0][0]=row["a1"]; |
98 | short_drift_func[0][1]=row["a2"]; |
99 | short_drift_func[0][2]=row["a3"]; |
100 | short_drift_func[1][0]=row["b1"]; |
101 | short_drift_func[1][1]=row["b2"]; |
102 | short_drift_func[1][2]=row["b3"]; |
103 | short_drift_func[2][0]=row["c1"]; |
104 | short_drift_func[2][1]=row["c2"]; |
105 | short_drift_func[2][2]=row["c3"]; |
106 | } |
107 | |
108 | |
109 | max_sag.clear(); |
110 | sag_phi_offset.clear(); |
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]; |
118 | |
119 | temp.push_back(row["offset"]); |
120 | temp2.push_back(row["phi"]); |
121 | |
122 | straw_count++; |
123 | if (straw_count==numstraws[ring_count]){ |
124 | max_sag.push_back(temp); |
125 | sag_phi_offset.push_back(temp2); |
126 | temp.clear(); |
127 | temp2.clear(); |
128 | straw_count=0; |
129 | ring_count++; |
130 | } |
131 | } |
132 | } |
133 | |
134 | |
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]; |
145 | upstreamEndplate=cdc_origin[2]; |
146 | |
147 | double endplate_dz=0.; |
148 | geom->GetCDCEndplate(downstreamEndplate,endplate_dz,cdc_endplate_rmin, |
149 | cdc_endplate_rmax); |
150 | downstreamEndplate-=0.5*endplate_dz; |
151 | |
152 | |
153 | if (geom->GetDIRCZ(dDIRCz)==false) dDIRCz=1000.; |
154 | geom->GetFCALZ(dFCALz); |
155 | vector<double>tof_face; |
156 | geom->Get("//section/composition/posXYZ[@volume='ForwardTOF']/@X_Y_Z", |
157 | tof_face); |
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]; |
163 | dTOFz*=0.5; |
164 | geom->GetTRDZ(dTRDz_vec); |
165 | |
166 | |
167 | if (geom->GetStartCounterGeom(sc_pos,sc_norm)){ |
168 | |
169 | for (int i=0;i<30;i++){ |
170 | vector<DVector3>temp; |
171 | for (unsigned int j=0;j<sc_pos[i].size()-1;j++){ |
172 | double dx=sc_pos[i][j+1].x()-sc_pos[i][j].x(); |
173 | double dy=sc_pos[i][j+1].y()-sc_pos[i][j].y(); |
174 | double dz=sc_pos[i][j+1].z()-sc_pos[i][j].z(); |
175 | temp.push_back(DVector3(dx/dz,dy/dz,1.)); |
176 | } |
177 | sc_dir.push_back(temp); |
178 | } |
179 | SC_END_NOSE_Z=sc_pos[0][12].z(); |
180 | SC_BARREL_R=sc_pos[0][0].Perp()+0.5; |
181 | SC_PHI_SECTOR1=sc_pos[0][0].Phi(); |
182 | } |
183 | |
184 | |
185 | |
186 | |
187 | |
188 | VERBOSE=0; |
189 | gPARMS->SetDefaultParameter("TRKFIT:VERBOSE",VERBOSE); |
190 | COSMICS=false; |
191 | gPARMS->SetDefaultParameter("TRKFIND:COSMICS",COSMICS); |
192 | CHI2CUT = 15.0; |
193 | gPARMS->SetDefaultParameter("TRKFIT:CHI2CUT",CHI2CUT); |
194 | DO_PRUNING = true; |
195 | gPARMS->SetDefaultParameter("TRKFIT:DO_PRUNING",DO_PRUNING); |
196 | } |
197 | |
198 | |
199 | |
200 | |
201 | DTrackFitterStraightTrack::~DTrackFitterStraightTrack() |
202 | { |
203 | |
204 | } |
205 | |
206 | |
207 | |
208 | |
209 | bool DTrackFitterStraightTrack_cdc_hit_cmp(const DCDCTrackHit *a, |
210 | const DCDCTrackHit *b){ |
211 | |
212 | return(a->wire->origin.Y()>b->wire->origin.Y()); |
213 | } |
214 | |
215 | bool DTrackFitterStraightTrack_cdc_hit_reverse_cmp(const DCDCTrackHit *a, |
216 | const DCDCTrackHit *b){ |
217 | |
218 | return(a->wire->origin.Y()<b->wire->origin.Y()); |
219 | } |
220 | |
221 | bool DTrackFitterStraightTrack_cdc_hit_radius_cmp(const DCDCTrackHit *a, |
222 | const DCDCTrackHit *b){ |
223 | if (a==NULL__null || b==NULL__null){ |
224 | cout << "Null pointer in CDC hit list??" << endl; |
225 | return false; |
226 | } |
227 | const DCDCWire *wire_a= a->wire; |
228 | const DCDCWire *wire_b= b->wire; |
229 | if(wire_a->ring == wire_b->ring){ |
230 | return wire_a->straw < wire_b->straw; |
231 | } |
232 | |
233 | return (wire_a->ring<wire_b->ring); |
234 | } |
235 | |
236 | bool DTrackFitterStraightTrack_fdc_hit_cmp(const DFDCPseudo *a, |
237 | const DFDCPseudo *b){ |
238 | |
239 | return(a->wire->origin.z()<b->wire->origin.z()); |
240 | } |
241 | |
242 | |
243 | |
244 | |
245 | |
246 | |
247 | inline double DTrackFitterStraightTrack::CDCDriftVariance(double t) const { |
248 | if (t<0.) t=0.; |
249 | double sigma=CDC_RES_PAR1/(t+1.)+CDC_RES_PAR2 + CDC_RES_PAR3*t; |
250 | return sigma*sigma; |
251 | } |
252 | |
253 | |
254 | void DTrackFitterStraightTrack::CDCDriftParameters(double dphi,double delta, |
255 | double t, double &d, |
256 | double &V) const{ |
257 | |
258 | |
259 | |
260 | |
261 | |
262 | |
263 | d=0.; |
264 | double dd_dt=0; |
265 | if (t>0){ |
266 | double f_0=0.; |
267 | double f_delta=0.; |
268 | |
269 | if (delta>0){ |
270 | double a1=long_drift_func[0][0]; |
271 | double a2=long_drift_func[0][1]; |
272 | double a3=long_drift_func[0][2]; |
273 | double b1=long_drift_func[1][0]; |
274 | double b2=long_drift_func[1][1]; |
275 | double b3=long_drift_func[1][2]; |
276 | double c1=long_drift_func[2][0]; |
277 | double c2=long_drift_func[2][1]; |
278 | double c3=long_drift_func[2][2]; |
279 | |
280 | |
281 | double my_t=0.001*t; |
282 | double sqrt_t=sqrt(my_t); |
283 | double t3=my_t*my_t*my_t; |
284 | double delta_mag=fabs(delta); |
285 | |
286 | double delta_sq=delta*delta; |
287 | double a=a1+a2*delta_mag+a3*delta_sq; |
288 | double b=b1+b2*delta_mag+b3*delta_sq; |
289 | double c=c1+c2*delta_mag+c3*delta_sq; |
290 | |
291 | f_delta=a*sqrt_t+b*my_t+c*t3; |
292 | f_0=a1*sqrt_t+b1*my_t+c1*t3; |
293 | |
294 | dd_dt=0.001*(0.5*a/sqrt_t+b+3.*c*my_t*my_t); |
295 | |
296 | } |
297 | else{ |
298 | double my_t=0.001*t; |
299 | double sqrt_t=sqrt(my_t); |
300 | double t3=my_t*my_t*my_t; |
301 | double delta_mag=fabs(delta); |
302 | |
303 | |
304 | double a1=short_drift_func[0][0]; |
305 | double a2=short_drift_func[0][1]; |
306 | double a3=short_drift_func[0][2]; |
307 | double b1=short_drift_func[1][0]; |
308 | double b2=short_drift_func[1][1]; |
309 | double b3=short_drift_func[1][2]; |
310 | double c1=short_drift_func[2][0]; |
311 | double c2=short_drift_func[2][1]; |
312 | double c3=short_drift_func[2][2]; |
313 | |
314 | double delta_sq=delta*delta; |
315 | double a=a1+a2*delta_mag+a3*delta_sq; |
316 | double b=b1+b2*delta_mag+b3*delta_sq; |
317 | double c=c1+c2*delta_mag+c3*delta_sq; |
318 | |
319 | f_delta=a*sqrt_t+b*my_t+c*t3; |
320 | f_0=a1*sqrt_t+b1*my_t+c1*t3; |
321 | |
322 | dd_dt=0.001*(0.5*a/sqrt_t+b+3.*c*my_t*my_t); |
323 | |
324 | } |
325 | |
326 | unsigned int max_index=cdc_drift_table.size()-1; |
327 | if (t>cdc_drift_table[max_index]){ |
328 | |
329 | d=f_delta; |
330 | } |
331 | |
332 | |
333 | unsigned int index=0; |
334 | index=Locate(cdc_drift_table,t); |
335 | double dt=cdc_drift_table[index+1]-cdc_drift_table[index]; |
336 | double frac=(t-cdc_drift_table[index])/dt; |
337 | double d_0=0.01*(double(index)+frac); |
338 | |
339 | double P=0.; |
340 | double tcut=250.0; |
341 | if (t<tcut) { |
342 | P=(tcut-t)/tcut; |
343 | } |
344 | d=f_delta*(d_0/f_0*P+1.-P); |
345 | } |
346 | double VarMs=0.001; |
347 | V=CDCDriftVariance(t)+mVarT0*dd_dt*dd_dt+VarMs; |
348 | } |
349 | |
350 | |
351 | jerror_t DTrackFitterStraightTrack::KalmanFilter(DMatrix4x1 &S,DMatrix4x4 &C, |
352 | vector<int>&used_hits, |
353 | vector<cdc_update_t>&updates, |
354 | double &chi2, |
355 | int &ndof, |
356 | unsigned int iter){ |
357 | DMatrix1x4 H; |
358 | DMatrix4x1 H_T; |
359 | DMatrix4x1 K; |
360 | DMatrix4x4 J; |
361 | DMatrix4x1 S0; |
362 | double V=1.15*(0.78*0.78/12.); |
363 | |
364 | const double d_EPS=1e-8; |
365 | |
366 | |
367 | for (unsigned int i=0;i<used_hits.size();i++) used_hits[i]=0; |
368 | |
369 | |
370 | chi2=0.; |
371 | ndof=0; |
372 | |
373 | |
374 | trajectory[0].Skk=S; |
375 | trajectory[0].Ckk=C; |
376 | |
377 | double doca2=0.; |
378 | |
379 | |
380 | unsigned int cdc_index=cdchits.size()-1; |
381 | bool more_hits=true; |
382 | const DCDCWire *wire=cdchits[cdc_index]->wire; |
383 | DVector3 origin=wire->origin; |
384 | double z0=origin.z(); |
385 | double vz=wire->udir.z(); |
386 | if (VERBOSE) jout << " Starting in Ring " << wire->ring << endl; |
387 | DVector3 wdir=(1./vz)*wire->udir; |
388 | DVector3 wirepos=origin+(trajectory[0].z-z0)*wdir; |
389 | |
390 | |
391 | double dx=S(state_x)-wirepos.X(); |
392 | double dy=S(state_y)-wirepos.Y(); |
393 | double old_doca2=dx*dx+dy*dy; |
394 | |
395 | |
396 | S0=trajectory[0].S; |
397 | J=trajectory[0].J; |
398 | for (unsigned int k=1;k<trajectory.size();k++){ |
399 | if (!C.IsPosDef()) return UNRECOVERABLE_ERROR; |
400 | |
401 | |
402 | S=trajectory[k].S+J*(S-S0); |
403 | C=J*C*J.Transpose(); |
404 | |
405 | |
406 | trajectory[k].Skk=S; |
407 | trajectory[k].Ckk=C; |
408 | |
409 | |
410 | S0=trajectory[k].S; |
411 | J=trajectory[k].J; |
412 | |
413 | |
414 | wirepos=origin+(trajectory[k].z-z0)*wdir; |
415 | |
416 | |
417 | dx=S(state_x)-wirepos.X(); |
418 | dy=S(state_y)-wirepos.Y(); |
419 | doca2=dx*dx+dy*dy; |
420 | if (VERBOSE > 10) jout<< "At Position " << S(state_x) << " " << S(state_y) << " " << trajectory[k].z << " doca2 " << doca2 << endl; |
421 | |
422 | if (doca2>old_doca2 && more_hits){ |
423 | |
424 | |
425 | double tx=S(state_tx),ty=S(state_ty); |
426 | DVector3 pos0(S(state_x),S(state_y),trajectory[k].z); |
427 | DVector3 tdir(tx,ty,1.); |
428 | |
429 | |
430 | DVector3 diff=pos0-origin; |
431 | double dx0=diff.x(),dy0=diff.y(); |
432 | double wdir_dot_diff=diff.Dot(wdir); |
433 | double tdir_dot_diff=diff.Dot(tdir); |
434 | double tdir_dot_wdir=tdir.Dot(wdir); |
435 | double tdir2=tdir.Mag2(); |
436 | double wdir2=wdir.Mag2(); |
437 | double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir; |
438 | double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff; |
439 | double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff; |
440 | double scale=1./D; |
441 | double s=scale*N; |
442 | double t=scale*N1; |
443 | diff+=s*tdir-t*wdir; |
444 | double d=diff.Mag()+d_EPS; |
445 | |
446 | |
447 | double dmeas=0.39,delta=0.; |
448 | double tdrift=cdchits[cdc_index]->tdrift-trajectory[k].t; |
449 | if (fit_type==kTimeBased){ |
450 | double phi_d=diff.Phi(); |
451 | double dphi=phi_d-origin.Phi(); |
452 | while (dphi>M_PI3.14159265358979323846) dphi-=2*M_PI3.14159265358979323846; |
453 | while (dphi<-M_PI3.14159265358979323846) dphi+=2*M_PI3.14159265358979323846; |
454 | |
455 | int ring_index=cdchits[cdc_index]->wire->ring-1; |
456 | int straw_index=cdchits[cdc_index]->wire->straw-1; |
457 | double dz=t*wdir.z(); |
458 | double delta=max_sag[ring_index][straw_index]*(1.-dz*dz/5625.) |
459 | *cos(phi_d+sag_phi_offset[ring_index][straw_index]); |
460 | CDCDriftParameters(dphi,delta,tdrift,dmeas,V); |
461 | } |
462 | |
463 | |
464 | double res=dmeas-d; |
465 | if (VERBOSE>5) jout << " Residual " << res << endl; |
466 | |
467 | |
468 | |
469 | double one_over_d=1./d; |
470 | double diffx=diff.x(),diffy=diff.y(),diffz=diff.z(); |
471 | double wx=wdir.x(),wy=wdir.y(); |
472 | |
473 | double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0; |
474 | double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx; |
475 | double dtdtx=scale*(dN1dtx-t*dDdtx); |
476 | |
477 | double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0; |
478 | double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy; |
479 | double dtdty=scale*(dN1dty-t*dDdty); |
480 | |
481 | double dNdtx=wx*wdir_dot_diff-wdir2*dx0; |
482 | double dsdtx=scale*(dNdtx-s*dDdtx); |
483 | |
484 | double dNdty=wy*wdir_dot_diff-wdir2*dy0; |
485 | double dsdty=scale*(dNdty-s*dDdty); |
486 | |
487 | H(state_tx)=H_T(state_tx) |
488 | =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx) |
489 | +diffz*(dsdtx-dtdtx)); |
490 | H(state_ty)=H_T(state_ty) |
491 | =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty) |
492 | +diffz*(dsdty-dtdty)); |
493 | |
494 | double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx); |
495 | double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx); |
496 | double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty); |
497 | double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty); |
498 | |
499 | H(state_x)=H_T(state_x) |
500 | =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy) |
501 | +diffz*(dsdx-dtdx)); |
502 | H(state_y)=H_T(state_y) |
503 | =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy) |
504 | +diffz*(dsdy-dtdy)); |
505 | |
506 | double InvV=1./(V+H*C*H_T); |
507 | |
508 | |
509 | double chi2check=res*res*InvV; |
510 | if (chi2check < CHI2CUT || DO_PRUNING == 0 || (COSMICS && iter == 0)){ |
511 | if (VERBOSE>5) jout << "Hit Added to track " << endl; |
512 | if (cdchits[cdc_index]->wire->ring!=RING_TO_SKIP){ |
513 | |
514 | |
515 | K=InvV*(C*H_T); |
516 | |
517 | |
518 | DMatrix4x4 Ctest=C-K*(H*C); |
519 | |
520 | |
521 | |
522 | |
523 | |
524 | |
525 | if (!Ctest.IsPosDef()) return VALUE_OUT_OF_RANGE; |
526 | C=Ctest; |
527 | if(VERBOSE>10) C.Print(); |
528 | |
529 | |
530 | S+=res*K; |
531 | if(VERBOSE>10) S.Print(); |
532 | |
533 | |
534 | |
535 | res=res-H*K*res; |
536 | |
537 | |
538 | double fit_V=V-H*C*H_T; |
539 | chi2+=res*res/fit_V; |
540 | ndof++; |
541 | |
542 | |
543 | updates[cdc_index].V=fit_V; |
544 | } |
545 | else { |
546 | updates[cdc_index].V=V; |
547 | } |
548 | |
549 | |
550 | used_hits[cdc_index]=1; |
551 | |
552 | |
553 | updates[cdc_index].resi=res; |
554 | updates[cdc_index].d=d; |
555 | updates[cdc_index].delta=delta; |
556 | updates[cdc_index].S=S; |
557 | updates[cdc_index].C=C; |
558 | updates[cdc_index].tdrift=tdrift; |
559 | updates[cdc_index].ddrift=dmeas; |
560 | updates[cdc_index].s=29.98*trajectory[k].t; |
561 | trajectory[k].id=cdc_index+1; |
562 | } |
563 | |
564 | if (cdc_index>0){ |
565 | cdc_index--; |
566 | |
567 | |
568 | wire=cdchits[cdc_index]->wire; |
569 | if (VERBOSE>5) { |
570 | jout << " Next Wire ring " << wire->ring << " straw " << wire->straw << " origin udir" << endl; |
571 | wire->origin.Print(); wire->udir.Print(); |
572 | } |
573 | origin=wire->origin; |
574 | z0=origin.z(); |
575 | vz=wire->udir.z(); |
576 | wdir=(1./vz)*wire->udir; |
577 | wirepos=origin+((trajectory[k].z-z0))*wdir; |
578 | |
579 | |
580 | dx=S(state_x)-wirepos.x(); |
581 | dy=S(state_y)-wirepos.y(); |
582 | doca2=dx*dx+dy*dy; |
583 | |
584 | } |
585 | else more_hits=false; |
586 | } |
587 | |
588 | old_doca2=doca2; |
589 | } |
590 | if (ndof<=4) return VALUE_OUT_OF_RANGE; |
591 | |
592 | ndof-=4; |
593 | |
594 | return NOERROR; |
595 | } |
596 | |
597 | |
598 | jerror_t DTrackFitterStraightTrack::Smooth(vector<cdc_update_t>&cdc_updates){ |
599 | unsigned int max=best_trajectory.size()-1; |
600 | DMatrix4x1 S=(best_trajectory[max].Skk); |
601 | DMatrix4x4 C=(best_trajectory[max].Ckk); |
602 | DMatrix4x4 JT=best_trajectory[max].J.Transpose(); |
603 | DMatrix4x1 Ss=S; |
604 | DMatrix4x4 Cs=C; |
605 | DMatrix4x4 A,dC; |
606 | DMatrix1x4 H; |
607 | DMatrix4x1 H_T; |
608 | |
609 | const double d_EPS=1e-8; |
610 | |
611 | for (unsigned int m=max-1;m>0;m--){ |
612 | if (best_trajectory[m].id>0){ |
613 | unsigned int id=best_trajectory[m].id-1; |
614 | A=cdc_updates[id].C*JT*C.Invert(); |
615 | Ss=cdc_updates[id].S+A*(Ss-S); |
616 | |
617 | dC=A*(Cs-C)*A.Transpose(); |
618 | Cs=cdc_updates[id].C+dC; |
619 | if (VERBOSE > 10) { |
620 | jout << " In Smoothing Step Using ID " << id << "/" << cdc_updates.size() << " for ring " << cdchits[id]->wire->ring << endl; |
621 | jout << " A cdc_updates[id].C Ss Cs " << endl; |
622 | A.Print(); cdc_updates[id].C.Print(); Ss.Print(); Cs.Print(); |
623 | } |
624 | if(!Cs.IsPosDef()) { |
625 | if (VERBOSE) jout << "Cs is not PosDef!" << endl; |
626 | return VALUE_OUT_OF_RANGE; |
627 | } |
628 | |
629 | const DCDCWire *wire=cdchits[id]->wire; |
630 | DVector3 origin=wire->origin; |
631 | double z0=origin.z(); |
632 | double vz=wire->udir.z(); |
633 | DVector3 wdir=(1./vz)*wire->udir; |
634 | DVector3 wirepos=origin+(best_trajectory[m].z-z0)*wdir; |
635 | |
636 | double x=Ss(state_x); |
637 | double y=Ss(state_y); |
638 | double tx=Ss(state_tx); |
639 | double ty=Ss(state_ty); |
640 | |
641 | DVector3 pos0(x,y,best_trajectory[m].z); |
642 | DVector3 tdir(tx,ty,1.); |
643 | |
644 | |
645 | DVector3 diff=pos0-origin; |
646 | double dx0=diff.x(),dy0=diff.y(); |
647 | double wdir_dot_diff=diff.Dot(wdir); |
648 | double tdir_dot_diff=diff.Dot(tdir); |
649 | double tdir_dot_wdir=tdir.Dot(wdir); |
650 | double tdir2=tdir.Mag2(); |
651 | double wdir2=wdir.Mag2(); |
652 | double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir; |
653 | double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff; |
654 | double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff; |
655 | double scale=1./D; |
656 | double s=scale*N; |
657 | double t=scale*N1; |
658 | diff+=s*tdir-t*wdir; |
659 | double d=diff.Mag()+d_EPS; |
660 | double ddrift = cdc_updates[id].ddrift; |
661 | |
662 | double resi = ddrift - d; |
663 | |
664 | |
665 | { |
666 | double one_over_d=1./d; |
667 | double diffx=diff.x(),diffy=diff.y(),diffz=diff.z(); |
668 | double wx=wdir.x(),wy=wdir.y(); |
669 | |
670 | double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0; |
671 | double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx; |
672 | double dtdtx=scale*(dN1dtx-t*dDdtx); |
673 | |
674 | double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0; |
675 | double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy; |
676 | double dtdty=scale*(dN1dty-t*dDdty); |
677 | |
678 | double dNdtx=wx*wdir_dot_diff-wdir2*dx0; |
679 | double dsdtx=scale*(dNdtx-s*dDdtx); |
680 | |
681 | double dNdty=wy*wdir_dot_diff-wdir2*dy0; |
682 | double dsdty=scale*(dNdty-s*dDdty); |
683 | |
684 | H(state_tx)=H_T(state_tx) |
685 | =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx) |
686 | +diffz*(dsdtx-dtdtx)); |
687 | H(state_ty)=H_T(state_ty) |
688 | =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty) |
689 | +diffz*(dsdty-dtdty)); |
690 | |
691 | double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx); |
692 | double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx); |
693 | double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty); |
694 | double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty); |
695 | |
696 | H(state_x)=H_T(state_x) |
697 | =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy) |
698 | +diffz*(dsdx-dtdx)); |
699 | H(state_y)=H_T(state_y) |
700 | =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy) |
701 | +diffz*(dsdy-dtdy)); |
702 | } |
703 | double V=cdc_updates[id].V; |
704 | |
705 | if (VERBOSE > 10) jout << " d " << d << " H*S " << H*S << endl; |
706 | if (cdchits[id]->wire->ring==RING_TO_SKIP){ |
707 | V=V+H*Cs*H_T; |
708 | } |
709 | else{ |
710 | V=V-H*Cs*H_T; |
711 | } |
712 | if (V<0) return VALUE_OUT_OF_RANGE; |
713 | |
714 | |
715 | double myscale=1./sqrt(1.+tx*tx+ty*ty); |
716 | double cosThetaRel=wire->udir.Dot(DVector3(myscale*tx,myscale*ty,myscale)); |
717 | pull_t thisPull(resi,sqrt(V), |
718 | best_trajectory[m].t*SPEED_OF_LIGHT29.9792458, |
719 | cdc_updates[id].tdrift, |
720 | d,cdchits[id], NULL__null, |
721 | diff.Phi(), |
722 | best_trajectory[m].z,cosThetaRel, |
723 | cdc_updates[id].tdrift); |
724 | |
725 | |
726 | double wtx=wire->udir.X(), wty=wire->udir.Y(), wtz=wire->udir.Z(); |
727 | double wx=wire->origin.X(), wy=wire->origin.Y(), wz=wire->origin.Z(); |
728 | |
729 | double z=best_trajectory[m].z; |
730 | double tx2=tx*tx, ty2=ty*ty; |
731 | double wtx2=wtx*wtx, wty2=wty*wty, wtz2=wtz*wtz; |
732 | double denom=(1 + ty2)*wtx2 + (1 + tx2)*wty2 - 2*ty*wty*wtz + (tx2 + ty2)*wtz2 - 2*tx*wtx*(ty*wty + wtz) +d_EPS; |
733 | double denom2=denom*denom; |
734 | double c1=-(wtx - tx*wtz)*(wy - y); |
735 | double c2=wty*(wx - tx*wz - x + tx*z); |
736 | double c3=ty*(-(wtz*wx) + wtx*wz + wtz*x - wtx*z); |
737 | double dscale=0.5*(1./d); |
738 | |
739 | vector<double> derivatives(11); |
740 | |
741 | derivatives[CDCTrackD::dDOCAdOriginX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3))/denom; |
742 | |
743 | derivatives[CDCTrackD::dDOCAdOriginY]=dscale*(2*(-wtx + tx*wtz)*(c1 + c2 + c3))/denom; |
744 | |
745 | derivatives[CDCTrackD::dDOCAdOriginZ]=dscale*(2*(ty*wtx - tx*wty)*(c1 + c2 + c3))/denom; |
746 | |
747 | derivatives[CDCTrackD::dDOCAdDirX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3)* |
748 | (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) + |
749 | wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2; |
750 | |
751 | derivatives[CDCTrackD::dDOCAdDirY]=dscale*(-2*(wtx - tx*wtz)*(c1 + c2 + c3)* |
752 | (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) + |
753 | wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2; |
754 | |
755 | derivatives[CDCTrackD::dDOCAdDirZ]=dscale*(-2*(ty*wtx - tx*wty)*(c1 + c2 + c3)* |
756 | (-(tx*(ty*wty + wtz)*(wx - x)) + tx2*(wty*(wy - y) + wtz*(wz - z)) + (wty - ty*wtz)*(wy - y + ty*(-wz + z)) + |
757 | wtx*((1 + ty2)*wx - (1 + ty2)*x + tx*(-(ty*wy) - wz + ty*y + z))))/denom2; |
758 | |
759 | derivatives[CDCTrackD::dDOCAdS0]=-derivatives[CDCTrackD::dDOCAdOriginX]; |
760 | |
761 | derivatives[CDCTrackD::dDOCAdS1]=-derivatives[CDCTrackD::dDOCAdOriginY]; |
762 | |
763 | derivatives[CDCTrackD::dDOCAdS2]=dscale*(2*(wty - ty*wtz)*(-c1 - c2 - c3)* |
764 | (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z + |
765 | tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) + |
766 | ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2; |
767 | |
768 | derivatives[CDCTrackD::dDOCAdS3]=dscale*(2*(wtx - tx*wtz)*(c1 + c2 + c3)* |
769 | (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z + |
770 | tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) + |
771 | ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2; |
772 | |
773 | thisPull.AddTrackDerivatives(derivatives); |
774 | pulls.push_back(thisPull); |
775 | } |
776 | else{ |
777 | A=best_trajectory[m].Ckk*JT*C.Invert(); |
778 | Ss=best_trajectory[m].Skk+A*(Ss-S); |
779 | Cs=best_trajectory[m].Ckk+A*(Cs-C)*A.Transpose(); |
780 | } |
781 | |
782 | S=best_trajectory[m].Skk; |
783 | C=best_trajectory[m].Ckk; |
784 | JT=best_trajectory[m].J.Transpose(); |
785 | } |
786 | |
787 | return NOERROR; |
788 | } |
789 | |
790 | |
791 | jerror_t DTrackFitterStraightTrack::SetReferenceTrajectory(double t0,double z, |
792 | DMatrix4x1 &S, |
793 | const DCDCTrackHit *last_cdc, |
794 | double &dzsign){ |
795 | DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.); |
796 | |
797 | double ds=0.5; |
798 | double t=t0; |
799 | |
800 | |
801 | double last_y=last_cdc->wire->origin.y(); |
802 | double last_r2=last_cdc->wire->origin.Perp2(); |
803 | |
804 | DVector3 dir(S(state_tx),S(state_ty),dzsign); |
805 | if (fabs(dir.Theta() - M_PI_21.57079632679489661923) < 0.2) ds = 0.1; |
806 | |
807 | |
808 | double dz=dzsign*ds/sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
809 | |
810 | if (VERBOSE) { |
811 | if (COSMICS) jout << " Swimming Reference Trajectory last CDC y " << last_y << " dz "<< dz << endl; |
812 | else jout << " Swimming Reference Trajectory last CDC r2 " << last_r2 << " dz "<< dz << endl; |
813 | jout << " S" << endl; S.Print(); jout << "z= "<< z << endl; |
814 | jout << " Last CDC ring " << last_cdc->wire->ring << " straw " << last_cdc->wire->straw << endl; |
815 | } |
816 | unsigned int numsteps=0; |
817 | const unsigned int MAX_STEPS=5000; |
818 | bool done=false; |
819 | do{ |
820 | numsteps++; |
821 | z+=dz; |
822 | J(state_x,state_tx)=-dz; |
823 | J(state_y,state_ty)=-dz; |
824 | |
825 | t+=ds/29.98; |
826 | |
827 | S(state_x)+=S(state_tx)*dz; |
828 | S(state_y)+=S(state_ty)*dz; |
829 | |
830 | if (z > downstreamEndplate && dz < 0) continue; |
831 | if (z < upstreamEndplate && dz > 0) continue; |
832 | trajectory.push_front(trajectory_t(z,t,S,J,DMatrix4x1(),DMatrix4x4())); |
833 | |
834 | done=((z>downstreamEndplate) || (z<upstreamEndplate)); |
835 | if (COSMICS==false){ |
836 | double r2=S(state_x)*S(state_x)+S(state_y)*S(state_y); |
837 | done |= (r2>last_r2); |
838 | } |
839 | }while (!done && numsteps<MAX_STEPS); |
840 | |
841 | if (VERBOSE) |
842 | { |
843 | if (VERBOSE > 10){ |
844 | printf("Trajectory:\n"); |
845 | for (unsigned int i=0;i<trajectory.size();i++){ |
846 | printf(" x %f y %f z %f\n",trajectory[i].S(state_x), |
847 | trajectory[i].S(state_y),trajectory[i].z); |
848 | } |
849 | } |
850 | else{ |
851 | printf("%i step trajectory Begin/End:\n", numsteps); |
852 | printf(" x %f y %f z %f\n",trajectory[0].S(state_x), trajectory[0].S(state_y), trajectory[0].z); |
853 | if (trajectory.size() > 1) printf(" x %f y %f z %f\n",trajectory[trajectory.size()-1].S(state_x), |
854 | trajectory[trajectory.size()-1].S(state_y),trajectory[trajectory.size()-1].z); |
855 | } |
856 | } |
857 | if (trajectory.size()<2) return UNRECOVERABLE_ERROR; |
858 | return NOERROR; |
859 | } |
860 | |
861 | |
862 | DTrackFitter::fit_status_t DTrackFitterStraightTrack::FitTrack(void){ |
863 | DTrackFitter::fit_status_t status=DTrackFitter::kFitNotDone; |
864 | if (cdchits.size()+fdchits.size() < 4) return status; |
865 | |
866 | |
867 | DVector3 input_pos=input_params.position(); |
868 | DVector3 input_mom=input_params.momentum(); |
869 | double pz=input_mom.z(); |
870 | DMatrix4x1 S(input_pos.x(),input_pos.y(),input_mom.x()/pz,input_mom.y()/pz); |
871 | |
872 | |
873 | DMatrix4x4 C; |
874 | C(state_x,state_x)=C(state_y,state_y)=1.0; |
875 | C(state_tx,state_tx)=C(state_ty,state_ty)=0.01; |
876 | |
877 | |
878 | double z=input_pos.z(); |
879 | double t0=input_params.t0(); |
880 | |
881 | switch(input_params.t0_detector()){ |
882 | case SYS_TOF: |
883 | mVarT0=0.01; |
884 | break; |
885 | case SYS_CDC: |
886 | mVarT0=25.; |
887 | break; |
888 | case SYS_FDC: |
889 | mVarT0=25.; |
890 | break; |
891 | case SYS_BCAL: |
892 | mVarT0=0.25; |
893 | break; |
894 | case SYS_START: |
895 | mVarT0=0.09; |
896 | break; |
897 | default: |
898 | mVarT0=0.; |
899 | break; |
900 | } |
901 | |
902 | |
903 | chisq=1e16; |
904 | Ndof=0; |
905 | |
906 | |
907 | if (cdchits.size()>0){ |
908 | if (COSMICS){ |
909 | stable_sort(cdchits.begin(),cdchits.end(),DTrackFitterStraightTrack_cdc_hit_reverse_cmp); |
910 | } |
911 | else{ |
912 | stable_sort(cdchits.begin(),cdchits.end(),DTrackFitterStraightTrack_cdc_hit_radius_cmp); |
913 | } |
914 | } |
915 | |
916 | if (fdchits.size()>0){ |
917 | |
918 | stable_sort(fdchits.begin(),fdchits.end(),DTrackFitterStraightTrack_fdc_hit_cmp); |
919 | status=FitForwardTrack(t0,z,S,C,chisq,Ndof); |
920 | } |
921 | else if (cdchits.size()>0){ |
922 | double dzsign=(pz>0)?1.:-1.; |
923 | if (COSMICS){ |
924 | dzsign=(S(state_ty)>0.)?1.:-1.; |
925 | } |
926 | status=FitCentralTrack(z,t0,dzsign,S,C,chisq,Ndof); |
927 | } |
928 | |
929 | if (status==DTrackFitter::kFitSuccess){ |
930 | |
931 | double tx=S(state_tx),ty=S(state_ty); |
932 | DVector3 mom(tx,ty,1.); |
933 | mom.SetMag(1.); |
934 | |
935 | |
936 | TMatrixFSym errMatrix(5); |
937 | for(unsigned int loc_i = 0; loc_i < 4; ++loc_i){ |
938 | for(unsigned int loc_j = 0; loc_j < 4; ++loc_j){ |
939 | errMatrix(loc_i, loc_j) = C(loc_i, loc_j); |
940 | } |
941 | } |
942 | errMatrix(4,4)=1.; |
943 | |
944 | |
945 | double sign=(mom.Theta()>M_PI_21.57079632679489661923)?-1.:1.; |
946 | fit_params.setErrorMatrix(Get7x7ErrorMatrix(errMatrix,S,sign)); |
947 | |
948 | DVector3 pos; |
949 | if (COSMICS==false){ |
950 | DVector3 beamdir(0.,0.,1.); |
951 | DVector3 beampos(0.,0.,65.); |
952 | finder->FindDoca(z,S,beamdir,beampos,&pos); |
953 | |
954 | |
955 | double dz=pos.z()-z; |
956 | DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.); |
957 | J(state_x,state_tx)=dz; |
958 | J(state_y,state_ty)=dz; |
959 | |
960 | C=J*C*J.Transpose(); |
961 | } |
962 | else{ |
963 | pos.SetXYZ(S(state_x),S(state_y),z); |
964 | } |
965 | |
966 | fit_params.setForwardParmFlag(true); |
967 | |
968 | fit_params.setPosition(pos); |
969 | fit_params.setMomentum(mom); |
970 | |
971 | |
972 | auto locTrackingCovarianceMatrix = dResourcePool_TMatrixFSym->Get_SharedResource(); |
973 | locTrackingCovarianceMatrix->ResizeTo(5, 5); |
974 | locTrackingCovarianceMatrix->Zero(); |
975 | for(unsigned int loc_i = 0; loc_i < 4; ++loc_i){ |
976 | for(unsigned int loc_j = 0; loc_j < 4; ++loc_j){ |
977 | (*locTrackingCovarianceMatrix)(loc_i, loc_j) = C(loc_i, loc_j); |
978 | } |
979 | } |
980 | (*locTrackingCovarianceMatrix)(4,4)=1.; |
981 | fit_params.setTrackingErrorMatrix(locTrackingCovarianceMatrix); |
982 | |
983 | |
984 | GetExtrapolations(pos,mom); |
985 | |
986 | } |
987 | |
988 | return status; |
989 | } |
990 | |
991 | |
992 | unsigned int DTrackFitterStraightTrack::Locate(const vector<double>&xx,double x) const{ |
993 | int n=xx.size(); |
994 | if (x==xx[0]) return 0; |
995 | else if (x==xx[n-1]) return n-2; |
996 | |
997 | int jl=-1; |
998 | int ju=n; |
999 | int ascnd=(xx[n-1]>=xx[0]); |
1000 | while(ju-jl>1){ |
1001 | int jm=(ju+jl)>>1; |
1002 | if ( (x>=xx[jm])==ascnd) |
1003 | jl=jm; |
1004 | else |
1005 | ju=jm; |
1006 | } |
1007 | return jl; |
1008 | } |
1009 | |
1010 | |
1011 | DTrackFitter::fit_status_t DTrackFitterStraightTrack::FitCentralTrack(double &z0,double t0, |
1012 | double dzsign, |
1013 | DMatrix4x1 &Sbest, |
1014 | DMatrix4x4 &Cbest, |
1015 | double &chi2_best, |
1016 | int &ndof_best){ |
1017 | |
1018 | DMatrix4x1 S(Sbest); |
1019 | DMatrix4x4 C(Cbest),C0(Cbest); |
1020 | |
1021 | double chi2=chi2_best; |
1022 | int ndof=ndof_best; |
1023 | |
1024 | unsigned int numhits=cdchits.size(); |
1025 | unsigned int maxindex=numhits-1; |
1026 | |
1027 | |
1028 | vector<int> used_cdc_hits(numhits); |
1029 | vector<int> used_cdc_hits_best_fit(numhits); |
1030 | |
1031 | |
1032 | vector<cdc_update_t>updates(numhits); |
1033 | vector<cdc_update_t>best_updates(numhits); |
1034 | |
1035 | |
1036 | best_trajectory.clear(); |
1037 | |
1038 | |
1039 | int iter=0; |
1040 | for(iter=0;iter<20;iter++){ |
1041 | if (VERBOSE) jout << " Performing Pass iter " << iter << endl; |
1042 | |
1043 | trajectory.clear(); |
1044 | if (SetReferenceTrajectory(t0,z0,S,cdchits[maxindex],dzsign) |
1045 | !=NOERROR) break; |
1046 | |
1047 | if (VERBOSE) jout << " Reference Trajectory Set " << endl; |
1048 | C=C0; |
1049 | if (KalmanFilter(S,C,used_cdc_hits,updates,chi2,ndof,iter)!=NOERROR) break; |
1050 | if (VERBOSE) jout << " Filter returns NOERROR" << endl; |
1051 | if (iter>0 && (fabs(chi2_best-chi2)<0.1 || chi2>chi2_best)) break; |
1052 | |
1053 | |
1054 | Cbest=C; |
1055 | Sbest=S; |
1056 | |
1057 | |
1058 | best_trajectory.assign(trajectory.begin(),trajectory.end()); |
1059 | used_cdc_hits_best_fit.assign(used_cdc_hits.begin(),used_cdc_hits.end()); |
1060 | best_updates.assign(updates.begin(),updates.end()); |
1061 | |
1062 | chi2_best=chi2; |
1063 | ndof_best=ndof; |
1064 | } |
1065 | if (iter==0) return DTrackFitter::kFitFailed; |
1066 | |
1067 | |
1068 | z0=best_trajectory[best_trajectory.size()-1].z; |
1069 | |
1070 | |
1071 | if (Smooth(best_updates) == NOERROR) IsSmoothed=true; |
1072 | |
1073 | |
1074 | cdchits_used_in_fit.clear(); |
1075 | for (unsigned int m=0;m<used_cdc_hits_best_fit.size();m++){ |
1076 | if (used_cdc_hits_best_fit[m]){ |
1077 | cdchits_used_in_fit.push_back(cdchits[m]); |
1078 | } |
1079 | } |
1080 | |
1081 | return DTrackFitter::kFitSuccess; |
1082 | } |
1083 | |
1084 | |
1085 | |
1086 | |
1087 | |
1088 | |
1089 | |
1090 | double DTrackFitterStraightTrack::fdc_drift_distance(double time) const { |
1091 | if (time<0.) return 0.; |
1092 | double d=0.; |
1093 | double tsq=time*time; |
1094 | double t_high=DRIFT_FUNC_PARMS[4]; |
1095 | |
1096 | if (time<t_high){ |
1097 | d=DRIFT_FUNC_PARMS[0]*sqrt(time)+DRIFT_FUNC_PARMS[1]*time |
1098 | +DRIFT_FUNC_PARMS[2]*tsq+DRIFT_FUNC_PARMS[3]*tsq*time; |
1099 | } |
1100 | else{ |
1101 | double t_high_sq=t_high*t_high; |
1102 | d=DRIFT_FUNC_PARMS[0]*sqrt(t_high)+DRIFT_FUNC_PARMS[1]*t_high |
1103 | +DRIFT_FUNC_PARMS[2]*t_high_sq+DRIFT_FUNC_PARMS[3]*t_high_sq*t_high; |
1104 | d+=DRIFT_FUNC_PARMS[5]*(time-t_high); |
1105 | } |
1106 | |
1107 | return d; |
1108 | |
1109 | } |
1110 | |
1111 | |
1112 | double DTrackFitterStraightTrack::fdc_drift_variance(double t) const{ |
1113 | |
1114 | if (t<5.) t=5.; |
1115 | double sigma=DRIFT_RES_PARMS[0]/(t+1.)+DRIFT_RES_PARMS[1]+DRIFT_RES_PARMS[2]*t*t; |
1116 | |
1117 | return sigma*sigma; |
1118 | } |
1119 | |
1120 | |
1121 | DTrackFitter::fit_status_t |
1122 | DTrackFitterStraightTrack::FitForwardTrack(double t0,double &start_z, |
1123 | DMatrix4x1 &Sbest,DMatrix4x4 &Cbest,double &chi2_best,int &ndof_best){ |
1124 | |
1125 | DMatrix4x1 S(Sbest); |
1126 | DMatrix4x4 C(Cbest),C0(Cbest); |
1127 | |
1128 | |
1129 | unsigned int numfdchits=fdchits.size(); |
1130 | vector<int> used_fdc_hits(numfdchits); |
1131 | vector<int> used_fdc_hits_best_fit(numfdchits); |
1132 | |
1133 | unsigned int numcdchits=cdchits.size(); |
1134 | vector<int> used_cdc_hits(numcdchits); |
1135 | vector<int> used_cdc_hits_best_fit(numcdchits); |
1136 | |
1137 | |
1138 | vector<fdc_update_t>updates(numfdchits); |
1139 | vector<fdc_update_t>best_updates(numfdchits); |
1140 | vector<cdc_update_t>cdc_updates(numcdchits); |
1141 | vector<cdc_update_t>best_cdc_updates(numcdchits); |
1142 | |
1143 | vector<const DCDCTrackHit *> matchedCDCHits; |
1144 | |
1145 | |
1146 | double chi2=chi2_best; |
1147 | int ndof=ndof_best; |
1148 | |
1149 | |
1150 | best_trajectory.clear(); |
1151 | |
1152 | unsigned iter=0; |
1153 | |
1154 | for(iter=0;iter<20;iter++){ |
1155 | trajectory.clear(); |
1156 | if (SetReferenceTrajectory(t0,start_z,S)!=NOERROR) break; |
1157 | |
1158 | C=C0; |
1159 | if (KalmanFilter(S,C,used_fdc_hits,used_cdc_hits,updates,cdc_updates, |
1160 | chi2,ndof)!=NOERROR) break; |
1161 | |
1162 | |
1163 | if (iter>0 && (chi2>chi2_best || fabs(chi2_best-chi2)<0.1)) break; |
1164 | |
1165 | |
1166 | Cbest=C; |
1167 | Sbest=S; |
1168 | |
1169 | |
1170 | best_trajectory.assign(trajectory.begin(),trajectory.end()); |
1171 | used_cdc_hits_best_fit.assign(used_cdc_hits.begin(),used_cdc_hits.end()); |
1172 | used_fdc_hits_best_fit.assign(used_fdc_hits.begin(),used_fdc_hits.end()); |
1173 | best_updates.assign(updates.begin(),updates.end()); |
1174 | best_cdc_updates.assign(cdc_updates.begin(),cdc_updates.end()); |
1175 | |
1176 | chi2_best=chi2; |
1177 | ndof_best=ndof; |
1178 | } |
1179 | if (iter==0) return DTrackFitter::kFitFailed; |
1180 | |
1181 | |
1182 | start_z=best_trajectory[best_trajectory.size()-1].z; |
1183 | |
1184 | |
1185 | if (Smooth(best_updates,best_cdc_updates) == NOERROR) IsSmoothed=true; |
1186 | |
1187 | |
1188 | cdchits_used_in_fit.clear(); |
1189 | for (unsigned int m=0;m<used_cdc_hits_best_fit.size();m++){ |
1190 | if (used_cdc_hits_best_fit[m]){ |
1191 | cdchits_used_in_fit.push_back(cdchits[m]); |
1192 | } |
1193 | } |
1194 | fdchits_used_in_fit.clear(); |
1195 | for (unsigned int m=0;m<used_fdc_hits_best_fit.size();m++){ |
1196 | if (used_fdc_hits_best_fit[m]){ |
1197 | fdchits_used_in_fit.push_back(fdchits[m]); |
1198 | } |
1199 | } |
1200 | |
1201 | |
1202 | return DTrackFitter::kFitSuccess; |
1203 | } |
1204 | |
1205 | |
1206 | |
1207 | jerror_t |
1208 | DTrackFitterStraightTrack::SetReferenceTrajectory(double t0,double z, |
1209 | DMatrix4x1 &S){ |
1210 | const double EPS=1e-3; |
1211 | |
1212 | |
1213 | DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.); |
1214 | |
1215 | double dz=1.1; |
1216 | double t=t0; |
1217 | trajectory.push_front(trajectory_t(z,t,S,J,DMatrix4x1(),DMatrix4x4())); |
1218 | |
1219 | double zhit=z; |
1220 | double old_zhit=z; |
1221 | for (unsigned int i=0;i<fdchits.size();i++){ |
1222 | zhit=fdchits[i]->wire->origin.z(); |
1223 | dz=1.1; |
1224 | |
1225 | if (fabs(zhit-old_zhit)<EPS && i>0){ |
1226 | trajectory[0].numhits++; |
1227 | continue; |
1228 | } |
1229 | |
1230 | bool done=false; |
1231 | while (!done){ |
1232 | double new_z=z+dz; |
1233 | |
1234 | if (new_z>zhit){ |
1235 | dz=zhit-z; |
1236 | new_z=zhit; |
1237 | done=true; |
1238 | } |
1239 | J(state_x,state_tx)=-dz; |
1240 | J(state_y,state_ty)=-dz; |
1241 | |
1242 | t+=dz*sqrt(1+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty))/29.98; |
1243 | |
1244 | S(state_x)+=S(state_tx)*dz; |
1245 | S(state_y)+=S(state_ty)*dz; |
1246 | |
1247 | trajectory.push_front(trajectory_t(new_z,t,S,J,DMatrix4x1(), |
1248 | DMatrix4x4())); |
1249 | if (done){ |
1250 | trajectory[0].id=i+1; |
1251 | trajectory[0].numhits=1; |
1252 | } |
1253 | |
1254 | z=new_z; |
1255 | } |
1256 | old_zhit=zhit; |
1257 | } |
1258 | |
1259 | dz=1.1; |
1260 | J(state_x,state_tx)=-dz; |
1261 | J(state_y,state_ty)=-dz; |
1262 | |
1263 | |
1264 | t+=dz*sqrt(1+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty))/29.98; |
1265 | |
1266 | |
1267 | S(state_x)+=S(state_tx)*dz; |
1268 | S(state_y)+=S(state_ty)*dz; |
1269 | trajectory.push_front(trajectory_t(z+dz,t,S,J,DMatrix4x1(),DMatrix4x4())); |
1270 | |
1271 | if (false) |
1272 | { |
1273 | printf("Trajectory:\n"); |
1274 | for (unsigned int i=0;i<trajectory.size();i++){ |
1275 | printf(" x %f y %f z %f first hit %d num in layer %d\n",trajectory[i].S(state_x), |
1276 | trajectory[i].S(state_y),trajectory[i].z,trajectory[i].id, |
1277 | trajectory[i].numhits); |
1278 | } |
1279 | } |
1280 | |
1281 | return NOERROR; |
1282 | } |
1283 | |
1284 | |
1285 | jerror_t DTrackFitterStraightTrack::KalmanFilter(DMatrix4x1 &S,DMatrix4x4 &C, |
1286 | vector<int>&used_fdc_hits, |
1287 | vector<int>&used_cdc_hits, |
1288 | vector<fdc_update_t>&updates, |
1289 | vector<cdc_update_t>&cdc_updates, |
1290 | double &chi2,int &ndof){ |
1291 | DMatrix2x4 H; |
1292 | DMatrix4x2 H_T; |
1293 | DMatrix4x2 K; |
1294 | double VarMs=0.001; |
1295 | DMatrix2x2 V(0.0833,0.,0.,0.000256+VarMs); |
1296 | DMatrix2x2 Vtemp,InvV; |
1297 | DMatrix2x1 Mdiff; |
1298 | DMatrix4x4 I; |
1299 | DMatrix4x4 J; |
1300 | DMatrix4x1 S0; |
1301 | |
1302 | DMatrix1x4 H_CDC; |
1303 | DMatrix4x1 H_T_CDC; |
1304 | DMatrix4x1 K_CDC; |
1305 | double V_CDC; |
1306 | |
1307 | const double d_EPS=1e-8; |
1308 | |
1309 | |
1310 | for (unsigned int i=0;i<used_fdc_hits.size();i++) used_fdc_hits[i]=0; |
1311 | for (unsigned int i=0;i<used_cdc_hits.size();i++) used_cdc_hits[i]=0; |
1312 | |
1313 | |
1314 | chi2=0.; |
1315 | ndof=0; |
1316 | |
1317 | |
1318 | trajectory[0].Skk=S; |
1319 | trajectory[0].Ckk=C; |
1320 | |
1321 | |
1322 | S0=trajectory[0].S; |
1323 | J=trajectory[0].J; |
1324 | |
1325 | |
1326 | bool more_hits = cdchits.size() == 0 ? false: true; |
1327 | bool firstCDCStep=true; |
1328 | unsigned int cdc_index=0; |
1329 | const DCDCWire *wire; |
1330 | DVector3 origin,wdir,wirepos; |
1331 | double doca2=0.0, old_doca2=0.0; |
1332 | if(more_hits){ |
1333 | cdc_index=cdchits.size()-1; |
1334 | wire=cdchits[cdc_index]->wire; |
1335 | origin=wire->origin; |
1336 | double vz=wire->udir.z(); |
1337 | if (VERBOSE) jout << " Additional CDC Hits in FDC track Starting in Ring " << wire->ring << endl; |
1338 | wdir=(1./vz)*wire->udir; |
1339 | } |
1340 | |
1341 | for (unsigned int k=1;k<trajectory.size();k++){ |
1342 | if (C(0,0)<=0. || C(1,1)<=0. || C(2,2)<=0. || C(3,3)<=0.) |
1343 | return UNRECOVERABLE_ERROR; |
1344 | |
1345 | |
1346 | S=trajectory[k].S+J*(S-S0); |
1347 | C=J*C*J.Transpose(); |
1348 | |
1349 | |
1350 | trajectory[k].Skk=S; |
1351 | trajectory[k].Ckk=C; |
1352 | |
1353 | |
1354 | S0=trajectory[k].S; |
1355 | J=trajectory[k].J; |
1356 | |
1357 | |
1358 | if (trajectory[k].id>0){ |
1359 | unsigned int id=trajectory[k].id-1; |
1360 | |
1361 | double cospsi=cos(fdchits[id]->wire->angle); |
1362 | double sinpsi=sin(fdchits[id]->wire->angle); |
1363 | |
1364 | |
1365 | double x = S(state_x) + fdchits[id]->wire->angles.Z()*S(state_y); |
1366 | double y = S(state_y) - fdchits[id]->wire->angles.Z()*S(state_x); |
1367 | |
1368 | double tx = (S(state_tx) + fdchits[id]->wire->angles.Z()*S(state_ty) - fdchits[id]->wire->angles.Y()); |
1369 | double ty = (S(state_ty) - fdchits[id]->wire->angles.Z()*S(state_tx) + fdchits[id]->wire->angles.X()); |
1370 | |
1371 | if (std::isnan(x) || std::isnan(y)) return UNRECOVERABLE_ERROR; |
1372 | |
1373 | |
1374 | |
1375 | |
1376 | |
1377 | |
1378 | double vpred_wire_plane=y*cospsi+x*sinpsi; |
1379 | double upred_wire_plane=x*cospsi-y*sinpsi; |
1380 | double tu=tx*cospsi-ty*sinpsi; |
1381 | double tv=tx*sinpsi+ty*cospsi; |
1382 | |
1383 | |
1384 | |
1385 | double alpha=atan(tu); |
1386 | double cosalpha=cos(alpha); |
1387 | double cos2_alpha=cosalpha*cosalpha; |
1388 | double sinalpha=sin(alpha); |
1389 | double sin2_alpha=sinalpha*sinalpha; |
1390 | double cos2_alpha_minus_sin2_alpha=cos2_alpha-sin2_alpha; |
1391 | |
1392 | |
1393 | for (int m=trajectory[k].numhits-1;m>=0;m--){ |
1394 | unsigned int my_id=id+m; |
1395 | double uwire=fdchits[my_id]->w; |
1396 | |
1397 | double du=upred_wire_plane-uwire; |
1398 | double doca=du*cosalpha; |
1399 | |
1400 | |
1401 | double vpred=vpred_wire_plane; |
1402 | |
1403 | |
1404 | double v=fdchits[my_id]->s; |
1405 | |
1406 | |
1407 | double drift=0.; |
1408 | if (fit_type==kTimeBased){ |
1409 | double drift_time=fdchits[my_id]->time-trajectory[k].t; |
1410 | drift=(du>0.0?1.:-1.)*fdc_drift_distance(drift_time); |
1411 | V(0,0)=fdc_drift_variance(drift_time)+VarMs; |
1412 | } |
1413 | Mdiff(0)=drift-doca; |
1414 | Mdiff(1)=v-vpred; |
1415 | |
1416 | |
1417 | H_T(state_x,0)=cospsi*cosalpha; |
1418 | H_T(state_y,0)=-sinpsi*cosalpha; |
1419 | double temp=-du*sinalpha*cos2_alpha; |
1420 | H_T(state_tx,0)=cospsi*temp; |
1421 | H_T(state_ty,0)=-sinpsi*temp; |
1422 | double temp2=cosalpha*sinalpha*tv; |
1423 | H_T(state_x,1)=sinpsi-temp2*cospsi; |
1424 | H_T(state_y,1)=cospsi+temp2*sinpsi; |
1425 | double temp4=sinalpha*doca; |
1426 | double temp5=tv*cos2_alpha*du*cos2_alpha_minus_sin2_alpha; |
1427 | H_T(state_tx,1)=-sinpsi*temp4-cospsi*temp5; |
1428 | H_T(state_ty,1)=-cospsi*temp4+sinpsi*temp5; |
1429 | |
1430 | |
1431 | H(0,state_x)=H_T(state_x,0); |
1432 | H(0,state_y)=H_T(state_y,0); |
1433 | H(0,state_tx)=H_T(state_tx,0); |
1434 | H(0,state_ty)=H_T(state_ty,0); |
1435 | H(1,state_x)=H_T(state_x,1); |
1436 | H(1,state_y)=H_T(state_y,1); |
1437 | H(1,state_tx)=H_T(state_tx,1); |
1438 | H(1,state_ty)=H_T(state_ty,1); |
1439 | |
1440 | |
1441 | InvV=(V+H*C*H_T).Invert(); |
1442 | |
1443 | |
1444 | K=(C*H_T)*InvV; |
1445 | |
1446 | if (fdchits[my_id]->wire->layer!=PLANE_TO_SKIP){ |
1447 | |
1448 | |
1449 | |
1450 | |
1451 | |
1452 | |
1453 | S+=K*Mdiff; |
1454 | if(VERBOSE) S.Print(); |
1455 | |
1456 | C=C-K*(H*C); |
1457 | |
1458 | |
1459 | |
1460 | DMatrix2x2 RC=V-H*C*H_T; |
1461 | DMatrix2x1 res=Mdiff-H*K*Mdiff; |
1462 | |
1463 | chi2+=RC.Chi2(res); |
1464 | ndof+=2; |
1465 | |
1466 | |
1467 | updates[my_id].V=RC; |
1468 | } |
1469 | else{ |
1470 | updates[my_id].V=V; |
1471 | } |
1472 | |
1473 | used_fdc_hits[my_id]=1; |
1474 | |
1475 | |
1476 | updates[my_id].d=doca; |
1477 | updates[my_id].S=S; |
1478 | updates[my_id].C=C; |
1479 | updates[my_id].tdrift=fdchits[my_id]->time-trajectory[k].t; |
1480 | updates[my_id].s=29.98*trajectory[k].t; |
1481 | } |
1482 | } |
1483 | |
1484 | if (more_hits && trajectory[k].z < downstreamEndplate){ |
1485 | |
1486 | double z0=origin.Z(); |
1487 | wirepos=origin+(trajectory[k].z-z0)*wdir; |
1488 | |
1489 | |
1490 | double dx=S(state_x)-wirepos.X(); |
1491 | double dy=S(state_y)-wirepos.Y(); |
1492 | doca2=dx*dx+dy*dy; |
1493 | if (VERBOSE > 10) jout<< "At Position " << S(state_x) << " " << S(state_y) << " " << trajectory[k].z << " doca2 " << doca2 << endl; |
1494 | |
1495 | if (doca2>old_doca2 && more_hits && !firstCDCStep){ |
1496 | |
1497 | |
1498 | double tx=S(state_tx),ty=S(state_ty); |
1499 | DVector3 pos0(S(state_x),S(state_y),trajectory[k].z); |
1500 | DVector3 tdir(tx,ty,1.); |
1501 | |
1502 | |
1503 | DVector3 diff=pos0-origin; |
1504 | double dx0=diff.x(),dy0=diff.y(); |
1505 | double wdir_dot_diff=diff.Dot(wdir); |
1506 | double tdir_dot_diff=diff.Dot(tdir); |
1507 | double tdir_dot_wdir=tdir.Dot(wdir); |
1508 | double tdir2=tdir.Mag2(); |
1509 | double wdir2=wdir.Mag2(); |
1510 | double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir; |
1511 | double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff; |
1512 | double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff; |
1513 | double scale=1./D; |
1514 | double s=scale*N; |
1515 | double t=scale*N1; |
1516 | diff+=s*tdir-t*wdir; |
1517 | double d=diff.Mag()+d_EPS; |
1518 | |
1519 | |
1520 | double dmeas=0.39,delta=0.; |
1521 | double tdrift=cdchits[cdc_index]->tdrift-trajectory[k].t; |
1522 | if (fit_type==kTimeBased){ |
1523 | double phi_d=diff.Phi(); |
1524 | double dphi=phi_d-origin.Phi(); |
1525 | while (dphi>M_PI3.14159265358979323846) dphi-=2*M_PI3.14159265358979323846; |
1526 | while (dphi<-M_PI3.14159265358979323846) dphi+=2*M_PI3.14159265358979323846; |
1527 | |
1528 | int ring_index=cdchits[cdc_index]->wire->ring-1; |
1529 | int straw_index=cdchits[cdc_index]->wire->straw-1; |
1530 | double dz=t*wdir.z(); |
1531 | double delta=max_sag[ring_index][straw_index]*(1.-dz*dz/5625.) |
1532 | *cos(phi_d+sag_phi_offset[ring_index][straw_index]); |
1533 | CDCDriftParameters(dphi,delta,tdrift,dmeas,V_CDC); |
1534 | } |
1535 | |
1536 | |
1537 | double res=dmeas-d; |
1538 | if (VERBOSE>5) jout << " Residual " << res << endl; |
1539 | |
1540 | double one_over_d=1./d; |
1541 | double diffx=diff.x(),diffy=diff.y(),diffz=diff.z(); |
1542 | double wx=wdir.x(),wy=wdir.y(); |
1543 | |
1544 | double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0; |
1545 | double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx; |
1546 | double dtdtx=scale*(dN1dtx-t*dDdtx); |
1547 | |
1548 | double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0; |
1549 | double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy; |
1550 | double dtdty=scale*(dN1dty-t*dDdty); |
1551 | |
1552 | double dNdtx=wx*wdir_dot_diff-wdir2*dx0; |
1553 | double dsdtx=scale*(dNdtx-s*dDdtx); |
1554 | |
1555 | double dNdty=wy*wdir_dot_diff-wdir2*dy0; |
1556 | double dsdty=scale*(dNdty-s*dDdty); |
1557 | |
1558 | H_CDC(state_tx)=H_T_CDC(state_tx) |
1559 | =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx) |
1560 | +diffz*(dsdtx-dtdtx)); |
1561 | H_CDC(state_ty)=H_T_CDC(state_ty) |
1562 | =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty) |
1563 | +diffz*(dsdty-dtdty)); |
1564 | |
1565 | double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx); |
1566 | double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx); |
1567 | double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty); |
1568 | double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty); |
1569 | |
1570 | H_CDC(state_x)=H_T_CDC(state_x) |
1571 | =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy) |
1572 | +diffz*(dsdx-dtdx)); |
1573 | H_CDC(state_y)=H_T_CDC(state_y) |
1574 | =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy) |
1575 | +diffz*(dsdy-dtdy)); |
1576 | |
1577 | double InvV=1./(V_CDC+H_CDC*C*H_T_CDC); |
1578 | |
1579 | |
1580 | double chi2check=res*res*InvV; |
1581 | if (chi2check < CHI2CUT || DO_PRUNING == 0){ |
1582 | if (VERBOSE) jout << "CDC Hit Added to FDC track " << endl; |
1583 | if (cdchits[cdc_index]->wire->ring!=RING_TO_SKIP){ |
1584 | |
1585 | |
1586 | K_CDC=InvV*(C*H_T_CDC); |
1587 | |
1588 | DMatrix4x4 Ctest=C-K_CDC*(H_CDC*C); |
1589 | |
1590 | |
1591 | |
1592 | |
1593 | |
1594 | |
1595 | if (!Ctest.IsPosDef()) return VALUE_OUT_OF_RANGE; |
1596 | C=Ctest; |
1597 | if(VERBOSE>10) C.Print(); |
1598 | |
1599 | |
1600 | S+=res*K_CDC; |
1601 | if(VERBOSE) {jout << "traj[z]=" << trajectory[k].z<< endl; S.Print();} |
1602 | |
1603 | |
1604 | |
1605 | res=res-H_CDC*K_CDC*res; |
1606 | |
1607 | |
1608 | double fit_V=V_CDC-H_CDC*C*H_T_CDC; |
1609 | chi2+=res*res/fit_V; |
1610 | ndof++; |
1611 | |
1612 | |
1613 | cdc_updates[cdc_index].V=fit_V; |
1614 | } |
1615 | else { |
1616 | cdc_updates[cdc_index].V=V_CDC; |
1617 | } |
1618 | |
1619 | |
1620 | cdc_updates[cdc_index].resi=res; |
1621 | cdc_updates[cdc_index].d=d; |
1622 | cdc_updates[cdc_index].delta=delta; |
1623 | cdc_updates[cdc_index].S=S; |
1624 | cdc_updates[cdc_index].C=C; |
1625 | cdc_updates[cdc_index].tdrift=tdrift; |
1626 | cdc_updates[cdc_index].ddrift=dmeas; |
1627 | cdc_updates[cdc_index].s=29.98*trajectory[k].t; |
1628 | trajectory[k].id=cdc_index+1000; |
1629 | |
1630 | used_cdc_hits[cdc_index]=1; |
1631 | } |
1632 | |
1633 | if (cdc_index>0){ |
1634 | cdc_index--; |
1635 | |
1636 | |
1637 | wire=cdchits[cdc_index]->wire; |
1638 | if (VERBOSE>5) jout << " Next Wire ring " << wire->ring << endl; |
1639 | origin=wire->origin; |
1640 | double vz=wire->udir.z(); |
1641 | wdir=(1./vz)*wire->udir; |
1642 | wirepos=origin+((trajectory[k].z-z0))*wdir; |
1643 | |
1644 | |
1645 | dx=S(state_x)-wirepos.x(); |
1646 | dy=S(state_y)-wirepos.y(); |
1647 | doca2=dx*dx+dy*dy; |
1648 | |
1649 | } |
1650 | else more_hits=false; |
1651 | } |
1652 | firstCDCStep=false; |
1653 | old_doca2=doca2; |
1654 | } |
1655 | } |
1656 | |
1657 | ndof-=4; |
1658 | |
1659 | return NOERROR; |
1660 | } |
1661 | |
1662 | |
1663 | |
1664 | |
1665 | |
1666 | |
1667 | |
1668 | jerror_t |
1669 | DTrackFitterStraightTrack::Smooth(vector<fdc_update_t>&fdc_updates, |
1670 | vector<cdc_update_t>&cdc_updates){ |
1671 | unsigned int max=best_trajectory.size()-1; |
1672 | DMatrix4x1 S=(best_trajectory[max].Skk); |
1673 | DMatrix4x4 C=(best_trajectory[max].Ckk); |
1674 | DMatrix4x4 JT=best_trajectory[max].J.Transpose(); |
1675 | DMatrix4x1 Ss=S; |
1676 | DMatrix4x4 Cs=C; |
1677 | DMatrix4x4 A,dC; |
1678 | |
1679 | const double d_EPS=1e-8; |
1680 | |
1681 | for (unsigned int m=max-1;m>0;m--){ |
1682 | if (best_trajectory[m].id>0 && best_trajectory[m].id<1000){ |
1683 | unsigned int id=best_trajectory[m].id-1; |
1684 | A=fdc_updates[id].C*JT*C.Invert(); |
1685 | Ss=fdc_updates[id].S+A*(Ss-S); |
1686 | |
1687 | dC=A*(Cs-C)*A.Transpose(); |
1688 | Cs=fdc_updates[id].C+dC; |
1689 | |
1690 | double cosa=cos(fdchits[id]->wire->angle); |
1691 | double cos2a=cos(2*fdchits[id]->wire->angle); |
1692 | double sina=sin(fdchits[id]->wire->angle); |
1693 | double u=fdchits[id]->w; |
1694 | double v=fdchits[id]->s; |
1695 | |
1696 | |
1697 | double x = Ss(state_x) + fdchits[id]->wire->angles.Z() * Ss(state_y); |
1698 | double y = Ss(state_y) - fdchits[id]->wire->angles.Z() * Ss(state_x); |
1699 | |
1700 | double tx = Ss(state_tx) + fdchits[id]->wire->angles.Z() * Ss(state_ty) - fdchits[id]->wire->angles.Y(); |
1701 | double ty = Ss(state_ty) - fdchits[id]->wire->angles.Z() * Ss(state_tx) + fdchits[id]->wire->angles.X(); |
1702 | |
1703 | |
1704 | double vpred=x*sina+y*cosa; |
1705 | |
1706 | |
1707 | double upred=x*cosa-y*sina; |
1708 | |
1709 | |
1710 | double tu=tx*cosa-ty*sina; |
1711 | double alpha=atan(tu); |
1712 | double cosalpha=cos(alpha); |
1713 | |
1714 | double sinalpha=sin(alpha); |
1715 | |
1716 | |
1717 | double du=upred-u; |
1718 | double doca=du*cosalpha; |
1719 | |
1720 | double tv=tx*sina+ty*cosa; |
1721 | double resi_c=v-vpred; |
1722 | |
1723 | |
1724 | double drift = 0.0; |
1725 | int left_right = -999; |
1726 | double drift_time = fdc_updates[id].tdrift; |
1727 | if (fit_type == kTimeBased) { |
1728 | drift = (du > 0.0 ? 1.0 : -1.0) * fdc_drift_distance(drift_time); |
1729 | left_right = (du > 0.0 ? +1 : -1); |
1730 | } |
1731 | double resi_a = drift - doca; |
1732 | |
1733 | |
1734 | DMatrix2x2 V=fdc_updates[id].V; |
1735 | |
1736 | DMatrix4x2 H_T; |
1737 | double temp2=-tv*sinalpha; |
1738 | H_T(state_x,1)=sina+cosa*cosalpha*temp2; |
1739 | H_T(state_y,1)=cosa-sina*cosalpha*temp2; |
1740 | |
1741 | double cos2_minus_sin2=cosalpha*cosalpha-sinalpha*sinalpha; |
1742 | double doca_cosalpha=doca*cosalpha; |
1743 | H_T(state_tx,1)=-doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2); |
1744 | H_T(state_ty,1)=-doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2); |
1745 | |
1746 | H_T(state_x,0)=cosa*cosalpha; |
1747 | H_T(state_y,0)=-sina*cosalpha; |
1748 | double one_plus_tu2=1.+tu*tu; |
1749 | double factor=du*tu/sqrt(one_plus_tu2)/one_plus_tu2; |
1750 | H_T(state_ty,0)=sina*factor; |
1751 | H_T(state_tx,0)=-cosa*factor; |
1752 | |
1753 | |
1754 | DMatrix2x4 H; |
1755 | H(0,state_x)=H_T(state_x,0); |
1756 | H(0,state_y)=H_T(state_y,0); |
1757 | H(0,state_tx)=H_T(state_tx,0); |
1758 | H(0,state_ty)=H_T(state_ty,0); |
1759 | H(1,state_x)=H_T(state_x,1); |
1760 | H(1,state_y)=H_T(state_y,1); |
1761 | H(1,state_tx)=H_T(state_tx,1); |
1762 | H(1,state_ty)=H_T(state_ty,1); |
1763 | |
1764 | if (fdchits[id]->wire->layer == PLANE_TO_SKIP) { |
1765 | |
1766 | V = V + H * Cs * H_T; |
1767 | } else { |
1768 | |
1769 | |
1770 | |
1771 | |
1772 | |
1773 | |
1774 | V = V - H * dC * H_T; |
1775 | } |
1776 | |
1777 | |
1778 | |
1779 | |
1780 | |
1781 | |
1782 | |
1783 | |
1784 | double scale=1./sqrt(1.+tx*tx+ty*ty); |
1785 | double cosThetaRel=fdchits[id]->wire->udir.Dot(DVector3(scale*tx,scale*ty,scale)); |
1786 | DTrackFitter::pull_t thisPull(resi_a,sqrt(V(0,0)), |
1787 | best_trajectory[m].t*SPEED_OF_LIGHT29.9792458, |
1788 | fdc_updates[id].tdrift, |
1789 | fdc_updates[id].d, |
1790 | NULL__null,fdchits[id], |
1791 | 0.0, |
1792 | best_trajectory[m].z,cosThetaRel, |
1793 | 0.0, |
1794 | resi_c, sqrt(V(1,1)) |
1795 | ); |
1796 | thisPull.left_right = left_right; |
1797 | |
1798 | if (fdchits[id]->wire->layer!=PLANE_TO_SKIP){ |
1799 | vector<double> derivatives; |
1800 | derivatives.resize(FDCTrackD::size); |
1801 | |
1802 | |
1803 | derivatives[FDCTrackD::dDOCAW_dDeltaX] = -(1/sqrt(1 + pow(tx*cosa - ty*sina,2))); |
1804 | |
1805 | |
1806 | derivatives[FDCTrackD::dDOCAW_dDeltaZ] = (tx*cosa - ty*sina)/sqrt(1 + pow(tx*cosa - ty*sina,2)); |
1807 | |
1808 | |
1809 | derivatives[FDCTrackD::dDOCAW_dDeltaPhiX] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
1810 | |
1811 | |
1812 | derivatives[FDCTrackD::dDOCAW_dDeltaPhiY] = (cosa*(tx*cosa - ty*sina)*(-u + x*cosa - y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
1813 | |
1814 | |
1815 | derivatives[FDCTrackD::dDOCAW_dDeltaPhiZ] = (tx*ty*u*cos2a + (x + pow(ty,2)*x - tx*ty*y)*sina + |
1816 | cosa*(-(tx*ty*x) + y + pow(tx,2)*y + (pow(tx,2) - pow(ty,2))*u*sina))/ |
1817 | pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
1818 | |
1819 | |
1820 | derivatives[FDCTrackD::dDOCAW_dx] = cosa/sqrt(1 + pow(tx*cosa - ty*sina,2)); |
1821 | |
1822 | |
1823 | derivatives[FDCTrackD::dDOCAW_dy] = -(sina/sqrt(1 + pow(tx*cosa - ty*sina,2))); |
1824 | |
1825 | |
1826 | derivatives[FDCTrackD::dDOCAW_dtx] = -((cosa*(tx*cosa - ty*sina)*(-u + x*cosa - y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5)); |
1827 | |
1828 | |
1829 | derivatives[FDCTrackD::dDOCAW_dty] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
1830 | |
1831 | |
1832 | double t0shift = 4.0; |
1833 | double drift_shift = 0.0; |
1834 | if (drift_time < 0.0) { |
1835 | drift_shift = drift; |
1836 | } else { |
1837 | drift_shift = |
1838 | (du > 0.0 ? 1.0 : -1.0) * |
1839 | fdc_drift_distance(drift_time + t0shift); |
1840 | } |
1841 | derivatives[FDCTrackD::dW_dt0] = (drift_shift - drift) / t0shift; |
1842 | |
1843 | |
1844 | |
1845 | derivatives[FDCTrackD::dDOCAC_dDeltaX] = 0.; |
1846 | |
1847 | |
1848 | derivatives[FDCTrackD::dDOCAC_dDeltaZ] = ty*cosa + tx*sina; |
1849 | |
1850 | |
1851 | derivatives[FDCTrackD::dDOCAC_dDeltaPhiX] = 0.; |
1852 | |
1853 | |
1854 | derivatives[FDCTrackD::dDOCAC_dDeltaPhiY] = 0.; |
1855 | |
1856 | |
1857 | derivatives[FDCTrackD::dDOCAC_dDeltaPhiZ] = -(x*cosa) + y*sina; |
1858 | |
1859 | |
1860 | derivatives[FDCTrackD::dDOCAC_dx] = sina; |
1861 | |
1862 | |
1863 | derivatives[FDCTrackD::dDOCAW_dy] = cosa; |
1864 | |
1865 | |
1866 | derivatives[FDCTrackD::dDOCAW_dtx] = 0.; |
1867 | |
1868 | |
1869 | derivatives[FDCTrackD::dDOCAW_dty] = 0.; |
1870 | |
1871 | thisPull.AddTrackDerivatives(derivatives); |
1872 | } |
1873 | pulls.push_back(thisPull); |
1874 | } |
1875 | else if (best_trajectory[m].id>=1000){ |
1876 | unsigned int id=best_trajectory[m].id-1000; |
1877 | A=cdc_updates[id].C*JT*C.Invert(); |
1878 | Ss=cdc_updates[id].S+A*(Ss-S); |
1879 | |
1880 | dC=A*(Cs-C)*A.Transpose(); |
1881 | Cs=cdc_updates[id].C+dC; |
1882 | if (VERBOSE > 10) { |
1883 | jout << " In Smoothing Step Using ID " << id << "/" << cdc_updates.size() << " for ring " << cdchits[id]->wire->ring << endl; |
1884 | jout << " A cdc_updates[id].C Ss Cs " << endl; |
1885 | A.Print(); cdc_updates[id].C.Print(); Ss.Print(); Cs.Print(); |
1886 | } |
1887 | if(!Cs.IsPosDef()) { |
1888 | if (VERBOSE) jout << "Cs is not PosDef!" << endl; |
1889 | return VALUE_OUT_OF_RANGE; |
1890 | } |
1891 | |
1892 | const DCDCWire *wire=cdchits[id]->wire; |
1893 | DVector3 origin=wire->origin; |
1894 | double z0=origin.z(); |
1895 | double vz=wire->udir.z(); |
1896 | DVector3 wdir=(1./vz)*wire->udir; |
1897 | DVector3 wirepos=origin+(best_trajectory[m].z-z0)*wdir; |
1898 | |
1899 | double x=Ss(state_x); |
1900 | double y=Ss(state_y); |
1901 | double tx=Ss(state_tx); |
1902 | double ty=Ss(state_ty); |
1903 | |
1904 | DVector3 pos0(x,y,best_trajectory[m].z); |
1905 | DVector3 tdir(tx,ty,1.); |
1906 | |
1907 | |
1908 | DVector3 diff=pos0-origin; |
1909 | double dx0=diff.x(),dy0=diff.y(); |
1910 | double wdir_dot_diff=diff.Dot(wdir); |
1911 | double tdir_dot_diff=diff.Dot(tdir); |
1912 | double tdir_dot_wdir=tdir.Dot(wdir); |
1913 | double tdir2=tdir.Mag2(); |
1914 | double wdir2=wdir.Mag2(); |
1915 | double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir; |
1916 | double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff; |
1917 | double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff; |
1918 | double scale=1./D; |
1919 | double s=scale*N; |
1920 | double t=scale*N1; |
1921 | diff+=s*tdir-t*wdir; |
1922 | double d=diff.Mag()+d_EPS; |
1923 | double ddrift = cdc_updates[id].ddrift; |
1924 | |
1925 | double resi = ddrift - d; |
1926 | |
1927 | |
1928 | |
1929 | DMatrix1x4 H; DMatrix4x1 H_T; |
1930 | { |
1931 | double one_over_d=1./d; |
1932 | double diffx=diff.x(),diffy=diff.y(),diffz=diff.z(); |
1933 | double wx=wdir.x(),wy=wdir.y(); |
1934 | |
1935 | double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0; |
1936 | double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx; |
1937 | double dtdtx=scale*(dN1dtx-t*dDdtx); |
1938 | |
1939 | double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0; |
1940 | double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy; |
1941 | double dtdty=scale*(dN1dty-t*dDdty); |
1942 | |
1943 | double dNdtx=wx*wdir_dot_diff-wdir2*dx0; |
1944 | double dsdtx=scale*(dNdtx-s*dDdtx); |
1945 | |
1946 | double dNdty=wy*wdir_dot_diff-wdir2*dy0; |
1947 | double dsdty=scale*(dNdty-s*dDdty); |
1948 | |
1949 | H(state_tx)=H_T(state_tx) |
1950 | =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx) |
1951 | +diffz*(dsdtx-dtdtx)); |
1952 | H(state_ty)=H_T(state_ty) |
1953 | =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty) |
1954 | +diffz*(dsdty-dtdty)); |
1955 | |
1956 | double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx); |
1957 | double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx); |
1958 | double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty); |
1959 | double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty); |
1960 | |
1961 | H(state_x)=H_T(state_x) |
1962 | =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy) |
1963 | +diffz*(dsdx-dtdx)); |
1964 | H(state_y)=H_T(state_y) |
1965 | =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy) |
1966 | +diffz*(dsdy-dtdy)); |
1967 | } |
1968 | double V=cdc_updates[id].V; |
1969 | |
1970 | if (VERBOSE > 10) jout << " d " << d << " H*S " << H*S << endl; |
1971 | if (cdchits[id]->wire->ring==RING_TO_SKIP){ |
1972 | V=V+H*Cs*H_T; |
1973 | } |
1974 | else{ |
1975 | V=V-H*Cs*H_T; |
1976 | } |
1977 | if (V<0) return VALUE_OUT_OF_RANGE; |
1978 | |
1979 | |
1980 | double myscale=1./sqrt(1.+tx*tx+ty*ty); |
1981 | double cosThetaRel=wire->udir.Dot(DVector3(myscale*tx,myscale*ty,myscale)); |
1982 | DTrackFitter::pull_t thisPull(resi,sqrt(V), |
1983 | best_trajectory[m].t*SPEED_OF_LIGHT29.9792458, |
1984 | cdc_updates[id].tdrift, |
1985 | d,cdchits[id], NULL__null, |
1986 | diff.Phi(), |
1987 | best_trajectory[m].z,cosThetaRel, |
1988 | cdc_updates[id].tdrift); |
1989 | |
1990 | |
1991 | double wtx=wire->udir.X(), wty=wire->udir.Y(), wtz=wire->udir.Z(); |
1992 | double wx=wire->origin.X(), wy=wire->origin.Y(), wz=wire->origin.Z(); |
1993 | |
1994 | double z=best_trajectory[m].z; |
1995 | double tx2=tx*tx, ty2=ty*ty; |
1996 | double wtx2=wtx*wtx, wty2=wty*wty, wtz2=wtz*wtz; |
1997 | double denom=(1 + ty2)*wtx2 + (1 + tx2)*wty2 - 2*ty*wty*wtz + (tx2 + ty2)*wtz2 - 2*tx*wtx*(ty*wty + wtz)+d_EPS; |
1998 | double denom2=denom*denom; |
1999 | double c1=-(wtx - tx*wtz)*(wy - y); |
2000 | double c2=wty*(wx - tx*wz - x + tx*z); |
2001 | double c3=ty*(-(wtz*wx) + wtx*wz + wtz*x - wtx*z); |
2002 | double dscale=0.5*(1/d); |
2003 | |
2004 | vector<double> derivatives(11); |
2005 | |
2006 | derivatives[CDCTrackD::dDOCAdOriginX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3))/denom; |
2007 | |
2008 | derivatives[CDCTrackD::dDOCAdOriginY]=dscale*(2*(-wtx + tx*wtz)*(c1 + c2 + c3))/denom; |
2009 | |
2010 | derivatives[CDCTrackD::dDOCAdOriginZ]=dscale*(2*(ty*wtx - tx*wty)*(c1 + c2 + c3))/denom; |
2011 | |
2012 | derivatives[CDCTrackD::dDOCAdDirX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3)* |
2013 | (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) + |
2014 | wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2; |
2015 | |
2016 | derivatives[CDCTrackD::dDOCAdDirY]=dscale*(-2*(wtx - tx*wtz)*(c1 + c2 + c3)* |
2017 | (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) + |
2018 | wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2; |
2019 | |
2020 | derivatives[CDCTrackD::dDOCAdDirZ]=dscale*(-2*(ty*wtx - tx*wty)*(c1 + c2 + c3)* |
2021 | (-(tx*(ty*wty + wtz)*(wx - x)) + tx2*(wty*(wy - y) + wtz*(wz - z)) + (wty - ty*wtz)*(wy - y + ty*(-wz + z)) + |
2022 | wtx*((1 + ty2)*wx - (1 + ty2)*x + tx*(-(ty*wy) - wz + ty*y + z))))/denom2; |
2023 | |
2024 | derivatives[CDCTrackD::dDOCAdS0]=-derivatives[CDCTrackD::dDOCAdOriginX]; |
2025 | |
2026 | derivatives[CDCTrackD::dDOCAdS1]=-derivatives[CDCTrackD::dDOCAdOriginY]; |
2027 | |
2028 | derivatives[CDCTrackD::dDOCAdS2]=dscale*(2*(wty - ty*wtz)*(-c1 - c2 - c3)* |
2029 | (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z + |
2030 | tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) + |
2031 | ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2; |
2032 | |
2033 | derivatives[CDCTrackD::dDOCAdS3]=dscale*(2*(wtx - tx*wtz)*(c1 + c2 + c3)* |
2034 | (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z + |
2035 | tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) + |
2036 | ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2; |
2037 | |
2038 | thisPull.AddTrackDerivatives(derivatives); |
2039 | |
2040 | pulls.push_back(thisPull); |
2041 | } |
2042 | else{ |
2043 | A=best_trajectory[m].Ckk*JT*C.Invert(); |
2044 | Ss=best_trajectory[m].Skk+A*(Ss-S); |
2045 | Cs=best_trajectory[m].Ckk+A*(Cs-C)*A.Transpose(); |
2046 | } |
2047 | |
2048 | S=best_trajectory[m].Skk; |
2049 | C=best_trajectory[m].Ckk; |
2050 | JT=best_trajectory[m].J.Transpose(); |
2051 | } |
2052 | |
2053 | return NOERROR; |
2054 | } |
2055 | |
2056 | shared_ptr<TMatrixFSym> |
2057 | DTrackFitterStraightTrack::Get7x7ErrorMatrix(TMatrixFSym C,DMatrix4x1 &S,double sign){ |
2058 | auto C7x7 = dResourcePool_TMatrixFSym->Get_SharedResource(); |
2059 | C7x7->ResizeTo(7, 7); |
2060 | DMatrix J(7,5); |
2061 | |
2062 | double p=5.; |
2063 | double tx_=S(state_tx); |
2064 | double ty_=S(state_ty); |
2065 | double x_=S(state_x); |
2066 | double y_=S(state_y); |
2067 | double tanl=sign/sqrt(tx_*tx_+ty_*ty_); |
2068 | double tanl2=tanl*tanl; |
2069 | double lambda=atan(tanl); |
2070 | double sinl=sin(lambda); |
2071 | double sinl3=sinl*sinl*sinl; |
2072 | |
2073 | J(state_X,state_x)=J(state_Y,state_y)=1.; |
2074 | J(state_Pz,state_ty)=-p*ty_*sinl3; |
2075 | J(state_Pz,state_tx)=-p*tx_*sinl3; |
2076 | J(state_Px,state_ty)=J(state_Py,state_tx)=-p*tx_*ty_*sinl3; |
2077 | J(state_Px,state_tx)=p*(1.+ty_*ty_)*sinl3; |
2078 | J(state_Py,state_ty)=p*(1.+tx_*tx_)*sinl3; |
2079 | J(state_Pz,4)=-p*p*sinl; |
2080 | J(state_Px,4)=tx_*J(state_Pz,4); |
2081 | J(state_Py,4)=ty_*J(state_Pz,4); |
2082 | J(state_Z,state_x)=-tx_*tanl2; |
2083 | J(state_Z,state_y)=-ty_*tanl2; |
2084 | double diff=tx_*tx_-ty_*ty_; |
2085 | double frac=tanl2*tanl2; |
2086 | J(state_Z,state_tx)=(x_*diff+2.*tx_*ty_*y_)*frac; |
2087 | J(state_Z,state_ty)=(2.*tx_*ty_*x_-y_*diff)*frac; |
2088 | |
2089 | |
2090 | *C7x7=C.Similarity(J); |
2091 | |
2092 | return C7x7; |
2093 | } |
2094 | |
2095 | |
2096 | void DTrackFitterStraightTrack::GetExtrapolations(const DVector3 &pos0, |
2097 | const DVector3 &dir){ |
2098 | double x0=pos0.x(),y0=pos0.y(),z0=pos0.z(); |
2099 | double s=0.,t=0.; |
2100 | DVector3 pos(0,0,0); |
2101 | DVector3 diff(0,0,0); |
2102 | ClearExtrapolations(); |
2103 | |
2104 | double z=z0; |
2105 | double dz=0.1; |
2106 | double uz=dir.z(); |
2107 | double ux=dir.x()/uz,ux2=ux*ux; |
2108 | double uy=dir.y()/uz,uy2=uy*uy; |
2109 | |
2110 | |
2111 | double Rd=SC_BARREL_R; |
2112 | double A=ux*x0 + uy*y0; |
2113 | double B=uy2*(Rd - x0)*(Rd + x0) + 2*ux*uy*x0*y0 + ux2*(Rd - y0)*(Rd + y0); |
2114 | double C=ux2 + uy2; |
2115 | if (sc_pos.empty()==false && z<SC_END_NOSE_Z && B>0){ |
2116 | dz=-(A-sqrt(B))/C; |
2117 | pos=pos0+(dz/uz)*dir; |
2118 | z=pos.z(); |
2119 | bool done=(z<sc_pos[0][0].z()||z>SC_END_NOSE_Z); |
2120 | while(!done){ |
2121 | double d_old=1000.,d=1000.; |
2122 | unsigned int index=0; |
2123 | |
2124 | for (unsigned int m=0;m<12;m++){ |
2125 | double dphi=pos.Phi()-SC_PHI_SECTOR1; |
2126 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
2127 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
2128 | if (index>29) index=0; |
2129 | d=sc_norm[index][m].Dot(pos-sc_pos[index][m]); |
2130 | if (d*d_old<0){ |
2131 | pos+=d*dir; |
2132 | s=(pos-pos0).Mag(); |
2133 | t=s/29.98; |
2134 | extrapolations[SYS_START].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2135 | done=true; |
2136 | break; |
2137 | } |
2138 | d_old=d; |
2139 | } |
2140 | pos-=(0.1/uz)*dir; |
2141 | z=pos.z(); |
| Value stored to 'z' is never read |
2142 | } |
2143 | } |
2144 | |
2145 | Rd=65.; |
2146 | B=uy2*(Rd - x0)*(Rd + x0) + 2*ux*uy*x0*y0 + ux2*(Rd - y0)*(Rd + y0); |
2147 | if (B>0){ |
2148 | diff=-(A-sqrt(B))/(C*uz)*dir; |
2149 | s=diff.Mag(); |
2150 | t=s/29.98; |
2151 | pos=pos0+diff; |
2152 | extrapolations[SYS_BCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2153 | Rd=89.; |
2154 | B=uy2*(Rd - x0)*(Rd + x0) + 2*ux*uy*x0*y0 + ux2*(Rd - y0)*(Rd + y0); |
2155 | if (B>0){ |
2156 | diff=-(A-sqrt(B))/(C*uz)*dir; |
2157 | s=diff.Mag(); |
2158 | t=s/29.98; |
2159 | pos=pos0+diff; |
2160 | extrapolations[SYS_BCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2161 | } |
2162 | } |
2163 | |
2164 | |
2165 | for (unsigned int i=0;i<dTRDz_vec.size();i++){ |
2166 | diff=((dTRDz_vec[i]-z0)/uz)*dir; |
2167 | pos=pos0+diff; |
2168 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
2169 | s=diff.Mag(); |
2170 | t=s/29.98; |
2171 | extrapolations[SYS_TRD].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2172 | } |
2173 | } |
2174 | |
2175 | |
2176 | diff=((dDIRCz-z0)/uz)*dir; |
2177 | pos=pos0+diff; |
2178 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
2179 | s=diff.Mag(); |
2180 | t=s/29.98; |
2181 | extrapolations[SYS_DIRC].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2182 | } |
2183 | |
2184 | |
2185 | diff=((dTOFz-z0)/uz)*dir; |
2186 | pos=pos0+diff; |
2187 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
2188 | double s=diff.Mag(); |
2189 | double t=s/29.98; |
2190 | s=diff.Mag(); |
2191 | t=s/29.98; |
2192 | extrapolations[SYS_TOF].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2193 | } |
2194 | |
2195 | |
2196 | diff=((dFCALz-z0)/uz)*dir; |
2197 | pos=pos0+diff; |
2198 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
2199 | s=diff.Mag(); |
2200 | t=s/29.98; |
2201 | extrapolations[SYS_FCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2202 | |
2203 | |
2204 | diff=((dFCALz+45.-z0)/uz)*dir; |
2205 | pos=pos0+diff; |
2206 | s=diff.Mag(); |
2207 | t=s/29.98; |
2208 | extrapolations[SYS_FCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2209 | } |
2210 | } |