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