diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 0d746d3a982daa43efdd1020576f049d6d0bb06e..20c52d68aa939af3c5cfe4567ffee72e60082b20 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -428,7 +428,8 @@ HEADERS += \ src/Wima/CircularSurveyComplexItem.h \ src/Wima/PlanimetryCalculus.h \ src/Wima/Circle.h \ - src/Wima/PolygonCalculus.h + src/Wima/PolygonCalculus.h \ + src/Wima/OptimisationTools.h SOURCES += \ src/api/QGCCorePlugin.cc \ src/api/QGCOptions.cc \ @@ -455,7 +456,8 @@ SOURCES += \ src/Wima/CircularSurveyComplexItem.cc \ src/Wima/PlanimetryCalculus.cc \ src/Wima/Circle.cc \ - src/Wima/PolygonCalculus.cc + src/Wima/PolygonCalculus.cc \ + src/Wima/OptimisationTools.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 be1cc6f0cd5d09793d345a23a3526a5d6450d29e..6222e94f3b35fbe6f5ee05119553d37c1a0efdae 100644 --- a/src/Wima/CircularSurveyComplexItem.cc +++ b/src/Wima/CircularSurveyComplexItem.cc @@ -47,6 +47,8 @@ double CircularSurveyComplexItem::additionalTimeDelay() const void CircularSurveyComplexItem::_rebuildTransectsPhase1() { _transects.clear(); + + } void CircularSurveyComplexItem::_recalcComplexDistance() diff --git a/src/Wima/OptimisationTools.cc b/src/Wima/OptimisationTools.cc new file mode 100644 index 0000000000000000000000000000000000000000..0a866225b3057a18e0f8dedfda1242eb9fc9ba47 --- /dev/null +++ b/src/Wima/OptimisationTools.cc @@ -0,0 +1,99 @@ +#include "OptimisationTools.h" + +namespace OptimisationTools { + namespace { + + } // end anonymous namespace + + /*! + * \fn bool dijkstraAlgorithm(int startIndex, int endIndex, const QList elements, QList &elementPath, double(*distance)(const T &t1, const T &t2)) + * Calculates the shortest path between the elements stored in \a elements. + * The \l {Dijkstra Algorithm} is used to find the shorest path. + * Stores the result inside \a elementPath when sucessfull. + * The function handle \a distance is used to calculate the distance between two elements. The distance must be positive. + * Returns \c true if successful, \c false else. + * + * \sa QList + */ + template + bool dijkstraAlgorithm(const QList elements, int startIndex, int endIndex, QList &elementPath, double(*distance)(const T &t1, const T &t2)) // 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; + } + + // Each element of type T gets stuff into a Node + /// @param distance is the distance between the Node and it's predecessor + struct Node{ + T element; + double distance = std::numeric_limits::infinity(); + Node* predecessorNode = nullptr; + }; + + // The list with all Nodes (elements) + 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; + + //append elements to node list + for (int i = 0; i < elements.size(); i++) { + Node node; + node.element = elements[i]; + nodeList.append(node); + workingSet.append(&nodeList[i]); + } + + nodeList[startIndex].distance = 0; + + // 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]; + double dist = distance(u->element, v->element); + // 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 + + + // reverse assemble path + Node* node = &nodeList[endIndex]; + while (1) { + if (node == nullptr) { + if (elementPath[0] == elementPath[startIndex])// check if starting point was reached + break; + return false; + } + elementPath.prepend(node->element); + + //Update Node + node = node->predecessorNode; + + } + return true; + } +} // end OptimisationTools namespace diff --git a/src/Wima/OptimisationTools.h b/src/Wima/OptimisationTools.h new file mode 100644 index 0000000000000000000000000000000000000000..c1768c6abc5bf49e956de1febfc62e313784ad1b --- /dev/null +++ b/src/Wima/OptimisationTools.h @@ -0,0 +1,11 @@ +#ifndef OPTIMISATIONTOOLS_H +#define OPTIMISATIONTOOLS_H + +#include + +namespace OptimisationTools { + template + bool dijkstraAlgorithm(const QList elements, int startIndex, int endIndex, QList &elementPath, double(*distance)(const T &t1, const T &t2)); +} + +#endif // OPTIMISATIONTOOLS_H diff --git a/src/Wima/PlanimetryCalculus.cc b/src/Wima/PlanimetryCalculus.cc index ae418d0b239f62bf20df4fea19115b9deac07145..aa4b9c159fba8db3f31dda17e49f2637dc830b00 100644 --- a/src/Wima/PlanimetryCalculus.cc +++ b/src/Wima/PlanimetryCalculus.cc @@ -1,18 +1,18 @@ -#include "PlanimetryCalculus.h" +#include "PlanimetryCalculus.h" #include "Circle.h" namespace PlanimetryCalculus { namespace { /*! - \fn IntersectType intersects(const Circle &circle, const QLineF &line, QList &intersectionPoints, bool calcInstersect) + \fn IntersectType intersects(const Circle &circle, const QLineF &line, PointList &intersectionPoints, bool calcInstersect) Returns the Intersection type of \a circle and \a line. Stores the intersection points in \a intersectionPoints if \a calcIntersect is \c true. Returns \c Error if either line or circe \c {isNull() == true}. \sa QPointF, Circle */ - IntersectType intersects(const Circle &circle, const QLineF &line, QList &intersectionPoints, 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(); @@ -23,8 +23,10 @@ namespace PlanimetryCalculus { double y = originCircleL.y(); double r = circle.radius(); - if (qAbs(y) > r) - return NoIntersection; + if (qAbs(y) > r) { + type = NoIntersection; + return false; + } else if ( qFuzzyCompare(qFabs(y), r) ) { // tangent double x_ori = originCircleL.x(); @@ -35,11 +37,13 @@ namespace PlanimetryCalculus { intersectionPoints.append(intersectionPt + translationVector); } - return Tangent; + type = Tangent; + return true; } - return NoIntersection; - } else { // sekant + type = NoIntersection; + return false; + } else { // secant double x_ori = originCircleL.x(); double y_ori = originCircleL.y(); double delta = qSqrt(qPow(r, 2)-qPow(y_ori, 2)); @@ -63,12 +67,101 @@ namespace PlanimetryCalculus { } doesIntersect = true; } - - return doesIntersect ? Secant : NoIntersection; + type = doesIntersect ? Secant : NoIntersection; + return doesIntersect ? true : false; } } - return Error; + type = Error; + return false; + } + + /*! + \fn bool intersects(const Circle &circle1, const Circle &circle2, PointList &intersectionPoints, IntersectType type) + Calculates the intersection points of two circles if present and stores the result in \a intersectionPoints. + Returns the intersection type of the two cirles \a circle1 and \a circle2. + + The function assumes that the list \a intersectionPoints is empty. + + \note Returns Error if circle.isNull() returns true; + + \sa Circle + */ + bool intersects(const Circle &circle1, const Circle &circle2, QPointFList &intersectionPoints, IntersectType type, bool calcIntersection) + { + if (!circle1.isNull() && !circle2.isNull()) { + double r1 = circle1.radius(); + double r2 = circle2.radius(); + double d = distance(circle1.origin(), circle2.origin()); + double r = 0; + double R = 0; + if (r1 > r2) { + R = r1; // large + r = r2; // small + } else { + // this branch is also choosen if r1 == r2 + R = r2; + r = r1; + } + + // determine intersection type + if (r + d < R) { + // this branch is also reached if d < rLarge && rSmall == 0 + type = InsideNoIntersection; + return false; + } else if (qFuzzyCompare(r + d, R)) { + if (qFuzzyIsNull(d)) + { + type = CirclesEqual; + return true; + } + else { + type = InsideTouching; + } + } else if (d < R) { + type = InsideIntersection; + } else if (d - r < R) { + type = OutsideIntersection; + } else if (qFuzzyCompare(d - r, R)) { + type = OutsideTouching; + } else { + type = OutsideNoIntersection; + return false; + } + + + if (calcIntersection) { + // calculate intersection + + // Coordinate system circle1: origin = circle1.origin(), x-axis towars circle2.origin() y-axis such that the + // coordinate system is dextrorse with z-axis outward faceing with respect to the drawing plane. + double alpha = angle(circle1.origin(), circle2.origin()); // angle between world and circle1 system + QPointF translationVector = circle1.origin(); // translation vector between world and circle1 system + + if ( type == InsideTouching + || type == OutsideTouching) { + // Intersection point in coordinate system of circle 1. + // Coordinate system circle1: origin = circle1.origin(), x-axis towars circle2.origin() y-axis such that the + // coordinate system is dextrorse with z-axis outward faceing with respect to the drawing plane. + intersectionPoints.append(rotateReturn(QPointF(0, r1), alpha)+translationVector); + } else { //triggered if ( type == InsideIntersection + // || type == OutsideIntersection) + // See fist branch for explanation + + // this equations are obtained by solving x^2+y^2=R^2 and (x - d)^2+y^2=r^2 + double x = (qPow(d, 2) - qPow(r, 2) + qPow(R, 2))/2/d; + double y = 1/2/d*qSqrt(4*qPow(d*R, 2) - qPow(qPow(d, 2) - qPow(r, 2) + qPow(R, 2), 2)); + + intersectionPoints.append(rotateReturn(QPointF(x, y), alpha)+translationVector); + intersectionPoints.append(rotateReturn(QPointF(x, -y), alpha)+translationVector); + } + // Transform the coordinate to the world coordinate system. Alpha is the angle between world and circle1 coordinate system. + + return true; + } + } + type = Error; + return false; } } // end anonymous namespace @@ -87,7 +180,7 @@ namespace PlanimetryCalculus { } } - void rotateReference(QList &points, double alpha) + void rotateReference(QPointFList &points, double alpha) { for (int i = 0; i < points.size(); i++) { rotateReference(points[i], alpha); @@ -103,121 +196,13 @@ namespace PlanimetryCalculus { rotateReference(point, alpha/180*M_PI); } - void rotateReferenceDegree(QList &points, double alpha) + void rotateReferenceDegree(QPointFList &points, double alpha) { for (int i = 0; i < points.size(); i++) { rotateReferenceDegree(points[i], alpha); } } - /*! - \fn IntersectType intersects(const Circle &circle1, const Circle &circle2) - Returns the intersection type of the two cirles \a circle1 and \a circle2. - - \note Returns Error if circle.isNull() returns true; - - \sa Circle - */ - IntersectType intersects(const Circle &circle1, const Circle &circle2) - { - // r1 == 0 || r2 == 0 results in indefined behavior - if (!circle1.isNull() && !circle2.isNull()) { - double r1 = circle1.radius(); - double r2 = circle2.radius(); - double d = distance(circle1.origin(), circle2.origin()); - double r = 0; - double R = 0; - if (r1 > r2) { - R = r1; // large - r = r2; // small - } else { - // this branch is also choosen if r1 == r2 - R = r2; - r = r1; - } - - if (r + d < R) { - // this branch is also reached if d < rLarge && rSmall == 0 - return InsideNoIntersection; - } else if (qFuzzyCompare(r + d, R)) { - if (qFuzzyIsNull(d)) - return CirclesEqual; - else - return InsideTouching; - } else if (d < R) { - return InsideIntersection; - } else if (d - r < R) { - return OutsideIntersection; - } else if (qFuzzyCompare(d - r, R)) { - return OutsideTouching; - } else { - return OutsideNoIntersection; - } - } - return Error; - } - - /*! - \fn IntersectType intersects(const Circle &circle1, const Circle &circle2, QList intersectionPoints) - Calculates the intersection points of two circles if present and stores the result in \a intersectionPoints. - Returns the intersection type of the two cirles \a circle1 and \a circle2. - - The function assumes that the list \a intersectionPoints is empty. - - \note Returns Error if circle.isNull() returns true; - - \sa Circle - */ - IntersectType intersects(const Circle &circle1, const Circle &circle2, QList &intersectionPoints) - { - IntersectType returnValue = intersects(circle1, circle2); - - if ( returnValue == InsideNoIntersection - || returnValue == OutsideNoIntersection - || returnValue == CirclesEqual - || returnValue == Error ) { - return returnValue; // No intersection Points, or infinitly many (in case of CirclesEqual). - } else { - double r1 = circle1.radius(); - double r2 = circle2.radius(); - double d = distance(circle1.origin(), circle2.origin()); - double alpha = angle(circle1.origin(), circle2.origin()); - double r = 0; - double R = 0; - if (r1 > r2) { - R = r1; - r = r2; - } else { - // this branch is also choosen if r1 == r2 - R = r2; - r = r1; - } - - if ( returnValue == InsideTouching - || returnValue == OutsideTouching) { - // Intersection point in coordinate system of circle 1. - // Coordinate system circle1: origin = circle1.origin(), x-axis towars circle2.origin() y-axis such that the - // coordinate system is dextrorse with z-axis outward faceing with respect to the drawing plane. - intersectionPoints.append(QPointF(0, r1)); - } else { //triggered if ( returnValue == InsideIntersection - // || returnValue == OutsideIntersection) - // See fist branch for explanation - - // this equations are obtained by solving x^2+y^2=R^2 and (x - d)^2+y^2=r^2 - double x = (qPow(d, 2) - qPow(r, 2) + qPow(R, 2))/2/d; - double y = 1/2/d*qSqrt(4*qPow(d*R, 2) - qPow(qPow(d, 2) - qPow(r, 2) + qPow(R, 2), 2)); - - intersectionPoints.append(QPointF(x, y)); - intersectionPoints.append(QPointF(x, -y)); - } - // Transform the coordinate to the world coordinate system. Alpha is the angle between world and circle1 coordinate system. - rotateReference(intersectionPoints, alpha); - - return returnValue; - } - } - - /*! \fn IntersectType intersects(const Circle &circle, const QLineF &line) Returns the Intersection type of \a circle and \a line. @@ -225,15 +210,17 @@ namespace PlanimetryCalculus { \sa QPointF, Circle */ - IntersectType intersects(const Circle &circle, const QLineF &line) + bool intersects(const Circle &circle, const QLineF &line) { - QList dummyList; - return intersects(circle, line, dummyList, false /* calculate intersection points*/); + QPointFList dummyList; + IntersectType type = NoIntersection; + return intersects(circle, line, dummyList, type, false /* calculate intersection points*/); } - IntersectType intersects(const Circle &circle, const QLineF &line, QList &intersectionPoints) + bool intersects(const Circle &circle, const QLineF &line, QPointFList &intersectionPoints) { - return intersects(circle, line, intersectionPoints, true /* calculate intersection points*/); + IntersectType type = NoIntersection; + return intersects(circle, line, intersectionPoints, type, true /* calculate intersection points*/); } /*! @@ -308,12 +295,15 @@ namespace PlanimetryCalculus { * \c Error; Returned if at least on line delivers isNULL() == true. * * - * \sa QGeoCoordinate + * \sa QPointF */ - IntersectType intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt) + bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt, IntersectType &type) { - if (line1.isNull() || line2.isNull()) - return Error; + if (line1.isNull() || line2.isNull()){ + type = Error; + return false; + } + // line 1 coordinate system: origin line1.p1(), x-axis towards line1.p2() QPointF translationVector = line1.p1(); // translation vector between world and line1 system @@ -336,13 +326,16 @@ namespace PlanimetryCalculus { double k = dy/dx; if (qFuzzyIsNull(k)) { - if (qFuzzyIsNull(x1) && qFuzzyIsNull(y1) && qFuzzyCompare(x2, l1)) - return LinesEqual; - else - return LinesParallel; + if (qFuzzyIsNull(x1) && qFuzzyIsNull(y1) && qFuzzyCompare(x2, l1)){ + type = LinesEqual; + return true; + } + else { + type = LinesParallel; + return false; + } } - double d = (y1*x2-y2*x1)/dx; xNull = -d/k; } else { @@ -350,39 +343,43 @@ namespace PlanimetryCalculus { if (signum(y1) != signum(y2)){ xNull = x1; } - else - return NoIntersection; + else { + type = NoIntersection; + return false; + } } - IntersectType returnValue; - if (xNull >= x1 && xNull <= x2){ // determine intersection type if(qFuzzyIsNull(xNull) || qFuzzyCompare(xNull, l1)) { if(qFuzzyIsNull(y1) || qFuzzyIsNull(y2)) - returnValue = CornerCornerIntersection; + type = CornerCornerIntersection; else - returnValue = EdgeCornerIntersection; + type = EdgeCornerIntersection; } else if (xNull > 0 && xNull < l1) { if(qFuzzyIsNull(y1) || qFuzzyIsNull(y2)) - returnValue = EdgeCornerIntersection; + type = EdgeCornerIntersection; else - returnValue = EdgeEdgeIntersection; - } else - return NoIntersection; - } else - return NoIntersection; + type = EdgeEdgeIntersection; + } else { + type = NoIntersection; + return false; + } + } else { + type = NoIntersection; + return false; + } intersectionPt = QPointF(xNull, 0); // intersection point in line1 system rotateReference(intersectionPt, alpha); intersectionPt += translationVector; - return returnValue; + return true; } - /*! - * \overload QList intersects(const QList &polygon, const QList &line, QList &intersectionList, QList > &neighbourList) + /*!IntersectType type = NoIntersection; + * \overload bool intersects(const QPolygonF &polygon, const QLineF &line, PointList &intersectionList, NeighbourList &neighbourList, IntersectList &typeList) * Checks if \a polygon intersect with \a line. * Stores the intersection points in \a intersectionList. * @@ -396,56 +393,54 @@ namespace PlanimetryCalculus { * * \sa QPair, QList */ - QList intersects(const QList &polygon, const Line &line, QList &intersectionList, QList > &neighbourList) + bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList, NeighbourList &neighbourList, IntersectList &typeList) { - if (polygon.size() >= 3) { // are line a proper line and poly a proper poly?other, - intersectionList.clear(); + + if (polygon.size() >= 2) { neighbourList.clear(); - QList intersectionTypeList; + 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++) { - Line interatorLine; - QGeoCoordinate currentVertex = polygon[i]; - QGeoCoordinate nextVertex = polygon[nextPolygonIndex(polygon.size(), i)]; - interatorLine.first = currentVertex; - interatorLine.second = nextVertex; + QLineF interatorLine; + QPointF currentVertex = polygon[i]; + QPointF nextVertex = polygon[PolygonCalculus::nextVertexIndex(polygon.size(), i)]; + interatorLine.setP1(currentVertex); + interatorLine.setP2(nextVertex); - QGeoCoordinate intersectionPoint; - IntersectionType returnValue = intersects(line, interatorLine, intersectionPoint); - if ( returnValue == IntersectionType::EdgeIntersection - || returnValue == IntersectionType::InteriorIntersection) { + QPointF intersectionPoint; + IntersectType type; + if ( intersects(line, interatorLine, intersectionPoint, type) ) { intersectionList.append(intersectionPoint); QPair neighbours; neighbours.first = i; - neighbours.second = nextPolygonIndex(polygon.size(), i); + neighbours.second = PolygonCalculus::nextVertexIndex(polygon.size(), i); neighbourList.append(neighbours); - intersectionTypeList.append(returnValue); + typeList.append(type); } } - if (intersectionList.count() > 0) { - return intersectionTypeList; + if (typeList.size() > 0) { + return true; } else { - return QList(); + return false; } } else { - qWarning("WimaArea::intersects(line, poly): line->count() != 2 || poly->count() < 3"); - return QList(); + return false; } } /*! - * \overload bool intersects(const QList &polygon, const QList &line) + * \overload IntersectType intersects(const QPolygonF &polygon, const QLineF &line) * Returns \c true if any intersection between \a polygon and \a line exists, \c false else. * * \sa QPair, QList */ - bool intersects(const QList &polygon, const Line &line) + bool intersects(const QPolygonF &polygon, const QLineF &line) { - QList dummyGeo; + QPointFList dummyGeo; QList> dummyNeighbour; intersects(polygon, line, dummyGeo, dummyNeighbour); @@ -467,7 +462,7 @@ namespace PlanimetryCalculus { return point; } - QList rotateReturn(QList points, double alpha) + QPointFList rotateReturn(QPointFList points, double alpha) { rotateReference(points, alpha); return points; @@ -490,6 +485,75 @@ namespace PlanimetryCalculus { return (T(0) < val) - (val < T(0)); } + bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt) + { + IntersectType dummyType; + return intersects(line1, line2, intersectionPt, dummyType); + } + + bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList, NeighbourList &neighbourList) + { + IntersectList typeList; + return intersects(polygon, line, intersectionList, neighbourList, typeList); + } + + bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList, IntersectList &typeList) + { + NeighbourList neighbourList; + return intersects(polygon, line, intersectionList, neighbourList, typeList); + } + + bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList) + { + NeighbourList neighbourList; + IntersectList typeList; + return intersects(polygon, line, intersectionList, neighbourList, typeList); + } + + /*! + \fn IntersectType intersects(const Circle &circle1, const Circle &circle2) + Returns the intersection type of the two cirles \a circle1 and \a circle2. + + \note Returns Error if circle.isNull() returns true; + + \sa Circle + */ + bool intersects(const Circle &circle1, const Circle &circle2) + { + IntersectType type = NoIntersection; + QPointFList intersectionPoints; + return intersects(circle1, circle2, intersectionPoints, type, false /*calculate intersection points*/); + } + + bool intersects(const Circle &circle1, const Circle &circle2, IntersectType &type) + { + QPointFList intersectionPoints; + return intersects(circle1, circle2, intersectionPoints, type, false /*calculate intersection points*/); + } + + bool intersects(const Circle &circle1, const Circle &circle2, QPointFList &intersectionPoints) + { + IntersectType type; + return intersects(circle1, circle2, intersectionPoints, type); + } + + bool intersects(const Circle &circle1, const Circle &circle2, QPointFList &intersectionPoints, IntersectType &type) + { + return intersects(circle1, circle2, intersectionPoints, type, true /*calculate intersection points*/); + } + + + + + + + + + + + + + } // end namespace PlanimetryCalculus diff --git a/src/Wima/PlanimetryCalculus.h b/src/Wima/PlanimetryCalculus.h index 3491f9a9ecb8b6d665459a3b48cc482d5ec5ad92..b649b6ce23b75a57f4e750b1fd5b2ca574def851 100644 --- a/src/Wima/PlanimetryCalculus.h +++ b/src/Wima/PlanimetryCalculus.h @@ -6,6 +6,8 @@ #include #include +#include "PolygonCalculus.h" + class Circle; namespace PlanimetryCalculus { @@ -14,33 +16,44 @@ namespace PlanimetryCalculus { OutsideIntersection, OutsideTouching, OutsideNoIntersection, CirclesEqual, //Circle Circle intersection - NoIntersection, Tangent, Secant, // Circle Line Intersetion + Tangent, Secant, // Circle Line Intersetion EdgeCornerIntersection, EdgeEdgeIntersection, CornerCornerIntersection, LinesParallel, LinesEqual, // Line Line intersection - Error // general + NoIntersection, Error // general }; + typedef QList> NeighbourList; + typedef QList QPointFList; + typedef QList IntersectList; + void rotateReference(QPointF &point, double alpha); - void rotateReference(QList &points, double alpha); + void rotateReference(QPointFList &points, double alpha); void rotateReference(QLineF &line, double alpha); //void rotateReference(QPolygonF &polygon, double alpha); - QPointF rotateReturn(QPointF point, double alpha); - QList rotateReturn(QList points, double alpha); - QLineF rotateReturn(QLineF line, double alpha); + QPointF rotateReturn(QPointF point, double alpha); + QPointFList rotateReturn(QPointFList points, double alpha); + QLineF rotateReturn(QLineF line, double alpha); //QPolygonF rotateReturn(QPolygonF &polygon, double alpha); - IntersectType intersects(const Circle &circle1, const Circle &circle2); - IntersectType intersects(const Circle &circle1, const Circle &circle2, QList &intersectionPoints); - IntersectType intersects(const Circle &circle, const QLineF &line); - IntersectType intersects(const Circle &circle, const QLineF &line, QList &intersectionPoints); - - - IntersectType intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt); - QList intersects(const QPolygonF &polygon, const QLineF &line, QList &intersectionList, QList> &neighbourList); - IntersectType intersects(const QPolygonF &polygon, const QLineF &line); + bool intersects(const Circle &circle1, const Circle &circle2); + bool intersects(const Circle &circle1, const Circle &circle2, IntersectType &type); + bool intersects(const Circle &circle1, const Circle &circle2, QPointFList &intersectionPoints); + bool intersects(const Circle &circle1, const Circle &circle2, QPointFList &intersectionPoints, IntersectType &type); + bool intersects(const Circle &circle, const QLineF &line); + bool intersects(const Circle &circle, const QLineF &line, IntersectType &type); + bool intersects(const Circle &circle, const QLineF &line, QPointFList &intersectionPoints); + bool intersects(const Circle &circle, const QLineF &line, QPointFList &intersectionPoints, IntersectType &type); + + + bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt); + bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt, IntersectType &type); + bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList); + bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList, IntersectList &typeList); + bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList, NeighbourList &neighbourList); + bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList, NeighbourList &neighbourList, IntersectList &typeList); double distance(const QPointF &p1, const QPointF p2); double angle(const QPointF &p1, const QPointF p2); diff --git a/src/Wima/PolygonCalculus.cc b/src/Wima/PolygonCalculus.cc index 8d3d03c25c18275ed35b6913447c9990bb737ff1..83b5638fb89461c6f116b1da53e02b6ad9d376fa 100644 --- a/src/Wima/PolygonCalculus.cc +++ b/src/Wima/PolygonCalculus.cc @@ -2,9 +2,52 @@ namespace PolygonCalculus { namespace { + bool isReflexVertex(const QPolygonF& polygon, const QPointF *vertex) { + // Original Code from SurveyComplexItem::_VertexIsReflex() + auto vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1; + auto vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1; + auto area = ( ((vertex->x() - vertexBefore->x())*(vertexAfter->y() - vertexBefore->y())) + -((vertexAfter->x() - vertexBefore->x())*(vertex->y() - vertexBefore->y()))); + return area > 0; + } } // end anonymous namespace + /*! + * \fn bool containsPath(QPolygonF polygon, const QPointF &c1, const QPointF &c2) + * Returns true if the shortest path between the two coordinates is not fully inside the \a area. + * + * \sa QPointF, QPolygonF + */ + bool containsPath(QPolygonF polygon, const QPointF &c1, const QPointF &c2) + { + if ( !polygon.isEmpty()) { + if ( !polygon.containsPoint(c1, Qt::FillRule::OddEvenFill) + || !polygon.containsPoint(c2, Qt::FillRule::OddEvenFill)) + return false; + + QList intersectionList; + QLineF line; + line.setP1(c1); + line.setP2(c2); + PlanimetryCalculus::IntersectList intersectTypeList; + + bool retValue = PlanimetryCalculus::intersects(polygon, line, intersectionList, intersectTypeList); + if (!retValue) { + for (int i = 0; i < intersectTypeList.size(); i++) { + PlanimetryCalculus::IntersectType type = intersectTypeList[i]; + if ( type == PlanimetryCalculus::EdgeEdgeIntersection + || type == PlanimetryCalculus::Error) + return false; + } + + } + return true; + } else { + return false; + } + } + /*! * \fn int closestVertexIndex(const QPolygonF &polygon, const QPointF &coordinate) * Returns the vertex index of \a polygon which has the least distance to \a coordinate. @@ -54,7 +97,7 @@ namespace PolygonCalculus { * 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 nextPolygonIndex(int pathsize, int index) + int nextVertexIndex(int pathsize, int index) { if (index >= 0 && index < pathsize-1) { return index + 1; @@ -72,7 +115,7 @@ namespace PolygonCalculus { * 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 previousPolygonIndex(int pathsize, int index) + int previousVertexIndex(int pathsize, int index) { if (index > 0 && index value(startIndex); QPointF startVertex = currentVertex; // possible nextVertex (if no intersection between currentVertex and protoVertex with crossPoly) - int nextVertexIndex = nextPolygonIndex(walkerPoly->size(), startIndex); + int nextVertexIndex = nextVertexIndex(walkerPoly->size(), startIndex); QPointF protoNextVertex = walkerPoly->value(nextVertexIndex); bool switchHappenedPreviously = false; // means switch between crossPoly and walkerPoly while (1) { @@ -150,7 +193,7 @@ namespace PolygonCalculus { //qDebug("IntersectionList.size(): %i", intersectionList.size()); - if (intersectionList.size() >= 1) { + if (intersectionList.size() >= 1) { bool containsPath (const QPointF &c1, const QPointF &c2, QPolygonF polygon); int minDistIndex = 0; // find the vertex with the least distance to currentVertex @@ -188,15 +231,15 @@ namespace PolygonCalculus { switchHappenedPreviously = true; } else { currentVertex = walkerPoly->value(nextVertexIndex); - nextVertexIndex = nextPolygonIndex(walkerPoly->size(), nextVertexIndex); + nextVertexIndex = nextVertexIndex(walkerPoly->size(), nextVertexIndex); protoNextVertex = walkerPoly->value(nextVertexIndex); switchHappenedPreviously = false; } } else { - currentVertex = walkerPoly->value(nextVertexIndex); - nextVertexIndex = nextPolygonIndex(walkerPoly->size(), nextVertexIndex); + currentVertex = walkerPoly->value(nex bool containsPath (const QPointF &c1, const QPointF &c2, QPolygonF polygon);tVertexIndex); + nextVertexIndex = nextVertexIndex(walkerPoly->size(), nextVertexIndex); protoNextVertex = walkerPoly->value(nextVertexIndex); } @@ -225,22 +268,33 @@ namespace PolygonCalculus { if (polygon.size() > 3) { // check if any edge of the area (formed by two adjacent vertices) intersects with any other edge of the area while(i < polygon.size()-1) { + double cCIntersectCounter = 0; // corner corner intersection counter QPointF refBeginCoordinate = polygon[i]; - QPointF refEndCoordinate = polygon[nextPolygonIndex(polygon.size(), i)]; + QPointF refEndCoordinate = polygon[nextVertexIndex(polygon.size(), i)]; QLineF refLine; refLine.setP1(refBeginCoordinate); refLine.setP2(refEndCoordinate); - int j = nextPolygonIndex(polygon.size(), i); + int j = nextVertexIndex(polygon.size(), i); while(j < polygon.size()) { QPointF intersectionPt; QLineF iteratorLine; iteratorLine.setP1(polygon[j]); - iteratorLine.setP2(polygon[nextPolygonIndex(polygon.size(), j)]); - if ( PlanimetryCalculus::intersects(refLine, iteratorLine, intersectionPt) - == PlanimetryCalculus::InteriorIntersection){ - return false; + iteratorLine.setP2(polygon[nextVertexIndex(polygon.size(), j)]); + PlanimetryCalculus::IntersectType intersectType; + PlanimetryCalculus::intersects(refLine, iteratorLine, intersectionPt, intersectType); + if ( intersectType == PlanimetryCalculus::CornerCornerIntersection) { + cCIntersectCounter++; + // max two corner corner intersections allowed, a specific coordinate can appear only once in a simple polygon + } + if ( cCIntersectCounter > 2 + || intersectType == PlanimetryCalculus::EdgeEdgeIntersection + || intersectType == PlanimetryCalculus::EdgeCornerIntersection + || intersectType == PlanimetryCalculus::LinesEqual + || intersectType == PlanimetryCalculus::Error){ + return false; } + j++; } i++; @@ -262,7 +316,7 @@ namespace PolygonCalculus { double sum = 0; for (int i=0; i 2) { + + // Walk the edges, offsetting by the specified distance + QList rgOffsetEdges; + for (int i = 0; i < polygon.size(); i++) { + int nextIndex = nextVertexIndex(polygon.size(), i); + QLineF offsetEdge; + QLineF originalEdge(polygon[i], polygon[nextIndex]); + + QLineF workerLine = originalEdge; + workerLine.setLength(offset); + workerLine.setAngle(workerLine.angle() - 90.0); + offsetEdge.setP1(workerLine.p2()); + + workerLine.setPoints(originalEdge.p2(), originalEdge.p1()); bool containsPath (const QPointF &c1, const QPointF &c2, QPolygonF polygon); + workerLine.setLength(offset); + workerLine.setAngle(workerLine.angle() + 90.0); + offsetEdge.setP2(workerLine.p2()); + + rgOffsetEdges << offsetEdge; + } + // Intersect the offset edges to generate new vertices + polygon.clear(); + QPointF newVertex; + for (int i=0; i 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 distanceInsidePolygon(const QPointF &c1, const QPointF &c2, QPolygonF polygon) + void decomposeToConvex(const QPolygonF &polygon, QList &convexPolygons) { - 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; - QLineF line; + // Original Code SurveyComplexItem::_PolygonDecomposeConvex() + // this follows "Mark Keil's Algorithm" https://mpen.ca/406/keil + int decompSize = std::numeric_limits::max(); + if (polygon.size() < 3) return; + if (polygon.size() == 3) { + convexPolygons << polygon; + return; + } - line.setP1(c1); - line.setP2(c2); - PlanimetryCalculus::intersects(polygon, line, intersectionList, neighbourlist); + QList decomposedPolygonsMin{}; + + for (const QPointF *vertex = polygon.begin(); vertex != polygon.end(); ++vertex) + { + // is vertex reflex? + bool vertexIsReflex = isReflexVertex(polygon, vertex); + + if (!vertexIsReflex) continue; + + for (const QPointF *vertexOther = polygon.begin(); vertexOther != polygon.end(); ++vertexOther) + { + const QPointF *vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1; + const QPointF *vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1; + if (vertexOther == vertex) continue; + if (vertexAfter == vertexOther) continue; + if (vertexBefore == vertexOther) continue; + bool canSee = containsPath(polygon, *vertex, *vertexOther); + if (!canSee) continue; + + QPolygonF polyLeft; + const QPointF *v = vertex; + bool polyLeftContainsReflex = false; + while ( v != vertexOther) { + if (v != vertex && isReflexVertex(polygon, v)) { + polyLeftContainsReflex = true; + } + polyLeft << *v; + ++v; + if (v == polygon.end()) v = polygon.begin(); + } + polyLeft << *vertexOther; + bool polyLeftValid = !(polyLeftContainsReflex && polyLeft.size() == 3); + + QPolygonF polyRight; + v = vertexOther; + bool polyRightContainsReflex = false; + while ( v != vertex) { + if (v != vertex && isReflexVertex(polygon, v)) { + polyRightContainsReflex = true; + } + polyRight << *v; + ++v; + if (v == polygon.end()) v = polygon.begin(); + } + polyRight << *vertex; + auto polyRightValid = !(polyRightContainsReflex && polyRight.size() == 3); + + if (!polyLeftValid || ! polyRightValid) { + // decompSize = std::numeric_limits::max(); + continue; + } + + // recursion + QList polyLeftDecomposed{}; + decomposeToConvex(polyLeft, polyLeftDecomposed); - if ( intersectionList.size() == 0 ){ // if an intersection was found the path between c1 and c2 is not fully inside polygon. - return PlanimetryCalculus::distance(c1, c2); - } else { - return std::numeric_limits::infinity(); + QList polyRightDecomposed{}; + decomposeToConvex(polyRight, polyRightDecomposed); + + // compositon + int subSize = polyLeftDecomposed.size() + polyRightDecomposed.size(); + if ( (polyLeftContainsReflex && polyLeftDecomposed.size() == 1) + || (polyRightContainsReflex && polyRightDecomposed.size() == 1)) + { + // don't accept polygons that contian reflex vertices and were not split + subSize = std::numeric_limits::max(); + } + if (subSize < decompSize) { + decompSize = subSize; + decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed; + } } + } + + // assemble output + if (decomposedPolygonsMin.size() > 0) { + convexPolygons << decomposedPolygonsMin; } else { - return std::numeric_limits::infinity(); + convexPolygons << polygon; } + + return; } + + bool shortestPath(const QPolygonF &polygon, const QPointF &startVertex, const QPointF &endVertex, QList &shortestPath) + { + if ( polygon.containsPoint(startVertex, Qt::FillRule::OddEvenFill) + && polygon.containsPoint(endVertex, Qt::FillRule::OddEvenFill)) { + // lambda + auto distance = [polygon](const QPointF &p1, const QPointF &p2){ + if (containsPath(polygon, p1, p2)){ + double dx = p1.x()-p2.x(); + double dy = p1.y()-p2.y(); + return sqrt(dx*dx+dy*dy); + } else + return std::numeric_limits::infinity(); + }; + + QList elementList; + elementList.append(startVertex); + elementList.append(endVertex); + for (int i = 0; i < polygon.size(); i++) { + elementList.append(polygon[i]); + } + + + OptimisationTools::dijkstraAlgorithm(elementList, 0, 1, shortestPath, distance); + + + } else { + return false; + } + } + + } // end PolygonCalculus namespace diff --git a/src/Wima/PolygonCalculus.h b/src/Wima/PolygonCalculus.h index 49f250db6cfa912896b8a364c7246db58801148e..4f903c002fd600b90bc013a88bb3dd269fffe596 100644 --- a/src/Wima/PolygonCalculus.h +++ b/src/Wima/PolygonCalculus.h @@ -1,11 +1,13 @@ #ifndef POLYGONCALCULUS_H #define POLYGONCALCULUS_H +#endif #include #include #include "PlanimetryCalculus.h" +#include "OptimisationTools.h" namespace PolygonCalculus { @@ -13,14 +15,16 @@ namespace PolygonCalculus { int closestVertexIndex (const QPolygonF &polygon, const QPointF &coordinate); QPointF closestVertex (const QPolygonF &polygon, const QPointF &coordinate); - int nextPolygonIndex (int pathsize, int index); - int previousPolygonIndex(int pathsize, int index); + int nextVertexIndex (int pathsize, int index); + int previousVertexIndex (int pathsize, int index); JoinPolygonError joinPolygon (QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon); bool isSimplePolygon (const QPolygonF &polygon); bool hasClockwiseWinding (const QPolygonF &path); void reversePath (QPolygonF &path); - bool offsetPolygon (QPolygonF &polygon, double offset); - double distanceInsidePolygon (const QPointF &c1, const QPointF &c2, QPolygonF polygon); + void offsetPolygon (QPolygonF &polygon, double offset); + 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); } -#endif // POLYGONCALCULUS_H +