From 7dbab562d1db4042bf94a809bf859d34529e173a Mon Sep 17 00:00:00 2001
From: Henry Winkel <henrywinkel@bundeswehr.org>
Date: Tue, 28 Mar 2023 16:18:08 +0200
Subject: [PATCH] ADD: added a valid value

---
 include/SimCore/Position.hpp |  6 +++--
 src/SimCore/Position.cpp     | 45 +++++++++++++++++++++++++-----------
 2 files changed, 35 insertions(+), 16 deletions(-)

diff --git a/include/SimCore/Position.hpp b/include/SimCore/Position.hpp
index d45585c..f30d935 100644
--- a/include/SimCore/Position.hpp
+++ b/include/SimCore/Position.hpp
@@ -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_;
 
     };
 
diff --git a/src/SimCore/Position.cpp b/src/SimCore/Position.cpp
index b5d55f7..fac49f3 100644
--- a/src/SimCore/Position.cpp
+++ b/src/SimCore/Position.cpp
@@ -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_;
+     }
+