Frame.h

Go to the documentation of this file.

/******************************************************************************
* Copyright (c) ATTX INC 2025. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, INC. 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, INC.
******************************************************************************/
/*
Frame header file
 
Author: Alex Reynolds
*/
#ifndef FRAMES_FRAME_H
#define FRAMES_FRAME_H
 
#include <string>
#include <vector>
 
#include "core/clockwerkerrors.h"
#include "core/vectormath.hpp"
#include "architecture/GraphTreeObject.h"
#include "architecture/DataIO.hpp"
#include "Joint.h"
#include "dynamics/DCM.h"
#include "dynamics/Quaternion.h"
#include "dynamics/MRP.h"
#include "dynamics/Euler321.h"
#include "dynamics/attitudemath.h"
#include "core/mathmacros.h"
 
#define NUM_INTEGRATED_STATES 13
 
namespace warptwin {
    constexpr uint32 FRAME_MAXIMUM_NUM_CHILDREN = 100;

    class Body;
    class Node;

    class Frame : public clockwerk::GraphTreeObject {
    public:
        Frame(const std::string &name, Frame* par=nullptr, bool free = false); 
 
        virtual ~Frame() {}

        std::vector<Frame*> frameChildren();

        Frame* getFrameByAddress(const std::string &address);

        Joint& tJoint() {return _t_joint;} 

        Joint& rJoint() {return _r_joint;} 

        int parent(Frame* new_parent);
        int parent(Frame& new_parent) {return parent(&new_parent);}
        Frame* parent() {return _parent;}

        clockwerk::DataIO<clockwerk::CartesianVector<3>> pos_f_p__p = 
            clockwerk::DataIO<clockwerk::CartesianVector<3>>(nullptr, "pos_f_p__p", clockwerk::CartesianVector<3>());
        clockwerk::DataIO<clockwerk::CartesianVector<3>> vel_f_p__p = 
            clockwerk::DataIO<clockwerk::CartesianVector<3>>(nullptr, "vel_f_p__p", clockwerk::CartesianVector<3>());
        clockwerk::DataIO<clockwerk::Quaternion> quat_f_p = 
            clockwerk::DataIO<clockwerk::Quaternion>(nullptr, "quat_f_p", clockwerk::Quaternion());
        clockwerk::DataIO<clockwerk::CartesianVector<3>> ang_vel_f_p__f = 
            clockwerk::DataIO<clockwerk::CartesianVector<3>>(nullptr, "ang_vel_f_p__f", clockwerk::CartesianVector<3>());
 
        /* Nice handle to the frame itself for mapping*/
        clockwerk::DataIO<Frame*> self_id = clockwerk::DataIO<Frame*>(nullptr, "self_id", this);

        void setParentRelAcceleration(clockwerk::CartesianVector<3> accel) {_acc_f_p__p = _t_joint.freedom()*accel;}

        void setParentRelAngularAcceleration(clockwerk::CartesianVector<3> alpha) {_alpha_f_p__f = _r_joint.freedom()*alpha;}

        clockwerk::CartesianVector<3> parentRelAcceleration() {return _t_joint.freedom()*_acc_f_p__p;}

        clockwerk::CartesianVector<3> parentRelAngularAcceleration() {return _r_joint.freedom()*_alpha_f_p__f;}

        void rootRelPosition(clockwerk::CartesianVector<3> &pos_o_root__root) const;
        clockwerk::CartesianVector<3> rootRelPosition() const;

        void rootRelVelocity(clockwerk::CartesianVector<3> &vel_o_root__root) const;
        clockwerk::CartesianVector<3> rootRelVelocity() const;

        void rootRelAcceleration(clockwerk::CartesianVector<3> &acc_o_root__root) const;
        clockwerk::CartesianVector<3> rootRelAcceleration() const;

        void rootRelQuaternion(clockwerk::Quaternion &quat_f_root) const;
        clockwerk::Quaternion rootRelQuaternion() const;

        void rootRelDCM(clockwerk::DCM &dcm_f_root) const;
        clockwerk::DCM rootRelDCM() const;

        void rootRelAngularVelocity(clockwerk::CartesianVector<3> &w_f_root__f) const;
        clockwerk::CartesianVector<3> rootRelAngularVelocity() const;

        void rootRelAngularAcceleration(clockwerk::CartesianVector<3> &alpha_f_root__f) const;
        clockwerk::CartesianVector<3> rootRelAngularAcceleration() const;

        int calcFrameTreeExtForcesMoments();

        int calcFrameTreeExtAcceleration();

        void integrator(void* integ_ptr) {_integrator_ptr = integ_ptr;}
        void* integrator() {return _integrator_ptr;}

        void setStateFromStateVector(const std::array<floating_point, NUM_INTEGRATED_STATES> &state_f_p__f);

        void getStateAsStateVector(std::array<floating_point, NUM_INTEGRATED_STATES> &state_f_p__f);

        void getStateVectorDot(std::array<floating_point, NUM_INTEGRATED_STATES> &state_dot);

        int setRootRelPosition(const clockwerk::CartesianVector<3> &pos_f_root__root);

        int setRootRelVelocity(const clockwerk::CartesianVector<3> &vel_f_root__root);

        int setRootRelAttitude(const clockwerk::Quaternion &quat_f_root);

        int setRootRelAngularVelocity(const clockwerk::CartesianVector<3> &w_f_root__f);

        Frame* getFrameRootPointer();

        void dump();
    protected:
        clockwerk::CartesianVector<3> _acc_f_p__p;
        clockwerk::CartesianVector<3> _alpha_f_p__f;

        Joint _t_joint;  
        Joint _r_joint;  

        void* _integrator_ptr = nullptr;

        Frame* _parent;
 
    private:
        // TODO: Use dynamic storage for frames. This is a temporary solution 
        // to get warptwin building, and thus does not get a cool defined constant.
        GraphTreeObject* _frame_storage[FRAME_MAXIMUM_NUM_CHILDREN];
    };
 
}
 
#endif