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