Adding AliRieman fitter (M.Ivanov)
[u/mrichter/AliRoot.git] / STEER / AliRieman.cxx
1 #include "TTree.h"
2 #include "TMatrixDSym.h"
3 #include "TMatrixD.h"
4 #include "AliRieman.h"
5 #include "TRandom.h"
6
7 ClassImp(AliRieman)
8
9
10
11 AliRieman::AliRieman(){
12   //
13   // default constructor
14   //
15   for (Int_t i=0;i<6;i++) fParams[i] = 0;
16   for (Int_t i=0;i<9;i++) fSumXY[i] = 0;
17   for (Int_t i=0;i<9;i++) fSumXZ[i] = 0;
18
19   fCapacity = 0;
20   fN =0;
21   fX  = 0;
22   fY  = 0;
23   fZ  = 0;
24   fSy = 0;
25   fSz = 0;
26   fCovar = 0;
27   fConv = kFALSE;
28 }
29
30
31 AliRieman::AliRieman(Int_t capacity){
32   //
33   // default constructor
34   //
35   for (Int_t i=0;i<6;i++) fParams[i] = 0;
36   for (Int_t i=0;i<9;i++) fSumXY[i] = 0;
37   for (Int_t i=0;i<9;i++) fSumXZ[i] = 0;
38
39   fCapacity = capacity;
40   fN =0;
41   fX  = new Float_t[fCapacity];
42   fY  = new Float_t[fCapacity];
43   fZ  = new Float_t[fCapacity];
44   fSy = new Float_t[fCapacity];
45   fSz = new Float_t[fCapacity];
46   fCovar = new TMatrixDSym(6);
47   fConv = kFALSE;
48 }
49
50 AliRieman::AliRieman(const AliRieman &rieman):TObject(){
51   //
52   // copy constructor
53   // 
54   for (Int_t i=0;i<6;i++) fParams[i] = rieman.fParams[i];
55   for (Int_t i=0;i<9;i++) fSumXY[i]  = rieman.fSumXY[i];
56   for (Int_t i=0;i<9;i++) fSumXZ[i]  = rieman.fSumXZ[i];
57   fCapacity = rieman.fN;
58   fN =rieman.fN;
59   fCovar = new TMatrixDSym(*(rieman.fCovar)); 
60   fConv = rieman.fConv;
61   fX  = new Float_t[fN];
62   fY  = new Float_t[fN];
63   fZ  = new Float_t[fN];
64   fSy = new Float_t[fN];
65   fSz = new Float_t[fN];
66   for (Int_t i=0;i<rieman.fN;i++){
67     fX[i]   = rieman.fX[i];
68     fY[i]   = rieman.fY[i];
69     fZ[i]   = rieman.fZ[i];
70     fSy[i]  = rieman.fSy[i];
71     fSz[i]  = rieman.fSz[i];
72   }
73 }
74
75 AliRieman::~AliRieman()
76 {
77   delete[]fX;
78   delete[]fY;
79   delete[]fZ;
80   delete[]fSy;
81   delete[]fSz;
82   delete fCovar;
83 }
84
85 void AliRieman::Reset()
86 {
87   fN=0;
88   for (Int_t i=0;i<6;i++) fParams[i] = 0;
89   for (Int_t i=0;i<9;i++) fSumXY[i] = 0;
90   for (Int_t i=0;i<9;i++) fSumXZ[i] = 0;
91   fConv =kFALSE;
92 }
93
94
95 void AliRieman::AddPoint(Float_t x, Float_t y, Float_t z, Float_t sy, Float_t sz)
96 {
97   //
98   //  Rieman update
99   //
100   //------------------------------------------------------
101   // XY direction
102   //
103   //  (x-x0)^2+(y-y0)^2-R^2=0 ===>
104   //
105   //  (x^2+y^2 -2*x*x0 - 2*y*y0+ x0^2 -y0^2 -R^2 =0;  ==>
106   //
107   //   substitution t = 1/(x^2+y^2),   u = 2*x*t, y = 2*y*t,  D0 = R^2 - x0^2- y0^2
108   //
109   //  1 - u*x0 - v*y0 - t *D0 =0 ;  - linear equation
110   //     
111   //  next substition   a = 1/y0    b = -x0/y0   c = -D0/y0
112   //
113   //  final linear equation :   a + u*b +t*c - v =0;
114   //
115   // Minimization :
116   //
117   // sum( (a + ui*b +ti*c - vi)^2)/(sigmai)^2 = min;
118   //
119   // where sigmai is the error of  maesurement  (a + ui*b +ti*c - vi)
120   //
121   // neglecting error of xi, and supposing  xi>>yi    sigmai ~ sigmaVi ~ 2*sigmay*t  
122   //
123   if (fN==fCapacity-1) return;  // out of capacity
124   fX[fN] = x; fY[fN]=y; fZ[fN]=z; fSy[fN]=sy; fSz[fN]=sz;
125   //
126   // XY part
127   //
128   Double_t  t  =  x*x+y*y;
129   if (t<2) return;
130   t            = 1./t;
131   Double_t  u  =  2.*x*t;
132   Double_t  v  =  2.*y*t;
133   Double_t  error = 2.*sy*t;
134   error *=error;
135   Double_t weight = 1./error;
136   fSumXY[0] +=weight;
137   fSumXY[1] +=u*weight;      fSumXY[2]+=v*weight;  fSumXY[3]+=t*weight;
138   fSumXY[4] +=u*u*weight;    fSumXY[5]+=t*t*weight;
139   fSumXY[6] +=u*v*weight;    fSumXY[7]+=u*t*weight; fSumXY[8]+=v*t*weight;
140   //
141   // XZ part
142   //
143   weight = 1./sz;
144   fSumXZ[0] +=weight;
145   fSumXZ[1] +=weight*x;   fSumXZ[2] +=weight*x*x; fSumXZ[3] +=weight*x*x*x; fSumXZ[4] += weight*x*x*x*x;
146   fSumXZ[5] +=weight*z;   fSumXZ[6] +=weight*x*z; fSumXZ[7] +=weight*x*x*z;
147   //
148   fN++;
149 }
150
151
152
153
154
155 void AliRieman::UpdatePol(){
156   //
157   //  Rieman update
158   //
159   //
160   if (fN==0) return;
161   for (Int_t i=0;i<6;i++)fParams[i]=0;
162   Int_t conv=0;
163   //
164   // XY part
165   //
166   TMatrixDSym     smatrix(3);
167   TMatrixD        sums(1,3);
168   //
169   //   smatrix(0,0) = s0; smatrix(1,1)=su2; smatrix(2,2)=st2;
170   //   smatrix(0,1) = su; smatrix(0,2)=st; smatrix(1,2)=sut;
171   //   sums(0,0)    = sv; sums(0,1)=suv; sums(0,2)=svt;
172
173   smatrix(0,0) = fSumXY[0]; smatrix(1,1)=fSumXY[4]; smatrix(2,2)=fSumXY[5];
174   smatrix(0,1) = fSumXY[1]; smatrix(0,2)=fSumXY[3]; smatrix(1,2)=fSumXY[7];
175   sums(0,0)    = fSumXY[2]; sums(0,1)   =fSumXY[6]; sums(0,2)   =fSumXY[8];
176   smatrix.Invert();
177   if (smatrix.IsValid()){
178     for (Int_t i=0;i<3;i++)
179       for (Int_t j=0;j<=i;j++){
180         (*fCovar)(i,j)=smatrix(i,j);
181       }
182     TMatrixD  res = sums*smatrix;
183     fParams[0] = res(0,0);
184     fParams[1] = res(0,1);
185     fParams[2] = res(0,2);
186     conv++;
187   }
188   //
189   // XZ part
190   //
191   TMatrixDSym     smatrixz(3);
192   smatrixz(0,0) = fSumXZ[0]; smatrixz(0,1) = fSumXZ[1]; smatrixz(0,2) = fSumXZ[2];
193   smatrixz(1,1) = fSumXZ[2]; smatrixz(1,2) = fSumXZ[3];
194   smatrixz(2,2) = fSumXZ[4];
195   smatrixz.Invert();
196   if (smatrixz.IsValid()){
197     sums(0,0)    = fSumXZ[5];
198     sums(0,1)    = fSumXZ[6];
199     sums(0,2)    = fSumXZ[7];
200     TMatrixD res = sums*smatrixz;
201     fParams[3] = res(0,0);
202     fParams[4] = res(0,1);
203     fParams[5] = res(0,2);
204     for (Int_t i=0;i<3;i++)
205       for (Int_t j=0;j<=i;j++){
206         (*fCovar)(i+2,j+2)=smatrixz(i,j);
207     }
208     conv++;
209   }
210
211   //  (x-x0)^2+(y-y0)^2-R^2=0 ===>
212   //
213   //  (x^2+y^2 -2*x*x0 - 2*y*y0+ x0^2 -y0^2 -R^2 =0;  ==>
214   //   substitution t = 1/(x^2+y^2),   u = 2*x*t, y = 2*y*t,  D0 = R^2 - x0^2- y0^2
215   //
216   //  1 - u*x0 - v*y0 - t *D0 =0 ;  - linear equation
217   //     
218   //  next substition   a = 1/y0    b = -x0/y0   c = -D0/y0
219   //  final linear equation :   a + u*b +t*c - v =0;
220   //
221   //  fParam[0]  = 1/y0
222   //  fParam[1]  = -x0/y0
223   //  fParam[2]  = - (R^2 - x0^2 - y0^2)/y0
224   if (conv>1) fConv =kTRUE;
225   else
226     fConv=kFALSE;
227 }
228
229 void AliRieman::Update(){
230   //
231   //  Rieman update
232   //
233   //
234   if (fN==0) return;
235   for (Int_t i=0;i<6;i++)fParams[i]=0;
236   Int_t conv=0;
237   //
238   // XY part
239   //
240   TMatrixDSym     smatrix(3);
241   TMatrixD        sums(1,3);
242   //
243   //   smatrix(0,0) = s0; smatrix(1,1)=su2; smatrix(2,2)=st2;
244   //   smatrix(0,1) = su; smatrix(0,2)=st; smatrix(1,2)=sut;
245   //   sums(0,0)    = sv; sums(0,1)=suv; sums(0,2)=svt;
246
247   smatrix(0,0) = fSumXY[0]; smatrix(1,1)=fSumXY[4]; smatrix(2,2)=fSumXY[5];
248   smatrix(0,1) = fSumXY[1]; smatrix(0,2)=fSumXY[3]; smatrix(1,2)=fSumXY[7];
249   sums(0,0)    = fSumXY[2]; sums(0,1)   =fSumXY[6]; sums(0,2)   =fSumXY[8];
250   smatrix.Invert();
251   if (smatrix.IsValid()){
252     for (Int_t i=0;i<3;i++)
253       for (Int_t j=0;j<3;j++){
254         (*fCovar)(i,j)=smatrix(i,j);
255       }
256     TMatrixD  res = sums*smatrix;
257     fParams[0] = res(0,0);
258     fParams[1] = res(0,1);
259     fParams[2] = res(0,2);
260     conv++;
261   }
262   if (conv==0){
263     fConv = kFALSE;   //non converged
264     return;
265   }
266   //
267   // XZ part
268   //
269   Double_t x0 = -fParams[1]/fParams[0];
270   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
271   Double_t sumXZ[9];
272
273   for (Int_t i=0;i<9;i++) sumXZ[i]=0;
274   for (Int_t i=0;i<fN;i++){
275     Double_t phi  = TMath::ASin((fX[i]-x0)*Rm1);
276     Double_t phi0 = TMath::ASin((0.-x0)*Rm1);
277     Double_t weight = 1/fSz[i];
278     weight *=weight;
279     Double_t dphi = (phi-phi0)/Rm1;
280     sumXZ[0] +=weight;
281     sumXZ[1] +=weight*dphi;
282     sumXZ[2] +=weight*dphi*dphi;
283     sumXZ[3] +=weight*fZ[i];
284     sumXZ[4] +=weight*dphi*fZ[i];
285
286   }
287
288   TMatrixDSym     smatrixz(2);
289   TMatrixD        sumsz(1,2);
290   smatrixz(0,0) = sumXZ[0]; smatrixz(0,1) = sumXZ[1]; smatrixz(1,1) = sumXZ[2];
291   smatrixz.Invert();
292   if (smatrixz.IsValid()){
293     sumsz(0,0)    = sumXZ[3];
294     sumsz(0,1)    = sumXZ[4];
295     TMatrixD res = sumsz*smatrixz;
296     fParams[3] = res(0,0);
297     fParams[4] = res(0,1);
298     for (Int_t i=0;i<2;i++)
299       for (Int_t j=0;j<2;j++){
300         (*fCovar)(i+3,j+3)=smatrixz(i,j);
301     }
302     conv++;
303   }
304
305   //  (x-x0)^2+(y-y0)^2-R^2=0 ===>
306   //
307   //  (x^2+y^2 -2*x*x0 - 2*y*y0+ x0^2 -y0^2 -R^2 =0;  ==>
308   //   substitution t = 1/(x^2+y^2),   u = 2*x*t, y = 2*y*t,  D0 = R^2 - x0^2- y0^2
309   //
310   //  1 - u*x0 - v*y0 - t *D0 =0 ;  - linear equation
311   //     
312   //  next substition   a = 1/y0    b = -x0/y0   c = -D0/y0
313   //  final linear equation :   a + u*b +t*c - v =0;
314   //
315   //  fParam[0]  = 1/y0
316   //  fParam[1]  = -x0/y0
317   //  fParam[2]  = - (R^2 - x0^2 - y0^2)/y0
318   if (conv>1) fConv =kTRUE;
319   else
320     fConv=kFALSE;
321 }
322
323
324
325 Double_t AliRieman::GetYat(Double_t x){
326   if (!fConv) return 0.;
327   Double_t res2 = (x*fParams[0]+fParams[1]);
328   res2*=res2;
329   res2 = 1.-fParams[2]*fParams[0]+fParams[1]*fParams[1]-res2;
330   if (res2<0) return 0;
331   res2 = TMath::Sqrt(res2);
332   res2 = (1-res2)/fParams[0];
333   return res2;
334 }
335
336 Double_t AliRieman::GetDYat(Double_t x){
337   if (!fConv) return 0.;
338   Double_t x0 = -fParams[1]/fParams[0];
339   if (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<0) return 0;
340   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
341   if ( 1./(Rm1*Rm1)-(x-x0)*(x-x0)<=0) return 0;
342   Double_t res = (x-x0)/TMath::Sqrt(1./(Rm1*Rm1)-(x-x0)*(x-x0));
343   if (fParams[0]<0) res*=-1.;
344   return res;
345 }
346
347
348
349 Double_t AliRieman::GetZat(Double_t x){
350   if (!fConv) return 0.;
351   Double_t x0 = -fParams[1]/fParams[0];
352   if (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<=0) return 0;
353   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
354   Double_t phi  = TMath::ASin((x-x0)*Rm1);
355   Double_t phi0 = TMath::ASin((0.-x0)*Rm1);
356   Double_t dphi = (phi-phi0);
357   Double_t res = fParams[3]+fParams[4]*dphi/Rm1;
358   return res;
359 }
360
361 Double_t AliRieman::GetDZat(Double_t x){
362   if (!fConv) return 0.;
363   Double_t x0 = -fParams[1]/fParams[0]; 
364   if  (-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1<=0) return 0;
365   Double_t Rm1  = fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1); 
366   Double_t res = fParams[4]/TMath::Cos(TMath::ASin((x-x0)*Rm1));
367   return res;
368 }
369
370
371 Double_t AliRieman::GetC(){
372   return fParams[0]/TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1);
373 }
374
375