#include "GeographicLib/Constants.hpp" #include "SimCore/Position.hpp" #include "SimCore/SimCore.hpp" #include "SimCore/UtilFunctions.hpp" #include #include 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 pos):ownPosition_(pos) { course_ = 0; speed_= 0; pitch_ = 0; ownOrientation_.setRoll(0); ownOrientation_.setHeading(0); ownOrientation_.setPitch(0); } Movement::Movement(std::shared_ptr pos, double course):ownPosition_(pos),course_(course) { speed_ = 0; pitch_ = 0; ownOrientation_.setRoll(0); ownOrientation_.setPitch(0); } Movement::Movement(std::shared_ptr pos, double course, double speed):ownPosition_(pos),course_(course),speed_(speed) { pitch_ = 0; ownOrientation_.setRoll(0); ownOrientation_.setPitch(0); } Movement::Movement(std::shared_ptr 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 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 Movement::getPosition() { std::lock_guard lock(mx); return ownPosition_; } void Movement::setPosition(double lat, double lon, double height ) { std::lock_guard lock(mx); ownPosition_->setGeodesicPos(lat, lon, height); } void Movement::setPosition(SimCore::Position pos) { std::lock_guard lock(mx); ownPosition_ = std::make_shared(pos); } SimCore::Orientation Movement::getownOrientation() { std::lock_guard lock(mx); return ownOrientation_; } Eigen::Vector3d Movement::getEulerAngles() { std::lock_guard lock(mx); return ownOrientation_.getEulerAngles(ownPosition_->getGeodesicPos()(SimCore::LATITUDE), ownPosition_->getGeodesicPos()(SimCore::LONGITUDE)); } double Movement::getCourse() { std::lock_guard lock(mx); return course_; } void Movement::setCourse(double course) { std::lock_guard lock(mx); ownOrientation_.setHeading(course); course_ = course; } void Movement::setSpeed(double speed) { std::lock_guard lock(mx); speed_ = speed; } void Movement::setPitch(double pitch) { std::lock_guard lock(mx); ownOrientation_.setPitch(pitch); pitch_ = pitch; } double Movement::getPitch() { std::lock_guard lock(mx); return pitch_; } }