EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
GFTools.cxx
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file GFTools.cxx
1 
2 #include <memory>
3 #include "GFTools.h"
4 #include <typeinfo>
5 
6 #include <GeaneTrackRep.h>
7 
8 TMatrixT<double> GFTools::getSmoothedPos(const GFTrack* trk, unsigned int irep, unsigned int ihit) {
9 
10  TMatrixT<double> smoothed_state;
11  TMatrixT<double> smoothed_cov;
12  TMatrixT<double> pos;
13 
14  if(GFTools::getSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov)) {
15 
16  TMatrixT<double> H = trk->getHit(ihit)->getHMatrix(trk->getTrackRep(irep));
17  TMatrixT<double> pos_tmp(H * smoothed_state);
18  pos.ResizeTo(pos_tmp);
19  pos = pos_tmp;
20 
21  }
22  return pos;
23 
24 }
25 
26 
27 TVector3 GFTools::getSmoothedPosXYZ(const GFTrack* trk, unsigned int irep, unsigned int ihit, bool *ret){
28 
29  TMatrixT<double> smoothed_state;
30  TMatrixT<double> smoothed_cov;
31  TMatrixT<double> pos;
32  GFDetPlane plane;
33 
34  if(GFTools::getSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov, plane)) {
35 
36  TMatrixT<double> H = trk->getHit(ihit)->getHMatrix(trk->getTrackRep(irep));
37  TMatrixT<double> pos_tmp(H * smoothed_state);
38  pos.ResizeTo(pos_tmp);
39  pos = pos_tmp;
40  } else {
41  if (ret) *ret = false;
42  return TVector3();
43  } //if
44 
45  // check dimension
46  if (pos.GetNrows() != 2 || pos.GetNcols() != 1){
47  GFException exc("GFTools::getSmoothedPosXYZ ==> dimension of hit in plane is not 2, cannot calculate (x,y,z) hit position",__LINE__,__FILE__);
48  throw exc;
49  }
50 
51  // calc 3D position
52  TVector3 pos3D(plane.getO());
53  pos3D += pos(0,0) * plane.getU();
54  pos3D += pos(1,0) * plane.getV();
55 
56  if (ret) *ret = true;
57  return pos3D;
58 }
59 // FIXME: yes, just a cut'n'paste for now;
60 TVector3 GFTools::getBiasedSmoothedPosXYZ(const GFTrack* trk, unsigned int irep, unsigned int ihit, bool *ret){
61 
62  TMatrixT<double> smoothed_state;
63  TMatrixT<double> smoothed_cov;
64  TMatrixT<double> pos;
65  GFDetPlane plane;
66 
67  //printf("GFTools::getBiasedSmoothedPosXYZ() #1\n");
68 
69  // 'false': extrapolation does not work here, for whatever reason (FIXME); therefore
70  // can not produce estimates in the TPC volume, unless it is either the very first
71  // or the very last hit of a given track (in which case a KF smoother lookup value
72  // is taken); otherwise for a TPC-like detector the virtual planes for forward
73  // and backward passes are *different*, so extrapolation branch is turned on
74  // and it crashes deeper in the G3 Geane codes that I ever wanted to debug;
75  if(GFTools::getBiasedSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov, plane, false)) {
76  //printf("GFTools::getBiasedSmoothedPosXYZ() #2\n");
77  TMatrixT<double> H = trk->getHit(ihit)->getHMatrix(trk->getTrackRep(irep));
78  TMatrixT<double> pos_tmp(H * smoothed_state);
79  pos.ResizeTo(pos_tmp);
80  pos = pos_tmp;
81  //printf("GFTools::getBiasedSmoothedPosXYZ() #3\n");
82  } else {
83  if (ret) *ret = false;
84  return TVector3();
85  } //if
86 
87  //printf("GFTools::getBiasedSmoothedPosXYZ() #4\n");
88 
89  // check dimension
90  if (pos.GetNrows() != 2 || pos.GetNcols() != 1){
91  //printf("@@@ GFTools::getBiasedSmoothedPosXYZ() #5\n");
92  if (ret) *ret = false;
93  return TVector3();
94  //GFException exc("GFTools::getSmoothedPosXYZ ==> dimension of hit in plane is not 2, cannot calculate (x,y,z) hit position",__LINE__,__FILE__);
95  //throw exc;
96  }
97 
98  //printf("GFTools::getBiasedSmoothedPosXYZ() #6\n");
99 
100  // calc 3D position
101  TVector3 pos3D(plane.getO());
102  pos3D += pos(0,0) * plane.getU();
103  pos3D += pos(1,0) * plane.getV();
104 
105  if (ret) *ret = true;
106  return pos3D;
107 }
108 
109 
110 //TVector3 GFTools::getSmoothedMomXYZ(const GFTrack* trk, unsigned int irep, unsigned int ihit){
111 TVector3 GFTools::getBiasedSmoothedMomXYZ(const GFTrack* trk, unsigned int irep, unsigned int ihit, bool *ret){
112 
113  std::auto_ptr<GFAbsTrackRep> rep(trk->getTrackRep(irep)->clone());
114 
115  TMatrixT<double> smoothed_state, smoothed_cov, auxInfo;
116  GFDetPlane smoothing_plane;
117 
118  // 'false': see comment in GFTools::getBiasedSmoothedPosXYZ();
119  if (!getBiasedSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov, smoothing_plane, auxInfo, false)) {
120  if (ret) *ret = false;
121  return TVector3();
122  } //if
123 
124  if(rep->hasAuxInfo()) {
125  rep->setData(smoothed_state, smoothing_plane, &smoothed_cov, &auxInfo);
126  } else {
127  rep->setData(smoothed_state, smoothing_plane, &smoothed_cov);
128  }
129 
130  if (ret) *ret = true;
131  return rep->getMom(smoothing_plane);
132 }
133 
134 
135 TMatrixT<double> GFTools::getBiasedSmoothedPos(const GFTrack* trk, unsigned int irep, unsigned int ihit) {
136 
137  TMatrixT<double> smoothed_state;
138  TMatrixT<double> smoothed_cov;
139  TMatrixT<double> pos;
140 
141  if(GFTools::getBiasedSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov)) {
142 
143  TMatrixT<double> H = trk->getHit(ihit)->getHMatrix(trk->getTrackRep(irep));
144  //H.Print();smoothed_state.Print();
145  TMatrixT<double> pos_tmp(H * smoothed_state);
146  pos.ResizeTo(pos_tmp);
147  pos = pos_tmp;
148 
149  }
150  return pos;
151 
152 }
153 
154 TMatrixT<double> GFTools::getSmoothedCov(const GFTrack* trk, unsigned int irep, unsigned int ihit) {
155 
156  TMatrixT<double> smoothed_state;
157  TMatrixT<double> smoothed_cov;
158  GFDetPlane pl;
159 
160  GFTools::getSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov, pl);
161  TMatrixT<double> H = trk->getHit(ihit)->getHMatrix(trk->getTrackRep(irep));
162 
163  TMatrixT<double> cov_tmp(smoothed_cov,TMatrixT<double>::kMultTranspose,H);
164  TMatrixT<double> cov(H,TMatrixT<double>::kMult,cov_tmp);
165 
166  return cov;
167 
168 }
169 
170 TMatrixT<double> GFTools::getBiasedSmoothedCov(const GFTrack* trk, unsigned int irep, unsigned int ihit) {
171 
172  TMatrixT<double> smoothed_state;
173  TMatrixT<double> smoothed_cov;
174  GFDetPlane pl;
175 
176  GFTools::getBiasedSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov, pl);
177  TMatrixT<double> H = trk->getHit(ihit)->getHMatrix(trk->getTrackRep(irep));
178 
179  TMatrixT<double> cov_tmp(smoothed_cov,TMatrixT<double>::kMultTranspose,H);
180  TMatrixT<double> cov(H,TMatrixT<double>::kMult,cov_tmp);
181 
182  return cov;
183 
184 }
185 
186 bool GFTools::getSmoothedData(const GFTrack* trk, unsigned int irep, unsigned int ihit, TMatrixT<double>& smoothed_state, TMatrixT<double>& smoothed_cov) {
187 
188  GFDetPlane pl;
189  TMatrixT<double> auxInfo;
190  return GFTools::getSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov, pl, auxInfo);
191 
192 }
193 
194 bool GFTools::getBiasedSmoothedData(const GFTrack* trk, unsigned int irep, unsigned int ihit, TMatrixT<double>& smoothed_state, TMatrixT<double>& smoothed_cov) {
195 
196  GFDetPlane pl;
197  TMatrixT<double> auxInfo;
198  return GFTools::getBiasedSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov, pl, auxInfo);
199 
200 }
201 
202 bool GFTools::getSmoothedData(const GFTrack* trk, unsigned int irep, unsigned int ihit, TMatrixT<double>& smoothed_state, TMatrixT<double>& smoothed_cov, GFDetPlane& smoothing_plane) {
203 
204  TMatrixT<double> auxInfo;
205  return GFTools::getSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov, smoothing_plane, auxInfo);
206 
207 }
208 
209 bool GFTools::getBiasedSmoothedData(const GFTrack* trk, unsigned int irep, unsigned int ihit, TMatrixT<double>& smoothed_state, TMatrixT<double>& smoothed_cov, GFDetPlane& smoothing_plane, bool extrapolation_allowed) {
210 
211  TMatrixT<double> auxInfo;
212  return GFTools::getBiasedSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov,
213  smoothing_plane, auxInfo, extrapolation_allowed);
214 
215 }
216 
217 bool GFTools::getSmoothedData(const GFTrack* trk, unsigned int irep, unsigned int ihit, TMatrixT<double>& smoothed_state, TMatrixT<double>& smoothed_cov, GFDetPlane& smoothing_plane, TMatrixT<double>& auxInfo) {
218 
219  //printf("Here!\n");
220 
221  if(!trk->getSmoothing()) {
222  std::cout << "Trying to get smoothed hit coordinates from a track without smoothing! Aborting..." << std::endl;
223  TMatrixT<double> failed(1,1);
224  return false;
225  }
226 
227  if(ihit >= trk->getNumHits()) {
228  std::cout << "Hit number out of bounds while trying to get smoothed hit coordinates! Aborting..." <<std::endl;
229  TMatrixT<double> failed(1,1);
230  failed(0,0) = 0;
231  return false;
232  }
233 
234  //std::auto_ptr<GFAbsTrackRep> rep(trk->getTrackRep(irep)->clone());
235  GeaneTrackRep *rep = dynamic_cast<GeaneTrackRep*>(trk->getTrackRep(irep)->clone());
236 
237  TMatrixT<double> fUpSt;
238  TMatrixT<double> fUpCov;
239  TMatrixT<double> fAuxInfo;
240  TMatrixT<double>* fAuxInfoP;
241  TMatrixT<double> bUpSt;
242  TMatrixT<double> bUpCov;
243  TMatrixT<double> bAuxInfo;
244  TMatrixT<double>* bAuxInfoP;
245 
246  TMatrixT<double> fSt;
247  TMatrixT<double> fCov;
248  TMatrixT<double> bSt;
249  TMatrixT<double> bCov;
250 
251  GFDetPlane fPl;
252  GFDetPlane bPl;
253 
254  if(trk->getTrackRep(irep)->hasAuxInfo()) {
255  trk->getBK(irep)->getMatrix("fAuxInfo",ihit,auxInfo);
256  }
257 
258  if(!(trk->getSmoothingFast())) {
259  //printf("Here!\n");
260  if(ihit == 0) {
261  rep->setPropDir(-1);
262  trk->getBK(irep)->getMatrix("bUpSt",ihit+1,bUpSt);
263  trk->getBK(irep)->getMatrix("bUpCov",ihit+1,bUpCov);
264  trk->getBK(irep)->getDetPlane("fPl",ihit,smoothing_plane);
265  trk->getBK(irep)->getDetPlane("bPl",ihit+1,bPl);
266  if(trk->getTrackRep(irep)->hasAuxInfo()) {
267  trk->getBK(irep)->getMatrix("bAuxInfo",ihit+1,bAuxInfo);
268  bAuxInfoP = &bAuxInfo;
269  } else {
270  bAuxInfoP = NULL;
271  }
272  if(bUpSt.GetNrows() == 0) { printf("Ku-0\n"); return false; };
273  rep->setData(bUpSt,bPl,&bUpCov,bAuxInfoP);
274  rep->extrapolate(smoothing_plane,smoothed_state,smoothed_cov);
275  return true;
276  }
277 
278  if(ihit == trk->getNumHits()-1) {
279  rep->setPropDir(1);
280  trk->getBK(irep)->getMatrix("fUpSt",ihit-1,fUpSt);
281  trk->getBK(irep)->getMatrix("fUpCov",ihit-1,fUpCov);
282  trk->getBK(irep)->getDetPlane("fPl",ihit-1,fPl);
283  trk->getBK(irep)->getDetPlane("fPl",ihit,smoothing_plane);
284  if(trk->getTrackRep(irep)->hasAuxInfo()) {
285  trk->getBK(irep)->getMatrix("fAuxInfo",ihit-1,fAuxInfo);
286  fAuxInfoP = &fAuxInfo;
287  } else {
288  fAuxInfoP = NULL;
289  }
290  if(fUpSt.GetNrows() == 0) { printf("Ku-1\n"); return false; };
291  rep->setData(fUpSt,fPl,&fUpCov,fAuxInfoP);
292  rep->extrapolate(smoothing_plane,smoothed_state,smoothed_cov);
293  return true;
294  }
295 
296  trk->getBK(irep)->getMatrix("fUpSt",ihit-1,fUpSt);
297  trk->getBK(irep)->getMatrix("fUpCov",ihit-1,fUpCov);
298  trk->getBK(irep)->getMatrix("bUpSt",ihit+1,bUpSt);
299  trk->getBK(irep)->getMatrix("bUpCov",ihit+1,bUpCov);
300  if(trk->getTrackRep(irep)->hasAuxInfo()) {
301  trk->getBK(irep)->getMatrix("fAuxInfo",ihit-1,fAuxInfo);
302  trk->getBK(irep)->getMatrix("bAuxInfo",ihit+1,bAuxInfo);
303  fAuxInfoP = &fAuxInfo;
304  bAuxInfoP = &bAuxInfo;
305  } else {
306  fAuxInfoP = bAuxInfoP = NULL;
307  }
308  trk->getBK(irep)->getDetPlane("fPl",ihit-1,fPl);
309  trk->getBK(irep)->getDetPlane("fPl",ihit,smoothing_plane);
310  trk->getBK(irep)->getDetPlane("bPl",ihit+1,bPl);
311 
312  if(fUpSt.GetNrows() == 0 || bUpSt.GetNrows() == 0) { printf("Ku-2\n"); return false; };
313 
314  //printf("There!\n");
315  rep->setPropDir(1);
316  rep->setData(fUpSt,fPl,&fUpCov,fAuxInfoP);
317  rep->extrapolate(smoothing_plane,fSt,fCov);
318  //printf("There!\n");
319  rep->setPropDir(-1);
320  rep->setData(bUpSt,bPl,&bUpCov,bAuxInfoP);
321  rep->extrapolate(smoothing_plane,bSt,bCov);
322  //printf("There!\n");
323  } else {
324 
325  if(ihit == 0) {
326  trk->getBK(irep)->getMatrix("bUpSt",ihit+1,bUpSt);
327  trk->getBK(irep)->getMatrix("bUpCov",ihit+1,bUpCov);
328  trk->getBK(irep)->getDetPlane("fPl",ihit,smoothing_plane);
329  trk->getBK(irep)->getDetPlane("bPl",ihit+1,bPl);
330  if(trk->getTrackRep(irep)->hasAuxInfo()) {
331  trk->getBK(irep)->getMatrix("bAuxInfo",ihit+1,bAuxInfo);
332  bAuxInfoP = &bAuxInfo;
333  } else {
334  bAuxInfoP = NULL;
335  }
336  if(bUpSt.GetNrows() == 0) return false;
337  rep->setData(bUpSt,bPl,&bUpCov,bAuxInfoP);
338  rep->extrapolate(smoothing_plane,smoothed_state,smoothed_cov);
339  return true;
340  }
341 
342  if(ihit == trk->getNumHits()-1) {
343  trk->getBK(irep)->getDetPlane("fPl", ihit, smoothing_plane);
344  trk->getBK(irep)->getMatrix("fSt", ihit, smoothed_state);
345  trk->getBK(irep)->getMatrix("fCov", ihit, smoothed_cov);
346  return true;
347  }
348 
349  trk->getBK(irep)->getDetPlane("fPl", ihit, smoothing_plane);
350  trk->getBK(irep)->getDetPlane("bPl", ihit, bPl);
351  trk->getBK(irep)->getMatrix("fSt", ihit, fSt);
352  trk->getBK(irep)->getMatrix("fCov", ihit, fCov);
353 
354  if(smoothing_plane == bPl) {
355  trk->getBK(irep)->getMatrix("bSt", ihit, bSt);
356  trk->getBK(irep)->getMatrix("bCov", ihit, bCov);
357  } else {
358  trk->getBK(irep)->getMatrix("bUpSt",ihit+1,bUpSt);
359  trk->getBK(irep)->getMatrix("bUpCov",ihit+1,bUpCov);
360  trk->getBK(irep)->getDetPlane("bPl", ihit+1, bPl);
361  if(trk->getTrackRep(irep)->hasAuxInfo()) {
362  trk->getBK(irep)->getMatrix("bAuxInfo", ihit+1, bAuxInfo);
363  bAuxInfoP = &bAuxInfo;
364  } else {
365  bAuxInfoP = NULL;
366  }
367  rep->setData(bUpSt, bPl, &bUpCov, bAuxInfoP);
368  rep->extrapolate(smoothing_plane, bSt, bCov);
369  }
370 
371  }
372 
373  TMatrixT<double> fCovInvert;
374  TMatrixT<double> bCovInvert;
375 
376  GFTools::invertMatrix(fCov, fCovInvert);
377  GFTools::invertMatrix(bCov, bCovInvert);
378 
379  GFTools::invertMatrix(fCovInvert + bCovInvert, smoothed_cov);
380 
381  smoothed_state.ResizeTo(smoothed_cov * (fCovInvert*fSt + bCovInvert*bSt));
382  smoothed_state = smoothed_cov * (fCovInvert*fSt + bCovInvert*bSt);
383 
384  return true;
385 
386 }
387 
388 bool GFTools::getBiasedSmoothedData(const GFTrack* trk, unsigned int irep, unsigned int ihit, TMatrixT<double>& smoothed_state, TMatrixT<double>& smoothed_cov, GFDetPlane& smoothing_plane, TMatrixT<double>& auxInfo, bool extrapolation_allowed) {
389 
390  if(!trk->getSmoothing()) {
391  std::cout << "Trying to get smoothed hit coordinates from a track without smoothing! Aborting..." << std::endl;
392  TMatrixT<double> failed(1,1);
393  return false;
394  }
395 
396  if(ihit >= trk->getNumHits()) {
397  std::cout << "Hit number out of bounds while trying to get smoothed hit coordinates! Aborting..." <<std::endl;
398  TMatrixT<double> failed(1,1);
399  failed(0,0) = 0;
400  return false;
401  }
402 
403  TMatrixT<double> bUpSt;
404  TMatrixT<double> bUpCov;
405  TMatrixT<double> bAuxInfo;
406  TMatrixT<double>* bAuxInfoP;
407  GFDetPlane bPl;
408 
409  //std::auto_ptr<GFAbsTrackRep> rep(trk->getTrackRep(irep)->clone());
410  GeaneTrackRep *rep = dynamic_cast<GeaneTrackRep*>(trk->getTrackRep(irep)->clone());
411 
412  if(trk->getTrackRep(irep)->hasAuxInfo()) {
413  trk->getBK(irep)->getMatrix("fAuxInfo",ihit,auxInfo);
414  }
415 
416  //printf("GFTools::getBiasedSmoothedData() #1\n");
417 
418  if(ihit == 0) {
419  trk->getBK(irep)->getMatrix("bUpSt",ihit,smoothed_state);
420  trk->getBK(irep)->getMatrix("bUpCov",ihit,smoothed_cov);
421  trk->getBK(irep)->getDetPlane("bPl",ihit,smoothing_plane);
422  return true;
423  }
424 
425  if(ihit == trk->getNumHits()-1) {
426  trk->getBK(irep)->getMatrix("fUpSt",ihit,smoothed_state);
427  trk->getBK(irep)->getMatrix("fUpCov",ihit,smoothed_cov);
428  trk->getBK(irep)->getDetPlane("fPl",ihit,smoothing_plane);
429  return true;
430  }
431 
432  //printf("GFTools::getBiasedSmoothedData() #2\n");
433 
434  TMatrixT<double> fSt;
435  TMatrixT<double> fCov;
436  TMatrixT<double> bSt;
437  TMatrixT<double> bCov;
438 
439  if(!(trk->getSmoothingFast())) {
440 
441  trk->getBK(irep)->getMatrix("fUpSt",ihit,fSt);
442  trk->getBK(irep)->getMatrix("fUpCov",ihit,fCov);
443  trk->getBK(irep)->getMatrix("bUpSt",ihit+1,bUpSt);
444  trk->getBK(irep)->getMatrix("bUpCov",ihit+1,bUpCov);
445  if(trk->getTrackRep(irep)->hasAuxInfo()) {
446  trk->getBK(irep)->getMatrix("bAuxInfo",ihit+1,bAuxInfo);
447  bAuxInfoP = &bAuxInfo;
448  } else {
449  bAuxInfoP = NULL;
450  }
451  trk->getBK(irep)->getDetPlane("fPl",ihit,smoothing_plane);
452  trk->getBK(irep)->getDetPlane("bPl",ihit+1,bPl);
453 
454  if(bUpSt.GetNrows() == 0) {
455  return false;
456  }
457 
458 
459  rep->setPropDir(-1);
460  rep->setData(bUpSt,bPl,&bUpCov,bAuxInfoP);
461  rep->extrapolate(smoothing_plane,bSt,bCov);
462 
463  } else {
464 
465  trk->getBK(irep)->getMatrix("fUpSt",ihit,fSt);
466  trk->getBK(irep)->getMatrix("fUpCov",ihit,fCov);
467  trk->getBK(irep)->getDetPlane("fPl",ihit,smoothing_plane);
468  trk->getBK(irep)->getDetPlane("bPl",ihit,bPl);
469 
470  if(smoothing_plane == bPl) {
471  //printf("GFTools::getBiasedSmoothedData() #3a\n");
472  //assert(0);
473  //printf("QQQ\n");
474  trk->getBK(irep)->getMatrix("bSt",ihit,bSt);
475  trk->getBK(irep)->getMatrix("bCov",ihit,bCov);
476  } else {
477  //printf("GFTools::getBiasedSmoothedData() #3b\n");
478  if (!extrapolation_allowed) return false;
479  //assert(0);
480  trk->getBK(irep)->getMatrix("bUpSt",ihit+1,bUpSt);
481  trk->getBK(irep)->getMatrix("bUpCov",ihit+1,bUpCov);
482  if(trk->getTrackRep(irep)->hasAuxInfo()) {
483  trk->getBK(irep)->getMatrix("bAuxInfo",ihit+1,bAuxInfo);
484  bAuxInfoP = &bAuxInfo;
485  } else {
486  bAuxInfoP = NULL;
487  }
488  trk->getBK(irep)->getDetPlane("fPl",ihit,smoothing_plane);
489  trk->getBK(irep)->getDetPlane("bPl",ihit+1,bPl);
490 
491  if(bUpSt.GetNrows() == 0) {
492  return false;
493  }
494 
495  rep->setData(bUpSt,bPl,&bUpCov,bAuxInfoP);
496  rep->extrapolate(smoothing_plane,bSt,bCov);
497  }
498 
499  }
500 
501  //printf("GFTools::getBiasedSmoothedData() #4\n");
502 
503  TMatrixT<double> fCovInvert;
504  TMatrixT<double> bCovInvert;
505 
506  GFTools::invertMatrix(fCov, fCovInvert);
507  GFTools::invertMatrix(bCov, bCovInvert);
508 
509  GFTools::invertMatrix(fCovInvert + bCovInvert, smoothed_cov);
510 
511  smoothed_state.ResizeTo(smoothed_cov * (fCovInvert*fSt + bCovInvert*bSt));
512  smoothed_state = smoothed_cov * (fCovInvert*fSt + bCovInvert*bSt);
513 
514  return true;
515 
516 }
517 
518 GFDetPlane GFTools::getSmoothingPlane(const GFTrack* trk, unsigned int irep, unsigned int ihit) {
519 
520  GFDetPlane pl;
521  trk->getBK(irep)->getDetPlane("fPl",ihit,pl);
522  return pl;
523 
524 }
525 
526 double GFTools::getTrackLength(const GFTrack* trk, unsigned int irep, unsigned int startHit, unsigned int endHit){
527 
528  if(!trk->getSmoothing()) {
529  GFException exc("Trying to get tracklength from a track without smoothing!",__LINE__,__FILE__);
530  throw exc;
531  }
532 
533  if(startHit >= trk->getNumHits() || endHit >= trk->getNumHits()) {
534  GFException exc("Hit number out of bounds while trying to get tracklength!",__LINE__,__FILE__);
535  throw exc;
536  }
537 
538  bool inv(false);
539  if(startHit > endHit) {
540  unsigned int biggerOne = startHit;
541  startHit = endHit;
542  endHit = biggerOne;
543  inv = true;
544  }
545 
546  if (startHit==0 && endHit==0) endHit = trk->getNumHits()-1;
547  if (startHit == endHit) return 0.;
548 
549  double totLen(0), fLen(0), bLen(0);
550 
551  for (unsigned int i=startHit; i!=endHit; ++i){
552  trk->getBK(irep)->getNumber("fExtLen", i+1, fLen);
553  trk->getBK(irep)->getNumber("bExtLen", i, bLen);
554  totLen += 0.5*(fLen - bLen);
555  }
556 
557  if (inv) return -totLen;
558  return totLen;
559 
560 }
561 
562 void GFTools::invertMatrix(const TMatrixT<double>& mat, TMatrixT<double>& inv){
563  inv.ResizeTo(mat);
564 
565  // check if numerical limits are reached (i.e at least one entry < 1E-100 and/or at least one entry > 1E100)
566  if (!(mat<1.E100) || !(mat>-1.E100)){
567  GFException e("cannot invert matrix GFTools::invertMatrix(), entries too big (>1e100)",
568  __LINE__,__FILE__);
569  e.setFatal();
570  throw e;
571  }
572  // do the trivial inversion for 2x2 matrices manually
573  if (mat.GetNrows() == 2){
574  double det = mat(0,0)*mat(1,1) - mat(1,0)*mat(0,1);
575  if(fabs(det) < 1E-50){
576  GFException e("cannot invert matrix GFTools::invertMatrix(), determinant = 0",
577  __LINE__,__FILE__);
578  e.setFatal();
579  throw e;
580  }
581  det = 1./det;
582  inv(0,0) = det * mat(1,1);
583  inv(0,1) = -1.*det * mat(0,1);
584  inv(1,0) = -1.*det * mat(1,0);
585  inv(1,1) = det * mat(0,0);
586  return;
587  }
588 
589  // else use TDecompSVD
590  bool status = 0;
591  TDecompSVD invertAlgo(mat);
592 
593  invertAlgo.SetTol(1E-50); //this is a hack because a tolerance of 1E-22 does not make any sense for doubles only for floats
594 
595  inv = invertAlgo.Invert(status);
596  if(status == 0){
597  GFException e("cannot invert matrix GFTools::invertMatrix(), status = 0",
598  __LINE__,__FILE__);
599  e.setFatal();
600  throw e;
601  }
602 }
603 
604 void GFTools::updateRepSmoothed(GFTrack* trk, unsigned int irep, unsigned int ihit) {
605 
606  TMatrixT<double> smoothed_state, smoothed_cov, auxInfo;
607  GFDetPlane smoothing_plane;
608 
609  getBiasedSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov, smoothing_plane, auxInfo);
610 
611  GFAbsTrackRep* rep = trk->getTrackRep(irep);
612  if(rep->hasAuxInfo()) {
613  rep->setData(smoothed_state, smoothing_plane, &smoothed_cov, &auxInfo);
614  } else {
615  rep->setData(smoothed_state, smoothing_plane, &smoothed_cov);
616  }
617 
618  trk->setRepAtHit(irep, ihit);
619 }
620 
621 double GFTools::getSmoothedChiSqu(const GFTrack* trk, unsigned int irep, unsigned int ihit){
622  TMatrixT<double> smoothed_state;
623  TMatrixT<double> smoothed_cov;
624  GFTools::getBiasedSmoothedData(trk, irep, ihit, smoothed_state, smoothed_cov);
625  TMatrixT<double> H = trk->getHit(ihit)->getHMatrix(trk->getTrackRep(irep));
626  TMatrixT<double> HT(TMatrixT<double>::kTransposed, H);
627  TMatrixT<double> pos = H * smoothed_state;
628  TMatrixT<double> m = trk->getHit(ihit)->getRawHitCoord(); //measurement of hit
629  TMatrixT<double> V = trk->getHit(ihit)->getRawHitCov(); //covariance matrix of hit
630  TMatrixT<double> res = m - pos;
631  TMatrixT<double> resT(TMatrixT<double>::kTransposed, res);
632  TMatrixT<double> R = V - H*smoothed_cov*HT;
633  TMatrixT<double> invR;
634  invertMatrix(R,invR);
635  TMatrixT<double> smoothedChiSqu = resT*invR*res;
636  return smoothedChiSqu(0,0);
637 }
638 
639 unsigned int GFTools::getClosestHit(const GFTrack* trk, unsigned int irep, const TVector3& pos, double& distance, bool checkEveryHit){
640 
641  if(!trk->getSmoothing()) {
642  GFException exc("Trying to get closest hit from a track without smoothing!",__LINE__,__FILE__);
643  throw exc;
644  }
645 
646  unsigned int nHits = trk->getNumHits();
647  if (nHits == 1) return 0;
648 
649  TVector3 hitPos;
650  double minDist(1.E99), dist;
651  unsigned int minId(0);
652 
653  if (checkEveryHit || nHits<4){
654  // brute force search
655  for (unsigned int i=0; i<nHits; ++i){
656  hitPos = getSmoothedPosXYZ(trk, irep, i);
657  dist = (pos-hitPos).Mag();
658  if (dist<minDist) {
659  minDist = dist;
660  minId = i;
661  }
662  }
663  }
664  else { // hill climbing algorithm
665  double distFirst = (pos-getSmoothedPosXYZ(trk, irep, 0)).Mag();
666  double distLast = (pos-getSmoothedPosXYZ(trk, irep, nHits-1)).Mag();
667  if (distFirst <= distLast){
668  minDist = distFirst;
669  minId = 0;
670  for (unsigned int i=1; i<nHits-1; ++i){
671  hitPos = getSmoothedPosXYZ(trk, irep, i);
672  dist = (pos-hitPos).Mag();
673  if (dist<=minDist) {
674  minDist = dist;
675  minId = i;
676  }
677  else break; // distance is increasing again!
678  }
679  }
680  else {
681  minDist = distLast;
682  minId = nHits-1;
683  for (unsigned int i=nHits-2; i>1; --i){
684  hitPos = getSmoothedPosXYZ(trk, irep, i);
685  dist = (pos-hitPos).Mag();
686  if (dist<=minDist) {
687  minDist = dist;
688  minId = i;
689  }
690  else break; // distance is increasing again!
691  }
692  }
693  }
694 
695  distance = minDist;
696  return minId;
697 }
698