216d42d1b7cd374c1ebbb6fdd347e1b1bb9ac33a
[u/mrichter/AliRoot.git] / HLT / TPCLib / tracking-ca / AliHLTTPCCATrackPar.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: Jochen Thaeder <thaeder@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 "AliHLTTPCCATrackPar.h"
20 #include "TMath.h"
21
22 ClassImp(AliHLTTPCCATrackPar);
23
24 void AliHLTTPCCATrackPar::Init()
25 {
26   //* Initialization 
27
28   for( Int_t i=0; i<7; i++ ) fP[i] = 0;
29   for( Int_t i=0; i<28; i++ ) fC[i] = 0;
30   fC[0] = fC[2] = fC[5] = 10000;
31   fC[9] = fC[14] = fC[20] = 10000.;
32   fC[27] = 10.;
33   fChi2 = 0;
34   fNDF = -5;
35 }
36
37
38 void AliHLTTPCCATrackPar::Normalize( Double_t Direction[3] )
39 {
40   //* Normalize the track
41
42   Double_t p2 = fP[3]*fP[3] + fP[4]*fP[4] + fP[5]*fP[5];
43   if( p2<1.e-4 ) return;
44   Double_t a2 = 1./p2;
45   Double_t a = sqrt(a2);
46
47   if( Direction && ( fP[3]*Direction[0] + fP[4]*Direction[1] + fP[5]*Direction[2] < 0 ) ) a = -a;
48   
49   Double_t ex = fP[3]*a, ey = fP[4]*a, ez = fP[5]*a, qp = fP[6]*a;
50
51   fP[3] = ex;
52   fP[4] = ey;
53   fP[5] = ez;
54   fP[6] = qp;
55
56   Double_t 
57     h0 = fC[ 6]*ex + fC[10]*ey + fC[15]*ez,
58     h1 = fC[ 7]*ex + fC[11]*ey + fC[16]*ez, 
59     h2 = fC[ 8]*ex + fC[12]*ey + fC[17]*ez,
60     h3 = fC[ 9]*ex + fC[13]*ey + fC[18]*ez, 
61     h4 = fC[13]*ex + fC[14]*ey + fC[19]*ez, 
62     h5 = fC[18]*ex + fC[19]*ey + fC[20]*ez,
63     h6 = fC[24]*ex + fC[25]*ey + fC[26]*ez,
64     d  = h3*ex + h4*ey + h5*ez, 
65     hh = h6 - qp*d ;
66
67   fC[ 6]= a*(fC[ 6] -ex*h0); fC[ 7]= a*(fC[ 7] -ex*h1); fC[ 8]= a*(fC[ 8] -ex*h2); 
68   fC[10]= a*(fC[10] -ey*h0); fC[11]= a*(fC[11] -ey*h1); fC[12]= a*(fC[12] -ey*h2); 
69   fC[15]= a*(fC[15] -ez*h0); fC[16]= a*(fC[16] -ez*h1); fC[17]= a*(fC[17] -ez*h2); 
70   fC[21]= a*(fC[21] -qp*h0); fC[22]= a*(fC[22] -qp*h1); fC[23]= a*(fC[23] -qp*h2);     
71
72   fC[ 9]= a2*( fC[ 9] -h3*ex -h3*ex + d*ex*ex );
73   fC[13]= a2*( fC[13] -h4*ex -h3*ey + d*ey*ex ); 
74   fC[14]= a2*( fC[14] -h4*ey -h4*ey + d*ey*ey );
75
76   fC[18]= a2*( fC[18] -h5*ex -h3*ez + d*ez*ex ); 
77   fC[19]= a2*( fC[19] -h5*ey -h4*ez + d*ez*ey ); 
78   fC[20]= a2*( fC[20] -h5*ez -h5*ez + d*ez*ez );
79
80   fC[24]= a2*( fC[24] -qp*h3 - hh*ex ); 
81   fC[25]= a2*( fC[25] -qp*h4 - hh*ey ); 
82   fC[26]= a2*( fC[26] -qp*h5 - hh*ez );
83   fC[27]= a2*( fC[27] -qp*h6 - hh*qp  ); 
84 }
85
86
87 Double_t AliHLTTPCCATrackPar::GetDsToPointBz( Double_t Bz, const Double_t xyz[3], const Double_t *T0 ) const
88 {
89   //* Get dS to a certain space point for Bz field
90   const Double_t kCLight = 0.000299792458;
91   Double_t bq = Bz*T0[6]*kCLight;
92   Double_t pt2 = T0[3]*T0[3] + T0[4]*T0[4];
93   if( pt2<1.e-4 ) return 0;
94   Double_t dx = xyz[0] - T0[0];
95   Double_t dy = xyz[1] - T0[1]; 
96   Double_t a = dx*T0[3]+dy*T0[4];
97   Double_t dS = 0;
98   if( TMath::Abs(bq)<1.e-8 ) dS = a/pt2;
99   else dS = TMath::ATan2( bq*a, pt2 + bq*(dy*T0[3] -dx*T0[4]) )/bq;
100   return dS;
101 }
102
103 Double_t AliHLTTPCCATrackPar::GetDsToPointBz( Double_t Bz, const Double_t xyz[3] ) const 
104 {
105   return GetDsToPointBz( Bz, xyz, fP );
106 }
107
108 void AliHLTTPCCATrackPar::TransportBz( Double_t Bz, Double_t S, Double_t *T0 ) 
109
110   //* Transport the particle on dS, for Bz field
111  
112   const Double_t kCLight = 0.000299792458;
113   Bz = Bz*kCLight;
114   Double_t bs= Bz*S;
115   Double_t bqs= bs*T0[6];
116   Double_t s = TMath::Sin(bqs), c = TMath::Cos(bqs);
117   Double_t sB, cB, dsB, dcB;
118   if( TMath::Abs(bqs)>1.e-10){
119     sB= s/Bz/T0[6];
120     cB= (1-c)/Bz/T0[6];
121     dsB = (c*S - sB)/T0[6];
122     dcB = (s*S-cB)/T0[6];
123   }else{
124     sB = (1. - bqs*bqs/6.)*S;
125     cB = .5*sB*bqs;
126     dsB = - T0[6]*bs*bs/3.*S;
127     dcB = .5*(sB*bs - dsB*bqs);
128   }
129   
130   Double_t px = T0[3];
131   Double_t py = T0[4];
132   Double_t pz = T0[5];
133   
134   Double_t d[7] = { fP[0]-T0[0], fP[1]-T0[1], fP[2]-T0[2], 
135                     fP[3]-T0[3], fP[4]-T0[4], fP[5]-T0[5], fP[6]-T0[6] };
136
137   T0[0] = T0[0] + sB*px + cB*py;
138   T0[1] = T0[1] - cB*px + sB*py;
139   T0[2] = T0[2] +  S*pz                       ;
140   T0[3] =          c*px + s*py;
141   T0[4] =         -s*px + c*py;
142   T0[5] = T0[5];
143   T0[6] = T0[6];
144
145  
146   Double_t mJ[7][7] = { {1,0,0,   sB, cB,  0,   dsB*px + dcB*py },
147                         {0,1,0,  -cB, sB,  0, - dcB*px + dsB*py },
148                         {0,0,1,    0,  0,  S,   0               },
149                         {0,0,0,    c,  s,  0,   (-s*px + c*py)*bs },
150                         {0,0,0,   -s,  c,  0,   (-c*px - s*py)*bs },
151                         {0,0,0,    0,  0,  1,   0                 },
152                         {0,0,0,    0,  0,  0,   1                 }  };
153
154   for( Int_t i=0; i<7; i++){
155     fP[i] = T0[i];
156     for( Int_t j=0; j<7; j++) fP[i] += mJ[i][j]*d[j];
157   }
158
159   Double_t mA[7][7];
160   for( Int_t k=0,i=0; i<7; i++)
161     for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fC[k]; 
162
163   Double_t mJC[7][7];
164   for( Int_t i=0; i<7; i++ )
165     for( Int_t j=0; j<7; j++ ){
166       mJC[i][j]=0;
167       for( Int_t k=0; k<7; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
168     }
169   
170   for( Int_t k=0,i=0; i<7; i++)
171     for( Int_t j=0; j<=i; j++, k++ ){
172       fC[k] = 0;
173       for( Int_t l=0; l<7; l++ ) fC[k]+=mJC[i][l]*mJ[j][l];
174     }
175 }
176
177 void AliHLTTPCCATrackPar::TransportBz( Double_t Bz, Double_t dS ) 
178
179   //* Transport the particle on dS, for Bz field
180   TransportBz( Bz, dS, fP );
181 }
182
183 void AliHLTTPCCATrackPar::GetConnectionMatrix( Double_t B, const Double_t p[3], Double_t G[6], const Double_t *T0  ) const 
184 {
185   //* Calculate connection matrix between track and point p
186   if( !G ) return;
187   const Double_t kLight = 0.000299792458;
188   B*=kLight;
189   Double_t dx = p[0]-T0[0], dy = p[1]-T0[1], dz = p[2]-T0[2];
190   Double_t px2= T0[3]*T0[3], py2= T0[4]*T0[4], pz2= T0[5]*T0[5];
191   //Double_t B2 = B*B;
192   Double_t s2 = (dx*dx + dy*dy + dz*dz);
193   Double_t p2 = px2 + py2 + pz2;
194   if( p2>1.e-4 ) s2/=p2;
195   Double_t x = T0[3]*s2;
196   Double_t xx= px2*s2, xy= x*T0[4], xz= x*T0[5], yy= py2*s2, yz= T0[4]*T0[5]*s2;
197   //Double_t Bxy= B*xy; 
198   G[ 0]= xx;
199   G[ 1]= xy;   G[ 2]= yy;
200   G[ 3]= xz;   G[ 4]= yz;   G[ 5]= pz2*s2;  
201   /*
202   C[ 0]+= xx;
203   C[ 1]+= xy;   C[ 2]+= yy;
204   C[ 3]+= xz;   C[ 4]+= yz;   C[ 5]+= pz2*s2;  
205   C[ 6]+= Bxy; C[ 7]+= B*yy; C[ 8]+= B*yz;    C[ 9]+=B2*yy;
206   C[10]-= B*xx; C[11]-= Bxy; C[12]-=B*xz;     C[13]-=B2*xy; C[14]+=B2*xx;  
207   */
208 }
209
210
211 void AliHLTTPCCATrackPar::Filter( const Double_t m[3], const Double_t V[6], const Double_t V1[6] )
212 {
213   //* !
214   Double_t 
215     c00 = fC[ 0],
216     c10 = fC[ 1], c11 = fC[ 2],
217     c20 = fC[ 3], c21 = fC[ 4], c22 = fC[ 5],
218     c30 = fC[ 6], c31 = fC[ 7], c32 = fC[ 8],
219     c40 = fC[10], c41 = fC[11], c42 = fC[12],
220     c50 = fC[15], c51 = fC[16], c52 = fC[17],
221     c60 = fC[21], c61 = fC[22], c62 = fC[23];
222   
223   double
224     z0 = m[0]-fP[0],
225     z1 = m[1]-fP[1],
226     z2 = m[2]-fP[2];
227   
228   Double_t mS[6] = { c00+V[0]+V1[0], c10+V[1]+V1[1], c11+V[2]+V1[2],
229                      c20+V[3]+V1[3], c21+V[4]+V1[4], c22+V[5]+V1[5] };
230   Double_t mSi[6];
231   mSi[0] = mS[4]*mS[4] - mS[2]*mS[5];
232   mSi[1] = mS[1]*mS[5] - mS[3]*mS[4];
233   mSi[3] = mS[2]*mS[3] - mS[1]*mS[4];
234   Double_t det = (mS[0]*mSi[0] + mS[1]*mSi[1] + mS[3]*mSi[3]);
235   if( TMath::Abs(det)<1.e-10 ) return;
236   det = 1./det;
237   mSi[0] *= det;
238   mSi[1] *= det;
239   mSi[3] *= det;
240   mSi[2] = ( mS[3]*mS[3] - mS[0]*mS[5] )*det;
241   mSi[4] = ( mS[0]*mS[4] - mS[1]*mS[3] )*det;
242   mSi[5] = ( mS[1]*mS[1] - mS[0]*mS[2] )*det;
243   
244   fNDF  += 2;
245   fChi2 += ( +(mSi[0]*z0 + mSi[1]*z1 + mSi[3]*z2)*z0
246             +(mSi[1]*z0 + mSi[2]*z1 + mSi[4]*z2)*z1
247             +(mSi[3]*z0 + mSi[4]*z1 + mSi[5]*z2)*z2 );
248         
249   Double_t k0, k1, k2 ; // k = CHtS
250     
251   k0 = c00*mSi[0] + c10*mSi[1] + c20*mSi[3];
252   k1 = c00*mSi[1] + c10*mSi[2] + c20*mSi[4];
253   k2 = c00*mSi[3] + c10*mSi[4] + c20*mSi[5];
254     
255   fP[ 0]+= k0*z0  + k1*z1  + k2*z2 ;
256   fC[ 0]-= k0*c00 + k1*c10 + k2*c20;
257   
258   k0 = c10*mSi[0] + c11*mSi[1] + c21*mSi[3];
259   k1 = c10*mSi[1] + c11*mSi[2] + c21*mSi[4];
260   k2 = c10*mSi[3] + c11*mSi[4] + c21*mSi[5];
261   
262   fP[ 1]+= k0*z0  + k1*z1  + k2*z2 ;
263   fC[ 1]-= k0*c00 + k1*c10 + k2*c20;
264   fC[ 2]-= k0*c10 + k1*c11 + k2*c21;
265   
266   k0 = c20*mSi[0] + c21*mSi[1] + c22*mSi[3];
267   k1 = c20*mSi[1] + c21*mSi[2] + c22*mSi[4];
268   k2 = c20*mSi[3] + c21*mSi[4] + c22*mSi[5];
269   
270   fP[ 2]+= k0*z0  + k1*z1  + k2*z2 ;
271   fC[ 3]-= k0*c00 + k1*c10 + k2*c20;
272   fC[ 4]-= k0*c10 + k1*c11 + k2*c21;
273   fC[ 5]-= k0*c20 + k1*c21 + k2*c22;
274   
275   k0 = c30*mSi[0] + c31*mSi[1] + c32*mSi[3];
276   k1 = c30*mSi[1] + c31*mSi[2] + c32*mSi[4];
277   k2 = c30*mSi[3] + c31*mSi[4] + c32*mSi[5];
278   
279   fP[ 3]+= k0*z0  + k1*z1  + k2*z2 ;
280   fC[ 6]-= k0*c00 + k1*c10 + k2*c20;
281   fC[ 7]-= k0*c10 + k1*c11 + k2*c21;
282   fC[ 8]-= k0*c20 + k1*c21 + k2*c22;
283   fC[ 9]-= k0*c30 + k1*c31 + k2*c32;
284   
285   k0 = c40*mSi[0] + c41*mSi[1] + c42*mSi[3];
286   k1 = c40*mSi[1] + c41*mSi[2] + c42*mSi[4];
287   k2 = c40*mSi[3] + c41*mSi[4] + c42*mSi[5];
288     
289   fP[ 4]+= k0*z0  + k1*z1  + k2*z2 ;
290   fC[10]-= k0*c00 + k1*c10 + k2*c20;
291   fC[11]-= k0*c10 + k1*c11 + k2*c21;
292   fC[12]-= k0*c20 + k1*c21 + k2*c22;
293   fC[13]-= k0*c30 + k1*c31 + k2*c32;
294   fC[14]-= k0*c40 + k1*c41 + k2*c42;
295
296   k0 = c50*mSi[0] + c51*mSi[1] + c52*mSi[3];
297   k1 = c50*mSi[1] + c51*mSi[2] + c52*mSi[4];
298   k2 = c50*mSi[3] + c51*mSi[4] + c52*mSi[5];
299   
300   fP[ 5]+= k0*z0  + k1*z1  + k2*z2 ;
301   fC[15]-= k0*c00 + k1*c10 + k2*c20;
302   fC[16]-= k0*c10 + k1*c11 + k2*c21;
303   fC[17]-= k0*c20 + k1*c21 + k2*c22;
304   fC[18]-= k0*c30 + k1*c31 + k2*c32;
305   fC[19]-= k0*c40 + k1*c41 + k2*c42;
306   fC[20]-= k0*c50 + k1*c51 + k2*c52;
307
308   k0 = c60*mSi[0] + c61*mSi[1] + c62*mSi[3];
309   k1 = c60*mSi[1] + c61*mSi[2] + c62*mSi[4];
310   k2 = c60*mSi[3] + c61*mSi[4] + c62*mSi[5];
311   
312   fP[ 6]+= k0*z0  + k1*z1  + k2*z2 ;
313   fC[21]-= k0*c00 + k1*c10 + k2*c20;
314   fC[22]-= k0*c10 + k1*c11 + k2*c21;
315   fC[23]-= k0*c20 + k1*c21 + k2*c22;
316   fC[24]-= k0*c30 + k1*c31 + k2*c32;
317   fC[25]-= k0*c40 + k1*c41 + k2*c42;
318   fC[26]-= k0*c50 + k1*c51 + k2*c52;
319   fC[27]-= k0*c60 + k1*c61 + k2*c62;
320 }
321
322
323 void AliHLTTPCCATrackPar::Rotate( Double_t alpha )
324 {
325   //* !
326   Double_t cA = TMath::Cos( alpha );
327   Double_t sA = TMath::Sin( alpha );
328   Double_t x= fP[0], y= fP[1], px= fP[3], py= fP[4];
329   fP[0] = x*cA + y*sA;
330   fP[1] =-x*sA + y*cA;
331   fP[2] = fP[2];
332   fP[3] = px*cA + py*sA;
333   fP[4] =-px*sA + py*cA;
334   fP[5] = fP[5];
335   fP[6] = fP[6];
336
337   Double_t mJ[7][7] = { { cA,sA, 0,  0,  0,  0,  0 },
338                         {-sA,cA, 0,  0,  0,  0,  0 },
339                         {  0, 0, 1,  0,  0,  0,  0 },
340                         {  0, 0, 0, cA, sA,  0,  0 },
341                         {  0, 0, 0,-sA, cA,  0,  0 },
342                         {  0, 0, 0,  0,  0,  1,  0 },
343                         {  0, 0, 0,  0,  0,  0,  1 }  };
344
345   Double_t mA[7][7];
346   for( Int_t k=0,i=0; i<7; i++)
347     for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = fC[k]; 
348
349   Double_t mJC[7][7];
350   for( Int_t i=0; i<7; i++ )
351     for( Int_t j=0; j<7; j++ ){
352       mJC[i][j]=0;
353       for( Int_t k=0; k<7; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
354     }
355   
356   for( Int_t k=0,i=0; i<7; i++)
357     for( Int_t j=0; j<=i; j++, k++ ){
358       fC[k] = 0;
359       for( Int_t l=0; l<7; l++ ) fC[k]+=mJC[i][l]*mJ[j][l];
360     }
361 }
362
363
364 void AliHLTTPCCATrackPar::ConvertTo5( Double_t alpha, Double_t T[], Double_t C[] )
365 const {
366   //* !
367   AliHLTTPCCATrackPar t = *this;
368   t.Rotate(alpha);
369   Double_t 
370     x= t.fP[0], y= t.fP[1], z = t.fP[2], 
371     ex= t.fP[3], ey= t.fP[4], ez = t.fP[5], qp = t.fP[6];
372
373   Double_t p2 = ex*ex+ey*ey+ez*ez;
374   if( p2<1.e-4 ) p2 = 1;
375   Double_t n2 = 1./p2;
376   Double_t n = sqrt(n2);
377
378   T[5] = x;
379   T[0] = y;
380   T[1] = z;
381   T[2] = ey/ex;
382   T[3] = ez/ex;
383   T[4] = qp*n;
384
385   Double_t mJ[5][7] = { { -T[2], 1, 0,  0,  0,  0,  0 },
386                         { -T[3], 0, 1,  0,  0,  0,  0 },
387                         { 0, 0, 0,  -T[2]/ex,  1./ex,  0,  0 },
388                         { 0, 0, 0, -T[3]/ex,  0,  1./ex,  0 },
389                         { 0, 0, 0, -T[4]*n2*ex, -T[4]*n2*ey, -T[4]*n2*ez, n }};
390
391   Double_t mA[7][7];
392   for( Int_t k=0,i=0; i<7; i++)
393     for( Int_t j=0; j<=i; j++, k++ ) mA[i][j] = mA[j][i] = t.fC[k]; 
394
395   Double_t mJC[5][7];
396   for( Int_t i=0; i<5; i++ )
397     for( Int_t j=0; j<7; j++ ){
398       mJC[i][j]=0;
399       for( Int_t k=0; k<7; k++ ) mJC[i][j]+=mJ[i][k]*mA[k][j];
400     }
401   
402   for( Int_t k=0,i=0; i<5; i++)
403     for( Int_t j=0; j<=i; j++, k++ ){
404       C[k] = 0;
405       for( Int_t l=0; l<7; l++ ) C[k]+=mJC[i][l]*mJ[j][l];
406     }
407 }