From 9ae5cc7e15b4c474939878cdc709f791ad5c6792 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 20 Aug 2018 07:45:59 +0200 Subject: [PATCH] [Survey transects generation] towards concave polygons --- src/MissionManager/SurveyComplexItem.cc | 165 ++++++++++++++++++++---- src/MissionManager/SurveyComplexItem.h | 5 +- 2 files changed, 142 insertions(+), 28 deletions(-) diff --git a/src/MissionManager/SurveyComplexItem.cc b/src/MissionManager/SurveyComplexItem.cc index 36e886a53..f65ef9045 100644 --- a/src/MissionManager/SurveyComplexItem.cc +++ b/src/MissionManager/SurveyComplexItem.cc @@ -20,6 +20,7 @@ #include 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 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& decomposedPolygons) { - qCDebug(SurveyComplexItemLog) << "_PolygonDecomposeConvex polygon.size() " << polygon.size(); +// qCDebug(SurveyComplexItemLog) << "_PolygonDecomposeConvex polygon.size() " << polygon.size(); + int decompSize = std::numeric_limits::max(); if (polygon.size() < 3) return; + if (polygon.size() == 3) { + decomposedPolygons << polygon; +// qCDebug(PolygonDecomposeLog) << polygon << " polygon of 3"; + return; + } - int decompSize = std::numeric_limits::max(); QList 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::max(); + continue; + } // recursion QList polyLeftDecomposed{}; +// qCDebug(PolygonDecomposeLog) << " polyLeft "<< polyLeft; _PolygonDecomposeConvex(polyLeft, polyLeftDecomposed); + QList 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::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> transects; - for (const QLineF& line: resultLines) { + + if (transitionPoint != nullptr) { + QList 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 transect; + QGeoCoordinate coord; convertNedToGeo(line.p1().y(), line.p1().x(), 0, tangentOrigin, &coord); transect.append(coord); diff --git a/src/MissionManager/SurveyComplexItem.h b/src/MissionManager/SurveyComplexItem.h index f1541fc56..017322660 100644 --- a/src/MissionManager/SurveyComplexItem.h +++ b/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& 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 _metaDataMap; -- 2.22.0