Lines getting the matched track moved to a method in AliCalorimeterUtils. Lines copie...
[u/mrichter/AliRoot.git] / PWG2 / FEMTOSCOPY / AliFemto / AliFmHelix.cxx
1 ///////////////////////////////////////////////////////////////////////////
2 //                                                                       //
3 // AliFmHelix: a helper helix class                                      //
4 // Includes all the operations and specifications of the helix. Can be   //
5 // used to determine path lengths, distance of closest approach etc.     //
6 //                                                                       //
7 ///////////////////////////////////////////////////////////////////////////
8 #if !defined(ST_NO_NUMERIC_LIMITS)
9 #    include <limits>
10 #    if !defined(ST_NO_NAMESPACES)
11 using std::numeric_limits;
12 #    endif
13 #endif
14 #define FOR_HELIX
15 #include <float.h>
16 #include <assert.h>
17
18 #include "AliFmHelix.h"
19 #include "PhysicalConstants.h" 
20 #include "SystemOfUnits.h"
21 #ifdef __ROOT__
22 ClassImpT(AliFmHelix,double);
23 #endif
24
25 #ifdef WIN32
26 #include "gcc2vs.h"
27 #endif
28
29 #include <iostream>
30 #include <fstream>
31 using namespace std;
32
33 const double AliFmHelix::fgkNoSolution = 3.e+33;
34
35 AliFmHelix::AliFmHelix() :
36   fSingularity(0),
37   fOrigin(0,0,0),
38   fDipAngle(0),
39   fCurvature(0),
40   fPhase(0),
41   fH(0),
42   fCosDipAngle(0),
43   fSinDipAngle(0),
44   fCosPhase(0),
45   fSinPhase(0)
46 {
47   //Default constructor
48 /*noop*/ 
49 }
50
51 AliFmHelix::AliFmHelix(double c, double d, double phase,
52                        const AliFmThreeVector<double>& o, int h) :
53   fSingularity(0),
54   fOrigin(0,0,0),
55   fDipAngle(0),
56   fCurvature(0),
57   fPhase(0),
58   fH(0),
59   fCosDipAngle(0),
60   fSinDipAngle(0),
61   fCosPhase(0),
62   fSinPhase(0)
63 {
64   // Constructor with helix parameters
65   SetParameters(c, d, phase, o, h);
66 }
67
68 AliFmHelix::~AliFmHelix() { 
69   // Default destructor
70 /* noop */ 
71 }
72
73 void AliFmHelix::SetParameters(double c, double dip, double phase,
74                                const AliFmThreeVector<double>& o, int h)
75 {
76         //
77         //  The order in which the parameters are set is important
78         //  since setCurvature might have to adjust the others.
79         //
80         fH = (h>=0) ? 1 : -1;    // Default is: positive particle
81         //             positive field
82         fOrigin   = o;
83         SetDipAngle(dip);
84         SetPhase(phase);
85
86         //
87         // Check for singularity and correct for negative curvature.           
88         // May change mH and mPhase. Must therefore be set last.
89         //
90         SetCurvature(c);
91
92         //
93         // For the case B=0, h is ill defined. In the following we
94         // always assume h = +1. Since phase = psi - h * pi/2
95         // we have to correct the phase in case h = -1.
96         // This assumes that the user uses the same h for phase
97         // as the one he passed to the constructor.
98         //
99         if (fSingularity && fH == -1) {
100                 fH = +1;
101                 SetPhase(fPhase-M_PI);
102         }
103 }
104
105 void AliFmHelix::SetCurvature(double val)
106 {
107   // Set helix curvature
108         if (val < 0) {
109                 fCurvature = -val;
110                 fH = -fH;
111                 SetPhase(fPhase+M_PI);
112         }
113         else
114                 fCurvature = val;
115
116 #ifndef ST_NO_NUMERIC_LIMITS
117         if (fabs(fCurvature) <= numeric_limits<double>::epsilon())
118 #else
119         if (fabs(fCurvature) <= static_cast<double>(0))
120 #endif    
121                 fSingularity = true;                    // straight line
122         else
123                 fSingularity = false;                   // curved
124 }
125
126 void AliFmHelix::SetPhase(double val)
127 {
128   // Set helix phase
129         fPhase       = val;
130         fCosPhase    = cos(fPhase);
131         fSinPhase    = sin(fPhase);
132         if (fabs(fPhase) > M_PI)
133                 fPhase = atan2(fSinPhase, fCosPhase);  // force range [-pi,pi]
134 }
135
136 void AliFmHelix::SetDipAngle(double val)
137 {
138   // Set helix dip angle
139         fDipAngle    = val;
140         fCosDipAngle = cos(fDipAngle);
141         fSinDipAngle = sin(fDipAngle);
142 }
143
144 double AliFmHelix::XCenter() const
145 {
146   // Set helix center in X
147         if (fSingularity)
148                 return 0;
149         else
150                 return fOrigin.x()-fCosPhase/fCurvature;
151 }
152
153 double AliFmHelix::YCenter() const
154 {
155   // Set helix center in Y
156         if (fSingularity)
157                 return 0;
158         else
159                 return fOrigin.y()-fSinPhase/fCurvature;
160 }
161
162 double AliFmHelix::FudgePathLength(const AliFmThreeVector<double>& p) const
163 {
164   // Path length
165         double s;
166         double dx = p.x()-fOrigin.x();
167         double dy = p.y()-fOrigin.y();
168
169         if (fSingularity) {
170                 s = (dy*fCosPhase - dx*fSinPhase)/fCosDipAngle;
171         }
172         else {
173                 s = atan2(dy*fCosPhase - dx*fSinPhase,
174                         1/fCurvature + dx*fCosPhase+dy*fSinPhase)/
175                         (fH*fCurvature*fCosDipAngle);
176         }
177         return s;
178 }
179
180 double AliFmHelix::Distance(const AliFmThreeVector<double>& p, bool scanPeriods) const
181 {
182   // calculate distance between origin an p along the helix
183         return abs(this->At(PathLength(p,scanPeriods))-p);
184 }
185
186 double AliFmHelix::PathLength(const AliFmThreeVector<double>& p, bool scanPeriods) const 
187 {
188         //
189         //  Returns the path length at the distance of closest 
190         //  approach between the helix and point p. 
191         //  For the case of B=0 (straight line) the path length
192         //  can be calculated analytically. For B>0 there is
193         //  unfortunately no easy solution to the problem.
194         //  Here we use the Newton method to find the root of the
195         //  referring equation. The 'fudgePathLength' serves
196         //  as a starting value.
197         //
198         double s;
199         double dx = p.x()-fOrigin.x();
200         double dy = p.y()-fOrigin.y();
201         double dz = p.z()-fOrigin.z();
202
203         if (fSingularity) {
204                 s = fCosDipAngle*(fCosPhase*dy-fSinPhase*dx) +
205                         fSinDipAngle*dz;
206         }
207         else { //
208 #ifndef ST_NO_NAMESPACES
209                 {
210                         using namespace units;
211 #endif
212                         const double ktMaxPrecisionNeeded = micrometer;
213                         const int    ktMaxIterations      = 100;
214
215                         //
216                         // The math is taken from Maple with C(expr,optimized) and
217                         // some hand-editing. It is not very nice but efficient.
218                         //
219                         double t34 = fCurvature*fCosDipAngle*fCosDipAngle;
220                         double t41 = fSinDipAngle*fSinDipAngle;
221                         double t6, t7, t11, t12, t19;
222
223                         //
224                         // Get a first guess by using the dca in 2D. Since
225                         // in some extreme cases we might be off by n periods
226                         // we add (subtract) periods in case we get any closer.
227                         // 
228                         s = FudgePathLength(p);
229
230                         if (scanPeriods) {
231                                 double ds = Period();
232                                 int    j, jmin = 0;
233                                 double d, dmin = abs(At(s) - p);
234                                 for(j=1; j<ktMaxIterations; j++) {
235                                         if ((d = abs(At(s+j*ds) - p)) < dmin) {
236                                                 dmin = d;
237                                                 jmin = j;
238                                         }
239                                         else
240                                                 break;
241                                 }
242                                 for(j=-1; -j<ktMaxIterations; j--) {
243                                         if ((d = abs(At(s+j*ds) - p)) < dmin) {
244                                                 dmin = d;
245                                                 jmin = j;
246                                         }
247                                         else
248                                                 break;
249                                 }
250                                 if (jmin) s += jmin*ds;
251                         }
252
253                         //
254                         // Newtons method:
255                         // Stops after ktMaxIterations iterations or if the required
256                         // precision is obtained. Whatever comes first.
257                         //
258                         double sOld = s;
259                         for (int i=0; i<ktMaxIterations; i++) {
260                                 t6  = fPhase+s*fH*fCurvature*fCosDipAngle;
261                                 t7  = cos(t6);
262                                 t11 = dx-(1/fCurvature)*(t7-fCosPhase);
263                                 t12 = sin(t6);
264                                 t19 = dy-(1/fCurvature)*(t12-fSinPhase);
265                                 s  -= (t11*t12*fH*fCosDipAngle-t19*t7*fH*fCosDipAngle -
266                                         (dz-s*fSinDipAngle)*fSinDipAngle)/
267                                         (t12*t12*fCosDipAngle*fCosDipAngle+t11*t7*t34 +
268                                         t7*t7*fCosDipAngle*fCosDipAngle +
269                                         t19*t12*t34+t41);
270                                 if (fabs(sOld-s) < ktMaxPrecisionNeeded) break;
271                                 sOld = s;
272                         }
273 #ifndef ST_NO_NAMESPACES
274                 }
275 #endif
276         }
277         return s;
278 }
279
280 double AliFmHelix::Period() const
281 {
282   // period
283         if (fSingularity)
284 #ifndef ST_NO_NUMERIC_LIMITS
285                 return numeric_limits<double>::max();
286 #else
287                 return DBL_MAX;
288 #endif    
289         else    
290                 return fabs(2*M_PI/(fH*fCurvature*fCosDipAngle)); 
291 }
292
293 pair<double, double> AliFmHelix::PathLength(double r) const
294 {
295         //
296         // The math is taken from Maple with C(expr,optimized) and
297         // some hand-editing. It is not very nice but efficient.
298         // 'first' is the smallest of the two solutions (may be negative)
299         // 'second' is the other.
300         //
301         pair<double,double> tvalue;
302         pair<double,double> tVALUE(999999999.,999999999.);
303         if (fSingularity) {
304                 double t1 = fCosDipAngle*(fOrigin.x()*fSinPhase-fOrigin.y()*fCosPhase);
305                 double t12 = fOrigin.y()*fOrigin.y();
306                 double t13 = fCosPhase*fCosPhase;
307                 double t15 = r*r;
308                 double t16 = fOrigin.x()*fOrigin.x();
309                 double t20 = -fCosDipAngle*fCosDipAngle*(2.0*fOrigin.x()*fSinPhase*fOrigin.y()*fCosPhase +
310                         t12-t12*t13-t15+t13*t16);
311                 if (t20<0.) return tVALUE;
312                 t20 = ::sqrt(t20);
313                 tvalue.first  = (t1-t20)/(fCosDipAngle*fCosDipAngle);
314                 tvalue.second = (t1+t20)/(fCosDipAngle*fCosDipAngle);
315         }
316         else {
317                 double t1 = fOrigin.y()*fCurvature;
318                 double t2 = fSinPhase;
319                 double t3 = fCurvature*fCurvature;
320                 double t4 = fOrigin.y()*t2;
321                 double t5 = fCosPhase;
322                 double t6 = fOrigin.x()*t5;
323                 double t8 = fOrigin.x()*fOrigin.x();
324                 double t11 = fOrigin.y()*fOrigin.y();
325                 double t14 = r*r;
326                 double t15 = t14*fCurvature;
327                 double t17 = t8*t8;
328                 double t19 = t11*t11;
329                 double t21 = t11*t3;
330                 double t23 = t5*t5;
331                 double t32 = t14*t14;
332                 double t35 = t14*t3;
333                 double t38 = 8.0*t4*t6 - 4.0*t1*t2*t8 - 4.0*t11*fCurvature*t6 +
334                         4.0*t15*t6 + t17*t3 + t19*t3 + 2.0*t21*t8 + 4.0*t8*t23 -
335                         4.0*t8*fOrigin.x()*fCurvature*t5 - 4.0*t11*t23 -
336                         4.0*t11*fOrigin.y()*fCurvature*t2 + 4.0*t11 - 4.0*t14 +
337                         t32*t3 + 4.0*t15*t4 - 2.0*t35*t11 - 2.0*t35*t8;
338                 double t40 = (-t3*t38);
339                 if (t40<0.) return tVALUE;
340                 t40 = ::sqrt(t40);
341
342                 double t43 = fOrigin.x()*fCurvature;
343                 double t45 = 2.0*t5 - t35 + t21 + 2.0 - 2.0*t1*t2 -2.0*t43 - 2.0*t43*t5 + t8*t3;
344                 double t46 = fH*fCosDipAngle*fCurvature;
345
346                 tvalue.first = (-fPhase + 2.0*atan((-2.0*t1 + 2.0*t2 + t40)/t45))/t46;
347                 tvalue.second = -(fPhase + 2.0*atan((2.0*t1 - 2.0*t2 + t40)/t45))/t46;
348
349                 //
350                 //   Solution can be off by +/- one Period, select smallest
351                 //
352                 double p = Period();
353                 //      malisa deletes "isnan" check 22apr2006  if (!isnan(value.first)) {
354                 if (fabs(tvalue.first-p) < fabs(tvalue.first)) tvalue.first = tvalue.first-p;
355                 else if (fabs(tvalue.first+p) < fabs(tvalue.first)) tvalue.first = tvalue.first+p;
356                 //      malisa  }
357                 //      malisa deletes "isnan" check 22apr2006          if (!isnan(tvalue.second)) {
358                 if (fabs(tvalue.second-p) < fabs(tvalue.second)) tvalue.second = tvalue.second-p;
359                 else if (fabs(tvalue.second+p) < fabs(tvalue.second)) tvalue.second = tvalue.second+p;
360                 //      malisa }
361         }
362         if (tvalue.first > tvalue.second)
363                 swap(tvalue.first,tvalue.second);
364         return(tvalue);
365 }
366
367 pair<double, double> AliFmHelix::PathLength(double r, double x, double y, bool /* scanPeriods */)
368 {
369   // path length
370         double x0 = fOrigin.x();
371         double y0 = fOrigin.y();
372         fOrigin.SetX(x0-x);
373         fOrigin.SetY(y0-y);
374         pair<double, double> result = this->PathLength(r);
375         fOrigin.SetX(x0);
376         fOrigin.SetY(y0);
377         return result;  
378 }
379
380 double AliFmHelix::PathLength(const AliFmThreeVector<double>& r,
381                                                    const AliFmThreeVector<double>& n) const
382 {
383         //
384         // Vector 'r' defines the position of the center and
385         // vector 'n' the normal vector of the plane.
386         // For a straight line there is a simple analytical
387         // solution. For curvatures > 0 the root is determined
388         // by Newton method. In case no valid s can be found
389         // the max. largest value for s is returned.
390         //
391         double s;
392
393         if (fSingularity) {
394                 double t = n.z()*fSinDipAngle +
395                         n.y()*fCosDipAngle*fCosPhase -
396                         n.x()*fCosDipAngle*fSinPhase;
397                 if (t == 0)
398                         s = fgkNoSolution;
399                 else
400                         s = ((r - fOrigin)*n)/t;
401         }
402         else {
403                 const double ktMaxPrecisionNeeded = micrometer;
404                 const int    ktMaxIterations      = 20;
405
406                 double tA = fCurvature*((fOrigin - r)*n) -
407                         n.x()*fCosPhase - 
408                         n.y()*fSinPhase;
409                 double t = fH*fCurvature*fCosDipAngle;
410                 double u = n.z()*fCurvature*fSinDipAngle;
411
412                 double a, f, fp;
413                 double sOld = s = 0;  
414                 double shiftOld = 0;
415                 double shift;
416                 //              (cos(kangMax)-1)/kangMax = 0.1
417                 const double kangMax = 0.21;
418                 double deltas = fabs(kangMax/(fCurvature*fCosDipAngle));
419                 //              dampingFactor = exp(-0.5);
420                 double dampingFactor = 0.60653;
421                 int i;
422
423                 for (i=0; i<ktMaxIterations; i++) {
424                         a  = t*s+fPhase;
425                         double sina = sin(a);
426                         double cosa = cos(a);
427                         f  = tA +
428                                 n.x()*cosa +
429                                 n.y()*sina +
430                                 u*s;
431                         fp = -n.x()*sina*t +
432                                 n.y()*cosa*t +
433                                 u;
434                         if ( fabs(fp)*deltas <= fabs(f) ) { //too big step
435                                 int sgn = 1;
436                                 if (fp<0.) sgn = -sgn;
437                                 if (f <0.) sgn = -sgn;
438                                 shift = sgn*deltas;
439                                 if (shift == -shiftOld) { // don't get stuck shifting +/-deltas
440                                         deltas *= dampingFactor; // dampen magnitude of shift
441                                         shift = sgn*deltas;
442                                         // allow iterations to run out
443                                 } else {
444                                         i--; // don't count against iterations
445                                 }
446                         } else {
447                                 shift = f/fp;
448                         }
449                         s -= shift;
450                         shiftOld = shift;
451                         if (fabs(sOld-s) < ktMaxPrecisionNeeded) break;
452                         sOld = s;
453                 }
454                 if (i == ktMaxIterations) return fgkNoSolution;
455         }
456         return s;
457 }
458
459 pair<double, double>
460 AliFmHelix::PathLengths(const AliFmHelix& h, bool scanPeriods) const
461 {
462         //
463         //      Cannot handle case where one is a helix
464         //  and the other one is a straight line.
465         //
466         if (fSingularity != h.fSingularity) 
467                 return pair<double, double>(fgkNoSolution, fgkNoSolution);
468
469         double s1, s2;
470
471         if (fSingularity) {
472                 //
473                 //  Analytic solution
474                 //
475                 AliFmThreeVector<double> dv = h.fOrigin - fOrigin;
476                 AliFmThreeVector<double> a(-fCosDipAngle*fSinPhase,
477                         fCosDipAngle*fCosPhase,
478                         fSinDipAngle);
479                 AliFmThreeVector<double> b(-h.fCosDipAngle*h.fSinPhase,
480                         h.fCosDipAngle*h.fCosPhase,
481                         h.fSinDipAngle);        
482                 double ab = a*b;
483                 double g  = dv*a;
484                 double k  = dv*b;
485                 s2 = (k-ab*g)/(ab*ab-1.);
486                 s1 = g+s2*ab;
487                 return pair<double, double>(s1, s2);
488         }
489         else {  
490                 //
491                 //  First step: get dca in the xy-plane as start value
492                 //
493                 double dx = h.XCenter() - XCenter();
494                 double dy = h.YCenter() - YCenter();
495                 double dd = ::sqrt(dx*dx + dy*dy);
496                 double r1 = 1/Curvature();
497                 double r2 = 1/h.Curvature();
498
499                 /* malisa 22apr2006 is commenting out the "isnan" check
500                  *              if ( !finite(r1) || isnan(r1) || !finite(r2) || isnan(r2) ) {
501                  *               cerr << __FUNCTION__ << " *** error *** ";
502                  *                      cerr << "  r1=" << r1;
503                  *                      cerr << "  r2=" << r2 << endl;
504                  *                      return pair<double, double>(fgkNoSolution, fgkNoSolution);
505                  *              }
506                  */
507
508                 double cosAlpha = (r1*r1 + dd*dd - r2*r2)/(2*r1*dd);
509
510                 double s;
511                 double x, y;
512                 if (fabs(cosAlpha) < 1) {           // two solutions
513                         double sinAlpha = sin(acos(cosAlpha));
514                         x = XCenter() + r1*(cosAlpha*dx - sinAlpha*dy)/dd;
515                         y = YCenter() + r1*(sinAlpha*dx + cosAlpha*dy)/dd;
516                         s = PathLength(x, y);
517                         x = XCenter() + r1*(cosAlpha*dx + sinAlpha*dy)/dd;
518                         y = YCenter() + r1*(cosAlpha*dy - sinAlpha*dx)/dd;
519                         double a = PathLength(x, y);
520                         if (h.Distance(At(a),scanPeriods) < h.Distance(At(s),scanPeriods)) s = a;
521                 }
522                 else {                              // no intersection (or exactly one)
523                         x = XCenter() + r1*dx/dd;
524                         y = YCenter() + r1*dy/dd;
525                         s = PathLength(x, y);
526                 }
527
528                 //
529                 //   Second step: scan in decreasing intervals around seed 's'
530                 // 
531                 const double ktMinStepSize = 10*micrometer;
532                 const double ktMinRange    = 10*centimeter;    
533                 double dmin              = h.Distance(At(s),scanPeriods);
534                 double range             = max(2*dmin, ktMinRange);
535
536                 /* malisa comments out the "isnan" check 22apr2006
537                  *              if ( !finite(range) || isnan(range)) {
538                  *                      cerr << __FUNCTION__ << " *** error *** ";
539                  *                      cerr << "  range=" << range << endl;
540                  *                      return pair<double, double>(fgkNoSolution, fgkNoSolution);
541                  *              }
542                  */
543
544                 double ds = range/10.;
545                 double slast=-999999, d;
546                 s1 = s - range/2.;
547                 s2 = s + range/2.;
548
549                 double tmp1;
550                 double tmp2;
551
552                 while (ds > ktMinStepSize) {
553                         if ( !(s1<s2) ) {
554                                 cerr << __FUNCTION__ << " *** error *** s1 = ";
555                                 cerr << "    s2 = " << s2;
556                                 cerr << "    range = " << range;
557                                 cerr << "     ds = " << ds << endl;
558                                 return pair<double, double>(fgkNoSolution, fgkNoSolution);
559                         }       
560                         tmp1 = (s2-s1);
561                         tmp2 = tmp1/10.0;
562                         ds = tmp2;
563                         if ( ds<0) {
564                             cerr << __FUNCTION__ << " *** error *** ds = " << ds << endl;
565                             return pair<double, double>(fgkNoSolution, fgkNoSolution);
566                         }
567                         int iterations = 0;
568                         for (double ss=s1; ss<(s2+ds); ss+=ds) {
569                             iterations++;
570                             if ( iterations > 100 ) {
571                                 cerr << __FUNCTION__ << " *** error *** iterations = " << iterations << endl;
572                                 return pair<double, double>(fgkNoSolution, fgkNoSolution);
573                             }
574                             d = h.Distance(At(ss),scanPeriods);
575                             if (d < dmin) {
576                                 dmin = d;
577                                 s = ss;
578                             }
579                             slast = ss;
580                         }
581                         //
582                         //  In the rare cases where the minimum is at the
583                         //  the border of the current range we shift the range
584                         //  and start all over, i.e we do not decrease 'ds'.
585                         //  Else we decrease the search intervall around the
586                         //  current minimum and redo the scan in smaller steps.
587                         //
588                         if (s == s1) {
589                                 d = 0.8*(s2-s1);
590                                 s1 -= d;
591                                 s2 -= d;
592                         }
593                         else if (s == slast) {
594                                 d = 0.8*(s2-s1);
595                                 s1 += d;
596                                 s2 += d;
597                         }
598                         else {           
599                                 s1 = s-ds;
600                                 s2 = s+ds;
601                         //      ds /= 10;
602                         }
603                 }
604                 return pair<double, double>(s, h.PathLength(At(s),scanPeriods));
605         }
606 }
607
608
609 void AliFmHelix::MoveOrigin(double s)
610 {
611   // Move the helix origin
612         if (fSingularity)
613                 fOrigin = At(s);
614         else {
615                 AliFmThreeVector<double> newOrigin = At(s);
616                 double newPhase = atan2(newOrigin.y() - YCenter(),
617                         newOrigin.x() - XCenter());
618                 fOrigin = newOrigin;
619                 SetPhase(newPhase);             
620         }
621 }
622
623 int operator== (const AliFmHelix& a, const AliFmHelix& b)
624 {
625         //
626         // Checks for numerical identity only !
627         //
628         return (a.Origin()    == b.Origin()    &&
629                 a.DipAngle()  == b.DipAngle()  &&
630                 a.Curvature() == b.Curvature() &&
631                 a.Phase()     == b.Phase()     &&
632                 a.H()         == b.H());
633 }
634
635 int operator!= (const AliFmHelix& a, const AliFmHelix& b) {return !(a == b);}
636
637 ostream& operator<<(ostream& os, const AliFmHelix& h)
638 {
639         return os << '('
640                 << "curvature = "  << h.Curvature() << ", " 
641                 << "dip angle = "  << h.DipAngle()  << ", "
642                 << "phase = "      << h.Phase()     << ", "  
643                 << "h = "          << h.H()         << ", "    
644                 << "origin = "     << h.Origin()    << ')';
645 }
646
647
648
649
650