FairRoot/PandaRoot
KFPTrack.cxx
Go to the documentation of this file.
1 #include "KFPTrack.h"
2 
3 #include <cmath>
4 
5 
6  // rotate on alpha in XY plane
7 void KFPTrack::RotateXY( float alpha )
8 {
9  const float cA = cos( alpha );
10  const float sA = sin( alpha );
11 
12 #if 0
13  //float J[6][6] = { { cA, sA, 0, 0, 0, 0 }, // X
14  // { -sA, cA, 0, 0, 0, 0 }, // Y
15  // { 0, 0, 1, 0, 0, 0 }, // Z
16  // { 0, 0, 0, cA, sA, 0 }, // Px
17  // { 0, 0, 0, -sA, cA, 0 }, // Py
18  // { 0, 0, 0, 0, 0, 1 } }; // Pz
19 
20 #if defined(PANDA_STT) || defined(PANDA_FTS)
21  float J[2][2] = { { sA, cA },
22  { cA, -sA } };
23 #else
24  float J[2][2] = { { cA, sA },
25  { -sA, cA } };
26 #endif
27 
28  { // convert x, y TODO optimize
29  const float x = GetX(), y = GetY();
30 
31 #if defined(PANDA_STT) || defined(PANDA_FTS)
32  SetX( x*sA + y*cA );
33  SetY( x*cA - y*sA );
34 #else
35  SetX( x*cA + y*sA );
36  SetY( -x*sA + y*cA );
37 #endif
38 
39  float Cov1[2][2]; // triangular -> symmetric matrix
40  Cov1[0][0] = fC[0];
41  Cov1[0][1] = fC[1];
42  Cov1[1][0] = fC[1];
43  Cov1[1][1] = fC[2];
44 
45  float Cov2[2][2]; // Cov2 = Cov1 * J^t
46  for (int i = 0; i < 2; i++)
47  for (int j = 0; j < 2; j++) {
48  Cov2[i][j] = 0;
49  for (int k = 0; k < 2; k++) {
50  Cov2[i][j] += Cov1[i][k] * J[j][k];
51  }
52  }
53 
54  // Cov1 = J * Cov2
55  for (int i = 0; i < 2; i++)
56  for (int j = 0; j < 2; j++) {
57  Cov1[i][j] = 0;
58  for (int k = 0; k < 2; k++) {
59  Cov1[i][j] += J[i][k] * Cov2[k][j];
60  }
61  }
62 
63  // symmetric matrix -> triangular
64  fC[0] = Cov1[0][0];
65  fC[1] = Cov1[0][1];
66  fC[2] = Cov1[1][1];
67  }
68 
69  { // convert Px, Py TODO optimize
70  const float x = GetPx(), y = GetPy();
71 #if defined(PANDA_STT) || defined(PANDA_FTS)
72  SetPx( -x*sA - y*cA );
73  SetPy( -x*cA + y*sA );
74  float J[2][2] = { { -sA, -cA },
75  { -cA, sA } };
76 #else
77  SetPx( x*cA + y*sA );
78  SetPy( -x*sA + y*cA );
79 #endif
80 
81  float Cov1[2][2]; // triangular -> symmetric matrix
82  Cov1[0][0] = fC[9];
83  Cov1[0][1] = fC[13];
84  Cov1[1][0] = fC[13];
85  Cov1[1][1] = fC[14];
86 
87 
88  float Cov2[2][2]; // Cov2 = Cov1 * J^t
89  for (int i = 0; i < 2; i++)
90  for (int j = 0; j < 2; j++) {
91  Cov2[i][j] = 0;
92  for (int k = 0; k < 2; k++) {
93  Cov2[i][j] += Cov1[i][k] * J[j][k];
94  }
95  }
96 
97  // Cov1 = J * Cov2
98  for (int i = 0; i < 2; i++)
99  for (int j = 0; j < 2; j++) {
100  Cov1[i][j] = 0;
101  for (int k = 0; k < 2; k++) {
102  Cov1[i][j] += J[i][k] * Cov2[k][j];
103  }
104  }
105 
106  // symmetric matrix -> triangular
107 
108  fC[9] = Cov1[0][0];
109  fC[13] = Cov1[0][1];
110  fC[14] = Cov1[1][1];
111  }
112 #if defined(PANDA_STT) || defined(PANDA_FTS)
113  //Change sign of px, py, z
114 // SetPx(-GetPx());
115 // SetPy(-GetPy());
116  SetZ( -GetZ() );
117 // fC[ 3] = -fC[ 3];
118 // fC[ 4] = -fC[ 4];
119 // fC[ 8] = -fC[ 8];
120 // fC[12] = -fC[12];
121 
122  fC[ 3] = -fC[ 3];
123  fC[ 4] = -fC[ 4];
124  fC[ 6] = -fC[ 6];
125  fC[ 7] = -fC[ 7];
126  fC[10] = -fC[10];
127  fC[11] = -fC[11];
128  fC[17] = -fC[17];
129  fC[18] = -fC[18];
130  fC[19] = -fC[19];
131 #endif
132 
133 #else
134 #if defined(PANDA_STT) || defined(PANDA_FTS)
135  //float J[6][6] = { { sA, cA, 0, 0, 0, 0 }, // X
136  // { cA, -sA, 0, 0, 0, 0 }, // Y
137  // { 0, 0, -1, 0, 0, 0 }, // Z
138  // { 0, 0, 0, -sA, -cA, 0 }, // Px
139  // { 0, 0, 0, -cA, sA, 0 }, // Py
140  // { 0, 0, 0, 0, 0, 1 } }; // Pz
141 
142  const float x = GetX(), y = GetY();
143 
144  SetX( x*sA + y*cA );
145  SetY( x*cA - y*sA );
146 
147  const float px = GetPx(), py = GetPy();
148 
149  SetPx( -px*sA - py*cA );
150  SetPy( -px*cA + py*sA );
151 
152  SetZ( -GetZ() );
153 
154  float cov[21];
155  for(int iC=0; iC<21; iC++)
156  cov[iC] = fC[iC];
157 
158  fC[0] = cA*cA* cov[2] + 2*cA*cov[1]*sA + cov[0]*sA*sA;
159  fC[1] = cA*cA*cov[1] + cA *(cov[0] - cov[2])* sA - cov[1]* sA*sA;
160  fC[2] = cA*cA*cov[0] - 2* cA *cov[1] *sA + cov[2]* sA*sA,
161 
162  fC[3] = -cA* cov[4] - cov[3]* sA;
163  fC[4] = -cA* cov[3] + cov[4]* sA;
164  fC[5] = cov[5];
165 
166  fC[6] = -cA*cA* cov[11] - cA* (cov[10] + cov[7])* sA - cov[6]* sA*sA;
167  fC[7] = -cA*cA* cov[10] + cA* (cov[11] - cov[6])* sA + cov[7]* sA*sA;
168  fC[8] = cA* cov[12] + cov[8]* sA;
169  fC[9] = cA*cA* cov[14] + 2* cA* cov[13]* sA + cov[9]* sA*sA;
170 
171  fC[10] = -cA*cA* cov[7] + cA* (cov[11] - cov[6])* sA + cov[10]* sA*sA;
172  fC[11] = -cA*cA* cov[6] + cA* (cov[10] + cov[7])* sA - cov[11]* sA*sA;
173  fC[12] = cA* cov[8] - cov[12]* sA;
174  fC[13] = cA*cA* cov[13] + cA* (-cov[14] + cov[9])* sA - cov[13]* sA*sA;
175  fC[14] = cA*cA* cov[9] - 2 *cA* cov[13]* sA + cov[14]* sA*sA;
176 
177  fC[15] = cA *cov[16] + cov[15]* sA;
178  fC[16] = cA *cov[15] - cov[16]* sA;
179  fC[17] = -cov[17];
180  fC[18] = -cA* cov[19] - cov[18]* sA;
181  fC[19] = -cA * cov[18] + cov[19]* sA;
182  fC[20] = cov[20];
183 #else
184  //float J[6][6] = { { cA, -sA, 0, 0, 0, 0 }, // X
185  // { sA, cA, 0, 0, 0, 0 }, // Y
186  // { 0, 0, 1, 0, 0, 0 }, // Z
187  // { 0, 0, 0, cA, -sA, 0 }, // Px
188  // { 0, 0, 0, sA, cA, 0 }, // Py
189  // { 0, 0, 0, 0, 0, 1 } }; // Pz
190 
191  const float x = GetX(), y = GetY();
192 
193  SetX( x*cA - y*sA );
194  SetY( x*sA + y*cA );
195 
196  const float px = GetPx(), py = GetPy();
197 
198  SetPx( px*cA - py*sA );
199  SetPy( px*sA + py*cA );
200 
201  float cov[21];
202  for(int iC=0; iC<21; iC++)
203  cov[iC] = fC[iC];
204 
205  fC[0] = cA*cA* cov[0] - 2*cA*cov[1]*sA + cov[2]*sA*sA;
206 
207  fC[1] = cA*cA*cov[1] + cA *(cov[0] - cov[2])* sA - cov[1]* sA*sA;
208  fC[2] = cA*cA*cov[2] + 2* cA *cov[1] *sA + cov[0]* sA*sA,
209 
210  fC[3] = cA* cov[3] - cov[4]* sA;
211  fC[4] = cA* cov[4] + cov[3]* sA;
212  fC[5] = cov[5];
213 
214  fC[6] = cA* (cA* cov[6] - cov[10]* sA) - sA* (cA* cov[7] - cov[11]* sA);
215  fC[7] = sA* (cA* cov[6] - cov[10]* sA) + cA* (cA* cov[7] - cov[11]* sA);
216  fC[8] = cA* cov[8] - cov[12]* sA;
217  fC[9] = cA* (cA* cov[9] - cov[13]* sA) - sA *(cA* cov[13] - cov[14]* sA);
218 
219  fC[10] = cA *(cov[10]* cA + cov[6]* sA) - (cov[11]* cA + cov[7] *sA) *sA;
220  fC[11] = cA *(cov[11]* cA + cov[7]* sA) + (cov[10]* cA + cov[6] *sA) *sA;
221  fC[12] = cov[12] *cA + cov[8]* sA;
222  fC[13] = -sA* (cA* cov[14] + cov[13]* sA) + cA* (cA* cov[13] + cov[9]* sA),
223  fC[14] = cA *(cA* cov[14] + cov[13]* sA) + sA* (cA* cov[13] + cov[9]* sA);
224 
225  fC[15] = cA *cov[15] - cov[16]* sA;
226  fC[16] = cA *cov[16] + cov[15]* sA;
227  fC[17] = cov[17];
228  fC[18] = cA* cov[18] - cov[19]* sA;
229  fC[19] = cA * cov[19] + cov[18]* sA;
230  fC[20] = cov[20];
231 #endif
232 #endif
233 
234 } // RotateXY
KFPTrack::GetY
float GetY() const
Definition: KFPTrack.h:45
cos
friend F32vec4 cos(const F32vec4 &a)
Definition: P4_F32vec4.h:112
i
Int_t i
Definition: run_full.C:25
KFPTrack::SetZ
void SetZ(float z)
Definition: KFPTrack.h:92
sin
friend F32vec4 sin(const F32vec4 &a)
Definition: P4_F32vec4.h:111
KFPTrack::GetZ
float GetZ() const
Definition: KFPTrack.h:46
KFPTrack::SetY
void SetY(float y)
Definition: KFPTrack.h:91
KFPTrack::SetX
void SetX(float x)
Definition: KFPTrack.h:90
KFPTrack::SetPx
void SetPx(float px)
Definition: KFPTrack.h:93
KFPTrack::GetX
float GetX() const
Definition: KFPTrack.h:44
KFPTrack::fC
float fC[21]
Definition: KFPTrack.h:115
KFPTrack::GetPx
float GetPx() const
Definition: KFPTrack.h:47
KFPTrack::SetPy
void SetPy(float py)
Definition: KFPTrack.h:94
x
Double_t x
Definition: createRootFscGeometryFile.C:159
y
Double_t y
Definition: createRootFscGeometryFile.C:159
alpha
double alpha
Definition: f_Init.h:9
KFPTrack::GetPy
float GetPy() const
Definition: KFPTrack.h:48
KFPTrack::RotateXY
void RotateXY(float alpha)
Definition: KFPTrack.cxx:7
Generated on Sun Aug 18 2019 03:01:14 for FairRoot/PandaRoot by   doxygen 1.8.8