Circular Obstacle Pathfinding (2017)

A* Pathfinding on Graphs

The A* pathfinding algorithm is a powerful method for quickly generating optimal paths. Typically, people demonstrate A* navigating grid-based maps, but A* isn't just a grid algorithm! It can work on any graph. We can use A* to find a path through this world of round obstacles. How does the same algorithm solve both problems?

Let's start with a review of how A* works. The A* algorithm finds the optimal path from the start point to the end point, avoiding obstacles along the way. It does this by gradually expanding a set of partial paths. Each partial path is a series of steps from the start point to some intermediate point on the way to the goal. As A* progresses, the partial paths get closer and closer to the goal point. The algorithm terminates once it finds a complete path that it can prove to be better than any of the remaining possibilities.

At each step in the algorithm, A* evaluates the set of partial paths and generates some new paths by expanding the most promising path in the set. To do this, A* keeps the partial paths in a priority queue, sorted by estimated length-the actual measured length of the path so far, plus a guess of the remaining distance to the goal. This guess must be an underestimate; that is, the guess can be less than the actual distance, but not greater. In most pathfinding problems, a good underestimate is the geometric straight-line distance from the end of the partial path to the goal. The actual best path to the goal from the end of the partial path might be longer than this straight line distance, but it can't be shorter.

When A* begins, the priority queue contains just one partial path: the start point. The algorithm works by repeatedly removing the most promising path from the priority queue, that is, the path with the smallest estimated length. If this path ends at the goal point, the algorithm is done-the priority queue ensures that no other path could possibly be better. Otherwise, starting from the end of the partial path it removed from the queue, A* generates some new paths by taking single steps in all possible directions. It places these new paths back into the priority queue and begins the process again.

Converting Obstacles to a Graph

A* works on a graph: a collection of nodes connected by edges. In a grid-based world, each node represents an individual grid location, while each edge represents a connection to a neighboring location to the north, south, east or west. Before A* can run on the forest of round obstacles, we need to convert it into a graph.

All the paths through the forest consist of alternating line segments and arc sections. These are edges in the path graph. The endpoints of these edges become nodes. A path through the graph is a series of nodes connected by edges: Both segments and arcs act as edges in the graph. We'll call the segments surfing edges, because the path uses them to surf between obstacles. The arcs we'll call hugging edges, as their purpose in the path is to hug the sides of the obstacles.

Next we'll explore a simple way to turn the obstacle forest into a graph: generate all of the possible surfing and hugging edges. This is called a tangent visibility graph.

Surfing Edges: Bitangents

The surfing edges between a pair of circles are the line segments which just barely kiss both circles; these segments are known as bitangents, and there are four of them for each pair of circles. The bitangents which cross between the circles are the internal bitangents, while the ones which go along the outside are the external bitangents.

Historically, internal bitangents were important for calculating the length of a belt which crosses over two different sized pulleys, and so the problem of constructing internal bitangents is known as the belt problem. To find the internal bitangents, calculate the angle (\theta) in the diagram below. It turns out that, given circles centered on points (A) and (B) with radii (r_A) and (r_B), and centers separated by distance (d):

[
\theta = \arccos{{r_A+r_B}\over{d}}
]

Once (\theta) is known, it's easy to find points (C), (D), (E) and (F).

Constructing external bitangents-the pulley problem-uses a similar technique. For external bitangents we can find (\theta) like this:

[
\theta = \arccos{{\lvert r_A - r_B \rvert} \over d}
]

It doesn't matter whether circle A or B is bigger, but as shown in the diagram, (\theta) appears on the side of A toward B, but on the side of B away from A.

Taken together, the internal and external bitangents between two circles constitute surfing edges between the circles.

Line of Sight for Surfing Edges

But what if a third circle blocks one or more of the surfing edges? If a surfing edge is blocked by another circle, we need to throw the edge out. To detect this case, we use a simple point-line-distance calculation. If the distance from the surfing edge to the obstacle's center is less than the obstacle's radius, then the obstacle blocks the surfing edge, and we should throw the edge away.

To calculate the distance from point (C) to line segment (\overline{AB}), use the following method:

1. First, compute (u), the fraction of the distance along segment (\overline{AB}) at which a perpendicular ascender hits point (C):
   [
   u = {(C - A) \cdot (B-A) \over (B-A)\cdot(B-A)}
   ]

2. Then compute the position (E) on (\overline{AB}):
   [
   E = A + \mathrm{clamp}(u, 0, 1) * (B - A)
   ]

3. The distance (d) from (C) to segment (\overline{AB}) is the distance from (C) to (E):
   [
   d = |E - C|
   ]

Since (d < r) means the obstacle blocks the surfing edge, we discard the edge in that case.

Hugging Edges

The hugging edges are the arcs along the surface of an obstacle between two surfing edges. To generate hugging edges, we need to know where the surfing edges touch the obstacle. For each pair of surfing edges that touch the same obstacle, we can create a hugging edge connecting them.

However, we must also check whether other obstacles block these hugging edges. For each hugging edge, we need to determine if any other obstacle intersects the arc. If the circles do not intersect ((d > r_A + r_B)), if one circle is inside the other ((d < |r_A - r_B|)), or the circles are coincident ((d = 0) and (r_A = r_B)), then the circles cannot interfere with each others' hugging edges.

If none of these cases hold, then the circles intersect at two points-if the circles are tangent to each other, these two points are coincident. Consider the radical line which connects the intersection points; it's perpendicular to the line connecting (A) and (B) at some point (C). We can calculate the distance (a) from (A) to (C) as follows:

[
a = {r_A^2 - r_B^2 + d^2 \over 2d }
]

Having found (a), we can find the angle (\theta):

[
\theta = \arccos {a \over r_A}
]

If (\theta) is zero, the circles are tangent at (C). Otherwise, there are two intersection points, corresponding to positive and negative (\theta).

Next, determine whether either of the intersection points fall between the start and end points of the hugging edge. If this is the case, then the obstacle blocks the hugging edge, and we should discard the edge. Note that we don't have to worry about the case where the hugging edge is entirely contained within an obstacle, as the line of sight culling for surfing edges will have already thrown the edge out.

Making It Work

After making the changes for bitangent calculation and line of sight for surfing and hugging edges, everything else just works.

When navigating a round object through a world of round obstacles, we can make a few observations that simplify the problem. First, we can make things easier by noticing that moving a circle of radius (r) through a forest of round obstacles is identical to moving a point through that same forest with one change: each obstacle has its radius increased by (r). This is an extremely simple application of Minkowski addition. If it has a radius larger than zero, we'll just increase the size of the obstacles before we start.

Performance Considerations

In general, the graph for a forest of (n) obstacles contains (O(n^2)) surfing edges, but since each of these must be checked for line of sight against (n) obstacles, the total time to generate the graph is (O(n^3)). Additionally, pairs of surfing edges can induce hugging edges, and each of these must be checked against every obstacle for line of sight.

However, because the A* algorithm is so efficient, it normally looks at only a fraction of this large graph to produce an optimal path. We can save time by generating small portions of the graph on the fly as we proceed through the A* algorithm, rather than doing all of the work up front. If A* finds a path quickly, we'll generate only a small part of the graph.

We do this by moving edge generation to the neighbors() function. There are several cases:

• At the beginning of the algorithm, we need the neighbors of the start point. These are surfing edges from the start point to the left and right edges of each obstacle.
• When A* has just arrived at point (p) on the edge of obstacle (C) along a surfing edge: neighbors() needs to return the hugging edges leading from (p). To do this, determine which surfing edges leave the obstacle by computing the bitangents between (C) and every other obstacle, throwing away any that do not have line of sight. Then find all the hugging edges that connect (p) to these surfing edges, discarding those that are blocked by other obstacles. Return all of these hugging edges, saving the surfing edges for return in a subsequent neighbors() call.
• When A* has traversed a hugging edge along obstacle (C) and needs to leave (C) via a surfing edge: because the previous step calculated and saved all the surfing edges, the correct set of edges can just be looked up and returned.

Culling Cusped Edges

Hugging edges connect surfing edges which touch the same circle, but it turns out that many of these hugging edges are not eligible to be used in any optimal path. We can speed up the algorithm by eliminating them.

An optimal path through the forest of obstacles always consists of alternating surfing and hugging edges. Suppose we're entering at node (A) and are trying to decide how to exit: Entering through (A) means we're going clockwise (\circlearrowright). We must exit through a node that keeps us going clockwise (\circlearrowright), so we can only exit through node (B) or (D). Exiting through (C) creates a cusp (\curlywedge) in the path, which will never be optimal.

We want to filter out these cusped edges. First note that A* already treats each undirected edge (P \longleftrightarrow Q) as two directed edges, (P \longrightarrow Q) and (Q \longrightarrow P). We can take advantage of this by annotating the edges and nodes with directions.

In our diagram, node (A) would become two nodes, (A\circlearrowright) and (A\circlearrowleft), and have an incoming surfing edge (\longrightarrow A\circlearrowright) and an outgoing surfing edge (A\circlearrowleft \longrightarrow). If the path entered through (A\circlearrowright) then it must exit through a (\circlearrowright) node, which would be either the (B\circlearrowright \longrightarrow) surfing edge (via the (A\circlearrowright \longrightarrow B\circlearrowright) hugging edge) or the (D\circlearrowright \longrightarrow) surfing edge (via the (A\circlearrowright \longrightarrow D\circlearrowright) hugging edge). It can't leave through (C\circlearrowleft \longrightarrow) because it changes rotation direction, and we have filtered out the (A\circlearrowright \longrightarrow C\circlearrowleft) hugging edge.

By filtering these cusped hugging edges out of the graph, we make the algorithm more efficient. Cull partial paths whose final surfing edge crosses the penultimate surfing edge. See Game Programming Gems 2, Chapter 3.10, Optimizing Points-of-Visibility Pathfinding by Thomas Young. It covers the node culling but for polygons instead of circles.