circularSurvey edited

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)

src/Wima/Circle.cc

@@ -88,29 +88,29 @@ QPointF Circle::origin() const
 
 
 /*!
- * \fn QPolygonF Circle::approximate(int numberOfCorners)
+ * \fn QList<QPointF> Circle::approximate(int numberOfCorners)
  * Returns a polygon with \a numberOfCorners corners which approximates the circle.
  *
  * \sa QPointF
  */
-QPolygonF Circle::approximate(int numberOfCorners) const
+QList<QPointF> Circle::approximate(int numberOfCorners) const
 {
     if ( numberOfCorners < 3)
-        return QPolygonF();
+        return QList<QPointF>();
     return approximateSektor(numberOfCorners, 0, 2*M_PI);
 }
 
-QPolygonF Circle::approximate(double angleDiscretisation) const
+QList<QPointF> Circle::approximate(double angleDiscretisation) const
 {
     return approximateSektor(angleDiscretisation, 0, 2*M_PI);
 }
 
-QPolygonF Circle::approximateSektor(int numberOfCorners, double alpha1, double alpha2) const
+QList<QPointF> 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<QPointF> 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<QPointF>();
 
 
-    QPolygonF polygon;
+    QList<QPointF> 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

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<QPointF> approximate      (int numberOfCorners) const;
+    QList<QPointF> approximate      (double angleDiscretisation) const;
+    QList<QPointF> approximateSektor(int numberOfCorners, double alpha1, double alpha2) const;
+    QList<QPointF> approximateSektor(double angleDiscretisation, double alpha1, double alpha2) const;
     bool isNull() const;
 
 signals:

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<double> distances;
+    for (const QPointF &p : boundingPolygon) distances.append(norm(p));
+    QVector<double> 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<QPolygonF> convexPolygons;
+    decomposeToConvex(surveyPolygon, convexPolygons);
+
+    QPointF origin(0, 0);
+    QList<QPointF> fullPath;
+    for (int i = 0; i < convexPolygons.size(); i++){
+        const QPolygonF &polygon = convexPolygons[i];
+        double r = r0;
+
+        QList<QPointF> currPolyPath;
+        int direction = 1;
+        while (r < r1) {
+            Circle circle(r, origin);
+            QList<QPointF> sector = circle.approximateSektor(dalpha, alpha1, alpha2);
+            QList<QPointF> 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<QPointF> path;
+            if(!shortestPath(surveyPolygon, start, end, path))
+                return;
+            path.removeFirst();
+            path.removeLast();
+            fullPath.append(path);
+            fullPath.append(fullPath);
+        }
+    }
+
+    QList<QGeoCoordinate> geoPath = toGeo(fullPath, _referencePoint);
+    for ( const QGeoCoordinate &coordinate : geoPath) {
+        CoordInfo_t coordinfo = {coordinate, CoordTypeInterior};
+        _transects.append(coordinfo);
+    }
+
+
+
+
+
 
 
 }

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
 };
 
 

src/Wima/OptimisationTools.cc

@@ -16,7 +16,7 @@ namespace OptimisationTools {
      *
      * \sa QList
      */
-    typedef QPointF T;
+    template <class T>
     bool dijkstraAlgorithm(const QList<T> &elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> 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{

src/Wima/OptimisationTools.h

@@ -5,7 +5,7 @@
 #include <QPointF>
 
 namespace OptimisationTools {
-    typedef QPointF T;
+    template <class T>
     bool dijkstraAlgorithm(const QList<T> &elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> distance);
 }
 

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());
+    }
+
 
 
 

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);

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<QPointF>   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<QPointF> &shortestPath)
     {
         using namespace PlanimetryCalculus;
-        offsetPolygon(polygon, 0.1);
+        offsetPolygon(polygon, 0.01);
         if (    contains(polygon, startVertex)
              && contains(polygon, endVertex)) {
             // lambda
-            std::function<double(const QPointF &, const QPointF &)> distance = [polygon](const QPointF &p1,  const QPointF &p2) -> double {
-                if (containsPath(polygon, p1, p2)){
+            QPolygonF polygon2 = polygon;
+            offsetPolygon(polygon2, 0.01);
+            std::function<double(const QPointF &, const QPointF &)> 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<QPointF>(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
 

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<QPolygon> &convexPolygons);
+    void             decomposeToConvex   (const QPolygonF &polygon, QList<QPolygonF> &convexPolygons);
     bool             shortestPath        (QPolygonF polygon, QPointF startVertex, const QPointF &endVertex, QList<QPointF> &shortestPath);
 
     QPolygonF toQPolygonF(const QVector3DList &polygon);

src/Wima/TestPolygonCalculus.cpp

+#include "TestPolygonCalculus.h"
+#include "PolygonCalculus.h"
+#include <QPolygonF>
+#include <QPointF>
+
+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);
+    }
+
+}

src/Wima/TestPolygonCalculus.h

+#ifndef TESTPOLYGONCALCULUS_H
+#define TESTPOLYGONCALCULUS_H
+
+
+
+namespace TestPolygonCalculus {
+    void test();
+}
+#endif // TESTPOLYGONCALCULUS_H

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

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
 {

src/Wima/testplanimetrycalculus.cpp

+#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);
+    }
+
+}

src/Wima/testplanimetrycalculus.h

+#pragma once
+
+#include "PlanimetryCalculus.h"
+
+#include <QPointF>
+#include <QLineF>
+
+namespace TestPlanimetryCalculus {
+
+    void test();
+
+}