23 #include <navgraph/generators/voronoi.h> 24 #include <core/exception.h> 25 #include <utils/math/polygon.h> 26 #include <utils/math/triangle.h> 29 #include <CGAL/Exact_predicates_inexact_constructions_kernel.h> 30 #include <CGAL/Delaunay_triangulation_2.h> 31 #include <CGAL/Voronoi_diagram_2.h> 32 #include <CGAL/Delaunay_triangulation_adaptation_traits_2.h> 33 #include <CGAL/Delaunay_triangulation_adaptation_policies_2.h> 36 typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
37 typedef CGAL::Delaunay_triangulation_2<K> DT;
38 typedef CGAL::Delaunay_triangulation_adaptation_traits_2<DT> AT;
39 typedef CGAL::Delaunay_triangulation_caching_degeneracy_removal_policy_2<DT> AP;
40 typedef CGAL::Voronoi_diagram_2<DT,AT,AP> VD;
43 typedef AT::Site_2 Site_2;
44 typedef AT::Point_2 Point_2;
46 typedef VD::Locate_result Locate_result;
47 typedef VD::Vertex_handle Vertex_handle;
48 typedef VD::Face_handle Face_handle;
49 typedef VD::Halfedge_handle Halfedge_handle;
50 typedef VD::Ccb_halfedge_circulator Ccb_halfedge_circulator;
52 typedef K::Iso_rectangle_2 Iso_rectangle;
54 typedef std::map<std::string, Point_2> Point_map;
68 : bbox_enabled_(false),
69 bbox_p1_x_(0.), bbox_p1_y_(0.), bbox_p2_x_(0.), bbox_p2_y_(0.),
86 float bbox_p2_x,
float bbox_p2_y,
88 : bbox_enabled_(true),
89 bbox_p1_x_(bbox_p1_x), bbox_p1_y_(bbox_p1_y),
90 bbox_p2_x_(bbox_p2_x), bbox_p2_y_(bbox_p2_y),
91 near_threshold_(near_threshold)
113 contains(Point_map points, Point_2 point, std::string &name,
float near_threshold)
115 for (
auto p : points) {
116 K::FT dist = sqrt(CGAL::squared_distance(p.second, point));
117 if (dist < near_threshold) {
134 if (asprintf(&name,
"V_%02u", ++i) != -1) {
135 std::string rv = name;
139 throw Exception(
"Failed to create node name");
154 float bbox_p2_x,
float bbox_p2_y)
156 bbox_enabled_ =
true;
157 bbox_p1_x_ = bbox_p1_x;
158 bbox_p1_y_ = bbox_p1_y;
159 bbox_p2_x_ = bbox_p2_x;
160 bbox_p2_y_ = bbox_p2_y;
171 near_threshold_ = near_threshold;
183 obstacles_.push_back(std::make_pair(x, y));
205 for (
auto o : obstacles_) {
206 vd.insert(Site_2(o.first, o.second));
211 Iso_rectangle rect(Point_2(bbox_p1_x_, bbox_p1_y_), Point_2(bbox_p2_x_, bbox_p2_y_));
213 std::map<std::string, Point_2> points;
214 std::map<std::string, std::string> props_gen;
215 props_gen[
"generated"] =
"true";
217 unsigned int num_nodes = 0;
221 for (e = vd.edges_begin(); e != vd.edges_end(); ++e) {
222 if (e->is_segment()) {
224 CGAL::Bounded_side source_side, target_side;
225 source_side = rect.bounded_side(e->source()->point());
226 target_side = rect.bounded_side(e->target()->point());
228 if (source_side == CGAL::ON_UNBOUNDED_SIDE || target_side == CGAL::ON_UNBOUNDED_SIDE)
233 std::string source_name, target_name;
234 bool have_source =
contains(points, e->source()->point(),
235 source_name, near_threshold_);
236 bool have_target =
contains(points, e->target()->point(),
237 target_name, near_threshold_);
240 source_name =
genname(num_nodes);
243 e->source()->point().x(), e->source()->point().y(),
245 points[source_name] = e->source()->point();
248 target_name =
genname(num_nodes);
251 e->target()->point().x(), e->target()->point().y(),
253 points[target_name] = e->target()->point();
263 VD::Bounded_faces_iterator f;
264 for (f = vd.bounded_faces_begin(); f != vd.bounded_faces_end(); ++f) {
265 unsigned int num_v = 0;
266 Ccb_halfedge_circulator ec_start = f->outer_ccb();
267 Ccb_halfedge_circulator ec = ec_start;
269 do { ++num_v; }
while ( ++ec != ec_start );
275 const Point_2 &p = ec->source()->point();
277 if (rect.has_on_unbounded_side(p)) {
282 poly[poly_i][0] = p.x();
283 poly[poly_i][1] = p.y();
285 }
while ( ++ec != ec_start );
286 if (f_ok) polygons_.push_back(poly);
289 std::list<Eigen::Vector2f> node_coords;
290 std::vector<NavGraphNode>::const_iterator n;
291 for (n = graph->
nodes().begin(); n != graph->
nodes().end(); ++n) {
292 node_coords.push_back(Eigen::Vector2f(n->x(), n->y()));
295 polygons_.remove_if([&node_coords](
const Polygon2D &poly) {
296 for (
const auto nc : node_coords) {
Fawkes library namespace.
void unlock() const
Unlock object mutex.
void add_node(const NavGraphNode &node)
Add a node.
void set_bounding_box(float bbox_p1_x, float bbox_p1_y, float bbox_p2_x, float bbox_p2_y)
Set bounding box.
void add_obstacle(float x, float y)
Add an obstacle point.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
void set_near_threshold(float near_threshold)
Set distance threshold for considering nodes to be the same.
float polygon_area(const Polygon2D &p)
Calculate area of a polygon.
Base class for exceptions in Fawkes.
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > Polygon2D
Polygon as a vector of 2D points.
static std::string genname(unsigned int &i)
Generate a new name.
static bool contains(Point_map points, Point_2 point, std::string &name, float near_threshold)
Check if a point is already contained in a map.
bool polygon_contains(const Polygon2D &polygon, const Eigen::Vector2f &point)
Check if given point lies inside the polygon.
virtual ~NavGraphGeneratorVoronoi()
Destructor.
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)
Compute graph.
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
void lock() const
Lock access to the encapsulated object.
void clear()
Clear all obstacle points.
NavGraphGeneratorVoronoi()
Default constructor.