Update for changes in Root TExMap, TExMapIter in trunk
[u/mrichter/AliRoot.git] / STEER / AliSymMatrix.cxx
CommitLineData
8a9ab0eb 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
10using namespace std;
11
12ClassImp(AliSymMatrix)
13
14
15AliSymMatrix* AliSymMatrix::fgBuffer = 0;
16Int_t AliSymMatrix::fgCopyCnt = 0;
17//___________________________________________________________
18AliSymMatrix::AliSymMatrix()
19: fElems(0),fElemsAdd(0)
20{
21 fSymmetric = kTRUE;
22 fgCopyCnt++;
23}
24
25//___________________________________________________________
26AliSymMatrix::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//___________________________________________________________
40AliSymMatrix::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//___________________________________________________________
67AliSymMatrix::~AliSymMatrix()
68{
69 Clear();
70 if (--fgCopyCnt < 1 && fgBuffer) {delete fgBuffer; fgBuffer = 0;}
71}
72
73//___________________________________________________________
74AliSymMatrix& 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 //
de34b538 85 fNrowIndex = src.GetSize();
86 fNcols = src.GetSize();
8a9ab0eb 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//___________________________________________________________
116void 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 }
de34b538 125 fNrowIndex = 0;
126 fNcols = 0;
127 fNrows = 0;
8a9ab0eb 128 //
129}
130
131//___________________________________________________________
132Float_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//___________________________________________________________
141void 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//___________________________________________________________
155void 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//___________________________________________________________
167AliSymMatrix* AliSymMatrix::DecomposeChol()
168{
169 // Return a matrix with Choleski decomposition
de34b538 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.
8a9ab0eb 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++) {
de34b538 192 Double_t *rowi = mchol.GetRow(i);
8a9ab0eb 193 for (int j=i;j<fNrowIndex;j++) {
de34b538 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];
8a9ab0eb 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 }
de34b538 203 rowi[i] = TMath::Sqrt(sum);
8a9ab0eb 204 //
de34b538 205 } else rowj[i] = sum/rowi[i];
8a9ab0eb 206 }
207 }
208 return fgBuffer;
209}
210
211//___________________________________________________________
212Bool_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}
de34b538 226
8a9ab0eb 227//___________________________________________________________
228void AliSymMatrix::InvertChol(AliSymMatrix* pmchol)
229{
230 // Invert matrix using Choleski decomposition, provided the Cholseki's L matrix
5d88242b 231 //
8a9ab0eb 232 Double_t sum;
233 AliSymMatrix& mchol = *pmchol;
234 //
235 // Invert decomposed triangular L matrix (Lower triangle is filled)
5d88242b 236 for (int i=0;i<fNrowIndex;i++) {
8a9ab0eb 237 mchol(i,i) = 1.0/mchol(i,i);
5d88242b 238 for (int j=i+1;j<fNrowIndex;j++) {
de34b538 239 Double_t *rowj = mchol.GetRow(j);
8a9ab0eb 240 sum = 0.0;
de34b538 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];
8a9ab0eb 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;
de34b538 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 }
8a9ab0eb 259 (*this)(j,i) = sum;
260 }
261 }
262 //
263}
264
265
266//___________________________________________________________
267Bool_t AliSymMatrix::SolveChol(Double_t *b, Bool_t invert)
268{
de34b538 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].
8a9ab0eb 275 //
276 Int_t i,k;
277 Double_t sum;
278 //
279 AliSymMatrix *pmchol = DecomposeChol();
280 if (!pmchol) {
281 printf("SolveChol failed\n");
5d88242b 282 // Print("l");
8a9ab0eb 283 return kFALSE;
284 }
285 AliSymMatrix& mchol = *pmchol;
286 //
287 for (i=0;i<fNrowIndex;i++) {
de34b538 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];
8a9ab0eb 291 }
de34b538 292 //
8a9ab0eb 293 for (i=fNrowIndex-1;i>=0;i--) {
de34b538 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 }
8a9ab0eb 297 b[i]=sum/mchol(i,i);
298 }
299 //
300 if (invert) InvertChol(pmchol);
301 return kTRUE;
302 //
303}
304
305//___________________________________________________________
306Bool_t AliSymMatrix::SolveChol(TVectorD &b, Bool_t invert)
307{
308 return SolveChol((Double_t*)b.GetMatrixArray(),invert);
309}
310
311
312//___________________________________________________________
313Bool_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//___________________________________________________________
320Bool_t AliSymMatrix::SolveChol(TVectorD &brhs, TVectorD &bsol,Bool_t invert)
321{
322 bsol = brhs;
323 return SolveChol(bsol,invert);
324}
325
326//___________________________________________________________
327void 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//___________________________________________________________
345void 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//___________________________________________________________
de34b538 361/*
362void 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//___________________________________________________________
393Double_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//___________________________________________________________
8a9ab0eb 406int 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--;) {
de34b538 426 double vl = TMath::Abs(Query(i,j));
8a9ab0eb 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++) {
de34b538 458 double vl = Query(i,j);
8a9ab0eb 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++) {
de34b538 462 double vl = Query(j,i);
8a9ab0eb 463 if (vl!=0) fgBuffer->SetEl(j,i,TMath::Sqrt(rowMax[i])*vl*TMath::Sqrt(colMax[j]) ); // Equilibrate the V matrix
464 }
465 }
466 //
de34b538 467 for (Int_t j=nGlo; j--;) fgBuffer->DiagElem(j) = TMath::Abs(QueryDiag(j)); // save diagonal elem absolute values
8a9ab0eb 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;
de34b538 475 if (bUnUsed[j] && (TMath::Abs(vl=QueryDiag(j))>TMath::Max(TMath::Abs(vPivot),eps*fgBuffer->QueryDiag(j)))) {
8a9ab0eb 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);
de34b538 491 r -= vPivot* ( j>iPivot ? Query(j,iPivot) : fgBuffer->Query(iPivot,j) )
492 * ( iPivot>jj ? Query(iPivot,jj) : fgBuffer->Query(jj,iPivot));
8a9ab0eb 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;
de34b538 527 if (j>=jj) vl = (*this)(j,jj) = -Query(j,jj);
528 else vl = (*fgBuffer)(j,jj) = -fgBuffer->Query(j,jj);
8a9ab0eb 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}
5d88242b 543
544