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