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