CalcXdotGravityJ2J3.hpp

/******************************************************************************
* 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.
******************************************************************************/
 
#ifndef CALC_XDOT_GRAVITY_J2_J3_HPP
#define CALC_XDOT_GRAVITY_J2_J3_HPP
 
#include "utils/Rates.hpp"
 
#define XDOT_GRAVITY_J2_J3_STATES 6
 
namespace clockwerk {
 
    template <typename T>
    class CalcXdotGravityJ2J3 : public clockwerk::Rates<T, XDOT_GRAVITY_J2_J3_STATES> {
        public:
            /// @brief   Function to calculate rate of change in 6-element orbit position with J2, J3
            /// @param   time    The reference time
            /// @param   state   The state vector for the system as [x, y, z, xDot, yDot, zDot]
            /// @param   out_rates   Implicit return of rates based on time, state as [xDot, yDot, zDot, xDDot, yDDot, zDDot]
            int calculateRates(T time, const std::array<T, XDOT_GRAVITY_J2_J3_STATES> &state,
                            std::array<T, XDOT_GRAVITY_J2_J3_STATES> &out_rates);
 
            /// @brief The gravitational parameter for the planet. Must be set prior to calculation
            T mu;
 
            /// @brief The J2 parameter for the planet. Must be set prior to calculation.
            T J2;
 
            /// @brief The J3 parameter for the planet. Must be set prior to calculation.
            T J3;
 
            /// @brief The reference radius for the planet. Must be set prior to calculation.
            T R;
        private:
 
    };
 
    template <typename T>
    int CalcXdotGravityJ2J3<T>::calculateRates(T time, const std::array<T, XDOT_GRAVITY_J2_J3_STATES> &state,
                                            std::array<T, XDOT_GRAVITY_J2_J3_STATES> &out_rates) {
        // Precalculate some parameters
        T muRR = mu*R*R;
        T pc_j2 = muRR*J2;
        T pc_j3 = muRR*J3*R;
        T r = std::sqrt(state[0]*state[0] + state[1]*state[1] + state[2]*state[2]);
        T r3 = r*r*r;
        T r5 = r3*r*r;
        T r7 = r5*r*r;
        T r9 = r7*r*r;
        T z2 = state[2]*state[2];
 
        // Now precalc divided parameters
        T mu_over_r3, pc_15_2_r7, pc_3_2_r5, pc_35_2_r9;
        int err = clockwerk::safeDivide(-mu, r3, mu_over_r3); if(err) {return err;}
        err = clockwerk::safeDivide(7.5*state[2], r7, pc_15_2_r7); if(err) {return err;}
        err = clockwerk::safeDivide(1.5, r5, pc_3_2_r5); if(err) {return err;}
        err = clockwerk::safeDivide(17.5*z2*state[2], r9, pc_35_2_r9); if(err) {return err;}
 
        // Finally, actually calculate gravity
        out_rates[0] = state[3];
        out_rates[1] = state[4];
        out_rates[2] = state[5];
        //              mu; J2; J3
        out_rates[3] =  mu_over_r3*state[0] +
                        pc_j2*(pc_15_2_r7*state[0]*state[2] - pc_3_2_r5*state[0]) +
                        pc_j3*(-pc_15_2_r7*state[0] + pc_35_2_r9*state[0]);
        out_rates[4] =  mu_over_r3*state[1] +
                        pc_j2*(pc_15_2_r7*state[1]*state[2] - pc_3_2_r5*state[1]) +
                        pc_j3*(-pc_15_2_r7*state[1] + pc_35_2_r9*state[1]);
        out_rates[5] =  mu_over_r3*state[2] +
                        pc_j2*(pc_15_2_r7*z2 - 3.0*pc_3_2_r5*state[2]) +
                        pc_j3*(-2.0*pc_15_2_r7*state[2] + pc_35_2_r9*state[2] + pc_3_2_r5);
 
        return NO_ERROR;
    }
 
}
 
#endif