snake lib edited

libs/snake/snake/CMakeLists.txt

@@ -13,9 +13,13 @@ add_library(snake
     snake_geometry.cpp
 )
 
-find_package (GeographicLib REQUIRED)
-target_link_libraries (snake ${GeographicLib_LIBRARIES})
 
+set(CLIPPER_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../clipper)
+
+add_library(polyclipping ${CLIPPER_PATH}/clipper.cpp)
+include_directories(${CLIPPER_PATH}) # external header only lib
+find_package (GeographicLib REQUIRED)
+target_link_libraries (snake ${GeographicLib_LIBRARIES} polyclipping)
 target_include_directories (snake PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 
@@ -27,3 +31,5 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../mason_packages/headers/var
 
 add_subdirectory(test)
 
+
+

libs/snake/snake/catch.hpp

@@ -8,6 +8,7 @@
  *  Distributed under the Boost Software License, Version 1.0. (See accompanying
  *  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
  */
+#pragma once
 #ifndef TWOBLUECUBES_SINGLE_INCLUDE_CATCH_HPP_INCLUDED
 #define TWOBLUECUBES_SINGLE_INCLUDE_CATCH_HPP_INCLUDED
 // start catch.hpp

libs/snake/snake/snake.h

@@ -16,25 +16,40 @@ namespace snake {
     enum AreaType {MeasurementArea, ServiceArea, Corridor};
 
     struct Area {
-        vector<GeoPoint3D>  geoPolygon;
+        GeoPoint2DList      geoPolygon;
         double              altitude;
         size_t              layers;
         AreaType            type;
     };
 
+    //========================================================================================
+    // Scenario
+    //========================================================================================
     class Scenario{
     public:
         Scenario();
 
-        bool setArea(Area &area);
+        bool addArea(Area &area);
 
-        Area getMeasurementArea() {return _measurementArea;}
-        Area getServiceArea() {return _serviceArea;}
-        Area getCorridor() {return _corridor;}
+        const Area &getMeasurementArea() const {return _measurementArea;}
+        const Area &getServiceArea() const {return _serviceArea;}
+        const Area &getCorridor() const {return _corridor;}
+
+        const BoostPolygon &getMeasurementAreaENU() {return _measurementAreaENU;}
+        const BoostPolygon &getServiceAreaENU()     {return _serviceAreaENU;}
+        const BoostPolygon &getCorridorENU()        {return _corridorENU;}
+        const BoostPolygon &getJoineAreaENU()       {return _joinedAreaENU;}
+        const GeoPoint3D   &getOrigin()             {return _geoOrigin;}
+
+        const vector<BoostPolygon> &getTilesENU()               {return _tilesENU;}
+        const BoostPointList       &getTileCenterPointsENU()    {return _tileCenterPointsENU;}
+        const min_bbox_rt          &getMeasurementAreaBBoxENU() {return _mAreaBoundingBox;}
+        const BoostPoint           &getHomePositonENU()         {return _homePositionENU;}
 
         bool defined(double tileWidth, double tileHeight, double minTileArea);
 
-        string error_str;
+        string errorString;
+
     private:
         bool _areas2enu();
         bool _setMeasurementArea(Area &area);
@@ -48,20 +63,52 @@ namespace snake {
         Area _serviceArea;
         Area _corridor;
 
-        Point2DList _measurementAreaENU;
-        Point2DList _serviceAreaENU;
-        Point2DList _corridorENU;
-        Point2DList _joinedAreaENU;
+        BoostPolygon _measurementAreaENU;
+        BoostPolygon _serviceAreaENU;
+        BoostPolygon _corridorENU;
+        BoostPolygon _joinedAreaENU;
 
         min_bbox_rt _mAreaBoundingBox;
 
-        vector<Point2DList>    _tilesENU;
-        vector<Point2D>        _tilesCenterPointsENU;
+        vector<BoostPolygon>    _tilesENU;
+        BoostPointList          _tileCenterPointsENU;
 
         GeoPoint3D _geoOrigin;
-        Point2D    _homePositionENU;
+        BoostPoint _homePositionENU;
+    };
+
+    //========================================================================================
+    // FlightPlan
+    //========================================================================================
+    class FlightPlan{
+    public:
+        FlightPlan();
+        FlightPlan(const Scenario &scenario);
 
-        bool _defined_bool;
+        void setScenario(const Scenario &scenario)          {_scenario = scenario;}
+        void setProgress(const vector<int8_t> &progress)    {_progress = progress;}
+
+        const Scenario          &getScenario(void)      {return _scenario;}
+        const BoostPointList    &getWaypointsENU(void)  {return _waypointsENU;}
+        const GeoPoint2DList    &getWaypoints(void)     {return _waypoints;}
+
+        bool generate(double lineDistance, double minTransectLength);
+
+        string errorString;
+
+    private:
+        bool _generateTransects(double lineDistance, double minTransectLength);
+        bool _generateRoutingModel();
+        Scenario                                _scenario;
+        BoostPointList                          _waypointsENU;
+        GeoPoint2DList                          _waypoints;
+        vector<tuple<BoostPoint, BoostPoint>>   _transects;
+        vector<int8_t>                          _progress;
+        BoostPolygon                            _joinedAreaOffset;
     };
+
+    namespace detail {
+        double polygonOffset = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
+    }
 }
 

libs/snake/snake/snake_geometry.h

 #pragma once
 #include <vector>
 #include <array>
+#include <boost/geometry.hpp>
 
 #include "WGS84toCartesian.hpp"
 
+namespace bg = boost::geometry;
+
 namespace snake_geometry {
 
-typedef std::array<double, 2>   Point2D;
-typedef std::vector<Point2D>         Point2DList;
+typedef std::array<double, 3>   Point2D;
 typedef std::array<double, 3>   Point3D;
-typedef std::vector<Point3D>         Point3DList;
-typedef std::array<double, 2>   GeoPoint2D;
-typedef std::vector<GeoPoint2D>      GeoPoint2DList;
 typedef std::array<double, 3>   GeoPoint3D;
-typedef std::vector<GeoPoint3D>      GeoPoint3DList;
+typedef std::array<double, 2>   GeoPoint2D;
+typedef std::vector<Point2D>    Point2DList;
+typedef std::vector<Point3D>    Point3DList;
+typedef std::vector<GeoPoint2D> GeoPoint2DList;
+typedef std::vector<GeoPoint3D> GeoPoint3DList;
+
+typedef bg::model::point<double, 2, bg::cs::cartesian>  BoostPoint;
+typedef std::vector<BoostPoint>                         BoostPointList;
+typedef bg::model::polygon<BoostPoint>                  BoostPolygon;
 
 typedef struct {
     double width;
     double height;
     double angle;
-    std::array<Point2D,4> corners;
+    BoostPolygon corners;
 }min_bbox_rt;
 
-    Point3D toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position);
-    GeoPoint3D fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition);
+void toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position, Point3D &ENUPosition);
+void fromENU(const Point3D &WGS84Reference, const Point3D &ENUPosition, GeoPoint3D &WGS84Position);
+
+void polygonCenter(const BoostPolygon &polygon, BoostPoint &center);
+void minimalBoundingBox(const BoostPolygon &polygon, min_bbox_rt &minBBox);
+void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset);
+
+void rotateDeg(const Point2DList &point_list, Point2DList &rotated_point_list, double degree);
+void rotateRad(const Point2DList &point_list, Point2DList &rotated_point_list, double rad);
+
+bool isClockwise(const Point2DList &point_list);
 
-    Point2D polygonCenter(const Point2DList &polygon);
-    min_bbox_rt minimalBoundingBox(const Point2DList &polygon);
+void toBoost(const Point2D &point, BoostPoint &boost_point);
+void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon);
 
+void fromBoost(const BoostPoint &boost_point, Point2D &point);
+void fromBoost(const BoostPolygon &boost_polygon, Point2DList &point_list);
 }

libs/snake/snake/test/CMakeLists.txt

-add_executable(snakeTest test_snake_geometry.cpp) # tests for snake library
+add_executable(snakeTest test_snake_geometry.cpp test_snake.cpp) # tests for snake library
 target_link_libraries(snakeTest snake)

libs/snake/snake/test/test_snake.cpp

+#include <chrono>
+#include <iostream>
+#include <fstream>
+
+#include "catch.hpp"
+#include "snake.h"
+
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+
+using namespace snake;
+using namespace std;
+
+TEST_CASE( "Test Scenario Class, check out Scenario0.svg" ) {
+    // Data
+    Area measurementArea{GeoPoint2DList{GeoPoint2D{47.768120, 16.530380},
+                                     GeoPoint2D{47.768205, 16.530558},
+                                     GeoPoint2D{47.768147, 16.530566},
+                                     GeoPoint2D{47.768326, 16.530926},
+                                     GeoPoint2D{47.768135, 16.531025},
+                                     GeoPoint2D{47.768144, 16.530845},
+                                     GeoPoint2D{47.768103, 16.530978},
+                                     GeoPoint2D{47.767940, 16.530393}},
+                        10, /*altitude*/
+                        1, /*layers*/
+                        AreaType::MeasurementArea};
+
+    Area     serviceArea{GeoPoint2DList{GeoPoint2D{47.767874, 16.530409},
+                                     GeoPoint2D{47.767968, 16.530677},
+                                     GeoPoint2D{47.767860, 16.530723},
+                                     GeoPoint2D{47.767777, 16.530447}},
+                        10, /*altitude*/
+                        1, /*layers*/
+                        AreaType::ServiceArea};
+
+    Area        corridor{GeoPoint2DList{GeoPoint2D{47.767853, 16.530482},
+                                        GeoPoint2D{47.768004, 16.530382},
+                                        GeoPoint2D{47.768035, 16.530549},
+                                        GeoPoint2D{47.767880, 16.530629}},
+                        10, /*altitude*/
+                        1, /*layers*/
+                        AreaType::Corridor};
+
+    Scenario scenario;
+    scenario.addArea(measurementArea);
+    scenario.addArea(serviceArea);
+    scenario.addArea(corridor);
+
+    // Scenario defined?
+    double tileWidth = 3; // m
+    double tileHeight = 3; // m
+    double minTileArea = 2; // m^2
+    auto start = std::chrono::high_resolution_clock::now();
+    scenario.defined(tileWidth, tileHeight, minTileArea);
+    cout << "Execution time: ";
+    cout << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start).count();
+    cout << " ms"  << endl;
+
+    // Store to svg.
+    std::ofstream svg("Scenario0.svg");
+    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 400, 400);
+
+    const BoostPolygon &boost_measurement_area  = scenario.getMeasurementAreaENU();
+    const BoostPolygon &boost_service_area      = scenario.getServiceAreaENU();
+    const BoostPolygon &boost_corridor_area     = scenario.getCorridorENU();
+    const BoostPolygon &boos_joined_area        = scenario.getJoineAreaENU();
+    const BoostPolygon &boost_bbox              = scenario.getMeasurementAreaBBoxENU().corners;
+
+    mapper.add(boost_measurement_area);
+    mapper.add(boost_service_area);
+    mapper.add(boost_corridor_area);
+    mapper.add(boos_joined_area);
+    mapper.add(boost_bbox);
+    mapper.map(boost_measurement_area, "fill-opacity:0.1;fill:rgb(0,255,0);stroke:rgb(0,255,0);stroke-width:2");
+    mapper.map(boost_service_area,     "fill-opacity:0.1;fill:rgb(235, 232, 52);stroke:rgb(235, 232, 52);stroke-width:2");
+    mapper.map(boost_corridor_area,    "fill-opacity:0.1;fill:rgb(52, 171, 235);stroke:rgb(52, 171, 235);stroke-width:2");
+    mapper.map(boos_joined_area,       "fill-opacity:0.0;fill:rgb(245, 66, 218);stroke:rgb(245, 66, 218);stroke-width:3");
+    mapper.map(boost_bbox,             "fill-opacity:0.0;fill:rgb(255, 55, 0);stroke:rgb(255, 55, 0);stroke-width:2");
+
+    auto tiles = scenario.getTilesENU();
+    //cout << "Tile count: " << tiles.size() << endl;
+    for (auto tile : tiles) {
+        mapper.add(tile);
+        mapper.map(tile, "fill-opacity:0.1;fill:rgb(0,0,0);stroke:rgb(0,0,0);stroke-width:2");
+    }
+
+    auto center_points = scenario.getTileCenterPointsENU();
+    for (auto point : center_points) {
+        mapper.add(point);
+        mapper.map(point, "fill-opacity:0.5;fill:rgb(255, 55, 0);stroke:rgb(255, 55, 0);stroke-width:2", 2);
+    }
+
+    REQUIRE(scenario.defined(tileWidth, tileHeight, minTileArea));
+    cout << scenario.errorString;
+
+}
+
+TEST_CASE( "Test Scenario Class. Negative Parameter." ) {
+    // Data
+    Area measurementArea{GeoPoint2DList{GeoPoint2D{47.768120, 16.530380},
+                                     GeoPoint2D{47.768205, 16.530558},
+                                     GeoPoint2D{47.768147, 16.530566},
+                                     GeoPoint2D{47.768326, 16.530926},
+                                     GeoPoint2D{47.768135, 16.531025},
+                                     GeoPoint2D{47.768144, 16.530845},
+                                     GeoPoint2D{47.768103, 16.530978},
+                                     GeoPoint2D{47.767940, 16.530393}},
+                        10, /*altitude*/
+                        1, /*layers*/
+                        AreaType::MeasurementArea};
+
+    Area     serviceArea{GeoPoint2DList{GeoPoint2D{47.767874, 16.530409},
+                                     GeoPoint2D{47.767968, 16.530677},
+                                     GeoPoint2D{47.767860, 16.530723},
+                                     GeoPoint2D{47.767777, 16.530447}},
+                        10, /*altitude*/
+                        1, /*layers*/
+                        AreaType::ServiceArea};
+
+    Area        corridor{GeoPoint2DList{GeoPoint2D{47.767853, 16.530482},
+                                        GeoPoint2D{47.768004, 16.530382},
+                                        GeoPoint2D{47.768035, 16.530549},
+                                        GeoPoint2D{47.767880, 16.530629}},
+                        10, /*altitude*/
+                        1, /*layers*/
+                        AreaType::Corridor};
+
+    Scenario scenario;
+    scenario.addArea(measurementArea);
+    scenario.addArea(serviceArea);
+    scenario.addArea(corridor);
+
+    // Scenario defined?
+    double tileWidth = 5; // m
+    double tileHeight = 5; // m
+    double minTileArea = -2; // m^2
+
+    REQUIRE(scenario.defined(tileWidth, tileHeight, minTileArea) == false);
+
+}
+
+TEST_CASE( "Test Scenario Class. Missing Corridor." ) {
+    // Data
+    Area measurementArea{GeoPoint2DList{GeoPoint2D{47.768120, 16.530380},
+                                     GeoPoint2D{47.768205, 16.530558},
+                                     GeoPoint2D{47.768147, 16.530566},
+                                     GeoPoint2D{47.768326, 16.530926},
+                                     GeoPoint2D{47.768135, 16.531025},
+                                     GeoPoint2D{47.768144, 16.530845},
+                                     GeoPoint2D{47.768103, 16.530978},
+                                     GeoPoint2D{47.767940, 16.530393}},
+                        10, /*altitude*/
+                        1, /*layers*/
+                        AreaType::MeasurementArea};
+
+    Area     serviceArea{GeoPoint2DList{GeoPoint2D{47.767874, 16.530409},
+                                     GeoPoint2D{47.767968, 16.530677},
+                                     GeoPoint2D{47.767860, 16.530723},
+                                     GeoPoint2D{47.767777, 16.530447}},
+                        10, /*altitude*/
+                        1, /*layers*/
+                        AreaType::ServiceArea};
+
+    Area        corridor{GeoPoint2DList{GeoPoint2D{47.767853, 16.530482},
+                                        GeoPoint2D{47.768004, 16.530382},
+                                        GeoPoint2D{47.768035, 16.530549},
+                                        GeoPoint2D{47.767880, 16.530629}},
+                        10, /*altitude*/
+                        1, /*layers*/
+                        AreaType::Corridor};
+
+    Scenario scenario;
+    scenario.addArea(measurementArea);
+    scenario.addArea(serviceArea);
+    //scenario.addArea(corridor);
+
+    // Scenario defined?
+    double tileWidth = 5; // m
+    double tileHeight = 5; // m
+    double minTileArea = 2; // m^2
+
+    REQUIRE(scenario.defined(tileWidth, tileHeight, minTileArea) == false);
+
+}