Circular Obstacle Pathfinding
Pathfinding around a set of circular obstacles
Navigating a forest
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.
This is called a tangent visibility graph.
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:
[following
method](http://paulbourke.net/geometry/pointlineplane/)
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) \]
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 hugging edges
The graph nodes connect a surfing edge to a hugging edge. We
generated the surfing edges in the previous sections. To
generate the hugging edges we start at the endpoint of a
surfing edge, traverse around the circle, and terminate 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]\), and it has no arccosine.
Surfing edge line of sight
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.
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 clamp \(u\) to the range \([0,1]\):
\[E = A + clamp(u, 0, 1) * (B - A)\]
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.
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.
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.
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 \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.
The nodes \(P\) become nodes with a direction, either
clockwise \(P\circlearrowright\) or counterclockwise
\(P\circlearrowleft\).
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\).
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.
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.
Polygonal obstacles
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.
References
Belt problem
Pulley problem
Point line distance
Intersection of two circles