LArSoft  v09_90_00
Liquid Argon Software toolkit - https://larsoft.org/
SlTrackRep.cxx
Go to the documentation of this file.
2 #include <cmath>
3 
5 
6 genf::SlTrackRep::SlTrackRep(const TMatrixT<double>& _state, const TMatrixT<double>& sigma)
7  : GFAbsTrackRep(4), _backw(0)
8 {
9  fState = _state;
10  for (int i = 0; i < sigma.GetNrows(); ++i) {
11  fCov[i][i] = sigma[i][0];
12  }
13  setReferencePlane(GFDetPlane(TVector3(0, 0, 0), TVector3(1, 0, 0), TVector3(0, 1, 0)));
14 }
15 
16 genf::SlTrackRep::SlTrackRep(const TVector3& pos, const TVector3& dir) : GFAbsTrackRep(4), _backw(0)
17 {
18  GFDetPlane d(pos, dir);
20  fState[0][0] = 0.;
21  fState[1][0] = 0.;
22  fState[2][0] = 0.;
23  fState[3][0] = 0.;
24  fCov[0][0] = 1.;
25  fCov[1][1] = 1.;
26  fCov[2][2] = 1.;
27  fCov[3][3] = 1.;
28 }
29 
31  const TMatrixT<double>& _state,
32  const TMatrixT<double>& sigma)
33  : GFAbsTrackRep(4), _backw(0)
34 {
35  fState = _state;
36  for (int i = 0; i < sigma.GetNrows(); ++i) {
37  fCov[i][i] = sigma[i][0];
38  }
40 }
41 
42 genf::SlTrackRep::SlTrackRep(const TMatrixT<double>& _state,
43  const TMatrixT<double>& sigma,
44  const double z)
45  : GFAbsTrackRep(4), _backw(0)
46 {
47 
48  fState = _state;
49  for (int i = 0; i < sigma.GetNrows(); ++i) {
50  fCov[i][i] = sigma[i][0];
51  }
52  setReferencePlane(GFDetPlane(TVector3(0, 0, z), TVector3(1, 0, 0), TVector3(0, 1, 0)));
53 }
54 
56 double genf::SlTrackRep::extrapolate(const GFDetPlane& pl, TMatrixT<double>& statePred)
57 {
58  statePred.ResizeTo(fDimension, 1);
59  TVector3 o = pl.getO();
60  TVector3 u = pl.getU();
61  TVector3 v = pl.getV();
62  TVector3 w = u.Cross(v);
63  TVector3 ofrom = fRefPlane.getO();
64  TVector3 ufrom = fRefPlane.getU();
65  TVector3 vfrom = fRefPlane.getV();
66  TVector3 wfrom = ufrom.Cross(vfrom);
67  TVector3 p1 = ofrom + fState[0][0] * ufrom + fState[1][0] * vfrom;
68 
69  TVector3 dir = (fState[2][0] * ufrom + fState[3][0] * vfrom + wfrom); //in global coordinates
70 
71  if (w * dir == 0) {
72  std::cerr << "track paralell to detector plane" << std::endl
73  << "extrapolation impossible, aborting" << std::endl;
74  throw;
75  }
76 
77  double t = (((w * o) - (w * p1)) / (w * dir));
78 
79  double dist = t * dir.Mag();
80 
81  TVector3 p2 = p1 + t * dir;
82 
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);
87 
88  statePred[0].Assign(state0);
89  statePred[1].Assign(state1);
90  statePred[2].Assign(state2);
91  statePred[3].Assign(state3);
92  return dist;
93 }
95  TMatrixT<double>& statePred,
96  TMatrixT<double>& covPred)
97 {
98  //std::cout<<std::endl<<std::endl<<"extrapolate to plane with state and cov"<<std::endl<<std::endl;
99  statePred.ResizeTo(fDimension, 1);
100  covPred.ResizeTo(fDimension, fDimension);
101  TVector3 o = pl.getO();
102  TVector3 u = pl.getU();
103  TVector3 v = pl.getV();
104  TVector3 w = u.Cross(v);
105  TVector3 ofrom = fRefPlane.getO();
106  TVector3 ufrom = fRefPlane.getU();
107  TVector3 vfrom = fRefPlane.getV();
108  TVector3 wfrom = ufrom.Cross(vfrom);
109  /*
110  std::cout<<"ufrom ";
111  ufrom.Print();
112  std::cout<<"vfrom ";
113  vfrom.Print();
114  std::cout<<"wfrom ";
115  wfrom.Print();
116  */
117  TVector3 p1 = ofrom + fState[0][0] * ufrom + fState[1][0] * vfrom;
118  /*
119  std::cout<<"p1 ";
120  p1.Print();
121  */
122  TVector3 dir = (fState[2][0] * ufrom + fState[3][0] * vfrom + wfrom); //in global coordinates
123  /*
124  std::cout<<"dir ";
125  dir.Print();
126  */
127  if (w * dir == 0) {
128  std::cerr << "track paralell to detector plane" << std::endl
129  << "extrapolation impossible, aborting" << std::endl;
130  throw;
131  }
132 
133  double t = (((w * o) - (w * p1)) / (w * dir));
134 
135  double dist = t * dir.Mag();
136 
137  TVector3 p2 = p1 + t * dir;
138  /*
139  std::cout<<"xtrapolate point p2 ";
140  p2.Print();
141  */
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);
146  /*
147  std::cout<<"start state:"<<std::endl;
148  std::cout<<fState[0][0]<<std::endl
149  <<fState[1][0]<<std::endl
150  <<fState[2][0]<<std::endl
151  <<fState[3][0]<<std::endl;
152  std::cout<<"end state:"<<std::endl;
153  std::cout<<fState0<<std::endl
154  <<fState1<<std::endl
155  <<fState2<<std::endl
156  <<fState3<<std::endl<<std::endl;
157  */
158  TMatrixT<double> jacobian(4, 4);
159  /*
160  cg3 = .(ufrom
161  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, ufrom) * c * ufrom
162  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, ufrom) * d * vfrom
163  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, ufrom) *wfrom, u);
164 
165  cg4 = .(vfrom
166  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, vfrom) * c * ufrom
167  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, vfrom) * d * vfrom
168  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, vfrom) *wfrom, u);
169 
170  cg5 = .(- pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) * c * ufrom * .(w, ufrom)
171  + 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, o) * ufrom
172  - pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) * d * vfrom * .(w, ufrom)
173  - pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) *wfrom * .(w, ufrom)
174  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) * c * ufrom * .(w, ufrom)
175  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, ofrom + a * ufrom + b * vfrom) * ufrom
176  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) * d * vfrom * .(w, ufrom)
177  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) *wfrom * .(w, ufrom), u);
178 
179  cg6 = .(-pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) * c * ufrom * .(w, vfrom)
180  - pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) * d * vfrom * .(w, vfrom)
181  + 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, o) * vfrom
182  - pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) *wfrom * .(w, vfrom)
183  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) * c * ufrom * .(w, vfrom)
184  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) * d * vfrom * .(w, vfrom)
185  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, ofrom + a * ufrom + b * vfrom) * vfrom
186  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) *wfrom * .(w, vfrom), u);
187 
188  cg7 = .(ufrom
189  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, ufrom) * c * ufrom
190  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, ufrom) * d * vfrom
191  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, ufrom) *wfrom, v);
192 
193  cg8 = .(vfrom
194  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, vfrom) * c * ufrom
195  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, vfrom) * d * vfrom
196  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, vfrom) *wfrom, v);
197 
198  cg9 = .(-pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) * c * ufrom * .(w, ufrom)
199  + 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, o) * ufrom
200  - pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) * d * vfrom * .(w, ufrom)
201  - pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) *wfrom * .(w, ufrom)
202  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) * c * ufrom * .(w, ufrom)
203  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, ofrom + a * ufrom + b * vfrom) * ufrom
204  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) * d * vfrom * .(w, ufrom)
205  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) *wfrom * .(w, ufrom), v);
206 
207  cg10 = .(-pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) * c * ufrom * .(w, vfrom)
208  - pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) * d * vfrom * .(w, vfrom)
209  + 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, o) * vfrom
210  - pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, o) *wfrom * .(w, vfrom)
211  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) * c * ufrom * .(w, vfrom)
212  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) * d * vfrom * .(w, vfrom)
213  - 1 / .(w, c * ufrom + d * vfrom +wfrom) * .(w, ofrom + a * ufrom + b * vfrom) * vfrom
214  + pow(.(w, c * ufrom + d * vfrom +wfrom), -2) * .(w, ofrom + a * ufrom + b * vfrom) *wfrom * .(w, vfrom), v);
215 
216  cg11 = 0;
217  cg12 = 0;
218 
219  cg13 = .(ufrom, u) / .(c * ufrom + d * vfrom +wfrom, w)
220  - .(c * ufrom + d * vfrom +wfrom, u) * pow(.(c * ufrom + d * vfrom +wfrom, w), -2) * .(ufrom, w);
221 
222  cg14 = .(vfrom, u) / .(c * ufrom + d * vfrom +wfrom, w)
223  - .(c * ufrom + d * vfrom +wfrom, u) * pow(.(c * ufrom + d * vfrom +wfrom, w), -2) * .(vfrom, w);
224  cg15 = 0;
225  cg16 = 0;
226 
227  cg17 = .(ufrom, v) / .(c * ufrom + d * vfrom +wfrom, w)
228  - .(c * ufrom + d * vfrom +wfrom, v) * pow(.(c * ufrom + d * vfrom +wfrom, w), -2) * .(ufrom, w);
229 
230  cg18 = .(vfrom, v) / .(c * ufrom + d * vfrom +wfrom, w)
231  - .(c * ufrom + d * vfrom +wfrom, v) * pow(.(c * ufrom + d * vfrom +wfrom, w), -2) * .(vfrom, w);
232 
233  */
234 
235  double jacobian0 =
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) *
238  u;
239 
240  double jacobian1 =
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) *
243  u;
244 
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)) *
253  u;
254 
255  double jacobian3 =
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)) *
262  u;
263 
264  double jacobian4 =
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) *
267  v;
268 
269  double jacobian5 =
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) *
272  v;
273 
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)) *
282  v;
283 
284  double jacobian7 =
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)) *
291  v;
292  double jacobian8 = 0;
293  double jacobian9 = 0;
294 
295  double jacobian10 = ((ufrom * u) / (w * dir) - (dir * u) * pow((w * dir), -2) * (w * ufrom));
296 
297  double jacobian11 = ((vfrom * u) / (w * dir) - (dir * u) * pow((w * dir), -2) * (w * vfrom));
298  double jacobian12 = 0;
299  double jacobian13 = 0;
300 
301  double jacobian14 = ((ufrom * v) / (w * dir) - (dir * v) * pow((w * dir), -2) * (w * ufrom));
302 
303  double jacobian15 = ((vfrom * v) / (w * dir) - (dir * v) * pow((w * dir), -2) * (w * vfrom));
304 
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);
322  jacobianT.T();
323  covPred = jacobian * fCov * (jacobianT);
324  // std::cout<<"covariance[0][0]"<<covPred[0][0]<<std::endl;
325  statePred[0].Assign(state0);
326  statePred[1].Assign(state1);
327  statePred[2].Assign(state2);
328  statePred[3].Assign(state3);
329  return dist;
330 }
331 void genf::SlTrackRep::extrapolateToPoint(const TVector3& pos, TVector3& poca, TVector3& dirInPoca)
332 {
333 
334  TVector3 ofrom = fRefPlane.getO();
335  TVector3 ufrom = fRefPlane.getU();
336  TVector3 vfrom = fRefPlane.getV();
337  TVector3 wfrom = ufrom.Cross(vfrom);
338  TVector3 pfrom = ofrom + fState[0][0] * ufrom + fState[1][0] * vfrom;
339  TVector3 dir = (fState[2][0] * ufrom + fState[3][0] * vfrom + wfrom); //in global coordinates
340  // pfrom.Print();
341  dir = dir.Unit();
342  //dir.Print();
343  double t = (dir * pos - dir * pfrom) / (dir * dir);
344 
345  poca = pfrom + t * dir;
346  dirInPoca = dir.Unit();
347 }
348 void genf::SlTrackRep::extrapolateToLine(const TVector3& point1,
349  const TVector3& point2,
350  TVector3& poca,
351  TVector3& dirInPoca,
352  TVector3& poca_onwire)
353 {
354 
355  TVector3 ofrom = fRefPlane.getO();
356  TVector3 ufrom = fRefPlane.getU();
357  TVector3 vfrom = fRefPlane.getV();
358  TVector3 wfrom = ufrom.Cross(vfrom);
359 
360  TVector3 pfrom = ofrom + fState[0][0] * ufrom + fState[1][0] * vfrom;
361  TVector3 dir = (fState[2][0] * ufrom + fState[3][0] * vfrom + wfrom); //in global coordinates
362  TVector3 lineDir = point2 - point1;
363  //normal normal to both lineDir and Dir
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());
367 
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;
374  dirInPoca = dir;
375 }
377 {
378  TMatrixT<double> statePred(fState);
379  if (pl != fRefPlane) { extrapolate(pl, statePred); }
380  return pl.getO() + (statePred[0][0] * pl.getU()) + (statePred[1][0] * pl.getV());
381 }
383 {
384  TMatrixT<double> statePred(fState);
385  if (pl != fRefPlane) { extrapolate(pl, statePred); }
386  TVector3 ret = pl.getNormal() + (statePred[2][0] * pl.getU()) + (statePred[3][0] * pl.getV());
387  ret.SetMag(1.);
388  return ret;
389 }
390 void genf::SlTrackRep::getPosMom(const GFDetPlane& pl, TVector3& pos, TVector3& mom)
391 {
392  pos = getPos(pl);
393  mom = getMom(pl);
394 }
virtual void getPosMom(const GFDetPlane &, TVector3 &pos, TVector3 &mom)
Definition: SlTrackRep.cxx:390
Double_t z
Definition: plot.C:276
unsigned int fDimension
Dimensionality of track representation.
Definition: GFAbsTrackRep.h:86
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...
Definition: SlTrackRep.cxx:331
Base Class for genfit track representations. Defines interface for track parameterizations.
Definition: GFAbsTrackRep.h:81
TVector3 getNormal() const
Definition: GFDetPlane.cxx:133
TMatrixT< Double_t > fCov
The covariance matrix.
Definition: GFAbsTrackRep.h:92
TVector3 getO() const
Definition: GFDetPlane.h:77
Float_t d
Definition: plot.C:235
virtual double extrapolate(const GFDetPlane &, TMatrixT< double > &statePred, TMatrixT< double > &covPred)
Definition: SlTrackRep.cxx:94
void setReferencePlane(const GFDetPlane &pl)
Definition: SlTrackRep.h:21
TTree * t2
Definition: plottest35.C:36
TVector3 getU() const
Definition: GFDetPlane.h:78
TDirectory * dir
Definition: macro.C:5
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.
Definition: SlTrackRep.cxx:348
TMatrixT< Double_t > fState
The vector of track parameters.
Definition: GFAbsTrackRep.h:89
virtual ~SlTrackRep()
Definition: SlTrackRep.cxx:55
TVector3 getV() const
Definition: GFDetPlane.h:79
Float_t w
Definition: plot.C:20