frameutils.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.
******************************************************************************/
/*
Frame Utils header file
-----------------------
The frame utils class is a class of utilities designed to 
work with frame objects.
 
Author: Alex Reynolds
*/
 
#ifndef FRAME_UTILS_HPP
#define FRAME_UTILS_HPP
 
#include "core/macros.h"
#include "core/clockwerkerrors.h"
#include "core/CartesianVector.hpp"
#include "six_dof_dynamics/Frame.hpp"
 
namespace clockwerk {
 
    /// @brief   A function to get the DCM represnting the relationship [f1/f2]
    /// @param   f1  The first frame for our DCM relationship
    /// @param   f2  The second frame for our DCM relationship
    /// @return  Error code corresponding to success/failure
    template <typename T>
    int getFrameRelativeDCM(Frame<T> &f1, Frame<T> &f2, DCM<T> &dcm_f1_f2) {
        /// First, verify that frames f1 and f2 are part of the same tree.
        /// Return an error if not.
        if(f1.getFrameRootPointer() != f2.getFrameRootPointer()) {
            return ERROR_DIFFERENT_TREE;
        }
 
        /// We want to construct the DCM [f1/f2]=[f1/root]*[root/f2]
        DCM<T> dcm_f1_root, dcm_f2_root, dcm_root_f2;
        f1.rootRelDCM(dcm_f1_root);
        f2.rootRelDCM(dcm_f2_root);
        dcm_f2_root.inverse(dcm_root_f2);
        multiply(dcm_f1_root, dcm_root_f2, dcm_f1_f2);
 
        return NO_ERROR;
    }
 
    /// @brief   A function to rotate vector vec__f1 from frame f1 into frame f2
    /// @param   vec__f1     A vector represented in frame f1
    /// @param   f1          The frame in which vec__f1 is represented
    /// @param   f2          The frame we want to rotate vec__f1 into
    /// @param   vec__f2     Implicit return of our rotated vector
    /// @return  Error code corresponding to success/failure
    template <typename T>
    int frameRotate(const CartesianVector<T, 3> &vec__f1, Frame<T> &f1,
                    Frame<T> &f2, CartesianVector<T, 3> &vec__f2) {
        /// First, verify that frames f1 and f2 are part of the same tree.
        /// Return an error if not.
        if(f1.getFrameRootPointer() != f2.getFrameRootPointer()) {
            return ERROR_DIFFERENT_TREE;
        }
 
        /// Our general process is to get our attitudes of each frame relative
        /// to root, construct a DCM representing the relative attitude, and rotate
        /// our vector accordingly
        /// We want to construct the DCM [f2/f1]=[f2/root]*[root/f1]
        DCM<T> dcm_f2_f1;
        getFrameRelativeDCM(f2, f1, dcm_f2_f1);
 
        /// Now rotate our vector from our DCM
        multiply(dcm_f2_f1, vec__f1, vec__f2);
 
        return NO_ERROR;
    }
 
    /// @brief   A function to transform a position vector relative to the origin of
    ///          f1 to a vector relative to the origin of f2
    /// @param   vec_f1__f1  A vector relative to the origin of f1
    /// @param   f1          The frame in which vec_f1__f1 is represented
    /// @param   f2          The frame we want to rotate vec_f2__f1 into
    /// @param   vec_f1__f2     Implicit return of our transformed vector
    /// @return  Error code corresponding to success/failure
    template <typename T>
    int framePositionTransform(const CartesianVector<T, 3> &vec_f1__f1, Frame<T> &f1,
                            Frame<T> &f2, CartesianVector<T, 3> &vec_f2__f2) {
        /// First, verify that frames f1 and f2 are part of the same tree.
        /// Return an error if not.
        if(f1.getFrameRootPointer() != f2.getFrameRootPointer()) {
            return ERROR_DIFFERENT_TREE;
        }
 
        // First get current position vector represented in root
        CartesianVector<T, 3> pos_tmp = f1.rootRelPosition() + f1.rootRelDCM().inverse()*vec_f1__f1;
 
        // Now get our position relative to f2
        vec_f2__f2 = f2.rootRelDCM()*(pos_tmp - f2.rootRelPosition());
 
        return NO_ERROR;
    }
 
    /// @brief   A function to transform a velocity vector represented in frame
    ///          f1 to a vector represented in f2
    /// @param   vel_f1__f1  Velocity represented in frame f1
    /// @param   pos_f1__f1  The position in f1 at which velocity is represented
    /// @param   f1          The frame in which vel_f1__f1 is represented
    /// @param   f2          The frame we want to represent velocity in
    /// @param   vel_f1__f2  Implicit return of velocity represented in f2
    /// @return  Error code corresponding to success/failure
    template <typename T>
    int frameVelocityTransform(const CartesianVector<T, 3> &vel_f1__f1, const CartesianVector<T, 3> &pos_f1__f1,
                               Frame<T> &f1, Frame<T> &f2, CartesianVector<T, 3> &vel_f2__f2) {
        /// First, verify that frames f1 and f2 are part of the same tree.
        /// Return an error if not.
        if(f1.getFrameRootPointer() != f2.getFrameRootPointer()) {
            return ERROR_DIFFERENT_TREE;
        }
 
        // First get current velocity vector represented in root. That comes from the velocity of the 
        // frame, velocity in the frame, and angular velocity of the frame
        DCM<T> dcm_root_f1 = f1.rootRelDCM().inverse();
        CartesianVector<T, 3> vel_tmp = f1.rootRelVelocity() + dcm_root_f1*vel_f1__f1;
        vel_tmp = vel_tmp + dcm_root_f1*cross(f1.rootRelAngularVelocity(), pos_f1__f1);
 
        // Now get position relative to frame f2
        CartesianVector<T, 3> pos_f2__f2;
        framePositionTransform(pos_f1__f1, f1, f2, pos_f2__f2);
 
        // Now perform the inverse operation with frame f2 to get velocity measured in frame f2
        DCM<T> dcm_root_f2 = f2.rootRelDCM().inverse();
        vel_tmp = vel_tmp - dcm_root_f2*cross(f2.rootRelAngularVelocity(), pos_f2__f2);
        vel_tmp = vel_tmp - f2.rootRelVelocity();
        vel_f2__f2 = f2.rootRelDCM()*vel_tmp;
 
        return NO_ERROR;
    }
 
}
 
#endif