Bug corrected.
[u/mrichter/AliRoot.git] / STEER / AliStrLine.cxx
CommitLineData
edc97986 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 **************************************************************************/
090026bf 15
16/* $Id$ */
17
edc97986 18///////////////////////////////////////////////////////////////////
19// //
20// A straight line is coded as a point (3 Double_t) and //
21// 3 direction cosines //
22// //
23///////////////////////////////////////////////////////////////////
090026bf 24
edc97986 25#include <Riostream.h>
090026bf 26#include <TMath.h>
27
edc97986 28#include "AliStrLine.h"
29
30ClassImp(AliStrLine)
31
32//________________________________________________________
fe12e09c 33AliStrLine::AliStrLine() :
34 TObject(),
c48f9ca2 35 fWMatrix(0),
36 fTpar(0)
fe12e09c 37 {
edc97986 38 // Default constructor
39 for(Int_t i=0;i<3;i++) {
40 fP0[i] = 0.;
28dc94e2 41 fSigma2P0[i] = 0.;
edc97986 42 fCd[i] = 0.;
43 }
4e1be1bf 44 SetIdPoints(65535,65535);
edc97986 45}
46
47//________________________________________________________
b26b5f2c 48AliStrLine::AliStrLine(const Double_t *const point, const Double_t *const cd, Bool_t twopoints, UShort_t id1, UShort_t id2) :
fe12e09c 49 TObject(),
c48f9ca2 50 fWMatrix(0),
51 fTpar(0)
fe12e09c 52{
edc97986 53 // Standard constructor
24a0c65f 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
b26b5f2c 59 for(Int_t i=0;i<3;i++)
146c29df 60 fSigma2P0[i] = 0.;
24a0c65f 61
b26b5f2c 62 if(twopoints)
2c9641ee 63 InitTwoPoints(point,cd);
b26b5f2c 64 else
2c9641ee 65 InitDirection(point,cd);
b26b5f2c 66
4e1be1bf 67 SetIdPoints(id1,id2);
2c9641ee 68}
24a0c65f 69
b26b5f2c 70
24a0c65f 71//________________________________________________________
b26b5f2c 72AliStrLine::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) :
28dc94e2 73 TObject(),
c48f9ca2 74 fWMatrix(0),
75 fTpar(0)
28dc94e2 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
b26b5f2c 83 for(Int_t i=0;i<3;i++)
146c29df 84 fSigma2P0[i] = sig2point[i];
28dc94e2 85
b26b5f2c 86 if(twopoints)
146c29df 87 InitTwoPoints(point,cd);
b26b5f2c 88 else
146c29df 89 InitDirection(point,cd);
b26b5f2c 90
4e1be1bf 91 SetIdPoints(id1,id2);
146c29df 92}
b26b5f2c 93
146c29df 94//________________________________________________________
b26b5f2c 95AliStrLine::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) :
146c29df 96 TObject(),
c48f9ca2 97 fWMatrix(0),
98 fTpar(0)
146c29df 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
c48f9ca2 106 Int_t k = 0;
107 fWMatrix = new Double_t [6];
146c29df 108 for(Int_t i=0;i<3;i++){
109 fSigma2P0[i] = sig2point[i];
c48f9ca2 110 for(Int_t j=0;j<3;j++)if(j>=i)fWMatrix[k++]=wmat[3*i+j];
146c29df 111 }
b26b5f2c 112 if(twopoints)
146c29df 113 InitTwoPoints(point,cd);
b26b5f2c 114 else
146c29df 115 InitDirection(point,cd);
146c29df 116
4e1be1bf 117 SetIdPoints(id1,id2);
28dc94e2 118}
c48f9ca2 119
120//________________________________________________________
121AliStrLine::AliStrLine(const AliStrLine &source):TObject(source),
122fWMatrix(0),
123fTpar(source.fTpar){
124 // copy constructor
c48f9ca2 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 }
45e278c6 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 }
4e1be1bf 134 for(Int_t i=0;i<2;i++) fIdPoint[i]=source.fIdPoint[i];
c48f9ca2 135}
136
137//________________________________________________________
138AliStrLine& 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//________________________________________________________
148void 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//________________________________________________________
165void 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
28dc94e2 175//________________________________________________________
b26b5f2c 176void AliStrLine::InitDirection(const Double_t *const point, const Double_t *const cd)
177{
24a0c65f 178 // Initialization from a point and a direction
b26b5f2c 179 Double_t norm = cd[0]*cd[0]+cd[1]*cd[1]+cd[2]*cd[2];
180
edc97986 181 if(norm) {
b26b5f2c 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.;
edc97986 188 }
b26b5f2c 189 else AliFatal("Null direction cosines!!!");
edc97986 190}
191
192//________________________________________________________
b26b5f2c 193void AliStrLine::InitTwoPoints(const Double_t *const pA, const Double_t *const pB)
194{
24a0c65f 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//________________________________________________________
edc97986 203AliStrLine::~AliStrLine() {
204 // destructor
c48f9ca2 205 if(fWMatrix)delete [] fWMatrix;
edc97986 206}
207
208//________________________________________________________
209void 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;
28dc94e2 218 cout <<"Error on known point: ";
219 for(Int_t i=0;i<3;i++)cout <<TMath::Sqrt(fSigma2P0[i])<<"; ";
220 cout <<endl;
edc97986 221 cout <<"Current value for the parameter: "<<fTpar<<endl;
edc97986 222}
223
224//________________________________________________________
b26b5f2c 225Int_t AliStrLine::IsParallelTo(const AliStrLine *line) const {
edc97986 226 // returns 1 if lines are parallel, 0 if not paralel
9e224d91 227 const Double_t prec=1e-14;
edc97986 228 Double_t cd2[3];
229 line->GetCd(cd2);
b26b5f2c 230
edc97986 231 Double_t vecpx=fCd[1]*cd2[2]-fCd[2]*cd2[1];
b26b5f2c 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
edc97986 235 Double_t vecpy=-fCd[0]*cd2[2]+fCd[2]*cd2[0];
b26b5f2c 236 mod=TMath::Abs(fCd[0]*cd2[2])+TMath::Abs(fCd[2]*cd2[0]);
237 if(TMath::Abs(vecpy) > prec*mod) return 0;
238
edc97986 239 Double_t vecpz=fCd[0]*cd2[1]-fCd[1]*cd2[0];
b26b5f2c 240 mod=TMath::Abs(fCd[0]*cd2[1])+TMath::Abs(fCd[1]*cd2[0]);
9e224d91 241 if(TMath::Abs(vecpz) > prec) return 0;
b26b5f2c 242
edc97986 243 return 1;
244}
245//________________________________________________________
b26b5f2c 246Int_t AliStrLine::Crossrphi(const AliStrLine *line)
247{
edc97986 248 // Cross 2 lines in the X-Y plane
9e224d91 249 const Double_t prec=1e-14;
250 const Double_t big=1e20;
edc97986 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;
b26b5f2c 262 Double_t mod=TMath::Abs(a*e)+TMath::Abs(b*d);
edc97986 263 Int_t retcode = 0;
b26b5f2c 264 if(TMath::Abs(deno) > prec*mod) {
edc97986 265 fTpar = (c*e-b*f)/deno;
266 }
267 else {
9e224d91 268 fTpar = big;
edc97986 269 retcode = -1;
270 }
271 return retcode;
272}
273
274//________________________________________________________
275Int_t AliStrLine::CrossPoints(AliStrLine *line, Double_t *point1, Double_t *point2){
276 // Looks for the crossing point estimated starting from the
277 // DCA segment
9e224d91 278 const Double_t prec=1e-14;
edc97986 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;
edc97986 285 Double_t k2 = 0;
edc97986 286 Double_t a11 = 0;
c48f9ca2 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 }
edc97986 292 Double_t a22 = -a11;
293 Double_t a21 = 0;
edc97986 294 Double_t a12 = 0;
c48f9ca2 295 for(i=0;i<3;i++){
296 a21+=cd2[i]*cd2[i];
297 a12-=fCd[i]*fCd[i];
298 }
edc97986 299 Double_t deno = a11*a22-a21*a12;
b26b5f2c 300 Double_t mod = TMath::Abs(a11*a22)+TMath::Abs(a21*a12);
301 if(TMath::Abs(deno) < prec*mod) return -1;
edc97986 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//________________________________________________________________
b26b5f2c 310Int_t AliStrLine::Cross(AliStrLine *line, Double_t *point)
311{
edc97986 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//___________________________________________________________
b26b5f2c 326Double_t AliStrLine::GetDCA(const AliStrLine *line) const
327{
edc97986 328 //Returns the distance of closest approach between two lines
9e224d91 329 const Double_t prec=1e-14;
edc97986 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 }
9e224d91 343 if(TMath::Abs(mod) > prec){
edc97986 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 }
b26b5f2c 357 if(TMath::Abs(mod) > prec) {
358 return TMath::Abs(dist/TMath::Sqrt(mod));
359 } else return -1;
edc97986 360 }
361}
362//________________________________________________________
363void 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}
2c9641ee 367
368//________________________________________________________
b26b5f2c 369Double_t AliStrLine::GetDistFromPoint(const Double_t *point) const
370{
2c9641ee 371 // computes distance from point
b26b5f2c 372 AliStrLine tmpline(point, fCd, kFALSE);
373 return GetDCA(&tmpline);
2c9641ee 374}