From 86efb38e036fe9039ad8e622264a94352ada33eb Mon Sep 17 00:00:00 2001
From: DonLakeFlyer <don@thegagnes.com>
Date: Tue, 17 Oct 2017 15:31:37 -0700
Subject: [PATCH] Add ::offset and :: nedPolygon support

---
 src/MissionManager/QGCMapPolygon.cc | 80 ++++++++++++++++++++++++++++-
 src/MissionManager/QGCMapPolygon.h  |  6 +++
 2 files changed, 85 insertions(+), 1 deletion(-)

diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc
index fddd166a5..460c7c0a5 100644
--- a/src/MissionManager/QGCMapPolygon.cc
+++ b/src/MissionManager/QGCMapPolygon.cc
@@ -15,6 +15,7 @@
 #include <QGeoRectangle>
 #include <QDebug>
 #include <QJsonArray>
+#include <QLineF>
 
 const char* QGCMapPolygon::jsonPolygonKey = "polygon";
 
@@ -290,7 +291,7 @@ void QGCMapPolygon::_updateCenter(void)
     if (!_ignoreCenterUpdates) {
         QGeoCoordinate center;
 
-        if (_polygonPath.count() > 2) {            
+        if (_polygonPath.count() > 2) {
             QPointF centroid(0, 0);
             QPolygonF polygonF = _toPolygonF();
             for (int i=0; i<polygonF.count(); i++) {
@@ -356,3 +357,80 @@ QGeoCoordinate QGCMapPolygon::vertexCoordinate(int vertex) const
         return QGeoCoordinate();
     }
 }
+
+QList<QPointF> QGCMapPolygon::nedPolygon(void)
+{
+    QList<QPointF>  nedPolygon;
+
+    if (count() > 0) {
+        QGeoCoordinate  tangentOrigin = vertexCoordinate(0);
+
+        for (int i=0; i<_polygonModel.count(); i++) {
+            double y, x, down;
+            QGeoCoordinate vertex = vertexCoordinate(i);
+            if (i == 0) {
+                // This avoids a nan calculation that comes out of convertGeoToNed
+                x = y = 0;
+            } else {
+                convertGeoToNed(vertex, tangentOrigin, &y, &x, &down);
+            }
+            nedPolygon += QPointF(x, y);
+        }
+    }
+
+    return nedPolygon;
+}
+
+
+void QGCMapPolygon::offset(double distance)
+{
+    QList<QGeoCoordinate> rgNewPolygon;
+
+    // I'm sure there is some beautiful famous algorithm to do this, but here is a brute force method
+
+    if (count() > 2) {
+        // Convert the polygon to NED
+        QList<QPointF> rgNedVertices = nedPolygon();
+
+        // Walk the edges, offsetting by the specified distance
+        QList<QLineF> rgOffsetEdges;
+        for (int i=0; i<rgNedVertices.count(); i++) {
+            int     lastIndex = i == rgNedVertices.count() - 1 ? 0 : i + 1;
+            QLineF  offsetEdge;
+            QLineF  originalEdge(rgNedVertices[i], rgNedVertices[lastIndex]);
+
+            QLineF workerLine = originalEdge;
+            workerLine.setLength(distance);
+            workerLine.setAngle(workerLine.angle() - 90.0);
+            offsetEdge.setP1(workerLine.p2());
+
+            workerLine.setPoints(originalEdge.p2(), originalEdge.p1());
+            workerLine.setLength(distance);
+            workerLine.setAngle(workerLine.angle() + 90.0);
+            offsetEdge.setP2(workerLine.p2());
+
+            rgOffsetEdges.append(offsetEdge);
+        }
+
+        // Intersect the offset edges to generate new vertices
+        QPointF         newVertex;
+        QGeoCoordinate  tangentOrigin = vertexCoordinate(0);
+        for (int i=0; i<rgOffsetEdges.count(); i++) {
+            int prevIndex = i == 0 ? rgOffsetEdges.count() - 1 : i - 1;
+            if (rgOffsetEdges[prevIndex].intersect(rgOffsetEdges[i], &newVertex) == QLineF::NoIntersection) {
+                // FIXME: Better error handling?
+                qWarning("Intersection failed");
+                return;
+            }
+            QGeoCoordinate coord;
+            convertNedToGeo(newVertex.y(), newVertex.x(), 0, tangentOrigin, &coord);
+            rgNewPolygon.append(coord);
+        }
+    }
+
+    // Update internals
+    clear();
+    for (int i=0; i<rgNewPolygon.count(); i++) {
+        appendVertex(rgNewPolygon[i]);
+    }
+}
diff --git a/src/MissionManager/QGCMapPolygon.h b/src/MissionManager/QGCMapPolygon.h
index e9abaa2ea..8eaddee49 100644
--- a/src/MissionManager/QGCMapPolygon.h
+++ b/src/MissionManager/QGCMapPolygon.h
@@ -52,6 +52,9 @@ public:
     /// Returns true if the specified coordinate is within the polygon
     Q_INVOKABLE bool containsCoordinate(const QGeoCoordinate& coordinate) const;
 
+    /// Offsets the current polygon edges by the specified distance in meters
+    Q_INVOKABLE void offset(double distance);
+
     /// Returns the path in a list of QGeoCoordinate's format
     QList<QGeoCoordinate> coordinateList(void) const;
 
@@ -69,6 +72,9 @@ public:
     /// @return true: success, false: failure (errorString set)
     bool loadFromJson(const QJsonObject& json, bool required, QString& errorString);
 
+    /// Convert polygon to NED and return (D is ignored)
+    QList<QPointF> nedPolygon(void);
+
     // Property methods
 
     int             count       (void) const { return _polygonPath.count(); }
-- 
2.22.0