LCFIVertex  0.7.2
trackstate.cpp
1 #include "../inc/trackstate.h"
2 
3 #include "../util/inc/helixrep.h"
4 #include "../util/inc/vector3.h"
5 #include "../util/inc/matrix.h"
6 #include "../inc/track.h"
7 #include <cmath>
8 
9 #define min(a,b) (((a)<(b))?(a):(b))
10 #define max(a,b) (((a)>(b))?(a):(b))
11 
12 namespace vertex_lcfi
13 {
14 
15  const double TrackState::_swimprecision = 0.0001; //.1 Micron
16 
17  TrackState::TrackState(Track* TTrack)
18  {
19  //TODO Check for invr==0?
20  if (TTrack != 0)
21  {
22  _ParentTrack=TTrack;
23  //Everything gets initalised by this call
24  this->resetToRef();
25  }
26  else
27  std::cerr << "Warning: Trackstate.cpp:28 Null Track Pointer" << std::endl;
28  }
29 
30  TrackState::TrackState(const HelixRep & init,const double & cha, const SymMatrix5x5 & cov, Track* const tra)
31  : _Charge(cha),_Init(init),_InitCovarMatrix(cov),_PosValid(0),_MomValid(0),_PositionCovarValid(0),_InversePositionCovarValid(0),_ParentTrack(tra)
32  {
33  //TODO Check for invr==0?
34  }
35 
36  void TrackState::swimDistance(const double s)
37  {
38  _DistanceSwum += s;
39  //std::cout << "Swum: " << s << " to " << _DistanceSwum << std::endl;
40  _PosValid = 0;
41  _MomValid = 0;
42  _InversePositionCovarValid = 0;
43  _PositionCovarValid = 0;
44  }
45 
47  {
48  if (this->isNeutral())
49  {
50  //P1 and P2 are points on the line (P2 in forward direction)
51  Vector3 P1 = this->position();
52  this->swimDistance(1);
53  Vector3 P2 = this->position();
54  this->swimDistance(-1);
55  Vector3 P3 = Point;
56 
57  //Parametric distance along line to perp to point
58  double u = (((P3.x()-P1.x())*(P2.x()-P1.x())) + ((P3.y()-P1.y())*(P2.y()-P1.y())) + ((P3.z()-P1.z())*(P2.z()-P1.z()))) / (P2.subtract(P1)).mag2();
59  this->swimDistance(u);
60  }
61 
62  if (this->isCharged())
63  {
64  //Use XY Nearest as starting point
65  this->swimToStateNearestXY(Point);
66  //Check we're not sitting on the point
67  if (this->distanceTo(Point)<_swimprecision) return;
68 
69  bool done;
70  short loopcount =0;
71  do
72  {
73  double s0 = _DistanceSwum;
74  Vector3 P = Point;
75  Vector3 initPos = this->position();
76  //Shift frame to the origin in xy plane
77  //Taylor expansion of d(Distancetopoint)/ds - finding roots finds minima and maxima
78  double invrval = -_Init.invR();
79  double c = (-2*(-(invrval*_Init.tanLambda()*(_Init.z0() - P.z() + s0*_Init.tanLambda())) + invrval*P.x()*cos(_Init.phi() + invrval*s0) + (-1.0 + invrval*-_Init.d0())*sin(invrval*s0) + invrval*P.y()*sin(_Init.phi() + invrval*s0)))/invrval;
80  double b = 2*(pow(_Init.tanLambda(),2) + (1.0 - invrval*-_Init.d0())*cos(invrval*s0) - invrval*P.y()*cos(_Init.phi() + invrval*s0) + invrval*P.x()*sin(_Init.phi() + invrval*s0));
81  double a = invrval*(invrval*P.x()*cos(_Init.phi() + invrval*s0) + (-1.0 + invrval*-_Init.d0())*sin(invrval*s0) + invrval*P.y()*sin(_Init.phi() + invrval*s0));
82 
83  //Get two solutions for s:
84  double s1,s2;
85  //Check a != 0
86  if (fabs(a)>std::numeric_limits<double>::epsilon())
87  {
88  double root = sqrt((b*b)-(4*a*c));
89  s1 = (-b + root)/(2*a);
90  s2 = (-b - root)/(2*a);
91  }
92  else
93  {
94  s1 = -c/b;
95  s2 = -c/b;
96  }
97  //std::cout << "s12: " << s1 <<" " <<s2<<std::endl;
98  double s1ToPoint,s2ToPoint;
99  //Swim to s1
100 #ifdef WIN32
101  if (_finite(s1) && fabs(s1) < 10000)
102 #else
103  if (std::isfinite(s1) && fabs(s1) < 10000)
104 #endif
105  {
106  this->swimDistance(s1);
107  s1ToPoint = this->distanceTo2(Point);
108  this->swimDistance(-s1);
109  }
110  else
111  {
112 #ifdef WIN32 //this template version could probably be used for both, but need to check first
113  s1ToPoint = std::numeric_limits<double>::infinity();
114 #else
115  s1ToPoint = INFINITY;
116 #endif
117  }
118 
119 #ifdef WIN32
120  if (_finite(s2) && fabs(s2) < 10000)
121 #else
122  if (std::isfinite(s2) && fabs(s2) < 10000)
123 #endif
124  {
125  this->swimDistance(s2);
126  s2ToPoint = this->distanceTo2(Point);
127  this->swimDistance(-s2);
128  }
129  else
130  {
131 #ifdef WIN32 //this template version could probably be used for both, but need to check first
132  s2ToPoint = std::numeric_limits<double>::infinity();
133 #else
134  s2ToPoint = INFINITY;
135 #endif
136  }
137 
138  if (std::isfinite(s1ToPoint) && std::isfinite(s2ToPoint))
139  {
140  if (s1ToPoint < s2ToPoint)
141  this->swimDistance(s1);
142  else
143  this->swimDistance(s2);
144  }
145  else
146  {
147  if (std::isfinite(s1ToPoint))
148  this->swimDistance(s1);
149  else
150  {
151  if (std::isfinite(s2ToPoint))
152  this->swimDistance(s1);
153  else
154  //Both Infinite give up
155  done = true;
156  }
157  }
158 
159  //If we moved a shorter distance than the precison so stop
160  done = (this->distanceTo(initPos) < _swimprecision);
161 
162  ++loopcount;
163  } while (!done && loopcount < 100);
164  if (loopcount == 100)
165  {
166  std::cerr << "Warning: TrackState.cpp:167:swimToStateNearest Max Iterations reached" << std::endl;
167  std::cerr << "Point " << Point << " Track:" << *this << std::endl;
168  }
169  }
170  //TODO Iterative fallback for non helical?
171  }
172 
173 
174  void TrackState::swimToStateNearest(TrackState* const TrackToSwimTo)
175  {
176  if (this->isNeutral() && TrackToSwimTo->isNeutral())
177  {
178  // Code for linear-linear PCA nicked from http://astronomy.swin.edu.au/~pbourke/geometry/lineline3d/
179  Vector3 p1 = this->position();
180  this->swimDistance(1);
181  Vector3 p2 = this->position();
182  this->swimDistance(-1);
183 
184  Vector3 p3 = TrackToSwimTo->position();
185  TrackToSwimTo->swimDistance(1);
186  Vector3 p4 = TrackToSwimTo->position();
187  TrackToSwimTo->swimDistance(-1);
188 
189  Vector3 p13,p43,p21;
190  double d1343,d4321,d1321,d4343,d2121;
191  double numer,denom;
192 
193  p13.x() = p1.x() - p3.x();
194  p13.y() = p1.y() - p3.y();
195  p13.z() = p1.z() - p3.z();
196  p43.x() = p4.x() - p3.x();
197  p43.y() = p4.y() - p3.y();
198  p43.z() = p4.z() - p3.z();
199  //TODO should the user know about this? Track isn't swum cos its either parallell or coincident with the other track.
200  if (fabs(p43.x()) < std::numeric_limits<double>::epsilon() && fabs(p43.y()) < std::numeric_limits<double>::epsilon() && fabs(p43.z()) < std::numeric_limits<double>::epsilon())
201  return ;
202  p21.x() = p2.x() - p1.x();
203  p21.y() = p2.y() - p1.y();
204  p21.z() = p2.z() - p1.z();
205  if (fabs(p21.x()) < std::numeric_limits<double>::epsilon() && fabs(p21.y()) < std::numeric_limits<double>::epsilon() && fabs(p21.z()) < std::numeric_limits<double>::epsilon())
206  return ;
207 
208  d1343 = p13.x() * p43.x() + p13.y() * p43.y() + p13.z() * p43.z();
209  d4321 = p43.x() * p21.x() + p43.y() * p21.y() + p43.z() * p21.z();
210  d1321 = p13.x() * p21.x() + p13.y() * p21.y() + p13.z() * p21.z();
211  d4343 = p43.x() * p43.x() + p43.y() * p43.y() + p43.z() * p43.z();
212  d2121 = p21.x() * p21.x() + p21.y() * p21.y() + p21.z() * p21.z();
213 
214  denom = d2121 * d4343 - d4321 * d4321;
215  if (fabs(denom) < std::numeric_limits<double>::epsilon())
216  return ;
217  numer = d1343 * d4321 - d1321 * d4343;
218 
219  this->swimDistance(numer / denom);
220  }
221 
222  //Iterative Fallback TODO - Can only do one cycle for helix helix
223  if (this->isCharged() && TrackToSwimTo->isCharged() || this->isCharged() && TrackToSwimTo->isNeutral() || this->isNeutral() && TrackToSwimTo->isCharged())
224  {
225  TrackState toswimto = *TrackToSwimTo;
226  toswimto.resetToRef();
227  this->resetToRef();
228 
229  int maxIters = 100;
230  double currentDelta=10.0;
231  double changeRate;
232  long int iterations=0;//how long it takes to find (for debug purposes)
233  do
234  {
235  do
236  {
237  iterations++;
238  //Find down hill vector, then make it the size of the current step
239  toswimto.swimToStateNearest(this->position());
240  double h2track = this->distanceTo2(toswimto.position());
241  double delta = (fabs(h2track)+1.0)*0.00001;
242  this->swimDistance(delta);
243  toswimto.swimToStateNearest(this->position());
244  double f2track = this->distanceTo2(toswimto.position());
245  this->swimDistance(-delta);
246 
247  changeRate = (f2track-h2track)/delta;
248  changeRate = changeRate/fabs(changeRate);
249  double currentStep = -changeRate*currentDelta;
250 
251  //If the step we are going to take takes us uphill then we have found a minimum so break the inner loop and go to higher precision
252  toswimto.swimToStateNearest(this->position());
253  h2track = this->distanceTo2(toswimto.position());
254  this->swimDistance(currentStep);
255  toswimto.swimToStateNearest(this->position());
256  f2track = this->distanceTo2(toswimto.position());
257  this->swimDistance(-currentStep);
258 
259  if (h2track<f2track) //Done
260  break;
261 
262  this->swimDistance(currentStep);
263 
264  } while (0!=changeRate && iterations < maxIters) ;
265  if (iterations == maxIters)
266  {
267  std::cerr << "Warning TrackState.cpp:278:_swimToStateNearest - Too many iterations" <<std::endl;
268  this->resetToRef();
269  std::cerr << *this << std::endl;
270  TrackToSwimTo->resetToRef();
271  std::cerr << *TrackToSwimTo << std::endl;
272 
273  }
274  currentDelta*=0.1;//now we have a min, look closer to find the exact min
275 
276  } while( currentDelta>_swimprecision );
277  }
278  }
279 
281  {
282  if (this->isNeutral())
283  {
284  {
285  //P1 and P2 are points on the line (P2 in forward direction)
286  Vector3 P1 = this->position();
287  this->swimDistance(1);
288  Vector3 P2 = this->position();
289  this->swimDistance(-1);
290  Vector3 P3 = Point;
291 
292  //Parametric distance along line to perp to point in xy plane
293  double u = (((P3.x()-P1.x())*(P2.x()-P1.x())) + ((P3.y()-P1.y())*(P2.y()-P1.y()))) / (((P2.x()-P1.x())*(P2.x()-P1.x()))+((P2.y()-P1.y())*(P2.y()-P1.y())));
294  //In 3D
295  //u = u/sin(_Init.theta());
296  //std::cout << "u:" << u << std::endl;
297 
298  this->swimDistance(u);
299  }
300  }
301 
302  if (this->isCharged())
303  {
304  //std::cout << "XY POCA" << std::endl;
305  this->resetToRef();
306  if (this->xyDistanceTo(Point)<_swimprecision) return;
307 
308  //So first find center of the circle in the XY plane
309  Vector3 center = Vector3(this->position().x()+ (sin(_Init.phi())/_Init.invR()) , this->position().y()- (cos(_Init.phi())/_Init.invR()),0);
310 
311  //Get the points vector from this circle center
312  Vector3 P2 = Point - center;
313 
314  //d distance to point on circle / ds solved for zero points gives:
315  double distanceRoundCircle = atan(P2.x()/P2.y())/_Init.invR();
316 
317  //This distance is relative to the top of the circle, wheras our refpoint is phi dependant
318  distanceRoundCircle = distanceRoundCircle + (_Init.phi()/_Init.invR());
319 
320  //Thats one of two possible minima, check which one we want, we reset earlier so we're swimming relative to the refpoint
321  this->swimDistance(distanceRoundCircle);
322  double dist1 = this->xyDistanceTo(Point);
323 
324  //Swim to the second point 180 degrees away
325  //where we started
326  this->swimDistance(_Init.halfCircum());
327  double dist2 = this->xyDistanceTo(Point);
328 
329  //If the first one was the best swim back to it.
330  if (dist1 < dist2) this->swimDistance(-_Init.halfCircum());
331 
332  //Some procedures use a z measurement from the XY POCA
333  //So we should be in the helix cycle nearest in z
334  //Work out how many cycles away we are and swim back the opposite
335  this->swimDistance(-_Init.circum() * round(this->position().subtract(Point).z() / _Init.zLength()));
336 
337  //std::cout << "Done - XY POCA" << std::endl;
338  //We're done
339  }
340  }
341 
342  double TrackState::chi2(const Vector3 &Point)
343  {
344  boost::numeric::ublas::bounded_vector<double,2> Residual;
345  //XY Dist in 2D
346  this->swimToStateNearestXY(Point);
347  Residual(0) = this->xyDistanceTo(Point);
348  //Z in 3D
349  this->swimToStateNearest(Point);
350  //The 3Ddist , 2Ddist and distance on z plane form a right triangle, convert to zaxis by dividing by sin theta
351  //Double check 3d is hypotenuse
352  //Our 2d might be shorter as it analytical and 3d is iterative so check within swimPrecision
353  if (this->distanceTo(Point) <= Residual(0))
354  {
355  if (fabs(this->distanceTo(Point)-Residual(0)) < _swimprecision)
356  {
357  //2D bigger than 3D, but within precison so set z = 0
358  Residual(1) = 0;
359 
360  }
361  else
362  {
363  std::cerr << "Warning trackstate.cpp:364:chi2 2D Distance to track was longer than 3D - swimming problem?" << std::endl;
364  }
365  }
366  else //Everything normal 3D>2D
367  {
368  //Residual(1) = (sqrt(this->distanceTo2(Point)-pow(Residual(0),2)))/(1.0/sqrt(1.0+pow(_Init.tanLambda(),2)));
369  Residual(1) = sqrt(this->distanceTo2(Point)-pow(Residual(0),2))*sqrt(pow(_Init.tanLambda(),2)+1.0);
370  }
371 
372  double chi2 = prec_inner_prod(trans(Residual),prec_prod(this->inversePositionCovarMatrix(), Residual));
373 
374  if (std::isnan(chi2))
375  {
376  std::cerr << "Warning trackstate.cpp:377 Chi2 is NAN, are your cov matrices ok?" << std::endl;
377  std::cerr << "3D: " << this->distanceTo(Point) << std::endl ;
378  std::cerr << "2D: " << Residual(0) << std::endl ;
379  std::cerr << "Res: " << Residual << std::endl ;
380  std::cerr << "Q: " << this->isCharged() << std::endl ;
381  std::cerr << "Err: " << this->inversePositionCovarMatrix()<< std::endl;
382  std::cerr << "Chi2: " << prec_inner_prod(trans(Residual),prec_prod(this->inversePositionCovarMatrix(), Residual))<< std::endl<< std::endl;
383  }
384 
385  return chi2;
386  }
387 
388  double TrackState::chi2_nomove(const Vector3 &Point)
389  {
390  boost::numeric::ublas::bounded_vector<double,2> Residual;
391  //XY Dist in 2D
392  //this->swimToStateNearestXY(Point);
393  Residual(0) = this->xyDistanceTo(Point);
394  //Z in 3D
395  //this->swimToStateNearest(Point);
396  //The 3Ddist , 2Ddist and distance on z plane form a right triangle, convert to zaxis by dividing by sin theta
397  //Double check 3d is hypotenuse
398  if (this->distanceTo(Point) <= Residual(0))
399  Residual(1) = 0;
400  else
401  {
402  //Residual(1) = (sqrt(this->distanceTo2(Point)-pow(Residual(0),2)))/(1.0/sqrt(1.0+pow(_Init.tanLambda(),2)));
403  Residual(1) = sqrt(this->distanceTo2(Point)-pow(Residual(0),2))*sqrt(pow(_Init.tanLambda(),2)+1.0);
404 
405  //std::cout << "3D: " << this->distanceTo(Point) << std::endl ;
406  //std::cout << "2D: " << Residual(0) << std::endl ;
407  //std::cout << "Res: " << Residual << std::endl ;
408  //std::cout << "Err: " << this->inversePositionCovarMatrix()<< std::endl;
409  //std::cout << "Chi2: " << prec_inner_prod(trans(Residual),prec_prod(this->inversePositionCovarMatrix(), Residual))<< std::endl<< std::endl;
410 
411  }
412  return prec_inner_prod(trans(Residual),prec_prod(this->inversePositionCovarMatrix(), Residual));
413  }
414  /*const Vector3 & TrackState::momentum() const
415  {
416  //TODO - Magnitude of momentum - from magfiled or initial track input?
417  if (!_MomValid)
418  {
419  if (this->isCharged())
420  {
421  _Momentum.x() = sin(_Init.theta()) * (cos(_Init.phi()-(_DistanceSwum*_Init.invR())));
422  _Momentum.y() = sin(_Init.theta()) * (sin(_Init.phi()-(_DistanceSwum*_Init.invR())));
423  _Momentum.z() = cos(_Init.theta());
424  _Momentum.makeUnit();
425  }
426  if (this->isNeutral())
427  {
428  _Momentum.x() = cos(-_Init.phi());
429  _Momentum.y() = sin(_Init.phi());
430  _Momentum.z() = cos(_Init.theta());
431  _Momentum.makeUnit();
432  }
433  _MomValid=1;
434  }
435 
436  return _Momentum;
437  }
438  */
439 
441  {
442  if (!_PosValid)
443  {
444  if (this->isCharged())
445  {
446  double phival = _Init.phi();
447  _Position.x() = -_Init.d0()*sin(phival) + (sin(phival)-sin(phival-_Init.invR()*_DistanceSwum))/_Init.invR();
448  // -d0 *Sin(phi) + (Sin(phi) -Sin(phi -invr *s ))/invr
449  _Position.y() = _Init.d0()*cos(phival) + (-cos(phival)+cos(phival-_Init.invR()*_DistanceSwum))/_Init.invR();
450  // d0 *Cos(phi) + (-Cos(phi) +Cos(phi -invr *s ))/invr
451  //_Position.x() = (_Init.d0()*sin(phival)) + ((sin(phival)-sin(phival-(_DistanceSwum*(-_Init.invR()))))/(-_Init.invR()));
452  //_Position.y() = (-_Init.d0()*cos(phival)) + ((cos(phival-(_DistanceSwum*(-_Init.invR())))-cos(phival))/(-_Init.invR()));
453  _Position.z() = (_DistanceSwum*_Init.tanLambda()) + _Init.z0();
454  //std::cout << "Cswim " << _DistanceSwum << " to " <<_Position << std::endl;
455 
456  //_Position.x() = (_Init.d0()*sin(phival)) + ((sin(phival)-sin(phival-(_DistanceSwum*_Init.invR())))/_Init.invR());
457  //_Position.y() = (-_Init.d0()*cos(phival)) + ((cos(phival-(_DistanceSwum*_Init.invR()))-cos(phival))/_Init.invR());
458  //_Position.z() = (_DistanceSwum/tan(_Init.theta())) + _Init.z0();
459  }
460  if (this->isNeutral()) //TODO Check z
461  {
462  _Position.x() = (_Init.d0()*sin(_Init.phi())) + (_DistanceSwum*cos(_Init.phi()));
463  _Position.y() = (-_Init.d0()*cos(_Init.phi())) + (_DistanceSwum*sin(_Init.phi()));
464  _Position.z() = (_DistanceSwum*_Init.tanLambda()) + _Init.z0();
465  //std::cout << "Nswim " << _DistanceSwum << " to " <<_Position << std::endl;
466  }
467  _PosValid = 1;
468  }
469  return _Position;
470  }
471 
473  {
474  //std::cout << "*";std::cout.flush();
475  //Copy from fortran for now
476 
477  //* calculate xy PCA in this system.
478  //Vector3 PCA;
479  //PCA.x() = -PARK(3)*SPHI
480  //PCA(2) = PARK(3)*CPHI
481  //PCA(3) = PARK(5)
482  //TL = PARK(4)
483  double STHE = 1.0/sqrt(1.0+pow(_Init.tanLambda(),2));
484  double CTHE = STHE*_Init.tanLambda();
485  //std::cout << STHE << " " << CTHE << std::endl;
486  //* calculate 3d PCA in this system.
487  //SS =-PCA(3)*CTHE
488  Vector3 PCA3;
489  //PCA3(1) = PCA(1) + SS*CPHI*STHE
490  //PCA3(2) = PCA(2) + SS*SPHI*STHE
491  //PCA3(3) = PCA(3) + SS*CTHE
492  PCA3 = this->position() - point;
493  //std::cout << PCA3 << std::endl;
494  //std::cout << "*";
495  //C check = pca3(1)*sthe*cphi+pca3(2)*sthe*sphi+pca3(3)*cthe
496  //PARK(1) = 0.0
497  //CALL UCOPY(COVK,COVJ,15*2)
498  //* calculate intersection of track with a plane perpendicular to the
499  //* track at s=0. The plane is defined by the equation
500  //* (x-x0)*tx + (y-y0)*ty + (z-z0)*tz = 0, where tx = sin(theta)*cos(phi),
501  //* ty = sin(theta)*sin(phi), and tz = cos(theta).
502  //* We use the fact that d0 and z0 are zero after the OUMOVE call.
503  //*
504  double DXDD = -sin(_Init.phi());
505  double DXDZ = -cos(_Init.phi())*STHE*CTHE;
506  double DYDD = cos(_Init.phi());
507  double DYDZ = -sin(_Init.phi())*STHE*CTHE;
508  double DZDZ = pow(STHE,2);
509  //*
510  //* calculate error matrix for this point. Must transform from track
511  //* parameter errors to x,y,z errors. Curvature is ignored. These
512  //* are the errors perpendicular to the track direction
513  //*
514  //std::cout << "*";std::cout.flush();
515  double EXYZ[6];
516  EXYZ[0] = pow(DXDD,2)*this->positionCovarMatrix()(0,0)+2.0*DXDD*DXDZ*this->positionCovarMatrix()(0,1)+ pow(DXDZ,2)*this->positionCovarMatrix()(1,1);
517  EXYZ[1] = DXDD*DYDD*this->positionCovarMatrix()(0,0)+ (DXDD*DYDZ+DXDZ*DYDD)*this->positionCovarMatrix()(0,1)+ DXDZ*DYDZ*this->positionCovarMatrix()(1,1);
518  EXYZ[2] = pow(DYDD,2)*this->positionCovarMatrix()(0,0)+ 2.0*DYDD*DYDZ*this->positionCovarMatrix()(0,1) + pow(DYDZ,2)*this->positionCovarMatrix()(1,1);
519  EXYZ[3] = DXDD*DZDZ*this->positionCovarMatrix()(0,1) + DXDZ*DZDZ*this->positionCovarMatrix()(1,1);
520  EXYZ[4] = DYDD*DZDZ*this->positionCovarMatrix()(0,1) + DYDZ*DZDZ*this->positionCovarMatrix()(1,1);
521  EXYZ[5] = pow(DZDZ,2)*this->positionCovarMatrix()(1,1);
522  //std::cout << "EXYZ: ";
523  //for (short i =0 ;i<6;++i)
524  // std::cout << EXYZ[i] <<" ";
525  //std::cout << std::endl;
526 
527  //* set up rotation matrix to take track into w direction by matrix
528  //* multiplication, vecnew_i = rot(i,j)vecold_j
529  Matrix3x3 ROTAT;
530  ROTAT(0,0)= CTHE*cos(_Init.phi());
531  ROTAT(0,1)= CTHE*sin(_Init.phi());
532  ROTAT(0,2)= -STHE;
533  ROTAT(1,0)= -sin(_Init.phi());
534  ROTAT(1,1)= cos(_Init.phi());
535  ROTAT(1,2)= 0.0;
536  ROTAT(2,0)= STHE*cos(_Init.phi());
537  ROTAT(2,1)= STHE*sin(_Init.phi());
538  ROTAT(2,2)= CTHE;
539  //* rotate track covariance into the new frame (uvw), where u and v are
540  //* perpendicular to the track and w is along the track
541  //* we only need calculate the 2x2 part; the rest is zero by construction
542  //* the u component of the matrix is the projected z0*sthe error
543  //* the v component is the d0 error
544  //std::cout << "*";std::cout.flush();
545  double EUV[6];
546  for (short i =0 ;i<6;++i)
547  EUV[i]=0;
548  for(short IIP = 1;IIP<3;++IIP)//DO 920 IIP=1,2
549  {
550  //std::cout << "IIP " << IIP<< std::endl;std::cout.flush();
551  for(short JJP = 1;JJP<IIP+1;++JJP)//DO 930 JJP=1,IIP
552  {
553  //std::cout << "\tJJP " << JJP<< std::endl;std::cout.flush();
554  short JLD = IIP*(IIP-1)/2 + JJP;
555  for(short KKP = 1;KKP<4;++KKP)//DO 940 KKP=1,3
556  {
557  //std::cout << "\t\tKKP " << KKP<< std::endl;std::cout.flush();
558  for(short LLP = 1;LLP<4;++LLP)//DO 950 LLP=1,3
559  {
560  //std::cout << "\t\t\tLLP " << LLP << std::endl;std::cout.flush();
561  short MINLD=min(KKP,LLP);
562  short MAXLD=max(KKP,LLP);
563  short ILD = MAXLD*(MAXLD-1)/2 + MINLD;
564  EUV[JLD-1]=EUV[JLD-1]+ROTAT(IIP-1,KKP-1)*EXYZ[ILD-1]*ROTAT(JJP-1,LLP-1);
565  }
566  }
567  }
568  }
569  double DETEUV = 1.0/(EUV[0]*EUV[2]-EUV[1]*EUV[1]);
570  //std::cout << "EUV: ";
571  //for (short i =0 ;i<6;++i)
572  // std::cout << EUV[i] <<" ";
573  //std::cout << std::endl;
574  //* calculate the miss distances in u and v coordinates; we need the
575  //* 3d miss here
576  //double U = ROTAT(0,0)*PCA3.x()+ ROTAT(0,1)*PCA3.y()+ ROTAT(0,2)*PCA3.z();
577  //double V = ROTAT(1,0)*PCA3.x()+ ROTAT(1,1)*PCA3.y()+ ROTAT(1,2)*PCA3.z();
578  //std::cout << "U,V:" << U << " " << V << std::endl;
579  //C check=rotat(3,1)*pca3(1)+rotat(3,2)*pca3(2)+rotat(3,3)*pca3(3)
580  //C
581  //C write(6,*) 'OU3FIT',iter,' u,v,euv ',u,v,euv(1),euv(2),euv(3)
582  //std::cout << "*";std::cout.flush();
583  //* chi**2 calculation
584  //CHT(JJ) = (U*U*EUV(3)-2.D0*U*V*EUV(2)+V*V*EUV(1))*DETEUV
585  //CHIAVE = CHIAVE + CHT(JJ)
586  //* dU/dX, dU/dY, dU/dZ; dV/dX, dV/dY, dV/dZ; are merely the
587  //* rotation matrix elements. The chi**2 we differentiate w.r.t. x,y,z
588  //* is the sum of (u_i,v_i) (EUV_i)^-1 (u_i,v_i)
589  //G(1) = G(1) + DETEUV*(ROTAT(1,1)*( U*EUV(3)-V*EUV(2))
590  //+ +ROTAT(2,1)*(-U*EUV(2)+V*EUV(1)))
591  //G(2) = G(2) + DETEUV*(ROTAT(1,2)*( U*EUV(3)-V*EUV(2))
592  //+ +ROTAT(2,2)*(-U*EUV(2)+V*EUV(1)))
593  //G(3) = G(3) + DETEUV*(ROTAT(1,3)*( U*EUV(3)-V*EUV(2))
594  //+ +ROTAT(2,3)*(-U*EUV(2)+V*EUV(1)))
595  //std::cout << "*";std::cout.flush();
596  SymMatrix3x3 GG;
597  GG.clear();
598  GG(0,0)=GG(0,0)+DETEUV*(ROTAT(0,0)*ROTAT(0,0)*EUV[2]-1.0*ROTAT(0,0)*ROTAT(1,0)*EUV[1]+ROTAT(1,0)*ROTAT(1,0)*EUV[0]);
599  GG(1,0)=GG(1,0)+DETEUV*(ROTAT(0,0)*ROTAT(0,1)*EUV[2]-ROTAT(0,0)*ROTAT(1,1)*EUV[1]-ROTAT(1,0)*ROTAT(0,1)*EUV[1]+ROTAT(1,0)*ROTAT(1,1)*EUV[0]);
600  GG(1,1)=GG(1,1)+DETEUV*(ROTAT(0,1)*ROTAT(0,1)*EUV[2]-1.0*ROTAT(0,1)*ROTAT(1,1)*EUV[1]+ROTAT(1,1)*ROTAT(1,1)*EUV[0]);
601  GG(2,0)=GG(2,0)+DETEUV*(ROTAT(0,0)*ROTAT(0,2)*EUV[2]-ROTAT(0,0)*ROTAT(1,2)*EUV[1]-ROTAT(1,0)*ROTAT(0,2)*EUV[1]+ROTAT(1,0)*ROTAT(1,2)*EUV[0]);
602  GG(2,1)=GG(2,1)+DETEUV*(ROTAT(0,1)*ROTAT(0,2)*EUV[2]-ROTAT(0,1)*ROTAT(1,2)*EUV[1]-ROTAT(1,1)*ROTAT(0,2)*EUV[1]+ROTAT(1,1)*ROTAT(1,2)*EUV[0]);
603  GG(2,2)=GG(2,2)+DETEUV*(ROTAT(0,2)*ROTAT(0,2)*EUV[2]-1.0*ROTAT(0,2)*ROTAT(1,2)*EUV[1]+ROTAT(1,2)*ROTAT(1,2)*EUV[0]);
604  return GG;
605  }
606 
608  {
609  if(!_PositionCovarValid)
610  {
611 // double s = _DistanceSwum;
612  /*double factor1 = -pow(_V(1,4),2) + _V(1,1)*_V(4,4) + s*sin(_Init.theta())*(2*_V(1,4)*_V(2,4) - 2*_V(1,2)*_V(4,4) + s*(-pow(_V(2,4),2) + _V(2,2)*_V(4,4))*sin(_Init.theta()));
613  double factor2 = pow(_V(1,4),2) - _V(1,1)*_V(4,4) + s*sin(_Init.theta())*(-2*_V(1,4)*_V(2,4) + 2*_V(1,2)*_V(4,4) + s*(pow(_V(2,4),2) - _V(2,2)*_V(4,4))*sin(_Init.theta()));
614  //std::cout << "V" << _V << std::endl;
615  //std::cout << "f1,2: " << factor1 << " " << factor2 << std::endl;
616  _InversePositionCovarMatrix(0,0) = (pow(_Init.invR(),4)*(_V(1,1) + s*sin(_Init.theta())*(-2*_V(1,2) + s*_V(2,2)*sin(_Init.theta()))))/factor1;
617  _InversePositionCovarMatrix(0,1) = (pow(_Init.invR(),2)*(-_V(1,4) + s*_V(2,4)*sin(_Init.theta())))/factor2;
618  //(1,0) filled by symmettry
619  _InversePositionCovarMatrix(1,1) = _V(4,4)/factor1;
620  */
621  /*double factor = -pow(pow(_Init.invR(),2)*_V(0,1) - _V(1,4) + s*(-(pow(_Init.invR(),2)*_V(0,2)) + _V(2,4))*sin(_Init.theta()),2) + (pow(_Init.invR(),4)*_V(0,0) - 2*pow(_Init.invR(),2)*_V(0,4) + _V(4,4))*(_V(1,1) + s*sin(_Init.theta())*(-2*_V(1,2) + s*_V(2,2)*sin(_Init.theta())));
622  _InversePositionCovarMatrix(0,0) = (pow(_Init.invR(),4)*(_V(1,1) + s*sin(_Init.theta())*(-2*_V(1,2) + s*_V(2,2)*sin(_Init.theta()))))/factor;
623  _InversePositionCovarMatrix(0,1) = (pow(_Init.invR(),4)*(-_V(0,1) + _V(1,4)/pow(_Init.invR(),2) + s*(_V(0,2) - _V(2,4)/pow(_Init.invR(),2))*sin(_Init.theta())))/factor;
624  //(1,0) filled by symmettry
625  _InversePositionCovarMatrix(1,1) = (pow(_Init.invR(),4)*(_V(0,0) + (-2*pow(_Init.invR(),2)*_V(0,4) + _V(4,4))/pow(_Init.invR(),4)))/factor;
626  */
627  //No propogation below
628  _PositionCovarMatrix(0,0) = _InitCovarMatrix(0,0);
629  _PositionCovarMatrix(0,1) = _InitCovarMatrix(0,3);
630  _PositionCovarMatrix(1,1) = _InitCovarMatrix(3,3);
631  _PositionCovarValid = 1;
632  }
633  return _PositionCovarMatrix;
634  }
635 
637  {
638  if(!_InversePositionCovarValid)
639  {
640  if(!_PositionCovarValid) this->positionCovarMatrix();
641 
642  double factor = pow(_PositionCovarMatrix(0,1),2) - _PositionCovarMatrix(0,0)*_PositionCovarMatrix(1,1);
643  _InversePositionCovarMatrix(0,0) = _PositionCovarMatrix(1,1)/-factor;
644  _InversePositionCovarMatrix(0,1) = _PositionCovarMatrix(0,1)/factor;
645  _InversePositionCovarMatrix(1,1) = _PositionCovarMatrix(0,0)/-factor;
646  _InversePositionCovarValid = 1;
647  }
648  return _InversePositionCovarMatrix;
649  }
650 
651  /*const SymMatrix3x3& TrackState::positionCovarMatrixXYZ() const
652  {
653  if(!_PositionCovarValidXYZ)
654  {
655  }
656  return _PositionCovarMatrixXYZ;
657  }
658 
659  const SymMatrix3x3& TrackState::inversePositionCovarMatrixXYZ() const
660  {
661  if(!_InversePositionCovarValidXYZ)
662  {
663  _InversePositionCovarMatrixXYZ = InvertMatrix(this->positionCovarMatrix());
664  }
665  return _InversePositionCovarMatrixXYZ;
666  }
667  */
668  /*const SymMatrix5x5 & TrackState::covarianceMatrix() const
669  {
670  if (!_CovarianceValid)
671  {
672  _D.clear();
673  //Diagonal terms
674  for (short i=0;i<5;i++)
675  _D(i,i) = 1.0;
676  // Matrix is in order d0,z0,phi,invr,theta
677  // rows = new, cols = old
678  //Dodgy
679  //d d0/d phi
680  _D(0,2) = _DistanceSwum;
681  //d d0/d invr
682  if (this->isCharged()) _D(0,3) = _DistanceSwum;
683  //d z0/d theta
684  _D(1,4) = -_DistanceSwum * sin(_Init.theta());
685  _CovarianceMatrix = prec_prod(_D,(Matrix5x5(prec_prod(_InitCovarianceMatrix,trans(_D)))));
686  _CovarianceValid = 1;
687  }
688  return _CovarianceMatrix;
689  }
690  */
692  {
693  std::cout << "Pos:" << this->position().x() << "," << this->position().y() << "," << this->position().z() << std::endl;
694 // std::cout << "Mom:" << this->momentum().x() << "," << this->momentum().y() << "," << this->momentum().z() << std::endl;
695  std::cout << "Cha:" << this->charge() << std::endl;
696  std::cout << "Par:" << this->parentTrack() << std::endl;
697  std::cout << "Sws:" << _DistanceSwum << std::endl;
698  std::cout.flush();
699  }
700 
702  {
703  _DistanceSwum = 0;
704  _Init=_ParentTrack->helixRep();
705  _Charge=_ParentTrack->charge();
706  _InitCovarMatrix=_ParentTrack->covarianceMatrix();
707 
708  _PosValid = 0;
709  _MomValid = 0;
710  _InversePositionCovarValid = 0;
711  _PositionCovarValid = 0;
712  }
713 }
void resetToRef()
Reset the track to distance swum = 0.
Definition: trackstate.cpp:701
Track * parentTrack() const
The Track that this TrackState belongs to, (if any)
const SymMatrix2x2 & positionCovarMatrix() const
Current position covariance matrix of the trackstate (d0,z0)
Definition: trackstate.cpp:607
const SymMatrix5x5 & covarianceMatrix() const
Covariance Matrix.
Definition: track.cpp:54
void swimToStateNearestXY(const Vector3 &Point)
Swim to the point of closest approach in the XY plane to Point.
Definition: trackstate.cpp:280
double distanceTo(const Vector3 &Point) const
Distance from the TrackStates current position to Point.
const SymMatrix2x2 & inversePositionCovarMatrix() const
Current inverse position covariance matrix of the trackstate (d0,z0)
Definition: trackstate.cpp:636
double xyDistanceTo(const Vector3 &Point) const
Distance in the XY plane from the TrackStates current position to Point.
double chi2_nomove(const Vector3 &Point)
Calculate this tracks chi squared to Point at the TrackStates current position.
Definition: trackstate.cpp:388
double distanceTo2(const Vector3 &Point) const
Distance squared from the TrackStates current position to Point.
bool isNeutral() const
Is this track neutral.
double chi2(const Vector3 &Point)
Calculate this tracks minimum chi squared to Point.
Definition: trackstate.cpp:342
Unique Track representation.
void swimDistance(const double s)
Swim the track a fixed distance.
Definition: trackstate.cpp:36
const HelixRep & helixRep() const
Helix represenation of this track.
Definition: track.cpp:35
void debugOut()
Print some info to std::cout.
Definition: trackstate.cpp:691
double charge() const
Track charge.
Definition: track.cpp:44
bool isCharged() const
Is this track charged.
const Matrix3x3 vertexErrorContribution(Vector3 point) const
The error contribution of this trackstate to a vertex at point.
Definition: trackstate.cpp:472
const Vector3 & position() const
Current position of the trackstate.
Definition: trackstate.cpp:440
void swimToStateNearest(const Vector3 &Point)
Swim to the point of closest approach to Point.
Definition: trackstate.cpp:46