From 7110c77552a6aa15e49e8d3a47beb94d93738275 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Mon, 8 Jul 2019 18:05:37 +0200 Subject: [PATCH] about to finish SphereCalculus --- qgroundcontrol.pro | 6 +- src/Wima/SphereCalculus.cc | 610 +++++++++++++++++++ src/Wima/SphereCalculus.h | 65 ++ src/Wima/SphericalGeometryCalculus.cc | 830 +++++++++++++++++++++++++- src/Wima/SphericalGeometryCalculus.h | 48 +- src/Wima/WimaArea.cc | 2 +- 6 files changed, 1551 insertions(+), 10 deletions(-) create mode 100644 src/Wima/SphereCalculus.cc create mode 100644 src/Wima/SphereCalculus.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index c04f6f32d..7013df0f3 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -424,7 +424,8 @@ HEADERS += \ src/Wima/WimaPlanData.h \ src/Wima/WimaJoinedArea.h \ src/Wima/WimaJoinedAreaData.h \ - src/Wima/SphericalGeometryCalculus.h + src/Wima/SphericalGeometryCalculus.h \ + src/Wima/SphereCalculus.h SOURCES += \ src/api/QGCCorePlugin.cc \ src/api/QGCOptions.cc \ @@ -447,7 +448,8 @@ SOURCES += \ src/Wima/WimaMeasurementAreaData.cc \ src/Wima/WimaJoinedArea.cc \ src/Wima/WimaJoinedAreaData.cc \ - src/Wima/SphericalGeometryCalculus.cc + src/Wima/SphericalGeometryCalculus.cc \ + src/Wima/SphereCalculus.cc # # Unit Test specific configuration goes here (requires full debug build with all plugins) diff --git a/src/Wima/SphereCalculus.cc b/src/Wima/SphereCalculus.cc new file mode 100644 index 000000000..51d96f36c --- /dev/null +++ b/src/Wima/SphereCalculus.cc @@ -0,0 +1,610 @@ +#include "SphereCalculus.h" + +SphereCalculus::SphereCalculus(const SphereCalculus &other, QObject *parent) + : QObject (parent) +{ + *this = other; +} + +SphereCalculus &SphereCalculus::operator=(const SphereCalculus &other) +{ + setEpsilonMeter(other.epsilonMeter()); + + return *this; +} + +void SphereCalculus::setEpsilonMeter(double epsilon) +{ + if (epsilon > 0) { + _epsilonMeter = epsilon; + } +} + +double SphereCalculus::epsilonMeter() const +{ + return _epsilonMeter; +} + +/*! + * \fn int SphereCalculus::closestVertexIndex(const QList &path, const QGeoCoordinate &coordinate) + * which has the least distance to \a coordinate. + * + * \sa QGeoCoordinate + */ +int SphereCalculus::closestVertexIndex(const QList &path, const QGeoCoordinate &coordinate) +{ + if (path.size() == 0) { + qWarning("Path is empty!"); + return -1; + }else if (path.size() == 1) { + return 0; + }else { + int index = 0; // the index of the closest vertex + double min_dist = coordinate.distanceTo(path[index]); + for(int i = 1; i < path.size(); i++){ + double dist = coordinate.distanceTo(path[i]); + if (dist < min_dist){ + min_dist = dist; + index = i; + } + } + return index; + } +} + +/*! + * \fn QGeoCoordinate SphereCalculus::closestVertex(const QList &path, const QGeoCoordinate &coordinate) + * Returns the vertex of the \a path with the least distance to \a coordinate. + * + * \sa QGeoCoordinate + */ +QGeoCoordinate SphereCalculus::closestVertex(const QList &path, const QGeoCoordinate &coordinate) +{ + int index = closestVertexIndex(path, coordinate); + if (index >=0 ) { + return path[index]; + } else { + return QGeoCoordinate(); + } +} + +/*! + * \fn int SphereCalculus::nextPolygonIndex(int pathsize, int index) + * Returns the index of the next vertex (of a polygon), which is \a index + 1 if \a index is smaller than \c {\a pathsize - 1}, + * or 0 if \a index equals \c {\a pathsize - 1}, or -1 if the \a index is out of bounds. + * \note \a pathsize is usually obtained by invoking polygon.size() + */ +int SphereCalculus::nextPolygonIndex(int pathsize, int index) +{ + if (index >= 0 && index < pathsize-1) { + return index + 1; + } else if (index == pathsize-1) { + return 0; + } else { + qWarning("SphereCalculus::nextPolygonIndex(): Index out of bounds! index:count = %i:%i", index, pathsize); + return -1; + } +} + +/*! + * \fn int SphereCalculus::previousPolygonIndex(int pathsize, int index) + * Returns the index of the previous vertex (of a polygon), which is \a index - 1 if \a index is larger 0, + * or \c {\a pathsize - 1} if \a index equals 0, or -1 if the \a index is out of bounds. + * \note pathsize is usually obtained by invoking polygon.size() + */ +int SphereCalculus::previousPolygonIndex(int pathsize, int index) +{ + if (index > 0 && index polygon1, QList polygon2, QList &joinedPolygon) + * Joins \a polygon1 and \a polygon2 such that a \l {Simple Polygon} is created. + * Stores the result inside \a joinedArea. + * Returns \c NotSimplePolygon1 if \a polygon1 isn't a Simple Polygon, \c NotSimplePolygon2 if \a polygon2 isn't a Simple Polygon, \c Disjoind if the polygons are disjoint, + * \c PathSizeLow if at least one polygon has a size samler than 3, or \c PolygonJoined on success. + * The algorithm will be able to join the areas, if either their edges intersect with each other, + * or one area is inside the other. + */ +SphereCalculus::JoinPolygonError SphereCalculus::joinPolygon(QList polygon1, QList polygon2, QList &joinedPolygon) +{ + if (polygon1.size() >= 3 && polygon2.size() >= 3) { + + if ( isSimplePolygon(polygon1) ) { + qWarning("Polygon 1 is self intersecting.\n"); + return SphereCalculus::JoinPolygonError::NotSimplePolygon1; + } + if ( isSimplePolygon(polygon2) ) { + qWarning("Area 2 is self intersecting.\n"); + return SphereCalculus::JoinPolygonError::NotSimplePolygon2; + } + + joinedPolygon.clear(); + + if ( !hasClockwiseWinding(polygon1)) { + reversePath(polygon1); + } + if ( !hasClockwiseWinding(polygon2)) { + reversePath(polygon2); + } + + const QList *walkerPoly = &polygon1; // "walk" on this polygon towards higher indices + const QList *crossPoly = &polygon2; // 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->size(); i++) { + if ( !crossPoly->contains(walkerPoly->value(i)) ) { + crossContainsWalker = false; + startIndex = i; + break; + } + } + + if ( crossContainsWalker == true) { + joinedPolygon.append(*crossPoly); + return SphereCalculus::JoinPolygonError::PolygonJoined; + } + + QGeoCoordinate currentVertex = walkerPoly->value(startIndex); + QGeoCoordinate startVertex = currentVertex; + // possible nextVertex (if no intersection between currentVertex and protoVertex with crossPoly) + int nextVertexIndex = nextPolygonIndex(walkerPoly->size(), startIndex); + QGeoCoordinate protoNextVertex = walkerPoly->value(nextVertexIndex); + bool switchHappenedPreviously = false; // means switch between crossPoly and walkerPoly + while (1) { + //qDebug("nextVertexIndex: %i", nextVertexIndex); + joinedPolygon.append(currentVertex); + + Line walkerPolySegment; + walkerPolySegment.first = currentVertex; + walkerPolySegment.second = protoNextVertex; + + QList> neighbourList; + QList intersectionList; + //qDebug("IntersectionList.size() on init: %i", intersectionList.size()); + intersects(*crossPoly, walkerPolySegment, intersectionList, neighbourList); + + //qDebug("IntersectionList.size(): %i", intersectionList.size()); + + if (intersectionList.size() >= 1) { + int minDistIndex = 0; + + // find the vertex with the least distance to currentVertex + 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); + // If the currentVertex is a intersection point a intersection ocisSelfIntersectingcures with the + // crossPoly. This would cause unwanted switching of crossPoly and walkerPoly, thus intersections + // are only token in to account if they occur beyond a certain distance (_epsilonMeter) or no switching happend in the + // previous step. + + if (switchHappenedPreviously == false + || protoCurrentVertex.distanceTo(currentVertex) > _epsilonMeter) { + currentVertex = protoCurrentVertex; + QPair neighbours = neighbourList.value(minDistIndex); + protoNextVertex = crossPoly->value(neighbours.second); + nextVertexIndex = neighbours.second; + + // switch walker and cross poly + const QList *temp = walkerPoly; + walkerPoly = crossPoly; + crossPoly = temp; + + switchHappenedPreviously = true; + } else { + currentVertex = walkerPoly->value(nextVertexIndex); + nextVertexIndex = nextPolygonIndex(walkerPoly->size(), nextVertexIndex); + protoNextVertex = walkerPoly->value(nextVertexIndex); + + switchHappenedPreviously = false; + } + + } else { + currentVertex = walkerPoly->value(nextVertexIndex); + nextVertexIndex = nextPolygonIndex(walkerPoly->size(), nextVertexIndex); + protoNextVertex = walkerPoly->value(nextVertexIndex); + } + + if (currentVertex == startVertex) { + if (polygon1.size() == joinedPolygon.size()) { + return SphereCalculus::JoinPolygonError::Disjoint; + } else { + return SphereCalculus::JoinPolygonError::PolygonJoined; + } + } + } + + } else { + return SphereCalculus::JoinPolygonError::PathSizeLow; + } +} + +/*! + * \fn SphereCalculus::IntersectionType SphereCalculus::intersects(const QList &line1, const QList &line2, QGeoCoordinate &intersectionPt) + * Determines wheter \a line1 and \a line2 intersect and of which type the intersection is. + * Stores the intersection point in \a intersectionPt + * Returns \c NoIntersection if no intersection occured, \c EdgeIntersection if a interisSelfIntersectingsection occured near a endpoint of a line (within epsilonMeter), or \c InteriorIntersection else. + * + * \sa QGeoCoordinate + */ +SphereCalculus::IntersectionType SphereCalculus::intersects(const Line &line1, const Line &line2, QGeoCoordinate &intersectionPt) +{ + QPointF pt11(0, 0); + + double x, y, z; + QGeoCoordinate origin = line1.first; + convertGeoToNed(line1.second, origin, &x, &y, &z); + QPointF pt12(x, y); + + QLineF kartLine1(pt11, pt12); + + + convertGeoToNed(line2.first, origin, &x, &y, &z); + QPointF pt21(x, y); + + convertGeoToNed(line2.second, 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); + if ( intersectionPt.distanceTo(line1.first) < _epsilonMeter + || intersectionPt.distanceTo(line1.second) < _epsilonMeter + || intersectionPt.distanceTo(line2.first) < _epsilonMeter + || intersectionPt.distanceTo(line2.second) < _epsilonMeter ) + return SphereCalculus::IntersectionType::EdgeIntersection; + return SphereCalculus::IntersectionType::InteriorIntersection; + } + else { + return SphereCalculus::IntersectionType::NoIntersection; + } +} + +/*! + * \overload QList SphereCalculus::intersects(const QList &polygon, const QList &line, QList &intersectionList, QList > &neighbourList) + * Checks if \a polygon intersect with \a line. + * 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. + * + * Returns the \c IntersectionType of each intersection point within a QList. + * + * \sa QPair, QList + */ +QList SphereCalculus::intersects(const QList &polygon, const Line &line, QList &intersectionList, QList > &neighbourList) +{ + if (polygon.size() >= 3) { // are line a proper line and poly a proper poly?other, + intersectionList.clear(); + neighbourList.clear(); + QList 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++) { + + Line interatorLine; + QGeoCoordinate currentVertex = polygon[i]; + QGeoCoordinate nextVertex = polygon[nextPolygonIndex(polygon.size(), i)]; + interatorLine.first = currentVertex; + interatorLine.second = nextVertex; + + QGeoCoordinate intersectionPoint; + SphereCalculus::IntersectionType returnValue = intersects(line, interatorLine, intersectionPoint); + if ( returnValue == SphereCalculus::IntersectionType::EdgeIntersection + || returnValue == SphereCalculus::IntersectionType::InteriorIntersection) { + intersectionList.append(intersectionPoint); + + QPair neighbours; + neighbours.first = i; + neighbours.second = nextPolygonIndex(polygon.size(), i); + neighbourList.append(neighbours); + + intersectionTypeList.append(returnValue); + } + } + + if (intersectionList.count() > 0) { + return intersectionTypeList; + } else { + return QList(); + } + } else { + qWarning("WimaArea::intersects(line, poly): line->count() != 2 || poly->count() < 3"); + return QList(); + } +} + +/*! + * \overload bool SphereCalculus::intersects(const QList &polygon, const QList &line) + * Returns \c true if any intersection between \a polygon and \a line exists, \c false else. + * + * \sa QPair, QList + */ +bool SphereCalculus::intersects(const QList &polygon, const Line &line) +{ + QList dummyGeo; + QList> dummyNeighbour; + intersects(polygon, line, dummyGeo, dummyNeighbour); + + if (dummyGeo.size() > 0) + return true; + + return false; +} + +/*! + * \fn bool SphereCalculus::dijkstraPath(const QList &polygon, const QGeoCoordinate &c1, const QGeoCoordinate &c2, QList &dijkstraPath) + * Calculates the shortest path (inside \a polygon) 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 + */ +SphereCalculus::DijkstraError SphereCalculus::dijkstraPath(const QList &polygon, const QGeoCoordinate &start, const QGeoCoordinate &end, QList &dijkstraPath) +{ + if ( isSimplePolygon(polygon) ) { + return SphereCalculus::DijkstraError::NotSimplePolygon; + } + + // 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 < polygon.size(); i++) { + Node node; + node.coordinate = polygon[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 = distanceInsidePolygon(u->coordinate, v->coordinate, polygon); + // is ther a alternative path which is shorter?QList + double alternative = u->distance + dist; + if (alternative < v->distance) { + v->distance = alternative; + v->predecessorNode = u; + } + } + + } + // end Djikstra Algorithm + + + + + // reverse assemble path + Node* Node = &nodeList.last(); + 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 SphereCalculus::DijkstraError::NoPathFound; + } + } + + return SphereCalculus::DijkstraError::PathFound; +} + +/*! + * \fn bool SphereCalculus::isSimplePolygon(const QList &polygon) + * Returns \c true if \a polygon is a \l {Simple Polygon}, \c false else. + * \note A polygon is a Simple Polygon iff it is not self intersecting. + */ +bool SphereCalculus::isSimplePolygon(const QList &polygon) +{ + int i = 0; + if (polygon.count() > 3) { + // check if any edge of the area (formed by two adjacent vertices) intersects with any other edge of the area + while(i < polygon.count()-1) { + QGeoCoordinate refBeginCoordinate = polygon[i]; + QGeoCoordinate refEndCoordinate = polygon[nextPolygonIndex(polygon.size(), i)]; + SphereCalculus::Line refLine; + refLine.first = refBeginCoordinate; + refLine.second = refEndCoordinate; + int j = nextPolygonIndex(polygon.size(), i); + while(j < polygon.size()) { + + QGeoCoordinate intersectionPt; + SphereCalculus::Line iteratorLine; + iteratorLine.first = polygon[j]; + iteratorLine.second = polygon[nextPolygonIndex(polygon.size(), j)]; + if ( intersects(refLine, iteratorLine, intersectionPt) + == SphereCalculus::IntersectionType::InteriorIntersection){ + return false; + } + j++; + } + i++; + } + } + return true; +} + +/*! + * \fn bool SphereCalculus::hasClockwiseWinding(const QList &path) + * Returns \c true if \a path has clockwise winding, \c false else. + */ +bool SphereCalculus::hasClockwiseWinding(const QList &path) +{ + if (path.size() <= 2) { + return false; + } + + double sum = 0; + for (int i=0; i &path) + * Reverses the path, i.e. changes the first Vertex with the last, the second with the befor last and so forth. + */ +void SphereCalculus::reversePath(QList &path) +{ + QList pathReversed; + for (int i = 0; i < path.size(); i++) { + pathReversed.prepend(path[i]); + } + path.clear(); + path.append(pathReversed); +} + +bool SphereCalculus::offsetPolygon(QList &polygon, double offset) +{ + +} + +/*! + * \fn double SphereCalculus::distanceInsidePolygon(const QGeoCoordinate &c1, const QGeoCoordinate &c2, QList polygon) + * 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. + * + * \sa QGeoCoordinate + */ +double SphereCalculus::distanceInsidePolygon(const QGeoCoordinate &c1, const QGeoCoordinate &c2, QList polygon) +{ + offsetPolygon(polygon, 0.1);// hack to compensate for numerical issues, migh be replaced in the future... + if ( polygon.contains(c1) && polygon.contains(c2)) { + QList intersectionList; + QList> neighbourlist; + SphereCalculus::Line line; + + line.first = c1; + line.second = c2; + intersects(polygon, line, intersectionList, neighbourlist); + + if ( intersectionList.size() == 0 ){ // if an intersection was found the path between c1 and c2 is not fully inside polygon. + return c1.distanceTo(c2); + } else { + return std::numeric_limits::infinity(); + } + + } else { + return std::numeric_limits::infinity(); + } +} + +/*! + \class SphereCalculus + \inmodule Wima + + \brief The \c WimaArea class provides the a base class for + all areas used within the Wima extension. + + \c WimaArea uses a \l {Simple Polygon} derived from \c {QGCMapPolygon} + to define areas inside which certain taskts are performed. The polygon (often refered to as the path) can + be displayed visually on a map. +*/ + + + +/*! + \externalpage https://en.wikipedia.org/wiki/Simple_polygon + \title Simple Polygon +*/ + +/*! + \externalpage https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm + \title Dijkstra Algorithm +*/ + + + + + + + diff --git a/src/Wima/SphereCalculus.h b/src/Wima/SphereCalculus.h new file mode 100644 index 000000000..555ed73cf --- /dev/null +++ b/src/Wima/SphereCalculus.h @@ -0,0 +1,65 @@ +#ifndef SphereCalculus_H +#define SphereCalculus_H + +#include +#include +#include +#include + +#include "QGCGeo.h" + +// Abstract class providing methods to do calculations on objects located on a sphere (e.g. earth) +class SphereCalculus : public QObject +{ + Q_OBJECT +public: + SphereCalculus(QObject *parent = nullptr); + SphereCalculus(const SphereCalculus &other, QObject *parent = nullptr); + SphereCalculus &operator=(const SphereCalculus &other); + + typedef QPair Line; + + + enum JoinPolygonError { NotSimplePolygon1, PolygonJoined, NotSimplePolygon2, Disjoint, PathSizeLow}; + enum IntersectionType { NoIntersection, EdgeIntersection, InteriorIntersection, Error}; + enum DijkstraError { NoPathFound, PathFound, NotSimplePolygon}; + + // Property setters + void setEpsilonMeter(double epsilon); + + // Property getters + double epsilonMeter() const; + + // Member Methodes + int closestVertexIndex (const QList &path, const QGeoCoordinate &coordinate); + QGeoCoordinate closestVertex (const QList &path, const QGeoCoordinate &coordinate); + int nextPolygonIndex (int pathsize, int index); + int previousPolygonIndex(int pathsize, int index); + JoinPolygonError joinPolygon (QList polygon1, QList polygon2, + QList &joinedPolygon); + IntersectionType intersects (const Line &line1, const Line &line2, + QGeoCoordinate &intersectionPt); + QList intersects (const QList &polygon, const Line &line, + QList &intersectionList, QList> &neighbourList); + bool intersects (const QList &polygon, const Line &line); + DijkstraError dijkstraPath (const QList &polygon, const QGeoCoordinate& start, + const QGeoCoordinate& end, QList& dijkstraPath); + bool isSimplePolygon (const QList &polygon); + bool hasClockwiseWinding (const QList &path); + void reversePath (QList &path); + bool offsetPolygon (QList &polygon, double offset); + + + +signals: + +public slots: + +private: + double distanceInsidePolygon (const QGeoCoordinate& c1, const QGeoCoordinate& c2, QList polygon); + + double _epsilonMeter; // The accuracy used for distance calculations (unit: m). + +}; + +#endif // SphereCalculus_H diff --git a/src/Wima/SphericalGeometryCalculus.cc b/src/Wima/SphericalGeometryCalculus.cc index 97a5b5382..1dc747c54 100644 --- a/src/Wima/SphericalGeometryCalculus.cc +++ b/src/Wima/SphericalGeometryCalculus.cc @@ -1,6 +1,832 @@ -#include "SphericalGeometryCalculus.h" +#include "SphereCalculus.h" -SphericalGeometryCalculus::SphericalGeometryCalculus(QObject *parent) : QObject(parent) + + + +// Constructors +WimaArea::WimaArea(QObject *parent) + : QGCMapPolygon (parent) +{ + init(); + _maxAltitude = 30; +} + +WimaArea::WimaArea(const WimaArea &other, QObject *parent) + : QGCMapPolygon (parent) +{ + init(); + *this = other; +} + +/*! + *\fn WimaArea &WimaArea::operator=(const WimaArea &other) + * + * Assigns \a other to this \c WimaArea and returns a reference to this \c WimaArea. + * + * Copies only path and maximum altitude. + */ +WimaArea &WimaArea::operator=(const WimaArea &other) +{ + QGCMapPolygon::operator=(other); + this->_maxAltitude = other.maxAltitude(); + this->setPath(other.path()); + + return *this; +} + +/*! + \fn void WimaArea::setMaxAltitude(double altitude) + + Sets the \c _maxAltitude member to \a altitude and emits the signal \c maxAltitudeChanged() + if \c _maxAltitude is not equal to altitude. + */ +void WimaArea::setMaxAltitude(double altitude) +{ + if ( altitude > 0 && qFuzzyCompare(altitude, _maxAltitude) ) { + _maxAltitude = altitude; + emit maxAltitudeChanged(); + } +} + + + +/*! + * \fn QGeoCoordinate WimaArea::getClosestVertex(const QGeoCoordinate& coordinate) const + * Returns the vertex of the polygon path with the least distance to \a coordinate. + * + * \sa QGeoCoordinate + */ +QGeoCoordinate WimaArea::getClosestVertex(const QGeoCoordinate& coordinate) const +{ + return this->vertexCoordinate(getClosestVertexIndex(coordinate)); +} + +/*! + * \fn QGCMapPolygon WimaArea::toQGCPolygon(const WimaArea &area) + * Converts the \c WimaArea \a area to \c QGCMapPolygon by copying the path only. + */ +QGCMapPolygon WimaArea::toQGCPolygon(const WimaArea &area) +{ + QGCMapPolygon qgcPoly; + qgcPoly.setPath(area.path()); + + return QGCMapPolygon(qgcPoly); +} + +/*! + * \fn QGCMapPolygon WimaArea::toQGCPolygon() const + * Converts the calling \c WimaArea to \c QGCMapPolygon by copying the path only. + */ +QGCMapPolygon WimaArea::toQGCPolygon() const +{ + return toQGCPolygon(*this); +} + + + + +/*! + * \fn bool WimaArea::join(WimaArea &area1, WimaArea &area2, WimaArea &joinedArea) + * Joins the areas \a area1 and \a area2 such that a \l {Simple Polygon} is created. + * Stores the result inside \a joinedArea. + * Returns \c true if the algorithm was able to join the areas; false else. + * 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 dummy; + return join(area1, area2, joinedArea, dummy); +} + +/*! + * \fn bool WimaArea::join(WimaArea &area) + * Joins the calling \c WimaArea and the \a area such that a \l {Simple Polygon} is created. + * Overwrites the calling \c WimaArea with the result, if the algorithm was successful. + * Returns \c true if the algorithm was able to join the areas; false else. + * 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 &area) +{ + WimaArea joinedArea; + if ( join(*this, area, joinedArea) ) { + this->setPath(joinedArea.path()); + return true; + } else { + return false; + } +} + + +/*! + * \fn bool WimaArea::join(WimaArea &area, QString &errorString) + * Joins the calling \c WimaArea and the \a area such that a \l {Simple Polygon} is created. + * Overwrites the calling \c WimaArea with the result, if the algorithm was successful. + * + * Returns \c true if the algorithm was able to join the areas; false else. + * Stores error messages in \a errorString. + * + * 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 &area, QString &errorString) +{ + WimaArea joinedArea; + if ( join(*this, area, joinedArea, errorString) ) { + this->setPath(joinedArea.path()); + return true; + } else { + return false; + } +} + +/*! + * \fn int WimaArea::nextVertexIndex(int index) const + * Returns the index of the next vertex (of the areas path), which is \a index + 1 if \a index is smaller than \c {area.count() - 1}, + * or 0 if \a index equals \c {area.count() - 1}, or -1 if the \a index is out of bounds. + * \note The function \c {area.count()} (derived from \c QGCMapPolygon) returns the number of vertices defining the area. + */ +int WimaArea::nextVertexIndex(int index) const +{ + if (index >= 0 && index < count()-1) { + return index + 1; + } else if (index == count()-1) { + return 0; + } else { + qWarning("WimaArea::nextVertexIndex(): Index out of bounds! index:count = %i:%i", index, count()); + return -1; + } +} + +/*! + * \fn int WimaArea::previousVertexIndex(int index) const + * Returns the index of the previous vertex (of the areas path), which is \a index - 1 if \a index is larger 0, + * or \c {area.count() - 1} if \a index equals 0, or -1 if the \a index is out of bounds. + * \note The function \c {area.count()} (derived from \c QGCMapPolygon) returns the number of vertices defining the area. + */ +int WimaArea::previousVertexIndex(int index) const +{ + if (index > 0 && index < count()) { + return index - 1; + } else if (index == 0) { + return count()-1; + } else { + qWarning("WimaArea::previousVertexIndex(): Index out of bounds! index:count = %i:%i", index, count()); + return -1; + } +} + +/*! + * \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() +{ + return isSelfIntersecting(*this); +} + +/*! + * \fn void WimaArea::saveToJson(QJsonObject &json) + * Saves the calling area to \c QJsonObject object and stores it inside \a json. + */*! +* \fn QGeoCoordinate WimaArea::getClosestVertex(const QGeoCoordinate& coordinate) const +* Returns the vertex of the polygon path with the least distance to \a coordinate. +* +* \sa QGeoCoordinate +*/ + * \sa QJsonObject + */ +void WimaArea::saveToJson(QJsonObject &json) +{ + this->QGCMapPolygon::saveToJson(json); + json[maxAltitudeName] = _maxAltitude; + json[areaTypeName] = wimaAreaName; + // add WimaVehicle if necessary.. +} + +/*! + * \fn bool WimaArea::loadFromJson(const QJsonObject &json, QString& errorString) + * Loads data from \a json and stores it inside the calling area. + * Returns \c true if loading was successful, \c false else. + * Stores error messages inside \a errorString. + * + * \sa QJsonObject + */ +bool WimaArea::loadFromJson(const QJsonObject &json, QString& errorString) +{ + if ( this->QGCMapPolygon::loadFromJson(json, false /*no poly required*/, errorString) ) { + if ( json.contains(maxAltitudeName) && json[maxAltitudeName].isDouble()) { + _maxAltitude = json[maxAltitudeName].toDouble(); + return true; + } else { + errorString.append(tr("Could not load Maximum Altitude value!\n")); + return false; + } + } else { + qWarning() << errorString; + return false; + } +} + +/*! + * \fn void WimaArea::init() + * Funtion to be called during construction. + */ +void WimaArea::init() +{ + this->setObjectName(wimaAreaName); +} + +/*! + * \fn void print(const WimaArea &area) + * Prints the data contained in \a area to the console. + */ +void print(const WimaArea &area) +{ + QString message; + print(area, message); + qWarning() << messa/*! + * \fn int WimaArea::getClosestVertexIndex(const QGeoCoordinate &coordinate) const + * Returns the index of the vertex (element of the polygon path) + * which has the least distance to \a coordinate. + * + * \sa QGeoCoordinate + */ + int WimaArea::getClosestVertexIndex(const QGeoCoordinate &coordinate) const + { + if (this->count() == 0) { + qWarning("Polygon count == 0!"); + return -1; + }else if (this->count() == 1) { + return 0; + }else { + int index = 0; + double min_dist = coordinate.distanceTo(this->vertexCoordinate(index)); + for(int i = 1; i < this->count(); i++){ + double dist = coordinate.distanceTo(this->vertexCoordinate(i)); + if (dist < min_dist){ + min_dist = dist; + index = i; + } + } + + return index; + } + }ge; +} + +/*! + * \fn void print(const WimaArea &area) + * Prints the data contained in \a area to the \a outputString. + */ +void print(const WimaArea &area, QString &outputString) +{ + outputString.append(QString("Type: %1\n").arg(area.objectName())); + print(static_cast(area), outputString); + outputString.append(QString("Maximum Altitude: %1\n").arg(area._maxAltitude)); +} + + +// QDoc Documentation + +/*! + \group WimaAreaGroup + \title Group of WimaAreas + + Every \c WimaArea of the equally named group uses a \l {Simple Polygon} derived from \c {QGCMapPolygon} + to define areas inside which certain taskts are performed. +*/ + +/*! + \class WimaArea + \inmodule Wima + \ingroup WimaArea + + \brief The \c WimaArea class provides the a base class for + all areas used within the Wima extension. + + \c WimaArea uses a \l {Simple Polygon} derived from \c {QGCMapPolygon} + to define areas inside which certain taskts are performed. The polygon (often refered to as the path) can + be displayed visually on a map. +*/ + +/*! + \variable WimaArea::_maxAltitude + \brief The maximum altitude vehicles are allowed to fly inside this area. +*/ + +/*! + \property WimaArea::maxAltitude + \brief The maximum altitude at which vehicles are allowed to fly. +*/ + +/*! + \property WimaArea::mapVisualQML + \brief A string containing the name of the QML file used to displays this area on a map. +*/ + +/*! + \property WimaArea::editorQML + \brief A string containing the name of the QML file allowing to edit the area's properties. +*/ + +/*! + \externalpage https://en.wikipedia.org/wiki/Simple_polygon + \title Simple Polygon +*/ + +/*! + \externalpage https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm + \title Dijkstra Algorithm +*/ + + + +/*! + * \fn QGeoCoordinate WimaArea::getClosestVertex(const QGeoCoordinate& coordinate) const + * Returns the vertex of the polygon path with the least distance to \a coordinate. + * + * \sa QGeoCoordinate + */ + +SphereCalculus::SphereCalculus(const SphereCalculus &other, QObject *parent) + : QObject (parent) +{ + *this = other; +} + +SphereCalculus &SphereCalculus::operator=(const SphereCalculus &other) +{ + setEpsilonMeter(other.epsilonMeter()); +} + +void SphereCalculus::setEpsilonMeter(double epsilon) +{ + if (epsilon > 0) { + _epsilonMeter = epsilon; + } +} + +double SphereCalculus::epsilonMeter() const { + return _epsilonMeter; +} + +/*! + * \fn int SphereCalculus::closestVertexIndex(const QList &path, const QGeoCoordinate &coordinate) const + * which has the least distance to \a coordinate. + * + * \sa QGeoCoordinate + */ +int SphereCalculus::closestVertexIndex(const QList &path, const QGeoCoordinate &coordinate) const +{ + if (path.size() == 0) { + qWarning("Path is empty!"); + return -1; + }else if (path.size() == 1) { + return 0; + }else { + int index = 0; // the index of the closest vertex + double min_dist = coordinate.distanceTo(path[index]); + for(int i = 1; i < path.size(); i++){ + double dist = coordinate.distanceTo(path[i]); + if (dist < min_dist){ + min_dist = dist; + index = i; + } + } + return index; + } +} + +/*! + * \fn QGeoCoordinate SphereCalculus::closestVertex(const QList &path, const QGeoCoordinate &coordinate) const + * Returns the vertex of the \a path with the least distance to \a coordinate. + * + * \sa QGeoCoordinate + */ +QGeoCoordinate SphereCalculus::closestVertex(const QList &path, const QGeoCoordinate &coordinate) const +{ + int index = closestVertexIndex(path, coordinate); + if (index >=0 ) { + return path[index]; + } else { + return QGeoCoordinate(); + } +} + + +/*! + * \fn SphereCalculus::JoinPolygonError SphereCalculus::joinPolygon(const SphereCalculus::QList &polygon1, const SphereCalculus::QList &polygon2, SphereCalculus::QList &joinedPolygon) const + * Joins \a polygon1 and \a polygon such that a \l {Simple Polygon} is created. + * Stores the result inside \a joinedArea. + * Stores error messages in \a errorString. + * Returns \c true if the algorithm was able to join the areas; false else. + * The algorithm will be able to join the areas, if either their edges intersect with each other, + * or one area contains the other. + */ +SphereCalculus::JoinPolygonError SphereCalculus::joinPolygon(const QList &polygon1, const QList &polygon2, QList &joinedPolygon) const +{ + if (polygon1.size() >= 3 && polygon2.size() >= 3) { + + if ( isSimplePolygon(area1) ) { + errorString.append("Area 1 is self intersecting.\n"); + return false; + } + if ( isSimplePolygon(area2) ) { + errorString.append("Area 2 is self intersecting.\n"); + return false; + } + + 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; + } + } + + + if ( crossContainsWalker == true) { + joinedArea.appendVertices(crossPoly->QList()); + 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 = protoCurrentVertex; + 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; + } + } + } + + } else { + return SphereCalculus::JoinPolygonError::PathSizeLow; + } } + + + diff --git a/src/Wima/SphericalGeometryCalculus.h b/src/Wima/SphericalGeometryCalculus.h index 3139352e4..63f0a0b68 100644 --- a/src/Wima/SphericalGeometryCalculus.h +++ b/src/Wima/SphericalGeometryCalculus.h @@ -1,17 +1,55 @@ -#ifndef SPHERICALGEOMETRYCALCULUS_H -#define SPHERICALGEOMETRYCALCULUS_H +#ifndef SphereCalculus_H +#define SphereCalculus_H #include +#include -class SphericalGeometryCalculus : public QObject +// Abstract class providing methods to do calculations on objects located on a sphere (e.g. earth) +class SphereCalculus : public QObject { Q_OBJECT public: - explicit SphericalGeometryCalculus(QObject *parent = nullptr); + SphereCalculus(QObject *parent = nullptr); + SphereCalculus(const SphereCalculus &other, QObject *parent = nullptr); + SphereCalculus &operator=(const SphereCalculus &other); + + + enum JoinPolygonError { NotSimplePolygon, Disjoint, NoError, PathSizeLow}; + enum IntersectionType { NoIntersection, EdgeIntersection, Inside}; + + // Property setters + void setEpsilonMeter(double epsilon); + + // Property getters + double epsilonMeter() const; + + // Member Methodes + int closestVertexIndex (const QList &path, const QGeoCoordinate &coordinate) const; + QGeoCoordinate closestVertex (const QList &path, const QGeoCoordinate &coordinate) const; + int nextVertexIndex (int index) const; + int previousVertexIndex (int index) const; + JoinPolygonError joinPolygon (const QList &polygon1, const QList &polygon2, + QList &joinedPolygon) const; + IntersectionType intersects (const QList &line1, const QList &line2, + QGeoCoordinate &intersectionPt)const; + IntersectionType intersects (const QList &polygon, const QList &line, + QList &intersectionList, QList> &neighbourList)const; + IntersectionType intersects (const QList &polygon, const QList &line) const; + bool dijkstraPath (const QList &polygon, const QGeoCoordinate& c1, + const QGeoCoordinate& c2, QList& dijkstraPath) const; + bool isSelfIntersecting (const QList &path); + + signals: public slots: + +private: + double distInsidePoly (const QGeoCoordinate& c1, const QGeoCoordinate& c2, QList polygon); + + double _epsilonMeter; // The accuracy used for distance calculations (unit: m). + }; -#endif // SPHERICALGEOMETRYCALCULUS_H \ No newline at end of file +#endif // SphereCalculus_H diff --git a/src/Wima/WimaArea.cc b/src/Wima/WimaArea.cc index ed5e233f1..2d936087c 100644 --- a/src/Wima/WimaArea.cc +++ b/src/Wima/WimaArea.cc @@ -221,7 +221,7 @@ bool WimaArea::join(WimaArea &area1, WimaArea &area2, WimaArea &joinedArea, QStr QGeoCoordinate protoCurrentVertex = intersectionList.value(minDistIndex); // take numerical erros into account if (protoCurrentVertex.distanceTo(currentVertex) > epsilonMeter) { - currentVertex = protoCurrentVertex; + currentVertex = protoCurrentVertexertexCoordinate; QPair neighbours = neighbourList.value(minDistIndex); protoNextVertex = crossPoly->vertexCoordinate(neighbours.second); nextVertexIndex = neighbours.second; -- 2.22.0