8809ce6382a4f7dd76a3099b4df0f9fa3866dc37
[u/mrichter/AliRoot.git] / HLT / TPCLib / tracking-ca / AliHLT3DTrackParam.cxx
1 // @(#) $Id$
2 //***************************************************************************
3 // This file is property of and copyright by the ALICE HLT Project          * 
4 // ALICE Experiment at CERN, All rights reserved.                           *
5 //                                                                          *
6 // Primary Authors: Sergey Gorbunov <sergey.gorbunov@kip.uni-heidelberg.de> *
7 //                  Ivan Kisel <kisel@kip.uni-heidelberg.de>                *
8 //                  for The ALICE HLT Project.                              *
9 //                                                                          *
10 // Permission to use, copy, modify and distribute this software and its     *
11 // documentation strictly for non-commercial purposes is hereby granted     *
12 // without fee, provided that the above copyright notice appears in all     *
13 // copies and that both the copyright notice and this permission notice     *
14 // appear in the supporting documentation. The authors make no claims       *
15 // about the suitability of this software for any purpose. It is            *
16 // provided "as is" without express or implied warranty.                    *
17 //***************************************************************************
18
19 #include "AliHLT3DTrackParam.h"
20 #include "TMath.h"
21
22 ClassImp(AliHLT3DTrackParam);
23
24 //* Transport utilities
25   
26 Double_t AliHLT3DTrackParam::GetDStoPoint( Double_t Bz, const Double_t xyz[3], const Double_t *T0 ) const
27 {
28   //* Get DS = Path/Momentum to a certain space point for Bz field
29
30   Double_t q = fSignQ;
31   if( !T0 ) T0 = fParam; 
32   else q = T0[6];
33
34   const Double_t kCLight = 0.000299792458;
35   Double_t bq = Bz*q*kCLight;
36   Double_t pt2 = T0[3]*T0[3] + T0[4]*T0[4];
37   if( pt2<1.e-4 ) return 0;
38   Double_t dx = xyz[0] - T0[0];
39   Double_t dy = xyz[1] - T0[1]; 
40   Double_t a = dx*T0[3]+dy*T0[4];
41   Double_t dS = 0;
42   if( TMath::Abs(bq)<1.e-8 ) dS = a/pt2;
43   else dS = TMath::ATan2( bq*a, pt2 + bq*(dy*T0[3] -dx*T0[4]) )/bq;
44   return dS;
45 }
46
47
48 void AliHLT3DTrackParam::TransportToDS( Double_t Bz, Double_t DS, Double_t *T0 )
49 {
50   //* Transport the particle on DS = Path/Momentum, for Bz field 
51
52   Double_t tmp[7];
53   if( !T0 ){
54     T0 = tmp;
55     T0[0] = fParam[0];
56     T0[1] = fParam[1];
57     T0[2] = fParam[2];
58     T0[3] = fParam[3];
59     T0[4] = fParam[4];
60     T0[5] = fParam[5];
61     T0[6] = fSignQ;
62   }
63   const Double_t kCLight = 0.000299792458;
64   Bz = Bz*T0[6]*kCLight;
65   Double_t bs= Bz*DS;
66   Double_t s = TMath::Sin(bs), c = TMath::Cos(bs);
67   Double_t sB, cB;
68   if( TMath::Abs(bs)>1.e-10){
69     sB= s/Bz;
70     cB= (1-c)/Bz;
71   }else{
72     sB = (1. - bs*bs/6.)*DS;
73     cB = .5*sB*bs;
74   }
75     
76   Double_t px = T0[3];
77   Double_t py = T0[4];
78   Double_t pz = T0[5];
79   
80   Double_t d[6] = { fParam[0]-T0[0], fParam[1]-T0[1], fParam[2]-T0[2], 
81                     fParam[3]-T0[3], fParam[4]-T0[4], fParam[5]-T0[5]  };
82
83   T0[0] = T0[0] + sB*px + cB*py;
84   T0[1] = T0[1] - cB*px + sB*py;
85   T0[2] = T0[2] + DS*pz                       ;
86   T0[3] =          c*px + s*py;
87   T0[4] =         -s*px + c*py;
88   T0[5] = T0[5];
89  
90   Double_t mJ[6][6] = { {1,0,0,   sB, cB,  0, },
91                         {0,1,0,  -cB, sB,  0, },
92                         {0,0,1,    0,  0, DS, },
93                         {0,0,0,    c,  s,  0, },
94                         {0,0,0,   -s,  c,  0, },
95                         {0,0,0,    0,  0,  1, }   };
96
97   for( Int_t i=0; i<6; i++){
98     fParam[i] = T0[i];
99     for( Int_t j=0; j<6; j++) fParam[i] += mJ[i][j]*d[j];
100   }
101
102   Double_t mA[6][6];
103   for( Int_t k=0,i=0; i<6; i++)
104     for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fCov[k]; 
105
106   Double_t mJC[6][6];
107   for( Int_t i=0; i<6; i++ )
108     for( Int_t j=0; j<6; j++ ){
109       mJC[i][j]=0;
110       for( Int_t k=0; k<6; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
111     }
112   
113   for( Int_t k=0,i=0; i<6; i++)
114     for( Int_t j=0; j<=i; j++, k++ ){
115       fCov[k] = 0;
116       for( Int_t l=0; l<6; l++ ) fCov[k]+=mJC[i][l]*mJ[j][l];
117     }
118 }
119
120
121 //* Fit utilities 
122
123 void AliHLT3DTrackParam::InitializeCovarianceMatrix()
124 {
125   //* Initialization of covariance matrix
126
127   for( Int_t i=0; i<21; i++ ) fCov[i] = 0;
128   fSignQ = 0;
129   fCov[0] = fCov[ 2] = fCov[ 5] = 100.;
130   fCov[9] = fCov[14] = fCov[20] = 10000.;
131   fChi2 = 0;
132   fNDF = -5;
133 }
134
135 void AliHLT3DTrackParam::GetGlueMatrix( const Double_t xyz[3], 
136                                         Double_t G[6], const Double_t *T0  ) const 
137 {
138   //* !
139
140   if( !T0 ) T0 = fParam;
141
142   Double_t dx = xyz[0]-T0[0], dy = xyz[1]-T0[1], dz = xyz[2]-T0[2];
143   Double_t px2= T0[3]*T0[3], py2= T0[4]*T0[4], pz2= T0[5]*T0[5];
144   Double_t s2 = (dx*dx + dy*dy + dz*dz);
145   Double_t p2 = px2 + py2 + pz2;
146   if( p2>1.e-4 ) s2/=p2;
147   Double_t x = T0[3]*s2;
148   Double_t xx= px2*s2, xy= x*T0[4], xz= x*T0[5], yy= py2*s2, yz= T0[4]*T0[5]*s2;
149   G[ 0]= xx;
150   G[ 1]= xy;   G[ 2]= yy;
151   G[ 3]= xz;   G[ 4]= yz;   G[ 5]= pz2*s2;  
152 }
153
154
155
156 void AliHLT3DTrackParam::Filter( const Double_t m[3], const Double_t V[6], const Double_t G[6] )
157
158   //* !
159   
160   Double_t 
161     c00 = fCov[ 0],
162     c10 = fCov[ 1], c11 = fCov[ 2],
163     c20 = fCov[ 3], c21 = fCov[ 4], c22 = fCov[ 5],
164     c30 = fCov[ 6], c31 = fCov[ 7], c32 = fCov[ 8],
165     c40 = fCov[10], c41 = fCov[11], c42 = fCov[12],
166     c50 = fCov[15], c51 = fCov[16], c52 = fCov[17];
167   
168   double
169     z0 = m[0]-fParam[0],
170     z1 = m[1]-fParam[1],
171     z2 = m[2]-fParam[2];
172   
173   Double_t mS[6] = { c00+V[0]+G[0], c10+V[1]+G[1], c11+V[2]+G[2],
174                      c20+V[3]+G[3], c21+V[4]+G[4], c22+V[5]+G[5] };
175   Double_t mSi[6];
176   mSi[0] = mS[4]*mS[4] - mS[2]*mS[5];
177   mSi[1] = mS[1]*mS[5] - mS[3]*mS[4];
178   mSi[3] = mS[2]*mS[3] - mS[1]*mS[4];
179   Double_t det = 1./(mS[0]*mSi[0] + mS[1]*mSi[1] + mS[3]*mSi[3]);
180   mSi[0] *= det;
181   mSi[1] *= det;
182   mSi[3] *= det;
183   mSi[2] = ( mS[3]*mS[3] - mS[0]*mS[5] )*det;
184   mSi[4] = ( mS[0]*mS[4] - mS[1]*mS[3] )*det;
185   mSi[5] = ( mS[1]*mS[1] - mS[0]*mS[2] )*det;
186   
187   fNDF  += 2;
188   fChi2 += ( +(mSi[0]*z0 + mSi[1]*z1 + mSi[3]*z2)*z0
189              +(mSi[1]*z0 + mSi[2]*z1 + mSi[4]*z2)*z1
190              +(mSi[3]*z0 + mSi[4]*z1 + mSi[5]*z2)*z2 );
191         
192   Double_t k0, k1, k2 ; // k = CHtS
193     
194   k0 = c00*mSi[0] + c10*mSi[1] + c20*mSi[3];
195   k1 = c00*mSi[1] + c10*mSi[2] + c20*mSi[4];
196   k2 = c00*mSi[3] + c10*mSi[4] + c20*mSi[5];
197     
198   fParam[ 0]+= k0*z0  + k1*z1  + k2*z2 ;
199   fCov  [ 0]-= k0*c00 + k1*c10 + k2*c20;
200   
201   k0 = c10*mSi[0] + c11*mSi[1] + c21*mSi[3];
202   k1 = c10*mSi[1] + c11*mSi[2] + c21*mSi[4];
203   k2 = c10*mSi[3] + c11*mSi[4] + c21*mSi[5];
204   
205   fParam[ 1]+= k0*z0  + k1*z1  + k2*z2 ;
206   fCov  [ 1]-= k0*c00 + k1*c10 + k2*c20;
207   fCov  [ 2]-= k0*c10 + k1*c11 + k2*c21;
208   
209   k0 = c20*mSi[0] + c21*mSi[1] + c22*mSi[3];
210   k1 = c20*mSi[1] + c21*mSi[2] + c22*mSi[4];
211   k2 = c20*mSi[3] + c21*mSi[4] + c22*mSi[5];
212   
213   fParam[ 2]+= k0*z0  + k1*z1  + k2*z2 ;
214   fCov  [ 3]-= k0*c00 + k1*c10 + k2*c20;
215   fCov  [ 4]-= k0*c10 + k1*c11 + k2*c21;
216   fCov  [ 5]-= k0*c20 + k1*c21 + k2*c22;
217   
218   k0 = c30*mSi[0] + c31*mSi[1] + c32*mSi[3];
219   k1 = c30*mSi[1] + c31*mSi[2] + c32*mSi[4];
220   k2 = c30*mSi[3] + c31*mSi[4] + c32*mSi[5];
221   
222   fParam[ 3]+= k0*z0  + k1*z1  + k2*z2 ;
223   fCov  [ 6]-= k0*c00 + k1*c10 + k2*c20;
224   fCov  [ 7]-= k0*c10 + k1*c11 + k2*c21;
225   fCov  [ 8]-= k0*c20 + k1*c21 + k2*c22;
226   fCov  [ 9]-= k0*c30 + k1*c31 + k2*c32;
227   
228   k0 = c40*mSi[0] + c41*mSi[1] + c42*mSi[3];
229   k1 = c40*mSi[1] + c41*mSi[2] + c42*mSi[4];
230   k2 = c40*mSi[3] + c41*mSi[4] + c42*mSi[5];
231     
232   fParam[ 4]+= k0*z0  + k1*z1  + k2*z2 ;
233   fCov  [10]-= k0*c00 + k1*c10 + k2*c20;
234   fCov  [11]-= k0*c10 + k1*c11 + k2*c21;
235   fCov  [12]-= k0*c20 + k1*c21 + k2*c22;
236   fCov  [13]-= k0*c30 + k1*c31 + k2*c32;
237   fCov  [14]-= k0*c40 + k1*c41 + k2*c42;
238
239   k0 = c50*mSi[0] + c51*mSi[1] + c52*mSi[3];
240   k1 = c50*mSi[1] + c51*mSi[2] + c52*mSi[4];
241   k2 = c50*mSi[3] + c51*mSi[4] + c52*mSi[5];
242   
243   fParam[ 5]+= k0*z0  + k1*z1  + k2*z2 ;
244   fCov  [15]-= k0*c00 + k1*c10 + k2*c20;
245   fCov  [16]-= k0*c10 + k1*c11 + k2*c21;
246   fCov  [17]-= k0*c20 + k1*c21 + k2*c22;
247   fCov  [18]-= k0*c30 + k1*c31 + k2*c32;
248   fCov  [19]-= k0*c40 + k1*c41 + k2*c42;
249   fCov  [20]-= k0*c50 + k1*c51 + k2*c52;
250
251   // fit charge
252
253   Double_t px = fParam[3];
254   Double_t py = fParam[4];
255   Double_t pz = fParam[5];
256   
257   Double_t p = TMath::Sqrt( px*px + py*py + pz*pz );
258   Double_t pi = 1./p;
259   Double_t qp = fSignQ*pi;
260   Double_t qp3 = qp*pi*pi;
261   Double_t 
262     c60 = qp3*(c30+c40+c50),
263     c61 = qp3*(c31+c41+c51),
264     c62 = qp3*(c32+c42+c52);
265
266   k0 = c60*mSi[0] + c61*mSi[1] + c62*mSi[3];
267   k1 = c60*mSi[1] + c61*mSi[2] + c62*mSi[4];
268   k2 = c60*mSi[3] + c61*mSi[4] + c62*mSi[5];
269   
270   qp+= k0*z0  + k1*z1  + k2*z2 ;
271   if( qp>0 ) fSignQ = 1;
272   else if(qp<0 ) fSignQ = -1;
273   else fSignQ = 0;
274 }
275
276
277 //* Other utilities
278
279 void AliHLT3DTrackParam::SetDirection( Double_t Direction[3] )
280 {
281   //* Change track direction 
282
283   if( fParam[3]*Direction[0] + fParam[4]*Direction[1] + fParam[5]*Direction[2] >= 0 ) return;
284
285   fParam[3] = -fParam[3];
286   fParam[4] = -fParam[4];
287   fParam[5] = -fParam[5];
288   fSignQ    = -fSignQ;
289
290   fCov[ 6]=-fCov[ 6]; fCov[ 7]=-fCov[ 7]; fCov[ 8]=-fCov[ 8];
291   fCov[10]=-fCov[10]; fCov[11]=-fCov[11]; fCov[12]=-fCov[12];
292   fCov[15]=-fCov[15]; fCov[16]=-fCov[16]; fCov[17]=-fCov[17];
293 }
294
295
296 void AliHLT3DTrackParam::RotateCoordinateSystem( Double_t alpha )
297 {
298   //* !
299
300   Double_t cA = TMath::Cos( alpha );
301   Double_t sA = TMath::Sin( alpha );
302   Double_t x= fParam[0], y= fParam[1], px= fParam[3], py= fParam[4];
303   fParam[0] = x*cA + y*sA;
304   fParam[1] =-x*sA + y*cA;
305   fParam[2] = fParam[2];
306   fParam[3] = px*cA + py*sA;
307   fParam[4] =-px*sA + py*cA;
308   fParam[5] = fParam[5];  
309
310   Double_t mJ[6][6] = { { cA,sA, 0,  0,  0,  0 },
311                         {-sA,cA, 0,  0,  0,  0 },
312                         {  0, 0, 1,  0,  0,  0 },
313                         {  0, 0, 0, cA, sA,  0 },
314                         {  0, 0, 0,-sA, cA,  0 },
315                         {  0, 0, 0,  0,  0,  1 }  };
316                   
317   Double_t mA[6][6];
318   for( Int_t k=0,i=0; i<6; i++)
319     for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fCov[k]; 
320
321   Double_t mJC[6][6];
322   for( Int_t i=0; i<6; i++ )
323     for( Int_t j=0; j<6; j++ ){
324       mJC[i][j]=0;
325       for( Int_t k=0; k<6; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
326     }
327   
328   for( Int_t k=0,i=0; i<6; i++)
329     for( Int_t j=0; j<=i; j++, k++ ){
330       fCov[k] = 0;
331       for( Int_t l=0; l<6; l++ ) fCov[k]+=mJC[i][l]*mJ[j][l];
332     }
333 }
334
335
336 void AliHLT3DTrackParam::Get5Parameters( Double_t alpha, Double_t T[6], Double_t C[15] ) const
337 {
338   //* !
339
340   AliHLT3DTrackParam t = *this;
341   t.RotateCoordinateSystem(alpha);
342   Double_t 
343     x= t.fParam[0], y= t.fParam[1], z = t.fParam[2], 
344     px= t.fParam[3], py= t.fParam[4], pz = t.fParam[5], q = t.fSignQ;
345
346   Double_t p2 = px*px+py*py+pz*pz;
347   if( p2<1.e-8 ) p2 = 1;
348   Double_t n2 = 1./p2;
349   Double_t n = sqrt(n2);
350
351   T[5] = x;
352   T[0] = y;
353   T[1] = z;
354   T[2] = py/px;
355   T[3] = pz/px;
356   T[4] = q*n;
357
358   Double_t mJ[5][6] = { { -T[2], 1, 0,  0,  0,  0 },
359                         { -T[3], 0, 1,  0,  0,  0 },
360                         { 0, 0, 0,  -T[2]/px,  1./px,  0 },
361                         { 0, 0, 0, -T[3]/px,  0,  1./px },
362                         { 0, 0, 0, -T[4]*n2*px, -T[4]*n2*py, -T[4]*n2*pz} };
363
364   Double_t mA[6][6];
365   for( Int_t k=0,i=0; i<6; i++)
366     for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = t.fCov[k]; 
367
368   Double_t mJC[5][6];
369   for( Int_t i=0; i<5; i++ )
370     for( Int_t j=0; j<6; j++ ){
371       mJC[i][j]=0;
372       for( Int_t k=0; k<6; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
373     }
374   
375   for( Int_t k=0,i=0; i<5; i++)
376     for( Int_t j=0; j<=i; j++, k++ ){
377       C[k] = 0;
378       for( Int_t l=0; l<6; l++ ) C[k]+=mJC[i][l]*mJ[j][l];
379     }
380 }