113 lines
3.5 KiB
C++
113 lines
3.5 KiB
C++
|
|
|
|
#include "Entities/Movement.hpp"
|
|
#include "SimCore/Position.hpp"
|
|
#include "SimCore/SimCore.hpp"
|
|
#include <memory>
|
|
#include <string>
|
|
#include <thread>
|
|
#define CATCH_CONFIG_MAIN
|
|
#include <catch2/catch.hpp>
|
|
#include <loguru.hpp>
|
|
|
|
|
|
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;
|
|
|
|
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;
|
|
|
|
|
|
Entities::Movement mov2(pos1);
|
|
mov2.setCourse(0);//degree
|
|
mov2.setSpeed(100);//m/s
|
|
mov2.setPitch(45);//degree
|
|
|
|
LOG_S(INFO)<<"LAT: "<<mov2.getPosition().getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov2.getPosition().getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT);
|
|
|
|
double Vd = 100 * sin(45* M_PI / 180.0);
|
|
double h = Vd*10;
|
|
LOG_S(INFO)<<"höhe: "<< h;
|
|
double i;
|
|
|
|
for ( i = 0; i <= 10; ) {
|
|
mov2.updatePosition(500);
|
|
i += 0.5;
|
|
}
|
|
|
|
MovementVec movc;
|
|
movc.dSpd = 10;
|
|
movc.dClb = 0;
|
|
movc.dCrs = 0;
|
|
// LOG_S(INFO)<< mov.getEulerAngles();
|
|
|
|
THEN("check if Track attributes are correct")
|
|
{
|
|
|
|
// LOG_S(INFO)<<mov.getPosition().getGeodesicPos()(SimCore::LATITUDE);
|
|
REQUIRE((pos2.getGeocentricPos() - mov.getPosition().getGeocentricPos()).norm() <= 0.1);
|
|
REQUIRE((h - mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT)) <= 0.1);
|
|
|
|
|
|
|
|
} //THEN
|
|
} // WHEN
|
|
} // GIVEN
|
|
} //SCENARIO
|