[Translation] Search for the way among the round obstacles

[Translation] Search for the way among the round obstacles


Forest navigation


The pathfinding algorithm A * is a powerful tool for fast generation of optimal paths. Usually, A * is demonstrated when navigating maps from grids, but it can be used not only for grids! It can work with any graphs. You can use A * to find the way in the world of round obstacles.


In original article all images are interactive.

How does one algorithm solve both these problems? Let's start with a brief description of how A * works.

Algorithm A *


Algorithm A * finds the best path from the starting point to the ending point, avoiding obstacles along the way. He realizes this by gradually expanding many partial paths . Each partial path is a series of steps from the starting point to some intermediate point on the road to the goal. In the process of working A *, partial paths are getting closer to the end point. The algorithm stops working when it finds the full path that is better than the remaining options, and this can be proved.

At each step of the algorithm, A * evaluates many partial paths and generates new paths, extending the most promising of many paths. For this, A * stores partial paths in a priority queue, sorted by approximate length —the true measured path length plus the approximate remaining distance to the target. This approximation should be an underestimation ; that is, the approximation may be less than the true distance, but not more. In most pathfinding tasks, a good understated estimate is the geometric distance in a straight line from the end of the partial path to the end point. The true best path to a target from the end of a partial path may be longer than this straight line, but cannot be shorter.

When A * starts, the priority queue contains only one partial path: the starting point. The algorithm repeatedly deletes the most promising path from the priority queue, that is, the path with the shortest approximate length. If this path ends at the end point, then the algorithm has completed the task — the priority queue ensures that no other path can be better. Otherwise, starting from the end of the partial path that he deleted from the queue, A * generates several more new paths, making single steps in all possible directions. He puts these new paths back into the priority queue and starts the process again.

Count


A * works with the graph : a set of nodes connected by edges . In a grid-based world, each node represents a separate grid cell, and each edge represents a connection with neighboring cells to the north, south, east, or west.

Before you can run A * for a forest of round obstacles, we need to convert it to a graph. All paths through the forest consist of alternating segments of straight lines and arcs. They are the edges of the path graph. The end points of these edges become nodes.

The path through the graph is a series of nodes connected by edges:


Both segments and arcs are edges of a graph. We will call the transition edges because the path uses them to transition between obstacles. The arcs will be called edge envelopes , because their task along the way is to round the sides of the obstacles.

Next, we explore a simple way to transform a forest with obstacles into a graph: we will generate all possible transition edges and envelope edges.


Transition edge generation


The edges of the transition between a pair of circles are segments of straight lines that barely touch both circles; such segments are called tangents to two points , and for each pair of circles there are four such tangents. The tangents to two points intersecting between the circles are called internal tangents to two points , and those passing outside are called external .

Internal tangents to two points


In the past, the search for internal tangents was important for calculating the length of a belt connecting two pulleys of different sizes, so the task of creating internal tangents to two points is called the belt task . To find the inner tangents to two points, you need to calculate the angle $ \ theta $ in the drawing below.


As it turned out, for circles with centers $ A $ and  $ B $ and radii  $ r_A $ and  $ r_B $ whose centers are  $ d $ :

$ \ theta = \ arccos {{r_A + r_B} \ over {d}} $


Having defined $ \ theta $ we can easily find the points $ C $ ,  $ D $ ,  $ E $ and  $ F $ .

Outer tangents to two points


When constructing external tangents (this is the pulley problem ), a similar technique is used.

For external tangents, we can find $ \ theta $ as follows:

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


It does not matter which of the circles is larger, A or B, but as can be seen from the figure, $ \ theta $ is deposited on side A, closest to B, but on side B, furthest from A.

Line of sight


Taken together, the inner and outer tangents to the two points between the two circles form the edges of the transition between the circles. But what if one or more transition edges closes the third circle?


If the edge of the transition is blocked by another circle, then we need to drop this edge. To recognize this case, we use a simple distance between a point and a line . If the distance from the transition edge to the center of the obstacle is less than the radius of the obstacle, then the obstacle overlaps the transition edge, therefore we must discard it.

To calculate the distance from the $ C $ to the line segment  $ \ overline {AB} $ use in the following way :

First, let's calculate $ u $ is the fraction of the distance along the  $ \ overline {AB} $ in which the perpendicular intersects the point  $ C $ :

$ u = {(C - A) \ cdot (BA) \ over (BA) \ cdot (BA)} $


Then we calculate the position $ E $ on $ \ overline {AB} $ :

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




Distance $ d $>/math> from $ C $ to $ \ overline {AB} $ is the distance from  $ C $ to  $ E $ :

$ d = \ | E - C \ | $


Since $ d & lt; r $ , the circle blocks the line of sight from  $ A $ in  $ B $ , and the edge must be discarded. If  $ d \ ge r $ , then the line of sight from  $ A $ in  $ B $ exists, and the edge should be left.

Edge Edge Generation


The nodes of the graph connect the edge of the transition with the envelope edge. In the previous sections, we generated transition edges. To generate edge envelopes, we start at the end point of the transition edge, circle the circle, and end at the end point of the other transition edge.


To find the set of envelopes of the edges of a circle, we first find all the transition edges that relate to the circle. Then create the edge envelopes between all the end points of the transition edges on the circle.

We put it all together


By generating transition edges, enveloping edges and nodes, and then discarding all overlapping transition edges, we can create a graph and start searching for a path using the A * algorithm.


Improvements


The graph generation procedure considered by us works quite well to explain the algorithm, but in many aspects can be improved. Such improvements will allow the algorithm to spend less processor and memory resources, as well as handle more situations. Let's look at some of the situations.

Obstacles that touch each other


You may have noticed that in the examples shown above, round obstacles did not overlap or even touch. If we assume that the circles can touch each other, then this slightly complicates the task of finding the way

Tangents to two points


Recall that tangents to two points can be found using this formula for internal tangents:

$ \ theta = \ arccos {{r_A + r_B} \ over {d}} $


and formulas for external tangents:

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


When two circles touch each other or intersect, there are no internal tangents between them. In this case, $ {r_A + r_B} \ over d $ greater than one. Since the arc cosine value is outside the definition area $ [- 1, 1] $ is not defined, it is important to check the intersection of circles before finding the arc cosine.

Similarly, if one circle is completely located in another, then there will be no external tangents between them. In this case, $ {r_A - r_B} \ over d $ is outside the range  $ [- 1, 1] $ , that is, does not have arc cosine.


Transition edge line of sight


If we allow the possibility of touching or crossing obstacles, then new cases arise in the calculations of the line of sight of the transition edges.

Recall the calculation $ u $ - the part of the distance along the transition edge, where the perpendicular to the point touches the edge. If touching the circles is not allowed, then the value of $ u $ is outside the interval  $ [0,1] $ , that is, the circle cannot touch the edge, because for this it would need to touch one of the end points of the edge. This is not possible because the end points of the edge are already tangent to other circles.

However, if we allow the possibility of overlaying circles, then the values ​​of $ u $ outside the interval  $ [0,1] $ can overlap the line of sight along the edge. This corresponds to the cases when the circle is located beyond the end of the transition edge, but overlaps the end point or touches it. To track such cases mathematically, we limit $ u $ at  $ [0,1] $ :

$ E = A + clamp (u, 0, 1) * (B - A) $


Line of sight of an envelope edge


When obstacles can touch each other or intersect, the edges of the edges can be blocked by obstacles as well as the edges of the transition. Consider the envelope edge in the figure below. If there is another obstacle around the edge, the edge is blocked and must be discarded.


To determine if the envelope of the edge is blocked by another obstacle, use the following method to determine the points at which two circles intersect. If the circles have centers at the points $ A $ and  $ B $ and radii  $ r_A $ and  $ r_B $ , where  $ d $ - the distance between  $ A $ and  $ B $ , then we first need to check a few cases. If the circles do not touch each other (i.e.  $ d & gt; r_A + r_B $ ),
are one inside the other ( $ d & lt; | r_A - r_B | $ ) or match (  $ d = 0 $ and  $ r_A = r_B $ ), the circles cannot influence the envelopes of each other's edges.

If none of these conditions are met, then the circles intersect at two points; if the circles are touching each other, then these two points coincide. Consider the radical axis connecting intersection points; it is 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} $


Finding $ a $ we can find the angle $ \ theta $ :

$ \ theta = \ arccos {a \ over r_A} $


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


Next, we determine whether any of the intersection points between the start and end points of the envelope edge. If so, then the obstacle blocks the envelope of the edge, and we must drop this edge.Notice that we do not need to consider the case when the envelope edge is entirely inside the obstacle, because the cut-off line of sight for the transition edges has already dropped this edge.

After making changes to the computation of the tangents to two points and the line of sight for the transition edges and the envelopes of the edges, everything works correctly.

Variable actor radius and Minkowski extension


When calculating the navigation of a circular object in the world of circular obstacles, observations can be taken into account to simplify the solution of the problem. First, you can simplify the work by noting that the movement of a circle with a radius of r in the forest is similar to the movement of a point through the same forest with a single change: the radius of each obstacle is increased by r . This is an extremely simple case of applying the Minkowski amount . If the actor's radius is greater than zero, then before the start we simply increase the size of the obstacles.

Delayed Edge Generation


In general, the graph for the forest from $ n $ obstacles contains  $ O (n ^ 2) $ transition edges, but since each of them needs to be checked for the line of sight with  $ n $ obstacles, the total graph generation time is $ O (n ^ 3) $ . In addition, a pair of transition edges can lead to the creation of envelopes of edges, and each of them must be checked with each obstacle on the line of sight. However, due to the high efficiency of the A * algorithm, it usually looks only part of this large graph to create the optimal path.

We can save time by generating small parts of the graph on the fly during the execution of the A * algorithm, rather than doing all the work in advance. If A * finds the path quickly, then we will 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 starting point. These are the edges of the transition from the starting point to the left and right edge of each obstacle.

The next case is when A * just got to the point $ p $ on the edge of the obstacle  $ C $ along the transition edge: neighbors () should return envelopes leading from  $ p $  . To do this, we will determine which transition edges get out of the obstacle by calculating the tangents to two points between $ C $ and all other obstacles, discarding all those that are not on the line of sight. Then find all the envelope edges that connect $ p $ with these transition edges, discarding those that are blocked by other obstacles. We return all these edges around the envelope, saving the transition edges to return in a subsequent call to neighbors () .

The last case was when A * went around the envelope edge along the obstacle $ C $ and he needs to leave  $ C $ along the transition edge. Since the previous stage calculated and saved all the edges of the transition, you can simply find and return the correct set of edges.

We cut off the sharp edges around the envelope


The bends around the edges connect the transition edges relating to one circle, but it turns out that many of these edge envelopes are not suitable for use in an optimal way. We can speed up the algorithm by eliminating them.

The optimal path through a forest of obstacles always consists of alternating transition edges and envelopes of edges. Suppose we are entering the $ A $ and decide how to get out of it:


Log in via $ A $ means we're moving clockwise ( $ \ circlearrowright $ ). We need to go through the node that allows us to continue moving clockwise ( $ \ circlearrowright $ ), that is, we can exit only through the node  $ B $ or  $ D $ . If you exit through $ C $ then inflection will be created ( $ \ curlywedge $ ) a way that will never be optimal. We need to filter out such sharp edges.

First, note that A * handles each undirected edge $ P \ longleftrightarrow Q $ as two oriented edges,  $ P \ longrightarrow Q $ and  $ Q \ longrightarrow P $ . We can use this by marking the edges and nodes with orientation.

  1. Nodes $ P $ become nodes with orientation - clockwise ( $ P \ circlearrowright $ ) or counterclockwise (  $ P \ circlearrowleft $ ) arrows.
  2. Non-oriented transition edges $ P \ longleftrightarrow Q $ become oriented edges  $ P, p \ longrightarrow Q, \ hat {q} $ and  $ Q, q \ longrightarrow P, \ hat {p} $  , where $ p $ and < img alt = "$ q $" data-tex = "inline" src = "https://habrastorage.org/getpro/habr/formulas/d68/cc4/926/d68cc4926bf74bae8fa3b51ca4a09ec8.svg"/> - this orientation and $ \ hat {x} $ means the opposite direction  $ x $ .
  3. Non-oriented edges envelopes $ P \ longleftrightarrow Q $ become oriented edges  $ P \ circlearrowright \ longrightarrow Q \ circlearrowright $ and  $ P \ circlearrowleft \ longrightarrow Q \ circlearrowleft $ . This is where the filtering happens: we do not turn on $ P \ circlearrowright \ longrightarrow Q \ circlearrowleft $ and  $ P \ circlearrowleft \ longrightarrow Q \ circlearrow \

    "src =" https://habrastorage.org/getpro/habr/formulas/25d/534/1cb/25d5341cb2af330b3d0433fa55f5f537.svg "/> because the change of direction creates excesses (   $ \ curlywedge $ ).

In our scheme, the node $ A $ turns into two nodes, $ A \ circlearrowright $ and  $ A \ circlearrowleft $ , it has an incoming transition edge  $ \ longrightarrow \ circlearrowright $ and outgoing transition edge $ A \ circlearrowleft \ longrightarar  $ . If we hit the path via $ A \ circlearrowright $ , then you must go through the  $ \ circlearrowright $ , which will be either a transition edge  $ B \ circlearrowright \ longrightarrow $ (via the envelope  $ A \ circlearrowright \ longrightrow B \ circlearrowright $ ), or $ D \ circlearrowright transition edge  \ longrightarrow $ (through the $ A \ circlearrowright \ longrightarrow D \ circlearrowright $ ). We cannot exit By $ C \ circlearrowleft \ longrightarrow $ because the direction of rotation changes this way, and we filter the envelope edge  $ A \ circlearrowright \ longrightarrow C \ circlearrowleft $ .

Filtering out these sharpened edge envelopes from the graph, we increased the efficiency of the algorithm.

Clipping the intersecting edges


You can cut off partial paths, the last edges of which intersect the last but one edge of the transition.

Polygonal obstacles


See Game Programming Gems 2 , Chapter 3.10, Optimizing Points-of-Visibility Pathfinding, written by Thomas Young. This chapter deals with the clipping of nodes, not for circles, but for polygons.

Reference materials


Source text: [Translation] Search for the way among the round obstacles