Visibility Graphs and Euclidean Shortest Paths

Visibility Graphs and Euclidean Shortest Paths

Part 21 changed the shape of motion planning. After expanding each workspace obstacle by the reflected robot, a translating polygonal robot became a point moving among configuration-space obstacles. That reduction is powerful, but it leaves a question: once free space is known, how do we find the shortest collision-free route through it?

For polygonal obstacles in the plane, the classical answer is a visibility graph. Its vertices are the start, the goal, and relevant obstacle vertices. Two vertices are connected when the open segment between them lies in free space; the edge weight is Euclidean length. A shortest-path algorithm on this finite graph then solves a seemingly continuous problem.

Why a shortest path bends at vertices

Imagine pulling a string from start $s$ to goal $t$ around rigid polygonal obstacles. Wherever the string lies in open free space, a bend can be shortened by replacing its two incident segments with their chord. A locally shortest route is therefore straight except where an obstacle prevents straightening.

Could an optimal path turn in the relative interior of a polygon edge? For an ordinary polygonal obstacle, no. If the path touches an edge away from its endpoints and turns there, sliding the contact point slightly along the edge either shortens the two adjacent segments or reveals a straight continuation. A persistent change of direction needs the constraint supplied by a vertex. Collinear travel along an obstacle edge can be represented using its endpoints and does not add a new kind of bend.

Thus, under the standard model of closed polygonal obstacles and point motion, there exists a shortest path whose internal vertices are obstacle vertices. Consecutive vertices along that path must see one another; otherwise the connecting segment would enter an obstacle. The continuous optimum is consequently a path in the visibility graph.

This structural theorem has assumptions. Obstacles must be represented consistently, and a shortest route must exist in the chosen free-space convention. Curved boundaries can force tangency points that are not input vertices. Weighted regions can bend paths according to refraction-like rules. If clearance rather than length is the objective, medial-axis or Voronoi structure may be more appropriate. The visibility graph is exact for the polygonal Euclidean problem, not a universal planner.

Building the graph

Let the obstacle polygons have $n$ vertices in total. Create a graph node for every obstacle vertex plus $s$ and $t$. For each unordered pair $u,v$, test whether segment $uv$ is admissible. If it is, add an undirected edge of weight

$$w(u,v)=\lVert u-v\rVert_2.$$

The phrase “admissible” matters. The segment interior must not enter the interior of any forbidden polygon. Depending on the application, travel may be allowed along obstacle boundaries, forbidden on them, or allowed only at isolated contacts. Decide this once and make the line-of-sight predicate implement that decision.

A clear baseline checks every candidate segment against every obstacle edge and then checks whether a midpoint lies inside an obstacle when boundary overlaps or shared endpoints make crossings inconclusive. There are $O(n^2)$ candidate pairs and $O(n)$ edges to inspect, so this direct construction costs $O(n^3)$ time and may store $\Theta(n^2)$ visibility edges. It is often the right first implementation: easy to audit, useful as a test oracle, and fast enough for modest maps.

A robust line-of-sight contract

Segment intersection alone does not define visibility. Suppose $u$ is an obstacle vertex. A ray from $u$ that touches an adjacent edge obviously intersects that edge at $u$, yet many such rays are valid. Conversely, a segment joining two vertices of the same concave polygon may lie through the polygon interior even though it has no proper crossing with an edge. Collinear overlaps and vertex touches demand explicit classification.

A useful predicate distinguishes:

• proper crossing: the interiors of two segments cross; visibility is blocked;
• endpoint touch: contact occurs at one or both endpoints; inspect local obstacle sectors;
• collinear overlap: follow the boundary policy and test the neighboring free-space side;
• no intersection: still reject the candidate if an interior sample lies inside a forbidden polygon.

Use exact or filtered orient2d signs for combinatorial decisions. Avoid using a global epsilon to turn every near-zero determinant into collinearity: coordinates that are close are not necessarily topologically coincident. If input snapping is desired, perform it as a separate normalization step and record the resulting topology.

from math import hypot

def visible(u, v, obstacles, policy):
    """Predicates return symbolic classifications, not fuzzy booleans."""
    if u == v:
        return False
    candidate = (u, v)
    for polygon in obstacles:
        for edge in boundary_edges(polygon):
            hit = classify_segment_intersection(candidate, edge)
            if hit == "PROPER":
                return False
            if hit in ("TOUCH", "OVERLAP"):
                if not policy.contact_is_admissible(candidate, edge, polygon):
                    return False

    # Catches a same-polygon diagonal through the forbidden interior.
    midpoint = ((u[0] + v[0]) / 2, (u[1] + v[1]) / 2)
    return all(not strict_inside(midpoint, poly) for poly in obstacles)

def visibility_graph(vertices, obstacles, policy):
    graph = {u: [] for u in vertices}
    for i, u in enumerate(vertices):
        for v in vertices[i + 1:]:
            if visible(u, v, obstacles, policy):
                d = hypot(u[0] - v[0], u[1] - v[1])
                graph[u].append((v, d))
                graph[v].append((u, d))
    return graph

The midpoint check is explanatory, not sufficient for every contact case: a candidate can alternate between boundary and interior, or the midpoint itself can land on a boundary. Production code can split the candidate at all contact parameters and classify every open interval, or use local angular tests at touched vertices. Preserve a single authoritative point-in-polygon and segment-intersection policy throughout the pipeline.

Rotational sweep from one source

Testing all obstacle edges for every candidate repeats work. Fix a source vertex $p$, sort all other vertices by polar angle around $p$, and rotate a ray through them. Maintain an ordered set of obstacle edges intersected by the ray, keyed by their distance from $p$. A target $q$ is visible when no active edge blocks segment $pq$, with special handling for edges incident to $p$ or $q$ and for equal-angle events.

Events at the same angle must be processed as a group. Otherwise a ray passing through a near vertex toward a farther vertex may remove an edge too early or insert one too late. A safe pattern is to classify all incident edges as entering, leaving, or collinear with respect to the infinitesimally rotated ray; test targets against the appropriate state; then apply updates in a documented order. The sweep reduces repeated intersection work, but it concentrates degeneracy handling in the event comparator and active-set order.

From visibility to a shortest route

Once the graph is built, run Dijkstra’s algorithm from $s$. Nonnegative Euclidean weights satisfy its assumptions, and predecessor pointers reconstruct the polyline. If only one goal matters, A* can prioritize a node $v$ with

$$f(v)=g(v)+h(v),\qquad h(v)=\lVert v-t\rVert_2.$$

Straight-line distance is admissible because no obstacle-avoiding continuation can be shorter than the direct segment. It is also consistent by the triangle inequality. A* therefore returns an optimal graph path and often explores fewer vertices, though it does not reduce graph-construction cost. For many queries in one static environment, preprocess obstacle-to-obstacle visibility once, then connect each new start and goal to visible graph nodes. Multi-source shortest paths or more specialized shortest-path maps may pay off at high query volumes.

From a point to a polygonal robot

The route planner should not test only the robot’s reference point against workspace obstacles. For a translating robot shape $R$, Part 21 replaces each obstacle $O$ by the configuration obstacle

$$C_O=O\oplus(-R).$$

Build the visibility graph around the boundaries of these expanded obstacles, and treat the start and goal as positions of the same chosen reference point in $R$. A path for that point in configuration space lifts directly to a collision-free translation of the entire robot in workspace.

Expansion can make previously separate obstacles overlap. Union the configuration obstacles, or at least compute the boundary of their union, before constructing visibility. Interior vertices and edges of overlapping expanded polygons are not meaningful route features. For a nonconvex robot or obstacle, Minkowski sums may be assembled from convex pieces, but the final free-space topology—not the decomposition seams—must determine visibility.

Rotation changes the problem qualitatively. A planar rigid body with orientation has a three-dimensional configuration space $(x,y,\theta)$; a 2D visibility graph over expanded obstacles at one fixed angle is no longer complete. One may discretize orientation with conservative transitions, construct a richer cell complex, or use a sampling-based planner. Do not silently apply the translational proof to a rotating robot.

Grazing, holes, and other boundary cases

A route that exactly grazes a vertex can be mathematically shortest yet physically fragile. Choose among three contracts:

1. Closed obstacles, contact forbidden: free space is open, so the infimum path may touch a boundary without an attained minimum. Inflate obstacles by a clearance radius and plan against the closed inflated set.
2. Contact allowed: boundary-following visibility edges are admissible, but local sectors must still prevent cutting through an obstacle at a reflex vertex.
3. Symbolic clearance: compute the contact path as a limit and offset it during execution under a certified error budget.

Polygonal domains with holes need orientation-independent containment logic. A “hole” in an obstacle is free space, while an island inside that hole is forbidden again. Represent the domain as a planar subdivision or containment tree rather than assuming every polygon ring is an independent solid. Visibility across a free hole is valid; visibility through the surrounding solid is not.

Repeated vertices, zero-length edges, overlapping boundaries, and start or goal on a boundary should be normalized or rejected according to an input contract. After graph search, validate every path segment against the original authoritative obstacle representation, verify endpoints and continuity, and recompute length. This last pass catches mismatches between construction predicates, graph bookkeeping, and configuration-space approximation.

Choosing the method responsibly

Visibility graphs expose a useful engineering split. Geometry determines the finite set of legal straight motions; graph search chooses the best combination. Either half can fail independently. A perfectly implemented Dijkstra cannot rescue a missing visibility edge, and a flawless graph is wasted if its contact convention disagrees with collision checking.

Part 23 turns that observation into a broader discipline: choosing exact versus approximate predicates, static versus dynamic structures, deterministic versus randomized algorithms, and then testing those choices against adversarial geometry. The shortest-path theorem gives us confidence in the model; careful engineering determines whether the implementation deserves the same confidence.