RK4Integrator.hpp

Go to the documentation of this file.

/******************************************************************************
* Copyright (c) ATTX LLC 2024. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, LLC. The Software is 
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX, LLC.
******************************************************************************/
/*
RK4 header file
---------------
 
Author: Alex Reynolds
*/
#ifndef INTEGRATOR_RK4_HPP
#define INTEGRATOR_RK4_HPP
 
#include <array>
 
#include "gncutils/dynamics/Rates.hpp"
#include "Integrator.hpp"
 
namespace warpos {
 
    template <uint32 N>
    class RK4Integrator : public Integrator<N> {
    public:
        RK4Integrator(Rates<N>& rate_calculator);

        int16 step(floating_point start_time, floating_point end_time, const std::array<floating_point, N> &start_state, std::array<floating_point, N> &out_state) override;

        void configureForStep(floating_point start_time, floating_point end_time, const std::array<floating_point, N> &start_state);
        int16 calculateK1(std::array<floating_point, N> &state_for_k2);
        int16 calculateK2(const std::array<floating_point, N> &state_in_k2, std::array<floating_point, N> &state_for_k3);
        int16 calculateK3(const std::array<floating_point, N> &state_in_k2, std::array<floating_point, N> &state_for_k4);
        int16 calculateK4(const std::array<floating_point, N> &state_in_k4);
        void getValueEndStep(std::array<floating_point, N> &end_state);
    protected:
        floating_point _full_step_size = 1.0;
        floating_point _half_step_size = 0.5;
        floating_point _step_average;

        floating_point _start_time;
        floating_point _end_time;
        std::array<floating_point, N> _start_state;

        std::array<floating_point, N> _k1;
        std::array<floating_point, N> _k2;
        std::array<floating_point, N> _k3;
        std::array<floating_point, N> _k4;

        int16 _error = NO_ERROR;
        int16 _saved_warning = NO_ERROR;
    };
 
    template <uint32 N>
    RK4Integrator<N>::RK4Integrator(Rates<N>& rate_calculator) 
        : Integrator<N>(rate_calculator) {
        _full_step_size = 0.0;
        _half_step_size = 0.0;
        _step_average = 0.0;
        _start_time = 0.0;
        _end_time = 0.0;

        for(uint32 i = 0; i < N; i++) {
            _start_state[i] = 0.0;
            _k1[i] = 0.0;
            _k2[i] = 0.0;
            _k3[i] = 0.0;
            _k4[i] = 0.0;
        }
    }
 
    template <uint32 N>
    void RK4Integrator<N>::configureForStep(floating_point start_time, floating_point end_time, 
                                            const std::array<floating_point, N> &start_state) {
        _full_step_size = end_time - start_time;
        _half_step_size = 0.5*_full_step_size;
        _step_average = _full_step_size/6.0;
        _start_time = start_time;
        _end_time = end_time;
        for(uint32 i = 0; i < N; i++) {
            _start_state[i] = start_state[i];
        }
    }
 
    template <uint32 N>
    int16 RK4Integrator<N>::calculateK1(std::array<floating_point, N> &state_for_k2) {
        _error = Integrator<N>::_rate_calculator.calculateRates(_start_time, _start_state, _k1);

        for(uint32 i = 0; i < N; i++) {
            state_for_k2[i] = _start_state[i] + _half_step_size*_k1[i];
        }
        return _error;
    }
 
    template <uint32 N>
    int16 RK4Integrator<N>::calculateK2(const std::array<floating_point, N> &state_in_k2, 
                                        std::array<floating_point, N> &state_for_k3) {
        _error = Integrator<N>::_rate_calculator.calculateRates(_start_time + _half_step_size, state_in_k2, _k2);

        for(uint32 i = 0; i < N; i++) {
            state_for_k3[i] = _start_state[i] + _half_step_size*_k2[i];
        }
        return _error;
    }
 
    template <uint32 N>
    int16 RK4Integrator<N>::calculateK3(const std::array<floating_point, N> &state_in_k3, 
                                        std::array<floating_point, N> &state_for_k4) {
        _error = Integrator<N>::_rate_calculator.calculateRates(_start_time + _half_step_size, state_in_k3, _k3);

        for(uint32 i = 0; i < N; i++) {
            state_for_k4[i] = _start_state[i] + _full_step_size*_k3[i];
        }
        return _error;
    }
 
    template <uint32 N>
    int16 RK4Integrator<N>::calculateK4(const std::array<floating_point, N> &state_in_k4) {
        _error = Integrator<N>::_rate_calculator.calculateRates(_start_time + _full_step_size, state_in_k4, _k4);
        return _error;
    }
 
    template <uint32 N>
    void RK4Integrator<N>::getValueEndStep(std::array<floating_point, N> &end_state) {
        for(uint32 i = 0; i < N; i++) {
            end_state[i] = _start_state[i] + _step_average*(_k1[i] + 2.0*_k2[i] + 2.0*_k3[i] + _k4[i]);
        }
    }
 
    template <uint32 N>
    int16 RK4Integrator<N>::step(floating_point start_time, floating_point end_time, 
                                const std::array<floating_point, N> &start_state, 
                                std::array<floating_point, N> &out_state) {
        configureForStep(start_time, end_time, start_state);
        _error = calculateK1(out_state);
        _saved_warning = NO_ERROR;
        if (_error > 0) {return _error;} else {_saved_warning = _error;}
        _error = calculateK2(out_state, out_state);
        if (_error > 0) {return _error;} else {_saved_warning = _error;}
        _error = calculateK3(out_state, out_state);
        if (_error > 0) {return _error;} else {_saved_warning = _error;}
        _error = calculateK4(out_state);
        if (_error > 0) {return _error;} else {_saved_warning = _error;}
        getValueEndStep(out_state);
 
        return _saved_warning;
    }
 
}
 
#endif