1 package org.lcsim.mc.fast.tracking;
2
3 import Jama.EigenvalueDecomposition;
4 import Jama.Matrix;
5 import Jama.util.Maths;
6
7 import java.util.Random;
8
9
10
11
12 class SmearTrack {
13
14
15
16
17
18 static DocaTrackParameters smearTrack(double bField, TrackParameters noSmear, Random rand) {
19
20
21 Matrix M = Maths.toJamaMatrix(noSmear.getErrorMatrix());
22 if (M.det() <= 0.) {
23 throw new RuntimeException("Error matrix not positive definite!");
24 }
25
26
27 EigenvalueDecomposition eig = M.eig();
28
29 Matrix T = eig.getV();
30 double[] er = eig.getRealEigenvalues();
31 double[] ei = eig.getImagEigenvalues();
32
33
34 if (T.det() == 0.) {
35 throw new RuntimeException("Non orthogonal basis!");
36 }
37
38
39 for (int i = 0; i < ei.length; i++)
40 if (ei[i] != 0.) {
41 throw new RuntimeException("Imaginary Eigenvalues seen!");
42 }
43
44
45 double[] dev = new double[5];
46 for (int i = 0; i < er.length; i++) {
47 if (er[i] <= 0) {
48 throw new RuntimeException("Non-positive Eigenvalue seen!");
49 }
50 dev[i] = Math.sqrt(er[i]) * rand.nextGaussian();
51 }
52
53 Matrix shift = T.times(new Matrix(dev, 5));
54 Matrix val = new Matrix(noSmear.getTrackParameters(), 5);
55 double[] newval = (val.plus(shift)).getColumnPackedCopy();
56
57
58 if (newval[1] > Math.PI) {
59 newval[1] -= (2. * Math.PI);
60 }
61 if (newval[1] < -Math.PI) {
62 newval[1] += (2. * Math.PI);
63 }
64
65
66 double chi2 = ((shift.transpose()).times((M.inverse()).times(shift))).get(0, 0);
67
68 DocaTrackParameters smeared = new DocaTrackParameters(bField, newval, noSmear.getErrorMatrix(), chi2);
69 if (noSmear instanceof DocaTrackParameters) {
70 smeared.setL0(((DocaTrackParameters) noSmear).getL0());
71 }
72
73 return smeared;
74 }
75 }