#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)
{
}
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)
{
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)
{
if (src.fMatrix) {
Init(src.fMaxPoints);
(*this) = src;
}
}
AliParamSolver& AliParamSolver::operator=(const AliParamSolver& src)
{
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()
{
delete fMatrix;
delete[] fSolGlo;
delete[] fSolLoc;
delete[] fRHSGlo;
delete[] fRHSLoc;
delete[] fMatGamma;
delete[] fMatG;
delete[] fCovDGl;
}
Bool_t AliParamSolver::SolveGlobals(Bool_t obtainCov)
{
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()
{
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;
for (int j=fNGlobal;j--;) beta -= mtG[j]*fSolGlo[j];
fSolLoc[i] = beta/fMatGamma[i];
}
SetBit(kBitLocSol);
return kTRUE;
}
AliSymMatrix* AliParamSolver::GetCovMatrix()
{
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)
{
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)
{
const double kTiny = 1e-16;
if (fNPoints+1 == fMaxPoints) ExpandStorage((fNPoints+1)*2);
ResetBit(kBitGloSol|kBitLocSol);
if (TestBit(kBitCInv)) {
fMatrix->InvertChol();
ResetBit(kBitCInv);
}
double *mtG = fMatG + fNPoints*fNGlobal;
double &beta = fRHSLoc[fNPoints];
double &gamma = fMatGamma[fNPoints];
double cDl[3];
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;
double *cDgi = fCovDGl+i*3;
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];
}
beta = res[kX]*cDl[kX] + res[kY]*cDl[kY] + res[kZ]*cDl[kZ];
gamma = dLc[kX]*cDl[kX] + dLc[kY]*cDl[kY] + dLc[kZ]*cDl[kZ];
double locSol = TMath::Abs(gamma)<kTiny ? 0. : beta/gamma;
AliSymMatrix &matC = *fMatrix;
for (int i=fNGlobal;i--;) {
const double *cDgi = fCovDGl+i*3;
fRHSGlo[i] += cDgi[kX]*res[kX] + cDgi[kY]*res[kY] + cDgi[kZ]*res[kZ]
- mtG[i]*locSol;
for (int j=i+1;j--;) {
const double *dGlj = dGl+j*3;
double add = dGlj[kX]*cDgi[kX] + dGlj[kY]*cDgi[kY] + dGlj[kZ]*cDgi[kZ];
if (TMath::Abs(gamma)>kTiny) add -= mtG[i]*mtG[j]/gamma;
matC(i,j) += add;
}
}
fNPoints++;
}
void AliParamSolver::AddConstraint(Int_t parID, Double_t val, Double_t err2inv)
{
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)
{
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)
{
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*)
{
fNPoints = 0;
fMatrix->Reset();
for (int i=fNGlobal;i--;) fRHSGlo[i] = 0;
ResetBit(kBitGloSol | kBitLocSol | kBitCInv);
}
void AliParamSolver::Print(Option_t*) const
{
AliInfo(Form("Solver with %d globals for %d points",fNGlobal,fNPoints));
}
void AliParamSolver::SetNGlobal(Int_t n)
{
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)
{
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);
}