23 #include <core/exception.h>
24 #include <navgraph/generators/voronoi.h>
25 #include <utils/math/polygon.h>
26 #include <utils/math/triangle.h>
29 #include <CGAL/Delaunay_triangulation_2.h>
30 #include <CGAL/Delaunay_triangulation_adaptation_policies_2.h>
31 #include <CGAL/Delaunay_triangulation_adaptation_traits_2.h>
32 #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
33 #include <CGAL/Voronoi_diagram_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;
84 contains(Point_map points, Point_2 point, std::string &name,
float near_threshold)
86 for (
auto p : points) {
87 K::FT dist = sqrt(CGAL::squared_distance(p.second, point));
88 if (dist < near_threshold) {
106 vd.insert(Site_2(o.first, o.second));
113 std::map<std::string, Point_2> points;
114 std::map<std::string, std::string> props_gen;
115 props_gen[
"generated"] =
"true";
117 unsigned int num_nodes = 0;
121 for (e = vd.edges_begin(); e != vd.edges_end(); ++e) {
122 if (e->is_segment()) {
124 CGAL::Bounded_side source_side, target_side;
125 source_side = rect.bounded_side(e->source()->point());
126 target_side = rect.bounded_side(e->target()->point());
128 if (source_side == CGAL::ON_UNBOUNDED_SIDE || target_side == CGAL::ON_UNBOUNDED_SIDE)
133 std::string source_name, target_name;
138 source_name =
genname(num_nodes);
141 source_name, e->source()->point().x(), e->source()->point().y(), props_gen));
142 points[source_name] = e->source()->point();
145 target_name =
genname(num_nodes);
148 target_name, e->target()->point().x(), e->target()->point().y(), props_gen));
149 points[target_name] = e->target()->point();
159 VD::Bounded_faces_iterator f;
160 for (f = vd.bounded_faces_begin(); f != vd.bounded_faces_end(); ++f) {
161 unsigned int num_v = 0;
162 Ccb_halfedge_circulator ec_start = f->outer_ccb();
163 Ccb_halfedge_circulator ec = ec_start;
167 }
while (++ec != ec_start);
173 const Point_2 &p = ec->source()->point();
175 if (rect.has_on_unbounded_side(p)) {
180 poly[poly_i][0] = p.x();
181 poly[poly_i][1] = p.y();
183 }
while (++ec != ec_start);
185 polygons_.push_back(poly);
188 std::list<Eigen::Vector2f> node_coords;
189 std::vector<NavGraphNode>::const_iterator n;
190 for (n = graph->
nodes().begin(); n != graph->
nodes().end(); ++n) {
191 node_coords.push_back(Eigen::Vector2f(n->x(), n->y()));
194 polygons_.remove_if([&node_coords](
const Polygon2D &poly) {
195 for (
const auto nc : node_coords) {
void lock() const
Lock access to the encapsulated object.
void unlock() const
Unlock object mutex.
virtual ~NavGraphGeneratorVoronoi()
Destructor.
NavGraphGeneratorVoronoi()
Default constructor.
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)
Compute graph.
float bbox_p1_x_
X part of P1 for bounding box.
float bbox_p2_x_
X part of P2 for bounding box.
std::list< std::pair< float, float > > obstacles_
Obstacles to consider during navgraph generation.
bool bbox_enabled_
True if bounding box requested, false otherwise.
static std::string genname(unsigned int &i)
Generate a new name.
float bbox_p1_y_
Y part of P1 for bounding box.
float bbox_p2_y_
Y part of P2 for bounding box.
float near_threshold_
distance threshold when to consider two nodes to be the same
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
void add_node(const NavGraphNode &node)
Add a node.
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Fawkes library namespace.
float polygon_area(const Polygon2D &p)
Calculate area of a polygon.
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.
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > Polygon2D
Polygon as a vector of 2D points.
bool polygon_contains(const Polygon2D &polygon, const Eigen::Vector2f &point)
Check if given point lies inside the polygon.