Skip to content

Q
qgroundcontrol
Commit fd7e0778 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

snake lib edited

parent d3ebb136
Hide whitespace changes
Inline Side-by-side
Showing with 958 additions and 444 deletions
libs/snake/snake/CMakeLists.txt
View file @ fd7e0778
......@@ -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
View file @ fd7e0778
......@@ -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.cpp
View file @ fd7e0778
#include <iostream>
#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<boost::geometry::degree, double, 2, 2> rotate(_mAreaBoundingBox.angle*180/M_PI);
trans::translate_transformer<double, 2, 2> 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<BoostPolygon2D>(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<boost::geometry::degree, double, 2, 2> rotate_back(-_mAreaBoundingBox.angle*180/M_PI);
trans::translate_transformer<double, 2, 2> 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<BoostPolygon> 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<Point2DList> 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<BoostPolygon> 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<BoostPolygon> processedTiles;
const auto &tiles = _scenario.getTilesENU();
for (size_t i=0; i<num_tiles; ++i) {
if (_progress[i] == 100)
processedTiles.push_back(tiles[i]);
}
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);
}
_transects.clear();
const min_bbox_rt &bbox = _scenario.getMeasurementAreaBBoxENU();
double alpha = bbox.angle;
double x0 = bbox.corners.outer()[0].get<0>();
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<double> 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<BoostPoint, BoostPoint>{v1, v2});
}
}
}
libs/snake/snake/snake.h
View file @ fd7e0778
......@@ -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.cpp
View file @ fd7e0778
......@@ -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 &center)
{
if (polygon.outer().empty())
return;
geometry::polygon<double> p;
geometry::linear_ring<double> lr1;
for (size_t i = 0; i < polygon.size(); ++i) {
geometry::point<double> vertex(polygon[i][0], polygon[i][1]);
for (size_t i = 0; i < polygon.outer().size(); ++i) {
geometry::point<double> vertex(polygon.outer()[i].get<0>(), polygon.outer()[i].get<1>());
lr1.push_back(vertex);
}
p.push_back(lr1);
geometry::point<double> center = polylabel(p);
geometry::point<double> 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<double, double> point_t;
typedef boost::geometry::model::polygon<point_t> 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<BoostPolygon2D>(convex_hull) << endl;
//# Compute edges (x2-x1,y2-y1)
std::vector<point_t> edges;
std::vector<BoostPoint> 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<long> 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<double> edge_angles;
for (auto a : angles_long)
edge_angles.push_back(double(a)/angle_scale);
// Calculate edge angles atan2(y/x)
std::set<double> 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<double>::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<boost::geometry::degree, double, 2, 2> rotate(angle*180/M_PI);
poly_t hull_rotated;
boost::geometry::transform(convex_hull, hull_rotated, rotate);
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle*180/M_PI);
BoostPolygon hull_rotated;
bg::transform(convex_hull, hull_rotated, rotate);
//cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated) << endl;
boost::geometry::model::box<point_t> box;
boost::geometry::envelope(hull_rotated, box);
bg::model::box<BoostPoint> box;
bg::envelope(hull_rotated, box);
// cout << "Bounding box: " << bg::wkt<bg::model::box<BoostPoint2D>>(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, 4>{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<bg::degree, double, 2, 2> 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<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);
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<bg::degree, double, 2, 2> 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<double> 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<BoostPolygon> 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
libs/snake/snake/snake_geometry.h
View file @ fd7e0778
#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
View file @ fd7e0778
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
View file @ fd7e0778
#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);
}
libs/snake/snake/test/test_snake_geometry.cpp
View file @ fd7e0778
......@@ -10,23 +10,24 @@
using namespace snake_geometry;
using namespace std;
typedef boost::geometry::model::d2::point_xy<double> point_type;
typedef boost::geometry::model::polygon<point_type> 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<point_type> 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<BoostPoint> 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<polygon_type>(boost_polygon) << endl;
//cout << boost::geometry::wkt<point_type>(boost_center) << endl;
//cout << boost::geometry::wkt<polygon_type>(polygon) << endl;
//cout << boost::geometry::wkt<point_type>(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<point_type> 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<BoostPoint> 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<polygon_type>(boost_polygon) << endl;
//cout << boost::geometry::wkt<point_type>(boost_center) << endl;
//cout << boost::geometry::wkt<polygon_type>(polygon) << endl;
//cout << boost::geometry::wkt<point_type>(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<point_type> 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<BoostPoint> 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<polygon_type>(boost_polygon) << endl;
//cout << boost::geometry::wkt<point_type>(boost_center) << endl;
//cout << boost::geometry::wkt<polygon_type>(polygon) << endl;
//cout << boost::geometry::wkt<point_type>(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<point_type> 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<BoostPoint> 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<polygon_type>(boost_polygon) << endl;
//cout << boost::geometry::wkt<point_type>(boost_center) << endl;
//cout << boost::geometry::wkt<polygon_type>(polygon) << endl;
//cout << boost::geometry::wkt<point_type>(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<point_type> 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<BoostPoint> 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<polygon_type>(boost_polygon) << endl;
//cout << boost::geometry::wkt<polygon_type>(boost_bbox) << endl;
//REQUIRE(boost::geometry::within(boost_polygon, boost_bbox) == true);
//cout << boost::geometry::wkt<polygon_type>(polygon) << endl;
//cout << boost::geometry::wkt<polygon_type>(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<boost::geometry::degree, double, 2, 2> 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<point_type> 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<BoostPoint> 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<polygon_type>(rotated_boost_polygon) << endl;
//cout << boost::geometry::wkt<polygon_type>(boost_bbox) << endl;
//REQUIRE(boost::geometry::within(rotated_boost_polygon, boost_bbox) == true);
//cout << boost::geometry::wkt<polygon_type>(rotated_polygon) << endl;
//cout << boost::geometry::wkt<polygon_type>(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<boost::geometry::degree, double, 2, 2> 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<BoostPoint> 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<polygon_type>(rotated_polygon) << endl;
//cout << boost::geometry::wkt<polygon_type>(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<boost::geometry::degree, double, 2, 2> 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<point_type> 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<BoostPoint> 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<polygon_type>(rotated_boost_polygon) << endl;
//cout << boost::geometry::wkt<polygon_type>(boost_bbox) << endl;
//cout << boost::geometry::wkt<polygon_type>(rotated_polygon) << endl;
//cout << boost::geometry::wkt<polygon_type>(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<BoostPoint> 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<BoostPoint> 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<BoostPoint> 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);
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment