WaypointRefGenTracker.h

#ifndef WaypointRefGenTracker_h__
#define WaypointRefGenTracker_h__
 
#include <string>
#include <vector>
 
#include <Eigen/Eigen>
#include <Eigen/StdVector>
 
#include <fhsim/simobject/SimObject.h>
 
#ifdef FH_VISUALIZATION
    #include "sfh/ogre/C3DLine.h"
#endif
namespace Ship{
    class WaypointRefGenTracker: public SimObject
    {
    public:
 
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        WaypointRefGenTracker(const std::string& simObjectName, ISimObjectCreator* const creator);
 
        const double * DesiredPosition(const double T, const double*const X);
        const double * DesiredVelocity(const double T, const double *const X);
        const double * DesiredHeading(const double T, const double *const X);
        const double * DesiredYawRate(const double T, const double *const X);
 
        void OdeFcn(const double T, const double *const X, double *const XDot) const;
 
        void InitialConditionSetup(const double T, const double *const currentIC, double* const updatedIC, ISimObjectCreator* const creator);
 
        std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > m_waypoints;
        std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > straightStart;
        std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > straightEnd;
 
        std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > circleCenter;
        std::vector<double> circleRadius;
        std::vector<double> circleArcRadians;
 
        std::vector<double> cummulativeLength;
        std::vector<double> segmentLength;
        std::vector<double> straightSegmentSpeed;
    private:
        unsigned int GetSegmentNumber(const double T,const double *const X) const;
 
        Eigen::Vector3d GetCurrentTrackDirection(const double T,const double *const X) const;
        Eigen::Vector3d GetCurrentTrackNormal(const double T,const double *const X) const;
        double GetCurrentYawDesired(const double T,const double *const X) const;
        double GetCurrentYawRateDesired(const double T,const double *const X) const;
 
        Eigen::Vector3d GetCurrentTargetPoint(const double T,const double *const X) const;
 
        ISignalPort *m_currentPosition;
        ISignalPort *m_currentVelocity;
 
        double m_desiredPosition[3];
        double m_desiredHeading;
        double m_desiedVelocity;
        double m_desiredYawRate;
 
        unsigned int m_trackState;
        double m_lookaheadDistance;
 
        int circleNoFromSegmentNo(int no)const {return no/2-1;}
        int straightNoFromSegmentNo(int no)const {return (no-1)/2;}
 
    #ifdef FH_VISUALIZATION
    public:
 
        C3DLine *sightline;
        C3DLine *radline;
        C3DLine *pathline;
        C3DLine *removedlines;
 
        Ogre::SceneNode ** m_legNodes;
        Ogre::SceneNode ** m_circleNodes;
        Ogre::SceneNode *m_targetNode;
        void    RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator); 
        void    RenderUpdate(const double T, const double* const X); 
    #endif
 
 
    };
}
#endif // WaypointRefGenTracker_h__