Calculating 'Diagonal Distance' in 3 dimensions for A* path-finding heuristic

Question

I'm running A* Path finding over a 3D grid of data. Available movements are the 26 surrounding nodes (i.e. you can move diagonally) I've been using euclidean distance as the heuristic and this works well but I'd also like to try 'diagonal distance' to see how that works (and if there are any speed gains)

I've found some logic online to do this in 2 dimensions...

function heuristic(node) =
dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

..,where D is the north/east/south/west distance (for example 1m) and D2 is the diagonal distance (for example sqrt(2))

I'm not exactly sure how to convert this to 3 dimensions - so any help would be greatly appreciated

As an extra question (as this is how much grid is in reality...) suppose nodes on the x and y axis are 5m apart, but 2m apart on the z axis...how would the formula work then?

Thanks for any help!

Answer 1

It can be extended to 3D relatively easily. It does require finding the "middle" of 3 values, there is a trick for that given that we have the minimum and maximum.

dx = absdiff(node.x, goal.x)
dy = absdiff(node.y, goal.y)
dz = absdiff(node.z, goal.z)
dmin = min(dx, dy, dz)
dmax = max(dx, dy, dz)
dmid = dx + dy + dz - dmin - dmax

This works for Python style integers and even for Java style int, for floats it can cause some rounding though.

Combine them like this:

return (D3 - D2) * dmin + (D2 - D1) * dmid + D1 * dmax