3 #include "../util/inc/helixrep.h"
4 #include "../util/inc/matrix.h"
5 #include "../util/inc/vector3.h"
7 #include <marlin/Global.h>
8 #include <gear/BField.h>
12 #include "../inc/TState.h"
27 TState::TState( TrackState* TrackState )
29 fParentState = TrackState;
30 fParentTrack = TrackState->parentTrack();
33 Vector3 vec = fParentTrack->
momentum();
34 HelixRep helix = fParentTrack->
helixRep();
36 fQ = ( TrackState->isNeutral() ? 0 : (int)TrackState->charge() );
37 fCLight = 0.000299792458;
38 fB = marlin::Global::GEAR->getBField().at(gear::Vector3D(0.,0.,0.)).z();
40 double sinp = sin(helix.phi());
double cosp = cos(helix.phi());
41 double d0 = helix.d0();
double z0 = helix.z0();
42 double invR = helix.invR();
double tanl = helix.tanLambda();
45 double pt = fabs(fCLight*fB*rr);
47 double ptdr = fCLight*fB*rr*qrr;
49 fP[0] = -d0*sinp; fP[1] = d0*cosp; fP[2] = z0;
50 fP[3] = pt*cosp; fP[4] = pt*sinp; fP[5] = pt*tanl;
52 double mJ[6][6] = { {-sinp, -d0*cosp, 0, 0 , 0, 0 },
53 { cosp, -d0*sinp, 0, 0 , 0, 0 },
54 { 0, 0, 0, 1., 0, 0 },
55 { 0, -pt*sinp, -ptdr*cosp, 0 , 0, cosp*qrr },
56 { 0, pt*cosp, -ptdr*sinp, 0 , 0, sinp*qrr },
57 { 0, 0, -ptdr*tanl, 0 , pt, tanl*qrr } };
60 for(
int i=0; i<5; i++ )
61 for(
int j=0; j<=i; j++ ) {
62 mA[i][j] = mA[j][i] = cov5x5(i,j);
65 for(
int i=0; i<5; i++) mA[i][5] = mA[5][i] = 0;
66 mA[5][5] = 0.001*0.001*fCLight*fCLight;
69 for(
int i=0; i<6; i++ )
70 for(
int j=0; j<6; j++ ){
72 for(
int k=0; k<6; k++ ) mJC[i][j] += mJ[i][k]*mA[k][j];
75 for(
int k=0,i=0; i<6; i++)
76 for(
int j=0; j<=i; j++, k++ ){
78 for(
int l=0; l<6; l++ ) fC[k]+= mJC[i][l]*mJ[j][l];
82 void TState::GetMeasurement(
const double xyz[],
double m[],
double V[] )
const
84 double b[3] = {0, 0, fB};
85 b[0]*=fCLight; b[1]*=fCLight; b[2]*=fCLight;
87 TransportBz( GetDStoPointBz(xyz), m, V );
89 double d[3] = { xyz[0]-m[0], xyz[1]-m[1], xyz[2]-m[2] };
90 double sigmaS = 0.1 + 10. * sqrt( (d[0]*d[0]+d[1]*d[1]+d[2]*d[2])/
91 (m[3]*m[3]+m[4]*m[4]+m[5]*m[5]) );
97 h[3] = ( h[1]*b[2]-h[2]*b[1] )*fQ;
98 h[4] = ( h[2]*b[0]-h[0]*b[2] )*fQ;
99 h[5] = ( h[0]*b[1]-h[1]*b[0] )*fQ;
106 V[ 1] + h[0]*h[1], V[ 2] + h[1]*h[1],
107 V[ 3] + h[0]*h[2], V[ 4] + h[1]*h[2], V[ 5] + h[2]*h[2] };
110 { V[ 6] + h[0]*h[3], V[ 7] + h[1]*h[3], V[ 8] + h[2]*h[3],
111 V[10] + h[0]*h[4], V[11] + h[1]*h[4], V[12] + h[2]*h[4],
112 V[15] + h[0]*h[5], V[16] + h[1]*h[5], V[17] + h[2]*h[5] };
115 { mVv[2]*mVv[5] - mVv[4]*mVv[4],
116 mVv[3]*mVv[4] - mVv[1]*mVv[5], mVv[0]*mVv[5] - mVv[3]*mVv[3],
117 mVv[1]*mVv[4] - mVv[2]*mVv[3], mVv[1]*mVv[3] - mVv[0]*mVv[4],
118 mVv[0]*mVv[2] - mVv[1]*mVv[1] };
120 double s = ( mVv[0]*mS[0] + mVv[1]*mS[1] + mVv[3]*mS[3] );
121 s = ( s > 1.E-20 ) ? 1./s : 0;
123 double mSz[3] = { mS[0]*d[0]+mS[1]*d[1]+mS[3]*d[2],
124 mS[1]*d[0]+mS[2]*d[1]+mS[4]*d[2],
125 mS[3]*d[0]+mS[4]*d[1]+mS[5]*d[2] };
127 double px = m[3] + s*( mVvp[0]*mSz[0] + mVvp[1]*mSz[1] + mVvp[2]*mSz[2] );
128 double py = m[4] + s*( mVvp[3]*mSz[0] + mVvp[4]*mSz[1] + mVvp[5]*mSz[2] );
129 double pz = m[5] + s*( mVvp[6]*mSz[0] + mVvp[7]*mSz[1] + mVvp[8]*mSz[2] );
134 h[3] = ( h[1]*b[2]-h[2]*b[1] )*fQ;
135 h[4] = ( h[2]*b[0]-h[0]*b[2] )*fQ;
136 h[5] = ( h[0]*b[1]-h[1]*b[0] )*fQ;
139 V[ 0]+= h[0]*h[0]; V[ 1]+= h[1]*h[0]; V[ 2]+= h[1]*h[1];
140 V[ 3]+= h[2]*h[0]; V[ 4]+= h[2]*h[1]; V[ 5]+= h[2]*h[2];
141 V[ 6]+= h[3]*h[0]; V[ 7]+= h[3]*h[1]; V[ 8]+= h[3]*h[2];
143 V[10]+= h[4]*h[0]; V[11]+= h[4]*h[1]; V[12]+= h[4]*h[2];
144 V[13]+= h[4]*h[3]; V[14]+= h[4]*h[4];
145 V[15]+= h[5]*h[0]; V[16]+= h[5]*h[1]; V[17]+= h[5]*h[2];
146 V[18]+= h[5]*h[3]; V[19]+= h[5]*h[4]; V[20]+= h[5]*h[5];
149 void TState::TransportBz(
double S,
double P[],
double C[] )
const
154 double B = fB*fQ*fCLight;
156 double s = sin(bs), c = cos(bs);
159 if( fabs(bs)>1.e-10){
163 sB = (1. - bs*bs/6.)*S;
171 P[0] = fP[0] + sB*px + cB*py;
172 P[1] = fP[1] - cB*px + sB*py;
178 double mJ[6][6] = { {1,0,0, sB, cB, 0 },
179 {0,1,0, -cB, sB, 0 },
186 for(
int k=0,i=0; i<6; i++)
187 for(
int j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fC[k];
190 for(
int i=0; i<6; i++ )
191 for(
int j=0; j<6; j++ ){
193 for(
int k=0; k<6; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
196 for(
int k=0,i=0; i<6; i++)
197 for(
int j=0; j<=i; j++, k++ ){
199 for(
int l=0; l<6; l++ ) C[k]+=mJC[i][l]*mJ[j][l];
203 double TState::GetDStoPointBz(
const double xyz[] )
const
208 double bq = fB*fQ*fCLight;
209 double pt2 = fP[3]*fP[3] + fP[4]*fP[4];
210 if( pt2<1.e-4 )
return 0;
211 double dx = xyz[0] - fP[0];
212 double dy = xyz[1] - fP[1];
213 double a = dx*fP[3]+dy*fP[4];
214 if( fabs(bq)<1.e-8 )
return a/pt2;
215 return atan2( bq*a, pt2 + bq*(dy*fP[3] -dx*fP[4]) )/bq;
219 bool TState::GetDStoTStateBz(
const TState *p,
220 double &DS,
double &DS1 )
const
229 double px1 = p->fP[3];
230 double py1 = p->fP[4];
231 double pz1 = p->fP[5];
233 double bq = fB*fQ*fCLight;
234 double bq1 = fB*(p->fQ)*fCLight;
235 double s=0, ds=0, s1=0, ds1=0;
237 if( fabs(bq)>1.e-8 || fabs(bq1)>1.e-8 ){
239 double dx = (p->fP[0] - fP[0]);
240 double dy = (p->fP[1] - fP[1]);
241 double d2 = (dx*dx+dy*dy);
243 double p2 = (px *px + py *py);
244 double p21 = (px1*px1 + py1*py1);
246 double a = (px*py1 - py*px1);
247 double b = (px*px1 + py*py1);
249 double ldx = bq*bq1*dx - bq1*py + bq*py1 ;
250 double ldy = bq*bq1*dy + bq1*px - bq*px1 ;
251 double l2 = ldx*ldx + ldy*ldy;
253 double cS = bq1*p2 + bq*bq1*(dy* px - dx* py) - bq*b;
254 double cS1= bq*p21 - bq*bq1*(dy*px1 - dx*py1) - bq1*b;
256 double ca = bq*bq*bq1*d2 +2*( cS + bq*bq*(py1*dx-px1*dy)) ;
257 double ca1 = bq*bq1*bq1*d2 +2*( cS1 - bq1*bq1*(py*dx-px*dy)) ;
259 double sa = 4*l2*p2 - ca*ca;
260 double sa1 = 4*l2*p21 - ca1*ca1;
265 if( fabs(bq)>1.e-8 ){
266 s = atan2(bq*( bq1*(dx*px +dy*py) + a ), cS)/bq;
267 ds = atan2(sqrt(sa),ca)/bq;
269 s = ((dx*px + dy*py) + (py*px1-px*py1)/bq1)/p2;
270 ds = s*s - (d2-2*(px1*dy-py1*dx)/bq1)/p2;
275 if( fabs(bq1)>1.e-8){
276 s1 = atan2( -bq1*( bq*(dx*px1+dy*py1) + a), cS1 )/bq1;
277 ds1 = atan2(sqrt(sa1),ca1)/bq1;
279 s1 = (-(dx*px1 + dy*py1) + (py*px1-px*py1)/bq)/p21;
280 ds1 = s1*s1 - (d2+2*(px*dy-py*dx)/bq)/p21;
286 double ss[2], ss1[2], g[2][5], g1[2][5];
293 for(
int i=0; i<2; i++ ){
294 double bs = bq*ss[i];
295 double c = cos(bs), s = sin(bs);
301 sB = (1. - bs*bs/6.)*ss[i];
305 g[i][0] = fP[0] + sB*px + cB*py;
306 g[i][1] = fP[1] - cB*px + sB*py;
307 g[i][2] = fP[2] + ss[i]*pz;
308 g[i][3] = + c*px + s*py;
309 g[i][4] = - s*px + c*py;
312 c = cos(bs); s = sin(bs);
313 if( fabs(bq1)>1.e-8){
317 sB = (1. - bs*bs/6.)*ss1[i];
321 g1[i][0] = p->fP[0] + sB*px1 + cB*py1;
322 g1[i][1] = p->fP[1] - cB*px1 + sB*py1;
323 g1[i][2] = p->fP[2] + ss[i]*pz1;
324 g1[i][3] = + c*px1 + s*py1;
325 g1[i][4] = - s*px1 + c*py1;
331 for(
int j=0; j<2; j++ ){
332 for(
int j1=0; j1<2; j1++ ){
333 bool flag = fabs(g[j][0])<60. && fabs(g1[j1][0])<60.
334 && fabs(g[j][1])<60. && fabs(g1[j1][1])<60.
335 && fabs(g[j][2])<125. && fabs(g1[j1][2])<125.
336 && (fabs(ss[j])+fabs(ss1[j1])) < 60.;
337 if ( !flag )
continue;
338 double xx = g[j][0]-g1[j1][0];
339 double yy = g[j][1]-g1[j1][1];
340 double zz = g[j][2]-g1[j1][2];
341 double d = xx*xx + yy*yy + zz*zz;
350 if( dMin > 0.2 )
return false;
355 double x = g[i][0], y = g[i][1], z = g[i][2];
356 double ppx = g[i][3], ppy = g[i][4];
357 double x1 = g1[i1][0], y1 = g1[i1][1], z1 = g1[i1][2];
358 double ppx1 = g1[i1][3], ppy1 = g1[i1][4];
362 double a = ppx*ppx1 + ppy*ppy1 + pz*pz1;
363 double b = dx*ppx1 + dy*ppy1 + dz*pz1;
364 double c = dx*ppx + dy*ppy + dz*pz ;
365 double pp2 = ppx*ppx + ppy*ppy + pz*pz;
366 double pp21= ppx1*ppx1 + ppy1*ppy1 + pz1*pz1;
367 double det = pp2*pp21 - a*a;
368 if( fabs(det)>1.e-8 ){
369 DS += (a*b-pp21*c)/det;
370 DS1 += (a*c-pp2*b)/det;
const SymMatrix5x5 & covarianceMatrix() const
Covariance Matrix.
const Vector3 & momentum() const
Track perigee momentum.
const HelixRep & helixRep() const
Helix represenation of this track.