Learning and Applying Mathematics using Computing

In the last post we saw about distances with horizontal and vertical movement, and how to move one object towards another using horizontal and vertical moves on an empty grid. But what if there are obstacles in the way? For that we need a pathfinding algorithm. This is not particularly mathematical, and it is a bit intricate, but it will come in handy for our next game.

Signposts

How do you calculate all the shortest paths to a particular destination square on a grid? Your initial inclination might be to have, for each source square, a list of directions. So something like this (blue star is the source, red square is the destination, with each arrow showing which square to move to next):

But in fact, consider the next square on that route — its path will look like this:

So actually, we don’t need a full list of directions for each source square, we just need one direction: to the next square. For a fixed destination, each square acts like a signpost, pointing the way to the next square. By following the signposts on each square, you can arrive at the destination. This means that for any given destination square, we need to form a complete grid of signposts, that will look something like this:

The Idea

So how do we build up this grid of signposts? You might think that we begin at the source square, but actually that doesn’t work so well: we’ve no idea which direction will be successful from the source, because we don’t know what walls might be in the way. Instead, we start at the destination, because from the squares exactly adjacent to the destination, we know which way to head:

Then from the squares adjacent to those, we also know which way to head:

And so we spider outwards, searching all adjacent squares. At each square we see if the route we are currently tracing from the destination is shorter than the one which has been recorded. If the current route is shorter, continue, otherwise (if the old route is shorter) then give up. We will almost certainly reach situations where we have multiple ways in which we can head, for example the squares with the multiple smaller red arrows above. In these situations, we use the rule we saw in our last post to decide which way to head: the way that gives the shortest diagonal distance. In the picture above this is equal both ways, so we just make an arbitrary choice.

Show me the Code

I’m not going to go into great depth on the code, but here it is if you want to see exactly what’s happening:

    private void calculatePathFrom(byte[][] paths, int[][] dist, int fromX, int fromY, byte newDir, int newDist)
    {
        if (outOfBounds(fromX, fromY) || occupied[fromX][fromY])
            return; //Outside world or blocked
            
        byte prevDir = paths[fromX][fromY];

        if (prevDir == STAY)
            return; // At destination square again
            
        int prevDist = dist[fromX][fromY];
        
        if (prevDir != 0 && prevDist != 0 && prevDist < newDist)
            return; //Already processed and already have shorter path
        
        if (prevDir != 0 && prevDist > 1 && prevDist == newDist
            && (prevDir == newDir || distanceSq(toX - moveX(fromX, newDir), toY - moveY(fromY, newDir)) > distanceSq(toX - moveX(fromX, prevDir), toY - moveY(fromY, prevDir))))
            return; //Already processed and already have shorter or equal path
        
        paths[fromX][fromY] = newDir;
        dist[fromX][fromY] = newDist;
        
        calculatePathFrom(paths, dist, fromX - 1, fromY, RIGHT, newDist + 1);
        calculatePathFrom(paths, dist, fromX + 1, fromY, LEFT, newDist + 1);
        calculatePathFrom(paths, dist, fromX, fromY - 1, UP, newDist + 1);
        calculatePathFrom(paths, dist, fromX, fromY + 1, DOWN, newDist + 1);
    }

The code has four early exits:

  1. The square we want to find a path from is occupied by an obstacle, or is outside the grid. It is not possible to make a path from such a location.
  2. We have arrived back at the destination square (e.g. while looking for a path to an adjacent square) — this never makes sense, because the shortest path to the destination will never go through the destination on the way (just stop there!).
  3. We have already found a shorter path from this square, so our latest path (and all extensions of it) will just be longer than what is already going to/through this square.
  4. We have already found a path of equal length from this square, but the previous one had a better diagonal distance (see last post), so prefer the old path. Otherwise, if the path is of equal length but the new one has a better diagonal distance, we continue and overwrite the old path.

Assuming none of the above are true, we replace the previous path with our new path, and spider outwards (last four lines) looking for other paths. You can see the scenario in action — run the scenario and move your mouse over a square to see all the shortest paths to that square.

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s