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

ADD: added a valid value

Browse Source
This commit is contained in:
Henry Winkel
2023-03-28 16:18:08 +02:00
parent e75337bfbb
commit 7dbab562d1
2 changed files with 35 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
include/SimCore/Position.hpp
View File

@@ -7,6 +7,7 @@
#include <GeographicLib/Geodesic.hpp>
#include <GeographicLib/GeodesicLine.hpp>
#include <GeographicLib/Constants.hpp>
#include <atomic>
#include <memory>
#include <mutex>
@@ -24,6 +25,7 @@ namespace SimCore {
*/
Eigen::Vector3d getGeocentricPos();
void getGeocentricPos(double &lat, double &lon,double &h);
/**
* @brief returns a eigen vector3d with the lat, lon, height coordinates
* @return Eigen::Vector3d
@@ -33,6 +35,7 @@ namespace SimCore {
void setGeocentricPos(double X, double Y, double Z);
void setGeodesicPos(double lat, double lon, int height);
bool isValid();
double distanceToPosition(Position &p2);
@@ -53,8 +56,7 @@ namespace SimCore {
mutable std::mutex mx;
std::atomic_bool valid_;
};

45
src/SimCore/Position.cpp
View File

@@ -10,6 +10,7 @@ namespace SimCore {
geod_ = std::make_unique<GeographicLib::Geodesic>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
wgs84_ = std::make_unique<GeographicLib::Ellipsoid>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
valid_ = false;
}
Position::Position(double X, double Y, double Z):mx()
{
@@ -17,6 +18,7 @@ namespace SimCore {
geod_ = std::make_unique<GeographicLib::Geodesic>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
wgs84_ = std::make_unique<GeographicLib::Ellipsoid>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
setGeocentricPos( X, Y, Z);
}
@@ -28,7 +30,7 @@ namespace SimCore {
earth_ = std::make_unique<GeographicLib::Geocentric>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
geod_ = std::make_unique<GeographicLib::Geodesic>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
wgs84_ = std::make_unique<GeographicLib::Ellipsoid>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
valid_ = true;
}
Eigen::Vector3d Position::getGeocentricPos()
@@ -41,10 +43,10 @@ namespace SimCore {
void Position::getGeocentricPos(double &lat, double &lon,double &h)
{
std::lock_guard<std::mutex> lock(mx);
lat = GeodesicPos_[LATITUDE];
lon = GeodesicPos_[LONGITUDE];
h = GeodesicPos_[HEIGHT];
std::lock_guard<std::mutex> lock(mx);
lat = GeodesicPos_[LATITUDE];
lon = GeodesicPos_[LONGITUDE];
h = GeodesicPos_[HEIGHT];
}
@@ -66,7 +68,8 @@ namespace SimCore {
double lat, lon, h;
earth_->Reverse(GeocentricPos_(GeocentricPosition::X),GeocentricPos_(GeocentricPosition::Y),GeocentricPos_(GeocentricPosition::Z),GeodesicPos_(GeodesicPosition::LATITUDE),GeodesicPos_(GeodesicPosition::LONGITUDE),GeodesicPos_(GeodesicPosition::HEIGHT));
earth_->Reverse(GeocentricPos_(GeocentricPosition::X),GeocentricPos_(GeocentricPosition::Y),GeocentricPos_(GeocentricPosition::Z),GeodesicPos_(GeodesicPosition::LATITUDE),GeodesicPos_(GeodesicPosition::LONGITUDE),GeodesicPos_(GeodesicPosition::HEIGHT));
valid_ = true;
}
void Position::setGeodesicPos(double lat, double lon, int height){
@@ -75,22 +78,36 @@ namespace SimCore {
GeodesicPos_(GeodesicPosition::LATITUDE) = lat;
GeodesicPos_(GeodesicPosition::LONGITUDE) = lon;
GeodesicPos_(GeodesicPosition::HEIGHT) = height;
earth_->Forward( GeodesicPos_(GeodesicPosition::LATITUDE),GeodesicPos_(GeodesicPosition::LONGITUDE),GeodesicPos_(GeodesicPosition::HEIGHT),GeocentricPos_(0),GeocentricPos_(1),GeocentricPos_(2));
earth_->Forward( GeodesicPos_(GeodesicPosition::LATITUDE),GeodesicPos_(GeodesicPosition::LONGITUDE),GeodesicPos_(GeodesicPosition::HEIGHT),GeocentricPos_(X),GeocentricPos_(Y),GeocentricPos_(Z));
valid_ = true;
}
double Position::distanceToPosition(Position &p2)
{
std::lock_guard<std::mutex> lock(mx);
double s12, azi1, azi2, m12;
geod_->Inverse(this->GeodesicPos_[LATITUDE],
this->GeodesicPos_[LONGITUDE],
p2.GeodesicPos_[LATITUDE],
p2.GeodesicPos_[LONGITUDE],
s12,azi1,azi2,m12);
if (valid_) {
std::lock_guard<std::mutex> lock(mx);
double s12, azi1, azi2, m12;
geod_->Inverse(this->GeodesicPos_[LATITUDE],
this->GeodesicPos_[LONGITUDE],
p2.GeodesicPos_[LATITUDE],
p2.GeodesicPos_[LONGITUDE],
s12);
return s12;
}else
{
return 0;
}
}
bool Position::isValid()
{
return valid_;
}