View on GitHub

Circular Obstacle Pathfinding

Pathfinding around a set of circular obstacles

Mar 2017

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.

A* algorithm

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.

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.

Generating surfing edges

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.

Internal 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\).

External bitangents

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.

Line of sight

Taken together, the internal and external bitangents between two circles constitute surfing edges between the circles. 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:

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)} \]

Then compute the position \(E\) on \(\overline{AB}\):

\[ E = A + \mathrm{clamp}(u, 0, 1) * (B - A) \]

0 1 u={{Math.round(100*(C.x-A.x)/(B.x-A.x))/100}} d r

The distance \(d\) from \(C\) to segment \(\overline{AB}\) is the distance from \(C\) to \(E\): \[d = \|E - C\|\] Since \(d < r\), the circle blocks the line of sight from \(A\) to \(B\), and the edge should be thrown out. \(d \ge r\), there is line of sight from \(A\) to \(B\), and the edge should be kept. Try moving the circle to see the case where \(d \ge r\) \(d < r\).

Generating Nodes

Other that the start and end points, the graph’s nodes are the endpoints of the surfing edges. A node—a surfing edge endpoint—has several edges leading out of it: the surfing edge itself, plus all of the hugging edges generated by that surfing edge.

[Need to work on the flow]

Generating hugging edges

Each hugging edge starts at the endpoint of a surfing edge, traverses around the circle, and terminates at the endpoint of a different surfing edge.

To find the set of hugging edges for a circle, first find all the surfing edges that touch the circle. Then, create hugging edges between all the surfing edge endpoints on the circle.

Putting it all together

Given the generation of surfing edges, hugging edges and nodes, and the culling of blocked surfing edges, we can produce a graph and run pathfinding using the A* algorithm.

Enhancements

The graph generation procedure we've discussed works well enough for explaining the algorithm, but there are many ways in which it can be improved. These enhancements make the algorithm use less CPU and memory, and allow it to handle more cases. Let's take a look at a few.

Obstacles that touch

Perhaps you picked up on it—none of the circular obstacles in the examples given so far have overlapped or even touched. Allowing circles to touch makes the pathfinding problem a little, but not much, harder.

Bitangents

Recall that bitangents can be found using this formula for internal bitangents: \[\theta = \arccos{{r_A+r_B}\over{d}}\] and this one for external bitangents: \[\theta = \arccos{{\lvert r_A - r_B \rvert} \over d}\]

When two circles touch or overlap, there are no internal bitangents between them. In this case \({r_A+r_B}\over d\) is greater than one. Since arccosine is undefined for inputs outside its domain of \([-1, 1]\), it's important to check for circle overlap before performing the arccosine.

Likewise, if one circle completely encloses the other, then there are no external bitangents between the circles. In this case \({r_A - r_B} \over d\) is outside the range \([-1, 1]\). Again, because arccosine is undefined for such inputs, it's important to check for this case first.

[it's not important *because* arccoside is undefined; it's important because the bitangents don't exist]

Surfing edge line of sight - MERGE ABOVE

When obstacles are allowed to touch or overlap, new cases arise in calculating surfing edge line of sight. Recall the calculation of \(u\), the fraction of distance along the surfing edge at which a perpendicular ascender to the point touches the edge. When circles are not allowed to touch, a value of \(u\) outside \([0,1]\) means that the circle cannot touch the edge because to do so it would have to touch one of the endpoints of the edge. This is impossible, because the endpoints of the edge are already tangent to (and thus touching) other circles.

[Diagram of line of sight allowing overlapping circles.]

However, if circles are allowed to overlap, then values of \(u\) outside \([0,1]\) might block line of sight along the edge. This corresponds to cases where the circle is off the end of the surfing edge, but covering or touching an endpoint. To catch these cases mathematically, we need only clamp \(u\) to the range \([0,1]\). Thus, instead of: \[D = A + u * (B - A)\] we have: \[D = A + clamp(u, 0, 1) * (B - A)\] Everything else remains the same.

Hugging edge line of sight

When obstacles are allowed to touch or overlap, hugging edges can be blocked by obstacles just as surfing edges can. Consider the hugging edge in the diagram below. If another obstacle touches the hugging edge, it’s blocked and should be thrown out.

[Diagram with a fixed hugging edge between two points on the circle. A second circle can be dragged; if it touches the hugging edge, the edge gets grayed out.]

To determine whether a hugging edge is blocked by another obstacle, use the following method to determine the points at which the two circles intersect. Given circles centered on points \(A\) and \(B\) with radii \(r_A\) and \(r_B\), where \(d\) is the distance between \(A\) and \(B\), there are a few cases to check for first. If the circles are not touching (that is, \(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.

After making the changes for bitangent calculation and line of sight for surfing and hugging edges, everything else just works. Here is a demo of navigating a collection of obstacles that may overlap:

[Diagram: A* working on problem where circles are allowed to overlap.]

Variable actor radius via Minkowski expansion

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.

Delayed edge generation

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.

[Diagram showing which graph edges were even looked at.]

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.

The next case is when A* has just arrived at point \(p\) on 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.

The last case is when A* has traversed a hugging edge along obstacle \(C\) and needs to leave the \(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.

Cull cusped hugging 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 \longrightarrow 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.

  1. The nodes \(P\) become nodes with a direction, either clockwise \(P\circlearrowright\) or counterclockwise \(P\circlearrowleft\).
  2. The undirected surfing edges \(P \longleftrightarrow Q\) become directed edges \(P,p \longrightarrow Q,\hat{q}\) and \(Q,q \longrightarrow P,\hat{p}\), where \(p\) and \(q\) are directions, and \(\hat{x}\) means the opposite direction of \(x\).
  3. The undirected hugging edges \(P \longleftrightarrow Q\) become directed edges \(P\circlearrowright \longrightarrow Q\circlearrowright\) and \(P\circlearrowleft \longrightarrow Q\circlearrowleft\). This is where the filtering happens: we don't include \(P\circlearrowright \longrightarrow Q\circlearrowleft\) and \(P\circlearrowleft \longrightarrow Q\circlearrowright\), because changing direction introduces cusps\(\curlywedge\).

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.

[Diagram similar to previous one, with valid/invalid surfing/hugging edges?]

By filtering these cusped hugging edges out of the graph, we make the algorithm more efficient.

Crossing edge culling

Cull partial paths whose final surfing edge crosses the penultimate surfing edge.

[amit not sure what this means]

Polygonal obstacles

exercise for the reader

Spatial index

Full demo

References