rkf45.hh

#ifndef VTESTBED_CORE_RKF45_HH
#define VTESTBED_CORE_RKF45_HH

#include <cmath>
#include <iostream>

#include <vtestbed/base/blitz.hh>
#include <vtestbed/core/math.hh>

namespace vtb {

    namespace core {

        template <class T>
        class RKF45 {

        public:
            typedef T value_type;

        private:
            T _minstep = T(1e-3);
            T _maxstep = T(1);
            T _tolerance = T(1e-6);
            bool _verbose = false;

        public:

            RKF45() = default;
            RKF45(const RKF45&) = default;
            RKF45& operator=(const RKF45&) = default;
            RKF45(RKF45&&) = default;
            RKF45& operator=(RKF45&&) = default;
            ~RKF45() = default;

            template <class Function, int N>
            inline blitz::TinyVector<T,N>
            solve(Function f, T t0, T t1, const blitz::TinyVector<T,N>& x0) {
                typedef blitz::TinyVector<T,N> vec;
                enum class state {
                    normal_iteration,
                    last_iteration,
                    end
                };
                state st = state::normal_iteration;
                const T tol = this->_tolerance;
                const T h_min = this->_minstep;
                const T h_max = this->_maxstep;
                T h = this->_minstep;
                T t = t0 + h;
                vec x_i(x0);
                if (this->_verbose) {
                    std::clog
                        << std::setw(8) << "rkf45"
                        << std::setw(10) << t0
                        << std::setw(10) << h
                        << std::setw(10) << x0
                        << std::endl;
                }
                while (st != state::end) {
                    const vec k_1 = h*f(t, x_i);
                    const vec k_2 = h*f(
                        t + (T(1)/T(4))*h,
                        x_i
                        + (T(1)/T(4))*k_1
                    );
                    const vec k_3 = h*f(
                        t + (T(3)/T(8))*h,
                        x_i
                        + (T(3)/T(32))*k_1
                        + (T(9)/T(32))*k_2
                    );
                    const vec k_4 = h*f(
                        t + (T(12)/T(13))*h,
                        x_i
                        + (T(1932)/T(2197))*k_1
                        - (T(7200)/T(2197))*k_2
                        + (T(7296)/T(2197))*k_3
                    );
                    const vec k_5 = h*f(
                        t + h,
                        x_i
                        + (T(439)/T(216))*k_1
                        - T(8)*k_2
                        + (T(3680)/T(513))*k_3
                        - (T(845)/T(4104))*k_4
                    );
                    const vec k_6 = h*f(
                        t + (T(1)/T(2))*h,
                        x_i
                        - (T(8)/T(27))*k_1
                        + T(2)*k_2
                        - (T(3544)/T(2565))*k_3
                        + (T(1859)/T(4104))*k_4
                        - (T(11)/T(40))*k_5
                    );
                    vec x_ip1 = x_i
                        + (T(25)/T(216))*k_1
                        + (T(1408)/T(2565))*k_3
                        + (T(2197)/T(4101))*k_4
                        - (T(1)/T(5))*k_5;
                    vec z_ip1 = x_i
                        + (T(16)/T(135))*k_1
                        + (T(6656)/T(12825))*k_3
                        + (T(28561)/T(56430))*k_4
                        - (T(9)/T(50))*k_5;
                        + (T(2)/T(55))*k_6;
                    if (this->_verbose) {
                        std::clog
                            << std::setw(8) << "rkf45"
                            << std::setw(10) << t
                            << std::setw(10) << h
                            << std::setw(10) << x_ip1
                            << std::endl;
                    }
                    if (st == state::last_iteration) {
                        st = state::end;
                    } else {
                        using blitz::abs;
                        using blitz::max;
                        const T diff = max(abs(x_ip1 - z_ip1));
                        const T s = std::pow((tol*h) / (T(2)*diff), T(1)/T(4));
                        h = blitz::clamp(s*h, h_min, h_max);
                        if (t + h + h_min > t1) {
                            h = t1 - t;
                            t = t1;
                            st = state::last_iteration;
                        } else {
                            t += h;
                        }
                    }
                    x_i = x_ip1;
                }
                return x_i;
            }

            template <class Function, int N>
            inline blitz::TinyVector<T,N>
            operator()(Function f, T t0, T t1, const blitz::TinyVector<T,N>& x0) {
                return this->solve<Function,N>(f, t0, t1, x0);
            }

            inline T
            min_step() const noexcept {
                return this->_minstep;
            }

            inline void
            min_step(T rhs) {
                if (!(rhs > T(0))) {
                    throw std::invalid_argument("RKF45::min_step <= 0");
                }
                this->_minstep = rhs;
            }

            inline T
            max_step() const noexcept {
                return this->_maxstep;
            }

            inline void
            max_step(T rhs) {
                if (!(rhs > T(0))) {
                    throw std::invalid_argument("RKF45::max_step <= 0");
                }
                this->_maxstep = rhs;
            }

            inline T
            tolerance() const noexcept {
                return this->_tolerance;
            }

            inline void
            tolerance(T rhs) {
                if (!(rhs > T(0))) {
                    throw std::invalid_argument("RKF45::tolerance <= 0");
                }
                this->_tolerance = rhs;
            }

            inline void
            verbose(bool rhs) noexcept {
                this->_verbose = rhs;
            }

            inline bool
            verbose() const noexcept {
                return this->_verbose;
            }

        };

    }

}

#endif // vim:filetype=cpp