10 for (
int i = 0; i < sigma.GetNrows(); ++i) {
11 fCov[i][i] = sigma[i][0];
31 const TMatrixT<double>& _state,
32 const TMatrixT<double>& sigma)
36 for (
int i = 0; i < sigma.GetNrows(); ++i) {
37 fCov[i][i] = sigma[i][0];
43 const TMatrixT<double>& sigma,
49 for (
int i = 0; i < sigma.GetNrows(); ++i) {
50 fCov[i][i] = sigma[i][0];
59 TVector3 o = pl.
getO();
60 TVector3 u = pl.
getU();
61 TVector3 v = pl.
getV();
62 TVector3
w = u.Cross(v);
66 TVector3 wfrom = ufrom.Cross(vfrom);
67 TVector3 p1 = ofrom +
fState[0][0] * ufrom +
fState[1][0] * vfrom;
72 std::cerr <<
"track paralell to detector plane" << std::endl
73 <<
"extrapolation impossible, aborting" << std::endl;
77 double t = (((w * o) - (w * p1)) / (w * dir));
79 double dist = t * dir.Mag();
81 TVector3 p2 = p1 + t *
dir;
83 double state0 = (p2 - o) * u;
84 double state1 = (p2 - o) * v;
85 double state2 = (dir * u) / (dir * w);
86 double state3 = (dir * v) / (dir * w);
88 statePred[0].Assign(state0);
89 statePred[1].Assign(state1);
90 statePred[2].Assign(state2);
91 statePred[3].Assign(state3);
95 TMatrixT<double>& statePred,
96 TMatrixT<double>& covPred)
101 TVector3 o = pl.
getO();
102 TVector3 u = pl.
getU();
103 TVector3 v = pl.
getV();
104 TVector3
w = u.Cross(v);
108 TVector3 wfrom = ufrom.Cross(vfrom);
117 TVector3 p1 = ofrom +
fState[0][0] * ufrom +
fState[1][0] * vfrom;
128 std::cerr <<
"track paralell to detector plane" << std::endl
129 <<
"extrapolation impossible, aborting" << std::endl;
133 double t = (((w * o) - (w * p1)) / (w * dir));
135 double dist = t * dir.Mag();
137 TVector3 p2 = p1 + t *
dir;
142 double state0 = (p2 - o) * u;
143 double state1 = (p2 - o) * v;
144 double state2 = (dir * u) / (dir * w);
145 double state3 = (dir * v) / (dir * w);
158 TMatrixT<double> jacobian(4, 4);
236 (ufrom - 1 / (w *
dir) * (w * ufrom) *
fState[2][0] * ufrom -
237 1 / (w *
dir) * (w * ufrom) *
fState[3][0] * vfrom - 1 / (w *
dir) * (w * ufrom) * wfrom) *
241 (vfrom - 1 / (w *
dir) * (w * vfrom) *
fState[2][0] * ufrom -
242 1 / (w *
dir) * (w * vfrom) *
fState[3][0] * vfrom - 1 / (w *
dir) * (w * vfrom) * wfrom) *
245 double jacobian2 = (-pow((w * dir), -2) * (w * o) *
fState[2][0] * ufrom * (w * ufrom) +
246 1 / (w *
dir) * (w * o) * ufrom -
247 pow((w * dir), -2) * (w * o) *
fState[3][0] * vfrom * (w * ufrom) -
248 pow((w * dir), -2) * (w * o) * wfrom * (w * ufrom) +
249 pow((w * dir), -2) * (w * p1) *
fState[2][0] * ufrom * (w * ufrom) -
250 1 / (w *
dir) * (w * p1) * ufrom +
251 pow((w * dir), -2) * (w * p1) *
fState[3][0] * vfrom * (w * ufrom) +
252 pow((w * dir), -2) * (w * p1) * wfrom * (w * ufrom)) *
256 (-pow((w * dir), -2) * (w * o) *
fState[2][0] * ufrom * (w * vfrom) -
257 pow((w * dir), -2) * (w * o) *
fState[3][0] * vfrom * (w * vfrom) +
258 1 / (w *
dir) * (w * o) * vfrom - pow((w * dir), -2) * (w * o) * wfrom * (w * vfrom) +
259 pow((w * dir), -2) * (w * p1) *
fState[2][0] * ufrom * (w * vfrom) +
260 pow((w * dir), -2) * (w * p1) *
fState[3][0] * vfrom * (w * vfrom) -
261 1 / (w *
dir) * (w * p1) * vfrom + pow((w * dir), -2) * (w * p1) * wfrom * (w * vfrom)) *
265 (ufrom - 1 / (w *
dir) * (w * ufrom) *
fState[2][0] * ufrom -
266 1 / (w *
dir) * (w * ufrom) *
fState[3][0] * vfrom - 1 / (w *
dir) * (w * ufrom) * wfrom) *
270 (vfrom - 1 / (w *
dir) * (w * vfrom) *
fState[2][0] * ufrom -
271 1 / (w *
dir) * (w * vfrom) *
fState[3][0] * vfrom - 1 / (w *
dir) * (w * vfrom) * wfrom) *
274 double jacobian6 = (-pow((w * dir), -2) * (w * o) *
fState[2][0] * ufrom * (w * ufrom) +
275 1 / (w *
dir) * (w * o) * ufrom -
276 pow((w * dir), -2) * (w * o) *
fState[3][0] * vfrom * (w * ufrom) -
277 pow((w * dir), -2) * (w * o) * wfrom * (w * ufrom) +
278 pow((w * dir), -2) * (w * p1) *
fState[2][0] * ufrom * (w * ufrom) -
279 1 / (w *
dir) * (w * p1) * ufrom +
280 pow((w * dir), -2) * (w * p1) *
fState[3][0] * vfrom * (w * ufrom) +
281 pow((w * dir), -2) * (w * p1) * wfrom * (w * ufrom)) *
285 (-pow((w * dir), -2) * (w * o) *
fState[2][0] * ufrom * (w * vfrom) -
286 pow((w * dir), -2) * (w * o) *
fState[3][0] * vfrom * (w * vfrom) +
287 1 / (w *
dir) * (w * o) * vfrom - pow((w * dir), -2) * (w * o) * wfrom * (w * vfrom) +
288 pow((w * dir), -2) * (w * p1) *
fState[2][0] * ufrom * (w * vfrom) +
289 pow((w * dir), -2) * (w * p1) *
fState[3][0] * vfrom * (w * vfrom) -
290 1 / (w *
dir) * (w * p1) * vfrom + pow((w * dir), -2) * (w * p1) * wfrom * (w * vfrom)) *
292 double jacobian8 = 0;
293 double jacobian9 = 0;
295 double jacobian10 = ((ufrom * u) / (w * dir) - (dir * u) * pow((w * dir), -2) * (w * ufrom));
297 double jacobian11 = ((vfrom * u) / (w * dir) - (dir * u) * pow((w * dir), -2) * (w * vfrom));
298 double jacobian12 = 0;
299 double jacobian13 = 0;
301 double jacobian14 = ((ufrom * v) / (w * dir) - (dir * v) * pow((w * dir), -2) * (w * ufrom));
303 double jacobian15 = ((vfrom * v) / (w * dir) - (dir * v) * pow((w * dir), -2) * (w * vfrom));
305 jacobian[0][0] = jacobian0;
306 jacobian[0][1] = jacobian1;
307 jacobian[0][2] = jacobian2;
308 jacobian[0][3] = jacobian3;
309 jacobian[1][0] = jacobian4;
310 jacobian[1][1] = jacobian5;
311 jacobian[1][2] = jacobian6;
312 jacobian[1][3] = jacobian7;
313 jacobian[2][0] = jacobian8;
314 jacobian[2][1] = jacobian9;
315 jacobian[2][2] = jacobian10;
316 jacobian[2][3] = jacobian11;
317 jacobian[3][0] = jacobian12;
318 jacobian[3][1] = jacobian13;
319 jacobian[3][2] = jacobian14;
320 jacobian[3][3] = jacobian15;
321 TMatrixT<double> jacobianT(jacobian);
323 covPred = jacobian *
fCov * (jacobianT);
325 statePred[0].Assign(state0);
326 statePred[1].Assign(state1);
327 statePred[2].Assign(state2);
328 statePred[3].Assign(state3);
337 TVector3 wfrom = ufrom.Cross(vfrom);
338 TVector3 pfrom = ofrom +
fState[0][0] * ufrom +
fState[1][0] * vfrom;
343 double t = (dir * pos - dir * pfrom) / (dir * dir);
345 poca = pfrom + t *
dir;
346 dirInPoca = dir.Unit();
349 const TVector3& point2,
352 TVector3& poca_onwire)
358 TVector3 wfrom = ufrom.Cross(vfrom);
360 TVector3 pfrom = ofrom +
fState[0][0] * ufrom +
fState[1][0] * vfrom;
362 TVector3 lineDir = point2 - point1;
364 TVector3 normal(dir.y() * lineDir.z() - dir.z() * lineDir.y(),
365 dir.x() * lineDir.z() - dir.z() * lineDir.x(),
366 dir.x() * lineDir.y() - dir.y() * lineDir.x());
368 normal = normal.Unit();
369 TVector3 planeNorm = dir.Cross(normal);
370 double t = (planeNorm * point2 - planeNorm * pfrom) / (planeNorm * dir);
371 poca = pfrom + t *
dir;
372 double t2 = (lineDir * poca - lineDir * point1) / (lineDir * lineDir);
373 poca_onwire = point1 + lineDir *
t2;
378 TMatrixT<double> statePred(
fState);
380 return pl.
getO() + (statePred[0][0] * pl.
getU()) + (statePred[1][0] * pl.
getV());
384 TMatrixT<double> statePred(
fState);
386 TVector3 ret = pl.
getNormal() + (statePred[2][0] * pl.
getU()) + (statePred[3][0] * pl.
getV());
virtual void getPosMom(const GFDetPlane &, TVector3 &pos, TVector3 &mom)
unsigned int fDimension
Dimensionality of track representation.
void extrapolateToPoint(const TVector3 &pos, TVector3 &poca, TVector3 &dirInPoca)
This method is to extrapolate the track to point of closest approach to a point in space...
Base Class for genfit track representations. Defines interface for track parameterizations.
TVector3 getNormal() const
TMatrixT< Double_t > fCov
The covariance matrix.
virtual double extrapolate(const GFDetPlane &, TMatrixT< double > &statePred, TMatrixT< double > &covPred)
void setReferencePlane(const GFDetPlane &pl)
constexpr double dist(const TReal *x, const TReal *y, const unsigned int dimension)
void extrapolateToLine(const TVector3 &point1, const TVector3 &point2, TVector3 &poca, TVector3 &dirInPoca, TVector3 &poca_onwire)
This method extrapolates to the point of closest approach to a line.
TMatrixT< Double_t > fState
The vector of track parameters.