WaypointRefGen.h

#pragma once
 
#include <Eigen/Eigen>
#include <vector>
#include <string>
#include <fhsim/simobject/SimObject.h>
#ifdef FH_VISUALIZATION
#pragma warning(push)
#pragma warning( disable : 4251 )
    #include <Ogre.h>
#pragma warning(pop)
#endif
#include "Waypoint.h"
using namespace Eigen;
 
 
namespace Ship{
    class WaypointRefGen : public SimObject
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        WaypointRefGen(const std::string& simObjectName, ISimObjectCreator* const creator); 
        ~WaypointRefGen();
        const double* DesiredQuaternion(const double T, const double* const X);
        const double* DesiredHeading(const double T, const double* const X);
        const double* DesiredHeadingRate(const double T, const double* const X);
        const double* CrossTrackError(const double T, const double* const X);
        const double* DesiredPosition(const double T,const double* const X);
        const double* DesiredSpeed(const double T,const double* const X);
        void OdeFcn(const double T, const double *const X,double *const ) const{}
        void FinalSetup(const double T,const double* const X,  ISimObjectCreator* const creator);
        void Compute(const double T,const double* const X) const;
        void AcceptedStep(double T, const double* X) override; 
 
    #ifdef FH_VISUALIZATION
        void    RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator); 
        void    RenderUpdate(const double T, const double* const X); 
    #endif
 
 
    private:
        ISignalPort*  m_adStatePositionPort; 
        ISignalPort*  m_adStateVelocityPort; 
        ICommonComputation *m_Compute;
 
        mutable std::vector<Waypoint> m_aWPList;    
 
        mutable Eigen::Vector3d m_adStatePosition;          
        Eigen::Vector3d m_adStateVelocity;          
        Eigen::Vector3d m_adStateVelocity_Desired;
 
        std::string sideslipCompensation;       
 
        double initialPosition[3];          
        double* pK;                         
        mutable double desiredHeading;              
        mutable double desiredHeadingRate;
        mutable double crosstrackError;             
        double m_dLookaheadDistance;        
        double m_lookahead;         
        double m_dSideSlipAngle;            
        double alphaK;                      
        double m_adStateTheta_Desired[4];   
        mutable double desiredPosition[3];          
        mutable double m_desiredSpeed;
        int numberOfPoints;                 
 
        int  iCurrentWaypointNumber;        
 
        std::vector<double> waypoint_n;                 
        std::vector<double> waypoint_e;                 
        std::vector<double> waypoint_d;                 
        std::vector<double> waypoint_r;                 
        std::vector<double> waypoint_leg_speed; 
        double m_sectionType;
 
        std::vector<Eigen::Vector3d> legstart;
        std::vector<Eigen::Vector3d> legend;
        std::vector<Eigen::Vector3d> circle_center;
        std::vector<double> circle_radius;
 
    #ifdef FH_VISUALIZATION
        Ogre::SceneNode**   m_wpNodes;
        Ogre::SceneNode**   m_legNodes;
 
        Ogre::SceneNode**   m_turnNodes;
        Ogre::SceneNode**   m_circNodes;
 
        Ogre::SceneNode*    m_LOSNode;
        Ogre::SceneNode*    m_TargetPointNode;
 
        Ogre::ManualObject* sigth;
    #endif
    };
}