CalcXdotPhiDotGravityJ2J3.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"
#include "dynamicsfunctions.hpp"
 
#define NUM_STATES_GRAV 6
#define XDOT_PHIDOT_GRAVITY_J2_J3_STATES 42
 
namespace clockwerk {
 
    template <typename T>
    class CalcXdotPhiDotGravityJ2J3 : public Rates<T, XDOT_PHIDOT_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; phi as 42x1]
        /// @param   out_rates   Implicit return of rates based on time, state as [xDot, yDot, zDot, xDDot, yDDot, zDDot, phiDot as 42x1]
        int calculateRates(T time, const std::array<T, XDOT_PHIDOT_GRAVITY_J2_J3_STATES> &state,
                        std::array<T, XDOT_PHIDOT_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:
        // A matrix for rate of phidot
        Matrix<T, NUM_STATES_GRAV, NUM_STATES_GRAV> _A;
        Matrix<T, NUM_STATES_GRAV, NUM_STATES_GRAV> _phi_tmp;
    };
 
    template <typename T>
    int CalcXdotPhiDotGravityJ2J3<T>::calculateRates(T time, const std::array<T, XDOT_PHIDOT_GRAVITY_J2_J3_STATES> &state,
                                            std::array<T, XDOT_PHIDOT_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 = safeDivide(-mu, r3, mu_over_r3); if(err) {return err;}
        err = safeDivide(7.5*state[2], r7, pc_15_2_r7); if(err) {return err;}
        err = safeDivide(1.5, r5, pc_3_2_r5); if(err) {return err;}
        err = safeDivide(17.5*z2*state[2], r9, pc_35_2_r9); if(err) {return err;}
 
        // 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);
 
        // Now calculate based on our Phi matrix
        _phi_tmp.setFromArray(&state[6]);               // Reconstruct our phi from input array
        calcDfDzGravityJ2J3(state, mu, J2, J3, R, _A);  // Calculate our A matrix
        _phi_tmp = _A*_phi_tmp;                         // Calculate phi dot
        _phi_tmp.getAsArray(&out_rates[6]);             // Set on our array
 
        return NO_ERROR;
    }
 
}
 
#endif