[u/mrichter/AliRoot.git] / STEER / AliSymMatrix.cxx

/**********************************************************************************************/
/* Fast symmetric matrix with dynamically expandable size.                                    */
/* Only part can be used for matrix operations. It is defined as:                             */ 
/* fNCols: rows built by constructor (GetSizeBooked)                                          */ 
/* fNRows: number of rows added dynamically (automatically added on assignment to row)        */ 
/*         GetNRowAdded                                                                       */ 
/* fNRowIndex: total size (fNCols+fNRows), GetSize                                            */ 
/* fRowLwb   : actual size to used for given operation, by default = total size, GetSizeUsed  */ 
/*                                                                                            */ 
/* Author: ruben.shahoyan@cern.ch                                                             */
/*                                                                                            */ 
/**********************************************************************************************/
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <float.h>
#include <string.h>
//
#include <TClass.h>
#include <TMath.h>
#include "AliSymMatrix.h"
#include "AliLog.h"
//

using namespace std;

ClassImp(AliSymMatrix)


AliSymMatrix* AliSymMatrix::fgBuffer = 0; 
Int_t         AliSymMatrix::fgCopyCnt = 0; 
//___________________________________________________________
AliSymMatrix::AliSymMatrix() 
: fElems(0),fElemsAdd(0)
{
  // default constructor
  fSymmetric = kTRUE;
  fgCopyCnt++;
}

//___________________________________________________________
AliSymMatrix::AliSymMatrix(Int_t size)
  : AliMatrixSq(),fElems(0),fElemsAdd(0)
{
  //constructor for matrix with defined size
  fNrows = 0;
  fNrowIndex = fNcols = fRowLwb = size;
  fElems     = new Double_t[fNcols*(fNcols+1)/2];
  fSymmetric = kTRUE;
  Reset();
  fgCopyCnt++;
  //
}

//___________________________________________________________
AliSymMatrix::AliSymMatrix(const AliSymMatrix &src) 
  : AliMatrixSq(src),fElems(0),fElemsAdd(0)
{
  // copy constructor
  fNrowIndex = fNcols = src.GetSize();
  fNrows = 0;
  fRowLwb = src.GetSizeUsed();
  if (fNcols) {
    int nmainel = fNcols*(fNcols+1)/2;
    fElems     = new Double_t[nmainel];
    nmainel = src.fNcols*(src.fNcols+1)/2;
    memcpy(fElems,src.fElems,nmainel*sizeof(Double_t));
    if (src.GetSizeAdded()) { // transfer extra rows to main matrix
      Double_t *pnt = fElems + nmainel;
      int ncl = src.GetSizeBooked() + 1;
      for (int ir=0;ir<src.GetSizeAdded();ir++) {
	memcpy(pnt,src.fElemsAdd[ir],ncl*sizeof(Double_t));
	pnt += ncl;
	ncl++; 
      }
    }
  }
  else fElems = 0;
  fElemsAdd = 0;
  fgCopyCnt++;
  //
}

//___________________________________________________________
AliSymMatrix::~AliSymMatrix() 
{
  Clear();
  if (--fgCopyCnt < 1 && fgBuffer) {delete fgBuffer; fgBuffer = 0;}
}

//___________________________________________________________
AliSymMatrix&  AliSymMatrix::operator=(const AliSymMatrix& src)
{
  // assignment operator
  if (this != &src) {
    TObject::operator=(src);
    if (GetSizeBooked()!=src.GetSizeBooked() && GetSizeAdded()!=src.GetSizeAdded()) {
      // recreate the matrix
      if (fElems) delete[] fElems;
      for (int i=0;i<GetSizeAdded();i++) delete[] fElemsAdd[i]; 
      delete[] fElemsAdd;
      //
      fNrowIndex = src.GetSize(); 
      fNcols = src.GetSize();
      fNrows = 0;
      fRowLwb = src.GetSizeUsed();
      fElems     = new Double_t[GetSize()*(GetSize()+1)/2];
      int nmainel = src.GetSizeBooked()*(src.GetSizeBooked()+1);
      memcpy(fElems,src.fElems,nmainel*sizeof(Double_t));
      if (src.GetSizeAdded()) { // transfer extra rows to main matrix
	Double_t *pnt = fElems + nmainel;//*sizeof(Double_t);
	int ncl = src.GetSizeBooked() + 1;
	for (int ir=0;ir<src.GetSizeAdded();ir++) {
	  ncl += ir; 
	  memcpy(pnt,src.fElemsAdd[ir],ncl*sizeof(Double_t));
	  pnt += ncl;//*sizeof(Double_t);
	}
      }
      //
    }
    else {
      memcpy(fElems,src.fElems,GetSizeBooked()*(GetSizeBooked()+1)/2*sizeof(Double_t));
      int ncl = GetSizeBooked() + 1;
      for (int ir=0;ir<GetSizeAdded();ir++) { // dynamic rows
	ncl += ir; 
	memcpy(fElemsAdd[ir],src.fElemsAdd[ir],ncl*sizeof(Double_t));
      }
    }
  }
  //
  return *this;
}

//___________________________________________________________
AliSymMatrix& AliSymMatrix::operator+=(const AliSymMatrix& src)
{
  // add operator
  if (GetSizeUsed() != src.GetSizeUsed()) {
    AliError("Matrix sizes are different");
    return *this;
  }
  for (int i=0;i<GetSizeUsed();i++) for (int j=i;j<GetSizeUsed();j++) (*this)(j,i) += src(j,i);
  return *this;
}

//___________________________________________________________
void AliSymMatrix::Clear(Option_t*)
{
  // clear dynamic part
  if (fElems) {delete[] fElems; fElems = 0;}
  //  
  if (fElemsAdd) {
    for (int i=0;i<GetSizeAdded();i++) delete[] fElemsAdd[i]; 
    delete[] fElemsAdd;
    fElemsAdd = 0;
  }
  fNrowIndex = fNcols = fNrows = fRowLwb = 0;
  //
}

//___________________________________________________________
Float_t AliSymMatrix::GetDensity() const
{
  // get fraction of non-zero elements
  Int_t nel = 0;
  for (int i=GetSizeUsed();i--;) for (int j=i+1;j--;) if (!IsZero(GetEl(i,j))) nel++;
  return 2.*nel/( (GetSizeUsed()+1)*GetSizeUsed() );
}

//___________________________________________________________
void AliSymMatrix::Print(Option_t* option) const
{
  // print itself
  printf("Symmetric Matrix: Size = %d (%d rows added dynamically), %d used\n",GetSize(),GetSizeAdded(),GetSizeUsed());
  TString opt = option; opt.ToLower();
  if (opt.IsNull()) return;
  opt = "%"; opt += 1+int(TMath::Log10(double(GetSize()))); opt+="d|";
  for (Int_t i=0;i<GetSizeUsed();i++) {
    printf(opt,i);
    for (Int_t j=0;j<=i;j++) printf("%+.3e|",GetEl(i,j));
    printf("\n");
  }
}

//___________________________________________________________
void AliSymMatrix::MultiplyByVec(const Double_t *vecIn,Double_t *vecOut) const
{
  // fill vecOut by matrix*vecIn
  // vector should be of the same size as the matrix
  for (int i=GetSizeUsed();i--;) {
    vecOut[i] = 0.0;
    for (int j=GetSizeUsed();j--;) vecOut[i] += vecIn[j]*GetEl(i,j);
  }
  //
}

//___________________________________________________________
AliSymMatrix* AliSymMatrix::DecomposeChol() 
{
  // Return a matrix with Choleski decomposition
  // Adopted from Numerical Recipes in C, ch.2-9, http://www.nr.com
  // consturcts Cholesky decomposition of SYMMETRIC and
  // POSITIVELY-DEFINED matrix a (a=L*Lt)
  // Only upper triangle of the matrix has to be filled.
  // In opposite to function from the book, the matrix is modified:
  // lower triangle and diagonal are refilled.
  //
  if (!fgBuffer || fgBuffer->GetSizeUsed()!=GetSizeUsed()) {
    delete fgBuffer; 
    try {
      fgBuffer = new AliSymMatrix(*this);
    }
    catch(bad_alloc&) {
      AliInfo("Failed to allocate memory for Choleski decompostions");
      return 0;
    }
  }
  else (*fgBuffer) = *this;
  //
  AliSymMatrix& mchol = *fgBuffer;
  //
  for (int i=0;i<GetSizeUsed();i++) {
    Double_t *rowi = mchol.GetRow(i);
    for (int j=i;j<GetSizeUsed();j++) {
      Double_t *rowj = mchol.GetRow(j);
      double sum = rowj[i];
      for (int k=i-1;k>=0;k--) if (rowi[k]&&rowj[k]) sum -= rowi[k]*rowj[k];
      if (i == j) {
	if (sum <= 0.0) { // not positive-definite
	  AliInfo(Form("The matrix is not positive definite [%e]\n"
		       "Choleski decomposition is not possible",sum));
	  Print("l");
	  return 0;
	}
	rowi[i] = TMath::Sqrt(sum);
	//
      } else rowj[i] = sum/rowi[i];
    }
  }
  return fgBuffer;
}

//___________________________________________________________
Bool_t AliSymMatrix::InvertChol() 
{
  // Invert matrix using Choleski decomposition
  //
  AliSymMatrix* mchol = DecomposeChol();
  if (!mchol) {
    AliInfo("Failed to invert the matrix");
    return kFALSE;
  }
  //
  InvertChol(mchol);
  return kTRUE;
  //
}

//___________________________________________________________
void AliSymMatrix::InvertChol(AliSymMatrix* pmchol) 
{
  // Invert matrix using Choleski decomposition, provided the Cholseki's L matrix
  //
  Double_t sum;
  AliSymMatrix& mchol = *pmchol;
  //
  // Invert decomposed triangular L matrix (Lower triangle is filled)
  for (int i=0;i<GetSizeUsed();i++) { 
    mchol(i,i) =  1.0/mchol(i,i);
    for (int j=i+1;j<GetSizeUsed();j++) { 
      Double_t *rowj = mchol.GetRow(j);
      sum = 0.0; 
      for (int k=i;k<j;k++) if (rowj[k]) { 
	double &mki = mchol(k,i); if (mki) sum -= rowj[k]*mki;
      }
      rowj[i] = sum/rowj[j];
    } 
  }
  //
  // take product of the inverted Choleski L matrix with its transposed
  for (int i=GetSizeUsed();i--;) {
    for (int j=i+1;j--;) {
      sum = 0;
      for (int k=i;k<GetSizeUsed();k++) {
	double &mik = mchol(i,k); 
	if (mik) {
	  double &mjk = mchol(j,k);
	  if (mjk) sum += mik*mjk;
	}
      }
      (*this)(j,i) = sum;
    }
  }
  //
}


//___________________________________________________________
Bool_t AliSymMatrix::SolveChol(Double_t *b, Bool_t invert) 
{
  // Adopted from Numerical Recipes in C, ch.2-9, http://www.nr.com
  // Solves the set of n linear equations A x = b, 
  // where a is a positive-definite symmetric matrix. 
  // a[1..n][1..n] is the output of the routine CholDecomposw. 
  // Only the lower triangle of a is accessed. b[1..n] is input as the 
  // right-hand side vector. The solution vector is returned in b[1..n].
  //
  Int_t i,k;
  Double_t sum;
  //
  AliSymMatrix *pmchol = DecomposeChol();
  if (!pmchol) {
    AliInfo("SolveChol failed");
    //    Print("l");
    return kFALSE;
  }
  AliSymMatrix& mchol = *pmchol;
  //
  for (i=0;i<GetSizeUsed();i++) {
    Double_t *rowi = mchol.GetRow(i);
    for (sum=b[i],k=i-1;k>=0;k--) if (rowi[k]&&b[k]) sum -= rowi[k]*b[k];
    b[i]=sum/rowi[i];
  }
  //
  for (i=GetSizeUsed()-1;i>=0;i--) {
    for (sum=b[i],k=i+1;k<GetSizeUsed();k++) if (b[k]) {
      double &mki=mchol(k,i); if (mki) sum -= mki*b[k];
    }
    b[i]=sum/mchol(i,i);
  }
  //
  if (invert) InvertChol(pmchol);
  return kTRUE;
  //
}

//___________________________________________________________
Bool_t AliSymMatrix::SolveChol(TVectorD &b, Bool_t invert) 
{
  return SolveChol((Double_t*)b.GetMatrixArray(),invert);
}


//___________________________________________________________
Bool_t AliSymMatrix::SolveChol(Double_t *brhs, Double_t *bsol,Bool_t invert) 
{
  memcpy(bsol,brhs,GetSizeUsed()*sizeof(Double_t));
  return SolveChol(bsol,invert);  
}

//___________________________________________________________
Bool_t AliSymMatrix::SolveChol(TVectorD &brhs, TVectorD &bsol,Bool_t invert) 
{
  bsol = brhs;
  return SolveChol(bsol,invert);
}

//___________________________________________________________
void AliSymMatrix::AddRows(int nrows)
{
  // add empty rows
  if (nrows<1) return;
  Double_t **pnew = new Double_t*[nrows+fNrows];
  for (int ir=0;ir<fNrows;ir++) pnew[ir] = fElemsAdd[ir]; // copy old extra rows
  for (int ir=0;ir<nrows;ir++) {
    int ncl = GetSize()+1;
    pnew[fNrows] = new Double_t[ncl];
    memset(pnew[fNrows],0,ncl*sizeof(Double_t));
    fNrows++;
    fNrowIndex++;
    fRowLwb++;
  }
  delete[] fElemsAdd;
  fElemsAdd = pnew;
  //
}

//___________________________________________________________
void AliSymMatrix::Reset()
{
  // if additional rows exist, regularize it
  if (fElemsAdd) {
    delete[] fElems;
    for (int i=0;i<fNrows;i++) delete[] fElemsAdd[i]; 
    delete[] fElemsAdd; fElemsAdd = 0;
    fNcols = fRowLwb = fNrowIndex;
    fElems = new Double_t[GetSize()*(GetSize()+1)/2];
    fNrows = 0;
  }
  if (fElems) memset(fElems,0,GetSize()*(GetSize()+1)/2*sizeof(Double_t));
  //
}

//___________________________________________________________
/*
void AliSymMatrix::AddToRow(Int_t r, Double_t *valc,Int_t *indc,Int_t n)
{
  //   for (int i=n;i--;) {
  //     (*this)(indc[i],r) += valc[i];
  //   }
  //   return;

  double *row;
  if (r>=fNrowIndex) {
    AddRows(r-fNrowIndex+1); 
    row = &((fElemsAdd[r-fNcols])[0]);
  }
  else row = &fElems[GetIndex(r,0)];
  //
  int nadd = 0;
  for (int i=n;i--;) {
    if (indc[i]>r) continue;
    row[indc[i]] += valc[i];
    nadd++;
  }
  if (nadd == n) return;
  //
  // add to col>row
  for (int i=n;i--;) {
    if (indc[i]>r) (*this)(indc[i],r) += valc[i];
  }
  //
}
*/

//___________________________________________________________
Double_t* AliSymMatrix::GetRow(Int_t r)
{
  // get pointer on the row
  if (r>=GetSize()) {
    int nn = GetSize();
    AddRows(r-GetSize()+1); 
    AliDebug(2,Form("create %d of %d\n",r, nn));
    return &((fElemsAdd[r-GetSizeBooked()])[0]);
  }
  else return &fElems[GetIndex(r,0)];
}


//___________________________________________________________
int AliSymMatrix::SolveSpmInv(double *vecB, Bool_t stabilize)
{
  //   Solution a la MP1: gaussian eliminations
  ///  Obtain solution of a system of linear equations with symmetric matrix 
  ///  and the inverse (using 'singular-value friendly' GAUSS pivot)
  //

  Int_t nRank = 0;
  int iPivot;
  double vPivot = 0.;
  double eps = 1e-14;
  int nGlo = GetSizeUsed();
  bool   *bUnUsed = new bool[nGlo];
  double *rowMax,*colMax=0;
  rowMax  = new double[nGlo];
  //
  if (stabilize) {
    colMax   = new double[nGlo];
    for (Int_t i=nGlo; i--;) rowMax[i] = colMax[i] = 0.0;
    for (Int_t i=nGlo; i--;) for (Int_t j=i+1;j--;) { 
	double vl = TMath::Abs(Query(i,j));
	if (IsZero(vl)) continue;
	if (vl > rowMax[i]) rowMax[i] = vl; // Max elemt of row i
	if (vl > colMax[j]) colMax[j] = vl; // Max elemt of column j
	if (i==j) continue;
	if (vl > rowMax[j]) rowMax[j] = vl; // Max elemt of row j
	if (vl > colMax[i]) colMax[i] = vl; // Max elemt of column i
      }
    //
    for (Int_t i=nGlo; i--;) {
      if (!IsZero(rowMax[i])) rowMax[i] = 1./rowMax[i]; // Max elemt of row i
      if (!IsZero(colMax[i])) colMax[i] = 1./colMax[i]; // Max elemt of column i
    }
    //
  }
  //
  for (Int_t i=nGlo; i--;) bUnUsed[i] = true;
  //  
  if (!fgBuffer || fgBuffer->GetSizeUsed()!=GetSizeUsed()) {
    delete fgBuffer; 
    try {
      fgBuffer = new AliSymMatrix(*this);
    }
    catch(bad_alloc&) {
      AliError("Failed to allocate memory for matrix inversion buffer");
      return 0;
    }
  }
  else (*fgBuffer) = *this;
  //
  if (stabilize) for (int i=0;i<nGlo; i++) { // Small loop for matrix equilibration (gives a better conditioning) 
      for (int j=0;j<=i; j++) {
	double vl = Query(i,j);
	if (!IsZero(vl)) SetEl(i,j, TMath::Sqrt(rowMax[i])*vl*TMath::Sqrt(colMax[j]) ); // Equilibrate the V matrix
      }
      for (int j=i+1;j<nGlo;j++) {
	double vl = Query(j,i);
	if (!IsZero(vl)) fgBuffer->SetEl(j,i,TMath::Sqrt(rowMax[i])*vl*TMath::Sqrt(colMax[j]) ); // Equilibrate the V matrix
      }
    }
  //
  for (Int_t j=nGlo; j--;) fgBuffer->DiagElem(j) = TMath::Abs(QueryDiag(j)); // save diagonal elem absolute values 
  //
  for (Int_t i=0; i<nGlo; i++) {
    vPivot = 0.0;
    iPivot = -1;
    //
    for (Int_t j=0; j<nGlo; j++) { // First look for the pivot, ie max unused diagonal element       
      double vl;
      if (bUnUsed[j] && (TMath::Abs(vl=QueryDiag(j))>TMath::Max(TMath::Abs(vPivot),eps*fgBuffer->QueryDiag(j)))) {    
	vPivot = vl;
	iPivot = j;
      }
    }
    //
    if (iPivot >= 0) {   // pivot found          
      nRank++;
      bUnUsed[iPivot] = false; // This value is used
      vPivot = 1.0/vPivot;
      DiagElem(iPivot) = -vPivot; // Replace pivot by its inverse
      //
      for (Int_t j=0; j<nGlo; j++) {      
	for (Int_t jj=0; jj<nGlo; jj++) {  
	  if (j != iPivot && jj != iPivot) {// Other elements (!!! do them first as you use old matV[k][j]'s !!!)	  
	    double &r = j>=jj ? (*this)(j,jj) : (*fgBuffer)(jj,j);
	    r -= vPivot* ( j>iPivot  ? Query(j,iPivot)  : fgBuffer->Query(iPivot,j) )
	      *          ( iPivot>jj ? Query(iPivot,jj) : fgBuffer->Query(jj,iPivot));
	  }
	}
      }
      //
      for (Int_t j=0; j<nGlo; j++) if (j != iPivot) { // Pivot row or column elements 
	  (*this)(j,iPivot)     *= vPivot;
	  (*fgBuffer)(iPivot,j) *= vPivot;
	}
      //
    }
    else {  // No more pivot value (clear those elements)
      for (Int_t j=0; j<nGlo; j++) {
	if (bUnUsed[j]) {
	  vecB[j] = 0.0;
	  for (Int_t k=0; k<nGlo; k++) {
	    (*this)(j,k) = 0.;
	    if (j!=k) (*fgBuffer)(j,k) = 0;
	  }
	}
      }
      break;  // No more pivots anyway, stop here
    }
  }
  //
  if (stabilize) for (Int_t i=0; i<nGlo; i++) for (Int_t j=0; j<nGlo; j++) {
	double vl = TMath::Sqrt(colMax[i])*TMath::Sqrt(rowMax[j]); // Correct matrix V
	if (i>=j) (*this)(i,j) *= vl;
	else      (*fgBuffer)(j,i) *= vl;
      }
  //
  for (Int_t j=0; j<nGlo; j++) {
    rowMax[j] = 0.0;
    for (Int_t jj=0; jj<nGlo; jj++) { // Reverse matrix elements
      double vl;
      if (j>=jj) vl = (*this)(j,jj)     = -Query(j,jj);
      else       vl = (*fgBuffer)(j,jj) = -fgBuffer->Query(j,jj);
      rowMax[j] += vl*vecB[jj];
    }		
  }
  
  for (Int_t j=0; j<nGlo; j++) {
    vecB[j] = rowMax[j]; // The final result
  }
  //
  delete [] bUnUsed;
  delete [] rowMax;
  if (stabilize) delete [] colMax;

  return nRank;
}
Commit	Line	Data
7c3070ec	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	/**********************************************************************************************/
8a9ab0eb	13	#include <stdlib.h>
	14	#include <stdio.h>
	15	#include <iostream>
7c3070ec	16	#include <float.h>
68f76645	17	#include <string.h>
8a9ab0eb	18	//
7c3070ec	19	#include <TClass.h>
7c3070ec	20	#include <TMath.h>
8a9ab0eb	21	#include "AliSymMatrix.h"
7c3070ec	22	#include "AliLog.h"
8a9ab0eb	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	{
68f76645	36	// default constructor
8a9ab0eb	37	fSymmetric = kTRUE;
	38	fgCopyCnt++;
	39	}
	40
	41	//___________________________________________________________
	42	AliSymMatrix::AliSymMatrix(Int_t size)
	43	: AliMatrixSq(),fElems(0),fElemsAdd(0)
	44	{
68f76645	45	//constructor for matrix with defined size
8a9ab0eb	46	fNrows = 0;
7c3070ec	47	fNrowIndex = fNcols = fRowLwb = size;
8a9ab0eb	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	{
68f76645	59	// copy constructor
8a9ab0eb	60	fNrowIndex = fNcols = src.GetSize();
8a9ab0eb	61	fNrows = 0;
7c3070ec	62	fRowLwb = src.GetSizeUsed();
8a9ab0eb	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));
7c3070ec	68	if (src.GetSizeAdded()) { // transfer extra rows to main matrix
8a9ab0eb	69	Double_t *pnt = fElems + nmainel;
7c3070ec	70	int ncl = src.GetSizeBooked() + 1;
7c3070ec	71	for (int ir=0;ir<src.GetSizeAdded();ir++) {
8a9ab0eb	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	{
68f76645	94	// assignment operator
8a9ab0eb	95	if (this != &src) {
8a9ab0eb	96	TObject::operator=(src);
7c3070ec	97	if (GetSizeBooked()!=src.GetSizeBooked() && GetSizeAdded()!=src.GetSizeAdded()) {
8a9ab0eb	98	// recreate the matrix
8a9ab0eb	99	if (fElems) delete[] fElems;
7c3070ec	100	for (int i=0;i<GetSizeAdded();i++) delete[] fElemsAdd[i];
8a9ab0eb	101	delete[] fElemsAdd;
8a9ab0eb	102	//
de34b538	103	fNrowIndex = src.GetSize();
de34b538	104	fNcols = src.GetSize();
8a9ab0eb	105	fNrows = 0;
7c3070ec	106	fRowLwb = src.GetSizeUsed();
	107	fElems = new Double_t[GetSize()*(GetSize()+1)/2];
	108	int nmainel = src.GetSizeBooked()*(src.GetSizeBooked()+1);
8a9ab0eb	109	memcpy(fElems,src.fElems,nmainel*sizeof(Double_t));
7c3070ec	110	if (src.GetSizeAdded()) { // transfer extra rows to main matrix
c8f37c50	111	Double_t pnt = fElems + nmainel;//sizeof(Double_t);
7c3070ec	112	int ncl = src.GetSizeBooked() + 1;
7c3070ec	113	for (int ir=0;ir<src.GetSizeAdded();ir++) {
8a9ab0eb	114	ncl += ir;
8a9ab0eb	115	memcpy(pnt,src.fElemsAdd[ir],ncl*sizeof(Double_t));
c8f37c50	116	pnt += ncl;//*sizeof(Double_t);
8a9ab0eb	117	}
	118	}
	119	//
	120	}
	121	else {
7c3070ec	122	memcpy(fElems,src.fElems,GetSizeBooked()(GetSizeBooked()+1)/2sizeof(Double_t));
	123	int ncl = GetSizeBooked() + 1;
	124	for (int ir=0;ir<GetSizeAdded();ir++) { // dynamic rows
8a9ab0eb	125	ncl += ir;
	126	memcpy(fElemsAdd[ir],src.fElemsAdd[ir],ncl*sizeof(Double_t));
	127	}
	128	}
	129	}
	130	//
	131	return *this;
	132	}
	133
	134	//___________________________________________________________
7c3070ec	135	AliSymMatrix& AliSymMatrix::operator+=(const AliSymMatrix& src)
7c3070ec	136	{
68f76645	137	// add operator
7c3070ec	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	//___________________________________________________________
8a9ab0eb	147	void AliSymMatrix::Clear(Option_t*)
8a9ab0eb	148	{
68f76645	149	// clear dynamic part
8a9ab0eb	150	if (fElems) {delete[] fElems; fElems = 0;}
	151	//
	152	if (fElemsAdd) {
7c3070ec	153	for (int i=0;i<GetSizeAdded();i++) delete[] fElemsAdd[i];
8a9ab0eb	154	delete[] fElemsAdd;
	155	fElemsAdd = 0;
	156	}
7c3070ec	157	fNrowIndex = fNcols = fNrows = fRowLwb = 0;
8a9ab0eb	158	//
	159	}
	160
	161	//___________________________________________________________
	162	Float_t AliSymMatrix::GetDensity() const
	163	{
	164	// get fraction of non-zero elements
	165	Int_t nel = 0;
7c185459	166	for (int i=GetSizeUsed();i--;) for (int j=i+1;j--;) if (!IsZero(GetEl(i,j))) nel++;
7c3070ec	167	return 2.nel/( (GetSizeUsed()+1)GetSizeUsed() );
8a9ab0eb	168	}
	169
	170	//___________________________________________________________
	171	void AliSymMatrix::Print(Option_t* option) const
	172	{
68f76645	173	// print itself
7c3070ec	174	printf("Symmetric Matrix: Size = %d (%d rows added dynamically), %d used\n",GetSize(),GetSizeAdded(),GetSizeUsed());
8a9ab0eb	175	TString opt = option; opt.ToLower();
	176	if (opt.IsNull()) return;
	177	opt = "%"; opt += 1+int(TMath::Log10(double(GetSize()))); opt+="d\|";
7c3070ec	178	for (Int_t i=0;i<GetSizeUsed();i++) {
8a9ab0eb	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	//___________________________________________________________
551c9e69	186	void AliSymMatrix::MultiplyByVec(const Double_t vecIn,Double_t vecOut) const
8a9ab0eb	187	{
	188	// fill vecOut by matrix*vecIn
	189	// vector should be of the same size as the matrix
7c3070ec	190	for (int i=GetSizeUsed();i--;) {
8a9ab0eb	191	vecOut[i] = 0.0;
7c3070ec	192	for (int j=GetSizeUsed();j--;) vecOut[i] += vecIn[j]*GetEl(i,j);
8a9ab0eb	193	}
	194	//
	195	}
	196
	197	//___________________________________________________________
	198	AliSymMatrix* AliSymMatrix::DecomposeChol()
	199	{
	200	// Return a matrix with Choleski decomposition
de34b538	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.
8a9ab0eb	207	//
7c3070ec	208	if (!fgBuffer \|\| fgBuffer->GetSizeUsed()!=GetSizeUsed()) {
8a9ab0eb	209	delete fgBuffer;
	210	try {
	211	fgBuffer = new AliSymMatrix(*this);
	212	}
	213	catch(bad_alloc&) {
7c185459	214	AliInfo("Failed to allocate memory for Choleski decompostions");
8a9ab0eb	215	return 0;
	216	}
	217	}
	218	else (fgBuffer) = this;
	219	//
	220	AliSymMatrix& mchol = *fgBuffer;
	221	//
7c3070ec	222	for (int i=0;i<GetSizeUsed();i++) {
de34b538	223	Double_t *rowi = mchol.GetRow(i);
7c3070ec	224	for (int j=i;j<GetSizeUsed();j++) {
de34b538	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];
8a9ab0eb	228	if (i == j) {
8a9ab0eb	229	if (sum <= 0.0) { // not positive-definite
7c185459	230	AliInfo(Form("The matrix is not positive definite [%e]\n"
	231	"Choleski decomposition is not possible",sum));
	232	Print("l");
8a9ab0eb	233	return 0;
8a9ab0eb	234	}
de34b538	235	rowi[i] = TMath::Sqrt(sum);
8a9ab0eb	236	//
de34b538	237	} else rowj[i] = sum/rowi[i];
8a9ab0eb	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) {
7c185459	250	AliInfo("Failed to invert the matrix");
8a9ab0eb	251	return kFALSE;
	252	}
	253	//
	254	InvertChol(mchol);
	255	return kTRUE;
	256	//
	257	}
de34b538	258
8a9ab0eb	259	//___________________________________________________________
	260	void AliSymMatrix::InvertChol(AliSymMatrix* pmchol)
	261	{
	262	// Invert matrix using Choleski decomposition, provided the Cholseki's L matrix
5d88242b	263	//
8a9ab0eb	264	Double_t sum;
	265	AliSymMatrix& mchol = *pmchol;
	266	//
	267	// Invert decomposed triangular L matrix (Lower triangle is filled)
7c3070ec	268	for (int i=0;i<GetSizeUsed();i++) {
8a9ab0eb	269	mchol(i,i) = 1.0/mchol(i,i);
7c3070ec	270	for (int j=i+1;j<GetSizeUsed();j++) {
de34b538	271	Double_t *rowj = mchol.GetRow(j);
8a9ab0eb	272	sum = 0.0;
de34b538	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];
8a9ab0eb	277	}
	278	}
	279	//
	280	// take product of the inverted Choleski L matrix with its transposed
7c3070ec	281	for (int i=GetSizeUsed();i--;) {
8a9ab0eb	282	for (int j=i+1;j--;) {
8a9ab0eb	283	sum = 0;
7c3070ec	284	for (int k=i;k<GetSizeUsed();k++) {
de34b538	285	double &mik = mchol(i,k);
	286	if (mik) {
	287	double &mjk = mchol(j,k);
	288	if (mjk) sum += mik*mjk;
	289	}
	290	}
8a9ab0eb	291	(*this)(j,i) = sum;
	292	}
	293	}
	294	//
	295	}
	296
	297
	298	//___________________________________________________________
	299	Bool_t AliSymMatrix::SolveChol(Double_t *b, Bool_t invert)
	300	{
de34b538	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].
8a9ab0eb	307	//
	308	Int_t i,k;
	309	Double_t sum;
	310	//
	311	AliSymMatrix *pmchol = DecomposeChol();
	312	if (!pmchol) {
7c185459	313	AliInfo("SolveChol failed");
5d88242b	314	// Print("l");
8a9ab0eb	315	return kFALSE;
	316	}
	317	AliSymMatrix& mchol = *pmchol;
	318	//
7c3070ec	319	for (i=0;i<GetSizeUsed();i++) {
de34b538	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];
8a9ab0eb	323	}
de34b538	324	//
7c3070ec	325	for (i=GetSizeUsed()-1;i>=0;i--) {
7c3070ec	326	for (sum=b[i],k=i+1;k<GetSizeUsed();k++) if (b[k]) {
de34b538	327	double &mki=mchol(k,i); if (mki) sum -= mki*b[k];
de34b538	328	}
8a9ab0eb	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	{
7c3070ec	347	memcpy(bsol,brhs,GetSizeUsed()*sizeof(Double_t));
8a9ab0eb	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	{
68f76645	361	// add empty rows
8a9ab0eb	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++;
7c3070ec	371	fRowLwb++;
8a9ab0eb	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;
7c3070ec	386	fNcols = fRowLwb = fNrowIndex;
7c3070ec	387	fElems = new Double_t[GetSize()*(GetSize()+1)/2];
8a9ab0eb	388	fNrows = 0;
8a9ab0eb	389	}
7c3070ec	390	if (fElems) memset(fElems,0,GetSize()(GetSize()+1)/2sizeof(Double_t));
8a9ab0eb	391	//
	392	}
	393
	394	//___________________________________________________________
de34b538	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	{
68f76645	429	// get pointer on the row
7c3070ec	430	if (r>=GetSize()) {
	431	int nn = GetSize();
	432	AddRows(r-GetSize()+1);
7c185459	433	AliDebug(2,Form("create %d of %d\n",r, nn));
7c3070ec	434	return &((fElemsAdd[r-GetSizeBooked()])[0]);
de34b538	435	}
	436	else return &fElems[GetIndex(r,0)];
	437	}
	438
	439
	440	//___________________________________________________________
8a9ab0eb	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.;
7c3070ec	451	double eps = 1e-14;
7c3070ec	452	int nGlo = GetSizeUsed();
8a9ab0eb	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--;) {
de34b538	461	double vl = TMath::Abs(Query(i,j));
7c185459	462	if (IsZero(vl)) continue;
8a9ab0eb	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--;) {
7c185459	471	if (!IsZero(rowMax[i])) rowMax[i] = 1./rowMax[i]; // Max elemt of row i
7c185459	472	if (!IsZero(colMax[i])) colMax[i] = 1./colMax[i]; // Max elemt of column i
8a9ab0eb	473	}
	474	//
	475	}
	476	//
	477	for (Int_t i=nGlo; i--;) bUnUsed[i] = true;
	478	//
7c3070ec	479	if (!fgBuffer \|\| fgBuffer->GetSizeUsed()!=GetSizeUsed()) {
8a9ab0eb	480	delete fgBuffer;
	481	try {
	482	fgBuffer = new AliSymMatrix(*this);
	483	}
	484	catch(bad_alloc&) {
7c185459	485	AliError("Failed to allocate memory for matrix inversion buffer");
8a9ab0eb	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++) {
de34b538	493	double vl = Query(i,j);
7c185459	494	if (!IsZero(vl)) SetEl(i,j, TMath::Sqrt(rowMax[i])vlTMath::Sqrt(colMax[j]) ); // Equilibrate the V matrix
8a9ab0eb	495	}
8a9ab0eb	496	for (int j=i+1;j<nGlo;j++) {
de34b538	497	double vl = Query(j,i);
7c185459	498	if (!IsZero(vl)) fgBuffer->SetEl(j,i,TMath::Sqrt(rowMax[i])vlTMath::Sqrt(colMax[j]) ); // Equilibrate the V matrix
8a9ab0eb	499	}
	500	}
	501	//
de34b538	502	for (Int_t j=nGlo; j--;) fgBuffer->DiagElem(j) = TMath::Abs(QueryDiag(j)); // save diagonal elem absolute values
8a9ab0eb	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;
de34b538	510	if (bUnUsed[j] && (TMath::Abs(vl=QueryDiag(j))>TMath::Max(TMath::Abs(vPivot),eps*fgBuffer->QueryDiag(j)))) {
8a9ab0eb	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);
de34b538	526	r -= vPivot* ( j>iPivot ? Query(j,iPivot) : fgBuffer->Query(iPivot,j) )
de34b538	527	* ( iPivot>jj ? Query(iPivot,jj) : fgBuffer->Query(jj,iPivot));
8a9ab0eb	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	//
c8f37c50	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	}
8a9ab0eb	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;
de34b538	562	if (j>=jj) vl = (*this)(j,jj) = -Query(j,jj);
de34b538	563	else vl = (*fgBuffer)(j,jj) = -fgBuffer->Query(j,jj);
8a9ab0eb	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	}
5d88242b	578
5d88242b	579