13 template <
typename E,
typename A>
15 std::shared_ptr<const MagneticFieldProvider>
bField,
double overstepLimit)
16 : m_bField(std::
move(bField)), m_overstepLimit(overstepLimit) {}
18 template <
typename E,
typename A>
20 std::reference_wrapper<const GeometryContext>
gctx,
21 std::reference_wrapper<const MagneticFieldContext> mctx,
23 return State{
gctx, m_bField->makeCache(mctx), par, ssize};
26 template <
typename E,
typename A>
36 boundParams, cov, surface);
43 state.
jacobian = BoundMatrix::Identity();
48 template <
typename E,
typename A>
57 freeToBoundCorrection);
60 template <
typename E,
typename A>
62 bool transportCov)
const
67 state.covTransport && transportCov,
state.pathAccumulated);
70 template <
typename E,
typename A>
76 state.
pars = freeParams;
82 template <
typename E,
typename A>
84 const Vector3& udirection,
double qOverP,
92 template <
typename E,
typename A>
100 template <
typename E,
typename A>
107 freeToBoundCorrection);
110 template <
typename E,
typename A>
111 template <
typename propagator_state_t,
typename navigator_t>
114 using namespace UnitLiterals;
117 auto& sd = state.stepping.stepData;
118 double error_estimate = 0.;
119 double h2 = 0, half_h = 0;
122 auto dir = direction(state.stepping);
125 auto fieldRes = getField(state.stepping,
pos);
126 if (!fieldRes.ok()) {
127 return fieldRes.error();
129 sd.B_first = *fieldRes;
130 if (!state.stepping.extension.validExtensionForStep(state, *
this,
132 !state.stepping.extension.k1(state, *
this, navigator, sd.k1, sd.B_first,
141 const auto tryRungeKuttaStep = [&](
const double h) ->
Result<bool> {
151 const Vector3 pos1 =
pos + half_h * dir +
h2 * 0.125 * sd.k1;
152 auto field = getField(state.stepping, pos1);
154 return failure(
field.error());
156 sd.B_middle = *
field;
158 if (!state.stepping.extension.k2(state, *
this, navigator, sd.k2,
159 sd.B_middle, sd.kQoP, half_h, sd.k1)) {
160 return success(
false);
164 if (!state.stepping.extension.k3(state, *
this, navigator, sd.k3,
165 sd.B_middle, sd.kQoP, half_h, sd.k2)) {
166 return success(
false);
171 field = getField(state.stepping, pos2);
173 return failure(
field.error());
176 if (!state.stepping.extension.k4(state, *
this, navigator, sd.k4, sd.B_last,
177 sd.kQoP, h, sd.k3)) {
178 return success(
false);
183 h2 * ((sd.k1 - sd.k2 - sd.k3 + sd.k4).
template lpNorm<1>() +
184 std::abs(sd.kQoP[0] - sd.kQoP[1] - sd.kQoP[2] + sd.kQoP[3]));
185 error_estimate = std::max(error_estimate, 1
e-20);
187 return success(error_estimate <= state.options.tolerance);
190 const double initialH =
191 state.stepping.stepSize.value() * state.options.direction;
193 size_t nStepTrials = 0;
197 auto res = tryRungeKuttaStep(
h);
205 const double stepSizeScaling =
206 std::min(std::max(0.25
f, std::sqrt(std::sqrt(static_cast<float>(
207 state.options.tolerance /
208 std::abs(2. * error_estimate))))),
210 h *= stepSizeScaling;
214 if (std::abs(
h) < std::abs(state.options.stepSizeCutOff)) {
216 return EigenStepperError::StepSizeStalled;
221 if (nStepTrials > state.options.maxRungeKuttaStepTrials) {
223 return EigenStepperError::StepSizeAdjustmentFailed;
229 if (state.stepping.covTransport) {
232 if (!state.stepping.extension.finalize(state, *
this, navigator,
h, D)) {
233 return EigenStepperError::StepInvalid;
237 state.stepping.jacTransport = D * state.stepping.jacTransport;
239 if (!state.stepping.extension.finalize(state, *
this, navigator,
h)) {
240 return EigenStepperError::StepInvalid;
245 state.stepping.pars.template segment<3>(
eFreePos0) +=
246 h * dir +
h2 / 6. * (sd.k1 + sd.k2 + sd.k3);
247 state.stepping.pars.template segment<3>(
eFreeDir0) +=
248 h / 6. * (sd.k1 + 2. * (sd.k2 + sd.k3) + sd.k4);
249 (state.stepping.pars.template segment<3>(
eFreeDir0)).normalize();
251 if (state.stepping.covTransport) {
252 state.stepping.derivative.template head<3>() =
253 state.stepping.pars.template segment<3>(
eFreeDir0);
254 state.stepping.derivative.template segment<3>(4) = sd.k4;
256 state.stepping.pathAccumulated +=
h;
257 const double stepSizeScaling =
std::min(
259 std::sqrt(std::sqrt(static_cast<float>(
260 state.options.tolerance / std::abs(error_estimate))))),
262 const double nextAccuracy = std::abs(
h * stepSizeScaling);
263 const double previousAccuracy = std::abs(state.stepping.stepSize.accuracy());
264 const double initialStepLength = std::abs(initialH);
265 if (nextAccuracy < initialStepLength || nextAccuracy > previousAccuracy) {
266 state.stepping.stepSize.setAccuracy(nextAccuracy);
268 state.stepping.stepSize.nStepTrials = nStepTrials;
273 template <
typename E,
typename A>
275 state.
jacobian = BoundMatrix::Identity();