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/Geodesic.hpp>
#include <GeographicLib/GeodesicLine.hpp> #include <GeographicLib/GeodesicLine.hpp>
#include <GeographicLib/Constants.hpp> #include <GeographicLib/Constants.hpp>
#include <atomic>
#include <memory> #include <memory>
#include <mutex> #include <mutex>
@@ -24,6 +25,7 @@ namespace SimCore {
*/ */
Eigen::Vector3d getGeocentricPos(); Eigen::Vector3d getGeocentricPos();
void getGeocentricPos(double &lat, double &lon,double &h); void getGeocentricPos(double &lat, double &lon,double &h);
/** /**
* @brief returns a eigen vector3d with the lat, lon, height coordinates * @brief returns a eigen vector3d with the lat, lon, height coordinates
* @return Eigen::Vector3d * @return Eigen::Vector3d
@@ -33,6 +35,7 @@ namespace SimCore {
void setGeocentricPos(double X, double Y, double Z); void setGeocentricPos(double X, double Y, double Z);
void setGeodesicPos(double lat, double lon, int height); void setGeodesicPos(double lat, double lon, int height);
bool isValid();
double distanceToPosition(Position &p2); double distanceToPosition(Position &p2);
@@ -53,8 +56,7 @@ namespace SimCore {
mutable std::mutex mx; 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()); 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()); 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() 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()); 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()); wgs84_ = std::make_unique<GeographicLib::Ellipsoid>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
setGeocentricPos( X, Y, Z); setGeocentricPos( X, Y, Z);
} }
@@ -28,7 +30,7 @@ namespace SimCore {
earth_ = std::make_unique<GeographicLib::Geocentric>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); 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()); 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()); wgs84_ = std::make_unique<GeographicLib::Ellipsoid>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
valid_ = true;
} }
Eigen::Vector3d Position::getGeocentricPos() Eigen::Vector3d Position::getGeocentricPos()
@@ -41,10 +43,10 @@ namespace SimCore {
void Position::getGeocentricPos(double &lat, double &lon,double &h) void Position::getGeocentricPos(double &lat, double &lon,double &h)
{ {
std::lock_guard<std::mutex> lock(mx); std::lock_guard<std::mutex> lock(mx);
lat = GeodesicPos_[LATITUDE]; lat = GeodesicPos_[LATITUDE];
lon = GeodesicPos_[LONGITUDE]; lon = GeodesicPos_[LONGITUDE];
h = GeodesicPos_[HEIGHT]; h = GeodesicPos_[HEIGHT];
} }
@@ -66,7 +68,8 @@ namespace SimCore {
double lat, lon, h; 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){ void Position::setGeodesicPos(double lat, double lon, int height){
@@ -75,22 +78,36 @@ namespace SimCore {
GeodesicPos_(GeodesicPosition::LATITUDE) = lat; GeodesicPos_(GeodesicPosition::LATITUDE) = lat;
GeodesicPos_(GeodesicPosition::LONGITUDE) = lon; GeodesicPos_(GeodesicPosition::LONGITUDE) = lon;
GeodesicPos_(GeodesicPosition::HEIGHT) = height; 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) double Position::distanceToPosition(Position &p2)
{ {
std::lock_guard<std::mutex> lock(mx); if (valid_) {
double s12, azi1, azi2, m12; std::lock_guard<std::mutex> lock(mx);
geod_->Inverse(this->GeodesicPos_[LATITUDE], double s12, azi1, azi2, m12;
this->GeodesicPos_[LONGITUDE], geod_->Inverse(this->GeodesicPos_[LATITUDE],
p2.GeodesicPos_[LATITUDE], this->GeodesicPos_[LONGITUDE],
p2.GeodesicPos_[LONGITUDE], p2.GeodesicPos_[LATITUDE],
s12,azi1,azi2,m12); p2.GeodesicPos_[LONGITUDE],
s12);
return s12;
}else
{
return 0;
}
} }
bool Position::isValid()
{
return valid_;
}