/** Get node closest to another node with a certain property.
* @param node_name the name of the node from which to start
* @param consider_unconnected consider unconnected node for the search
* of the closest node
* @param property property the node must have to be considered,
* empty string to not check for any property
* @return node closest to the given point in the global frame, or an
* invalid node if such a node cannot be found. The node will obviously
* not be the node with the name @p node_name.
*/
NavGraphNode
NavGraph::closest_node_to(const std::string &node_name, bool consider_unconnected,
const std::string &property) const
{
NavGraphNode n = node(node_name);
std::vector<NavGraphNode> nodes = search_nodes(property);
float min_dist = HUGE;
std::vector<NavGraphNode>::iterator i;
std::vector<NavGraphNode>::iterator elem = nodes.begin();
for (i = nodes.begin(); i != nodes.end(); ++i) {
if (! consider_unconnected && i->unconnected()) continue;
float dx = i->x() - n.x();
float dy = i->y() - n.y();
float dist = sqrtf(dx * dx + dy * dy);
if ((sqrtf(dx * dx + dy * dy) < min_dist) && (i->name() != node_name)) {
min_dist = dist;
elem = i;
}
}
if (elem == nodes.end()) {
return NavGraphNode();
} else {
return *elem;
}
}
/** Get a specified node.
* @param name name of the node to get
* @return the node representation of the searched node, if not
* found returns an invalid node.
*/
NavGraphNode
NavGraph::node(const std::string &name) const
{
std::vector<NavGraphNode>::const_iterator n =
std::find_if(nodes_.begin(), nodes_.end(),
[&name](const NavGraphNode &node) {
return node.name() == name;
});
if (n != nodes_.end()) {
return *n;
} else {
return NavGraphNode();
}
}
/** Connect node to closest edge
* @param n node to connect to closest node
*/
void
NavGraph::connect_node_to_closest_edge(const NavGraphNode &n)
{
NavGraphEdge closest = closest_edge(n.x(), n.y());
cart_coord_2d_t p = closest.closest_point_on_edge(n.x(), n.y());
NavGraphNode closest_conn = closest_node(p.x, p.y);
NavGraphNode cn;
if (almost_equal(closest_conn.distance(p.x, p.y), 0.f, 2)) {
cn = closest_conn;
} else {
cn = NavGraphNode(NavGraph::format_name("C-%s", n.name().c_str()), p.x, p.y);
}
if (closest.from() == cn.name() || closest.to() == cn.name()) {
// we actually want to connect to one of the end nodes of the edge,
// simply add the new edge and we are done
NavGraphEdge new_edge(cn.name(), n.name());
new_edge.set_property("generated", true);
new_edge.set_property("created-for", cn.name() + "--" + n.name());
add_edge(new_edge);
} else {
// we are inserting a new point into the edge
remove_edge(closest);
NavGraphEdge new_edge_1(closest.from(), cn.name());
NavGraphEdge new_edge_2(closest.to(), cn.name());
NavGraphEdge new_edge_3(cn.name(), n.name());
new_edge_1.set_properties(closest.properties());
new_edge_2.set_properties(closest.properties());
new_edge_3.set_property("created-for", cn.name() + "--" + n.name());
new_edge_3.set_property("generated", true);
if (! node_exists(cn)) add_node(cn);
add_edge(new_edge_1);
add_edge(new_edge_2);
add_edge(new_edge_3);
}
}
void
NavGraph::edge_add_split_intersection(const NavGraphEdge &edge)
{
std::list<std::pair<cart_coord_2d_t, NavGraphEdge>> intersections;
const NavGraphNode &n1 = node(edge.from());
const NavGraphNode &n2 = node(edge.to());
try {
for (const NavGraphEdge &e : edges_) {
cart_coord_2d_t ip;
if (e.intersection(n1.x(), n1.y(), n2.x(), n2.y(), ip)) {
// we need to split the edge at the given intersection point,
// and the new line segments as well
intersections.push_back(std::make_pair(ip, e));
}
}
std::list<std::list<std::pair<cart_coord_2d_t, NavGraphEdge> >::iterator> deletions;
for (auto i1 = intersections.begin(); i1 != intersections.end(); ++i1) {
const std::pair<cart_coord_2d_t, NavGraphEdge> &p1 = *i1;
const cart_coord_2d_t &c1 = p1.first;
const NavGraphEdge &e1 = p1.second;
const NavGraphNode &n1_from = node(e1.from());
const NavGraphNode &n1_to = node(e1.to());
for (auto i2 = std::next(i1); i2 != intersections.end(); ++i2) {
const std::pair<cart_coord_2d_t, NavGraphEdge> &p2 = *i2;
const cart_coord_2d_t &c2 = p2.first;
const NavGraphEdge &e2 = p2.second;
if (points_different(c1.x, c1.y, c2.x, c2.y)) continue;
float d = 1.;
if (e1.from() == e2.from() || e1.from() == e2.to()) {
d = point_dist(n1_from.x(), n1_from.y(), c1.x, c1.y);
} else if (e1.to() == e2.to() || e1.to() == e2.from()) {
d = point_dist(n1_to.x(), n1_to.y(), c1.x, c1.y);
}
if (d < 1e-4) {
// the intersection point is the same as a common end
// point of the two edges, only keep it once
deletions.push_back(i1);
break;
}
}
}
for (auto d = deletions.rbegin(); d != deletions.rend(); ++d) {
intersections.erase(*d);
}
if (intersections.empty()) {
NavGraphEdge e(edge);
e.set_property("created-for", edge.from() + "--" + edge.to());
add_edge(e, EDGE_FORCE);
} else {
Eigen::Vector2f e_origin(n1.x(), n1.y());
Eigen::Vector2f e_target(n2.x(), n2.y());
Eigen::Vector2f e_dir = (e_target - e_origin).normalized();
intersections.sort([&e_origin, &e_dir](const std::pair<cart_coord_2d_t, NavGraphEdge> &p1,
const std::pair<cart_coord_2d_t, NavGraphEdge> &p2)
{
const Eigen::Vector2f p1p(p1.first.x, p1.first.y);
const Eigen::Vector2f p2p(p2.first.x, p2.first.y);
const float k1 = e_dir.dot(p1p - e_origin);
const float k2 = e_dir.dot(p2p - e_origin);
return k1 < k2;
});
std::string en_from = edge.from();
cart_coord_2d_t ec_from(n1.x(), n1.y());
std::string prev_to;
for (const auto &i : intersections) {
const cart_coord_2d_t &c = i.first;
const NavGraphEdge &e = i.second;
// add intersection point (if necessary)
NavGraphNode ip = closest_node(c.x, c.y);
if (! ip || points_different(c.x, c.y, ip.x(), ip.y())) {
ip = NavGraphNode(gen_unique_name(), c.x, c.y);
add_node(ip);
}
// if neither edge end node is the intersection point, split the edge
if (ip.name() != e.from() && ip.name() != e.to()) {
NavGraphEdge e1(e.from(), ip.name(), e.is_directed());
NavGraphEdge e2(ip.name(), e.to(), e.is_directed());
remove_edge(e);
e1.set_properties(e.properties());
e2.set_properties(e.properties());
add_edge(e1, EDGE_FORCE, /* allow existing */ true);
add_edge(e2, EDGE_FORCE, /* allow existing */ true);
// this is a special case: we might intersect an edge
// which has the same end node and thus the new edge
// from the intersection node to the end node would
// be added twice
prev_to = e.to();
}
// add segment edge
if (en_from != ip.name() && prev_to != ip.name()) {
NavGraphEdge e3(en_from, ip.name(), edge.is_directed());
e3.set_property("created-for", en_from + "--" + ip.name());
add_edge(e3, EDGE_FORCE, /* allow existing */ true);
}
en_from = ip.name();
ec_from = c;
}
if (en_from != edge.to()) {
NavGraphEdge e3(en_from, edge.to(), edge.is_directed());
e3.set_property("created-for", en_from + "--" + edge.to());
add_edge(e3, EDGE_FORCE, /* allow existing */ true);
}
}
} catch (Exception &ex) {
throw Exception("Failed to add edge %s-%s%s: %s",
edge.from().c_str(), edge.is_directed() ? ">" : "-", edge.to().c_str(),
ex.what_no_backtrace());
}
}
/** 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();
}
}
