Visibility Graph, in depth: exact shortest paths among polygonal obstacles, construction, robust visibility tests, A* search and configuration space

Why shortest paths live on obstacle vertices

The key fact is a theorem from computational geometry: among polygonal obstacles in the plane, any shortest path from s to g is a polygonal chain whose interior bend points are obstacle vertices. The intuition is a taut string. If a path bends anywhere in free space, you can cut the corner with a short straight segment and get something strictly shorter, so a shortest path only bends where it is pressed against an obstacle, and with polygons that happens only at a vertex. Between bends the path is a straight segment in free space, which is exactly a visibility edge.

A sharper version helps the implementation. The string can only wrap around a vertex that points into free space, a convex vertex of the obstacle (a reflex vertex of the free space). Concave vertices of an obstacle, the inside corners of a U shape, can never be bend points, because the string would pull away from them. So the shortest path lives in the visibility graph, and Dijkstra on that graph with Euclidean edge weights finds it exactly. The graph turns a continuous problem into a discrete one with no loss of optimality.

The graph, defined

Formally, let O be a set of disjoint simple polygons with n vertices in total. The nodes V are s, g and the polygon vertices. An edge (u, v) exists when the open segment uv does not intersect the interior of any polygon. Segments may touch obstacle boundaries: an edge along an obstacle side is valid, and so is a segment that grazes a vertex. Each edge weighs its Euclidean length. The diagram shows the smallest interesting case, one square between start and goal.

Building it: the O(n^3) method

The direct construction tests every pair of nodes against every obstacle edge. With n vertices there are O(n^2) pairs and n edges to test, so it costs O(n^3). That sounds slow, but for a few hundred vertices it runs in well under a second in compiled code and is the right first implementation: simple, easy to test, and a reference for faster versions.

Two tests decide visibility. The first is whether segment uv properly crosses any obstacle edge, using the orientation predicate: the cross product sign tells you which side of a line a point is on, and two segments properly cross when each one's endpoints lie strictly on opposite sides of the other. The second catches segments that cross no edge yet still pass through an interior, such as a diagonal between two non-adjacent vertices of the same polygon, or a segment that passes exactly through a vertex into the inside. Testing whether the segment's midpoint lies strictly inside any polygon handles the diagonal case; points where the segment touches a vertex need the sub-segments on each side checked too, which the code below does by splitting at touched vertices.

from itertools import combinations
from math import dist

def orient(a, b, c):
    """> 0 if a, b, c turn left, < 0 if right, 0 if collinear."""
    return (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0])

def proper_cross(p1, p2, q1, q2):
    d1, d2 = orient(q1, q2, p1), orient(q1, q2, p2)
    d3, d4 = orient(p1, p2, q1), orient(p1, p2, q2)
    return d1 * d2 < 0 and d3 * d4 < 0

def on_open_segment(a, b, p):
    return (orient(a, b, p) == 0 and p != a and p != b
            and min(a[0], b[0]) <= p[0] <= max(a[0], b[0])
            and min(a[1], b[1]) <= p[1] <= max(a[1], b[1]))

def strictly_inside(poly, p):
    """Ray casting; points on the boundary count as outside."""
    inside = False
    for i in range(len(poly)):
        a, b = poly[i], poly[(i + 1) % len(poly)]
        if p == a or on_open_segment(a, b, p):
            return False
        if (a[1] > p[1]) != (b[1] > p[1]):
            x = a[0] + (p[1] - a[1]) * (b[0] - a[0]) / (b[1] - a[1])
            if x > p[0]:
                inside = not inside
    return inside

def visible(u, v, polygons, vertices):
    edges = [(P[i], P[(i + 1) % len(P)]) for P in polygons for i in range(len(P))]
    if any(proper_cross(u, v, a, b) for a, b in edges):
        return False
    # split at vertices lying on the segment, then test each piece's midpoint
    cuts = sorted([w for w in vertices if on_open_segment(u, v, w)], key=lambda w: dist(u, w))
    pts = [u, *cuts, v]
    for a, b in zip(pts, pts[1:]):
        mid = ((a[0] + b[0]) / 2, (a[1] + b[1]) / 2)
        if any(strictly_inside(P, mid) for P in polygons):
            return False
    return True

def visibility_graph(s, g, polygons):
    vertices = [v for P in polygons for v in P]
    nodes = [s, g, *vertices]
    adj = {v: [] for v in nodes}
    for u, v in combinations(nodes, 2):
        if visible(u, v, polygons, vertices):
            w = dist(u, v)
            adj[u].append((v, w))
            adj[v].append((u, w))
    return adj

Searching it with A*

Searching the graph is ordinary shortest-path work. Dijkstra is correct because all weights are non-negative; A* with the straight-line distance to the goal as heuristic is better, because Euclidean distance is admissible and consistent in this setting (no path can be shorter than the straight line, and the triangle inequality holds edge by edge). A* typically expands far fewer nodes when the goal is not boxed in.

import heapq
from math import dist

def a_star(adj, s, g):
    best = {s: 0.0}
    parent = {s: None}
    frontier = [(dist(s, g), 0.0, s)]
    while frontier:
        _, d, u = heapq.heappop(frontier)
        if u == g:
            path = []
            while u is not None:
                path.append(u)
                u = parent[u]
            return d, path[::-1]
        if d > best[u]:
            continue                      # stale heap entry
        for v, w in adj[u]:
            nd = d + w
            if nd < best.get(v, float("inf")):
                best[v], parent[v] = nd, u
                heapq.heappush(frontier, (nd + dist(v, g), nd, v))
    return float("inf"), []

Worked example

Take the diagram's setup: s = (0, 2), g = (6, 2), and a square with corners A = (2, 1), B = (4, 1), C = (4, 3), D = (2, 3). The segment s-g runs along y = 2 through the square's interior, so it fails visibility. From s, the segments to A and D touch only their endpoints and are visible; the segments to B and C cross the square's left side, so they fail. Symmetrically, g sees B and C. The four square sides are edges, and the diagonals A-C and B-D cross no edge but their midpoints (3, 2) lie inside the square, so the midpoint test rejects them. That is exactly the dashed edge set in the figure.

The candidate paths are s-D-C-g and s-A-B-g. Each has length sqrt(4 + 1) + 2 + sqrt(4 + 1) = 2 x 2.236 + 2 = 6.472, against 6.0 for the blocked straight line. A* pops s, then D and A with f = 2.236 + 4.123, then C or B, and reaches g at 6.472, returning one of the two tied paths. Notice the path runs exactly along the square's top side: it touches the obstacle, which is the mathematically shortest route and a practical problem, discussed below.

Faster construction and the reduced graph

Faster constructions exist. Lee's rotational sweep, from 1978, handles each node v in turn: sort the other vertices by angle around v, then rotate a ray from v through them while a balanced search tree holds the obstacle edges the ray currently crosses, ordered by distance from v. A vertex is visible exactly when no stored edge lies between it and v, which is a check of the tree's closest element. Each node costs O(n log n), so the whole graph costs O(n^2 log n). Later work brought this to O(n^2) worst case, which matches the size of the output in the worst case, and output-sensitive algorithms such as Ghosh and Mount's run in O(n log n + E), where E is the number of edges; they pay off when the graph is sparse.

In practice the bigger win is building less. The reduced visibility graph keeps only edges that could be on a shortest path: an edge between obstacle vertices u and v is needed only if the line through them is tangent to both obstacles at those vertices (it does not cut into either obstacle locally; edges along obstacle sides are kept too), and only convex obstacle vertices are kept as nodes. That pruning routinely removes most edges. For queries with changing starts and goals, build the obstacle-to-obstacle graph once and, per query, compute only the edges from s and g, which costs O(n log n) per endpoint with a single rotational sweep.

Robots with size: configuration space

A real robot has a size. The standard trick is configuration space: shrink the robot to a point and grow every obstacle by the robot's shape. For a translating convex robot the grown obstacle is the Minkowski sum of the obstacle and the reflected robot, which for convex polygons is computable in linear time by merging edge directions. A disc-shaped robot of radius r grows obstacles by r in every direction, which rounds corners into arcs; the usual approximation replaces each arc with a few polygon edges, slightly overestimating the obstacle so the path stays safe. After growing, overlapping obstacles must be merged with a polygon union, or the visibility tests will see gaps that are not there.

Inflation also solves the touching problem from the worked example: a path that grazes the inflated boundary keeps the robot exactly r from the real obstacle. Add a safety margin to r for sensor and control error, since the shortest path otherwise runs every corner at the minimum legal distance.

Failure modes

Trade-offs and alternatives

Visibility graphs give exact shortest paths in 2D with polygonal obstacles and a modest vertex count, and they produce few waypoints, which suits robots that drive straight lines. Their costs are quadratic edge counts in dense maps, paths that hug obstacles, and no notion of terrain cost. Grids with A* handle weighted terrain and are trivial to update, at the cost of zigzag paths that are longer than the true optimum; any-angle variants such as Theta* recover most of the gap. Navigation meshes, the default in games, decompose free space into convex cells and search those, with smoothing to approximate shortest paths and far fewer nodes in large open levels. In three dimensions the picture changes completely: shortest paths among polyhedra can bend on edges, not just vertices, and the exact problem is NP-hard (Canny and Reif, 1987), which is why 3D and high-dimensional planning uses sampling planners such as PRM and RRT.

To go deeper, read A* search in depth, Dijkstra's algorithm in depth, the robust orientation predicate, segment intersection and 2D geometry fundamentals.

What to do next

1. Implement the O(n^3) builder above and the midpoint and vertex-splitting tests, then write unit tests for touching edges, collinear vertices and polygon diagonals.
2. Use integer coordinates or a robust orientation predicate before trusting results on real map data.
3. Inflate obstacles for your robot's footprint plus a safety margin, and merge overlaps with a polygon union.
4. Search with A* and the Euclidean heuristic; compare its expansions with Dijkstra on your maps.
5. Prune to the reduced visibility graph and precompute obstacle edges; connect s and g per query with a rotational sweep.
6. Benchmark against grid A*, Theta* and a navigation mesh on your own environments before committing, and move to sampling planners if you need 3D.