EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
AtlasStepper.hpp
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file AtlasStepper.hpp
1 // This file is part of the Acts project.
2 //
3 // Copyright (C) 2016-2020 CERN for the benefit of the Acts project
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
8 
9 #pragma once
10 
11 // Workaround for building on clang+libstdc++
12 #include "Acts/Utilities/detail/ReferenceWrapperAnyCompat.hpp"
13 
14 #include "Acts/EventData/TrackParameters.hpp"
15 #include "Acts/EventData/detail/TransformationBoundToFree.hpp"
16 #include "Acts/Geometry/GeometryContext.hpp"
17 #include "Acts/MagneticField/MagneticFieldContext.hpp"
18 #include "Acts/Propagator/ConstrainedStep.hpp"
19 #include "Acts/Propagator/detail/SteppingHelper.hpp"
20 #include "Acts/Surfaces/Surface.hpp"
21 #include "Acts/Utilities/Definitions.hpp"
22 #include "Acts/Utilities/Intersection.hpp"
23 #include "Acts/Utilities/Result.hpp"
24 #include "Acts/Utilities/Units.hpp"
25 
26 #include <cmath>
27 #include <functional>
28 
29 // This is based original stepper code from the ATLAS RungeKuttePropagagor
30 namespace Acts {
31 
32 using namespace Acts::UnitLiterals;
33 
35 template <typename bfield_t>
36 class AtlasStepper {
37  public:
38  using Jacobian = BoundMatrix;
39  using Covariance = BoundSymMatrix;
40  using BoundState = std::tuple<BoundTrackParameters, Jacobian, double>;
41  using CurvilinearState =
42  std::tuple<CurvilinearTrackParameters, Jacobian, double>;
43 
44  using BField = bfield_t;
45 
47  struct State {
49  State() = delete;
50 
61  template <typename Parameters>
62  State(std::reference_wrapper<const GeometryContext> gctx,
63  std::reference_wrapper<const MagneticFieldContext> mctx,
64  const Parameters& pars, NavigationDirection ndir = forward,
65  double ssize = std::numeric_limits<double>::max(),
66  double stolerance = s_onSurfaceTolerance)
67  : navDir(ndir),
68  useJacobian(false),
69  step(0.),
70  maxPathLength(0.),
71  mcondition(false),
72  needgradient(false),
73  newfield(true),
74  field(0., 0., 0.),
75  covariance(nullptr),
76  stepSize(ndir * std::abs(ssize)),
77  tolerance(stolerance),
78  fieldCache(mctx),
79  geoContext(gctx) {
80  // The rest of this constructor is copy&paste of AtlasStepper::update() -
81  // this is a nasty but working solution for the stepper state without
82  // functions
83 
84  const auto pos = pars.position(gctx);
85  const auto Vp = pars.parameters();
86 
87  double Sf, Cf, Ce, Se;
88  Sf = sin(Vp[eBoundPhi]);
89  Cf = cos(Vp[eBoundPhi]);
90  Se = sin(Vp[eBoundTheta]);
91  Ce = cos(Vp[eBoundTheta]);
92 
93  pVector[0] = pos[ePos0];
94  pVector[1] = pos[ePos1];
95  pVector[2] = pos[ePos2];
96  pVector[3] = pars.time();
97  pVector[4] = Cf * Se;
98  pVector[5] = Sf * Se;
99  pVector[6] = Ce;
100  pVector[7] = Vp[eBoundQOverP];
101 
102  // @todo: remove magic numbers - is that the charge ?
103  if (std::abs(pVector[7]) < .000000000000001) {
104  pVector[7] < 0. ? pVector[7] = -.000000000000001
105  : pVector[7] = .000000000000001;
106  }
107 
108  // prepare the jacobian if we have a covariance
109  if (pars.covariance()) {
110  // copy the covariance matrix
111  covariance = new ActsSymMatrixD<eBoundSize>(*pars.covariance());
112  covTransport = true;
113  useJacobian = true;
114  const auto transform = pars.referenceSurface().referenceFrame(
115  geoContext, pos, pars.unitDirection());
116 
117  pVector[8] = transform(0, eBoundLoc0);
118  pVector[16] = transform(0, eBoundLoc1);
119  pVector[24] = 0.;
120  pVector[32] = 0.;
121  pVector[40] = 0.;
122  pVector[48] = 0.; // dX /
123 
124  pVector[9] = transform(1, eBoundLoc0);
125  pVector[17] = transform(1, eBoundLoc1);
126  pVector[25] = 0.;
127  pVector[33] = 0.;
128  pVector[41] = 0.;
129  pVector[49] = 0.; // dY /
130 
131  pVector[10] = transform(2, eBoundLoc0);
132  pVector[18] = transform(2, eBoundLoc1);
133  pVector[26] = 0.;
134  pVector[34] = 0.;
135  pVector[42] = 0.;
136  pVector[50] = 0.; // dZ /
137 
138  pVector[11] = 0.;
139  pVector[19] = 0.;
140  pVector[27] = 0.;
141  pVector[35] = 0.;
142  pVector[43] = 0.;
143  pVector[51] = 1.; // dT/
144 
145  pVector[12] = 0.;
146  pVector[20] = 0.;
147  pVector[28] = -Sf * Se; // - sin(phi) * cos(theta)
148  pVector[36] = Cf * Ce; // cos(phi) * cos(theta)
149  pVector[44] = 0.;
150  pVector[52] = 0.; // dAx/
151 
152  pVector[13] = 0.;
153  pVector[21] = 0.;
154  pVector[29] = Cf * Se; // cos(phi) * sin(theta)
155  pVector[37] = Sf * Ce; // sin(phi) * cos(theta)
156  pVector[45] = 0.;
157  pVector[53] = 0.; // dAy/
158 
159  pVector[14] = 0.;
160  pVector[22] = 0.;
161  pVector[30] = 0.;
162  pVector[38] = -Se; // - sin(theta)
163  pVector[46] = 0.;
164  pVector[54] = 0.; // dAz/
165 
166  pVector[15] = 0.;
167  pVector[23] = 0.;
168  pVector[31] = 0.;
169  pVector[39] = 0.;
170  pVector[47] = 1.;
171  pVector[55] = 0.; // dCM/
172 
173  pVector[56] = 0.;
174  pVector[57] = 0.;
175  pVector[58] = 0.;
176  pVector[59] = 0.;
177 
178  // special treatment for surface types
179  const auto& surface = pars.referenceSurface();
180  // the disc needs polar coordinate adaptations
181  if (surface.type() == Surface::Disc) {
182  double lCf = cos(Vp[1]);
183  double lSf = sin(Vp[1]);
184  double Ax[3] = {transform(0, 0), transform(1, 0), transform(2, 0)};
185  double Ay[3] = {transform(0, 1), transform(1, 1), transform(2, 1)};
186  double d0 = lCf * Ax[0] + lSf * Ay[0];
187  double d1 = lCf * Ax[1] + lSf * Ay[1];
188  double d2 = lCf * Ax[2] + lSf * Ay[2];
189  pVector[8] = d0;
190  pVector[9] = d1;
191  pVector[10] = d2;
192  pVector[16] = Vp[0] * (lCf * Ay[0] - lSf * Ax[0]);
193  pVector[17] = Vp[0] * (lCf * Ay[1] - lSf * Ax[1]);
194  pVector[18] = Vp[0] * (lCf * Ay[2] - lSf * Ax[2]);
195  }
196  // the line needs components that relate direction change
197  // with global frame change
198  if (surface.type() == Surface::Perigee ||
199  surface.type() == Surface::Straw) {
200  // sticking to the nomenclature of the original RkPropagator
201  // - axis pointing along the drift/transverse direction
202  double B[3] = {transform(0, 0), transform(1, 0), transform(2, 0)};
203  // - axis along the straw
204  double A[3] = {transform(0, 1), transform(1, 1), transform(2, 1)};
205  // - normal vector of the reference frame
206  double C[3] = {transform(0, 2), transform(1, 2), transform(2, 2)};
207 
208  // projection of direction onto normal vector of reference frame
209  double PC = pVector[4] * C[0] + pVector[5] * C[1] + pVector[6] * C[2];
210  double Bn = 1. / PC;
211 
212  double Bx2 = -A[2] * pVector[29];
213  double Bx3 = A[1] * pVector[38] - A[2] * pVector[37];
214 
215  double By2 = A[2] * pVector[28];
216  double By3 = A[2] * pVector[36] - A[0] * pVector[38];
217 
218  double Bz2 = A[0] * pVector[29] - A[1] * pVector[28];
219  double Bz3 = A[0] * pVector[37] - A[1] * pVector[36];
220 
221  double B2 = B[0] * Bx2 + B[1] * By2 + B[2] * Bz2;
222  double B3 = B[0] * Bx3 + B[1] * By3 + B[2] * Bz3;
223 
224  Bx2 = (Bx2 - B[0] * B2) * Bn;
225  Bx3 = (Bx3 - B[0] * B3) * Bn;
226  By2 = (By2 - B[1] * B2) * Bn;
227  By3 = (By3 - B[1] * B3) * Bn;
228  Bz2 = (Bz2 - B[2] * B2) * Bn;
229  Bz3 = (Bz3 - B[2] * B3) * Bn;
230 
231  // /dPhi | /dThe |
232  pVector[24] = Bx2 * Vp[0];
233  pVector[32] = Bx3 * Vp[0]; // dX/
234  pVector[25] = By2 * Vp[0];
235  pVector[33] = By3 * Vp[0]; // dY/
236  pVector[26] = Bz2 * Vp[0];
237  pVector[34] = Bz3 * Vp[0]; // dZ/
238  }
239  }
240  // now declare the state as ready
241  state_ready = true;
242  }
243 
244  // optimisation that init is not called twice
245  bool state_ready = false;
246  // configuration
247  NavigationDirection navDir;
248  bool useJacobian;
249  double step;
250  double maxPathLength;
251  bool mcondition;
252  bool needgradient;
253  bool newfield;
254  // internal parameters to be used
255  Vector3D field;
256  std::array<double, 60> pVector;
257 
269 
270  // result
271  double parameters[eBoundSize] = {0., 0., 0., 0., 0., 0.};
272  const Covariance* covariance;
273  Covariance cov = Covariance::Zero();
274  bool covTransport = false;
275  double jacobian[eBoundSize * eBoundSize];
276 
277  // accummulated path length cache
278  double pathAccumulated = 0.;
279 
280  // Adaptive step size of the runge-kutta integration
281  ConstrainedStep stepSize = std::numeric_limits<double>::max();
282 
283  // Previous step size for overstep estimation
284  double previousStepSize = 0.;
285 
287  double tolerance = s_onSurfaceTolerance;
288 
291  typename bfield_t::Cache fieldCache;
292 
294  std::reference_wrapper<const GeometryContext> geoContext;
295 
298  bool debug = false;
299  std::string debugString = "";
301  size_t debugPfxWidth = 30;
302  size_t debugMsgWidth = 50;
303  };
304 
305  AtlasStepper(bfield_t bField) : m_bField(std::move(bField)){};
306 
315  void resetState(
316  State& state, const BoundVector& boundParams, const BoundSymMatrix& cov,
317  const Surface& surface, const NavigationDirection navDir = forward,
318  const double stepSize = std::numeric_limits<double>::max()) const {
319  // Update the stepping state
320  update(state,
321  detail::transformBoundToFreeParameters(surface, state.geoContext,
322  boundParams),
323  cov);
324  state.navDir = navDir;
325  state.stepSize = ConstrainedStep(stepSize);
326  state.pathAccumulated = 0.;
327 
328  // Reinitialize the stepping jacobian
329  // copy the covariance matrix
330  const auto transform = surface.referenceFrame(
331  state.geoContext, position(state), momentum(state) * direction(state));
332 
333  double Sf, Cf, Ce, Se;
334  Sf = sin(boundParams[eBoundPhi]);
335  Cf = cos(boundParams[eBoundPhi]);
336  Se = sin(boundParams[eBoundTheta]);
337  Ce = cos(boundParams[eBoundTheta]);
338 
339  state.pVector[8] = transform(0, eBoundLoc0);
340  state.pVector[16] = transform(0, eBoundLoc1);
341  state.pVector[24] = 0.;
342  state.pVector[32] = 0.;
343  state.pVector[40] = 0.;
344  state.pVector[48] = 0.; // dX /
345 
346  state.pVector[9] = transform(1, eBoundLoc0);
347  state.pVector[17] = transform(1, eBoundLoc1);
348  state.pVector[25] = 0.;
349  state.pVector[33] = 0.;
350  state.pVector[41] = 0.;
351  state.pVector[49] = 0.; // dY /
352 
353  state.pVector[10] = transform(2, eBoundLoc0);
354  state.pVector[18] = transform(2, eBoundLoc1);
355  state.pVector[26] = 0.;
356  state.pVector[34] = 0.;
357  state.pVector[42] = 0.;
358  state.pVector[50] = 0.; // dZ /
359 
360  state.pVector[11] = 0.;
361  state.pVector[19] = 0.;
362  state.pVector[27] = 0.;
363  state.pVector[35] = 0.;
364  state.pVector[43] = 0.;
365  state.pVector[51] = 1.; // dT/
366 
367  state.pVector[12] = 0.;
368  state.pVector[20] = 0.;
369  state.pVector[28] = -Sf * Se; // - sin(phi) * cos(theta)
370  state.pVector[36] = Cf * Ce; // cos(phi) * cos(theta)
371  state.pVector[44] = 0.;
372  state.pVector[52] = 0.; // dAx/
373 
374  state.pVector[13] = 0.;
375  state.pVector[21] = 0.;
376  state.pVector[29] = Cf * Se; // cos(phi) * sin(theta)
377  state.pVector[37] = Sf * Ce; // sin(phi) * cos(theta)
378  state.pVector[45] = 0.;
379  state.pVector[53] = 0.; // dAy/
380 
381  state.pVector[14] = 0.;
382  state.pVector[22] = 0.;
383  state.pVector[30] = 0.;
384  state.pVector[38] = -Se; // - sin(theta)
385  state.pVector[46] = 0.;
386  state.pVector[54] = 0.; // dAz/
387 
388  state.pVector[15] = 0.;
389  state.pVector[23] = 0.;
390  state.pVector[31] = 0.;
391  state.pVector[39] = 0.;
392  state.pVector[47] = 1.;
393  state.pVector[55] = 0.; // dCM/
394 
395  state.pVector[56] = 0.;
396  state.pVector[57] = 0.;
397  state.pVector[58] = 0.;
398  state.pVector[59] = 0.;
399 
400  // special treatment for surface types
401  // the disc needs polar coordinate adaptations
402  if (surface.type() == Surface::Disc) {
403  double lCf = cos(boundParams[eBoundLoc1]);
404  double lSf = sin(boundParams[eBoundLoc1]);
405  double Ax[3] = {transform(0, 0), transform(1, 0), transform(2, 0)};
406  double Ay[3] = {transform(0, 1), transform(1, 1), transform(2, 1)};
407  double d0 = lCf * Ax[0] + lSf * Ay[0];
408  double d1 = lCf * Ax[1] + lSf * Ay[1];
409  double d2 = lCf * Ax[2] + lSf * Ay[2];
410  state.pVector[8] = d0;
411  state.pVector[9] = d1;
412  state.pVector[10] = d2;
413  state.pVector[16] = boundParams[eBoundLoc0] * (lCf * Ay[0] - lSf * Ax[0]);
414  state.pVector[17] = boundParams[eBoundLoc0] * (lCf * Ay[1] - lSf * Ax[1]);
415  state.pVector[18] = boundParams[eBoundLoc0] * (lCf * Ay[2] - lSf * Ax[2]);
416  }
417  // the line needs components that relate direction change
418  // with global frame change
419  if (surface.type() == Surface::Perigee ||
420  surface.type() == Surface::Straw) {
421  // sticking to the nomenclature of the original RkPropagator
422  // - axis pointing along the drift/transverse direction
423  double B[3] = {transform(0, 0), transform(1, 0), transform(2, 0)};
424  // - axis along the straw
425  double A[3] = {transform(0, 1), transform(1, 1), transform(2, 1)};
426  // - normal vector of the reference frame
427  double C[3] = {transform(0, 2), transform(1, 2), transform(2, 2)};
428 
429  // projection of direction onto normal vector of reference frame
430  double PC = state.pVector[4] * C[0] + state.pVector[5] * C[1] +
431  state.pVector[6] * C[2];
432  double Bn = 1. / PC;
433 
434  double Bx2 = -A[2] * state.pVector[29];
435  double Bx3 = A[1] * state.pVector[38] - A[2] * state.pVector[37];
436 
437  double By2 = A[2] * state.pVector[28];
438  double By3 = A[2] * state.pVector[36] - A[0] * state.pVector[38];
439 
440  double Bz2 = A[0] * state.pVector[29] - A[1] * state.pVector[28];
441  double Bz3 = A[0] * state.pVector[37] - A[1] * state.pVector[36];
442 
443  double B2 = B[0] * Bx2 + B[1] * By2 + B[2] * Bz2;
444  double B3 = B[0] * Bx3 + B[1] * By3 + B[2] * Bz3;
445 
446  Bx2 = (Bx2 - B[0] * B2) * Bn;
447  Bx3 = (Bx3 - B[0] * B3) * Bn;
448  By2 = (By2 - B[1] * B2) * Bn;
449  By3 = (By3 - B[1] * B3) * Bn;
450  Bz2 = (Bz2 - B[2] * B2) * Bn;
451  Bz3 = (Bz3 - B[2] * B3) * Bn;
452 
453  // /dPhi | /dThe |
454  state.pVector[24] = Bx2 * boundParams[eBoundLoc0];
455  state.pVector[32] = Bx3 * boundParams[eBoundLoc0]; // dX/
456  state.pVector[25] = By2 * boundParams[eBoundLoc0];
457  state.pVector[33] = By3 * boundParams[eBoundLoc0]; // dY/
458  state.pVector[26] = Bz2 * boundParams[eBoundLoc0];
459  state.pVector[34] = Bz3 * boundParams[eBoundLoc0]; // dZ/
460  }
461  }
462 
470  Vector3D getField(State& state, const Vector3D& pos) const {
471  // get the field from the cell
472  state.field = m_bField.getField(pos, state.fieldCache);
473  return state.field;
474  }
475 
476  Vector3D position(const State& state) const {
477  return Vector3D(state.pVector[0], state.pVector[1], state.pVector[2]);
478  }
479 
480  Vector3D direction(const State& state) const {
481  return Vector3D(state.pVector[4], state.pVector[5], state.pVector[6]);
482  }
483 
484  double momentum(const State& state) const {
485  return 1. / std::abs(state.pVector[7]);
486  }
487 
489  double charge(const State& state) const {
490  return state.pVector[7] > 0. ? 1. : -1.;
491  }
492 
496  double overstepLimit(const State& /*state*/) const { return m_overstepLimit; }
497 
499  double time(const State& state) const { return state.pVector[3]; }
500 
511  Intersection3D::Status updateSurfaceStatus(
512  State& state, const Surface& surface, const BoundaryCheck& bcheck) const {
513  return detail::updateSingleSurfaceStatus<AtlasStepper>(*this, state,
514  surface, bcheck);
515  }
516 
525  template <typename object_intersection_t>
526  void updateStepSize(State& state, const object_intersection_t& oIntersection,
527  bool release = true) const {
528  detail::updateSingleStepSize<AtlasStepper>(state, oIntersection, release);
529  }
530 
536  void setStepSize(State& state, double stepSize,
537  ConstrainedStep::Type stype = ConstrainedStep::actor) const {
538  state.previousStepSize = state.stepSize;
539  state.stepSize.update(stepSize, stype, true);
540  }
541 
545  void releaseStepSize(State& state) const {
546  state.stepSize.release(ConstrainedStep::actor);
547  }
548 
552  std::string outputStepSize(const State& state) const {
553  return state.stepSize.toString();
554  }
555 
566  BoundState boundState(State& state, const Surface& surface) const {
567  // the convert method invalidates the state (in case it's reused)
568  state.state_ready = false;
569  // extract state information
570  Acts::Vector4D pos4;
571  pos4[ePos0] = state.pVector[0];
572  pos4[ePos1] = state.pVector[1];
573  pos4[ePos2] = state.pVector[2];
574  pos4[eTime] = state.pVector[3];
575  Acts::Vector3D dir;
576  dir[eMom0] = state.pVector[4];
577  dir[eMom1] = state.pVector[5];
578  dir[eMom2] = state.pVector[6];
579  const auto qOverP = state.pVector[7];
580 
581  // The transport of the covariance
582  std::optional<Covariance> covOpt = std::nullopt;
583  if (state.covTransport) {
584  covarianceTransport(state, surface);
585  covOpt = state.cov;
586  }
587 
588  // Fill the end parameters
589  BoundTrackParameters parameters(surface.getSharedPtr(), state.geoContext,
590  pos4, dir, qOverP, std::move(covOpt));
591 
592  return BoundState(std::move(parameters), state.jacobian,
593  state.pathAccumulated);
594  }
595 
605  CurvilinearState curvilinearState(State& state) const {
606  // the convert method invalidates the state (in case it's reused)
607  state.state_ready = false;
608  // extract state information
609  Acts::Vector4D pos4;
610  pos4[ePos0] = state.pVector[0];
611  pos4[ePos1] = state.pVector[1];
612  pos4[ePos2] = state.pVector[2];
613  pos4[eTime] = state.pVector[3];
614  Acts::Vector3D dir;
615  dir[eMom0] = state.pVector[4];
616  dir[eMom1] = state.pVector[5];
617  dir[eMom2] = state.pVector[6];
618  const auto qOverP = state.pVector[7];
619 
620  std::optional<Covariance> covOpt = std::nullopt;
621  if (state.covTransport) {
622  covarianceTransport(state);
623  covOpt = state.cov;
624  }
625 
626  CurvilinearTrackParameters parameters(pos4, dir, qOverP, std::move(covOpt));
627 
628  return CurvilinearState(std::move(parameters), state.jacobian,
629  state.pathAccumulated);
630  }
631 
636  void update(State& state, const FreeVector& parameters,
637  const Covariance& covariance) const {
638  Vector3D direction = parameters.template segment<3>(eFreeDir0).normalized();
639  state.pVector[0] = parameters[eFreePos0];
640  state.pVector[1] = parameters[eFreePos1];
641  state.pVector[2] = parameters[eFreePos2];
642  state.pVector[3] = parameters[eFreeTime];
643  state.pVector[4] = direction.x();
644  state.pVector[5] = direction.y();
645  state.pVector[6] = direction.z();
646  state.pVector[7] = std::copysign(parameters[eFreeQOverP], state.pVector[7]);
647 
648  // @todo: remove magic numbers - is that the charge ?
649  if (std::abs(state.pVector[7]) < .000000000000001) {
650  state.pVector[7] < 0. ? state.pVector[7] = -.000000000000001
651  : state.pVector[7] = .000000000000001;
652  }
653 
654  // prepare the jacobian if we have a covariance
655  // copy the covariance matrix
656  state.covariance = new ActsSymMatrixD<eBoundSize>(covariance);
657  state.covTransport = true;
658  state.useJacobian = true;
659 
660  // declare the state as ready
661  state.state_ready = true;
662  }
663 
669  void update(State& state, const Vector3D& uposition,
670  const Vector3D& udirection, double up, double time) const {
671  // update the vector
672  state.pVector[0] = uposition[0];
673  state.pVector[1] = uposition[1];
674  state.pVector[2] = uposition[2];
675  state.pVector[3] = time;
676  state.pVector[4] = udirection[0];
677  state.pVector[5] = udirection[1];
678  state.pVector[6] = udirection[2];
679  state.pVector[7] = charge(state) / up;
680  }
681 
689  void covarianceTransport(State& state) const {
690  double P[60];
691  for (unsigned int i = 0; i < 60; ++i) {
692  P[i] = state.pVector[i];
693  }
694 
695  double p = 1. / P[7];
696  P[40] *= p;
697  P[41] *= p;
698  P[42] *= p;
699  P[44] *= p;
700  P[45] *= p;
701  P[46] *= p;
702 
703  double An = sqrt(P[4] * P[4] + P[5] * P[5]);
704  double Ax[3];
705  if (An != 0.) {
706  Ax[0] = -P[5] / An;
707  Ax[1] = P[4] / An;
708  Ax[2] = 0.;
709  } else {
710  Ax[0] = 1.;
711  Ax[1] = 0.;
712  Ax[2] = 0.;
713  }
714 
715  double Ay[3] = {-Ax[1] * P[6], Ax[0] * P[6], An};
716  double S[3] = {P[4], P[5], P[6]};
717 
718  double A = P[4] * S[0] + P[5] * S[1] + P[6] * S[2];
719  if (A != 0.) {
720  A = 1. / A;
721  }
722  S[0] *= A;
723  S[1] *= A;
724  S[2] *= A;
725 
726  double s0 = P[8] * S[0] + P[9] * S[1] + P[10] * S[2];
727  double s1 = P[16] * S[0] + P[17] * S[1] + P[18] * S[2];
728  double s2 = P[24] * S[0] + P[25] * S[1] + P[26] * S[2];
729  double s3 = P[32] * S[0] + P[33] * S[1] + P[34] * S[2];
730  double s4 = P[40] * S[0] + P[41] * S[1] + P[42] * S[2];
731 
732  P[8] -= (s0 * P[4]);
733  P[9] -= (s0 * P[5]);
734  P[10] -= (s0 * P[6]);
735  P[11] -= (s0 * P[59]);
736  P[12] -= (s0 * P[56]);
737  P[13] -= (s0 * P[57]);
738  P[14] -= (s0 * P[58]);
739  P[16] -= (s1 * P[4]);
740  P[17] -= (s1 * P[5]);
741  P[18] -= (s1 * P[6]);
742  P[19] -= (s1 * P[59]);
743  P[20] -= (s1 * P[56]);
744  P[21] -= (s1 * P[57]);
745  P[22] -= (s1 * P[58]);
746  P[24] -= (s2 * P[4]);
747  P[25] -= (s2 * P[5]);
748  P[26] -= (s2 * P[6]);
749  P[27] -= (s2 * P[59]);
750  P[28] -= (s2 * P[56]);
751  P[29] -= (s2 * P[57]);
752  P[30] -= (s2 * P[58]);
753  P[32] -= (s3 * P[4]);
754  P[33] -= (s3 * P[5]);
755  P[34] -= (s3 * P[6]);
756  P[35] -= (s3 * P[59]);
757  P[36] -= (s3 * P[56]);
758  P[37] -= (s3 * P[57]);
759  P[38] -= (s3 * P[58]);
760  P[40] -= (s4 * P[4]);
761  P[41] -= (s4 * P[5]);
762  P[42] -= (s4 * P[6]);
763  P[43] -= (s4 * P[59]);
764  P[44] -= (s4 * P[56]);
765  P[45] -= (s4 * P[57]);
766  P[46] -= (s4 * P[58]);
767 
768  double P3, P4, C = P[4] * P[4] + P[5] * P[5];
769  if (C > 1.e-20) {
770  C = 1. / C;
771  P3 = P[4] * C;
772  P4 = P[5] * C;
773  C = -sqrt(C);
774  } else {
775  C = -1.e10;
776  P3 = 1.;
777  P4 = 0.;
778  }
779 
780  // Jacobian production
781  //
782  state.jacobian[0] = Ax[0] * P[8] + Ax[1] * P[9]; // dL0/dL0
783  state.jacobian[1] = Ax[0] * P[16] + Ax[1] * P[17]; // dL0/dL1
784  state.jacobian[2] = Ax[0] * P[24] + Ax[1] * P[25]; // dL0/dPhi
785  state.jacobian[3] = Ax[0] * P[32] + Ax[1] * P[33]; // dL0/dThe
786  state.jacobian[4] = Ax[0] * P[40] + Ax[1] * P[41]; // dL0/dCM
787  state.jacobian[5] = 0.; // dL0/dT
788 
789  state.jacobian[6] = Ay[0] * P[8] + Ay[1] * P[9] + Ay[2] * P[10]; // dL1/dL0
790  state.jacobian[7] =
791  Ay[0] * P[16] + Ay[1] * P[17] + Ay[2] * P[18]; // dL1/dL1
792  state.jacobian[8] =
793  Ay[0] * P[24] + Ay[1] * P[25] + Ay[2] * P[26]; // dL1/dPhi
794  state.jacobian[9] =
795  Ay[0] * P[32] + Ay[1] * P[33] + Ay[2] * P[34]; // dL1/dThe
796  state.jacobian[10] =
797  Ay[0] * P[40] + Ay[1] * P[41] + Ay[2] * P[42]; // dL1/dCM
798  state.jacobian[11] = 0.; // dL1/dT
799 
800  state.jacobian[12] = P3 * P[13] - P4 * P[12]; // dPhi/dL0
801  state.jacobian[13] = P3 * P[21] - P4 * P[20]; // dPhi/dL1
802  state.jacobian[14] = P3 * P[29] - P4 * P[28]; // dPhi/dPhi
803  state.jacobian[15] = P3 * P[37] - P4 * P[36]; // dPhi/dThe
804  state.jacobian[16] = P3 * P[45] - P4 * P[44]; // dPhi/dCM
805  state.jacobian[17] = 0.; // dPhi/dT
806 
807  state.jacobian[18] = C * P[14]; // dThe/dL0
808  state.jacobian[19] = C * P[22]; // dThe/dL1
809  state.jacobian[20] = C * P[30]; // dThe/dPhi
810  state.jacobian[21] = C * P[38]; // dThe/dThe
811  state.jacobian[22] = C * P[46]; // dThe/dCM
812  state.jacobian[23] = 0.; // dThe/dT
813 
814  state.jacobian[24] = 0.; // dCM /dL0
815  state.jacobian[25] = 0.; // dCM /dL1
816  state.jacobian[26] = 0.; // dCM /dPhi
817  state.jacobian[27] = 0.; // dCM /dTheta
818  state.jacobian[28] = P[47]; // dCM /dCM
819  state.jacobian[29] = 0.; // dCM/dT
820 
821  state.jacobian[30] = P[11]; // dT/dL0
822  state.jacobian[31] = P[19]; // dT/dL1
823  state.jacobian[32] = P[27]; // dT/dPhi
824  state.jacobian[33] = P[35]; // dT/dThe
825  state.jacobian[34] = P[43]; // dT/dCM
826  state.jacobian[35] = P[51]; // dT/dT
827 
828  Eigen::Map<Eigen::Matrix<double, eBoundSize, eBoundSize, Eigen::RowMajor>>
829  J(state.jacobian);
830  state.cov = J * (*state.covariance) * J.transpose();
831  }
832 
839  void covarianceTransport(State& state, const Surface& surface) const {
840  Acts::Vector3D gp(state.pVector[0], state.pVector[1], state.pVector[2]);
841  Acts::Vector3D mom(state.pVector[4], state.pVector[5], state.pVector[6]);
842  mom /= std::abs(state.pVector[7]);
843 
844  double p = 1. / state.pVector[7];
845  state.pVector[40] *= p;
846  state.pVector[41] *= p;
847  state.pVector[42] *= p;
848  state.pVector[44] *= p;
849  state.pVector[45] *= p;
850  state.pVector[46] *= p;
851 
852  const auto fFrame = surface.referenceFrame(state.geoContext, gp, mom);
853 
854  double Ax[3] = {fFrame(0, 0), fFrame(1, 0), fFrame(2, 0)};
855  double Ay[3] = {fFrame(0, 1), fFrame(1, 1), fFrame(2, 1)};
856  double S[3] = {fFrame(0, 2), fFrame(1, 2), fFrame(2, 2)};
857 
858  // this is the projection of direction onto the local normal vector
859  double A = state.pVector[4] * S[0] + state.pVector[5] * S[1] +
860  state.pVector[6] * S[2];
861 
862  if (A != 0.) {
863  A = 1. / A;
864  }
865 
866  S[0] *= A;
867  S[1] *= A;
868  S[2] *= A;
869 
870  double s0 = state.pVector[8] * S[0] + state.pVector[9] * S[1] +
871  state.pVector[10] * S[2];
872  double s1 = state.pVector[16] * S[0] + state.pVector[17] * S[1] +
873  state.pVector[18] * S[2];
874  double s2 = state.pVector[24] * S[0] + state.pVector[25] * S[1] +
875  state.pVector[26] * S[2];
876  double s3 = state.pVector[32] * S[0] + state.pVector[33] * S[1] +
877  state.pVector[34] * S[2];
878  double s4 = state.pVector[40] * S[0] + state.pVector[41] * S[1] +
879  state.pVector[42] * S[2];
880 
881  // in case of line-type surfaces - we need to take into account that
882  // the reference frame changes with variations of all local
883  // parameters
884  if (surface.type() == Surface::Straw ||
885  surface.type() == Surface::Perigee) {
886  // vector from position to center
887  double x = state.pVector[0] - surface.center(state.geoContext).x();
888  double y = state.pVector[1] - surface.center(state.geoContext).y();
889  double z = state.pVector[2] - surface.center(state.geoContext).z();
890 
891  // this is the projection of the direction onto the local y axis
892  double d = state.pVector[4] * Ay[0] + state.pVector[5] * Ay[1] +
893  state.pVector[6] * Ay[2];
894 
895  // this is cos(beta)
896  double a = (1. - d) * (1. + d);
897  if (a != 0.) {
898  a = 1. / a; // i.e. 1./(1-d^2)
899  }
900 
901  // that's the modified norm vector
902  double X = d * Ay[0] - state.pVector[4]; //
903  double Y = d * Ay[1] - state.pVector[5]; //
904  double Z = d * Ay[2] - state.pVector[6]; //
905 
906  // d0 to d1
907  double d0 = state.pVector[12] * Ay[0] + state.pVector[13] * Ay[1] +
908  state.pVector[14] * Ay[2];
909  double d1 = state.pVector[20] * Ay[0] + state.pVector[21] * Ay[1] +
910  state.pVector[22] * Ay[2];
911  double d2 = state.pVector[28] * Ay[0] + state.pVector[29] * Ay[1] +
912  state.pVector[30] * Ay[2];
913  double d3 = state.pVector[36] * Ay[0] + state.pVector[37] * Ay[1] +
914  state.pVector[38] * Ay[2];
915  double d4 = state.pVector[44] * Ay[0] + state.pVector[45] * Ay[1] +
916  state.pVector[46] * Ay[2];
917 
918  s0 = (((state.pVector[8] * X + state.pVector[9] * Y +
919  state.pVector[10] * Z) +
920  x * (d0 * Ay[0] - state.pVector[12])) +
921  (y * (d0 * Ay[1] - state.pVector[13]) +
922  z * (d0 * Ay[2] - state.pVector[14]))) *
923  (-a);
924 
925  s1 = (((state.pVector[16] * X + state.pVector[17] * Y +
926  state.pVector[18] * Z) +
927  x * (d1 * Ay[0] - state.pVector[20])) +
928  (y * (d1 * Ay[1] - state.pVector[21]) +
929  z * (d1 * Ay[2] - state.pVector[22]))) *
930  (-a);
931  s2 = (((state.pVector[24] * X + state.pVector[25] * Y +
932  state.pVector[26] * Z) +
933  x * (d2 * Ay[0] - state.pVector[28])) +
934  (y * (d2 * Ay[1] - state.pVector[29]) +
935  z * (d2 * Ay[2] - state.pVector[30]))) *
936  (-a);
937  s3 = (((state.pVector[32] * X + state.pVector[33] * Y +
938  state.pVector[34] * Z) +
939  x * (d3 * Ay[0] - state.pVector[36])) +
940  (y * (d3 * Ay[1] - state.pVector[37]) +
941  z * (d3 * Ay[2] - state.pVector[38]))) *
942  (-a);
943  s4 = (((state.pVector[40] * X + state.pVector[41] * Y +
944  state.pVector[42] * Z) +
945  x * (d4 * Ay[0] - state.pVector[44])) +
946  (y * (d4 * Ay[1] - state.pVector[45]) +
947  z * (d4 * Ay[2] - state.pVector[46]))) *
948  (-a);
949  }
950 
951  state.pVector[8] -= (s0 * state.pVector[4]);
952  state.pVector[9] -= (s0 * state.pVector[5]);
953  state.pVector[10] -= (s0 * state.pVector[6]);
954  state.pVector[11] -= (s0 * state.pVector[59]);
955  state.pVector[12] -= (s0 * state.pVector[56]);
956  state.pVector[13] -= (s0 * state.pVector[57]);
957  state.pVector[14] -= (s0 * state.pVector[58]);
958 
959  state.pVector[16] -= (s1 * state.pVector[4]);
960  state.pVector[17] -= (s1 * state.pVector[5]);
961  state.pVector[18] -= (s1 * state.pVector[6]);
962  state.pVector[19] -= (s1 * state.pVector[59]);
963  state.pVector[20] -= (s1 * state.pVector[56]);
964  state.pVector[21] -= (s1 * state.pVector[57]);
965  state.pVector[22] -= (s1 * state.pVector[58]);
966 
967  state.pVector[24] -= (s2 * state.pVector[4]);
968  state.pVector[25] -= (s2 * state.pVector[5]);
969  state.pVector[26] -= (s2 * state.pVector[6]);
970  state.pVector[27] -= (s2 * state.pVector[59]);
971  state.pVector[28] -= (s2 * state.pVector[56]);
972  state.pVector[29] -= (s2 * state.pVector[57]);
973  state.pVector[30] -= (s2 * state.pVector[58]);
974 
975  state.pVector[32] -= (s3 * state.pVector[4]);
976  state.pVector[33] -= (s3 * state.pVector[5]);
977  state.pVector[34] -= (s3 * state.pVector[6]);
978  state.pVector[35] -= (s3 * state.pVector[59]);
979  state.pVector[36] -= (s3 * state.pVector[56]);
980  state.pVector[37] -= (s3 * state.pVector[57]);
981  state.pVector[38] -= (s3 * state.pVector[58]);
982 
983  state.pVector[40] -= (s4 * state.pVector[4]);
984  state.pVector[41] -= (s4 * state.pVector[5]);
985  state.pVector[42] -= (s4 * state.pVector[6]);
986  state.pVector[43] -= (s4 * state.pVector[59]);
987  state.pVector[44] -= (s4 * state.pVector[56]);
988  state.pVector[45] -= (s4 * state.pVector[57]);
989  state.pVector[46] -= (s4 * state.pVector[58]);
990 
991  double P3, P4,
992  C = state.pVector[4] * state.pVector[4] +
993  state.pVector[5] * state.pVector[5];
994  if (C > 1.e-20) {
995  C = 1. / C;
996  P3 = state.pVector[4] * C;
997  P4 = state.pVector[5] * C;
998  C = -sqrt(C);
999  } else {
1000  C = -1.e10;
1001  P3 = 1.;
1002  P4 = 0.;
1003  }
1004 
1005  double MA[3] = {Ax[0], Ax[1], Ax[2]};
1006  double MB[3] = {Ay[0], Ay[1], Ay[2]};
1007  // Jacobian production of transport and to_local
1008  if (surface.type() == Surface::Disc) {
1009  // the vector from the disc surface to the p
1010  const auto& sfc = surface.center(state.geoContext);
1011  double d[3] = {state.pVector[0] - sfc(0), state.pVector[1] - sfc(1),
1012  state.pVector[2] - sfc(2)};
1013  // this needs the transformation to polar coordinates
1014  double RC = d[0] * Ax[0] + d[1] * Ax[1] + d[2] * Ax[2];
1015  double RS = d[0] * Ay[0] + d[1] * Ay[1] + d[2] * Ay[2];
1016  double R2 = RC * RC + RS * RS;
1017 
1018  // inverse radius
1019  double Ri = 1. / sqrt(R2);
1020  MA[0] = (RC * Ax[0] + RS * Ay[0]) * Ri;
1021  MA[1] = (RC * Ax[1] + RS * Ay[1]) * Ri;
1022  MA[2] = (RC * Ax[2] + RS * Ay[2]) * Ri;
1023  MB[0] = (RC * Ay[0] - RS * Ax[0]) * (Ri = 1. / R2);
1024  MB[1] = (RC * Ay[1] - RS * Ax[1]) * Ri;
1025  MB[2] = (RC * Ay[2] - RS * Ax[2]) * Ri;
1026  }
1027 
1028  state.jacobian[0] = MA[0] * state.pVector[8] + MA[1] * state.pVector[9] +
1029  MA[2] * state.pVector[10]; // dL0/dL0
1030  state.jacobian[1] = MA[0] * state.pVector[16] + MA[1] * state.pVector[17] +
1031  MA[2] * state.pVector[18]; // dL0/dL1
1032  state.jacobian[2] = MA[0] * state.pVector[24] + MA[1] * state.pVector[25] +
1033  MA[2] * state.pVector[26]; // dL0/dPhi
1034  state.jacobian[3] = MA[0] * state.pVector[32] + MA[1] * state.pVector[33] +
1035  MA[2] * state.pVector[34]; // dL0/dThe
1036  state.jacobian[4] = MA[0] * state.pVector[40] + MA[1] * state.pVector[41] +
1037  MA[2] * state.pVector[42]; // dL0/dCM
1038  state.jacobian[5] = 0.; // dL0/dT
1039 
1040  state.jacobian[6] = MB[0] * state.pVector[8] + MB[1] * state.pVector[9] +
1041  MB[2] * state.pVector[10]; // dL1/dL0
1042  state.jacobian[7] = MB[0] * state.pVector[16] + MB[1] * state.pVector[17] +
1043  MB[2] * state.pVector[18]; // dL1/dL1
1044  state.jacobian[8] = MB[0] * state.pVector[24] + MB[1] * state.pVector[25] +
1045  MB[2] * state.pVector[26]; // dL1/dPhi
1046  state.jacobian[9] = MB[0] * state.pVector[32] + MB[1] * state.pVector[33] +
1047  MB[2] * state.pVector[34]; // dL1/dThe
1048  state.jacobian[10] = MB[0] * state.pVector[40] + MB[1] * state.pVector[41] +
1049  MB[2] * state.pVector[42]; // dL1/dCM
1050  state.jacobian[11] = 0.; // dL1/dT
1051 
1052  state.jacobian[12] =
1053  P3 * state.pVector[13] - P4 * state.pVector[12]; // dPhi/dL0
1054  state.jacobian[13] =
1055  P3 * state.pVector[21] - P4 * state.pVector[20]; // dPhi/dL1
1056  state.jacobian[14] =
1057  P3 * state.pVector[29] - P4 * state.pVector[28]; // dPhi/dPhi
1058  state.jacobian[15] =
1059  P3 * state.pVector[37] - P4 * state.pVector[36]; // dPhi/dThe
1060  state.jacobian[16] =
1061  P3 * state.pVector[45] - P4 * state.pVector[44]; // dPhi/dCM
1062  state.jacobian[17] = 0.; // dPhi/dT
1063 
1064  state.jacobian[18] = C * state.pVector[14]; // dThe/dL0
1065  state.jacobian[19] = C * state.pVector[22]; // dThe/dL1
1066  state.jacobian[20] = C * state.pVector[30]; // dThe/dPhi
1067  state.jacobian[21] = C * state.pVector[38]; // dThe/dThe
1068  state.jacobian[22] = C * state.pVector[46]; // dThe/dCM
1069  state.jacobian[23] = 0.; // dThe/dT
1070 
1071  state.jacobian[24] = 0.; // dCM /dL0
1072  state.jacobian[25] = 0.; // dCM /dL1
1073  state.jacobian[26] = 0.; // dCM /dPhi
1074  state.jacobian[27] = 0.; // dCM /dTheta
1075  state.jacobian[28] = state.pVector[47]; // dCM /dCM
1076  state.jacobian[29] = 0.; // dCM/dT
1077 
1078  state.jacobian[30] = state.pVector[11]; // dT/dL0
1079  state.jacobian[31] = state.pVector[19]; // dT/dL1
1080  state.jacobian[32] = state.pVector[27]; // dT/dPhi
1081  state.jacobian[33] = state.pVector[35]; // dT/dThe
1082  state.jacobian[34] = state.pVector[43]; // dT/dCM
1083  state.jacobian[35] = state.pVector[51]; // dT/dT
1084 
1085  Eigen::Map<Eigen::Matrix<double, eBoundSize, eBoundSize, Eigen::RowMajor>>
1086  J(state.jacobian);
1087  state.cov = J * (*state.covariance) * J.transpose();
1088  }
1089 
1093  template <typename propagator_state_t>
1094  Result<double> step(propagator_state_t& state) const {
1095  // we use h for keeping the nominclature with the original atlas code
1096  auto& h = state.stepping.stepSize;
1097  bool Jac = state.stepping.useJacobian;
1098 
1099  double* R = &(state.stepping.pVector[0]); // Coordinates
1100  double* A = &(state.stepping.pVector[4]); // Directions
1101  double* sA = &(state.stepping.pVector[56]);
1102  // Invert mometum/2.
1103  double Pi = 0.5 * state.stepping.pVector[7];
1104  // double dltm = 0.0002 * .03;
1105  Vector3D f0, f;
1106 
1107  // if new field is required get it
1108  if (state.stepping.newfield) {
1109  const Vector3D pos(R[0], R[1], R[2]);
1110  // This is sd.B_first in EigenStepper
1111  f0 = getField(state.stepping, pos);
1112  } else {
1113  f0 = state.stepping.field;
1114  }
1115 
1116  bool Helix = false;
1117  // if (std::abs(S) < m_cfg.helixStep) Helix = true;
1118 
1119  while (h != 0.) {
1120  // PS2 is h/(2*momentum) in EigenStepper
1121  double S3 = (1. / 3.) * h, S4 = .25 * h, PS2 = Pi * h;
1122 
1123  // First point
1124  //
1125  // H0 is (h/(2*momentum) * sd.B_first) in EigenStepper
1126  double H0[3] = {f0[0] * PS2, f0[1] * PS2, f0[2] * PS2};
1127  // { A0, B0, C0 } is (h/2 * sd.k1) in EigenStepper
1128  double A0 = A[1] * H0[2] - A[2] * H0[1];
1129  double B0 = A[2] * H0[0] - A[0] * H0[2];
1130  double C0 = A[0] * H0[1] - A[1] * H0[0];
1131  // { A2, B2, C2 } is (h/2 * sd.k1 + direction) in EigenStepper
1132  double A2 = A0 + A[0];
1133  double B2 = B0 + A[1];
1134  double C2 = C0 + A[2];
1135  // { A1, B1, C1 } is (h/2 * sd.k1 + 2*direction) in EigenStepper
1136  double A1 = A2 + A[0];
1137  double B1 = B2 + A[1];
1138  double C1 = C2 + A[2];
1139 
1140  // Second point
1141  //
1142  if (!Helix) {
1143  // This is pos1 in EigenStepper
1144  const Vector3D pos(R[0] + A1 * S4, R[1] + B1 * S4, R[2] + C1 * S4);
1145  // This is sd.B_middle in EigenStepper
1146  f = getField(state.stepping, pos);
1147  } else {
1148  f = f0;
1149  }
1150 
1151  // H1 is (h/(2*momentum) * sd.B_middle) in EigenStepper
1152  double H1[3] = {f[0] * PS2, f[1] * PS2, f[2] * PS2};
1153  // { A3, B3, C3 } is (direction + h/2 * sd.k2) in EigenStepper
1154  double A3 = (A[0] + B2 * H1[2]) - C2 * H1[1];
1155  double B3 = (A[1] + C2 * H1[0]) - A2 * H1[2];
1156  double C3 = (A[2] + A2 * H1[1]) - B2 * H1[0];
1157  // { A4, B4, C4 } is (direction + h/2 * sd.k3) in EigenStepper
1158  double A4 = (A[0] + B3 * H1[2]) - C3 * H1[1];
1159  double B4 = (A[1] + C3 * H1[0]) - A3 * H1[2];
1160  double C4 = (A[2] + A3 * H1[1]) - B3 * H1[0];
1161  // { A5, B5, C5 } is (direction + h * sd.k3) in EigenStepper
1162  double A5 = 2. * A4 - A[0];
1163  double B5 = 2. * B4 - A[1];
1164  double C5 = 2. * C4 - A[2];
1165 
1166  // Last point
1167  //
1168  if (!Helix) {
1169  // This is pos2 in EigenStepper
1170  const Vector3D pos(R[0] + h * A4, R[1] + h * B4, R[2] + h * C4);
1171  // This is sd.B_last in Eigen stepper
1172  f = getField(state.stepping, pos);
1173  } else {
1174  f = f0;
1175  }
1176 
1177  // H2 is (h/(2*momentum) * sd.B_last) in EigenStepper
1178  double H2[3] = {f[0] * PS2, f[1] * PS2, f[2] * PS2};
1179  // { A6, B6, C6 } is (h/2 * sd.k4) in EigenStepper
1180  double A6 = B5 * H2[2] - C5 * H2[1];
1181  double B6 = C5 * H2[0] - A5 * H2[2];
1182  double C6 = A5 * H2[1] - B5 * H2[0];
1183 
1184  // Test approximation quality on give step and possible step reduction
1185  //
1186  // This is (h2 * (sd.k1 - sd.k2 - sd.k3 + sd.k4).template lpNorm<1>())
1187  // in EigenStepper
1188  double EST =
1189  2. * h *
1190  (std::abs((A1 + A6) - (A3 + A4)) + std::abs((B1 + B6) - (B3 + B4)) +
1191  std::abs((C1 + C6) - (C3 + C4)));
1192  if (EST > state.options.tolerance) {
1193  h = h * .5;
1194  // dltm = 0.;
1195  continue;
1196  }
1197 
1198  // if (EST < dltm) h *= 2.;
1199 
1200  // Parameters calculation
1201  //
1202  double A00 = A[0], A11 = A[1], A22 = A[2];
1203 
1204  A[0] = 2. * A3 + (A0 + A5 + A6);
1205  A[1] = 2. * B3 + (B0 + B5 + B6);
1206  A[2] = 2. * C3 + (C0 + C5 + C6);
1207 
1208  double D = (A[0] * A[0] + A[1] * A[1]) + (A[2] * A[2] - 9.);
1209  double Sl = 2. / h;
1210  D = (1. / 3.) - ((1. / 648.) * D) * (12. - D);
1211 
1212  R[0] += (A2 + A3 + A4) * S3;
1213  R[1] += (B2 + B3 + B4) * S3;
1214  R[2] += (C2 + C3 + C4) * S3;
1215  A[0] *= D;
1216  A[1] *= D;
1217  A[2] *= D;
1218  sA[0] = A6 * Sl;
1219  sA[1] = B6 * Sl;
1220  sA[2] = C6 * Sl;
1221 
1222  // Evaluate the time propagation
1223  state.stepping.pVector[3] +=
1224  h * std::hypot(1, state.options.mass / momentum(state.stepping));
1225  state.stepping.pVector[59] =
1226  std::hypot(1, state.options.mass / momentum(state.stepping));
1227  state.stepping.field = f;
1228  state.stepping.newfield = false;
1229 
1230  if (Jac) {
1231  double dtdl =
1232  h * state.options.mass * state.options.mass *
1233  charge(state.stepping) /
1234  (momentum(state.stepping) *
1235  std::hypot(1., state.options.mass / momentum(state.stepping)));
1236  state.stepping.pVector[43] += dtdl;
1237 
1238  // Jacobian calculation
1239  //
1240  double* d2A = &state.stepping.pVector[28];
1241  double* d3A = &state.stepping.pVector[36];
1242  double* d4A = &state.stepping.pVector[44];
1243  double d2A0 = H0[2] * d2A[1] - H0[1] * d2A[2];
1244  double d2B0 = H0[0] * d2A[2] - H0[2] * d2A[0];
1245  double d2C0 = H0[1] * d2A[0] - H0[0] * d2A[1];
1246  double d3A0 = H0[2] * d3A[1] - H0[1] * d3A[2];
1247  double d3B0 = H0[0] * d3A[2] - H0[2] * d3A[0];
1248  double d3C0 = H0[1] * d3A[0] - H0[0] * d3A[1];
1249  double d4A0 = (A0 + H0[2] * d4A[1]) - H0[1] * d4A[2];
1250  double d4B0 = (B0 + H0[0] * d4A[2]) - H0[2] * d4A[0];
1251  double d4C0 = (C0 + H0[1] * d4A[0]) - H0[0] * d4A[1];
1252  double d2A2 = d2A0 + d2A[0];
1253  double d2B2 = d2B0 + d2A[1];
1254  double d2C2 = d2C0 + d2A[2];
1255  double d3A2 = d3A0 + d3A[0];
1256  double d3B2 = d3B0 + d3A[1];
1257  double d3C2 = d3C0 + d3A[2];
1258  double d4A2 = d4A0 + d4A[0];
1259  double d4B2 = d4B0 + d4A[1];
1260  double d4C2 = d4C0 + d4A[2];
1261  double d0 = d4A[0] - A00;
1262  double d1 = d4A[1] - A11;
1263  double d2 = d4A[2] - A22;
1264  double d2A3 = (d2A[0] + d2B2 * H1[2]) - d2C2 * H1[1];
1265  double d2B3 = (d2A[1] + d2C2 * H1[0]) - d2A2 * H1[2];
1266  double d2C3 = (d2A[2] + d2A2 * H1[1]) - d2B2 * H1[0];
1267  double d3A3 = (d3A[0] + d3B2 * H1[2]) - d3C2 * H1[1];
1268  double d3B3 = (d3A[1] + d3C2 * H1[0]) - d3A2 * H1[2];
1269  double d3C3 = (d3A[2] + d3A2 * H1[1]) - d3B2 * H1[0];
1270  double d4A3 = ((A3 + d0) + d4B2 * H1[2]) - d4C2 * H1[1];
1271  double d4B3 = ((B3 + d1) + d4C2 * H1[0]) - d4A2 * H1[2];
1272  double d4C3 = ((C3 + d2) + d4A2 * H1[1]) - d4B2 * H1[0];
1273  double d2A4 = (d2A[0] + d2B3 * H1[2]) - d2C3 * H1[1];
1274  double d2B4 = (d2A[1] + d2C3 * H1[0]) - d2A3 * H1[2];
1275  double d2C4 = (d2A[2] + d2A3 * H1[1]) - d2B3 * H1[0];
1276  double d3A4 = (d3A[0] + d3B3 * H1[2]) - d3C3 * H1[1];
1277  double d3B4 = (d3A[1] + d3C3 * H1[0]) - d3A3 * H1[2];
1278  double d3C4 = (d3A[2] + d3A3 * H1[1]) - d3B3 * H1[0];
1279  double d4A4 = ((A4 + d0) + d4B3 * H1[2]) - d4C3 * H1[1];
1280  double d4B4 = ((B4 + d1) + d4C3 * H1[0]) - d4A3 * H1[2];
1281  double d4C4 = ((C4 + d2) + d4A3 * H1[1]) - d4B3 * H1[0];
1282  double d2A5 = 2. * d2A4 - d2A[0];
1283  double d2B5 = 2. * d2B4 - d2A[1];
1284  double d2C5 = 2. * d2C4 - d2A[2];
1285  double d3A5 = 2. * d3A4 - d3A[0];
1286  double d3B5 = 2. * d3B4 - d3A[1];
1287  double d3C5 = 2. * d3C4 - d3A[2];
1288  double d4A5 = 2. * d4A4 - d4A[0];
1289  double d4B5 = 2. * d4B4 - d4A[1];
1290  double d4C5 = 2. * d4C4 - d4A[2];
1291  double d2A6 = d2B5 * H2[2] - d2C5 * H2[1];
1292  double d2B6 = d2C5 * H2[0] - d2A5 * H2[2];
1293  double d2C6 = d2A5 * H2[1] - d2B5 * H2[0];
1294  double d3A6 = d3B5 * H2[2] - d3C5 * H2[1];
1295  double d3B6 = d3C5 * H2[0] - d3A5 * H2[2];
1296  double d3C6 = d3A5 * H2[1] - d3B5 * H2[0];
1297  double d4A6 = d4B5 * H2[2] - d4C5 * H2[1];
1298  double d4B6 = d4C5 * H2[0] - d4A5 * H2[2];
1299  double d4C6 = d4A5 * H2[1] - d4B5 * H2[0];
1300 
1301  double* dR = &state.stepping.pVector[24];
1302  dR[0] += (d2A2 + d2A3 + d2A4) * S3;
1303  dR[1] += (d2B2 + d2B3 + d2B4) * S3;
1304  dR[2] += (d2C2 + d2C3 + d2C4) * S3;
1305  d2A[0] = ((d2A0 + 2. * d2A3) + (d2A5 + d2A6)) * (1. / 3.);
1306  d2A[1] = ((d2B0 + 2. * d2B3) + (d2B5 + d2B6)) * (1. / 3.);
1307  d2A[2] = ((d2C0 + 2. * d2C3) + (d2C5 + d2C6)) * (1. / 3.);
1308 
1309  dR = &state.stepping.pVector[32];
1310  dR[0] += (d3A2 + d3A3 + d3A4) * S3;
1311  dR[1] += (d3B2 + d3B3 + d3B4) * S3;
1312  dR[2] += (d3C2 + d3C3 + d3C4) * S3;
1313  d3A[0] = ((d3A0 + 2. * d3A3) + (d3A5 + d3A6)) * (1. / 3.);
1314  d3A[1] = ((d3B0 + 2. * d3B3) + (d3B5 + d3B6)) * (1. / 3.);
1315  d3A[2] = ((d3C0 + 2. * d3C3) + (d3C5 + d3C6)) * (1. / 3.);
1316 
1317  dR = &state.stepping.pVector[40];
1318  dR[0] += (d4A2 + d4A3 + d4A4) * S3;
1319  dR[1] += (d4B2 + d4B3 + d4B4) * S3;
1320  dR[2] += (d4C2 + d4C3 + d4C4) * S3;
1321  d4A[0] = ((d4A0 + 2. * d4A3) + (d4A5 + d4A6 + A6)) * (1. / 3.);
1322  d4A[1] = ((d4B0 + 2. * d4B3) + (d4B5 + d4B6 + B6)) * (1. / 3.);
1323  d4A[2] = ((d4C0 + 2. * d4C3) + (d4C5 + d4C6 + C6)) * (1. / 3.);
1324  }
1325 
1326  state.stepping.pathAccumulated += h;
1327  return h;
1328  }
1329 
1330  // that exit path should actually not happen
1331  state.stepping.pathAccumulated += h;
1332  return h;
1333  }
1334 
1335  private:
1336  bfield_t m_bField;
1337 
1339  double m_overstepLimit = -50_um;
1340 };
1341 
1342 } // namespace Acts