From 5d00376619d78b754924cc068ca21d2babd8a28e Mon Sep 17 00:00:00 2001
From: Don Gagne <Don@laptop-3.local>
Date: Mon, 8 May 2017 18:49:59 -0700
Subject: [PATCH] Survey area is always convex polygon

---
 src/MissionManager/SurveyMissionItem.cc | 80 ++++++++++++++++++++++---
 src/MissionManager/SurveyMissionItem.h  |  4 ++
 2 files changed, 77 insertions(+), 7 deletions(-)

diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc
index 54bab4591..25f05da87 100644
--- a/src/MissionManager/SurveyMissionItem.cc
+++ b/src/MissionManager/SurveyMissionItem.cc
@@ -437,7 +437,7 @@ void SurveyMissionItem::_convertTransectToGeo(const QList<QList<QPointF>>& trans
         for (int j=0; j<transectPoints.count(); j++) {
             QGeoCoordinate coord;
             const QPointF& point = transectPoints[j];
-            convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &coord);
+            convertNedToGeo(point.y(), point.x(), 0, tangentOrigin, &coord);
             transectCoords.append(coord);
         }
         transectSegmentsGeo.append(transectCoords);
@@ -497,6 +497,70 @@ void SurveyMissionItem::_appendGridPointsFromTransects(QList<QList<QGeoCoordinat
     }
 }
 
+qreal SurveyMissionItem::_ccw(QPointF pt1, QPointF pt2, QPointF pt3)
+{
+    return (pt2.x()-pt1.x())*(pt3.y()-pt1.y()) - (pt2.y()-pt1.y())*(pt3.x()-pt1.x());
+}
+
+qreal SurveyMissionItem::_dp(QPointF pt1, QPointF pt2)
+{
+    return (pt2.x()-pt1.x())/qSqrt((pt2.x()-pt1.x())*(pt2.x()-pt1.x()) + (pt2.y()-pt1.y())*(pt2.y()-pt1.y()));
+}
+
+void SurveyMissionItem::_swapPoints(QList<QPointF>& points, int index1, int index2)
+{
+    QPointF temp = points[index1];
+    points[index1] = points[index2];
+    points[index2] = temp;
+}
+
+QList<QPointF> SurveyMissionItem::_convexPolygon(const QList<QPointF>& polygon)
+{
+    // We use the Graham scan algorithem to convert the possibly concave polygon to a convex polygon
+    // https://en.wikipedia.org/wiki/Graham_scan
+
+    QList<QPointF> workPolygon(polygon);
+
+    // First point must be lowest y-coordinate point
+    for (int i=1; i<workPolygon.count(); i++) {
+        if (workPolygon[i].y() < workPolygon[0].y()) {
+            _swapPoints(workPolygon, i, 0);
+        }
+    }
+
+    // Sort the points by angle with first point
+    for (int i=1; i<workPolygon.count(); i++) {
+        qreal angle = _dp(workPolygon[0], workPolygon[i]);
+        for (int j=i+1; j<workPolygon.count(); j++) {
+            if (_dp(workPolygon[0], workPolygon[j]) > angle) {
+                _swapPoints(workPolygon, i, j);
+                angle = _dp(workPolygon[0], workPolygon[j]);
+            }
+        }
+    }
+
+    // Perform the the Graham scan
+
+    workPolygon.insert(0, workPolygon.last());  // Sentinel for algo stop
+    int convexCount = 1;                        // Number of points on the convex hull.
+
+    for (int i=2; i<=polygon.count(); i++) {
+        while (_ccw(workPolygon[convexCount-1], workPolygon[convexCount], workPolygon[i]) <= 0) {
+            if (convexCount > 1) {
+                convexCount -= 1;
+            } else if (i == polygon.count()) {
+                break;
+            } else {
+                i++;
+            }
+        }
+        convexCount++;
+        _swapPoints(workPolygon, convexCount, i);
+    }
+
+    return workPolygon.mid(1, convexCount);
+}
+
 void SurveyMissionItem::_generateGrid(void)
 {
     if (_ignoreRecalc) {
@@ -515,17 +579,19 @@ void SurveyMissionItem::_generateGrid(void)
     QList<QPointF>          polygonPoints;
     QList<QList<QPointF>>   transectSegments;
 
-    // Convert polygon to Qt coordinate system (y positive is down)
+    // Convert polygon to NED
     qCDebug(SurveyMissionItemLog) << "Convert polygon";
     QGeoCoordinate tangentOrigin = _mapPolygon.path()[0].value<QGeoCoordinate>();
     for (int i=0; i<_mapPolygon.count(); i++) {
         double y, x, down;
         QGeoCoordinate vertex = _mapPolygon.pathModel().value<QGCQGeoCoordinate*>(i)->coordinate();
         convertGeoToNed(vertex, tangentOrigin, &y, &x, &down);
-        polygonPoints += QPointF(x, -y);
+        polygonPoints += QPointF(x, y);
         qCDebug(SurveyMissionItemLog) << vertex << polygonPoints.last().x() << polygonPoints.last().y();
     }
 
+    polygonPoints = _convexPolygon(polygonPoints);
+
     double coveredArea = 0.0;
     for (int i=0; i<polygonPoints.count(); i++) {
         if (i != 0) {
@@ -745,10 +811,10 @@ int SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints,  QLis
 
     // Rotate the bounding rect around it's center to generate the larger bounding rect
     QPolygonF boundPolygon;
-    boundPolygon << _rotatePoint(smallBoundRect.topLeft(),       center, gridAngle);
-    boundPolygon << _rotatePoint(smallBoundRect.topRight(),      center, gridAngle);
-    boundPolygon << _rotatePoint(smallBoundRect.bottomRight(),   center, gridAngle);
-    boundPolygon << _rotatePoint(smallBoundRect.bottomLeft(),    center, gridAngle);
+    boundPolygon << _rotatePoint(smallBoundRect.topLeft(),      center, gridAngle);
+    boundPolygon << _rotatePoint(smallBoundRect.topRight(),     center, gridAngle);
+    boundPolygon << _rotatePoint(smallBoundRect.bottomRight(),  center, gridAngle);
+    boundPolygon << _rotatePoint(smallBoundRect.bottomLeft(),   center, gridAngle);
     boundPolygon << boundPolygon[0];
     QRectF largeBoundRect = boundPolygon.boundingRect();
     qCDebug(SurveyMissionItemLog) << "Rotated bounding rect" << largeBoundRect.topLeft().x() << largeBoundRect.topLeft().y() << largeBoundRect.bottomRight().x() << largeBoundRect.bottomRight().y();
diff --git a/src/MissionManager/SurveyMissionItem.h b/src/MissionManager/SurveyMissionItem.h
index 77ce7c73f..3d50d3bbe 100644
--- a/src/MissionManager/SurveyMissionItem.h
+++ b/src/MissionManager/SurveyMissionItem.h
@@ -199,6 +199,10 @@ private:
     bool _appendMissionItemsWorker(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly);
     void _optimizeReflySegments(void);
     void _appendGridPointsFromTransects(QList<QList<QGeoCoordinate>>& rgTransectSegments);
+    qreal _ccw(QPointF pt1, QPointF pt2, QPointF pt3);
+    qreal _dp(QPointF pt1, QPointF pt2);
+    void _swapPoints(QList<QPointF>& points, int index1, int index2);
+    QList<QPointF> _convexPolygon(const QList<QPointF>& polygon);
 
     int                             _sequenceNumber;
     bool                            _dirty;
-- 
2.22.0