This site uses cookies to ensure you get the best experience. More info
Got it!

Pathfinding basics



Pathfinding

Pathfinding, as the name suggests is a process of finding a path between two given points.
It is very widely used in game development - whenever some kind of AI needs to move from point A to point B it will very likely need to utilise some pathfinding.

Although, there might be cases where pathfinding is not really a must. Let’s put Goombas from Super Mario Bros as an exmaple. Goombas move in one direction until they collide with something, and if that happens, they go in the opposite direction.

A Goomba. A Goomba.

This kind of AI is very simple and does not need to rely on finding a path at all.

But in more complex situations like strategy games, where we tell some units to go to given destination - we usually have many different possible paths and some obstacles along the way. This is when we have to implement some pathfinding algorithms for AI to handle such orders properly.

I’m not going to delve into the subject very deeply as it is way too big for a single, short blog post, but I hope to cover it well enough for even less gamedev-savvy readers to understand.

Pathfinding in Praedium

When it comes to Praedium, we will be dealing with 2D grids, with some of the tiles acting as obstacles. For the sake of simplicity, we will be dealing with an example of a single controllable character within a non-changing environment.

First of all we have to decide on what algorithm to choose. The algorithm I have decided to use for my pathfinding needs is called A*.

A* is a variation of Dijkstra’s algorithm. It’s a very common choice which can be easily tinkered with to one’s needs.

Let’s take a look onto some pseudo-code.

closed = {};
q = emptyqueue;
q.enqueue(0.0, makepath(start))
while q is not empty
    p = q.dequeueCheapest
    if closed contains p.last then continue;
    if p.last == destination then return p
    closed.add(p.last)
    foreach n in p.last.neighbours 
        newpath = p.continuepath(n)
        q.enqueue(newpath.TotalCost + estimateCost(n, destination), newpath)
return null

The algorithm utilises a priority queue to order any found passable subpaths by their calculated traversal cost.

  1. Firstly we add a 0 cost path with the starting point into the queue.
  2. Then, we loop until we find the complete path to the destination.
  3. Inside the loop, we take the first path inside the queue.
  4. If that path was already checked, we skip this loop step.
  5. If we reached the destination, we return that path.
  6. If not, we continue by marking this path as checked.
  7. Next, we iterate through the current path’s last node’s neighbours.
  8. For each neighbour, we create a new subpath and add it to the queue with calculated cost.

Now, the trickiest part is cost calculation. While we may quite easily calculate costs of traversal from neighbour to neighbour, we need to estimate the cost from given node to the path’s destination. This estimation function is known as heuristic.

Now, the funny part about A*, is the fact that depending on how we change the heuristic the algorithm behaves very differently.

Some great examples and explanations from Amit Patel can be found here.

Let’s move on to actual implementation inside the engine.

Priority Queue

Firstly, let’s create a simple priority queue class for our pathfinding needs.
We will base the priority queue on System.Collections.Generic.SortedDictionary.

public class PriorityQueue<P, V>
{
    private SortedDictionary<P, Queue<V>> list = new SortedDictionary<P, Queue<V>>();

    public void Enqueue(P priority, V value)
    {
        Queue<V> q;
        if(!list.TryGetValue(priority, out q))
        {
            q = new Queue<V>();
            list.Add(priority, q);
        }
        q.Enqueue(value);
    }

    public V Dequeue()
    {
        var pair = list.First();
        var v = pair.Value.Dequeue();

        if (pair.Value.Count == 0)
            list.Remove(pair.Key);

        return v;
    }

    public bool IsEmpty
    {
        get
        {
            return !list.Any();
        }
    }
}

The PriorityQueue is a generic class, accepting a P priority type, and V value type to be kept inside the dictionary.

As you can see the dictionary contains subqueues for each given priority.

The class has a very simple API consisting of Enqueue, Dequeue methods and a single IsEmpty property.

Now, the next step is to define how our path structure will look like.

Paths

The paths are basically collections of nodes in given order.

In case of my implementation the path is an immutable structure that keeps the last node and a subpath of previous steps. This way, iterating through the nodes, means iterating through every subpath’s last node which create one big path.

The Path class is a generic class, which allows for much more flexible pathing for the engine’s users. It subclasses from IEnumerable<Node> for some fancy enumeration API.

public class Path<Node> : IEnumerable<Node>
{
    public Node LastStep
    {
        get;
        private set;
    }

    public Path<Node> PreviousSteps
    {
        get;
        private set;
    }

    public double TotalCost
    {
        get;
        private set;
    }

    public Path(Node start)
        :this(start, null, 0)
    { }

    public Path<Node> AddStep(Node step, double stepCost)
    {
        return new Path<Node>(step, this, TotalCost + stepCost);
    }

    private Path(Node lastStep, Path<Node> previousSteps, double totalCost)
    {
        LastStep = lastStep;
        PreviousSteps = previousSteps;
        TotalCost = totalCost;
    }

    public IEnumerator<Node> GetEnumerator()
    {
        for(Path<Node> p = this; p != null; p = p.PreviousSteps)
        {
            yield return p.LastStep;
        }
    }

    IEnumerator IEnumerable.GetEnumerator()
    {
        return this.GetEnumerator();
    }
}

Adding a new step to the path via AddStep method actually returns a new structure. That structure keeps previous steps as current path and a last step as the new step passed into the method with given cost.
The new path’s cost is equal to the current path’s cost plus the last step’s individual cost.

Pretty straightforward.

Now, when it comes to the actual algorithm, it is kept as a static method that returns a new instance of Path class based on the input.

Here is how the code for it looks like.

public static Path<Node> FindPath(
    Node start,
    Node destination,
    Func<Node, Node, double> distance,
    Func<Node, Node, double> estimate,
    Func<Node, IEnumerable<Node>> neighbours)
{
    var closed = new HashSet<Node>();
    var queue = new PriorityQueue<double, Path<Node>>();
    queue.Enqueue(0, new Path<Node>(start));

    while(!queue.IsEmpty)
    {
        var path = queue.Dequeue();

        if(closed.Contains(path.LastStep))
        {
            continue;
        }

        if(path.LastStep.Equals(destination))
        {
            return path;
        }

        closed.Add(path.LastStep);

        foreach (Node n in neighbours(path.LastStep))
        {
            double d = distance(path.LastStep, n);
            var newPath = path.AddStep(n, d);
            queue.Enqueue(newPath.TotalCost + estimate(n, destination), newPath);
        }
    }

    return null;
}

Now, let’s go through what’s going on.

To the FindPath method, we pass a starting node, a destination, and three functions to be used for distance calculation (between neighbours), estimating function (our heuristic) and a function which returns neighbours for given node.

The code besides that is quite straightforward and very similar to the original pseudocode.

Having implemented the basic pathfinding, let’s take a look at implementing some actual example.

The Example

Our example will be all about getting our (previously controlled by keyboard) character to move to given destination, after selecting him and pressing right mouse button somewhere on the fields.

Let’s begin by integrating pathfinding into the Level class, which contains information about all static elements.

The tiles are kept as arrays inside the TileLayer instances so instead of changing that, I decided to add a Bramble.Core.Array2D instance of bool values, which are set to true when given tile is not traversable.

Now, whenever we add a new tile layer, we iterate through the tiles and mark given indexes as true for each tile that is not traversable.

public void AddTileLayer(TileLayer layer)
{
    Layers.Add(layer.ZIndex, layer);
    foreach (var tile in layer.Tiles)
    {
        if (tile.Collideable)
            movementMap[tile.Position.X, tile.Position.Y] = true;
    }
}

Having the map data mapped correctly, let’s move on to actual pathfinding API.

The Level class now contains a FindPath method that accepts two positions - start and destination. This method calls the underlying Path class’ static method.

public Path<Vector2D> FindPath(Vector2D start, Vector2D end)
{
    return Path<Vector2D>.FindPath(start, end, NeighbourDistance, Heuristic, GetNeighbours);
}

Now let’s go through the functions that calculate the costs and get neighbours for each given node.

private readonly double DIAGONAL_COST = Math.Sqrt(2);
private readonly int STRAIGHT_COST = 1;

private double NeighbourDistance(Vector2D a, Vector2D b)
{
    if (a.X != b.X && a.Y != b.Y)
        return DIAGONAL_COST;
    else
        return STRAIGHT_COST;
}

The NeighbourDistance method returns one of the two values. It returns 1, when we deal with straight movement, and Sqrt(2) for diagonal movement.

Diagonal movement is more costly (equal to square’s diagonal length), because it technically covers more distance with a single move.

private double Heuristic(Vector2D a, Vector2D b)
{
    var dx = Math.Abs(a.X - b.X);
    var dy = Math.Abs(a.Y - b.Y);
    return STRAIGHT_COST * (dx + dy) + (DIAGONAL_COST - 2 * STRAIGHT_COST) * Math.Min(dx, dy);
}

Now, our heuristic is a bit more complex. If you gave a read to the Amit Patel’s article you might have seen this under the Diagonal Distance section.

This is the usual heuristic used if our game allows diagonal movement along the 2D grid. Using it creates paths with nice diagonal movements along them.

Getting neighbours for given tile basically means getting all traversable tiles other than the given one so we iterate through the vectors in a small rectangle and get all vectors different than given one, that are mapped as traversable.


Our next step is changing the PlayerMovementHandler component to utilise the newly implemented path finding.

public class PlayerMovementHandler : Component
{
    // Limit movement speed to 5 tiles per second
    private const double LAG = 0.2f;

    private double elaspedTime;

    private Player player;

    private bool processingMovement = false;

    private Vector2D targetPosition;

    protected override void OnStart()
    {
        player = (Player)GameObject;

        Game.MouseDown += Game_MouseDown;
    }

    void Game_MouseDown(object sender, MouseInfoEventArgs e)
    {
        if(player.Selected && e.MouseInfo.Button == MouseButton.Right)
        {
            targetPosition = Game.ToWorldPosition(e.MouseInfo.Position);

            var path = Game.Level.FindPath(player.Position, targetPosition);

            if (path != null)
            {
                player.MovementPath.Clear();

                player.MovementPath.Push(path.LastStep);
                foreach (var step in path.PreviousSteps)
                {
                    player.MovementPath.Push(step);
                }

                processingMovement = true;
            }
        }
    }

    public override void Update()
    {
        if(processingMovement)
        {
            if (player.Position == targetPosition || player.MovementPath.Count == 0)
            {
                processingMovement = false;
                return;
            }

            if (elaspedTime < LAG)
                elaspedTime += Game.DeltaTime;

            if (elaspedTime > LAG)
            {
                elaspedTime -= LAG;

                GameObject.Position = player.MovementPath.Pop();
            }
        }
    }
}

The key change here is the MouseDown event handler.
If we now press the right mouse button when the player is marked as selected, we calculate the target position and find a path to it.

If such path exists, we clear the node stack from player and push every step from the path to it.

In the Update method we set the player’s position to the popped node from the stack every LAG part of second.

The result - working pathfinding The result - working pathfinding, with a simple path rendering.

Wrapping up

I may have skipped through some of the new additions to the engine (such as the path rendering for debugging), but I hope this walkthrough of implementation process was quite simple.

As you can see, pathfinding might not be so scary. In case of 2D grids, pathfinding is actually a very simple feature to implement.
Although, there is always room for improvement. We will need to handle cases, where the paths need to be recalculated, because there are some moving objects on the way.

The algorithm will grow more complex as the game itself will grow too. I’ll definitely move the implementation out of the engine in the near future as it is too limiting at this moment.

Moreover, handling pathfinding for groups of objects is also an interesting topic to tackle as it is a big optimization to handle multiple objects’ movement in one swoop instead of every one of them individually.

And that’s it for today’s post! I hope you might have learned something new along the way, or if not - then maybe found the implementation interesting.

See you next time!