RK45Integrator.hpp

Go to the documentation of this file.

#pragma once

 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <icarus/core/Error.hpp>
#include <icarus/sim/integrators/Integrator.hpp>
#include <janus/math/Arithmetic.hpp>
#include <janus/math/IntegratorStep.hpp>
#include <janus/math/Logic.hpp>
#include <type_traits>
 
namespace icarus {

template <typename Scalar> class RK45Integrator : public AdaptiveIntegrator<Scalar> {
  public:
    using typename Integrator<Scalar>::DerivativeFunc;

    explicit RK45Integrator(Scalar abs_tol = Scalar{1e-6}, Scalar rel_tol = Scalar{1e-6})
        : abs_tol_(abs_tol), rel_tol_(rel_tol) {}

    JanusVector<Scalar> Step(const DerivativeFunc &f, const JanusVector<Scalar> &x, Scalar t,
                             Scalar dt) override {
        auto result = janus::rk45_step(f, x, t, dt);
        return result.y5;
    }

    AdaptiveStepResult<Scalar> AdaptiveStep(const DerivativeFunc &f, const JanusVector<Scalar> &x,
                                            Scalar t, Scalar dt) override {
        Scalar current_dt = dt;
 
        // Retry loop with step size reduction
        constexpr int max_attempts = 20; // Prevent infinite loops
        for (int attempt = 0; attempt < max_attempts; ++attempt) {
            // Compute RK45 step
            auto result = janus::rk45_step(f, x, t, current_dt);
 
            // Compute error tolerance
            Scalar tol = ComputeTolerance(x, result.y5);
 
            // Check if step is accepted
            bool accepted = LessThanOrEqual(result.error, tol);
 
            if (accepted) {
                stats_.accepted_steps++;
                return AdaptiveStepResult<Scalar>{result.y5, current_dt, result.error, true};
            }
 
            // Step rejected - reduce dt and retry
            stats_.rejected_steps++;
 
            // Compute new step size
            Scalar dt_new = SuggestDt(current_dt, result.error, tol);
 
            // Check for minimum step size (only in numeric mode)
            if constexpr (std::is_same_v<Scalar, double>) {
                if (dt_new <= min_dt_) {
                    throw StepSizeTooSmallError(static_cast<double>(min_dt_));
                }
            }
 
            current_dt = dt_new;
        }
 
        // Max attempts reached - return last result as rejected
        auto result = janus::rk45_step(f, x, t, current_dt);
        return AdaptiveStepResult<Scalar>{result.y5, current_dt, result.error, false};
    }
 
    // Tolerance setters/getters
    void SetAbsTol(Scalar tol) override { abs_tol_ = tol; }
    void SetRelTol(Scalar tol) override { rel_tol_ = tol; }
    [[nodiscard]] Scalar GetAbsTol() const override { return abs_tol_; }
    [[nodiscard]] Scalar GetRelTol() const override { return rel_tol_; }
 
    [[nodiscard]] std::string Name() const override { return "RK45"; }
    [[nodiscard]] int Order() const override { return 5; }
    [[nodiscard]] IntegratorType Type() const override { return IntegratorType::RK45; }
 
    // Step size control parameters
    void SetMinDt(Scalar min_dt) { min_dt_ = min_dt; }
    void SetMaxDt(Scalar max_dt) { max_dt_ = max_dt; }
    void SetSafetyFactor(Scalar factor) { safety_ = factor; }
 
    [[nodiscard]] Scalar GetMinDt() const { return min_dt_; }
    [[nodiscard]] Scalar GetMaxDt() const { return max_dt_; }

    [[nodiscard]] Scalar SuggestDt(Scalar dt, Scalar error, Scalar tol) const {
        // Optimal step size formula: dt_new = dt * safety * (tol/error)^(1/5)
        // Use 5th root for RK45 (5th order method)
        Scalar ratio = tol / (error + Scalar{1e-15}); // Avoid division by zero
        Scalar factor = safety_ * janus::pow(ratio, Scalar{0.2});
 
        // Limit growth/shrinkage
        factor = janus::max(Scalar{0.1}, janus::min(Scalar{5.0}, factor));
 
        Scalar dt_new = dt * factor;
 
        // Clamp to min/max
        return janus::max(min_dt_, janus::min(max_dt_, dt_new));
    }

    struct Statistics {
        std::size_t accepted_steps = 0;
        std::size_t rejected_steps = 0;
 
        void Reset() {
            accepted_steps = 0;
            rejected_steps = 0;
        }
 
        [[nodiscard]] double AcceptanceRate() const {
            auto total = accepted_steps + rejected_steps;
            return total > 0 ? static_cast<double>(accepted_steps) / static_cast<double>(total)
                             : 1.0;
        }
    };
 
    [[nodiscard]] const Statistics &GetStatistics() const { return stats_; }
    void ResetStatistics() { stats_.Reset(); }
 
  private:
    Scalar abs_tol_;
    Scalar rel_tol_;
    Scalar min_dt_{1e-10};
    Scalar max_dt_{1.0};
    Scalar safety_{0.9};
    Statistics stats_;

    [[nodiscard]] Scalar ComputeTolerance(const JanusVector<Scalar> &x,
                                          const JanusVector<Scalar> &x_new) const {
        Scalar max_norm = Scalar{0};
        for (Eigen::Index i = 0; i < x.size(); ++i) {
            Scalar xi_abs = janus::abs(x[i]);
            Scalar xi_new_abs = janus::abs(x_new[i]);
            max_norm = janus::max(max_norm, janus::max(xi_abs, xi_new_abs));
        }
        return abs_tol_ + rel_tol_ * max_norm;
    }

    [[nodiscard]] bool LessThanOrEqual(Scalar a, Scalar b) const {
        // For numeric mode, direct comparison
        // For symbolic mode, this is only used in statistics (not traced)
        if constexpr (std::is_same_v<Scalar, double>) {
            return a <= b;
        } else {
            // For symbolic, evaluate numerically if possible
            // This branch only used in adaptive control, not traced
            return true; // Accept in symbolic mode
        }
    }
};
 
} // namespace icarus