/** Get edge closest to a specified point. * The point must be within an imaginery line segment parallel to * the edge, that is a line perpendicular to the edge must go * through the point and a point on the edge line segment. * @param pos_x X coordinate in global (map) frame of point * @param pos_y X coordinate in global (map) frame of point * @return edge closest to the given point, or invalid edge if * such an edge does not exist. */ NavGraphEdge NavGraph::closest_edge(float pos_x, float pos_y) const { float min_dist = std::numeric_limits<float>::max(); NavGraphEdge rv; Eigen::Vector2f point(pos_x, pos_y); for (const NavGraphEdge &edge : edges_) { const Eigen::Vector2f origin(edge.from_node().x(), edge.from_node().y()); const Eigen::Vector2f target(edge.to_node().x(), edge.to_node().y()); const Eigen::Vector2f direction(target - origin); const Eigen::Vector2f direction_norm = direction.normalized(); const Eigen::Vector2f diff = point - origin; const float t = direction.dot(diff) / direction.squaredNorm(); if (t >= 0.0 && t <= 1.0) { // projection of the point onto the edge is within the line segment float distance = (diff - direction_norm.dot(diff) * direction_norm).norm(); if (distance < min_dist) { min_dist = distance; rv = edge; } } } return rv; }
/** Get the point on edge closest to a given point. * The method determines a line perpendicular to the edge which goes through * the given point, i.e. the point must be within the imaginary line segment. * Then the point on the edge which crosses with that perpendicular line * is returned. * @param x X coordinate of point to get point on edge for * @param y Y coordinate of point to get point on edge for * @return coordinate of point on edge closest to given point * @throw Exception thrown if the point is out of the line segment and * no line perpendicular to the edge going through the given point can * be found. */ cart_coord_2d_t NavGraphEdge::closest_point_on_edge(float x, float y) const { const Eigen::Vector2f point(x, y); const Eigen::Vector2f origin(from_node_.x(), from_node_.y()); const Eigen::Vector2f target(to_node_.x(), to_node_.y()); const Eigen::Vector2f direction(target - origin); const Eigen::Vector2f direction_norm = direction.normalized(); const Eigen::Vector2f diff = point - origin; const float t = direction.dot(diff) / direction.squaredNorm(); if (t >= 0.0 && t <= 1.0) { // projection of the point onto the edge is within the line segment Eigen::Vector2f point_on_line = origin + direction_norm.dot(diff) * direction_norm; return cart_coord_2d_t(point_on_line[0], point_on_line[1]); } throw Exception("Point (%f,%f) is not on edge %s--%s", x, y, from_.c_str(), to_.c_str()); }