GENFIT  Rev:NoNumberAvailable
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules
RKTrackRep.cc
Go to the documentation of this file.
1 /* Copyright 2008-2013, Technische Universitaet Muenchen, Ludwig-Maximilians-Universität München
2  Authors: Christian Hoeppner & Sebastian Neubert & Johannes Rauch & Tobias Schlüter
3 
4  This file is part of GENFIT.
5 
6  GENFIT is free software: you can redistribute it and/or modify
7  it under the terms of the GNU Lesser General Public License as published
8  by the Free Software Foundation, either version 3 of the License, or
9  (at your option) any later version.
10 
11  GENFIT is distributed in the hope that it will be useful,
12  but WITHOUT ANY WARRANTY; without even the implied warranty of
13  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  GNU Lesser General Public License for more details.
15 
16  You should have received a copy of the GNU Lesser General Public License
17  along with GENFIT. If not, see <http://www.gnu.org/licenses/>.
18 */
19 
20 #include "RKTrackRep.h"
21 #include "IO.h"
22 
23 #include <Exception.h>
24 #include <FieldManager.h>
25 #include <MaterialEffects.h>
26 #include <MeasuredStateOnPlane.h>
27 #include <MeasurementOnPlane.h>
28 
29 #include <TBuffer.h>
30 #include <TDecompLU.h>
31 #include <TMath.h>
32 
33 #include <algorithm>
34 
35 #define MINSTEP 0.001 // minimum step [cm] for Runge Kutta and iteration to POCA
36 
37 namespace {
38  // Use fast inversion instead of LU decomposition?
39  const bool useInvertFast = false;
40 }
41 
42 namespace genfit {
43 
44 
46  AbsTrackRep(),
47  lastStartState_(this),
48  lastEndState_(this),
49  RKStepsFXStart_(0),
50  RKStepsFXStop_(0),
51  fJacobian_(5,5),
52  fNoise_(5),
53  useCache_(false),
54  cachePos_(0)
55 {
56  initArrays();
57 }
58 
59 
60 RKTrackRep::RKTrackRep(int pdgCode, char propDir) :
61  AbsTrackRep(pdgCode, propDir),
62  lastStartState_(this),
63  lastEndState_(this),
64  RKStepsFXStart_(0),
65  RKStepsFXStop_(0),
66  fJacobian_(5,5),
67  fNoise_(5),
68  useCache_(false),
69  cachePos_(0)
70 {
71  initArrays();
72 }
73 
74 
76  ;
77 }
78 
79 
81  const SharedPlanePtr& plane,
82  bool stopAtBoundary,
83  bool calcJacobianNoise) const {
84 
85  if (debugLvl_ > 0) {
86  debugOut << "RKTrackRep::extrapolateToPlane()\n";
87  }
88 
89 
90  if (state.getPlane() == plane) {
91  if (debugLvl_ > 0) {
92  debugOut << "state is already defined at plane. Do nothing! \n";
93  }
94  return 0;
95  }
96 
97  checkCache(state, &plane);
98 
99  // to 7D
100  M1x7 state7 = {{0, 0, 0, 0, 0, 0, 0}};
101  getState7(state, state7);
102 
103  TMatrixDSym* covPtr(NULL);
104  bool fillExtrapSteps(false);
105  if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
106  covPtr = &(static_cast<MeasuredStateOnPlane*>(&state)->getCov());
107  fillExtrapSteps = true;
108  }
109  else if (calcJacobianNoise)
110  fillExtrapSteps = true;
111 
112  // actual extrapolation
113  bool isAtBoundary(false);
114  double flightTime( 0. );
115  double coveredDistance( Extrap(*(state.getPlane()), *plane, getCharge(state), getMass(state), isAtBoundary, state7, flightTime, fillExtrapSteps, covPtr, false, stopAtBoundary) );
116 
117  if (stopAtBoundary && isAtBoundary) {
118  state.setPlane(SharedPlanePtr(new DetPlane(TVector3(state7[0], state7[1], state7[2]),
119  TVector3(state7[3], state7[4], state7[5]))));
120  }
121  else {
122  state.setPlane(plane);
123  }
124 
125  // back to 5D
126  getState5(state, state7);
127  setTime(state, getTime(state) + flightTime);
128 
129  lastEndState_ = state;
130 
131  return coveredDistance;
132 }
133 
134 
136  const TVector3& linePoint,
137  const TVector3& lineDirection,
138  bool stopAtBoundary,
139  bool calcJacobianNoise) const {
140 
141  if (debugLvl_ > 0) {
142  debugOut << "RKTrackRep::extrapolateToLine()\n";
143  }
144 
145  checkCache(state, NULL);
146 
147  static const unsigned int maxIt(1000);
148 
149  // to 7D
150  M1x7 state7;
151  getState7(state, state7);
152 
153  bool fillExtrapSteps(false);
154  if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
155  fillExtrapSteps = true;
156  }
157  else if (calcJacobianNoise)
158  fillExtrapSteps = true;
159 
160  double step(0.), lastStep(0.), maxStep(1.E99), angle(0), distToPoca(0), tracklength(0);
161  double charge = getCharge(state);
162  double mass = getMass(state);
163  double flightTime = 0;
164  TVector3 dir(state7[3], state7[4], state7[5]);
165  TVector3 lastDir(0,0,0);
166  TVector3 poca, poca_onwire;
167  bool isAtBoundary(false);
168 
169  DetPlane startPlane(*(state.getPlane()));
170  SharedPlanePtr plane(new DetPlane(linePoint, dir.Cross(lineDirection), lineDirection));
171  unsigned int iterations(0);
172 
173  while(true){
174  if(++iterations == maxIt) {
175  Exception exc("RKTrackRep::extrapolateToLine ==> extrapolation to line failed, maximum number of iterations reached",__LINE__,__FILE__);
176  exc.setFatal();
177  throw exc;
178  }
179 
180  lastStep = step;
181  lastDir = dir;
182 
183  step = this->Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime, false, NULL, true, stopAtBoundary, maxStep);
184  tracklength += step;
185 
186  dir.SetXYZ(state7[3], state7[4], state7[5]);
187  poca.SetXYZ(state7[0], state7[1], state7[2]);
188  poca_onwire = pocaOnLine(linePoint, lineDirection, poca);
189 
190  // check break conditions
191  if (stopAtBoundary && isAtBoundary) {
192  plane->setON(dir, poca);
193  break;
194  }
195 
196  angle = fabs(dir.Angle((poca_onwire-poca))-TMath::PiOver2()); // angle between direction and connection to point - 90 deg
197  distToPoca = (poca_onwire-poca).Mag();
198  if (angle*distToPoca < 0.1*MINSTEP) break;
199 
200  // if lastStep and step have opposite sign, the real normal vector lies somewhere between the last two normal vectors (i.e. the directions)
201  // -> try mean value of the two (normalization not needed)
202  if (lastStep*step < 0){
203  dir += lastDir;
204  maxStep = 0.5*fabs(lastStep); // make it converge!
205  }
206 
207  startPlane = *plane;
208  plane->setU(dir.Cross(lineDirection));
209  }
210 
211  if (fillExtrapSteps) { // now do the full extrapolation with covariance matrix
212  // make use of the cache
213  lastEndState_.setPlane(plane);
214  getState5(lastEndState_, state7);
215 
216  tracklength = extrapolateToPlane(state, plane, false, true);
217  lastEndState_.getAuxInfo()(1) = state.getAuxInfo()(1); // Flight time
218  }
219  else {
220  state.setPlane(plane);
221  getState5(state, state7);
222  state.getAuxInfo()(1) += flightTime;
223  }
224 
225  if (debugLvl_ > 0) {
226  debugOut << "RKTrackRep::extrapolateToLine(): Reached POCA after " << iterations+1 << " iterations. Distance: " << (poca_onwire-poca).Mag() << " cm. Angle deviation: " << dir.Angle((poca_onwire-poca))-TMath::PiOver2() << " rad \n";
227  }
228 
229  lastEndState_ = state;
230 
231  return tracklength;
232 }
233 
234 
236  const TVector3& point,
237  const TMatrixDSym* G,
238  bool stopAtBoundary,
239  bool calcJacobianNoise) const {
240 
241  if (debugLvl_ > 0) {
242  debugOut << "RKTrackRep::extrapolateToPoint()\n";
243  }
244 
245  checkCache(state, NULL);
246 
247  static const unsigned int maxIt(1000);
248 
249  // to 7D
250  M1x7 state7;
251  getState7(state, state7);
252 
253  bool fillExtrapSteps(false);
254  if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
255  fillExtrapSteps = true;
256  }
257  else if (calcJacobianNoise)
258  fillExtrapSteps = true;
259 
260  double step(0.), lastStep(0.), maxStep(1.E99), angle(0), distToPoca(0), tracklength(0);
261  TVector3 dir(state7[3], state7[4], state7[5]);
262  if (G != NULL) {
263  if(G->GetNrows() != 3) {
264  Exception exc("RKTrackRep::extrapolateToLine ==> G is not 3x3",__LINE__,__FILE__);
265  exc.setFatal();
266  throw exc;
267  }
268  dir = TMatrix(*G) * dir;
269  }
270  TVector3 lastDir(0,0,0);
271 
272  TVector3 poca;
273  bool isAtBoundary(false);
274 
275  DetPlane startPlane(*(state.getPlane()));
276  SharedPlanePtr plane(new DetPlane(point, dir));
277  unsigned int iterations(0);
278  double charge = getCharge(state);
279  double mass = getMass(state);
280  double flightTime = 0;
281 
282  while(true){
283  if(++iterations == maxIt) {
284  Exception exc("RKTrackRep::extrapolateToPoint ==> extrapolation to point failed, maximum number of iterations reached",__LINE__,__FILE__);
285  exc.setFatal();
286  throw exc;
287  }
288 
289  lastStep = step;
290  lastDir = dir;
291 
292  step = this->Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime, false, NULL, true, stopAtBoundary, maxStep);
293  tracklength += step;
294 
295  dir.SetXYZ(state7[3], state7[4], state7[5]);
296  if (G != NULL) {
297  dir = TMatrix(*G) * dir;
298  }
299  poca.SetXYZ(state7[0], state7[1], state7[2]);
300 
301  // check break conditions
302  if (stopAtBoundary && isAtBoundary) {
303  plane->setON(dir, poca);
304  break;
305  }
306 
307  angle = fabs(dir.Angle((point-poca))-TMath::PiOver2()); // angle between direction and connection to point - 90 deg
308  distToPoca = (point-poca).Mag();
309  if (angle*distToPoca < 0.1*MINSTEP) break;
310 
311  // if lastStep and step have opposite sign, the real normal vector lies somewhere between the last two normal vectors (i.e. the directions)
312  // -> try mean value of the two
313  if (lastStep*step < 0){
314  if (G != NULL) { // after multiplication with G, dir has not length 1 anymore in general
315  dir.SetMag(1.);
316  lastDir.SetMag(1.);
317  }
318  dir += lastDir;
319  maxStep = 0.5*fabs(lastStep); // make it converge!
320  }
321 
322  startPlane = *plane;
323  plane->setNormal(dir);
324  }
325 
326  if (fillExtrapSteps) { // now do the full extrapolation with covariance matrix
327  // make use of the cache
328  lastEndState_.setPlane(plane);
329  getState5(lastEndState_, state7);
330 
331  tracklength = extrapolateToPlane(state, plane, false, true);
332  lastEndState_.getAuxInfo()(1) = state.getAuxInfo()(1); // Flight time
333  }
334  else {
335  state.setPlane(plane);
336  getState5(state, state7);
337  state.getAuxInfo()(1) += flightTime;
338  }
339 
340 
341  if (debugLvl_ > 0) {
342  debugOut << "RKTrackRep::extrapolateToPoint(): Reached POCA after " << iterations+1 << " iterations. Distance: " << (point-poca).Mag() << " cm. Angle deviation: " << dir.Angle((point-poca))-TMath::PiOver2() << " rad \n";
343  }
344 
345  lastEndState_ = state;
346 
347  return tracklength;
348 }
349 
350 
352  double radius,
353  const TVector3& linePoint,
354  const TVector3& lineDirection,
355  bool stopAtBoundary,
356  bool calcJacobianNoise) const {
357 
358  if (debugLvl_ > 0) {
359  debugOut << "RKTrackRep::extrapolateToCylinder()\n";
360  }
361 
362  checkCache(state, NULL);
363 
364  static const unsigned int maxIt(1000);
365 
366  // to 7D
367  M1x7 state7;
368  getState7(state, state7);
369 
370  bool fillExtrapSteps(false);
371  if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
372  fillExtrapSteps = true;
373  }
374  else if (calcJacobianNoise)
375  fillExtrapSteps = true;
376 
377  double tracklength(0.), maxStep(1.E99);
378 
379  TVector3 dest, pos, dir;
380 
381  bool isAtBoundary(false);
382 
383  DetPlane startPlane(*(state.getPlane()));
384  SharedPlanePtr plane(new DetPlane());
385  unsigned int iterations(0);
386  double charge = getCharge(state);
387  double mass = getMass(state);
388  double flightTime = 0;
389 
390  while(true){
391  if(++iterations == maxIt) {
392  Exception exc("RKTrackRep::extrapolateToCylinder ==> maximum number of iterations reached",__LINE__,__FILE__);
393  exc.setFatal();
394  throw exc;
395  }
396 
397  pos.SetXYZ(state7[0], state7[1], state7[2]);
398  dir.SetXYZ(state7[3], state7[4], state7[5]);
399 
400  // solve quadratic equation
401  TVector3 AO = (pos - linePoint);
402  TVector3 AOxAB = (AO.Cross(lineDirection));
403  TVector3 VxAB = (dir.Cross(lineDirection));
404  float ab2 = (lineDirection * lineDirection);
405  float a = (VxAB * VxAB);
406  float b = 2 * (VxAB * AOxAB);
407  float c = (AOxAB * AOxAB) - (radius*radius * ab2);
408  double arg = b*b - 4.*a*c;
409  if(arg < 0) {
410  Exception exc("RKTrackRep::extrapolateToCylinder ==> cannot solve",__LINE__,__FILE__);
411  exc.setFatal();
412  throw exc;
413  }
414  double term = sqrt(arg);
415  double k1, k2;
416  if (b<0) {
417  k1 = (-b + term)/(2.*a);
418  k2 = 2.*c/(-b + term);
419  }
420  else {
421  k1 = 2.*c/(-b - term);
422  k2 = (-b - term)/(2.*a);
423  }
424 
425  // select smallest absolute solution -> closest cylinder surface
426  double k = k1;
427  if (fabs(k2)<fabs(k))
428  k = k2;
429 
430  if (debugLvl_ > 0) {
431  debugOut << "RKTrackRep::extrapolateToCylinder(); k = " << k << "\n";
432  }
433 
434  dest = pos + k * dir;
435 
436  plane->setO(dest);
437  plane->setUV((dest-linePoint).Cross(lineDirection), lineDirection);
438 
439  tracklength += this->Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime, false, NULL, true, stopAtBoundary, maxStep);
440 
441  // check break conditions
442  if (stopAtBoundary && isAtBoundary) {
443  pos.SetXYZ(state7[0], state7[1], state7[2]);
444  dir.SetXYZ(state7[3], state7[4], state7[5]);
445  plane->setO(pos);
446  plane->setUV((pos-linePoint).Cross(lineDirection), lineDirection);
447  break;
448  }
449 
450  if(fabs(k)<MINSTEP) break;
451 
452  startPlane = *plane;
453 
454  }
455 
456  if (fillExtrapSteps) { // now do the full extrapolation with covariance matrix
457  // make use of the cache
458  lastEndState_.setPlane(plane);
459  getState5(lastEndState_, state7);
460 
461  tracklength = extrapolateToPlane(state, plane, false, true);
462  lastEndState_.getAuxInfo()(1) = state.getAuxInfo()(1); // Flight time
463  }
464  else {
465  state.setPlane(plane);
466  getState5(state, state7);
467  state.getAuxInfo()(1) += flightTime;
468  }
469 
470  lastEndState_ = state;
471 
472  return tracklength;
473 }
474 
475 
477  double openingAngle,
478  const TVector3& conePoint,
479  const TVector3& coneDirection,
480  bool stopAtBoundary,
481  bool calcJacobianNoise) const {
482 
483  if (debugLvl_ > 0) {
484  debugOut << "RKTrackRep::extrapolateToCone()\n";
485  }
486 
487  checkCache(state, NULL);
488 
489  static const unsigned int maxIt(1000);
490 
491  // to 7D
492  M1x7 state7;
493  getState7(state, state7);
494 
495  bool fillExtrapSteps(false);
496  if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
497  fillExtrapSteps = true;
498  }
499  else if (calcJacobianNoise)
500  fillExtrapSteps = true;
501 
502  double tracklength(0.), maxStep(1.E99);
503 
504  TVector3 dest, pos, dir;
505 
506  bool isAtBoundary(false);
507 
508  DetPlane startPlane(*(state.getPlane()));
509  SharedPlanePtr plane(new DetPlane());
510  unsigned int iterations(0);
511  double charge = getCharge(state);
512  double mass = getMass(state);
513  double flightTime = 0;
514 
515  while(true){
516  if(++iterations == maxIt) {
517  Exception exc("RKTrackRep::extrapolateToCone ==> maximum number of iterations reached",__LINE__,__FILE__);
518  exc.setFatal();
519  throw exc;
520  }
521 
522  pos.SetXYZ(state7[0], state7[1], state7[2]);
523  dir.SetXYZ(state7[3], state7[4], state7[5]);
524 
525  // solve quadratic equation a k^2 + 2 b k + c = 0
526  // a = (U . D)^2 - cos^2 alpha * U^2
527  // b = (Delta . D) * (U . D) - cos^2 alpha * (U . Delta)
528  // c = (Delta . D)^2 - cos^2 alpha * Delta^2
529  // Delta = P - V, P track point, U track direction, V cone point, D cone direction, alpha opening angle of cone
530  TVector3 cDirection = coneDirection.Unit();
531  TVector3 Delta = (pos - conePoint);
532  double DirDelta = cDirection * Delta;
533  double Delta2 = Delta*Delta;
534  double UDir = dir * cDirection;
535  double UDelta = dir * Delta;
536  double U2 = dir * dir;
537  double cosAngle2 = cos(openingAngle)*cos(openingAngle);
538  double a = UDir*UDir - cosAngle2*U2;
539  double b = UDir*DirDelta - cosAngle2*UDelta;
540  double c = DirDelta*DirDelta - cosAngle2*Delta2;
541 
542  double arg = b*b - a*c;
543  if(arg < -1e-9) {
544  Exception exc("RKTrackRep::extrapolateToCone ==> cannot solve",__LINE__,__FILE__);
545  exc.setFatal();
546  throw exc;
547  } else if(arg < 0) {
548  arg = 0;
549  }
550 
551  double term = sqrt(arg);
552  double k1, k2;
553  k1 = (-b + term) / a;
554  k2 = (-b - term) / a;
555 
556  // select smallest absolute solution -> closest cone surface
557  double k = k1;
558  if(fabs(k2) < fabs(k)) {
559  k = k2;
560  }
561 
562  if (debugLvl_ > 0) {
563  debugOut << "RKTrackRep::extrapolateToCone(); k = " << k << "\n";
564  }
565 
566  dest = pos + k * dir;
567  // debugOut << "In cone extrapolation ";
568  // dest.Print();
569 
570  plane->setO(dest);
571  plane->setUV((dest-conePoint).Cross(coneDirection), dest-conePoint);
572 
573  tracklength += this->Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime, false, NULL, true, stopAtBoundary, maxStep);
574 
575  // check break conditions
576  if (stopAtBoundary && isAtBoundary) {
577  pos.SetXYZ(state7[0], state7[1], state7[2]);
578  dir.SetXYZ(state7[3], state7[4], state7[5]);
579  plane->setO(pos);
580  plane->setUV((pos-conePoint).Cross(coneDirection), pos-conePoint);
581  break;
582  }
583 
584  if(fabs(k)<MINSTEP) break;
585 
586  startPlane = *plane;
587 
588  }
589 
590  if (fillExtrapSteps) { // now do the full extrapolation with covariance matrix
591  // make use of the cache
592  lastEndState_.setPlane(plane);
593  getState5(lastEndState_, state7);
594 
595  tracklength = extrapolateToPlane(state, plane, false, true);
596  lastEndState_.getAuxInfo()(1) = state.getAuxInfo()(1); // Flight time
597  }
598  else {
599  state.setPlane(plane);
600  getState5(state, state7);
601  state.getAuxInfo()(1) += flightTime;
602  }
603 
604  lastEndState_ = state;
605 
606  return tracklength;
607 }
608 
609 
611  double radius,
612  const TVector3& point, // center
613  bool stopAtBoundary,
614  bool calcJacobianNoise) const {
615 
616  if (debugLvl_ > 0) {
617  debugOut << "RKTrackRep::extrapolateToSphere()\n";
618  }
619 
620  checkCache(state, NULL);
621 
622  static const unsigned int maxIt(1000);
623 
624  // to 7D
625  M1x7 state7;
626  getState7(state, state7);
627 
628  bool fillExtrapSteps(false);
629  if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
630  fillExtrapSteps = true;
631  }
632  else if (calcJacobianNoise)
633  fillExtrapSteps = true;
634 
635  double tracklength(0.), maxStep(1.E99);
636 
637  TVector3 dest, pos, dir;
638 
639  bool isAtBoundary(false);
640 
641  DetPlane startPlane(*(state.getPlane()));
642  SharedPlanePtr plane(new DetPlane());
643  unsigned int iterations(0);
644  double charge = getCharge(state);
645  double mass = getMass(state);
646  double flightTime = 0;
647 
648  while(true){
649  if(++iterations == maxIt) {
650  Exception exc("RKTrackRep::extrapolateToSphere ==> maximum number of iterations reached",__LINE__,__FILE__);
651  exc.setFatal();
652  throw exc;
653  }
654 
655  pos.SetXYZ(state7[0], state7[1], state7[2]);
656  dir.SetXYZ(state7[3], state7[4], state7[5]);
657 
658  // solve quadratic equation
659  TVector3 AO = (pos - point);
660  double dirAO = dir * AO;
661  double arg = dirAO*dirAO - AO*AO + radius*radius;
662  if(arg < 0) {
663  Exception exc("RKTrackRep::extrapolateToSphere ==> cannot solve",__LINE__,__FILE__);
664  exc.setFatal();
665  throw exc;
666  }
667  double term = sqrt(arg);
668  double k1, k2;
669  k1 = -dirAO + term;
670  k2 = -dirAO - term;
671 
672  // select smallest absolute solution -> closest cylinder surface
673  double k = k1;
674  if (fabs(k2)<fabs(k))
675  k = k2;
676 
677  if (debugLvl_ > 0) {
678  debugOut << "RKTrackRep::extrapolateToSphere(); k = " << k << "\n";
679  }
680 
681  dest = pos + k * dir;
682 
683  plane->setON(dest, dest-point);
684 
685  tracklength += this->Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime, false, NULL, true, stopAtBoundary, maxStep);
686 
687  // check break conditions
688  if (stopAtBoundary && isAtBoundary) {
689  pos.SetXYZ(state7[0], state7[1], state7[2]);
690  dir.SetXYZ(state7[3], state7[4], state7[5]);
691  plane->setON(pos, pos-point);
692  break;
693  }
694 
695  if(fabs(k)<MINSTEP) break;
696 
697  startPlane = *plane;
698 
699  }
700 
701  if (fillExtrapSteps) { // now do the full extrapolation with covariance matrix
702  // make use of the cache
703  lastEndState_.setPlane(plane);
704  getState5(lastEndState_, state7);
705 
706  tracklength = extrapolateToPlane(state, plane, false, true);
707  lastEndState_.getAuxInfo()(1) = state.getAuxInfo()(1); // Flight time
708  }
709  else {
710  state.setPlane(plane);
711  getState5(state, state7);
712  state.getAuxInfo()(1) += flightTime;
713  }
714 
715  lastEndState_ = state;
716 
717  return tracklength;
718 }
719 
720 
722  double step,
723  bool stopAtBoundary,
724  bool calcJacobianNoise) const {
725 
726  if (debugLvl_ > 0) {
727  debugOut << "RKTrackRep::extrapolateBy()\n";
728  }
729 
730  checkCache(state, NULL);
731 
732  static const unsigned int maxIt(1000);
733 
734  // to 7D
735  M1x7 state7;
736  getState7(state, state7);
737 
738  bool fillExtrapSteps(false);
739  if (dynamic_cast<MeasuredStateOnPlane*>(&state) != NULL) {
740  fillExtrapSteps = true;
741  }
742  else if (calcJacobianNoise)
743  fillExtrapSteps = true;
744 
745  double tracklength(0.);
746 
747  TVector3 dest, pos, dir;
748 
749  bool isAtBoundary(false);
750 
751  DetPlane startPlane(*(state.getPlane()));
752  SharedPlanePtr plane(new DetPlane());
753  unsigned int iterations(0);
754  double charge = getCharge(state);
755  double mass = getMass(state);
756  double flightTime = 0;
757 
758  while(true){
759  if(++iterations == maxIt) {
760  Exception exc("RKTrackRep::extrapolateBy ==> maximum number of iterations reached",__LINE__,__FILE__);
761  exc.setFatal();
762  throw exc;
763  }
764 
765  pos.SetXYZ(state7[0], state7[1], state7[2]);
766  dir.SetXYZ(state7[3], state7[4], state7[5]);
767 
768  dest = pos + 1.5*(step-tracklength) * dir;
769 
770  plane->setON(dest, dir);
771 
772  tracklength += this->Extrap(startPlane, *plane, charge, mass, isAtBoundary, state7, flightTime, false, NULL, true, stopAtBoundary, (step-tracklength));
773 
774  // check break conditions
775  if (stopAtBoundary && isAtBoundary) {
776  pos.SetXYZ(state7[0], state7[1], state7[2]);
777  dir.SetXYZ(state7[3], state7[4], state7[5]);
778  plane->setON(pos, dir);
779  break;
780  }
781 
782  if (fabs(tracklength-step) < MINSTEP) {
783  if (debugLvl_ > 0) {
784  debugOut << "RKTrackRep::extrapolateBy(): reached after " << iterations << " iterations. \n";
785  }
786  pos.SetXYZ(state7[0], state7[1], state7[2]);
787  dir.SetXYZ(state7[3], state7[4], state7[5]);
788  plane->setON(pos, dir);
789  break;
790  }
791 
792  startPlane = *plane;
793 
794  }
795 
796  if (fillExtrapSteps) { // now do the full extrapolation with covariance matrix
797  // make use of the cache
798  lastEndState_.setPlane(plane);
799  getState5(lastEndState_, state7);
800 
801  tracklength = extrapolateToPlane(state, plane, false, true);
802  lastEndState_.getAuxInfo()(1) = state.getAuxInfo()(1); // Flight time
803  }
804  else {
805  state.setPlane(plane);
806  getState5(state, state7);
807  state.getAuxInfo()(1) += flightTime;
808  }
809 
810  lastEndState_ = state;
811 
812  return tracklength;
813 }
814 
815 
816 TVector3 RKTrackRep::getPos(const StateOnPlane& state) const {
817  M1x7 state7;
818  getState7(state, state7);
819 
820  return TVector3(state7[0], state7[1], state7[2]);
821 }
822 
823 
824 TVector3 RKTrackRep::getMom(const StateOnPlane& state) const {
825  M1x7 state7 = {{0, 0, 0, 0, 0, 0, 0}};
826  getState7(state, state7);
827 
828  TVector3 mom(state7[3], state7[4], state7[5]);
829  mom.SetMag(getCharge(state)/state7[6]);
830  return mom;
831 }
832 
833 
834 void RKTrackRep::getPosMom(const StateOnPlane& state, TVector3& pos, TVector3& mom) const {
835  M1x7 state7 = {{0, 0, 0, 0, 0, 0, 0}};
836  getState7(state, state7);
837 
838  pos.SetXYZ(state7[0], state7[1], state7[2]);
839  mom.SetXYZ(state7[3], state7[4], state7[5]);
840  mom.SetMag(getCharge(state)/state7[6]);
841 }
842 
843 
844 void RKTrackRep::getPosMomCov(const MeasuredStateOnPlane& state, TVector3& pos, TVector3& mom, TMatrixDSym& cov) const {
845  getPosMom(state, pos, mom);
846  cov.ResizeTo(6,6);
847  transformPM6(state, *((M6x6*) cov.GetMatrixArray()));
848 }
849 
850 
851 TMatrixDSym RKTrackRep::get6DCov(const MeasuredStateOnPlane& state) const {
852  TMatrixDSym cov(6);
853  transformPM6(state, *((M6x6*) cov.GetMatrixArray()));
854 
855  return cov;
856 }
857 
858 
859 double RKTrackRep::getCharge(const StateOnPlane& state) const {
860 
861  if (dynamic_cast<const MeasurementOnPlane*>(&state) != NULL) {
862  Exception exc("RKTrackRep::getCharge - cannot get charge from MeasurementOnPlane",__LINE__,__FILE__);
863  exc.setFatal();
864  throw exc;
865  }
866 
867  double pdgCharge( this->getPDGCharge() );
868 
869  // return pdgCharge with sign of q/p
870  if (state.getState()(0) * pdgCharge < 0)
871  return -pdgCharge;
872  else
873  return pdgCharge;
874 }
875 
876 
877 double RKTrackRep::getMomMag(const StateOnPlane& state) const {
878  // p = q / qop
879  double p = getCharge(state)/state.getState()(0);
880  assert (p>=0);
881  return p;
882 }
883 
884 
885 double RKTrackRep::getMomVar(const MeasuredStateOnPlane& state) const {
886 
887  if (dynamic_cast<const MeasurementOnPlane*>(&state) != NULL) {
888  Exception exc("RKTrackRep::getMomVar - cannot get momVar from MeasurementOnPlane",__LINE__,__FILE__);
889  exc.setFatal();
890  throw exc;
891  }
892 
893  // p(qop) = q/qop
894  // dp/d(qop) = - q / (qop^2)
895  // (delta p) = (delta qop) * |dp/d(qop)| = delta qop * |q / (qop^2)|
896  // (var p) = (var qop) * q^2 / (qop^4)
897 
898  // delta means sigma
899  // cov(0,0) is sigma^2
900 
901  return state.getCov()(0,0) * pow(getCharge(state), 2) / pow(state.getState()(0), 4);
902 }
903 
904 
905 double RKTrackRep::getSpu(const StateOnPlane& state) const {
906 
907  if (dynamic_cast<const MeasurementOnPlane*>(&state) != NULL) {
908  Exception exc("RKTrackRep::getSpu - cannot get spu from MeasurementOnPlane",__LINE__,__FILE__);
909  exc.setFatal();
910  throw exc;
911  }
912 
913  const TVectorD& auxInfo = state.getAuxInfo();
914  if (auxInfo.GetNrows() == 2
915  || auxInfo.GetNrows() == 1) // backwards compatibility with old RKTrackRep
916  return state.getAuxInfo()(0);
917  else
918  return 1.;
919 }
920 
921 double RKTrackRep::getTime(const StateOnPlane& state) const {
922 
923  const TVectorD& auxInfo = state.getAuxInfo();
924  if (auxInfo.GetNrows() == 2)
925  return state.getAuxInfo()(1);
926  else
927  return 0.;
928 }
929 
930 
931 void RKTrackRep::calcForwardJacobianAndNoise(const M1x7& startState7, const DetPlane& startPlane,
932  const M1x7& destState7, const DetPlane& destPlane) const {
933 
934  if (debugLvl_ > 0) {
935  debugOut << "RKTrackRep::calcForwardJacobianAndNoise " << std::endl;
936  }
937 
938  if (ExtrapSteps_.size() == 0) {
939  Exception exc("RKTrackRep::calcForwardJacobianAndNoise ==> cache is empty. Extrapolation must run with a MeasuredStateOnPlane.",__LINE__,__FILE__);
940  throw exc;
941  }
942 
943  // The Jacobians returned from RKutta are transposed.
944  TMatrixD jac(TMatrixD::kTransposed, TMatrixD(7, 7, ExtrapSteps_.back().jac7_.begin()));
945  TMatrixDSym noise(7, ExtrapSteps_.back().noise7_.begin());
946  for (int i = ExtrapSteps_.size() - 2; i >= 0; --i) {
947  noise += TMatrixDSym(7, ExtrapSteps_[i].noise7_.begin()).Similarity(jac);
948  jac *= TMatrixD(TMatrixD::kTransposed, TMatrixD(7, 7, ExtrapSteps_[i].jac7_.begin()));
949  }
950 
951  // Project into 5x5 space.
952  M1x3 pTilde = {{startState7[3], startState7[4], startState7[5]}};
953  const TVector3& normal = startPlane.getNormal();
954  double pTildeW = pTilde[0] * normal.X() + pTilde[1] * normal.Y() + pTilde[2] * normal.Z();
955  double spu = pTildeW > 0 ? 1 : -1;
956  for (unsigned int i=0; i<3; ++i) {
957  pTilde[i] *= spu/pTildeW; // | pTilde * W | has to be 1 (definition of pTilde)
958  }
959  M5x7 J_pM;
960  calcJ_pM_5x7(J_pM, startPlane.getU(), startPlane.getV(), pTilde, spu);
961  M7x5 J_Mp;
962  calcJ_Mp_7x5(J_Mp, destPlane.getU(), destPlane.getV(), destPlane.getNormal(), *((M1x3*) &destState7[3]));
963  jac.Transpose(jac); // Because the helper function wants transposed input.
964  RKTools::J_pMTTxJ_MMTTxJ_MpTT(J_Mp, *(M7x7 *)jac.GetMatrixArray(),
965  J_pM, *(M5x5 *)fJacobian_.GetMatrixArray());
966  RKTools::J_MpTxcov7xJ_Mp(J_Mp, *(M7x7 *)noise.GetMatrixArray(),
967  *(M5x5 *)fNoise_.GetMatrixArray());
968 
969  if (debugLvl_ > 0) {
970  debugOut << "total jacobian : "; fJacobian_.Print();
971  debugOut << "total noise : "; fNoise_.Print();
972  }
973 
974 }
975 
976 
977 void RKTrackRep::getForwardJacobianAndNoise(TMatrixD& jacobian, TMatrixDSym& noise, TVectorD& deltaState) const {
978 
979  jacobian.ResizeTo(5,5);
980  jacobian = fJacobian_;
981 
982  noise.ResizeTo(5,5);
983  noise = fNoise_;
984 
985  // lastEndState_ = jacobian * lastStartState_ + deltaState
986  deltaState.ResizeTo(5);
987  // Calculate this without temporaries:
988  //deltaState = lastEndState_.getState() - jacobian * lastStartState_.getState()
989  deltaState = lastStartState_.getState();
990  deltaState *= jacobian;
991  deltaState -= lastEndState_.getState();
992  deltaState *= -1;
993 
994 
995  if (debugLvl_ > 0) {
996  debugOut << "delta state : "; deltaState.Print();
997  }
998 }
999 
1000 
1001 void RKTrackRep::getBackwardJacobianAndNoise(TMatrixD& jacobian, TMatrixDSym& noise, TVectorD& deltaState) const {
1002 
1003  if (debugLvl_ > 0) {
1004  debugOut << "RKTrackRep::getBackwardJacobianAndNoise " << std::endl;
1005  }
1006 
1007  if (ExtrapSteps_.size() == 0) {
1008  Exception exc("RKTrackRep::getBackwardJacobianAndNoise ==> cache is empty. Extrapolation must run with a MeasuredStateOnPlane.",__LINE__,__FILE__);
1009  throw exc;
1010  }
1011 
1012  jacobian.ResizeTo(5,5);
1013  jacobian = fJacobian_;
1014  if (!useInvertFast) {
1015  bool status = TDecompLU::InvertLU(jacobian, 0.0);
1016  if(status == 0){
1017  Exception e("cannot invert matrix, status = 0", __LINE__,__FILE__);
1018  e.setFatal();
1019  throw e;
1020  }
1021  } else {
1022  double det;
1023  jacobian.InvertFast(&det);
1024  if(det < 1e-80){
1025  Exception e("cannot invert matrix, status = 0", __LINE__,__FILE__);
1026  e.setFatal();
1027  throw e;
1028  }
1029  }
1030 
1031  noise.ResizeTo(5,5);
1032  noise = fNoise_;
1033  noise.Similarity(jacobian);
1034 
1035  // lastStartState_ = jacobian * lastEndState_ + deltaState
1036  deltaState.ResizeTo(5);
1037  deltaState = lastStartState_.getState() - jacobian * lastEndState_.getState();
1038 }
1039 
1040 
1041 std::vector<genfit::MatStep> RKTrackRep::getSteps() const {
1042 
1043  // Todo: test
1044 
1045  if (RKSteps_.size() == 0) {
1046  Exception exc("RKTrackRep::getSteps ==> cache is empty.",__LINE__,__FILE__);
1047  throw exc;
1048  }
1049 
1050  std::vector<MatStep> retVal;
1051  retVal.reserve(RKSteps_.size());
1052 
1053  for (unsigned int i = 0; i<RKSteps_.size(); ++i) {
1054  retVal.push_back(RKSteps_[i].matStep_);
1055  }
1056 
1057  return retVal;
1058 }
1059 
1060 
1062 
1063  // Todo: test
1064 
1065  if (RKSteps_.size() == 0) {
1066  Exception exc("RKTrackRep::getRadiationLenght ==> cache is empty.",__LINE__,__FILE__);
1067  throw exc;
1068  }
1069 
1070  double radLen(0);
1071 
1072  for (unsigned int i = 0; i<RKSteps_.size(); ++i) {
1073  radLen += RKSteps_.at(i).matStep_.stepSize_ / RKSteps_.at(i).matStep_.materialProperties_.getRadLen();
1074  }
1075 
1076  return radLen;
1077 }
1078 
1079 
1080 
1081 void RKTrackRep::setPosMom(StateOnPlane& state, const TVector3& pos, const TVector3& mom) const {
1082 
1083  if (state.getRep() != this){
1084  Exception exc("RKTrackRep::setPosMom ==> state is defined wrt. another TrackRep",__LINE__,__FILE__);
1085  throw exc;
1086  }
1087 
1088  if (dynamic_cast<MeasurementOnPlane*>(&state) != NULL) {
1089  Exception exc("RKTrackRep::setPosMom - cannot set pos/mom of a MeasurementOnPlane",__LINE__,__FILE__);
1090  exc.setFatal();
1091  throw exc;
1092  }
1093 
1094  if (mom.Mag2() == 0) {
1095  Exception exc("RKTrackRep::setPosMom - momentum is 0",__LINE__,__FILE__);
1096  exc.setFatal();
1097  throw exc;
1098  }
1099 
1100  // init auxInfo if that has not yet happened
1101  TVectorD& auxInfo = state.getAuxInfo();
1102  if (auxInfo.GetNrows() != 2) {
1103  bool alreadySet = auxInfo.GetNrows() == 1; // backwards compatibility: don't overwrite old setting
1104  auxInfo.ResizeTo(2);
1105  if (!alreadySet)
1106  setSpu(state, 1.);
1107  }
1108 
1109  if (state.getPlane() != NULL && state.getPlane()->distance(pos) < MINSTEP) { // pos is on plane -> do not change plane!
1110 
1111  M1x7 state7;
1112 
1113  state7[0] = pos.X();
1114  state7[1] = pos.Y();
1115  state7[2] = pos.Z();
1116 
1117  state7[3] = mom.X();
1118  state7[4] = mom.Y();
1119  state7[5] = mom.Z();
1120 
1121  // normalize dir
1122  double norm = 1. / sqrt(state7[3]*state7[3] + state7[4]*state7[4] + state7[5]*state7[5]);
1123  for (unsigned int i=3; i<6; ++i)
1124  state7[i] *= norm;
1125 
1126  state7[6] = getCharge(state) * norm;
1127 
1128  getState5(state, state7);
1129 
1130  }
1131  else { // pos is not on plane -> create new plane!
1132 
1133  // TODO: Raise Warning that a new plane has been created!
1134  SharedPlanePtr plane(new DetPlane(pos, mom));
1135  state.setPlane(plane);
1136 
1137  TVectorD& state5(state.getState());
1138 
1139  state5(0) = getCharge(state)/mom.Mag(); // q/p
1140  state5(1) = 0.; // u'
1141  state5(2) = 0.; // v'
1142  state5(3) = 0.; // u
1143  state5(4) = 0.; // v
1144 
1145  setSpu(state, 1.);
1146  }
1147 
1148 }
1149 
1150 
1151 void RKTrackRep::setPosMom(StateOnPlane& state, const TVectorD& state6) const {
1152  if (state6.GetNrows()!=6){
1153  Exception exc("RKTrackRep::setPosMom ==> state has to be 6d (x, y, z, px, py, pz)",__LINE__,__FILE__);
1154  throw exc;
1155  }
1156  setPosMom(state, TVector3(state6(0), state6(1), state6(2)), TVector3(state6(3), state6(4), state6(5)));
1157 }
1158 
1159 
1160 void RKTrackRep::setPosMomErr(MeasuredStateOnPlane& state, const TVector3& pos, const TVector3& mom, const TVector3& posErr, const TVector3& momErr) const {
1161 
1162  // TODO: test!
1163 
1164  setPosMom(state, pos, mom);
1165 
1166  const TVector3& U(state.getPlane()->getU());
1167  const TVector3& V(state.getPlane()->getV());
1168  TVector3 W(state.getPlane()->getNormal());
1169 
1170  double pw = mom * W;
1171  double pu = mom * U;
1172  double pv = mom * V;
1173 
1174  TMatrixDSym& cov(state.getCov());
1175 
1176  cov(0,0) = pow(getCharge(state), 2) / pow(mom.Mag(), 6) *
1177  (mom.X()*mom.X() * momErr.X()*momErr.X()+
1178  mom.Y()*mom.Y() * momErr.Y()*momErr.Y()+
1179  mom.Z()*mom.Z() * momErr.Z()*momErr.Z());
1180 
1181  cov(1,1) = pow((U.X()/pw - W.X()*pu/(pw*pw)),2.) * momErr.X()*momErr.X() +
1182  pow((U.Y()/pw - W.Y()*pu/(pw*pw)),2.) * momErr.Y()*momErr.Y() +
1183  pow((U.Z()/pw - W.Z()*pu/(pw*pw)),2.) * momErr.Z()*momErr.Z();
1184 
1185  cov(2,2) = pow((V.X()/pw - W.X()*pv/(pw*pw)),2.) * momErr.X()*momErr.X() +
1186  pow((V.Y()/pw - W.Y()*pv/(pw*pw)),2.) * momErr.Y()*momErr.Y() +
1187  pow((V.Z()/pw - W.Z()*pv/(pw*pw)),2.) * momErr.Z()*momErr.Z();
1188 
1189  cov(3,3) = posErr.X()*posErr.X() * U.X()*U.X() +
1190  posErr.Y()*posErr.Y() * U.Y()*U.Y() +
1191  posErr.Z()*posErr.Z() * U.Z()*U.Z();
1192 
1193  cov(4,4) = posErr.X()*posErr.X() * V.X()*V.X() +
1194  posErr.Y()*posErr.Y() * V.Y()*V.Y() +
1195  posErr.Z()*posErr.Z() * V.Z()*V.Z();
1196 
1197 }
1198 
1199 
1200 
1201 
1202 void RKTrackRep::setPosMomCov(MeasuredStateOnPlane& state, const TVector3& pos, const TVector3& mom, const TMatrixDSym& cov6x6) const {
1203 
1204  if (cov6x6.GetNcols()!=6 || cov6x6.GetNrows()!=6){
1205  Exception exc("RKTrackRep::setPosMomCov ==> cov has to be 6x6 (x, y, z, px, py, pz)",__LINE__,__FILE__);
1206  throw exc;
1207  }
1208 
1209  setPosMom(state, pos, mom); // charge does not change!
1210 
1211  M1x7 state7;
1212  getState7(state, state7);
1213 
1214  const M6x6& cov6x6_( *((M6x6*) cov6x6.GetMatrixArray()) );
1215 
1216  transformM6P(cov6x6_, state7, state);
1217 
1218 }
1219 
1220 void RKTrackRep::setPosMomCov(MeasuredStateOnPlane& state, const TVectorD& state6, const TMatrixDSym& cov6x6) const {
1221 
1222  if (state6.GetNrows()!=6){
1223  Exception exc("RKTrackRep::setPosMomCov ==> state has to be 6d (x, y, z, px, py, pz)",__LINE__,__FILE__);
1224  throw exc;
1225  }
1226 
1227  if (cov6x6.GetNcols()!=6 || cov6x6.GetNrows()!=6){
1228  Exception exc("RKTrackRep::setPosMomCov ==> cov has to be 6x6 (x, y, z, px, py, pz)",__LINE__,__FILE__);
1229  throw exc;
1230  }
1231 
1232  TVector3 pos(state6(0), state6(1), state6(2));
1233  TVector3 mom(state6(3), state6(4), state6(5));
1234  setPosMom(state, pos, mom); // charge does not change!
1235 
1236  M1x7 state7;
1237  getState7(state, state7);
1238 
1239  const M6x6& cov6x6_( *((M6x6*) cov6x6.GetMatrixArray()) );
1240 
1241  transformM6P(cov6x6_, state7, state);
1242 
1243 }
1244 
1245 
1246 void RKTrackRep::setChargeSign(StateOnPlane& state, double charge) const {
1247 
1248  if (dynamic_cast<MeasurementOnPlane*>(&state) != NULL) {
1249  Exception exc("RKTrackRep::setChargeSign - cannot set charge of a MeasurementOnPlane",__LINE__,__FILE__);
1250  exc.setFatal();
1251  throw exc;
1252  }
1253 
1254  if (state.getState()(0) * charge < 0) {
1255  state.getState()(0) *= -1.;
1256  }
1257 }
1258 
1259 
1260 void RKTrackRep::setSpu(StateOnPlane& state, double spu) const {
1261  state.getAuxInfo().ResizeTo(2);
1262  (state.getAuxInfo())(0) = spu;
1263 }
1264 
1265 void RKTrackRep::setTime(StateOnPlane& state, double time) const {
1266  state.getAuxInfo().ResizeTo(2);
1267  (state.getAuxInfo())(1) = time;
1268 }
1269 
1270 
1271 
1273  M7x7* jacobianT,
1274  M1x3& SA,
1275  double S,
1276  bool varField,
1277  bool calcOnlyLastRowOfJ) const {
1278  // The algorithm is
1279  // E Lund et al 2009 JINST 4 P04001 doi:10.1088/1748-0221/4/04/P04001
1280  // "Track parameter propagation through the application of a new adaptive Runge-Kutta-Nyström method in the ATLAS experiment"
1281  // http://inspirehep.net/search?ln=en&ln=en&p=10.1088/1748-0221/4/04/P04001&of=hb&action_search=Search&sf=earliestdate&so=d&rm=&rg=25&sc=0
1282  // where the transport of the Jacobian is described in
1283  // L. Bugge, J. Myrheim Nucl.Instrum.Meth. 160 (1979) 43-48
1284  // "A Fast Runge-kutta Method For Fitting Tracks In A Magnetic Field"
1285  // http://inspirehep.net/record/145692
1286  // and
1287  // L. Bugge, J. Myrheim Nucl.Instrum.Meth. 179 (1981) 365-381
1288  // "Tracking And Track Fitting"
1289  // http://inspirehep.net/record/160548
1290 
1291  // important fixed numbers
1292  static const double EC ( 0.000149896229 ); // c/(2*10^12) resp. c/2Tera
1293  static const double P3 ( 1./3. ); // 1/3
1294  static const double DLT ( .0002 ); // max. deviation for approximation-quality test
1295  // Aux parameters
1296  M1x3& R = *((M1x3*) &state7[0]); // Start coordinates [cm] (x, y, z)
1297  M1x3& A = *((M1x3*) &state7[3]); // Start directions (ax, ay, az); ax^2+ay^2+az^2=1
1298  double S3(0), S4(0), PS2(0);
1299  M1x3 H0 = {{0.,0.,0.}}, H1 = {{0.,0.,0.}}, H2 = {{0.,0.,0.}};
1300  M1x3 r = {{0.,0.,0.}};
1301  // Variables for Runge Kutta solver
1302  double A0(0), A1(0), A2(0), A3(0), A4(0), A5(0), A6(0);
1303  double B0(0), B1(0), B2(0), B3(0), B4(0), B5(0), B6(0);
1304  double C0(0), C1(0), C2(0), C3(0), C4(0), C5(0), C6(0);
1305 
1306  //
1307  // Runge Kutta Extrapolation
1308  //
1309  S3 = P3*S;
1310  S4 = 0.25*S;
1311  PS2 = state7[6]*EC * S;
1312 
1313  // First point
1314  r[0] = R[0]; r[1] = R[1]; r[2]=R[2];
1315  FieldManager::getInstance()->getFieldVal(r[0], r[1], r[2], H0[0], H0[1], H0[2]); // magnetic field in 10^-1 T = kGauss
1316  H0[0] *= PS2; H0[1] *= PS2; H0[2] *= PS2; // H0 is PS2*(Hx, Hy, Hz) @ R0
1317  A0 = A[1]*H0[2]-A[2]*H0[1]; B0 = A[2]*H0[0]-A[0]*H0[2]; C0 = A[0]*H0[1]-A[1]*H0[0]; // (ax, ay, az) x H0
1318  A2 = A[0]+A0 ; B2 = A[1]+B0 ; C2 = A[2]+C0 ; // (A0, B0, C0) + (ax, ay, az)
1319  A1 = A2+A[0] ; B1 = B2+A[1] ; C1 = C2+A[2] ; // (A0, B0, C0) + 2*(ax, ay, az)
1320 
1321  // Second point
1322  if (varField) {
1323  r[0] += A1*S4; r[1] += B1*S4; r[2] += C1*S4;
1324  FieldManager::getInstance()->getFieldVal(r[0], r[1], r[2], H1[0], H1[1], H1[2]);
1325  H1[0] *= PS2; H1[1] *= PS2; H1[2] *= PS2; // H1 is PS2*(Hx, Hy, Hz) @ (x, y, z) + 0.25*S * [(A0, B0, C0) + 2*(ax, ay, az)]
1326  }
1327  else { H1 = H0; };
1328  A3 = B2*H1[2]-C2*H1[1]+A[0]; B3 = C2*H1[0]-A2*H1[2]+A[1]; C3 = A2*H1[1]-B2*H1[0]+A[2]; // (A2, B2, C2) x H1 + (ax, ay, az)
1329  A4 = B3*H1[2]-C3*H1[1]+A[0]; B4 = C3*H1[0]-A3*H1[2]+A[1]; C4 = A3*H1[1]-B3*H1[0]+A[2]; // (A3, B3, C3) x H1 + (ax, ay, az)
1330  A5 = A4-A[0]+A4 ; B5 = B4-A[1]+B4 ; C5 = C4-A[2]+C4 ; // 2*(A4, B4, C4) - (ax, ay, az)
1331 
1332  // Last point
1333  if (varField) {
1334  r[0]=R[0]+S*A4; r[1]=R[1]+S*B4; r[2]=R[2]+S*C4; //setup.Field(r,H2);
1335  FieldManager::getInstance()->getFieldVal(r[0], r[1], r[2], H2[0], H2[1], H2[2]);
1336  H2[0] *= PS2; H2[1] *= PS2; H2[2] *= PS2; // H2 is PS2*(Hx, Hy, Hz) @ (x, y, z) + 0.25*S * (A4, B4, C4)
1337  }
1338  else { H2 = H0; };
1339  A6 = B5*H2[2]-C5*H2[1]; B6 = C5*H2[0]-A5*H2[2]; C6 = A5*H2[1]-B5*H2[0]; // (A5, B5, C5) x H2
1340 
1341 
1342  //
1343  // Derivatives of track parameters
1344  //
1345  if(jacobianT != NULL){
1346 
1347  // jacobianT
1348  // 1 0 0 0 0 0 0 x
1349  // 0 1 0 0 0 0 0 y
1350  // 0 0 1 0 0 0 0 z
1351  // x x x x x x 0 a_x
1352  // x x x x x x 0 a_y
1353  // x x x x x x 0 a_z
1354  // x x x x x x 1 q/p
1355  M7x7& J = *jacobianT;
1356 
1357  double dA0(0), dA2(0), dA3(0), dA4(0), dA5(0), dA6(0);
1358  double dB0(0), dB2(0), dB3(0), dB4(0), dB5(0), dB6(0);
1359  double dC0(0), dC2(0), dC3(0), dC4(0), dC5(0), dC6(0);
1360 
1361  int start(0);
1362 
1363  if (!calcOnlyLastRowOfJ) {
1364 
1365  if (!varField) {
1366  // d(x, y, z)/d(x, y, z) submatrix is unit matrix
1367  J(0, 0) = 1; J(1, 1) = 1; J(2, 2) = 1;
1368  // d(ax, ay, az)/d(ax, ay, az) submatrix is 0
1369  // start with d(x, y, z)/d(ax, ay, az)
1370  start = 3;
1371  }
1372 
1373  for(int i=start; i<6; ++i) {
1374 
1375  //first point
1376  dA0 = H0[2]*J(i, 4)-H0[1]*J(i, 5); // dA0/dp }
1377  dB0 = H0[0]*J(i, 5)-H0[2]*J(i, 3); // dB0/dp } = dA x H0
1378  dC0 = H0[1]*J(i, 3)-H0[0]*J(i, 4); // dC0/dp }
1379 
1380  dA2 = dA0+J(i, 3); // }
1381  dB2 = dB0+J(i, 4); // } = (dA0, dB0, dC0) + dA
1382  dC2 = dC0+J(i, 5); // }
1383 
1384  //second point
1385  dA3 = J(i, 3)+dB2*H1[2]-dC2*H1[1]; // dA3/dp }
1386  dB3 = J(i, 4)+dC2*H1[0]-dA2*H1[2]; // dB3/dp } = dA + (dA2, dB2, dC2) x H1
1387  dC3 = J(i, 5)+dA2*H1[1]-dB2*H1[0]; // dC3/dp }
1388 
1389  dA4 = J(i, 3)+dB3*H1[2]-dC3*H1[1]; // dA4/dp }
1390  dB4 = J(i, 4)+dC3*H1[0]-dA3*H1[2]; // dB4/dp } = dA + (dA3, dB3, dC3) x H1
1391  dC4 = J(i, 5)+dA3*H1[1]-dB3*H1[0]; // dC4/dp }
1392 
1393  //last point
1394  dA5 = dA4+dA4-J(i, 3); // }
1395  dB5 = dB4+dB4-J(i, 4); // } = 2*(dA4, dB4, dC4) - dA
1396  dC5 = dC4+dC4-J(i, 5); // }
1397 
1398  dA6 = dB5*H2[2]-dC5*H2[1]; // dA6/dp }
1399  dB6 = dC5*H2[0]-dA5*H2[2]; // dB6/dp } = (dA5, dB5, dC5) x H2
1400  dC6 = dA5*H2[1]-dB5*H2[0]; // dC6/dp }
1401 
1402  // this gives the same results as multiplying the old with the new Jacobian
1403  J(i, 0) += (dA2+dA3+dA4)*S3; J(i, 3) = ((dA0+2.*dA3)+(dA5+dA6))*P3; // dR := dR + S3*[(dA2, dB2, dC2) + (dA3, dB3, dC3) + (dA4, dB4, dC4)]
1404  J(i, 1) += (dB2+dB3+dB4)*S3; J(i, 4) = ((dB0+2.*dB3)+(dB5+dB6))*P3; // dA := 1/3*[(dA0, dB0, dC0) + 2*(dA3, dB3, dC3) + (dA5, dB5, dC5) + (dA6, dB6, dC6)]
1405  J(i, 2) += (dC2+dC3+dC4)*S3; J(i, 5) = ((dC0+2.*dC3)+(dC5+dC6))*P3;
1406  }
1407 
1408  } // end if (!calcOnlyLastRowOfJ)
1409 
1410  J(6, 3) *= state7[6]; J(6, 4) *= state7[6]; J(6, 5) *= state7[6];
1411 
1412  //first point
1413  dA0 = H0[2]*J(6, 4)-H0[1]*J(6, 5) + A0; // dA0/dp }
1414  dB0 = H0[0]*J(6, 5)-H0[2]*J(6, 3) + B0; // dB0/dp } = dA x H0 + (A0, B0, C0)
1415  dC0 = H0[1]*J(6, 3)-H0[0]*J(6, 4) + C0; // dC0/dp }
1416 
1417  dA2 = dA0+J(6, 3); // }
1418  dB2 = dB0+J(6, 4); // } = (dA0, dB0, dC0) + dA
1419  dC2 = dC0+J(6, 5); // }
1420 
1421  //second point
1422  dA3 = J(6, 3)+dB2*H1[2]-dC2*H1[1] + (A3-A[0]); // dA3/dp }
1423  dB3 = J(6, 4)+dC2*H1[0]-dA2*H1[2] + (B3-A[1]); // dB3/dp } = dA + (dA2, dB2, dC2) x H1
1424  dC3 = J(6, 5)+dA2*H1[1]-dB2*H1[0] + (C3-A[2]); // dC3/dp }
1425 
1426  dA4 = J(6, 3)+dB3*H1[2]-dC3*H1[1] + (A4-A[0]); // dA4/dp }
1427  dB4 = J(6, 4)+dC3*H1[0]-dA3*H1[2] + (B4-A[1]); // dB4/dp } = dA + (dA3, dB3, dC3) x H1
1428  dC4 = J(6, 5)+dA3*H1[1]-dB3*H1[0] + (C4-A[2]); // dC4/dp }
1429 
1430  //last point
1431  dA5 = dA4+dA4-J(6, 3); // }
1432  dB5 = dB4+dB4-J(6, 4); // } = 2*(dA4, dB4, dC4) - dA
1433  dC5 = dC4+dC4-J(6, 5); // }
1434 
1435  dA6 = dB5*H2[2]-dC5*H2[1] + A6; // dA6/dp }
1436  dB6 = dC5*H2[0]-dA5*H2[2] + B6; // dB6/dp } = (dA5, dB5, dC5) x H2 + (A6, B6, C6)
1437  dC6 = dA5*H2[1]-dB5*H2[0] + C6; // dC6/dp }
1438 
1439  // this gives the same results as multiplying the old with the new Jacobian
1440  J(6, 0) += (dA2+dA3+dA4)*S3/state7[6]; J(6, 3) = ((dA0+2.*dA3)+(dA5+dA6))*P3/state7[6]; // dR := dR + S3*[(dA2, dB2, dC2) + (dA3, dB3, dC3) + (dA4, dB4, dC4)]
1441  J(6, 1) += (dB2+dB3+dB4)*S3/state7[6]; J(6, 4) = ((dB0+2.*dB3)+(dB5+dB6))*P3/state7[6]; // dA := 1/3*[(dA0, dB0, dC0) + 2*(dA3, dB3, dC3) + (dA5, dB5, dC5) + (dA6, dB6, dC6)]
1442  J(6, 2) += (dC2+dC3+dC4)*S3/state7[6]; J(6, 5) = ((dC0+2.*dC3)+(dC5+dC6))*P3/state7[6];
1443 
1444  }
1445 
1446  //
1447  // Track parameters in last point
1448  //
1449  R[0] += (A2+A3+A4)*S3; A[0] += (SA[0]=((A0+2.*A3)+(A5+A6))*P3-A[0]); // R = R0 + S3*[(A2, B2, C2) + (A3, B3, C3) + (A4, B4, C4)]
1450  R[1] += (B2+B3+B4)*S3; A[1] += (SA[1]=((B0+2.*B3)+(B5+B6))*P3-A[1]); // A = 1/3*[(A0, B0, C0) + 2*(A3, B3, C3) + (A5, B5, C5) + (A6, B6, C6)]
1451  R[2] += (C2+C3+C4)*S3; A[2] += (SA[2]=((C0+2.*C3)+(C5+C6))*P3-A[2]); // SA = A_new - A_old
1452 
1453  // normalize A
1454  double CBA ( 1./sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]) ); // 1/|A|
1455  A[0] *= CBA; A[1] *= CBA; A[2] *= CBA;
1456 
1457 
1458  // Test approximation quality on given step
1459  double EST ( fabs((A1+A6)-(A3+A4)) +
1460  fabs((B1+B6)-(B3+B4)) +
1461  fabs((C1+C6)-(C3+C4)) ); // EST = ||(ABC1+ABC6)-(ABC3+ABC4)||_1 = ||(axzy x H0 + ABC5 x H2) - (ABC2 x H1 + ABC3 x H1)||_1
1462  if (debugLvl_ > 0) {
1463  debugOut << " RKTrackRep::RKPropagate. Step = "<< S << "; quality EST = " << EST << " \n";
1464  }
1465 
1466  // Prevent the step length increase from getting too large, this is
1467  // just the point where it becomes 10.
1468  if (EST < DLT*1e-5)
1469  return 10;
1470 
1471  // Step length increase for a fifth order Runge-Kutta, see e.g. 17.2
1472  // in Numerical Recipes. FIXME: move to caller.
1473  return pow(DLT/EST, 1./5.);
1474 }
1475 
1476 
1477 
1479  std::fill(noiseArray_.begin(), noiseArray_.end(), 0);
1480  std::fill(noiseProjection_.begin(), noiseProjection_.end(), 0);
1481  for (unsigned int i=0; i<7; ++i) // initialize as diagonal matrix
1482  noiseProjection_[i*8] = 1;
1483  std::fill(J_MMT_.begin(), J_MMT_.end(), 0);
1484 
1485  fJacobian_.UnitMatrix();
1486  fNoise_.Zero();
1487  limits_.reset();
1488 
1489  RKSteps_.reserve(100);
1490  ExtrapSteps_.reserve(100);
1491 
1492  lastStartState_.getAuxInfo().ResizeTo(2);
1493  lastEndState_.getAuxInfo().ResizeTo(2);
1494 }
1495 
1496 
1497 void RKTrackRep::getState7(const StateOnPlane& state, M1x7& state7) const {
1498 
1499  if (dynamic_cast<const MeasurementOnPlane*>(&state) != NULL) {
1500  Exception exc("RKTrackRep::getState7 - cannot get pos or mom from a MeasurementOnPlane",__LINE__,__FILE__);
1501  exc.setFatal();
1502  throw exc;
1503  }
1504 
1505  const TVector3& U(state.getPlane()->getU());
1506  const TVector3& V(state.getPlane()->getV());
1507  const TVector3& O(state.getPlane()->getO());
1508  const TVector3& W(state.getPlane()->getNormal());
1509 
1510  assert(state.getState().GetNrows() == 5);
1511  const double* state5 = state.getState().GetMatrixArray();
1512 
1513  double spu = getSpu(state);
1514 
1515  state7[0] = O.X() + state5[3]*U.X() + state5[4]*V.X(); // x
1516  state7[1] = O.Y() + state5[3]*U.Y() + state5[4]*V.Y(); // y
1517  state7[2] = O.Z() + state5[3]*U.Z() + state5[4]*V.Z(); // z
1518 
1519  state7[3] = spu * (W.X() + state5[1]*U.X() + state5[2]*V.X()); // a_x
1520  state7[4] = spu * (W.Y() + state5[1]*U.Y() + state5[2]*V.Y()); // a_y
1521  state7[5] = spu * (W.Z() + state5[1]*U.Z() + state5[2]*V.Z()); // a_z
1522 
1523  // normalize dir
1524  double norm = 1. / sqrt(state7[3]*state7[3] + state7[4]*state7[4] + state7[5]*state7[5]);
1525  for (unsigned int i=3; i<6; ++i) state7[i] *= norm;
1526 
1527  state7[6] = state5[0]; // q/p
1528 }
1529 
1530 
1531 void RKTrackRep::getState5(StateOnPlane& state, const M1x7& state7) const {
1532 
1533  // state5: (q/p, u', v'. u, v)
1534 
1535  double spu(1.);
1536 
1537  const TVector3& O(state.getPlane()->getO());
1538  const TVector3& U(state.getPlane()->getU());
1539  const TVector3& V(state.getPlane()->getV());
1540  const TVector3& W(state.getPlane()->getNormal());
1541 
1542  // force A to be in normal direction and set spu accordingly
1543  double AtW( state7[3]*W.X() + state7[4]*W.Y() + state7[5]*W.Z() );
1544  if (AtW < 0.) {
1545  //fDir *= -1.;
1546  //AtW *= -1.;
1547  spu = -1.;
1548  }
1549 
1550  double* state5 = state.getState().GetMatrixArray();
1551 
1552  state5[0] = state7[6]; // q/p
1553  state5[1] = (state7[3]*U.X() + state7[4]*U.Y() + state7[5]*U.Z()) / AtW; // u' = (A * U) / (A * W)
1554  state5[2] = (state7[3]*V.X() + state7[4]*V.Y() + state7[5]*V.Z()) / AtW; // v' = (A * V) / (A * W)
1555  state5[3] = ((state7[0]-O.X())*U.X() +
1556  (state7[1]-O.Y())*U.Y() +
1557  (state7[2]-O.Z())*U.Z()); // u = (pos - O) * U
1558  state5[4] = ((state7[0]-O.X())*V.X() +
1559  (state7[1]-O.Y())*V.Y() +
1560  (state7[2]-O.Z())*V.Z()); // v = (pos - O) * V
1561 
1562  setSpu(state, spu);
1563 
1564 }
1565 
1566 
1567 
1569  M7x7& out7x7) const {
1570 
1571  // get vectors and aux variables
1572  const TVector3& U(state.getPlane()->getU());
1573  const TVector3& V(state.getPlane()->getV());
1574  const TVector3& W(state.getPlane()->getNormal());
1575 
1576  const TVectorD& state5(state.getState());
1577  double spu(getSpu(state));
1578 
1579  M1x3 pTilde;
1580  pTilde[0] = spu * (W.X() + state5(1)*U.X() + state5(2)*V.X()); // a_x
1581  pTilde[1] = spu * (W.Y() + state5(1)*U.Y() + state5(2)*V.Y()); // a_y
1582  pTilde[2] = spu * (W.Z() + state5(1)*U.Z() + state5(2)*V.Z()); // a_z
1583 
1584  M5x7 J_pM;
1585  calcJ_pM_5x7(J_pM, U, V, pTilde, spu);
1586 
1587  // since the Jacobian contains a lot of zeros, and the resulting cov has to be symmetric,
1588  // the multiplication can be done much faster directly on array level
1589  // out = J_pM^T * in5x5 * J_pM
1590  const M5x5& in5x5_ = *((M5x5*) state.getCov().GetMatrixArray());
1591  RKTools::J_pMTxcov5xJ_pM(J_pM, in5x5_, out7x7);
1592 }
1593 
1594 
1595 void RKTrackRep::calcJ_pM_5x7(M5x7& J_pM, const TVector3& U, const TVector3& V, const M1x3& pTilde, double spu) const {
1596  /*if (debugLvl_ > 1) {
1597  debugOut << "RKTrackRep::calcJ_pM_5x7 \n";
1598  debugOut << " U = "; U.Print();
1599  debugOut << " V = "; V.Print();
1600  debugOut << " pTilde = "; RKTools::printDim(pTilde, 3,1);
1601  debugOut << " spu = " << spu << "\n";
1602  }*/
1603 
1604  std::fill(J_pM.begin(), J_pM.end(), 0);
1605 
1606  const double pTildeMag = sqrt(pTilde[0]*pTilde[0] + pTilde[1]*pTilde[1] + pTilde[2]*pTilde[2]);
1607  const double pTildeMag2 = pTildeMag*pTildeMag;
1608 
1609  const double utpTildeOverpTildeMag2 = (U.X()*pTilde[0] + U.Y()*pTilde[1] + U.Z()*pTilde[2]) / pTildeMag2;
1610  const double vtpTildeOverpTildeMag2 = (V.X()*pTilde[0] + V.Y()*pTilde[1] + V.Z()*pTilde[2]) / pTildeMag2;
1611 
1612  //J_pM matrix is d(x,y,z,ax,ay,az,q/p) / d(q/p,u',v',u,v) (out is 7x7)
1613 
1614  // d(x,y,z)/d(u)
1615  J_pM[21] = U.X(); // [3][0]
1616  J_pM[22] = U.Y(); // [3][1]
1617  J_pM[23] = U.Z(); // [3][2]
1618  // d(x,y,z)/d(v)
1619  J_pM[28] = V.X(); // [4][0]
1620  J_pM[29] = V.Y(); // [4][1]
1621  J_pM[30] = V.Z(); // [4][2]
1622  // d(q/p)/d(q/p)
1623  J_pM[6] = 1.; // not needed for array matrix multiplication
1624  // d(ax,ay,az)/d(u')
1625  double fact = spu / pTildeMag;
1626  J_pM[10] = fact * ( U.X() - pTilde[0]*utpTildeOverpTildeMag2 ); // [1][3]
1627  J_pM[11] = fact * ( U.Y() - pTilde[1]*utpTildeOverpTildeMag2 ); // [1][4]
1628  J_pM[12] = fact * ( U.Z() - pTilde[2]*utpTildeOverpTildeMag2 ); // [1][5]
1629  // d(ax,ay,az)/d(v')
1630  J_pM[17] = fact * ( V.X() - pTilde[0]*vtpTildeOverpTildeMag2 ); // [2][3]
1631  J_pM[18] = fact * ( V.Y() - pTilde[1]*vtpTildeOverpTildeMag2 ); // [2][4]
1632  J_pM[19] = fact * ( V.Z() - pTilde[2]*vtpTildeOverpTildeMag2 ); // [2][5]
1633 
1634  /*if (debugLvl_ > 1) {
1635  debugOut << " J_pM_5x7_ = "; RKTools::printDim(J_pM_5x7_, 5,7);
1636  }*/
1637 }
1638 
1639 
1641  M6x6& out6x6) const {
1642 
1643  // get vectors and aux variables
1644  const TVector3& U(state.getPlane()->getU());
1645  const TVector3& V(state.getPlane()->getV());
1646  const TVector3& W(state.getPlane()->getNormal());
1647 
1648  const TVectorD& state5(state.getState());
1649  double spu(getSpu(state));
1650 
1651  M1x3 pTilde;
1652  pTilde[0] = spu * (W.X() + state5(1)*U.X() + state5(2)*V.X()); // a_x
1653  pTilde[1] = spu * (W.Y() + state5(1)*U.Y() + state5(2)*V.Y()); // a_y
1654  pTilde[2] = spu * (W.Z() + state5(1)*U.Z() + state5(2)*V.Z()); // a_z
1655 
1656  const double pTildeMag = sqrt(pTilde[0]*pTilde[0] + pTilde[1]*pTilde[1] + pTilde[2]*pTilde[2]);
1657  const double pTildeMag2 = pTildeMag*pTildeMag;
1658 
1659  const double utpTildeOverpTildeMag2 = (U.X()*pTilde[0] + U.Y()*pTilde[1] + U.Z()*pTilde[2]) / pTildeMag2;
1660  const double vtpTildeOverpTildeMag2 = (V.X()*pTilde[0] + V.Y()*pTilde[1] + V.Z()*pTilde[2]) / pTildeMag2;
1661 
1662  //J_pM matrix is d(x,y,z,px,py,pz) / d(q/p,u',v',u,v) (out is 6x6)
1663 
1664  const double qop = state5(0);
1665  const double p = getCharge(state)/qop; // momentum
1666 
1667  M5x6 J_pM_5x6;
1668  std::fill(J_pM_5x6.begin(), J_pM_5x6.end(), 0);
1669 
1670  // d(px,py,pz)/d(q/p)
1671  double fact = -1. * p / (pTildeMag * qop);
1672  J_pM_5x6[3] = fact * pTilde[0]; // [0][3]
1673  J_pM_5x6[4] = fact * pTilde[1]; // [0][4]
1674  J_pM_5x6[5] = fact * pTilde[2]; // [0][5]
1675  // d(px,py,pz)/d(u')
1676  fact = p * spu / pTildeMag;
1677  J_pM_5x6[9] = fact * ( U.X() - pTilde[0]*utpTildeOverpTildeMag2 ); // [1][3]
1678  J_pM_5x6[10] = fact * ( U.Y() - pTilde[1]*utpTildeOverpTildeMag2 ); // [1][4]
1679  J_pM_5x6[11] = fact * ( U.Z() - pTilde[2]*utpTildeOverpTildeMag2 ); // [1][5]
1680  // d(px,py,pz)/d(v')
1681  J_pM_5x6[15] = fact * ( V.X() - pTilde[0]*vtpTildeOverpTildeMag2 ); // [2][3]
1682  J_pM_5x6[16] = fact * ( V.Y() - pTilde[1]*vtpTildeOverpTildeMag2 ); // [2][4]
1683  J_pM_5x6[17] = fact * ( V.Z() - pTilde[2]*vtpTildeOverpTildeMag2 ); // [2][5]
1684  // d(x,y,z)/d(u)
1685  J_pM_5x6[18] = U.X(); // [3][0]
1686  J_pM_5x6[19] = U.Y(); // [3][1]
1687  J_pM_5x6[20] = U.Z(); // [3][2]
1688  // d(x,y,z)/d(v)
1689  J_pM_5x6[24] = V.X(); // [4][0]
1690  J_pM_5x6[25] = V.Y(); // [4][1]
1691  J_pM_5x6[26] = V.Z(); // [4][2]
1692 
1693 
1694  // do the transformation
1695  // out = J_pM^T * in5x5 * J_pM
1696  const M5x5& in5x5_ = *((M5x5*) state.getCov().GetMatrixArray());
1697  RKTools::J_pMTxcov5xJ_pM(J_pM_5x6, in5x5_, out6x6);
1698 
1699 }
1700 
1701 
1702 void RKTrackRep::transformM7P(const M7x7& in7x7,
1703  const M1x7& state7,
1704  MeasuredStateOnPlane& state) const { // plane must already be set!
1705 
1706  // get vectors and aux variables
1707  const TVector3& U(state.getPlane()->getU());
1708  const TVector3& V(state.getPlane()->getV());
1709  const TVector3& W(state.getPlane()->getNormal());
1710 
1711  M1x3& A = *((M1x3*) &state7[3]);
1712 
1713  M7x5 J_Mp;
1714  calcJ_Mp_7x5(J_Mp, U, V, W, A);
1715 
1716  // since the Jacobian contains a lot of zeros, and the resulting cov has to be symmetric,
1717  // the multiplication can be done much faster directly on array level
1718  // out5x5 = J_Mp^T * in * J_Mp
1719  M5x5& out5x5_ = *((M5x5*) state.getCov().GetMatrixArray());
1720  RKTools::J_MpTxcov7xJ_Mp(J_Mp, in7x7, out5x5_);
1721 
1722 }
1723 
1724 
1725 void RKTrackRep::calcJ_Mp_7x5(M7x5& J_Mp, const TVector3& U, const TVector3& V, const TVector3& W, const M1x3& A) const {
1726 
1727  /*if (debugLvl_ > 1) {
1728  debugOut << "RKTrackRep::calcJ_Mp_7x5 \n";
1729  debugOut << " U = "; U.Print();
1730  debugOut << " V = "; V.Print();
1731  debugOut << " W = "; W.Print();
1732  debugOut << " A = "; RKTools::printDim(A, 3,1);
1733  }*/
1734 
1735  std::fill(J_Mp.begin(), J_Mp.end(), 7*5);
1736 
1737  const double AtU = A[0]*U.X() + A[1]*U.Y() + A[2]*U.Z();
1738  const double AtV = A[0]*V.X() + A[1]*V.Y() + A[2]*V.Z();
1739  const double AtW = A[0]*W.X() + A[1]*W.Y() + A[2]*W.Z();
1740 
1741  // J_Mp matrix is d(q/p,u',v',u,v) / d(x,y,z,ax,ay,az,q/p) (in is 7x7)
1742 
1743  // d(u')/d(ax,ay,az)
1744  double fact = 1./(AtW*AtW);
1745  J_Mp[16] = fact * (U.X()*AtW - W.X()*AtU); // [3][1]
1746  J_Mp[21] = fact * (U.Y()*AtW - W.Y()*AtU); // [4][1]
1747  J_Mp[26] = fact * (U.Z()*AtW - W.Z()*AtU); // [5][1]
1748  // d(v')/d(ax,ay,az)
1749  J_Mp[17] = fact * (V.X()*AtW - W.X()*AtV); // [3][2]
1750  J_Mp[22] = fact * (V.Y()*AtW - W.Y()*AtV); // [4][2]
1751  J_Mp[27] = fact * (V.Z()*AtW - W.Z()*AtV); // [5][2]
1752  // d(q/p)/d(q/p)
1753  J_Mp[30] = 1.; // [6][0] - not needed for array matrix multiplication
1754  //d(u)/d(x,y,z)
1755  J_Mp[3] = U.X(); // [0][3]
1756  J_Mp[8] = U.Y(); // [1][3]
1757  J_Mp[13] = U.Z(); // [2][3]
1758  //d(v)/d(x,y,z)
1759  J_Mp[4] = V.X(); // [0][4]
1760  J_Mp[9] = V.Y(); // [1][4]
1761  J_Mp[14] = V.Z(); // [2][4]
1762 
1763  /*if (debugLvl_ > 1) {
1764  debugOut << " J_Mp_7x5_ = "; RKTools::printDim(J_Mp, 7,5);
1765  }*/
1766 
1767 }
1768 
1769 
1770 void RKTrackRep::transformM6P(const M6x6& in6x6,
1771  const M1x7& state7,
1772  MeasuredStateOnPlane& state) const { // plane and charge must already be set!
1773 
1774  // get vectors and aux variables
1775  const TVector3& U(state.getPlane()->getU());
1776  const TVector3& V(state.getPlane()->getV());
1777  const TVector3& W(state.getPlane()->getNormal());
1778 
1779  const double AtU = state7[3]*U.X() + state7[4]*U.Y() + state7[5]*U.Z();
1780  const double AtV = state7[3]*V.X() + state7[4]*V.Y() + state7[5]*V.Z();
1781  const double AtW = state7[3]*W.X() + state7[4]*W.Y() + state7[5]*W.Z();
1782 
1783  // J_Mp matrix is d(q/p,u',v',u,v) / d(x,y,z,px,py,pz) (in is 6x6)
1784 
1785  const double qop = state7[6];
1786  const double p = getCharge(state)/qop; // momentum
1787 
1788  M6x5 J_Mp_6x5;
1789  std::fill(J_Mp_6x5.begin(), J_Mp_6x5.end(), 0);
1790 
1791  //d(u)/d(x,y,z)
1792  J_Mp_6x5[3] = U.X(); // [0][3]
1793  J_Mp_6x5[8] = U.Y(); // [1][3]
1794  J_Mp_6x5[13] = U.Z(); // [2][3]
1795  //d(v)/d(x,y,z)
1796  J_Mp_6x5[4] = V.X(); // [0][4]
1797  J_Mp_6x5[9] = V.Y(); // [1][4]
1798  J_Mp_6x5[14] = V.Z(); // [2][4]
1799  // d(q/p)/d(px,py,pz)
1800  double fact = (-1.) * qop / p;
1801  J_Mp_6x5[15] = fact * state7[3]; // [3][0]
1802  J_Mp_6x5[20] = fact * state7[4]; // [4][0]
1803  J_Mp_6x5[25] = fact * state7[5]; // [5][0]
1804  // d(u')/d(px,py,pz)
1805  fact = 1./(p*AtW*AtW);
1806  J_Mp_6x5[16] = fact * (U.X()*AtW - W.X()*AtU); // [3][1]
1807  J_Mp_6x5[21] = fact * (U.Y()*AtW - W.Y()*AtU); // [4][1]
1808  J_Mp_6x5[26] = fact * (U.Z()*AtW - W.Z()*AtU); // [5][1]
1809  // d(v')/d(px,py,pz)
1810  J_Mp_6x5[17] = fact * (V.X()*AtW - W.X()*AtV); // [3][2]
1811  J_Mp_6x5[22] = fact * (V.Y()*AtW - W.Y()*AtV); // [4][2]
1812  J_Mp_6x5[27] = fact * (V.Z()*AtW - W.Z()*AtV); // [5][2]
1813 
1814  // do the transformation
1815  // out5x5 = J_Mp^T * in * J_Mp
1816  M5x5& out5x5_ = *((M5x5*) state.getCov().GetMatrixArray());
1817  RKTools::J_MpTxcov6xJ_Mp(J_Mp_6x5, in6x6, out5x5_);
1818 
1819 }
1820 
1821 
1822 //
1823 // Runge-Kutta method for tracking a particles through a magnetic field.
1824 // Uses Nystroem algorithm (See Handbook Nat. Bur. of Standards, procedure 25.5.20)
1825 // in the way described in
1826 // E Lund et al 2009 JINST 4 P04001 doi:10.1088/1748-0221/4/04/P04001
1827 // "Track parameter propagation through the application of a new adaptive Runge-Kutta-Nyström method in the ATLAS experiment"
1828 // http://inspirehep.net/search?ln=en&ln=en&p=10.1088/1748-0221/4/04/P04001&of=hb&action_search=Search&sf=earliestdate&so=d&rm=&rg=25&sc=0
1829 //
1830 // Input parameters:
1831 // SU - plane parameters
1832 // SU[0] - direction cosines normal to surface Ex
1833 // SU[1] - ------- Ey
1834 // SU[2] - ------- Ez; Ex*Ex+Ey*Ey+Ez*Ez=1
1835 // SU[3] - distance to surface from (0,0,0) > 0 cm
1836 //
1837 // state7 - initial parameters (coordinates(cm), direction,
1838 // charge/momentum (Gev-1)
1839 // cov and derivatives this parameters (7x7)
1840 //
1841 // X Y Z Ax Ay Az q/P
1842 // state7[0] state7[1] state7[2] state7[3] state7[4] state7[5] state7[6]
1843 //
1844 // dX/dp dY/dp dZ/dp dAx/dp dAy/dp dAz/dp d(q/P)/dp
1845 // cov[ 0] cov[ 1] cov[ 2] cov[ 3] cov[ 4] cov[ 5] cov[ 6] d()/dp1
1846 //
1847 // cov[ 7] cov[ 8] cov[ 9] cov[10] cov[11] cov[12] cov[13] d()/dp2
1848 // ............................................................................ d()/dpND
1849 //
1850 // Authors: R.Brun, M.Hansroul, V.Perevoztchikov (Geant3)
1851 //
1852 bool RKTrackRep::RKutta(const M1x4& SU,
1853  const DetPlane& plane,
1854  double charge,
1855  double mass,
1856  M1x7& state7,
1857  M7x7* jacobianT,
1858  M1x7* J_MMT_unprojected_lastRow,
1859  double& coveredDistance,
1860  double& flightTime,
1861  bool& checkJacProj,
1862  M7x7& noiseProjection,
1863  StepLimits& limits,
1864  bool onlyOneStep,
1865  bool calcOnlyLastRowOfJ) const {
1866 
1867  // limits, check-values, etc. Can be tuned!
1868  static const double Wmax ( 3000. ); // max. way allowed [cm]
1869  static const double AngleMax ( 6.3 ); // max. total angle change of momentum. Prevents extrapolating a curler round and round if no active plane is found.
1870  static const double Pmin ( 4.E-3 ); // minimum momentum for propagation [GeV]
1871  static const unsigned int maxNumIt ( 1000 ); // maximum number of iterations in main loop
1872  // Aux parameters
1873  M1x3& R ( *((M1x3*) &state7[0]) ); // Start coordinates [cm] (x, y, z)
1874  M1x3& A ( *((M1x3*) &state7[3]) ); // Start directions (ax, ay, az); ax^2+ay^2+az^2=1
1875  M1x3 SA = {{0.,0.,0.}}; // Start directions derivatives dA/S
1876  double Way ( 0. ); // Sum of absolute values of all extrapolation steps [cm]
1877  double momentum ( fabs(charge/state7[6]) ); // momentum [GeV]
1878  double relMomLoss ( 0 ); // relative momentum loss in RKutta
1879  double deltaAngle ( 0. ); // total angle by which the momentum has changed during extrapolation
1880  double An(0), S(0), Sl(0), CBA(0);
1881 
1882 
1883  if (debugLvl_ > 0) {
1884  debugOut << "RKTrackRep::RKutta \n";
1885  debugOut << "position: "; TVector3(R[0], R[1], R[2]).Print();
1886  debugOut << "direction: "; TVector3(A[0], A[1], A[2]).Print();
1887  debugOut << "momentum: " << momentum << " GeV\n";
1888  debugOut << "destination: "; plane.Print();
1889  }
1890 
1891  checkJacProj = false;
1892 
1893  // check momentum
1894  if(momentum < Pmin){
1895  std::ostringstream sstream;
1896  sstream << "RKTrackRep::RKutta ==> momentum too low: " << momentum*1000. << " MeV";
1897  Exception exc(sstream.str(),__LINE__,__FILE__);
1898  exc.setFatal();
1899  throw exc;
1900  }
1901 
1902  unsigned int counter(0);
1903 
1904  // Step estimation (signed)
1905  S = estimateStep(state7, SU, plane, charge, relMomLoss, limits);
1906 
1907  //
1908  // Main loop of Runge-Kutta method
1909  //
1910  while (fabs(S) >= MINSTEP || counter == 0) {
1911 
1912  if(++counter > maxNumIt){
1913  Exception exc("RKTrackRep::RKutta ==> maximum number of iterations exceeded",__LINE__,__FILE__);
1914  exc.setFatal();
1915  throw exc;
1916  }
1917 
1918  if (debugLvl_ > 0) {
1919  debugOut << "------ RKutta main loop nr. " << counter-1 << " ------\n";
1920  }
1921 
1922  M1x3 ABefore = {{ A[0], A[1], A[2] }};
1923  RKPropagate(state7, jacobianT, SA, S, true, calcOnlyLastRowOfJ); // the actual Runge Kutta propagation
1924 
1925  // update paths
1926  coveredDistance += S; // add stepsize to way (signed)
1927  Way += fabs(S);
1928 
1929  double beta = 1/hypot(1, mass*state7[6]/charge);
1930  flightTime += S / beta / 29.9792458; // in ns
1931 
1932  // check way limit
1933  if(Way > Wmax){
1934  std::ostringstream sstream;
1935  sstream << "RKTrackRep::RKutta ==> Total extrapolation length is longer than length limit : " << Way << " cm !";
1936  Exception exc(sstream.str(),__LINE__,__FILE__);
1937  exc.setFatal();
1938  throw exc;
1939  }
1940 
1941  if (onlyOneStep) return(true);
1942 
1943  // if stepsize has been limited by material, break the loop and return. No linear extrapolation!
1944  if (limits.getLowestLimit().first == stp_momLoss) {
1945  if (debugLvl_ > 0) {
1946  debugOut<<" momLossExceeded -> return(true); \n";
1947  }
1948  return(true);
1949  }
1950 
1951  // if stepsize has been limited by material boundary, break the loop and return. No linear extrapolation!
1952  if (limits.getLowestLimit().first == stp_boundary) {
1953  if (debugLvl_ > 0) {
1954  debugOut<<" at boundary -> return(true); \n";
1955  }
1956  return(true);
1957  }
1958 
1959 
1960  // estimate Step for next loop or linear extrapolation
1961  Sl = S; // last S used
1962  limits.removeLimit(stp_fieldCurv);
1963  limits.removeLimit(stp_momLoss);
1964  limits.removeLimit(stp_boundary);
1965  limits.removeLimit(stp_plane);
1966  S = estimateStep(state7, SU, plane, charge, relMomLoss, limits);
1967 
1968  if (limits.getLowestLimit().first == stp_plane &&
1969  fabs(S) < MINSTEP) {
1970  if (debugLvl_ > 0) {
1971  debugOut<<" (at Plane && fabs(S) < MINSTEP) -> break and do linear extrapolation \n";
1972  }
1973  break;
1974  }
1975  if (limits.getLowestLimit().first == stp_momLoss &&
1976  fabs(S) < MINSTEP) {
1977  if (debugLvl_ > 0) {
1978  debugOut<<" (momLossExceeded && fabs(S) < MINSTEP) -> return(true), no linear extrapolation; \n";
1979  }
1980  RKSteps_.erase(RKSteps_.end()-1);
1981  --RKStepsFXStop_;
1982  return(true); // no linear extrapolation!
1983  }
1984 
1985  // check if total angle is bigger than AngleMax. Can happen if a curler should be fitted and it does not hit the active area of the next plane.
1986  double arg = ABefore[0]*A[0] + ABefore[1]*A[1] + ABefore[2]*A[2];
1987  arg = arg > 1 ? 1 : arg;
1988  arg = arg < -1 ? -1 : arg;
1989  deltaAngle += acos(arg);
1990  if (fabs(deltaAngle) > AngleMax){
1991  std::ostringstream sstream;
1992  sstream << "RKTrackRep::RKutta ==> Do not get to an active plane! Already extrapolated " << deltaAngle * 180 / TMath::Pi() << "°.";
1993  Exception exc(sstream.str(),__LINE__,__FILE__);
1994  exc.setFatal();
1995  throw exc;
1996  }
1997 
1998  // check if we went back and forth multiple times -> we don't come closer to the plane!
1999  if (counter > 3){
2000  if (S *RKSteps_.at(counter-1).matStep_.stepSize_ < 0 &&
2001  RKSteps_.at(counter-1).matStep_.stepSize_*RKSteps_.at(counter-2).matStep_.stepSize_ < 0 &&
2002  RKSteps_.at(counter-2).matStep_.stepSize_*RKSteps_.at(counter-3).matStep_.stepSize_ < 0){
2003  Exception exc("RKTrackRep::RKutta ==> Do not get closer to plane!",__LINE__,__FILE__);
2004  exc.setFatal();
2005  throw exc;
2006  }
2007  }
2008 
2009  } //end of main loop
2010 
2011 
2012  //
2013  // linear extrapolation to plane
2014  //
2015  if (limits.getLowestLimit().first == stp_plane) {
2016 
2017  if (fabs(Sl) > 0.001*MINSTEP){
2018  if (debugLvl_ > 0) {
2019  debugOut << " RKutta - linear extrapolation to surface\n";
2020  }
2021  Sl = 1./Sl; // Sl = inverted last Stepsize Sl
2022 
2023  // normalize SA
2024  SA[0]*=Sl; SA[1]*=Sl; SA[2]*=Sl; // SA/Sl = delta A / delta way; local derivative of A with respect to the length of the way
2025 
2026  // calculate A
2027  A[0] += SA[0]*S; // S = distance to surface
2028  A[1] += SA[1]*S; // A = A + S * SA*Sl
2029  A[2] += SA[2]*S;
2030 
2031  // normalize A
2032  CBA = 1./sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]); // 1/|A|
2033  A[0] *= CBA; A[1] *= CBA; A[2] *= CBA;
2034 
2035  R[0] += S*(A[0]-0.5*S*SA[0]); // R = R + S*(A - 0.5*S*SA); approximation for final point on surface
2036  R[1] += S*(A[1]-0.5*S*SA[1]);
2037  R[2] += S*(A[2]-0.5*S*SA[2]);
2038 
2039 
2040  coveredDistance += S;
2041  Way += fabs(S);
2042 
2043  double beta = 1/hypot(1, mass*state7[6]/charge);
2044  flightTime += S / beta / 29.9792458; // in ns;
2045  }
2046  else if (debugLvl_ > 0) {
2047  debugOut << " RKutta - last stepsize too small -> can't do linear extrapolation! \n";
2048  }
2049 
2050  //
2051  // Project Jacobian of extrapolation onto destination plane
2052  //
2053  if (jacobianT != NULL) {
2054 
2055  // projected jacobianT
2056  // x x x x x x 0
2057  // x x x x x x 0
2058  // x x x x x x 0
2059  // x x x x x x 0
2060  // x x x x x x 0
2061  // x x x x x x 0
2062  // x x x x x x 1
2063 
2064  if (checkJacProj && RKSteps_.size()>0){
2065  Exception exc("RKTrackRep::Extrap ==> covariance is projected onto destination plane again",__LINE__,__FILE__);
2066  throw exc;
2067  }
2068 
2069  if (debugLvl_ > 0) {
2070  //debugOut << " Jacobian^T of extrapolation before Projection:\n";
2071  //RKTools::printDim(*jacobianT, 7,7);
2072  debugOut << " Project Jacobian of extrapolation onto destination plane\n";
2073  }
2074  An = A[0]*SU[0] + A[1]*SU[1] + A[2]*SU[2];
2075  An = (fabs(An) > 1.E-7 ? 1./An : 0); // 1/A_normal
2076  double norm;
2077  int i=0;
2078  if (calcOnlyLastRowOfJ)
2079  i = 42;
2080 
2081  double* jacPtr = jacobianT->begin();
2082 
2083  for(unsigned int j=42; j<49; j+=7) {
2084  (*J_MMT_unprojected_lastRow)[j-42] = jacPtr[j];
2085  }
2086 
2087  for(; i<49; i+=7) {
2088  norm = (jacPtr[i]*SU[0] + jacPtr[i+1]*SU[1] + jacPtr[i+2]*SU[2]) * An; // dR_normal / A_normal
2089  jacPtr[i] -= norm*A [0]; jacPtr[i+1] -= norm*A [1]; jacPtr[i+2] -= norm*A [2];
2090  jacPtr[i+3] -= norm*SA[0]; jacPtr[i+4] -= norm*SA[1]; jacPtr[i+5] -= norm*SA[2];
2091  }
2092  checkJacProj = true;
2093 
2094 
2095  if (debugLvl_ > 0) {
2096  //debugOut << " Jacobian^T of extrapolation after Projection:\n";
2097  //RKTools::printDim(*jacobianT, 7,7);
2098  }
2099 
2100  if (!calcOnlyLastRowOfJ) {
2101  for (int iRow = 0; iRow < 3; ++iRow) {
2102  for (int iCol = 0; iCol < 3; ++iCol) {
2103  noiseProjection[iRow*7 + iCol] = (iRow == iCol) - An * SU[iCol] * A[iRow];
2104  noiseProjection[(iRow + 3)*7 + iCol] = - An * SU[iCol] * SA[iRow];
2105  }
2106  }
2107 
2108  // noiseProjection will look like this:
2109  // x x x 0 0 0 0
2110  // x x x 0 0 0 0
2111  // x x x 0 0 0 0
2112  // x x x 1 0 0 0
2113  // x x x 0 1 0 0
2114  // x x x 0 0 1 0
2115  // 0 0 0 0 0 0 1
2116  }
2117 
2118  }
2119  } // end of linear extrapolation to surface
2120 
2121  return(true);
2122 
2123 }
2124 
2125 
2126 double RKTrackRep::estimateStep(const M1x7& state7,
2127  const M1x4& SU,
2128  const DetPlane& plane,
2129  const double& charge,
2130  double& relMomLoss,
2131  StepLimits& limits) const {
2132 
2133  if (useCache_) {
2134  if (cachePos_ >= RKSteps_.size()) {
2135  useCache_ = false;
2136  }
2137  else {
2138  if (RKSteps_.at(cachePos_).limits_.getLowestLimit().first == stp_plane) {
2139  // we need to step exactly to the plane, so don't use the cache!
2140  useCache_ = false;
2141  RKSteps_.erase(RKSteps_.begin() + cachePos_, RKSteps_.end());
2142  }
2143  else {
2144  if (debugLvl_ > 0) {
2145  debugOut << " RKTrackRep::estimateStep: use stepSize " << cachePos_ << " from cache: " << RKSteps_.at(cachePos_).matStep_.stepSize_ << "\n";
2146  }
2147  //for(int n = 0; n < 1*7; ++n) RKSteps_[cachePos_].state7_[n] = state7[n];
2148  ++RKStepsFXStop_;
2149  limits = RKSteps_.at(cachePos_).limits_;
2150  return RKSteps_.at(cachePos_++).matStep_.stepSize_;
2151  }
2152  }
2153  }
2154 
2155  limits.setLimit(stp_sMax, 25.); // max. step allowed [cm]
2156 
2157  if (debugLvl_ > 0) {
2158  debugOut << " RKTrackRep::estimateStep \n";
2159  debugOut << " position: "; TVector3(state7[0], state7[1], state7[2]).Print();
2160  debugOut << " direction: "; TVector3(state7[3], state7[4], state7[5]).Print();
2161  }
2162 
2163  // calculate SL distance to surface
2164  double Dist ( SU[3] - (state7[0]*SU[0] +
2165  state7[1]*SU[1] +
2166  state7[2]*SU[2]) ); // Distance between start coordinates and surface
2167  double An ( state7[3]*SU[0] +
2168  state7[4]*SU[1] +
2169  state7[5]*SU[2] ); // An = dir * N; component of dir normal to surface
2170 
2171  double SLDist; // signed
2172  if (fabs(An) > 1.E-10)
2173  SLDist = Dist/An;
2174  else {
2175  SLDist = Dist*1.E10;
2176  if (An<0) SLDist *= -1.;
2177  }
2178 
2179  limits.setLimit(stp_plane, SLDist);
2180  limits.setStepSign(SLDist);
2181 
2182  if (debugLvl_ > 0) {
2183  debugOut << " Distance to plane: " << Dist << "\n";
2184  debugOut << " SL distance to plane: " << SLDist << "\n";
2185  if (limits.getStepSign()>0)
2186  debugOut << " Direction is pointing towards surface.\n";
2187  else
2188  debugOut << " Direction is pointing away from surface.\n";
2189  }
2190  // DONE calculate SL distance to surface
2191 
2192  //
2193  // Limit according to curvature and magnetic field inhomogenities
2194  // and improve stepsize estimation to reach plane
2195  //
2196  double fieldCurvLimit( limits.getLowestLimitSignedVal() ); // signed
2197  std::pair<double, double> distVsStep (9.E99, 9.E99); // first: smallest straight line distances to plane; second: RK steps
2198 
2199  static const unsigned int maxNumIt = 10;
2200  unsigned int counter(0);
2201 
2202  while (fabs(fieldCurvLimit) > MINSTEP) {
2203 
2204  if(++counter > maxNumIt){
2205  // if max iterations are reached, take a safe value
2206  // (in previous iteration, fieldCurvLimit has been not more than doubled)
2207  // and break.
2208  fieldCurvLimit *= 0.5;
2209  break;
2210  }
2211 
2212  M1x7 state7_temp = state7;
2213  M1x3 SA = {{0, 0, 0}};
2214 
2215  double q ( RKPropagate(state7_temp, NULL, SA, fieldCurvLimit, true) );
2216  if (debugLvl_ > 0) {
2217  debugOut << " maxStepArg = " << fieldCurvLimit << "; q = " << q << " \n";
2218  }
2219 
2220  // remember steps and resulting SL distances to plane for stepsize improvement
2221  // calculate distance to surface
2222  Dist = SU[3] - (state7_temp[0] * SU[0] +
2223  state7_temp[1] * SU[1] +
2224  state7_temp[2] * SU[2]); // Distance between position and surface
2225 
2226  An = state7_temp[3] * SU[0] +
2227  state7_temp[4] * SU[1] +
2228  state7_temp[5] * SU[2]; // An = dir * N; component of dir normal to surface
2229 
2230  if (fabs(Dist/An) < fabs(distVsStep.first)) {
2231  distVsStep.first = Dist/An;
2232  distVsStep.second = fieldCurvLimit;
2233  }
2234 
2235  // resize limit according to q never grow step size more than
2236  // two-fold to avoid infinite grow-shrink loops with strongly
2237  // inhomogeneous fields.
2238  if (q>2) {
2239  fieldCurvLimit *= 2;
2240  break;
2241  }
2242 
2243  fieldCurvLimit *= q * 0.95;
2244 
2245  if (fabs(q-1) < 0.25 || // good enough!
2246  fabs(fieldCurvLimit) > limits.getLowestLimitVal()) // other limits are lower!
2247  break;
2248  }
2249  if (fabs(fieldCurvLimit) < MINSTEP)
2250  limits.setLimit(stp_fieldCurv, MINSTEP);
2251  else
2252  limits.setLimit(stp_fieldCurv, fieldCurvLimit);
2253 
2254  double stepToPlane(limits.getLimitSigned(stp_plane));
2255  if (fabs(distVsStep.first) < 8.E99) {
2256  stepToPlane = distVsStep.first + distVsStep.second;
2257  }
2258  limits.setLimit(stp_plane, stepToPlane);
2259 
2260 
2261  //
2262  // Select direction
2263  //
2264  // auto select
2265  if (propDir_ == 0 || !plane.isFinite()){
2266  if (debugLvl_ > 0) {
2267  debugOut << " auto select direction";
2268  if (!plane.isFinite()) debugOut << ", plane is not finite";
2269  debugOut << ".\n";
2270  }
2271  }
2272  // see if straight line approximation is ok
2273  else if ( limits.getLimit(stp_plane) < 0.2*limits.getLimit(stp_fieldCurv) ){
2274  if (debugLvl_ > 0) {
2275  debugOut << " straight line approximation is fine.\n";
2276  }
2277 
2278  // if direction is pointing to active part of surface
2279  if( plane.isInActive(state7[0], state7[1], state7[2], state7[3], state7[4], state7[5]) ) {
2280  if (debugLvl_ > 0) {
2281  debugOut << " direction is pointing to active part of surface. \n";
2282  }
2283  }
2284  // if we are near the plane, but not pointing to the active area, make a big step!
2285  else {
2286  limits.removeLimit(stp_plane);
2287  limits.setStepSign(propDir_);
2288  if (debugLvl_ > 0) {
2289  debugOut << " we are near the plane, but not pointing to the active area. make a big step! \n";
2290  }
2291  }
2292  }
2293  // propDir_ is set and we are not pointing to an active part of a plane -> propDir_ decides!
2294  else {
2295  if (limits.getStepSign() * propDir_ < 0){
2296  limits.removeLimit(stp_plane);
2297  limits.setStepSign(propDir_);
2298  if (debugLvl_ > 0) {
2299  debugOut << " invert Step according to propDir_ and make a big step. \n";
2300  }
2301  }
2302  }
2303 
2304 
2305  // call stepper and reduce stepsize if step not too small
2306  static const RKStep defaultRKStep;
2307  RKSteps_.push_back( defaultRKStep );
2308  std::vector<RKStep>::iterator lastStep = RKSteps_.end() - 1;
2309  lastStep->state7_ = state7;
2310  ++RKStepsFXStop_;
2311 
2312  if(limits.getLowestLimitVal() > MINSTEP){ // only call stepper if step estimation big enough
2313  M1x7 state7_temp = {{ state7[0], state7[1], state7[2], state7[3], state7[4], state7[5], state7[6] }};
2314 
2316  state7_temp,
2317  charge/state7[6], // |p|
2318  relMomLoss,
2319  pdgCode_,
2320  lastStep->matStep_.materialProperties_,
2321  limits,
2322  true);
2323  } else { //assume material has not changed
2324  if (RKSteps_.size()>1) {
2325  lastStep->matStep_.materialProperties_ = (lastStep - 1)->matStep_.materialProperties_;
2326  }
2327  }
2328 
2329  if (debugLvl_ > 0) {
2330  debugOut << " final limits:\n";
2331  limits.Print();
2332  }
2333 
2334  double finalStep = limits.getLowestLimitSignedVal();
2335 
2336  lastStep->matStep_.stepSize_ = finalStep;
2337  lastStep->limits_ = limits;
2338 
2339  if (debugLvl_ > 0) {
2340  debugOut << " --> Step to be used: " << finalStep << "\n";
2341  }
2342 
2343  return finalStep;
2344 
2345 }
2346 
2347 
2348 TVector3 RKTrackRep::pocaOnLine(const TVector3& linePoint, const TVector3& lineDirection, const TVector3& point) const {
2349 
2350  TVector3 retVal(lineDirection);
2351 
2352  double t = 1./(retVal.Mag2()) * ((point*retVal) - (linePoint*retVal));
2353  retVal *= t;
2354  retVal += linePoint;
2355  return retVal; // = linePoint + t*lineDirection
2356 
2357 }
2358 
2359 
2360 double RKTrackRep::Extrap(const DetPlane& startPlane,
2361  const DetPlane& destPlane,
2362  double charge,
2363  double mass,
2364  bool& isAtBoundary,
2365  M1x7& state7,
2366  double& flightTime,
2367  bool fillExtrapSteps,
2368  TMatrixDSym* cov, // 5D
2369  bool onlyOneStep,
2370  bool stopAtBoundary,
2371  double maxStep) const
2372 {
2373 
2374  static const unsigned int maxNumIt(500);
2375  unsigned int numIt(0);
2376 
2377  double coveredDistance(0.);
2378  double dqop(0.);
2379 
2380  const TVector3 W(destPlane.getNormal());
2381  M1x4 SU = {{W.X(), W.Y(), W.Z(), destPlane.distance(0., 0., 0.)}};
2382 
2383  // make SU vector point away from origin
2384  if (W*destPlane.getO() < 0) {
2385  SU[0] *= -1;
2386  SU[1] *= -1;
2387  SU[2] *= -1;
2388  }
2389 
2390 
2391  M1x7 startState7 = state7;
2392 
2393  while(true){
2394 
2395  if (debugLvl_ > 0) {
2396  debugOut << "\n============ RKTrackRep::Extrap loop nr. " << numIt << " ============\n";
2397  debugOut << "Start plane: "; startPlane.Print();
2398  debugOut << "fillExtrapSteps " << fillExtrapSteps << "\n";
2399  }
2400 
2401  if(++numIt > maxNumIt){
2402  Exception exc("RKTrackRep::Extrap ==> maximum number of iterations exceeded",__LINE__,__FILE__);
2403  exc.setFatal();
2404  throw exc;
2405  }
2406 
2407  // initialize jacobianT with unit matrix
2408  for(int i = 0; i < 7*7; ++i) J_MMT_[i] = 0;
2409  for(int i=0; i<7; ++i) J_MMT_[8*i] = 1.;
2410 
2411  M7x7* noise = NULL;
2412  isAtBoundary = false;
2413 
2414  // propagation
2415  bool checkJacProj = false;
2416  limits_.reset();
2417  limits_.setLimit(stp_sMaxArg, maxStep-fabs(coveredDistance));
2418 
2419  M1x7 J_MMT_unprojected_lastRow = {{0, 0, 0, 0, 0, 0, 1}};
2420 
2421  if( ! RKutta(SU, destPlane, charge, mass, state7, &J_MMT_, &J_MMT_unprojected_lastRow,
2422  coveredDistance, flightTime, checkJacProj, noiseProjection_,
2423  limits_, onlyOneStep, !fillExtrapSteps) ) {
2424  Exception exc("RKTrackRep::Extrap ==> Runge Kutta propagation failed",__LINE__,__FILE__);
2425  exc.setFatal();
2426  throw exc;
2427  }
2428 
2429  bool atPlane(limits_.getLowestLimit().first == stp_plane);
2430  if (limits_.getLowestLimit().first == stp_boundary)
2431  isAtBoundary = true;
2432 
2433 
2434  if (debugLvl_ > 0) {
2435  debugOut<<"RKSteps \n";
2436  for (std::vector<RKStep>::iterator it = RKSteps_.begin(); it != RKSteps_.end(); ++it){
2437  debugOut << "stepSize = " << it->matStep_.stepSize_ << "\t";
2438  it->matStep_.materialProperties_.Print();
2439  }
2440  debugOut<<"\n";
2441  }
2442 
2443 
2444 
2445  // call MatFX
2446  if(fillExtrapSteps) {
2447  noise = &noiseArray_;
2448  for(int i = 0; i < 7*7; ++i) noiseArray_[i] = 0; // set noiseArray_ to 0
2449  }
2450 
2451  unsigned int nPoints(RKStepsFXStop_ - RKStepsFXStart_);
2452  if ( nPoints>0){
2453  // momLoss has a sign - negative loss means momentum gain
2454  double momLoss = MaterialEffects::getInstance()->effects(RKSteps_,
2457  fabs(charge/state7[6]), // momentum
2458  pdgCode_,
2459  noise);
2460 
2462 
2463  if (debugLvl_ > 0) {
2464  debugOut << "momLoss: " << momLoss << " GeV; relative: " << momLoss/fabs(charge/state7[6])
2465  << "; coveredDistance = " << coveredDistance << "\n";
2466  if (debugLvl_ > 1 && noise != NULL) {
2467  debugOut << "7D noise: \n";
2468  RKTools::printDim(noise->begin(), 7, 7);
2469  }
2470  }
2471 
2472  // do momLoss only for defined 1/momentum .ne.0
2473  if(fabs(state7[6])>1.E-10) {
2474 
2475  if (debugLvl_ > 0) {
2476  debugOut << "correct state7 with dx/dqop, dy/dqop ...\n";
2477  }
2478 
2479  dqop = charge/(fabs(charge/state7[6])-momLoss) - state7[6];
2480 
2481  // Correct coveredDistance and flightTime and momLoss if checkJacProj == true
2482  // The idea is to calculate the state correction (based on the mometum loss) twice:
2483  // Once with the unprojected Jacobian (which preserves coveredDistance),
2484  // and once with the projected Jacobian (which is constrained to the plane and does NOT preserve coveredDistance).
2485  // The difference of these two corrections can then be used to calculate a correction factor.
2486  if (checkJacProj && fabs(coveredDistance) > MINSTEP) {
2487  M1x3 state7_correction_unprojected = {{0, 0, 0}};
2488  for (unsigned int i=0; i<3; ++i) {
2489  state7_correction_unprojected[i] = 0.5 * dqop * J_MMT_unprojected_lastRow[i];
2490  //debugOut << "J_MMT_unprojected_lastRow[i] " << J_MMT_unprojected_lastRow[i] << "\n";
2491  //debugOut << "state7_correction_unprojected[i] " << state7_correction_unprojected[i] << "\n";
2492  }
2493 
2494  M1x3 state7_correction_projected = {{0, 0, 0}};
2495  for (unsigned int i=0; i<3; ++i) {
2496  state7_correction_projected[i] = 0.5 * dqop * J_MMT_[6*7 + i];
2497  //debugOut << "J_MMT_[6*7 + i] " << J_MMT_[6*7 + i] << "\n";
2498  //debugOut << "state7_correction_projected[i] " << state7_correction_projected[i] << "\n";
2499  }
2500 
2501  // delta distance
2502  M1x3 delta_state = {{0, 0, 0}};
2503  for (unsigned int i=0; i<3; ++i) {
2504  delta_state[i] = state7_correction_unprojected[i] - state7_correction_projected[i];
2505  }
2506 
2507  double Dist( sqrt(delta_state[0]*delta_state[0]
2508  + delta_state[1]*delta_state[1]
2509  + delta_state[2]*delta_state[2] ) );
2510 
2511  // sign: delta * a
2512  if (delta_state[0]*state7[3] + delta_state[1]*state7[4] + delta_state[2]*state7[5] > 0)
2513  Dist *= -1.;
2514 
2515  double correctionFactor( 1. + Dist / coveredDistance );
2516  flightTime *= correctionFactor;
2517  momLoss *= correctionFactor;
2518  coveredDistance = coveredDistance + Dist;
2519 
2520  if (debugLvl_ > 0) {
2521  debugOut << "correctionFactor-1 = " << correctionFactor-1. << "; Dist = " << Dist << "\n";
2522  debugOut << "corrected momLoss: " << momLoss << " GeV; relative: " << momLoss/fabs(charge/state7[6])
2523  << "; corrected coveredDistance = " << coveredDistance << "\n";
2524  }
2525  }
2526 
2527  // correct state7 with dx/dqop, dy/dqop ... Greatly improves extrapolation accuracy!
2528  double qop( charge/(fabs(charge/state7[6])-momLoss) );
2529  dqop = qop - state7[6];
2530  state7[6] = qop;
2531 
2532  for (unsigned int i=0; i<6; ++i) {
2533  state7[i] += 0.5 * dqop * J_MMT_[6*7 + i];
2534  }
2535  // normalize direction, just to make sure
2536  double norm( 1. / sqrt(state7[3]*state7[3] + state7[4]*state7[4] + state7[5]*state7[5]) );
2537  for (unsigned int i=3; i<6; ++i)
2538  state7[i] *= norm;
2539 
2540  }
2541  } // finished MatFX
2542 
2543 
2544  // fill ExtrapSteps_
2545  if (fillExtrapSteps) {
2546  static const ExtrapStep defaultExtrapStep;
2547  ExtrapSteps_.push_back(defaultExtrapStep);
2548  std::vector<ExtrapStep>::iterator lastStep = ExtrapSteps_.end() - 1;
2549 
2550  // Store Jacobian of this step for final calculation.
2551  lastStep->jac7_ = J_MMT_;
2552 
2553  if( checkJacProj == true ){
2554  //project the noise onto the destPlane
2556 
2557  if (debugLvl_ > 1) {
2558  debugOut << "7D noise projected onto plane: \n";
2560  }
2561  }
2562 
2563  // Store this step's noise for final calculation.
2564  lastStep->noise7_ = noiseArray_;
2565 
2566  if (debugLvl_ > 2) {
2567  debugOut<<"ExtrapSteps \n";
2568  for (std::vector<ExtrapStep>::iterator it = ExtrapSteps_.begin(); it != ExtrapSteps_.end(); ++it){
2569  debugOut << "7D Jacobian: "; RKTools::printDim((it->jac7_.begin()), 5,5);
2570  debugOut << "7D noise: "; RKTools::printDim((it->noise7_.begin()), 5,5);
2571  }
2572  debugOut<<"\n";
2573  }
2574  }
2575 
2576 
2577 
2578  // check if at boundary
2579  if (stopAtBoundary) {
2580  if (debugLvl_ > 0) {
2581  debugOut << "stopAtBoundary -> break; \n ";
2582  }
2583  break;
2584  }
2585 
2586  if (onlyOneStep) {
2587  if (debugLvl_ > 0) {
2588  debugOut << "onlyOneStep -> break; \n ";
2589  }
2590  break;
2591  }
2592 
2593  //break if we arrived at destPlane
2594  if(atPlane) {
2595  if (debugLvl_ > 0) {
2596  debugOut << "arrived at destPlane with a distance of " << destPlane.distance(state7[0], state7[1], state7[2]) << " cm left. ";
2597  if (destPlane.isInActive(state7[0], state7[1], state7[2], state7[3], state7[4], state7[5]))
2598  debugOut << "In active area of destPlane. \n";
2599  else
2600  debugOut << "NOT in active area of plane. \n";
2601 
2602  debugOut << " position: "; TVector3(state7[0], state7[1], state7[2]).Print();
2603  debugOut << " direction: "; TVector3(state7[3], state7[4], state7[5]).Print();
2604  }
2605  break;
2606  }
2607 
2608  }
2609 
2610  if (fillExtrapSteps) {
2611  // propagate cov and add noise
2612  calcForwardJacobianAndNoise(startState7, startPlane, state7, destPlane);
2613 
2614  if (cov != NULL) {
2615  cov->Similarity(fJacobian_);
2616  *cov += fNoise_;
2617  }
2618 
2619  if (debugLvl_ > 0) {
2620  if (cov != NULL) {
2621  debugOut << "final covariance matrix after Extrap: "; cov->Print();
2622  }
2623  }
2624  }
2625 
2626  return coveredDistance;
2627 }
2628 
2629 
2630 void RKTrackRep::checkCache(const StateOnPlane& state, const SharedPlanePtr* plane) const {
2631 
2632  if (state.getRep() != this){
2633  Exception exc("RKTrackRep::checkCache ==> state is defined wrt. another TrackRep",__LINE__,__FILE__);
2634  exc.setFatal();
2635  throw exc;
2636  }
2637 
2638  if (dynamic_cast<const MeasurementOnPlane*>(&state) != NULL) {
2639  Exception exc("RKTrackRep::checkCache - cannot extrapolate MeasurementOnPlane",__LINE__,__FILE__);
2640  exc.setFatal();
2641  throw exc;
2642  }
2643 
2644  cachePos_ = 0;
2645  RKStepsFXStart_ = 0;
2646  RKStepsFXStop_ = 0;
2647  ExtrapSteps_.clear();
2648  initArrays();
2649 
2650 
2651  if (plane &&
2653  lastEndState_.getPlane() &&
2654  state.getPlane() == lastStartState_.getPlane() &&
2655  state.getState() == lastStartState_.getState() &&
2656  (*plane)->distance(getPos(lastEndState_)) <= MINSTEP) {
2657  useCache_ = true;
2658 
2659  // clean up cache. Only use steps with same sign.
2660  double firstStep(0);
2661  for (unsigned int i=0; i<RKSteps_.size(); ++i) {
2662  if (i == 0) {
2663  firstStep = RKSteps_.at(0).matStep_.stepSize_;
2664  continue;
2665  }
2666  if (RKSteps_.at(i).matStep_.stepSize_ * firstStep < 0) {
2667  if (RKSteps_.at(i-1).matStep_.materialProperties_ == RKSteps_.at(i).matStep_.materialProperties_) {
2668  RKSteps_.at(i-1).matStep_.stepSize_ += RKSteps_.at(i).matStep_.stepSize_;
2669  }
2670  RKSteps_.erase(RKSteps_.begin()+i, RKSteps_.end());
2671  }
2672  }
2673 
2674  if (debugLvl_ > 0) {
2675  debugOut << "RKTrackRep::checkCache: use cached material and step values.\n";
2676  }
2677  }
2678  else {
2679 
2680  if (debugLvl_ > 0) {
2681  debugOut << "RKTrackRep::checkCache: can NOT use cached material and step values.\n";
2682 
2683  if (plane != NULL) {
2684  if (state.getPlane() != lastStartState_.getPlane()) {
2685  debugOut << "state.getPlane() != lastStartState_.getPlane()\n";
2686  }
2687  else {
2688  if (! (state.getState() == lastStartState_.getState())) {
2689  debugOut << "state.getState() != lastStartState_.getState()\n";
2690  }
2691  else if (lastEndState_.getPlane().get() != NULL) {
2692  debugOut << "distance " << (*plane)->distance(getPos(lastEndState_)) << "\n";
2693  }
2694  }
2695  }
2696  }
2697 
2698  useCache_ = false;
2699  RKSteps_.clear();
2700 
2701  lastStartState_.setStatePlane(state.getState(), state.getPlane());
2702  }
2703 }
2704 
2705 
2706 double RKTrackRep::momMag(const M1x7& state7) const {
2707  // FIXME given this interface this function cannot work for charge =/= +-1
2708  return fabs(1/state7[6]);
2709 }
2710 
2711 
2713  if (dynamic_cast<const RKTrackRep*>(other) == NULL)
2714  return false;
2715 
2716  return true;
2717 }
2718 
2719 
2720 bool RKTrackRep::isSame(const AbsTrackRep* other) {
2721  if (getPDG() != other->getPDG())
2722  return false;
2723 
2724  return isSameType(other);
2725 }
2726 
2727 
2728 void RKTrackRep::Streamer(TBuffer &R__b)
2729 {
2730  // Stream an object of class genfit::RKTrackRep.
2731 
2732  //This works around a msvc bug and should be harmless on other platforms
2733  typedef ::genfit::RKTrackRep thisClass;
2734  UInt_t R__s, R__c;
2735  if (R__b.IsReading()) {
2736  ::genfit::AbsTrackRep::Streamer(R__b);
2737  Version_t R__v = R__b.ReadVersion(&R__s, &R__c); if (R__v) { }
2738  R__b.CheckByteCount(R__s, R__c, thisClass::IsA());
2739  lastStartState_.setRep(this);
2740  lastEndState_.setRep(this);
2741  } else {
2742  ::genfit::AbsTrackRep::Streamer(R__b);
2743  R__c = R__b.WriteVersion(thisClass::IsA(), kTRUE);
2744  R__b.SetByteCount(R__c, kTRUE);
2745  }
2746 }
2747 
2748 } /* End of namespace genfit */
void transformPM7(const MeasuredStateOnPlane &state, M7x7 &out7x7) const
Definition: RKTrackRep.cc:1568
void Np_N_NpT(const M7x7 &Np, M7x7 &N)
Definition: RKTools.cc:637
void calcForwardJacobianAndNoise(const M1x7 &startState7, const DetPlane &startPlane, const M1x7 &destState7, const DetPlane &destPlane) const
Definition: RKTrackRep.cc:931
Helper to store different limits on the stepsize for the RKTRackRep.
Definition: StepLimits.h:54
const AbsTrackRep * getRep() const
Definition: StateOnPlane.h:66
void stepper(const RKTrackRep *rep, M1x7 &state7, const double &mom, double &relMomLoss, const int &pdg, MaterialProperties &currentMaterial, StepLimits &limits, bool varField=true)
Returns maximum length so that a specified momentum loss will not be exceeded.
virtual TVector3 getPos(const StateOnPlane &state) const
Get the cartesian position of a state.
Definition: RKTrackRep.cc:816
void setNormal(const TVector3 &n)
Definition: DetPlane.cc:165
M7x7 noiseProjection_
noise matrix of the last extrapolation
Definition: RKTrackRep.h:308
char propDir_
propagation direction (-1, 0, 1) -> (backward, auto, forward)
Definition: AbsTrackRep.h:366
StepLimits limits_
Definition: RKTrackRep.h:306
void setStatePlane(const TVectorD &state, const SharedPlanePtr &plane)
Definition: StateOnPlane.h:70
#define MINSTEP
Definition: RKTrackRep.cc:35
TVector3 pocaOnLine(const TVector3 &linePoint, const TVector3 &lineDirection, const TVector3 &point) const
Definition: RKTrackRep.cc:2348
boost::shared_ptr< genfit::DetPlane > SharedPlanePtr
Shared Pointer to a DetPlane.
double * end()
Definition: RKTools.h:46
double getLowestLimitSignedVal(double margin=1.E-3) const
Get the numerical value of the lowest limit, signed with stepSign_.
Definition: StepLimits.h:80
virtual void getPosMomCov(const MeasuredStateOnPlane &state, TVector3 &pos, TVector3 &mom, TMatrixDSym &cov) const
Translates MeasuredStateOnPlane into 3D position, momentum and 6x6 covariance.
Definition: RKTrackRep.cc:844
double momMag(const M1x7 &state7) const
Definition: RKTrackRep.cc:2706
virtual double extrapolateToPlane(StateOnPlane &state, const SharedPlanePtr &plane, bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state to plane, and returns the extrapolation length and, via reference, the extrapolated state.
Definition: RKTrackRep.cc:80
double RKPropagate(M1x7 &state7, M7x7 *jacobian, M1x3 &SA, double S, bool varField=true, bool calcOnlyLastRowOfJ=false) const
The actual Runge Kutta propagation.
Definition: RKTrackRep.cc:1272
bool RKutta(const M1x4 &SU, const DetPlane &plane, double charge, double mass, M1x7 &state7, M7x7 *jacobianT, M1x7 *J_MMT_unprojected_lastRow, double &coveredDistance, double &flightTime, bool &checkJacProj, M7x7 &noiseProjection, StepLimits &limits, bool onlyOneStep=false, bool calcOnlyLastRowOfJ=false) const
Propagates the particle through the magnetic field.
Definition: RKTrackRep.cc:1852
virtual double extrapolateBy(StateOnPlane &state, double step, bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state by step (cm) and returns the extrapolation length and, via reference...
Definition: RKTrackRep.cc:721
double getMass(const StateOnPlane &state) const
Get tha particle mass in GeV/c^2.
Definition: AbsTrackRep.cc:100
void checkCache(const StateOnPlane &state, const SharedPlanePtr *plane) const
Definition: RKTrackRep.cc:2630
virtual ~RKTrackRep()
Definition: RKTrackRep.cc:75
const TVectorD & getState() const
Definition: StateOnPlane.h:61
virtual double extrapolateToSphere(StateOnPlane &state, double radius, const TVector3 &point=TVector3(0., 0., 0.), bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state to the sphere surface, and returns the extrapolation length and...
Definition: RKTrackRep.cc:610
virtual void setPosMom(StateOnPlane &state, const TVector3 &pos, const TVector3 &mom) const
Set position and momentum of state.
Definition: RKTrackRep.cc:1081
double distance(const TVector3 &point) const
absolute distance from a point to the plane
Definition: DetPlane.cc:267
void J_MpTxcov6xJ_Mp(const M6x5 &J_Mp, const M6x6 &cov6, M5x5 &out5)
Definition: RKTools.cc:290
void transformM6P(const M6x6 &in6x6, const M1x7 &state7, MeasuredStateOnPlane &state) const
Definition: RKTrackRep.cc:1770
const TVectorD & getAuxInfo() const
Definition: StateOnPlane.h:63
void calcJ_pM_5x7(M5x7 &J_pM, const TVector3 &U, const TVector3 &V, const M1x3 &pTilde, double spu) const
Definition: RKTrackRep.cc:1595
void printDim(const double *mat, unsigned int dimX, unsigned int dimY)
Definition: RKTools.cc:710
int RKStepsFXStart_
RungeKutta steps made in the last extrapolation.
Definition: RKTrackRep.h:294
void setPlane(const SharedPlanePtr &plane)
Definition: StateOnPlane.h:69
virtual double getRadiationLenght() const
Get the accumulated X/X0 (path / radiation length) of the material crossed in the last extrapolation...
Definition: RKTrackRep.cc:1061
TMatrixD fJacobian_
steps made in Extrap during last extrapolation
Definition: RKTrackRep.h:298
virtual void setPosMomErr(MeasuredStateOnPlane &state, const TVector3 &pos, const TVector3 &mom, const TVector3 &posErr, const TVector3 &momErr) const
Set position and momentum and error of state.
Definition: RKTrackRep.cc:1160
std::ostream debugOut
double getLimitSigned(StepLimitType type) const
Definition: StepLimits.h:64
void initArrays() const
Definition: RKTrackRep.cc:1478
TVector3 getFieldVal(const TVector3 &position)
This does NOT use the cache!
Definition: FieldManager.h:63
void setSpu(StateOnPlane &state, double spu) const
Definition: RKTrackRep.cc:1260
const TVector3 & getV() const
Definition: DetPlane.h:87
void setLimit(StepLimitType type, double value)
absolute of value will be taken! If limit is already lower, it will be set to value anyway...
Definition: StepLimits.h:89
Helper for RKTrackRep.
Definition: RKTrackRep.h:53
void Print(const Option_t *="") const
Definition: DetPlane.cc:219
void setRep(const AbsTrackRep *rep)
Definition: StateOnPlane.h:72
std::pair< StepLimitType, double > getLowestLimit(double margin=1.E-3) const
Get the lowest limit.
Definition: StepLimits.cc:44
void setTime(StateOnPlane &state, double time) const
Set time at which the state was defined.
Definition: RKTrackRep.cc:1265
StateOnPlane lastEndState_
state where the last extrapolation has started
Definition: RKTrackRep.h:292
virtual double extrapolateToLine(StateOnPlane &state, const TVector3 &linePoint, const TVector3 &lineDirection, bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state to the POCA to a line, and returns the extrapolation length and...
Definition: RKTrackRep.cc:135
virtual bool isSameType(const AbsTrackRep *other)
check if other is of same type (e.g. RKTrackRep).
Definition: RKTrackRep.cc:2712
static FieldManager * getInstance()
Get singleton instance.
Definition: FieldManager.h:112
StateOnPlane with additional covariance matrix.
virtual double getCharge(const StateOnPlane &state) const
Get the (fitted) charge of a state. This is not always equal the pdg charge (e.g. if the charge sign ...
Definition: RKTrackRep.cc:859
virtual TVector3 getMom(const StateOnPlane &state) const
Get the cartesian momentum vector of a state.
Definition: RKTrackRep.cc:824
virtual TMatrixDSym get6DCov(const MeasuredStateOnPlane &state) const
Get the 6D covariance.
Definition: RKTrackRep.cc:851
double * begin()
Definition: RKTools.h:45
unsigned int cachePos_
use cached RKSteps_ for extrapolation
Definition: RKTrackRep.h:302
void setFatal(bool b=true)
Set fatal flag.
Definition: Exception.h:61
bool isFinite() const
Definition: DetPlane.h:171
virtual void getPosMom(const StateOnPlane &state, TVector3 &pos, TVector3 &mom) const
Get cartesian position and momentum vector of a state.
Definition: RKTrackRep.cc:834
virtual void getBackwardJacobianAndNoise(TMatrixD &jacobian, TMatrixDSym &noise, TVectorD &deltaState) const
Get the jacobian and noise matrix of the last extrapolation if it would have been done in opposite di...
Definition: RKTrackRep.cc:1001
Abstract base class for a track representation.
Definition: AbsTrackRep.h:66
virtual void setPosMomCov(MeasuredStateOnPlane &state, const TVector3 &pos, const TVector3 &mom, const TMatrixDSym &cov6x6) const
Set position, momentum and covariance of state.
Definition: RKTrackRep.cc:1202
double estimateStep(const M1x7 &state7, const M1x4 &SU, const DetPlane &plane, const double &charge, double &relMomLoss, StepLimits &limits) const
Definition: RKTrackRep.cc:2126
double getSpu(const StateOnPlane &state) const
Definition: RKTrackRep.cc:905
void setU(const TVector3 &u)
Definition: DetPlane.cc:115
Helper for RKTrackRep.
Definition: RKTrackRep.h:39
virtual double extrapolateToCone(StateOnPlane &state, double radius, const TVector3 &linePoint=TVector3(0., 0., 0.), const TVector3 &lineDirection=TVector3(0., 0., 1.), bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state to the cone surface, and returns the extrapolation length and, via reference, the extrapolated state.
Definition: RKTrackRep.cc:476
void getState7(const StateOnPlane &state, M1x7 &state7) const
Definition: RKTrackRep.cc:1497
double getTime(const StateOnPlane &state) const
Get the time corresponding to the StateOnPlane. Extrapolation.
Definition: RKTrackRep.cc:921
Exception class for error handling in GENFIT (provides storage for diagnostic information) ...
Definition: Exception.h:48
void calcJ_Mp_7x5(M7x5 &J_Mp, const TVector3 &U, const TVector3 &V, const TVector3 &W, const M1x3 &A) const
Definition: RKTrackRep.cc:1725
void removeLimit(StepLimitType type)
Definition: StepLimits.h:95
std::vector< genfit::MatStep > getSteps() const
Get stepsizes and material properties of crossed materials of the last extrapolation.
Definition: RKTrackRep.cc:1041
TMatrixDSym fNoise_
Definition: RKTrackRep.h:299
StateOnPlane lastStartState_
Definition: RKTrackRep.h:291
void getState5(StateOnPlane &state, const M1x7 &state7) const
Definition: RKTrackRep.cc:1531
void J_MpTxcov7xJ_Mp(const M7x5 &J_Mp, const M7x7 &cov7, M5x5 &out5)
Definition: RKTools.cc:209
int pdgCode_
Particle code.
Definition: AbsTrackRep.h:364
unsigned int debugLvl_
Definition: AbsTrackRep.h:368
double getPDGCharge() const
Get the charge of the particle of the pdg code.
Definition: AbsTrackRep.cc:93
const TVector3 & getU() const
Definition: DetPlane.h:86
double Extrap(const DetPlane &startPlane, const DetPlane &destPlane, double charge, double mass, bool &isAtBoundary, M1x7 &state7, double &flightTime, bool fillExtrapSteps, TMatrixDSym *cov=nullptr, bool onlyOneStep=false, bool stopAtBoundary=false, double maxStep=1.E99) const
Handles propagation and material effects.
Definition: RKTrackRep.cc:2360
virtual double getMomVar(const MeasuredStateOnPlane &state) const
get the variance of the absolute value of the momentum .
Definition: RKTrackRep.cc:885
virtual bool isSame(const AbsTrackRep *other)
check if other is of same type (e.g. RKTrackRep) and has same pdg code.
Definition: RKTrackRep.cc:2720
const SharedPlanePtr & getPlane() const
Definition: StateOnPlane.h:65
Detector plane.
Definition: DetPlane.h:61
A state with arbitrary dimension defined in a DetPlane.
Definition: StateOnPlane.h:45
virtual double getMomMag(const StateOnPlane &state) const
get the magnitude of the momentum in GeV.
Definition: RKTrackRep.cc:877
std::vector< RKStep > RKSteps_
state where the last extrapolation has ended
Definition: RKTrackRep.h:293
void setStepSign(char signedVal)
sets stepSign_ to sign of signedVal
Definition: StepLimits.cc:93
virtual void getForwardJacobianAndNoise(TMatrixD &jacobian, TMatrixDSym &noise, TVectorD &deltaState) const
Get the jacobian and noise matrix of the last extrapolation.
Definition: RKTrackRep.cc:977
virtual double extrapToPoint(StateOnPlane &state, const TVector3 &point, const TMatrixDSym *G=NULL, bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Definition: RKTrackRep.cc:235
void J_pMTxcov5xJ_pM(const M5x7 &J_pM, const M5x5 &cov5, M7x7 &out7)
Definition: RKTools.cc:34
const TVector3 & getO() const
Definition: DetPlane.h:85
bool isInActive(const TVector3 &point, const TVector3 &dir) const
intersect in the active area? C.f. AbsFinitePlane
Definition: DetPlane.h:146
static MaterialEffects * getInstance()
virtual double extrapolateToCylinder(StateOnPlane &state, double radius, const TVector3 &linePoint=TVector3(0., 0., 0.), const TVector3 &lineDirection=TVector3(0., 0., 1.), bool stopAtBoundary=false, bool calcJacobianNoise=false) const
Extrapolates the state to the cylinder surface, and returns the extrapolation length and...
Definition: RKTrackRep.cc:351
char getStepSign() const
Definition: StepLimits.h:84
double getLimit(StepLimitType type) const
Get limit of type. If that limit has not yet been set, return max double value.
Definition: StepLimits.h:63
const TMatrixDSym & getCov() const
TVector3 getNormal() const
Definition: DetPlane.cc:156
void transformPM6(const MeasuredStateOnPlane &state, M6x6 &out6x6) const
Definition: RKTrackRep.cc:1640
double getLowestLimitVal(double margin=1.E-3) const
Get the unsigned numerical value of the lowest limit.
Definition: StepLimits.cc:65
int getPDG() const
Get the pdg code.
Definition: AbsTrackRep.h:272
std::vector< ExtrapStep > ExtrapSteps_
Definition: RKTrackRep.h:296
void J_pMTTxJ_MMTTxJ_MpTT(const M7x5 &J_pMT, const M7x7 &J_MMT, const M5x7 &J_MpT, M5x5 &J_pp)
Definition: RKTools.cc:527
void transformM7P(const M7x7 &in7x7, const M1x7 &state7, MeasuredStateOnPlane &state) const
Definition: RKTrackRep.cc:1702
virtual void setChargeSign(StateOnPlane &state, double charge) const
Set the sign of the charge according to charge.
Definition: RKTrackRep.cc:1246
double effects(const std::vector< RKStep > &steps, int materialsFXStart, int materialsFXStop, const double &mom, const int &pdg, M7x7 *noise=nullptr)
Calculates energy loss in the traveled path, optional calculation of noise matrix.
Defines for I/O streams used for error and debug printing.