Custom Pathfinding with A*

Describes how to create a custom 2D pathfinding system using the A* algo.

by ForeverDev

Author Avatar

Introduction

This lesson will outline the basics of implemeting the A* (A star) pathfinding algorithm for usage in a two dimensional grid.

Why?

Roblox has a built-in PathfindingService, but it often isn't the best choice for a game. PathfindingService is great for pathfinding in a 3 dimensional space. However, the nodes along the path are disorganized and often not well-aligned. If you'd like to constrain your path nodes into a grid-space (think Minecraft or any other voxel-based game), or you'd like to use pathfinding in a two-dimensional grid-space, it may be in your best interest to implement a custom pathfinding algorithm.

How?

A common algorithm used to implement pathfinding is known as 'A*', (pronounced 'A star'). It is used in many modern-day products, including Google Maps, Uber, Lyft, etc.

The Algorithm

Input

The input to the pathfinding function is a two-dimensional table of numbers that represents the world. Further, a starting position and a goal position (represented here as Vector2) are inputted.

function CalculatePath(worldGrid, start, goal)

worldGrid is a two-dimensional table made up of costs. These costs can be used to describe a two dimensional world. The purpose of A* is to find a path within the world such that the total cost of a path is minimized. This is best understood with an example:

local worldGrid = {
    {1, 10, 1,  1},
    {1, 10, 10, 1},
    {1, 1,  1,  1},
    {1, 1,  1,  1}
}
local start = Vector2.new(1, 1)
local goal = Vector2.new(4, 1)
local bestPath = CalculatePath(worldGrid, start, goal)

Imagine that worldGrid represents a bird's eye view of a grid. Each value represents how "hard" it is to walk through that part of the grid. In this example, imagine that the value 1 represents an empty grid-space and that the value 10 represents a wall, which should be avoided by the path. The values start and goal represent coordinates in the grid. In this example, start represents the top left spot in worldGrid and goal represents the top right spot. bestPath, the value returned by the call to CalculatePath, is a table of Vector2 coordinates. Let's print out the coordinates and see what we get:

for _, coord in ipairs(bestPath) do
  print(string.format("[x: %d, y: %d]", coord.X, coord.Y))
end
-- output:
-- [x: 1, y: 1]
-- [x: 1, y: 2]
-- [x: 1, y: 3]
-- [x: 2, y: 3]
-- [x: 3, y: 3]
-- [x: 4, y: 3]
-- [x: 4, y: 2]
-- [x: 4, y: 1]

Notice that these coordinates trace out a path inside of worldGrid and that the walls, denoted by cost 10, are avoided. Also notice that the first element in bestPath is start and the last element in bestPath is goal.

Output

The output from CalculatePath is a table of coordinates that represent the best-possible path, where the first element in the table is start and the last element is goal. Please see the example provided above.

How A* Works

Note: From here on, I will use the term node to refer to a single point in the 2D grid.

openSet

openSet is a table containing nodes that we're currently exploring. Initially, it contains only the start node, as that is the first node in the path. Repeatedly, the algorithm picks the least-cost node from the openSet, then checks the other nodes next to it to see if they should be added to the path.

cameFrom

The cameFrom table is used to determine, well, where a node came from. It'll make more sense in the code below.

costSoFar

The costSoFar table is used to count the total cost it took to reach a node. Imagine node A has cost 10 and node B has cost 20. If node C is reached by exploring A, then B, then finally reaching C, then costSoFar[C] = 30.

The Cost Function

As mentioned previously, A Star works by finding the path of least-cost from start to goal. Along the way, we calculate a cost value at each node that represents how much cost we've accumulated so far. The cost function f(node) is given as follows:

f(node) = g(node) + h(node)

Easy enough! The value g(node) is called the backward cost and the value h(node) is called the forward cost. If you've ever learned about other pathfinding algorithms, you might recognize the idea of a backward cost. The backward cost is how much cost was accumulated to reach node. It can be found using the costSoFar table mentioned earlier. The forward cost is specific to the A Star algorithm. It's simply a value that says how far away the node we're at is from the goal node. There are many different functions that can be used for h(node). These functions are called heuristic functions. In this lesson, I will use the Manhattan Distance function, but there are many other valid functions that can be used. The Manhattan Distance function is given by:

manhattanDistance(node) = abs(goal.X - node.X) + abs(goal.Y - node.Y)

A close reader might notice that the manhattenDistance is zero if node == goal. This is not a coincidence. When we've reached the goal, there is no more forward cost!

Implementation

function CalculatePath(worldGrid, start, goal)
    local WIDTH = #worldGrid
    local HEIGHT = #worldGrid[1]

    -- make sure input coordinates are within the grid
    assert(start.X >= 1 and start.X <= WIDTH)
    assert(start.Y >= 1 and start.Y <= HEIGHT)
    assert(goal.X >= 1  and goal.X <= WIDTH)
    assert(goal.Y >= 1  and goal.Y <= HEIGHT)

    -- tables used to represent the pathfinding state
    local nodeSet = {}
    local openSet = {}
    local cameFrom = {}
    local costSoFar = {}
    local path = {}
    local winnerNode = nil

    -- initialize nodeSet.  each entry gets its coordinate
    -- value, its cost value, and f, which represents f(node)
    local nodeSet = {}
    for i = 1, WIDTH do
        nodeSet[i] = {}
        for j = 1, HEIGHT do
            nodeSet[i][j] = {
                coord = Vector2.new(i, j),
                cost = worldGrid[i][j],
                f = nil
            }
        end
    end

    -- references to our start and goal nodes
    local startNode = nodeSet[start.X][start.Y]
    local goalNode  = nodeSet[goal.X][goal.Y]

    -- helper function used to insert into the openSet.  openSet is
    -- sorted by its nodes' cost values, so make sure to
    -- insert at the correct index
    local function putInOpenSet(node, newCost)
        node.f = newCost + manhattanDistance(node, goalNode)
        for i, existingNode in ipairs(openSet) do
            if node.f < existingNode.f then
                table.insert(openSet, i, node)
                return
            end
        end
        table.insert(openSet, node)
    end

    -- initial state of the A* algorithm
    costSoFar[startNode] = 0
    openSet[1] = startNode

    -- main A* loop
    while #openSet > 0 do

        -- openSet is sorted, so the least-cost node
        -- is located at index 1
        local currentNode = openSet[1]
        table.remove(openSet, 1)

        -- if we've reached the goal coordinate, then we're done
        if currentNode.coord.X == goal.X and currentNode.coord.Y == goal.Y then
            winnerNode = currentNode
            break
        end

        -- explore all neighbors of currentNode
        local neighbors = getNeighbors(currentNode)
        for _, neighbor in ipairs(neighbors) do
            local newCost = costSoFar[currentNode] + worldGrid[neighbor.coord.X][neighbor.coord.Y]
            if not costSoFar[neighbor] or newCost < costSoFar[neighbor] then
                costSoFar[neighbor] = newCost
                putInOpenSet(neighbor, newCost)
                cameFrom[neighbor] = currentNode
            end
        end
    end

    -- follow the 'breadcrumbs' backwards to the starting node
    -- so we can retrace the path we took
    while winnerNode ~= startNode do
        table.insert(path, 1, winnerNode.coord)
        winnerNode = cameFrom[winnerNode]
    end

    return path
end

You might notice that there's no implementation yet for manhattanDistance or getNeighbors. These functions are implementation-dependent, meaning you may implement them however you'd like. For this tutorial, I will make getNeighbors return nodes that are north, south, east and west of the parent node, but I will ignore diagonal neighbors. I will also implement manhattanDistance as I described earlier in this lesson.

-- helper function for getNeighbors
local function isValidPosition(x, y)
    return x >= 1 and x <= WIDTH and y >= 1 and y <= HEIGHT
end

local function getNeighbors(node)
    local neighbors = {}
    if isValidPosition(node.coord.X - 1, node.coord.Y) then
        table.insert(neighbors, nodeSet[node.coord.X - 1][node.coord.Y])
    end
    if isValidPosition(node.coord.X + 1, node.coord.Y) then
        table.insert(neighbors, nodeSet[node.coord.X + 1][node.coord.Y])
    end
    if isValidPosition(node.coord.X, node.coord.Y - 1) then
        table.insert(neighbors, nodeSet[node.coord.X][node.coord.Y - 1])
    end
    if isValidPosition(node.coord.X, node.coord.Y + 1) then
        table.insert(neighbors, nodeSet[node.coord.X][node.coord.Y + 1])
    end
    return neighbors
end

local function manhattanDistance(node, goalNode)
    local xDist = math.abs(goalNode.coord.X - node.coord.X)
    local yDist = math.abs(goalNode.coord.Y - node.coord.Y)
    return xDist + yDist
end

Wrapping Up

Unlike PathfindingService, if you want to use this algorithm, it is up to you to call CalculatePath and convert the path coordinates into their respective world coordinates.

There are definitely faster ways of implementing A* than the code I provided here. If you'd like to speed the algorithm up, try using a MinHeap or a FibonacciHeap instead of a simple sorted array to represent openSet. I wanted to try to keep the explanation here simple, and I thought going with a sorted array was my best bet.

To close, I'd like to suggest using other tutorials on A* along with this one. This doesn't go super in-depth into how the algorithm works, but instead gives a brief overview. I think it's helpful to see how A* can be implemented in a Roblox context, hence this tutorial.