Snake lib edited

libs/snake/snake/CMakeLists.txt

 cmake_minimum_required(VERSION 3.5)
 
-project(snake LANGUAGES CXX)
+project(SNAKE LANGUAGES CXX)
 
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-add_library(snake SHARED
+add_library(snake
     snake.h
     snake_geometry.h
     snake.cpp
     snake_geometry.cpp
 )
 
+find_package (GeographicLib REQUIRED)
+target_link_libraries (snake ${GeographicLib_LIBRARIES})
 
+target_include_directories (snake PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../WGS84toCartesian)
-#add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../WGS84toCartesian WGS84toCartesian)
-#target_link_libraries(${PROJECT_NAME} WGS84toCartesian)
 
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../WGS84toCartesian) # external header only lib
 
-include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../mason_packages/headers/geometry/1.0.0/include)
-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}/../../mason_packages/headers/geometry/1.0.0/include) # external header only lib
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../mason_packages/headers/polylabel/1.0.3/include) # external header only lib
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../mason_packages/headers/variant/1.1.0/include) # external header only lib
 
+add_subdirectory(test)
 
-
-target_compile_definitions(snake PRIVATE SNAKE_LIBRARY)

libs/snake/snake/snake.cpp

@@ -22,7 +22,7 @@ namespace snake {
         if (_measurementArea.geoPolygon.size() > 0){
             _measurementAreaENU.clear();
             for(auto vertex : _measurementArea.geoPolygon) {
-                    Point3D ENUVertex = toENU(_geoOrigin, vertex);
+                    Point3D ENUVertex = toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _measurementArea.altitude});
                     _measurementAreaENU.push_back(Point2D{ENUVertex[0], ENUVertex[1]});
             }
             _homePositionENU = polygonCenter(_measurementAreaENU);
@@ -30,7 +30,7 @@ namespace snake {
             if (_serviceArea.geoPolygon.size() > 0){
                 _serviceAreaENU.clear();
                 for(auto vertex : _serviceArea.geoPolygon) {
-                        Point3D ENUVertex = toENU(_geoOrigin, vertex);
+                        Point3D ENUVertex = toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _serviceArea.altitude});
                         _serviceAreaENU.push_back(Point2D{ENUVertex[0], ENUVertex[1]});
                 }
             } else{
@@ -41,7 +41,7 @@ namespace snake {
             if (_corridor.geoPolygon.size() > 0){
                 _corridorENU.clear();
                 for(auto vertex : _corridor.geoPolygon) {
-                        Point3D ENUVertex = toENU(_geoOrigin, vertex);
+                        Point3D ENUVertex = toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _corridor.altitude});
                         _corridorENU.push_back(Point2D{ENUVertex[0], ENUVertex[1]});
                 }
             }

libs/snake/snake/snake.h

@@ -7,7 +7,7 @@
 #include "snake_geometry.h"
 
 using namespace std;
-using namespace snake_geometrie;
+using namespace snake_geometry;
 
 namespace snake {
 

libs/snake/snake/snake_geometry.cpp

@@ -6,37 +6,37 @@
 #include <boost/geometry/geometries/polygon.hpp>
 #include <boost/geometry/geometries/adapted/boost_tuple.hpp>
 
+
+#include <GeographicLib/Geocentric.hpp>
+#include <GeographicLib/LocalCartesian.hpp>
+
 using namespace mapbox;
-using namespace snake_geometrie;
+using namespace snake_geometry;
+using namespace std;
 
 BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)
 
 
 namespace snake_geometry {
 
-Point3D toENU(const Point3D &WGS84Reference, const Point3D &WGS84Position)
+Point3D toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position)
 {
-    Point2D WGS84Reference2D{WGS84Reference[0], WGS84Reference[1]};
-    Point2D WGS84Position2D{WGS84Position[0], WGS84Position[1]};
+    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
+    GeographicLib::LocalCartesian proj(WGS84Reference[0], WGS84Reference[1], WGS84Reference[2], earth);
 
-    Point2D ENUPosition2D = wgs84::toCartesian(WGS84Reference2D, WGS84Position2D);
-
-    double deltaH = WGS84Position[3] - WGS84Reference[3];
-    Point3D ENUPosition{ENUPosition2D[0], ENUPosition2D[1], deltaH};
+    Point3D ENUPosition;
+    proj.Forward(WGS84Position[0], WGS84Position[1], WGS84Position[2], ENUPosition[0], ENUPosition[1], ENUPosition[2]);
     return ENUPosition;
 }
 
-Point3D fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition)
+GeoPoint3D fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition)
 {
-    Point2D WGS84Reference2D{WGS84Reference[0], WGS84Reference[1]};
-    Point2D CartesianPosition2D{CartesianPosition[0], CartesianPosition[1]};
-
-    Point2D WGS84Position2D = wgs84::fromCartesian(WGS84Reference2D, CartesianPosition2D);
-
-    double H = WGS84Reference[3] + CartesianPosition[3];
-    Point3D WGS84Position{WGS84Position2D[0], WGS84Position2D[1], H};
-    return WGS84Position;
+    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
+    GeographicLib::LocalCartesian proj(WGS84Reference[0], WGS84Reference[1], WGS84Reference[2], earth);
 
+    GeoPoint3D GeoPosition;
+    proj.Reverse(CartesianPosition[0], CartesianPosition[1], CartesianPosition[2], GeoPosition[0], GeoPosition[1], GeoPosition[2]);
+    return GeoPosition;
 }
 
 Point2D polygonCenter(const Point2DList &polygon)
@@ -53,9 +53,6 @@ Point2D polygonCenter(const Point2DList &polygon)
    return Point2D{center.x, center.y};
 }
 
-
-}
-
 min_bbox_rt minimalBoundingBox(const Point2DList &polygon)
 {
     /*
@@ -117,7 +114,7 @@ min_bbox_rt minimalBoundingBox(const Point2DList &polygon)
     //# Compute edges (x2-x1,y2-y1)
     std::vector<point_t> edges;
     auto convex_hull_outer = convex_hull.outer();
-    for (size_t i=0; i < convex_hull_outer.size(); ++i) {
+    for (size_t i=0; i < convex_hull_outer.size()-1; ++i) {
         point_t p1      = convex_hull_outer.at(i);
         point_t p2      = convex_hull_outer.at(i+1);
         double edge_x   = p2.get<0>() - p1.get<0>();
@@ -133,7 +130,7 @@ min_bbox_rt minimalBoundingBox(const Point2DList &polygon)
     //# print "Unique edge angles: \n", edge_angles
 
 
-    min_bbox_rt min_bbox{0, 0, 0, Point2D{0,0}};
+    min_bbox_rt min_bbox{0, 0, 0, {Point2D{0,0}}};
     double min_area = std::numeric_limits<double>::infinity();
     // Test each angle to find bounding box with smallest area
     // print "Testing", len(edge_angles), "possible rotations for bounding box... \n"
@@ -150,8 +147,8 @@ min_bbox_rt minimalBoundingBox(const Point2DList &polygon)
         point_t min_corner = box.min_corner();
         point_t max_corner = box.max_corner();
         double min_x = min_corner.get<0>();
-        double max_x = min_corner.get<0>();
-        double min_y = max_corner.get<1>();
+        double max_x = max_corner.get<0>();
+        double min_y = min_corner.get<1>();
         double max_y = max_corner.get<1>();
         //# print "Min x:", min_x, " Max x: ", max_x, "   Min y:", min_y, " Max y: ", max_y
 
@@ -167,12 +164,30 @@ min_bbox_rt minimalBoundingBox(const Point2DList &polygon)
             min_bbox.width  = width;
             min_bbox.height = height;
 
-            point_t origin;
-            trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(-angle*180/M_PI);
-            boost::geometry::transform(min_corner, origin, rotate);
-            min_bbox.origin = Point2D{origin.get<0>(), origin.get<1>()};
+            min_bbox.corners = std::array<Point2D, 4>{Point2D{min_x, min_y},
+                                                      Point2D{max_x, min_y},
+                                                      Point2D{max_x, max_y},
+                                                      Point2D{min_x, max_y}};
         }
     }
 
+
+    // Transform corners of minimal bounding box.
+    poly_t boost_polygon;
+    for (auto vertex : min_bbox.corners){
+        boost_polygon.outer().push_back(point_t{vertex[0], vertex[1]});
+    }
+    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(-min_bbox.angle*180/M_PI);
+    poly_t rotated_polygon;
+    boost::geometry::transform(boost_polygon, rotated_polygon, rotate);
+
+    for (size_t i = 0; i < 4; ++i){
+        point_t boost_vertex{rotated_polygon.outer()[i]};
+        min_bbox.corners[i] = Point2D{boost_vertex.get<0>(), boost_vertex.get<1>()};
+    }
+
+
     return min_bbox;
 }
+
+} // end namespace snake_geometry

libs/snake/snake/snake_geometry.h

@@ -4,7 +4,7 @@
 
 #include "WGS84toCartesian.hpp"
 
-namespace snake_geometrie {
+namespace snake_geometry {
 
 typedef std::array<double, 2>   Point2D;
 typedef std::vector<Point2D>         Point2DList;
@@ -19,11 +19,11 @@ typedef struct {
     double width;
     double height;
     double angle;
-    Point2D origin;
+    std::array<Point2D,4> corners;
 }min_bbox_rt;
 
-    Point3D toENU(const Point3D &WGS84Reference, const Point3D &WGS84Position);
-    Point3D fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition);
+    Point3D toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position);
+    GeoPoint3D fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition);
 
     Point2D polygonCenter(const Point2DList &polygon);
     min_bbox_rt minimalBoundingBox(const Point2DList &polygon);

libs/snake/snake/snake_global.h

-#ifndef SNAKE_GLOBAL_H
-#define SNAKE_GLOBAL_H
-
-#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
-#  define Q_DECL_EXPORT __declspec(dllexport)
-#  define Q_DECL_IMPORT __declspec(dllimport)
-#else
-#  define Q_DECL_EXPORT     __attribute__((visibility("default")))
-#  define Q_DECL_IMPORT     __attribute__((visibility("default")))
-#endif
-
-#if defined(SNAKE_LIBRARY)
-#  define SNAKE_EXPORT Q_DECL_EXPORT
-#else
-#  define SNAKE_EXPORT Q_DECL_IMPORT
-#endif
-
-#endif // SNAKE_GLOBAL_H

libs/snake/snake/test/CMakeLists.txt

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