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