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