editing dijkstra

src/MissionManager/SurveyComplexItem.h

@@ -112,6 +112,7 @@ private:
     bool _triggerCamera(void) const;
     bool _hasTurnaround(void) const;
     double _turnaroundDistance(void) const;
+
     bool _hoverAndCaptureEnabled(void) const;
     bool _loadV3(const QJsonObject& complexObject, int sequenceNumber, QString& errorString);
     bool _loadV4V5(const QJsonObject& complexObject, int sequenceNumber, QString& errorString, int version);

src/Wima/WimaArea.cc

@@ -22,10 +22,7 @@ WimaArea::WimaArea(QGCMapPolygon *other, QObject *parent)
 }
 
 
-WimaArea::~WimaArea()
-{
 
-}
 
 
 void WimaArea::setMaxAltitude(double alt)
@@ -152,7 +149,7 @@ void WimaArea::join(WimaArea *poly1, WimaArea *poly2,  WimaArea* joinedPoly)
 
             int nextVertexIndex = walkerPoly->nextVertexIndex(startIndex);
             while (1) {
-                qDebug("nextVertexIndex: %i", nextVertexIndex);
+                //qDebug("nextVertexIndex: %i", nextVertexIndex);
                 joinedPoly->appendVertex(currentVertex);
 
                 QGCMapPolyline walkerPolySegment;
@@ -161,10 +158,10 @@ void WimaArea::join(WimaArea *poly1, WimaArea *poly2,  WimaArea* joinedPoly)
 
                 QList<QPair<int, int>> neighbourList;
                 QList<QGeoCoordinate> intersectionList;
-                qDebug("IntersectionList.size() on init: %i", intersectionList.size());
+                //qDebug("IntersectionList.size() on init: %i", intersectionList.size());
                 intersects(&walkerPolySegment, crossPoly, &intersectionList, &neighbourList);
 
-                qDebug("IntersectionList.size(): %i", intersectionList.size());
+                //qDebug("IntersectionList.size(): %i", intersectionList.size());
 
                 if (intersectionList.size() >= 1) {
                     int minDistIndex = 0;
@@ -181,7 +178,7 @@ void WimaArea::join(WimaArea *poly1, WimaArea *poly2,  WimaArea* joinedPoly)
                         }
                     }
 
-                    qDebug("MinDistIndex: %i", minDistIndex);
+                    //qDebug("MinDistIndex: %i", minDistIndex);
                     QGeoCoordinate protoCurrentVertex = intersectionList.value(minDistIndex);
                     // take numerical erros into account
                     if (protoCurrentVertex.distanceTo(currentVertex) > WimaArea::numericalAccuracy) {
@@ -349,6 +346,7 @@ bool WimaArea::intersects(QGCMapPolyline *line, WimaArea *poly, QList<QGeoCoordi
                 polySegment.appendVertex(nextVertex);
 
                 QGeoCoordinate intersectionPoint;
+                //bool retVal2 = intersects(line, line, &intersectionPoint);
                 bool retVal = intersects(line, &polySegment, &intersectionPoint);
 
 
@@ -381,35 +379,128 @@ bool WimaArea::intersects(QGCMapPolyline *line, WimaArea *poly, QList<QGeoCoordi
 double WimaArea::distInsidePoly(QGeoCoordinate *c1, QGeoCoordinate *c2, WimaArea *poly)
 {
     if (c1 != nullptr && c2 != nullptr && poly != nullptr) {
-        QGCMapPolyline line;
-        line.appendVertex(*c1);
-        line.appendVertex(*c2);
-        QList<QGeoCoordinate> intersectionList;
-        QList<QPair<int, int> > neighbourlist;
-        if (poly->containsCoordinate(*c1) && poly->containsCoordinate(*c2) && !intersects(&line, poly, &intersectionList, &neighbourlist)) {
-            return c1->distanceTo(*c2);
-        } else {
-            return std::numeric_limits<qreal>::infinity();
+        // conditions which must be fullfilled
+        bool c1InPolyRim   = poly->coordinateList().contains(*c1);
+        bool c2InPolyRim   = poly->coordinateList().contains(*c2);
+        bool c1InsidePoly  = poly->containsCoordinate(*c1);
+        bool c2InsidePoly  = poly->containsCoordinate(*c2);
+
+        WimaArea bigPoly = *poly;
+        bigPoly.offset(0.1);
+        if ( (c1InPolyRim || c1InsidePoly) && (c2InPolyRim || c2InsidePoly)) {
+            QList<QGeoCoordinate>   intersectionList;
+            QList<QPair<int, int>>  neighbourlist;
+            QGCMapPolyline line;
+
+            line.appendVertex(*c1);
+            line.appendVertex(*c2);
+            intersects(&line, &bigPoly, &intersectionList, &neighbourlist);
+
+            //if ( intersectionList.size() == (c1InPolyRim || c2InPolyRim ? 2:0) ){
+            if ( intersectionList.size() == 0 ){
+                return c1->distanceTo(*c2);
+            }
+
         }
+        return std::numeric_limits<qreal>::infinity();
     } else {
         qWarning("WimaArea::distInsidePoly(QGeoCoordinate*, QGeoCoordinate*, WimaArea*): nullptr!");
         return std::numeric_limits<qreal>::infinity();
     }
 }
 
-void WimaArea::dijkstraPath(QGeoCoordinate *c1, QGeoCoordinate *c2, WimaArea *poly, QGCMapPolyline* dijkistraPath)
+void WimaArea::dijkstraPath(QGeoCoordinate *start, QGeoCoordinate *end, WimaArea *poly, QList<QGeoCoordinate>* dijkistraPath)
 {
-    if (c1 != nullptr && c2 != nullptr && poly != nullptr && dijkistraPath != nullptr) {
+    if (start != nullptr && end != nullptr && poly != nullptr && dijkistraPath != nullptr) {
         struct Node{
-            QGeoCoordinate c;
-            double predecessorIndex;
-            double distane;
+            QGeoCoordinate coordinate;
+            double distance = std::numeric_limits<qreal>::infinity();
+            Node* predecessorNode = nullptr;
         };
 
         QList<Node> nodeList;
-        QList<Node> visitedNodesList;
+        QList<Node*> workingSet;
+
+
+        // initialize
+        // start
+        Node startNode;
+        startNode.coordinate    = *start;
+        startNode.distance      = 0;
+        nodeList.append(startNode);
+
+        //poly
+        for (int i = 0; i < poly->count(); i++) {
+            Node node;
+            node.coordinate = poly->vertexCoordinate(i);
+            nodeList.append(node);
+        }
+
+        //end
+        Node endNode;
+        endNode.coordinate  = *end;
+        nodeList.append(endNode);
+
+        // 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?
+                double dist = distInsidePoly(&u->coordinate, &v->coordinate, poly);
+                double alternative = u->distance + dist;
+                if (alternative < v->distance)  {
+                    v->distance         = alternative;
+                    v->predecessorNode  = u;
+                }
+            }
+
+        }
+
+        // create path
+        Node* Node = &nodeList.last();
+        if (Node->predecessorNode == nullptr) {
+            qWarning("WimaArea::dijkstraPath(): Error, no path found!");
+            return;
+        }
+
+        while (1) {
+            dijkistraPath->prepend(Node->coordinate);
+
+            //Update Node
+            Node = Node->predecessorNode;
+            if (Node == nullptr) {
+                break;
+            }
+        }
+
+
     } else {
         qWarning(" WimaArea::dijkstraPath(QGeoCoordinate*, QGeoCoordinate*, WimaArea*): nullptr!");
+        return;
     }
 }
 

src/Wima/WimaArea.h

@@ -17,7 +17,6 @@ class WimaArea : public QGCMapPolygon //abstract base class for all WimaAreas
 public:
     WimaArea(QObject* parent = nullptr);
     WimaArea(QGCMapPolygon* other = nullptr, QObject* parent = nullptr);
-    ~WimaArea();
 
 
 
@@ -77,7 +76,7 @@ public:
     /// @return the distance if the path lies within the polygon and inf. else.
     static double          distInsidePoly(QGeoCoordinate *c1, QGeoCoordinate *c2, WimaArea *poly);
     /// calculates the shortes path between two geo coordinates inside a polygon using the Dijkstra Algorithm
-    static void            dijkstraPath(QGeoCoordinate *c1, QGeoCoordinate *c2, WimaArea *poly, QGCMapPolyline *dijkstraPath);
+    static void            dijkstraPath(QGeoCoordinate *c1, QGeoCoordinate *c2, WimaArea *poly, QList<QGeoCoordinate> *dijkstraPath);
 
     // Accurracy used to compute isDisjunct
     static const double numericalAccuracy;

src/Wima/WimaController.cc

@@ -134,14 +134,19 @@ bool WimaController::updateMission()
         }
     }
 
-
     // join service area and op area
     WimaArea* joinedArea = new WimaArea(this);
-    WimaArea::join(corridor, serArea, joinedArea);
-    joinedArea->join(opArea);
+    if (corridor != nullptr) {
+        WimaArea::join(corridor, serArea, joinedArea);
+        joinedArea->join(opArea);
+    } else {
+        WimaArea::join(serArea, opArea, joinedArea);
+    }
+
 
     _visualItems->append(joinedArea);
-    
+
+
     // reset visual items
     _missionController->removeAll();
     QmlObjectListModel* missionItems = _missionController->visualItems();
@@ -152,10 +157,13 @@ bool WimaController::updateMission()
         return false;
     }
     settingsItem->setCoordinate(serArea->center());
-    // set take off position item
-    _missionController->insertSimpleMissionItem(serArea->center(), 1);
+
+    // create take off position item
+    int index = 1;
+    _missionController->insertSimpleMissionItem(serArea->center(), index++);
+
     // create survey item, will be extened with more mission types in the future
-    _missionController->insertComplexMissionItem(_missionController->surveyComplexItemName(), opArea->center(), 2);
+    _missionController->insertComplexMissionItem(_missionController->surveyComplexItemName(), opArea->center(), index++);
     SurveyComplexItem* survey = qobject_cast<SurveyComplexItem*>(missionItems->get(missionItems->count()-1));
     if (survey == nullptr){
         qWarning("WimaController::updateMission(): survey == nullptr");
@@ -164,8 +172,28 @@ bool WimaController::updateMission()
         survey->surveyAreaPolygon()->clear();
         survey->surveyAreaPolygon()->appendVertices(opArea->coordinateList());
     }
+
+    // calculate path from take off to opArea
+    QGeoCoordinate start = serArea->center();
+    QGeoCoordinate end = survey->visualTransectPoints().first().value<QGeoCoordinate>();
+    QList<QGeoCoordinate> path;
+    WimaArea::dijkstraPath(&start, &end, joinedArea, &path);
+    for (int i = 1; i < path.count()-1; i++) {
+        _missionController->insertSimpleMissionItem(path.value(i), i+1);
+        index++;
+    }
+
+    // calculate return path
+    start   = survey->visualTransectPoints().last().value<QGeoCoordinate>();
+    end     = serArea->center();
+    path.clear();
+    WimaArea::dijkstraPath(&start, &end, joinedArea, &path);
+    for (int i = 1; i < path.count()-1; i++) {
+        _missionController->insertSimpleMissionItem(path.value(i), index++);
+    }
+
     // create land position item
-    _missionController->insertSimpleMissionItem(serArea->center(), 3);
+    _missionController->insertSimpleMissionItem(serArea->center(), index++);
     SimpleMissionItem* landItem = qobject_cast<SimpleMissionItem*>(missionItems->get(missionItems->count()-1));
     if (landItem == nullptr){
         qWarning("WimaController::updateMission(): landItem == nullptr");
@@ -177,6 +205,7 @@ bool WimaController::updateMission()
             landItem->setCommand(landCmd);
         }
     }
+
     return true;
 }