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