Snake lib edited

libs/snake/snake/CMakeLists.txt

@@ -24,6 +24,8 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../mason_packages/headers/geo
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../mason_packages/headers/polylabel/1.0.3/include)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../mason_packages/headers/variant/1.1.0/include)
 
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../clipper)
+
 
 
 target_compile_definitions(snake PRIVATE SNAKE_LIBRARY)

libs/snake/snake/snake.cpp

 #include "snake.h"
 
+#include "clipper.hpp"
+#include "assert.h"
+
+
 namespace snake {
 
-    bool Scenario::setArea(Area &area)
+Scenario::Scenario() :
+    _mAreaBoundingBox(min_bbox_rt{0, 0, 0, Point2D{0,0}})
+{
+
+}
+
+bool Scenario::setArea(Area &area)
     {
         if (area.geoPolygon.size() < 3){
             error_str = "Area has less than three vertices.";
@@ -84,4 +94,128 @@ namespace snake {
         return true;
     }
 
+    bool Scenario::_calculateBoundingBox()
+    {
+        _mAreaBoundingBox = minimalBoundingBox(_measurementAreaENU);
+        return true;
+    }
+    /**
+     * Devides the (measurement area) bounding  box into tiles and clips it to the measurement area.
+     *
+     * Devides the (measurement area) bounding  box into tiles of width \p tileWidth and height \p tileHeight.
+     * Clips the resulting tiles to the measurement area. Tiles are rejected, if their area is smaller than \p minTileArea.
+     * The function assumes that \a _measurementAreaENU and \a _mAreaBoundingBox have correct values. \see \ref Scenario::_areas2enu() and \ref
+     * Scenario::_calculateBoundingBox().
+     *
+     * @param tileWidth The width of a tile.
+     * @param tileHeight The heigth of a tile.
+     * @param minTileArea The minimal area of a tile.
+     *
+     * @return Returns true if successful.
+     */
+    bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTileArea)
+    {
+        std::vector<Point2DList> tiles_unclipped;
+
+        return true;
+    }
+
+    bool Scenario::_calculateJoinedArea()
+    {
+        ClipperLib::Path measurementArea;
+        ClipperLib::Path serviceArea;
+        ClipperLib::Path corridor;
+
+        // Apply scaling and convert ENU polygons (double) to ClipperLib::cInt polygons.
+        for (auto vertex : _measurementAreaENU){
+            ClipperLib::IntPoint intVertex{ClipperLib::cInt(vertex[0]*clipper_scale),
+                                           ClipperLib::cInt(vertex[1]*clipper_scale)};
+            measurementArea.push_back(intVertex);
+        }
+
+        for (auto vertex : _serviceAreaENU){
+            ClipperLib::IntPoint intVertex{ClipperLib::cInt(vertex[0]*clipper_scale),
+                                           ClipperLib::cInt(vertex[1]*clipper_scale)};
+            serviceArea.push_back(intVertex);
+        }
+
+        bool corridorValid = false;
+        if (_corridorENU.size() > 0) {
+            for (auto vertex : _corridorENU){
+                ClipperLib::IntPoint intVertex{ClipperLib::cInt(vertex[0]*clipper_scale),
+                                               ClipperLib::cInt(vertex[1]*clipper_scale)};
+                corridor.push_back(intVertex);
+            }
+            corridorValid = true;
+        }
+
+        // Check if measurement area and service area are overlapping.
+        ClipperLib::Clipper cp1;
+        cp1.AddPath(measurementArea, ClipperLib::ptClip, true);
+        cp1.AddPath(serviceArea, ClipperLib::ptSubject, true);
+
+        // Execute clipper
+        ClipperLib::Paths solution;
+        cp1.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
+
+        // Measurement area and service area overlapping?
+        bool overlaping_s_m = false;
+        if (solution.size() > 0){
+            overlaping_s_m = true;
+        }
+
+
+        // Check if corridor is connecting measurement area and service area.
+        bool corridor_is_connection = false;
+        if (corridorValid) {
+            ClipperLib::Clipper cp2;
+            solution.clear();
+            cp2.AddPath(measurementArea, ClipperLib::ptClip, true);
+            cp2.AddPath(corridor, ClipperLib::ptSubject, true);
+            cp2.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
+
+            // Corridor overlaping with measurement area?
+            if (solution.size() > 0) {
+                // Check if corridor overlaps with service area.
+                cp2.Clear();
+                solution.clear();
+                cp2.AddPath(serviceArea, ClipperLib::ptClip, true);
+                cp2.AddPath(corridor, ClipperLib::ptSubject, true);
+                cp2.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
+
+                // Corridor overlaping with service area?
+                if (solution.size() > 0) {
+                    corridor_is_connection = true;
+                }
+            }
+        }
+
+        // Are areas joinable?
+        if (overlaping_s_m){
+            if(corridor_is_connection){
+                cp1.AddPath(corridor, ClipperLib::ptSubject, true);
+            }
+        } else if (corridor_is_connection){
+            cp1.AddPath(corridor, ClipperLib::ptSubject, true);
+        } else {
+            error_str = "Areas are not overlapping";
+            return false;
+        }
+
+        // Join areas.
+        solution.clear();
+        cp1.Execute(ClipperLib::ctUnion, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
+
+        if (solution.size() != 1)
+            assert(0);
+
+        _joinedAreaENU.clear();
+        for (auto intVertex : solution[0]){
+            Point2D vertex{double(intVertex.X)/clipper_scale, double(intVertex.Y)/clipper_scale};
+            _joinedAreaENU.push_back(vertex);
+        }
+
+        return true;
+    }
+
 }

libs/snake/snake/snake.h

@@ -6,6 +6,8 @@
 
 #include "snake_geometry.h"
 
+#define clipper_scale 10000.0
+
 using namespace std;
 using namespace snake_geometrie;
 
@@ -14,10 +16,10 @@ namespace snake {
     enum AreaType {MeasurementArea, ServiceArea, Corridor};
 
     struct Area {
-        vector<GeoPoint3D> geoPolygon;
-        double          altitude;
-        size_t          layers;
-        AreaType        type;
+        vector<GeoPoint3D>  geoPolygon;
+        double              altitude;
+        size_t              layers;
+        AreaType            type;
     };
 
     class Scenario{
@@ -39,11 +41,8 @@ namespace snake {
         bool _setServiceArea(Area &area);
         bool _setCorridor(Area &area);
         bool _calculateBoundingBox();
-        bool _calculateTiles();
+        bool _calculateTiles(double tileWidth, double tileHeight, double minTileArea);
         bool _calculateJoinedArea();
-        bool _calculateMAreaLocal();
-        bool _calculateSAreaLocal();
-        bool _calculateCorridorLocal();
 
         Area _measurementArea;
         Area _serviceArea;
@@ -54,6 +53,8 @@ namespace snake {
         Point2DList _corridorENU;
         Point2DList _joinedAreaENU;
 
+        min_bbox_rt _mAreaBoundingBox;
+
         vector<Point2DList>    _tilesENU;
         vector<Point2D>        _tilesCenterPointsENU;
 

libs/snake/snake/snake_geometry.h

@@ -22,10 +22,10 @@ typedef struct {
     Point2D origin;
 }min_bbox_rt;
 
-    Point3D toENU(const Point3D &WGS84Reference, const Point3D &WGS84Position);
-    Point3D fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition);
+Point3D toENU(const Point3D &WGS84Reference, const Point3D &WGS84Position);
+Point3D fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition);
 
-    Point2D polygonCenter(const Point2DList &polygon);
-    min_bbox_rt minimalBoundingBox(const Point2DList &polygon);
+Point2D polygonCenter(const Point2DList &polygon);
+min_bbox_rt minimalBoundingBox(const Point2DList &polygon);
 
 }