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;
-    double h   = origin.altitude();
+    QGeoCoordinate toGeo(const QVector3D &point, const QGeoCoordinate &origin)
+    {
+        using namespace PolygonCalculus;
+        double z = point.z();
+        double h   = origin.altitude();
 
-    if (!qFuzzyCompare(lat, M_PI_2)) // this could (unlikely) be a problem, replace with different coordinate transformation
-        return QGeoCoordinate(/* lat */ qAtan(x/cos(lat)/earthRadius) + lon,
-                              /* lon */ qAtan(y/earthRadius) + lat,
-                              /* alt */ h + z);
-    else
-        return QGeoCoordinate(); // singularity occurred (1/cos(pi/2) = inf)
-}
+        QGeoCoordinate coordinate(toGeo(point.toPointF(), origin));
+        coordinate.setAltitude(z + h);
+        return coordinate;
 
-QVector3D GeoUtilites::toCartesian(const QGeoCoordinate &point, const QGeoCoordinate &origin)
-{
-    double lat = point.latitude()/180*M_PI;
-    double lon = point.longitude()/180*M_PI;
-    double h   = point.altitude();
+    }
 
-    double latO = origin.latitude()/180*M_PI;
-    double lonO = origin.longitude()/180*M_PI;
-    double hO   = origin.altitude();
+    QVector3D toCartesian(const QGeoCoordinate &coordinate, const QGeoCoordinate &origin)
+    {
 
-    double dlon = lon-lonO;
-    double dlat = lat-latO;
+        double h   = coordinate.altitude();
+        double hO   = origin.altitude();
 
+        QVector3D point(toCartesian2D(coordinate, origin));
+        point.setZ(h - hO);
 
-    if (!qFuzzyCompare(dlon, M_PI_2) && !qFuzzyCompare(dlat, M_PI_2))
-        return QVector3D(/* x */ qTan(dlon)*earthRadius*qCos(latO),
-                         /* y */ qTan(dlat)*earthRadius,
-                         /* z */ h - hO);
-    else
-        return QVector3D(); // singularity occurred (tan(pi/2) = inf)
+        return  point;
+    }
 
-}
+    QGeoList toGeo(const QVector3DList &points, const QGeoCoordinate &origin)
+    {
+        QGeoList coordinates;
+        for (auto point : points)
+            coordinates.append(toGeo(point, origin));
 
-GeoUtilites::QGeoList GeoUtilites::toGeo(const GeoUtilites::QVector3DFList &points, const QGeoCoordinate &origin)
-{
-    GeoUtilites::QGeoList coordinates;
-    for (auto point : points)
-        coordinates.append(toGeo(point, origin));
+        return coordinates;
+    }
 
-    return coordinates;
-}
+    QVector3DList toCartesian(const QGeoList &coordinates, const QGeoCoordinate &origin)
+    {
+        QVector3DList points;
+        for (auto coordinate : coordinates )
+            points.append(toCartesian(coordinate, origin));
+
+        return points;
+    }
+
+    QPointF toCartesian2D(const QGeoCoordinate &point, const QGeoCoordinate &origin)
+    {
+        double lat = point.latitude()/180*M_PI;
+        double lon = point.longitude()/180*M_PI;
+
+        double latO = origin.latitude()/180*M_PI;
+        double lonO = origin.longitude()/180*M_PI;
+
+        double dlon = lon-lonO;
+        double dlat = lat-latO;
+
+
+        if (!qFuzzyCompare(dlon, M_PI_2) && !qFuzzyCompare(dlat, M_PI_2))
+            return QPointF(/* x */ qTan(dlon)*earthRadius*qCos(latO),
+                           /* y */ qTan(dlat)*earthRadius);
+        else
+            return QPointF(); // singularity occurred (tan(pi/2) = inf)
+    }
 
-GeoUtilites::QVector3DFList GeoUtilites::toCartesian(const GeoUtilites::QGeoList &coordinates, const QGeoCoordinate &origin)
-{
-    GeoUtilites::QVector3DFList points;
-    for (auto coordinate : coordinates )
-        points.append(toCartesian(coordinate, origin));
+    QPointFList toCartesian2D(const QGeoList &coordinates, const QGeoCoordinate &origin)
+    {
+        QPointFList listF;
+
+        for ( auto coordinate : coordinates)
+            listF.append(toCartesian2D(coordinate, origin));
+
+        return listF;
+    }
+
+    QGeoCoordinate toGeo(const QPointF &point, const QGeoCoordinate &origin)
+    {
+        double x = point.x();
+        double y = point.y();
+
+        double lat = origin.latitude();
+        double lon = origin.longitude();
+
+        //qWarning("%lf %lf %lf %lf", x, y, lat, lon);
+
+        if (!qFuzzyCompare(lat, M_PI_2)) // this could (unlikely) be a problem, replace with different coordinate transformation
+            return QGeoCoordinate(/* lat */ qAtan(y/earthRadius)*180/M_PI + lat,
+                                  /* lon */ qAtan(x/cos(lat/180*M_PI)/earthRadius)*180/M_PI + lon,
+                                  /* alt */ origin.altitude());
+        else
+            return QGeoCoordinate(); // singularity occurred (1/cos(pi/2) = inf)
+    }
+
+    QGeoList toGeo(const QPointFList &points, const QGeoCoordinate &origin)
+    {
+        QGeoList coordinates;
+        for ( auto point : points)
+            coordinates.append(toGeo(point, origin));
+
+        return coordinates;
+    }
 
-    return points;
 }
+

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);
+    QGeoCoordinate  toGeo         (const QVector3D &point, const QGeoCoordinate &origin);
+    QGeoList        toGeo         (const QVector3DList &points, const QGeoCoordinate &origin);
+    QGeoCoordinate  toGeo         (const QPointF &point, const QGeoCoordinate &origin);
+    QGeoList        toGeo         (const QPointFList &points, const QGeoCoordinate &origin);
+    QVector3D       toCartesian   (const QGeoCoordinate &coordinate, const QGeoCoordinate &origin);
+    QVector3DList   toCartesian   (const QGeoList &coordinates, const QGeoCoordinate &origin);
+    QPointF         toCartesian2D (const QGeoCoordinate &point, const QGeoCoordinate &origin);
+    QPointFList     toCartesian2D (const QGeoList &coordinates, const QGeoCoordinate &origin);
 }
-#endif // GEOPOLYGONUTILITIES_H
+

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.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