11 const TMatrixT<double>& sigma)
15 for(
int i=0;i<sigma.GetNrows();++i){
16 fCov[i][i]=sigma[i][0];
36 const TMatrixT<double>& _state,
37 const TMatrixT<double>& sigma)
41 for(
int i=0;i<sigma.GetNrows();++i){
42 fCov[i][i]=sigma[i][0];
48 const TMatrixT<double>& sigma,
54 for(
int i=0;i<sigma.GetNrows();++i){
55 fCov[i][i]=sigma[i][0];
66 TMatrixT<double>& statePred){
71 TVector3
w=u.Cross(v);
75 TVector3 wfrom=ufrom.Cross(vfrom);
81 std::cerr<<
"track paralell to detector plane"<<
std::endl 82 <<
"extrapolation impossible, aborting"<<
std::endl;
86 double t= ((( w * o) - (w * p1)) /
89 double dist=t*dir.Mag();
93 double state0=(p2-o)*u;
94 double state1=(p2-o)*v;
95 double state2=(dir*u)/(dir*w);
96 double state3=(dir*v)/(dir*w);
98 statePred[0].Assign(state0);
99 statePred[1].Assign(state1);
100 statePred[2].Assign(state2);
101 statePred[3].Assign(state3);
105 TMatrixT<double>& statePred,
106 TMatrixT<double>& covPred)
111 TVector3 o=pl.
getO();
112 TVector3 u=pl.
getU();
113 TVector3 v=pl.
getV();
114 TVector3
w=u.Cross(v);
118 TVector3 wfrom=ufrom.Cross(vfrom);
127 TVector3 p1=ofrom+
fState[0][0]*ufrom+
fState[1][0]*vfrom;
138 std::cerr<<
"track paralell to detector plane"<<
std::endl 139 <<
"extrapolation impossible, aborting"<<
std::endl;
143 double t= ((( w * o) - (w * p1)) /
146 double dist=t*dir.Mag();
148 TVector3 p2=p1+t*
dir;
153 double state0=(p2-o)*u;
154 double state1=(p2-o)*v;
155 double state2=(dir*u)/(dir*w);
156 double state3=(dir*v)/(dir*w);
169 TMatrixT<double> jacobian(4,4);
247 double jacobian0 = (ufrom
248 - 1 / (w *
dir) * (w * ufrom) *
fState[2][0] * ufrom
249 - 1 / (w *
dir) * (w * ufrom) *
fState[3][0] * vfrom
250 - 1 / (w *
dir) * (w * ufrom) * wfrom) * u;
252 double jacobian1 = (vfrom
253 - 1 / (w *
dir) * (w * vfrom) *
fState[2][0] * ufrom
254 - 1 / (w *
dir) * (w * vfrom) *
fState[3][0] * vfrom
255 - 1 / (w *
dir) * (w * vfrom) * wfrom)* u;
257 double jacobian2 = (-
pow( (w * dir), -2) * (w * o) *
fState[2][0] * ufrom * (w * ufrom)
258 + 1 / (w *
dir) * (w * o) * ufrom
259 -
pow((w * dir), -2) * (w * o) *
fState[3][0] * vfrom * (w * ufrom)
260 -
pow((w * dir), -2) * (w * o) * wfrom * (w * ufrom)
261 +
pow((w * dir), -2) * (w * p1) *
fState[2][0] * ufrom * (w * ufrom)
262 - 1 / (w *
dir) * (w * p1) * ufrom
263 +
pow((w * dir), -2) * (w * p1) *
fState[3][0] * vfrom * (w * ufrom)
264 +
pow((w * dir), -2) * (w * p1) * wfrom * (w * ufrom))* u;
266 double jacobian3 = (-
pow( ( w * dir), -2) * (w * o) *
fState[2][0] * ufrom * (w * vfrom)
267 -
pow((w * dir), -2) * (w * o) *
fState[3][0] * vfrom * (w * vfrom)
268 + 1 / (w *
dir) * (w * o) * vfrom
269 -
pow((w * dir), -2) * (w * o) * wfrom * (w * vfrom)
270 +
pow((w * dir), -2) * (w * p1) *
fState[2][0] * ufrom * (w * vfrom)
271 +
pow((w * dir), -2) * (w * p1) *
fState[3][0] * vfrom * (w * vfrom)
272 - 1 / (w *
dir) * (w * p1) * vfrom
273 +
pow((w * dir), -2) * (w * p1) * wfrom * (w * vfrom))* u;
275 double jacobian4 = (ufrom
276 - 1 / (w *
dir) * (w * ufrom) *
fState[2][0] * ufrom
277 - 1 / (w *
dir) * (w * ufrom) *
fState[3][0] * vfrom
278 - 1 / (w *
dir) * (w * ufrom) * wfrom)* v;
280 double jacobian5 = (vfrom
281 - 1 / (w *
dir) * (w * vfrom) *
fState[2][0] * ufrom
282 - 1 / (w *
dir) * (w * vfrom) *
fState[3][0] * vfrom
283 - 1 / (w *
dir) * (w * vfrom) * wfrom)* v;
285 double jacobian6 = (-
pow( ( w * dir), -2) * (w * o) *
fState[2][0] * ufrom * (w * ufrom)
286 + 1 / (w *
dir) * (w * o) * ufrom
287 -
pow((w * dir), -2) * (w * o) *
fState[3][0] * vfrom * (w * ufrom)
288 -
pow((w * dir), -2) * (w * o) * wfrom * (w * ufrom)
289 +
pow((w * dir), -2) * (w * p1) *
fState[2][0] * ufrom * (w * ufrom)
290 - 1 / (w *
dir) * (w * p1) * ufrom
291 +
pow((w * dir), -2) * (w * p1) *
fState[3][0] * vfrom * (w * ufrom)
292 +
pow((w * dir), -2) * (w * p1) * wfrom * (w * ufrom))* v;
294 double jacobian7 = (-
pow( ( w * dir), -2) * (w * o) *
fState[2][0] * ufrom * (w * vfrom)
295 -
pow((w * dir), -2) * (w * o) *
fState[3][0] * vfrom * (w * vfrom)
296 + 1 / (w *
dir) * (w * o) * vfrom
297 -
pow((w * dir), -2) * (w * o) * wfrom * (w * vfrom)
298 +
pow((w * dir), -2) * (w * p1) *
fState[2][0] * ufrom * (w * vfrom)
299 +
pow((w * dir), -2) * (w * p1) *
fState[3][0] * vfrom * (w * vfrom)
300 - 1 / (w *
dir) * (w * p1) * vfrom
301 +
pow((w * dir), -2) * (w * p1) * wfrom * (w * vfrom))* v;
302 double jacobian8 = 0;
303 double jacobian9 = 0;
305 double jacobian10 = ((ufrom * u) / (w * dir)
306 - (dir * u) *
pow((w * dir), -2) * (w * ufrom));
308 double jacobian11 = ((vfrom * u) / (w * dir)
309 - (dir * u) *
pow((w * dir), -2) * (w * vfrom));
310 double jacobian12 = 0;
311 double jacobian13 = 0;
313 double jacobian14 = ((ufrom * v) / (w * dir)
314 - (dir * v) *
pow((w * dir), -2) * (w * ufrom));
316 double jacobian15 = ((vfrom * v) / (w * dir)
317 - (dir * v) *
pow((w * dir), -2) * (w * vfrom));
319 jacobian[0][0]=jacobian0; jacobian[0][1]=jacobian1; jacobian[0][2]=jacobian2; jacobian[0][3]=jacobian3;
320 jacobian[1][0]=jacobian4; jacobian[1][1]=jacobian5; jacobian[1][2]=jacobian6; jacobian[1][3]=jacobian7;
321 jacobian[2][0]=jacobian8; jacobian[2][1]=jacobian9; jacobian[2][2]=jacobian10; jacobian[2][3]=jacobian11;
322 jacobian[3][0]=jacobian12; jacobian[3][1]=jacobian13; jacobian[3][2]=jacobian14; jacobian[3][3]=jacobian15;
323 TMatrixT<double> jacobianT(jacobian);
325 covPred=jacobian*
fCov*(jacobianT);
327 statePred[0].Assign(state0);
328 statePred[1].Assign(state1);
329 statePred[2].Assign(state2);
330 statePred[3].Assign(state3);
335 TVector3& dirInPoca){
340 TVector3 wfrom=ufrom.Cross(vfrom);
341 TVector3 pfrom=ofrom +
fState[0][0]*ufrom +
fState[1][0]*vfrom;
346 double t = (dir * pos - dir * pfrom) / (dir * dir);
348 poca=pfrom + t *
dir;
349 dirInPoca=dir.Unit();
353 const TVector3& point2,
356 TVector3& poca_onwire){
361 TVector3 wfrom=ufrom.Cross(vfrom);
363 TVector3 pfrom=ofrom +
fState[0][0]*ufrom +
fState[1][0]*vfrom;
365 TVector3 lineDir=point2-point1;
367 TVector3 normal(dir.y()*lineDir.z()-dir.z()*lineDir.y(),
368 dir.x()*lineDir.z()-dir.z()*lineDir.x(),
369 dir.x()*lineDir.y()-dir.y()*lineDir.x());
371 normal=normal.Unit();
372 TVector3 planeNorm=dir.Cross(normal);
373 double t=(planeNorm*point2-planeNorm*pfrom)/(planeNorm*dir);
375 double t2=(lineDir*poca - lineDir*point1)/(lineDir*lineDir);
376 poca_onwire=point1+lineDir*t2;
381 TMatrixT<double> statePred(
fState);
385 return pl.
getO()+(statePred[0][0]*pl.
getU())+(statePred[1][0]*pl.
getV());
388 TMatrixT<double> statePred(
fState);
392 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.
QTextStream & endl(QTextStream &s)