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