From 426293a7bc3ddeb83272b053e0337a1e1acb7716 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Wed, 17 Jul 2019 21:02:01 +0200 Subject: [PATCH] debugging, containsPoint in dijkstra not working --- qgroundcontrol.pro | 4 +- src/Wima/CircularSurveyComplexItem.cc | 4 +- src/Wima/GeoUtilities.cc | 135 +++++--- src/Wima/GeoUtilities.h | 25 +- src/Wima/OptimisationTools.cc | 7 +- src/Wima/OptimisationTools.h | 11 +- src/Wima/PlanimetryCalculus.cc | 29 +- src/Wima/PlanimetryCalculus.h | 1 + src/Wima/PolygonCalculus.cc | 51 ++- src/Wima/PolygonCalculus.h | 21 +- src/Wima/WimaArea.cc | 444 +++----------------------- src/Wima/WimaArea.h | 16 +- src/Wima/WimaPlaner.cc | 37 ++- src/Wima/WimaPlaner.h | 5 + 14 files changed, 280 insertions(+), 510 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 90354514d..1c21f5070 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -457,9 +457,9 @@ SOURCES += \ src/Wima/CircularSurveyComplexItem.cc \ src/Wima/PlanimetryCalculus.cc \ src/Wima/Circle.cc \ - src/Wima/PolygonCalculus.cc \ src/Wima/OptimisationTools.cc \ - src/Wima/GeoUtilities.cc + src/Wima/GeoUtilities.cc \ + src/Wima/PolygonCalculus.cc # # Unit Test specific configuration goes here (requires full debug build with all plugins) diff --git a/src/Wima/CircularSurveyComplexItem.cc b/src/Wima/CircularSurveyComplexItem.cc index 6222e94f3..2b2a3c2a8 100644 --- a/src/Wima/CircularSurveyComplexItem.cc +++ b/src/Wima/CircularSurveyComplexItem.cc @@ -11,12 +11,12 @@ CircularSurveyComplexItem::CircularSurveyComplexItem(Vehicle *vehicle, bool flyV bool CircularSurveyComplexItem::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString) { - return TransectStyleComplexItem::load(complexObject, sequenceNumber, errorString); + return false; } void CircularSurveyComplexItem::save(QJsonArray &planItems) { - TransectStyleComplexItem::save(planItems); + } void CircularSurveyComplexItem::appendMissionItems(QList &items, QObject *missionItemParent) diff --git a/src/Wima/GeoUtilities.cc b/src/Wima/GeoUtilities.cc index 0984e2fdd..d2480b670 100644 --- a/src/Wima/GeoUtilities.cc +++ b/src/Wima/GeoUtilities.cc @@ -1,62 +1,105 @@ #include "GeoUtilities.h" -#include -QGeoCoordinate GeoUtilites::toGeo(const QVector3D &point, const QGeoCoordinate &origin) -{ - double x = point.x(); - double y = point.y(); - double z = point.z(); +namespace GeoUtilities { - double lat = origin.latitude()/180*M_PI; - double lon = origin.longitude()/180*M_PI; - double h = origin.altitude(); + QGeoCoordinate toGeo(const QVector3D &point, const QGeoCoordinate &origin) + { + using namespace PolygonCalculus; + double z = point.z(); + double h = origin.altitude(); - if (!qFuzzyCompare(lat, M_PI_2)) // this could (unlikely) be a problem, replace with different coordinate transformation - return QGeoCoordinate(/* lat */ qAtan(x/cos(lat)/earthRadius) + lon, - /* lon */ qAtan(y/earthRadius) + lat, - /* alt */ h + z); - else - return QGeoCoordinate(); // singularity occurred (1/cos(pi/2) = inf) -} + QGeoCoordinate coordinate(toGeo(point.toPointF(), origin)); + coordinate.setAltitude(z + h); + return coordinate; -QVector3D GeoUtilites::toCartesian(const QGeoCoordinate &point, const QGeoCoordinate &origin) -{ - double lat = point.latitude()/180*M_PI; - double lon = point.longitude()/180*M_PI; - double h = point.altitude(); + } - double latO = origin.latitude()/180*M_PI; - double lonO = origin.longitude()/180*M_PI; - double hO = origin.altitude(); + QVector3D toCartesian(const QGeoCoordinate &coordinate, const QGeoCoordinate &origin) + { - double dlon = lon-lonO; - double dlat = lat-latO; + double h = coordinate.altitude(); + double hO = origin.altitude(); + QVector3D point(toCartesian2D(coordinate, origin)); + point.setZ(h - hO); - if (!qFuzzyCompare(dlon, M_PI_2) && !qFuzzyCompare(dlat, M_PI_2)) - return QVector3D(/* x */ qTan(dlon)*earthRadius*qCos(latO), - /* y */ qTan(dlat)*earthRadius, - /* z */ h - hO); - else - return QVector3D(); // singularity occurred (tan(pi/2) = inf) + return point; + } -} + QGeoList toGeo(const QVector3DList &points, const QGeoCoordinate &origin) + { + QGeoList coordinates; + for (auto point : points) + coordinates.append(toGeo(point, origin)); -GeoUtilites::QGeoList GeoUtilites::toGeo(const GeoUtilites::QVector3DFList &points, const QGeoCoordinate &origin) -{ - GeoUtilites::QGeoList coordinates; - for (auto point : points) - coordinates.append(toGeo(point, origin)); + return coordinates; + } - return coordinates; -} + QVector3DList toCartesian(const QGeoList &coordinates, const QGeoCoordinate &origin) + { + QVector3DList points; + for (auto coordinate : coordinates ) + points.append(toCartesian(coordinate, origin)); + + return points; + } + + QPointF toCartesian2D(const QGeoCoordinate &point, const QGeoCoordinate &origin) + { + double lat = point.latitude()/180*M_PI; + double lon = point.longitude()/180*M_PI; + + double latO = origin.latitude()/180*M_PI; + double lonO = origin.longitude()/180*M_PI; + + double dlon = lon-lonO; + double dlat = lat-latO; + + + if (!qFuzzyCompare(dlon, M_PI_2) && !qFuzzyCompare(dlat, M_PI_2)) + return QPointF(/* x */ qTan(dlon)*earthRadius*qCos(latO), + /* y */ qTan(dlat)*earthRadius); + else + return QPointF(); // singularity occurred (tan(pi/2) = inf) + } -GeoUtilites::QVector3DFList GeoUtilites::toCartesian(const GeoUtilites::QGeoList &coordinates, const QGeoCoordinate &origin) -{ - GeoUtilites::QVector3DFList points; - for (auto coordinate : coordinates ) - points.append(toCartesian(coordinate, origin)); + QPointFList toCartesian2D(const QGeoList &coordinates, const QGeoCoordinate &origin) + { + QPointFList listF; + + for ( auto coordinate : coordinates) + listF.append(toCartesian2D(coordinate, origin)); + + return listF; + } + + QGeoCoordinate toGeo(const QPointF &point, const QGeoCoordinate &origin) + { + double x = point.x(); + double y = point.y(); + + double lat = origin.latitude(); + double lon = origin.longitude(); + + //qWarning("%lf %lf %lf %lf", x, y, lat, lon); + + if (!qFuzzyCompare(lat, M_PI_2)) // this could (unlikely) be a problem, replace with different coordinate transformation + return QGeoCoordinate(/* lat */ qAtan(y/earthRadius)*180/M_PI + lat, + /* lon */ qAtan(x/cos(lat/180*M_PI)/earthRadius)*180/M_PI + lon, + /* alt */ origin.altitude()); + else + return QGeoCoordinate(); // singularity occurred (1/cos(pi/2) = inf) + } + + QGeoList toGeo(const QPointFList &points, const QGeoCoordinate &origin) + { + QGeoList coordinates; + for ( auto point : points) + coordinates.append(toGeo(point, origin)); + + return coordinates; + } - return points; } + diff --git a/src/Wima/GeoUtilities.h b/src/Wima/GeoUtilities.h index 14b9eb28d..aff98f031 100644 --- a/src/Wima/GeoUtilities.h +++ b/src/Wima/GeoUtilities.h @@ -1,22 +1,29 @@ -#ifndef GEOPOLYGONUTILITIES_H -#define GEOPOLYGONUTILITIES_H +#pragma once #include #include #include #include +#include +#include "PolygonCalculus.h" -namespace GeoUtilites { - typedef QList QVector3DFList; +namespace GeoUtilities { + + typedef QList QVector3DList; + typedef QList QPointFList; typedef QList QGeoList; const double earthRadius = 6378137; // meter - QGeoCoordinate toGeo (const QVector3D &point, const QGeoCoordinate &origin); - QGeoList toGeo (const QVector3DFList &points, const QGeoCoordinate &origin); - QVector3D toCartesian (const QGeoCoordinate &point, const QGeoCoordinate &origin); - QVector3DFList toCartesian (const QGeoList &coordinates, const QGeoCoordinate &origin); + QGeoCoordinate toGeo (const QVector3D &point, const QGeoCoordinate &origin); + QGeoList toGeo (const QVector3DList &points, const QGeoCoordinate &origin); + QGeoCoordinate toGeo (const QPointF &point, const QGeoCoordinate &origin); + QGeoList toGeo (const QPointFList &points, const QGeoCoordinate &origin); + QVector3D toCartesian (const QGeoCoordinate &coordinate, const QGeoCoordinate &origin); + QVector3DList toCartesian (const QGeoList &coordinates, const QGeoCoordinate &origin); + QPointF toCartesian2D (const QGeoCoordinate &point, const QGeoCoordinate &origin); + QPointFList toCartesian2D (const QGeoList &coordinates, const QGeoCoordinate &origin); } -#endif // GEOPOLYGONUTILITIES_H + diff --git a/src/Wima/OptimisationTools.cc b/src/Wima/OptimisationTools.cc index 9b7448934..63c83186f 100644 --- a/src/Wima/OptimisationTools.cc +++ b/src/Wima/OptimisationTools.cc @@ -1,4 +1,5 @@ #include "OptimisationTools.h" +#include namespace OptimisationTools { namespace { @@ -15,15 +16,15 @@ namespace OptimisationTools { * * \sa QList */ - template - bool dijkstraAlgorithm(const QList elements, int startIndex, int endIndex, QList &elementPath, std::function distance) // don't seperate parameters with new lines or documentation will break + typedef QPointF T; + bool dijkstraAlgorithm(const QList &elements, int startIndex, int endIndex, QList &elementPath, std::function distance) // don't seperate parameters with new lines or documentation will break { if ( elements.isEmpty() || startIndex < 0 || startIndex >= elements.size() || endIndex < 0 || endIndex >= elements.size()) { return false; } - + qWarning("optitools"); // Each element of type T gets stuff into a Node /// @param distance is the distance between the Node and it's predecessor struct Node{ diff --git a/src/Wima/OptimisationTools.h b/src/Wima/OptimisationTools.h index d32d4a58a..c632e7ab5 100644 --- a/src/Wima/OptimisationTools.h +++ b/src/Wima/OptimisationTools.h @@ -1,13 +1,12 @@ -#ifndef OPTIMISATIONTOOLS_H -#define OPTIMISATIONTOOLS_H - +#pragma once #include #include +#include namespace OptimisationTools { - template - bool dijkstraAlgorithm(const QList elements, int startIndex, int endIndex, QList &elementPath, std::function distance); + typedef QPointF T; + bool dijkstraAlgorithm(const QList &elements, int startIndex, int endIndex, QList &elementPath, std::function distance); } -#endif // OPTIMISATIONTOOLS_H + diff --git a/src/Wima/PlanimetryCalculus.cc b/src/Wima/PlanimetryCalculus.cc index aa4b9c159..cd79c63b7 100644 --- a/src/Wima/PlanimetryCalculus.cc +++ b/src/Wima/PlanimetryCalculus.cc @@ -12,14 +12,14 @@ namespace PlanimetryCalculus { \sa QPointF, Circle */ - bool intersects(const Circle &circle, const QLineF &line, QPointFList &intersectionPoints, IntersectType type, bool calcInstersect) + bool intersects(const Circle &circle, const QLineF &line, QPointFList &intersectionPoints, IntersectType &type, bool calcInstersect) { if (!circle.isNull() && ! line.isNull()) { QPointF translationVector = line.p1(); - double angleWLDegree = line.angle(); // angle between wold and line coordinate system + double alpha = angle(line); // angle between wold and line coordinate system QPointF originCircleL = circle.origin() - translationVector; - rotateReference(originCircleL, -angleWLDegree); // circle origin in line corrdinate system + rotateReference(originCircleL, -alpha); // circle origin in line corrdinate system double y = originCircleL.y(); double r = circle.radius(); @@ -33,7 +33,7 @@ namespace PlanimetryCalculus { if (x_ori >= 0 && x_ori <= line.length()) { if (calcInstersect) { QPointF intersectionPt = QPointF(x_ori, 0); - rotateReference(intersectionPt, angleWLDegree); + rotateReference(intersectionPt, alpha); intersectionPoints.append(intersectionPt + translationVector); } @@ -54,7 +54,7 @@ namespace PlanimetryCalculus { if (x1 >= 0 && x1 <= line.length()) { // check if intersection point is on the line if (calcInstersect) { QPointF intersectionPt = QPointF(x1, 0); // first intersection point (line system) - rotateReference(intersectionPt, angleWLDegree); + rotateReference(intersectionPt, alpha); intersectionPoints.append(intersectionPt + translationVector); // transform (to world system) and append first intersection point } doesIntersect = true; @@ -62,7 +62,7 @@ namespace PlanimetryCalculus { if (x2 >= 0 && x2 <= line.length()) { // check if intersection point is on the line if (calcInstersect) { QPointF intersectionPt = QPointF(x2, 0); // second intersection point (line system) - rotateReference(intersectionPt, angleWLDegree); + rotateReference(intersectionPt, alpha); intersectionPoints.append(intersectionPt + translationVector); // transform (to world system) and append second intersection point } doesIntersect = true; @@ -97,7 +97,7 @@ namespace PlanimetryCalculus { double R = 0; if (r1 > r2) { R = r1; // large - r = r2; // small + r = r2; // smallline1 } else { // this branch is also choosen if r1 == r2 R = r2; @@ -256,7 +256,7 @@ namespace PlanimetryCalculus { \fn double distance(const QPointF &p1, const QPointF p2) Calculates the angle (in degrees) between the line defined by \a p1 and \a p2 and the x-axis according to the following rule. Angle = qAtan2(dy, dx)*180/pi, where dx = p2.x()-p1.x() and dy = p2.y()-p1.y(). - +angle \note The order of \a p1 and \a p2 matters. Swapping \a p1 and \a p2 will result in a angle of oposite sign. \sa QPointF */ @@ -307,10 +307,10 @@ namespace PlanimetryCalculus { // line 1 coordinate system: origin line1.p1(), x-axis towards line1.p2() QPointF translationVector = line1.p1(); // translation vector between world and line1 system - double alpha = line1.angle(); + double alpha = angle(line1); double l1 = line1.length(); - QLineF line2L1 = line1; + QLineF line2L1 = line2; line2L1.translate(-translationVector); rotateReference(line2L1, -alpha); @@ -350,7 +350,7 @@ namespace PlanimetryCalculus { } if (xNull >= x1 && xNull <= x2){ - // determine intersection type + // determine intersection type#include "QVector3D" if(qFuzzyIsNull(xNull) || qFuzzyCompare(xNull, l1)) { if(qFuzzyIsNull(y1) || qFuzzyIsNull(y2)) @@ -397,7 +397,6 @@ namespace PlanimetryCalculus { { if (polygon.size() >= 2) { - neighbourList.clear(); IntersectList intersectionTypeList; // Assemble a line form each tow consecutive polygon vertices and check whether it intersects with line for (int i = 0; i < polygon.size(); i++) { @@ -542,6 +541,12 @@ namespace PlanimetryCalculus { return intersects(circle1, circle2, intersectionPoints, type, true /*calculate intersection points*/); } + ; + + double angle(const QLineF &line) + { + return angle(line.p1(), line.p2()); + } diff --git a/src/Wima/PlanimetryCalculus.h b/src/Wima/PlanimetryCalculus.h index b0cc4bbcf..ac9319071 100644 --- a/src/Wima/PlanimetryCalculus.h +++ b/src/Wima/PlanimetryCalculus.h @@ -57,6 +57,7 @@ namespace PlanimetryCalculus { double distance(const QPointF &p1, const QPointF p2); double angle(const QPointF &p1, const QPointF p2); + double angle(const QLineF &line); double angleDegree(const QPointF &p1, const QPointF p2); double truncateAngle(double angle); double truncateAngleDegree(double angle); diff --git a/src/Wima/PolygonCalculus.cc b/src/Wima/PolygonCalculus.cc index dc7f4a3dc..f66d833f3 100644 --- a/src/Wima/PolygonCalculus.cc +++ b/src/Wima/PolygonCalculus.cc @@ -1,4 +1,10 @@ #include "PolygonCalculus.h" +#include "PlanimetryCalculus.h" +#include "OptimisationTools.h" + +#include + +#include namespace PolygonCalculus { namespace { @@ -75,7 +81,7 @@ namespace PolygonCalculus { } } - /*! + /*!auto distance * \fn QPointF closestVertex(const QPolygonF &polygon, const QPointF &coordinate); * Returns the vertex of \a polygon with the least distance to \a coordinate. * @@ -137,8 +143,9 @@ namespace PolygonCalculus { * or one area is inside the other. * The algorithm assumes that \a joinedPolygon is empty. */ - JoinPolygonError joinPolygon(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon) + JoinPolygonError join(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon) { + using namespace PolygonCalculus; if (polygon1.size() >= 3 && polygon2.size() >= 3) { if ( !isSimplePolygon(polygon1) || !isSimplePolygon(polygon2)) { @@ -193,7 +200,7 @@ namespace PolygonCalculus { //qDebug("IntersectionList.size(): %i", intersectionList.size()); - if (intersectionList.size() >= 1) { bool containsPath (const QPointF &c1, const QPointF &c2, QPolygonF polygon); + if (intersectionList.size() >= 1) { int minDistIndex = 0; // find the vertex with the least distance to currentVertex @@ -305,7 +312,7 @@ namespace PolygonCalculus { /*! * \fn bool hasClockwiseWinding(const QPolygonF &polygon) - * Returns \c true if \a path has clockwise winding, \c false else. + * Returns \c true if \a path has clockwiauto distancese winding, \c false else. */ bool hasClockwiseWinding(const QPolygonF &polygon) { @@ -351,7 +358,7 @@ namespace PolygonCalculus { QPolygonF newPolygon; if (polygon.size() > 2) { - // Walk the edges, offsetting by the specified distance + // Walk the edges, offsetting by theauto distance specified distance QList rgOffsetEdges; for (int i = 0; i < polygon.size(); i++) { int nextIndex = nextVertexIndex(polygon.size(), i); @@ -469,7 +476,6 @@ namespace PolygonCalculus { decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed; } } - } // assemble output @@ -487,7 +493,7 @@ namespace PolygonCalculus { if ( polygon.containsPoint(startVertex, Qt::FillRule::OddEvenFill) && polygon.containsPoint(endVertex, Qt::FillRule::OddEvenFill)) { // lambda - auto distance = [polygon](const QPointF &p1, const QPointF &p2) -> double { + std::function distance = [polygon](const QPointF &p1, const QPointF &p2) -> double { if (containsPath(polygon, p1, p2)){ double dx = p1.x()-p2.x(); double dy = p1.y()-p2.y(); @@ -502,13 +508,40 @@ namespace PolygonCalculus { for (int i = 0; i < polygon.size(); i++) { elementList.append(polygon[i]); } - - return OptimisationTools::dijkstraAlgorithm(elementList, 0, 1, shortestPath, distance); + qWarning("Hi"); + return OptimisationTools::dijkstraAlgorithm(elementList, 0, 1, shortestPath, distance); } else { return false; } } + QVector3DList toQVector3DList(const QPolygonF &polygon) + { + QVector3DList list; + for ( auto vertex : polygon ) + list.append(QVector3D(vertex)); + + return list; + } + + QPolygonF toQPolygonF(const QPointFList &listF) + { + QPolygonF polygon; + for ( auto vertex : listF ) + polygon.append(vertex); + + return polygon; + } + + QPointFList toQPointFList(const QPolygonF &polygon) + { + QPointFList listF; + for ( auto vertex : polygon ) + listF.append(vertex); + + return listF; + } + } // end PolygonCalculus namespace diff --git a/src/Wima/PolygonCalculus.h b/src/Wima/PolygonCalculus.h index 4f903c002..3fae8a3d2 100644 --- a/src/Wima/PolygonCalculus.h +++ b/src/Wima/PolygonCalculus.h @@ -1,23 +1,24 @@ -#ifndef POLYGONCALCULUS_H -#define POLYGONCALCULUS_H -#endif +#pragma once #include #include +#include + -#include "PlanimetryCalculus.h" -#include "OptimisationTools.h" namespace PolygonCalculus { enum JoinPolygonError { NotSimplePolygon, PolygonJoined, Disjoint, PathSizeLow}; + typedef QList QVector3DList; + typedef QList QPointFList; + int closestVertexIndex (const QPolygonF &polygon, const QPointF &coordinate); QPointF closestVertex (const QPolygonF &polygon, const QPointF &coordinate); int nextVertexIndex (int pathsize, int index); int previousVertexIndex (int pathsize, int index); - JoinPolygonError joinPolygon (QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon); + JoinPolygonError join (QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon); bool isSimplePolygon (const QPolygonF &polygon); bool hasClockwiseWinding (const QPolygonF &path); void reversePath (QPolygonF &path); @@ -25,6 +26,14 @@ namespace PolygonCalculus { bool containsPath (QPolygonF polygon, const QPointF &c1, const QPointF &c2); void decomposeToConvex (const QPolygonF &polygon, QList &convexPolygons); bool shortestPath (const QPolygonF &polygon, const QPointF &startVertex, const QPointF &endVertex, QList &shortestPath); + + QPolygonF toQPolygonF(const QVector3DList &polygon); + QPolygonF toQPolygonF(const QPointFList &polygon); + QLineF toQLineF(const QVector3DList &line); + QPointFList toQPointFList(const QVector3DList &list); + QPointFList toQPointFList(const QPolygonF &list); + QVector3DList toQVector3DList(const QPointFList &listF); + QVector3DList toQVector3DList(const QPolygonF &listF); } diff --git a/src/Wima/WimaArea.cc b/src/Wima/WimaArea.cc index 0fad49c99..898fe3f79 100644 --- a/src/Wima/WimaArea.cc +++ b/src/Wima/WimaArea.cc @@ -1,4 +1,5 @@ -#include "WimaArea.h" +#include "WimaArea.h" + /*! * \variable WimaArea::epsilonMeter @@ -138,122 +139,55 @@ QGCMapPolygon WimaArea::toQGCPolygon() const * The algorithm will be able to join the areas, if either their edges intersect with each other, * or one area contains the other. */ -bool WimaArea::join(WimaArea &area1, WimaArea &area2, WimaArea &joinedArea, QString &errorString) +bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &joinedArea, QString &errorString) { + using namespace GeoUtilities; + using namespace PolygonCalculus; - if (area1.count() >= 3 && area2.count() >= 3) { + QList GeoPolygon1 = area1.coordinateList(); + QList GeoPolygon2 = area2.coordinateList(); - if ( isSelfIntersecting(area1) ) { - errorString.append("Area 1 is self intersecting.\n"); - return false; - } + /*qWarning("befor joining"); + qWarning() << GeoPolygon1; + qWarning() << GeoPolygon2;*/ - if ( isSelfIntersecting(area2) ) { - errorString.append("Area 2 is self intersecting.\n"); - return false; - } + QGeoCoordinate origin = GeoPolygon1[0]; - joinedArea.clear(); - - area1.verifyClockwiseWinding(); - area2.verifyClockwiseWinding(); - - WimaArea* walkerPoly = &area1; // "walk" on this polygon towards higher indices - WimaArea* crossPoly = &area2; // check for crossings with this polygon while "walking" - // and swicht to this polygon on a intersection, - // continue to walk towards higher indices - - // begin with the first index which is not inside crosspoly, if all Vertices are inside crosspoly return crosspoly - int startIndex = 0; - bool crossContainsWalker = true; - for (int i = 0; i < walkerPoly->count(); i++) { - if ( !crossPoly->containsCoordinate(walkerPoly->vertexCoordinate(i)) ) { - crossContainsWalker = false; - startIndex = i; - break; - } - } + QGeoCoordinate tset = GeoPolygon1[2]; + //qWarning() << tset;qWarning() << toGeo(toCartesian2D(tset, origin), origin); - if ( crossContainsWalker == true) { - joinedArea.appendVertices(crossPoly->coordinateList()); - return true; - } - QGeoCoordinate currentVertex = walkerPoly->vertexCoordinate(startIndex); - QGeoCoordinate startVertex = currentVertex; - // possible nextVertex (if no intersection between currentVertex and protoVertex with crossPoly) - QGeoCoordinate protoNextVertex = walkerPoly->vertexCoordinate(walkerPoly->nextVertexIndex(startIndex)); - - int nextVertexIndex = walkerPoly->nextVertexIndex(startIndex); - while (1) { - //qDebug("nextVertexIndex: %i", nextVertexIndex); - joinedArea.appendVertex(currentVertex); - - QGCMapPolyline walkerPolySegment; - walkerPolySegment.appendVertex(currentVertex); - walkerPolySegment.appendVertex(protoNextVertex); - - QList> neighbourList; - QList intersectionList; - //qDebug("IntersectionList.size() on init: %i", intersectionList.size()); - intersects(walkerPolySegment, *crossPoly, intersectionList, neighbourList); - - //qDebug("IntersectionList.size(): %i", intersectionList.size()); - - if (intersectionList.size() >= 1) { - int minDistIndex = 0; - - if (intersectionList.size() > 1) { - double minDist = currentVertex.distanceTo(intersectionList.value(minDistIndex)); - for (int i = 1; i < intersectionList.size(); i++) { - double currentDist = currentVertex.distanceTo(intersectionList.value(i)); - - if ( minDist > currentDist ) { - minDist = currentDist; - minDistIndex = i; - } - } - } - - //qDebug("MinDistIndex: %i", minDistIndex); - QGeoCoordinate protoCurrentVertex = intersectionList.value(minDistIndex); - // take numerical erros into account - if (protoCurrentVertex.distanceTo(currentVertex) > epsilonMeter) { - currentVertex = protoNextVertex; - QPair neighbours = neighbourList.value(minDistIndex); - protoNextVertex = crossPoly->vertexCoordinate(neighbours.second); - nextVertexIndex = neighbours.second; - - // swap walker and cross poly - WimaArea* temp = walkerPoly; - walkerPoly = crossPoly; - crossPoly = temp; - } else { - currentVertex = walkerPoly->vertexCoordinate(nextVertexIndex); - protoNextVertex = walkerPoly->vertexCoordinate(walkerPoly->nextVertexIndex(nextVertexIndex)); - nextVertexIndex = walkerPoly->nextVertexIndex(nextVertexIndex); - } - - } else { - currentVertex = walkerPoly->vertexCoordinate(nextVertexIndex); - protoNextVertex = walkerPoly->vertexCoordinate(walkerPoly->nextVertexIndex(nextVertexIndex)); - nextVertexIndex = walkerPoly->nextVertexIndex(nextVertexIndex); - } - - if (currentVertex == startVertex) { - if (area1.count() == joinedArea.count()) { // is the case if poly1 and poly2 don't intersect - return false; - } else { - return true; - } - } - } + QPolygonF polygon1 = toQPolygonF(toCartesian2D(GeoPolygon1, origin)); + QPolygonF polygon2 = toQPolygonF(toCartesian2D(GeoPolygon2, origin)); - } else { - return false; - } + /*qWarning("after 1 transform"); + qWarning() << polygon1; + qWarning() << polygon2;*/ + + QPolygonF joinedPolygon; + JoinPolygonError retValue = PolygonCalculus::join(polygon1, polygon2, joinedPolygon); + + + /*qWarning("after joining"); + qWarning() << joinedPolygon;*/ + + if (retValue == JoinPolygonError::Disjoint) { + qWarning("Polygons are disjoint."); + } else if (retValue == JoinPolygonError::NotSimplePolygon) { + qWarning("Not a simple polygon."); + } else if (retValue == JoinPolygonError::PathSizeLow) { + qWarning("Polygon vertex count is low."); + } else { + QList path = toGeo(toQPointFList(joinedPolygon), origin); + //qWarning("after transform"); + //qWarning() << path; + joinedArea.setPath(path); + return true; + } + + return false; } @@ -349,298 +283,22 @@ int WimaArea::previousVertexIndex(int index) const } } -/*! - * \fn bool WimaArea::intersects(const QGCMapPolyline &line1, const QGCMapPolyline &line2, QGeoCoordinate &intersectionPt) - * Returns \c true if \a line1 and \a line2 intersect with each other. - * Stores the intersection point in \a intersectionPt - * - * \sa QGeoCoordinate - */ -bool WimaArea::intersects(const QGCMapPolyline &line1, const QGCMapPolyline &line2, QGeoCoordinate &intersectionPt) -{ - - if (line1.count() == 2 && line2.count() == 2 ) { - QPointF pt11(0, 0); - - double x, y, z; - QGeoCoordinate origin = line1.vertexCoordinate(0); - convertGeoToNed(line1.vertexCoordinate(1), origin, &x, &y, &z); - QPointF pt12(x, y); - - QLineF kartLine1(pt11, pt12); - - - convertGeoToNed(line2.vertexCoordinate(0), origin, &x, &y, &z); - QPointF pt21(x, y); - - convertGeoToNed(line2.vertexCoordinate(1), origin, &x, &y, &z); - QPointF pt22(x, y);; - - QLineF kartLine2(pt21, pt22); - - QPointF intersectionPoint; - if (kartLine1.intersect(kartLine2, &intersectionPoint) == QLineF::BoundedIntersection) { - convertNedToGeo(intersectionPoint.x(), intersectionPoint.y(), origin.altitude(), origin, &intersectionPt); - return true; - } - else { - return false; - } - - - } else { - qWarning("WimaArea::intersect(line1, line2): line1->count() != 2 || line2->count() != 2!"); - return false; - } -} - -/*! - * \fn bool WimaArea::intersects(const QGCMapPolyline &line, const WimaArea &area, QList &intersectionList, QList> &neighbourList) - * Returns \c true if \a line and \a area intersect with each other at least once.bool WimaArea::intersects(const QGCMapPolyline &line, const WimaArea &area, QList &intersectionList, QList> &neighbourList) - * Stores the intersection points in \a intersectionList. - * Stores the indices of the closest two \a area vetices for each of coorespoinding intersection points in \a neighbourList. - * - * For example if an intersection point is found between the first and the second vertex of the \a area the intersection point will - * be stored in \a intersectionList and the indices 1 and 2 will be stored in \a neighbourList. - * \a neighbourList has entries of type \c {QPair}, where \c{pair.first} would contain 1 and \c{pair.second} would contain 2, when - * relating to the above example. - * - * \sa QPair, QList - */ -bool WimaArea::intersects(const QGCMapPolyline &line, const WimaArea &area, QList &intersectionList, QList> &neighbourList)// don't seperate parameters with new lines or documentation will break -{ - intersectionList.clear(); - neighbourList.clear(); - - - if (line.count() == 2 && area.count() >= 3) { // are line a proper line and poly a proper poly?other, - - // Asseble a line form each tow consecutive polygon vertices and check whether it intersects with line - for (int i = 0; i < area.count(); i++) { - - QGCMapPolyline interatorLine; - QGeoCoordinate currentVertex = area.vertexCoordinate(i); - QGeoCoordinate nextVertex = area.vertexCoordinate(area.nextVertexIndex(i)); - interatorLine.appendVertex(currentVertex); - interatorLine.appendVertex(nextVertex); - - QGeoCoordinate intersectionPoint; - if ( intersects(line, interatorLine, intersectionPoint) ){ - intersectionList.append(intersectionPoint); - - QPair neighbours; - neighbours.first = i; - neighbours.second = area.nextVertexIndex(i); - neighbourList.append(neighbours); - } - } - - if (intersectionList.count() > 0) { - return true; - } else { - return false; - } - } else { - qWarning("WimaArea::intersects(line, poly): line->count() != 2 || poly->count() < 3"); - return false; - } -} - -/*!other, - * \fn double WimaArea::distInsidePoly(const QGeoCoordinate &c1, const QGeoCoordinate &c2, WimaArea area) - * Returns the distance between the coordinate \a c1 and coordinate \a c2, or infinity if the shortest path between - * the two coordinates is not fully inside the \a area. - * \note Both coordinates must lie inside the \a area. - * - * \sa QGeoCoordinate - */ -double WimaArea::distInsidePoly(const QGeoCoordinate &c1, const QGeoCoordinate &c2, WimaArea area) -{ - area.offset(0.1); // hack to compensate for numerical issues, migh be replaced in the future... - if ( area.containsCoordinate(c1) && area.containsCoordinate(c2)) { - QList intersectionList; - QList> neighbourlist; - QGCMapPolyline line; - - line.appendVertex(c1); - line.appendVertex(c2); - intersects(line, area, intersectionList, neighbourlist); - - if ( intersectionList.size() == 0 ){ // if an intersection was found the path between c1 and c2 is not fully inside area. - return c1.distanceTo(c2); - } else { - return std::numeric_limits::infinity(); - } - - } else { - return std::numeric_limits::infinity(); - } -} - -/*! - * \fn bool WimaArea::dijkstraPath(const QGeoCoordinate &start, const QGeoCoordinate &end, const WimaArea &area, QList &dijkstraPath, QString &errorstring) - * Calculates the shortest path (inside \a area) between \a start and \a end. - * The \l {Dijkstra Algorithm} is used to find the shorest path. - * Stores the result inside \a dijkstraPath when sucessfull. - * Stores error messages in \a errorString. - * Returns \c true if successful, \c false else. - * - * \sa QList - */ -bool WimaArea::dijkstraPath(const QGeoCoordinate &start, const QGeoCoordinate &end, const WimaArea &area, QList &dijkstraPath, QString &errorString) // don't seperate parameters with new lines or documentation will break -{ - if ( isSelfIntersecting(area) ) { - errorString.append("Area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"); - return false; - } - - // Each QGeoCoordinate gets stuff into a Node - /// @param distance is the distance between the Node and it's predecessor - struct Node{ - QGeoCoordinate coordinate; - double distance = std::numeric_limits::infinity(); - Node* predecessorNode = nullptr; - }; - - // The list with all Nodes (start, end + poly.path()) - QList nodeList; - // This list will be initalized with (pointer to) all elements of nodeList. - // Elements will be successively remove during the execution of the Dijkstra Algorithm. - QList workingSet; - - // initialize nodeList_maxAltitude - // start cooridnate - Node startNode; - startNode.coordinate = start; - startNode.distance = 0; - nodeList.append(startNode); - - //poly cooridnates - for (int i = 0; i < area.count(); i++) { - Node node; - node.coordinate = area.vertexCoordinate(i); - nodeList.append(node); - } - - //end coordinate - Node endNode; - endNode.coordinate = end; - nodeList.append(endNode); - - // initialize working set - for (int i = 0; i < nodeList.size(); i++) { - Node* nodePtr = &nodeList[i]; - workingSet.append(nodePtr); - } - - - // Dijkstra Algorithm - // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus - while (workingSet.size() > 0) { - // serach Node with minimal distance - double minDist = std::numeric_limits::infinity(); - int minDistIndex = 0; - for (int i = 0; i < workingSet.size(); i++) { - Node* node = workingSet.value(i); - double dist = node->distance; - if (dist < minDist) { - minDist = dist; - minDistIndex = i; - } - } - Node* u = workingSet.takeAt(minDistIndex); - - - //update distance - for (int i = 0; i < workingSet.size(); i++) { - Node* v = workingSet[i]; - - // is neighbour? dist == infinity if no neihbour - double dist = distInsidePoly(u->coordinate, v->coordinate, area); - // is ther a alternative path which is shorter? - double alternative = u->distance + dist; - if (alternative < v->distance) { - v->distance = alternative; - v->predecessorNode = u; - } - } - - } - // end Djikstra Algorithm - - - // check it the Algorithm was sucessfulepsilonMeter - Node* Node = &nodeList.last(); - if (Node->predecessorNode == nullptr) { - - } - - // reverse assemble path - while (1) { - dijkstraPath.prepend(Node->coordinate); - - //Update Node - Node = Node->predecessorNode; - if (Node == nullptr) { - if (dijkstraPath[0].distanceTo(start) < epsilonMeter)// check if starting point was reached - break; - qWarning("WimaArea::dijkstraPath(): Error, no path found!\n"); - return false; - } - } - - return true; -} - -/*! - * \fn bool WimaArea::isSelfIntersecting(const WimaArea &area) - * Returns \c true if the \a area is self intersecting, \c false else. - * \note If the \a area is self intersecting, it's not a \l {Simple Polygon}. - */ -bool WimaArea::isSelfIntersecting(const WimaArea &area) -{ - int i = 0; - if (area.count() > 3) { - // check if any edge of the area (formed by two adjacent vertices) intersects with any other edge of the area - while(i < area.count()-1) { - QGeoCoordinate refBeginCoordinate = area.vertexCoordinate(i); - QGeoCoordinate refEndCoordinate = area.vertexCoordinate(area.nextVertexIndex(i)); - QGCMapPolyline refLine; - refLine.appendVertex(refBeginCoordinate); - refLine.appendVertex(refEndCoordinate); - int j = area.nextVertexIndex(i); - while(j < area.count()) { - QGeoCoordinate intersectionPt; - QGCMapPolyline iteratorLine; - iteratorLine.appendVertex(area.vertexCoordinate(j)); - iteratorLine.appendVertex(area.vertexCoordinate(area.nextVertexIndex(j))); - - if ( intersects(refLine, iteratorLine, intersectionPt) ){ - if ( !(intersectionPt.distanceTo(refBeginCoordinate) < epsilonMeter) - && !(intersectionPt.distanceTo(refEndCoordinate) < epsilonMeter) ) { - return true; - } - } - - - - j++; - } - i++; - } - } - - return false; -} - /*! * \fn bool WimaArea::isSelfIntersecting() * Returns \c true if the calling area is self intersecting, \c false else. * \note If the calling area is self intersecting, it's not a \l {Simple Polygon}. */ -bool WimaArea::isSelfIntersecting() +bool WimaArea::isSimplePolygon() { - return isSelfIntersecting(*this); + using namespace PolygonCalculus; + using namespace GeoUtilities; + + if (this->count() > 2) { + QPolygonF polygon = toQPolygonF(toCartesian2D(this->coordinateList(), this->vertexCoordinate(0))); + return PolygonCalculus::isSimplePolygon(polygon); + } else + return false; + } /*! diff --git a/src/Wima/WimaArea.h b/src/Wima/WimaArea.h index f0adc793f..824711ae4 100644 --- a/src/Wima/WimaArea.h +++ b/src/Wima/WimaArea.h @@ -11,6 +11,9 @@ #include "QGCGeo.h" #include +#include "GeoUtilities.h" +#include "PolygonCalculus.h" + class WimaArea : public QGCMapPolygon //abstract base class for all WimaAreas { Q_OBJECT @@ -46,18 +49,9 @@ public: // static Methodes static QGCMapPolygon toQGCPolygon (const WimaArea& area); - static bool join (WimaArea &area1, WimaArea &area2, WimaArea& joinedArea, QString &errorString); + static bool join (const WimaArea &area1, const WimaArea &area2, WimaArea& joinedArea, QString &errorString); static bool join (WimaArea &area1, WimaArea &area2, WimaArea& joinedArea); - static bool intersects (const QGCMapPolyline& line1, const QGCMapPolyline& line2, - QGeoCoordinate& intersectionPt); - static bool intersects (const QGCMapPolyline& line, const WimaArea& area, - QList& intersectionList, - QList>& neighbourList); - static double distInsidePoly (const QGeoCoordinate& c1, const QGeoCoordinate& c2, WimaArea area); - static bool dijkstraPath (const QGeoCoordinate& c1, const QGeoCoordinate& c2, - const WimaArea& area, QList& dijkstraPath, QString &errorstring); - static bool isSelfIntersecting (const WimaArea& area); - bool isSelfIntersecting (); + bool isSimplePolygon (); // Friends friend void print(const WimaArea& area, QString& outputString); diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 11f960972..440087b50 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -1,5 +1,7 @@ #include "WimaPlaner.h" + + const char* WimaPlaner::wimaFileExtension = "wima"; const char* WimaPlaner::areaItemsName = "AreaItems"; const char* WimaPlaner::missionItemsName = "MissionItems"; @@ -253,10 +255,8 @@ bool WimaPlaner::updateMission() QGeoCoordinate start = _serviceArea.center(); QGeoCoordinate end = survey->visualTransectPoints().first().value(); QList path; - if ( !WimaArea::dijkstraPath(start, end, _joinedArea, path, errorString)) { - qgcApp()->showMessage(QString( QString(tr("Not able to calculate the path from takeoff position to measurement area.")) - + errorString - ).toLocal8Bit().data()); + if ( !calcShortestPath(start, end, path)) { + qgcApp()->showMessage( QString(tr("Not able to calculate the path from takeoff position to measurement area.")).toLocal8Bit().data()); return false; } for (int i = 1; i < path.count()-1; i++) { @@ -268,10 +268,8 @@ bool WimaPlaner::updateMission() start = survey->visualTransectPoints().last().value(); end = _serviceArea.center(); path.clear(); - if ( ! WimaArea::dijkstraPath(start, end, _joinedArea, path, errorString)) { - qgcApp()->showMessage(QString( QString(tr("Not able to calculate the path from measurement area to landing position.")) - + errorString - ).toLocal8Bit().data()); + if ( !calcShortestPath(start, end, path)) { + qgcApp()->showMessage(QString(tr("Not able to calculate the path from measurement area to landing position.")).toLocal8Bit().data()); return false; } for (int i = 1; i < path.count()-1; i++) { @@ -500,22 +498,24 @@ void WimaPlaner::recalcPolygonInteractivity(int index) bool WimaPlaner::recalcJoinedArea(QString &errorString) { // check if area paths form simple polygons - if ( WimaArea::isSelfIntersecting(_serviceArea) ) { + if ( !_serviceArea.isSimplePolygon() ) { errorString.append(tr("Service area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n")); return false; } - if ( WimaArea::isSelfIntersecting(_corridor) ) { + if ( !_corridor.isSimplePolygon() && _corridor.count() > 0) { errorString.append(tr("Corridor is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n")); return false; } - if ( WimaArea::isSelfIntersecting(_measurementArea) ) { + if ( !_measurementArea.isSimplePolygon() ) { errorString.append(tr("Measurement area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n")); return false; } // join service area, op area and corridor + if (!_visualItems.contains(&_joinedArea)) + _visualItems.append(&_joinedArea); _joinedArea.setPath(_serviceArea.path()); _joinedArea.join(_corridor); if ( !_joinedArea.join(_measurementArea) ) { @@ -547,6 +547,21 @@ void WimaPlaner::pushToContainer() } } +bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path) +{ + using namespace GeoUtilities; + using namespace PolygonCalculus; + QList path2D; + bool retVal = PolygonCalculus::shortestPath( + toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/start)), + /*start point*/ QPointF(0,0), + /*destination*/toCartesian2D(destination, start), + /*shortest path*/path2D); + path.append(toGeo(path2D, /*origin*/start)); + + return retVal; +} + void WimaPlaner::resetAllInteractive() { // Marks all areas as inactive (area.interactive == false) diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index c1207ba5f..4b13e5ada 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -25,6 +25,10 @@ #include "JsonHelper.h" #include "QGCApplication.h" +#include "OptimisationTools.h" +#include "PlanimetryCalculus.h" +#include "GeoUtilities.h" + class WimaPlaner : public QObject { @@ -117,6 +121,7 @@ private slots: void recalcPolygonInteractivity (int index); bool recalcJoinedArea (QString &errorString); void pushToContainer (); + bool calcShortestPath (const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path); private: // Member Functions -- 2.22.0