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