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