Addons for html documentation
[u/mrichter/AliRoot.git] / HLT / src / AliL3ConfMapFit.cxx
1 // Author: Anders Vestbo <mailto:vestbo@fi.uib.no>
2 //*-- Copyright &copy ASV 
3
4 #include <math.h>
5
6 #include "AliL3Logging.h"
7 #include "AliL3ConfMapFit.h"
8 #include "AliL3Vertex.h"
9 #include "AliL3ConfMapTrack.h"
10 #include "AliL3ConfMapPoint.h"
11
12 //_____________________________________________________________
13 // AliL3ConfMapFit
14 //
15 // Fit class for conformal mapping tracking
16
17 ClassImp(AliL3ConfMapFit)
18
19 Double_t AliL3ConfMapFit::pi=3.14159265358979323846;
20
21 AliL3ConfMapFit::AliL3ConfMapFit(AliL3ConfMapTrack *track,AliL3Vertex *vertex)
22 {
23   //constructor
24   fTrack = track;
25   fVertex = vertex;
26   BFACT = 0.0029980;
27   bField = 0.2;
28   
29 }
30
31 Int_t AliL3ConfMapFit::FitHelix()
32 {
33   if(FitCircle())
34     {
35       LOG(AliL3Log::kError,"AliL3ConfMapFit::FitHelix","TrackFit")<<AliL3Log::kDec<<
36         "Problems during circle fit"<<ENDLOG;
37       return 1;
38     }
39   if(FitLine())
40     {
41       LOG(AliL3Log::kError,"AliL3ConfMapFit::FitHelix","TrackFit")<<AliL3Log::kDec<<
42         "Problems during line fit"<<ENDLOG;
43       return 1;
44     }
45   return 0;
46 }
47
48 Int_t AliL3ConfMapFit::FitCircle()
49 {
50   //-----------------------------------------------------------------
51   //Fits circle parameters using algorithm
52   //described by ChErnov and Oskov in Computer Physics
53   //Communications.
54   // 
55   //Written in FORTRAN by Jawluen Tang, Physics department , UT-Austin 
56   //Moved to C by Pablo Yepes
57   //Moved to AliROOT by ASV.
58   //------------------------------------------------------------------
59   
60   Double_t wsum  = 0.0 ;
61   Double_t xav   = 0.0 ;
62   Double_t yav   = 0.0 ;
63   
64   Int_t num_of_hits = fTrack->GetNumberOfPoints();
65   //
66   //     Loop over hits calculating average
67   Int_t co=0;
68   
69   for(fTrack->StartLoop(); fTrack->LoopDone(); fTrack->GetNextHit())
70     {
71       co++;
72       AliL3ConfMapPoint *cHit = (AliL3ConfMapPoint*)fTrack->currentHit;
73       cHit->SetXYWeight( 1./ (Double_t)(cHit->GetXerr()*cHit->GetXerr() + cHit->GetYerr()*cHit->GetYerr()) );
74       wsum      += cHit->GetXYWeight() ;
75       xav       += cHit->GetXYWeight() * cHit->GetX() ;
76       yav       += cHit->GetXYWeight() * cHit->GetY() ;
77     }
78   if(co!=num_of_hits) 
79     LOG(AliL3Log::kError,"AliL3ConfMapFit::FitCircle","TrackFit")<<AliL3Log::kDec<<
80       "Mismatch of hits. Counter: "<<co<<" nHits: "<<num_of_hits<<ENDLOG;
81   if (fTrack->ComesFromMainVertex() == true)
82     {    
83       wsum += fVertex->GetXYWeight() ;
84       xav  += fVertex->GetX() ;
85       yav  += fVertex->GetY() ;
86     }
87   
88   xav = xav / wsum ;
89   yav = yav / wsum ;
90 //
91 //  CALCULATE <X**2>, <XY>, AND <Y**2> WITH <X> = 0, & <Y> = 0
92 //
93   Double_t xxav  = 0.0 ;
94   Double_t xyav  = 0.0 ; 
95   Double_t yyav  = 0.0 ;
96   Double_t xi, yi ;
97
98   for(fTrack->StartLoop(); fTrack->LoopDone(); fTrack->GetNextHit())
99     { 
100       //AliL3ConfMapPoint *cHit = (AliL3ConfMapPoint *)hits->At(hit_counter);
101       AliL3ConfMapPoint *cHit = (AliL3ConfMapPoint*)fTrack->currentHit;
102       xi        = cHit->GetX() - xav ;
103       yi        = cHit->GetY() - yav ;
104       xxav     += xi * xi * cHit->GetXYWeight() ;
105       xyav     += xi * yi * cHit->GetXYWeight() ;
106       yyav     += yi * yi * cHit->GetXYWeight() ;
107     }
108   
109   if (fTrack->ComesFromMainVertex() == true)
110     {
111       xi        = fVertex->GetX() - xav ;
112       yi        = fVertex->GetY() - yav ;
113       xxav     += xi * xi * fVertex->GetXYWeight() ;
114       xyav     += xi * yi * fVertex->GetXYWeight() ;
115       yyav     += yi * yi * fVertex->GetXYWeight() ; 
116     }
117   xxav = xxav / wsum ;
118   xyav = xyav / wsum ;
119   yyav = yyav / wsum ;
120 //
121 //-->  ROTATE COORDINATES SO THAT <XY> = 0
122 //
123 //-->  SIGN(C**2 - S**2) = SIGN(XXAV - YYAV) >
124 //-->  &                                     > ==> NEW : (XXAV-YYAV) > 0
125 //-->  SIGN(S) = SIGN(XYAV)                  >
126
127   Double_t a = fabs( xxav - yyav ) ;
128   Double_t b = 4.0 * xyav * xyav ;
129
130   Double_t asqpb  = a * a + b  ;
131   Double_t rasqpb = sqrt ( asqpb) ;
132
133   Double_t splus  = 1.0 + a / rasqpb ;
134   Double_t sminus = b / (asqpb * splus) ;
135
136   splus  = sqrt (0.5 * splus ) ;
137   sminus = sqrt (0.5 * sminus) ;
138 //
139 //->  FIRST REQUIRE : SIGN(C**2 - S**2) = SIGN(XXAV - YYAV)
140 //
141   Double_t sinrot, cosrot ;
142   if ( xxav <= yyav ) {
143          cosrot = sminus ;
144          sinrot = splus  ;
145   }
146   else {
147           cosrot = splus ;
148           sinrot = sminus ;
149   }
150 //
151 //->  REQUIRE : SIGN(S) = SIGN(XYAV) * SIGN(C) (ASSUMING SIGN(C) > 0)
152 //
153   if ( xyav < 0.0 ) sinrot = - sinrot ;
154 //
155 //-->  WE NOW HAVE THE SMALLEST ANGLE THAT GUARANTEES <X**2> > <Y**2>
156 //-->  TO GET THE SIGN OF THE CHARGE RIGHT, THE NEW X-AXIS MUST POINT
157 //-->  OUTWARD FROM THE ORGIN.  WE ARE FREE TO CHANGE SIGNS OF BOTH
158 //-->  COSROT AND SINROT SIMULTANEOUSLY TO ACCOMPLISH THIS.
159 //
160 //-->  CHOOSE SIGN OF C WISELY TO BE ABLE TO GET THE SIGN OF THE CHARGE
161 //
162   if ( cosrot*xav+sinrot*yav < 0.0 ) {
163           cosrot = -cosrot ;
164           sinrot = -sinrot ;
165   }
166 //
167 //->  NOW GET <R**2> AND RSCALE= SQRT(<R**2>)
168 //
169   Double_t rrav   = xxav + yyav ;
170   Double_t rscale = sqrt(rrav) ;
171
172   xxav   = 0.0 ;
173   yyav   = 0.0 ;
174   xyav   = 0.0 ;
175   Double_t xrrav = 0.0 ;
176   Double_t yrrav = 0.0 ;
177   Double_t rrrrav  = 0.0 ;
178
179   Double_t xixi, yiyi, riri, wiriri, xold, yold ;
180   
181   //for (hit_counter=0; hit_counter<num_of_hits; hit_counter++) 
182   for(fTrack->StartLoop(); fTrack->LoopDone(); fTrack->GetNextHit())  
183     { 
184       //AliL3ConfMapPoint *cHit = (AliL3ConfMapPoint*)hits->At(hit_counter);  
185       AliL3ConfMapPoint* cHit = (AliL3ConfMapPoint*)fTrack->currentHit;
186
187       xold = cHit->GetX() - xav ;
188       yold = cHit->GetY() - yav ;
189       //
190       //-->  ROTATE SO THAT <XY> = 0 & DIVIDE BY RSCALE SO THAT <R**2> = 1
191       //
192       xi = (  cosrot * xold + sinrot * yold ) / rscale ;
193       yi = ( -sinrot * xold + cosrot * yold ) / rscale ;
194       
195       xixi   = xi * xi ;
196       yiyi   = yi * yi ;
197       riri   = xixi + yiyi ;
198       wiriri = cHit->GetXYWeight() * riri ;
199       
200       xyav   += cHit->GetXYWeight() * xi * yi ;
201       xxav   += cHit->GetXYWeight() * xixi ;
202       yyav   += cHit->GetXYWeight() * yiyi ;
203       
204       xrrav  += wiriri * xi ;
205       yrrav  += wiriri * yi ;
206       rrrrav += wiriri * riri ;
207     }
208   //
209 //   Include vertex if required
210 //
211   if (fTrack->ComesFromMainVertex() == true)
212     {
213         xold = fVertex->GetX() - xav ;
214         yold = fVertex->GetY() - yav ;
215         //
216         //-->  ROTATE SO THAT <XY> = 0 & DIVIDE BY RSCALE SO THAT <R**2> = 1
217         //
218         xi = (  cosrot * xold + sinrot * yold ) / rscale ;
219         yi = ( -sinrot * xold + cosrot * yold ) / rscale ;
220         
221         xixi   = xi * xi ;
222         yiyi   = yi * yi ;
223         riri   = xixi + yiyi ;
224         wiriri = fVertex->GetXYWeight() * riri ;
225
226         xyav   += fVertex->GetXYWeight() * xi * yi ;
227         xxav   += fVertex->GetXYWeight() * xixi ;
228         yyav   += fVertex->GetXYWeight() * yiyi ;
229
230         xrrav  += wiriri * xi ;
231         yrrav  += wiriri * yi ;
232         rrrrav += wiriri * riri ;
233   }
234   //
235   //    
236   //
237   //-->  DIVIDE BY WSUM TO MAKE AVERAGES
238   //
239   xxav    = xxav   / wsum ;
240   yyav    = yyav   / wsum ;
241   xrrav   = xrrav  / wsum ;
242   yrrav   = yrrav  / wsum ;
243   rrrrav  = rrrrav / wsum ;
244   xyav    = xyav   / wsum ;
245
246   Int_t const ntry = 5 ;
247 //
248 //-->  USE THESE TO GET THE COEFFICIENTS OF THE 4-TH ORDER POLYNIMIAL
249 //-->  DON'T PANIC - THE THIRD ORDER TERM IS ZERO !
250 //
251   Double_t xrrxrr = xrrav * xrrav ;
252   Double_t yrryrr = yrrav * yrrav ;
253   Double_t rrrrm1 = rrrrav - 1.0  ;
254   Double_t xxyy   = xxav  * yyav  ;        
255
256   Double_t c0  =          rrrrm1*xxyy - xrrxrr*yyav - yrryrr*xxav ;
257   Double_t c1  =        - rrrrm1      + xrrxrr      + yrryrr   - 4.0*xxyy ;        
258   Double_t c2  =   4.0  + rrrrm1                               - 4.0*xxyy ;           
259   Double_t c4  = - 4.0  ;                
260 //
261 //-->  COEFFICIENTS OF THE DERIVATIVE - USED IN NEWTON-RAPHSON ITERATIONS
262 //
263   Double_t c2d =   2.0 * c2 ;
264   Double_t c4d =   4.0 * c4 ;
265 //
266 //-->  0'TH VALUE OF LAMDA - LINEAR INTERPOLATION BETWEEN P(0) & P(YYAV)
267 //
268 //   LAMDA = YYAV * C0 / (C0 + YRRSQ * (XXAV-YYAV))
269   Double_t lamda  = 0.0 ;
270   Double_t dlamda = 0.0 ;
271 //
272   Double_t chiscl = wsum * rscale * rscale ;
273   Double_t dlamax = 0.001 / chiscl ;   
274    
275   Double_t p, pd ;
276   for ( int itry = 1 ; itry <= ntry ; itry++ ) {
277      p      = c0 + lamda * (c1 + lamda * (c2 + lamda * lamda * c4 )) ;
278      pd     = (c1 + lamda * (c2d + lamda * lamda * c4d)) ;
279      dlamda = -p / pd ;
280      lamda  = lamda + dlamda ;
281      if (fabs(dlamda)<   dlamax) break ;
282   }
283
284   Double_t chi2 = (Double_t)(chiscl * lamda) ;
285  
286   fTrack->SetChiSq1(chi2);
287   // Double_t dchisq = chiscl * dlamda ;             
288 //
289 //-->  NOW CALCULATE THE MATRIX ELEMENTS FOR ALPHA, BETA & KAPPA
290 //
291   Double_t h11   = xxav  -     lamda ;
292   Double_t h14   = xrrav ;
293   Double_t h22   = yyav  -     lamda ; 
294   Double_t h24   = yrrav ;
295   Double_t h34   = 1.0   + 2.0*lamda ;
296   if ( h11 == 0.0 || h22 == 0.0 ){
297     LOG(AliL3Log::kError,"AliL3ConfMapFit::FitCircle","TrackFit")<<AliL3Log::kDec<<
298       "Problems fitting circle"<<ENDLOG;
299     return 1 ;
300   }
301   Double_t rootsq = (h14*h14)/(h11*h11) + 4.0*h34 ;
302
303   Double_t ratio, kappa, beta ;
304   if ( fabs(h22) > fabs(h24) ) {
305      ratio  = h24 / h22 ;
306      rootsq = ratio * ratio + rootsq ;
307      kappa = 1.0 / sqrt(rootsq) ;
308      beta  = - ratio * kappa ;
309   }
310   else {
311      ratio  = h22 / h24 ;
312      rootsq = 1.0 + ratio * ratio * rootsq ;
313      beta  = 1.0 / sqrt(rootsq) ;
314      if ( h24 > 0 ) beta = - beta ;
315      kappa = -ratio * beta ;
316   }            
317   Double_t alpha = - (h14/h11) * kappa ;
318 //
319 //-->  transform these into the lab coordinate system
320 //-->  first get kappa and back to real dimensions
321 //
322   Double_t kappa1 = kappa / rscale ;
323   Double_t dbro   = 0.5   / kappa1 ;
324 //
325 //-->  next rotate alpha and beta and scale
326 //
327   Double_t alphar = (cosrot * alpha - sinrot * beta)* dbro ;
328   Double_t betar  = (sinrot * alpha + cosrot * beta)* dbro ;
329 //
330 //-->  then translate by (xav,yav)
331 //
332   Double_t acent  = (double)(xav - alphar) ;
333   Double_t bcent  = (double)(yav - betar ) ;
334   Double_t radius = (double)dbro ;
335 //
336 //   Get charge
337 //
338   Int_t q = ( ( yrrav < 0 ) ? 1 : -1 ) ;
339
340   fTrack->SetCharge(q);
341   
342 //
343 //    Get other track parameters
344 //
345   Double_t x0, y0,phi0,r0,psi,pt ;
346   if ( fTrack->ComesFromMainVertex() == true ) 
347     {
348       //flag = 1 ; // primary track flag
349       x0   = fVertex->GetX() ;
350       y0   = fVertex->GetY() ;
351       phi0 = fVertex->GetPhi() ;
352       r0   = fVertex->GetR() ;
353       fTrack->SetPhi0(phi0);
354       fTrack->SetR0(r0);
355     } 
356   else 
357     {
358       //AliL3ConfMapPoint *lHit = (AliL3ConfMapPoint*)hits->Last();
359       AliL3ConfMapPoint *lHit = (AliL3ConfMapPoint*)fTrack->lastHit;
360       //flag =  0 ; // primary track flag
361       x0   =  lHit->GetX()  ;
362       y0   =  lHit->GetY()  ;
363       phi0 =  atan2(lHit->GetY(),lHit->GetX());
364       if ( phi0 < 0 ) phi0 += 2*pi;
365       r0   =  sqrt ( lHit->GetX() * lHit->GetX() + lHit->GetY() * lHit->GetY() )  ;
366       fTrack->SetPhi0(phi0);
367       fTrack->SetR0(r0);
368     }
369   //
370   psi  = (Double_t)atan2(bcent-y0,acent-x0) ;
371   psi  = psi + q * 0.5F * pi ;
372   if ( psi < 0 ) psi = psi + 2*pi;
373   
374   pt   = (Double_t)(BFACT * bField * radius ) ;
375   fTrack->SetPsi(psi);
376   fTrack->SetPt(pt);
377
378   //
379 //    Get errors from fast fit
380 //
381   //if ( getPara()->getErrors ) getErrorsCircleFit ( acent, bcent, radius ) ;
382 //
383   return 0 ;
384   
385 }
386
387 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
388 //    Fit Line in s-z plane
389 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
390 Int_t AliL3ConfMapFit::FitLine ( )
391 {
392   //
393   //Initialization 
394   //
395   Double_t sum = 0.F ;
396   Double_t ss  = 0.F ;
397   Double_t sz  = 0.F ;
398   Double_t sss = 0.F ;
399   Double_t ssz = 0.F ;
400   //
401   //find sum , sums ,sumz, sumss 
402   // 
403   Double_t dx, dy ;
404   Double_t radius = (Double_t)(fTrack->GetPt() / ( BFACT * bField ) ) ;
405
406   //TObjArray *hits = fTrack->GetHits();
407   //Int_t num_of_hits = fTrack->GetNumberOfPoints();
408
409   if ( fTrack->ComesFromMainVertex() == true ) 
410     {
411       dx = ((AliL3ConfMapPoint*)fTrack->firstHit)->GetX() - fVertex->GetX();
412       dy = ((AliL3ConfMapPoint*)fTrack->firstHit)->GetY() - fVertex->GetY() ;
413     }
414   else 
415     {
416       dx = ((AliL3ConfMapPoint *)fTrack->firstHit)->GetX() - ((AliL3ConfMapPoint *)fTrack->lastHit)->GetX() ;
417       dy = ((AliL3ConfMapPoint *)fTrack->firstHit)->GetY() - ((AliL3ConfMapPoint *)fTrack->lastHit)->GetY() ;
418       //dx = ((AliL3ConfMapPoint *)hits->First())->GetX() - ((AliL3ConfMapPoint *)hits->Last())->GetX() ;
419       //dy = ((AliL3ConfMapPoint *)hits->First())->GetY() - ((AliL3ConfMapPoint *)hits->Last())->GetY() ;
420     }
421   
422   Double_t localPsi = 0.5F * sqrt ( dx*dx + dy*dy ) / radius ;
423   Double_t total_s ;
424   
425   if ( fabs(localPsi) < 1. ) 
426     {
427       total_s = 2.0 * radius * asin ( localPsi ) ;
428     } 
429   else 
430     { 
431       total_s = 2.0 * radius * pi ;
432     } 
433   
434   AliL3ConfMapPoint *previousHit = NULL;
435
436   // FtfBaseHit *previousHit = 0  ;
437   
438   //for ( startLoop() ; done() ; nextHit() ) {
439   Double_t dpsi,s;
440
441   //  for(hit_counter=0; hit_counter<num_of_hits; hit_counter++)
442   for(fTrack->StartLoop(); fTrack->LoopDone(); fTrack->GetNextHit())  
443     {
444       // AliL3ConfMapPoint *cHit = (AliL3ConfMapPoint*)hits->At(hit_counter);
445       AliL3ConfMapPoint *cHit = (AliL3ConfMapPoint*)fTrack->currentHit;
446       // if ( currentHit != firstHit ) 
447       if(cHit != fTrack->firstHit)//  hits->First())
448         {
449           dx   = cHit->GetX() - previousHit->GetX() ;
450           dy   = cHit->GetY() - previousHit->GetY() ;
451           dpsi = 0.5 * (Double_t)sqrt ( dx*dx + dy*dy ) / radius ;
452           fTrack->SetPsierr(dpsi);
453           s = previousHit->GetS() - 2.0 * radius * (Double_t)asin ( dpsi ) ;
454           cHit->SetS(s);
455         }
456       else
457         cHit->SetS(total_s);
458       //        cHit->s = total_s ;
459     
460       sum += cHit->GetZWeight() ;
461       ss  += cHit->GetZWeight() * cHit->GetS() ;
462       sz  += cHit->GetZWeight() * cHit->GetZ() ;
463       sss += cHit->GetZWeight() * cHit->GetS() * cHit->GetS() ;
464       ssz += cHit->GetZWeight() * cHit->GetS() * cHit->GetZ() ;
465       previousHit = cHit ;
466     }
467   
468   Double_t chi2,det = sum * sss - ss * ss;
469   if ( fabs(det) < 1e-20)
470     { 
471       chi2 = 99999.F ;
472       fTrack->SetChiSq2(chi2);
473       return 0 ;
474     }
475   
476   //Compute the best fitted parameters A,B
477   Double_t tanl,z0,dtanl,dz0;
478
479   tanl = (Double_t)((sum * ssz - ss * sz ) / det );
480   z0   = (Double_t)((sz * sss - ssz * ss ) / det );
481
482   fTrack->SetTgl(tanl);
483   fTrack->SetZ0(z0);
484   
485   //     calculate chi-square 
486   
487   chi2 = 0.;
488   Double_t r1 ;
489   
490   //for(hit_counter=0; hit_counter<num_of_hits; hit_counter++)
491   for(fTrack->StartLoop(); fTrack->LoopDone(); fTrack->GetNextHit())  
492     {
493       //AliL3ConfMapPoint *cHit = (AliL3ConfMapPoint*)hits->At(hit_counter);
494       AliL3ConfMapPoint *cHit = (AliL3ConfMapPoint*)fTrack->currentHit;
495       r1   = cHit->GetZ() - tanl * cHit->GetS() - z0 ;
496       chi2 += (Double_t) ( (Double_t)cHit->GetZWeight() * (r1 * r1) );
497     }
498   fTrack->SetChiSq2(chi2);
499   //
500   //     calculate estimated variance
501   //      varsq=chi/(double(n)-2.) 
502   //     calculate covariance matrix 
503   //      siga=sqrt(varsq*sxx/det) 
504   //      sigb=sqrt(varsq*sum/det) 
505   //
506   dtanl = (Double_t) ( sum / det );
507   dz0   = (Double_t) ( sss / det );
508   
509   fTrack->SetTglerr(dtanl);
510   fTrack->SetZ0err(dz0);
511   
512   return 0 ;
513