From 670dffaeb83ac2003751b3f76c17be2282b126d7 Mon Sep 17 00:00:00 2001
From: Valentin Platzgummer <valentin.platzgummer@ymail.com>
Date: Sun, 31 Jan 2021 10:46:50 +0100
Subject: [PATCH] geometry.h toENU nan issue solved

---
 src/MeasurementComplexItem/AreaData.cc        |  5 +-
 .../geometry/geometry.h                       | 67 +++++++++----------
 2 files changed, 33 insertions(+), 39 deletions(-)

diff --git a/src/MeasurementComplexItem/AreaData.cc b/src/MeasurementComplexItem/AreaData.cc
index 48aafe9e9..7320c12c5 100644
--- a/src/MeasurementComplexItem/AreaData.cc
+++ b/src/MeasurementComplexItem/AreaData.cc
@@ -222,7 +222,7 @@ void AreaData::intersection(bool showError) {
       geometry::areaToEnu(origin, safeArea->pathModel(), safeAreaENU);
       geometry::FPolygon measurementAreaENU;
       geometry::areaToEnu(origin, measurementArea->pathModel(),
-                       measurementAreaENU);
+                          measurementAreaENU);
 
       // do intersection
       std::deque<geometry::FPolygon> outputENU;
@@ -248,8 +248,9 @@ void AreaData::intersection(bool showError) {
       auto large = std::move(outputENU[0]);
       geometry::FPolygon small;
       while (!bg::covered_by(large, safeAreaENU)) {
-        geometry::offsetPolygon(large, small, -0.1);
+        geometry::offsetPolygon(large, small, -0.01);
         large = std::move(small);
+        qDebug() << "shrink";
       }
 
       // Check if result is different from input.
diff --git a/src/MeasurementComplexItem/geometry/geometry.h b/src/MeasurementComplexItem/geometry/geometry.h
index b43cc2383..f19dee468 100644
--- a/src/MeasurementComplexItem/geometry/geometry.h
+++ b/src/MeasurementComplexItem/geometry/geometry.h
@@ -18,11 +18,9 @@
 namespace bg = boost::geometry;
 namespace bu = boost::units;
 
-#include <GeographicLib/Geocentric.hpp>
-#include <GeographicLib/LocalCartesian.hpp>
-
 #include "QGCQGeoCoordinate.h"
 #include "QmlObjectListModel.h"
+#include <QPointF>
 
 namespace geometry {
 //=========================================================================
@@ -109,34 +107,6 @@ static constexpr int earth_radius = 6371000; // meters (m)
 static constexpr double epsilon =
     std::numeric_limits<double>::epsilon(); // meters (m)
 
-template <class GeoPoint1, class GeoPoint2>
-void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) {
-
-  double lat_rad = in.latitude() * M_PI / 180;
-  double lon_rad = in.longitude() * M_PI / 180;
-
-  double ref_lon_rad = origin.longitude() * M_PI / 180;
-  double ref_lat_rad = origin.latitude() * M_PI / 180;
-
-  double sin_lat = std::sin(lat_rad);
-  double cos_lat = std::cos(lat_rad);
-  double cos_d_lon = std::cos(lon_rad - ref_lon_rad);
-
-  double ref_sin_lat = std::sin(ref_lat_rad);
-  double ref_cos_lat = std::cos(ref_lat_rad);
-
-  double c =
-      std::acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
-  double k = (std::fabs(c) < epsilon) ? 1.0 : (c / std::sin(c));
-
-  double x = k * cos_lat * std::sin(lon_rad - ref_lon_rad) * earth_radius;
-  double y = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) *
-             earth_radius;
-
-  out.set<0>(x);
-  out.set<1>(y);
-}
-
 template <class GeoPoint1, class GeoPoint2, class Point>
 void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
 
@@ -155,7 +125,10 @@ void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
   double ref_sin_lat = sin(ref_lat_rad);
   double ref_cos_lat = cos(ref_lat_rad);
 
-  double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
+  double value = ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon;
+  value = value > 1 ? 1 : value;
+  value = value < -1 ? -1 : value;
+  double c = acos(value);
   double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c));
 
   double x = k * cos_lat * sin(lon_rad - ref_lon_rad) * earth_radius;
@@ -166,13 +139,13 @@ void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
   out.setY(y);
 }
 
-template <class GeoPoint>
-void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
+template <class GeoPoint1, class Point, class GeoPoint2>
+void fromENU(const GeoPoint1 &origin, const Point &in, GeoPoint2 &out) {
 
   using namespace std;
 
-  double x_rad = in.get<0>() / earth_radius;
-  double y_rad = in.get<1>() / earth_radius;
+  double x_rad = in.x() / earth_radius;
+  double y_rad = in.y() / earth_radius;
   double c = sqrt(y_rad * y_rad + x_rad * x_rad);
   double sin_c = sin(c);
   double cos_c = cos(c);
@@ -187,7 +160,11 @@ void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
   double lon_rad;
 
   if (fabs(c) > epsilon) {
-    lat_rad = asin(cos_c * ref_sin_lat + (y_rad * sin_c * ref_cos_lat) / c);
+    double v1 = cos_c * ref_sin_lat + (y_rad * sin_c * ref_cos_lat) / c;
+    v1 = v1 > 1 ? 1 : v1;
+    v1 = v1 < -1 ? -1 : v1;
+    lat_rad = asin(v1);
+
     lon_rad =
         (ref_lon_rad + atan2(x_rad * sin_c, c * ref_cos_lat * cos_c -
                                                 y_rad * ref_sin_lat * sin_c));
@@ -202,6 +179,22 @@ void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
   out.setAltitude(origin.altitude());
 }
 
+template <class GeoPoint1, class GeoPoint2>
+void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) {
+
+  QPointF p;
+  toENU(origin, in, p);
+  out.set<0>(p.x());
+  out.set<1>(p.y());
+}
+
+template <class GeoPoint1, class GeoPoint2>
+void fromENU(const GeoPoint1 &origin, const FPoint &in, GeoPoint2 &out) {
+
+  QPointF p(in.get<0>(), in.get<1>());
+  fromENU(origin, p, out);
+}
+
 template <class GeoPoint, class Container1, class Container2>
 void areaToEnu(const GeoPoint &origin, const Container1 &in, Container2 &out) {
   for (auto &vertex : in) {
-- 
2.22.0