Fawkes API Fawkes Development Version
voronoi.cpp
1
2/***************************************************************************
3 * voronoi.cpp - generate navgraph from Voronoi using CGAL
4 *
5 * Created: Tue Jan 13 11:53:29 2015
6 * Copyright 2015 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version. A runtime exception applies to
13 * this software (see LICENSE.GPL_WRE file mentioned below for details).
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21 */
22
23#include <core/exception.h>
24#include <navgraph/generators/voronoi.h>
25#include <utils/math/polygon.h>
26#include <utils/math/triangle.h>
27
28// includes for defining the Voronoi diagram adaptor
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>
34
35// typedefs for defining the adaptor
36typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
37typedef CGAL::Delaunay_triangulation_2<K> DT;
38typedef CGAL::Delaunay_triangulation_adaptation_traits_2<DT> AT;
39typedef CGAL::Delaunay_triangulation_caching_degeneracy_removal_policy_2<DT> AP;
40typedef CGAL::Voronoi_diagram_2<DT, AT, AP> VD;
41
42// typedef for the result type of the point location
43typedef AT::Site_2 Site_2;
44typedef AT::Point_2 Point_2;
45
46typedef VD::Locate_result Locate_result;
47typedef VD::Vertex_handle Vertex_handle;
48typedef VD::Face_handle Face_handle;
49typedef VD::Halfedge_handle Halfedge_handle;
50typedef VD::Ccb_halfedge_circulator Ccb_halfedge_circulator;
51
52typedef K::Iso_rectangle_2 Iso_rectangle;
53
54typedef std::map<std::string, Point_2> Point_map;
55
56namespace fawkes {
57
58/** @class NavGraphGeneratorVoronoi <navgraph/generators/voronoi.h>
59 * Generate navgraph using a Voronoi diagram.
60 * @author Tim Niemueller
61 */
62
63/** Default constructor. */
65{
66}
67
68/** Destructor. */
70{
71}
72
73/** Check if a point is already contained in a map.
74 * @param points map of points to check for @p point
75 * @param point point to check whether it already exists
76 * @param name if the point was found in the map will be assigned
77 * the name of the point in the map upon return
78 * @param near_threshold distance threshold for which to consider
79 * nodes to be the same if the distance is smaller than this
80 * threshold.
81 * @return true if the point has been found in the map, false otherwise
82 */
83static bool
84contains(Point_map points, Point_2 point, std::string &name, float near_threshold)
85{
86 for (auto p : points) {
87 K::FT dist = sqrt(CGAL::squared_distance(p.second, point));
88 if (dist < near_threshold) {
89 name = p.first;
90 return true;
91 }
92 }
93 return false;
94}
95
96/** Compute graph.
97 * @param graph the resulting nodes and edges will be added to this graph.
98 * The graph will *not* be cleared automatically. The graph will be locked
99 * while adding nodes.
100 */
101void
103{
104 VD vd;
105 for (auto o : obstacles_) {
106 vd.insert(Site_2(o.first, o.second));
107 }
108
109 polygons_.clear();
110
111 Iso_rectangle rect(Point_2(bbox_p1_x_, bbox_p1_y_), Point_2(bbox_p2_x_, bbox_p2_y_));
112
113 std::map<std::string, Point_2> points;
114 std::map<std::string, std::string> props_gen;
115 props_gen["generated"] = "true";
116
117 unsigned int num_nodes = 0;
118 if (vd.is_valid()) {
119 VD::Edge_iterator e;
120 graph.lock();
121 for (e = vd.edges_begin(); e != vd.edges_end(); ++e) {
122 if (e->is_segment()) {
123 if (bbox_enabled_) {
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());
127
128 if (source_side == CGAL::ON_UNBOUNDED_SIDE || target_side == CGAL::ON_UNBOUNDED_SIDE)
129 continue;
130 }
131
132 // check if we have a point in the vicinity
133 std::string source_name, target_name;
134 bool have_source = contains(points, e->source()->point(), source_name, near_threshold_);
135 bool have_target = contains(points, e->target()->point(), target_name, near_threshold_);
136
137 if (!have_source) {
138 source_name = genname(num_nodes);
139 //printf("Adding source %s\n", source_name.c_str());
140 graph->add_node(NavGraphNode(
141 source_name, e->source()->point().x(), e->source()->point().y(), props_gen));
142 points[source_name] = e->source()->point();
143 }
144 if (!have_target) {
145 target_name = genname(num_nodes);
146 //printf("Adding target %s\n", target_name.c_str());
147 graph->add_node(NavGraphNode(
148 target_name, e->target()->point().x(), e->target()->point().y(), props_gen));
149 points[target_name] = e->target()->point();
150 }
151
152 graph->add_edge(NavGraphEdge(source_name, target_name, props_gen));
153 } else {
154 //printf("Unbounded edge\n");
155 }
156 }
157
158 // Store Polygons
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;
164
165 do {
166 ++num_v;
167 } while (++ec != ec_start);
168
169 Polygon2D poly(num_v);
170 size_t poly_i = 0;
171 bool f_ok = true;
172 do {
173 const Point_2 &p = ec->source()->point();
174 if (bbox_enabled_) {
175 if (rect.has_on_unbounded_side(p)) {
176 f_ok = false;
177 break;
178 }
179 }
180 poly[poly_i][0] = p.x();
181 poly[poly_i][1] = p.y();
182 ++poly_i;
183 } while (++ec != ec_start);
184 if (f_ok)
185 polygons_.push_back(poly);
186 }
187
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()));
192 }
193
194 polygons_.remove_if([&node_coords](const Polygon2D &poly) {
195 for (const auto &nc : node_coords) {
196 if (polygon_contains(poly, nc))
197 return true;
198 }
199 return false;
200 });
201
202 polygons_.sort(
203 [](const Polygon2D &p1, const Polygon2D &p2) { return polygon_area(p2) < polygon_area(p1); });
204
205 graph->calc_reachability();
206 graph.unlock();
207 }
208}
209
210} // end of namespace fawkes
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:257
void unlock() const
Unlock object mutex.
Definition: lockptr.h:273
Topological graph edge.
Definition: navgraph_edge.h:38
virtual ~NavGraphGeneratorVoronoi()
Destructor.
Definition: voronoi.cpp:69
NavGraphGeneratorVoronoi()
Default constructor.
Definition: voronoi.cpp:64
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)
Compute graph.
Definition: voronoi.cpp:102
float bbox_p1_x_
X part of P1 for bounding box.
Definition: generator.h:47
float bbox_p2_x_
X part of P2 for bounding box.
Definition: generator.h:49
std::list< std::pair< float, float > > obstacles_
Obstacles to consider during navgraph generation.
Definition: generator.h:54
bool bbox_enabled_
True if bounding box requested, false otherwise.
Definition: generator.h:46
static std::string genname(unsigned int &i)
Generate a new name.
Definition: generator.cpp:90
float bbox_p1_y_
Y part of P1 for bounding box.
Definition: generator.h:48
float bbox_p2_y_
Y part of P2 for bounding box.
Definition: generator.h:50
float near_threshold_
distance threshold when to consider two nodes to be the same
Definition: generator.h:51
Topological graph node.
Definition: navgraph_node.h:36
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
Definition: navgraph.cpp:584
void add_node(const NavGraphNode &node)
Add a node.
Definition: navgraph.cpp:460
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
Definition: navgraph.cpp:1410
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:133
Fawkes library namespace.
float polygon_area(const Polygon2D &p)
Calculate area of a polygon.
Definition: polygon.h:40
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.
Definition: voronoi.cpp:84
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > Polygon2D
Polygon as a vector of 2D points.
Definition: polygon.h:33
bool polygon_contains(const Polygon2D &polygon, const Eigen::Vector2f &point)
Check if given point lies inside the polygon.
Definition: polygon.h:67