Added AliMillePede2 + related matrix and solver classes
[u/mrichter/AliRoot.git] / STEER / AliSymMatrix.cxx
1 #include <stdlib.h>
2 #include <stdio.h>
3 #include <iostream>
4 //
5 #include "TClass.h"
6 #include "TMath.h"
7 #include "AliSymMatrix.h"
8 //
9
10 using namespace std;
11
12 ClassImp(AliSymMatrix)
13
14
15 AliSymMatrix* AliSymMatrix::fgBuffer = 0; 
16 Int_t         AliSymMatrix::fgCopyCnt = 0; 
17 //___________________________________________________________
18 AliSymMatrix::AliSymMatrix() 
19 : fElems(0),fElemsAdd(0)
20 {
21   fSymmetric = kTRUE;
22   fgCopyCnt++;
23 }
24
25 //___________________________________________________________
26 AliSymMatrix::AliSymMatrix(Int_t size)
27   : AliMatrixSq(),fElems(0),fElemsAdd(0)
28 {
29   //
30   fNrows = 0;
31   fNrowIndex = fNcols = size;
32   fElems     = new Double_t[fNcols*(fNcols+1)/2];
33   fSymmetric = kTRUE;
34   Reset();
35   fgCopyCnt++;
36   //
37 }
38
39 //___________________________________________________________
40 AliSymMatrix::AliSymMatrix(const AliSymMatrix &src) 
41   : AliMatrixSq(src),fElems(0),fElemsAdd(0)
42 {
43   fNrowIndex = fNcols = src.GetSize();
44   fNrows = 0;
45   if (fNcols) {
46     int nmainel = fNcols*(fNcols+1)/2;
47     fElems     = new Double_t[nmainel];
48     nmainel = src.fNcols*(src.fNcols+1)/2;
49     memcpy(fElems,src.fElems,nmainel*sizeof(Double_t));
50     if (src.fNrows) { // transfer extra rows to main matrix
51       Double_t *pnt = fElems + nmainel;
52       int ncl = src.fNcols + 1;
53       for (int ir=0;ir<src.fNrows;ir++) {
54         memcpy(pnt,src.fElemsAdd[ir],ncl*sizeof(Double_t));
55         pnt += ncl;
56         ncl++; 
57       }
58     }
59   }
60   else fElems = 0;
61   fElemsAdd = 0;
62   fgCopyCnt++;
63   //
64 }
65
66 //___________________________________________________________
67 AliSymMatrix::~AliSymMatrix() 
68 {
69   Clear();
70   if (--fgCopyCnt < 1 && fgBuffer) {delete fgBuffer; fgBuffer = 0;}
71 }
72
73 //___________________________________________________________
74 AliSymMatrix&  AliSymMatrix::operator=(const AliSymMatrix& src)
75 {
76   //
77   if (this != &src) {
78     TObject::operator=(src);
79     if (fNcols!=src.fNcols && fNrows!=src.fNrows) {
80       // recreate the matrix
81       if (fElems) delete[] fElems;
82       for (int i=0;i<fNrows;i++) delete[] fElemsAdd[i]; 
83       delete[] fElemsAdd;
84       //
85       fNrowIndex = fNcols = src.GetSize();
86       fNrows = 0;
87       fElems     = new Double_t[fNcols*(fNcols+1)/2];
88       int nmainel = src.fNcols*(src.fNcols+1);
89       memcpy(fElems,src.fElems,nmainel*sizeof(Double_t));
90       if (src.fNrows) { // transfer extra rows to main matrix
91         Double_t *pnt = fElems + nmainel*sizeof(Double_t);
92         int ncl = src.fNcols + 1;
93         for (int ir=0;ir<src.fNrows;ir++) {
94           ncl += ir; 
95           memcpy(pnt,src.fElemsAdd[ir],ncl*sizeof(Double_t));
96           pnt += ncl*sizeof(Double_t);
97         }
98       }
99       //
100     }
101     else {
102       memcpy(fElems,src.fElems,fNcols*(fNcols+1)/2*sizeof(Double_t));
103       int ncl = fNcols + 1;
104       for (int ir=0;ir<fNrows;ir++) { // dynamic rows
105         ncl += ir; 
106         memcpy(fElemsAdd[ir],src.fElemsAdd[ir],ncl*sizeof(Double_t));
107       }
108     }
109   }
110   //
111   return *this;
112 }
113
114 //___________________________________________________________
115 void AliSymMatrix::Clear(Option_t*)
116 {
117   if (fElems) {delete[] fElems; fElems = 0;}
118   //  
119   if (fElemsAdd) {
120     for (int i=0;i<fNrows;i++) delete[] fElemsAdd[i]; 
121     delete[] fElemsAdd;
122     fElemsAdd = 0;
123   }
124   fNrowIndex = fNcols = fNrows = 0;
125   //
126 }
127
128 //___________________________________________________________
129 Float_t AliSymMatrix::GetDensity() const
130 {
131   // get fraction of non-zero elements
132   Int_t nel = 0;
133   for (int i=GetSize();i--;) for (int j=i+1;j--;) if (GetEl(i,j)!=0) nel++;
134   return 2.*nel/( (GetSize()+1)*GetSize() );
135 }
136
137 //___________________________________________________________
138 void AliSymMatrix::Print(Option_t* option) const
139 {
140   printf("Symmetric Matrix: Size = %d (%d rows added dynamically)\n",GetSize(),fNrows);
141   TString opt = option; opt.ToLower();
142   if (opt.IsNull()) return;
143   opt = "%"; opt += 1+int(TMath::Log10(double(GetSize()))); opt+="d|";
144   for (Int_t i=0;i<fNrowIndex;i++) {
145     printf(opt,i);
146     for (Int_t j=0;j<=i;j++) printf("%+.3e|",GetEl(i,j));
147     printf("\n");
148   }
149 }
150
151 //___________________________________________________________
152 void AliSymMatrix::MultiplyByVec(Double_t *vecIn,Double_t *vecOut) const
153 {
154   // fill vecOut by matrix*vecIn
155   // vector should be of the same size as the matrix
156   for (int i=fNrowIndex;i--;) {
157     vecOut[i] = 0.0;
158     for (int j=fNrowIndex;j--;) vecOut[i] += vecIn[j]*GetEl(i,j);
159   }
160   //
161 }
162
163 //___________________________________________________________
164 AliSymMatrix* AliSymMatrix::DecomposeChol() 
165 {
166   // Return a matrix with Choleski decomposition
167   /*Adopted from Numerical Recipes in C, ch.2-9, http://www.nr.com
168     consturcts Cholesky decomposition of SYMMETRIC and
169     POSITIVELY-DEFINED matrix a (a=L*Lt)
170     Only upper triangle of the matrix has to be filled.
171     In opposite to function from the book, the matrix is modified:
172     lower triangle and diagonal are refilled. */
173   //
174   if (!fgBuffer || fgBuffer->GetSize()!=GetSize()) {
175     delete fgBuffer; 
176     try {
177       fgBuffer = new AliSymMatrix(*this);
178     }
179     catch(bad_alloc&) {
180       printf("Failed to allocate memory for Choleski decompostions\n");
181       return 0;
182     }
183   }
184   else (*fgBuffer) = *this;
185   //
186   AliSymMatrix& mchol = *fgBuffer;
187   //
188   for (int i=0;i<fNrowIndex;i++) {
189     for (int j=i;j<fNrowIndex;j++) {
190       double sum=(*this)(i,j);
191       for (int k=i-1;k>=0;k--) sum -= mchol(i,k)*mchol(j,k);
192       if (i == j) {
193         if (sum <= 0.0) { // not positive-definite
194           printf("The matrix is not positive definite [%e]\n"
195                  "Choleski decomposition is not possible\n",sum);
196           return 0;
197         }
198         mchol(i,i) = TMath::Sqrt(sum);
199         //
200       } else mchol(j,i) = sum/mchol(i,i);
201     }
202   }
203   return fgBuffer;
204 }
205
206 //___________________________________________________________
207 Bool_t AliSymMatrix::InvertChol() 
208 {
209   // Invert matrix using Choleski decomposition
210   //
211   AliSymMatrix* mchol = DecomposeChol();
212   if (!mchol) {
213     printf("Failed to invert the matrix\n");
214     return kFALSE;
215   }
216   //
217   InvertChol(mchol);
218   return kTRUE;
219   //
220 }
221 //___________________________________________________________
222 void AliSymMatrix::InvertChol(AliSymMatrix* pmchol) 
223 {
224   // Invert matrix using Choleski decomposition, provided the Cholseki's L matrix
225   Int_t i,j,k;
226   Double_t sum;
227   AliSymMatrix& mchol = *pmchol;
228   //
229   // Invert decomposed triangular L matrix (Lower triangle is filled)
230   for (i=0;i<fNrowIndex;i++) { 
231     mchol(i,i) =  1.0/mchol(i,i);
232     for (j=i+1;j<fNrowIndex;j++) { 
233       sum = 0.0; 
234       for (k=i;k<j;k++) sum -= mchol(j,k)*mchol(k,i);
235       mchol(j,i) = sum/mchol(j,j);
236     } 
237   }
238   //
239   // take product of the inverted Choleski L matrix with its transposed
240   for (int i=fNrowIndex;i--;) {
241     for (int j=i+1;j--;) {
242       sum = 0;
243       for (int k=i;k<fNrowIndex;k++) sum += mchol(i,k)*mchol(j,k);
244       (*this)(j,i) = sum;
245     }
246   }
247   //
248 }
249
250
251 //___________________________________________________________
252 Bool_t AliSymMatrix::SolveChol(Double_t *b, Bool_t invert) 
253 {
254   /* Adopted from Numerical Recipes in C, ch.2-9, http://www.nr.com
255      Solves the set of n linear equations A x = b, 
256      where a is a positive-definite symmetric matrix. 
257      a[1..n][1..n] is the output of the routine CholDecomposw. 
258      Only the lower triangle of a is accessed. b[1..n] is input as the 
259      right-hand side vector. The solution vector is returned in b[1..n].*/
260   //
261   Int_t i,k;
262   Double_t sum;
263   //
264   AliSymMatrix *pmchol = DecomposeChol();
265   if (!pmchol) {
266     printf("SolveChol failed\n");
267     return kFALSE;
268   }
269   AliSymMatrix& mchol = *pmchol;
270   //
271   for (i=0;i<fNrowIndex;i++) {
272     for (sum=b[i],k=i-1;k>=0;k--) sum -= mchol(i,k)*b[k];
273     b[i]=sum/mchol(i,i);
274   }
275   for (i=fNrowIndex-1;i>=0;i--) {
276     for (sum=b[i],k=i+1;k<fNrowIndex;k++) sum -= mchol(k,i)*b[k];
277     b[i]=sum/mchol(i,i);
278   }
279   //
280   if (invert) InvertChol(pmchol);
281   return kTRUE;
282   //
283 }
284
285 //___________________________________________________________
286 Bool_t AliSymMatrix::SolveChol(TVectorD &b, Bool_t invert) 
287 {
288   return SolveChol((Double_t*)b.GetMatrixArray(),invert);
289 }
290
291
292 //___________________________________________________________
293 Bool_t AliSymMatrix::SolveChol(Double_t *brhs, Double_t *bsol,Bool_t invert) 
294 {
295   memcpy(bsol,brhs,GetSize()*sizeof(Double_t));
296   return SolveChol(bsol,invert);  
297 }
298
299 //___________________________________________________________
300 Bool_t AliSymMatrix::SolveChol(TVectorD &brhs, TVectorD &bsol,Bool_t invert) 
301 {
302   bsol = brhs;
303   return SolveChol(bsol,invert);
304 }
305
306 //___________________________________________________________
307 void AliSymMatrix::AddRows(int nrows)
308 {
309   if (nrows<1) return;
310   Double_t **pnew = new Double_t*[nrows+fNrows];
311   for (int ir=0;ir<fNrows;ir++) pnew[ir] = fElemsAdd[ir]; // copy old extra rows
312   for (int ir=0;ir<nrows;ir++) {
313     int ncl = GetSize()+1;
314     pnew[fNrows] = new Double_t[ncl];
315     memset(pnew[fNrows],0,ncl*sizeof(Double_t));
316     fNrows++;
317     fNrowIndex++;
318   }
319   delete[] fElemsAdd;
320   fElemsAdd = pnew;
321   //
322 }
323
324 //___________________________________________________________
325 void AliSymMatrix::Reset()
326 {
327   // if additional rows exist, regularize it
328   if (fElemsAdd) {
329     delete[] fElems;
330     for (int i=0;i<fNrows;i++) delete[] fElemsAdd[i]; 
331     delete[] fElemsAdd; fElemsAdd = 0;
332     fNcols = fNrowIndex;
333     fElems = new Double_t[fNcols*(fNcols+1)/2];
334     fNrows = 0;
335   }
336   if (fElems) memset(fElems,0,fNcols*(fNcols+1)/2*sizeof(Double_t));
337   //
338 }
339
340 //___________________________________________________________
341 int AliSymMatrix::SolveSpmInv(double *vecB, Bool_t stabilize)
342 {
343   //   Solution a la MP1: gaussian eliminations
344   ///  Obtain solution of a system of linear equations with symmetric matrix 
345   ///  and the inverse (using 'singular-value friendly' GAUSS pivot)
346   //
347
348   Int_t nRank = 0;
349   int iPivot;
350   double vPivot = 0.;
351   double eps = 0.00000000000001;
352   int nGlo = GetSize();
353   bool   *bUnUsed = new bool[nGlo];
354   double *rowMax,*colMax=0;
355   rowMax  = new double[nGlo];
356   //
357   if (stabilize) {
358     colMax   = new double[nGlo];
359     for (Int_t i=nGlo; i--;) rowMax[i] = colMax[i] = 0.0;
360     for (Int_t i=nGlo; i--;) for (Int_t j=i+1;j--;) { 
361         double vl = TMath::Abs(Querry(i,j));
362         if (vl==0) continue;
363         if (vl > rowMax[i]) rowMax[i] = vl; // Max elemt of row i
364         if (vl > colMax[j]) colMax[j] = vl; // Max elemt of column j
365         if (i==j) continue;
366         if (vl > rowMax[j]) rowMax[j] = vl; // Max elemt of row j
367         if (vl > colMax[i]) colMax[i] = vl; // Max elemt of column i
368       }
369     //
370     for (Int_t i=nGlo; i--;) {
371       if (0.0 != rowMax[i]) rowMax[i] = 1./rowMax[i]; // Max elemt of row i
372       if (0.0 != colMax[i]) colMax[i] = 1./colMax[i]; // Max elemt of column i
373     }
374     //
375   }
376   //
377   for (Int_t i=nGlo; i--;) bUnUsed[i] = true;
378   //  
379   if (!fgBuffer || fgBuffer->GetSize()!=GetSize()) {
380     delete fgBuffer; 
381     try {
382       fgBuffer = new AliSymMatrix(*this);
383     }
384     catch(bad_alloc&) {
385       printf("Failed to allocate memory for matrix inversion buffer\n");
386       return 0;
387     }
388   }
389   else (*fgBuffer) = *this;
390   //
391   if (stabilize) for (int i=0;i<nGlo; i++) { // Small loop for matrix equilibration (gives a better conditioning) 
392       for (int j=0;j<=i; j++) {
393         double vl = Querry(i,j);
394         if (vl!=0) SetEl(i,j, TMath::Sqrt(rowMax[i])*vl*TMath::Sqrt(colMax[j]) ); // Equilibrate the V matrix
395       }
396       for (int j=i+1;j<nGlo;j++) {
397         double vl = Querry(j,i);
398         if (vl!=0) fgBuffer->SetEl(j,i,TMath::Sqrt(rowMax[i])*vl*TMath::Sqrt(colMax[j]) ); // Equilibrate the V matrix
399       }
400     }
401   //
402   for (Int_t j=nGlo; j--;) fgBuffer->DiagElem(j) = TMath::Abs(QuerryDiag(j)); // save diagonal elem absolute values 
403   //
404   for (Int_t i=0; i<nGlo; i++) {
405     vPivot = 0.0;
406     iPivot = -1;
407     //
408     for (Int_t j=0; j<nGlo; j++) { // First look for the pivot, ie max unused diagonal element       
409       double vl;
410       if (bUnUsed[j] && (TMath::Abs(vl=QuerryDiag(j))>TMath::Max(TMath::Abs(vPivot),eps*fgBuffer->QuerryDiag(j)))) {    
411         vPivot = vl;
412         iPivot = j;
413       }
414     }
415     //
416     if (iPivot >= 0) {   // pivot found          
417       nRank++;
418       bUnUsed[iPivot] = false; // This value is used
419       vPivot = 1.0/vPivot;
420       DiagElem(iPivot) = -vPivot; // Replace pivot by its inverse
421       //
422       for (Int_t j=0; j<nGlo; j++) {      
423         for (Int_t jj=0; jj<nGlo; jj++) {  
424           if (j != iPivot && jj != iPivot) {// Other elements (!!! do them first as you use old matV[k][j]'s !!!)         
425             double &r = j>=jj ? (*this)(j,jj) : (*fgBuffer)(jj,j);
426             r -= vPivot* ( j>iPivot  ? Querry(j,iPivot)  : fgBuffer->Querry(iPivot,j) )
427               *          ( iPivot>jj ? Querry(iPivot,jj) : fgBuffer->Querry(jj,iPivot));
428           }
429         }
430       }
431       //
432       for (Int_t j=0; j<nGlo; j++) if (j != iPivot) { // Pivot row or column elements 
433           (*this)(j,iPivot)     *= vPivot;
434           (*fgBuffer)(iPivot,j) *= vPivot;
435         }
436       //
437     }
438     else {  // No more pivot value (clear those elements)
439       for (Int_t j=0; j<nGlo; j++) {
440         if (bUnUsed[j]) {
441           vecB[j] = 0.0;
442           for (Int_t k=0; k<nGlo; k++) {
443             (*this)(j,k) = 0.;
444             if (j!=k) (*fgBuffer)(j,k) = 0;
445           }
446         }
447       }
448       break;  // No more pivots anyway, stop here
449     }
450   }
451   //
452   for (Int_t i=0; i<nGlo; i++) for (Int_t j=0; j<nGlo; j++) {
453       double vl = TMath::Sqrt(colMax[i])*TMath::Sqrt(rowMax[j]); // Correct matrix V
454       if (i>=j) (*this)(i,j) *= vl;
455       else      (*fgBuffer)(j,i) *= vl;
456     }
457   //
458   for (Int_t j=0; j<nGlo; j++) {
459     rowMax[j] = 0.0;
460     for (Int_t jj=0; jj<nGlo; jj++) { // Reverse matrix elements
461       double vl;
462       if (j>=jj) vl = (*this)(j,jj)     = -Querry(j,jj);
463       else       vl = (*fgBuffer)(j,jj) = -fgBuffer->Querry(j,jj);
464       rowMax[j] += vl*vecB[jj];
465     }           
466   }
467   
468   for (Int_t j=0; j<nGlo; j++) {
469     vecB[j] = rowMax[j]; // The final result
470   }
471   //
472   delete [] bUnUsed;
473   delete [] rowMax;
474   if (stabilize) delete [] colMax;
475
476   return nRank;
477 }