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