implement distanceTo and courseTo

doc/Notes and TODOs/TODO allgemein.txt

@@ -3,6 +3,5 @@
     -> Program battery pack tracking
     -> Add an beeper
     -> Program the beeper
-    -> GPS-Menu without Wlan NtripClient
     -> CaptureRoute Modus crash when someone try's to save a point
 

lib/Navigation/route.cpp

@@ -11,6 +11,34 @@
 
 #include "route.h"
 
+double Point::distanceTo(const Point &point) const {
+    // distance = sqrt(dx * dx + dy * dy)
+
+    // mit distance: Entfernung in km 
+    // dx = 111.3 * cos(lat) * (lon1 - lon2)
+    // lat = (lat1 + lat2) / 2 * 0.01745
+    // dy = 111.3 * (lat1 - lat2)
+    // lat1, lat2, lon1, lon2: Breite, Länge in Grad
+
+    double lat = (this->lat + point.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
+    double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (this->lat - point.lat);
+    double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (this->lon - point.lon);
+
+    return sqrt(dx * dx + dy * dy);
+}
+
+double Point::courseTo(const Point &point) const {
+    double phi = log( tan(point.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(this->lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
+    double lon = (this->lon * ROUTE_DEGREE_TO_RADIANT - point.lon * ROUTE_DEGREE_TO_RADIANT);
+
+    double res = atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT;
+
+    // if (res < 0)
+    //     res += 360.0; 
+    
+    return res;
+}
+
 Route::Route() {
 
 }

lib/Navigation/route.h

@@ -14,7 +14,11 @@
 
 #include <cstdint>
 #include <list> 
+#include <cmath>   
 
+#define ROUTE_DEGREE_TO_RADIANT 0.01745
+#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300
+#define ROUTE_PI 3.14159265358979323846
 
 /**
  * @brief A to handle points on the earth
@@ -55,9 +59,8 @@ class Point{
          */
         bool isValid() const { return this->lat + this->lon; }
 
-        // FIXME: Add real implementations!
-        double distanceTo(const Point& point) const { return 0; }
-        double courseTo(const Point& point) const { return 0; }
+        double distanceTo(const Point& point) const;
+        double courseTo(const Point& point) const;
 };
 
 /**