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

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.

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.

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: