793d77bbb1abbe6ed3d9a5d86d5e01fa6b3aecc2
[u/mrichter/AliRoot.git] / STEER / AliTrackFitterRieman.cxx
1 #include "TMatrixDSym.h"
2 #include "TMatrixD.h"
3 #include "AliTrackFitterRieman.h"
4
5 ClassImp(AliTrackFitterRieman)
6
7 AliTrackFitterRieman::AliTrackFitterRieman():
8   AliTrackFitter()
9 {
10   //
11   // default constructor
12   //
13   fAlpha = 0.;
14   for (Int_t i=0;i<9;i++) fSumXY[i] = 0;
15   for (Int_t i=0;i<9;i++) fSumXZ[i] = 0;
16   fConv = kFALSE;
17 }
18
19
20 AliTrackFitterRieman::AliTrackFitterRieman(AliTrackPointArray *array, Bool_t owner):
21   AliTrackFitter(array,owner)
22 {
23   //
24   // Constructor
25   //
26   fAlpha = 0.;
27   for (Int_t i=0;i<9;i++) fSumXY[i] = 0;
28   for (Int_t i=0;i<9;i++) fSumXZ[i] = 0;
29   fConv = kFALSE;
30 }
31
32 AliTrackFitterRieman::AliTrackFitterRieman(const AliTrackFitterRieman &rieman):
33   AliTrackFitter(rieman)
34 {
35   //
36   // copy constructor
37   //
38   fAlpha = rieman.fAlpha;
39   for (Int_t i=0;i<9;i++) fSumXY[i]  = rieman.fSumXY[i];
40   for (Int_t i=0;i<9;i++) fSumXZ[i]  = rieman.fSumXZ[i];
41   fConv = rieman.fConv;
42 }
43
44 //_____________________________________________________________________________
45 AliTrackFitterRieman &AliTrackFitterRieman::operator =(const AliTrackFitterRieman& rieman)
46 {
47   // assignment operator
48   //
49   if(this==&rieman) return *this;
50   ((AliTrackFitter *)this)->operator=(rieman);
51
52   fAlpha = rieman.fAlpha;
53   for (Int_t i=0;i<9;i++) fSumXY[i]  = rieman.fSumXY[i];
54   for (Int_t i=0;i<9;i++) fSumXZ[i]  = rieman.fSumXZ[i];
55   fConv = rieman.fConv;
56
57   return *this;
58 }
59
60 AliTrackFitterRieman::~AliTrackFitterRieman()
61 {
62   // destructor
63   //
64 }
65
66 void AliTrackFitterRieman::Reset()
67 {
68   // Reset the track parameters and
69   // rieman sums
70   AliTrackFitter::Reset();
71   fAlpha = 0.;
72   for (Int_t i=0;i<9;i++) fSumXY[i] = 0;
73   for (Int_t i=0;i<9;i++) fSumXZ[i] = 0;
74   fConv =kFALSE;
75 }
76
77 Bool_t AliTrackFitterRieman::Fit(UShort_t volId,
78                                  AliTrackPointArray *pVolId, AliTrackPointArray *pTrack,
79                                  AliAlignObj::ELayerID layerRangeMin,
80                                  AliAlignObj::ELayerID layerRangeMax)
81 {
82   // Fit the track points. The method takes as an input
83   // the id (volid) of the volume to be skipped from fitting.
84   // The following two parameters are used to define the
85   // range of volumes to be used in the fitting
86   // As a result two AliTrackPointArray's obects are filled.
87   // The first one contains the space points with
88   // volume id = volid. The second array of points represents
89   // the track extrapolation corresponding to the space points
90   // in the first array. The two arrays can be used to find
91   // the residuals in the volid and consequently construct a
92   // chi2 function to be minimized during the alignment
93   // procedures. For the moment the track extrapolation is taken
94   // as follows: in XY plane - at the CDA between track circle
95   // and the space point; in Z - the track extrapolation on the Z
96   // plane defined by the space point.
97
98   pVolId = pTrack = 0x0;
99   fConv = kFALSE;
100
101   Int_t npoints = fPoints->GetNPoints();
102   if (npoints < 3) return kFALSE;
103
104   AliTrackPoint p;
105   fPoints->GetPoint(p,0);
106   fAlpha = TMath::ATan2(p.GetY(),p.GetX());
107   Double_t sin = TMath::Sin(fAlpha);
108   Double_t cos = TMath::Cos(fAlpha);
109
110   Int_t npVolId = 0;
111   Int_t npused = 0;
112   Int_t *pindex = new Int_t[npoints];
113   for (Int_t ipoint = 0; ipoint < npoints; ipoint++)
114     {
115       fPoints->GetPoint(p,ipoint);
116       UShort_t iVolId = p.GetVolumeID();
117       if (iVolId == volId) {
118         pindex[npVolId] = ipoint;
119         npVolId++;
120       }
121       if (iVolId < AliAlignObj::LayerToVolUID(layerRangeMin,0) ||
122           iVolId >= AliAlignObj::LayerToVolUID(layerRangeMax,0)) continue;
123       Float_t x = p.GetX()*cos + p.GetY()*sin;
124       Float_t y = p.GetY()*cos - p.GetX()*sin;
125       AddPoint(x,y,p.GetZ(),1,1);
126       npused++;
127     }
128
129   if (npused < 3) {
130     delete [] pindex;
131     return kFALSE;
132   }
133
134   Update();
135
136   if (!fConv) {
137     delete [] pindex;
138     return kFALSE;
139   }
140
141   if ((fParams[0] == 0) ||
142       ((-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1) <= 0)) {
143     delete [] pindex;
144     return kFALSE;
145   }
146
147   pVolId = new AliTrackPointArray(npVolId);
148   pTrack = new AliTrackPointArray(npVolId);
149   AliTrackPoint p2;
150   for (Int_t ipoint = 0; ipoint < npVolId; ipoint++)
151     {
152       Int_t index = pindex[ipoint];
153       fPoints->GetPoint(p,index);
154       if (GetPCA(p,p2)) {
155         pVolId->AddPoint(ipoint,&p);
156         pTrack->AddPoint(ipoint,&p2);
157       }
158     }  
159
160   delete [] pindex;
161
162   return kTRUE;
163 }
164
165 void AliTrackFitterRieman::AddPoint(Float_t x, Float_t y, Float_t z, Float_t sy, Float_t sz)
166 {
167   //
168   //  Rieman update
169   //
170   //------------------------------------------------------
171   // XY direction
172   //
173   //  (x-x0)^2+(y-y0)^2-R^2=0 ===>
174   //
175   //  (x^2+y^2 -2*x*x0 - 2*y*y0+ x0^2 -y0^2 -R^2 =0;  ==>
176   //
177   //   substitution t = 1/(x^2+y^2),   u = 2*x*t, y = 2*y*t,  D0 = R^2 - x0^2- y0^2
178   //
179   //  1 - u*x0 - v*y0 - t *D0 =0 ;  - linear equation
180   //     
181   //  next substition   a = 1/y0    b = -x0/y0   c = -D0/y0
182   //
183   //  final linear equation :   a + u*b +t*c - v =0;
184   //
185   // Minimization :
186   //
187   // sum( (a + ui*b +ti*c - vi)^2)/(sigmai)^2 = min;
188   //
189   // where sigmai is the error of  maesurement  (a + ui*b +ti*c - vi)
190   //
191   // neglecting error of xi, and supposing  xi>>yi    sigmai ~ sigmaVi ~ 2*sigmay*t  
192   //
193   //
194   // XY part
195   //
196   Double_t  t  =  x*x+y*y;
197   if (t<2) return;
198   t            = 1./t;
199   Double_t  u  =  2.*x*t;
200   Double_t  v  =  2.*y*t;
201   Double_t  error = 2.*sy*t;
202   error *=error;
203   Double_t weight = 1./error;
204   fSumXY[0] +=weight;
205   fSumXY[1] +=u*weight;      fSumXY[2]+=v*weight;  fSumXY[3]+=t*weight;
206   fSumXY[4] +=u*u*weight;    fSumXY[5]+=t*t*weight;
207   fSumXY[6] +=u*v*weight;    fSumXY[7]+=u*t*weight; fSumXY[8]+=v*t*weight;
208   //
209   // XZ part
210   //
211   weight = 1./sz;
212   fSumXZ[0] +=weight;
213   fSumXZ[1] +=weight*x;   fSumXZ[2] +=weight*x*x; fSumXZ[3] +=weight*x*x*x; fSumXZ[4] += weight*x*x*x*x;
214   fSumXZ[5] +=weight*z;   fSumXZ[6] +=weight*x*z; fSumXZ[7] +=weight*x*x*z;
215 }
216
217 void AliTrackFitterRieman::Update(){
218   //
219   //  Rieman update
220   //
221   //
222   for (Int_t i=0;i<6;i++)fParams[i]=0;
223   Int_t conv=0;
224   //
225   // XY part
226   //
227   TMatrixDSym     smatrix(3);
228   TMatrixD        sums(1,3);
229   //
230   //   smatrix(0,0) = s0; smatrix(1,1)=su2; smatrix(2,2)=st2;
231   //   smatrix(0,1) = su; smatrix(0,2)=st; smatrix(1,2)=sut;
232   //   sums(0,0)    = sv; sums(0,1)=suv; sums(0,2)=svt;
233
234   smatrix(0,0) = fSumXY[0]; smatrix(1,1)=fSumXY[4]; smatrix(2,2)=fSumXY[5];
235   smatrix(0,1) = fSumXY[1]; smatrix(0,2)=fSumXY[3]; smatrix(1,2)=fSumXY[7];
236   sums(0,0)    = fSumXY[2]; sums(0,1)   =fSumXY[6]; sums(0,2)   =fSumXY[8];
237   smatrix.Invert();
238   if (smatrix.IsValid()){
239     for (Int_t i=0;i<3;i++)
240       for (Int_t j=0;j<=i;j++){
241         (*fCov)(i,j)=smatrix(i,j);
242       }
243     TMatrixD  res = sums*smatrix;
244     fParams[0] = res(0,0);
245     fParams[1] = res(0,1);
246     fParams[2] = res(0,2);
247     conv++;
248   }
249   //
250   // XZ part
251   //
252   TMatrixDSym     smatrixz(3);
253   smatrixz(0,0) = fSumXZ[0]; smatrixz(0,1) = fSumXZ[1]; smatrixz(0,2) = fSumXZ[2];
254   smatrixz(1,1) = fSumXZ[2]; smatrixz(1,2) = fSumXZ[3];
255   smatrixz(2,2) = fSumXZ[4];
256   smatrixz.Invert();
257   if (smatrixz.IsValid()){
258     sums(0,0)    = fSumXZ[5];
259     sums(0,1)    = fSumXZ[6];
260     sums(0,2)    = fSumXZ[7];
261     TMatrixD res = sums*smatrixz;
262     fParams[3] = res(0,0);
263     fParams[4] = res(0,1);
264     fParams[5] = res(0,2);
265     for (Int_t i=0;i<3;i++)
266       for (Int_t j=0;j<=i;j++){
267         (*fCov)(i+2,j+2)=smatrixz(i,j);
268     }
269     conv++;
270   }
271
272   //  (x-x0)^2+(y-y0)^2-R^2=0 ===>
273   //
274   //  (x^2+y^2 -2*x*x0 - 2*y*y0+ x0^2 -y0^2 -R^2 =0;  ==>
275   //   substitution t = 1/(x^2+y^2),   u = 2*x*t, y = 2*y*t,  D0 = R^2 - x0^2- y0^2
276   //
277   //  1 - u*x0 - v*y0 - t *D0 =0 ;  - linear equation
278   //     
279   //  next substition   a = 1/y0    b = -x0/y0   c = -D0/y0
280   //  final linear equation :   a + u*b +t*c - v =0;
281   //
282   //  fParam[0]  = 1/y0
283   //  fParam[1]  = -x0/y0
284   //  fParam[2]  = - (R^2 - x0^2 - y0^2)/y0
285   if (conv>1) fConv =kTRUE;
286   else
287     fConv=kFALSE;
288 }
289
290 Double_t AliTrackFitterRieman::GetYat(Double_t x){
291   if (!fConv) return 0.;
292   Double_t res2 = (x*fParams[0]+fParams[1]);
293   res2*=res2;
294   res2 = 1.-fParams[2]*fParams[0]+fParams[1]*fParams[1]-res2;
295   if (res2<0) return 0;
296   res2 = TMath::Sqrt(res2);
297   res2 = (1-res2)/fParams[0];
298   return res2;
299 }
300
301 Double_t AliTrackFitterRieman::GetDYat(Double_t x){
302   if (!fConv) return 0.;
303   Double_t x0 = -fParams[1]/fParams[0];
304   if (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<0) return 0;
305   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
306   if ( 1./(Rm1*Rm1)-(x-x0)*(x-x0)<=0) return 0;
307   Double_t res = (x-x0)/TMath::Sqrt(1./(Rm1*Rm1)-(x-x0)*(x-x0));
308   if (fParams[0]<0) res*=-1.;
309   return res;
310 }
311
312
313
314 Double_t AliTrackFitterRieman::GetZat(Double_t x){
315   if (!fConv) return 0.;
316   Double_t x0 = -fParams[1]/fParams[0];
317   if (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<=0) return 0;
318   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
319   Double_t phi  = TMath::ASin((x-x0)*Rm1);
320   Double_t phi0 = TMath::ASin((0.-x0)*Rm1);
321   Double_t dphi = (phi-phi0);
322   Double_t res = fParams[3]+fParams[4]*dphi/Rm1;
323   return res;
324 }
325
326 Double_t AliTrackFitterRieman::GetDZat(Double_t x){
327   if (!fConv) return 0.;
328   Double_t x0 = -fParams[1]/fParams[0]; 
329   if  (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<=0) return 0;
330   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
331   Double_t res = fParams[4]/TMath::Cos(TMath::ASin((x-x0)*Rm1));
332   return res;
333 }
334
335
336 Double_t AliTrackFitterRieman::GetC(){
337   return fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1);
338 }
339
340 Bool_t AliTrackFitterRieman::GetXYZat(Double_t r, Float_t *xyz){
341   if (!fConv) return kFALSE;
342   Double_t res2 = (r*fParams[0]+fParams[1]);
343   res2*=res2;
344   res2 = 1.-fParams[2]*fParams[0]+fParams[1]*fParams[1]-res2;
345   if (res2<0) return kFALSE;
346   res2 = TMath::Sqrt(res2);
347   res2 = (1-res2)/fParams[0];
348
349   if (!fConv) return kFALSE;
350   Double_t x0 = -fParams[1]/fParams[0];
351   if (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<=0) return 0;
352   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
353   Double_t phi  = TMath::ASin((r-x0)*Rm1);
354   Double_t phi0 = TMath::ASin((0.-x0)*Rm1);
355   Double_t dphi = (phi-phi0);
356   Double_t res = fParams[3]+fParams[4]*dphi/Rm1;
357
358   Double_t sin = TMath::Sin(fAlpha);
359   Double_t cos = TMath::Cos(fAlpha);
360   xyz[0] = r*cos - res2*sin;
361   xyz[1] = res2*cos + r*sin;
362   xyz[2] = res;
363
364   return kTRUE;
365 }
366
367 Bool_t AliTrackFitterRieman::GetPCA(const AliTrackPoint &p, AliTrackPoint &p2) const
368 {
369   // Get the closest to a given spacepoint track trajectory point
370   // Look for details in the description of the Fit() method
371
372   if (!fConv) return kFALSE;
373
374   // First X and Y coordinates
375   Double_t sin = TMath::Sin(fAlpha);
376   Double_t cos = TMath::Cos(fAlpha);
377   //  fParam[0]  = 1/y0
378   //  fParam[1]  = -x0/y0
379   //  fParam[2]  = - (R^2 - x0^2 - y0^2)/y0
380   if (fParams[0] == 0) return kFALSE;
381   Double_t x0 = -fParams[1]/fParams[0]*cos -         1./fParams[0]*sin;
382   Double_t y0 =          1./fParams[0]*cos - fParams[1]/fParams[0]*sin;
383   if ((-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1) <= 0) return kFALSE;
384   Double_t R  = TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1)/
385                 fParams[0];
386
387   Double_t x  = p.GetX();
388   Double_t y  = p.GetY();
389   Double_t dR = TMath::Sqrt((x-x0)*(x-x0)+(y-y0)*(y-y0));
390   Double_t xprime = TMath::Abs(R)*(x-x0)/dR + x0;
391   Double_t yprime = TMath::Abs(R)*(y-y0)/dR + y0;
392
393   // Now Z coordinate
394   Double_t phi  = TMath::ASin((x-x0)/R);
395   Double_t phi0 = TMath::ASin((0.-x0)/R);
396   Double_t dphi = (phi-phi0);
397   Double_t zprime = fParams[3]+fParams[4]*dphi*R;
398
399   p2.SetXYZ(xprime,yprime,zprime);
400
401   return kTRUE;
402 }