1 | #include "DRiemannFit.h" |
2 | #include <TDecompLU.h> |
3 | #include <math.h> |
4 | |
5 | #include <iostream> |
6 | #include <algorithm> |
7 | using std::cerr; |
8 | using std::endl; |
9 | |
10 | #define qBr2p0.003 0.003 // conversion for converting q*B*r to GeV/c |
11 | #define Z_TARGET65.0 65.0 |
12 | #define EPS1.0e-8 1.0e-8 |
13 | #define MIN_TANL2.0 2.0 |
14 | |
15 | |
16 | bool DRiemannFit_hit_cmp(DRiemannHit_t *a,DRiemannHit_t *b){ |
17 | return (a->z>b->z); |
18 | } |
19 | |
20 | |
21 | jerror_t DRiemannFit::AddHit(double r, double phi, double z) |
22 | { |
23 | return AddHitXYZ(r*cos(phi), r*sin(phi), z); |
24 | } |
25 | |
26 | |
27 | |
28 | jerror_t DRiemannFit::AddHitXYZ(double x,double y, double z){ |
29 | DRiemannHit_t *hit = new DRiemannHit_t; |
30 | hit->x = x; |
31 | hit->y = y; |
32 | hit->z = z; |
33 | hit->covx=1.; |
34 | hit->covy=1.; |
35 | hit->covxy=0.; |
36 | |
37 | hits.push_back(hit); |
38 | |
39 | return NOERROR; |
40 | |
41 | } |
42 | |
43 | |
44 | jerror_t DRiemannFit::AddHit(double x,double y, double z,double covx, |
45 | double covy, double covxy){ |
46 | DRiemannHit_t *hit = new DRiemannHit_t; |
47 | hit->x = x; |
48 | hit->y = y; |
49 | hit->z = z; |
50 | hit->covx=covx; |
51 | hit->covy=covy; |
52 | hit->covxy=covxy; |
53 | |
54 | hits.push_back(hit); |
55 | |
56 | return NOERROR; |
57 | } |
58 | |
59 | |
60 | jerror_t DRiemannFit::DoFit(double rc_input){ |
61 | jerror_t error=NOERROR; |
62 | |
63 | if (CovR_!=NULL__null) delete CovR_; |
64 | if (CovRPhi_!=NULL__null) delete CovRPhi_; |
65 | CovR_=NULL__null; |
66 | CovRPhi_=NULL__null; |
67 | |
68 | error=FitCircle(rc_input); |
69 | error=FitLine(); |
70 | q=GetCharge();; |
71 | |
72 | return error; |
73 | } |
74 | |
75 | |
76 | |
77 | jerror_t DRiemannFit::CalcNormal(DMatrix A,double lambda,DMatrix &N){ |
78 | double sum=0; |
79 | |
80 | N(0,0)=1.; |
81 | N(1,0)=N(0,0)*(A(1,0)*A(0,2)-(A(0,0)-lambda)*A(1,2)) |
82 | /(A(0,1)*A(2,1)-(A(1,1)-lambda)*A(0,2)); |
83 | N(2,0)=N(0,0)*(A(2,0)*(A(1,1)-lambda)-A(1,0)*A(2,1)) |
84 | /(A(1,2)*A(2,1)-(A(2,2)-lambda)*(A(1,1)-lambda)); |
85 | |
86 | |
87 | for (int i=0;i<3;i++){ |
88 | sum+=N(i,0)*N(i,0); |
89 | } |
90 | for (int i=0;i<3;i++){ |
91 | N(i,0)/=sqrt(sum); |
92 | } |
93 | |
94 | return NOERROR; |
95 | } |
96 | |
97 | |
98 | jerror_t DRiemannFit::FitCircle(double rc){ |
99 | |
100 | DMatrix CRPhi(hits.size(),hits.size()); |
101 | DMatrix CR(hits.size(),hits.size()); |
102 | |
103 | |
104 | |
105 | |
106 | DMatrix C(hits.size(),hits.size()); |
107 | DMatrix S(hits.size(),hits.size()); |
108 | for (unsigned int i=0;i<hits.size();i++){ |
109 | double rtemp=sqrt(hits[i]->x*hits[i]->x+hits[i]->y*hits[i]->y); |
110 | double stemp=rtemp/4./rc; |
111 | double ctemp=1.-stemp*stemp; |
112 | if (ctemp>0){ |
113 | S(i,i)=stemp; |
114 | C(i,i)=sqrt(ctemp); |
115 | } |
116 | else{ |
117 | S(i,i)=0.; |
118 | C(i,i)=1.; |
119 | } |
120 | } |
121 | |
122 | |
123 | if (CovRPhi_==NULL__null){ |
124 | CovRPhi_ = new DMatrix(hits.size(),hits.size()); |
125 | for (unsigned int i=0;i<hits.size();i++){ |
126 | double Phi=atan2(hits[i]->y,hits[i]->x); |
127 | CovRPhi_->operator()(i,i) |
128 | =(Phi*cos(Phi)-sin(Phi))*(Phi*cos(Phi)-sin(Phi))*hits[i]->covx |
129 | +(Phi*sin(Phi)+cos(Phi))*(Phi*sin(Phi)+cos(Phi))*hits[i]->covy |
130 | +2.*(Phi*sin(Phi)+cos(Phi))*(Phi*cos(Phi)-sin(Phi))*hits[i]->covxy; |
131 | } |
132 | } |
133 | if (CovR_==NULL__null){ |
134 | CovR_= new DMatrix(hits.size(),hits.size()); |
135 | for (unsigned int m=0;m<hits.size();m++){ |
136 | double Phi=atan2(hits[m]->y,hits[m]->x); |
137 | CovR_->operator()(m,m)=cos(Phi)*cos(Phi)*hits[m]->covx |
138 | +sin(Phi)*sin(Phi)*hits[m]->covy |
139 | +2.*sin(Phi)*cos(Phi)*hits[m]->covxy; |
140 | } |
141 | } |
142 | for (unsigned int i=0;i<hits.size();i++){ |
143 | for (unsigned int j=0;j<hits.size();j++){ |
144 | CR(i,j)=CovR_->operator()(i, j); |
145 | CRPhi(i,j)=CovRPhi_->operator()(i, j); |
146 | } |
147 | } |
148 | |
149 | |
150 | CRPhi=C*CRPhi*C+S*CR*S; |
151 | for (unsigned int i=0;i<hits.size();i++) |
152 | for (unsigned int j=0;j<hits.size();j++) |
153 | CovRPhi_->operator()(i,j)=CRPhi(i,j); |
154 | return FitCircle(); |
155 | } |
156 | |
157 | |
158 | |
159 | |
160 | |
161 | |
162 | |
163 | |
164 | jerror_t DRiemannFit::FitCircle(){ |
165 | if (hits.size()==0) return RESOURCE_UNAVAILABLE; |
166 | DMatrix X(hits.size(),3); |
167 | DMatrix Xavg(1,3); |
168 | DMatrix A(3,3); |
169 | double B0,B1,B2,Q,Q1,R,sum,diff; |
170 | double theta,lambda_min=0.; |
171 | |
172 | DMatrix Ones(hits.size(),1),OnesT(1,hits.size()); |
173 | DMatrix W_sum(1,1); |
174 | DMatrix W(hits.size(),hits.size()); |
175 | |
176 | DMatrix N1(3,1); |
177 | |
178 | |
179 | std::sort(hits.begin(),hits.end(),DRiemannFit_hit_cmp); |
180 | |
181 | |
182 | DMatrix CRPhi(hits.size(),hits.size()); |
183 | if (CovRPhi_==NULL__null){ |
184 | CovRPhi_ = new DMatrix(hits.size(),hits.size()); |
185 | for (unsigned int i=0;i<hits.size();i++){ |
186 | double Phi=atan2(hits[i]->y,hits[i]->x); |
187 | CovRPhi_->operator()(i,i) |
188 | =(Phi*cos(Phi)-sin(Phi))*(Phi*cos(Phi)-sin(Phi))*hits[i]->covx |
189 | +(Phi*sin(Phi)+cos(Phi))*(Phi*sin(Phi)+cos(Phi))*hits[i]->covy |
190 | +2.*(Phi*sin(Phi)+cos(Phi))*(Phi*cos(Phi)-sin(Phi))*hits[i]->covxy; |
191 | } |
192 | } |
193 | for (unsigned int i=0;i<hits.size();i++) |
194 | for (unsigned int j=0;j<hits.size();j++) |
195 | CRPhi(i,j)=CovRPhi_->operator()(i, j); |
196 | |
197 | |
198 | |
199 | |
200 | |
201 | |
202 | |
203 | |
204 | |
205 | |
206 | for (unsigned int i=0;i<hits.size();i++){ |
207 | X(i,0)=hits[i]->x; |
208 | X(i,1)=hits[i]->y; |
209 | X(i,2)=hits[i]->x*hits[i]->x+hits[i]->y*hits[i]->y; |
210 | Ones(i,0)=OnesT(0,i)=1.; |
211 | } |
212 | |
213 | |
214 | TDecompLU lu(CRPhi); |
215 | if (lu.Decompose()==false){ |
216 | return UNRECOVERABLE_ERROR; |
217 | } |
218 | W=DMatrix(DMatrix::kInverted,CRPhi); |
219 | W_sum=OnesT*(W*Ones); |
220 | Xavg=(1./W_sum(0,0))*(OnesT*(W*X)); |
221 | |
222 | A=DMatrix(DMatrix::kTransposed,X)*(W*X) |
223 | -W_sum(0,0)*(DMatrix(DMatrix::kTransposed,Xavg)*Xavg); |
224 | if(!A.IsValid())return UNRECOVERABLE_ERROR; |
225 | |
226 | |
227 | |
228 | |
229 | B2=-(A(0,0)+A(1,1)+A(2,2)); |
230 | B1=A(0,0)*A(1,1)-A(1,0)*A(0,1)+A(0,0)*A(2,2)-A(2,0)*A(0,2)+A(1,1)*A(2,2) |
231 | -A(2,1)*A(1,2); |
232 | B0=-A.Determinant(); |
233 | if(B0==0 || !finite(B0))return UNRECOVERABLE_ERROR; |
234 | |
235 | |
236 | |
237 | |
238 | |
239 | |
240 | |
241 | |
242 | |
243 | |
244 | |
245 | |
246 | |
247 | |
248 | |
249 | Q=(3.*B1-B2*B2)/9.e4; |
250 | R=(9.*B2*B1-27.*B0-2.*B2*B2*B2)/54.e6; |
251 | Q1=Q*Q*Q+R*R; |
252 | if (Q1<0) Q1=sqrt(-Q1); |
253 | else{ |
254 | return VALUE_OUT_OF_RANGE; |
255 | } |
256 | |
257 | |
258 | |
259 | |
260 | |
261 | double temp=100.*pow(R*R+Q1*Q1,0.16666666666666666667); |
262 | theta=atan2(Q1,R)/3.; |
263 | sum=2.*temp*cos(theta); |
264 | diff=-2.*temp*sin(theta); |
265 | |
266 | lambda_min=-B2/3.-sum/2.+sqrt(3.)/2.*diff; |
267 | |
268 | |
269 | CalcNormal(A,lambda_min,N1); |
270 | |
271 | N[0]=N1(0,0); |
272 | N[1]=N1(1,0); |
273 | N[2]=N1(2,0); |
274 | |
275 | |
276 | dist_to_origin=-(N1(0,0)*Xavg(0,0)+N1(1,0)*Xavg(0,1)+N1(2,0)*Xavg(0,2)); |
277 | |
278 | |
279 | xc=-N1(0,0)/2./N1(2,0); |
280 | yc=-N1(1,0)/2./N1(2,0); |
281 | rc=sqrt(1.-N1(2,0)*N1(2,0)-4.*dist_to_origin*N1(2,0))/2./fabs(N1(2,0)); |
282 | |
283 | return NOERROR; |
284 | } |
285 | |
286 | |
287 | double DRiemannFit::GetCharge(double rc_input){ |
288 | |
289 | DMatrix CRPhi(hits.size(),hits.size()); |
290 | DMatrix CR(hits.size(),hits.size()); |
291 | |
292 | |
293 | |
294 | |
295 | DMatrix C(hits.size(),hits.size()); |
296 | DMatrix S(hits.size(),hits.size()); |
297 | for (unsigned int i=0;i<hits.size();i++){ |
298 | double rtemp=sqrt(hits[i]->x*hits[i]->x+hits[i]->y*hits[i]->y); |
299 | double stemp=rtemp/4./rc_input; |
300 | double ctemp=1.-stemp*stemp; |
301 | if (ctemp>0){ |
302 | S(i,i)=stemp; |
303 | C(i,i)=sqrt(ctemp); |
304 | } |
305 | else{ |
306 | S(i,i)=0.; |
307 | C(i,i)=1; |
308 | } |
309 | } |
310 | |
311 | |
312 | if (CovRPhi_==NULL__null){ |
313 | CovRPhi_ = new DMatrix(hits.size(),hits.size()); |
314 | for (unsigned int i=0;i<hits.size();i++){ |
315 | double Phi=atan2(hits[i]->y,hits[i]->x); |
316 | CovRPhi_->operator()(i,i) |
317 | =(Phi*cos(Phi)-sin(Phi))*(Phi*cos(Phi)-sin(Phi))*hits[i]->covx |
318 | +(Phi*sin(Phi)+cos(Phi))*(Phi*sin(Phi)+cos(Phi))*hits[i]->covy |
319 | +2.*(Phi*sin(Phi)+cos(Phi))*(Phi*cos(Phi)-sin(Phi))*hits[i]->covxy; |
320 | } |
321 | } |
322 | if (CovR_==NULL__null){ |
323 | CovR_= new DMatrix(hits.size(),hits.size()); |
324 | for (unsigned int m=0;m<hits.size();m++){ |
325 | double Phi=atan2(hits[m]->y,hits[m]->x); |
326 | CovR_->operator()(m,m)=cos(Phi)*cos(Phi)*hits[m]->covx |
327 | +sin(Phi)*sin(Phi)*hits[m]->covy |
328 | +2.*sin(Phi)*cos(Phi)*hits[m]->covxy; |
329 | } |
330 | } |
331 | for (unsigned int i=0;i<hits.size();i++){ |
332 | for (unsigned int j=0;j<hits.size();j++){ |
333 | CR(i,j)=CovR_->operator()(i, j); |
334 | CRPhi(i,j)=CovRPhi_->operator()(i, j); |
335 | } |
336 | } |
337 | |
338 | CRPhi=C*CRPhi*C+S*CR*S; |
339 | for (unsigned int i=0;i<hits.size();i++) |
340 | for (unsigned int j=0;j<hits.size();j++) |
341 | CovRPhi_->operator()(i,j)=CRPhi(i,j); |
342 | return GetCharge(); |
343 | } |
344 | |
345 | |
346 | |
347 | double DRiemannFit::GetCharge(){ |
348 | |
349 | DMatrix CRPhi(hits.size(),hits.size()); |
350 | DMatrix CR(hits.size(),hits.size()); |
351 | if (CovRPhi_==NULL__null){ |
352 | CovRPhi_ = new DMatrix(hits.size(),hits.size()); |
353 | for (unsigned int i=0;i<hits.size();i++){ |
354 | double Phi=atan2(hits[i]->y,hits[i]->x); |
355 | CovRPhi_->operator()(i,i) |
356 | =(Phi*cos(Phi)-sin(Phi))*(Phi*cos(Phi)-sin(Phi))*hits[i]->covx |
357 | +(Phi*sin(Phi)+cos(Phi))*(Phi*sin(Phi)+cos(Phi))*hits[i]->covy |
358 | +2.*(Phi*sin(Phi)+cos(Phi))*(Phi*cos(Phi)-sin(Phi))*hits[i]->covxy; |
359 | } |
360 | } |
361 | if (CovR_==NULL__null){ |
362 | CovR_= new DMatrix(hits.size(),hits.size()); |
363 | for (unsigned int m=0;m<hits.size();m++){ |
364 | double Phi=atan2(hits[m]->y,hits[m]->x); |
365 | CovR_->operator()(m,m)=cos(Phi)*cos(Phi)*hits[m]->covx |
366 | +sin(Phi)*sin(Phi)*hits[m]->covy |
367 | +2.*sin(Phi)*cos(Phi)*hits[m]->covxy; |
368 | } |
369 | } |
370 | for (unsigned int i=0;i<hits.size();i++){ |
371 | for (unsigned int j=0;j<hits.size();j++){ |
372 | CR(i,j)=CovR_->operator()(i, j); |
373 | CRPhi(i,j)=CovRPhi_->operator()(i, j); |
374 | } |
375 | } |
376 | |
377 | double phi_old=atan2(hits[0]->y,hits[0]->x); |
378 | double sumv=0,sumy=0,sumx=0,sumxx=0,sumxy=0; |
379 | for (unsigned int k=0;k<hits.size();k++){ |
380 | DRiemannHit_t *hit=hits[k]; |
381 | double phi_z=atan2(hit->y,hit->x); |
382 | |
383 | |
384 | if (fabs(phi_z-phi_old)>M_PI3.14159265358979323846){ |
385 | if (phi_old<0) phi_z-=2.*M_PI3.14159265358979323846; |
386 | else phi_z+=2.*M_PI3.14159265358979323846; |
387 | } |
388 | double inv_var=(hit->x*hit->x+hit->y*hit->y) |
389 | /(CRPhi(k,k)+phi_z*phi_z*CR(k,k)); |
390 | sumv+=inv_var; |
391 | sumy+=phi_z*inv_var; |
392 | sumx+=hit->z*inv_var; |
393 | sumxx+=hit->z*hit->z*inv_var; |
394 | sumxy+=phi_z*hit->z*inv_var; |
395 | phi_old=phi_z; |
396 | } |
397 | double slope=(sumv*sumxy-sumy*sumx)/(sumv*sumxx-sumx*sumx); |
398 | |
399 | |
400 | if (slope<0.) return -1.; |
401 | |
402 | return 1.; |
403 | } |
404 | |
405 | |
406 | |
407 | |
408 | |
409 | |
410 | jerror_t DRiemannFit::FitLine(){ |
411 | |
412 | DMatrix CR(hits.size(),hits.size()); |
413 | if (CovR_==NULL__null){ |
| |
414 | CovR_= new DMatrix(hits.size(),hits.size()); |
415 | for (unsigned int m=0;m<hits.size();m++){ |
416 | double Phi=atan2(hits[m]->y,hits[m]->x); |
417 | CovR_->operator()(m,m)=cos(Phi)*cos(Phi)*hits[m]->covx |
418 | +sin(Phi)*sin(Phi)*hits[m]->covy |
419 | +2.*sin(Phi)*cos(Phi)*hits[m]->covxy; |
420 | } |
421 | } |
422 | for (unsigned int i=0;i<hits.size();i++) |
| 2 | | Loop condition is false. Execution continues on line 427 | |
|
423 | for (unsigned int j=0;j<hits.size();j++) |
424 | CR(i,j)=CovR_->operator()(i, j); |
425 | |
426 | |
427 | double x_int0,temp,y_int0; |
428 | double denom= N[0]*N[0]+N[1]*N[1]; |
429 | double numer; |
430 | vector<int>bad(hits.size()); |
431 | int numbad=0; |
432 | |
433 | projections.clear(); |
434 | for (unsigned int m=0;m<hits.size();m++){ |
| 3 | | Loop condition is true. Entering loop body | |
|
| 7 | | Loop condition is false. Execution continues on line 478 | |
|
435 | double r2=hits[m]->x*hits[m]->x+hits[m]->y*hits[m]->y; |
436 | numer=dist_to_origin+r2*N[2]; |
437 | DRiemannHit_t *temphit = new DRiemannHit_t; |
438 | temphit->z=hits[m]->z; |
439 | |
440 | if (r2==0){ |
| |
441 | temphit->x=0.; |
442 | temphit->y=0.; |
443 | |
444 | } |
445 | else{ |
446 | x_int0=-N[0]*numer/denom; |
447 | y_int0=-N[1]*numer/denom; |
448 | temp=denom*r2-numer*numer; |
449 | if (temp<0){ |
| |
450 | bad[m]=1; |
451 | numbad++; |
452 | temphit->x=x_int0; |
453 | temphit->y=y_int0; |
454 | projections.push_back(temphit); |
455 | continue; |
456 | } |
457 | temp=sqrt(temp)/denom; |
458 | |
459 | |
460 | double diffx1=x_int0+N[1]*temp-hits[m]->x; |
461 | double diffy1=y_int0-N[0]*temp-hits[m]->y; |
462 | double diffx2=x_int0-N[1]*temp-hits[m]->x; |
463 | double diffy2=y_int0+N[0]*temp-hits[m]->y; |
464 | if (diffx1*diffx1+diffy1*diffy1 > diffx2*diffx2+diffy2*diffy2){ |
| |
465 | temphit->x=x_int0-N[1]*temp; |
466 | temphit->y=y_int0+N[0]*temp; |
467 | } |
468 | else{ |
469 | temphit->x=x_int0+N[1]*temp; |
470 | temphit->y=y_int0-N[0]*temp; |
471 | } |
472 | } |
473 | projections.push_back(temphit); |
474 | } |
475 | |
476 | |
477 | |
478 | unsigned int start=0; |
479 | for (unsigned int i=0;i<bad.size();i++){ |
| 8 | | Loop condition is false. Execution continues on line 487 | |
|
480 | if (!bad[i]){ |
481 | start=i; |
482 | break; |
483 | } |
484 | } |
485 | |
486 | |
487 | unsigned int n=projections.size(); |
488 | double sumv=0.,sumx=0.,sumy=0.,sumxx=0.,sumxy=0.; |
489 | double sperp=0., chord=0,ratio=0, Delta; |
490 | for (unsigned int k=start;k<n;k++){ |
| 9 | | Loop condition is false. Execution continues on line 510 | |
|
491 | if (!bad[k]) |
492 | { |
493 | double diffx=projections[k]->x-projections[start]->x; |
494 | double diffy=projections[k]->y-projections[start]->y; |
495 | chord=sqrt(diffx*diffx+diffy*diffy); |
496 | ratio=chord/2./rc; |
497 | |
498 | if (ratio>1.) |
499 | sperp=2.*rc*(M_PI3.14159265358979323846/2.); |
500 | else |
501 | sperp=2.*rc*asin(ratio); |
502 | |
503 | sumv+=1./CR(k,k); |
504 | sumy+=sperp/CR(k,k); |
505 | sumx+=projections[k]->z/CR(k,k); |
506 | sumxx+=projections[k]->z*projections[k]->z/CR(k,k); |
507 | sumxy+=sperp*projections[k]->z/CR(k,k); |
508 | } |
509 | } |
510 | chord=sqrt(projections[start]->x*projections[start]->x |
| 10 | | Dereference of null pointer |
|
511 | +projections[start]->y*projections[start]->y); |
512 | ratio=chord/2./rc; |
513 | |
514 | if (ratio>1.) |
515 | sperp=2.*rc*(M_PI3.14159265358979323846/2.); |
516 | else |
517 | sperp=2.*rc*asin(ratio); |
518 | Delta=sumv*sumxx-sumx*sumx; |
519 | |
520 | tanl=-Delta/(sumv*sumxy-sumy*sumx); |
521 | |
522 | |
523 | zvertex=projections[start]->z-sperp*tanl; |
524 | double zvertex_temp=projections[start]->z-(2.*rc*M_PI3.14159265358979323846-sperp)*tanl; |
525 | |
526 | |
527 | if (fabs(zvertex-Z_TARGET65.0)>fabs(zvertex_temp-Z_TARGET65.0)) zvertex=zvertex_temp; |
528 | |
529 | return NOERROR; |
530 | } |
531 | |