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 <navgraph/generators/voronoi.h>
24 #include <core/exception.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/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>
34 
35 // typedefs for defining the adaptor
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;
41 
42 // typedef for the result type of the point location
43 typedef AT::Site_2 Site_2;
44 typedef AT::Point_2 Point_2;
45 
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;
51 
52 typedef K::Iso_rectangle_2 Iso_rectangle;
53 
54 typedef std::map<std::string, Point_2> Point_map;
55 
56 namespace fawkes {
57 #if 0 /* just to make Emacs auto-indent happy */
58 }
59 #endif
60 
61 /** @class NavGraphGeneratorVoronoi <navgraph/generators/voronoi.h>
62  * Generate navgraph using a Voronoi diagram.
63  * @author Tim Niemueller
64  */
65 
66 /** Default constructor. */
68  : bbox_enabled_(false),
69  bbox_p1_x_(0.), bbox_p1_y_(0.), bbox_p2_x_(0.), bbox_p2_y_(0.),
70  near_threshold_(0.01)
71 {
72 }
73 
74 /** Constructor with bounding box.
75  * This constructor will cause compute() to ignore any edge with a vertex
76  * out of the given bounding box area.
77  * @param bbox_p1_x X coordinate of first (lower) bounding box point
78  * @param bbox_p1_y y coordinate of first (lower) bounding box point
79  * @param bbox_p2_x X coordinate of second (upper) bounding box point
80  * @param bbox_p2_y y coordinate of second (upper) bounding box point
81  * @param near_threshold distance threshold for which to consider
82  * nodes to be the same if the distance is smaller than this
83  * threshold.
84  */
85 NavGraphGeneratorVoronoi::NavGraphGeneratorVoronoi(float bbox_p1_x, float bbox_p1_y,
86  float bbox_p2_x, float bbox_p2_y,
87  float near_threshold)
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)
92 {
93 }
94 
95 
96 /** Destructor. */
98 {
99 }
100 
101 
102 /** Check if a point is already contained in a map.
103  * @param points map of points to check for @p point
104  * @param point point to check whether it already exists
105  * @param name if the point was found in the map will be assigned
106  * the name of the point in the map upon return
107  * @param near_threshold distance threshold for which to consider
108  * nodes to be the same if the distance is smaller than this
109  * threshold.
110  * @return true if the point has been found in the map, false otherwise
111  */
112 static bool
113 contains(Point_map points, Point_2 point, std::string &name, float near_threshold)
114 {
115  for (auto p : points) {
116  K::FT dist = sqrt(CGAL::squared_distance(p.second, point));
117  if (dist < near_threshold) {
118  name = p.first;
119  return true;
120  }
121  }
122  return false;
123 }
124 
125 
126 /** Generate a new name
127  * @param i number parameter for point name, will be incremented by one
128  * @return string with a new point name
129  */
130 static std::string
131 genname(unsigned int &i)
132 {
133  char * name;
134  if (asprintf(&name, "V_%02u", ++i) != -1) {
135  std::string rv = name;
136  free(name);
137  return rv;
138  } else {
139  throw Exception("Failed to create node name");
140  }
141 }
142 
143 
144 /** Set bounding box.
145  * Setting a bounding box will cause compute() to ignore any edge with
146  * a vertex out of the given bounding box area.
147  * @param bbox_p1_x X coordinate of first (lower) bounding box point
148  * @param bbox_p1_y y coordinate of first (lower) bounding box point
149  * @param bbox_p2_x X coordinate of second (upper) bounding box point
150  * @param bbox_p2_y y coordinate of second (upper) bounding box point
151  */
152 void
153 NavGraphGeneratorVoronoi::set_bounding_box(float bbox_p1_x, float bbox_p1_y,
154  float bbox_p2_x, float bbox_p2_y)
155 {
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;
161 }
162 
163 /** Set distance threshold for considering nodes to be the same.
164  * @param near_threshold distance threshold for which to consider
165  * nodes to be the same if the distance is smaller than this
166  * threshold.
167  */
168 void
170 {
171  near_threshold_ = near_threshold;
172 }
173 
174 /** Add an obstacle point.
175  * An obstacle point will be the representative for a Voronoi
176  * face in the newly generated graph.
177  * @param x X coordinate of point
178  * @param y Y coordinate of point
179  */
180 void
182 {
183  obstacles_.push_back(std::make_pair(x, y));
184 }
185 
186 
187 /** Clear all obstacle points. */
188 void
190 {
191  obstacles_.clear();
192  polygons_.clear();
193 }
194 
195 
196 /** Compute graph.
197  * @param graph the resulting nodes and edges will be added to this graph.
198  * The graph will *not* be cleared automatically. The graph will be locked
199  * while adding nodes.
200  */
201 void
203 {
204  VD vd;
205  for (auto o : obstacles_) {
206  vd.insert(Site_2(o.first, o.second));
207  }
208 
209  polygons_.clear();
210 
211  Iso_rectangle rect(Point_2(bbox_p1_x_, bbox_p1_y_), Point_2(bbox_p2_x_, bbox_p2_y_));
212 
213  std::map<std::string, Point_2> points;
214  std::map<std::string, std::string> props_gen;
215  props_gen["generated"] = "true";
216 
217  unsigned int num_nodes = 0;
218  if (vd.is_valid()) {
219  VD::Edge_iterator e;
220  graph.lock();
221  for (e = vd.edges_begin(); e != vd.edges_end(); ++e) {
222  if (e->is_segment()) {
223  if (bbox_enabled_) {
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());
227 
228  if (source_side == CGAL::ON_UNBOUNDED_SIDE || target_side == CGAL::ON_UNBOUNDED_SIDE)
229  continue;
230  }
231 
232  // check if we have a point in the vicinity
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_);
238 
239  if (! have_source) {
240  source_name = genname(num_nodes);
241  //printf("Adding source %s\n", source_name.c_str());
242  graph->add_node(NavGraphNode(source_name,
243  e->source()->point().x(), e->source()->point().y(),
244  props_gen));
245  points[source_name] = e->source()->point();
246  }
247  if (! have_target) {
248  target_name = genname(num_nodes);
249  //printf("Adding target %s\n", target_name.c_str());
250  graph->add_node(NavGraphNode(target_name,
251  e->target()->point().x(), e->target()->point().y(),
252  props_gen));
253  points[target_name] = e->target()->point();
254  }
255 
256  graph->add_edge(NavGraphEdge(source_name, target_name, props_gen));
257  } else {
258  //printf("Unbounded edge\n");
259  }
260  }
261 
262  // Store Polygons
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;
268 
269  do { ++num_v; } while ( ++ec != ec_start );
270 
271  Polygon2D poly(num_v);
272  size_t poly_i = 0;
273  bool f_ok = true;
274  do {
275  const Point_2 &p = ec->source()->point();
276  if (bbox_enabled_) {
277  if (rect.has_on_unbounded_side(p)) {
278  f_ok = false;
279  break;
280  }
281  }
282  poly[poly_i][0] = p.x();
283  poly[poly_i][1] = p.y();
284  ++poly_i;
285  } while ( ++ec != ec_start );
286  if (f_ok) polygons_.push_back(poly);
287  }
288 
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()));
293  }
294 
295  polygons_.remove_if([&node_coords](const Polygon2D &poly) {
296  for (const auto nc : node_coords) {
297  if (polygon_contains(poly, nc)) return true;
298  }
299  return false;
300  }
301  );
302 
303  polygons_.sort([](const Polygon2D &p1, const Polygon2D &p2)
304  {
305  return polygon_area(p2) < polygon_area(p1);
306  }
307  );
308 
309  graph->calc_reachability();
310  graph.unlock();
311  }
312 }
313 
314 
315 } // end of namespace fawkes
Fawkes library namespace.
void unlock() const
Unlock object mutex.
Definition: lockptr.h:255
void add_node(const NavGraphNode &node)
Add a node.
Definition: navgraph.cpp:461
void set_bounding_box(float bbox_p1_x, float bbox_p1_y, float bbox_p2_x, float bbox_p2_y)
Set bounding box.
Definition: voronoi.cpp:153
void add_obstacle(float x, float y)
Add an obstacle point.
Definition: voronoi.cpp:181
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:124
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
Definition: navgraph.cpp:592
void set_near_threshold(float near_threshold)
Set distance threshold for considering nodes to be the same.
Definition: voronoi.cpp:169
float polygon_area(const Polygon2D &p)
Calculate area of a polygon.
Definition: polygon.h:43
Base class for exceptions in Fawkes.
Definition: exception.h:36
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > Polygon2D
Polygon as a vector of 2D points.
Definition: polygon.h:36
static std::string genname(unsigned int &i)
Generate a new name.
Definition: voronoi.cpp:131
Topological graph edge.
Definition: navgraph_edge.h:39
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:113
Topological graph node.
Definition: navgraph_node.h:38
bool polygon_contains(const Polygon2D &polygon, const Eigen::Vector2f &point)
Check if given point lies inside the polygon.
Definition: polygon.h:70
virtual ~NavGraphGeneratorVoronoi()
Destructor.
Definition: voronoi.cpp:97
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)
Compute graph.
Definition: voronoi.cpp:202
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
Definition: navgraph.cpp:1387
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:247
void clear()
Clear all obstacle points.
Definition: voronoi.cpp:189
NavGraphGeneratorVoronoi()
Default constructor.
Definition: voronoi.cpp:67