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
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 
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  */
83 static bool
84 contains(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  */
101 void
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
fawkes::NavGraphNode
Definition: navgraph_node.h:40
fawkes::Polygon2D
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > Polygon2D
Polygon as a vector of 2D points.
Definition: polygon.h:39
fawkes::LockPtr< fawkes::NavGraph >
fawkes::polygon_contains
bool polygon_contains(const Polygon2D &polygon, const Eigen::Vector2f &point)
Check if given point lies inside the polygon.
Definition: polygon.h:73
fawkes::NavGraphGenerator::bbox_p2_y_
float bbox_p2_y_
Y part of P2 for bounding box.
Definition: generator.h:60
fawkes::NavGraphEdge
Definition: navgraph_edge.h:42
fawkes::contains
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
fawkes::NavGraphGenerator::bbox_p1_y_
float bbox_p1_y_
Y part of P1 for bounding box.
Definition: generator.h:58
fawkes::NavGraph::add_edge
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
Definition: navgraph.cpp:589
fawkes::polygon_area
float polygon_area(const Polygon2D &p)
Calculate area of a polygon.
Definition: polygon.h:46
fawkes::NavGraphGeneratorVoronoi::compute
virtual void compute(fawkes::LockPtr< fawkes::NavGraph > graph)
Compute graph.
Definition: voronoi.cpp:102
fawkes::NavGraphGenerator::bbox_enabled_
bool bbox_enabled_
True if bounding box requested, false otherwise.
Definition: generator.h:56
fawkes::NavGraphGeneratorVoronoi::~NavGraphGeneratorVoronoi
virtual ~NavGraphGeneratorVoronoi()
Destructor.
Definition: voronoi.cpp:69
fawkes::NavGraphGenerator::near_threshold_
float near_threshold_
distance threshold when to consider two nodes to be the same
Definition: generator.h:61
fawkes::LockPtr::unlock
void unlock() const
Unlock object mutex.
Definition: lockptr.h:289
fawkes::NavGraph::calc_reachability
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
Definition: navgraph.cpp:1415
fawkes::NavGraphGeneratorVoronoi::NavGraphGeneratorVoronoi
NavGraphGeneratorVoronoi()
Default constructor.
Definition: voronoi.cpp:64
fawkes
fawkes::NavGraphGenerator::obstacles_
std::list< std::pair< float, float > > obstacles_
Obstacles to consider during navgraph generation.
Definition: generator.h:64
fawkes::NavGraphGenerator::genname
static std::string genname(unsigned int &i)
Generate a new name.
Definition: generator.cpp:95
fawkes::NavGraphGenerator::bbox_p1_x_
float bbox_p1_x_
X part of P1 for bounding box.
Definition: generator.h:57
fawkes::NavGraph::add_node
void add_node(const NavGraphNode &node)
Add a node.
Definition: navgraph.cpp:465
fawkes::NavGraph::nodes
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:138
fawkes::NavGraphGenerator::bbox_p2_x_
float bbox_p2_x_
X part of P2 for bounding box.
Definition: generator.h:59
fawkes::LockPtr::lock
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:273