/** 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(); } }