/** Get a specified edge.
* @param from originating node name
* @param to target node name
* @return the edge representation for the edge with the given
* originating and target nodes or an invalid edge if the edge
* cannot be found
*/
NavGraphEdge
NavGraph::edge(const std::string &from, const std::string &to) const
{
std::vector<NavGraphEdge>::const_iterator e =
std::find_if(edges_.begin(), edges_.end(),
[&from, &to](const NavGraphEdge &edge) {
return (edge.from() == from && edge.to() == to) ||
(! edge.is_directed() && (edge.to() == from && edge.from() == to));
});
if (e != edges_.end()) {
return *e;
} else {
return NavGraphEdge();
}
}
/** Compute graph.
* @param graph the resulting nodes and edges will be added to this graph.
* The graph will *not* be cleared automatically. The graph will be locked
* while adding nodes.
*/
void
NavGraphGeneratorVoronoi::compute(fawkes::LockPtr<fawkes::NavGraph> graph)
{
VD vd;
for (auto o : obstacles_) {
vd.insert(Site_2(o.first, o.second));
}
polygons_.clear();
Iso_rectangle rect(Point_2(bbox_p1_x_, bbox_p1_y_), Point_2(bbox_p2_x_, bbox_p2_y_));
std::map<std::string, Point_2> points;
std::map<std::string, std::string> props_gen;
props_gen["generated"] = "true";
unsigned int num_nodes = 0;
if (vd.is_valid()) {
VD::Edge_iterator e;
graph.lock();
for (e = vd.edges_begin(); e != vd.edges_end(); ++e) {
if (e->is_segment()) {
if (bbox_enabled_) {
CGAL::Bounded_side source_side, target_side;
source_side = rect.bounded_side(e->source()->point());
target_side = rect.bounded_side(e->target()->point());
if (source_side == CGAL::ON_UNBOUNDED_SIDE || target_side == CGAL::ON_UNBOUNDED_SIDE)
continue;
}
// check if we have a point in the vicinity
std::string source_name, target_name;
bool have_source = contains(points, e->source()->point(),
source_name, near_threshold_);
bool have_target = contains(points, e->target()->point(),
target_name, near_threshold_);
if (! have_source) {
source_name = genname(num_nodes);
//printf("Adding source %s\n", source_name.c_str());
graph->add_node(NavGraphNode(source_name,
e->source()->point().x(), e->source()->point().y(),
props_gen));
points[source_name] = e->source()->point();
}
if (! have_target) {
target_name = genname(num_nodes);
//printf("Adding target %s\n", target_name.c_str());
graph->add_node(NavGraphNode(target_name,
e->target()->point().x(), e->target()->point().y(),
props_gen));
points[target_name] = e->target()->point();
}
graph->add_edge(NavGraphEdge(source_name, target_name, props_gen));
} else {
//printf("Unbounded edge\n");
}
}
// Store Polygons
VD::Bounded_faces_iterator f;
for (f = vd.bounded_faces_begin(); f != vd.bounded_faces_end(); ++f) {
unsigned int num_v = 0;
Ccb_halfedge_circulator ec_start = f->outer_ccb();
Ccb_halfedge_circulator ec = ec_start;
do { ++num_v; } while ( ++ec != ec_start );
Polygon2D poly(num_v);
size_t poly_i = 0;
bool f_ok = true;
do {
const Point_2 &p = ec->source()->point();
if (bbox_enabled_) {
if (rect.has_on_unbounded_side(p)) {
f_ok = false;
break;
}
}
poly[poly_i][0] = p.x();
poly[poly_i][1] = p.y();
++poly_i;
} while ( ++ec != ec_start );
if (f_ok) polygons_.push_back(poly);
}
std::list<Eigen::Vector2f> node_coords;
std::vector<NavGraphNode>::const_iterator n;
for (n = graph->nodes().begin(); n != graph->nodes().end(); ++n) {
node_coords.push_back(Eigen::Vector2f(n->x(), n->y()));
}
polygons_.remove_if([&node_coords](const Polygon2D &poly) {
for (const auto nc : node_coords) {
if (polygon_contains(poly, nc)) return true;
}
return false;
}
);
polygons_.sort([](const Polygon2D &p1, const Polygon2D &p2)
{
return polygon_area(p2) < polygon_area(p1);
}
);
graph->calc_reachability();
graph.unlock();
}
}
