Quaternion.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.
******************************************************************************/
/*
Quaternion header file
 
Author: Alex Reynolds
*/
#ifndef QUATERNION_HPP
#define QUATERNION_HPP
 
#include <cmath>
 
#include "core/Matrix.hpp"
#include "core/CartesianVector.hpp"
#include "core/safemath.hpp"
#include "core/matrixmath.hpp"
 
#include "DCM.hpp"
 
namespace clockwerk {
 
    /// Forward declaration of MRP for conversion
    template<typename T>
    class MRP;
 
    /// @brief  Quaternion class for attitude representation
    /// 
    /// This file defines a simple Quaternion attitude representation for cartesian
    /// coordinate systems. It is largely the same as the base vector,
    /// with the following exceptions:
    /// - Set to be 4 elements
    /// - Functions defined to convert to other attitude representations
    /// - Function defined to calculate rate of change as a function of omega
    /// 
    /// 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 Quaternion : public CartesianVector<T, 4> {
    public:
        /// @brief   Default constructor generates Quaternion sequence as zero rotation
        ///          -- 1 0 0 0 
        Quaternion<T>();
 
        /// Constructor for Matrix class with initialization. 
        /// Initializes matrix to values passed in via array
        Quaternion<T>(const T(&initial)[4]);
 
        /// Copy constructor for Quaternion class. Copies data from Quaternion
        /// object to the current instance
        Quaternion<T>(const Quaternion<T> &initial);
 
        /// Constructor for Matrix class with initialization. 
        /// Initializes matrix to values passed in via array
        Quaternion<T>(const std::array<T, 4> &initial);
 
        /// Destructor -- doesn't do anything because we don't dynamically
        /// allocate
        ~Quaternion<T>() {}
 
        /// @brief    Equals operator overload for quaternion 
        Quaternion<T>& operator=(const Quaternion<T>& other);
 
        /// ---------------------------------------------------------------------------
        /// 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   quatdot_f1_f2       Rate of change in the current Quaternion
        void rate(const CartesianVector<T, 3> &omega_f1_f2__f1, Matrix<T, 4, 1> &quatdot_f1_f2) const;
 
        /// ---------------------------------------------------------------------------
        /// 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
        void toDCM(DCM<T> &dcm_f1_f2) const;
        DCM<T> toDCM() const;
 
        /// @brief   Function to convert current attitude to MRP
        /// @param   PBR return of MRP in PBR case
        /// @return  Integer error code corresponding to errors in clockwerkerrors.h
        void toMRP(MRP<T> &mrp_f1_f2) const;
        MRP<T> toMRP() const;
 
        /// @brief  Calculate the rotation angle represented by the quaternion
        /// @param val Implicit return of rotation angle
        /// @return Error code corresponding to success/failure
        int rotationAngle(double &val) const;
    };
 
    template <typename T>
    Quaternion<T>::Quaternion() :
        CartesianVector<T, 4>({1, 0, 0, 0}) {}
 
    template <typename T>
    Quaternion<T>::Quaternion(const T(&initial)[4]) :
        CartesianVector<T, 4>(initial) {}
 
    template <typename T>
    Quaternion<T>::Quaternion(const Quaternion<T> &initial) :
        CartesianVector<T, 4>(initial) {}
 
    template <typename T>
    Quaternion<T>::Quaternion(const std::array<T, 4> &initial) :
        CartesianVector<T, 4>(initial) {}
 
    template <typename T>
    Quaternion<T>& Quaternion<T>::operator=(const Quaternion<T>& other)
    {
        /// Guard self assignment
        if (this == &other)
            return *this;
 
        /// Copy values from current matrix to return
        unsigned int i;
        for(i = 0; i < 4; i++) {
            this->values[i][0] = other.values[i][0];
        }
        return *this;
    }
 
    template <typename T>
    void Quaternion<T>::rate(const CartesianVector<T, 3> &omega_f1_f2__f1, Matrix<T, 4, 1> &quatdot_f1_f2) const {
        quatdot_f1_f2.values[0][0] = -omega_f1_f2__f1.values[0][0]*CartesianVector<T, 4>::values[1][0] - omega_f1_f2__f1.values[1][0]*CartesianVector<T, 4>::values[2][0] - omega_f1_f2__f1.values[2][0]*CartesianVector<T, 4>::values[3][0];
        quatdot_f1_f2.values[1][0] = omega_f1_f2__f1.values[0][0]*CartesianVector<T, 4>::values[0][0] + omega_f1_f2__f1.values[2][0]*CartesianVector<T, 4>::values[2][0] - omega_f1_f2__f1.values[1][0]*CartesianVector<T, 4>::values[3][0];
        quatdot_f1_f2.values[2][0] = omega_f1_f2__f1.values[1][0]*CartesianVector<T, 4>::values[0][0] - omega_f1_f2__f1.values[2][0]*CartesianVector<T, 4>::values[1][0] + omega_f1_f2__f1.values[0][0]*CartesianVector<T, 4>::values[3][0];
        quatdot_f1_f2.values[3][0] = omega_f1_f2__f1.values[2][0]*CartesianVector<T, 4>::values[0][0] + omega_f1_f2__f1.values[1][0]*CartesianVector<T, 4>::values[1][0] - omega_f1_f2__f1.values[0][0]*CartesianVector<T, 4>::values[2][0];
        multiply(static_cast<T>(0.5), quatdot_f1_f2, quatdot_f1_f2);
    }
 
    template <typename T>
    void Quaternion<T>::toDCM(DCM<T> &dcm_f1_f2) const {
        /// Calculate parameters to simplify calculation
        T q02, q12, q22, q32, q0q1, q0q2, q0q3, q1q2, q1q3, q2q3;
        q02 = CartesianVector<T, 4>::values[0][0]*CartesianVector<T, 4>::values[0][0];
        q12 = CartesianVector<T, 4>::values[1][0]*CartesianVector<T, 4>::values[1][0];
        q22 = CartesianVector<T, 4>::values[2][0]*CartesianVector<T, 4>::values[2][0];
        q32 = CartesianVector<T, 4>::values[3][0]*CartesianVector<T, 4>::values[3][0];
        q0q1 = CartesianVector<T, 4>::values[0][0]*CartesianVector<T, 4>::values[1][0];
        q0q2 = CartesianVector<T, 4>::values[0][0]*CartesianVector<T, 4>::values[2][0];
        q0q3 = CartesianVector<T, 4>::values[0][0]*CartesianVector<T, 4>::values[3][0];
        q1q2 = CartesianVector<T, 4>::values[1][0]*CartesianVector<T, 4>::values[2][0];
        q1q3 = CartesianVector<T, 4>::values[1][0]*CartesianVector<T, 4>::values[3][0];
        q2q3 = CartesianVector<T, 4>::values[2][0]*CartesianVector<T, 4>::values[3][0];
 
        /// Now calculate DCM
        dcm_f1_f2.values[0][0] = q02 + q12 - q22 - q32;
        dcm_f1_f2.values[0][1] = 2*(q1q2 + q0q3);
        dcm_f1_f2.values[0][2] = 2*(q1q3 - q0q2);
        dcm_f1_f2.values[1][0] = 2*(q1q2 - q0q3);
        dcm_f1_f2.values[1][1] = q02 - q12 + q22 - q32;
        dcm_f1_f2.values[1][2] = 2*(q2q3 + q0q1);
        dcm_f1_f2.values[2][0] = 2*(q1q3 + q0q2);
        dcm_f1_f2.values[2][1] = 2*(q2q3 - q0q1);
        dcm_f1_f2.values[2][2] = q02 - q12 - q22 + q32;
    }
    template <typename T>
    DCM<T> Quaternion<T>::toDCM() const {
        DCM<T> tmp;
        toDCM(tmp);
        return tmp;
    }
 
    template <typename T>
    void Quaternion<T>::toMRP(MRP<T> &mrp_f1_f2) const {
        /// Calculate intermediate parameter
        T divisor = 1 + CartesianVector<T, 4>::values[0][0];
 
        mrp_f1_f2.values[0][0] = CartesianVector<T, 4>::values[1][0]/divisor;
        mrp_f1_f2.values[1][0] = CartesianVector<T, 4>::values[2][0]/divisor;
        mrp_f1_f2.values[2][0] = CartesianVector<T, 4>::values[3][0]/divisor;
    }
    template <typename T>
    MRP<T> Quaternion<T>::toMRP() const {
        MRP<T> tmp;
        toMRP(tmp);
        return tmp;
    }
 
    template <typename T>
    int Quaternion<T>::rotationAngle(double &val) const {
        if(CartesianVector<T, 4>::values[0][0] > 1.0 || CartesianVector<T, 4>::values[0][0] < -1.0) {
            return ERROR_INVALID_RANGE;
        }
 
        val = 2.0*std::acos(CartesianVector<T, 4>::values[0][0]);
        return NO_ERROR;
    }
 
}
 
#endif