MRP.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.
******************************************************************************/
/*
MRP header file
 
Author: Alex Reynolds
*/
#ifndef MRP_HPP
#define MRP_HPP
 
#include <cmath>
 
#include "core/Matrix.hpp"
#include "core/CartesianVector.hpp"
#include "core/safemath.hpp"
#include "core/matrixmath.hpp"
 
#include "DCM.hpp"
#include "Quaternion.hpp"
 
namespace clockwerk {
 
    /// @brief  Modified Rodrigues Parameter class definiton
    /// 
    /// This file defines a simple Modified Rodrigues Parameter attitude representation for cartesian
    /// coordinate systems. It is largely the same as the base vector,
    /// with the following exceptions:
    /// - Set to be 3 elements
    /// - Functions defined to convert to other attitude representations
    /// - Function defined to calculate rate of change as a function of omega
    /// - Function defined to convert to shadow set of MRPs
    /// 
    /// Naming conventions
    /// ------------------
    /// - All naming conventions and equations are per Analytical Mechanics of
    ///   Space Systems by Schaub and Junkins
    /// - The omega vector is sometimes denoted by w and assumed frame consistent with 
    ///   the attitude representation. For example, if a DCM represents the 
    ///   relative orientation between two frames [BN] the omega vector is 
    ///   assumed to be w_(B/N)
    /// - The naming convention for all vectors is:
    ///   variablename_obj1_obj2__frameN       (note the double underscore preceding frame)
    ///   and reads back as:
    ///   "variable variablename representing the relationship between obj1 and obj2
    ///   represented in frameN"
    /// - Unless otherwise noted angles are in RADIANS
    /// 
    /// NOTE: All attitude representations, including DCMs, are assumed to represent a 
    ///       three dimensional, cartesian coordinate system because that is what they 
    ///       are used, and in many cases defined, for. 
    template<typename T>
    class MRP : public CartesianVector<T, 3> {
    public:
        /// @brief   Default constructor initializes to all zeroes
        MRP<T>();
 
        /// Constructor for MRP class with initialization. 
        /// Initializes MRP to values passed in via array
        MRP<T>(const T(&initial)[3]);
 
        /// Copy constructor for MRP class. Copies data from MRP
        /// object to the current instance
        MRP<T>(const MRP<T> &initial);
 
        /// Constructor for MRP class with initialization. 
        /// Initializes MRP to values passed in via array
        MRP<T>(const std::array<T, 3> &initial);
 
        /// Destructor -- doesn't do anything because we don't dynamically
        /// allocate
        ~MRP<T>() {}
 
        /// @brief   Function to convert current MRP to its shadow set
        void shadow();
 
        /// ---------------------------------------------------------------------------
        /// Dynamics and kinematics functions
        /// ---------------------------------------------------------------------------
        /// @brief   Function to calculate the rate of change in the current 
        ///          representation based on the omega vector 
        /// @param   omega_f1_f2__f1     Angular velocity vector for the attitude
        ///                              representation - omega of frame 1 wrt frame 2
        ///                              expressed in frame 1
        /// @param   mrpdot_f1_f2        Rate of change in the current Quaternion
        void rate(const CartesianVector<T, 3> &omega_f1_f2__f1, Matrix<T, 3, 1> &mrpdot_f1_f2);
 
        /// ---------------------------------------------------------------------------
        /// Conversions to other attitude representations
        /// ---------------------------------------------------------------------------
        /// @brief   Function to convert current attitude to DCM
        /// @param   PBR return of DCM in PBR case
        /// @return  Integer error code corresponding to errors in clockwerkerrors.h
        int toDCM(DCM<T> &dcm_f1_f2) const;
        DCM<T> toDCM() const;
 
        /// @brief   Function to convert current attitude to Quaternion
        /// @param   PBR return of Quaternion in PBR case
        /// @return  Integer error code corresponding to errors in clockwerkerrors.h
        int toQuaternion(Quaternion<T> &quat_f1_f2) const;
        Quaternion<T> toQuaternion() const;
    };
 
    template <typename T>
    MRP<T>::MRP() :
        CartesianVector<T, 3>() {}
 
    template <typename T>
    MRP<T>::MRP(const T(&initial)[3]) :
        CartesianVector<T, 3>(initial) {}
 
    template <typename T>
    MRP<T>::MRP(const MRP<T> &initial) :
        CartesianVector<T, 3>(initial) {}
 
    template <typename T>
    MRP<T>::MRP(const std::array<T, 3> &initial) :
        CartesianVector<T, 3>(initial) {}
 
    template <typename T>
    void MRP<T>::shadow() {
        /// Calculate intermediate parameter
        T divisor;
        dot(*this, *this, divisor);
 
        /// Set shadow values
        CartesianVector<T, 3>::values[0][0] = -CartesianVector<T, 3>::values[0][0]/divisor;
        CartesianVector<T, 3>::values[1][0] = -CartesianVector<T, 3>::values[1][0]/divisor;
        CartesianVector<T, 3>::values[2][0] = -CartesianVector<T, 3>::values[2][0]/divisor;
    }
 
    template <typename T>
    void MRP<T>::rate(const CartesianVector<T, 3> &omega_f1_f2__f1, Matrix<T, 3, 1> &mrpdot_f1_f2) {
        /// Pre-calculate parameters used multiple times for speed
        T s1Squared, s2Squared, s3Squared, s1s2, s1s3, s2s3;
        T sigSquared = T();
        CartesianVector<T, 3>::normSquared(sigSquared);
        s1Squared = CartesianVector<T, 3>::values[0][0]*CartesianVector<T, 3>::values[0][0];
        s2Squared = CartesianVector<T, 3>::values[1][0]*CartesianVector<T, 3>::values[1][0];
        s3Squared = CartesianVector<T, 3>::values[2][0]*CartesianVector<T, 3>::values[2][0];
        s1s2 = CartesianVector<T, 3>::values[0][0]*CartesianVector<T, 3>::values[1][0];
        s1s3 = CartesianVector<T, 3>::values[0][0]*CartesianVector<T, 3>::values[2][0];
        s2s3 = CartesianVector<T, 3>::values[1][0]*CartesianVector<T, 3>::values[2][0];
 
        /// Calculate MRP rate of change
        mrpdot_f1_f2.values[0][0] = 0.25*(1 - sigSquared + 2.0*s1Squared)*omega_f1_f2__f1.values[0][0] +
                                    0.5*((s1s2 - CartesianVector<T, 3>::values[2][0])*omega_f1_f2__f1.values[1][0] + (s1s3 + CartesianVector<T, 3>::values[1][0])*omega_f1_f2__f1.values[2][0]);
        mrpdot_f1_f2.values[1][0] = 0.25*(1 - sigSquared + 2.0*s2Squared)*omega_f1_f2__f1.values[1][0] +
                                    0.5*((s1s2 + CartesianVector<T, 3>::values[2][0])*omega_f1_f2__f1.values[0][0] + (s2s3 - CartesianVector<T, 3>::values[0][0])*omega_f1_f2__f1.values[2][0]);
        mrpdot_f1_f2.values[2][0] = 0.25*(1 - sigSquared + 2.0*s3Squared)*omega_f1_f2__f1.values[2][0] +
                                    0.5*((s1s3 - CartesianVector<T, 3>::values[1][0])*omega_f1_f2__f1.values[0][0] + (s2s3 + CartesianVector<T, 3>::values[0][0])*omega_f1_f2__f1.values[1][0]);
    }
 
    template <typename T>
    int MRP<T>::toDCM(DCM<T> &dcm_f1_f2) const {
        /// Pre-calculate parameters used multiple times for speed
        T s1Squared, s2Squared, s3Squared, s1s2, s1s3, s2s3,
            onePlusSigSquared, oneMinusSigSquared, oneMinusSigSquaredSquared,
            multiplier;
        T sigSquared = T();
        CartesianVector<T, 3>::normSquared(sigSquared);
        s1Squared = CartesianVector<T, 3>::values[0][0]*CartesianVector<T, 3>::values[0][0];
        s2Squared = CartesianVector<T, 3>::values[1][0]*CartesianVector<T, 3>::values[1][0];
        s3Squared = CartesianVector<T, 3>::values[2][0]*CartesianVector<T, 3>::values[2][0];
        s1s2 = CartesianVector<T, 3>::values[0][0]*CartesianVector<T, 3>::values[1][0];
        s1s3 = CartesianVector<T, 3>::values[0][0]*CartesianVector<T, 3>::values[2][0];
        s2s3 = CartesianVector<T, 3>::values[1][0]*CartesianVector<T, 3>::values[2][0];
        onePlusSigSquared = 1 + sigSquared;
        oneMinusSigSquared = 1 - sigSquared;
        oneMinusSigSquaredSquared = oneMinusSigSquared*oneMinusSigSquared;
 
        /// Now calculate our DCM
        dcm_f1_f2.values[0][0] = 4*(s1Squared - s2Squared - s3Squared) + oneMinusSigSquaredSquared;
        dcm_f1_f2.values[0][1] = 8*s1s2 + 4*CartesianVector<T, 3>::values[2][0]*oneMinusSigSquared;
        dcm_f1_f2.values[0][2] = 8*s1s3 - 4*CartesianVector<T, 3>::values[1][0]*oneMinusSigSquared;
        dcm_f1_f2.values[1][0] = 8*s1s2 - 4*CartesianVector<T, 3>::values[2][0]*oneMinusSigSquared;
        dcm_f1_f2.values[1][1] = 4*(s2Squared - s1Squared - s3Squared) + oneMinusSigSquaredSquared;
        dcm_f1_f2.values[1][2] = 8*s2s3 + 4*CartesianVector<T, 3>::values[0][0]*oneMinusSigSquared;
        dcm_f1_f2.values[2][0] = 8*s1s3 + 4*CartesianVector<T, 3>::values[1][0]*oneMinusSigSquared;
        dcm_f1_f2.values[2][1] = 8*s2s3 - 4*CartesianVector<T, 3>::values[0][0]*oneMinusSigSquared;
        dcm_f1_f2.values[2][2] = 4*(s3Squared - s1Squared - s2Squared) + oneMinusSigSquaredSquared;
 
        /// Multiply our DCM by our multiplication factor
        int err = safeDivide(1, onePlusSigSquared*onePlusSigSquared, multiplier);
        if(err) {return err;}
        multiply(multiplier, dcm_f1_f2, dcm_f1_f2);
 
        return NO_ERROR;
    }
    template <typename T>
    DCM<T> MRP<T>::toDCM() const {
        DCM<T> tmp;
        toDCM(tmp);
        return tmp;
    }
 
    template <typename T>
    int MRP<T>::toQuaternion(Quaternion<T> &quat_f1_f2) const {
        /// Intermediate parameters
        T divisor;
        T sigSquared = T();
        CartesianVector<T, 3>::normSquared(sigSquared);
        divisor = 1 + sigSquared;
        int err;
 
        /// Now get our beta values
        err = safeDivide(1 - sigSquared, divisor, quat_f1_f2.values[0][0]);
        if(err) {return err;}
        err = safeDivide(2*CartesianVector<T, 3>::values[0][0], divisor, quat_f1_f2.values[1][0]);
        if(err) {return err;}
        err = safeDivide(2*CartesianVector<T, 3>::values[1][0], divisor, quat_f1_f2.values[2][0]);
        if(err) {return err;}
        err = safeDivide(2*CartesianVector<T, 3>::values[2][0], divisor, quat_f1_f2.values[3][0]);
        if(err) {return err;}
 
        return NO_ERROR;
    }
    template <typename T>
    Quaternion<T> MRP<T>::toQuaternion() const {
        Quaternion<T> tmp;
        toQuaternion(tmp);
        return tmp;
    }
 
}
 
#endif