From 730535d43820587c6992d31d076384553a11e4dd Mon Sep 17 00:00:00 2001
From: Valentin Platzgummer <valentin.platzgummer@nt.tuwien.ac.at>
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 eda8ef30b5..793ed98793 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 6288a87327..5adacdd93e 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<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;
}
}
diff --git a/src/Wima/WimaArea.h b/src/Wima/WimaArea.h
index 533e45a814..9b42af3010 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<QGeoCoordinate> *dijkstraPath);
// Accurracy used to compute isDisjunct
static const double numericalAccuracy;
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index 83ec4179ba..ceaf3b40d5 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<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;
}
--
GitLab