Exemple #1
0
/** 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;
}
Exemple #2
0
/** 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());
}