ADD: added use of round to position calculation

include/Entities/Movement.hpp

@@ -145,6 +145,7 @@ namespace Entities
 
             // Eigen::Matrix3d getRotationMatrix(double lat, double lon);
 
+
             /**
             * @brief transforms from local ENU (East North Up) Vector to ECEF (Earth Centred Earth Fixed)  
             * @param Eigen::Vector3d ENU 
@@ -197,7 +198,7 @@ namespace Entities
             double speed_;
             double climbAngle_;
 
-
+            const double precision = 0.01;
 
             mutable std::mutex mx;
             std::condition_variable c;

src/Entities/Movement.cpp

@@ -1,4 +1,5 @@
 #include "GeographicLib/Constants.hpp"
+#include "GeographicLib/LocalCartesian.hpp"
 #include "SimCore/Position.hpp"
 #include "SimCore/SimCore.hpp"
 #include "SimCore/UtilFunctions.hpp"
@@ -6,6 +7,8 @@
 #include <Eigen/Geometry>
 #include <memory>
 #include <tuple>
+#include <GeographicLib/Geocentric.hpp>
+
 
 using namespace Eigen;
 
@@ -96,6 +99,8 @@ namespace Entities
     void  Movement::updatePosition(double dt)
     {
 
+        
+
         if (ownPosition_->isValid()) {
         
             auto ecefVelocityVector = this->ToECEFFromENU();
@@ -110,6 +115,9 @@ namespace Entities
             // kinematicVec_.x() += accelerationVec_.x() * dt / 1000.0;
             // kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0;
             // kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;
+            X = SimCore::UtilFunctions::round(X,precision);
+            Y = SimCore::UtilFunctions::round(Y,precision);
+            Z = SimCore::UtilFunctions::round(Z,precision);
 
             ownPosition_->setGeocentricPos(X, Y, Z);
             // c.notify_one();
@@ -140,7 +148,7 @@ namespace Entities
         geod.Direct(lat,lon,this->course_,movedDistance,lat2,lon2);
 
         
-        ownPosition_->setGeodesicPos(lat2,lon2,climedHeight);
+        ownPosition_->setGeodesicPos(lat2,lon2,alt + climedHeight);
         
 
     }
@@ -162,8 +170,13 @@ namespace Entities
              cos(lon), - sin(lon)* sin(lat), sin(lon) *cos(lat),
             0          ,    cos(lat)            , sin(lat);
 
+        auto vec = LocalRotationMatrix * ENU;
         
-        return LocalRotationMatrix * ENU;
+        return Eigen::Vector3d(
+             SimCore::UtilFunctions::round(vec[0],precision ),
+              SimCore::UtilFunctions::round(vec[1],precision ), 
+              SimCore::UtilFunctions::round(vec[2],precision )
+        );
     }
 
     Eigen::Vector3d  Movement::ENUFromCourseSpeedClbToLocalSpeedVector()
@@ -172,7 +185,6 @@ namespace Entities
             ENU[0] = this->speed_ * cos(this->climbAngle_) * sin(this->course_);
             ENU[1] = this->speed_ * cos(this->climbAngle_) * cos(this->course_);
             ENU[2] = this->speed_ * sin(this->climbAngle_);
-
             this->ENUVec_ = ENU;
             return ENU;
     }