henry/EntityLibrary
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ADD: added use of round to position calculation

Browse Source
This commit is contained in:
Henry Winkel
2024-02-15 14:23:18 +01:00
parent 2a279f59ec
commit f839ea4815
3 changed files with 19 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/Entities/Movement.cpp
View File

@@ -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);
return LocalRotationMatrix * ENU;
auto vec = 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;
}