From 730535d43820587c6992d31d076384553a11e4dd Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Wed, 12 Jun 2019 18:24:44 +0200 Subject: [PATCH] editing dijkstra --- src/MissionManager/SurveyComplexItem.h | 1 + src/Wima/WimaArea.cc | 135 +++++++++++++++++++++---- src/Wima/WimaArea.h | 3 +- src/Wima/WimaController.cc | 45 +++++++-- 4 files changed, 152 insertions(+), 32 deletions(-) diff --git a/src/MissionManager/SurveyComplexItem.h b/src/MissionManager/SurveyComplexItem.h index eda8ef30b..793ed9879 100644 --- a/src/MissionManager/SurveyComplexItem.h +++ b/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); diff --git a/src/Wima/WimaArea.cc b/src/Wima/WimaArea.cc index 6288a8732..5adacdd93 100644 --- a/src/Wima/WimaArea.cc +++ b/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> neighbourList; QList 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 intersectionList; - QList > neighbourlist; - if (poly->containsCoordinate(*c1) && poly->containsCoordinate(*c2) && !intersects(&line, poly, &intersectionList, &neighbourlist)) { - return c1->distanceTo(*c2); - } else { - return std::numeric_limits::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 intersectionList; + QList> 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::infinity(); } else { qWarning("WimaArea::distInsidePoly(QGeoCoordinate*, QGeoCoordinate*, WimaArea*): nullptr!"); return std::numeric_limits::infinity(); } } -void WimaArea::dijkstraPath(QGeoCoordinate *c1, QGeoCoordinate *c2, WimaArea *poly, QGCMapPolyline* dijkistraPath) +void WimaArea::dijkstraPath(QGeoCoordinate *start, QGeoCoordinate *end, WimaArea *poly, QList* 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::infinity(); + Node* predecessorNode = nullptr; }; QList nodeList; - QList visitedNodesList; + QList 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::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; } } diff --git a/src/Wima/WimaArea.h b/src/Wima/WimaArea.h index 533e45a81..9b42af301 100644 --- a/src/Wima/WimaArea.h +++ b/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 *dijkstraPath); // Accurracy used to compute isDisjunct static const double numericalAccuracy; diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 83ec4179b..ceaf3b40d 100644 --- a/src/Wima/WimaController.cc +++ b/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(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(); + QList 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(); + 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(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; } -- 2.22.0