11 #include <TDecompLU.h>
15 #define qBr2p 0.003 // conversion for converting q*B*r to GeV/c
17 #define ONE_THIRD 0.33333333333333333
18 #define SQRT3 1.73205080756887719
21 #define M_TWO_PI 6.28318530717958647692
41 if(fabs(a.
z - b.
z) > 0.0)
43 return (a.
xy.Mod2() > b.
xy.Mod2());
52 x0 = 0., y0 = 0., r0=0., tanl=0., z_vertex = 0., phi=0., theta = 0.,dzdphi=0.;
62 normal.SetXYZ(0.,0.,0.);
88 normal.SetXYZ(0.,0.,0.);
127 const vector<DHFHit_t*> myhits = fit.
GetHits();
128 for(
unsigned int i=0; i<myhits.size(); i++){
140 if(
this == &fit)
return *
this;
158 hit->
x = fdchit->
xy.X();
159 hit->
y = fdchit->
xy.Y();
167 double Phi=atan2(hit->
y,hit->
x);
172 double temp1=u*Phi-v;
173 double temp2=v*Phi+
u;
174 double var_u=0.08333;
176 double one_over_R2=1./fdchit->
xy.Mod2();
177 hit->
covrphi=one_over_R2*(var_v*temp1*temp1+var_u*temp2*temp2);
178 hit->
covr=one_over_R2*(var_u*u*u+var_v*v*v);
194 return AddHitXYZ(r*cos(phi), r*
sin(phi), z);
213 double Phi=atan2(hit->
y,hit->
x);
214 double cosPhi=cos(Phi);
215 double sinPhi=
sin(Phi);
216 double Phi_cosPhi_minus_sinPhi=Phi*cosPhi-sinPhi;
217 double Phi_sinPhi_plus_cosPhi=Phi*sinPhi+cosPhi;
218 hit->
covrphi=Phi_cosPhi_minus_sinPhi*Phi_cosPhi_minus_sinPhi*hit->
covx
219 +Phi_sinPhi_plus_cosPhi*Phi_sinPhi_plus_cosPhi*hit->
covy
220 +2.*Phi_sinPhi_plus_cosPhi*Phi_cosPhi_minus_sinPhi*hit->
covxy;
221 hit->
covr=cosPhi*cosPhi*hit->
covx+sinPhi*sinPhi*hit->
covy
222 +2.*sinPhi*cosPhi*hit->
covxy;
231 float covy,
float covxy,
bool is_axial){
241 double Phi=atan2(hit->
y,hit->
x);
242 double cosPhi=cos(Phi);
243 double sinPhi=
sin(Phi);
244 double Phi_cosPhi_minus_sinPhi=Phi*cosPhi-sinPhi;
245 double Phi_sinPhi_plus_cosPhi=Phi*sinPhi+cosPhi;
246 hit->
covrphi=Phi_cosPhi_minus_sinPhi*Phi_cosPhi_minus_sinPhi*hit->
covx
247 +Phi_sinPhi_plus_cosPhi*Phi_sinPhi_plus_cosPhi*hit->
covy
248 +2.*Phi_sinPhi_plus_cosPhi*Phi_cosPhi_minus_sinPhi*hit->
covxy;
249 hit->
covr=cosPhi*cosPhi*hit->
covx+sinPhi*sinPhi*hit->
covy
250 +2.*sinPhi*cosPhi*hit->
covxy;
261 double dx=origin.x()-x0;
262 double dy=origin.y()-y0;
265 double temp1=ux*ux+uy*uy;
266 double temp2=ux*dy-uy*dx;
267 double b=-ux*dx-uy*dy;
269 double A=r0_sq*temp1-temp2*temp2;
272 if(A<0.0)
return VALUE_OUT_OF_RANGE;
276 double dz1 = (b-B)/temp1;
277 double dz2 = (b+B)/temp1;
283 if (fabs(dz2)<fabs(dz1)){
287 double s=dz/cos(wire->
stereo);
291 if(fabs(s) > 0.5*wire->
L)
return VALUE_OUT_OF_RANGE;
298 double var_r=0.01*r0_sq+0.213;
310 double Phi=atan2(hit->
y,hit->
x);
311 double cosPhi=cos(Phi);
312 double sinPhi=
sin(Phi);
313 double Phi_cosPhi_minus_sinPhi=Phi*cosPhi-sinPhi;
314 double Phi_sinPhi_plus_cosPhi=Phi*sinPhi+cosPhi;
315 hit->
covrphi=Phi_cosPhi_minus_sinPhi*Phi_cosPhi_minus_sinPhi*hit->
covx
316 +Phi_sinPhi_plus_cosPhi*Phi_sinPhi_plus_cosPhi*hit->
covy;
335 if(idx<0 || idx>=(
int)hits.size())
return VALUE_OUT_OF_RANGE;
338 hits.erase(hits.begin() + idx);
349 for(
unsigned int i=0; i<hits.size(); i++)
delete hits[i];
365 cout<<
"Chisq vector from DHelicalFit: (source=";
366 switch(chisq_source){
367 case NOFIT: cout<<
"NOFIT";
break;
368 case CIRCLE: cout<<
"CIRCLE";
break;
369 case TRACK: cout<<
"TRACK";
break;
372 cout<<
"----------------------------"<<endl;
374 for(
unsigned int i=0;i<hits.size();i++){
375 cout<<i<<
" "<<hits[i]->chisq<<endl;
377 cout<<
"Total: "<<chisq<<endl<<endl;
406 float alpha=0.0, beta=0.0, gamma=0.0, deltax=0.0, deltay=0.0;
407 chisq_source = NOFIT;
408 size_t num_hits=hits.size();
414 for(
unsigned int i=0;i<num_hits;i++){
420 float x_sq_plus_y_sq=x_sq+y_sq;
424 deltax += 0.5*x*x_sq_plus_y_sq;
425 deltay += 0.5*y*x_sq_plus_y_sq;
429 double denom = alpha*beta-gamma*gamma;
430 if(fabs(denom)<1.0E-20)
return UNRECOVERABLE_ERROR;
431 x0 = (deltax*beta-deltay*gamma)/denom;
432 y0 = (deltay*alpha-deltax*gamma)/denom;
433 r0 =
sqrt(x0*x0 + y0*y0);
435 phi = atan2(y0,x0) - M_PI_2;
441 chisq_source = CIRCLE;
456 for(
unsigned int i=0;i<hits.size();i++){
462 float c =
sqrt(r2) - r0;
480 chisq_source = NOFIT;
483 float beam_var=BeamRMS*
BeamRMS;
484 AddHitXYZ(0.,0.,z_vertex,beam_var,beam_var,0.);
486 jerror_t err = FitCircleRiemann();
487 if(err!=NOERROR)
return err;
497 normal.SetXYZ(N[0],N[1],N[2]);
511 size_t num_hits=hits.size();
524 DMatrix CRPhi(num_hits,num_hits);
525 for (
unsigned int i=0;i<num_hits;i++){
526 CRPhi(i,i)=hits[i]->covrphi;
535 for (
unsigned int i=0;i<num_hits;i++){
538 CR(i,i)=hits[i]->covr;
540 double rtemp=
sqrt(hits[i]->
x*hits[i]->
x+hits[i]->
y*hits[i]->
y);
541 double stemp=rtemp/(4.*rc);
542 double ctemp=1.-stemp*stemp;
549 CRPhi=C*CRPhi*C+S*CR*
S;
561 for (
unsigned int i=0;i<num_hits;i++){
564 X(i,2)=hits[i]->x*hits[i]->x+hits[i]->y*hits[i]->y;
566 W(i,i)=1./CRPhi(i,i);
569 Xavg=(1./W_sum)*(OnesT*(W*X));
571 A=
DMatrix(DMatrix::kTransposed,X)*(W*
X)
572 -W_sum*(
DMatrix(DMatrix::kTransposed,Xavg)*Xavg);
573 if(!A.IsValid())
return UNRECOVERABLE_ERROR;
578 double B2=-(A(0,0)+A(1,1)+A(2,2));
579 double 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)
580 +A(1,1)*A(2,2)-A(2,1)*A(1,2);
581 double B0=-A.Determinant();
582 if(B0==0 || !isfinite(B0))
return UNRECOVERABLE_ERROR;
598 long double Q=(3.*B1-B2*B2)/9.e6;
599 long double R=(9.*B2*B1-27.*B0-2.*B2*B2*B2)/54.e9;
600 long double Q1=Q*Q*Q+R*R;
601 if (Q1<0) Q1=sqrtl(-Q1);
603 return VALUE_OUT_OF_RANGE;
611 long double temp=1000.*sqrtl(cbrtl(R*R+Q1*Q1));
612 long double theta1=
ONE_THIRD*atan2l(Q1,R);
613 long double sum_over_2=temp*cosl(theta1);
614 long double diff_over_2=-temp*sinl(theta1);
616 long double lambda_min=-
ONE_THIRD*B2-sum_over_2+
SQRT3*diff_over_2;
619 double A11_minus_lambda_min=A(1,1)-lambda_min;
621 N[1]=(A(1,0)*A(0,2)-(A(0,0)-lambda_min)*A(1,2))
622 /(A(0,1)*A(2,1)-A11_minus_lambda_min*A(0,2));
623 N[2]=(A(2,0)*A11_minus_lambda_min-A(1,0)*A(2,1))
624 /(A(1,2)*A(2,1)-(A(2,2)-lambda_min)*A11_minus_lambda_min);
627 double denom=
sqrt(N[0]*N[0]+N[1]*N[1]+N[2]*N[2]);
628 for (
int i=0;i<3;i++){
633 c_origin=-(N[0]*Xavg(0,0)+N[1]*Xavg(0,1)+N[2]*Xavg(0,2));
636 double one_over_2Nz=1./(2.*N[2]);
640 x0=-N[0]*one_over_2Nz;
641 y0=-N[1]*one_over_2Nz;
642 r0=
sqrt(1.-N[2]*N[2]-4.*c_origin*N[2])*fabs(one_over_2Nz);
651 chisq_source = CIRCLE;
667 size_t num_hits=hits.size();
670 double denom= N[0]*N[0]+N[1]*N[1];
672 vector<DHFProjection_t>projections;
673 for (
unsigned int m=0;m<num_hits;m++){
674 if (hits[m]->is_axial)
continue;
676 double r2=hits[m]->x*hits[m]->x+hits[m]->y*hits[m]->y;
677 double numer=c_origin+r2*N[2];
678 double ratio=numer/denom;
679 double x_int0=-N[0]*ratio;
680 double y_int0=-N[1]*ratio;
681 double temp=denom*r2-numer*numer;
686 temp=
sqrt(temp)/denom;
690 temp_proj.
z=hits[m]->z;
691 temp_proj.
covR=hits[m]->covr;
694 double deltax=N[1]*
temp;
695 double deltay=-N[0]*
temp;
696 double x1=x_int0+deltax;
697 double y1=y_int0+deltay;
698 double x2=x_int0-deltax;
699 double y2=y_int0-deltay;
700 double diffx1=x1-hits[m]->x;
701 double diffy1=y1-hits[m]->y;
702 double diffx2=x2-hits[m]->x;
703 double diffy2=y2-hits[m]->y;
704 if (diffx1*diffx1+diffy1*diffy1 > diffx2*diffx2+diffy2*diffy2){
710 projections.push_back(temp_proj);
712 if (projections.size()==0)
return RESOURCE_UNAVAILABLE;
716 unsigned int n=projections.size();
717 double sumv=0.,sumx=0.,sumy=0.,sumxx=0.,sumxy=0.;
718 double sperp=0.,sperp_old=0., ratio=0, Delta;
720 DVector2 old_proj=projections[0].xy;
722 for (
unsigned int k=0;k<n;k++){
724 double chord=(projections[k].xy-old_proj).Mod();
728 double ds=(ratio>1)? two_r0*M_PI_2 : two_r0*asin(ratio);
732 double weight=1./projections[k].covR;
737 sumxy+=sperp*z*weight;
740 old_proj=projections[k].xy;
742 Delta=sumv*sumxx-sumx*sumx;
743 double tanl_denom=sumv*sumxy-sumy*sumx;
744 if (fabs(Delta)<
EPS || fabs(tanl_denom)<
EPS)
return VALUE_OUT_OF_RANGE;
747 tanl=Delta/tanl_denom;
749 double chord=projections[0].xy.Mod();
751 sperp=(ratio>1? two_r0*M_PI_2 : two_r0*asin(ratio));
752 z_vertex=projections[0].z-sperp*tanl;
754 theta=M_PI_2-atan(tanl);
782 size_t num_hits=hits.size();
783 for(
unsigned int i=0;i<num_hits;i++){
785 double r =
sqrt(a->
x*a->
x+a->
y*a->
y);
797 SearchPtrans(9.0, 0.5);
803 float Sxx=0.0, Syy=0.0, Sxy=0.0;
804 chisq_source = NOFIT;
808 for(
unsigned int i=0;i<num_hits;i++){
817 double B = Sxx - Syy;
818 phi = B>A ? atan2(A,B)/2.0 : 1.0/atan2(B,A)/2.0;
844 double min_chisq=1.0E6;
845 double min_x0=0.0, min_y0=0.0, min_r0=0.0;
846 for(
double my_p_trans=ptrans_step; my_p_trans<=ptrans_max; my_p_trans+=ptrans_step){
848 r0 = my_p_trans/(0.003*2.0);
849 double alpha, my_chisq;
852 alpha = phi + M_PI_2;
855 my_chisq = ChisqCircle();
856 if(my_chisq<min_chisq){
865 alpha = phi - M_PI_2;
868 my_chisq = ChisqCircle();
869 if(my_chisq<min_chisq){
904 for(
unsigned int i=0; i<hits.size(); i++){
906 double x = hits[i]->x;
907 double y = hits[i]->y;
908 if((x*y0 - y*x0) < 0.0)N++;
949 Fill_phi_circle(hits, x0, y0);
952 float Sxx=0.0, Syy=0.0, Sxy=0.0;
953 for(
unsigned int i=0;i<hits.size();i++){
955 float deltaZ = a->
z - z_mean;
957 Syy += deltaZ*deltaZ;
958 Sxx += deltaPhi*deltaPhi;
959 Sxy += deltaZ*deltaPhi;
962 float dzdphi = Syy/Sxy;
963 z_vertex = z_mean - phi_mean*dzdphi;
967 return FillTrackParams();
974 jerror_t error=FitCircleRiemann(rc_input);
975 if (error!=NOERROR)
return error;
976 error=FitLineRiemann();
977 FindSenseOfRotation();
984 jerror_t error=FitCircleRiemann(rc_input);
985 if (error!=NOERROR)
return error;
986 error=FitLineRiemann();
1008 return FitLine_FixedZvertex(z_vertex);
1042 this->z_vertex = z_vertex;
1045 Fill_phi_circle(hits, x0, y0);
1049 float Sxx=0, Syy=0, Sxy = 0;
1050 float r0 =
sqrt(x0*x0 + y0*y0);
1051 for(
unsigned int i=0; i<hits.size(); i++){
1054 float dz = a->
z - z_vertex;
1058 Syy += r0*dphi*r0*dphi;
1063 float k = (Syy-Sxx)/(2.0*Sxy);
1064 float s =
sqrt(k*k + 1.0);
1065 float cot_theta1 = -k+s;
1066 float cot_theta2 = -k-s;
1067 float cot_theta_lin = Sx/Sy;
1069 if(fabs(cot_theta_lin-cot_theta1) < fabs(cot_theta_lin-cot_theta2)){
1070 cot_theta = cot_theta1;
1072 cot_theta = cot_theta2;
1075 dzdphi = r0*cot_theta;
1078 return FillTrackParams();
1088 float phi_last = 0.0;
1089 z_mean = phi_mean = 0.0;
1090 for(
unsigned int i=0; i<hits.size(); i++){
1093 float dx = a->
x - x0;
1094 float dy = a->
y - y0;
1095 float dphi =
atan2f(dx*y_last - dy*x_last, dx*x_last + dy*y_last);
1096 float my_phi = phi_last +dphi;
1106 z_mean /= (float)hits.size();
1107 phi_mean /= (float)hits.size();
1121 float r0 =
sqrt(x0*x0 + y0*y0);
1122 theta = atan(r0/fabs(dzdphi));
1135 chisq_source = TRACK;
1142 if(z_mean<z_vertex){
1144 theta = M_PI - theta;
1150 tanl=tan(M_PI_2-theta);
1160 cout<<
"-- DHelicalFit Params ---------------"<<endl;
1161 cout<<
" x0 = "<<x0<<endl;
1162 cout<<
" y0 = "<<y0<<endl;
1163 cout<<
" h = "<<
h<<endl;
1164 cout<<
" phi = "<<phi<<endl;
1165 cout<<
" theta = "<<theta<<endl;
1166 cout<<
" z_vertex = "<<z_vertex<<endl;
1167 cout<<
" chisq = "<<chisq<<endl;
1168 cout<<
"chisq_source = ";
1169 switch(chisq_source){
1170 case NOFIT: cout<<
"NOFIT";
break;
1171 case CIRCLE: cout<<
"CIRCLE";
break;
1172 case TRACK: cout<<
"TRACK";
break;
1187 for(
unsigned int i=0;i<hits.size();i++){
1189 cout<<
" x="<<v->
x<<
" y="<<v->
y<<
" z="<<v->
z;
1201 for (;i<hits.size()-1;i++){
1202 if (fabs(hits[i-1]->z-hits[i]->z)>0.01
1203 && fabs(hits[i]->z-hits[i+1]->z)>0.01)
break;
1206 double Phi1=atan2(hits[i]->
y-y0,hits[i]->
x-x0);
1207 double z0=hits[i]->z;
1208 double plus_sum=0.,minus_sum=0.;
1209 for (
unsigned int j=0;j<hits.size();j++){
1211 double dphi=(hit->
z-z0)/(r0*tanl);
1213 double phiplus=Phi1+dphi;
1214 double dxplus=x0+r0*cos(phiplus)-hit->
x;
1215 double dyplus=y0+r0*
sin(phiplus)-hit->
y;
1216 double dxplus2=dxplus*dxplus;
1217 double dyplus2=dyplus*dyplus;
1218 double d2plus=dxplus2+dyplus2;
1219 double varplus=(dxplus2*hit->
covx+dyplus2*hit->
covy
1220 +2.*dyplus*dxplus*hit->
covxy)/d2plus;
1221 plus_sum+=d2plus/varplus;
1223 double phiminus=Phi1-dphi;
1224 double dxminus=x0+r0*cos(phiminus)-hit->
x;
1225 double dyminus=y0+r0*
sin(phiminus)-hit->
y;
1226 double dxminus2=dxminus*dxminus;
1227 double dyminus2=dyminus*dyminus;
1228 double d2minus=dxminus2+dyminus2;
1229 double varminus=(dxminus2*hit->
covx+dyminus2*hit->
covy
1230 +2.*dyminus*dxminus*hit->
covxy)/d2minus;
1231 minus_sum+=d2minus/varminus;
1240 if (minus_sum<plus_sum)
h=-1.;
DVector2 xy
rough x,y coordinates in lab coordinate system
jerror_t Dump(void) const
jerror_t FitCircleAndLineRiemann(float rc)
bool DHFProjection_cmp(const DHFProjection_t &a, const DHFProjection_t &b)
const DFDCWire * wire
DFDCWire for this wire.
bool DHFHitLessThanZ_C(DHFHit_t *const &a, DHFHit_t *const &b)
jerror_t FitCircleRiemann(float rc=0.)
class DFDCPseudo: definition for a reconstructed point in the FDC
void SearchPtrans(double ptrans_max=9.0, double ptrans_step=0.5)
jerror_t AddHitXYZ(float x, float y, float z)
jerror_t Print(void) const
float chisq
chi-sq contribution of this hit
jerror_t AddHit(float r, float phi, float z)
jerror_t GuessChargeFromCircleFit(void)
DHelicalFit & operator=(const DHelicalFit &fit)
jerror_t Fill_phi_circle(vector< DHFHit_t * > hits, float x0, float y0)
ChiSqSourceType_t chisq_source
double s
< wire position computed from cathode data, assuming the avalanche occurs at the wire ...
jerror_t PrintChiSqVector(void) const
jerror_t FitLine_FixedZvertex(float z_vertex)
const vector< DHFHit_t * > GetHits() const
double covyy
Covariance terms for (x,y)
jerror_t FitTrackRiemann(float rc)
jerror_t FitLineRiemann(void)
jerror_t FillTrackParams(void)
float phi_circle
< error info in radial coordinates
void FindSenseOfRotation(void)
jerror_t FitCircleStraightTrack()
bool operator()(DHFHit_t *const &a, DHFHit_t *const &b) const
double covxy
error info for x and y coordinates
jerror_t FitTrack_FixedZvertex(float z_vertex)
void Copy(const DHelicalFit &fit)
jerror_t PruneHit(int idx)
jerror_t AddStereoHit(const DCDCWire *wire)
bool RiemannFit_hit_cmp(DHFHit_t *a, DHFHit_t *b)
float z
point in lab coordinates
double covr
< error info in RPhi coordinates