Fixes for bug #52499: Field polarities inconsistiency
[u/mrichter/AliRoot.git] / STEER / AliTrackFitterStraight.cxx
1 /**************************************************************************
2  * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
3  *                                                                        *
4  * Author: The ALICE Off-line Project.                                    *
5  * Contributors are mentioned in the code where appropriate.              *
6  *                                                                        *
7  * Permission to use, copy, modify and distribute this software and its   *
8  * documentation strictly for non-commercial purposes is hereby granted   *
9  * without fee, provided that the above copyright notice appears in all   *
10  * copies and that both the copyright notice and this permission notice   *
11  * appear in the supporting documentation. The authors make no claims     *
12  * about the suitability of this software for any purpose. It is          *
13  * provided "as is" without express or implied warranty.                  *
14  **************************************************************************/
15
16 /* $Id$ */
17
18 #include <TMath.h>
19 #include <TMatrixDSym.h>
20 #include <TMatrixD.h>
21
22 #include "AliTrackFitterStraight.h"
23
24 ClassImp(AliTrackFitterStraight)
25
26 AliTrackFitterStraight::AliTrackFitterStraight():
27   AliTrackFitter(),
28   fAlpha(0.),
29   fSumYY(0),
30   fSumZZ(0),
31   fNUsed(0),
32   fConv(kFALSE)
33 {
34   //
35   // default constructor
36   //
37   for (Int_t i=0;i<5;i++) fSumXY[i] = 0;
38   for (Int_t i=0;i<5;i++) fSumXZ[i] = 0;
39 }
40
41
42 AliTrackFitterStraight::AliTrackFitterStraight(AliTrackPointArray *array, Bool_t owner):
43   AliTrackFitter(array,owner),
44   fAlpha(0.),
45   fSumYY(0),
46   fSumZZ(0),
47   fNUsed(0),
48   fConv(kFALSE)
49 {
50   //
51   // Constructor
52   //
53   for (Int_t i=0;i<5;i++) fSumXY[i] = 0;
54   for (Int_t i=0;i<5;i++) fSumXZ[i] = 0;
55 }
56
57 AliTrackFitterStraight::AliTrackFitterStraight(const AliTrackFitterStraight &fitter):
58   AliTrackFitter(fitter),
59   fAlpha(fitter.fAlpha),
60   fSumYY(fitter.fSumYY),
61   fSumZZ(fitter.fSumZZ),
62   fNUsed(fitter.fNUsed),
63   fConv(fitter.fConv)
64 {
65   //
66   // copy constructor
67   //
68   for (Int_t i=0;i<5;i++) fSumXY[i]  = fitter.fSumXY[i];
69   for (Int_t i=0;i<5;i++) fSumXZ[i]  = fitter.fSumXZ[i];
70 }
71
72 //_____________________________________________________________________________
73 AliTrackFitterStraight &AliTrackFitterStraight::operator =(const AliTrackFitterStraight& fitter)
74 {
75   // assignment operator
76   //
77   if(this==&fitter) return *this;
78   ((AliTrackFitter *)this)->operator=(fitter);
79
80   fAlpha = fitter.fAlpha;
81   for (Int_t i=0;i<5;i++) fSumXY[i]  = fitter.fSumXY[i];
82   fSumYY = fitter.fSumYY;
83   for (Int_t i=0;i<5;i++) fSumXZ[i]  = fitter.fSumXZ[i];
84   fSumZZ = fitter.fSumZZ;
85   fNUsed = fitter.fNUsed;
86   fConv = fitter.fConv;
87
88   return *this;
89 }
90
91 AliTrackFitterStraight::~AliTrackFitterStraight()
92 {
93   // destructor
94   //
95 }
96
97 void AliTrackFitterStraight::Reset()
98 {
99   // Reset the track parameters and
100   // sums
101   AliTrackFitter::Reset();
102   fAlpha = 0.;
103   for (Int_t i=0;i<5;i++) fSumXY[i] = 0;
104   fSumYY = 0;
105   for (Int_t i=0;i<5;i++) fSumXZ[i] = 0;
106   fSumZZ = 0;
107   fNUsed = 0;
108   fConv =kFALSE;
109 }
110
111 Bool_t AliTrackFitterStraight::Fit(const TArrayI *volIds,const TArrayI *volIdsFit,
112                                  AliGeomManager::ELayerID layerRangeMin,
113                                  AliGeomManager::ELayerID layerRangeMax)
114 {
115   // Fit the track points. The method takes as an input
116   // the set of id's (volids) of the volumes in which
117   // one wants to calculate the residuals.
118   // The following parameters are used to define the
119   // range of volumes to be used in the fitting
120   // As a result two AliTrackPointArray's obects are filled.
121   // The first one contains the space points with
122   // volume id's from volids list. The second array of points represents
123   // the track extrapolations corresponding to the space points
124   // in the first array. The two arrays can be used to find
125   // the residuals in the volids and consequently construct a
126   // chi2 function to be minimized during the alignment
127   // procedures. For the moment the track extrapolation is taken
128   // at the space-point reference plane. The reference plane is
129   // found using the covariance matrix of the point
130   // (assuming sigma(x)=0 at the reference coordinate system.
131
132   Reset();
133
134   Int_t npoints = fPoints->GetNPoints();
135   if (npoints < 2) return kFALSE;
136
137   Bool_t isAlphaCalc = kFALSE;
138   AliTrackPoint p,plocal;
139
140   Int_t npVolId = 0;
141   fNUsed = 0;
142   Int_t *pindex = new Int_t[npoints];
143   for (Int_t ipoint = 0; ipoint < npoints; ipoint++)
144     {
145       fPoints->GetPoint(p,ipoint);
146       UShort_t iVolId = p.GetVolumeID();
147       if (FindVolId(volIds,iVolId)) {
148         pindex[npVolId] = ipoint;
149         npVolId++;
150       }
151       if (volIdsFit != 0x0) {
152         if (!FindVolId(volIdsFit,iVolId)) continue;
153       }
154       else {
155         if (iVolId < AliGeomManager::LayerToVolUID(layerRangeMin,0) ||
156             iVolId > AliGeomManager::LayerToVolUID(layerRangeMax,
157                                                 AliGeomManager::LayerSize(layerRangeMax))) continue;
158       }
159       if (!isAlphaCalc) {
160         fAlpha = p.GetAngle();
161         isAlphaCalc = kTRUE;
162       }
163       plocal = p.Rotate(fAlpha);
164       AddPoint(plocal.GetX(),plocal.GetY(),plocal.GetZ(),
165                TMath::Sqrt(plocal.GetCov()[3]),TMath::Sqrt(plocal.GetCov()[5]));
166       fNUsed++;
167     }
168
169   if (fNUsed < 2) {
170     delete [] pindex;
171     return kFALSE;
172   }
173
174   Update();
175
176   if (!fConv) {
177     delete [] pindex;
178     return kFALSE;
179   }
180
181   if (fNUsed < fMinNPoints ) {
182     delete [] pindex;
183     return kFALSE;
184   }
185
186   fPVolId = new AliTrackPointArray(npVolId);
187   fPTrack = new AliTrackPointArray(npVolId);
188   AliTrackPoint p2;
189   for (Int_t ipoint = 0; ipoint < npVolId; ipoint++)
190     {
191       Int_t index = pindex[ipoint];
192       fPoints->GetPoint(p,index);
193       if (GetPCA(p,p2)) {
194         Float_t xyz[3],xyz2[3];
195         p.GetXYZ(xyz); p2.GetXYZ(xyz2);
196         //      printf("residuals %f %d %d %f %f %f %f %f %f\n",fChi2,fNUsed,fConv,xyz[0],xyz[1],xyz[2],xyz2[0]-xyz[0],xyz2[1]-xyz[1],xyz2[2]-xyz[2]);
197         fPVolId->AddPoint(ipoint,&p);
198         fPTrack->AddPoint(ipoint,&p2);
199       }
200     }  
201
202   delete [] pindex;
203
204   return kTRUE;
205 }
206
207 void AliTrackFitterStraight::AddPoint(Float_t x, Float_t y, Float_t z, Float_t sy, Float_t sz)
208 {
209   // Straight track fitter
210   // The method add a point to the sums
211   // used to extract track parameters
212
213   //
214   // XY part
215   //
216   Double_t weight = 1./(sy*sy);
217   fSumXY[0] +=weight;
218   fSumXY[1] +=x*weight;      fSumXY[2] +=x*x*weight;
219   fSumXY[3] +=y*weight;      fSumXY[4] +=x*y*weight;
220   fSumYY += y*y*weight;
221   //
222   // XZ part
223   //
224   weight = 1./(sz*sz);
225   fSumXZ[0] +=weight;
226   fSumXZ[1] +=x*weight;      fSumXZ[2] +=x*x*weight;
227   fSumXZ[3] +=z*weight;      fSumXZ[4] +=x*z*weight;
228   fSumZZ += z*z*weight;
229 }
230
231 Bool_t AliTrackFitterStraight::Update(){
232   //
233   //  Track fitter update
234   //
235   //
236   for (Int_t i=0;i<6;i++)fParams[i]=0;
237   fChi2 = 0;
238   fNdf = 0;
239   Int_t conv=0;
240   //
241   // XY part
242   //
243   TMatrixDSym     smatrix(2);
244   TMatrixD        sums(1,2);
245   smatrix(0,0) = fSumXY[0]; smatrix(1,1)=fSumXY[2];
246   smatrix(0,1) = fSumXY[1]; smatrix(1,0)=fSumXY[1];
247   sums(0,0)    = fSumXY[3]; sums(0,1)   =fSumXY[4];
248   smatrix.Invert();
249   if (smatrix.IsValid()){
250     for (Int_t i=0;i<2;i++)
251       for (Int_t j=0;j<=i;j++){
252         (*fCov)(i,j)=smatrix(i,j);
253       }
254     TMatrixD  res = sums*smatrix;
255     fParams[0] = res(0,0);
256     fParams[1] = res(0,1);
257     TMatrixD  tmp = res*sums.T();
258     fChi2 += fSumYY - tmp(0,0);
259     fNdf  += fNUsed - 2;
260     conv++;
261   }
262   //
263   // XZ part
264   //
265   TMatrixDSym     smatrixz(2);
266   TMatrixD        sumsxz(1,2);
267   smatrixz(0,0) = fSumXZ[0]; smatrixz(1,1) = fSumXZ[2];
268   smatrixz(0,1) = fSumXZ[1]; smatrixz(1,0) = fSumXZ[1];
269   sumsxz(0,0)   = fSumXZ[3]; sumsxz(0,1)   = fSumXZ[4];
270   smatrixz.Invert();
271   if (smatrixz.IsValid()){
272     TMatrixD res = sumsxz*smatrixz;
273     fParams[2] = res(0,0);
274     fParams[3] = res(0,1);
275     fParams[4] = fParams[5] = 0;
276     for (Int_t i=0;i<2;i++)
277       for (Int_t j=0;j<=i;j++){
278         (*fCov)(i+2,j+2)=smatrixz(i,j);
279       }
280     TMatrixD  tmp = res*sumsxz.T();
281     fChi2 += fSumZZ - tmp(0,0);
282     fNdf  += fNUsed - 2;
283     conv++;
284   }
285
286   if (conv>1)
287     fConv =kTRUE;
288   else
289     fConv=kFALSE;
290
291   return kTRUE;
292 }
293
294 Double_t AliTrackFitterStraight::GetYat(Double_t x) const {
295   if (!fConv) return 0.;
296   return (fParams[0]+x*fParams[1]);
297 }
298
299 Double_t AliTrackFitterStraight::GetDYat(Double_t x) const {
300   if (!fConv) return 0.;
301   return fParams[1]+0.*x;
302 }
303
304
305
306 Double_t AliTrackFitterStraight::GetZat(Double_t x) const {
307   if (!fConv) return 0.;
308   return (fParams[2]+x*fParams[3]);
309 }
310
311 Double_t AliTrackFitterStraight::GetDZat(Double_t x) const {
312   if (!fConv) return 0.;
313   return fParams[3]+0.*x;
314 }
315
316 Bool_t AliTrackFitterStraight::GetXYZat(Double_t r, Float_t *xyz) const {
317   if (!fConv) return kFALSE;
318   Double_t y = (fParams[0]+r*fParams[1]);
319   Double_t z = (fParams[2]+r*fParams[3]);
320
321   Double_t sin = TMath::Sin(fAlpha);
322   Double_t cos = TMath::Cos(fAlpha);
323   xyz[0] = r*cos - y*sin;
324   xyz[1] = y*cos + r*sin;
325   xyz[2] = z;
326
327   return kTRUE;
328 }
329
330 Bool_t AliTrackFitterStraight::GetPCA(const AliTrackPoint &p, AliTrackPoint &p2) const
331 {
332   // Get the closest to a given spacepoint track trajectory point
333   // Look for details in the description of the Fit() method
334
335   if (!fConv) return kFALSE;
336
337   // First X and Y coordinates
338   Double_t sin = TMath::Sin(fAlpha);
339   Double_t cos = TMath::Cos(fAlpha);
340   // Track parameters in the global coordinate system
341   Double_t x0 = -fParams[0]*sin;
342   Double_t y0 =  fParams[0]*cos;
343   if ((cos - fParams[1]*sin) == 0) return kFALSE;
344   Double_t dydx = (fParams[1]*cos + sin)/(cos - fParams[1]*sin);
345
346   // Define space-point refence plane
347   Double_t alphap = p.GetAngle();
348   Double_t sinp = TMath::Sin(alphap);
349   Double_t cosp = TMath::Cos(alphap);
350   Double_t x  = p.GetX()*cosp + p.GetY()*sinp;
351   //  Double_t y  = p.GetY()*cosp - p.GetX()*sinp;
352   Double_t x0p= x0*cosp + y0*sinp;
353   Double_t y0p= y0*cosp - x0*sinp;
354   if ((cos + dydx*sin) == 0) return kFALSE;
355   Double_t dydxp = (dydx*cos - sin)/(cos + dydx*sin);
356   Double_t yprime = y0p + dydxp*(x-x0p);
357
358   // Back to the global coordinate system
359   Double_t xsecond = x*cosp - yprime*sinp;
360   Double_t ysecond = yprime*cosp + x*sinp;
361
362   // Now Z coordinate and track angles
363   Double_t x2 = xsecond*cos + ysecond*sin;
364   Double_t zsecond = GetZat(x2);
365   Double_t dydx2 = fParams[1];
366   Double_t dzdx = fParams[3];
367
368   // Fill the cov matrix of the track extrapolation point
369   Double_t cov[6] = {0,0,0,0,0,0};
370   Double_t sigmax = 100*100.;
371   cov[0] = sigmax;             cov[1] = sigmax*dydx2;      cov[2] = sigmax*dzdx;
372   cov[3] = sigmax*dydx2*dydx2; cov[4] = sigmax*dydx2*dzdx;
373   cov[5] = sigmax*dzdx*dzdx;
374
375   Float_t  newcov[6];
376   newcov[0] = cov[0]*cos*cos-
377             2*cov[1]*sin*cos+
378               cov[3]*sin*sin;
379   newcov[1] = cov[1]*(cos*cos-sin*sin)-
380              (cov[3]-cov[0])*sin*cos;
381   newcov[2] = cov[2]*cos-
382               cov[4]*sin;
383   newcov[3] = cov[0]*sin*sin+
384             2*cov[1]*sin*cos+
385               cov[3]*cos*cos;
386   newcov[4] = cov[4]*cos+
387               cov[2]*sin;
388   newcov[5] = cov[5];
389
390   p2.SetXYZ(xsecond,ysecond,zsecond,newcov);
391
392   return kTRUE;
393 }