updated Position with distance calc

include/SimCore/Position.hpp

@@ -23,7 +23,7 @@ namespace SimCore {
             * @return Eigen::Vector3d
             */
             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,9 +33,13 @@ namespace SimCore {
             void setGeocentricPos(double X, double Y, double Z);
             void setGeodesicPos(double lat, double lon, int height);
 
-            bool operator== ( Position &lhs);
 
+            double distanceToPosition(Position &p2);
+
+
+            bool operator== ( Position &lhs);
             Position& operator=(const Position other); 
+
            
 
 

src/SimCore/Position.cpp

@@ -35,6 +35,16 @@ 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];
+
+    }
+
+
     Eigen::Vector3d Position::getGeodesicPos()
     {
         std::lock_guard<std::mutex> lock(mx);
@@ -61,6 +71,22 @@ namespace SimCore {
         earth_->Forward( GeodesicPos_(GeodesicPosition::LATITUDE),GeodesicPos_(GeodesicPosition::LONGITUDE),GeodesicPos_(GeodesicPosition::HEIGHT),GeocentricPos_(0),GeocentricPos_(1),GeocentricPos_(2));
     }
 
+
+    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);
+                            
+    }
+
+
+
+
     bool Position::operator==( Position &lhs)
     {
         std::lock_guard<std::mutex> lock(mx);