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

/* ----------------------------------------------------------------------------------------
   Class to solve a set of N linearized parametric equations of the type
   Eq(k): sum_i=0^n { g_i G_ik }  + t_k T_k = res_k
   whith n "global" parameters gi and one "local" parameter (per equation) t_k.
   Each measured points provides 3 measured coordinates, with proper covariance matrix.

   Used for Newton-Raphson iteration step in solution of non-linear parametric equations
   F(g,t_k) - res_k = 0, with G_ik = dF(g,t_k)/dg_i and T_k = dF(g,t_k)/dt_k
   Solution is obtained by elimination of local parameters via large (n+N) matrix partitioning 

   Author: ruben.shahoyan@cern.ch
-------------------------------------------------------------------------------------------*/ 
#include "AliParamSolver.h"
#include "AliSymMatrix.h"
#include "AliLog.h"
#include <TMath.h>

ClassImp(AliParamSolver)

//______________________________________________________________________________________
AliParamSolver::AliParamSolver()
: fMatrix(0),fSolGlo(0),fSolLoc(0),fMaxGlobal(0),fNGlobal(0),fNPoints(0),fMaxPoints(0),
  fRHSGlo(0),fRHSLoc(0),fMatGamma(0),fMatG(0),fCovDGl(0)
{ 
  // default constructor
}

//______________________________________________________________________________________
AliParamSolver::AliParamSolver(Int_t maxglo,Int_t locbuff)
: fMatrix(0),fSolGlo(0),fSolLoc(0),fMaxGlobal(maxglo),fNGlobal(maxglo),fNPoints(0),fMaxPoints(0),
  fRHSGlo(0),fRHSLoc(0),fMatGamma(0),fMatG(0),fCovDGl(0)
{ 
  // constructor for nglo globals
  Init(locbuff);
}

//______________________________________________________________________________________
AliParamSolver::AliParamSolver(const AliParamSolver& src)
  : TObject(src),fMatrix(0),fSolGlo(0),fSolLoc(0),fMaxGlobal(src.fMaxGlobal),fNGlobal(src.fNGlobal),
    fNPoints(0),fMaxPoints(0),fRHSGlo(0),fRHSLoc(0),fMatGamma(0),fMatG(0),fCovDGl(0)
{ 
  // copy constructor 
  if (src.fMatrix) {
    Init(src.fMaxPoints);
    (*this) = src;
  }
}

//______________________________________________________________________________________
AliParamSolver& AliParamSolver::operator=(const AliParamSolver& src)
{
  // assignment operator
  if (this==&src) return *this;
  TObject::operator=(src);
  if (src.fMatrix && (fNGlobal!=src.fNGlobal || fMaxPoints<src.fNPoints)) {
    fNGlobal   = src.fNGlobal;
    fMaxGlobal = src.fMaxGlobal;
    if (fMatrix)   delete   fMatrix; fMatrix = 0;
    if (fSolGlo)   delete[] fSolGlo; fSolGlo = 0;
    if (fSolLoc)   delete[] fSolLoc; fSolLoc = 0;
    if (fRHSGlo)   delete[] fRHSGlo; fRHSGlo = 0;
    if (fRHSLoc)   delete[] fRHSLoc; fRHSLoc = 0;
    if (fMatGamma) delete[] fMatGamma; fMatGamma = 0;
    if (fMatG)     delete[] fMatG; fMatG = 0;
    if (fCovDGl)   delete[] fCovDGl; fCovDGl = 0;
    Init(src.fMaxPoints);
  }
  if (src.fMatrix) {
    (*fMatrix) = *(src.fMatrix);
    memcpy(fSolGlo,src.fSolGlo,fNGlobal*sizeof(double));
    memcpy(fSolLoc,src.fSolLoc,fNPoints*sizeof(double));
    memcpy(fRHSGlo,src.fRHSGlo,fNGlobal*sizeof(double));
    memcpy(fRHSLoc,src.fRHSLoc,fNPoints*sizeof(double));
    memcpy(fMatGamma,src.fMatGamma,fNPoints*sizeof(double));
    memcpy(fMatG,src.fMatG,fNPoints*fNGlobal*sizeof(double));
  }
  //
  return *this;
}

//______________________________________________________________________________________
AliParamSolver::~AliParamSolver()
{ 
  // destructor
  delete fMatrix;
  delete[] fSolGlo;
  delete[] fSolLoc;
  delete[] fRHSGlo;
  delete[] fRHSLoc;
  delete[] fMatGamma;
  delete[] fMatG;
  delete[] fCovDGl;
}

//______________________________________________________________________________________
Bool_t AliParamSolver::SolveGlobals(Bool_t obtainCov)
{
  // solve against global vars.
  if (fNPoints<fNGlobal/3) {
    AliError(Form("Number of points: %d is not enough for %d globals",fNPoints,fNGlobal));
    return kFALSE;
  }
  //
  if (!TestBit(kBitGloSol)) {
    if (!fMatrix->SolveChol(fRHSGlo, fSolGlo, obtainCov)) {
      AliError("Solution Failed");
      return kFALSE;
    }
    SetBit(kBitGloSol);
    if (obtainCov) SetBit(kBitCInv);
  }
  return kTRUE;
}

//______________________________________________________________________________________
Bool_t AliParamSolver::SolveLocals()
{
  // solve for locals
  const double kTiny = 1e-16;
  if (TestBit(kBitLocSol)) return kTRUE;
  if (!TestBit(kBitGloSol)) {
    AliError("Cannot solve for Locals before SolveGlobals is called");
    return kFALSE;
  }
  for (int i=fNPoints;i--;) {
    if (TMath::Abs(fMatGamma[i])<kTiny) {fSolLoc[i] = 0; continue;}
    double beta = fRHSLoc[i];
    double *mtG = fMatG + i*fNGlobal;  // G_i
    for (int j=fNGlobal;j--;) beta -= mtG[j]*fSolGlo[j];
    fSolLoc[i] = beta/fMatGamma[i];   // Gamma^-1 * (beta - G_i * a)
  }
  SetBit(kBitLocSol);
  return kTRUE;
}

//______________________________________________________________________________________
AliSymMatrix* AliParamSolver::GetCovMatrix()
{
  // obtain cov.mat
  if (!TestBit(kBitGloSol)) {
    AliError("Cannot obtain Cov.Matrix before SolveGlobals is called");
    return 0;
  }
  if (TestBit(kBitCInv)) return fMatrix;
  //
  if (fMatrix->InvertChol()) {
    SetBit(kBitCInv);
    return fMatrix;
  }
  return 0;
}

//______________________________________________________________________________________
Bool_t AliParamSolver::Solve(Bool_t obtainCov)
{
  // solve all
  return (SolveGlobals(obtainCov) && SolveLocals()) ? kTRUE : kFALSE; 
}

//______________________________________________________________________________________
void AliParamSolver::AddEquation(const Double_t* dGl,const Double_t *dLc,const Double_t *res, const Double_t *covI,Double_t sclErrI)
{
  // add the measured point to chi2 normal equations
  // Input: 
  // dGl : NGlo x 3 matrix of derivative for each of the 3 coordinates vs global params
  // dLc : 3-vector of derivative for each coordinate vs local param
  // res : residual of the point (extrapolated - measured)
  // covI: 3 x (3+1)/2 matrix of the (inverted) errors (symmetric)
  // sclErrI: scaling coefficient to apply to inverted errors (used if >0)
  // The contribution of the point to chi2 is  V * covI * V 
  // with component of the vector V(i) = dGl[i]*glob + dLc[i]*loc - res[i] 
  //
  // Instead of the NGlo + NMeasPoints size matrix we create only NGlo size matrix using the 
  // reduction a la millepede : http://www.desy.de/~blobel/millepede1.ps
  const double kTiny = 1e-16;
  //
  if (fNPoints+1 == fMaxPoints) ExpandStorage((fNPoints+1)*2);
  ResetBit(kBitGloSol|kBitLocSol);
  //
  if (TestBit(kBitCInv)) { // solution was obtained and the matrix was inverted for previous points
    fMatrix->InvertChol();
    ResetBit(kBitCInv);
  }
  //
  double *mtG   = fMatG + fNPoints*fNGlobal;  // G_i
  double &beta  = fRHSLoc[fNPoints];
  double &gamma = fMatGamma[fNPoints];
  double cDl[3];
  //
  // cov * dR/dloc
  cDl[kX] = covI[kXX]*dLc[kX] + covI[kXY]*dLc[kY] + covI[kXZ]*dLc[kZ];
  cDl[kY] = covI[kXY]*dLc[kX] + covI[kYY]*dLc[kY] + covI[kYZ]*dLc[kZ];
  cDl[kZ] = covI[kXZ]*dLc[kX] + covI[kYZ]*dLc[kY] + covI[kZZ]*dLc[kZ];
  if (sclErrI>0) { cDl[kX] *= sclErrI; cDl[kY] *= sclErrI; cDl[kZ] *= sclErrI;}
  //
  for (int i=fNGlobal;i--;) {
    const double *dGli = dGl+i*3;  // derivatives of XYZ vs i-th global param
    double *cDgi = fCovDGl+i*3;    // cov * dR/dGl_i
    cDgi[kX] = covI[kXX]*dGli[kX] + covI[kXY]*dGli[kY] + covI[kXZ]*dGli[kZ];
    cDgi[kY] = covI[kXY]*dGli[kX] + covI[kYY]*dGli[kY] + covI[kYZ]*dGli[kZ];
    cDgi[kZ] = covI[kXZ]*dGli[kX] + covI[kYZ]*dGli[kY] + covI[kZZ]*dGli[kZ];
    if (sclErrI>0) { cDgi[kX] *= sclErrI; cDgi[kY] *= sclErrI; cDgi[kZ] *= sclErrI;}
    //
    mtG[i]   = cDl[kX]*dGli[kX] + cDl[kY]*dGli[kY] + cDl[kZ]*dGli[kZ];  // dR/dGl_i * cov * dR/dLoc
  }
  beta    = res[kX]*cDl[kX] + res[kY]*cDl[kY] + res[kZ]*cDl[kZ];  //RHS: res*cov*dR/dloc
  gamma   = dLc[kX]*cDl[kX] + dLc[kY]*cDl[kY] + dLc[kZ]*cDl[kZ];  //RHS: dR/dloc*cov*dR/dloc
  double locSol  = TMath::Abs(gamma)<kTiny ? 0. : beta/gamma;     //local solution: gamma^-1 beta
  //
  AliSymMatrix &matC = *fMatrix;
  for (int i=fNGlobal;i--;) {
    const double *cDgi = fCovDGl+i*3;    // cov * dR/dGl_i
    //
    fRHSGlo[i] += cDgi[kX]*res[kX] + cDgi[kY]*res[kY] + cDgi[kZ]*res[kZ]      // b_i = dR/dGi * cov * res
      -           mtG[i]*locSol;                                              //     - [G gamma^-1 beta ]_i
    //
    for (int j=i+1;j--;) {
      //      const double *cDgj = fCovDGl+j*3;  // cov * dR/dGl_j
      const double *dGlj = dGl+j*3;        // derivatives of XYZ vs i-th global param
      double add = dGlj[kX]*cDgi[kX] + dGlj[kY]*cDgi[kY] + dGlj[kZ]*cDgi[kZ];  // C_ij = dR/dGi * cov * dR/dGj
      if (TMath::Abs(gamma)>kTiny) add -= mtG[i]*mtG[j]/gamma;                 //        - [G gamma^-1 T(G) ]_ij      
      matC(i,j) += add;       
    }
  }
  //
  fNPoints++;
  //
}

//______________________________________________________________________________________
void AliParamSolver::AddConstraint(Int_t parID, Double_t val, Double_t err2inv)
{
  // add gassian constriant to parameter parID
  if (parID>=fNGlobal) {
    AliError(Form("Attempt to constraint non-existing parameter %d",parID));
    return;
  }
  //
  (*fMatrix)(parID,parID) += err2inv;
  fRHSGlo[parID] += val*err2inv;
  //
}

//______________________________________________________________________________________
void AliParamSolver::Init(Int_t npini)
{
  // create storage assuming maximum npini measured points
  fMatrix = new AliSymMatrix(fMaxGlobal);
  fSolGlo = new Double_t[fMaxGlobal];
  fRHSGlo = new Double_t[fMaxGlobal];
  //
  fCovDGl = new Double_t[3*fMaxGlobal];
  ExpandStorage(npini);
  fMatrix->SetSizeUsed(fNGlobal);
  //
}

//______________________________________________________________________________________
void AliParamSolver::ExpandStorage(Int_t newSize)
{
  // increase space to newSize measured points
  newSize = newSize>fMaxPoints ? newSize : fMaxPoints+1;
  double* tmp;
  tmp = new Double_t[newSize];
  if (fSolLoc) delete[] fSolLoc; 
  fSolLoc = tmp;
  //
  tmp = new Double_t[newSize];
  if (fMatGamma) {
    memcpy(tmp,fMatGamma,fNPoints*sizeof(Double_t));
    delete[] fMatGamma;
  }
  fMatGamma = tmp;
  //
  tmp = new Double_t[newSize];
  if (fRHSLoc) {
    memcpy(tmp,fRHSLoc,fNPoints*sizeof(Double_t));
    delete[] fRHSLoc;
  }
  fRHSLoc = tmp;
  //
  tmp = new Double_t[newSize*fMaxGlobal];
  if (fMatG) {
    memcpy(tmp,fMatG,fNPoints*fMaxGlobal*sizeof(Double_t));
    delete[] fMatG;
  }
  fMatG = tmp;
  //
  fMaxPoints = newSize;
  //
}

//______________________________________________________________________________________
void AliParamSolver::Clear(Option_t*)
{
  // reset all
  fNPoints = 0;
  fMatrix->Reset();
  for (int i=fNGlobal;i--;) fRHSGlo[i] = 0;
  ResetBit(kBitGloSol | kBitLocSol | kBitCInv);
}

//______________________________________________________________________________________
void AliParamSolver::Print(Option_t*) const
{
  // print itself
  AliInfo(Form("Solver with %d globals for %d points",fNGlobal,fNPoints));
}

//______________________________________________________________________________________
void AliParamSolver::SetNGlobal(Int_t n) 
{
  // set N global params
  if (n>fMaxGlobal) {
    AliError(Form("Maximum number of globals was set to %d",fMaxGlobal));
    return;
  }
  fNGlobal = n;
  fMatrix->SetSizeUsed(fNGlobal);
}

//______________________________________________________________________________________
void AliParamSolver::SetMaxGlobal(Int_t n) 
{
  // set limit on N glob.
  if (n>0 && n==fMaxGlobal) return;
  fMaxGlobal = n;
  fNGlobal = n;
  if (fMatrix)   delete   fMatrix;   fMatrix = 0;
  if (fSolGlo)   delete[] fSolGlo;   fSolGlo = 0;
  if (fSolLoc)   delete[] fSolLoc;   fSolLoc = 0;
  if (fRHSGlo)   delete[] fRHSGlo;   fRHSGlo = 0;
  if (fRHSLoc)   delete[] fRHSLoc;   fRHSLoc = 0;
  if (fMatGamma) delete[] fMatGamma; fMatGamma = 0;
  if (fMatG)     delete[] fMatG;     fMatG = 0;
  if (fCovDGl)   delete[] fCovDGl;   fCovDGl = 0;
  n = TMath::Max(16,fMaxPoints);
  Init(n);
  //
}
Commit	Line	Data
339fbe23	1	/* ----------------------------------------------------------------------------------------
	2	Class to solve a set of N linearized parametric equations of the type
	3	Eq(k): sum_i=0^n { g_i G_ik } + t_k T_k = res_k
	4	whith n "global" parameters gi and one "local" parameter (per equation) t_k.
	5	Each measured points provides 3 measured coordinates, with proper covariance matrix.
	6
	7	Used for Newton-Raphson iteration step in solution of non-linear parametric equations
	8	F(g,t_k) - res_k = 0, with G_ik = dF(g,t_k)/dg_i and T_k = dF(g,t_k)/dt_k
	9	Solution is obtained by elimination of local parameters via large (n+N) matrix partitioning
	10
	11	Author: ruben.shahoyan@cern.ch
	12	-------------------------------------------------------------------------------------------*/
3a11fb02	13	#include "AliParamSolver.h"
	14	#include "AliSymMatrix.h"
	15	#include "AliLog.h"
	16	#include <TMath.h>
	17
	18	ClassImp(AliParamSolver)
	19
	20	//______________________________________________________________________________________
	21	AliParamSolver::AliParamSolver()
	22	: fMatrix(0),fSolGlo(0),fSolLoc(0),fMaxGlobal(0),fNGlobal(0),fNPoints(0),fMaxPoints(0),
	23	fRHSGlo(0),fRHSLoc(0),fMatGamma(0),fMatG(0),fCovDGl(0)
	24	{
	25	// default constructor
	26	}
	27
	28	//______________________________________________________________________________________
	29	AliParamSolver::AliParamSolver(Int_t maxglo,Int_t locbuff)
	30	: fMatrix(0),fSolGlo(0),fSolLoc(0),fMaxGlobal(maxglo),fNGlobal(maxglo),fNPoints(0),fMaxPoints(0),
	31	fRHSGlo(0),fRHSLoc(0),fMatGamma(0),fMatG(0),fCovDGl(0)
	32	{
	33	// constructor for nglo globals
	34	Init(locbuff);
	35	}
	36
	37	//______________________________________________________________________________________
339fbe23	38	AliParamSolver::AliParamSolver(const AliParamSolver& src)
3a11fb02	39	: TObject(src),fMatrix(0),fSolGlo(0),fSolLoc(0),fMaxGlobal(src.fMaxGlobal),fNGlobal(src.fNGlobal),
	40	fNPoints(0),fMaxPoints(0),fRHSGlo(0),fRHSLoc(0),fMatGamma(0),fMatG(0),fCovDGl(0)
	41	{
	42	// copy constructor
	43	if (src.fMatrix) {
	44	Init(src.fMaxPoints);
	45	(*this) = src;
	46	}
	47	}
	48
	49	//______________________________________________________________________________________
	50	AliParamSolver& AliParamSolver::operator=(const AliParamSolver& src)
	51	{
	52	// assignment operator
	53	if (this==&src) return *this;
	54	TObject::operator=(src);
	55	if (src.fMatrix && (fNGlobal!=src.fNGlobal \|\| fMaxPoints<src.fNPoints)) {
	56	fNGlobal = src.fNGlobal;
	57	fMaxGlobal = src.fMaxGlobal;
c8f37c50	58	if (fMatrix) delete fMatrix; fMatrix = 0;
	59	if (fSolGlo) delete[] fSolGlo; fSolGlo = 0;
	60	if (fSolLoc) delete[] fSolLoc; fSolLoc = 0;
	61	if (fRHSGlo) delete[] fRHSGlo; fRHSGlo = 0;
	62	if (fRHSLoc) delete[] fRHSLoc; fRHSLoc = 0;
	63	if (fMatGamma) delete[] fMatGamma; fMatGamma = 0;
	64	if (fMatG) delete[] fMatG; fMatG = 0;
	65	if (fCovDGl) delete[] fCovDGl; fCovDGl = 0;
3a11fb02	66	Init(src.fMaxPoints);
	67	}
	68	if (src.fMatrix) {
	69	(fMatrix) = (src.fMatrix);
	70	memcpy(fSolGlo,src.fSolGlo,fNGlobal*sizeof(double));
	71	memcpy(fSolLoc,src.fSolLoc,fNPoints*sizeof(double));
	72	memcpy(fRHSGlo,src.fRHSGlo,fNGlobal*sizeof(double));
	73	memcpy(fRHSLoc,src.fRHSLoc,fNPoints*sizeof(double));
	74	memcpy(fMatGamma,src.fMatGamma,fNPoints*sizeof(double));
	75	memcpy(fMatG,src.fMatG,fNPointsfNGlobalsizeof(double));
	76	}
	77	//
	78	return *this;
	79	}
	80
	81	//______________________________________________________________________________________
	82	AliParamSolver::~AliParamSolver()
	83	{
	84	// destructor
	85	delete fMatrix;
	86	delete[] fSolGlo;
	87	delete[] fSolLoc;
	88	delete[] fRHSGlo;
	89	delete[] fRHSLoc;
	90	delete[] fMatGamma;
	91	delete[] fMatG;
	92	delete[] fCovDGl;
	93	}
	94
	95	//______________________________________________________________________________________
	96	Bool_t AliParamSolver::SolveGlobals(Bool_t obtainCov)
	97	{
339fbe23	98	// solve against global vars.
3a11fb02	99	if (fNPoints<fNGlobal/3) {
	100	AliError(Form("Number of points: %d is not enough for %d globals",fNPoints,fNGlobal));
	101	return kFALSE;
	102	}
	103	//
	104	if (!TestBit(kBitGloSol)) {
	105	if (!fMatrix->SolveChol(fRHSGlo, fSolGlo, obtainCov)) {
	106	AliError("Solution Failed");
	107	return kFALSE;
	108	}
	109	SetBit(kBitGloSol);
	110	if (obtainCov) SetBit(kBitCInv);
	111	}
	112	return kTRUE;
	113	}
	114
	115	//______________________________________________________________________________________
	116	Bool_t AliParamSolver::SolveLocals()
	117	{
339fbe23	118	// solve for locals
5a611869	119	const double kTiny = 1e-16;
3a11fb02	120	if (TestBit(kBitLocSol)) return kTRUE;
	121	if (!TestBit(kBitGloSol)) {
	122	AliError("Cannot solve for Locals before SolveGlobals is called");
	123	return kFALSE;
	124	}
	125	for (int i=fNPoints;i--;) {
5a611869	126	if (TMath::Abs(fMatGamma[i])<kTiny) {fSolLoc[i] = 0; continue;}
3a11fb02	127	double beta = fRHSLoc[i];
	128	double mtG = fMatG + ifNGlobal; // G_i
	129	for (int j=fNGlobal;j--;) beta -= mtG[j]*fSolGlo[j];
	130	fSolLoc[i] = beta/fMatGamma[i]; // Gamma^-1 * (beta - G_i * a)
	131	}
	132	SetBit(kBitLocSol);
	133	return kTRUE;
	134	}
	135
	136	//______________________________________________________________________________________
	137	AliSymMatrix* AliParamSolver::GetCovMatrix()
	138	{
339fbe23	139	// obtain cov.mat
3a11fb02	140	if (!TestBit(kBitGloSol)) {
	141	AliError("Cannot obtain Cov.Matrix before SolveGlobals is called");
	142	return 0;
	143	}
	144	if (TestBit(kBitCInv)) return fMatrix;
	145	//
	146	if (fMatrix->InvertChol()) {
	147	SetBit(kBitCInv);
	148	return fMatrix;
	149	}
	150	return 0;
	151	}
	152
	153	//______________________________________________________________________________________
	154	Bool_t AliParamSolver::Solve(Bool_t obtainCov)
	155	{
339fbe23	156	// solve all
3a11fb02	157	return (SolveGlobals(obtainCov) && SolveLocals()) ? kTRUE : kFALSE;
	158	}
	159
	160	//______________________________________________________________________________________
56881eaf	161	void AliParamSolver::AddEquation(const Double_t* dGl,const Double_t dLc,const Double_t res, const Double_t *covI,Double_t sclErrI)
3a11fb02	162	{
	163	// add the measured point to chi2 normal equations
	164	// Input:
	165	// dGl : NGlo x 3 matrix of derivative for each of the 3 coordinates vs global params
	166	// dLc : 3-vector of derivative for each coordinate vs local param
	167	// res : residual of the point (extrapolated - measured)
	168	// covI: 3 x (3+1)/2 matrix of the (inverted) errors (symmetric)
56881eaf	169	// sclErrI: scaling coefficient to apply to inverted errors (used if >0)
3a11fb02	170	// The contribution of the point to chi2 is V * covI * V
	171	// with component of the vector V(i) = dGl[i]glob + dLc[i]loc - res[i]
	172	//
	173	// Instead of the NGlo + NMeasPoints size matrix we create only NGlo size matrix using the
	174	// reduction a la millepede : http://www.desy.de/~blobel/millepede1.ps
	175	const double kTiny = 1e-16;
	176	//
	177	if (fNPoints+1 == fMaxPoints) ExpandStorage((fNPoints+1)*2);
	178	ResetBit(kBitGloSol\|kBitLocSol);
	179	//
	180	if (TestBit(kBitCInv)) { // solution was obtained and the matrix was inverted for previous points
	181	fMatrix->InvertChol();
	182	ResetBit(kBitCInv);
	183	}
	184	//
	185	double mtG = fMatG + fNPointsfNGlobal; // G_i
	186	double &beta = fRHSLoc[fNPoints];
	187	double &gamma = fMatGamma[fNPoints];
	188	double cDl[3];
	189	//
	190	// cov * dR/dloc
	191	cDl[kX] = covI[kXX]dLc[kX] + covI[kXY]dLc[kY] + covI[kXZ]*dLc[kZ];
	192	cDl[kY] = covI[kXY]dLc[kX] + covI[kYY]dLc[kY] + covI[kYZ]*dLc[kZ];
	193	cDl[kZ] = covI[kXZ]dLc[kX] + covI[kYZ]dLc[kY] + covI[kZZ]*dLc[kZ];
56881eaf	194	if (sclErrI>0) { cDl[kX] = sclErrI; cDl[kY] = sclErrI; cDl[kZ] *= sclErrI;}
3a11fb02	195	//
	196	for (int i=fNGlobal;i--;) {
	197	const double dGli = dGl+i3; // derivatives of XYZ vs i-th global param
	198	double cDgi = fCovDGl+i3; // cov * dR/dGl_i
	199	cDgi[kX] = covI[kXX]dGli[kX] + covI[kXY]dGli[kY] + covI[kXZ]*dGli[kZ];
	200	cDgi[kY] = covI[kXY]dGli[kX] + covI[kYY]dGli[kY] + covI[kYZ]*dGli[kZ];
	201	cDgi[kZ] = covI[kXZ]dGli[kX] + covI[kYZ]dGli[kY] + covI[kZZ]*dGli[kZ];
56881eaf	202	if (sclErrI>0) { cDgi[kX] = sclErrI; cDgi[kY] = sclErrI; cDgi[kZ] *= sclErrI;}
3a11fb02	203	//
	204	mtG[i] = cDl[kX]dGli[kX] + cDl[kY]dGli[kY] + cDl[kZ]dGli[kZ]; // dR/dGl_i cov * dR/dLoc
	205	}
	206	beta = res[kX]cDl[kX] + res[kY]cDl[kY] + res[kZ]cDl[kZ]; //RHS: rescov*dR/dloc
	207	gamma = dLc[kX]cDl[kX] + dLc[kY]cDl[kY] + dLc[kZ]cDl[kZ]; //RHS: dR/dloccov*dR/dloc
	208	double locSol = TMath::Abs(gamma)<kTiny ? 0. : beta/gamma; //local solution: gamma^-1 beta
	209	//
	210	AliSymMatrix &matC = *fMatrix;
	211	for (int i=fNGlobal;i--;) {
	212	const double cDgi = fCovDGl+i3; // cov * dR/dGl_i
	213	//
	214	fRHSGlo[i] += cDgi[kX]res[kX] + cDgi[kY]res[kY] + cDgi[kZ]res[kZ] // b_i = dR/dGi cov * res
	215	- mtG[i]*locSol; // - [G gamma^-1 beta ]_i
	216	//
	217	for (int j=i+1;j--;) {
	218	// const double cDgj = fCovDGl+j3; // cov * dR/dGl_j
	219	const double dGlj = dGl+j3; // derivatives of XYZ vs i-th global param
5a611869	220	double add = dGlj[kX]cDgi[kX] + dGlj[kY]cDgi[kY] + dGlj[kZ]cDgi[kZ]; // C_ij = dR/dGi cov * dR/dGj
	221	if (TMath::Abs(gamma)>kTiny) add -= mtG[i]*mtG[j]/gamma; // - [G gamma^-1 T(G) ]_ij
	222	matC(i,j) += add;
3a11fb02	223	}
	224	}
	225	//
	226	fNPoints++;
	227	//
	228	}
	229
	230	//______________________________________________________________________________________
	231	void AliParamSolver::AddConstraint(Int_t parID, Double_t val, Double_t err2inv)
	232	{
	233	// add gassian constriant to parameter parID
	234	if (parID>=fNGlobal) {
	235	AliError(Form("Attempt to constraint non-existing parameter %d",parID));
	236	return;
	237	}
	238	//
	239	(*fMatrix)(parID,parID) += err2inv;
	240	fRHSGlo[parID] += val*err2inv;
	241	//
	242	}
	243
	244	//______________________________________________________________________________________
	245	void AliParamSolver::Init(Int_t npini)
	246	{
	247	// create storage assuming maximum npini measured points
	248	fMatrix = new AliSymMatrix(fMaxGlobal);
	249	fSolGlo = new Double_t[fMaxGlobal];
	250	fRHSGlo = new Double_t[fMaxGlobal];
	251	//
	252	fCovDGl = new Double_t[3*fMaxGlobal];
	253	ExpandStorage(npini);
	254	fMatrix->SetSizeUsed(fNGlobal);
	255	//
	256	}
	257
	258	//______________________________________________________________________________________
	259	void AliParamSolver::ExpandStorage(Int_t newSize)
	260	{
	261	// increase space to newSize measured points
	262	newSize = newSize>fMaxPoints ? newSize : fMaxPoints+1;
	263	double* tmp;
	264	tmp = new Double_t[newSize];
	265	if (fSolLoc) delete[] fSolLoc;
	266	fSolLoc = tmp;
	267	//
	268	tmp = new Double_t[newSize];
	269	if (fMatGamma) {
	270	memcpy(tmp,fMatGamma,fNPoints*sizeof(Double_t));
	271	delete[] fMatGamma;
	272	}
	273	fMatGamma = tmp;
	274	//
	275	tmp = new Double_t[newSize];
	276	if (fRHSLoc) {
	277	memcpy(tmp,fRHSLoc,fNPoints*sizeof(Double_t));
	278	delete[] fRHSLoc;
	279	}
	280	fRHSLoc = tmp;
	281	//
	282	tmp = new Double_t[newSize*fMaxGlobal];
	283	if (fMatG) {
	284	memcpy(tmp,fMatG,fNPointsfMaxGlobalsizeof(Double_t));
	285	delete[] fMatG;
	286	}
287	fMatG = tmp;
288	//
289	fMaxPoints = newSize;
290	//
291	}
292
293	//______________________________________________________________________________________
294	void AliParamSolver::Clear(Option_t*)
295	{
339fbe23	296	// reset all
3a11fb02	297	fNPoints = 0;
	298	fMatrix->Reset();
	299	for (int i=fNGlobal;i--;) fRHSGlo[i] = 0;
	300	ResetBit(kBitGloSol \| kBitLocSol \| kBitCInv);
	301	}
	302
	303	//______________________________________________________________________________________
	304	void AliParamSolver::Print(Option_t*) const
	305	{
339fbe23	306	// print itself
3a11fb02	307	AliInfo(Form("Solver with %d globals for %d points",fNGlobal,fNPoints));
	308	}
	309
	310	//______________________________________________________________________________________
	311	void AliParamSolver::SetNGlobal(Int_t n)
	312	{
339fbe23	313	// set N global params
3a11fb02	314	if (n>fMaxGlobal) {
65b25288	315	AliError(Form("Maximum number of globals was set to %d",fMaxGlobal));
3a11fb02	316	return;
	317	}
	318	fNGlobal = n;
	319	fMatrix->SetSizeUsed(fNGlobal);
	320	}
	321
	322	//______________________________________________________________________________________
	323	void AliParamSolver::SetMaxGlobal(Int_t n)
	324	{
339fbe23	325	// set limit on N glob.
3a11fb02	326	if (n>0 && n==fMaxGlobal) return;
	327	fMaxGlobal = n;
	328	fNGlobal = n;
c8f37c50	329	if (fMatrix) delete fMatrix; fMatrix = 0;
	330	if (fSolGlo) delete[] fSolGlo; fSolGlo = 0;
	331	if (fSolLoc) delete[] fSolLoc; fSolLoc = 0;
	332	if (fRHSGlo) delete[] fRHSGlo; fRHSGlo = 0;
	333	if (fRHSLoc) delete[] fRHSLoc; fRHSLoc = 0;
	334	if (fMatGamma) delete[] fMatGamma; fMatGamma = 0;
	335	if (fMatG) delete[] fMatG; fMatG = 0;
	336	if (fCovDGl) delete[] fCovDGl; fCovDGl = 0;
3a11fb02	337	n = TMath::Max(16,fMaxPoints);
	338	Init(n);
	339	//
	340	}
	341