Acts/Propagator/EigenStepper.ipp

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2019-2022 CERN for the benefit of the Acts project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
0008
0009 #include "Acts/EventData/TransformationHelpers.hpp"
0010 #include "Acts/Propagator/ConstrainedStep.hpp"
0011 #include "Acts/Propagator/detail/CovarianceEngine.hpp"
0012
0013 template <typename E, typename A>
0014 Acts::EigenStepper<E, A>::EigenStepper(
0015     std::shared_ptr<const MagneticFieldProvider> bField, double overstepLimit)
0016     : m_bField(std::move(bField)), m_overstepLimit(overstepLimit) {}
0017
0018 template <typename E, typename A>
0019 auto Acts::EigenStepper<E, A>::makeState(
0020     std::reference_wrapper<const GeometryContext> gctx,
0021     std::reference_wrapper<const MagneticFieldContext> mctx,
0022     const BoundTrackParameters& par, double ssize) const -> State {
0023   return State{gctx, m_bField->makeCache(mctx), par, ssize};
0024 }
0025
0026 template <typename E, typename A>
0027 void Acts::EigenStepper<E, A>::resetState(State& state,
0028                                           const BoundVector& boundParams,
0029                                           const BoundSquareMatrix& cov,
0030                                           const Surface& surface,
0031                                           const double stepSize) const {
0032   FreeVector freeParams =
0033       transformBoundToFreeParameters(surface, state.geoContext, boundParams);
0034
0035   // Update the stepping state
0036   state.pars = freeParams;
0037   state.cov = cov;
0038   state.stepSize = ConstrainedStep(stepSize);
0039   state.pathAccumulated = 0.;
0040
0041   // Reinitialize the stepping jacobian
0042   state.jacToGlobal = surface.boundToFreeJacobian(
0043       state.geoContext, freeParams.template segment<3>(eFreePos0),
0044       freeParams.template segment<3>(eFreeDir0));
0045   state.jacobian = BoundMatrix::Identity();
0046   state.jacTransport = FreeMatrix::Identity();
0047   state.derivative = FreeVector::Zero();
0048 }
0049
0050 template <typename E, typename A>
0051 auto Acts::EigenStepper<E, A>::boundState(
0052     State& state, const Surface& surface, bool transportCov,
0053     const FreeToBoundCorrection& freeToBoundCorrection) const
0054     -> Result<BoundState> {
0055   return detail::boundState(
0056       state.geoContext, surface, state.cov, state.jacobian, state.jacTransport,
0057       state.derivative, state.jacToGlobal, state.pars, state.particleHypothesis,
0058       state.covTransport && transportCov, state.pathAccumulated,
0059       freeToBoundCorrection);
0060 }
0061
0062 template <typename E, typename A>
0063 template <typename propagator_state_t, typename navigator_t>
0064 bool Acts::EigenStepper<E, A>::prepareCurvilinearState(
0065     propagator_state_t& prop_state, const navigator_t& navigator) const {
0066   // test whether the accumulated path has still its initial value.
0067   if (prop_state.stepping.pathAccumulated == 0.) {
0068     // if no step was executed the path length derivates have not been
0069     // computed but are needed to compute the curvilinear covariance. The
0070     // derivates are given by k1 for a zero step width.
0071     if (prop_state.stepping.extension.validExtensionForStep(prop_state, *this,
0072                                                             navigator)) {
0073       // First Runge-Kutta point (at current position)
0074       auto& sd = prop_state.stepping.stepData;
0075       auto pos = position(prop_state.stepping);
0076       auto fieldRes = getField(prop_state.stepping, pos);
0077       if (fieldRes.ok()) {
0078         sd.B_first = *fieldRes;
0079         if (prop_state.stepping.extension.k1(prop_state, *this, navigator,
0080                                              sd.k1, sd.B_first, sd.kQoP)) {
0081           // dr/ds :
0082           prop_state.stepping.derivative.template head<3>() =
0083               prop_state.stepping.pars.template segment<3>(eFreeDir0);
0084           // d (dr/ds) / ds :
0085           prop_state.stepping.derivative.template segment<3>(4) = sd.k1;
0086           // to set dt/ds :
0087           prop_state.stepping.extension.finalize(
0088               prop_state, *this, navigator,
0089               prop_state.stepping.pathAccumulated);
0090           return true;
0091         }
0092       }
0093     }
0094     return false;
0095   }
0096   return true;
0097 }
0098
0099 template <typename E, typename A>
0100 auto Acts::EigenStepper<E, A>::curvilinearState(State& state,
0101                                                 bool transportCov) const
0102     -> CurvilinearState {
0103   return detail::curvilinearState(
0104       state.cov, state.jacobian, state.jacTransport, state.derivative,
0105       state.jacToGlobal, state.pars, state.particleHypothesis,
0106       state.covTransport && transportCov, state.pathAccumulated);
0107 }
0108
0109 template <typename E, typename A>
0110 void Acts::EigenStepper<E, A>::update(State& state,
0111                                       const FreeVector& freeParams,
0112                                       const BoundVector& /*boundParams*/,
0113                                       const Covariance& covariance,
0114                                       const Surface& surface) const {
0115   state.pars = freeParams;
0116   state.cov = covariance;
0117   state.jacToGlobal = surface.boundToFreeJacobian(
0118       state.geoContext, freeParams.template segment<3>(eFreePos0),
0119       freeParams.template segment<3>(eFreeDir0));
0120 }
0121
0122 template <typename E, typename A>
0123 void Acts::EigenStepper<E, A>::update(State& state, const Vector3& uposition,
0124                                       const Vector3& udirection, double qOverP,
0125                                       double time) const {
0126   state.pars.template segment<3>(eFreePos0) = uposition;
0127   state.pars.template segment<3>(eFreeDir0) = udirection;
0128   state.pars[eFreeTime] = time;
0129   state.pars[eFreeQOverP] = qOverP;
0130 }
0131
0132 template <typename E, typename A>
0133 void Acts::EigenStepper<E, A>::transportCovarianceToCurvilinear(
0134     State& state) const {
0135   detail::transportCovarianceToCurvilinear(state.cov, state.jacobian,
0136                                            state.jacTransport, state.derivative,
0137                                            state.jacToGlobal, direction(state));
0138 }
0139
0140 template <typename E, typename A>
0141 void Acts::EigenStepper<E, A>::transportCovarianceToBound(
0142     State& state, const Surface& surface,
0143     const FreeToBoundCorrection& freeToBoundCorrection) const {
0144   detail::transportCovarianceToBound(state.geoContext.get(), surface, state.cov,
0145                                      state.jacobian, state.jacTransport,
0146                                      state.derivative, state.jacToGlobal,
0147                                      state.pars, freeToBoundCorrection);
0148 }
0149
0150 template <typename E, typename A>
0151 template <typename propagator_state_t, typename navigator_t>
0152 Acts::Result<double> Acts::EigenStepper<E, A>::step(
0153     propagator_state_t& state, const navigator_t& navigator) const {
0154   // Runge-Kutta integrator state
0155   auto& sd = state.stepping.stepData;
0156   double error_estimate = 0.;
0157   double h2 = 0, half_h = 0;
0158
0159   auto pos = position(state.stepping);
0160   auto dir = direction(state.stepping);
0161
0162   // First Runge-Kutta point (at current position)
0163   auto fieldRes = getField(state.stepping, pos);
0164   if (!fieldRes.ok()) {
0165     return fieldRes.error();
0166   }
0167   sd.B_first = *fieldRes;
0168   if (!state.stepping.extension.validExtensionForStep(state, *this,
0169                                                       navigator) ||
0170       !state.stepping.extension.k1(state, *this, navigator, sd.k1, sd.B_first,
0171                                    sd.kQoP)) {
0172     return 0.;
0173   }
0174
0175   // The following functor starts to perform a Runge-Kutta step of a certain
0176   // size, going up to the point where it can return an estimate of the local
0177   // integration error. The results are stated in the local variables above,
0178   // allowing integration to continue once the error is deemed satisfactory
0179   const auto tryRungeKuttaStep = [&](const double h) -> Result<bool> {
0180     // helpers because bool and std::error_code are ambiguous
0181     constexpr auto success = &Result<bool>::success;
0182     constexpr auto failure = &Result<bool>::failure;
0183
0184     // State the square and half of the step size
0185     h2 = h * h;
0186     half_h = h * 0.5;
0187
0188     // Second Runge-Kutta point
0189     const Vector3 pos1 = pos + half_h * dir + h2 * 0.125 * sd.k1;
0190     auto field = getField(state.stepping, pos1);
0191     if (!field.ok()) {
0192       return failure(field.error());
0193     }
0194     sd.B_middle = *field;
0195
0196     if (!state.stepping.extension.k2(state, *this, navigator, sd.k2,
0197                                      sd.B_middle, sd.kQoP, half_h, sd.k1)) {
0198       return success(false);
0199     }
0200
0201     // Third Runge-Kutta point
0202     if (!state.stepping.extension.k3(state, *this, navigator, sd.k3,
0203                                      sd.B_middle, sd.kQoP, half_h, sd.k2)) {
0204       return success(false);
0205     }
0206
0207     // Last Runge-Kutta point
0208     const Vector3 pos2 = pos + h * dir + h2 * 0.5 * sd.k3;
0209     field = getField(state.stepping, pos2);
0210     if (!field.ok()) {
0211       return failure(field.error());
0212     }
0213     sd.B_last = *field;
0214     if (!state.stepping.extension.k4(state, *this, navigator, sd.k4, sd.B_last,
0215                                      sd.kQoP, h, sd.k3)) {
0216       return success(false);
0217     }
0218
0219     // Compute and check the local integration error estimate
0220     error_estimate =
0221         h2 * ((sd.k1 - sd.k2 - sd.k3 + sd.k4).template lpNorm<1>() +
0222               std::abs(sd.kQoP[0] - sd.kQoP[1] - sd.kQoP[2] + sd.kQoP[3]));
0223     error_estimate = std::max(error_estimate, 1e-20);
0224
0225     return success(error_estimate <= state.options.stepTolerance);
0226   };
0227
0228   const double initialH =
0229       state.stepping.stepSize.value() * state.options.direction;
0230   double h = initialH;
0231   std::size_t nStepTrials = 0;
0232   // Select and adjust the appropriate Runge-Kutta step size as given
0233   // ATL-SOFT-PUB-2009-001
0234   while (true) {
0235     auto res = tryRungeKuttaStep(h);
0236     if (!res.ok()) {
0237       return res.error();
0238     }
0239     if (!!res.value()) {
0240       break;
0241     }
0242
0243     // double std::sqrt is 3x faster than std::pow
0244     const double stepSizeScaling =
0245         std::clamp(std::sqrt(std::sqrt(state.options.stepTolerance /
0246                                        std::abs(2. * error_estimate))),
0247                    0.25, 4.0);
0248     h *= stepSizeScaling;
0249
0250     // If step size becomes too small the particle remains at the initial
0251     // place
0252     if (std::abs(h) < std::abs(state.options.stepSizeCutOff)) {
0253       // Not moving due to too low momentum needs an aborter
0254       return EigenStepperError::StepSizeStalled;
0255     }
0256
0257     // If the parameter is off track too much or given stepSize is not
0258     // appropriate
0259     if (nStepTrials > state.options.maxRungeKuttaStepTrials) {
0260       // Too many trials, have to abort
0261       return EigenStepperError::StepSizeAdjustmentFailed;
0262     }
0263     nStepTrials++;
0264   }
0265
0266   // When doing error propagation, update the associated Jacobian matrix
0267   if (state.stepping.covTransport) {
0268     // The step transport matrix in global coordinates
0269     FreeMatrix D;
0270     if (!state.stepping.extension.finalize(state, *this, navigator, h, D)) {
0271       return EigenStepperError::StepInvalid;
0272     }
0273
0274     // See the documentation of Acts::blockedMult for a description of blocked
0275     // matrix multiplication. However, we can go one further. Let's assume that
0276     // some of these sub-matrices are zero matrices 0₈ and identity matrices
0277     // I₈, namely:
0278     //
0279     // D₁₁ = I₈, J₁₁ = I₈, D₂₁ = 0₈, J₂₁ = 0₈
0280     //
0281     // Which gives:
0282     //
0283     // K₁₁ = I₈  * I₈  + D₁₂ * 0₈  = I₈
0284     // K₁₂ = I₈  * J₁₂ + D₁₂ * J₂₂ = J₁₂ + D₁₂ * J₂₂
0285     // K₂₁ = 0₈  * I₈  + D₂₂ * 0₈  = 0₈
0286     // K₂₂ = 0₈  * J₁₂ + D₂₂ * J₂₂ = D₂₂ * J₂₂
0287     //
0288     // Furthermore, we're constructing K in place of J, and since
0289     // K₁₁ = I₈ = J₁₁ and K₂₁ = 0₈ = D₂₁, we don't actually need to touch those
0290     // sub-matrices at all!
0291     assert((D.topLeftCorner<4, 4>().isIdentity()));
0292     assert((D.bottomLeftCorner<4, 4>().isZero()));
0293     assert((state.stepping.jacTransport.template topLeftCorner<4, 4>()
0294                 .isIdentity()));
0295     assert((state.stepping.jacTransport.template bottomLeftCorner<4, 4>()
0296                 .isZero()));
0297
0298     state.stepping.jacTransport.template topRightCorner<4, 4>() +=
0299         D.topRightCorner<4, 4>() *
0300         state.stepping.jacTransport.template bottomRightCorner<4, 4>();
0301     state.stepping.jacTransport.template bottomRightCorner<4, 4>() =
0302         (D.bottomRightCorner<4, 4>() *
0303          state.stepping.jacTransport.template bottomRightCorner<4, 4>())
0304             .eval();
0305   } else {
0306     if (!state.stepping.extension.finalize(state, *this, navigator, h)) {
0307       return EigenStepperError::StepInvalid;
0308     }
0309   }
0310
0311   // Update the track parameters according to the equations of motion
0312   state.stepping.pars.template segment<3>(eFreePos0) +=
0313       h * dir + h2 / 6. * (sd.k1 + sd.k2 + sd.k3);
0314   state.stepping.pars.template segment<3>(eFreeDir0) +=
0315       h / 6. * (sd.k1 + 2. * (sd.k2 + sd.k3) + sd.k4);
0316   (state.stepping.pars.template segment<3>(eFreeDir0)).normalize();
0317
0318   if (state.stepping.covTransport) {
0319     state.stepping.derivative.template head<3>() =
0320         state.stepping.pars.template segment<3>(eFreeDir0);
0321     state.stepping.derivative.template segment<3>(4) = sd.k4;
0322   }
0323   state.stepping.pathAccumulated += h;
0324   // double std::sqrt is 3x faster than std::pow
0325   const double stepSizeScaling =
0326       std::clamp(std::sqrt(std::sqrt(state.options.stepTolerance /
0327                                      std::abs(error_estimate))),
0328                  0.25, 4.0);
0329   const double nextAccuracy = std::abs(h * stepSizeScaling);
0330   const double previousAccuracy = std::abs(state.stepping.stepSize.accuracy());
0331   const double initialStepLength = std::abs(initialH);
0332   if (nextAccuracy < initialStepLength || nextAccuracy > previousAccuracy) {
0333     state.stepping.stepSize.setAccuracy(nextAccuracy);
0334   }
0335   state.stepping.stepSize.nStepTrials = nStepTrials;
0336
0337   return h;
0338 }
0339
0340 template <typename E, typename A>
0341 void Acts::EigenStepper<E, A>::setIdentityJacobian(State& state) const {
0342   state.jacobian = BoundMatrix::Identity();
0343 }