[Survey transects generation] towards concave polygons

src/MissionManager/SurveyComplexItem.cc

@@ -20,6 +20,7 @@
 #include <QPolygonF>
 
 QGC_LOGGING_CATEGORY(SurveyComplexItemLog, "SurveyComplexItemLog")
+QGC_LOGGING_CATEGORY(PolygonDecomposeLog, "PolygonDecomposeLog")
 
 const char* SurveyComplexItem::jsonComplexItemTypeValue =   "survey";
 const char* SurveyComplexItem::jsonV3ComplexItemTypeValue = "survey";
@@ -1116,78 +1117,132 @@ void SurveyComplexItem::_rebuildTransectsPhase1Worker(bool refly)
 
     // Create list of separate polygons
     QList<QPolygonF> polygons{};
+    qCDebug(PolygonDecomposeLog) << "*********_PolygonDecomposeConvex begin of recursion**************";
     _PolygonDecomposeConvex(polygon, polygons);
+    qCDebug(PolygonDecomposeLog) << "polygons.size() " << polygons.size() ;
 
     // iterate over polygons
-    for (auto& p : polygons) {
+    for (auto p = polygons.begin(); p != polygons.end(); ++p) {
+        QPointF* vMatch = nullptr;
+        // find matching vertex in previous polygon
+        if (p != polygons.begin()) {
+            auto pLast = p - 1;
+            for (auto& i : *p) {
+                for (auto& j : *pLast) {
+                   if (i == j) {
+                       vMatch = &i;
+                       break;
+                   }
+                   if (vMatch) break;
+                }
+            }
+            if (nullptr == vMatch) qCDebug(PolygonDecomposeLog) << "no match found";
+
+        }
+
+
         // close polygon
-        p << p.front();
+        *p << p->front();
         // build transects for this polygon
         // TODO figure out tangent origin
-        qCDebug(SurveyComplexItemLog) << "Transects from polynom p " << p;
-        _rebuildTranscetsFromPolygon(refly, p, tangentOrigin);
+        // TODO improve selection of entry points
+//        qCDebug(SurveyComplexItemLog) << "Transects from polynom p " << p;
+        _rebuildTranscetsFromPolygon(refly, *p, tangentOrigin, vMatch);
     }
 }
 
 void SurveyComplexItem::_PolygonDecomposeConvex(const QPolygonF& polygon, QList<QPolygonF>& decomposedPolygons)
 {
-    qCDebug(SurveyComplexItemLog) << "_PolygonDecomposeConvex polygon.size() " << polygon.size();
+//    qCDebug(SurveyComplexItemLog) << "_PolygonDecomposeConvex polygon.size() " << polygon.size();
+    int decompSize = std::numeric_limits<int>::max();
     if (polygon.size() < 3) return;
+    if (polygon.size() == 3) {
+        decomposedPolygons << polygon;
+//        qCDebug(PolygonDecomposeLog) << polygon << " polygon of 3";
+        return;
+    }
 
-    int decompSize = std::numeric_limits<int>::max();
     QList<QPolygonF> decomposedPolygonsMin{};
 
     for (auto vertex = polygon.begin(); vertex != polygon.end(); ++vertex)
     {
         // is vertex reflex?
-        auto vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
-        auto vertexAfter = vertex == polygon.end() -1 ? polygon.begin() : vertex + 1;
-        auto area = (((vertex->x() - vertexBefore->x())*(vertexAfter->y() - vertexBefore->y()))-((vertexAfter->x() - vertexBefore->x())*(vertex->y() - vertexBefore->y())));
-        bool vertexIsReflex = area > 0;
-        qCDebug(SurveyComplexItemLog) << "area " << area << " vertexIsReflex " << vertexIsReflex;
+        bool vertexIsReflex = _VertexIsReflex(polygon, vertex);
+//        qCDebug(SurveyComplexItemLog) << "area " << area << " vertexIsReflex " << vertexIsReflex;
 
         if (!vertexIsReflex) continue;
 
         for (auto vertexOther = polygon.begin(); vertexOther != polygon.end(); ++vertexOther)
         {
+            auto vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
+            auto vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
             if (vertexOther == vertex) continue;
+            if (vertexAfter == vertexOther) continue;
+            if (vertexBefore == vertexOther) continue;
             bool canSee = _VertexCanSeeOther(polygon, vertex, vertexOther);
+//            qCDebug(SurveyComplexItemLog) << "canSee " << canSee;
             if (!canSee) continue;
 
             QPolygonF polyLeft;
             auto v = vertex;
+            auto polyLeftContainsReflex = false;
             while ( v != vertexOther) {
+                if (v != vertex && _VertexIsReflex(polygon, v)) {
+                    polyLeftContainsReflex = true;
+                }
                 polyLeft << *v;
                 ++v;
                 if (v == polygon.end()) v = polygon.begin();
             }
             polyLeft << *vertexOther;
-            qCDebug(SurveyComplexItemLog) << "polyLeft.size() " << polyLeft.size();
+            auto polyLeftValid = !(polyLeftContainsReflex && polyLeft.size() == 3);
 
             QPolygonF polyRight;
             v = vertexOther;
+            auto polyRightContainsReflex = false;
             while ( v != vertex) {
+                if (v != vertex && _VertexIsReflex(polygon, v)) {
+                    polyRightContainsReflex = true;
+                }
                 polyRight << *v;
                 ++v;
                 if (v == polygon.end()) v = polygon.begin();
             }
             polyRight << *vertex;
-            qCDebug(SurveyComplexItemLog) << "polyRight.size() " << polyRight.size();
+            auto polyRightValid = !(polyRightContainsReflex && polyRight.size() == 3);
+
+            if (!polyLeftValid || ! polyRightValid) {
+//                decompSize = std::numeric_limits<int>::max();
+                continue;
+            }
 
             // recursion
             QList<QPolygonF> polyLeftDecomposed{};
+//            qCDebug(PolygonDecomposeLog) << " polyLeft "<< polyLeft;
             _PolygonDecomposeConvex(polyLeft, polyLeftDecomposed);
+
             QList<QPolygonF> polyRightDecomposed{};
+//            qCDebug(PolygonDecomposeLog) << " polyRight "<< polyRight;
             _PolygonDecomposeConvex(polyRight, polyRightDecomposed);
 
             // compositon
-            if (polyLeftDecomposed.size() + polyRightDecomposed.size() < decompSize) {
-                decompSize = polyLeftDecomposed.size() + polyRightDecomposed.size();
+            auto subSize = polyLeftDecomposed.size() + polyRightDecomposed.size();
+            if ((polyLeftContainsReflex && polyLeftDecomposed.size() == 1)
+                    || (polyRightContainsReflex && polyRightDecomposed.size() == 1))
+            {
+                // don't accept polygons that contian reflex vertices and were not split
+                subSize = std::numeric_limits<int>::max();
+            }
+            if (subSize < decompSize) {
+                decompSize = subSize;
                 decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed;
-                qCDebug(SurveyComplexItemLog) << "changing decomposedPolygonsMin";
+//            qCDebug(PolygonDecomposeLog) << "_PolygonDecomposeConvex polygon " << polygon;
+//            qCDebug(PolygonDecomposeLog) << "polyLeft.size() " << polyLeft.size() << " polyRight.size() " << polyRight.size() << " out of " << polygon.size();
+//            qCDebug(PolygonDecomposeLog) << "vertex " << *vertex << " vertexOther " << *vertexOther << " vertexAfter " << *vertexAfter << " vertexBefore " << *vertexBefore;
+//            qCDebug(SurveyComplexItemLog) << "changing decomposedPolygonsMin";
             }
             else {
-                qCDebug(SurveyComplexItemLog) << "NOT changing decomposedPolygonsMin";
+//                qCDebug(SurveyComplexItemLog) << "NOT changing decomposedPolygonsMin";
             }
         }
 
@@ -1195,23 +1250,71 @@ void SurveyComplexItem::_PolygonDecomposeConvex(const QPolygonF& polygon, QList<
 
     // assemble output
     if (decomposedPolygonsMin.size() > 0) {
-        qCDebug(SurveyComplexItemLog) << "use decomposed polygon, decomposedPolygonsMin.size() " << decomposedPolygonsMin.size();
+//        qCDebug(SurveyComplexItemLog) << "use decomposed polygon, decomposedPolygonsMin.size() " << decomposedPolygonsMin.size();
         decomposedPolygons << decomposedPolygonsMin;
+//        qCDebug(PolygonDecomposeLog) << decomposedPolygonsMin;
     } else {
-        qCDebug(SurveyComplexItemLog) << "use default polygon";
+//        qCDebug(SurveyComplexItemLog) << "use default polygon";
         decomposedPolygons << polygon;
+//        qCDebug(PolygonDecomposeLog) << polygon << " empty polygon";
     }
+
+    return;
 }
 
-bool SurveyComplexItem::_VertexCanSeeOther(const QPolygonF& polygon, const QPointF* VertexA, const QPointF* VertexB) {
-    if (VertexA == VertexB) return false;
-    if (VertexA + 1 == VertexB) return false;
-    if (VertexA - 1 == VertexB) return false;
+bool SurveyComplexItem::_VertexCanSeeOther(const QPolygonF& polygon, const QPointF* vertexA, const QPointF* vertexB) {
+    if (vertexA == vertexB) return false;
+    auto vertexAAfter = vertexA + 1 == polygon.end() ? polygon.begin() : vertexA + 1;
+    auto vertexABefore = vertexA == polygon.begin() ? polygon.end() - 1 : vertexA - 1;
+    if (vertexAAfter == vertexB) return false;
+    if (vertexABefore == vertexB) return false;
+//    qCDebug(SurveyComplexItemLog) << "_VertexCanSeeOther false after first checks ";
+
+    bool visible = true;
+//    auto diff = *vertexA - *vertexB;
+    QLineF lineAB{*vertexA, *vertexB};
+    auto distanceAB = lineAB.length();//sqrtf(diff.x() * diff.x() + diff.y()*diff.y());
+
+//    qCDebug(SurveyComplexItemLog) << "_VertexCanSeeOther distanceAB " << distanceAB;
+    for (auto vertexC = polygon.begin(); vertexC != polygon.end(); ++vertexC)
+    {
+        if (vertexC == vertexA) continue;
+        if (vertexC == vertexB) continue;
+        auto vertexD = vertexC + 1 == polygon.end() ? polygon.begin() : vertexC + 1;
+        if (vertexD == vertexA) continue;
+        if (vertexD == vertexB) continue;
+        QLineF lineCD(*vertexC, *vertexD);
+        QPointF intersection{};
+        auto intersects = lineAB.intersect(lineCD, &intersection);
+        if (intersects == QLineF::IntersectType::BoundedIntersection) {
+//            auto diffIntersection = *vertexA - intersection;
+//            auto distanceIntersection = sqrtf(diffIntersection.x() * diffIntersection.x() + diffIntersection.y()*diffIntersection.y());
+//            qCDebug(SurveyComplexItemLog) << "*vertexA " << *vertexA << "*vertexB " << *vertexB  << " intersection " << intersection;
+
+            QLineF lineIntersection{*vertexA, intersection};
+            auto distanceIntersection = lineIntersection.length();//sqrtf(diff.x() * diff.x() + diff.y()*diff.y());
+            qCDebug(SurveyComplexItemLog) << "_VertexCanSeeOther distanceIntersection " << distanceIntersection;
+            if (distanceIntersection < distanceAB) {
+                visible = false;
+                break;
+            }
+        }
 
-    return true;
+    }
+
+    return visible;
 }
 
-void SurveyComplexItem::_rebuildTranscetsFromPolygon(bool refly, const QPolygonF& polygon, const QGeoCoordinate& tangentOrigin)
+bool SurveyComplexItem::_VertexIsReflex(const QPolygonF& polygon, const QPointF* vertex) {
+    auto vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
+    auto vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
+    auto area = (((vertex->x() - vertexBefore->x())*(vertexAfter->y() - vertexBefore->y()))-((vertexAfter->x() - vertexBefore->x())*(vertex->y() - vertexBefore->y())));
+    return area > 0;
+
+}
+
+
+void SurveyComplexItem::_rebuildTranscetsFromPolygon(bool refly, const QPolygonF& polygon, const QGeoCoordinate& tangentOrigin, const QPointF* const transitionPoint)
 {
     // Generate transects
 
@@ -1282,9 +1385,19 @@ void SurveyComplexItem::_rebuildTranscetsFromPolygon(bool refly, const QPolygonF
 
     // Convert from NED to Geo
     QList<QList<QGeoCoordinate>> transects;
-    for (const QLineF& line: resultLines) {
+
+    if (transitionPoint != nullptr) {
+        QList<QGeoCoordinate>   transect;
         QGeoCoordinate          coord;
+        convertNedToGeo(transitionPoint->y(), transitionPoint->x(), 0, tangentOrigin, &coord);
+        transect.append(coord);
+        transect.append(coord); //TODO
+        transects.append(transect);
+    }
+
+    for (const QLineF& line: resultLines) {
         QList<QGeoCoordinate>   transect;
+        QGeoCoordinate          coord;
 
         convertNedToGeo(line.p1().y(), line.p1().x(), 0, tangentOrigin, &coord);
         transect.append(coord);

src/MissionManager/SurveyComplexItem.h

@@ -112,11 +112,12 @@ private:
     bool _loadV4(const QJsonObject& complexObject, int sequenceNumber, QString& errorString);
     void _rebuildTransectsPhase1Worker(bool refly);
     /// Adds to the _transects array from one polygon
-    void _rebuildTranscetsFromPolygon(bool refly, const QPolygonF& polygon, const QGeoCoordinate& tangentOrigin);
+    void _rebuildTranscetsFromPolygon(bool refly, const QPolygonF& polygon, const QGeoCoordinate& tangentOrigin, const QPointF* const transitionPoint);
     // Decompose polygon into list of convex sub polygons
     void _PolygonDecomposeConvex(const QPolygonF& polygon, QList<QPolygonF>& decomposedPolygons);
     // return true if vertex a can see vertex b
-    bool _VertexCanSeeOther(const QPolygonF& polygon, const QPointF* VertexA, const QPointF* VertexB);
+    bool _VertexCanSeeOther(const QPolygonF& polygon, const QPointF* vertexA, const QPointF* vertexB);
+    bool _VertexIsReflex(const QPolygonF& polygon, const QPointF* vertex);
 
     QMap<QString, FactMetaData*> _metaDataMap;