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