From 89e1c7a36e1a226f36b12c15898c142a3369f5a5 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Sat, 20 Jul 2019 12:50:24 +0200 Subject: [PATCH] circularSurvey edited --- qgroundcontrol.pro | 8 ++- src/Wima/Circle.cc | 26 +++++----- src/Wima/Circle.h | 8 +-- src/Wima/CircularSurveyComplexItem.cc | 74 +++++++++++++++++++++++++++ src/Wima/CircularSurveyComplexItem.h | 8 +++ src/Wima/OptimisationTools.cc | 4 +- src/Wima/OptimisationTools.h | 2 +- src/Wima/PlanimetryCalculus.cc | 17 +++++- src/Wima/PlanimetryCalculus.h | 3 ++ src/Wima/PolygonCalculus.cc | 25 ++++++--- src/Wima/PolygonCalculus.h | 3 +- src/Wima/TestPolygonCalculus.cpp | 33 ++++++++++++ src/Wima/TestPolygonCalculus.h | 9 ++++ src/Wima/WimaPlaner.cc | 4 ++ src/Wima/WimaPlaner.h | 6 +++ src/Wima/testplanimetrycalculus.cpp | 45 ++++++++++++++++ src/Wima/testplanimetrycalculus.h | 12 +++++ 17 files changed, 256 insertions(+), 31 deletions(-) create mode 100644 src/Wima/TestPolygonCalculus.cpp create mode 100644 src/Wima/TestPolygonCalculus.h create mode 100644 src/Wima/testplanimetrycalculus.cpp create mode 100644 src/Wima/testplanimetrycalculus.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 1c21f5070..a927410df 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -430,7 +430,9 @@ HEADERS += \ src/Wima/Circle.h \ src/Wima/PolygonCalculus.h \ src/Wima/OptimisationTools.h \ - src/Wima/GeoUtilities.h + src/Wima/GeoUtilities.h \ + src/Wima/TestPolygonCalculus.h \ + src/Wima/testplanimetrycalculus.h SOURCES += \ src/api/QGCCorePlugin.cc \ src/api/QGCOptions.cc \ @@ -459,7 +461,9 @@ SOURCES += \ src/Wima/Circle.cc \ src/Wima/OptimisationTools.cc \ src/Wima/GeoUtilities.cc \ - src/Wima/PolygonCalculus.cc + src/Wima/PolygonCalculus.cc \ + src/Wima/TestPolygonCalculus.cpp \ + src/Wima/testplanimetrycalculus.cpp # # Unit Test specific configuration goes here (requires full debug build with all plugins) diff --git a/src/Wima/Circle.cc b/src/Wima/Circle.cc index c8372fec0..ece29d5df 100644 --- a/src/Wima/Circle.cc +++ b/src/Wima/Circle.cc @@ -88,29 +88,29 @@ QPointF Circle::origin() const /*! - * \fn QPolygonF Circle::approximate(int numberOfCorners) + * \fn QList Circle::approximate(int numberOfCorners) * Returns a polygon with \a numberOfCorners corners which approximates the circle. * * \sa QPointF */ -QPolygonF Circle::approximate(int numberOfCorners) const +QList Circle::approximate(int numberOfCorners) const { if ( numberOfCorners < 3) - return QPolygonF(); + return QList(); return approximateSektor(numberOfCorners, 0, 2*M_PI); } -QPolygonF Circle::approximate(double angleDiscretisation) const +QList Circle::approximate(double angleDiscretisation) const { return approximateSektor(angleDiscretisation, 0, 2*M_PI); } -QPolygonF Circle::approximateSektor(int numberOfCorners, double alpha1, double alpha2) const +QList Circle::approximateSektor(int numberOfCorners, double alpha1, double alpha2) const { return approximateSektor((alpha2-alpha1)/double(numberOfCorners-1), alpha1, alpha2); } -QPolygonF Circle::approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const +QList Circle::approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const { // truncate alpha1 to [0, 2*pi], fmod() does not work in this case alpha1 = PlanimetryCalculus::truncateAngle(alpha1); @@ -119,28 +119,28 @@ QPolygonF Circle::approximateSektor(double angleDiscretisation, double alpha1, d angleDiscretisation = PlanimetryCalculus::truncateAngle(angleDiscretisation); if (angleDiscretisation > deltaAlpha || qFuzzyIsNull(angleDiscretisation)) - return QPolygonF(); + return QList(); - QPolygonF polygon; + QList sector; QPointF vertex(-_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); - polygon.append(vertex + _circleOrigin); + sector.append(vertex + _circleOrigin); currentAngle = PlanimetryCalculus::truncateAngle(currentAngle + angleDiscretisation); } // append last point if necessarry PlanimetryCalculus::rotateReference(vertex, alpha2); vertex = vertex + _circleOrigin; - if ( !qFuzzyIsNull(PlanimetryCalculus::distance(polygon.first(), vertex)) - && !qFuzzyIsNull(PlanimetryCalculus::distance(polygon.last(), vertex )) ){ - polygon.append(vertex); + if ( !qFuzzyIsNull(PlanimetryCalculus::distance(sector.first(), vertex)) + && !qFuzzyIsNull(PlanimetryCalculus::distance(sector.last(), vertex )) ){ + sector.append(vertex); } - return polygon; + return sector; } bool Circle::isNull() const diff --git a/src/Wima/Circle.h b/src/Wima/Circle.h index 9aa0488bb..4fc25d4c5 100644 --- a/src/Wima/Circle.h +++ b/src/Wima/Circle.h @@ -27,10 +27,10 @@ public: QPointF origin() const; // Member methodes - QPolygonF approximate (int numberOfCorners) const; - QPolygonF approximate (double angleDiscretisation) const; - QPolygonF approximateSektor(int numberOfCorners, double alpha1, double alpha2) const; - QPolygonF approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const; + QList approximate (int numberOfCorners) const; + QList approximate (double angleDiscretisation) const; + QList approximateSektor(int numberOfCorners, double alpha1, double alpha2) const; + QList approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const; bool isNull() const; signals: diff --git a/src/Wima/CircularSurveyComplexItem.cc b/src/Wima/CircularSurveyComplexItem.cc index 2b2a3c2a8..1c5f7509d 100644 --- a/src/Wima/CircularSurveyComplexItem.cc +++ b/src/Wima/CircularSurveyComplexItem.cc @@ -46,7 +46,81 @@ double CircularSurveyComplexItem::additionalTimeDelay() const void CircularSurveyComplexItem::_rebuildTransectsPhase1() { + using namespace GeoUtilities; + using namespace PolygonCalculus; + using namespace PlanimetryCalculus; + _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))); + + 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 + + QList convexPolygons; + decomposeToConvex(surveyPolygon, convexPolygons); + + QPointF origin(0, 0); + QList fullPath; + for (int i = 0; i < convexPolygons.size(); i++){ + const QPolygonF &polygon = convexPolygons[i]; + double r = r0; + + QList currPolyPath; + int direction = 1; + while (r < r1) { + 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); + } + + 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); + } + } + + QList geoPath = toGeo(fullPath, _referencePoint); + for ( const QGeoCoordinate &coordinate : geoPath) { + CoordInfo_t coordinfo = {coordinate, CoordTypeInterior}; + _transects.append(coordinfo); + } + + + + + } diff --git a/src/Wima/CircularSurveyComplexItem.h b/src/Wima/CircularSurveyComplexItem.h index 887a4d858..296fbce62 100644 --- a/src/Wima/CircularSurveyComplexItem.h +++ b/src/Wima/CircularSurveyComplexItem.h @@ -2,6 +2,12 @@ #include "TransectStyleComplexItem.h" +#include "PolygonCalculus.h" +#include "PlanimetryCalculus.h" +#include "GeoUtilities.h" +#include "QVector" +#include "Circle.h" + class CircularSurveyComplexItem :public TransectStyleComplexItem { Q_OBJECT @@ -36,6 +42,8 @@ private slots: void _rebuildTransectsPhase1 (void) final; void _recalcComplexDistance (void) final; void _recalcCameraShots (void) final; + + QGeoCoordinate _referencePoint; // center of the circular lanes, e.g. base station }; diff --git a/src/Wima/OptimisationTools.cc b/src/Wima/OptimisationTools.cc index 15ea047a7..9ac82d5aa 100644 --- a/src/Wima/OptimisationTools.cc +++ b/src/Wima/OptimisationTools.cc @@ -16,7 +16,7 @@ namespace OptimisationTools { * * \sa QList */ - typedef QPointF T; + 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 @@ -24,7 +24,7 @@ namespace OptimisationTools { || endIndex >= elements.size()) { return false; } - qWarning("optitools"); + //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{ diff --git a/src/Wima/OptimisationTools.h b/src/Wima/OptimisationTools.h index c632e7ab5..e4194ca62 100644 --- a/src/Wima/OptimisationTools.h +++ b/src/Wima/OptimisationTools.h @@ -5,7 +5,7 @@ #include namespace OptimisationTools { - typedef QPointF T; + template bool dijkstraAlgorithm(const QList &elements, int startIndex, int endIndex, QList &elementPath, std::function distance); } diff --git a/src/Wima/PlanimetryCalculus.cc b/src/Wima/PlanimetryCalculus.cc index 3f94cbf84..bdcf2a0dc 100644 --- a/src/Wima/PlanimetryCalculus.cc +++ b/src/Wima/PlanimetryCalculus.cc @@ -233,7 +233,7 @@ namespace PlanimetryCalculus { double dx = p2.x()-p1.x(); double dy = p2.y()-p1.y(); - return qSqrt(dx*dx+dy*dy); + return norm(dx, dy); } /*! @@ -617,6 +617,21 @@ angle return contains(polygon, point, dummyType); } + double norm(double x, double y) + { + return qSqrt(x*x+y*y); + } + + double norm(const QPointF &p) + { + return norm(p.x(), p.y()); + } + + double angle(const QPointF &p1) + { + return qAtan2(p1.y(), p1.x()); + } + diff --git a/src/Wima/PlanimetryCalculus.h b/src/Wima/PlanimetryCalculus.h index 5c92f9adf..5677eed60 100644 --- a/src/Wima/PlanimetryCalculus.h +++ b/src/Wima/PlanimetryCalculus.h @@ -64,7 +64,10 @@ namespace PlanimetryCalculus { double distance(const QPointF &p1, const QPointF p2); + double norm(double x, double y); + double norm(const QPointF &p); double angle(const QPointF &p1, const QPointF p2); + double angle(const QPointF &p1); double angle(const QLineF &line); double angleDegree(const QPointF &p1, const QPointF p2); double truncateAngle(double angle); diff --git a/src/Wima/PolygonCalculus.cc b/src/Wima/PolygonCalculus.cc index fa7fd7431..daecc6689 100644 --- a/src/Wima/PolygonCalculus.cc +++ b/src/Wima/PolygonCalculus.cc @@ -27,11 +27,11 @@ namespace PolygonCalculus { */ bool containsPath(QPolygonF polygon, const QPointF &c1, const QPointF &c2) { + if ( !polygon.isEmpty()) { if ( !polygon.containsPoint(c1, Qt::FillRule::OddEvenFill) || !polygon.containsPoint(c2, Qt::FillRule::OddEvenFill)) return false; - QList intersectionList; QLineF line; line.setP1(c1); @@ -39,7 +39,7 @@ namespace PolygonCalculus { PlanimetryCalculus::IntersectList intersectTypeList; bool retValue = PlanimetryCalculus::intersects(polygon, line, intersectionList, intersectTypeList); - if (!retValue) { + if (retValue) { for (int i = 0; i < intersectTypeList.size(); i++) { PlanimetryCalculus::IntersectType type = intersectTypeList[i]; if ( type == PlanimetryCalculus::EdgeEdgeIntersection @@ -482,12 +482,14 @@ namespace PolygonCalculus { bool shortestPath(QPolygonF polygon, QPointF startVertex, const QPointF &endVertex, QList &shortestPath) { using namespace PlanimetryCalculus; - offsetPolygon(polygon, 0.1); + offsetPolygon(polygon, 0.01); if ( contains(polygon, startVertex) && contains(polygon, endVertex)) { // lambda - std::function distance = [polygon](const QPointF &p1, const QPointF &p2) -> double { - if (containsPath(polygon, p1, p2)){ + QPolygonF polygon2 = polygon; + offsetPolygon(polygon2, 0.01); + std::function distance = [polygon2](const QPointF &p1, const QPointF &p2) -> double { + if (containsPath(polygon2, p1, p2)){ double dx = p1.x()-p2.x(); double dy = p1.y()-p2.y(); return sqrt(dx*dx+dy*dy); @@ -501,8 +503,7 @@ namespace PolygonCalculus { for (int i = 0; i < polygon.size(); i++) { elementList.append(polygon[i]); } - qWarning("Hi"); - return OptimisationTools::dijkstraAlgorithm(elementList, 0, 1, shortestPath, distance); + return OptimisationTools::dijkstraAlgorithm(elementList, 0, 1, shortestPath, distance); } else { return false; } @@ -535,6 +536,16 @@ namespace PolygonCalculus { return listF; } + void reversePath(QPointFList &path) + { + QPointFList pathReversed; + for ( auto element : path) { + pathReversed.prepend(element); + } + path.clear(); + path.append(pathReversed); + } + } // end PolygonCalculus namespace diff --git a/src/Wima/PolygonCalculus.h b/src/Wima/PolygonCalculus.h index 5206c66c4..96f57c1ae 100644 --- a/src/Wima/PolygonCalculus.h +++ b/src/Wima/PolygonCalculus.h @@ -22,9 +22,10 @@ namespace PolygonCalculus { bool isSimplePolygon (const QPolygonF &polygon); bool hasClockwiseWinding (const QPolygonF &path); void reversePath (QPolygonF &path); + void reversePath (QPointFList &path); void offsetPolygon (QPolygonF &polygon, double offset); bool containsPath (QPolygonF polygon, const QPointF &c1, const QPointF &c2); - void decomposeToConvex (const QPolygonF &polygon, QList &convexPolygons); + void decomposeToConvex (const QPolygonF &polygon, QList &convexPolygons); bool shortestPath (QPolygonF polygon, QPointF startVertex, const QPointF &endVertex, QList &shortestPath); QPolygonF toQPolygonF(const QVector3DList &polygon); diff --git a/src/Wima/TestPolygonCalculus.cpp b/src/Wima/TestPolygonCalculus.cpp new file mode 100644 index 000000000..474e43b25 --- /dev/null +++ b/src/Wima/TestPolygonCalculus.cpp @@ -0,0 +1,33 @@ +#include "TestPolygonCalculus.h" +#include "PolygonCalculus.h" +#include +#include + +namespace TestPolygonCalculus { + void test() + { + using namespace PolygonCalculus; + + QPolygonF polygon; + polygon << QPointF(0, 0) << QPointF(1, 0) << QPointF(1, 1) << QPointF(0, 1); + + bool retVal = containsPath(polygon, QPointF(0.1, 0.1), QPointF(0.2, 0.1)); + qDebug("true %i", retVal); + retVal = containsPath(polygon, QPointF(0.2, 0.1), QPointF(0.2, 0.1)); + qDebug("true %i", retVal); + retVal = containsPath(polygon, QPointF(1, 0.1), QPointF(0.2, 0.1)); + qDebug("true %i", retVal); + retVal = containsPath(polygon, QPointF(0.1, 0.1), QPointF(1, 0.1)); + qDebug("true %i", retVal); + retVal = containsPath(polygon, QPointF(2, 0.1), QPointF(0.2, 0.1)); + qDebug("false %i", retVal); + retVal = containsPath(polygon, QPointF(-2, 0.1), QPointF(0.2, 0.1)); + qDebug("false %i", retVal); + polygon << QPointF(0.5, 0.6) << QPointF(0.5, 0.4); + retVal = containsPath(polygon, QPointF(0.2, 0.1), QPointF(0.2, 0.9)); + qDebug("false %i", retVal); + retVal = containsPath(polygon, QPointF(0.1, 0.05), QPointF(0.51, 0.6)); + qDebug("false %i", retVal); + } + +} diff --git a/src/Wima/TestPolygonCalculus.h b/src/Wima/TestPolygonCalculus.h new file mode 100644 index 000000000..0212676bb --- /dev/null +++ b/src/Wima/TestPolygonCalculus.h @@ -0,0 +1,9 @@ +#ifndef TESTPOLYGONCALCULUS_H +#define TESTPOLYGONCALCULUS_H + + + +namespace TestPolygonCalculus { + void test(); +} +#endif // TESTPOLYGONCALCULUS_H diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index c402e51a9..4e3150d25 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -196,6 +196,10 @@ void WimaPlaner::resumeMission() bool WimaPlaner::updateMission() { +#if TEST_FUN + TestPlanimetryCalculus::test(); + TestPolygonCalculus::test(); +#endif QString errorString; setMissionReady(false); #define debug 0 diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index 4b13e5ada..50a4f59e6 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -29,6 +29,12 @@ #include "PlanimetryCalculus.h" #include "GeoUtilities.h" +#define TEST_FUN 1 + +#if TEST_FUN + #include "TestPolygonCalculus.h" + #include "testplanimetrycalculus.h" +#endif class WimaPlaner : public QObject { diff --git a/src/Wima/testplanimetrycalculus.cpp b/src/Wima/testplanimetrycalculus.cpp new file mode 100644 index 000000000..8d5bc1e96 --- /dev/null +++ b/src/Wima/testplanimetrycalculus.cpp @@ -0,0 +1,45 @@ +#include "testplanimetrycalculus.h" + +namespace TestPlanimetryCalculus { + void test(){ + using namespace PlanimetryCalculus; + + QLineF line(QPointF(0,0), QPointF(1,0)); + + IntersectType type; + QPointF point; + bool retVal = intersects(line, QLineF(QPointF(0, 0.1), QPointF(1, 0.1)), point, type); + qDebug("false %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, 0), QPointF(0, 0)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, 0), QPointF(0, 1)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, 1), QPointF(0, -1)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, 1), QPointF(1, -1)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, -1), QPointF(1, 1)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, 0), QPointF(1, 1)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, -1), QPointF(1, 0)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, 1), QPointF(1, -1)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(1, 1), QPointF(0, -1)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(1, 1), QPointF(0, 0)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(1, 0), QPointF(0, -1)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, 1), QPointF(1, 0)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, -1), QPointF(1, 0)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0, -1), QPointF(1, 0)), point, type); + qDebug("true %i", retVal); + retVal = intersects(line, QLineF(QPointF(0.5, 0), QPointF(1, 1)), point, type); + qDebug("true %i", retVal); + } + +} diff --git a/src/Wima/testplanimetrycalculus.h b/src/Wima/testplanimetrycalculus.h new file mode 100644 index 000000000..5977e3803 --- /dev/null +++ b/src/Wima/testplanimetrycalculus.h @@ -0,0 +1,12 @@ +#pragma once + +#include "PlanimetryCalculus.h" + +#include +#include + +namespace TestPlanimetryCalculus { + + void test(); + +} -- 2.22.0