diff --git a/CMakeLists.txt b/CMakeLists.txt index aa75c24..d00d85d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,5 +91,9 @@ IF (${TEST_ENTITIY_LIBRARY}) target_link_libraries(test_Tracklist Catch2::Catch2 EntityLibrary loguru) catch_discover_tests(test_Tracklist) + + add_executable(test_MovementClass tests/test_MovementClass.cpp) + target_link_libraries(test_MovementClass Catch2::Catch2 EntityLibrary loguru) + catch_discover_tests(test_MovementClass) ENDIF() diff --git a/include/Entities/Movement.hpp b/include/Entities/Movement.hpp index fe36c28..5f33eba 100644 --- a/include/Entities/Movement.hpp +++ b/include/Entities/Movement.hpp @@ -16,10 +16,42 @@ namespace Entities { public: Movement(); + /** + * @brief Movement constructor + * @param std::shared_ptr pos + */ Movement(std::shared_ptr pos); - Movement(std::shared_ptr, double course); - Movement(std::shared_ptr, double course, double speed); - Movement(std::shared_ptr, double course, double speed, double pitch); + + /** + * @brief Movement constructor + * @param std::shared_ptr pos + * @param double course in Degree + */ + Movement(std::shared_ptr pos, double course); + + /** + * @brief Movement constructor + * @param std::shared_ptr pos + * @param double course in Degree + * @param double speed in m/s + */ + Movement(std::shared_ptr pos, double course, double speed); + + /** + * @brief Movement constructor + * @param std::shared_ptr pos + * @param double course in Degree + * @param double speed in m/s + * @param double sppitch (climbangle) in Degree + */ + Movement(std::shared_ptr pos, double course, double speed, double pitch); + + /** + * @brief Movement constructor + * @param std::shared_ptr pos + * @param Eigen::Vector3d ECEF vector + */ + Movement(std::shared_ptr pos, Eigen::Vector3d ECEF); @@ -31,34 +63,67 @@ namespace Entities /** - * @brief returns the current Course representing the direction the front of the entity is pointing - * + * @brief returns the current Course in rad representing the direction the front of the entity is pointing + * @return double course in rad */ double getCourse(); + /** + * @brief set the course + * @param double course in degree + */ void setCourse(double course); + /** + * @brief set the course + * @param double course in rad + */ + void setCourseInRad(double courseInRad); + + /** * @brief returns the speed in m/s * */ double getSpeed(); - + + /** + * @brief set the speed in m/s + * @param set speed in m/s + */ void setSpeed(double speed); /** - * returns the climbing angle of the entity of the entity + * @brief set the speed in knots + * @param set speed in knots + */ + void setSpeedInKnots(double speed); + + + /** + * returns the climbing angle in rad of the entity of the entity * */ double getPitch(); - + + /** + * @brief set the course + * @param double pitch /climbangle in degree + */ void setPitch(double pitch); + /** + * @brief set the course + * @param double pitch /climbangle in rad + */ + void setPitchInRad(double pitch); + /** * @brief returns the kinematicVector of Entity * */ Eigen::Vector3d getKinematicVector(); + // void calKinematicVector(); std::shared_ptr getPosition(); @@ -66,29 +131,72 @@ namespace Entities void setPosition(SimCore::Position pos); - + /** + * @return returns copy of orientation class + * + */ SimCore::Orientation getownOrientation(); + /** + * @return Eigen::Vector3d of the current eulerangles + * + */ Eigen::Vector3d getEulerAngles(); + // 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 + * @param SimCore::Position Position + * @return Eigen::Vector3d ECEF + * from https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates + */ + static Eigen::Vector3d ToECEFFromENU(Eigen::Vector3d ENU, SimCore::Position position); + /** + * @brief transforms Course, Speed and ClimbAngle to local ENU (East North Up) Vector + * @param double course + * @param double speed + * @param double climbAngle + * @return Eigen::Vector3d ENU + */ + static Eigen::Vector3d ENUFromCourseSpeedClbToLocalSpeedVector(double course, double speed, double climbAngle); + /** + * @brief transforms get ENU (East North Up) from ECEF Vector + * @param Eigen::Vector3d ECEF + * @param SimCore::Position position + * @return Eigen::Vector3d ENU + */ + static Eigen::Vector3d ENUFromECEF(Eigen::Vector3d ECEF, SimCore::Position position); + + /** + * @brief converts ENU (East North Up) velocity Vector to course speed and climbangles + * @param Eigen::Vector3d ENU + * @return std::tuple course speed, climbangle + */ + static std::tuple getCourseSpeedClimbAngleFromENU(Eigen::Vector3d ENU); private: std::shared_ptr ownPosition_; SimCore::Orientation ownOrientation_; + + Eigen::Vector3d ToECEFFromENU(); + + Eigen::Vector3d ENUFromCourseSpeedClbToLocalSpeedVector(); + + /// East North Up Vector wich represents the local Speed Vector + Eigen::Vector3d ENUVec_; - Eigen::Vector3d kinematicVec_; Eigen::Vector3d accelerationVec_; double course_; double speed_; - double pitch_; + double climbAngle_; - void calKinematicVector(); - Eigen::Matrix3d getRotationMatrix(double lat, double lon); mutable std::mutex mx; diff --git a/libs/SimCore b/libs/SimCore index e879a12..49dd5e2 160000 --- a/libs/SimCore +++ b/libs/SimCore @@ -1 +1 @@ -Subproject commit e879a12f2667f7a82c8a8ea2aad2d0ced957cb69 +Subproject commit 49dd5e295ff6db3ee6f416ffb66855d0ac6b7232 diff --git a/src/Entities/Movement.cpp b/src/Entities/Movement.cpp index 36a0361..6fa11c9 100644 --- a/src/Entities/Movement.cpp +++ b/src/Entities/Movement.cpp @@ -4,6 +4,7 @@ #include "SimCore/UtilFunctions.hpp" #include #include +#include using namespace Eigen; @@ -14,7 +15,7 @@ namespace Entities { course_ = 0; speed_= 0; - pitch_ = 0; + climbAngle_ = 0; ownOrientation_.setRoll(0); ownOrientation_.setHeading(0); @@ -23,12 +24,12 @@ namespace Entities ownPosition_->setGeodesicPos(0,0,0); } - + Movement::Movement(std::shared_ptr pos):ownPosition_(pos) { course_ = 0; speed_= 0; - pitch_ = 0; + climbAngle_ = 0; ownOrientation_.setRoll(0); ownOrientation_.setHeading(0); @@ -37,60 +38,80 @@ namespace Entities } - Movement::Movement(std::shared_ptr pos, double course):ownPosition_(pos),course_(course) + Movement::Movement(std::shared_ptr pos, double course):ownPosition_(pos),course_(SimCore::UtilFunctions::DegToRad(course)) { speed_ = 0; - pitch_ = 0; + climbAngle_ = 0; ownOrientation_.setRoll(0); ownOrientation_.setPitch(0); } - Movement::Movement(std::shared_ptr pos, double course, double speed):ownPosition_(pos),course_(course),speed_(speed) + + + Movement::Movement(std::shared_ptr pos, double course, double speed):ownPosition_(pos),course_(SimCore::UtilFunctions::DegToRad(course)),speed_(speed) { - pitch_ = 0; + climbAngle_ = 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) + + + Movement::Movement(std::shared_ptr pos, double course, double speed, double pitch):ownPosition_(pos),course_(SimCore::UtilFunctions::DegToRad(course)),speed_(speed),climbAngle_(SimCore::UtilFunctions::DegToRad(pitch)) { ownOrientation_.setRoll(0); } + + Movement::Movement(std::shared_ptr pos, Eigen::Vector3d ECEF):ownPosition_(pos) + { + + course_ = 0; + speed_= 0; + climbAngle_ = 0; + + ownOrientation_.setRoll(0); + ownOrientation_.setHeading(0); + ownOrientation_.setPitch(0); + + ENUVec_ = this->ENUFromECEF(ECEF,*pos.get()); + + auto mov = getCourseSpeedClimbAngleFromENU(ENUVec_); + + auto [course, speed, climbAngle] = mov; + course_ = course; + speed_= speed; + climbAngle_ = climbAngle; + + ownOrientation_.setHeading(course_); + ownOrientation_.setPitch(climbAngle_); + + } + + 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_; - + auto ecefVelocityVector = this->ToECEFFromENU(); 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); + // 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; + // 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(); + // c.notify_one(); } @@ -124,51 +145,118 @@ namespace Entities } + Eigen::Vector3d Movement::ToECEFFromENU() + { + if (!ownPosition_->isValid()) { + return Eigen::Vector3d(0,0,0); + } + Eigen::Vector3d ENU = this->ENUFromCourseSpeedClbToLocalSpeedVector(); + + double lat = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LATITUDE)); // in radians + double lon = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LONGITUDE)); // in radians LocalRotationMatrix << + + Eigen::Matrix3d LocalRotationMatrix; + LocalRotationMatrix << + -sin(lon), - cos(lon)* sin(lat), cos(lon)*cos(lat) , + cos(lon), - sin(lon)* sin(lat), sin(lon) *cos(lat), + 0 , cos(lat) , sin(lat); + + + return LocalRotationMatrix * ENU; + } + + Eigen::Vector3d Movement::ENUFromCourseSpeedClbToLocalSpeedVector() + { + Eigen::Vector3d ENU; + 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; + } + Eigen::Vector3d Movement::getKinematicVector() { - return kinematicVec_; + return ENUVec_; } - Eigen::Matrix3d Movement::getRotationMatrix(double lat, double lon) + Eigen::Vector3d Movement::ToECEFFromENU(Eigen::Vector3d ENU, SimCore::Position position) { - 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; + if (!position.isValid()) { + return Eigen::Vector3d(0,0,0); + } + + double lat = SimCore::UtilFunctions::DegToRad(position.getGeodesicPos()(SimCore::LATITUDE)); // in radians + double lon = SimCore::UtilFunctions::DegToRad(position.getGeodesicPos()(SimCore::LONGITUDE)); // in radians + + Eigen::Matrix3d LocalRotationMatrix; + LocalRotationMatrix << + -sin(lon), - cos(lon)* sin(lat), cos(lon)*cos(lat) , + cos(lon), - sin(lon)* sin(lat), sin(lon) *cos(lat), + 0 , cos(lat) , sin(lat); + + + return LocalRotationMatrix * ENU; } - - void Movement::calKinematicVector() + Eigen::Vector3d Movement::ENUFromCourseSpeedClbToLocalSpeedVector(double course, double speed, double climbAngle) { + Eigen::Vector3d ENU; + ENU[0] = speed * cos(climbAngle) * sin(course); + ENU[1] = speed * cos(climbAngle) * cos(course); + ENU[2] = speed * sin(climbAngle); - Eigen::Vector3d UVW_Coord; + return ENU; + } - // Convert heading and climbing angle to radians - double heading_rad = SimCore::UtilFunctions::DegToRad( this->getCourse()); - double climbing_angle_rad = SimCore::UtilFunctions::DegToRad(this->getPitch()); + Eigen::Vector3d Movement::ENUFromECEF(Eigen::Vector3d ECEF, SimCore::Position position) + { - // 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); + double lat = SimCore::UtilFunctions::DegToRad(position.getGeodesicPos()(SimCore::LATITUDE)); // in radians + double lon = SimCore::UtilFunctions::DegToRad(position.getGeodesicPos()(SimCore::LONGITUDE)); // in radians + - // Create NED velocity vector using Eigen vector - Vector3d Vn2(Vn, Ve, -Vd); + Eigen::Matrix3d LocalRotationMatrix; + LocalRotationMatrix<< + - sin(lon) , cos(lon) , 0 , + - cos(lon) * sin(lat) , - sin(lon) * sin(lat) , cos(lat), + cos(lon) * cos(lat) , sin(lon) * cos(lat) , sin(lat); - // Create transformation matrix from NED to ENU frame using Eigen matrix - Matrix3d T; - T << 0, 1, 0, - 1, 0, 0, - 0, 0, -1; + + + return LocalRotationMatrix * ECEF; + } + + std::tuple Movement::getCourseSpeedClimbAngleFromENU(Eigen::Vector3d ENU) + { + const double dEPS = 0.000001; /// to filter out clalculation inaccuracies + + std::tuple Result; + double Course, Speed, ClimbAngle; + if (abs(ENU[0]) < dEPS && abs(ENU[1]) < dEPS && abs(ENU[2]) < dEPS) + { + Speed = 0; + + Course = 0; + + ClimbAngle = 0; + + } else + { + Speed = pow( pow(ENU[0],2) + pow(ENU[1],2) + pow(ENU[2],2), 0.5); + + Course = SimCore::UtilFunctions::ShiftRadCrs(atan2( ENU[0], ENU[1])); + + ClimbAngle = SimCore::UtilFunctions::ShiftRadClb(atan2( ENU[2], pow( pow(ENU[0],2) + pow(ENU[1],2), 0.5) )); + + } + return std::make_tuple(Course,Speed,ClimbAngle); + } - // Transform NED velocity vector to ENU velocity vector using Eigen matrix multiplication - Vector3d Venu = T * Vn2; - kinematicVec_ = Venu; - } std::shared_ptr Movement::getPosition() @@ -215,7 +303,20 @@ namespace Entities { std::lock_guard lock(mx); ownOrientation_.setHeading(course); - course_ = course; + course_ = SimCore::UtilFunctions::DegToRad(course); + } + + + void Movement::setCourseInRad(double courseInRad) + { + std::lock_guard lock(mx); + ownOrientation_.setHeading(SimCore::UtilFunctions::RadToDeg(courseInRad)); + course_ = courseInRad; + } + + double Movement::getSpeed() + { + return speed_; } void Movement::setSpeed(double speed) @@ -224,18 +325,32 @@ namespace Entities speed_ = speed; } + void Movement::setSpeedInKnots(double speed) + { + std::lock_guard lock(mx); + speed_ = SimCore::UtilFunctions::KnotsToMs(speed); + } + + void Movement::setPitch(double pitch) { std::lock_guard lock(mx); ownOrientation_.setPitch(pitch); - pitch_ = pitch; + climbAngle_ = SimCore::UtilFunctions::DegToRad(pitch); } - double Movement::getPitch() - { + void Movement::setPitchInRad(double pitch) + { std::lock_guard lock(mx); - return pitch_; - } + ownOrientation_.setPitch(SimCore::UtilFunctions::RadToDeg(pitch)); + climbAngle_ = pitch; + } + + double Movement::getPitch() + { + std::lock_guard lock(mx); + return climbAngle_; + } diff --git a/tests/MovementCalculationTryOuts.txt b/tests/MovementCalculationTryOuts.txt new file mode 100644 index 0000000..7fc7d5c --- /dev/null +++ b/tests/MovementCalculationTryOuts.txt @@ -0,0 +1,183 @@ +struct KinVec{ + double x,y,z; +}; +struct MovementVec{ + double dSpd,dClb,dCrs; +}; + +struct LLA_Rad{ + double dLon,dLat; +}; +struct UVW{ + double dU,dV,dW; +}; + +struct Movement +{ + double dCrs, dSpd, dClb; +}; + +Eigen::Vector3d Generate_Kin_Vect(MovementVec Mov_Dat, LLA_Rad LLA_rCoord) +{ + + //Transform Mov_Dat to topocentric vector + // UVW UVW_Coord; + Eigen::Vector3d ENU; + ENU[0] = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * sin(Mov_Dat.dCrs); + ENU[1] = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * cos(Mov_Dat.dCrs); + ENU[2] = Mov_Dat.dSpd * sin(Mov_Dat.dClb); + + //Transform topocentric vector to geocentric vector + // Eigen::Vector3d UVW(UVW_Coord.dU,UVW_Coord.dV, UVW_Coord.dW); + LOG_S(INFO)<<"UVW: "<< ENU[0]<< " "<< ENU[1]<< " " << ENU[2] ; + + Eigen::Matrix3d LocalRotationMatrix; + + LocalRotationMatrix << + -sin(LLA_rCoord.dLon), - cos(LLA_rCoord.dLon)*sin(LLA_rCoord.dLat) , cos(LLA_rCoord.dLon)*cos(LLA_rCoord.dLat) , + cos(LLA_rCoord.dLon), - sin(LLA_rCoord.dLon)* sin(LLA_rCoord.dLat), sin(LLA_rCoord.dLon) *cos(LLA_rCoord.dLat), + 0 , cos(LLA_rCoord.dLat) , sin(LLA_rCoord.dLat); + + + return LocalRotationMatrix*ENU; +}; + + + + +struct Kin_Vect +{ + double dVx, dVy, dVz; +}; + +struct XYZ +{ + double dX,dY,dZ; +}; +const double dEPS = 0.000001; + +double ShiftRadCrs(double Rad) +{ + while(Rad < 0) + { + Rad += 2 * M_PI; + } + + while (Rad > 2 * M_PI) + { + Rad -= 2 * M_PI; + } + return Rad; + +}; + +double ShiftRadClb(double Rad) +{ + while(Rad < -M_PI) + { + Rad += 2 * M_PI; + } + + while (Rad > M_PI) + { + Rad -= 2 * M_PI; + } + return Rad; +}; + + +Movement Generate_Movement(Eigen::Vector3d Kin_Dat, LLA_Rad LLA_rCoord, XYZ XYZ_Coord) +{ + Movement Result; + + // Transform geocentric vector to topocentric vector + + + Eigen::Vector3d ENU; + // UVW UVW_Coord; + + // UVW_Coord.dU = -(Kin_Dat.dVx) * sin(LLA_rCoord.dLon) + + // (Kin_Dat.dVy) * cos(LLA_rCoord.dLon); + + // UVW_Coord.dV = -(Kin_Dat.dVx) * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) - + // (Kin_Dat.dVy) * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) + + // (Kin_Dat.dVz) * cos(LLA_rCoord.dLat); + + // UVW_Coord.dW = (Kin_Dat.dVx) * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) + + // (Kin_Dat.dVy) * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) + + // (Kin_Dat.dVz) * sin(LLA_rCoord.dLat); + + Eigen::Matrix3d LocalRotationMatrix; + LocalRotationMatrix<< + - sin(LLA_rCoord.dLon) , cos(LLA_rCoord.dLon) , 0, + - sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon), - sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon), cos(LLA_rCoord.dLat), + cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon), cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon), sin(LLA_rCoord.dLat); + + + + ENU = LocalRotationMatrix * Kin_Dat; + + + LOG_S(INFO)<<"ENU: "<< ENU[0]<< " "< +#include #include #include #include #define CATCH_CONFIG_MAIN #include #include +#include +#include -struct KinVec{ - double x,y,z; -} -struct MovementVec{ - double dSpd,dClb,dCrs; -} -KinVec Generate_Kin_Vect(MovementVec Mov_Dat, LLA_Rad LLA_rCoord) -{ - KinVec Result; - - //Transform Mov_Dat to topocentric vector - UVW UVW_Coord; - - UVW_Coord.dU = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * sin(Mov_Dat.dCrs); - UVW_Coord.dV = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * cos(Mov_Dat.dCrs); - UVW_Coord.dW = Mov_Dat.dSpd * sin(Mov_Dat.dClb); - - //Transform topocentric vector to geocentric vector - - Result.x = -UVW_Coord.dU * sin(LLA_rCoord.dLon) - - UVW_Coord.dV * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) + - UVW_Coord.dW * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon); - - Result.y = UVW_Coord.dU * cos(LLA_rCoord.dLon) - - UVW_Coord.dV * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) + - UVW_Coord.dW * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon); - - Result.z = UVW_Coord.dV * cos(LLA_rCoord.dLat) + - UVW_Coord.dW * sin(LLA_rCoord.dLat); - - - return Result; -}; - SCENARIO("Testing the SimCore Sensor") { GIVEN("different Attributes for a Movement ") { - SimCore::Position pos1; + std::shared_ptr pos1 = std::make_shared(); WHEN("constructing Track Object with data") { - pos1.setGeodesicPos(55, 6, 0); - LOG_S(INFO)<<"LAT: "<setGeodesicPos(55, 6, 0); + LOG_S(INFO)<<"LAT: "<getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< pos1->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< pos1->getGeodesicPos()(SimCore::HEIGHT); Entities::Movement mov(pos1); mov.setCourse(90); @@ -64,7 +35,7 @@ SCENARIO("Testing the SimCore Sensor") mov.updatePosition(10000);// 10 seconds - LOG_S(INFO)<<"LAT: "<getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov.getPosition()->getGeodesicPos()(SimCore::HEIGHT); GeographicLib::Geodesic geod(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); double h2, lat2,lon2; @@ -74,36 +45,41 @@ SCENARIO("Testing the SimCore Sensor") GeographicLib::GeodesicLine line = geod.InverseLine(55,6 , lat2, lon2); LOG_S(INFO)<<"distance: "<getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov2.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov2.getPosition()->getGeodesicPos()(SimCore::HEIGHT); + + + std::shared_ptr pos3 = std::make_shared(); + pos3->setGeodesicPos(55, 6, 0); + Entities::Movement mov3(pos3,0,1,90); + mov3.updatePosition(1000); + + LOG_S(INFO)<<"LAT: "<getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov3.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov3.getPosition()->getGeodesicPos()(SimCore::HEIGHT); + + + + THEN("check if Track attributes are correct") { - // LOG_S(INFO)<getGeocentricPos()).norm() <= 0.1); + REQUIRE((pos2.getGeodesicPos() - pos1->getGeodesicPos()).norm() <= 0.1); + + REQUIRE(round(mov3.getPosition()->getGeodesicPos()(SimCore::HEIGHT)) == 1); + + REQUIRE(mov2.getCourse() == SimCore::UtilFunctions::DegToRad(course)); + REQUIRE(mov2.getPitch() == SimCore::UtilFunctions::DegToRad(pitch)); + REQUIRE(mov2.getSpeed() == speed);