debugging, containsPoint in dijkstra not working

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)

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<MissionItem *> &items, QObject *missionItemParent)

src/Wima/GeoUtilities.cc

 #include "GeoUtilities.h"
-#include <QtMath>
 
 
-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;
+    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 toCartesian(const QGeoCoordinate &coordinate, const QGeoCoordinate &origin)
+    {
+
+        double h   = coordinate.altitude();
+        double hO   = origin.altitude();
+
+        QVector3D point(toCartesian2D(coordinate, origin));
+        point.setZ(h - hO);
+
+        return  point;
+    }
+
+    QGeoList toGeo(const QVector3DList &points, const QGeoCoordinate &origin)
+    {
+        QGeoList coordinates;
+        for (auto point : points)
+            coordinates.append(toGeo(point, origin));
+
+        return coordinates;
+    }
+
+    QVector3DList toCartesian(const QGeoList &coordinates, const QGeoCoordinate &origin)
+    {
+        QVector3DList points;
+        for (auto coordinate : coordinates )
+            points.append(toCartesian(coordinate, origin));
 
-QVector3D GeoUtilites::toCartesian(const QGeoCoordinate &point, const QGeoCoordinate &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 h   = point.altitude();
 
         double latO = origin.latitude()/180*M_PI;
         double lonO = origin.longitude()/180*M_PI;
-    double hO   = origin.altitude();
 
         double dlon = lon-lonO;
         double dlat = lat-latO;
 
 
         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);
+            return QPointF(/* x */ qTan(dlon)*earthRadius*qCos(latO),
+                           /* y */ qTan(dlat)*earthRadius);
         else
-        return QVector3D(); // singularity occurred (tan(pi/2) = inf)
+            return QPointF(); // singularity occurred (tan(pi/2) = inf)
+    }
 
-}
+    QPointFList toCartesian2D(const QGeoList &coordinates, const QGeoCoordinate &origin)
+    {
+        QPointFList listF;
 
-GeoUtilites::QGeoList GeoUtilites::toGeo(const GeoUtilites::QVector3DFList &points, const QGeoCoordinate &origin)
-{
-    GeoUtilites::QGeoList coordinates;
-    for (auto point : points)
+        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;
-}
+    }
 
-GeoUtilites::QVector3DFList GeoUtilites::toCartesian(const GeoUtilites::QGeoList &coordinates, const QGeoCoordinate &origin)
-{
-    GeoUtilites::QVector3DFList points;
-    for (auto coordinate : coordinates )
-        points.append(toCartesian(coordinate, origin));
-
-    return points;
 }
+

src/Wima/GeoUtilities.h

-#ifndef GEOPOLYGONUTILITIES_H
-#define GEOPOLYGONUTILITIES_H
+#pragma once
 
 #include <QPointF>
 #include <QGeoCoordinate>
 #include <QVector3D>
 #include <QGeoCoordinate>
+#include <QtMath>
 
+#include "PolygonCalculus.h"
 
-namespace GeoUtilites {
 
-    typedef QList<QVector3D> QVector3DFList;
+namespace GeoUtilities {
+
+    typedef QList<QVector3D> QVector3DList;
+    typedef QList<QPointF>   QPointFList;
     typedef QList<QGeoCoordinate> 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);
+    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
+

src/Wima/OptimisationTools.cc

 #include "OptimisationTools.h"
+#include <QPointF>
 
 namespace OptimisationTools {
     namespace  {
@@ -15,15 +16,15 @@ namespace OptimisationTools {
      *
      * \sa QList
      */
-    template <typename T>
-    bool dijkstraAlgorithm(const QList<T> elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> distance) // don't seperate parameters with new lines or documentation will break
+    typedef QPointF T;
+    bool dijkstraAlgorithm(const QList<T> &elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> 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{

src/Wima/OptimisationTools.h

-#ifndef OPTIMISATIONTOOLS_H
-#define OPTIMISATIONTOOLS_H
-
+#pragma once
 #include <QObject>
 
 #include <functional>
+#include <QPointF>
 
 namespace OptimisationTools {
-    template <typename T>
-    bool dijkstraAlgorithm(const QList<T> elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> distance);
+    typedef QPointF T;
+    bool dijkstraAlgorithm(const QList<T> &elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> distance);
 }
 
-#endif // OPTIMISATIONTOOLS_H
+

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());
+    }
 
 
 

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);

src/Wima/PolygonCalculus.cc

 #include "PolygonCalculus.h"
+#include "PlanimetryCalculus.h"
+#include "OptimisationTools.h"
+
+#include <QVector3D>
+
+#include <functional>
 
 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<QLineF> 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<double(const QPointF &, const QPointF &)> 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<QPointF>(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
 

src/Wima/PolygonCalculus.h

-#ifndef POLYGONCALCULUS_H
-#define POLYGONCALCULUS_H
-#endif
+#pragma once
 
 
 #include <QPointF>
 #include <QPolygonF>
+#include <QVector3D>
+
 
-#include "PlanimetryCalculus.h"
-#include "OptimisationTools.h"
 
 namespace PolygonCalculus {
 
     enum JoinPolygonError { NotSimplePolygon, PolygonJoined, Disjoint, PathSizeLow};
 
+    typedef QList<QVector3D> QVector3DList;
+    typedef QList<QPointF> 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<QPolygon> &convexPolygons);
     bool             shortestPath        (const QPolygonF &polygon, const QPointF &startVertex, const QPointF &endVertex, QList<QPointF> &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);
 }
 
 

src/Wima/WimaArea.cc

-#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) {
-
-            if ( isSelfIntersecting(area1) ) {
-                errorString.append("Area 1 is self intersecting.\n");
-                return false;
-            }
-
-            if ( isSelfIntersecting(area2) ) {
-                errorString.append("Area 2 is self intersecting.\n");
-                return false;
-            }
-
-            joinedArea.clear();
-
-            area1.verifyClockwiseWinding();
-            area2.verifyClockwiseWinding();
+    QList<QGeoCoordinate> GeoPolygon1 = area1.coordinateList();
+    QList<QGeoCoordinate> GeoPolygon2 = area2.coordinateList();
 
-            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;
-                }
-            }
+    /*qWarning("befor joining");
+    qWarning() << GeoPolygon1;
+    qWarning() << GeoPolygon2;*/
 
+    QGeoCoordinate origin = GeoPolygon1[0];
 
-            if ( crossContainsWalker == true) {
-                joinedArea.appendVertices(crossPoly->coordinateList());
-                return true;
-            }
+    QGeoCoordinate tset = GeoPolygon1[2];
 
+    //qWarning() << tset;qWarning() << toGeo(toCartesian2D(tset, origin), origin);
 
-            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);
+    QPolygonF polygon1 = toQPolygonF(toCartesian2D(GeoPolygon1, origin));
+    QPolygonF polygon2 = toQPolygonF(toCartesian2D(GeoPolygon2, origin));
 
-                QList<QPair<int, int>> neighbourList;
-                QList<QGeoCoordinate> intersectionList;
-                //qDebug("IntersectionList.size() on init: %i", intersectionList.size());
-                intersects(walkerPolySegment, *crossPoly, intersectionList, neighbourList);
+    /*qWarning("after 1 transform");
+    qWarning() << polygon1;
+    qWarning() << polygon2;*/
 
-                //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;
-                            }
-                        }
-                    }
+    QPolygonF joinedPolygon;
+    JoinPolygonError retValue = PolygonCalculus::join(polygon1, polygon2, joinedPolygon);
 
-                    //qDebug("MinDistIndex: %i", minDistIndex);
-                    QGeoCoordinate protoCurrentVertex = intersectionList.value(minDistIndex);
-                    // take numerical erros into account
-                    if (protoCurrentVertex.distanceTo(currentVertex) > epsilonMeter) {
-                        currentVertex                   = protoNextVertex;
-                        QPair<int, int> 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);
-                }
+    /*qWarning("after joining");
+    qWarning() << joinedPolygon;*/
 
-                if (currentVertex == startVertex) {
-                    if (area1.count() == joinedArea.count()) { // is the case if poly1 and poly2 don't intersect
-                        return false;
+    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<QGeoCoordinate> path = toGeo(toQPointFList(joinedPolygon), origin);
+        //qWarning("after transform");
+        //qWarning() << path;
+        joinedArea.setPath(path);
         return true;
     }
-                }
-            }
 
-        } else {
     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<QGeoCoordinate> &intersectionList, QList<QPair<int, int>> &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<QGeoCoordinate> &intersectionList, QList<QPair<int, int>> &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<int, int>}, 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<QGeoCoordinate> &intersectionList, QList<QPair<int, int>> &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<int, int>     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<QGeoCoordinate>   intersectionList;
-            QList<QPair<int, int>>  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<qreal>::infinity();
-            }
-
-        } else {
-            return std::numeric_limits<qreal>::infinity();
-        }
-}
-
-/*!
- * \fn bool WimaArea::dijkstraPath(const QGeoCoordinate &start, const QGeoCoordinate &end, const WimaArea &area, QList<QGeoCoordinate> &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<QGeoCoordinate> &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<qreal>::infinity();
-        Node* predecessorNode = nullptr;
-    };
-
-    // The list with all Nodes (start, end + poly.path())
-    QList<Node> 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<Node*> 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<qreal>::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;
+
 }
 
 /*!

src/Wima/WimaArea.h

@@ -11,6 +11,9 @@
 #include "QGCGeo.h"
 #include <QPair>
 
+#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<QGeoCoordinate>& intersectionList,
-                                                 QList<QPair<int, int>>& 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<QGeoCoordinate>& dijkstraPath, QString &errorstring);
-    static bool            isSelfIntersecting   (const WimaArea& area);
-           bool            isSelfIntersecting   ();
+           bool             isSimplePolygon   ();
 
     // Friends
     friend void print(const WimaArea& area, QString& outputString);

src/Wima/WimaPlaner.cc

 #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<QGeoCoordinate>();
     QList<QGeoCoordinate> 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<QGeoCoordinate>();
     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<QGeoCoordinate> &path)
+{
+    using namespace GeoUtilities;
+    using namespace PolygonCalculus;
+    QList<QPointF> 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)

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<QGeoCoordinate> &path);
 
 private:
     // Member Functions