Fixing Coverity 18328
[u/mrichter/AliRoot.git] / STEER / ESD / AliStrLine.cxx
1 /**************************************************************************
2  * Copyright(c) 1998-2003, 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 ///////////////////////////////////////////////////////////////////
19 //                                                               //
20 // A straight line is coded as a point (3 Double_t) and           //
21 // 3 direction cosines                                           //
22 //                                                               //
23 ///////////////////////////////////////////////////////////////////
24
25 #include <Riostream.h>
26 #include <TMath.h>
27
28 #include "AliStrLine.h"
29
30 ClassImp(AliStrLine)
31
32 //________________________________________________________
33 AliStrLine::AliStrLine() :
34   TObject(),
35   fWMatrix(0),
36   fTpar(0)
37  {
38   // Default constructor
39   for(Int_t i=0;i<3;i++) {
40     fP0[i] = 0.;
41     fSigma2P0[i] = 0.;
42     fCd[i] = 0.;
43   }
44   SetIdPoints(65535,65535);
45 }
46
47 //________________________________________________________
48 AliStrLine::AliStrLine(const Double_t *const point, const Double_t *const cd, Bool_t twopoints, UShort_t id1, UShort_t id2) :
49   TObject(),
50   fWMatrix(0),
51   fTpar(0)
52 {
53   // Standard constructor
54   // if twopoints is true:  point and cd are the 3D coordinates of
55   //                        two points defininig the straight line
56   // if twopoint is false: point represents the 3D coordinates of a point
57   //                       belonging to the straight line and cd is the
58   //                       direction in space
59   for(Int_t i=0;i<3;i++)
60     fSigma2P0[i] = 0.;
61
62   if(twopoints)
63     InitTwoPoints(point,cd);
64   else 
65     InitDirection(point,cd);
66
67   SetIdPoints(id1,id2);
68 }
69
70
71 //________________________________________________________
72 AliStrLine::AliStrLine(const Double_t *const point, const Double_t *const sig2point, const Double_t *const cd, Bool_t twopoints, UShort_t id1, UShort_t id2) :
73   TObject(),
74   fWMatrix(0),
75   fTpar(0)
76 {
77   // Standard constructor
78   // if twopoints is true:  point and cd are the 3D coordinates of
79   //                        two points defininig the straight line
80   // if twopoint is false: point represents the 3D coordinates of a point
81   //                       belonging to the straight line and cd is the
82   //                       direction in space
83   for(Int_t i=0;i<3;i++)
84     fSigma2P0[i] = sig2point[i];
85
86   if(twopoints)
87     InitTwoPoints(point,cd);
88   else 
89     InitDirection(point,cd);
90
91   SetIdPoints(id1,id2);
92 }
93
94 //________________________________________________________
95 AliStrLine::AliStrLine(const Double_t *const point, const Double_t *const sig2point, const Double_t *const wmat, const Double_t *const cd, Bool_t twopoints, UShort_t id1, UShort_t id2) :
96   TObject(),
97   fWMatrix(0),
98   fTpar(0)
99 {
100   // Standard constructor
101   // if twopoints is true:  point and cd are the 3D coordinates of
102   //                        two points defininig the straight line
103   // if twopoint is false: point represents the 3D coordinates of a point
104   //                       belonging to the straight line and cd is the
105   //                       direction in space
106   Int_t k = 0;
107   fWMatrix = new Double_t [6];
108   for(Int_t i=0;i<3;i++){ 
109     fSigma2P0[i] = sig2point[i];
110     for(Int_t j=0;j<3;j++)if(j>=i)fWMatrix[k++]=wmat[3*i+j];
111   }
112   if(twopoints)
113     InitTwoPoints(point,cd);
114   else 
115     InitDirection(point,cd);
116
117   SetIdPoints(id1,id2);
118 }
119
120 //________________________________________________________
121 AliStrLine::AliStrLine(const AliStrLine &source):
122   TObject(source),
123   fWMatrix(0),
124   fTpar(source.fTpar)
125 {
126   //
127   // copy constructor
128   //
129   for(Int_t i=0;i<3;i++){
130     fP0[i]=source.fP0[i];
131     fSigma2P0[i]=source.fSigma2P0[i];
132     fCd[i]=source.fCd[i];
133   }
134   if(source.fWMatrix){
135     fWMatrix = new Double_t [6];
136     for(Int_t i=0;i<6;i++)fWMatrix[i]=source.fWMatrix[i];
137   }
138   for(Int_t i=0;i<2;i++) fIdPoint[i]=source.fIdPoint[i];
139 }
140
141 //________________________________________________________
142 AliStrLine& AliStrLine::operator=(const AliStrLine& source)
143 {
144   // Assignment operator
145   if(this !=&source){
146     for(Int_t i=0;i<3;i++){
147       fP0[i]=source.fP0[i];
148       fSigma2P0[i]=source.fSigma2P0[i];
149       fCd[i]=source.fCd[i];
150     } 
151
152     delete [] fWMatrix;
153     fWMatrix=0;
154     if(source.fWMatrix){
155       fWMatrix = new Double_t [6];
156       for(Int_t i=0;i<6;i++)fWMatrix[i]=source.fWMatrix[i];
157     } 
158     for(Int_t i=0;i<2;i++) fIdPoint[i]=source.fIdPoint[i];
159   }
160   return *this;
161 }
162
163 //________________________________________________________
164 void AliStrLine::GetWMatrix(Double_t *wmat)const {
165 // Getter for weighting matrix, as a [9] dim. array
166   if(!fWMatrix)return;
167   Int_t k = 0;
168   for(Int_t i=0;i<3;i++){
169     for(Int_t j=0;j<3;j++){
170       if(j>=i){
171         wmat[3*i+j]=fWMatrix[k++];
172       }
173       else{
174         wmat[3*i+j]=wmat[3*j+i];
175       }
176     }
177   }
178
179
180 //________________________________________________________
181 void AliStrLine::SetWMatrix(const Double_t *wmat) {
182 // Setter for weighting matrix, strating from a [9] dim. array
183   if(fWMatrix)delete [] fWMatrix;
184   fWMatrix = new Double_t [6];
185   Int_t k = 0;
186   for(Int_t i=0;i<3;i++){
187     for(Int_t j=0;j<3;j++)if(j>=i)fWMatrix[k++]=wmat[3*i+j];
188   }
189 }
190
191 //________________________________________________________
192 void AliStrLine::InitDirection(const Double_t *const point, const Double_t *const cd)
193 {
194   // Initialization from a point and a direction
195   Double_t norm = cd[0]*cd[0]+cd[1]*cd[1]+cd[2]*cd[2];
196
197   if(norm) {
198     norm = TMath::Sqrt(1./norm);
199     for(Int_t i=0;i<3;++i) {
200       fP0[i]=point[i];
201       fCd[i]=cd[i]*norm;
202     }
203     fTpar = 0.;
204   }
205   else AliFatal("Null direction cosines!!!");
206 }
207
208 //________________________________________________________
209 void AliStrLine::InitTwoPoints(const Double_t *const pA, const Double_t *const pB)
210 {
211   // Initialization from the coordinates of two
212   // points in the space
213   Double_t cd[3];
214   for(Int_t i=0;i<3;i++)cd[i] = pB[i]-pA[i];
215   InitDirection(pA,cd);
216 }
217
218 //________________________________________________________
219 AliStrLine::~AliStrLine() {
220   // destructor
221   if(fWMatrix)delete [] fWMatrix;
222 }
223
224 //________________________________________________________
225 void AliStrLine::PrintStatus() const {
226   // Print current status
227   cout <<"=======================================================\n";
228   cout <<"Direction cosines: ";
229   for(Int_t i=0;i<3;i++)cout <<fCd[i]<<"; ";
230   cout <<endl;
231   cout <<"Known point: ";
232   for(Int_t i=0;i<3;i++)cout <<fP0[i]<<"; ";
233   cout <<endl;
234   cout <<"Error on known point: ";
235   for(Int_t i=0;i<3;i++)cout <<TMath::Sqrt(fSigma2P0[i])<<"; ";
236   cout <<endl;
237   cout <<"Current value for the parameter: "<<fTpar<<endl;
238 }
239
240 //________________________________________________________
241 Int_t AliStrLine::IsParallelTo(const AliStrLine *line) const {
242   // returns 1 if lines are parallel, 0 if not paralel
243   const Double_t prec=1e-14;
244   Double_t cd2[3];
245   line->GetCd(cd2);
246
247   Double_t vecpx=fCd[1]*cd2[2]-fCd[2]*cd2[1];
248   Double_t mod=TMath::Abs(fCd[1]*cd2[2])+TMath::Abs(fCd[2]*cd2[1]);
249   if(TMath::Abs(vecpx) > prec*mod) return 0;
250
251   Double_t vecpy=-fCd[0]*cd2[2]+fCd[2]*cd2[0];
252   mod=TMath::Abs(fCd[0]*cd2[2])+TMath::Abs(fCd[2]*cd2[0]);
253   if(TMath::Abs(vecpy) > prec*mod) return 0;
254
255   Double_t vecpz=fCd[0]*cd2[1]-fCd[1]*cd2[0];
256   mod=TMath::Abs(fCd[0]*cd2[1])+TMath::Abs(fCd[1]*cd2[0]);
257   if(TMath::Abs(vecpz) > prec) return 0;
258
259   return 1;
260 }
261 //________________________________________________________
262 Int_t AliStrLine::Crossrphi(const AliStrLine *line)
263 {
264   // Cross 2 lines in the X-Y plane
265   const Double_t prec=1e-14;
266   const Double_t big=1e20;
267   Double_t p2[3];
268   Double_t cd2[3];
269   line->GetP0(p2);
270   line->GetCd(cd2);
271   Double_t a=fCd[0];
272   Double_t b=-cd2[0];
273   Double_t c=p2[0]-fP0[0];
274   Double_t d=fCd[1];
275   Double_t e=-cd2[1];
276   Double_t f=p2[1]-fP0[1];
277   Double_t deno = a*e-b*d;
278   Double_t mod=TMath::Abs(a*e)+TMath::Abs(b*d);
279   Int_t retcode = 0;
280   if(TMath::Abs(deno) > prec*mod) {
281     fTpar = (c*e-b*f)/deno;
282   }
283   else {
284     fTpar = big;
285     retcode = -1;
286   }
287   return retcode;
288 }
289
290 //________________________________________________________
291 Int_t AliStrLine::CrossPoints(AliStrLine *line, Double_t *point1, Double_t *point2){
292   // Looks for the crossing point estimated starting from the
293   // DCA segment
294   const Double_t prec=1e-14;
295   Double_t p2[3];
296   Double_t cd2[3];
297   line->GetP0(p2);
298   line->GetCd(cd2);
299   Int_t i;
300   Double_t k1 = 0;
301   Double_t k2 = 0;
302   Double_t a11 = 0;
303   for(i=0;i<3;i++){
304     k1+=(fP0[i]-p2[i])*fCd[i];
305     k2+=(fP0[i]-p2[i])*cd2[i];
306     a11+=fCd[i]*cd2[i];
307   }
308   Double_t a22 = -a11;
309   Double_t a21 = 0;
310   Double_t a12 = 0;
311   for(i=0;i<3;i++){
312     a21+=cd2[i]*cd2[i];
313     a12-=fCd[i]*fCd[i];
314   }
315   Double_t deno = a11*a22-a21*a12;
316   Double_t mod = TMath::Abs(a11*a22)+TMath::Abs(a21*a12);
317   if(TMath::Abs(deno) < prec*mod) return -1;
318   fTpar = (a11*k2-a21*k1) / deno;
319   Double_t par2 = (k1*a22-k2*a12) / deno;
320   line->SetPar(par2);
321   GetCurrentPoint(point1);
322   line->GetCurrentPoint(point2);
323   return 0;
324 }
325 //________________________________________________________________
326 Int_t AliStrLine::Cross(AliStrLine *line, Double_t *point)
327 {
328
329   //Finds intersection between lines
330   Double_t point1[3];
331   Double_t point2[3];
332   Int_t retcod=CrossPoints(line,point1,point2);
333   if(retcod==0){
334     for(Int_t i=0;i<3;i++)point[i]=(point1[i]+point2[i])/2.;
335     return 0;
336   }else{
337     return -1;
338   }
339 }
340
341 //___________________________________________________________
342 Double_t AliStrLine::GetDCA(const AliStrLine *line) const
343 {
344   //Returns the distance of closest approach between two lines
345   const Double_t prec=1e-14;
346   Double_t p2[3];
347   Double_t cd2[3];
348   line->GetP0(p2);
349   line->GetCd(cd2);
350   Int_t i;
351   Int_t ispar=IsParallelTo(line);
352   if(ispar){
353     Double_t dist1q=0,dist2=0,mod=0;
354     for(i=0;i<3;i++){
355       dist1q+=(fP0[i]-p2[i])*(fP0[i]-p2[i]);
356       dist2+=(fP0[i]-p2[i])*fCd[i];
357       mod+=fCd[i]*fCd[i];
358     }
359     if(TMath::Abs(mod) > prec){
360       dist2/=mod;
361       return TMath::Sqrt(dist1q-dist2*dist2);
362     }else{return -1;}
363   }else{
364      Double_t perp[3];
365      perp[0]=fCd[1]*cd2[2]-fCd[2]*cd2[1];
366      perp[1]=-fCd[0]*cd2[2]+fCd[2]*cd2[0];
367      perp[2]=fCd[0]*cd2[1]-fCd[1]*cd2[0];
368      Double_t mod=0,dist=0;
369      for(i=0;i<3;i++){
370        mod+=perp[i]*perp[i];
371        dist+=(fP0[i]-p2[i])*perp[i];
372      }
373      if(TMath::Abs(mod) > prec) {
374        return TMath::Abs(dist/TMath::Sqrt(mod));
375      } else return -1;
376   }
377 }
378 //________________________________________________________
379 void AliStrLine::GetCurrentPoint(Double_t *point) const {
380   // Fills the array point with the current value on the line
381   for(Int_t i=0;i<3;i++)point[i]=fP0[i]+fCd[i]*fTpar;
382 }
383
384 //________________________________________________________
385 Double_t AliStrLine::GetDistFromPoint(const Double_t *point) const 
386 {
387   // computes distance from point 
388   AliStrLine tmpline(point, fCd, kFALSE);
389   return GetDCA(&tmpline);
390 }
391
392 //________________________________________________________
393 Bool_t AliStrLine::GetParamAtRadius(Double_t r,Double_t &t1,Double_t &t2) const
394 {
395   // Input: radial distance from the origin (x=0, x=0) in the bending plane
396   // Returns a boolean: kTRUE if the line crosses the cylinder of radius r
397   // and axis coincident with the z axis. It returns kFALSE otherwise
398   // Output: t1 and t2 in ascending order. The parameters of the line at 
399   // the two intersections with the cylinder
400   Double_t p1= fCd[0]*fP0[0]+fCd[1]*fP0[1];
401   Double_t p2=fCd[0]*fCd[0]+fCd[1]*fCd[1];
402   Double_t delta=p1*p1-p2*(fP0[0]*fP0[0]+fP0[1]*fP0[1]-r*r);
403   if(delta<0.){
404     t1=-1000.;
405     t2=t1;
406     return kFALSE;
407   }
408   delta=TMath::Sqrt(delta);
409   t1=(-p1-delta)/p2;
410   t2=(-p1+delta)/p2;
411
412   if(t2<t1){
413     // use delta as a temporary buffer
414     delta=t1;
415     t1=t2;
416     t2=delta;
417   }
418   if(TMath::AreEqualAbs(t1,t2,1.e-9))t1=t2;
419   return kTRUE;
420 }