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