ADD: added Movement calculation and a Tracklist class

src/Entities/Movement.cpp

@@ -0,0 +1,215 @@
+#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(SimCore::Position pos):ownPosition_(pos)
+    {
+        course_ = 0;
+        speed_= 0;
+        pitch_ = 0;
+
+        ownOrientation_.setRoll(0);
+        ownOrientation_.setHeading(0);
+        ownOrientation_.setPitch(0);
+
+
+    }
+    
+    Movement::Movement(SimCore::Position pos, double course):ownPosition_(pos),course_(course)
+    {
+        speed_ = 0;
+        pitch_ = 0;
+        ownOrientation_.setRoll(0);
+        ownOrientation_.setPitch(0);
+
+
+    }
+    Movement::Movement(SimCore::Position pos, double course, double speed):ownPosition_(pos),course_(course),speed_(speed)
+    {
+        pitch_ = 0;
+        ownOrientation_.setRoll(0);
+        ownOrientation_.setPitch(0);
+
+
+    }
+    Movement::Movement(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();
+
+        }
+
+
+    }
+
+    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;
+
+    }
+
+
+    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_ = 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_;
+      }
+
+
+
+}