89 lines
2.9 KiB
C++
89 lines
2.9 KiB
C++
|
|
#include "Entities/Movement.hpp"
|
|
#include "SimCore/Position.hpp"
|
|
#include "SimCore/SimCore.hpp"
|
|
#include <math.h>
|
|
#include <cmath>
|
|
#include <memory>
|
|
#include <string>
|
|
#include <thread>
|
|
#define CATCH_CONFIG_MAIN
|
|
#include <catch2/catch.hpp>
|
|
#include <loguru.hpp>
|
|
#include <SimCore/UtilFunctions.hpp>
|
|
#include <math.h>
|
|
|
|
|
|
|
|
SCENARIO("Testing the SimCore Sensor")
|
|
{
|
|
GIVEN("different Attributes for a Movement ")
|
|
{
|
|
std::shared_ptr<SimCore::Position> pos1 = std::make_shared<SimCore::Position>();
|
|
|
|
WHEN("constructing Track Object with data")
|
|
{
|
|
|
|
|
|
pos1->setGeodesicPos(55, 6, 0);
|
|
LOG_S(INFO)<<"LAT: "<<pos1->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< pos1->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< pos1->getGeodesicPos()(SimCore::HEIGHT);
|
|
|
|
Entities::Movement mov(pos1);
|
|
mov.setCourse(90);
|
|
mov.setSpeed(100);//m/s
|
|
mov.setPitch(0);
|
|
|
|
mov.updatePosition(10000);// 10 seconds
|
|
|
|
LOG_S(INFO)<<"LAT: "<<mov.getPosition()->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;
|
|
geod.Direct(55,6,90,1000,lat2,lon2);
|
|
SimCore::Position pos2;
|
|
pos2.setGeodesicPos(lat2, lon2, 0);
|
|
GeographicLib::GeodesicLine line = geod.InverseLine(55,6 , lat2, lon2);
|
|
LOG_S(INFO)<<"distance: "<<line.Distance()<< " lat2: "<< lat2 << " lon2: " << lon2;
|
|
|
|
|
|
double pitch = 0;
|
|
double course = 360;
|
|
double speed = 1;
|
|
|
|
|
|
Entities::Movement mov2(pos1,course,speed,pitch);
|
|
|
|
|
|
LOG_S(INFO)<<"LAT: "<<mov2.getPosition()->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov2.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov2.getPosition()->getGeodesicPos()(SimCore::HEIGHT);
|
|
|
|
|
|
|
|
std::shared_ptr<SimCore::Position> pos3 = std::make_shared<SimCore::Position>();
|
|
pos3->setGeodesicPos(55, 6, 0);
|
|
Entities::Movement mov3(pos3,0,1,90);
|
|
mov3.updatePosition(1000);
|
|
|
|
LOG_S(INFO)<<"LAT: "<<mov3.getPosition()->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov3.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov3.getPosition()->getGeodesicPos()(SimCore::HEIGHT);
|
|
|
|
|
|
|
|
|
|
THEN("check if Track attributes are correct")
|
|
{
|
|
|
|
|
|
// REQUIRE((pos2.getGeocentricPos() - mov.getPosition()->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);
|
|
|
|
|
|
|
|
} //THEN
|
|
} // WHEN
|
|
} // GIVEN
|
|
} //SCENARIO
|