diff --git a/libs/snake/snake/snake.cpp b/libs/snake/snake/snake.cpp index 4152b810d6eda3a05cab4aa606bdcc1eeab170eb..95202d1aa10fb2185bac37c7acaaee21f6862674 100644 --- a/libs/snake/snake/snake.cpp +++ b/libs/snake/snake/snake.cpp @@ -1,8 +1,18 @@ #include "snake.h" +#include "clipper.hpp" +#include "assert.h" + + namespace snake { - bool Scenario::setArea(Area &area) +Scenario::Scenario() : + _mAreaBoundingBox(min_bbox_rt{0, 0, 0, Point2D{0,0}}) +{ + +} + +bool Scenario::setArea(Area &area) { if (area.geoPolygon.size() < 3){ error_str = "Area has less than three vertices."; @@ -84,4 +94,128 @@ namespace snake { return true; } + bool Scenario::_calculateBoundingBox() + { + _mAreaBoundingBox = minimalBoundingBox(_measurementAreaENU); + return true; + } + /** + * Devides the (measurement area) bounding box into tiles and clips it to the measurement area. + * + * Devides the (measurement area) bounding box into tiles of width \p tileWidth and height \p tileHeight. + * Clips the resulting tiles to the measurement area. Tiles are rejected, if their area is smaller than \p minTileArea. + * The function assumes that \a _measurementAreaENU and \a _mAreaBoundingBox have correct values. \see \ref Scenario::_areas2enu() and \ref + * Scenario::_calculateBoundingBox(). + * + * @param tileWidth The width of a tile. + * @param tileHeight The heigth of a tile. + * @param minTileArea The minimal area of a tile. + * + * @return Returns true if successful. + */ + bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTileArea) + { + std::vector tiles_unclipped; + + return true; + } + + bool Scenario::_calculateJoinedArea() + { + ClipperLib::Path measurementArea; + ClipperLib::Path serviceArea; + ClipperLib::Path corridor; + + // Apply scaling and convert ENU polygons (double) to ClipperLib::cInt polygons. + for (auto vertex : _measurementAreaENU){ + ClipperLib::IntPoint intVertex{ClipperLib::cInt(vertex[0]*clipper_scale), + ClipperLib::cInt(vertex[1]*clipper_scale)}; + measurementArea.push_back(intVertex); + } + + for (auto vertex : _serviceAreaENU){ + ClipperLib::IntPoint intVertex{ClipperLib::cInt(vertex[0]*clipper_scale), + ClipperLib::cInt(vertex[1]*clipper_scale)}; + serviceArea.push_back(intVertex); + } + + bool corridorValid = false; + if (_corridorENU.size() > 0) { + for (auto vertex : _corridorENU){ + ClipperLib::IntPoint intVertex{ClipperLib::cInt(vertex[0]*clipper_scale), + ClipperLib::cInt(vertex[1]*clipper_scale)}; + corridor.push_back(intVertex); + } + corridorValid = true; + } + + // Check if measurement area and service area are overlapping. + ClipperLib::Clipper cp1; + cp1.AddPath(measurementArea, ClipperLib::ptClip, true); + cp1.AddPath(serviceArea, ClipperLib::ptSubject, true); + + // Execute clipper + ClipperLib::Paths solution; + cp1.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd); + + // Measurement area and service area overlapping? + bool overlaping_s_m = false; + if (solution.size() > 0){ + overlaping_s_m = true; + } + + + // Check if corridor is connecting measurement area and service area. + bool corridor_is_connection = false; + if (corridorValid) { + ClipperLib::Clipper cp2; + solution.clear(); + cp2.AddPath(measurementArea, ClipperLib::ptClip, true); + cp2.AddPath(corridor, ClipperLib::ptSubject, true); + cp2.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd); + + // Corridor overlaping with measurement area? + if (solution.size() > 0) { + // Check if corridor overlaps with service area. + cp2.Clear(); + solution.clear(); + cp2.AddPath(serviceArea, ClipperLib::ptClip, true); + cp2.AddPath(corridor, ClipperLib::ptSubject, true); + cp2.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd); + + // Corridor overlaping with service area? + if (solution.size() > 0) { + corridor_is_connection = true; + } + } + } + + // Are areas joinable? + if (overlaping_s_m){ + if(corridor_is_connection){ + cp1.AddPath(corridor, ClipperLib::ptSubject, true); + } + } else if (corridor_is_connection){ + cp1.AddPath(corridor, ClipperLib::ptSubject, true); + } else { + error_str = "Areas are not overlapping"; + return false; + } + + // Join areas. + solution.clear(); + cp1.Execute(ClipperLib::ctUnion, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd); + + if (solution.size() != 1) + assert(0); + + _joinedAreaENU.clear(); + for (auto intVertex : solution[0]){ + Point2D vertex{double(intVertex.X)/clipper_scale, double(intVertex.Y)/clipper_scale}; + _joinedAreaENU.push_back(vertex); + } + + return true; + } + } diff --git a/libs/snake/snake/snake.h b/libs/snake/snake/snake.h index cd497532fdcf934363a3c6e417cfe2f82b63d20e..54aa2ec1b7376cad3596bf9bbc4760da7b490ccd 100644 --- a/libs/snake/snake/snake.h +++ b/libs/snake/snake/snake.h @@ -6,6 +6,8 @@ #include "snake_geometry.h" +#define clipper_scale 10000.0 + using namespace std; using namespace snake_geometry; @@ -14,10 +16,10 @@ namespace snake { enum AreaType {MeasurementArea, ServiceArea, Corridor}; struct Area { - vector geoPolygon; - double altitude; - size_t layers; - AreaType type; + vector geoPolygon; + double altitude; + size_t layers; + AreaType type; }; class Scenario{ @@ -39,11 +41,8 @@ namespace snake { bool _setServiceArea(Area &area); bool _setCorridor(Area &area); bool _calculateBoundingBox(); - bool _calculateTiles(); + bool _calculateTiles(double tileWidth, double tileHeight, double minTileArea); bool _calculateJoinedArea(); - bool _calculateMAreaLocal(); - bool _calculateSAreaLocal(); - bool _calculateCorridorLocal(); Area _measurementArea; Area _serviceArea; @@ -54,6 +53,8 @@ namespace snake { Point2DList _corridorENU; Point2DList _joinedAreaENU; + min_bbox_rt _mAreaBoundingBox; + vector _tilesENU; vector _tilesCenterPointsENU;