Possibility to convolute the original cov.matrix of the point with the matrix coming...
[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 <TMatrixD.h>
27 #include <TMatrixDSym.h>
28
29 #include "AliTrackPointArray.h"
30
31 ClassImp(AliTrackPointArray)
32
33 //______________________________________________________________________________
34 AliTrackPointArray::AliTrackPointArray() :
35   TObject(),
36   fNPoints(0),
37   fX(0),
38   fY(0),
39   fZ(0),
40   fSize(0),
41   fCov(0),
42   fVolumeID(0)
43 {
44 }
45
46 //______________________________________________________________________________
47 AliTrackPointArray::AliTrackPointArray(Int_t npoints):
48   TObject(),
49   fNPoints(npoints),
50   fX(new Float_t[npoints]),
51   fY(new Float_t[npoints]),
52   fZ(new Float_t[npoints]),
53   fSize(6*npoints),
54   fCov(new Float_t[fSize]),
55   fVolumeID(new UShort_t[npoints])
56 {
57   // Constructor
58   //
59 }
60
61 //______________________________________________________________________________
62 AliTrackPointArray::AliTrackPointArray(const AliTrackPointArray &array):
63   TObject(array),
64   fNPoints(array.fNPoints),
65   fX(new Float_t[fNPoints]),
66   fY(new Float_t[fNPoints]),
67   fZ(new Float_t[fNPoints]),
68   fSize(array.fSize),
69   fCov(new Float_t[fSize]),
70   fVolumeID(new UShort_t[fNPoints])
71 {
72   // Copy constructor
73   //
74   memcpy(fX,array.fX,fNPoints*sizeof(Float_t));
75   memcpy(fY,array.fY,fNPoints*sizeof(Float_t));
76   memcpy(fZ,array.fZ,fNPoints*sizeof(Float_t));
77   memcpy(fVolumeID,array.fVolumeID,fNPoints*sizeof(UShort_t));
78   memcpy(fCov,array.fCov,fSize*sizeof(Float_t));
79 }
80
81 //_____________________________________________________________________________
82 AliTrackPointArray &AliTrackPointArray::operator =(const AliTrackPointArray& array)
83 {
84   // assignment operator
85   //
86   if(this==&array) return *this;
87   ((TObject *)this)->operator=(array);
88
89   fNPoints = array.fNPoints;
90   fSize = array.fSize;
91   fX = new Float_t[fNPoints];
92   fY = new Float_t[fNPoints];
93   fZ = new Float_t[fNPoints];
94   fVolumeID = new UShort_t[fNPoints];
95   fCov = new Float_t[fSize];
96   memcpy(fX,array.fX,fNPoints*sizeof(Float_t));
97   memcpy(fY,array.fY,fNPoints*sizeof(Float_t));
98   memcpy(fZ,array.fZ,fNPoints*sizeof(Float_t));
99   memcpy(fVolumeID,array.fVolumeID,fNPoints*sizeof(UShort_t));
100   memcpy(fCov,array.fCov,fSize*sizeof(Float_t));
101
102   return *this;
103 }
104
105 //______________________________________________________________________________
106 AliTrackPointArray::~AliTrackPointArray()
107 {
108   // Destructor
109   //
110   delete [] fX;
111   delete [] fY;
112   delete [] fZ;
113   delete [] fVolumeID;
114   delete [] fCov;
115 }
116
117
118 //______________________________________________________________________________
119 Bool_t AliTrackPointArray::AddPoint(Int_t i, const AliTrackPoint *p)
120 {
121   // Add a point to the array at position i
122   //
123   if (i >= fNPoints) return kFALSE;
124   fX[i] = p->GetX();
125   fY[i] = p->GetY();
126   fZ[i] = p->GetZ();
127   fVolumeID[i] = p->GetVolumeID();
128   memcpy(&fCov[6*i],p->GetCov(),6*sizeof(Float_t));
129   return kTRUE;
130 }
131
132
133 //______________________________________________________________________________
134 Bool_t AliTrackPointArray::GetPoint(AliTrackPoint &p, Int_t i) const
135 {
136   // Get the point at position i
137   //
138   if (i >= fNPoints) return kFALSE;
139   p.SetXYZ(fX[i],fY[i],fZ[i],&fCov[6*i]);
140   p.SetVolumeID(fVolumeID[i]);
141   return kTRUE;
142 }
143
144 //______________________________________________________________________________
145 Bool_t AliTrackPointArray::HasVolumeID(UShort_t volid) const
146 {
147   // This method checks if the array
148   // has at least one hit in the detector
149   // volume defined by volid
150   Bool_t check = kFALSE;
151   for (Int_t ipoint = 0; ipoint < fNPoints; ipoint++)
152     if (fVolumeID[ipoint] == volid) check = kTRUE;
153
154   return check;
155 }
156
157 ClassImp(AliTrackPoint)
158
159 //______________________________________________________________________________
160 AliTrackPoint::AliTrackPoint() :
161   TObject(),
162   fX(0),
163   fY(0),
164   fZ(0),
165   fVolumeID(0)
166 {
167   // Default constructor
168   //
169   memset(fCov,0,6*sizeof(Float_t));
170 }
171
172
173 //______________________________________________________________________________
174 AliTrackPoint::AliTrackPoint(Float_t x, Float_t y, Float_t z, const Float_t *cov, UShort_t volid) :
175   TObject(),
176   fX(0),
177   fY(0),
178   fZ(0),
179   fVolumeID(0)
180 {
181   // Constructor
182   //
183   SetXYZ(x,y,z,cov);
184   SetVolumeID(volid);
185 }
186
187 //______________________________________________________________________________
188 AliTrackPoint::AliTrackPoint(const Float_t *xyz, const Float_t *cov, UShort_t volid) :
189   TObject(),
190   fX(0),
191   fY(0),
192   fZ(0),
193   fVolumeID(0)
194 {
195   // Constructor
196   //
197   SetXYZ(xyz[0],xyz[1],xyz[2],cov);
198   SetVolumeID(volid);
199 }
200
201 //______________________________________________________________________________
202 AliTrackPoint::AliTrackPoint(const AliTrackPoint &p):
203   TObject(p),
204   fX(0),
205   fY(0),
206   fZ(0),
207   fVolumeID(0)
208 {
209   // Copy constructor
210   //
211   SetXYZ(p.fX,p.fY,p.fZ,&(p.fCov[0]));
212   SetVolumeID(p.fVolumeID);
213 }
214
215 //_____________________________________________________________________________
216 AliTrackPoint &AliTrackPoint::operator =(const AliTrackPoint& p)
217 {
218   // assignment operator
219   //
220   if(this==&p) return *this;
221   ((TObject *)this)->operator=(p);
222
223   SetXYZ(p.fX,p.fY,p.fZ,&(p.fCov[0]));
224   SetVolumeID(p.fVolumeID);
225
226   return *this;
227 }
228
229 //______________________________________________________________________________
230 void AliTrackPoint::SetXYZ(Float_t x, Float_t y, Float_t z, const Float_t *cov)
231 {
232   // Set XYZ coordinates and their cov matrix
233   //
234   fX = x;
235   fY = y;
236   fZ = z;
237   if (cov)
238     memcpy(fCov,cov,6*sizeof(Float_t));
239 }
240
241 //______________________________________________________________________________
242 void AliTrackPoint::SetXYZ(const Float_t *xyz, const Float_t *cov)
243 {
244   // Set XYZ coordinates and their cov matrix
245   //
246   SetXYZ(xyz[0],xyz[1],xyz[2],cov);
247 }
248
249 //______________________________________________________________________________
250 void AliTrackPoint::GetXYZ(Float_t *xyz, Float_t *cov) const
251 {
252   xyz[0] = fX;
253   xyz[1] = fY;
254   xyz[2] = fZ;
255   if (cov)
256     memcpy(cov,fCov,6*sizeof(Float_t));
257 }
258
259 //______________________________________________________________________________
260 Float_t AliTrackPoint::GetResidual(const AliTrackPoint &p, Bool_t weighted) const
261 {
262   // This method calculates the track to space-point residuals. The track
263   // interpolation is also stored as AliTrackPoint. Using the option
264   // 'weighted' one can calculate the residual either with or without
265   // taking into account the covariance matrix of the space-point and
266   // track interpolation. The second case the residual becomes a pull.
267
268   Float_t res = 0;
269
270   if (!weighted) {
271     Float_t xyz[3],xyzp[3];
272     GetXYZ(xyz);
273     p.GetXYZ(xyzp);
274     res = (xyz[0]-xyzp[0])*(xyz[0]-xyzp[0])+
275           (xyz[1]-xyzp[1])*(xyz[1]-xyzp[1])+
276           (xyz[2]-xyzp[2])*(xyz[2]-xyzp[2]);
277   }
278   else {
279     Float_t xyz[3],xyzp[3];
280     Float_t cov[6],covp[6];
281     GetXYZ(xyz,cov);
282     TMatrixDSym mcov(3);
283     mcov(0,0) = cov[0]; mcov(0,1) = cov[1]; mcov(0,2) = cov[2];
284     mcov(1,0) = cov[1]; mcov(1,1) = cov[3]; mcov(1,2) = cov[4];
285     mcov(2,0) = cov[2]; mcov(2,1) = cov[4]; mcov(2,2) = cov[5];
286     p.GetXYZ(xyzp,covp);
287     TMatrixDSym mcovp(3);
288     mcovp(0,0) = covp[0]; mcovp(0,1) = covp[1]; mcovp(0,2) = covp[2];
289     mcovp(1,0) = covp[1]; mcovp(1,1) = covp[3]; mcovp(1,2) = covp[4];
290     mcovp(2,0) = covp[2]; mcovp(2,1) = covp[4]; mcovp(2,2) = covp[5];
291     TMatrixDSym msum = mcov + mcovp;
292     msum.Invert();
293     //    mcov.Print(); mcovp.Print(); msum.Print();
294     if (msum.IsValid()) {
295       for (Int_t i = 0; i < 3; i++)
296         for (Int_t j = 0; j < 3; j++)
297           res += (xyz[i]-xyzp[i])*(xyz[j]-xyzp[j])*msum(i,j);
298     }
299   }
300
301   return res;
302 }
303
304 //_____________________________________________________________________________
305 Bool_t AliTrackPoint::GetPCA(const AliTrackPoint &p, AliTrackPoint &out) const
306 {
307   //
308   // Get the intersection point between this point and
309   // the point "p" belongs to.
310   // The result is stored as a point 'out'
311   // return kFALSE in case of failure.
312   out.SetXYZ(0,0,0);
313
314   TMatrixD t(3,1);
315   t(0,0)=GetX();
316   t(1,0)=GetY();
317   t(2,0)=GetZ();
318  
319   TMatrixDSym tC(3);
320   {
321   const Float_t *cv=GetCov();
322   tC(0,0)=cv[0]; tC(0,1)=cv[1]; tC(0,2)=cv[2];
323   tC(1,0)=cv[1]; tC(1,1)=cv[3]; tC(1,2)=cv[4];
324   tC(2,0)=cv[2]; tC(2,1)=cv[4]; tC(2,2)=cv[5];
325   }
326
327   TMatrixD m(3,1);
328   m(0,0)=p.GetX();
329   m(1,0)=p.GetY();
330   m(2,0)=p.GetZ();
331  
332   TMatrixDSym mC(3);
333   {
334   const Float_t *cv=p.GetCov();
335   mC(0,0)=cv[0]; mC(0,1)=cv[1]; mC(0,2)=cv[2];
336   mC(1,0)=cv[1]; mC(1,1)=cv[3]; mC(1,2)=cv[4];
337   mC(2,0)=cv[2]; mC(2,1)=cv[4]; mC(2,2)=cv[5];
338   }
339
340   TMatrixDSym tmW(tC);
341   tmW+=mC;
342   tmW.Invert();
343   if (!tmW.IsValid()) return kFALSE; 
344
345   TMatrixD mW(tC,TMatrixD::kMult,tmW);
346   TMatrixD tW(mC,TMatrixD::kMult,tmW);
347
348   TMatrixD mi(mW,TMatrixD::kMult,m);
349   TMatrixD ti(tW,TMatrixD::kMult,t);
350   ti+=mi;
351
352   TMatrixD iC(tC,TMatrixD::kMult,tmW);
353   iC*=mC;
354
355   out.SetXYZ(ti(0,0),ti(1,0),ti(2,0));
356   UShort_t id=p.GetVolumeID();
357   out.SetVolumeID(id);
358
359   return kTRUE;
360 }
361
362 //______________________________________________________________________________
363 Float_t AliTrackPoint::GetAngle() const
364 {
365   // The method uses the covariance matrix of
366   // the space-point in order to extract the
367   // orientation of the detector plane.
368   // The rotation in XY plane only is calculated.
369
370   Float_t phi= TMath::ATan2(TMath::Sqrt(fCov[0]),TMath::Sqrt(fCov[3]));
371   if (fCov[1] > 0) {
372     phi = TMath::Pi() - phi;
373     if ((fY-fX) < 0) phi += TMath::Pi();
374   }
375   else {
376     if ((fX+fY) < 0) phi += TMath::Pi();
377   }
378
379   return phi;
380
381 }
382
383 //_____________________________________________________________________________
384 AliTrackPoint& AliTrackPoint::Rotate(Float_t alpha) const
385 {
386   // Transform the space-point coordinates
387   // and covariance matrix from global to
388   // local (detector plane) coordinate system
389   // XY plane rotation only
390
391   static AliTrackPoint p;
392   p = *this;
393
394   Float_t xyz[3],cov[6];
395   GetXYZ(xyz,cov);
396
397   Float_t sin = TMath::Sin(alpha), cos = TMath::Cos(alpha);
398
399   Float_t newxyz[3],newcov[6];
400   newxyz[0] = cos*xyz[0] + sin*xyz[1];
401   newxyz[1] = cos*xyz[1] - sin*xyz[0];
402   newxyz[2] = xyz[2];
403
404   newcov[0] = cov[0]*cos*cos+
405             2*cov[1]*sin*cos+
406               cov[3]*sin*sin;
407   newcov[1] = cov[1]*(cos*cos-sin*sin)+
408              (cov[3]-cov[0])*sin*cos;
409   newcov[2] = cov[2]*cos+
410               cov[4]*sin;
411   newcov[3] = cov[0]*sin*sin-
412             2*cov[1]*sin*cos+
413               cov[3]*cos*cos;
414   newcov[4] = cov[4]*cos-
415               cov[2]*sin;
416   newcov[5] = cov[5];
417
418   p.SetXYZ(newxyz,newcov);
419   p.SetVolumeID(GetVolumeID());
420
421   return p;
422 }
423
424 //_____________________________________________________________________________
425 AliTrackPoint& AliTrackPoint::MasterToLocal() const
426 {
427   // Transform the space-point coordinates
428   // and the covariance matrix from the
429   // (master) to the local (tracking)
430   // coordinate system
431
432   Float_t alpha = GetAngle();
433   return Rotate(alpha);
434 }
435
436 //_____________________________________________________________________________
437 void AliTrackPoint::Print(Option_t *) const
438 {
439   // Print the space-point coordinates and
440   // covariance matrix
441
442   printf("VolumeID=%d\n", GetVolumeID());
443   printf("X = %12.6f    Tx = %12.6f%12.6f%12.6f\n", fX, fCov[0], fCov[1], fCov[2]);
444   printf("Y = %12.6f    Ty = %12.6f%12.6f%12.6f\n", fY, fCov[1], fCov[3], fCov[4]);
445   printf("Z = %12.6f    Tz = %12.6f%12.6f%12.6f\n", fZ, fCov[2], fCov[4], fCov[5]);
446
447 }
448
449
450 //________________________________
451 void AliTrackPoint::SetAlignCovMatrix(const TMatrixDSym alignparmtrx){
452   // Add the uncertainty on the cluster position due to alignment
453   // (using the 6x6 AliAlignObj Cov. Matrix alignparmtrx) to the already
454   // present Cov. Matrix 
455
456   TMatrixDSym cov(3);
457   TMatrixD coval(3,3);
458   TMatrixD jacob(3,6);
459   Float_t newcov[6];
460
461   cov(0,0)=fCov[0];
462   cov(1,0)=cov(0,1)=fCov[1];
463   cov(2,0)=cov(0,2)=fCov[2];
464   cov(1,1)=fCov[3];
465   cov(2,1)=cov(1,2)=fCov[4];
466   cov(2,2)=fCov[5];
467   
468   jacob(0,0) = 1;      jacob(1,0) = 0;       jacob(2,0) = 0;
469   jacob(0,1) = 0;      jacob(1,1) = 1;       jacob(2,1) = 0;
470   jacob(0,2) = 0;      jacob(1,2) = 0;       jacob(2,2) = 1;
471   jacob(0,3) = 0;      jacob(1,3) =-fZ;      jacob(2,3) = fY;
472   jacob(0,4) = fZ;     jacob(1,4) = 0;       jacob(2,4) =-fX;
473   jacob(0,5) = fY;     jacob(1,5) = fX;      jacob(2,5) = 0;
474   
475   TMatrixD jacobT=jacob.T();jacob.T();
476   
477   coval=jacob*alignparmtrx*jacobT+cov;
478
479
480   newcov[0]=coval(0,0);
481   newcov[1]=coval(1,0);
482   newcov[2]=coval(2,0);
483   newcov[3]=coval(1,1);
484   newcov[4]=coval(2,1);
485   newcov[5]=coval(2,2);
486  
487   SetXYZ(fX,fY,fZ,newcov);
488
489 }