New version of alignment framework.
[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   fSumYY = 0;
16   for (Int_t i=0;i<9;i++) fSumXZ[i] = 0;
17   fSumZZ = 0;
18   fNUsed = 0;
19   fConv = kFALSE;
20 }
21
22
23 AliTrackFitterRieman::AliTrackFitterRieman(AliTrackPointArray *array, Bool_t owner):
24   AliTrackFitter(array,owner)
25 {
26   //
27   // Constructor
28   //
29   fAlpha = 0.;
30   for (Int_t i=0;i<9;i++) fSumXY[i] = 0;
31   fSumYY = 0;
32   for (Int_t i=0;i<9;i++) fSumXZ[i] = 0;
33   fSumZZ = 0;
34   fNUsed = 0;
35   fConv = kFALSE;
36 }
37
38 AliTrackFitterRieman::AliTrackFitterRieman(const AliTrackFitterRieman &rieman):
39   AliTrackFitter(rieman)
40 {
41   //
42   // copy constructor
43   //
44   fAlpha = rieman.fAlpha;
45   for (Int_t i=0;i<9;i++) fSumXY[i]  = rieman.fSumXY[i];
46   fSumYY = rieman.fSumYY;
47   for (Int_t i=0;i<9;i++) fSumXZ[i]  = rieman.fSumXZ[i];
48   fSumZZ = rieman.fSumZZ;
49   fNUsed = rieman.fNUsed;
50   fConv = rieman.fConv;
51 }
52
53 //_____________________________________________________________________________
54 AliTrackFitterRieman &AliTrackFitterRieman::operator =(const AliTrackFitterRieman& rieman)
55 {
56   // assignment operator
57   //
58   if(this==&rieman) return *this;
59   ((AliTrackFitter *)this)->operator=(rieman);
60
61   fAlpha = rieman.fAlpha;
62   for (Int_t i=0;i<9;i++) fSumXY[i]  = rieman.fSumXY[i];
63   fSumYY = rieman.fSumYY;
64   for (Int_t i=0;i<9;i++) fSumXZ[i]  = rieman.fSumXZ[i];
65   fSumZZ = rieman.fSumZZ;
66   fNUsed = rieman.fNUsed;
67   fConv = rieman.fConv;
68
69   return *this;
70 }
71
72 AliTrackFitterRieman::~AliTrackFitterRieman()
73 {
74   // destructor
75   //
76 }
77
78 void AliTrackFitterRieman::Reset()
79 {
80   // Reset the track parameters and
81   // rieman sums
82   AliTrackFitter::Reset();
83   fAlpha = 0.;
84   for (Int_t i=0;i<9;i++) fSumXY[i] = 0;
85   fSumYY = 0;
86   for (Int_t i=0;i<9;i++) fSumXZ[i] = 0;
87   fSumZZ = 0;
88   fNUsed = 0;
89   fConv =kFALSE;
90 }
91
92 Bool_t AliTrackFitterRieman::Fit(UShort_t volId,UShort_t volIdFit,
93                                  AliAlignObj::ELayerID layerRangeMin,
94                                  AliAlignObj::ELayerID layerRangeMax)
95 {
96   // Fit the track points. The method takes as an input
97   // the id (volid) of the volume to be skipped from fitting.
98   // The following two parameters are used to define the
99   // range of volumes to be used in the fitting
100   // As a result two AliTrackPointArray's obects are filled.
101   // The first one contains the space points with
102   // volume id = volid. The second array of points represents
103   // the track extrapolation corresponding to the space points
104   // in the first array. The two arrays can be used to find
105   // the residuals in the volid and consequently construct a
106   // chi2 function to be minimized during the alignment
107   // procedures. For the moment the track extrapolation is taken
108   // as follows: in XY plane - at the CDA between track circle
109   // and the space point; in Z - the track extrapolation on the Z
110   // plane defined by the space point.
111
112   Reset();
113
114   Int_t npoints = fPoints->GetNPoints();
115   if (npoints < 3) return kFALSE;
116
117   AliTrackPoint p,plocal;
118   fPoints->GetPoint(p,0);
119   fAlpha = TMath::ATan2(p.GetY(),p.GetX());
120
121   Int_t npVolId = 0;
122   fNUsed = 0;
123   Int_t *pindex = new Int_t[npoints];
124   for (Int_t ipoint = 0; ipoint < npoints; ipoint++)
125     {
126       fPoints->GetPoint(p,ipoint);
127       UShort_t iVolId = p.GetVolumeID();
128       if (iVolId == volId) {
129         pindex[npVolId] = ipoint;
130         npVolId++;
131       }
132       if (volIdFit != 0) {
133         if (iVolId != volIdFit) continue;
134       }
135       else {
136         if (iVolId < AliAlignObj::LayerToVolUID(layerRangeMin,0) ||
137             iVolId > AliAlignObj::LayerToVolUID(layerRangeMax,
138                                                 AliAlignObj::LayerSize(layerRangeMax-
139                                                                        AliAlignObj::kFirstLayer))) continue;
140       }
141       plocal = p.Rotate(fAlpha);
142       AddPoint(plocal.GetX(),plocal.GetY(),plocal.GetZ(),
143                TMath::Sqrt(plocal.GetCov()[3]),TMath::Sqrt(plocal.GetCov()[5]));
144       fNUsed++;
145     }
146
147   if (fNUsed < 3) {
148     delete [] pindex;
149     return kFALSE;
150   }
151
152   Update();
153
154   if (!fConv) {
155     delete [] pindex;
156     return kFALSE;
157   }
158
159   if ((fParams[0] == 0) ||
160       ((-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1) <= 0)) {
161     delete [] pindex;
162     return kFALSE;
163   }
164
165   fPVolId = new AliTrackPointArray(npVolId);
166   fPTrack = new AliTrackPointArray(npVolId);
167   AliTrackPoint p2;
168   for (Int_t ipoint = 0; ipoint < npVolId; ipoint++)
169     {
170       Int_t index = pindex[ipoint];
171       fPoints->GetPoint(p,index);
172       if (GetPCA(p,p2)) {
173         fPVolId->AddPoint(ipoint,&p);
174         fPTrack->AddPoint(ipoint,&p2);
175       }
176     }  
177
178   delete [] pindex;
179
180   // debug info
181 //   Float_t chi2 = 0, chi22 = 0;
182 //   for (Int_t ipoint = 0; ipoint < npoints; ipoint++)
183 //     {
184 //       fPoints->GetPoint(p,ipoint);
185 //       UShort_t iVolId = p.GetVolumeID();
186 //       if (volIdFit != 0) {
187 //      if (iVolId != volIdFit) continue;
188 //       }
189 //       else {
190 //      if (iVolId < AliAlignObj::LayerToVolUID(layerRangeMin,0) ||
191 //          iVolId > AliAlignObj::LayerToVolUID(layerRangeMax,AliAlignObj::LayerSize(layerRangeMax-
192 //                                                                                   AliAlignObj::kFirstLayer))) continue;
193 //       }
194 //       plocal = p.Rotate(fAlpha);
195 //       Float_t delta = (fParams[0]*(plocal.GetX()*plocal.GetX()+plocal.GetY()*plocal.GetY())+
196 //                     2.*plocal.GetX()*fParams[1]+
197 //                     fParams[2]-
198 //                     2.*plocal.GetY())/
199 //                    (2.*TMath::Sqrt(plocal.GetCov()[3]));
200 // //       Float_t delta2 = (fParams[3]+
201 // //                  plocal.GetX()*fParams[4]+
202 // //                  plocal.GetX()*plocal.GetX()*fParams[5]-
203 // //                  plocal.GetZ())/
204 // //                 (TMath::Sqrt(plocal.GetCov()[5]));
205 //       Double_t r = TMath::Sqrt(plocal.GetX()*plocal.GetX()+plocal.GetY()*plocal.GetY());
206 //       Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
207 //       Float_t delta2 = (fParams[3]+
208 //                      r*fParams[4]+r*r*r*fParams[4]*Rm1*Rm1/24-
209 //                      plocal.GetZ())/
210 //                     (TMath::Sqrt(plocal.GetCov()[5]));
211 //       chi2 += delta*delta;
212 //       chi22 += delta2*delta2;
213 //       //      printf("pulls %d %d %f %f\n",ipoint,iVolId,delta,delta2);
214       
215 //     }
216 //   printf("My chi2 = %f + %f = %f\n",chi2,chi22,chi2+chi22);
217
218   return kTRUE;
219 }
220
221 void AliTrackFitterRieman::AddPoint(Float_t x, Float_t y, Float_t z, Float_t sy, Float_t sz)
222 {
223   //
224   //  Rieman update
225   //
226   //------------------------------------------------------
227   // XY direction
228   //
229   //  (x-x0)^2+(y-y0)^2-R^2=0 ===>
230   //
231   //  (x^2+y^2 -2*x*x0 - 2*y*y0+ x0^2 -y0^2 -R^2 =0;  ==>
232   //
233   //   substitution t = 1/(x^2+y^2),   u = 2*x*t, v = 2*y*t,  D0 = R^2 - x0^2- y0^2
234   //
235   //  1 - u*x0 - v*y0 - t *D0 =0 ;  - linear equation
236   //     
237   //  next substition   a = 1/y0    b = -x0/y0   c = -D0/y0
238   //
239   //  final linear equation :   a + u*b +t*c - v =0;
240   //
241   // Minimization :
242   //
243   // sum( (a + ui*b +ti*c - vi)^2)/(sigmai)^2 = min;
244   //
245   // where sigmai is the error of  maesurement  (a + ui*b +ti*c - vi)
246   //
247   // neglecting error of xi, and supposing  xi>>yi    sigmai ~ sigmaVi ~ 2*sigmay*t  
248   //
249   //
250   // XY part
251   //
252   Double_t  t  =  x*x+y*y;
253   if (t<2) return;
254   t            = 1./t;
255   Double_t  u  =  2.*x*t;
256   Double_t  v  =  2.*y*t;
257   Double_t  error = 2.*sy*t;
258   error *=error;
259   Double_t weight = 1./error;
260   fSumXY[0] +=weight;
261   fSumXY[1] +=u*weight;      fSumXY[2]+=v*weight;  fSumXY[3]+=t*weight;
262   fSumXY[4] +=u*u*weight;    fSumXY[5]+=t*t*weight;
263   fSumXY[6] +=u*v*weight;    fSumXY[7]+=u*t*weight; fSumXY[8]+=v*t*weight;
264   fSumYY += v*v*weight;
265   //
266   // XZ part
267   //
268   if (0) {
269     weight = 1./(sz*sz);
270     fSumXZ[0] +=weight;
271     fSumXZ[1] +=weight*x;   fSumXZ[2] +=weight*x*x; fSumXZ[3] +=weight*x*x*x; fSumXZ[4] += weight*x*x*x*x;
272     fSumXZ[5] +=weight*z;   fSumXZ[6] +=weight*x*z; fSumXZ[7] +=weight*x*x*z;
273     fSumZZ += z*z*weight;
274   }
275   else {
276     weight = 1./(sz*sz);
277     fSumXZ[0] +=weight;
278     Double_t r = TMath::Sqrt(x*x+y*y);
279     fSumXZ[1] +=weight*r;   fSumXZ[2] +=weight*r*r; fSumXZ[3] +=weight*z; fSumXZ[4] += weight*r*z;
280     // Now the auxulary sums
281     fSumXZ[5] +=weight*r*r*r/24; fSumXZ[6] +=weight*r*r*r*r/12; fSumXZ[7] +=weight*r*r*r*z/24;
282     fSumXZ[8] +=weight*r*r*r*r*r*r/(24*24);
283     fSumZZ += z*z*weight;
284   }
285 }
286
287 void AliTrackFitterRieman::Update(){
288   //
289   //  Rieman update
290   //
291   //
292   for (Int_t i=0;i<6;i++)fParams[i]=0;
293   fChi2 = 0;
294   fNdf = 0;
295   Int_t conv=0;
296   //
297   // XY part
298   //
299   TMatrixDSym     smatrix(3);
300   TMatrixD        sums(1,3);
301   //
302   //   smatrix(0,0) = s0; smatrix(1,1)=su2; smatrix(2,2)=st2;
303   //   smatrix(0,1) = su; smatrix(0,2)=st; smatrix(1,2)=sut;
304   //   sums(0,0)    = sv; sums(0,1)=suv; sums(0,2)=svt;
305
306   smatrix(0,0) = fSumXY[0]; smatrix(1,1)=fSumXY[4]; smatrix(2,2)=fSumXY[5];
307   smatrix(0,1) = fSumXY[1]; smatrix(0,2)=fSumXY[3]; smatrix(1,2)=fSumXY[7];
308   sums(0,0)    = fSumXY[2]; sums(0,1)   =fSumXY[6]; sums(0,2)   =fSumXY[8];
309   smatrix.Invert();
310   if (smatrix.IsValid()){
311     for (Int_t i=0;i<3;i++)
312       for (Int_t j=0;j<=i;j++){
313         (*fCov)(i,j)=smatrix(i,j);
314       }
315     TMatrixD  res = sums*smatrix;
316     fParams[0] = res(0,0);
317     fParams[1] = res(0,1);
318     fParams[2] = res(0,2);
319     TMatrixD  tmp = res*sums.T();
320     fChi2 += fSumYY - tmp(0,0);
321     fNdf  += fNUsed - 3;
322     conv++;
323   }
324   //
325   // XZ part
326   //
327   if (0) {
328     TMatrixDSym     smatrixz(3);
329     smatrixz(0,0) = fSumXZ[0]; smatrixz(0,1) = fSumXZ[1]; smatrixz(0,2) = fSumXZ[2];
330     smatrixz(1,1) = fSumXZ[2]; smatrixz(1,2) = fSumXZ[3];
331     smatrixz(2,2) = fSumXZ[4];
332     smatrixz.Invert();
333     TMatrixD        sumsxz(1,3);
334     if (smatrixz.IsValid()){
335       sumsxz(0,0)    = fSumXZ[5];
336       sumsxz(0,1)    = fSumXZ[6];
337       sumsxz(0,2)    = fSumXZ[7];
338       TMatrixD res = sumsxz*smatrixz;
339       fParams[3] = res(0,0);
340       fParams[4] = res(0,1);
341       fParams[5] = res(0,2);
342       for (Int_t i=0;i<3;i++)
343         for (Int_t j=0;j<=i;j++){
344           (*fCov)(i+2,j+2)=smatrixz(i,j);
345         }
346       TMatrixD  tmp = res*sumsxz.T();
347       fChi2 += fSumZZ - tmp(0,0);
348       fNdf  += fNUsed - 3;
349       conv++;
350     }
351   }
352   else {
353     Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
354     fSumXZ[1] += fSumXZ[5]*Rm1*Rm1;
355     fSumXZ[2] += fSumXZ[6]*Rm1*Rm1 + fSumXZ[8]*Rm1*Rm1*Rm1*Rm1;
356     fSumXZ[4] += fSumXZ[7]*Rm1*Rm1;
357
358     TMatrixDSym     smatrixz(2);
359     smatrixz(0,0) = fSumXZ[0]; smatrixz(0,1) = fSumXZ[1]; smatrixz(1,1) = fSumXZ[2];
360     smatrixz.Invert();
361     TMatrixD        sumsxz(1,2);
362     if (smatrixz.IsValid()){
363       sumsxz(0,0)    = fSumXZ[3];
364       sumsxz(0,1)    = fSumXZ[4];
365       TMatrixD res = sumsxz*smatrixz;
366       fParams[3] = res(0,0);
367       fParams[4] = res(0,1);
368       fParams[5] = 0;
369       for (Int_t i=0;i<2;i++)
370         for (Int_t j=0;j<=i;j++){
371           (*fCov)(i+3,j+3)=smatrixz(i,j);
372         }
373       TMatrixD  tmp = res*sumsxz.T();
374       fChi2 += fSumZZ - tmp(0,0);
375       fNdf  += fNUsed - 2;
376       conv++;
377     }
378   }
379
380   //  (x-x0)^2+(y-y0)^2-R^2=0 ===>
381   //
382   //  (x^2+y^2 -2*x*x0 - 2*y*y0+ x0^2 -y0^2 -R^2 =0;  ==>
383   //   substitution t = 1/(x^2+y^2),   u = 2*x*t, y = 2*y*t,  D0 = R^2 - x0^2- y0^2
384   //
385   //  1 - u*x0 - v*y0 - t *D0 =0 ;  - linear equation
386   //     
387   //  next substition   a = 1/y0    b = -x0/y0   c = -D0/y0
388   //  final linear equation :   a + u*b +t*c - v =0;
389   //
390   //  fParam[0]  = 1/y0
391   //  fParam[1]  = -x0/y0
392   //  fParam[2]  = - (R^2 - x0^2 - y0^2)/y0
393   if (conv>1) fConv =kTRUE;
394   else
395     fConv=kFALSE;
396 }
397
398 Double_t AliTrackFitterRieman::GetYat(Double_t x){
399   if (!fConv) return 0.;
400   Double_t res2 = (x*fParams[0]+fParams[1]);
401   res2*=res2;
402   res2 = 1.-fParams[2]*fParams[0]+fParams[1]*fParams[1]-res2;
403   if (res2<0) return 0;
404   res2 = TMath::Sqrt(res2);
405   res2 = (1-res2)/fParams[0];
406   return res2;
407 }
408
409 Double_t AliTrackFitterRieman::GetDYat(Double_t x){
410   if (!fConv) return 0.;
411   Double_t x0 = -fParams[1]/fParams[0];
412   if (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<0) return 0;
413   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
414   if ( 1./(Rm1*Rm1)-(x-x0)*(x-x0)<=0) return 0;
415   Double_t res = (x-x0)/TMath::Sqrt(1./(Rm1*Rm1)-(x-x0)*(x-x0));
416   if (fParams[0]<0) res*=-1.;
417   return res;
418 }
419
420
421
422 Double_t AliTrackFitterRieman::GetZat(Double_t x){
423   if (!fConv) return 0.;
424   Double_t x0 = -fParams[1]/fParams[0];
425   if (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<=0) return 0;
426   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
427   Double_t phi  = TMath::ASin((x-x0)*Rm1);
428   Double_t phi0 = TMath::ASin((0.-x0)*Rm1);
429   Double_t dphi = (phi-phi0);
430   Double_t res = fParams[3]+fParams[4]*dphi/Rm1;
431   return res;
432 }
433
434 Double_t AliTrackFitterRieman::GetDZat(Double_t x){
435   if (!fConv) return 0.;
436   Double_t x0 = -fParams[1]/fParams[0]; 
437   if  (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<=0) return 0;
438   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
439   Double_t res = fParams[4]/TMath::Cos(TMath::ASin((x-x0)*Rm1));
440   return res;
441 }
442
443
444 Double_t AliTrackFitterRieman::GetC(){
445   return fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1);
446 }
447
448 Bool_t AliTrackFitterRieman::GetXYZat(Double_t r, Float_t *xyz){
449   if (!fConv) return kFALSE;
450   Double_t res2 = (r*fParams[0]+fParams[1]);
451   res2*=res2;
452   res2 = 1.-fParams[2]*fParams[0]+fParams[1]*fParams[1]-res2;
453   if (res2<0) return kFALSE;
454   res2 = TMath::Sqrt(res2);
455   res2 = (1-res2)/fParams[0];
456
457   if (!fConv) return kFALSE;
458   Double_t x0 = -fParams[1]/fParams[0];
459   if (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<=0) return 0;
460   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
461   Double_t phi  = TMath::ASin((r-x0)*Rm1);
462   Double_t phi0 = TMath::ASin((0.-x0)*Rm1);
463   Double_t dphi = (phi-phi0);
464   Double_t res = fParams[3]+fParams[4]*dphi/Rm1;
465
466   Double_t sin = TMath::Sin(fAlpha);
467   Double_t cos = TMath::Cos(fAlpha);
468   xyz[0] = r*cos - res2*sin;
469   xyz[1] = res2*cos + r*sin;
470   xyz[2] = res;
471
472   return kTRUE;
473 }
474
475 Bool_t AliTrackFitterRieman::GetPCA(const AliTrackPoint &p, AliTrackPoint &p2) const
476 {
477   // Get the closest to a given spacepoint track trajectory point
478   // Look for details in the description of the Fit() method
479
480   if (!fConv) return kFALSE;
481
482   // First X and Y coordinates
483   Double_t sin = TMath::Sin(fAlpha);
484   Double_t cos = TMath::Cos(fAlpha);
485   //  fParam[0]  = 1/y0
486   //  fParam[1]  = -x0/y0
487   //  fParam[2]  = - (R^2 - x0^2 - y0^2)/y0
488   if (fParams[0] == 0) return kFALSE;
489   Double_t x0 = -fParams[1]/fParams[0]*cos -         1./fParams[0]*sin;
490   Double_t y0 =          1./fParams[0]*cos - fParams[1]/fParams[0]*sin;
491   if ((-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1) <= 0) return kFALSE;
492   Double_t R  = TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1)/
493                 fParams[0];
494
495   Double_t x  = p.GetX();
496   Double_t y  = p.GetY();
497   Double_t dR = TMath::Sqrt((x-x0)*(x-x0)+(y-y0)*(y-y0));
498   Double_t xprime = TMath::Abs(R)*(x-x0)/dR + x0;
499   Double_t yprime = TMath::Abs(R)*(y-y0)/dR + y0;
500
501   // Now Z coordinate
502   Double_t x2 = xprime*cos + yprime*sin;
503   Double_t x02 = -fParams[1]/fParams[0];
504   Double_t phi  = TMath::ASin((x2-x02)/R);
505   Double_t phi0 = TMath::ASin((0.-x02)/R);
506   Double_t dphi = (phi-phi0);
507   Double_t zprime = fParams[3]+fParams[4]*dphi*R;
508
509   p2.SetXYZ(xprime,yprime,zprime);
510
511   return kTRUE;
512 }