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