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