LCFIVertex  0.7.2
TState.cpp
1 // Include files
2 
3 #include "../util/inc/helixrep.h"
4 #include "../util/inc/matrix.h"
5 #include "../util/inc/vector3.h"
6 
7 #include <marlin/Global.h>
8 #include <gear/BField.h>
9 
10 // local
11 
12 #include "../inc/TState.h"
13 
14 //-----------------------------------------------------------------------------
15 // Implementation file for class : TState
16 //
17 // 2007-07-26 : Tomas Lastovicka (LCFI)
18 //-----------------------------------------------------------------------------
19 
20 namespace vertex_lcfi
21 {
22 
23 //=============================================================================
24 // Standard constructor, initializes variables
25 //=============================================================================
26 
27 TState::TState( TrackState* TrackState )
28 {
29  fParentState = TrackState;
30  fParentTrack = TrackState->parentTrack();
31 
32  const SymMatrix5x5 cov5x5 = fParentTrack->covarianceMatrix();
33  Vector3 vec = fParentTrack->momentum();
34  HelixRep helix = fParentTrack->helixRep();
35 
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();
39 
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();
43 
44  double rr = 1./invR;
45  double pt = fabs(fCLight*fB*rr);
46  double qrr = rr*fQ;
47  double ptdr = fCLight*fB*rr*qrr;
48 
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;
51 
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 } };
58 
59  double mA[6][6];
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);
63  }
64 
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; // known to 0.001 Tesla
67 
68  double mJC[6][6];
69  for( int i=0; i<6; i++ )
70  for( int j=0; j<6; j++ ){
71  mJC[i][j]=0;
72  for( int k=0; k<6; k++ ) mJC[i][j] += mJ[i][k]*mA[k][j];
73  }
74 
75  for( int k=0,i=0; i<6; i++)
76  for( int j=0; j<=i; j++, k++ ){
77  fC[k] = 0;
78  for( int l=0; l<6; l++ ) fC[k]+= mJC[i][l]*mJ[j][l];
79  }
80 }
81 
82  void TState::GetMeasurement( const double xyz[], double m[], double V[] ) const
83 {
84  double b[3] = {0, 0, fB};
85  b[0]*=fCLight; b[1]*=fCLight; b[2]*=fCLight;
86 
87  TransportBz( GetDStoPointBz(xyz), m, V );
88 
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]) );
92  double h[6];
93 
94  h[0] = m[3]*sigmaS;
95  h[1] = m[4]*sigmaS;
96  h[2] = m[5]*sigmaS;
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;
100 
101  //* Fit of momentum (Px,Py,Pz) to XYZ point: optional
102 
103  if( true ){
104  double mVv[6] =
105  { V[ 0] + h[0]*h[0],
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] };
108 
109  double mVvp[9]=
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] };
113 
114  double mS[6] =
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] };
119 
120  double s = ( mVv[0]*mS[0] + mVv[1]*mS[1] + mVv[3]*mS[3] );
121  s = ( s > 1.E-20 ) ? 1./s : 0;
122 
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] };
126 
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] );
130 
131  h[0] = px*sigmaS;
132  h[1] = py*sigmaS;
133  h[2] = pz*sigmaS;
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;
137  }
138 
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];
142  V[ 9]+= h[3]*h[3];
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];
147 }
148 
149 void TState::TransportBz( double S, double P[], double C[] ) const
150 {
151 
152  //* Transport the particle on dS(=S), output to P[],C[], for Bz field
153 
154  double B = fB*fQ*fCLight;
155  double bs= B*S;
156  double s = sin(bs), c = cos(bs);
157  double sB, cB;
158 
159  if( fabs(bs)>1.e-10){
160  sB= s/B;
161  cB= (1-c)/B;
162  } else {
163  sB = (1. - bs*bs/6.)*S;
164  cB = .5*sB*bs;
165  }
166 
167  double px = fP[3];
168  double py = fP[4];
169  double pz = fP[5];
170 
171  P[0] = fP[0] + sB*px + cB*py;
172  P[1] = fP[1] - cB*px + sB*py;
173  P[2] = fP[2] + S*pz;
174  P[3] = c*px + s*py;
175  P[4] = -s*px + c*py;
176  P[5] = fP[5];
177 
178  double mJ[6][6] = { {1,0,0, sB, cB, 0 },
179  {0,1,0, -cB, sB, 0 },
180  {0,0,1, 0, 0, S },
181  {0,0,0, c, s, 0 },
182  {0,0,0, -s, c, 0 },
183  {0,0,0, 0, 0, 1 } };
184 
185  double mA[6][6];
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];
188 
189  double mJC[6][6];
190  for( int i=0; i<6; i++ )
191  for( int j=0; j<6; j++ ){
192  mJC[i][j]=0;
193  for( int k=0; k<6; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
194  }
195 
196  for( int k=0,i=0; i<6; i++)
197  for( int j=0; j<=i; j++, k++ ){
198  C[k] = 0;
199  for( int l=0; l<6; l++ ) C[k]+=mJC[i][l]*mJ[j][l];
200  }
201 }
202 
203 double TState::GetDStoPointBz( const double xyz[] ) const
204 {
205 
206  //* Get dS to a certain space point for Bz field, based on x-y
207 
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;
216 }
217 
218 
219 bool TState::GetDStoTStateBz( const TState *p,
220  double &DS, double &DS1 ) const
221 {
222 
223  //* Get dS to another particle for Bz field
224 
225  double px = fP[3];
226  double py = fP[4];
227  double pz = fP[5];
228 
229  double px1 = p->fP[3];
230  double py1 = p->fP[4];
231  double pz1 = p->fP[5];
232 
233  double bq = fB*fQ*fCLight;
234  double bq1 = fB*(p->fQ)*fCLight;
235  double s=0, ds=0, s1=0, ds1=0;
236 
237  if( fabs(bq)>1.e-8 || fabs(bq1)>1.e-8 ){
238 
239  double dx = (p->fP[0] - fP[0]);
240  double dy = (p->fP[1] - fP[1]);
241  double d2 = (dx*dx+dy*dy);
242 
243  double p2 = (px *px + py *py);
244  double p21 = (px1*px1 + py1*py1);
245 
246  double a = (px*py1 - py*px1);
247  double b = (px*px1 + py*py1);
248 
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;
252 
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;
255 
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)) ;
258 
259  double sa = 4*l2*p2 - ca*ca;
260  double sa1 = 4*l2*p21 - ca1*ca1;
261 
262  if( sa<0 ) sa=0;
263  if( sa1<0 ) sa1=0;
264 
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;
268  } else {
269  s = ((dx*px + dy*py) + (py*px1-px*py1)/bq1)/p2;
270  ds = s*s - (d2-2*(px1*dy-py1*dx)/bq1)/p2;
271  if( ds<0 ) ds = 0;
272  ds = sqrt(ds);
273  }
274 
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;
278  } else {
279  s1 = (-(dx*px1 + dy*py1) + (py*px1-px*py1)/bq)/p21;
280  ds1 = s1*s1 - (d2+2*(px*dy-py*dx)/bq)/p21;
281  if( ds1<0 ) ds1 = 0;
282  ds1 = sqrt(ds1);
283  }
284  }
285 
286  double ss[2], ss1[2], g[2][5], g1[2][5];
287 
288  ss[0] = s + ds;
289  ss[1] = s - ds;
290  ss1[0] = s1 + ds1;
291  ss1[1] = s1 - ds1;
292 
293  for( int i=0; i<2; i++ ){
294  double bs = bq*ss[i];
295  double c = cos(bs), s = sin(bs);
296  double cB,sB;
297  if( fabs(bq)>1.e-8){
298  cB= (1-c)/bq;
299  sB= s/bq;
300  }else{
301  sB = (1. - bs*bs/6.)*ss[i];
302  cB = .5*sB*bs;
303  }
304 
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;
310 
311  bs = bq1*ss1[i];
312  c = cos(bs); s = sin(bs);
313  if( fabs(bq1)>1.e-8){
314  cB= (1-c)/bq1;
315  sB= s/bq1;
316  }else{
317  sB = (1. - bs*bs/6.)*ss1[i];
318  cB = .5*sB*bs;
319  }
320 
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;
326  }
327 
328  int i=0, i1=0;
329  double dMin = 1.e10;
330 
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;
342  if( d < dMin ){
343  dMin = d;
344  i = j;
345  i1 = j1;
346  }
347  }
348  }
349 
350  if( dMin > 0.2 ) return false;
351 
352  DS = ss[i];
353  DS1 = ss1[i1];
354 
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];
359  double dx = x1-x;
360  double dy = y1-y;
361  double dz = z1-z;
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;
371  }
372  return true;
373 }
374 
375 //=============================================================================
376 // Destructor
377 //=============================================================================
378 
379 TState::~TState() {}
380 
381 //=============================================================================
382 
383 }
384 
vertex_lcfi::Track::covarianceMatrix
const SymMatrix5x5 & covarianceMatrix() const
Covariance Matrix.
Definition: track.cpp:54
vertex_lcfi::Track::momentum
const Vector3 & momentum() const
Track perigee momentum.
Definition: track.cpp:49
vertex_lcfi::Track::helixRep
const HelixRep & helixRep() const
Helix represenation of this track.
Definition: track.cpp:35
Generated on Fri Dec 2 2016 12:36:49 for LCFIVertex by   doxygen 1.8.6