From fd7e07780105a54ecf30a6809c56a0ac414efb40 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Fri, 12 Jun 2020 12:43:58 +0200 Subject: [PATCH] snake lib edited --- libs/snake/snake/CMakeLists.txt | 10 +- libs/snake/snake/catch.hpp | 1 + libs/snake/snake/snake.cpp | 425 ++++++++++------- libs/snake/snake/snake.h | 75 ++- libs/snake/snake/snake_geometry.cpp | 214 ++++++--- libs/snake/snake/snake_geometry.h | 40 +- libs/snake/snake/test/CMakeLists.txt | 2 +- libs/snake/snake/test/test_snake.cpp | 186 ++++++++ libs/snake/snake/test/test_snake_geometry.cpp | 449 ++++++++++-------- 9 files changed, 958 insertions(+), 444 deletions(-) diff --git a/libs/snake/snake/CMakeLists.txt b/libs/snake/snake/CMakeLists.txt index dd585f7fd..1f58e85aa 100644 --- a/libs/snake/snake/CMakeLists.txt +++ b/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) + + diff --git a/libs/snake/snake/catch.hpp b/libs/snake/snake/catch.hpp index f64422ae1..2c1b9cdbc 100644 --- a/libs/snake/snake/catch.hpp +++ b/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 diff --git a/libs/snake/snake/snake.cpp b/libs/snake/snake/snake.cpp index 95202d1aa..a3610bc8e 100644 --- a/libs/snake/snake/snake.cpp +++ b/libs/snake/snake/snake.cpp @@ -1,21 +1,27 @@ +#include + #include "snake.h" #include "clipper.hpp" -#include "assert.h" +using namespace snake_geometry; +using namespace std; + +namespace bg = boost::geometry; +namespace trans = bg::strategy::transform; namespace snake { Scenario::Scenario() : - _mAreaBoundingBox(min_bbox_rt{0, 0, 0, Point2D{0,0}}) + _mAreaBoundingBox(min_bbox_rt{0, 0, 0, BoostPolygon{}}) { } -bool Scenario::setArea(Area &area) +bool Scenario::addArea(Area &area) { if (area.geoPolygon.size() < 3){ - error_str = "Area has less than three vertices."; + errorString = "Area has less than three vertices."; return false; } if (area.type == MeasurementArea) @@ -25,197 +31,290 @@ bool Scenario::setArea(Area &area) else if (area.type == Corridor) return Scenario::_setCorridor(area); return false; - } +} - bool Scenario::_areas2enu() - { - if (_measurementArea.geoPolygon.size() > 0){ - _measurementAreaENU.clear(); - for(auto vertex : _measurementArea.geoPolygon) { - Point3D ENUVertex = toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _measurementArea.altitude}); - _measurementAreaENU.push_back(Point2D{ENUVertex[0], ENUVertex[1]}); - } - _homePositionENU = polygonCenter(_measurementAreaENU); +bool Scenario::defined(double tileWidth, double tileHeight, double minTileArea) +{ + if (!_areas2enu()) + return false; + if (!_calculateBoundingBox()) + return false; + if (!_calculateTiles(tileWidth, tileHeight, minTileArea)) + return false; + if (!_calculateJoinedArea()) + return false; - if (_serviceArea.geoPolygon.size() > 0){ - _serviceAreaENU.clear(); - for(auto vertex : _serviceArea.geoPolygon) { - Point3D ENUVertex = toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _serviceArea.altitude}); - _serviceAreaENU.push_back(Point2D{ENUVertex[0], ENUVertex[1]}); - } - } else{ - error_str = "Service area has no vertices."; - return false; - } + return true; +} - if (_corridor.geoPolygon.size() > 0){ - _corridorENU.clear(); - for(auto vertex : _corridor.geoPolygon) { - Point3D ENUVertex = toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _corridor.altitude}); - _corridorENU.push_back(Point2D{ENUVertex[0], ENUVertex[1]}); - } +bool Scenario::_areas2enu() +{ + if (_measurementArea.geoPolygon.size() > 0){ + _measurementAreaENU.clear(); + for(auto vertex : _measurementArea.geoPolygon) { + Point3D ENUVertex; + toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _measurementArea.altitude}, ENUVertex); + _measurementAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]}); + } + bg::correct(_measurementAreaENU); + + _serviceAreaENU.clear(); + if (_serviceArea.geoPolygon.size() > 0){ + for(auto vertex : _serviceArea.geoPolygon) { + Point3D ENUVertex; + toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _serviceArea.altitude}, ENUVertex); + _serviceAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]}); } + } else{ + errorString = "Service area has no vertices."; + return false; + } + bg::correct(_serviceAreaENU); + polygonCenter(_serviceAreaENU, _homePositionENU); - return true; + _corridorENU.clear(); + if (_corridor.geoPolygon.size() > 0){ + for(auto vertex : _corridor.geoPolygon) { + Point3D ENUVertex; + toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _corridor.altitude}, ENUVertex); + _corridorENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]}); + } } + bg::correct(_corridorENU); - error_str = "Measurement area has no vertices."; - return false; + return true; } - bool Scenario::_setMeasurementArea(Area &area) - { - if (area.geoPolygon.size() <= 0) - return false; - _geoOrigin = area.geoPolygon[0]; - _measurementArea = area; - _measurementAreaENU.clear(); - _serviceAreaENU.clear(); - _corridorENU.clear(); - return true; + errorString = "Measurement area has no vertices."; + return false; +} - } +bool Scenario::_setMeasurementArea(Area &area) +{ + if (area.geoPolygon.size() <= 0) + return false; + GeoPoint2D origin2D = area.geoPolygon[0]; + _geoOrigin = GeoPoint3D{origin2D[0], origin2D[1], 0}; + _measurementArea = area; + _measurementAreaENU.clear(); + _serviceAreaENU.clear(); + _corridorENU.clear(); + return true; - bool Scenario::_setServiceArea(Area &area) - { - if (area.geoPolygon.size() <= 0) - return false; - _serviceArea = area; - _serviceAreaENU.clear(); - return true; +} + +bool Scenario::_setServiceArea(Area &area) +{ + if (area.geoPolygon.size() <= 0) + return false; + _serviceArea = area; + _serviceAreaENU.clear(); + return true; +} + +bool Scenario::_setCorridor(Area &area) +{ + if (area.geoPolygon.size() <= 0) + return false; + _corridor = area; + _corridorENU.clear(); + return true; +} + +bool Scenario::_calculateBoundingBox() +{ + minimalBoundingBox(_measurementAreaENU, _mAreaBoundingBox); + 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 (>0) of a tile. + * @param tileHeight The heigth (>0) of a tile. + * @param minTileArea The minimal area (>0) of a tile. + * + * @return Returns true if successful. + */ +bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTileArea) +{ + _tilesENU.clear(); + _tileCenterPointsENU.clear(); + + if (tileWidth <= 0 || tileHeight <= 0 || minTileArea <= 0) { + errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive."; + return false; } - bool Scenario::_setCorridor(Area &area) - { - if (area.geoPolygon.size() <= 0) - return false; - _corridor = area; - _corridorENU.clear(); - return true; + double bbox_width = _mAreaBoundingBox.width; + double bbox_height = _mAreaBoundingBox.height; + + BoostPoint origin = _mAreaBoundingBox.corners.outer()[0]; + + //cout << "Origin: " << origin[0] << " " << origin[1] << endl; + // Transform _measurementAreaENU polygon to bounding box coordinate system. + trans::rotate_transformer rotate(_mAreaBoundingBox.angle*180/M_PI); + trans::translate_transformer translate(-origin.get<0>(), -origin.get<1>()); + BoostPolygon translated_polygon; + BoostPolygon rotated_polygon; + boost::geometry::transform(_measurementAreaENU, translated_polygon, translate); + boost::geometry::transform(translated_polygon, rotated_polygon, rotate); + bg::correct(rotated_polygon); + + //cout << bg::wkt(rotated_polygon) << endl; + + size_t i_max = ceil(bbox_width/tileWidth); + size_t j_max = ceil(bbox_height/tileHeight); + + if (i_max < 1 || j_max < 1) { + errorString = "tileWidth or tileHeight to small."; + return false; } - bool Scenario::_calculateBoundingBox() - { - _mAreaBoundingBox = minimalBoundingBox(_measurementAreaENU); - return true; + + trans::rotate_transformer rotate_back(-_mAreaBoundingBox.angle*180/M_PI); + trans::translate_transformer translate_back(origin.get<0>(), origin.get<1>()); + for (size_t i = 0; i < i_max; ++i){ + double x_min = tileWidth*i; + double x_max = x_min + tileWidth; + for (size_t j = 0; j < j_max; ++j){ + double y_min = tileHeight*j; + double y_max = y_min + tileHeight; + + BoostPolygon tile_unclipped; + tile_unclipped.outer().push_back(BoostPoint{x_min, y_min}); + tile_unclipped.outer().push_back(BoostPoint{x_min, y_max}); + tile_unclipped.outer().push_back(BoostPoint{x_max, y_max}); + tile_unclipped.outer().push_back(BoostPoint{x_max, y_min}); + tile_unclipped.outer().push_back(BoostPoint{x_min, y_min}); + + + std::deque boost_tiles; + if (!boost::geometry::intersection(tile_unclipped, rotated_polygon, boost_tiles)) + continue; + + for (BoostPolygon t : boost_tiles) + { + if (bg::area(t) > minTileArea){ + // Transform boost_tile to world coordinate system. + BoostPolygon rotated_tile; + BoostPolygon translated_tile; + boost::geometry::transform(t, rotated_tile, rotate_back); + boost::geometry::transform(rotated_tile, translated_tile, translate_back); + + // Store tile and calculate center point. + _tilesENU.push_back(translated_tile); + BoostPoint tile_center; + polygonCenter(translated_tile, tile_center); + _tileCenterPointsENU.push_back(tile_center); + } + } + } } - /** - * 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 tiles_unclipped; - return true; + if (_tilesENU.size() < 1){ + errorString = "No tiles calculated. Is the minTileArea parameter large enough?"; + return false; } - 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); - } + return true; +} - for (auto vertex : _serviceAreaENU){ - ClipperLib::IntPoint intVertex{ClipperLib::cInt(vertex[0]*clipper_scale), - ClipperLib::cInt(vertex[1]*clipper_scale)}; - serviceArea.push_back(intVertex); +bool Scenario::_calculateJoinedArea() +{ + _joinedAreaENU.clear(); + // Measurement area and service area overlapping? + bool overlapingSerMeas = bg::intersects(_measurementAreaENU, _serviceAreaENU) ? true : false; + bool corridorValid = _corridorENU.outer().size() > 0 ? true : false; + + + // Check if corridor is connecting measurement area and service area. + bool corridor_is_connection = false; + if (corridorValid) { + // Corridor overlaping with measurement area? + if ( bg::intersects(_corridorENU, _measurementAreaENU) ) { + // Corridor overlaping with service area? + if ( bg::intersects(_corridorENU, _serviceAreaENU) ) + corridor_is_connection = true; } + } - 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; + // Are areas joinable? + std::deque sol; + BoostPolygon partialArea = _measurementAreaENU; + if (overlapingSerMeas){ + if(corridor_is_connection){ + bg::union_(partialArea, _corridorENU, sol); } + } else if (corridor_is_connection){ + bg::union_(partialArea, _corridorENU, sol); + } else { + errorString = "Areas are not overlapping"; + return false; + } - // 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); + if (sol.size() > 0) { + partialArea = sol[0]; + sol.clear(); + } - // Measurement area and service area overlapping? - bool overlaping_s_m = false; - if (solution.size() > 0){ - overlaping_s_m = true; - } + // Join areas. + bg::union_(partialArea, _serviceAreaENU, sol); + if (sol.size() > 0) { + _joinedAreaENU = sol[0]; + } else { + return false; + } - // 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; - } - } - } + return 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; - } +bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength) +{ + if (_scenario.getTilesENU().size() != _progress.size()){ + errorString = "Number of tiles is not equal progress array length"; + return false; + } - // Join areas. - solution.clear(); - cp1.Execute(ClipperLib::ctUnion, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd); + size_t num_tiles = _progress.size(); + vector processedTiles; + const auto &tiles = _scenario.getTilesENU(); + for (size_t i=0; i(); + double y0 = bbox.corners.outer()[0].get<1>(); + double bboxWidth = bbox.width; + double bboxHeight = bbox.height; + double delta = detail::polygonOffset; + + size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects + vector yCoords; + yCoords.reserve(num_t); + double y = -delta; + for (size_t i=0; i < num_t; ++i) { + yCoords.push_back(y); + y += lineDistance; + } - return true; + for (size_t i=0; i < num_t; ++i) { + BoostPoint v1{-delta, yCoords[i]}; + BoostPoint v2{bboxWidth+delta, yCoords[i]}; + _transects.push_back(tuple{v1, v2}); } + +} + } diff --git a/libs/snake/snake/snake.h b/libs/snake/snake/snake.h index 54aa2ec1b..b85e66640 100644 --- a/libs/snake/snake/snake.h +++ b/libs/snake/snake/snake.h @@ -16,25 +16,40 @@ namespace snake { enum AreaType {MeasurementArea, ServiceArea, Corridor}; struct Area { - vector 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 &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 _tilesENU; - vector _tilesCenterPointsENU; + vector _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 &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> _transects; + vector _progress; + BoostPolygon _joinedAreaOffset; }; + + namespace detail { + double polygonOffset = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies. + } } diff --git a/libs/snake/snake/snake_geometry.cpp b/libs/snake/snake/snake_geometry.cpp index 8ea5e7d9e..43f8ba1d4 100644 --- a/libs/snake/snake/snake_geometry.cpp +++ b/libs/snake/snake/snake_geometry.cpp @@ -14,46 +14,48 @@ using namespace mapbox; using namespace snake_geometry; using namespace std; +namespace bg = bg; +namespace trans = bg::strategy::transform; + BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian) namespace snake_geometry { -Point3D toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position) +void toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position, Point3D &ENUPosition) { GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); GeographicLib::LocalCartesian proj(WGS84Reference[0], WGS84Reference[1], WGS84Reference[2], earth); - Point3D ENUPosition; proj.Forward(WGS84Position[0], WGS84Position[1], WGS84Position[2], ENUPosition[0], ENUPosition[1], ENUPosition[2]); - return ENUPosition; } -GeoPoint3D fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition) +void fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition, GeoPoint3D &GeoPosition) { 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) +void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er) { + if (polygon.outer().empty()) + return; geometry::polygon p; geometry::linear_ring lr1; - for (size_t i = 0; i < polygon.size(); ++i) { - geometry::point vertex(polygon[i][0], polygon[i][1]); + for (size_t i = 0; i < polygon.outer().size(); ++i) { + geometry::point vertex(polygon.outer()[i].get<0>(), polygon.outer()[i].get<1>()); lr1.push_back(vertex); } p.push_back(lr1); - geometry::point center = polylabel(p); + geometry::point c = polylabel(p); - return Point2D{center.x, center.y}; + center.set<0>(c.x); + center.set<1>(c.y); } -min_bbox_rt minimalBoundingBox(const Point2DList &polygon) +void minimalBoundingBox(const BoostPolygon &polygon, min_bbox_rt &minBBox) { /* Find the minimum-area bounding box of a set of 2D points @@ -96,98 +98,184 @@ min_bbox_rt minimalBoundingBox(const Point2DList &polygon) POSSIBILITY OF SUCH DAMAGE. */ - typedef boost::tuple point_t; - typedef boost::geometry::model::polygon poly_t; - namespace trans = boost::geometry::strategy::transform; - - poly_t poly; - for (auto vertex : polygon) - poly.outer().push_back(point_t{vertex[0], vertex[1]}); - - poly_t convex_hull; - boost::geometry::convex_hull(poly, convex_hull); + if (polygon.outer().empty()) + return; + BoostPolygon convex_hull; + bg::convex_hull(polygon, convex_hull); - //hull_points_2d = array(convex_polygon[0]) - //# print "Input convex hull points: " - //# print hull_points_2d + //cout << "Convex hull: " << bg::wkt(convex_hull) << endl; //# Compute edges (x2-x1,y2-y1) - std::vector edges; + std::vector edges; auto convex_hull_outer = convex_hull.outer(); 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); + BoostPoint p1 = convex_hull_outer.at(i); + BoostPoint p2 = convex_hull_outer.at(i+1); double edge_x = p2.get<0>() - p1.get<0>(); double edge_y = p2.get<1>() - p1.get<1>(); - edges.push_back(point_t{edge_x, edge_y}); + edges.push_back(BoostPoint{edge_x, edge_y}); + } + +// cout << "Edges: "; +// for (auto e : edges) +// cout << e.get<0>() << " " << e.get<1>() << ","; +// cout << endl; + + // Calculate unique edge angles atan2(y/x) + double angle_scale = 1e3; + std::set angles_long; + for (auto vertex : edges) { + double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2); + angle = angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers + angles_long.insert(long(round(angle*angle_scale))); } - // print "Edges: \n", edges + std::vector edge_angles; + for (auto a : angles_long) + edge_angles.push_back(double(a)/angle_scale); - // Calculate edge angles atan2(y/x) - std::set edge_angles; - for (auto vertex : edges) - edge_angles.insert(std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2)); // want strictly positive answers - //# print "Unique edge angles: \n", edge_angles +// cout << "Unique angles: "; +// for (auto e : edge_angles) +// cout << e*180/M_PI << ","; +// cout << endl; - min_bbox_rt min_bbox{0, 0, 0, {Point2D{0,0}}}; double min_area = std::numeric_limits::infinity(); // Test each angle to find bounding box with smallest area // print "Testing", len(edge_angles), "possible rotations for bounding box... \n" for (double angle : edge_angles){ - trans::rotate_transformer rotate(angle*180/M_PI); - poly_t hull_rotated; - boost::geometry::transform(convex_hull, hull_rotated, rotate); + trans::rotate_transformer rotate(angle*180/M_PI); + BoostPolygon hull_rotated; + bg::transform(convex_hull, hull_rotated, rotate); + //cout << "Convex hull rotated: " << bg::wkt(hull_rotated) << endl; - boost::geometry::model::box box; - boost::geometry::envelope(hull_rotated, box); + bg::model::box box; + bg::envelope(hull_rotated, box); +// cout << "Bounding box: " << bg::wkt>(box) << endl; //# print "Rotated hull points are \n", rot_points - point_t min_corner = box.min_corner(); - point_t max_corner = box.max_corner(); + BoostPoint min_corner = box.min_corner(); + BoostPoint max_corner = box.max_corner(); double min_x = min_corner.get<0>(); 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 +// cout << "min_x: " << min_x << endl; +// cout << "max_x: " << max_x << endl; +// cout << "min_y: " << min_y << endl; +// cout << "max_y: " << max_y << endl; // Calculate height/width/area of this bounding rectangle double width = max_x - min_x; double height = max_y - min_y; double area = width * height; - // print "Potential bounding box ", i, ": width: ", width, " height: ", height, " area: ", area +// cout << "Width: " << width << endl; +// cout << "Height: " << height << endl; +// cout << "area: " << area << endl; +// cout << "angle: " << angle*180/M_PI << endl; // Store the smallest rect found first (a simple convex hull might have 2 answers with same area) if (area < min_area){ - min_bbox.angle = angle; - min_bbox.width = width; - min_bbox.height = height; - - min_bbox.corners = std::array{Point2D{min_x, min_y}, - Point2D{max_x, min_y}, - Point2D{max_x, max_y}, - Point2D{min_x, max_y}}; + min_area = area; + minBBox.angle = angle; + minBBox.width = width; + minBBox.height = height; + + minBBox.corners.clear(); + minBBox.corners.outer().push_back(BoostPoint{min_x, min_y}); + minBBox.corners.outer().push_back(BoostPoint{min_x, max_y}); + minBBox.corners.outer().push_back(BoostPoint{max_x, max_y}); + minBBox.corners.outer().push_back(BoostPoint{max_x, min_y}); + minBBox.corners.outer().push_back(BoostPoint{min_x, min_y}); } + //cout << endl << endl; } // 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 rotate(-minBBox.angle*180/M_PI); + BoostPolygon rotated_polygon; + bg::transform(minBBox.corners, rotated_polygon, rotate); + minBBox.corners = rotated_polygon; +} + +void toBoost(const Point2D &point, BoostPoint &boost_point) +{ + boost_point.set<0>(point[0]); + boost_point.set<1>(point[1]); +} + +void fromBoost(const BoostPoint &boost_point, Point2D &point) +{ + point[0] = boost_point.get<0>(); + point[1] = boost_point.get<1>(); +} + +void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon) +{ + for (auto vertex : point_list) { + BoostPoint boost_vertex; + toBoost(vertex, boost_vertex); + boost_polygon.outer().push_back(boost_vertex); } - trans::rotate_transformer rotate(-min_bbox.angle*180/M_PI); - poly_t rotated_polygon; - boost::geometry::transform(boost_polygon, rotated_polygon, rotate); + bg::correct(boost_polygon); +} - 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>()}; +void fromBoost(const BoostPolygon &boost_polygon, Point2DList &point_list) +{ + for (auto boost_vertex : boost_polygon.outer()) { + Point2D vertex; + fromBoost(boost_vertex, vertex); + point_list.push_back(vertex); } +} + +void rotateDeg(const Point2DList &point_list, Point2DList &rotated_point_list, double degree) +{ + trans::rotate_transformer rotate(degree); + BoostPolygon boost_polygon; + toBoost(point_list, boost_polygon); + BoostPolygon rotated_polygon; + bg::transform(boost_polygon, rotated_polygon, rotate); + fromBoost(rotated_polygon, rotated_point_list); +} + +void rotateRad(const Point2DList &point_list, Point2DList &rotated_point_list, double rad) +{ + rotateDeg(point_list, rotated_point_list, rad*180/M_PI); +} + +bool isClockwise(const Point2DList &point_list) +{ + double orientaion = 0; + double len = point_list.size(); + for (size_t i=0; i < len-1; ++i){ + Point2D v1 = point_list[i]; + Point2D v2 = point_list[i+1]; + orientaion += (v2[0]-v1[0])*(v2[1]+v1[1]); + } + Point2D v1 = point_list[len-1]; + Point2D v2 = point_list[0]; + orientaion += (v2[0]-v1[0])*(v2[1]+v1[1]); + + return orientaion > 0 ? true : false; +} + +void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset) +{ + bg::strategy::buffer::distance_symmetric distance_strategy(offset); + bg::strategy::buffer::join_miter join_strategy(3); + bg::strategy::buffer::end_flat end_strategy; + bg::strategy::buffer::point_square point_strategy; + bg::strategy::buffer::side_straight side_strategy; + + + bg::model::multi_polygon result; + + bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy); + polygonOffset = result[0]; - return min_bbox; } } // end namespace snake_geometry diff --git a/libs/snake/snake/snake_geometry.h b/libs/snake/snake/snake_geometry.h index dd93f3533..34d61d32b 100644 --- a/libs/snake/snake/snake_geometry.h +++ b/libs/snake/snake/snake_geometry.h @@ -1,31 +1,49 @@ #pragma once #include #include +#include #include "WGS84toCartesian.hpp" +namespace bg = boost::geometry; + namespace snake_geometry { -typedef std::array Point2D; -typedef std::vector Point2DList; +typedef std::array Point2D; typedef std::array Point3D; -typedef std::vector Point3DList; -typedef std::array GeoPoint2D; -typedef std::vector GeoPoint2DList; typedef std::array GeoPoint3D; -typedef std::vector GeoPoint3DList; +typedef std::array GeoPoint2D; +typedef std::vector Point2DList; +typedef std::vector Point3DList; +typedef std::vector GeoPoint2DList; +typedef std::vector GeoPoint3DList; + +typedef bg::model::point BoostPoint; +typedef std::vector BoostPointList; +typedef bg::model::polygon BoostPolygon; typedef struct { double width; double height; double angle; - std::array 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 ¢er); +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); } diff --git a/libs/snake/snake/test/CMakeLists.txt b/libs/snake/snake/test/CMakeLists.txt index a0af8dab4..163ea4512 100644 --- a/libs/snake/snake/test/CMakeLists.txt +++ b/libs/snake/snake/test/CMakeLists.txt @@ -1,2 +1,2 @@ -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) diff --git a/libs/snake/snake/test/test_snake.cpp b/libs/snake/snake/test/test_snake.cpp index e69de29bb..4f479aa82 100644 --- a/libs/snake/snake/test/test_snake.cpp +++ b/libs/snake/snake/test/test_snake.cpp @@ -0,0 +1,186 @@ +#include +#include +#include + +#include "catch.hpp" +#include "snake.h" + + +#include +#include +#include + +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::high_resolution_clock::now()-start).count(); + cout << " ms" << endl; + + // Store to svg. + std::ofstream svg("Scenario0.svg"); + boost::geometry::svg_mapper 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); + +} diff --git a/libs/snake/snake/test/test_snake_geometry.cpp b/libs/snake/snake/test/test_snake_geometry.cpp index d1ae18cc8..9fc3f6a7c 100644 --- a/libs/snake/snake/test/test_snake_geometry.cpp +++ b/libs/snake/snake/test/test_snake_geometry.cpp @@ -10,23 +10,24 @@ using namespace snake_geometry; using namespace std; -typedef boost::geometry::model::d2::point_xy point_type; -typedef boost::geometry::model::polygon polygon_type; - TEST_CASE( "Test toENU() and fromENU()", "[WGS84]" ) { GeoPoint3D ref{48.230612, 16.297824, 0}; GeoPoint3D poi{48.231159, 16.298406, 2}; - Point3D zero = toENU(ref, ref); + Point3D zero; + toENU(ref, ref, zero); REQUIRE( zero[0] == Approx(0)); REQUIRE( zero[1] == Approx(0)); REQUIRE( zero[2] == Approx(0)); - Point3D poiENU = toENU(ref, poi); - GeoPoint3D poi_same = fromENU(ref, poiENU); + Point3D poiENU; + toENU(ref, poi, poiENU); + GeoPoint3D poi_same; + fromENU(ref, poiENU, poi_same); - Point3D diff = toENU(poi_same, poi); + Point3D diff; + toENU(poi_same, poi, diff); REQUIRE( abs(diff[0]) <= 1e6); REQUIRE( abs(diff[1]) <= 1e6); @@ -34,268 +35,336 @@ TEST_CASE( "Test toENU() and fromENU()", "[WGS84]" ) { } TEST_CASE( "Test polygonCenter(), check out polygonCenter0.svg!" ) { - Point2DList polygon{Point2D{0, 0}, - Point2D{100, 0}, - Point2D{100, 100}, - Point2D{0, 100}}; - Point2D center = polygonCenter(polygon); + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{100, 0}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{0, 100}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); + BoostPoint center; + polygonCenter(polygon, center); - // Convert to boost. - polygon_type boost_polygon; - for (auto vertex : polygon) { - point_type boost_vertex = point_type{vertex[0], vertex[1]}; - boost_polygon.outer().push_back(boost_vertex); - } - point_type boost_center(center[0], center[1]); // Write results to svg file. std::ofstream svg("polygonCenter0.svg"); - boost::geometry::svg_mapper mapper(svg, 200, 200); - mapper.add(boost_center); - mapper.add(boost_polygon); - mapper.map(boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); - mapper.map(boost_center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(center); + mapper.add(polygon); + mapper.map(polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); + mapper.map(center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5); // Print to standard output. - //cout << boost::geometry::wkt(boost_polygon) << endl; - //cout << boost::geometry::wkt(boost_center) << endl; + //cout << boost::geometry::wkt(polygon) << endl; + //cout << boost::geometry::wkt(center) << endl; // Center inside polygon? - REQUIRE(boost::geometry::within(boost_center, boost_polygon) == true); + REQUIRE(boost::geometry::within(center, polygon) == true); } TEST_CASE( "Test polygonCenter(), check out polygonCenter1.svg!" ) { - Point2DList polygon{Point2D{0, 0}, - Point2D{50, 0}, - Point2D{50, 50}, - Point2D{100, 50}, - Point2D{100, 100}, - Point2D{0, 100}}; - Point2D center = polygonCenter(polygon); - - - // Convert to boost. - polygon_type boost_polygon; - for (auto vertex : polygon) { - point_type boost_vertex = point_type{vertex[0], vertex[1]}; - boost_polygon.outer().push_back(boost_vertex); - } - point_type boost_center(center[0], center[1]); + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{50, 0}); + polygon.outer().push_back(BoostPoint{50, 50}); + polygon.outer().push_back(BoostPoint{100, 50}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{0, 100}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); + + BoostPoint center; + polygonCenter(polygon, center); // Write results to svg file. std::ofstream svg("polygonCenter1.svg"); - boost::geometry::svg_mapper mapper(svg, 200, 200); - mapper.add(boost_center); - mapper.add(boost_polygon); - mapper.map(boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); - mapper.map(boost_center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(center); + mapper.add(polygon); + mapper.map(polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); + mapper.map(center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5); // Print to standard output. - //cout << boost::geometry::wkt(boost_polygon) << endl; - //cout << boost::geometry::wkt(boost_center) << endl; + //cout << boost::geometry::wkt(polygon) << endl; + //cout << boost::geometry::wkt(center) << endl; // Center inside polygon? - REQUIRE(boost::geometry::within(boost_center, boost_polygon) == true); + REQUIRE(boost::geometry::within(center, polygon) == true); } TEST_CASE( "Test polygonCenter(), check out polygonCenter2.svg!" ) { - Point2DList polygon{Point2D{0, 0}, - Point2D{20, 0}, - Point2D{20, 50}, - Point2D{100, 50}, - Point2D{100, 100}, - Point2D{0, 100}}; - Point2D center = polygonCenter(polygon); + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{20, 0}); + polygon.outer().push_back(BoostPoint{20, 50}); + polygon.outer().push_back(BoostPoint{100, 50}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{0, 100}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); + + BoostPoint center; + polygonCenter(polygon, center); // Convert to boost. - polygon_type boost_polygon; - for (auto vertex : polygon) { - point_type boost_vertex = point_type{vertex[0], vertex[1]}; - boost_polygon.outer().push_back(boost_vertex); - } - point_type boost_center(center[0], center[1]); // Write results to svg file. std::ofstream svg("polygonCenter2.svg"); - boost::geometry::svg_mapper mapper(svg, 200, 200); - mapper.add(boost_center); - mapper.add(boost_polygon); - mapper.map(boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); - mapper.map(boost_center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(center); + mapper.add(polygon); + mapper.map(polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); + mapper.map(center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5); // Print to standard output. - //cout << boost::geometry::wkt(boost_polygon) << endl; - //cout << boost::geometry::wkt(boost_center) << endl; + //cout << boost::geometry::wkt(polygon) << endl; + //cout << boost::geometry::wkt(center) << endl; // Center inside polygon? - REQUIRE(boost::geometry::within(boost_center, boost_polygon) == true); + REQUIRE(boost::geometry::within(center, polygon) == true); } TEST_CASE( "Test polygonCenter(), check out polygonCenter3.svg!" ) { - Point2DList polygon{Point2D{0, 0}, - Point2D{70, 0}, - Point2D{100, 100}, - Point2D{0, 100}}; - Point2D center = polygonCenter(polygon); - + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{70, 0}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{0, 100}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); - // Convert to boost. - polygon_type boost_polygon; - for (auto vertex : polygon) { - point_type boost_vertex = point_type{vertex[0], vertex[1]}; - boost_polygon.outer().push_back(boost_vertex); - } - point_type boost_center(center[0], center[1]); + BoostPoint center; + polygonCenter(polygon, center); // Write results to svg file. std::ofstream svg("polygonCenter3.svg"); - boost::geometry::svg_mapper mapper(svg, 200, 200); - mapper.add(boost_center); - mapper.add(boost_polygon); - mapper.map(boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); - mapper.map(boost_center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(center); + mapper.add(polygon); + mapper.map(polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); + mapper.map(center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5); // Print to standard output. - //cout << boost::geometry::wkt(boost_polygon) << endl; - //cout << boost::geometry::wkt(boost_center) << endl; + //cout << boost::geometry::wkt(polygon) << endl; + //cout << boost::geometry::wkt(center) << endl; // Center inside polygon? - REQUIRE(boost::geometry::within(boost_center, boost_polygon) == true); + REQUIRE(boost::geometry::within(center, polygon) == true); } TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox0.svg!" ) { - Point2DList polygon{Point2D{0, 0}, - Point2D{70, 0}, - Point2D{100, 100}, - Point2D{0, 100}}; - - min_bbox_rt bbox = minimalBoundingBox(polygon); + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{70, 0}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{0, 100}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); - - // Convert to boost. - polygon_type boost_polygon; - for (auto vertex : polygon) { - point_type boost_vertex = point_type{vertex[0], vertex[1]}; - boost_polygon.outer().push_back(boost_vertex); - } - - polygon_type boost_bbox; - for (auto vertex : bbox.corners) { - point_type boost_vertex = point_type{vertex[0], vertex[1]}; - boost_bbox.outer().push_back(boost_vertex); - } + min_bbox_rt bbox; + minimalBoundingBox(polygon, bbox); // Write results to svg file. std::ofstream svg("minimalBoundingBox0.svg"); - boost::geometry::svg_mapper mapper(svg, 200, 200); - mapper.add(boost_bbox); - mapper.add(boost_polygon); - mapper.map(boost_bbox, "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2"); - mapper.map(boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(bbox.corners); + mapper.add(polygon); + mapper.map(bbox.corners, "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2"); + mapper.map(polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); // Print to standard output. - //cout << boost::geometry::wkt(boost_polygon) << endl; - //cout << boost::geometry::wkt(boost_bbox) << endl; - - //REQUIRE(boost::geometry::within(boost_polygon, boost_bbox) == true); + //cout << boost::geometry::wkt(polygon) << endl; + //cout << boost::geometry::wkt(bbox.corners) << endl; } TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox1.svg! Rotated polygon." ) { - Point2DList polygon{Point2D{0, 0}, - Point2D{70, 0}, - Point2D{100, 100}, - Point2D{0, 100}}; - - // Convert to boost. - polygon_type boost_polygon; - for (auto vertex : polygon) { - point_type boost_vertex = point_type{vertex[0], vertex[1]}; - boost_polygon.outer().push_back(boost_vertex); - } + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{70, 0}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{0, 100}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); // Rotate polygon boost::geometry::strategy::transform::rotate_transformer rotate(45); - polygon_type rotated_boost_polygon; - boost::geometry::transform(boost_polygon, rotated_boost_polygon, rotate); - Point2DList rotated_polygon; - auto outer = rotated_boost_polygon.outer(); - for (auto vertex : outer){ - rotated_polygon.push_back(Point2D{vertex.get<0>(), vertex.get<1>()}); - } + BoostPolygon rotated_polygon; + boost::geometry::transform(polygon, rotated_polygon, rotate); - - min_bbox_rt bbox = minimalBoundingBox(rotated_polygon); - - polygon_type boost_bbox; - for (auto vertex : bbox.corners) { - point_type boost_vertex = point_type{vertex[0], vertex[1]}; - boost_bbox.outer().push_back(boost_vertex); - } + min_bbox_rt bbox; + minimalBoundingBox(rotated_polygon, bbox); // Write results to svg file. std::ofstream svg("minimalBoundingBox1.svg"); - boost::geometry::svg_mapper mapper(svg, 200, 200); - mapper.add(boost_bbox); - mapper.add(rotated_boost_polygon); - mapper.map(boost_bbox, "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2"); - mapper.map(rotated_boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(bbox.corners); + mapper.add(rotated_polygon); + mapper.map(bbox.corners, "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2"); + mapper.map(rotated_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); // Print to standard output. - //cout << boost::geometry::wkt(rotated_boost_polygon) << endl; - //cout << boost::geometry::wkt(boost_bbox) << endl; - - //REQUIRE(boost::geometry::within(rotated_boost_polygon, boost_bbox) == true); + //cout << boost::geometry::wkt(rotated_polygon) << endl; + //cout << boost::geometry::wkt(bbox.corners) << endl; } TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox2.svg! Non convex." ) { - Point2DList polygon{Point2D{0, 0}, - Point2D{70, 0}, - Point2D{30, 50}, - Point2D{100, 100}, - Point2D{0, 100}}; - - // Convert to boost. - polygon_type boost_polygon; - for (auto vertex : polygon) { - point_type boost_vertex = point_type{vertex[0], vertex[1]}; - boost_polygon.outer().push_back(boost_vertex); - } + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{70, 0}); + polygon.outer().push_back(BoostPoint{30, 50}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{0, 100}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); // Rotate polygon boost::geometry::strategy::transform::rotate_transformer rotate(45); - polygon_type rotated_boost_polygon; - boost::geometry::transform(boost_polygon, rotated_boost_polygon, rotate); - Point2DList rotated_polygon; - auto outer = rotated_boost_polygon.outer(); - for (auto vertex : outer){ - rotated_polygon.push_back(Point2D{vertex.get<0>(), vertex.get<1>()}); - } + BoostPolygon rotated_polygon; + boost::geometry::transform(polygon, rotated_polygon, rotate); + min_bbox_rt bbox; + minimalBoundingBox(rotated_polygon, bbox); - min_bbox_rt bbox = minimalBoundingBox(rotated_polygon); + // Write results to svg file. + std::ofstream svg("minimalBoundingBox2.svg"); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(bbox.corners); + mapper.add(rotated_polygon); + mapper.map(bbox.corners, "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2"); + mapper.map(rotated_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); - polygon_type boost_bbox; - for (auto vertex : bbox.corners) { - point_type boost_vertex = point_type{vertex[0], vertex[1]}; - boost_bbox.outer().push_back(boost_vertex); - } + // Print to standard output. + //cout << boost::geometry::wkt(rotated_polygon) << endl; + //cout << boost::geometry::wkt(bbox.corners) << endl; +} + +TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox3.svg! Rotated polygon." ) { + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{70, 0}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{10, 70}); + polygon.outer().push_back(BoostPoint{30, 50}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); + + // Rotate polygon + boost::geometry::strategy::transform::rotate_transformer rotate(45); + BoostPolygon rotated_polygon; + boost::geometry::transform(polygon, rotated_polygon, rotate); + + min_bbox_rt bbox; + minimalBoundingBox(rotated_polygon, bbox); // Write results to svg file. - std::ofstream svg("minimalBoundingBox2.svg"); - boost::geometry::svg_mapper mapper(svg, 200, 200); - mapper.add(boost_bbox); - mapper.add(rotated_boost_polygon); - mapper.map(boost_bbox, "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2"); - mapper.map(rotated_boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); + std::ofstream svg("minimalBoundingBox3.svg"); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(bbox.corners); + mapper.add(rotated_polygon); + mapper.map(bbox.corners, "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2"); + mapper.map(rotated_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); // Print to standard output. - //cout << boost::geometry::wkt(rotated_boost_polygon) << endl; - //cout << boost::geometry::wkt(boost_bbox) << endl; + //cout << boost::geometry::wkt(rotated_polygon) << endl; + //cout << boost::geometry::wkt(bbox.corners) << endl; +} + +TEST_CASE( "Various tests with empty polygons" ) { + BoostPolygon empty_polygon; + min_bbox_rt bbox; + minimalBoundingBox(empty_polygon, bbox); + BoostPoint center; + polygonCenter(empty_polygon, center); +} + +TEST_CASE( "Test isClockwise()" ) { + Point2DList list1{Point2D{0,0}, + Point2D{0,1}, + Point2D{1,1}}; + REQUIRE(isClockwise(list1) == true); + Point2DList list2{Point2D{0,0}, + Point2D{1,1}, + Point2D{0,1}}; + REQUIRE(isClockwise(list2) == false); +} + +TEST_CASE( "Test offsetPolygon(), positive." ) { + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{70, 0}); + polygon.outer().push_back(BoostPoint{30, 50}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{0, 100}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); + + BoostPolygon polygonOffset; + offsetPolygon(polygon, polygonOffset, 10); + + + // Write results to svg file. + std::ofstream svg("offsetPolygon0.svg"); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(polygon); + mapper.add(polygonOffset); + mapper.map(polygon, "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2"); + mapper.map(polygonOffset, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); + + REQUIRE(bg::within(polygon, polygonOffset) == true); - //REQUIRE(boost::geometry::within(rotated_boost_polygon, boost_bbox) == true); } +TEST_CASE( "Test offsetPolygon(), negative." ) { + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{70, 0}); + polygon.outer().push_back(BoostPoint{30, 50}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{0, 100}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); + + BoostPolygon polygonOffset; + offsetPolygon(polygon, polygonOffset, -10); + + + // Write results to svg file. + std::ofstream svg("offsetPolygon1.svg"); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(polygon); + mapper.add(polygonOffset); + mapper.map(polygon, "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2"); + mapper.map(polygonOffset, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); + + REQUIRE(bg::within(polygonOffset, polygon) == true); + +} + +TEST_CASE( "Test offsetPolygon(), positive, huge (miter test)." ) { + BoostPolygon polygon; + polygon.outer().push_back(BoostPoint{0, 0}); + polygon.outer().push_back(BoostPoint{70, 0}); + polygon.outer().push_back(BoostPoint{30, 50}); + polygon.outer().push_back(BoostPoint{100, 100}); + polygon.outer().push_back(BoostPoint{0, 40}); + polygon.outer().push_back(BoostPoint{0, 0}); + bg::correct(polygon); + + BoostPolygon polygonOffset; + offsetPolygon(polygon, polygonOffset, 100); + + + // Write results to svg file. + std::ofstream svg("offsetPolygon2.svg"); + boost::geometry::svg_mapper mapper(svg, 200, 200); + mapper.add(polygon); + mapper.add(polygonOffset); + mapper.map(polygon, "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2"); + mapper.map(polygonOffset, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2"); + + REQUIRE(bg::within(polygon, polygonOffset) == true); +} -- 2.22.0