From 6ac2e3822ee9656e881c605501ba94d855799e4d Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Wed, 7 Aug 2019 18:21:57 +0200 Subject: [PATCH] circular survey edited --- bugs.txt | 1 + qgroundcontrol.qrc | 1 + src/MissionManager/MissionController.cc | 36 ++-- src/MissionManager/MissionController.h | 24 +-- src/MissionManager/SurveyComplexItem.cc | 1 - .../QGroundControl.Controls.qmldir | 2 + src/Wima/Circle.cc | 34 ++-- src/Wima/CircularSurveyComplexItem.cc | 156 ++++++++++++------ src/Wima/CircularSurveyComplexItem.h | 18 +- src/Wima/CircularSurveyToDo.txt | 1 + src/Wima/OptimisationTools.cc | 80 +-------- src/Wima/OptimisationTools.h | 91 +++++++++- src/Wima/PlanimetryCalculus.cc | 75 +++++++-- src/Wima/PlanimetryCalculus.h | 18 +- src/Wima/WimaArea.cc | 24 +-- src/Wima/WimaController.cc | 8 +- src/Wima/WimaDataContainer.cc | 11 +- src/Wima/WimaPlaner.cc | 18 +- src/Wima/WimaPlaner.h | 2 +- src/WimaView/SphericalSurveyMapVisual.qml | 155 +++++++++++++++++ 20 files changed, 549 insertions(+), 207 deletions(-) create mode 100644 bugs.txt create mode 100644 src/Wima/CircularSurveyToDo.txt create mode 100644 src/WimaView/SphericalSurveyMapVisual.qml diff --git a/bugs.txt b/bugs.txt new file mode 100644 index 000000000..b8960af9f --- /dev/null +++ b/bugs.txt @@ -0,0 +1 @@ +error when loading wima file diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 1487b504f..16d17199e 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -220,6 +220,7 @@ src/WimaView/WimaMeasurementAreaMapVisual.qml src/WimaView/WimaCorridorMapVisual.qml src/WimaView/WimaMeasurementAreaEditor.qml + src/WimaView/SphericalSurveyMapVisual.qml src/Settings/APMMavlinkStreamRate.SettingsGroup.json diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index c1e3fd48e..275792480 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -31,6 +31,8 @@ #include "KML.h" #include "QGCCorePlugin.h" +#include "src/Wima/CircularSurveyComplexItem.h" + #ifndef __mobile__ #include "MainWindow.h" #include "QGCQFileDialog.h" @@ -61,19 +63,20 @@ const QString MissionController::patternStructureScanName (QT_TRANSLATE_NOOP("M const QString MissionController::patternCorridorScanName (QT_TRANSLATE_NOOP("MissionController", "Corridor Scan")); MissionController::MissionController(PlanMasterController* masterController, QObject *parent) - : PlanElementController (masterController, parent) - , _missionManager (_managerVehicle->missionManager()) - , _missionItemCount (0) - , _visualItems (nullptr) - , _settingsItem (nullptr) - , _firstItemsFromVehicle (false) - , _itemsRequested (false) - , _inRecalcSequence (false) - , _surveyMissionItemName (tr("Survey")) - , _appSettings (qgcApp()->toolbox()->settingsManager()->appSettings()) - , _progressPct (0) - , _currentPlanViewIndex (-1) - , _currentPlanViewItem (nullptr) + : PlanElementController (masterController, parent) + , _missionManager (_managerVehicle->missionManager()) + , _missionItemCount (0) + , _visualItems (nullptr) + , _settingsItem (nullptr) + , _firstItemsFromVehicle (false) + , _itemsRequested (false) + , _inRecalcSequence (false) + , _surveyMissionItemName (tr("Survey")) + , _circularSurveyMissionItemName(tr("Circular Survey")) + , _appSettings (qgcApp()->toolbox()->settingsManager()->appSettings()) + , _progressPct (0) + , _currentPlanViewIndex (-1) + , _currentPlanViewItem (nullptr) { _resetMissionFlightStatus(); managerVehicleChanged(_managerVehicle); @@ -254,7 +257,7 @@ void MissionController::sendToVehicle(void) } /// Converts from visual items to MissionItems -/// @param missionItemParent QObject parent for newly allocated MissionItems +/// @param missionItemParent QObject parent for newly allocated MissionItems, _surveyMissionItemName (tr("Survey")) /// @return true: Mission end action was added to end of list bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMissionItems, QList& rgMissionItems, QObject* missionItemParent) { @@ -422,6 +425,8 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */); } else if (itemName == patternCorridorScanName) { newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */); + } else if (itemName == _circularSurveyMissionItemName) { + newItem = new CircularSurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */); } else { qWarning() << "Internal error: Unknown complex item:" << itemName; return sequenceNumber; @@ -436,6 +441,8 @@ int MissionController::insertComplexMissionItemFromKMLOrSHP(QString itemName, QS if (itemName == _surveyMissionItemName) { newItem = new SurveyComplexItem(_controllerVehicle, _flyView, file, _visualItems); + } else if (itemName == _circularSurveyMissionItemName) { + newItem = new CircularSurveyComplexItem(_controllerVehicle, _flyView, file, _visualItems); } else if (itemName == patternStructureScanName) { newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, file, _visualItems); } else if (itemName == patternCorridorScanName) { @@ -1952,6 +1959,7 @@ QStringList MissionController::complexMissionItemNames(void) const { QStringList complexItems; + complexItems.append(_circularSurveyMissionItemName); complexItems.append(_surveyMissionItemName); complexItems.append(patternCorridorScanName); if (_controllerVehicle->fixedWing()) { diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index fa748b706..a86c8a865 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -20,6 +20,8 @@ #include + + class CoordinateVector; class VisualMissionItem; class MissionItem; @@ -168,16 +170,17 @@ public: // Property accessors - QmlObjectListModel* visualItems (void) { return _visualItems; } - QmlObjectListModel* waypointLines (void) { return &_waypointLines; } - QVariantList waypointPath (void) { return _waypointPath; } - QStringList complexMissionItemNames (void) const; - QGeoCoordinate plannedHomePosition (void) const; - VisualMissionItem* currentPlanViewItem (void) const; - double progressPct (void) const { return _progressPct; } - QString surveyComplexItemName (void) const { return _surveyMissionItemName; } - QString corridorScanComplexItemName (void) const { return patternCorridorScanName; } - QString structureScanComplexItemName(void) const { return patternStructureScanName; } + QmlObjectListModel* visualItems (void) { return _visualItems; } + QmlObjectListModel* waypointLines (void) { return &_waypointLines; } + QVariantList waypointPath (void) { return _waypointPath; } + QStringList complexMissionItemNames (void) const; + QGeoCoordinate plannedHomePosition (void) const; + VisualMissionItem* currentPlanViewItem (void) const; + double progressPct (void) const { return _progressPct; } + QString surveyComplexItemName (void) const { return _surveyMissionItemName; } + QString circularSurveyComplexItemName (void) const { return _circularSurveyMissionItemName; } + QString corridorScanComplexItemName (void) const { return patternCorridorScanName; } + QString structureScanComplexItemName (void) const { return patternStructureScanName; } int missionItemCount (void) const { return _missionItemCount; } int currentMissionIndex (void) const; @@ -292,6 +295,7 @@ private: bool _inRecalcSequence; MissionFlightStatus_t _missionFlightStatus; QString _surveyMissionItemName; + QString _circularSurveyMissionItemName; AppSettings* _appSettings; double _progressPct; int _currentPlanViewIndex; diff --git a/src/MissionManager/SurveyComplexItem.cc b/src/MissionManager/SurveyComplexItem.cc index b26ab892b..a1f6e867b 100644 --- a/src/MissionManager/SurveyComplexItem.cc +++ b/src/MissionManager/SurveyComplexItem.cc @@ -1245,7 +1245,6 @@ void SurveyComplexItem::_rebuildTransectsFromPolygon(bool refly, const QPolygonF while (transectX < transectXMax) { double transectYTop = boundingCenter.y() - halfWidth; double transectYBottom = boundingCenter.y() + halfWidth; - lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle)); transectX += gridSpacing; } diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index 65c3d97d7..e335b7e39 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -95,4 +95,6 @@ WimaCorridorMapVisual 1.0 WimaCorridorMapVisual.qml WimaItemEditor 1.0 WimaItemEditor.qml WimaMapPolygonVisuals 1.0 WimaMapPolygonVisuals.qml WimaMapPolylineVisuals 1.0 WimaMapPolylineVisuals.qml +SphericalSurveyMapVisual 1.0 SphericalSurveyMapVisual.qml + diff --git a/src/Wima/Circle.cc b/src/Wima/Circle.cc index ece29d5df..a50e76517 100644 --- a/src/Wima/Circle.cc +++ b/src/Wima/Circle.cc @@ -97,7 +97,7 @@ QList Circle::approximate(int numberOfCorners) const { if ( numberOfCorners < 3) return QList(); - return approximateSektor(numberOfCorners, 0, 2*M_PI); + return approximateSektor(numberOfCorners+1, 0, 2*M_PI); } QList Circle::approximate(double angleDiscretisation) const @@ -107,37 +107,39 @@ QList Circle::approximate(double angleDiscretisation) const QList Circle::approximateSektor(int numberOfCorners, double alpha1, double alpha2) const { + if ( numberOfCorners < 3) + return QList(); return approximateSektor((alpha2-alpha1)/double(numberOfCorners-1), alpha1, alpha2); } QList Circle::approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const { + using namespace PlanimetryCalculus; // truncate alpha1 to [0, 2*pi], fmod() does not work in this case - alpha1 = PlanimetryCalculus::truncateAngle(alpha1); - alpha2 = PlanimetryCalculus::truncateAngle(alpha2); - double deltaAlpha = PlanimetryCalculus::truncateAngle(alpha2 - alpha1); - angleDiscretisation = PlanimetryCalculus::truncateAngle(angleDiscretisation); + alpha1 = truncateAngle(alpha1); + alpha2 = truncateAngle(alpha2); + double deltaAlpha = truncateAngle(alpha2 - alpha1); + angleDiscretisation = truncateAngle(angleDiscretisation)*signum(angleDiscretisation); - if (angleDiscretisation > deltaAlpha || qFuzzyIsNull(angleDiscretisation)) + if (fabs(angleDiscretisation) > deltaAlpha || qFuzzyIsNull(angleDiscretisation)) return QList(); QList sector; - QPointF vertex(-_circleRadius,0); // initial vertex + QPointF vertex0(_circleRadius,0); // initial vertex double currentAngle = alpha1; // rotate the vertex numberOfCorners-1 times add the origin and append to the polygon. - while(currentAngle < alpha2) { - PlanimetryCalculus::rotateReference(vertex, currentAngle); - sector.append(vertex + _circleOrigin); + do { + QPointF currentVertex = rotateReturn(vertex0, currentAngle); + sector.append(currentVertex + _circleOrigin); currentAngle = PlanimetryCalculus::truncateAngle(currentAngle + angleDiscretisation); - } + }while(currentAngle < alpha2); // append last point if necessarry - PlanimetryCalculus::rotateReference(vertex, alpha2); - vertex = vertex + _circleOrigin; - if ( !qFuzzyIsNull(PlanimetryCalculus::distance(sector.first(), vertex)) - && !qFuzzyIsNull(PlanimetryCalculus::distance(sector.last(), vertex )) ){ - sector.append(vertex); + QPointF currentVertex = rotateReturn(vertex0, alpha2) + _circleOrigin; + if ( !qFuzzyIsNull(PlanimetryCalculus::distance(sector.first(), currentVertex)) + && !qFuzzyIsNull(PlanimetryCalculus::distance(sector.last(), currentVertex )) ){ + sector.append(currentVertex); } return sector; diff --git a/src/Wima/CircularSurveyComplexItem.cc b/src/Wima/CircularSurveyComplexItem.cc index 1c5f7509d..e3eb33d20 100644 --- a/src/Wima/CircularSurveyComplexItem.cc +++ b/src/Wima/CircularSurveyComplexItem.cc @@ -4,9 +4,25 @@ const char* CircularSurveyComplexItem::settingsGroup = "Survey"; CircularSurveyComplexItem::CircularSurveyComplexItem(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile, QObject *parent) - : TransectStyleComplexItem (vehicle, flyView, settingsGroup, parent) + : TransectStyleComplexItem (vehicle, flyView, settingsGroup, parent) + , _referencePoint(QGeoCoordinate(47.770859, 16.531076,0)) + +{ + +} + +void CircularSurveyComplexItem::setRefPoint(const QGeoCoordinate &refPt) { + if (refPt != _referencePoint){ + _referencePoint = refPt; + emit refPointChanged(); + } +} + +QGeoCoordinate CircularSurveyComplexItem::refPoint() const +{ + return _referencePoint; } bool CircularSurveyComplexItem::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString) @@ -50,78 +66,120 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() using namespace PolygonCalculus; using namespace PlanimetryCalculus; + if ( _surveyAreaPolygon.count() < 3) + return; + _transects.clear(); QPolygonF surveyPolygon = toQPolygonF(toCartesian2D(_surveyAreaPolygon.coordinateList(), _referencePoint)); - QPolygonF boundingPolygon = surveyPolygon.boundingRect(); QVector distances; - for (const QPointF &p : boundingPolygon) distances.append(norm(p)); - QVector angles; - for (const QPointF &p : boundingPolygon) angles.append(truncateAngle(angle(p))); + for (const QPointF &p : surveyPolygon) distances.append(norm(p)); + - double alpha1 = 0.95*(*std::min_element(angles.begin(), angles.end()));; // radiants - double alpha2 = 1.05*(*std::max_element(angles.begin(), angles.end())); // radiants; // radiants double dalpha = 0.5/180*M_PI; // radiants - double r0 = 0.9*(*std::min_element(distances.begin(), distances.end())); // meter - double r1 = 1.1*(*std::max_element(distances.begin(), distances.end())); // meter - double dr = 1; // meter + double dr = 30; // meter + double r_min = dr; // meter + double r_max = (*std::max_element(distances.begin(), distances.end())); // meter + + + QPointF origin(0, 0); + IntersectType type; + if (!contains(surveyPolygon, origin, type)) { + QVector angles; + for (const QPointF &p : surveyPolygon) angles.append(truncateAngle(angle(p))); + + // determine r_min by successive approximation + double r = r_max; + while ( r > r_min) { + Circle circle(r, origin); + + if (!intersects(circle, surveyPolygon)) { + r_min = r; + break; + } + + r -= dr; + } + } + + qWarning("r_min, r_max:"); + qWarning() << r_min; + qWarning() << r_max; QList convexPolygons; decomposeToConvex(surveyPolygon, convexPolygons); - QPointF origin(0, 0); - QList fullPath; + QList> fullPath; for (int i = 0; i < convexPolygons.size(); i++){ const QPolygonF &polygon = convexPolygons[i]; - double r = r0; + double r = r_min; - QList currPolyPath; + QList> currPolyPath; + bool currentPolyPathUpdated = false; int direction = 1; - while (r < r1) { + while (r < r_max) { Circle circle(r, origin); - QList sector = circle.approximateSektor(dalpha, alpha1, alpha2); - QList sectorPath; - for ( const QPointF &p : sector) { - if (polygon.containsPoint(p, Qt::FillRule::OddEvenFill)) - sectorPath.append(p); + QList intersectPoints; + QList typeList; + if (intersects(circle, polygon, intersectPoints, typeList)) { + if (intersectPoints.size() >= 1) { + // continue here + QPointF p1 = intersectPoints.first()[0]; + QPointF p2 = intersectPoints.first()[1]; + double beta1 = angle(p1); + double beta2 = angle(p2); + double alpha1 = fmin(beta1, beta2); + double alpha2 = fmax(beta1, beta2); + + QList sector = circle.approximateSektor(direction*dalpha, alpha1, alpha2); + QList sectorPath; + for ( const QPointF &p : sector) { + if (polygon.containsPoint(p, Qt::FillRule::OddEvenFill)) + sectorPath.append(p); + } + + if (sectorPath.size() > 0 ) { + if (direction == -1){ + direction = 1; + } else + direction = -1; + + // use shortestPath() here if necessary, could be a problem if dr >> + currPolyPath.append(sectorPath); + currentPolyPathUpdated = true; + } + } } - if (direction == -1){ - reversePath(sectorPath); - direction = 1; - } else - direction = -1;; - - // use shortestPath() here if necessary, could be a problem if dr >> - currPolyPath.append(sectorPath); - r += dr; } - if (i > 0) { - QPointF start = fullPath.last(); - QPointF end = currPolyPath.first(); - QList path; - if(!shortestPath(surveyPolygon, start, end, path)) - return; - path.removeFirst(); - path.removeLast(); - fullPath.append(path); - fullPath.append(fullPath); + if (currentPolyPathUpdated) { + if (fullPath.size() > 1) { + QPointF start = fullPath.last().last(); + QPointF end = currPolyPath.last().first(); + QList path; + if(!shortestPath(surveyPolygon, start, end, path)) + return; + path.removeFirst(); + path.removeLast(); + currPolyPath.last().append(path); + } + + fullPath.append(currPolyPath); } - } - QList geoPath = toGeo(fullPath, _referencePoint); - for ( const QGeoCoordinate &coordinate : geoPath) { - CoordInfo_t coordinfo = {coordinate, CoordTypeInterior}; - _transects.append(coordinfo); } - - - - - + for ( const QList &transect : fullPath) { + QList geoPath = toGeo(transect, _referencePoint); + QList transectList; + for ( const QGeoCoordinate &coordinate : geoPath) { + CoordInfo_t coordinfo = {coordinate, CoordTypeInterior}; + transectList.append(coordinfo); + } + _transects.append(transectList); + } } diff --git a/src/Wima/CircularSurveyComplexItem.h b/src/Wima/CircularSurveyComplexItem.h index 296fbce62..249de0f0f 100644 --- a/src/Wima/CircularSurveyComplexItem.h +++ b/src/Wima/CircularSurveyComplexItem.h @@ -17,6 +17,14 @@ public: /// @param kmlOrShpFile Polygon comes from this file, empty for default polygon CircularSurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent); + Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY refPointChanged) + + // Property setters + void setRefPoint(const QGeoCoordinate &refPt); + + // Property getters + QGeoCoordinate refPoint() const; + // Overrides from ComplexMissionItem bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; QString mapVisualQML (void) const final { return QStringLiteral("SpericalSurveyMapVisual.qml"); } @@ -29,20 +37,24 @@ public: double timeBetweenShots (void) final; // Overrides from VisualMissionionItem - QString commandDescription (void) const final { return tr("Spherical Survey"); } - QString commandName (void) const final { return tr("SphericalSurvey"); } - QString abbreviation (void) const final { return tr("Sph.S"); } + QString commandDescription (void) const final { return tr("Circular Survey"); } + QString commandName (void) const final { return tr("Circular Survey"); } + QString abbreviation (void) const final { return tr("C.S."); } bool readyForSave (void) const final; double additionalTimeDelay (void) const final; static const char* settingsGroup; +signals: + void refPointChanged(); + private slots: // Overrides from TransectStyleComplexItem void _rebuildTransectsPhase1 (void) final; void _recalcComplexDistance (void) final; void _recalcCameraShots (void) final; +private: QGeoCoordinate _referencePoint; // center of the circular lanes, e.g. base station }; diff --git a/src/Wima/CircularSurveyToDo.txt b/src/Wima/CircularSurveyToDo.txt new file mode 100644 index 000000000..ea4e175b2 --- /dev/null +++ b/src/Wima/CircularSurveyToDo.txt @@ -0,0 +1 @@ +add refpt gui and edit constructor diff --git a/src/Wima/OptimisationTools.cc b/src/Wima/OptimisationTools.cc index 9ac82d5aa..6475c865d 100644 --- a/src/Wima/OptimisationTools.cc +++ b/src/Wima/OptimisationTools.cc @@ -17,84 +17,6 @@ namespace OptimisationTools { * \sa QList */ template - bool dijkstraAlgorithm(const QList &elements, int startIndex, int endIndex, QList &elementPath, std::function 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{ - T element; - double distance = std::numeric_limits::infinity(); - Node* predecessorNode = nullptr; - }; + bool dijkstraAlgorithm(const QList &elements, int startIndex, int endIndex, QList &elementPath, std::function distance); // don't seperate parameters with new lines or documentation will break - // The list with all Nodes (elements) - QList 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 workingSet; - - //append elements to node list - for (int i = 0; i < elements.size(); i++) { - Node node; - node.element = elements[i]; - nodeList.append(node); - workingSet.append(&nodeList[i]); - } - - nodeList[startIndex].distance = 0; - - // 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]; - double dist = distance(u->element, v->element); - // 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 - - - // reverse assemble path - Node* node = &nodeList[endIndex]; - while (1) { - if (node == nullptr) { - if (elementPath[0] == elements[startIndex])// check if starting point was reached - break; - return false; - } - elementPath.prepend(node->element); - - //Update Node - node = node->predecessorNode; - - } - return true; - } } // end OptimisationTools namespace diff --git a/src/Wima/OptimisationTools.h b/src/Wima/OptimisationTools.h index e4194ca62..e1c94bbda 100644 --- a/src/Wima/OptimisationTools.h +++ b/src/Wima/OptimisationTools.h @@ -5,8 +5,97 @@ #include namespace OptimisationTools { + /*! + * \fn bool dijkstraAlgorithm(int startIndex, int endIndex, const QList elements, QList &elementPath, double(*distance)(const T &t1, const T &t2)) + * Calculates the shortest path between the elements stored in \a elements. + * The \l {Dijkstra Algorithm} is used to find the shorest path. + * Stores the result inside \a elementPath when sucessfull. + * The function handle \a distance is used to calculate the distance between two elements. The distance must be positive. + * Returns \c true if successful, \c false else. + * + * \sa QList + */ template - bool dijkstraAlgorithm(const QList &elements, int startIndex, int endIndex, QList &elementPath, std::function distance); + bool dijkstraAlgorithm(const QList &elements, int startIndex, int endIndex, QList &elementPath, std::function distance) + { + 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{ + T element; + double distance = std::numeric_limits::infinity(); + Node* predecessorNode = nullptr; + }; + + // The list with all Nodes (elements) + QList 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 workingSet; + + //append elements to node list + for (int i = 0; i < elements.size(); i++) { + Node node; + node.element = elements[i]; + nodeList.append(node); + workingSet.append(&nodeList[i]); + } + + nodeList[startIndex].distance = 0; + + // 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]; + double dist = distance(u->element, v->element); + // 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 + + + // reverse assemble path + Node* node = &nodeList[endIndex]; + while (1) { + if (node == nullptr) { + if (elementPath[0] == elements[startIndex])// check if starting point was reached + break; + return false; + } + elementPath.prepend(node->element); + + //Update Node + node = node->predecessorNode; + + } + return true; + } } diff --git a/src/Wima/PlanimetryCalculus.cc b/src/Wima/PlanimetryCalculus.cc index bdcf2a0dc..93f396268 100644 --- a/src/Wima/PlanimetryCalculus.cc +++ b/src/Wima/PlanimetryCalculus.cc @@ -480,16 +480,7 @@ angle return line; } - /*! - * \fntemplate int signum(T val) - * Returns the signum of a value \a val. - * - * \sa QPair, QList - */ - template int signum(T val) - { - return (T(0) < val) - (val < T(0)); - } + bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt) { @@ -632,6 +623,70 @@ angle return qAtan2(p1.y(), p1.x()); } + bool intersects(const Circle &circle, const QPolygonF &polygon, QList &intersectionPoints, NeighbourList &neighbourList, IntersectList &typeList) + { + using namespace PolygonCalculus; + for (int i = 0; i < polygon.size(); i++) { + QPointF p1 = polygon[i]; + int j = nextVertexIndex(polygon.size(), i); + QPointF p2 = polygon[j]; + QLineF line(p1, p2); + + QPointFList lineIntersecPts; + IntersectType type; + if (intersects(circle, line, lineIntersecPts, type)) { + QPair neigbours; + neigbours.first = i; + neigbours.second = j; + neighbourList.append(neigbours); + typeList.append(type); + intersectionPoints.append(lineIntersecPts); + } + + } + + if (intersectionPoints.size() > 0) + return true; + else { + return false; + } + } + + bool intersects(const Circle &circle, const QPolygonF &polygon, QList &intersectionPoints, NeighbourList &neighbourList) + { + QList types; + return intersects(circle, polygon, intersectionPoints, neighbourList, types); + } + + bool intersects(const Circle &circle, const QPolygonF &polygon, QList &intersectionPoints, IntersectList &typeList) + { + NeighbourList neighbourList; + return intersects(circle, polygon, intersectionPoints, neighbourList, typeList); + } + + bool intersects(const Circle &circle, const QPolygonF &polygon, QList &intersectionPoints) + { + NeighbourList neighbourList; + return intersects(circle, polygon, intersectionPoints, neighbourList); + } + + bool intersects(const Circle &circle, const QPolygonF &polygon) + { + QList intersectionPoints; + return intersects(circle, polygon, intersectionPoints); + } + + bool intersects(const QLineF &line1, const QLineF &line2) + { + QPointF intersectionPoint; + return intersects(line1, line2, intersectionPoint); + } + + bool intersects(const Circle &circle, const QLineF &line, QPointFList &intersectionPoints, IntersectType &type) + { + return intersects(circle, line, intersectionPoints, type, true /* calculate intersection points*/); + } + diff --git a/src/Wima/PlanimetryCalculus.h b/src/Wima/PlanimetryCalculus.h index 5677eed60..b1812bbbb 100644 --- a/src/Wima/PlanimetryCalculus.h +++ b/src/Wima/PlanimetryCalculus.h @@ -44,12 +44,19 @@ namespace PlanimetryCalculus { bool intersects(const Circle &circle1, const Circle &circle2, IntersectType &type); bool intersects(const Circle &circle1, const Circle &circle2, QPointFList &intersectionPoints); bool intersects(const Circle &circle1, const Circle &circle2, QPointFList &intersectionPoints, IntersectType &type); + bool intersects(const Circle &circle, const QLineF &line); bool intersects(const Circle &circle, const QLineF &line, IntersectType &type); bool intersects(const Circle &circle, const QLineF &line, QPointFList &intersectionPoints); bool intersects(const Circle &circle, const QLineF &line, QPointFList &intersectionPoints, IntersectType &type); + bool intersects(const Circle &circle, const QPolygonF &polygon); + bool intersects(const Circle &circle, const QPolygonF &polygon, QList &intersectionPoints); + bool intersects(const Circle &circle, const QPolygonF &polygon, QList &intersectionPoints, IntersectList &typeList); + bool intersects(const Circle &circle, const QPolygonF &polygon, QList &intersectionPoints, NeighbourList &neighbourList); + bool intersects(const Circle &circle, const QPolygonF &polygon, QList &intersectionPoints, NeighbourList &neighbourList, IntersectList &typeList); + bool intersects(const QLineF &line1, const QLineF &line2); bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt); bool intersects(const QLineF &line1, const QLineF &line2, QPointF &intersectionPt, IntersectType &type); bool intersects(const QPolygonF &polygon, const QLineF &line, QPointFList &intersectionList); @@ -73,7 +80,16 @@ namespace PlanimetryCalculus { double truncateAngle(double angle); double truncateAngleDegree(double angle); - template int signum(T val); + /*! + * \fntemplate int signum(T val) + * Returns the signum of a value \a val. + * + * \sa QPair, QList + */ + template int signum(T val) + { + return (T(0) < val) - (val < T(0)); + } } diff --git a/src/Wima/WimaArea.cc b/src/Wima/WimaArea.cc index ebc8901ee..4205280f6 100644 --- a/src/Wima/WimaArea.cc +++ b/src/Wima/WimaArea.cc @@ -147,31 +147,31 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &join QList GeoPolygon1 = area1.coordinateList(); QList GeoPolygon2 = area2.coordinateList(); - qWarning("befor joining"); - qWarning() << GeoPolygon1; - qWarning() << GeoPolygon2; +// qWarning("befor joining"); +// qWarning() << GeoPolygon1; +// qWarning() << GeoPolygon2; QGeoCoordinate origin = GeoPolygon1[0]; - QGeoCoordinate tset = GeoPolygon1[2]; +// QGeoCoordinate tset = GeoPolygon1[2]; - qWarning() << tset;qWarning() << toGeo(toCartesian2D(tset, origin), origin); +// qWarning() << tset;qWarning() << toGeo(toCartesian2D(tset, origin), origin); QPolygonF polygon1 = toQPolygonF(toCartesian2D(GeoPolygon1, origin)); QPolygonF polygon2 = toQPolygonF(toCartesian2D(GeoPolygon2, origin)); - qWarning("after 1 transform"); - qWarning() << polygon1; - qWarning() << polygon2; +// qWarning("after 1 transform"); +// qWarning() << polygon1; +// qWarning() << polygon2; QPolygonF joinedPolygon; JoinPolygonError retValue = PolygonCalculus::join(polygon1, polygon2, joinedPolygon); - qWarning("after joining"); - qWarning() << joinedPolygon; +// qWarning("after joining"); +// qWarning() << joinedPolygon; if (retValue == JoinPolygonError::Disjoint) { qWarning("Polygons are disjoint."); @@ -181,8 +181,8 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &join qWarning("Polygon vertex count is low."); } else { QList path = toGeo(toQPointFList(joinedPolygon), origin); - qWarning("after transform"); - qWarning() << path; +// qWarning("after transform"); +// qWarning() << path; joinedArea.setPath(path); return true; } diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 1d2ed915a..9b5e9c32a 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -160,7 +160,7 @@ void WimaController::containerDataValidChanged(bool valid) qWarning() << areaData->type(); if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area? _serviceArea = *qobject_cast(areaData); - qWarning("Service area, wuhuuu!"); +// qWarning("Service area, wuhuuu!"); areaCounter++; _visualItems.append(&_serviceArea); @@ -169,7 +169,7 @@ void WimaController::containerDataValidChanged(bool valid) if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area? _measurementArea = *qobject_cast(areaData); - qWarning("Measurement area, wuhuuu!"); +// qWarning("Measurement area, wuhuuu!"); areaCounter++; _visualItems.append(&_measurementArea); @@ -178,7 +178,7 @@ void WimaController::containerDataValidChanged(bool valid) if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor? _corridor = *qobject_cast(areaData); - qWarning("WimaCorridorData, wuhuuu!"); +// qWarning("WimaCorridorData, wuhuuu!"); areaCounter++; //_visualItems.append(&_corridor); // not needed @@ -187,7 +187,7 @@ void WimaController::containerDataValidChanged(bool valid) if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor? _joinedArea = *qobject_cast(areaData); - qWarning("WimaJoinedAreaData, wuhuuu!"); +// qWarning("WimaJoinedAreaData, wuhuuu!"); areaCounter++; _visualItems.append(&_joinedArea); diff --git a/src/Wima/WimaDataContainer.cc b/src/Wima/WimaDataContainer.cc index 4d0d1ae52..0190d333a 100644 --- a/src/Wima/WimaDataContainer.cc +++ b/src/Wima/WimaDataContainer.cc @@ -12,7 +12,16 @@ WimaDataContainer::WimaDataContainer(QObject *parent) * \fn bool WimaDataContainer::dataValid() const * Returns \c true if the data is valid, \c false else. */ -bool WimaDataContainer::dataValid() const +bool WimaDataContainer::dataValid() const/*! + * \fn bool dijkstraAlgorithm(int startIndex, int endIndex, const QList elements, QList &elementPath, double(*distance)(const T &t1, const T &t2)) + * Calculates the shortest path between the elements stored in \a elements. + * The \l {Dijkstra Algorithm} is used to find the shorest path. + * Stores the result inside \a elementPath when sucessfull. + * The function handle \a distance is used to calculate the distance between two elements. The distance must be positive. + * Returns \c true if successful, \c false else. + * + * \sa QList + */ { return _dataValid; } diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 4e3150d25..c20053793 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -1,5 +1,7 @@ #include "WimaPlaner.h" +#include "CircularSurveyComplexItem.h" + const char* WimaPlaner::wimaFileExtension = "wima"; @@ -244,18 +246,21 @@ bool WimaPlaner::updateMission() // create survey item, will be extened with more()-> mission types in the future - _missionController->insertComplexMissionItem(_missionController->surveyComplexItemName(), _measurementArea.center(), missionItems->count()); - SurveyComplexItem* survey = qobject_cast(missionItems->get(missionItems->count()-1)); + _missionController->insertComplexMissionItem(_missionController->circularSurveyComplexItemName(), _measurementArea.center(), missionItems->count()); + CircularSurveyComplexItem* survey = qobject_cast(missionItems->get(missionItems->count()-1)); if (survey == nullptr){ qWarning("WimaPlaner::updateMission(): survey == nullptr"); return false; } else { survey->surveyAreaPolygon()->clear(); survey->surveyAreaPolygon()->appendVertices(_measurementArea.coordinateList()); - //survey-> } // calculate path from take off to opArea + if (survey->visualTransectPoints().size() == 0) { + qWarning("WimaPlaner::updateMission(): survey no points."); + return false; + } QGeoCoordinate start = _serviceArea.center(); QGeoCoordinate end = survey->visualTransectPoints().first().value(); QList path; @@ -440,7 +445,7 @@ bool WimaPlaner::loadFromFile(const QString &filename) emit currentFileChanged(); //recalcJoinedArea(); - // MissionItems + // MissionItems4 // extrac MissionItems part QJsonDocument missionJsonDoc = QJsonDocument(json[missionItemsName].toObject()); // create temporary file with missionItems @@ -529,7 +534,10 @@ bool WimaPlaner::recalcJoinedArea(QString &errorString) // remove if debugging finished WimaServiceArea *test = new WimaServiceArea(this); - test->setPath(_joinedArea.path()); + Circle circle(25, this); + using namespace GeoUtilities; + + test->setPath(toGeo(circle.approximateSektor(10, -1, 1), _joinedArea.center())); _visualItems.append(test); return true; diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index 50a4f59e6..69d1b40be 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -29,7 +29,7 @@ #include "PlanimetryCalculus.h" #include "GeoUtilities.h" -#define TEST_FUN 1 +#define TEST_FUN 0 #if TEST_FUN #include "TestPolygonCalculus.h" diff --git a/src/WimaView/SphericalSurveyMapVisual.qml b/src/WimaView/SphericalSurveyMapVisual.qml new file mode 100644 index 000000000..43f23a9e6 --- /dev/null +++ b/src/WimaView/SphericalSurveyMapVisual.qml @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +// original file: SurveyMapVisual.qml + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtLocation 5.3 +import QtPositioning 5.3 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FlightMap 1.0 + +/// Survey Complex Mission Item visuals +Item { + id: _root + + property var map ///< Map control to place item in + property var qgcView ///< QGCView to use for popping dialogs + + property var _missionItem: object + property var _mapPolygon: object.surveyAreaPolygon + property var _visualTransectsComponent + property var _entryCoordinate + property var _exitCoordinate + + signal clicked(int sequenceNumber) + + function _addVisualElements() { + _visualTransectsComponent = visualTransectsComponent.createObject(map) + _entryCoordinate = entryPointComponent.createObject(map) + _exitCoordinate = exitPointComponent.createObject(map) + map.addMapItem(_visualTransectsComponent) + map.addMapItem(_entryCoordinate) + map.addMapItem(_exitCoordinate) + } + + function _destroyVisualElements() { + _visualTransectsComponent.destroy() + _entryCoordinate.destroy() + _exitCoordinate.destroy() + } + + /// Add an initial 4 sided polygon if there is none + function _addInitialPolygon() { + if (_mapPolygon.count < 3) { + // Initial polygon is inset to take 2/3rds space + var rect = Qt.rect(map.centerViewport.x, map.centerViewport.y, map.centerViewport.width, map.centerViewport.height) + rect.x += (rect.width * 0.25) / 2 + rect.y += (rect.height * 0.25) / 2 + rect.width *= 0.75 + rect.height *= 0.75 + + var centerCoord = map.toCoordinate(Qt.point(rect.x + (rect.width / 2), rect.y + (rect.height / 2)), false /* clipToViewPort */) + var topLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */) + var topRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y), false /* clipToViewPort */) + var bottomLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y + rect.height), false /* clipToViewPort */) + var bottomRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */) + + // Initial polygon has max width and height of 3000 meters + var halfWidthMeters = Math.min(topLeftCoord.distanceTo(topRightCoord), 3000) / 2 + var halfHeightMeters = Math.min(topLeftCoord.distanceTo(bottomLeftCoord), 3000) / 2 + topLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0) + topRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0) + bottomLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180) + bottomRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180) + + _mapPolygon.appendVertex(topLeftCoord) + _mapPolygon.appendVertex(topRightCoord) + _mapPolygon.appendVertex(bottomRightCoord) + _mapPolygon.appendVertex(bottomLeftCoord) + } + } + + Component.onCompleted: { + _addInitialPolygon() + _addVisualElements() + } + + Component.onDestruction: { + _destroyVisualElements() + } + + QGCMapPolygonVisuals { + id: mapPolygonVisuals + qgcView: _root.qgcView + mapControl: map + mapPolygon: _mapPolygon + interactive: _missionItem.isCurrentItem + borderWidth: 1 + borderColor: "black" + interiorColor: "green" + interiorOpacity: 0.5 + } + + // Transect lines + Component { + id: visualTransectsComponent + + MapPolyline { + line.color: "white" + line.width: 2 + path: _missionItem.visualTransectPoints + } + } + + // Entry point + Component { + id: entryPointComponent + + MapQuickItem { + anchorPoint.x: sourceItem.anchorPointX + anchorPoint.y: sourceItem.anchorPointY + z: QGroundControl.zOrderMapItems + coordinate: _missionItem.coordinate + visible: _missionItem.exitCoordinate.isValid + + sourceItem: MissionItemIndexLabel { + index: _missionItem.sequenceNumber + label: "Entry" + checked: _missionItem.isCurrentItem + onClicked: _root.clicked(_missionItem.sequenceNumber) + } + } + } + + // Exit point + Component { + id: exitPointComponent + + MapQuickItem { + anchorPoint.x: sourceItem.anchorPointX + anchorPoint.y: sourceItem.anchorPointY + z: QGroundControl.zOrderMapItems + coordinate: _missionItem.exitCoordinate + visible: _missionItem.exitCoordinate.isValid + + sourceItem: MissionItemIndexLabel { + index: _missionItem.lastSequenceNumber + label: "Exit" + checked: _missionItem.isCurrentItem + onClicked: _root.clicked(_missionItem.sequenceNumber) + } + } + } +} -- 2.22.0