← Back to branch summary

~durga/maus/online

« back to all changes in this revision

Viewing changes to src/legacy/Optics/CovarianceMatrix.cc

Builds but segv on executation of test_cpp

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/legacy/Optics/CovarianceMatrix.cc
 
1
#include "CovarianceMatrix.hh"
 
2
#include "src/legacy/Config/MiceModule.hh"
 
3
#include "src/legacy/Interface/Squeal.hh"
 
4
#include "Tensor.hh"
 
5
#include "Tensor3.hh"
 
6
#include "src/legacy/Interface/Differentiator.hh"
 
7
#include "src/legacy/Config/ModuleConverter.hh"
 
8
 
 
9
#include <sstream>
 
10
#include <iomanip>
 
11
 
 
12
using CLHEP::HepSymMatrix;
 
13
using CLHEP::HepVector;
 
14
using CLHEP::c_light;
 
15
 
 
16
const std::string CovarianceMatrix::_psvVars[5]    = {"amplitude_t", "amplitude_x", "amplitude_y", "amplitude_4d", "amplitude_6d"};
 
17
const std::string CovarianceMatrix::_bunchVars[23] = {"weight", "emit_6d", 
 
18
                                                     "emit_4d",  "emit_t",  "emit_x",  "emit_y",
 
19
                                                     "beta_4d",  "beta_t",  "beta_x",  "beta_y",
 
20
                                                     "alpha_4d", "alpha_t", "alpha_x", "alpha_y",
 
21
                                                     "gamma_4d", "gamma_t", "gamma_x", "gamma_y",
 
22
                                                     "disp_x", "disp_y",
 
23
                                                     "ltwiddle", "lcan", "lkin"};
 
24
const std::string CovarianceMatrix::_varTypes[5]   = {"Mean", "Covariance", "Standard_Deviation", "Correlation", "Bunch_Parameter"};
 
25
const std::string CovarianceMatrix::_phaseSpace[6] = {"t", "E", "x", "px", "y", "py"};
 
26
const std::string CovarianceMatrix::_traceSpace[6] = {"t", "t\'", "x", "x\'", "y", "y\'"};
 
27
 
 
28
CovarianceMatrix::CovarianceMatrix()
 
29
        : _covNorm(HepSymMatrix(6,1)), _norm(-1), _mean(PhaseSpaceVector()), _weight(1)
 
30
{}
 
31
 
 
32
CovarianceMatrix::CovarianceMatrix(HepSymMatrix covariances, PhaseSpaceVector mean, double weight, bool covIsKinetic)
 
33
                : _mean(mean), _weight(weight)
 
34
{
 
35
        HepSymMatrix cov;
 
36
        if(!covIsKinetic) cov = TransformToKinetic(covariances, mean.Bz(), mean.Ez(), 0.);
 
37
        else cov = covariances;
 
38
        SetCovariances(cov);
 
39
}
 
40
 
 
41
CovarianceMatrix::CovarianceMatrix(HepSymMatrix covariances, PhaseSpaceVector mean, 
 
42
                                   double norm, double weight, bool covIsKinetic)
 
43
                : _norm(norm), _mean(mean), _weight(weight)
 
44
{
 
45
        HepSymMatrix cov;
 
46
        if(!covIsKinetic) cov = TransformToKinetic(covariances, mean.Bz(), mean.Ez(), 0.);
 
47
        else cov = covariances;
 
48
        _covNorm = cov;
 
49
}
 
50
 
 
51
CovarianceMatrix::CovarianceMatrix(HepSymMatrix covariances, PhaseSpaceVector mean, bool covIsKinetic)
 
52
                : _mean(mean), _weight(1)
 
53
{
 
54
        HepSymMatrix cov;
 
55
        if(!covIsKinetic) cov = TransformToKinetic(covariances, mean.Bz(), mean.Ez(), 0.);
 
56
        else cov = covariances;
 
57
        SetCovariances(cov);
 
58
}
 
59
 
 
60
CovarianceMatrix::CovarianceMatrix(double emittance_t, double beta_t, double alpha_t, double Ltwiddle_t,
 
61
                                   double emittance_l, double beta_l, double alpha_l,
 
62
                                   PhaseSpaceVector mean, double weight)
 
63
                : _covNorm(HepSymMatrix(6,0)), _mean(mean), _weight(weight)
 
64
{
 
65
        SetCovariances(emittance_t, beta_t, alpha_t, Ltwiddle_t, emittance_l, beta_l, alpha_l);
 
66
}
 
67
 
 
68
CovarianceMatrix::CovarianceMatrix(double emittance_x, double beta_x, double alpha_x,
 
69
                                   double emittance_y, double beta_y, double alpha_y,
 
70
                                   double emittance_l, double beta_l, double alpha_l,
 
71
                                   PhaseSpaceVector mean, double weight)
 
72
                : _covNorm(HepSymMatrix(6,0)), _mean(mean), _weight(weight)
 
73
{
 
74
        SetCovariances(emittance_x, beta_x, alpha_x, emittance_y, beta_y, alpha_y, emittance_l, beta_l, alpha_l);
 
75
}
 
76
 
 
77
CovarianceMatrix::CovarianceMatrix(const MiceModule* mod) : _covNorm(HepSymMatrix(6,0)), _norm(1), _mean(PhaseSpaceVector()), _weight(1)
 
78
{
 
79
  MCHit               hit = ModuleConverter::ModuleToHit(mod);
 
80
  CLHEP::HepSymMatrix mat = ModuleConverter::ModuleToBeamEllipse(mod, hit);
 
81
  SetMean(PhaseSpaceVector(&hit));
 
82
  SetCovariances(mat);
 
83
}
 
84
 
 
85
 
 
86
HepSymMatrix CovarianceMatrix::GetCanonicalCovariances() const
 
87
{
 
88
        double b = _mean.Bz();
 
89
        double e = _mean.Ez();
 
90
        return _norm*TransformToCanonical(_covNorm, b, e, 0.);
 
91
}
 
92
 
 
93
HepSymMatrix CovarianceMatrix::GetRawNormCanCovariances(PhaseSpaceVector reference) const
 
94
{
 
95
        HepSymMatrix out = GetNormCanCovariances();
 
96
        HepVector centre = _mean.getCanonicalSixVector() - reference.getCanonicalSixVector(); 
 
97
        //Loop over upper diagonal
 
98
        for(int i=0; i<6; i++)
 
99
                for(int j=i; j<6; j++)
 
100
                        out[i][j] += centre[i]*centre[j]/_norm;
 
101
        return out;
 
102
}
 
103
 
 
104
 
 
105
HepSymMatrix CovarianceMatrix::GetNormCanCovariances() const
 
106
{
 
107
        double b = _mean.Bz();
 
108
        double e = _mean.Ez();
 
109
        return TransformToCanonical(_covNorm, b, e, 0.);
 
110
}
 
111
 
 
112
//use units where the covariances are ~1 to avoid double point precision errors
 
113
double CovarianceMatrix::GetSixDEmit() const
 
114
{
 
115
        return pow(GetKineticCovariances().determinant()*c_light, 1./2.) / _mean.m()/ _mean.m()/ _mean.m();
 
116
}
 
117
 
 
118
double CovarianceMatrix::GetTwoDEmit(int axis) const
 
119
{
 
120
        double determinant = GetNormKinCovariances().sub(axis*2+1,axis*2+2).determinant();
 
121
        double emittance   = _norm*sqrt(determinant)/_mean.m();
 
122
        return emittance;       
 
123
}
 
124
 
 
125
double CovarianceMatrix::GetTwoDBeta(int axis) const
 
126
{
 
127
        double p  = _mean.P();
 
128
        double m  = _mean.m();
 
129
        double em = GetTwoDEmit(axis);
 
130
        if(em<=0) return -1;
 
131
        return _norm*_covNorm[axis*2][axis*2]*p/(m*em);
 
132
}
 
133
 
 
134
double CovarianceMatrix::GetTwoDAlpha(int axis) const
 
135
{
 
136
        double m  = _mean.m();
 
137
        double em = GetTwoDEmit(axis);
 
138
        if(em<=0) return -1;
 
139
        return -_norm*_covNorm[axis*2][axis*2+1] / (m*em);
 
140
}
 
141
 
 
142
double CovarianceMatrix::GetTwoDGamma(int axis) const
 
143
{
 
144
        double p  = _mean.P();
 
145
        double m  = _mean.m();
 
146
        double em = GetTwoDEmit(axis);
 
147
        if(em<=0) return -1;
 
148
        return _norm*_covNorm[axis*2+1][axis*2+1] / (m*p*em);
 
149
}
 
150
 
 
151
double CovarianceMatrix::GetBetaTrans() const
 
152
{
 
153
        double em = GetEmitTrans();
 
154
        double p  = _mean.P();
 
155
        double m  = _mean.m();
 
156
        if(em<=0) return -1;
 
157
        return _norm*(_covNorm(3,3)+_covNorm(5,5))*p/(2*m*em);
 
158
}
 
159
 
 
160
double CovarianceMatrix::GetAlphaTrans() const
 
161
{
 
162
        double em = GetEmitTrans();
 
163
        double m  = _mean.m();
 
164
        if(em<=0) return -1;
 
165
        return -_norm*(_covNorm(3,4)+_covNorm(5,6))/(2*m*em);
 
166
}
 
167
 
 
168
double CovarianceMatrix::GetGammaTrans() const
 
169
{
 
170
        double em = GetEmitTrans();
 
171
        double p  = _mean.P();
 
172
        double m  = _mean.m();
 
173
        return _norm*(_covNorm(3,3)+_covNorm(5,5))/(2*m*p*em);
 
174
}
 
175
 
 
176
double CovarianceMatrix::GetEmitTrans() const
 
177
{
 
178
        HepSymMatrix localCov = GetNormCanCovariances().sub(3,6);
 
179
        double determinant = localCov.determinant();
 
180
        if(!(determinant>0)) return -1;
 
181
        double em = sqrt(sqrt(determinant))*_norm/_mean.m();
 
182
        return em;
 
183
}
 
184
 
 
185
double CovarianceMatrix::GetLKin() const
 
186
{
 
187
        return _norm*(_covNorm(3,6) - _covNorm(4,5));
 
188
}
 
189
 
 
190
double CovarianceMatrix::GetLCan() const
 
191
{
 
192
/*
 
193
        double m  = _mean.m();
 
194
        double p  = _mean.P();
 
195
        double em = GetEmitTrans();
 
196
        double b  = GetBetaTrans();
 
197
        double bz = _mean.Bz();
 
198
        double k  = GetKappa(bz,p);
 
199
*/
 
200
        return -_norm*( (_covNorm[4][3]-_covNorm[4][4]*_mean.Bz()*_mean.q()*c_light/2.) 
 
201
                       -(_covNorm[2][5]+_covNorm[2][2]*_mean.Bz()*_mean.q()*c_light/2.) );
 
202
}
 
203
 
 
204
double CovarianceMatrix::GetLTwiddle() const
 
205
{
 
206
        double em = GetEmitTrans();
 
207
        double m  = _mean.m();
 
208
        return GetLCan()/(m*em);
 
209
}
 
210
 
 
211
void CovarianceMatrix::SetCovariances(HepSymMatrix covariances, double determinant)
 
212
{
 
213
        if(determinant < 0) 
 
214
        {
 
215
                _norm    = pow(covariances.determinant(),1./6.);
 
216
                if(_norm>1e-9) _covNorm = covariances/_norm;
 
217
                else        {_covNorm = HepSymMatrix(6,0); _norm = 1;}
 
218
        }
 
219
        else 
 
220
        {
 
221
                _norm    = determinant;
 
222
                _covNorm = covariances;
 
223
        }
 
224
}
 
225
 
 
226
void CovarianceMatrix::SetCovariances(double emittance_t, double beta_t, double alpha_t, double Ltwiddle_t,
 
227
                                      double emittance_l, double beta_l, double alpha_l)
 
228
{
 
229
        double m = _mean.m();
 
230
        double p = _mean.P();
 
231
        double k = GetKappa(_mean.Bz(), p);
 
232
        double gamma_t = (1+alpha_t*alpha_t+(beta_t*k - Ltwiddle_t)*(beta_t*k-Ltwiddle_t))/beta_t;
 
233
        double gamma_l = (1+alpha_l*alpha_l)/beta_l;
 
234
        double sigmaxx   =  emittance_t*m*beta_t/p;
 
235
        double sigmaxpx  = -emittance_t*m*alpha_t;
 
236
        double sigmapxpx =  emittance_t*m*p*gamma_t;
 
237
        double sigmaxpy  = -emittance_t*m*(beta_t*k-Ltwiddle_t)*_mean.q();
 
238
 
 
239
        _covNorm[0][0] =  emittance_l*m*beta_l/p;
 
240
        _covNorm[1][1] =  emittance_l*m*gamma_l*p;
 
241
        _covNorm[0][1] = -emittance_l*m*alpha_l;
 
242
        _covNorm[2][2] =  _covNorm[4][4] = sigmaxx;
 
243
        _covNorm[3][3] =  _covNorm[5][5] = sigmapxpx;
 
244
        _covNorm[2][3] =  _covNorm[4][5] = sigmaxpx;
 
245
        _covNorm[3][4] = -sigmaxpy;
 
246
        _covNorm[2][5] =  sigmaxpy;
 
247
 
 
248
        _norm          = pow(_covNorm.determinant(), 1./6.);
 
249
        _covNorm      /= _norm;
 
250
}
 
251
 
 
252
void CovarianceMatrix::SetCovariances(double emittance_x, double beta_x, double alpha_x,
 
253
                                      double emittance_y, double beta_y, double alpha_y,
 
254
                                      double emittance_l, double beta_l, double alpha_l)
 
255
{
 
256
        double m = _mean.m();
 
257
        double p = _mean.P();
 
258
        double gamma_x = (1+alpha_x*alpha_x)/beta_x;
 
259
        double gamma_y = (1+alpha_y*alpha_y)/beta_y;
 
260
        double gamma_l = (1+alpha_l*alpha_l)/beta_l;
 
261
        _covNorm[0][0] =  emittance_l*m*beta_l/p;
 
262
        _covNorm[1][1] =  emittance_l*m*gamma_l*p;
 
263
        _covNorm[0][1] = -emittance_l*m*alpha_l;
 
264
        _covNorm[2][2] =  emittance_x*m*beta_x/p;
 
265
        _covNorm[3][3] =  emittance_x*m*p*gamma_x;
 
266
        _covNorm[2][3] = -emittance_x*m*alpha_x;
 
267
        _covNorm[4][4] =  emittance_y*m*beta_y/p;
 
268
        _covNorm[5][5] =  emittance_y*m*p*gamma_y;
 
269
        _covNorm[4][5] = -emittance_y*m*alpha_y;
 
270
 
 
271
        _norm          = pow(_covNorm.determinant(), 1./6.);
 
272
        _covNorm      /= _norm;
 
273
}
 
274
 
 
275
void CovarianceMatrix::SetDispersions(double dispersion_x, double dispersion_y)
 
276
{
 
277
    _covNorm[1][2] =  dispersion_x*_covNorm[1][1]/_mean.E();
 
278
    _covNorm[1][4] =  dispersion_y*_covNorm[1][1]/_mean.E();
 
279
}
 
280
 
 
281
void CovarianceMatrix::SetDispersionsPrime(double disp_prime_x, double disp_prime_y)
 
282
{
 
283
    _covNorm[1][3] =  disp_prime_x*_covNorm[1][1]/_mean.E();
 
284
    _covNorm[1][5] =  disp_prime_y*_covNorm[1][1]/_mean.E();
 
285
}
 
286
 
 
287
 
 
288
void CovarianceMatrix::SetRawNormCanCovariances(HepSymMatrix rawCov, double norm, PhaseSpaceVector reference)
 
289
{
 
290
        HepVector centre = _mean.getCanonicalSixVector() - reference.getCanonicalSixVector(); 
 
291
        HepSymMatrix out = rawCov;
 
292
        //Loop over upper diagonal
 
293
        for(int i=0; i<6; i++)
 
294
                for(int j=i; j<6; j++)
 
295
                        out[i][j] -= centre[i]*centre[j]/norm;
 
296
        _covNorm         = TransformToKinetic(out, reference.Bz(), reference.Ez(),0); //BUG what about quads?
 
297
        _norm            = norm;
 
298
}
 
299
 
 
300
HepSymMatrix CovarianceMatrix::TransformToCanonical(HepSymMatrix kin, double Bz, double E, double dBdx) const
 
301
{
 
302
        HepSymMatrix can = kin;
 
303
        double b = 2*GetB0(Bz);
 
304
 
 
305
        can[3][2] = kin[3][2]-kin[4][2]*b/2;
 
306
        can[3][3] = kin[3][3]+kin[4][4]*b*b/4-kin[4][3]*b;
 
307
        can[4][3] = kin[4][3]-kin[4][4]*b/2;
 
308
        can[5][2] = kin[5][2]+kin[2][2]*b/2;
 
309
        can[5][3] = kin[5][3]-kin[4][2]*b*b/4+kin[3][2]*b/2-kin[5][4]*b/2;
 
310
        can[5][4] = kin[5][4]+kin[4][2]*b/2;
 
311
        can[5][5] = kin[5][5]+kin[2][2]*b*b/4+kin[5][2]*b;
 
312
 
 
313
        return can; 
 
314
}
 
315
 
 
316
HepSymMatrix CovarianceMatrix::TransformToKinetic(HepSymMatrix can, double Bz, double E, double dBdx) const
 
317
{
 
318
        HepSymMatrix kin = can;
 
319
        double b = 2*GetB0(Bz);
 
320
 
 
321
        kin[3][2] = can[3][2]+can[4][2]*b/2;
 
322
        kin[3][3] = can[3][3]+can[4][4]*b*b/4+can[4][3]*b;
 
323
        kin[4][3] = can[4][3]+can[4][4]*b/2;
 
324
        kin[5][2] = can[5][2]-can[2][2]*b/2;
 
325
        kin[5][3] = can[5][3]-can[4][2]*b*b/4+can[5][4]*b/2-can[3][2]*b/2;
 
326
        kin[5][4] = can[5][4]-can[4][2]*b/2;
 
327
        kin[5][5] = can[5][5]+can[2][2]*b*b/4-can[5][2]*b;
 
328
 
 
329
        return kin;
 
330
}
 
331
 
 
332
AnalysisPlaneBank CovarianceMatrix::GetAnalysisPlaneBank() const
 
333
{
 
334
        AnalysisPlaneBank out;
 
335
        out.CovarianceMatrix = GetKineticCovariances();
 
336
        out.planeNumber      = 0; 
 
337
        out.planeType        = 7;
 
338
        out.weight           = _weight; 
 
339
        out.z                = _mean.z();
 
340
        out.t                = _mean.t();
 
341
        out.pz               = _mean.Pz();
 
342
        out.E                = _mean.E();
 
343
        out.beta_p           = GetBetaTrans();
 
344
        out.l_can            = GetLCan();
 
345
        out.em4dXY           = GetEmitTrans();
 
346
        out.em6dTXY          = GetSixDEmit();
 
347
 
 
348
        for(int i=0; i<3; i++) out.beta[i] = GetTwoDBeta(i);
 
349
        for(int i=0; i<3; i++) out.em2d[i] = GetTwoDEmit(i);
 
350
        for(int i=0; i<6; i++) out.sums[i] = _mean.getSixVector()[i]; 
 
351
        out.amplitudePzCovariance = 0;
 
352
 
 
353
        return out;
 
354
}
 
355
 
 
356
std::ostream& operator<<(std::ostream& out, CovarianceMatrix cov) 
 
357
{
 
358
        cov.Print(out); 
 
359
        return out;
 
360
}
 
361
 
 
362
double  GetTwoDAmp(int axis, PhaseSpaceVector event, CovarianceMatrix Covariance)
 
363
{
 
364
        int ierr=0;
 
365
        CLHEP::HepSymMatrix matrix = Covariance.GetKineticCovariances().sub(axis+1, axis+2).inverse(ierr);
 
366
        CLHEP::HepVector    mean   = Covariance.GetMean().getSixVector();
 
367
        CLHEP::HepVector    vector = (event.getSixVector() - mean).sub(axis+1, axis+2);
 
368
        double              em2D   = Covariance.GetTwoDEmit(axis);
 
369
        return em2D*(vector.T()*matrix*vector)(1,1);
 
370
}
 
371
 
 
372
double  GetAmpTrans(PhaseSpaceVector event, CovarianceMatrix Covariance)
 
373
{
 
374
        int ierr=0;
 
375
        CLHEP::HepSymMatrix matrix = Covariance.GetKineticCovariances().sub(3, 6).inverse(ierr);
 
376
        CLHEP::HepVector    vector = (event.getSixVector()-Covariance.GetMean().getSixVector()).sub(3,6);
 
377
        double              em4D   = Covariance.GetEmitTrans();
 
378
        return em4D*(vector.T()*matrix*vector)(1,1);
 
379
}
 
380
 
 
381
double  GetSixDAmp(PhaseSpaceVector event, CovarianceMatrix Covariance)
 
382
{
 
383
        int ierr=0;
 
384
        CLHEP::HepSymMatrix matrix = Covariance.GetKineticCovariances().inverse(ierr);
 
385
        CLHEP::HepVector    vector = event.getSixVector()-Covariance.GetMean().getSixVector();
 
386
        double              em6D   = Covariance.GetSixDEmit();
 
387
        return em6D*(vector.T()*matrix*vector)(1,1);
 
388
 
 
389
}
 
390
 
 
391
double  GetTwoDAmp(int axis, PhaseSpaceVector event, PhaseSpaceVector mean, HepSymMatrix inverseCovariance, double emittance)
 
392
{
 
393
        CLHEP::HepVector    vector = (event.getSixVector()-mean.getSixVector()).sub(axis+1, axis+2);
 
394
        return emittance*(vector.T()*inverseCovariance*vector)(1,1);
 
395
}
 
396
 
 
397
double  GetAmpTrans(PhaseSpaceVector event, PhaseSpaceVector mean, HepSymMatrix inverseCovariance, double emittance)
 
398
{
 
399
        CLHEP::HepVector    vector = (event.getSixVector()-mean.getSixVector()).sub(3, 6);
 
400
        return emittance*(vector.T()*inverseCovariance*vector)(1,1);
 
401
}
 
402
 
 
403
double  GetSixDAmp(PhaseSpaceVector event, PhaseSpaceVector mean, HepSymMatrix inverseCovariance, double emittance)
 
404
{
 
405
        CLHEP::HepVector    vector = event.getSixVector()-mean.getSixVector();
 
406
        return emittance*(vector.T()*inverseCovariance*vector)(1,1);
 
407
}
 
408
 
 
409
CovarianceMatrix CovarianceMatrix::Interpolate(std::vector<CovarianceMatrix> cov, std::string variableName, double variable)
 
410
{
 
411
  if(cov.size() < 1) throw(Squeal(Squeal::recoverable, "Interpolating CovarianceMatrix array with length 0", "CovarianceMatrix::interpolate"));
 
412
        int    point = -1;
 
413
        int    i     = 0;
 
414
        double delta = 0;
 
415
        std::vector<PhaseSpaceVector> mean;
 
416
        for(unsigned int j=0; j<cov.size(); j++)
 
417
        {
 
418
                mean.push_back(cov[j].GetMean());
 
419
        }
 
420
 
 
421
        while(i<(int)mean.size())
 
422
        {
 
423
                if(variable < mean[i](variableName))
 
424
                {
 
425
                        point = i;
 
426
                        if(i>0) delta = ( variable - mean[i-1](variableName) )/(mean[i](variableName) - mean[i-1](variableName));
 
427
                        i     = mean.size(); //break
 
428
                }
 
429
                else    i++;
 
430
        }
 
431
        if(point == -1)  return cov.back(); //variable > all of psv
 
432
        if(point == 0)   return cov[0];     //variable < all of psv
 
433
 
 
434
        HepSymMatrix covMatrix = (delta*cov[point].GetKineticCovariances()
 
435
                               + (1-delta)*cov[point-1].GetKineticCovariances());
 
436
        HepVector    sixVector = delta*(mean[point].getSixVector()
 
437
                               - mean[point-1].getSixVector())+mean[point-1].getSixVector();
 
438
 
 
439
        return           CovarianceMatrix(covMatrix, PhaseSpaceVector(sixVector, mean[0].m()));
 
440
}
 
441
 
 
442
bool CovarianceMatrix::IsPositiveDefinite(HepSymMatrix mat)
 
443
{
 
444
        bool posDef = true; 
 
445
        for(int i=0; i<mat.num_row(); i++) posDef = (mat.sub(1,i+1).determinant()>0 && posDef); //sylvester criterion
 
446
        return posDef;
 
447
}
 
448
 
 
449
HepMatrix CovarianceMatrix::Rotation(double angle)
 
450
{
 
451
        HepMatrix rotation(6,6,1);
 
452
        double fcos = cos(angle);
 
453
        double fsin = sin(angle);
 
454
 
 
455
        rotation      *=  fcos;
 
456
        rotation[0][0] =  1.;
 
457
        rotation[1][1] =  1.;
 
458
        rotation[4][2] = -fsin;
 
459
        rotation[5][3] = -fsin;
 
460
        rotation[2][4] =  fsin;
 
461
        rotation[3][5] =  fsin;
 
462
        return rotation;
 
463
}
 
464
 
 
465
CovarianceMatrix CovarianceMatrix::Rotate(double angle) const
 
466
{
 
467
        HepMatrix        rotation    = Rotation(angle);
 
468
        PhaseSpaceVector rotatedMean = GetMean().Rotate(angle);
 
469
        HepSymMatrix     rotatedCovs = GetKineticCovariances().similarity(rotation);
 
470
        return CovarianceMatrix(rotatedCovs, rotatedMean);
 
471
}
 
472
 
 
473
double CovarianceMatrix::SingleParticleVariable(PhaseSpaceVector psv, std::string variable)
 
474
{
 
475
        std::vector<std::string> list = psvVariables();
 
476
        int covStart = PhaseSpaceVector::listOfVariables().size();
 
477
        int var = -1;
 
478
        for(unsigned int i=0; i<list.size(); i++)
 
479
                if(LowerCase(variable) == LowerCase(list[i])) var = i;
 
480
        switch(var - covStart)
 
481
        {
 
482
                case 0: case 1: case 2:
 
483
                        return GetTwoDAmp(var - covStart, psv, *this);  
 
484
                case 3:
 
485
                        return GetAmpTrans(psv, *this);  
 
486
                case 4:
 
487
                        return GetSixDAmp(psv, *this);  
 
488
                default:
 
489
                        return psv(variable);
 
490
        }
 
491
 
 
492
}
 
493
 
 
494
double CovarianceMatrix::BunchVariable(std::string variable) const
 
495
{
 
496
        std::vector<std::string> list = bunchVariables();
 
497
        int var = -1;
 
498
        for(unsigned int i=0; i<list.size(); i++)
 
499
                if(LowerCase(variable) == LowerCase(list[i])) var = i;
 
500
        switch(var)
 
501
        {
 
502
                case 0: return GetWeight();
 
503
                case 1: return GetSixDEmit();
 
504
                case 2: return GetEmitTrans();
 
505
                case 3: case 4: case 5: return GetTwoDEmit(var-3);
 
506
                case 6: return GetBetaTrans();
 
507
                case 7: case 8: case 9: return GetTwoDBeta(var-7);
 
508
                case 10: return GetAlphaTrans();
 
509
                case 11: case 12: case 13: return GetTwoDAlpha(var-11);
 
510
                case 14: return GetGammaTrans();
 
511
                case 15: case 16: case 17: return GetTwoDGamma(var-15);
 
512
                case 18: case 19: return GetDispersion(var-17);
 
513
                case 20: return GetLTwiddle(); 
 
514
                case 21: return GetLCan(); 
 
515
                case 22: return GetLKin();
 
516
                default:
 
517
                        throw(Squeal(Squeal::recoverable, "Bunch variable not recognised", "CovarianceMatrix::BunchVariable(std::string)"));
 
518
        }
 
519
}
 
520
 
 
521
void CovarianceMatrix::SetFullVariable(std::string variable, double value)
 
522
{
 
523
        std::vector<std::string> list    = variableTypes();
 
524
        std::vector<std::string> psvList = listOfMyPhaseSpaceVariables();
 
525
        variable = LowerCase(variable);
 
526
        for(unsigned int i=0; i<list.size(); i++) list[i]    = LowerCase(list[i]);
 
527
        for(unsigned int i=0; i<list.size(); i++) psvList[i] = LowerCase(psvList[i]);
 
528
 
 
529
        std::stringstream varStream(variable);
 
530
        std::string       variableType = "";
 
531
        std::string       aux1         = "";
 
532
        std::string       aux2         = "";
 
533
        int               aux1Int      = -1;
 
534
        int               aux2Int      = -1;
 
535
 
 
536
        varStream >> variableType >> aux1 >> aux2;
 
537
        for(unsigned int i=0; i<psvList.size(); i++)
 
538
        {
 
539
                if(aux1 == psvList[i]) aux1Int = i;
 
540
                if(aux2 == psvList[i]) aux2Int = i;
 
541
        }
 
542
 
 
543
        
 
544
        if     (variableType == list[0]) _mean.setConservingMass(aux1, value);
 
545
        else if(variableType == list[1]){HepSymMatrix covMatrix = GetKineticCovariances(); covMatrix[aux1Int][aux2Int] = value; SetCovariances(covMatrix);}
 
546
        else if(variableType == list[4]) SetBunchVariable(aux1, value);
 
547
        else throw(Squeal(Squeal::recoverable, "Did not recognise selection "+varStream.str(), "CovarianceMatrix::SetFullVariable()"));
 
548
        
 
549
 
 
550
}
 
551
 
 
552
void CovarianceMatrix::SetBunchVariable(std::string variable, double value)
 
553
{
 
554
        HepSymMatrix cov = GetKineticCovariances();
 
555
        int varInt = -1;
 
556
        for(unsigned int i=0; i<bunchVariables().size(); i++ )
 
557
                if(LowerCase(variable) == LowerCase(bunchVariables()[i]) ) varInt = i;
 
558
 
 
559
        switch(varInt)
 
560
        {
 
561
                case 0: {SetWeight(value); break;}
 
562
                case 1: {_norm = _mean.m()*value; break;} //scale 6x6 matrix
 
563
                case 2: 
 
564
                {
 
565
                        double emit_4d_old = GetEmitTrans();
 
566
                        for(int i=2; i<6; i++) for(int j=i; j<6; j++) 
 
567
                                cov[i][j] *= value/emit_4d_old; 
 
568
                        break;
 
569
                }
 
570
                case 6:  //beta_4d
 
571
                {
 
572
                        double l_can_old   = GetLCan();
 
573
                        double beta_4d_old = GetBetaTrans(); //mod to keep product sxx*spxpx constant
 
574
                        cov[2][2] *= value/beta_4d_old; 
 
575
                        cov[4][4] *= value/beta_4d_old; 
 
576
                        cov[3][3] /= value/beta_4d_old; 
 
577
                        cov[5][5] /= value/beta_4d_old; 
 
578
                        double sqrtDetM = sqrt(cov.sub(3,6).determinant()); //lcan = xpyc-ypxc; xpyc = xpyk+x2 B0/2
 
579
                        cov[2][5]       =  (l_can_old/2.-cov[2][2]*_mean.Bz()*_mean.q()*c_light/2.);
 
580
                        cov[3][4]       = -(l_can_old/2.-cov[4][4]*_mean.Bz()*_mean.q()*c_light/2.);
 
581
                        double px2      = (sqrtDetM+cov[2][3]*cov[4][5]-cov[3][4]*cov[2][5])/(0.5*cov[2][2]+0.5*cov[4][4]);
 
582
                        cov[3][3]       = cov[5][5] = px2;
 
583
                        break;
 
584
                }
 
585
                case 10: //alpha_4d
 
586
                {
 
587
                        double em        = GetEmitTrans (); //mod to keep product sxx*spxpx constant
 
588
                        double gamma_old = GetGammaTrans();
 
589
                        double gamma_new = (1. + value*value+(GetLKin()/_mean.m()/em)*(GetLKin()/_mean.m()/em))/GetBetaTrans();
 
590
                        cov[2][3]  = -_mean.m()*em*value; //alpha_2d
 
591
                        cov[4][5]  = -_mean.m()*em*value; //alpha_2d
 
592
                        cov[3][3] *= gamma_new/gamma_old; //gamma_2d 
 
593
                        cov[5][5] *= gamma_new/gamma_old; //gamma_2d 
 
594
                        break;
 
595
                }
 
596
                case 14: //gamma_4d
 
597
                {
 
598
                        double gamma_4d_old = GetGammaTrans(); //mod to keep product sxx*spxpx constant
 
599
                        cov[3][3] *= value/gamma_4d_old; 
 
600
                        cov[5][5] *= value/gamma_4d_old; 
 
601
                        cov[2][2] /= value/gamma_4d_old; 
 
602
                        cov[4][4] /= value/gamma_4d_old; 
 
603
                        break;
 
604
                }
 
605
                case 3: case 4: case 5:  //2d em        
 
606
                {
 
607
                        double emit_2d_old = GetTwoDEmit(varInt - 3);
 
608
                        for(int i=varInt*2-6; i<varInt*2-4; i++) for(int j=i; j<varInt*2-4; j++) 
 
609
                                cov[i][j] *= value/emit_2d_old;
 
610
                        break;
 
611
                }
 
612
                case 7: case 8: case 9:  //2d beta
 
613
                {
 
614
                        double beta_2d_old = GetTwoDBeta(varInt - 7); //mod to keep product sxx*spxpx constant
 
615
                        cov[2*varInt-14][2*varInt-14] *= value/beta_2d_old; 
 
616
                        cov[2*varInt-13][2*varInt-13] /= value/beta_2d_old; 
 
617
                        break;
 
618
                }
 
619
                case 11: case 12: case 13: //2d alpha
 
620
                {
 
621
                        double em        = GetTwoDEmit (varInt-11); //mod to keep product sxx*spxpx constant
 
622
                        double gamma_old = GetTwoDGamma(varInt-11);
 
623
                        double gamma_new = (1. + value*value)/GetTwoDBeta(varInt-11);
 
624
                        cov[2*varInt-22][2*varInt-21]  = -_mean.m()*em*value; //alpha_2d
 
625
                        cov[2*varInt-21][2*varInt-21] *= gamma_new/gamma_old; //gamma_2d 
 
626
                        break;
 
627
                }
 
628
                case 15: case 16: case 17: //2d gamma
 
629
                {
 
630
                        double gamma_2d_old = GetTwoDGamma(varInt - 15); //mod to keep product sxx*spxpx constant
 
631
                        cov[2*varInt-29][2*varInt-29] *= value/gamma_2d_old; 
 
632
                        cov[2*varInt-30][2*varInt-30] /= value/gamma_2d_old; 
 
633
                        break;
 
634
                }
 
635
                case 20: //ltwiddle 
 
636
                {
 
637
                        value           = value*GetEmitTrans()*_mean.m()*2.; //then roll into standard lcan
 
638
                        double sqrtDetM = sqrt(cov.sub(3,6).determinant()); //lcan = xpyc-ypxc; xpyc = xpyk+x2 B0/2
 
639
                        cov[2][5]       =  (value/2.-cov[2][2]*_mean.Bz()*_mean.q()*c_light/2.);
 
640
                        cov[3][4]       = -(value/2.-cov[4][4]*_mean.Bz()*_mean.q()*c_light/2.);
 
641
                        double px2      = (sqrtDetM+cov[2][3]*cov[4][5]-cov[3][4]*cov[2][5])/(0.5*cov[2][2]+0.5*cov[4][4]);
 
642
                        cov[3][3]       = cov[5][5] = px2;
 
643
                        break;
 
644
                }
 
645
                case 21:
 
646
                {
 
647
                        double sqrtDetM = sqrt(cov.sub(3,6).determinant()); //lcan = xpyc-ypxc; xpyc = xpyk+x2 B0/2
 
648
                        cov[2][5]       =  (value/2.-cov[2][2]*_mean.Bz()*_mean.q()*c_light/2.);
 
649
                        cov[3][4]       = -(value/2.-cov[4][4]*_mean.Bz()*_mean.q()*c_light/2.);
 
650
                        double px2      = (sqrtDetM+cov[2][3]*cov[4][5]-cov[3][4]*cov[2][5])/(0.5*cov[2][2]+0.5*cov[4][4]);
 
651
                        cov[3][3]       = cov[5][5] = px2;
 
652
                        break;
 
653
                } //LKin()+ 2*m*em*b*k/1e3;
 
654
                case 22: //lkin
 
655
                {
 
656
                        double sqrtDetM = sqrt(cov.sub(3,6).determinant());
 
657
                        cov[2][5]       =  value/2.;
 
658
                        cov[3][4]       = -value/2.;
 
659
                        double px2      = (sqrtDetM+cov[2][3]*cov[4][5]-cov[3][4]*cov[2][5])/(0.5*cov[2][2]+0.5*cov[4][4]);
 
660
                        cov[3][3]       = cov[5][5] = px2;
 
661
                        break;
 
662
                }
 
663
                default: throw(Squeal(Squeal::recoverable, "Bunch variable "+variable+" not recognised", "CovarianceMatrix::SetBunchVariable(string, double)"));
 
664
        }
 
665
        SetCovariances(cov);
 
666
}// return _norm*(_covNorm(3,6) - _covNorm(4,5)); 
 
667
/*
 
668
const std::string CovarianceMatrix::_bunchVars[21] = {"weight", "emit_6d", 
 
669
                                                     "emit_4d",  "emit_t",  "emit_x",  "emit_y",
 
670
                                                     "beta_4d",  "beta_t",  "beta_x",  "beta_y",
 
671
                                                     "alpha_4d", "alpha_t", "alpha_x", "alpha_y",
 
672
                                                     "gamma_4d", "gamma_t", "gamma_x", "gamma_y",
 
673
                                                     "ltwiddle", "lcan", "lkin"};
 
674
 
 
675
}
 
676
*/
 
677
std::vector<std::string> CovarianceMatrix::psvVariables() 
 
678
{
 
679
        std::vector<std::string> bunchList(_psvVars,   _psvVars+5);
 
680
        std::vector<std::string> psvList   = PhaseSpaceVector::listOfVariables();
 
681
        bunchList.insert(bunchList.begin(), psvList.begin(), psvList.end());
 
682
        return bunchList;
 
683
}
 
684
 
 
685
double CovarianceMatrix::FullVariable(std::string variable) const
 
686
{
 
687
        std::vector<std::string> list    = variableTypes();
 
688
        std::vector<std::string> psvList = listOfMyPhaseSpaceVariables();
 
689
        variable = LowerCase(variable);
 
690
        for(unsigned int i=0; i<list.size(); i++) list[i]    = LowerCase(list[i]);
 
691
        for(unsigned int i=0; i<list.size(); i++) psvList[i] = LowerCase(psvList[i]);
 
692
        std::stringstream varStream(variable);
 
693
        std::string       variableType = "";
 
694
        std::string       aux1         = "";
 
695
        std::string       aux2         = "";
 
696
        int               aux1Int      = -1;
 
697
        int               aux2Int      = -1;
 
698
 
 
699
        varStream >> variableType >> aux1 >> aux2;
 
700
        for(unsigned int i=0; i<psvList.size(); i++)
 
701
        {
 
702
                if(aux1 == psvList[i]) aux1Int = i;
 
703
                if(aux2 == psvList[i]) aux2Int = i;
 
704
        }
 
705
 
 
706
        if(variableType == list[0])      return GetMean()(aux1);
 
707
        else if(variableType == list[1]) return GetKineticCovariances()[aux1Int][aux2Int];
 
708
        else if(variableType == list[2]) return sqrt( GetKineticCovariances()[aux1Int][aux1Int] );
 
709
        else if(variableType == list[3]) return GetKineticCovariances()[aux1Int][aux2Int] / sqrt( GetKineticCovariances()[aux1Int][aux1Int] ) 
 
710
                                                                                          / sqrt( GetKineticCovariances()[aux2Int][aux2Int] );
 
711
        else if(variableType == list[4]) return BunchVariable(aux1);
 
712
        else throw(Squeal(Squeal::recoverable, "Did not recognise selection "+varStream.str(), "CovarianceMatrix::FullVariable()"));
 
713
}
 
714
 
 
715
bool CovarianceMatrix::isBad()
 
716
{
 
717
        if(_norm<0 || _mean!=_mean || _weight<0 || _norm!=_norm || _weight!=_weight) 
 
718
                return true;
 
719
        for(int i=1; i<=6; i++)
 
720
                for(int j=i; j<=6; j++)
 
721
                        if( _covNorm.fast(j,i)!=_covNorm.fast(j,i) ) return true;
 
722
        return false;
 
723
}
 
724
 
 
725
std::vector<std::string> CovarianceMatrix::listOfAuxiliaryVariables1(std::string variable)
 
726
{
 
727
        std::vector<std::string> list       = variableTypes();
 
728
        for(unsigned int i=0; i<list.size(); i++)
 
729
                if     (LowerCase(list[i]) == LowerCase(variable) && i!=4) return PhaseSpaceVector::listOfVariables();
 
730
                else if(LowerCase(list[i]) == LowerCase(variable))         return bunchVariables();
 
731
 
 
732
        throw(Squeal(Squeal::recoverable, "Unrecognised covariance variable type "+variable, "CovarianceMatrix::listOfAuxiliaryVariables1(std::string)"));
 
733
}
 
734
 
 
735
std::vector<std::string> CovarianceMatrix::listOfAuxiliaryVariables2(std::string variable)
 
736
{
 
737
        std::vector<std::string> list       = variableTypes();
 
738
        for(unsigned int i=0; i<list.size(); i++)
 
739
                if(LowerCase(list[i]) == LowerCase(variable) && (i==1 || i==3) ) return PhaseSpaceVector::listOfVariables();
 
740
                else  if(LowerCase(list[i]) == LowerCase(variable))              return std::vector<std::string>();
 
741
        throw(Squeal(Squeal::recoverable, "Unrecognised covariance variable type "+variable, "CovarianceMatrix::listOfAuxiliaryVariables2(std::string)"));
 
742
}
 
743
 
 
744
 
 
745
////////////// MomentHeap /////////////
 
746
MomentHeap::MomentHeap(int maxOrder, HepVector mean, double mass) : m_mass(mass), m_mean(mean), m_vars(6,0)
 
747
{
 
748
        if(maxOrder>2) m_higherMoments = std::vector<Tensor*>(maxOrder-2);
 
749
        if(m_higherMoments.size()>0) m_higherMoments[0] = new Tensor3(6,6,6, 0.);
 
750
        for(unsigned int i=1; i<m_higherMoments.size(); i++) m_higherMoments[i] = new Tensor(6, i+3, 0);
 
751
}
 
752
 
 
753
MomentHeap::MomentHeap(CovarianceMatrix covMatrix, std::vector<Tensor*> higherMoments)
 
754
          : m_mass(covMatrix.GetMean().m()), m_mean(covMatrix.GetMean().getSixVector()), m_vars(covMatrix.GetKineticCovariances()), m_higherMoments(higherMoments)
 
755
{}
 
756
 
 
757
MomentHeap::~MomentHeap()
 
758
{
 
759
//      std::cout << m_higherMoments.size() << " " << << std::endl;
 
760
//      for(unsigned int i=0; i<m_higherMoments.size(); i++) delete m_higherMoments[i];
 
761
}
 
762
 
 
763
 
 
764
const Tensor* MomentHeap::GetTensor(int order) const
 
765
{
 
766
        if(order<=2 || order-2>int(m_higherMoments.size()) )
 
767
                throw(Squeal(Squeal::recoverable, "Could not find moment", "MomentHeap::GetTensor"));
 
768
        return m_higherMoments[order-3];
 
769
}
 
770
 
 
771
void MomentHeap::Add(double value, std::vector<int> place)
 
772
{
 
773
        if(place.size() == 2) 
 
774
                m_vars[place[0]][place[1]] += value;
 
775
        else if(int(place.size()) <= MaxOrder()) m_higherMoments[place.size()-3]->Add(place, value);
 
776
        else throw(Squeal(Squeal::recoverable, "Add rank > largest rank on heap or <= 1", "MomentHeap::Add"));
 
777
}
 
778
 
 
779
double MomentHeap::GetMoment(std::vector<int> position) const
 
780
{
 
781
        if(position.size()>2 && int(position.size())<=MaxOrder())
 
782
        {
 
783
                sort(position.begin(), position.end()); //better to design proper symmetric tensor class
 
784
                return GetTensor(position.size())->Get(position);
 
785
        }
 
786
        else if(position.size() == 2)
 
787
                return m_vars[position[0]][position[1]];
 
788
        else if(position.size() == 1)
 
789
                return m_mean[position[0]];
 
790
        throw(Squeal(Squeal::recoverable, "Index out of range", "MomentHeap::GetMoment"));
 
791
}
 
792
 
 
793
std::ostream& MomentHeap::Print(std::ostream& out) const
 
794
{
 
795
        out << std::scientific << std::setprecision(5) << "MomentHeap order " << MaxOrder() <<  std::endl;
 
796
/*      out << m_mean.T() << "\n ************** "  << std::endl;
 
797
        out << m_vars << "\n ************* " << std::endl;
 
798
        for(unsigned int i=0; i<m_higherMoments.size(); i++)
 
799
                out << *(m_higherMoments[i]) << "\n ************* " << std::endl;*/
 
800
        for(int i=1; i<MaxOrder()+1; i++) 
 
801
        {
 
802
                int kvec_front = 1;
 
803
                for(unsigned int j=PolynomialVector::NumberOfPolynomialCoefficients(6, i); j<PolynomialVector::NumberOfPolynomialCoefficients(6, i+1); j++) 
 
804
                {
 
805
                        std::vector<int> kvec = PolynomialVector::IndexByVector(j, 6);
 
806
                        if(kvec.front() != kvec_front) {std::cout << "\n"; kvec_front = kvec.front();}
 
807
                        for(unsigned int k=0; k<kvec.size()-1; k++) std::cout << kvec[k] << ".";
 
808
                        double mom = GetMoment(kvec);
 
809
                        std::cout << kvec.back() << " * ";
 
810
                        if(mom >=0 ) std::cout << " ";
 
811
                        std::cout << mom << "    ";
 
812
 
 
813
                }
 
814
                std::cout << std::endl;
 
815
        }
 
816
        return out;
 
817
}
 
818
 
 
819
std::ostream& operator<<(std::ostream& out, const MomentHeap& heap)
 
820
{
 
821
        return heap.Print(out);
 
822
}
 
823
 
 
824
PolynomialVector MomentHeap::Weighting(MomentHeap in, MomentHeap target, int order)
 
825
{
 
826
        size_t dimension = 6;
 
827
        size_t size      = PolynomialVector::NumberOfPolynomialCoefficients(dimension, order+1);
 
828
        MVector<double> u(size-1);
 
829
        MMatrix<double> M(size-1, size-1);
 
830
        for(size_t i=1; i<size; i++)
 
831
        {
 
832
                std::vector<int> index1 = PolynomialVector::IndexByVector(i, dimension);
 
833
                u(i)                  = target.GetMoment(index1) - in.GetMoment(index1);
 
834
                for(size_t j=1; j<size; j++)
 
835
                {
 
836
                        std::vector<int> index2 = PolynomialVector::IndexByVector(j, dimension);
 
837
                        std::vector<int> index3 = index2;
 
838
                        index3.insert(index3.begin(), index1.begin(), index1.end());
 
839
                        M(j,i)             = in.GetMoment(index3) - in.GetMoment(index1)*target.GetMoment(index2);
 
840
                }
 
841
        }
 
842
        M.invert();
 
843
        MVector<double> a  = M*u;
 
844
        MVector<double> a2 = MVector<double>(a.num_row()+1, 1.);
 
845
        for(size_t i=0; i<a.num_row(); i++) a2(i+2) = a(i+1);
 
846
        return PolynomialVector(dimension, a2.T() );
 
847
}
 
848