EntityLibrary/src/Entities/Movement.cpp

#include "GeographicLib/Constants.hpp"
#include "SimCore/Position.hpp"
#include "SimCore/SimCore.hpp"
#include "SimCore/UtilFunctions.hpp"
#include <Entities/Movement.hpp>
#include <Eigen/Geometry>

using namespace Eigen;

namespace Entities 
{

    Movement::Movement()
    {
        course_ = 0;
        speed_= 0;
        pitch_ = 0;

        ownOrientation_.setRoll(0);
        ownOrientation_.setHeading(0);
        ownOrientation_.setPitch(0);

        ownPosition_->setGeodesicPos(0,0,0);
    }


    Movement::Movement(std::shared_ptr<SimCore::Position> pos):ownPosition_(pos)
    {
        course_ = 0;
        speed_= 0;
        pitch_ = 0;

        ownOrientation_.setRoll(0);
        ownOrientation_.setHeading(0);
        ownOrientation_.setPitch(0);


    }
    
    Movement::Movement(std::shared_ptr<SimCore::Position> pos, double course):ownPosition_(pos),course_(course)
    {
        speed_ = 0;
        pitch_ = 0;
        ownOrientation_.setRoll(0);
        ownOrientation_.setPitch(0);


    }
    Movement::Movement(std::shared_ptr<SimCore::Position> pos, double course, double speed):ownPosition_(pos),course_(course),speed_(speed)
    {
        pitch_ = 0;
        ownOrientation_.setRoll(0);
        ownOrientation_.setPitch(0);


    }
    Movement::Movement(std::shared_ptr<SimCore::Position> pos, double course, double speed, double pitch):ownPosition_(pos),course_(course),speed_(speed),pitch_(pitch)
    {
        ownOrientation_.setRoll(0);
    }


    void  Movement::updatePosition(double dt)
    {

        if (ownPosition_->isValid()) {
        
            calKinematicVector();
            

            // Create ECEF coordinate system based on reference point
            double lat = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LATITUDE));  // in radians
            double lon = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LONGITUDE));  // in radians
            double alt = ownPosition_->getGeodesicPos()(SimCore::HEIGHT);  // in meters

            Matrix3d RotationMatrix = getRotationMatrix(lat, lon);        
            
            Vector3d ecefVelocityVector = RotationMatrix * kinematicVec_;


            double X,Y,Z;    
            X = ownPosition_->getGeocentricPos()(SimCore::X) +  ecefVelocityVector.x() * (dt / 1000.0) + 0.5 * accelerationVec_.x() * pow((dt / 1000.0), 2);
            Y = ownPosition_->getGeocentricPos()(SimCore::Y) +  ecefVelocityVector.y() * (dt / 1000.0) + 0.5 * accelerationVec_.y() * pow((dt / 1000.0), 2);
            Z = ownPosition_->getGeocentricPos()(SimCore::Z) +  ecefVelocityVector.z() * (dt / 1000.0) + 0.5 * accelerationVec_.z() * pow((dt / 1000.0), 2);

            std::lock_guard<std::mutex> lock(mx);

            kinematicVec_.x() += accelerationVec_.x() * dt / 1000.0;
            kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0;
            kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;

            ownPosition_->setGeocentricPos(X, Y, Z);
            c.notify_one();

        }


    }


    void Movement::updatePositionSimple(double dt)
    {
        double lat = ownPosition_->getGeodesicPos()(SimCore::LATITUDE);  // in degree
        double lon = ownPosition_->getGeodesicPos()(SimCore::LONGITUDE);  // in degree
        double alt = ownPosition_->getGeodesicPos()(SimCore::HEIGHT);  // in meters

        double vx = this->speed_ * cos(this->getPitch());
        double vy = this->speed_ * sin(this->getPitch());


        double movedDistance = vx * dt / 1000;
        double climedHeight = vy * dt / 1000;

        GeographicLib::Geodesic geod(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());

        double lat2,lon2;

        geod.Direct(lat,lon,this->course_,movedDistance,lat2,lon2);

        
        ownPosition_->setGeodesicPos(lat2,lon2,climedHeight);
        

    }


    Eigen::Vector3d  Movement::getKinematicVector()
    {

        return kinematicVec_;

    }

    Eigen::Matrix3d  Movement::getRotationMatrix(double lat, double lon)
    {
        Matrix3d RotationMatrix;
        RotationMatrix <<  -sin(lon),  -sin(lat)*cos(lon),  cos(lat)*cos(lon),
                            cos(lon),  -sin(lat)*sin(lon),  cos(lat)*sin(lon),
                                0 ,              cos(lat),               sin(lat);
        return RotationMatrix;
    }


    void Movement::calKinematicVector()
    {

        Eigen::Vector3d UVW_Coord;

        // Convert heading and climbing angle to radians
        double heading_rad = SimCore::UtilFunctions::DegToRad( this->getCourse());
        double climbing_angle_rad = SimCore::UtilFunctions::DegToRad(this->getPitch());
        
        // Calculate NED velocity vector components
        double Vn = speed_ * cos(heading_rad) * cos (climbing_angle_rad) ;
        double Ve = speed_ * sin(heading_rad) * cos (climbing_angle_rad);
        double Vd = speed_ * sin(climbing_angle_rad);

        // Create NED velocity vector using Eigen vector
        Vector3d Vn2(Vn, Ve, -Vd);

        // Create transformation matrix from NED to ENU frame using Eigen matrix
        Matrix3d T;
        T << 0, 1, 0,
             1, 0, 0,
             0, 0, -1;

        // Transform NED velocity vector to ENU velocity vector using Eigen matrix multiplication
        Vector3d Venu = T * Vn2;
        kinematicVec_ = Venu;

    }


    std::shared_ptr<SimCore::Position>  Movement::getPosition()
    {
        std::lock_guard<std::mutex> lock(mx);

        return ownPosition_;
    }

    void Movement::setPosition(double lat, double lon, double height )
    {
        std::lock_guard<std::mutex> lock(mx);

        ownPosition_->setGeodesicPos(lat, lon, height);
    }

    void Movement::setPosition(SimCore::Position pos)
    {
        std::lock_guard<std::mutex> lock(mx);
        ownPosition_ = std::make_shared<SimCore::Position>(pos);
    }

    SimCore::Orientation Movement::getownOrientation()
    {
        std::lock_guard<std::mutex> lock(mx);

        return ownOrientation_;
    }

    Eigen::Vector3d Movement::getEulerAngles()
    { 
        std::lock_guard<std::mutex> lock(mx);
        return ownOrientation_.getEulerAngles(ownPosition_->getGeodesicPos()(SimCore::LATITUDE), ownPosition_->getGeodesicPos()(SimCore::LONGITUDE));
    }



    double Movement::getCourse()
    {
        std::lock_guard<std::mutex> lock(mx);
        return course_;
    }
    void Movement::setCourse(double course)
    {
        std::lock_guard<std::mutex> lock(mx);
        ownOrientation_.setHeading(course);
        course_ = course;
    }

    void Movement::setSpeed(double speed)
    {
        std::lock_guard<std::mutex> lock(mx);
        speed_ = speed;
    }

    void Movement::setPitch(double pitch)
    {
        std::lock_guard<std::mutex> lock(mx);
        ownOrientation_.setPitch(pitch);
        pitch_ = pitch;
    }

      double Movement::getPitch() 
      {
        std::lock_guard<std::mutex> lock(mx);
        return pitch_;
      }



}