Lesson 20: A* Pathfinding

🗺️ Maps Are Graphs

A map app doesn't see streets and houses; it sees places and the roads between them, each road with a travel time. Pathfinding works the same way. To a pathfinder, your tile map from the Tile Maps lesson is a graph:

• every walkable cell is a node, named by its (col, row) tuple;
• every step to a neighboring cell is an edge;
• every edge has a cost: 1 for plain ground, more for mud or water, and walls have no edges at all.

The question "how do I get from here to there?" becomes "which chain of edges from start to goal has the smallest total cost?". This lesson builds three answers, each one smarter than the last.

🌊 Flood Fill With Breadth-First Search

Pour water on the start cell. First it reaches the cells one step away, then two steps, then three, spreading around walls until it has filled everything it can reach. That is breadth-first search (BFS): visit cells in order of how many steps away they are.

The tool that makes BFS easy is collections.deque, a list that is fast to add to at one end and take from at the other. New cells go on the right with append(); the oldest cell comes off the left with popleft(). Because the oldest cells are always the nearest, the first time BFS reaches a cell is by the fewest steps.

from collections import deque

MAP = [
    "..........",
    ".####.....",
    ".#..#..##.",
    ".####..#..",
    ".......#..",
]


def bfs_steps(grid, start):
    """Flood fill: the number of 4-way steps from start to every reachable cell."""
    steps = {start: 0}
    queue = deque([start])
    while queue:
        col, row = queue.popleft()                 # oldest first: nearest cells first
        for dx, dy in ((1, 0), (-1, 0), (0, 1), (0, -1)):
            c, r = col + dx, row + dy
            if 0 <= c < len(grid[0]) and 0 <= r < len(grid) and grid[r][c] != "#":
                if (c, r) not in steps:            # the first visit is the shortest
                    steps[(c, r)] = steps[(col, row)] + 1
                    queue.append((c, r))
    return steps


steps = bfs_steps(MAP, (0, 0))
for r, line in enumerate(MAP):
    cells = []
    for c, ch in enumerate(line):
        if ch == "#":
            cells.append("##")
        elif (c, r) in steps:
            cells.append(f"{steps[(c, r)]:2d}")
        else:
            cells.append(" ?")                     # walled in: unreachable
    print(" ".join(cells))

It prints the number of steps to every cell, and a ? for the two cells walled in completely:

 0  1  2  3  4  5  6  7  8  9
 1 ## ## ## ##  6  7  8  9 10
 2 ##  ?  ? ##  7  8 ## ## 11
 3 ## ## ## ##  8  9 ## 13 12
 4  5  6  7  8  9 10 ## 14 13

Flood fill is useful on its own: "which cells can the player reach?", "which room is this cell in?", or the validate() question from the level editor, "can the start reach the goal?". But BFS counts steps. As soon as some steps cost more than others, it can pick a short route straight through a swamp over a slightly longer road around it.

💰 Costs and Dijkstra

Dijkstra's algorithm fixes that by always expanding the cell with the smallest total cost so far (called g), instead of the oldest one. For that, it needs a priority queue: a collection that always hands back its smallest item. Python has one built in.

Here is the core of Dijkstra, written so that it becomes A* with a one-line change in the next section:

counter = 0
open_heap = [(0, counter, start)]           # (g so far, tie-breaker, cell)
g = {start: 0}                              # cheapest known cost to each cell
came_from = {}                              # parent pointers, to rebuild the path
closed = set()                              # cells whose cheapest cost is final
while open_heap:
    _, _, cell = heapq.heappop(open_heap)
    if cell in closed:
        continue                            # a stale, more expensive copy: skip it
    if cell == goal:
        break
    closed.add(cell)
    for nxt, step in neighbors(cell):
        new_g = g[cell] + step
        if new_g < g.get(nxt, math.inf):    # found a cheaper way to nxt
            g[nxt] = new_g
            came_from[nxt] = cell
            counter += 1
            heapq.heappush(open_heap, (new_g, counter, nxt))

Look closely at if cell in closed: continue. When the search finds a cheaper route to a cell that is already waiting in the heap, it can't reach into the heap and change the old entry, so it simply pushes a new, cheaper one. The old entry is now stale. When it eventually comes out of the heap, the cell has already been finished through the cheaper entry, so it is skipped. This trick is called lazy deletion. Forget it, and the stale entry can expand the cell a second time from the more expensive route and overwrite good parent pointers.

g.get(nxt, math.inf) treats a cell the search has never reached as infinitely expensive: math.inf is a float bigger than any number, so the first route found to a cell always counts as cheaper. When the goal comes out of the heap, walk came_from backward from the goal to the start and reverse the list: that is the path.

⭐ A*: Search With a Hint

Dijkstra spreads out evenly in every direction, even away from the goal, like looking for your keys by searching the whole house in widening circles. A* adds a hint: an estimate h of how far each cell still is from the goal. It expands the cell with the smallest

f = g + h  =  cost so far + estimated cost to go

so cells that are both cheap to reach and in the right direction come first. The only code change is the priority you push: new_g + h(nxt, goal) instead of new_g. With h always 0, A* is Dijkstra.

Picking the heuristic

A* only promises the cheapest path if h never overestimates the real remaining cost. The right choice depends on how units can move:

Why not Manhattan everywhere? With diagonals, going 3 cells right and 3 down takes three diagonal steps costing 3 × √2 ≈ 4.24, but Manhattan guesses 6. An estimate that is too high makes A* avoid routes that are actually cheaper, so it can return a longer path than necessary. Octile distance is exactly the cost of the best 8-way route on open ground, so it never overestimates. Because the cheapest terrain costs 1, both formulas stay honest on mud, too.

Here is a complete program you can run in the terminal. It searches the same map twice, once as Dijkstra and once as A* with octile distance, and draws the path. One piece of Python in it is new: neighbors() uses yield instead of return. A function containing yield hands back one value each time the for loop asks for the next item, then carries on from where it stopped, so for nxt, step in neighbors(cell): loops over the neighbors without building a list first.

import heapq
import math

MAP = [
    "S.......#...........",
    "........#...........",
    "..~~~~..#.....####..",
    "..~~~~..#........#..",
    "..~~~~..#........#..",
    "..~~~~......######..",
    "....................",
    "...........#.......G",
]
COSTS = {".": 1, "~": 3, "#": None, "S": 1, "G": 1}
SQRT2 = math.sqrt(2)


def find(ch):
    for row, line in enumerate(MAP):
        if ch in line:
            return line.index(ch), row


def cost_at(cell):
    col, row = cell
    if 0 <= col < len(MAP[0]) and 0 <= row < len(MAP):
        return COSTS[MAP[row][col]]
    return None


def neighbors(cell):
    """8-way moves; a diagonal may not cut past a wall corner."""
    col, row = cell
    for dx, dy in ((1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1)):
        nxt = (col + dx, row + dy)
        cost = cost_at(nxt)
        if cost is None:
            continue
        if dx and dy:
            if cost_at((col + dx, row)) is None or cost_at((col, row + dy)) is None:
                continue
            yield nxt, SQRT2 * cost
        else:
            yield nxt, cost


def zero(a, b):
    return 0                                          # no hint at all: Dijkstra


def octile(a, b):
    dx, dy = abs(a[0] - b[0]), abs(a[1] - b[1])
    return max(dx, dy) + (SQRT2 - 1) * min(dx, dy)


def search(start, goal, heuristic):
    """A* (Dijkstra when heuristic is zero). Returns (path, cost, cells_explored)."""
    counter = 0
    open_heap = [(heuristic(start, goal), counter, start)]
    g = {start: 0}
    came_from = {}
    closed = set()
    while open_heap:
        _, _, cell = heapq.heappop(open_heap)
        if cell in closed:
            continue                                  # stale copy: already done
        if cell == goal:
            path = [cell]
            while cell in came_from:
                cell = came_from[cell]
                path.append(cell)
            return path[::-1], g[goal], len(closed)
        closed.add(cell)
        for nxt, step in neighbors(cell):
            new_g = g[cell] + step
            if new_g < g.get(nxt, math.inf):
                g[nxt] = new_g
                came_from[nxt] = cell
                counter += 1
                heapq.heappush(open_heap, (new_g + heuristic(nxt, goal), counter, nxt))
    return None, math.inf, len(closed)


start, goal = find("S"), find("G")
for name, h in (("Dijkstra", zero), ("A* (octile)", octile)):
    path, cost, explored = search(start, goal, h)
    print(f"{name:12} cost {cost:.2f}   explored {explored} cells")

rows = [list(line) for line in MAP]
for col, row in path[1:-1]:
    rows[row][col] = "*"
print("\n".join("".join(r) for r in rows))

On this map it prints the same cost for both, 23.07, but Dijkstra expands 131 cells while A* expands 43. Same answer, a third of the work: that is the whole point of the hint.

Try it: press Run with the heuristic on none (Dijkstra) and watch the purple area spread in every direction; then switch back to octile and run again. Tap cells to build walls and see the path react.

↗️ Diagonals Without Cutting Corners

Allowing 8-way movement makes paths shorter and more natural, with two rules. First, a diagonal step is longer: it costs √2 times the terrain cost, not 1. Second, a unit must never squeeze through the corner between two walls that touch at a point:

. #        Moving diagonally from S (bottom-left) to the top-right cell
S .        would clip the corner of the wall #. Not allowed.

. .        Here both side cells are open, so the diagonal is fine.
S .

The rule: a diagonal move from (col, row) by (dx, dy) is allowed only if both side cells, (col + dx, row) and (col, row + dy), are walkable.

def neighbors(grid, cell, diagonal=True):
    """Yield (next_cell, step_cost). Diagonals may not squeeze past a wall corner."""
    col, row = cell
    steps = [(1, 0), (-1, 0), (0, 1), (0, -1)]
    if diagonal:
        steps += [(1, 1), (1, -1), (-1, 1), (-1, -1)]
    for dx, dy in steps:
        nxt = (col + dx, row + dy)
        cost = cost_at(grid, nxt)
        if cost is None:
            continue
        if dx and dy:
            # Corner rule: both cells beside the diagonal must be walkable.
            if cost_at(grid, (col + dx, row)) is None or cost_at(grid, (col, row + dy)) is None:
                continue
            yield nxt, SQRT2 * cost
        else:
            yield nxt, cost

As before, yield hands back one neighbor at a time, so for nxt, step in neighbors(grid, cell): reads naturally. The cost of a step is the length of the step times the cost of the cell you step into, and cost_at() returns None both for walls and for cells off the edge of the map, so one check covers both.

🚶 Following and Smoothing the Path

A path is a list of cells. To walk it, turn each cell into the pixel position of its center and move toward the first one with the move_toward() helper from the NPC State Machines lesson; when you arrive, drop it from the list and head for the next.

def cell_center(cell):
    return pygame.Vector2(cell[0] * TILE + TILE / 2, cell[1] * TILE + TILE / 2)


waypoints = [cell_center(c) for c in path[1:]]      # skip the cell we stand in
if waypoints:
    unit, arrived = move_toward(unit, waypoints[0], UNIT_SPEED, dt)
    if arrived:
        waypoints.pop(0)

Plan when something changes, not every frame. Run A* when the player clicks a new goal or a wall is added or removed, and store the result. Searching again every frame repeats the same work sixty times a second for the same answer.

Smoothing, without cheating

Grid paths zigzag: a unit heading slightly off the eight directions walks a staircase. Smoothing removes waypoints the unit could skip by walking straight. From each waypoint, look for the farthest later waypoint it can reach in a straight line, jump to it, and repeat.

"Can reach in a straight line" needs care. It must not clip a wall corner, and it must not cut straight across mud that the A* path deliberately walked around: that would make the smoothed path more expensive than the one A* chose. So a shortcut is allowed only if the straight line touches plain ground only. The check walks along the line in small steps (math.dist(a, b) is the straight-line distance between two points) and tests four points around the unit's body at each step:

def line_is_cheap(grid, a, b, radius=0.3):
    """True if walking straight from the center of a to the center of b only
    touches plain ground (cost 1): no walls, no mud, no clipped corners."""
    ax, ay = a[0] + 0.5, a[1] + 0.5
    bx, by = b[0] + 0.5, b[1] + 0.5
    samples = int(math.dist(a, b) / 0.1) + 1
    for i in range(samples + 1):
        t = i / samples
        x, y = ax + (bx - ax) * t, ay + (by - ay) * t
        for ox, oy in ((-radius, -radius), (radius, -radius), (-radius, radius), (radius, radius)):
            if cost_at(grid, (math.floor(x + ox), math.floor(y + oy))) != 1:
                return False
    return True


def smooth_path(grid, path):
    """Skip waypoints only where the straight line stays on plain ground."""
    if path is None or len(path) < 3:
        return path
    result = [path[0]]
    i = 0
    while i < len(path) - 1:
        j = len(path) - 1
        while j > i + 1 and not line_is_cheap(grid, path[i], path[j]):
            j -= 1                              # try a closer waypoint
        result.append(path[j])
        i = j
    return result

Where a stretch of the A* path crosses mud, no shortcut is taken and the unit follows A*'s own steps. Everywhere else, the straight line is never longer than the zigzag it replaces, so smoothing can only make the walk shorter. The sampling step (a tenth of a cell) and the body radius (0.3 cells) are cautious choices; the lab's tests check that every shortcut stays off walls and mud on random maps.

🏋️ Practice Exercise: Click to Move

"""Click to Move: Intermediate Lesson 20 practice exercise (solution).

Left-click anywhere and the unit plans the cheapest route with A*, then walks
it. Walls block, mud costs 3x, diagonal steps never cut corners, and the path
is smoothed only where that can't make it more expensive.

Mouse: left-click sets the goal, right-click adds or removes a wall.
Keys: D diagonal moves on/off, S smoothing on/off, V show explored cells.
"""
import heapq
import math

import pygame


TILE = 32
MAP = [
    "..............................",
    "..............................",
    "......#.............~~~~......",
    "......#.............~~~~......",
    "......#......#......~~~~......",
    "......#......#......~~~~......",
    "......#......#................",
    "......#......#########........",
    ".............#................",
    ".............#.......#........",
    "......~~~~~~~#.......#........",
    "......~~~~~~~#.......#........",
    "......~~~~~~~........#........",
    "......###############.........",
    "..............................",
    "..............................",
    "..............................",
]
COLS, ROWS = len(MAP[0]), len(MAP)
HUD_H = 30
WIDTH, HEIGHT = COLS * TILE, ROWS * TILE + HUD_H
COSTS = {".": 1, "~": 3, "#": None}          # None = can't walk there
UNIT_SPEED = 140                            # pixels per second
SQRT2 = math.sqrt(2)


def make_grid(rows=MAP):
    return [list(row) for row in rows]


def cost_at(grid, cell):
    """Cost of stepping INTO cell, or None for walls and cells off the map."""
    col, row = cell
    if 0 <= col < len(grid[0]) and 0 <= row < len(grid):
        return COSTS[grid[row][col]]
    return None


def neighbors(grid, cell, diagonal=True):
    """Yield (next_cell, step_cost). Diagonals may not squeeze past a wall corner."""
    col, row = cell
    steps = [(1, 0), (-1, 0), (0, 1), (0, -1)]
    if diagonal:
        steps += [(1, 1), (1, -1), (-1, 1), (-1, -1)]
    for dx, dy in steps:
        nxt = (col + dx, row + dy)
        cost = cost_at(grid, nxt)
        if cost is None:
            continue
        if dx and dy:
            # Corner rule: both cells beside the diagonal must be walkable.
            if cost_at(grid, (col + dx, row)) is None or cost_at(grid, (col, row + dy)) is None:
                continue
            yield nxt, SQRT2 * cost
        else:
            yield nxt, cost


def manhattan(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])


def octile(a, b):
    dx, dy = abs(a[0] - b[0]), abs(a[1] - b[1])
    return max(dx, dy) + (SQRT2 - 1) * min(dx, dy)


def astar(grid, start, goal, diagonal=True):
    """Return (path, cost, explored). path is a list of cells, or None if unreachable."""
    h = octile if diagonal else manhattan
    counter = 0                                 # tie-breaker, so heapq never compares cells
    open_heap = [(h(start, goal), counter, start)]
    g = {start: 0}
    came_from = {}
    closed = set()
    while open_heap:
        _, _, cell = heapq.heappop(open_heap)
        if cell in closed:
            continue                            # a stale, more expensive copy: skip it
        if cell == goal:
            path = [cell]
            while cell in came_from:
                cell = came_from[cell]
                path.append(cell)
            path.reverse()
            return path, g[goal], closed
        closed.add(cell)
        for nxt, step in neighbors(grid, cell, diagonal):
            new_g = g[cell] + step
            if new_g < g.get(nxt, math.inf):
                g[nxt] = new_g
                came_from[nxt] = cell
                counter += 1
                heapq.heappush(open_heap, (new_g + h(nxt, goal), counter, nxt))
    return None, math.inf, closed


def path_cost(grid, path):
    """What it costs to walk a list of cells step by step (the A* cost model)."""
    total = 0
    for a, b in zip(path, path[1:]):
        length = math.dist(a, b)
        total += length * cost_at(grid, b)
    return total


def line_is_cheap(grid, a, b, radius=0.3):
    """True if walking straight from the center of a to the center of b only
    touches plain ground (cost 1): no walls, no mud, no clipped corners."""
    ax, ay = a[0] + 0.5, a[1] + 0.5
    bx, by = b[0] + 0.5, b[1] + 0.5
    samples = int(math.dist(a, b) / 0.1) + 1
    for i in range(samples + 1):
        t = i / samples
        x, y = ax + (bx - ax) * t, ay + (by - ay) * t
        for ox, oy in ((-radius, -radius), (radius, -radius), (-radius, radius), (radius, radius)):
            if cost_at(grid, (math.floor(x + ox), math.floor(y + oy))) != 1:
                return False
    return True


def smooth_path(grid, path):
    """Skip waypoints only where the straight line stays on plain ground."""
    if path is None or len(path) < 3:
        return path
    result = [path[0]]
    i = 0
    while i < len(path) - 1:
        j = len(path) - 1
        while j > i + 1 and not line_is_cheap(grid, path[i], path[j]):
            j -= 1                              # try a closer waypoint
        result.append(path[j])
        i = j
    return result


def cell_center(cell):
    return pygame.Vector2(cell[0] * TILE + TILE / 2, cell[1] * TILE + TILE / 2)


def pixel_to_cell(pos):
    return int(pos[0] // TILE), int(pos[1] // TILE)


def move_toward(pos, target, speed, dt):
    """Return (new_pos, arrived) without overshooting (from the NPC State Machines lesson)."""
    offset = target - pos
    dist = offset.length()
    step = speed * dt
    if dist <= step:
        return pygame.Vector2(target), True
    return pos + offset / dist * step, False


def plan(grid, unit, goal, diagonal, smoothing):
    """Run A* from the unit's cell. Returns (raw_path, waypoints, explored, cost)."""
    raw_path, cost, explored = astar(grid, pixel_to_cell(unit), goal, diagonal)
    route = smooth_path(grid, raw_path) if smoothing else raw_path
    waypoints = [cell_center(c) for c in route[1:]] if route else []
    return raw_path, waypoints, explored, cost


def main():
    pygame.init()
    screen = pygame.display.set_mode((WIDTH, HEIGHT))
    pygame.display.set_caption("Click to Move")
    clock = pygame.time.Clock()
    font = pygame.font.Font(None, 22)

    grid = make_grid()
    unit = cell_center((2, 8))
    goal = None
    raw_path, waypoints, explored = None, [], set()
    cost = 0.0
    diagonal, smoothing, show_explored = True, True, True
    plans = []

    running = True
    while running:
        dt = clock.tick(60) / 1000
        replan = False
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
            elif event.type == pygame.MOUSEBUTTONDOWN and event.pos[1] < ROWS * TILE:
                cell = pixel_to_cell(event.pos)
                if event.button == 1 and cost_at(grid, cell) is not None:
                    goal = cell
                    replan = True
                elif event.button == 3 and cell != pixel_to_cell(unit):
                    col, row = cell
                    grid[row][col] = "." if grid[row][col] == "#" else "#"
                    replan = True               # the map changed: replan once, not every frame
            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_d:
                    diagonal = not diagonal
                    replan = True
                elif event.key == pygame.K_s:
                    smoothing = not smoothing
                    replan = True
                elif event.key == pygame.K_v:
                    show_explored = not show_explored

        if replan and goal is not None:
            raw_path, waypoints, explored, cost = plan(grid, unit, goal, diagonal, smoothing)
            plans.append("unreachable" if raw_path is None else f"{cost:.2f}")
        if waypoints:
            unit, arrived = move_toward(unit, waypoints[0], UNIT_SPEED, dt)
            if arrived:
                waypoints.pop(0)

        screen.fill((20, 22, 30))
        for row in range(ROWS):
            for col in range(COLS):
                ch = grid[row][col]
                rect = (col * TILE, row * TILE, TILE - 1, TILE - 1)
                color = {".": (44, 50, 64), "~": (96, 72, 44), "#": (130, 136, 150)}[ch]
                if show_explored and (col, row) in explored and ch != "#":
                    color = (70, 60, 100) if ch == "." else (120, 80, 90)
                pygame.draw.rect(screen, color, rect)
        if raw_path:
            pygame.draw.lines(screen, (120, 120, 200), False, [cell_center(c) for c in raw_path], 2)
        if waypoints:
            pygame.draw.lines(screen, (255, 215, 90), False, [unit] + waypoints, 3)
        if goal is not None:
            pygame.draw.circle(screen, (240, 80, 80), cell_center(goal), 9)
        pygame.draw.circle(screen, (90, 230, 140), unit, 11)
        status = (f"diagonal {'ON' if diagonal else 'off'} (D)   smoothing {'ON' if smoothing else 'off'} (S)"
                  f"   explored {len(explored)} (V)   path cost {cost:.2f}")
        screen.blit(font.render(status, True, (220, 220, 230)), (10, ROWS * TILE + 8))
        pygame.display.flip()

    pygame.quit()
    print("Plans:", ", ".join(plans) if plans else "none")


if __name__ == "__main__":
    main()

📓 Learning Journal

📝 Summary

You treated the tile map as a graph. Breadth-first search flooded it to count steps and find what is reachable. Dijkstra added costs, using heapq with a tie-breaking counter and skipping stale entries, and A* added a hint, f = g + h, that finds the same cheapest path with far less searching as long as the hint never overestimates: Manhattan for 4-way moves, octile for 8-way. Diagonal steps cost √2 and never cut corners. Finally, the unit followed the path, planned only when something changed, and walked straight wherever a shortcut stayed on plain ground.

🔭 Looking Ahead

Your NPCs can now think and find their way. Next, in Screen Shake, Tweens & Juice, you make every hit, jump and pickup feel great with shake, hit stop, flashes and tweens.

❓ Common Questions

🎯 Quick Quiz

🌟 Going Further

• Guards that find their way: give the guard from the NPC State Machines lesson A* for its SEARCH and INVESTIGATE states, planning once on enter().
• Reachability check: add a flood fill to the level editor's validate() so it refuses levels where the goal can't be reached from the start.
• A flow field: run Dijkstra once from the goal over the whole map, then let fifty units each step to their cheapest neighbor.
• Measure it: time astar() with time.perf_counter() on your own maps, with and without the heuristic, and write down the numbers.
• Read the docs: Python's heapq and collections.deque.
• Coming up in Game Dev III: Advanced: Advanced Pathfinding covers heuristics in depth, weighted A* and jump point search.