New version of alignment framework.
[u/mrichter/AliRoot.git] / STEER / AliTrackPointArray.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 //////////////////////////////////////////////////////////////////////////////
17 //                          Class AliTrackPointArray                        //
18 //   This class contains the ESD track space-points which are used during   //
19 //   the alignment procedures. Each space-point consist of 3 coordinates    //
20 //   (and their errors) and the index of the sub-detector which contains    //
21 //   the space-point.                                                       //
22 //   cvetan.cheshkov@cern.ch 3/11/2005                                      //
23 //////////////////////////////////////////////////////////////////////////////
24
25 #include <TMath.h>
26 #include <TMatrixDSym.h>
27
28 #include "AliTrackPointArray.h"
29
30 ClassImp(AliTrackPointArray)
31
32 //______________________________________________________________________________
33 AliTrackPointArray::AliTrackPointArray()
34 {
35   fNPoints = fSize = 0;
36   fX = fY = fZ = 0;
37   fVolumeID = 0;
38   fCov = 0;
39 }
40
41 //______________________________________________________________________________
42 AliTrackPointArray::AliTrackPointArray(Int_t npoints):
43   fNPoints(npoints)
44 {
45   // Constructor
46   //
47   fSize = 6*npoints;
48   fX = new Float_t[npoints];
49   fY = new Float_t[npoints];
50   fZ = new Float_t[npoints];
51   fVolumeID = new UShort_t[npoints];
52   fCov = new Float_t[fSize];
53 }
54
55 //______________________________________________________________________________
56 AliTrackPointArray::AliTrackPointArray(const AliTrackPointArray &array):
57   TObject(array)
58 {
59   // Copy constructor
60   //
61   fNPoints = array.fNPoints;
62   fSize = array.fSize;
63   fX = new Float_t[fNPoints];
64   fY = new Float_t[fNPoints];
65   fZ = new Float_t[fNPoints];
66   fVolumeID = new UShort_t[fNPoints];
67   fCov = new Float_t[fSize];
68   memcpy(fX,array.fX,fNPoints*sizeof(Float_t));
69   memcpy(fY,array.fY,fNPoints*sizeof(Float_t));
70   memcpy(fZ,array.fZ,fNPoints*sizeof(Float_t));
71   memcpy(fVolumeID,array.fVolumeID,fNPoints*sizeof(UShort_t));
72   memcpy(fCov,array.fCov,fSize*sizeof(Float_t));
73 }
74
75 //_____________________________________________________________________________
76 AliTrackPointArray &AliTrackPointArray::operator =(const AliTrackPointArray& array)
77 {
78   // assignment operator
79   //
80   if(this==&array) return *this;
81   ((TObject *)this)->operator=(array);
82
83   fNPoints = array.fNPoints;
84   fSize = array.fSize;
85   fX = new Float_t[fNPoints];
86   fY = new Float_t[fNPoints];
87   fZ = new Float_t[fNPoints];
88   fVolumeID = new UShort_t[fNPoints];
89   fCov = new Float_t[fSize];
90   memcpy(fX,array.fX,fNPoints*sizeof(Float_t));
91   memcpy(fY,array.fY,fNPoints*sizeof(Float_t));
92   memcpy(fZ,array.fZ,fNPoints*sizeof(Float_t));
93   memcpy(fVolumeID,array.fVolumeID,fNPoints*sizeof(UShort_t));
94   memcpy(fCov,array.fCov,fSize*sizeof(Float_t));
95
96   return *this;
97 }
98
99 //______________________________________________________________________________
100 AliTrackPointArray::~AliTrackPointArray()
101 {
102   // Destructor
103   //
104   delete [] fX;
105   delete [] fY;
106   delete [] fZ;
107   delete [] fVolumeID;
108   delete [] fCov;
109 }
110
111
112 //______________________________________________________________________________
113 Bool_t AliTrackPointArray::AddPoint(Int_t i, const AliTrackPoint *p)
114 {
115   // Add a point to the array at position i
116   //
117   if (i >= fNPoints) return kFALSE;
118   fX[i] = p->GetX();
119   fY[i] = p->GetY();
120   fZ[i] = p->GetZ();
121   fVolumeID[i] = p->GetVolumeID();
122   memcpy(&fCov[6*i],p->GetCov(),6*sizeof(Float_t));
123   return kTRUE;
124 }
125
126 //______________________________________________________________________________
127 Bool_t AliTrackPointArray::GetPoint(AliTrackPoint &p, Int_t i) const
128 {
129   // Get the point at position i
130   //
131   if (i >= fNPoints) return kFALSE;
132   p.SetXYZ(fX[i],fY[i],fZ[i],&fCov[6*i]);
133   p.SetVolumeID(fVolumeID[i]);
134   return kTRUE;
135 }
136
137 //______________________________________________________________________________
138 Bool_t AliTrackPointArray::HasVolumeID(UShort_t volid) const
139 {
140   // This method checks if the array
141   // has at least one hit in the detector
142   // volume defined by volid
143   Bool_t check = kFALSE;
144   for (Int_t ipoint = 0; ipoint < fNPoints; ipoint++)
145     if (fVolumeID[ipoint] == volid) check = kTRUE;
146
147   return check;
148 }
149
150 ClassImp(AliTrackPoint)
151
152 //______________________________________________________________________________
153 AliTrackPoint::AliTrackPoint()
154 {
155   // Default constructor
156   //
157   fX = fY = fZ = 0;
158   fVolumeID = 0;
159   memset(fCov,0,6*sizeof(Float_t));
160 }
161
162
163 //______________________________________________________________________________
164 AliTrackPoint::AliTrackPoint(Float_t x, Float_t y, Float_t z, const Float_t *cov, UShort_t volid)
165 {
166   // Constructor
167   //
168   SetXYZ(x,y,z,cov);
169   SetVolumeID(volid);
170 }
171
172 //______________________________________________________________________________
173 AliTrackPoint::AliTrackPoint(const Float_t *xyz, const Float_t *cov, UShort_t volid)
174 {
175   // Constructor
176   //
177   SetXYZ(xyz[0],xyz[1],xyz[2],cov);
178   SetVolumeID(volid);
179 }
180
181 //______________________________________________________________________________
182 AliTrackPoint::AliTrackPoint(const AliTrackPoint &p):
183   TObject(p)
184 {
185   // Copy constructor
186   //
187   SetXYZ(p.fX,p.fY,p.fZ,&(p.fCov[0]));
188   SetVolumeID(p.fVolumeID);
189 }
190
191 //_____________________________________________________________________________
192 AliTrackPoint &AliTrackPoint::operator =(const AliTrackPoint& p)
193 {
194   // assignment operator
195   //
196   if(this==&p) return *this;
197   ((TObject *)this)->operator=(p);
198
199   SetXYZ(p.fX,p.fY,p.fZ,&(p.fCov[0]));
200   SetVolumeID(p.fVolumeID);
201
202   return *this;
203 }
204
205 //______________________________________________________________________________
206 void AliTrackPoint::SetXYZ(Float_t x, Float_t y, Float_t z, const Float_t *cov)
207 {
208   // Set XYZ coordinates and their cov matrix
209   //
210   fX = x;
211   fY = y;
212   fZ = z;
213   if (cov)
214     memcpy(fCov,cov,6*sizeof(Float_t));
215 }
216
217 //______________________________________________________________________________
218 void AliTrackPoint::SetXYZ(const Float_t *xyz, const Float_t *cov)
219 {
220   // Set XYZ coordinates and their cov matrix
221   //
222   SetXYZ(xyz[0],xyz[1],xyz[2],cov);
223 }
224
225 //______________________________________________________________________________
226 void AliTrackPoint::GetXYZ(Float_t *xyz, Float_t *cov) const
227 {
228   xyz[0] = fX;
229   xyz[1] = fY;
230   xyz[2] = fZ;
231   if (cov)
232     memcpy(cov,fCov,6*sizeof(Float_t));
233 }
234
235 //______________________________________________________________________________
236 Float_t AliTrackPoint::GetResidual(const AliTrackPoint &p, Bool_t weighted) const
237 {
238   // This method calculates the track to space-point residuals. The track
239   // interpolation is also stored as AliTrackPoint. Using the option
240   // 'weighted' one can calculate the residual either with or without
241   // taking into account the covariance matrix of the space-point and
242   // track interpolation. The second case the residual becomes a pull.
243
244   Float_t res = 0;
245
246   if (!weighted) {
247     Float_t xyz[3],xyzp[3];
248     GetXYZ(xyz);
249     p.GetXYZ(xyzp);
250     res = (xyz[0]-xyzp[0])*(xyz[0]-xyzp[0])+
251           (xyz[1]-xyzp[1])*(xyz[1]-xyzp[1])+
252           (xyz[2]-xyzp[2])*(xyz[2]-xyzp[2]);
253   }
254   else {
255     Float_t xyz[3],xyzp[3];
256     Float_t cov[6],covp[6];
257     GetXYZ(xyz,cov);
258     TMatrixDSym mcov(3);
259     mcov(0,0) = cov[0]; mcov(0,1) = cov[1]; mcov(0,2) = cov[2];
260     mcov(1,0) = cov[1]; mcov(1,1) = cov[3]; mcov(1,2) = cov[4];
261     mcov(2,0) = cov[2]; mcov(2,1) = cov[4]; mcov(2,2) = cov[5];
262     p.GetXYZ(xyzp,covp);
263     TMatrixDSym mcovp(3);
264     mcovp(0,0) = covp[0]; mcovp(0,1) = covp[1]; mcovp(0,2) = covp[2];
265     mcovp(1,0) = covp[1]; mcovp(1,1) = covp[3]; mcovp(1,2) = covp[4];
266     mcovp(2,0) = covp[2]; mcovp(2,1) = covp[4]; mcovp(2,2) = covp[5];
267     TMatrixDSym msum = mcov + mcovp;
268     msum.Invert();
269     if (msum.IsValid()) {
270       for (Int_t i = 0; i < 3; i++)
271         for (Int_t j = 0; j < 3; j++)
272           res += (xyz[i]-xyzp[i])*(xyz[j]-xyzp[j])*msum(i,j);
273     }
274   }
275
276   return res;
277 }
278
279 //______________________________________________________________________________
280 Float_t AliTrackPoint::GetAngle() const
281 {
282   // The method uses the covariance matrix of
283   // the space-point in order to extract the
284   // orientation of the detector plane.
285   // The rotation in XY plane only is calculated.
286
287   if ((fCov[2] != 0) || (fCov[4] != 0))
288     return TMath::ATan2(-fCov[4],-fCov[2]);
289   else {
290     if (fCov[1] == 0) return 0;
291     Float_t phi= TMath::ATan2(TMath::Abs(fCov[1]),fCov[3]);
292     if (fCov[1] < 0) {
293       phi += TMath::PiOver2();
294       if ((fY-fX) < 0) phi += TMath::Pi();
295     }
296     else {
297       if ((fX+fY) < 0) phi += TMath::Pi();
298     }
299     return phi;
300   } 
301 //     return TMath::ATan2(TMath::Sign((Float_t)1.0,fY)*TMath::Abs(fCov[1]),
302 //                      TMath::Sign((Float_t)1.0,fX)*fCov[3]);
303 }
304
305 //_____________________________________________________________________________
306 AliTrackPoint& AliTrackPoint::Rotate(Float_t alpha) const
307 {
308   // Transform the space-point coordinates
309   // and covariance matrix from global to
310   // local (detector plane) coordinate system
311   // XY plane rotation only
312
313   static AliTrackPoint p;
314   p = *this;
315
316   Float_t xyz[3],cov[6];
317   GetXYZ(xyz,cov);
318
319   Float_t sin = TMath::Sin(alpha), cos = TMath::Cos(alpha);
320
321   Float_t newxyz[3],newcov[6];
322   newxyz[0] = cos*xyz[0] + sin*xyz[1];
323   newxyz[1] = cos*xyz[1] - sin*xyz[0];
324   newxyz[2] = xyz[2];
325
326   newcov[0] = cov[0]*cos*cos+
327             2*cov[1]*sin*cos+
328               cov[3]*sin*sin;
329   newcov[1] = cov[1]*(cos*cos-sin*sin)+
330              (cov[3]-cov[0])*sin*cos;
331   newcov[2] = cov[2]*cos+
332               cov[4]*sin;
333   newcov[3] = cov[0]*sin*sin-
334             2*cov[1]*sin*cos+
335               cov[3]*cos*cos;
336   newcov[4] = cov[4]*cos-
337               cov[2]*sin;
338   newcov[5] = cov[5];
339
340   p.SetXYZ(newxyz,newcov);
341   p.SetVolumeID(GetVolumeID());
342
343   return p;
344 }
345
346 //_____________________________________________________________________________
347 AliTrackPoint& AliTrackPoint::MasterToLocal() const
348 {
349   // Transform the space-point coordinates
350   // and the covariance matrix from the
351   // (master) to the local (tracking)
352   // coordinate system
353
354   Float_t alpha = GetAngle();
355   return Rotate(alpha);
356 }
357
358 //_____________________________________________________________________________
359 void AliTrackPoint::Print(Option_t *) const
360 {
361   // Print the space-point coordinates and
362   // covariance matrix
363
364   printf("VolumeID=%d\n", GetVolumeID());
365   printf("X = %12.6f    Tx = %12.6f%12.6f%12.6f\n", fX, fCov[0], fCov[1], fCov[2]);
366   printf("Y = %12.6f    Ty = %12.6f%12.6f%12.6f\n", fY, fCov[1], fCov[3], fCov[4]);
367   printf("Z = %12.6f    Tz = %12.6f%12.6f%12.6f\n", fZ, fCov[2], fCov[4], fCov[5]);
368
369 }