Initialization of all returned variables in GetRPHIintersections
[u/mrichter/AliRoot.git] / STEER / AliHelix.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 /* $Id$ */
17
18 //-------------------------------------------------------------------------
19 //                Implementation of the AliHelix class
20 //        Origin: Marian Ivanov, CERN, marian.ivanov@cern.ch
21 //-------------------------------------------------------------------------
22
23
24 #include "AliHelix.h"
25 #include "AliKalmanTrack.h"
26 #include "AliExternalTrackParam.h"
27 #include "TMath.h"
28 ClassImp(AliHelix)
29
30
31 //_______________________________________________________________________
32 AliHelix::AliHelix()
33 {
34   //
35   // Default constructor
36   //
37   for (Int_t i =0;i<9;i++) fHelix[i]=0;
38 }
39
40 //_______________________________________________________________________
41 AliHelix::AliHelix(const AliHelix &t):TObject(t){
42   //
43   //
44   for (Int_t i=0;i<9;i++) 
45     fHelix[i]=t.fHelix[i];
46 }
47
48 AliHelix::AliHelix(const AliKalmanTrack &t)
49 {
50   //
51   // 
52   Double_t alpha,x,cs,sn;
53   t.GetExternalParameters(x,fHelix); 
54   alpha=t.GetAlpha();
55   //
56   //circle parameters
57   fHelix[4]=fHelix[4]/t.GetConvConst();    // C
58   cs=TMath::Cos(alpha); sn=TMath::Sin(alpha);
59
60   Double_t xc, yc, rc;
61   rc  =  1/fHelix[4];
62   xc  =  x-fHelix[2]*rc;
63   yc  =  fHelix[0]+TMath::Sqrt(1-(x-xc)*(x-xc)*fHelix[4]*fHelix[4])/fHelix[4];
64   
65   fHelix[6] = xc*cs - yc*sn;
66   fHelix[7] = xc*sn + yc*cs;
67   fHelix[8] =  TMath::Abs(rc);
68   //
69   //
70   fHelix[5]=x*cs - fHelix[0]*sn;            // x0
71   fHelix[0]=x*sn + fHelix[0]*cs;            // y0
72   //fHelix[1]=                               // z0
73   fHelix[2]=TMath::ATan2(-(fHelix[5]-fHelix[6]),fHelix[0]-fHelix[7]); // phi0
74   if (fHelix[4]>0) fHelix[2]-=TMath::Pi();
75
76   //fHelix[3]=                               // tgl
77   //
78   //
79   fHelix[5]   = fHelix[6];
80   fHelix[0]   = fHelix[7];
81 }
82
83
84 AliHelix::AliHelix(const AliExternalTrackParam &t)
85 {
86   //
87   // 
88   Double_t alpha,x,cs,sn;
89   const Double_t *param =t.GetParameter(); 
90   for (Int_t i=0;i<5;i++) fHelix[i]=param[i]; 
91   x = t.X();
92   alpha=t.Alpha();
93   //
94   //circle parameters
95   fHelix[4]=fHelix[4]/AliKalmanTrack::GetConvConst();    // C
96   cs=TMath::Cos(alpha); sn=TMath::Sin(alpha);
97
98   Double_t xc, yc, rc;
99   rc  =  1/fHelix[4];
100   xc  =  x-fHelix[2]*rc;
101   yc  =  fHelix[0]+TMath::Sqrt(1-(x-xc)*(x-xc)*fHelix[4]*fHelix[4])/fHelix[4];
102   
103   fHelix[6] = xc*cs - yc*sn;
104   fHelix[7] = xc*sn + yc*cs;
105   fHelix[8] =  TMath::Abs(rc);
106   //
107   //
108   fHelix[5]=x*cs - fHelix[0]*sn;            // x0
109   fHelix[0]=x*sn + fHelix[0]*cs;            // y0
110   //fHelix[1]=                               // z0
111   fHelix[2]=TMath::ASin(fHelix[2]) + alpha; // phi0
112   //fHelix[3]=                               // tgl
113   //
114   //
115   fHelix[5]   = fHelix[6];
116   fHelix[0]   = fHelix[7];
117 }
118
119 AliHelix::AliHelix(Double_t x[3], Double_t p[3], Double_t charge, Double_t conversion)
120 {
121   //
122   //
123   //
124   Double_t pt = TMath::Sqrt(p[0]*p[0]+p[1]*p[1]);
125   if (TMath::Abs(conversion)<0.00000001) 
126     conversion = AliKalmanTrack::GetConvConst();
127   //
128   //  
129   fHelix[4] = charge/(conversion*pt); // C
130   fHelix[3] = p[2]/pt;    // tgl
131   //  
132   Double_t xc, yc, rc;
133   rc  =  1/fHelix[4];
134   xc  =  x[0]  -rc*p[1]/pt;
135   yc  =  x[1]  +rc*p[0]/pt; 
136   //
137   fHelix[5] = x[0];   // x0
138   fHelix[0] = x[1];   // y0
139   fHelix[1] = x[2];   // z0
140   //
141   fHelix[6] = xc;
142   fHelix[7] = yc;
143   fHelix[8] = TMath::Abs(rc);
144   //
145   fHelix[5]=xc; 
146   fHelix[0]=yc; 
147   //
148   if (TMath::Abs(p[1])<TMath::Abs(p[0])){     
149     fHelix[2]=TMath::ASin(p[1]/pt);
150     if (charge*yc<charge*x[1])  fHelix[2] = TMath::Pi()-fHelix[2];
151   }
152   else{
153     fHelix[2]=TMath::ACos(p[0]/pt);
154     if (charge*xc>charge*x[0])  fHelix[2] = -fHelix[2];
155   }
156
157 }
158
159 void  AliHelix::GetMomentum(Double_t phase, Double_t p[4],Double_t conversion, Double_t *xr)
160 {
161   // return  momentum at given phase
162   Double_t x[3],g[3],gg[3];
163   Evaluate(phase,x,g,gg);
164   if (TMath::Abs(conversion)<0.0001) conversion = AliKalmanTrack::GetConvConst();
165   Double_t mt = TMath::Sqrt(g[0]*g[0]+g[1]*g[1]);
166   p[0] = fHelix[8]*g[0]/(mt*conversion);
167   p[1] = fHelix[8]*g[1]/(mt*conversion);
168   p[2] = fHelix[8]*g[2]/(mt*conversion);
169   if (xr){
170     xr[0] = x[0]; xr[1] = x[1]; xr[2] = x[2];
171   }
172 }
173
174 void   AliHelix::GetAngle(Double_t t1, AliHelix &h, Double_t t2, Double_t angle[3])
175 {
176   //
177   //
178   //
179   Double_t x1[3],g1[3],gg1[3];
180   Double_t x2[3],g2[3],gg2[3];
181   Evaluate(t1,x1,g1,gg1);
182   h.Evaluate(t2,x2,g2,gg2);
183
184   //
185   Double_t norm1r = g1[0]*g1[0]+g1[1]*g1[1];
186   Double_t norm1  = TMath::Sqrt(norm1r+g1[2]*g1[2]);
187   norm1r         = TMath::Sqrt(norm1r);
188   //
189   Double_t norm2r = g2[0]*g2[0]+g2[1]*g2[1];
190   Double_t norm2  = TMath::Sqrt(norm2r+g2[2]*g2[2]);
191   norm2r         = TMath::Sqrt(norm2r);
192   //
193   angle[0]  = (g1[0]*g2[0]+g1[1]*g2[1])/(norm1r*norm2r);   // angle in phi projection
194   if (TMath::Abs(angle[0])<1.) angle[0] = TMath::ACos(angle[0]);
195   else{ 
196     if (angle[0]>0) angle[0] = 0;
197     if (angle[0]<0) angle[0] = TMath::Pi();
198   }
199   //
200   angle[1]  = ((norm1r*norm2r)+g1[2]*g2[2])/(norm1*norm2); // angle in rz  projection
201   if (TMath::Abs(angle[1])<1.) angle[1] = TMath::ACos(angle[1]);
202   else 
203     angle[1]=0;
204
205   angle[2]  = (g1[0]*g2[0]+g1[1]*g2[1]+g1[2]*g2[2])/(norm1*norm2); //3D angle
206   if (TMath::Abs(angle[2])<1.) angle[2] = TMath::ACos(angle[2]);
207   else 
208     angle[2]=0;
209
210   
211   
212
213 }
214
215
216 void AliHelix::Evaluate(Double_t t,
217                      Double_t r[3],  //radius vector
218                      Double_t g[3],  //first defivatives
219                      Double_t gg[3]) //second derivatives
220 {
221   //--------------------------------------------------------------------
222   // Calculate position of a point on a track and some derivatives at given phase
223   //--------------------------------------------------------------------
224   Double_t phase=fHelix[4]*t+fHelix[2];
225   Double_t sn=TMath::Sin(phase), cs=TMath::Cos(phase);
226
227   r[0] = fHelix[5] + sn/fHelix[4];
228   r[1] = fHelix[0] - cs/fHelix[4];  
229   r[2] = fHelix[1] + fHelix[3]*t;
230
231   g[0] = cs; g[1]=sn; g[2]=fHelix[3];
232   
233   gg[0]=-fHelix[4]*sn; gg[1]=fHelix[4]*cs; gg[2]=0.;
234 }
235
236 Int_t     AliHelix::GetClosestPhases(AliHelix &h, Double_t phase[2][2])
237 {
238   //
239   // get phases to minimize distances
240   //
241   Double_t xyz0[3];
242   Double_t xyz1[3];
243
244   for (Int_t i=0;i<2;i++){  
245     Evaluate(phase[i][0]  ,xyz0);       
246     h.Evaluate(phase[i][1],xyz1);
247     Double_t mindist = TMath::Sqrt((xyz0[0]-xyz1[0])*(xyz0[0]-xyz1[0])+
248                                    (xyz0[1]-xyz1[1])*(xyz0[1]-xyz1[1])+
249                                    (xyz0[2]-xyz1[2])*(xyz0[2]-xyz1[2]));  
250     Double_t tbest[2]={phase[i][0],phase[i][1]};
251     for (Int_t i0=-1;i0<=1;i0++){
252       Double_t t0 = ((phase[i][0]*fHelix[4])+i0*2.*TMath::Pi())/fHelix[4];
253       Evaluate(t0,xyz0);
254       for (Int_t i1=-1;i1<=1;i1++){
255         Double_t t1 = ((phase[i][1]*h.fHelix[4])+i1*2.*TMath::Pi())/h.fHelix[4];    
256         h.Evaluate(t1,xyz1);    
257         Double_t dist = TMath::Sqrt((xyz0[0]-xyz1[0])*(xyz0[0]-xyz1[0])+
258                                     (xyz0[1]-xyz1[1])*(xyz0[1]-xyz1[1])+
259                                     (xyz0[2]-xyz1[2])*(xyz0[2]-xyz1[2])); 
260         if (dist<=mindist){
261           tbest[0] = t0;
262           tbest[1] = t1;
263           mindist=dist;
264         }
265       }
266     }
267     phase[i][0] = tbest[0];
268     phase[i][1] = tbest[1];
269   } 
270   return 1;
271 }
272
273 Double_t  AliHelix::GetPointAngle(AliHelix &h, Double_t phase[2], const Float_t * vertex)
274 {
275   //
276   // get point angle bettwen two helixes
277   // 
278   Double_t r0[3],p0[4];
279   Double_t r1[3],p1[4];
280   GetMomentum(phase[0],p0,1,r0);
281   h.GetMomentum(phase[1],p1,1,r1);
282   //
283   Double_t r[3] = {(r0[0]+r1[0])*0.5-vertex[0],(r0[1]+r1[1])*0.5-vertex[1],(r0[2]+r1[2])*0.5-vertex[2]};
284   //intersection point - relative to the prim vertex
285   Double_t p[3] = { p0[0]+p1[0], p0[1]+p1[1],p0[2]+p1[2]};
286   // derivation vector
287   Double_t normr = TMath::Sqrt(r[0]*r[0]+r[1]*r[1]+r[2]*r[2]);
288   Double_t normp = TMath::Sqrt(p[0]*p[0]+p[1]*p[1]+p[2]*p[2]);   
289   Double_t pointAngle = (r[0]*p[0]+r[1]*p[1]+r[2]*p[2])/(normr*normp);
290   return pointAngle;
291 }
292
293 Double_t  AliHelix::GetPhase(Double_t x, Double_t y )
294                         
295 {
296   //
297   //calculate helix param at given x,y  point
298   //
299   //Double_t phase2 = TMath::ATan2((y-fHelix[0]), (x-fHelix[5]))- TMath::Pi()/2.;
300   Double_t phase2 = TMath::ATan2(-(x-fHelix[5]),(y-fHelix[0]));
301   Int_t sign = (fHelix[4]>0)? 1:-1;
302   if (sign>0) phase2 = phase2-TMath::Pi();
303   //
304   Float_t delta = TMath::Nint((phase2-fHelix[2])/(2.*TMath::Pi()));
305   phase2-= 2*TMath::Pi()*delta;
306   if ( (phase2-fHelix[2])>TMath::Pi()) phase2 -=2.*TMath::Pi();
307   if ( (phase2-fHelix[2])<-TMath::Pi()) phase2+=2.*TMath::Pi();
308
309   Double_t t     = (phase2-fHelix[2]);
310   t/=fHelix[4];
311   return t;
312 }
313
314 Int_t AliHelix::GetPhase(Double_t /*r0*/, Double_t * /*t[2]*/) 
315 {
316   //
317   //calculate helix param at given r  point - return nearest point ()
318   //
319   // not implemented yet
320   
321
322   return 0;
323 }
324
325
326 Double_t  AliHelix::GetPhaseZ(Double_t z0)
327 {
328   //
329   //
330   return (z0-fHelix[1])/fHelix[3];
331 }
332
333
334 Int_t    AliHelix::GetRPHIintersections(AliHelix &h, Double_t phase[2][2], Double_t ri[2], Double_t cut)
335 {
336   //--------------------------------------------------------------------
337   // This function returns  phase vectors with intesection between helix (0, 1 or 2)
338   // in x-y plane projection  
339   //--------------------------------------------------------------------
340   //    
341   //  Double_t * c1 = &fHelix[6];
342   //Double_t * c2 = &(h.fHelix[6]);
343   //  Double_t  c1[3] = {fHelix[5],fHelix[0],fHelix[8]};
344
345   // PH initiaziation in case of return
346   phase[0][0]=phase[0][1]=phase[1][0]=phase[1][1]=0;
347   ri[0]=ri[1]=1000000;
348
349   Double_t  c1[3] = {0,0,fHelix[8]};
350   Double_t  c2[3] = {h.fHelix[5]-fHelix[5],h.fHelix[0]-fHelix[0],h.fHelix[8]};
351
352   Double_t d  = TMath::Sqrt(c2[0]*c2[0]+c2[1]*c2[1]); 
353   if (d<0.000000000001) return 0;
354   //
355   Double_t x0[2];
356   Double_t y0[2];
357   //  
358   if ( d>=(c1[2]+c2[2])){
359     if (d>=(c1[2]+c2[2]+cut)) return 0;
360     x0[0] = (d+c1[2]-c2[2])*c2[0]/(2*d)+ fHelix[5];
361     y0[0] = (d+c1[2]-c2[2])*c2[1]/(2*d)+ fHelix[0];
362     //    return 0;
363     phase[1][0] = phase[0][0] = GetPhase(x0[0],y0[0]);
364     phase[1][1] = phase[0][1] = h.GetPhase(x0[0],y0[0]);
365     ri[1] = ri[0] = x0[0]*x0[0]+y0[0]*y0[0];
366     return 1;
367   }
368   if ( (d+c2[2])<c1[2]){
369     if ( (d+c2[2])+cut<c1[2]) return 0;
370     //
371     Double_t xx = c2[0]+ c2[0]*c2[2]/d+ fHelix[5];
372     Double_t yy = c2[1]+ c2[1]*c2[2]/d+ fHelix[0]; 
373     phase[1][1] = phase[0][1] = h.GetPhase(xx,yy);
374     //
375     Double_t xx2 = c2[0]*c1[2]/d+ fHelix[5];
376     Double_t yy2 = c2[1]*c1[2]/d+ fHelix[0]; 
377     phase[1][0] = phase[0][0] = GetPhase(xx2,yy2);
378     ri[1] = ri[0] = xx*xx+yy*yy;
379     return 1;
380   }
381
382   if ( (d+c1[2])<c2[2]){
383     if ( (d+c1[2])+cut<c2[2]) return 0;
384     //
385     Double_t xx = -c2[0]*c1[2]/d+ fHelix[5];
386     Double_t yy = -c2[1]*c1[2]/d+ fHelix[0]; 
387     phase[1][1] = phase[0][1] = GetPhase(xx,yy);
388     //
389     Double_t xx2 = c2[0]- c2[0]*c2[2]/d+ fHelix[5];
390     Double_t yy2 = c2[1]- c2[1]*c2[2]/d+ fHelix[0]; 
391     phase[1][0] = phase[0][0] = h.GetPhase(xx2,yy2);
392     ri[1] = ri[0] = xx*xx+yy*yy;
393     return 1;
394   }
395
396   Double_t d1 = (d*d+c1[2]*c1[2]-c2[2]*c2[2])/(2.*d);
397   Double_t v1 = c1[2]*c1[2]-d1*d1;
398   if (v1<0) return 0;
399   v1 = TMath::Sqrt(v1);
400   //
401   x0[0] = (c2[0]*d1+c2[1]*v1)/d + fHelix[5];
402   y0[0] = (c2[1]*d1-c2[0]*v1)/d + fHelix[0];            
403   //
404   x0[1] = (c2[0]*d1-c2[1]*v1)/d + fHelix[5];
405   y0[1] = (c2[1]*d1+c2[0]*v1)/d + fHelix[0];      
406   //
407   for (Int_t i=0;i<2;i++){
408     phase[i][0] = GetPhase(x0[i],y0[i]);
409     phase[i][1] = h.GetPhase(x0[i],y0[i]);
410     ri[i] = x0[i]*x0[i]+y0[i]*y0[i];    
411   }      
412   return 2;
413
414
415
416
417 Int_t   AliHelix::LinearDCA(AliHelix &h, Double_t &t1, Double_t &t2, 
418                       Double_t &R, Double_t &dist)
419 {
420   //
421   //
422   // find intersection using linear approximation
423   Double_t r1[3],g1[3],gg1[3];
424   Double_t r2[3],g2[3],gg2[3];
425   //
426   Evaluate(t1,r1,g1,gg1);
427   h.Evaluate(t2,r2,g2,gg2);
428   // 
429   Double_t g1_2 = g1[0]*g1[0] +g1[1]*g1[1] +g1[2]*g1[2];
430   Double_t g2_2 = g2[0]*g2[0] +g2[1]*g2[1] +g2[2]*g2[2];
431   Double_t g1x2 = g1[0]*g2[0] +g1[1]*g2[1] +g1[2]*g2[2];  
432   Double_t det  = g1_2*g2_2   - g1x2*g1x2;
433   //  
434   if (TMath::Abs(det)>0){
435     //
436     Double_t r1g1 = r1[0]*g1[0] +r1[1]*g1[1] +r1[2]*g1[2];    
437     Double_t r2g1 = r2[0]*g1[0] +r2[1]*g1[1] +r2[2]*g1[2];      
438     Double_t r1g2 = r1[0]*g2[0] +r1[1]*g2[1] +r1[2]*g2[2];
439     Double_t r2g2 = r2[0]*g2[0] +r2[1]*g2[1] +r2[2]*g2[2];
440     //    
441     Double_t dt    = - ( g2_2*(r1g1-r2g1) - g1x2*(r1g2-r2g2)) / det;      
442     Double_t dp    = - ( g1_2*(r2g2-r1g2) - g1x2*(r2g1-r1g1)) / det;
443     //
444     t1+=dt;
445     t2+=dp;
446     Evaluate(t1,r1);
447     h.Evaluate(t2,r2);
448     //
449     dist = (r1[0]-r2[0])*(r1[0]-r2[0])+
450                                   (r1[1]-r2[1])*(r1[1]-r2[1])+
451                                   (r1[2]-r2[2])*(r1[2]-r2[2]);    
452     R = ((r1[0]+r2[0])*(r1[0]+r2[0])+(r1[1]+r2[1])*(r1[1]+r2[1]))/4.;
453   }     
454   return 0;
455 }
456
457
458 Int_t  AliHelix::ParabolicDCA(AliHelix&h,  //helixes
459                                Double_t &t1, Double_t &t2, 
460                                Double_t &R, Double_t &dist, Int_t iter)
461 {
462   //
463   //
464   // find intersection using linear fit
465   Double_t r1[3],g1[3],gg1[3];
466   Double_t r2[3],g2[3],gg2[3];
467   //
468   Evaluate(t1,r1,g1,gg1);
469   h.Evaluate(t2,r2,g2,gg2);
470
471   //
472   Double_t dx2=1.;
473   Double_t dy2=1.;
474   Double_t dz2=1.;
475   //
476   Double_t dx=r2[0]-r1[0], dy=r2[1]-r1[1], dz=r2[2]-r1[2];
477   Double_t dm=dx*dx/dx2 + dy*dy/dy2 + dz*dz/dz2;
478   //
479
480  iter++;
481  while (iter--) {
482     Double_t gt1=-(dx*g1[0]/dx2 + dy*g1[1]/dy2 + dz*g1[2]/dz2);
483     Double_t gt2=+(dx*g2[0]/dx2 + dy*g2[1]/dy2 + dz*g2[2]/dz2);
484     
485     Double_t h11=(g1[0]*g1[0] - dx*gg1[0])/dx2 + 
486       (g1[1]*g1[1] - dy*gg1[1])/dy2 +
487       (g1[2]*g1[2] - dz*gg1[2])/dz2;
488     Double_t h22=(g2[0]*g2[0] + dx*gg2[0])/dx2 + 
489       (g2[1]*g2[1] + dy*gg2[1])/dy2 +
490       (g2[2]*g2[2] + dz*gg2[2])/dz2;
491     Double_t h12=-(g1[0]*g2[0]/dx2 + g1[1]*g2[1]/dy2 + g1[2]*g2[2]/dz2);
492     
493     Double_t det=h11*h22-h12*h12;
494     
495     Double_t dt1,dt2;
496     if (TMath::Abs(det)<1.e-33) {
497       //(quasi)singular Hessian
498       dt1=-gt1; dt2=-gt2;
499     } else {
500       dt1=-(gt1*h22 - gt2*h12)/det; 
501       dt2=-(h11*gt2 - h12*gt1)/det;
502     }
503     
504     if ((dt1*gt1+dt2*gt2)>0) {dt1=-dt1; dt2=-dt2;}
505     
506     //if (TMath::Abs(dt1)/(TMath::Abs(t1)+1.e-3) < 1.e-4)
507     //  if (TMath::Abs(dt2)/(TMath::Abs(t2)+1.e-3) < 1.e-4) {
508     //  break;
509     //  }
510     
511     Double_t dd=dm;
512     for (Int_t div=1 ; div<512 ; div*=2) {
513       Evaluate(t1+dt1,r1,g1,gg1);
514       h.Evaluate(t2+dt2,r2,g2,gg2);
515       dx=r2[0]-r1[0]; dy=r2[1]-r1[1]; dz=r2[2]-r1[2];
516       dd=dx*dx/dx2 + dy*dy/dy2 + dz*dz/dz2;
517       if (dd<dm) break;
518       dt1*=0.5; dt2*=0.5;
519       if (div==0){
520         div =1;
521       }
522       if (div>512) {      
523         break;
524       }   
525     }
526     dm=dd;
527     t1+=dt1;
528     t2+=dt2;
529  }
530  Evaluate(t1,r1,g1,gg1);
531  h.Evaluate(t2,r2,g2,gg2);
532  //
533  dist = (r1[0]-r2[0])*(r1[0]-r2[0])+
534    (r1[1]-r2[1])*(r1[1]-r2[1])+
535    (r1[2]-r2[2])*(r1[2]-r2[2]);    
536  
537  R = ((r1[0]+r2[0])*(r1[0]+r2[0])+(r1[1]+r2[1])*(r1[1]+r2[1]))/4;
538  return 0;
539  
540 }
541
542
543 Int_t  AliHelix::ParabolicDCA2(AliHelix&h,  //helixes
544                                Double_t &t1, Double_t &t2, 
545                                Double_t &R, Double_t &dist,  Double_t err[3], Int_t iter)
546 {
547   //
548   //
549   // find intersection using linear fit
550   Double_t r1[3],g1[3],gg1[3];
551   Double_t r2[3],g2[3],gg2[3];
552   //
553   Evaluate(t1,r1,g1,gg1);
554   h.Evaluate(t2,r2,g2,gg2);
555
556   //
557   Double_t dx2=err[0];
558   Double_t dy2=err[1];
559   Double_t dz2=err[2];
560   //
561   Double_t dx=r2[0]-r1[0], dy=r2[1]-r1[1], dz=r2[2]-r1[2];
562   Double_t dm=dx*dx/dx2 + dy*dy/dy2 + dz*dz/dz2;
563   //
564
565  iter++;
566  while (iter--) {
567     Double_t gt1=-(dx*g1[0]/dx2 + dy*g1[1]/dy2 + dz*g1[2]/dz2);
568     Double_t gt2=+(dx*g2[0]/dx2 + dy*g2[1]/dy2 + dz*g2[2]/dz2);
569     
570     Double_t h11=(g1[0]*g1[0] - dx*gg1[0])/dx2 + 
571       (g1[1]*g1[1] - dy*gg1[1])/dy2 +
572       (g1[2]*g1[2] - dz*gg1[2])/dz2;
573     Double_t h22=(g2[0]*g2[0] + dx*gg2[0])/dx2 + 
574       (g2[1]*g2[1] + dy*gg2[1])/dy2 +
575       (g2[2]*g2[2] + dz*gg2[2])/dz2;
576     Double_t h12=-(g1[0]*g2[0]/dx2 + g1[1]*g2[1]/dy2 + g1[2]*g2[2]/dz2);
577     
578     Double_t det=h11*h22-h12*h12;
579     
580     Double_t dt1,dt2;
581     if (TMath::Abs(det)<1.e-33) {
582       //(quasi)singular Hessian
583       dt1=-gt1; dt2=-gt2;
584     } else {
585       dt1=-(gt1*h22 - gt2*h12)/det; 
586       dt2=-(h11*gt2 - h12*gt1)/det;
587     }
588     
589     if ((dt1*gt1+dt2*gt2)>0) {dt1=-dt1; dt2=-dt2;}
590     
591     //if (TMath::Abs(dt1)/(TMath::Abs(t1)+1.e-3) < 1.e-4)
592     //  if (TMath::Abs(dt2)/(TMath::Abs(t2)+1.e-3) < 1.e-4) {
593     //  break;
594     //  }
595     
596     Double_t dd=dm;
597     for (Int_t div=1 ; div<512 ; div*=2) {
598       Evaluate(t1+dt1,r1,g1,gg1);
599       h.Evaluate(t2+dt2,r2,g2,gg2);
600       dx=r2[0]-r1[0]; dy=r2[1]-r1[1]; dz=r2[2]-r1[2];
601       dd=dx*dx/dx2 + dy*dy/dy2 + dz*dz/dz2;
602       if (dd<dm) break;
603       dt1*=0.5; dt2*=0.5;
604       if (div==0){
605         div =1;
606       }
607       if (div>512) {      
608         break;
609       }   
610     }
611     dm=dd;
612     t1+=dt1;
613     t2+=dt2;
614  }
615  Evaluate(t1,r1,g1,gg1);
616  h.Evaluate(t2,r2,g2,gg2);
617  //
618  dist = (r1[0]-r2[0])*(r1[0]-r2[0])+
619    (r1[1]-r2[1])*(r1[1]-r2[1])+
620    (r1[2]-r2[2])*(r1[2]-r2[2]);    
621  
622  R = ((r1[0]+r2[0])*(r1[0]+r2[0])+(r1[1]+r2[1])*(r1[1]+r2[1]))/4;
623  return 0;
624  
625 }
626