Update master to aliroot
[u/mrichter/AliRoot.git] / STEER / STEER / AliParamSolver.cxx
CommitLineData
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
18ClassImp(AliParamSolver)
19
20//______________________________________________________________________________________
21AliParamSolver::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//______________________________________________________________________________________
29AliParamSolver::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 38AliParamSolver::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//______________________________________________________________________________________
50AliParamSolver& 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,fNPoints*fNGlobal*sizeof(double));
76 }
77 //
78 return *this;
79}
80
81//______________________________________________________________________________________
82AliParamSolver::~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//______________________________________________________________________________________
96Bool_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//______________________________________________________________________________________
116Bool_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 + i*fNGlobal; // 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//______________________________________________________________________________________
137AliSymMatrix* 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//______________________________________________________________________________________
154Bool_t AliParamSolver::Solve(Bool_t obtainCov)
155{
339fbe23 156 // solve all
3a11fb02 157 return (SolveGlobals(obtainCov) && SolveLocals()) ? kTRUE : kFALSE;
158}
159
160//______________________________________________________________________________________
56881eaf 161void 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 + fNPoints*fNGlobal; // 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+i*3; // derivatives of XYZ vs i-th global param
198 double *cDgi = fCovDGl+i*3; // 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: res*cov*dR/dloc
207 gamma = dLc[kX]*cDl[kX] + dLc[kY]*cDl[kY] + dLc[kZ]*cDl[kZ]; //RHS: dR/dloc*cov*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+i*3; // 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+j*3; // cov * dR/dGl_j
219 const double *dGlj = dGl+j*3; // 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//______________________________________________________________________________________
231void 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//______________________________________________________________________________________
245void 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//______________________________________________________________________________________
259void 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,fNPoints*fMaxGlobal*sizeof(Double_t));
285 delete[] fMatG;
286 }
287 fMatG = tmp;
288 //
289 fMaxPoints = newSize;
290 //
291}
292
293//______________________________________________________________________________________
294void 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//______________________________________________________________________________________
304void 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//______________________________________________________________________________________
311void 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//______________________________________________________________________________________
323void 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