File indexing completed on 2025-01-18 09:10:57
0001
0002
0003
0004
0005
0006
0007
0008
0009 #include "Acts/Definitions/Direction.hpp"
0010 #include "Acts/EventData/TransformationHelpers.hpp"
0011 #include "Acts/Propagator/ConstrainedStep.hpp"
0012 #include "Acts/Propagator/EigenStepperError.hpp"
0013 #include "Acts/Propagator/detail/CovarianceEngine.hpp"
0014
0015 #include <limits>
0016
0017 template <typename E>
0018 Acts::EigenStepper<E>::EigenStepper(
0019 std::shared_ptr<const MagneticFieldProvider> bField)
0020 : m_bField(std::move(bField)) {}
0021
0022 template <typename E>
0023 auto Acts::EigenStepper<E>::makeState(
0024 const Options& options, const BoundTrackParameters& par) const -> State {
0025 State state{options, m_bField->makeCache(options.magFieldContext)};
0026
0027 state.particleHypothesis = par.particleHypothesis();
0028
0029 Vector3 position = par.position(options.geoContext);
0030 Vector3 direction = par.direction();
0031 state.pars.template segment<3>(eFreePos0) = position;
0032 state.pars.template segment<3>(eFreeDir0) = direction;
0033 state.pars[eFreeTime] = par.time();
0034 state.pars[eFreeQOverP] = par.parameters()[eBoundQOverP];
0035
0036
0037 if (par.covariance()) {
0038
0039 const auto& surface = par.referenceSurface();
0040
0041 state.covTransport = true;
0042 state.cov = BoundSquareMatrix(*par.covariance());
0043 state.jacToGlobal =
0044 surface.boundToFreeJacobian(options.geoContext, position, direction);
0045 }
0046
0047 state.stepSize = ConstrainedStep(options.maxStepSize);
0048
0049 return state;
0050 }
0051
0052 template <typename E>
0053 void Acts::EigenStepper<E>::resetState(State& state,
0054 const BoundVector& boundParams,
0055 const BoundSquareMatrix& cov,
0056 const Surface& surface,
0057 const double stepSize) const {
0058 FreeVector freeParams = transformBoundToFreeParameters(
0059 surface, state.options.geoContext, boundParams);
0060
0061
0062 state.pars = freeParams;
0063 state.cov = cov;
0064 state.stepSize = ConstrainedStep(stepSize);
0065 state.pathAccumulated = 0.;
0066
0067
0068 state.jacToGlobal = surface.boundToFreeJacobian(
0069 state.options.geoContext, freeParams.template segment<3>(eFreePos0),
0070 freeParams.template segment<3>(eFreeDir0));
0071 state.jacobian = BoundMatrix::Identity();
0072 state.jacTransport = FreeMatrix::Identity();
0073 state.derivative = FreeVector::Zero();
0074 }
0075
0076 template <typename E>
0077 auto Acts::EigenStepper<E>::boundState(
0078 State& state, const Surface& surface, bool transportCov,
0079 const FreeToBoundCorrection& freeToBoundCorrection) const
0080 -> Result<BoundState> {
0081 return detail::boundState(
0082 state.options.geoContext, surface, state.cov, state.jacobian,
0083 state.jacTransport, state.derivative, state.jacToGlobal, state.pars,
0084 state.particleHypothesis, state.covTransport && transportCov,
0085 state.pathAccumulated, freeToBoundCorrection);
0086 }
0087
0088 template <typename E>
0089 template <typename propagator_state_t, typename navigator_t>
0090 bool Acts::EigenStepper<E>::prepareCurvilinearState(
0091 propagator_state_t& prop_state, const navigator_t& navigator) const {
0092
0093 if (prop_state.stepping.pathAccumulated == 0.) {
0094
0095
0096
0097
0098 auto& sd = prop_state.stepping.stepData;
0099 auto pos = position(prop_state.stepping);
0100 auto fieldRes = getField(prop_state.stepping, pos);
0101 if (fieldRes.ok()) {
0102 sd.B_first = *fieldRes;
0103 if (prop_state.stepping.extension.template k<0>(
0104 prop_state, *this, navigator, sd.k1, sd.B_first, sd.kQoP)) {
0105
0106 prop_state.stepping.derivative.template head<3>() =
0107 prop_state.stepping.pars.template segment<3>(eFreeDir0);
0108
0109 prop_state.stepping.derivative.template segment<3>(4) = sd.k1;
0110
0111 prop_state.stepping.extension.finalize(
0112 prop_state, *this, navigator, prop_state.stepping.pathAccumulated);
0113 return true;
0114 }
0115 }
0116 return false;
0117 }
0118 return true;
0119 }
0120
0121 template <typename E>
0122 auto Acts::EigenStepper<E>::curvilinearState(
0123 State& state, bool transportCov) const -> CurvilinearState {
0124 return detail::curvilinearState(
0125 state.cov, state.jacobian, state.jacTransport, state.derivative,
0126 state.jacToGlobal, state.pars, state.particleHypothesis,
0127 state.covTransport && transportCov, state.pathAccumulated);
0128 }
0129
0130 template <typename E>
0131 void Acts::EigenStepper<E>::update(State& state, const FreeVector& freeParams,
0132 const BoundVector& ,
0133 const Covariance& covariance,
0134 const Surface& surface) const {
0135 state.pars = freeParams;
0136 state.cov = covariance;
0137 state.jacToGlobal = surface.boundToFreeJacobian(
0138 state.options.geoContext, freeParams.template segment<3>(eFreePos0),
0139 freeParams.template segment<3>(eFreeDir0));
0140 }
0141
0142 template <typename E>
0143 void Acts::EigenStepper<E>::update(State& state, const Vector3& uposition,
0144 const Vector3& udirection, double qOverP,
0145 double time) const {
0146 state.pars.template segment<3>(eFreePos0) = uposition;
0147 state.pars.template segment<3>(eFreeDir0) = udirection;
0148 state.pars[eFreeTime] = time;
0149 state.pars[eFreeQOverP] = qOverP;
0150 }
0151
0152 template <typename E>
0153 void Acts::EigenStepper<E>::transportCovarianceToCurvilinear(
0154 State& state) const {
0155 detail::transportCovarianceToCurvilinear(state.cov, state.jacobian,
0156 state.jacTransport, state.derivative,
0157 state.jacToGlobal, direction(state));
0158 }
0159
0160 template <typename E>
0161 void Acts::EigenStepper<E>::transportCovarianceToBound(
0162 State& state, const Surface& surface,
0163 const FreeToBoundCorrection& freeToBoundCorrection) const {
0164 detail::transportCovarianceToBound(
0165 state.options.geoContext, surface, state.cov, state.jacobian,
0166 state.jacTransport, state.derivative, state.jacToGlobal, state.pars,
0167 freeToBoundCorrection);
0168 }
0169
0170 template <typename E>
0171 template <typename propagator_state_t, typename navigator_t>
0172 Acts::Result<double> Acts::EigenStepper<E>::step(
0173 propagator_state_t& state, const navigator_t& navigator) const {
0174
0175 auto& sd = state.stepping.stepData;
0176
0177 double errorEstimate = 0;
0178 double h2 = 0;
0179 double half_h = 0;
0180
0181 auto pos = position(state.stepping);
0182 auto dir = direction(state.stepping);
0183
0184
0185 auto fieldRes = getField(state.stepping, pos);
0186 if (!fieldRes.ok()) {
0187 return fieldRes.error();
0188 }
0189 sd.B_first = *fieldRes;
0190 if (!state.stepping.extension.template k<0>(state, *this, navigator, sd.k1,
0191 sd.B_first, sd.kQoP)) {
0192 return 0.;
0193 }
0194
0195 const auto calcStepSizeScaling = [&](const double errorEstimate_) -> double {
0196
0197 constexpr double lower = 0.25;
0198 constexpr double upper = 4.0;
0199
0200 constexpr double exponent = 0.25;
0201
0202 double x = state.options.stepping.stepTolerance / errorEstimate_;
0203
0204 if constexpr (exponent == 0.25) {
0205
0206 x = std::sqrt(std::sqrt(x));
0207 } else {
0208 x = std::pow(x, exponent);
0209 }
0210
0211 return std::clamp(x, lower, upper);
0212 };
0213
0214 const auto isErrorTolerable = [&](const double errorEstimate_) {
0215
0216 constexpr double marginFactor = 4.0;
0217
0218 return errorEstimate_ <=
0219 marginFactor * state.options.stepping.stepTolerance;
0220 };
0221
0222
0223
0224
0225
0226 const auto tryRungeKuttaStep = [&](const double h) -> Result<bool> {
0227
0228 constexpr auto success = &Result<bool>::success;
0229 constexpr auto failure = &Result<bool>::failure;
0230
0231
0232 h2 = h * h;
0233 half_h = h * 0.5;
0234
0235
0236 const Vector3 pos1 = pos + half_h * dir + h2 * 0.125 * sd.k1;
0237 auto field = getField(state.stepping, pos1);
0238 if (!field.ok()) {
0239 return failure(field.error());
0240 }
0241 sd.B_middle = *field;
0242
0243 if (!state.stepping.extension.template k<1>(state, *this, navigator, sd.k2,
0244 sd.B_middle, sd.kQoP, half_h,
0245 sd.k1)) {
0246 return success(false);
0247 }
0248
0249
0250 if (!state.stepping.extension.template k<2>(state, *this, navigator, sd.k3,
0251 sd.B_middle, sd.kQoP, half_h,
0252 sd.k2)) {
0253 return success(false);
0254 }
0255
0256
0257 const Vector3 pos2 = pos + h * dir + h2 * 0.5 * sd.k3;
0258 field = getField(state.stepping, pos2);
0259 if (!field.ok()) {
0260 return failure(field.error());
0261 }
0262 sd.B_last = *field;
0263 if (!state.stepping.extension.template k<3>(state, *this, navigator, sd.k4,
0264 sd.B_last, sd.kQoP, h, sd.k3)) {
0265 return success(false);
0266 }
0267
0268
0269 errorEstimate =
0270 h2 * ((sd.k1 - sd.k2 - sd.k3 + sd.k4).template lpNorm<1>() +
0271 std::abs(sd.kQoP[0] - sd.kQoP[1] - sd.kQoP[2] + sd.kQoP[3]));
0272
0273 errorEstimate = std::max(1e-20, errorEstimate);
0274
0275 return success(isErrorTolerable(errorEstimate));
0276 };
0277
0278 const double initialH =
0279 state.stepping.stepSize.value() * state.options.direction;
0280 double h = initialH;
0281 std::size_t nStepTrials = 0;
0282
0283
0284 while (true) {
0285 ++nStepTrials;
0286 ++state.stepping.statistics.nAttemptedSteps;
0287
0288 auto res = tryRungeKuttaStep(h);
0289 if (!res.ok()) {
0290 return res.error();
0291 }
0292 if (!!res.value()) {
0293 break;
0294 }
0295
0296 ++state.stepping.statistics.nRejectedSteps;
0297
0298 const double stepSizeScaling = calcStepSizeScaling(errorEstimate);
0299 h *= stepSizeScaling;
0300
0301
0302
0303 if (std::abs(h) < std::abs(state.options.stepping.stepSizeCutOff)) {
0304
0305 return EigenStepperError::StepSizeStalled;
0306 }
0307
0308
0309
0310 if (nStepTrials > state.options.stepping.maxRungeKuttaStepTrials) {
0311
0312 return EigenStepperError::StepSizeAdjustmentFailed;
0313 }
0314 }
0315
0316
0317 if (state.stepping.covTransport) {
0318
0319
0320
0321 FreeMatrix D;
0322 if (!state.stepping.extension.finalize(state, *this, navigator, h, D)) {
0323 return EigenStepperError::StepInvalid;
0324 }
0325
0326
0327
0328
0329
0330
0331
0332
0333
0334
0335
0336
0337
0338
0339
0340
0341
0342
0343 assert((D.topLeftCorner<4, 4>().isIdentity()));
0344 assert((D.bottomLeftCorner<4, 4>().isZero()));
0345 assert((state.stepping.jacTransport.template topLeftCorner<4, 4>()
0346 .isIdentity()));
0347 assert((state.stepping.jacTransport.template bottomLeftCorner<4, 4>()
0348 .isZero()));
0349
0350 state.stepping.jacTransport.template topRightCorner<4, 4>() +=
0351 D.topRightCorner<4, 4>() *
0352 state.stepping.jacTransport.template bottomRightCorner<4, 4>();
0353 state.stepping.jacTransport.template bottomRightCorner<4, 4>() =
0354 (D.bottomRightCorner<4, 4>() *
0355 state.stepping.jacTransport.template bottomRightCorner<4, 4>())
0356 .eval();
0357 } else {
0358 if (!state.stepping.extension.finalize(state, *this, navigator, h)) {
0359 return EigenStepperError::StepInvalid;
0360 }
0361 }
0362
0363
0364 state.stepping.pars.template segment<3>(eFreePos0) +=
0365 h * dir + h2 / 6. * (sd.k1 + sd.k2 + sd.k3);
0366 state.stepping.pars.template segment<3>(eFreeDir0) +=
0367 h / 6. * (sd.k1 + 2. * (sd.k2 + sd.k3) + sd.k4);
0368 (state.stepping.pars.template segment<3>(eFreeDir0)).normalize();
0369
0370 if (state.stepping.covTransport) {
0371
0372 state.stepping.derivative.template head<3>() =
0373 state.stepping.pars.template segment<3>(eFreeDir0);
0374 state.stepping.derivative.template segment<3>(4) = sd.k4;
0375 }
0376
0377 state.stepping.pathAccumulated += h;
0378 ++state.stepping.nSteps;
0379 state.stepping.nStepTrials += nStepTrials;
0380
0381 ++state.stepping.statistics.nSuccessfulSteps;
0382 if (state.options.direction !=
0383 Direction::fromScalarZeroAsPositive(initialH)) {
0384 ++state.stepping.statistics.nReverseSteps;
0385 }
0386 state.stepping.statistics.pathLength += h;
0387 state.stepping.statistics.absolutePathLength += std::abs(h);
0388
0389 const double stepSizeScaling = calcStepSizeScaling(errorEstimate);
0390 const double nextAccuracy = std::abs(h * stepSizeScaling);
0391 const double previousAccuracy = std::abs(state.stepping.stepSize.accuracy());
0392 const double initialStepLength = std::abs(initialH);
0393 if (nextAccuracy < initialStepLength || nextAccuracy > previousAccuracy) {
0394 state.stepping.stepSize.setAccuracy(nextAccuracy);
0395 }
0396
0397 return h;
0398 }
0399
0400 template <typename E>
0401 void Acts::EigenStepper<E>::setIdentityJacobian(State& state) const {
0402 state.jacobian = BoundMatrix::Identity();
0403 }