0f7e4e7b41a398eb16d2d96e42bbb0dc9c2a50a2
[u/mrichter/AliRoot.git] / ITS / AliITSAlignmentModule.cxx
1 /* Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
2  * See cxx source for full Copyright notice                               */
3 /* $Id$ */
4 /* $Author$ */
5 /* $Date$ */
6 /* $Name$ */
7 /* $Header$ */
8 /*
9    $Log$
10    Revision 1.1.2.3  2000/06/04 16:35:09  nilsen
11    One more try to fix log comments.
12
13    Revision 1.1.2.2  2000/03/04 23:39:36  nilsen
14    Fixed the logs???
15  */
16 /* Revision 1.1.2.1  2000/03/02 20:12:23  nilsen */
17 /* A new class usefull for ITS detector Alignment Studdies */
18 /* */
19 /* $Revision$ */
20
21 // Standard C & C++ libraries
22 #include <TObject.h>
23
24 // Standard Root Libraries
25 #include <TParticle.h>
26
27 // ITS libraries
28 #include "AliITSgeom.h"
29 #include "AliITSAlignmentTrack.h"
30 #include "AliITSAlignmentModule.h"
31
32 ClassImp(AliITSAlignmentModule)
33
34 //______________________________________________________________________
35 AliITSAlignmentModule::AliITSAlignmentModule(){
36
37     findex = -1;
38     flay   = -1;
39     flad   = -1;
40     fdet   = -1;
41     fx0[0] = 0.0;
42     fx0[1] = 0.0;
43     fx0[2] = 0.0;
44     fangles[0] = 0.0;
45     fangles[1] = 0.0;
46     fangles[2] = 0.0;
47     fM[0][0]   = 0.0;
48     fM[0][1]   = 0.0;
49     fM[0][2]   = 0.0;
50     fM[1][0]   = 0.0;
51     fM[1][1]   = 0.0;
52     fM[1][2]   = 0.0;
53     fM[2][0]   = 0.0;
54     fM[2][1]   = 0.0;
55     fM[2][2]   = 0.0;
56     ftrksM =0;
57     fChi2 = 0.0;
58 }
59 //______________________________________________________________________
60 AliITSAlignmentModule::AliITSAlignmentModule(Int_t index,AliITSgeom *gm,
61                                          Int_t ntrk,AliITSAlignmentTrack *trk){
62     Float_t x,y,z,n;
63     Int_t   i,j;
64
65     findex = index;
66     gm->GetModuleId(index,flay,flad,fdet);
67     gm->GetRotMatrix(index,(Double_t *) &(fM[0][0]));
68     gm->GetTrans(flay,flad,fdet,x,y,z);
69     fx0[0] = (Double_t )x;
70     fx0[1] = (Double_t )y;
71     fx0[2] = (Double_t )z;
72     gm->GetAngles(flay,flad,fdet,x,y,z);
73     fangles[0] = (Double_t )x;
74     fangles[1] = (Double_t )y;
75     fangles[2] = (Double_t )z;
76     ftrksM = new TObjArray();
77     fChi2 = 0.0;
78
79     for(i=0;i<ntrk;i++){
80         n = 0;
81         for(j=0;j<trk[i].GetNumberOfClustersSl();j++){
82             if(trk[i].GetIndex(j,index)==findex){
83                 ftrksM->AddAtFree((TObject *) &trk[i]);
84                 break; // break out of the j loop
85             } // end if
86         } // end for j
87     } // end for i
88 }
89 //______________________________________________________________________
90 Double_t AliITSAlignmentModule::ComputeChi2(){
91     Float_t n;
92     Int_t   i,j,ntrk,ntr;
93
94     ntrk = ftrksM->GetEntriesFast();
95     fChi2 = 0.0;
96     for(i=0;i<ntrk;i++){
97         n = 0;
98         ntr =((AliITSAlignmentTrack *)(ftrksM->At(i)))->GetNumberOfClustersSl();
99         for(j=0;j<ntr;j++){
100                 fChi2 += (Double_t) ((AliITSAlignmentTrack *)(ftrksM->At(i)))->
101                                      GetChi2();
102                 n++;
103         } // end for j
104         if(n==0){
105             fChi2 = -1.0;
106         }else{
107             fChi2 /= (Double_t) n;
108         } // end if n==0
109     } // end for i
110     return fChi2;
111 }
112 //______________________________________________________________________
113 void AliITSAlignmentModule::lnsrch(Int_t npar,Double_t *xold,Double_t fold,
114                                    Double_t *g,Double_t *p,Double_t *x,
115                                    Double_t &f, Double_t stpmax,Int_t &check){
116     Double_t ALF = 1.0e-4, TOLX = 1.0E-7;
117
118     Int_t    i;
119     Double_t a,alam,alam2=0.0,alamin,b,disc,f2=0.0,rhs1,rhs2,slope,sum,temp,
120              test,tmplam;
121
122     check = 0;
123     for(sum=0.0,i=0;i<npar;i++) sum += p[i]*p[i];
124     sum = TMath::Sqrt(sum);
125     if(sum>stpmax) for(i=0;i<npar;i++) p[i] *= stpmax/sum;
126     for(slope=0.0,i=0;i<npar;i++) slope += g[i]*p[i];
127     if(slope >=0.0) printf("Error: round off problem in lnsrch.\n");
128     test = 0.0;
129     for(i=0;i<npar;i++){
130         temp = TMath::Abs(p[i])/TMath::Max(TMath::Abs(xold[i]),1.0);
131         if(temp > test) test = temp;
132     } // end for i
133     alamin = TOLX/test;
134     alam = 1.0;
135     for(;;){
136         for(i=0;i<npar;i++) x[i] = xold[i] + alam*p[i];
137         f = Chi2(x);
138         if(alam < alamin){
139             for(i=0;i<npar;i++) x[i] = xold[i];
140             check = 1;
141             return;
142         }else if(f <= fold+ALF*alam*slope) return;
143         else{
144             if(alam == 1.0) tmplam = -slope/(2.0*(f-fold-slope));
145             else{
146                 rhs1 = f-fold-alam*slope;
147                 rhs2 = f2-fold-alam2*slope;
148                 a = (rhs1/(alam*alam)-rhs2/(alam2*alam2))/(alam-alam2);
149                 b = (-alam2*rhs1/(alam*alam)+alam*rhs2/(alam2*alam2))/(alam*alam);
150                 if(a==0.0) tmplam = -slope/(2.0*b);
151                 else{
152                     disc = b*b - 3.0*a*slope;
153                     if(disc<0.0) tmplam = 0.5*alam;
154                     else if(b<=0.0) tmplam = (-b+TMath::Sqrt(disc))/(3.0*a);
155                     else tmplam = -slope/(b+TMath::Sqrt(disc));
156                 } // end if a == 0.0
157                 if(tmplam > 0.5*alam) tmplam = 0.5*alam;
158             } // end if alam == 1.0
159         } // end if alam < alamin
160         alam2 = alam;
161         f2 = f;
162         alam = TMath::Max(tmplam,0.1*alam);
163     } // end for ever loop
164
165 }
166 //______________________________________________________________________
167 void AliITSAlignmentModule::MRVMminimization(Int_t npar,Double_t *p,
168                                              Double_t &fret,Double_t gtol,
169                                              Int_t &iter){
170     Int_t ITMAX = 200;
171     Double_t EPS = 3.0e-8, TOLX = 4.0*EPS, STPMX = 100.0;
172
173     Int_t    check,i,its,j;
174     Double_t den,fac,fad,fae,fp,stpmax,sum=0.0,sumdg,sumxi,temp,test;
175     Double_t *dg,*g,*hdg,**hessin,*pnew,*xi;
176
177     // allocate space.
178     dg     = new Double_t[npar];
179     g      = new Double_t[npar];
180     hdg    = new Double_t[npar];
181     hessin = new Double_t * [npar];
182     for(i=0;i<npar;i++) hessin[i] = new Double_t[npar];
183     pnew   = new Double_t[npar];
184     xi     = new Double_t[npar];
185
186     // init function values
187     fp = Chi2(p);
188     dChi2(p,g);
189
190     for(i=0;i<npar;i++){
191         for(j=0;j<npar;j++) hessin[i][j] = 0.0;
192         hessin[i][i] = 1.0;
193         xi[i] = -g[i];
194         sum += p[i]*p[i];
195     } // end for i
196     stpmax = STPMX*TMath::Max(TMath::Sqrt(sum),(Double_t)(npar+1));
197
198     for(its=0;its<ITMAX;its++){
199         iter = its;
200         lnsrch(npar,p,fp,g,xi,pnew,fret,stpmax,check);
201         fp = fret;
202         for(i=0;i<npar;i++){
203             xi[i] = pnew[i] - p[i];
204             p[i] = pnew[i];
205         } // end for i
206         test = 0.0;
207         for(i=0;i<npar;i++){
208             temp = TMath::Abs(xi[i])/TMath::Max(TMath::Abs(p[i]),1.0);
209             if(temp > test) test = temp;
210         } // end for i
211         if(test<TOLX) break;
212         for(i=0;i<npar;i++) dg[i] = g[i];
213         dChi2(p,g);
214         test = 0.0;
215         den = TMath::Max(fret,1.0);
216         for(i=0;i<npar;i++){
217             temp = TMath::Abs(g[i])*TMath::Max(TMath::Abs(p[i]),1.0)/den;
218             if(temp>test) test=temp;
219         } // end for i
220         if(test<gtol) break;
221         for(i=0;i<npar;i++) dg[i] = g[i] - dg[i];
222         for(i=0;i<npar;i++){
223             hdg[i] = 0.0;
224             for(j=0;j<npar;j++) hdg[i] += hessin[i][j]*dg[j];
225         } // end for i
226         fac = fae = sumdg = sumxi = 0.0;
227         for(i=0;i<npar;i++){
228             fac += dg[i]*xi[i];
229             fae += dg[i]*hdg[i];
230             sumdg += TMath::Sqrt(dg[i]);
231             sumxi += TMath::Sqrt(xi[i]);
232         } // end for i
233         if(fac>TMath::Sqrt(EPS*sumdg*sumxi)){
234             fac = 1.0/fac;
235             fad = 1.0/fae;
236             for(i=0;i<npar;i++) dg[i] = fac*xi[i]-fad*hdg[i];
237             for(i=0;i<npar;i++) for(j=i;j<npar;j++){
238                 hessin[i][j] += fac*xi[i]*xi[j] - fad*hdg[i]*hdg[j] + 
239                                                   fae*dg[i]*dg[j];
240                 hessin[j][i] = hessin[i][j];
241             } // end for i,j
242         } // end if fac>...
243         for(i=0;i<npar;i++){
244             xi[i] = 0.0;
245             for(j=0;j<npar;j++) xi[i] -= hessin[i][j]*g[j];
246         } // end for i
247     } // end for its
248     if(its==ITMAX) printf("Error: too many iterations\n");
249     delete[] dg;
250     delete[] g;
251     delete[] hdg;
252     delete[] pnew;
253     delete[] xi;
254     for(i=0;i<npar;i++) delete[] hessin[i];
255     delete[] hessin;
256 }
257 //______________________________________________________________________
258 void AliITSAlignmentModule::SetByAngles(Double_t *th){
259    Double_t  sx,cx,sy,cy,sz,cz;
260
261    sx = TMath::Sin(th[0]); cx = TMath::Cos(th[0]);
262    sy = TMath::Sin(th[1]); cy = TMath::Cos(th[1]);
263    sz = TMath::Sin(th[2]); cz = TMath::Cos(th[2]);
264    for(Int_t i=0;i<3;i++) fangles[i]   = th[i];
265    fM[0][0] =  cz*cy;
266    fM[0][1] = -cz*sy*sx - sz*cx;
267    fM[0][2] = -cz*sy*cx + sz*sx;
268    fM[1][0] =  sz*cy;
269    fM[1][1] = -sz*sy*sx + cz*cx;
270    fM[1][2] = -sz*sy*cx - cz*sx;
271    fM[2][0] =  sy;
272    fM[2][1] =  cy*sx;
273    fM[2][2] =  cy*cx;
274 }
275 //______________________________________________________________________
276 void AliITSAlignmentModule::dfMdthx(Double_t dfMx[3][3]){
277    Double_t  sx,cx,sy,cy,sz,cz;
278
279    sx = TMath::Sin(fangles[0]); cx = TMath::Cos(fangles[0]);
280    sy = TMath::Sin(fangles[1]); cy = TMath::Cos(fangles[1]);
281    sz = TMath::Sin(fangles[2]); cz = TMath::Cos(fangles[2]);
282    dfMx[0][0] =  0.0;
283    dfMx[0][1] = -cz*sy*cx + sz*sx;
284    dfMx[0][2] =  cz*sy*sx + sz*cx;
285    dfMx[1][0] =  0.0;
286    dfMx[1][1] = -sz*sy*cx - cz*sx;
287    dfMx[1][2] =  sz*sy*sx - cz*cx;
288    dfMx[2][0] =  0.0;
289    dfMx[2][1] =  cy*cx;
290    dfMx[2][2] = -cy*sx;
291 }
292 //______________________________________________________________________
293 void AliITSAlignmentModule::dfMdthy(Double_t dfMx[3][3]){
294    Double_t  sx,cx,sy,cy,sz,cz;
295
296    sx = TMath::Sin(fangles[0]); cx = TMath::Cos(fangles[0]);
297    sy = TMath::Sin(fangles[1]); cy = TMath::Cos(fangles[1]);
298    sz = TMath::Sin(fangles[2]); cz = TMath::Cos(fangles[2]);
299    dfMx[0][0] = -cz*sy;
300    dfMx[0][1] = -cz*cy*sx;
301    dfMx[0][2] = -cz*cy*cx;
302    dfMx[1][0] = -sz*sy;
303    dfMx[1][1] = -sz*cy*sx;
304    dfMx[1][2] = -sz*cy*cx;
305    dfMx[2][0] =  cy;
306    dfMx[2][1] = -sy*sx;
307    dfMx[2][2] = -sy*cx;
308 }
309 //______________________________________________________________________
310 void AliITSAlignmentModule::dfMdthz(Double_t dfMx[3][3]){
311    Double_t  sx,cx,sy,cy,sz,cz;
312
313    sx = TMath::Sin(fangles[0]); cx = TMath::Cos(fangles[0]);
314    sy = TMath::Sin(fangles[1]); cy = TMath::Cos(fangles[1]);
315    sz = TMath::Sin(fangles[2]); cz = TMath::Cos(fangles[2]);
316    dfMx[0][0] = -sz*cy;
317    dfMx[0][1] =  sz*sy*sx - cz*cx;
318    dfMx[0][2] =  sz*sy*cx + cz*sx;
319    dfMx[1][0] =  cz*cy;
320    dfMx[1][1] = -cz*sy*sx - sz*cx;
321    dfMx[1][2] = -cz*sy*cx + sz*sx;
322    dfMx[2][0] =  0.0;
323    dfMx[2][1] =  0.0;
324    dfMx[2][2] =  0.0;
325 }
326 //______________________________________________________________________
327 void AliITSAlignmentModule::LtoG(Double_t xl[],Double_t xg[]){
328
329     Int_t i;
330     for(i=0;i<3;i++) xg[i] = fx0[i];
331     for(i=0;i<3;i++)for(Int_t j=0;j<3;j++) xg[i] += fM[j][i]*xl[j];
332 }
333 //______________________________________________________________________
334 void AliITSAlignmentModule::GtoL(Double_t xg[],Double_t xl[]){
335     Int_t i;
336     for(i=0;i<3;i++) xl[i] = 0.0;
337     for(i=0;i<3;i++)for(Int_t j=0;j<3;j++)xl[i]+=fM[i][j]*(xg[j]-fx0[j]);
338 }
339 //______________________________________________________________________
340 Double_t AliITSAlignmentModule::Chi2(Double_t p[]){
341     Int_t    i,j,k,l,n=0,indx;
342     Double_t chi=0.0,xo[3],xi[3],xg[3],Ex[3][3];
343     AliITSAlignmentTrack *tr;
344
345     for(i=0;i<3;i++) fx0[i] = p[i];
346     SetByAngles(&(p[3]));
347
348     for(i=0;i<ftrksM->GetEntriesFast();i++) {
349         tr = (AliITSAlignmentTrack *)(ftrksM->At(i));
350         for(j=0;j<tr->GetNumberOfClustersSl();j++){
351             tr->GetIndex(j,indx);
352             if(indx==findex){
353                 n++;
354                 tr->GetPointG(j,(Double_t *)xi);
355                 tr->func(xi,xo);
356                 tr->GetPointL(j,(Double_t *)xi);
357                 tr->GetErrorG(j,(Double_t **)Ex);
358                 LtoG(xi,xg);
359                 for(k=0;k<3;k++)for(l=0;l<3;l++)
360                     chi += (xg[k] - xo[k])*Ex[k][l]*(xg[l]-xo[l]);
361             } // end if indx==findex
362         } // end for j
363     } // end for i
364     if(n<7) return chi;
365     return chi/(Double_t)(n-6);
366 }
367 //______________________________________________________________________
368 void AliITSAlignmentModule::dChi2(Double_t p[],Double_t dChi2[]){
369     Int_t    i,j,k,l,m,n=0,indx;
370     Double_t chi[6]={0.0,0.0,0.0,0.0,0.0,0.0},xo[3],xi[3],xg[3],Ex[3][3];
371     Double_t dxdp[3][6],fMx[3][3],fMy[3][3],fMz[3][3];
372     AliITSAlignmentTrack *tr;
373
374     for(i=0;i<3;i++) fx0[i] = p[i];
375     SetByAngles(&(p[3]));
376     dfMdthx(fMx);
377     dfMdthy(fMy);
378     dfMdthz(fMz);
379     for(i=0;i<3;i++)for(j=0;j<6;j++)  dxdp[i][j] = 0.0;
380     dxdp[0][0] = 1.0; // dx/dx
381     dxdp[1][1] = 1.0; // dy/dy
382     dxdp[2][2] = 1.0; // dz/dz
383
384     for(i=0;i<ftrksM->GetEntriesFast();i++) {
385         tr = (AliITSAlignmentTrack *)(ftrksM->At(i));
386         for(j=0;j<tr->GetNumberOfClustersSl();j++){
387             tr->GetIndex(j,indx);
388             if(indx==findex){
389                 n++;
390                 tr->GetPointG(j,(Double_t *)xi);
391                 tr->func(xi,xo);
392                 tr->GetPointL(j,(Double_t *)xi);
393                 tr->GetErrorG(j,(Double_t **)Ex);
394                 LtoG(xi,xg);
395                 for(m=0;m<3;m++) for(k=0;k<3;k++){
396                     dxdp[m][3] += fMx[m][k]*xi[k];
397                     dxdp[m][4] += fMy[m][k]*xi[k];
398                     dxdp[m][5] += fMz[m][k]*xi[k];
399                 } // end for m
400                 for(m=0;m<6;m++){
401                     for(k=0;k<3;k++)for(l=0;l<3;l++)
402                         chi[m] += (xg[k] - xo[k])*Ex[k][l]*dxdp[l][m];
403                 } // end for m
404             } // end if indx==findex
405         } // end for j
406     } // end for i
407     if(n<7) return ;
408     for(m=0;m<6;m++) chi[m] /= (Double_t)(n-6);
409     return;
410 }
411 //______________________________________________________________________
412 void AliITSAlignmentModule::AlignModule(){
413     static Int_t npar=6;
414     Int_t    iter,i;
415     //Double_t p[npar],fret,gtol=1.0E-5;
416     Double_t p[6],fret,gtol=1.0E-5;
417
418     for(i=0;i<3;i++) {p[i] = fx0[i]; p[i+3] = fangles[i];}
419     MRVMminimization(npar,(Double_t *)p,fret,gtol,iter);
420     for(i=0;i<3;i++) {fx0[i] = p[i]; fangles[i] = p[i+3];}
421     printf("AlignModule #%d:Xt=(%e,%e,%e) cm angles=(%e,%e,%e)rad,"
422            " Chi2=%e loops=%d\n",findex,
423            fx0[0],fx0[1],fx0[2],fangles[0],fangles[1],fangles[2],fret,iter);
424 }
425 //______________________________________________________________________
426 void AliITSAlignmentModule::Streamer(TBuffer &R__b){
427    // Stream an object of class AliITSAlignmentModule.
428
429    if (R__b.IsReading()) {
430       Version_t R__v = R__b.ReadVersion(); if (R__v) { }
431       TObject::Streamer(R__b);
432       R__b >> findex;
433       R__b >> flay;
434       R__b >> flad;
435       R__b >> fdet;
436       R__b >> ftrksM;
437       R__b >> fChi2;
438 //      R__b.ReadStaticArray(fx0);
439 //      R__b.ReadStaticArray((double*)fM);
440 //      R__b.ReadStaticArray(fangles);
441    } else {
442       R__b.WriteVersion(AliITSAlignmentModule::IsA());
443       TObject::Streamer(R__b);
444       R__b << findex;
445       R__b << flay;
446       R__b << flad;
447       R__b << fdet;
448       R__b << ftrksM;
449       R__b << fChi2;
450 //      R__b.WriteArray(fx0, 3);
451 //      R__b.WriteArray((double*)fM, 9);
452 //      R__b.WriteArray(fangles, 3);
453    }
454 }