# How can I favour vertical travel before horizontal with my A* path-finding heuristic?

I'm writing an AI of a 'police car' that 'patrols' over a 2d grid. Right now, the patrol car randomly selects a destination, finds a path between it's current intersection and the destination intersection, follows the planned path then rinse and repeat. That's the intended behaviour and it works as expected.

Now the A* algorithm I wrote based on Wikipedia's page about it will give me "step style" paths.

And I don't like it in this context. I want the police car to go straight in a direction first (vertical, or up and down, or along the y-axis), then straight in the other direction (i.e. horizontal, or left or right, or along the x-axis) before it reaches its destination.

I though about (and tried) hacking the heuristics method to "inflate" the values for x-axis paths, the idea being that it would find a path for the y-axis first, then for the x-axis, but there was no improvement to the police car behaviour.

In the same idea, I tried to tell the algorithm that x-axis distances were more expensive than the y-axis distances, but I got the same result: there was no improvement to the style of the paths the cars used.

How can I modify the heuristic evaluation method of the A* algorithm so that the police car prefers going all the way through vertically, then horizontally?

• There is no obstacle on the grid.
• All the grids are separated evenly by a floating point value distance.

In my Graph.cpp, I have these two methods that basically find a path between aStart and aEnd. (Sorry for the bad name of "plot".)

// Find a path between aStart and aEnd.
std::vector<int>
Graph::plot( int aStart, int aEnd )
{
std::set<int> closedSet;
std::set<int> openSet = {aStart};

std::map<int, int> cameFrom;

std::map<int, float> gScore;
for ( auto node : mNodes )
gScore[node.first] = std::numeric_limits<float>::infinity();

gScore[aStart] = 0.0f;
std::map<int, float> fScore;
for ( auto node : mNodes )
fScore[node.first] = std::numeric_limits<float>::infinity();

fScore[aStart] = getHeuristicBetween( aStart, aEnd );

while (openSet.size() > 0)
{
int current = -1;
float currentMinVal = std::numeric_limits<float>::infinity();
for ( auto nodeIndex : openSet )
{
if ( fScore[nodeIndex] < currentMinVal )
{
current = nodeIndex;
currentMinVal = fScore[nodeIndex];
}
}

if ( current == aEnd )
return reconstructPath(cameFrom, aEnd);

openSet.erase( current );
closedSet.insert( current );
for ( auto outArc : mNodes[current]->getOutArcs() )
{
int neighbourIndex = outArc->getNodeTo()->getId();
if ( closedSet.find(neighbourIndex) != closedSet.end() )
continue;
// The distance will be artificially higher if it's on an horizontal edge:
float tentativeGscore = gScore[current] + outArc->getInfluencedDistance();
if ( openSet.find(neighbourIndex) == openSet.end() )
openSet.insert(neighbourIndex);
else if ( tentativeGscore >= gScore[neighbourIndex] )
continue;

cameFrom[neighbourIndex] = current;
gScore[neighbourIndex] = tentativeGscore;
fScore[neighbourIndex] = gScore[neighbourIndex] + getHeuristicBetween( neighbourIndex, aEnd );
}
}
}

// Estimates the shortest path between aFromNode and aToNode.
float
Graph::getHeuristicBetween( int aFromNode, int aToNode ) const
{
auto nodeStartIt = mNodes.find( aFromNode );
auto nodeEndIt   = mNodes.find( aToNode );

if ( nodeStartIt == mNodes.end() || nodeEndIt == mNodes.end() )
return std::numeric_limits<float>::infinity();

float dx = nodeEndIt->second->getX() - nodeStartIt->second->getX();
float dy = nodeEndIt->second->getY() - nodeStartIt->second->getY();

float length =
(   glm::vec2( nodeStartIt->second->getX(), nodeStartIt->second->getY() )
- glm::vec2( nodeEndIt->second->getX()  , nodeEndIt->second->getY() ) ).length();

if ( dx > dy )
{
return length * 0.1; // Artificially inflate the cost of going left-right.
}
return length;
}


With or without the modification to the algorithm, I get this kind of behaviour:

• To discourage the pathfinding algorithm to zig-zag, you could add a cost to turning. – Phinet Jan 2 '18 at 4:21
• @Phinet How would you do that? – Vaillancourt Jan 2 '18 at 4:35
• How does your solution perform if there are obstacles (missing vertices or missing edges)? Because if it's a homogenous grid and will stay as such, i guess there is not really any need for pathfinding (doing an "L" would be sufficient)? You are probably catering for special cases later on? – Stormwind Jan 2 '18 at 21:35
• @Stormwind Yes, you have valid points. I think I was stuck in the XY problem or I did not think enough about changing strategy: I already had implemented the A* but wanted to have the "L" style travel. Having a dumb "L" search would most likely be sufficient in this case. I'll have to experiment with the obstacles. That's my next step :) – Vaillancourt Jan 2 '18 at 21:38
• Ok :-). I kind of can intuitively feel that carrying a bit with each vertex and dealing with it (promoting 1, or discouraging a change) might provide a solution. But... urgent times, must stay focused on w0rk :-). – Stormwind Jan 2 '18 at 21:50

The naive approach to simply inflate the cost when going in a specific direction will never work in the heuristic evaluation function, because that function is unaware of the departure and final destinations. Also, it has to return the value of the most optimal path. To make it work from there, it would need to return the cost for the most optimal path, and the cost for the not-most-optimal-path. This does not make sense. The same applies when trying to hack the distance between two nodes (when asking the arc about it).

After thinking about it, I took a closer look into the algorithm, specifically the Wikipedia article:

Typical implementations of A* use a priority queue to perform the repeated selection of minimum (estimated) cost nodes to expand. This priority queue is known as the open set or fringe. At each step of the algorithm, the node with the lowest f(x) value is removed from the queue [...] (Wikipedia)

To make it work the way I wanted, I had to change how the node with the "lowest cost" was selected. I modified the algorithm like this:

1. Pick the candidate node if its score is lower than the current best score.
2. If the candidate node score is equal to the current selected node's score, pick the node where we have the shortest distance in y direction to go.

The code goes about like this:

  while (openSet.size() > 0)
{
int current = -1;
float currentMinVal = std::numeric_limits<float>::infinity();

for ( auto nodeIndex : openSet )
{
auto epsilonLessThan = []( float aLhs, float aRhs, float aEpsilon ) {
if ( std::abs( aLhs - aRhs ) < std::abs( aEpsilon ) )
return false;
return aLhs < aRhs;
};

if ( epsilonLessThan( fScore[nodeIndex], currentMinVal, 0.00005f) )
{
current = nodeIndex;
currentMinVal = fScore[nodeIndex];
}
else if ( glm::epsilonEqual( fScore[nodeIndex], currentMinVal, 0.00005f ) )
{
auto goalNode        = mNodes.find( aEnd );
auto selectedNode    = mNodes.find( current );
auto replacementNode = mNodes.find( nodeIndex );

assert( goalNode != mNodes.end() && selectedNode != mNodes.end() && replacementNode != mNodes.end() );
auto goalNodePosition        = glm::vec2( goalNode       ->second->getX(), goalNode       ->second->getY() );
auto selectedNodePosition    = glm::vec2( selectedNode   ->second->getX(), selectedNode   ->second->getY() );
auto replacementNodePosition = glm::vec2( replacementNode->second->getX(), replacementNode->second->getY() );

auto deltaSelected = selectedNodePosition - goalNodePosition;
auto deltaReplacement = replacementNodePosition - goalNodePosition;

// We prefer going in y first
if ( std::abs( deltaReplacement.y ) < std::abs( deltaSelected.y ) )
{
// we prefer that.
current = nodeIndex;
currentMinVal = fScore[nodeIndex];
}
}
}

if ( current == aEnd )
// [...]


While the usage of glm::epsilonEqual is quite obvious, the epsilonLessThan was useful: tiny differences between fScore[nodeIndex] and currentMinVal would make the evaluation estimate them as being "less than" instead of "equal". (I could have checked the conditions in the other order and I guess I would have arrived at the same result.)

Here is the result:

During a discussion a few years ago with some grad students studying pathfinding, I came up with a few solutions to the problem "How do I ensure the A* solution is exactly the same as the solution obtained by BFS (in particular, choosing certain edges over others when there is more than one best path)"

It's been a few years since I've thought about it, so I will just copy+paste the final solution I came up with from the email chain.

I’ve come up with two ways of doing it, if any of you are still interested in this problem. Both have worse computational-complexity than BFS/Djikstra’s, but may still be faster in practice on large graphs or graphs with many obstacles. I’ll try implementing these if I can't find anything better, and see how they compare to BFS for my problem-space.

The first way is more widely applicable. The second is specifically for 4- or 8-connected graphs with all edge-weights 1 (or infinity).

The problem, again, is that there may be multiple best-paths, and we want to use A* to find the same path obtained by BFS/Djikstra’s. To do this, we must alter A* slightly – instead of stopping when we reach the end-node, we continue expanding all nodes with f-value <= f(end-node). Since the A* heuristic is admissible, this guarantees that all nodes that belong to a best-path will be expanded eventually: in particular, the nodes belonging to the BFS-best-path will be expanded.

In the first algorithm, when a successor B of a node A is seen for the first time, we set its distance-to-start g(B) = g(A) + c(A,B) and its previous-node property p(B) = A, like usual. If B is also a successor to node C, and later C is expanded, we compare B’s current distance-to-start g(B) with its potential new distance-to-start g’(B) = g(C) + c(C, B):

• If g’(B) < g(B) (only possible if h is not consistent), we overwrite g(B) and p(B)
• If g’(B) > g(B) we do nothing.
• If g’(B) == g(B), we have a problem. We need to figure out which path has higher lexicographical ordering - we need to find the first node that the path passing through A and the path passing through B deviate from one another. Once we know that, we can simply check which path takes the higher-priority direction from that node.

Finding the first node that the paths deviate is simple enough: we walk backwards along each path's previous-node property p(n) until we find the first node belonging to both paths. Since every node has only one previous-node property, every node before this one will be the same on both paths. Since this node will have the same g(x) value for both paths, we can find it by always stepping backwards on the path with the higher g(x) value.

If C is considered part of the current best-path at this point in our algorithm, and both B and C (or A and C) are part of the BFS-best-path, then B (or A) will be considered part of the current best-path at this point in our algorithm. And since all best-paths start at the same node, we can show inductively that this will give us the BFS-best-path.

The second algorithm is similar to the first, but we use the fact that, since there is (thus far) exactly one node where the paths containing A and B go from being the same to differing, instead of searching backwards from the end for the first place they're the same, we can search forwards from the start for the first place they're different. We can do this by storing the entire best-path found so far for each node in that node. In most cases, this would be unwieldy, but in 4- (or 8-)connected graphs with all edge-weights at 1, we can store the best path using 2 (or 3) bits-per-node, with higher-priority paths having higher values.

We can treat the paths as arbitrary-length integers, with the most-significant-2-bits representing the first direction moved. Then (assuming the two bit-strings have the same length) we can simply check which integer is larger to determine which path has higher priority. This allows us to compare 16 nodes of the path at a time with 32-bit registers; 32 nodes with 64-bit registers; and 64(!) nodes at a time with 128-bit registers (like the SSE registers in x86 and x64 processors), making this search very inexpensive even for paths with 100's of nodes.

As I was writing this, a user on Stackoverflow.com posted a third potential solution: run A* as above to expand all nodes that belong to a best path; work backwards from the finish to find only those nodes that belong to a best path; work forward from the start again, finding the BFS-best-path by walking along the nodes that belong to a best-path, always moving upwards in g-value and breaking ties using our lexicographical-ordering.

• Thanks! I'll take a closer look at it eventually! (Time is a limited resource for me now :/) – Vaillancourt Jan 3 '18 at 2:42

The heuristic you are using is creating a bias in favor of "diagonal" paths. Using Euclidean distance in this scenario causes the distance between points along a diagonal to be underestimated. This isn't technically incorrect since the result will still be a shortest path, but since actual diagonal movement isn't possible in your case, using Manhattan distance is probably a better fit.

This won't guarantee that the found path will not have a staircase pattern, but you may be able to fix that by carefully selecting which node you select first if multiple nodes have equal f scores.