3e088149595b19df92a62e5335dd5c75ea2d2b58
[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):TObject(source),
122 fWMatrix(0),
123 fTpar(source.fTpar){
124   // copy constructor
125   for(Int_t i=0;i<3;i++){
126     fP0[i]=source.fP0[i];
127     fSigma2P0[i]=source.fSigma2P0[i];
128     fCd[i]=source.fCd[i];
129   }
130   if(source.fWMatrix){
131     fWMatrix = new Double_t [6];
132     for(Int_t i=0;i<6;i++)fWMatrix[i]=source.fWMatrix[i];
133   }
134   for(Int_t i=0;i<2;i++) fIdPoint[i]=source.fIdPoint[i];
135 }
136
137 //________________________________________________________
138 AliStrLine& AliStrLine::operator=(const AliStrLine& source){
139   // Assignment operator
140   if(this !=&source){
141     this->~AliStrLine();
142     new(this)AliStrLine(source);
143   }
144   return *this;
145 }
146
147 //________________________________________________________
148 void AliStrLine::GetWMatrix(Double_t *wmat)const {
149 // Getter for weighting matrix, as a [9] dim. array
150   if(!fWMatrix)return;
151   Int_t k = 0;
152   for(Int_t i=0;i<3;i++){
153     for(Int_t j=0;j<3;j++){
154       if(j>=i){
155         wmat[3*i+j]=fWMatrix[k++];
156       }
157       else{
158         wmat[3*i+j]=wmat[3*j+i];
159       }
160     }
161   }
162
163
164 //________________________________________________________
165 void AliStrLine::SetWMatrix(const Double_t *wmat) {
166 // Setter for weighting matrix, strating from a [9] dim. array
167   if(fWMatrix)delete [] fWMatrix;
168   fWMatrix = new Double_t [6];
169   Int_t k = 0;
170   for(Int_t i=0;i<3;i++){
171     for(Int_t j=0;j<3;j++)if(j>=i)fWMatrix[k++]=wmat[3*i+j];
172   }
173 }
174
175 //________________________________________________________
176 void AliStrLine::InitDirection(const Double_t *const point, const Double_t *const cd)
177 {
178   // Initialization from a point and a direction
179   Double_t norm = cd[0]*cd[0]+cd[1]*cd[1]+cd[2]*cd[2];
180
181   if(norm) {
182     norm = TMath::Sqrt(1./norm);
183     for(Int_t i=0;i<3;++i) {
184       fP0[i]=point[i];
185       fCd[i]=cd[i]*norm;
186     }
187     fTpar = 0.;
188   }
189   else AliFatal("Null direction cosines!!!");
190 }
191
192 //________________________________________________________
193 void AliStrLine::InitTwoPoints(const Double_t *const pA, const Double_t *const pB)
194 {
195   // Initialization from the coordinates of two
196   // points in the space
197   Double_t cd[3];
198   for(Int_t i=0;i<3;i++)cd[i] = pB[i]-pA[i];
199   InitDirection(pA,cd);
200 }
201
202 //________________________________________________________
203 AliStrLine::~AliStrLine() {
204   // destructor
205   if(fWMatrix)delete [] fWMatrix;
206 }
207
208 //________________________________________________________
209 void AliStrLine::PrintStatus() const {
210   // Print current status
211   cout <<"=======================================================\n";
212   cout <<"Direction cosines: ";
213   for(Int_t i=0;i<3;i++)cout <<fCd[i]<<"; ";
214   cout <<endl;
215   cout <<"Known point: ";
216   for(Int_t i=0;i<3;i++)cout <<fP0[i]<<"; ";
217   cout <<endl;
218   cout <<"Error on known point: ";
219   for(Int_t i=0;i<3;i++)cout <<TMath::Sqrt(fSigma2P0[i])<<"; ";
220   cout <<endl;
221   cout <<"Current value for the parameter: "<<fTpar<<endl;
222 }
223
224 //________________________________________________________
225 Int_t AliStrLine::IsParallelTo(const AliStrLine *line) const {
226   // returns 1 if lines are parallel, 0 if not paralel
227   const Double_t prec=1e-14;
228   Double_t cd2[3];
229   line->GetCd(cd2);
230
231   Double_t vecpx=fCd[1]*cd2[2]-fCd[2]*cd2[1];
232   Double_t mod=TMath::Abs(fCd[1]*cd2[2])+TMath::Abs(fCd[2]*cd2[1]);
233   if(TMath::Abs(vecpx) > prec*mod) return 0;
234
235   Double_t vecpy=-fCd[0]*cd2[2]+fCd[2]*cd2[0];
236   mod=TMath::Abs(fCd[0]*cd2[2])+TMath::Abs(fCd[2]*cd2[0]);
237   if(TMath::Abs(vecpy) > prec*mod) return 0;
238
239   Double_t vecpz=fCd[0]*cd2[1]-fCd[1]*cd2[0];
240   mod=TMath::Abs(fCd[0]*cd2[1])+TMath::Abs(fCd[1]*cd2[0]);
241   if(TMath::Abs(vecpz) > prec) return 0;
242
243   return 1;
244 }
245 //________________________________________________________
246 Int_t AliStrLine::Crossrphi(const AliStrLine *line)
247 {
248   // Cross 2 lines in the X-Y plane
249   const Double_t prec=1e-14;
250   const Double_t big=1e20;
251   Double_t p2[3];
252   Double_t cd2[3];
253   line->GetP0(p2);
254   line->GetCd(cd2);
255   Double_t a=fCd[0];
256   Double_t b=-cd2[0];
257   Double_t c=p2[0]-fP0[0];
258   Double_t d=fCd[1];
259   Double_t e=-cd2[1];
260   Double_t f=p2[1]-fP0[1];
261   Double_t deno = a*e-b*d;
262   Double_t mod=TMath::Abs(a*e)+TMath::Abs(b*d);
263   Int_t retcode = 0;
264   if(TMath::Abs(deno) > prec*mod) {
265     fTpar = (c*e-b*f)/deno;
266   }
267   else {
268     fTpar = big;
269     retcode = -1;
270   }
271   return retcode;
272 }
273
274 //________________________________________________________
275 Int_t AliStrLine::CrossPoints(AliStrLine *line, Double_t *point1, Double_t *point2){
276   // Looks for the crossing point estimated starting from the
277   // DCA segment
278   const Double_t prec=1e-14;
279   Double_t p2[3];
280   Double_t cd2[3];
281   line->GetP0(p2);
282   line->GetCd(cd2);
283   Int_t i;
284   Double_t k1 = 0;
285   Double_t k2 = 0;
286   Double_t a11 = 0;
287   for(i=0;i<3;i++){
288     k1+=(fP0[i]-p2[i])*fCd[i];
289     k2+=(fP0[i]-p2[i])*cd2[i];
290     a11+=fCd[i]*cd2[i];
291   }
292   Double_t a22 = -a11;
293   Double_t a21 = 0;
294   Double_t a12 = 0;
295   for(i=0;i<3;i++){
296     a21+=cd2[i]*cd2[i];
297     a12-=fCd[i]*fCd[i];
298   }
299   Double_t deno = a11*a22-a21*a12;
300   Double_t mod = TMath::Abs(a11*a22)+TMath::Abs(a21*a12);
301   if(TMath::Abs(deno) < prec*mod) return -1;
302   fTpar = (a11*k2-a21*k1) / deno;
303   Double_t par2 = (k1*a22-k2*a12) / deno;
304   line->SetPar(par2);
305   GetCurrentPoint(point1);
306   line->GetCurrentPoint(point2);
307   return 0;
308 }
309 //________________________________________________________________
310 Int_t AliStrLine::Cross(AliStrLine *line, Double_t *point)
311 {
312
313   //Finds intersection between lines
314   Double_t point1[3];
315   Double_t point2[3];
316   Int_t retcod=CrossPoints(line,point1,point2);
317   if(retcod==0){
318     for(Int_t i=0;i<3;i++)point[i]=(point1[i]+point2[i])/2.;
319     return 0;
320   }else{
321     return -1;
322   }
323 }
324
325 //___________________________________________________________
326 Double_t AliStrLine::GetDCA(const AliStrLine *line) const
327 {
328   //Returns the distance of closest approach between two lines
329   const Double_t prec=1e-14;
330   Double_t p2[3];
331   Double_t cd2[3];
332   line->GetP0(p2);
333   line->GetCd(cd2);
334   Int_t i;
335   Int_t ispar=IsParallelTo(line);
336   if(ispar){
337     Double_t dist1q=0,dist2=0,mod=0;
338     for(i=0;i<3;i++){
339       dist1q+=(fP0[i]-p2[i])*(fP0[i]-p2[i]);
340       dist2+=(fP0[i]-p2[i])*fCd[i];
341       mod+=fCd[i]*fCd[i];
342     }
343     if(TMath::Abs(mod) > prec){
344       dist2/=mod;
345       return TMath::Sqrt(dist1q-dist2*dist2);
346     }else{return -1;}
347   }else{
348      Double_t perp[3];
349      perp[0]=fCd[1]*cd2[2]-fCd[2]*cd2[1];
350      perp[1]=-fCd[0]*cd2[2]+fCd[2]*cd2[0];
351      perp[2]=fCd[0]*cd2[1]-fCd[1]*cd2[0];
352      Double_t mod=0,dist=0;
353      for(i=0;i<3;i++){
354        mod+=perp[i]*perp[i];
355        dist+=(fP0[i]-p2[i])*perp[i];
356      }
357      if(TMath::Abs(mod) > prec) {
358        return TMath::Abs(dist/TMath::Sqrt(mod));
359      } else return -1;
360   }
361 }
362 //________________________________________________________
363 void AliStrLine::GetCurrentPoint(Double_t *point) const {
364   // Fills the array point with the current value on the line
365   for(Int_t i=0;i<3;i++)point[i]=fP0[i]+fCd[i]*fTpar;
366 }
367
368 //________________________________________________________
369 Double_t AliStrLine::GetDistFromPoint(const Double_t *point) const 
370 {
371   // computes distance from point 
372   AliStrLine tmpline(point, fCd, kFALSE);
373   return GetDCA(&tmpline);
374 }
375
376 //________________________________________________________
377 Bool_t AliStrLine::GetParamAtRadius(Double_t r,Double_t &t1,Double_t &t2) const
378 {
379   // Input: radial distance from the origin (x=0, x=0) in the bending plane
380   // Returns a boolean: kTRUE if the line crosses the cylinder of radius r
381   // and axis coincident with the z axis. It returns kFALSE otherwise
382   // Output: t1 and t2 in ascending order. The parameters of the line at 
383   // the two intersections with the cylinder
384   Double_t p1= fCd[0]*fP0[0]+fCd[1]*fP0[1];
385   Double_t p2=fCd[0]*fCd[0]+fCd[1]*fCd[1];
386   Double_t delta=p1*p1-p2*(fP0[0]*fP0[0]+fP0[1]*fP0[1]-r*r);
387   if(delta<0.){
388     t1=-1000.;
389     t2=t1;
390     return kFALSE;
391   }
392   delta=TMath::Sqrt(delta);
393   t1=(-p1-delta)/p2;
394   t2=(-p1+delta)/p2;
395
396   if(t2<t1){
397     // use delta as a temporary buffer
398     delta=t1;
399     t1=t2;
400     t2=delta;
401   }
402   if(TMath::AreEqualAbs(t1,t2,1.e-9))t1=t2;
403   return kTRUE;
404 }