Lesson 6: Advanced Pathfinding

🎯 When Can You Trust A*?

A* ranks cells by f = g + h: the known cost from the start plus a guess of the cost still to go. The guess is what makes A* fast, and it is also the only thing that can make A* wrong. Think of a taxi driver's estimate of the fare: if it is never more than the real fare, you can plan around it; if it sometimes overshoots, you may skip the route that was actually cheapest.

Two properties tell you how far to trust h:

• Admissible: h(n) never overestimates the true cheapest cost from n to the goal. With an admissible heuristic, A* returns an optimal path.
• Consistent (also called monotone): for every move from n to a neighbor m with cost c, h(n) ≤ c + h(m). Every consistent heuristic (with h(goal) = 0) is also admissible. The extra promise is that the first time A* pops a cell from the heap, its g is already the cheapest, so the closed set and the lazy-deletion skip you wrote in the A* Pathfinding lesson never throw away a better route.

These are admissible on grids where every step costs at least the listed amount. If some terrain is cheaper than 1 per step (a road at 0.5, say), scale the heuristic by the cheapest step cost, or it can overestimate.

An overestimate in action

A common grid bug, and one you will see in plenty of strategy-game code, is allowing diagonal moves but guiding A* with Manhattan distance. Manhattan counts one diagonal step as 2, and even a diagonal that costs √2 ≈ 1.41 is cheaper than that, so the heuristic overestimates. Here is a complete program that searches the same small map with both heuristics:

import heapq
import math

MAP = [
    "S.....#.",
    "#.......",
    "....#...",
    "#...#...",
    "#...#...",
    "..#....G",
]
SQRT2 = math.sqrt(2)


def walkable(x, y):
    return 0 <= y < len(MAP) and 0 <= x < len(MAP[0]) and MAP[y][x] != "#"


def moves(x, y):
    """8-way moves, no corner cutting. Yields ((nx, ny), step_cost)."""
    for dx in (-1, 0, 1):
        for dy in (-1, 0, 1):
            if (dx or dy) and walkable(x + dx, y + dy):
                if dx and dy and not (walkable(x + dx, y) and walkable(x, y + dy)):
                    continue
                yield (x + dx, y + dy), SQRT2 if dx and dy else 1.0


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 manhattan(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])


def astar(start, goal, h):
    heap = [(h(start, goal), 0, start)]
    g = {start: 0.0}
    closed = set()
    counter = 0
    while heap:
        _, _, cell = heapq.heappop(heap)
        if cell in closed:
            continue
        closed.add(cell)
        if cell == goal:
            return g[cell], len(closed)
        for nxt, step in moves(*cell):
            new_g = g[cell] + step
            if nxt not in closed and new_g < g.get(nxt, math.inf):
                g[nxt] = new_g
                counter += 1
                heapq.heappush(heap, (new_g + h(nxt, goal), counter, nxt))
    return math.inf, len(closed)


start, goal = (0, 0), (7, 5)
for h in (octile, manhattan):
    cost, expanded = astar(start, goal, h)
    print(f"{h.__name__:9}: path cost {cost:.2f}, cells expanded {expanded}")

It prints:

octile   : path cost 10.24, cells expanded 24
manhattan: path cost 10.83, cells expanded 11

Manhattan expanded fewer cells and returned a path about 6% longer than the best one. That is the pattern to recognize: an overestimating heuristic makes A* greedier, which is faster and no longer optimal. Sometimes that's a fine trade. It just shouldn't happen by accident.

⚖️ Weighted A*

Sometimes you want the greedy trade, deliberately and with a guarantee. Weighted A* multiplies the heuristic by a weight w ≥ 1:

heapq.heappush(open_heap, (new_g + weight * h(nxt, goal), counter, nxt))

With an admissible h, the path weighted A* returns costs at most w times the optimal cost (the idea of weighting the heuristic goes back to Ira Pohl's work around 1970). A weight of 1.5 can never return a path more than 50% longer than the best, and in practice the paths are usually much closer. Weight 0 turns the heuristic off entirely, which is Dijkstra.

Here is the warm-up program, weighted_astar_solution.py. It searches one map with five weights and draws the expanded cells; press 0–4 to switch.

"""Weighted A*: Advanced Lesson 6 warm-up (solution).

The same map searched with different heuristic weights. Keys: 0 = Dijkstra
(no heuristic), 1 = A* (w = 1), 2 = w = 1.5, 3 = w = 2, 4 = w = 5.
Blue cells were expanded; the gold line is the path. Close the window to quit.
"""
import heapq
import math

import pygame


MAP = [
    "..............................",
    "..............................",
    "......#################.......",
    "......................#.......",
    "..S...................#.......",
    "......................#.......",
    "......................#.....G.",
    "......................#.......",
    "..........#############.......",
    "..............................",
    "..............................",
]
CELL = 30
ROWS, COLS = len(MAP), len(MAP[0])
SQRT2 = math.sqrt(2)
WEIGHTS = {pygame.K_0: 0.0, pygame.K_1: 1.0, pygame.K_2: 1.5, pygame.K_3: 2.0, pygame.K_4: 5.0}


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


def walkable(cell):
    x, y = cell
    return 0 <= x < COLS and 0 <= y < ROWS and MAP[y][x] != "#"


def neighbors(cell):
    """8-way moves; a diagonal may not cut a wall corner. Yields (next_cell, step_cost)."""
    x, y = cell
    for dx in (-1, 0, 1):
        for dy in (-1, 0, 1):
            if dx == dy == 0:
                continue
            nxt = (x + dx, y + dy)
            if not walkable(nxt):
                continue
            if dx and dy and not (walkable((x + dx, y)) and walkable((x, y + dy))):
                continue                               # no corner cutting
            yield nxt, SQRT2 if dx and dy else 1.0


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 manhattan(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])


def astar(start, goal, weight=1.0, h=octile):
    """Weighted A*: f = g + weight * h. weight 0 is Dijkstra, 1 is plain A*.

    Returns (path, cost, expanded) where expanded is the set of closed cells.
    """
    counter = 0
    open_heap = [(weight * h(start, goal), counter, start)]
    g = {start: 0.0}
    parent = {start: None}
    closed = set()
    while open_heap:
        _, _, cell = heapq.heappop(open_heap)
        if cell in closed:
            continue                                   # stale entry (lazy deletion)
        closed.add(cell)
        if cell == goal:
            path = []
            while cell is not None:
                path.append(cell)
                cell = parent[cell]
            return path[::-1], g[goal], closed
        for nxt, step in neighbors(cell):
            new_g = g[cell] + step
            if nxt not in closed and new_g < g.get(nxt, math.inf):
                g[nxt] = new_g
                parent[nxt] = cell
                counter += 1
                heapq.heappush(open_heap, (new_g + weight * h(nxt, goal), counter, nxt))
    return None, math.inf, closed


def main():
    pygame.init()
    screen = pygame.display.set_mode((COLS * CELL, ROWS * CELL + 60))
    pygame.display.set_caption("Weighted A* (0-4 change the weight)")
    clock = pygame.time.Clock()
    font = pygame.font.Font(None, 26)
    start, goal = find("S"), find("G")

    results = {w: astar(start, goal, w) for w in WEIGHTS.values()}
    weight = 1.0

    running = True
    while running:
        clock.tick(60)
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
            elif event.type == pygame.KEYDOWN and event.key in WEIGHTS:
                weight = WEIGHTS[event.key]

        path, cost, expanded = results[weight]
        screen.fill((15, 23, 42))
        for y in range(ROWS):
            for x in range(COLS):
                rect = (x * CELL, y * CELL, CELL - 1, CELL - 1)
                if MAP[y][x] == "#":
                    color = (71, 85, 105)
                elif (x, y) in expanded:
                    color = (30, 64, 120)
                else:
                    color = (30, 41, 59)
                pygame.draw.rect(screen, color, rect)
        points = [(x * CELL + CELL / 2, y * CELL + CELL / 2) for x, y in path]
        pygame.draw.lines(screen, (250, 204, 21), False, points, 4)
        for cell, color in ((start, (74, 222, 128)), (goal, (248, 113, 113))):
            pygame.draw.circle(screen, color, (cell[0] * CELL + CELL / 2, cell[1] * CELL + CELL / 2), 10)
        name = "Dijkstra" if weight == 0 else f"A*  w = {weight}"
        text = f"{name}:  path cost {cost:.2f}   cells expanded {len(expanded)}"
        screen.blit(font.render(text, True, (226, 232, 240)), (10, ROWS * CELL + 10))
        screen.blit(font.render("keys 0-4 change the weight", True, (148, 163, 184)), (10, ROWS * CELL + 34))
        pygame.display.flip()

    pygame.quit()
    for w in WEIGHTS.values():
        path, cost, expanded = results[w]
        print(f"w={w}: cost {cost:.2f}, expanded {len(expanded)}")


if __name__ == "__main__":
    main()

When the window closes it prints the table below (the lab's checker verifies these exact numbers, so you'll get them too):

On this map, weights up to 2 still found the optimal path: weight 1.5 expanded about two-thirds as many cells as plain A* (108 vs 160), and weight 2 about 60% (99). Weight 5 gave up 4% of path quality and saved only 4 more cells. Another map will give other numbers, which is exactly why you measure on your own levels before picking a weight.

🦘 Jump Point Search (Concept)

On an open grid where every step costs the same, there are usually many equally short paths: right-right-up, right-up-right and up-right-right all tie. Plain A* dutifully puts cells from all of them on the heap. Jump Point Search (Daniel Harabor and Alban Grastien, 2011) prunes those symmetric paths:

• From a cell, it keeps moving in a straight line (a "jump") instead of adding every cell on the way to the heap.
• It stops only at a jump point: the goal, or a cell with a forced neighbor, which appears where an obstacle creates a detour that a straight line can't cover.
• Only jump points go on the heap, so the heap stays small, and the result is still an optimal path.

The catch is the assumption: JPS relies on a uniform-cost grid. Mud, roads or any per-cell cost break the symmetry argument, and then you are back to A* (or a variant that the paper's later work describes). The Crowd Commander map in this lesson has mud, so JPS would not apply to it as is.

🌊 Flow Fields for Crowds

Picture a stadium emptying: nobody plans their own route. Signs at every corridor point toward the exits, and everyone just follows the nearest sign. A flow field is those signs. Instead of searching once per unit from the unit to the goal, you search once from the goal outward and record, for every cell, which way to go.

1. Integration field. Run Dijkstra (A* with no heuristic) starting at the goal. Each cell gets cost[cell], the cheapest cost from that cell to the goal. A unit stepping into a cell pays that cell's terrain cost, so when the search at cell C relaxes a neighbor N, the move it is pricing is N → C.
2. Direction field. For each cell, point at the neighbor with the lowest cost. Store it as a unit Vector2.
3. Follow. Each frame, a unit looks up the arrow under its feet and steers toward it with the steer_toward() you built in Steering & Flocking.

def integration_field(goal):
    """Dijkstra outward from the goal: cost[cell] = cheapest cost from cell TO the goal."""
    cost = {goal: 0.0}
    heap = [(0.0, 0, goal)]
    counter = 0
    done = set()
    while heap:
        c, _, cell = heapq.heappop(heap)
        if cell in done:
            continue
        done.add(cell)
        cx, cy = cell
        enter_cost = TERRAIN_COST[MAP[cy][cx]]          # a unit stepping INTO this cell pays it
        for nxt, step in moves(cell):
            new_c = c + step * enter_cost
            if new_c < cost.get(nxt, math.inf):
                cost[nxt] = new_c
                counter += 1
                heapq.heappush(heap, (new_c, counter, nxt))
    return cost


def flow_field(cost):
    """For each reachable cell, a unit Vector2 toward its cheapest neighbor."""
    field = {}
    for cell, c in cost.items():
        best, best_c = None, c
        for nxt, _ in moves(cell):
            if cost.get(nxt, math.inf) < best_c:
                best, best_c = nxt, cost[nxt]
        if best is not None:
            field[cell] = pygame.Vector2(best[0] - cell[0], best[1] - cell[1]).normalize()
    return field

The trade-off is clear once you count. One search on the exercise map visits every reachable floor cell (469 of them) once, however many units follow it. Per-unit A* is cheaper for one unit and gets more expensive with every unit you add. Flow fields win when many units share a goal (rally points, a base under attack, the end of a tower-defense lane); per-unit A* wins when every unit wants somewhere different. Cells missing from cost are unreachable, and a unit standing on one simply has no arrow, which is an honest answer rather than a crash.

Try both ideas in the demo below: the search buttons show which cells each algorithm expanded on the same map, and Flow field draws the arrows every cell would follow.

🧹 Pathfinding Hygiene

The algorithms are only half the story. Three habits keep pathfinding from eating your frame budget:

• Don't search every frame. Re-plan when something relevant changes (the goal moved, a wall appeared) or on a timer measured in seconds, not frames.
• Remember failures. A search for an unreachable goal is the most expensive one of all, because it explores every reachable cell before giving up. Re-running it every frame is a classic way to lose your frame rate. Cache the failure and retry only after a delay or a map change.
• Share work. Units heading to the same place can share one path or one flow field. Units that are close together can share a path and use steering to spread out.

RETRY_AFTER = 2.0                              # seconds

if unit.path is None and unit.retry_timer <= 0:
    unit.path = find_path(unit.cell, unit.goal)
    if unit.path is None:                      # unreachable: don't ask again next frame
        unit.retry_timer = RETRY_AFTER
unit.retry_timer -= dt

🏋️ Practice Exercise: Crowd Commander

"""Crowd Commander: Advanced Lesson 6 practice exercise (solution).

One Dijkstra search from the goal builds a flow field, and 120 units follow it
at once. Click a floor cell to move the goal. F toggles the arrows, H the cost
heat map. Brown cells are mud (3x cost). Close the window to quit.
"""
import heapq
import math
import random

import pygame


MAP = [
    "..............................",
    "..............................",
    "....#########.................",
    "............#......~~~~.......",
    "............#......~~~~.......",
    "............#......~~~~.......",
    "....#.......#......~~~~.......",
    "....#.......#......~~~~.......",
    "....#.......#......~~~~.......",
    "....#.......########~~~#####..",
    "....#.........................",
    "....#.........................",
    "....#######...................",
    "..............................",
    "..............................",
    "..............................",
    "..............................",
]
CELL = 32
ROWS, COLS = len(MAP), len(MAP[0])
WIDTH, HEIGHT = COLS * CELL, ROWS * CELL
TERRAIN_COST = {".": 1.0, "~": 3.0}          # "#" is a wall: not in the table
SQRT2 = math.sqrt(2)
NUM_UNITS = 120
MAX_SPEED = 110.0          # px/s
MAX_FORCE = 400.0          # px/s^2
REACTION_TIME = 0.2        # s
UNIT_RADIUS = 5.0
SEP_RADIUS = 12.0          # px
GOAL_RADIUS = 1.5 * CELL   # px: "close enough" to count as arrived


def walkable(cell):
    x, y = cell
    return 0 <= x < COLS and 0 <= y < ROWS and MAP[y][x] in TERRAIN_COST


def moves(cell):
    """8-way moves with no corner cutting. Yields (neighbor, step_length)."""
    x, y = cell
    for dx in (-1, 0, 1):
        for dy in (-1, 0, 1):
            if dx == dy == 0:
                continue
            nxt = (x + dx, y + dy)
            if not walkable(nxt):
                continue
            if dx and dy and not (walkable((x + dx, y)) and walkable((x, y + dy))):
                continue
            yield nxt, SQRT2 if dx and dy else 1.0


def integration_field(goal):
    """Dijkstra outward from the goal: cost[cell] = cheapest cost from cell TO the goal."""
    cost = {goal: 0.0}
    heap = [(0.0, 0, goal)]
    counter = 0
    done = set()
    while heap:
        c, _, cell = heapq.heappop(heap)
        if cell in done:
            continue
        done.add(cell)
        cx, cy = cell
        enter_cost = TERRAIN_COST[MAP[cy][cx]]          # a unit stepping INTO this cell pays it
        for nxt, step in moves(cell):
            new_c = c + step * enter_cost
            if new_c < cost.get(nxt, math.inf):
                cost[nxt] = new_c
                counter += 1
                heapq.heappush(heap, (new_c, counter, nxt))
    return cost


def flow_field(cost):
    """For each reachable cell, a unit Vector2 toward its cheapest neighbor."""
    field = {}
    for cell, c in cost.items():
        best, best_c = None, c
        for nxt, _ in moves(cell):
            if cost.get(nxt, math.inf) < best_c:
                best, best_c = nxt, cost[nxt]
        if best is not None:
            field[cell] = pygame.Vector2(best[0] - cell[0], best[1] - cell[1]).normalize()
    return field


def cell_of(pos):
    return (int(pos.x // CELL), int(pos.y // CELL))


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


def steer_toward(desired, vel):
    return ((desired - vel) / REACTION_TIME).clamp_magnitude(MAX_FORCE)


class Unit:
    def __init__(self, pos):
        self.pos = pygame.Vector2(pos)
        self.vel = pygame.Vector2()


def desired_velocity(unit, field, goal):
    """Follow the field; arrive (slow down) inside the goal cell's neighborhood."""
    to_goal = cell_center(goal) - unit.pos
    dist = to_goal.length()
    if dist < GOAL_RADIUS:
        if dist < 1:
            return pygame.Vector2()
        return to_goal * (MAX_SPEED * dist / GOAL_RADIUS / dist)
    direction = field.get(cell_of(unit.pos))
    if direction is None:
        return pygame.Vector2()                        # unreachable cell: wait
    return direction * MAX_SPEED


def buckets(units):
    """Spatial hash by grid cell, so separation only checks nearby units."""
    table = {}
    for unit in units:
        table.setdefault(cell_of(unit.pos), []).append(unit)
    return table


def separation(unit, table):
    push = pygame.Vector2()
    cx, cy = cell_of(unit.pos)
    for dx in (-1, 0, 1):
        for dy in (-1, 0, 1):
            for other in table.get((cx + dx, cy + dy), ()):
                offset = unit.pos - other.pos
                d2 = offset.length_squared()
                if other is not unit and 0 < d2 < SEP_RADIUS ** 2:
                    push += offset / d2
    if push.length_squared() == 0:
        return pygame.Vector2()
    return steer_toward(push.normalize() * MAX_SPEED, unit.vel)


def move_unit(unit, accel, dt):
    """Integrate, then resolve walls per axis: undo the axis that entered a wall."""
    unit.vel = (unit.vel + accel * dt).clamp_magnitude(MAX_SPEED)
    old_x = unit.pos.x
    unit.pos.x += unit.vel.x * dt
    if not walkable(cell_of(unit.pos)):
        unit.pos.x = old_x
        unit.vel.x = 0
    old_y = unit.pos.y
    unit.pos.y += unit.vel.y * dt
    if not walkable(cell_of(unit.pos)):
        unit.pos.y = old_y
        unit.vel.y = 0


def update(units, field, goal, dt):
    table = buckets(units)
    forces = []
    for unit in units:
        follow = steer_toward(desired_velocity(unit, field, goal), unit.vel)
        forces.append((follow + separation(unit, table) * 1.5).clamp_magnitude(MAX_FORCE))
    for unit, accel in zip(units, forces):
        move_unit(unit, accel, dt)


def spawn_units(seed=None):
    rng = random.Random(seed)
    floor = [(x, y) for y in range(ROWS) for x in range(0, 4) if walkable((x, y))]
    units = []
    for _ in range(NUM_UNITS):
        cx, cy = rng.choice(floor)
        units.append(Unit((cx * CELL + rng.uniform(4, CELL - 4), cy * CELL + rng.uniform(4, CELL - 4))))
    return units


def arrived(units, goal):
    center = cell_center(goal)
    return sum(1 for u in units if u.pos.distance_to(center) < GOAL_RADIUS * 2)


def main():
    pygame.init()
    screen = pygame.display.set_mode((WIDTH, HEIGHT))
    pygame.display.set_caption("Crowd Commander (click to move the goal, F arrows, H heat map)")
    clock = pygame.time.Clock()
    font = pygame.font.Font(None, 24)

    goal = (26, 7)
    cost = integration_field(goal)
    field = flow_field(cost)
    rebuilds = 1
    units = spawn_units(seed=3)
    show_arrows, show_heat = True, False

    running = True
    while running:
        dt = min(clock.tick(60) / 1000, 0.05)
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
            elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
                clicked = cell_of(pygame.Vector2(event.pos))
                if walkable(clicked) and clicked != goal:
                    goal = clicked
                    cost = integration_field(goal)          # ONE search for every unit
                    field = flow_field(cost)
                    rebuilds += 1
            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_f:
                    show_arrows = not show_arrows
                elif event.key == pygame.K_h:
                    show_heat = not show_heat

        update(units, field, goal, dt)

        screen.fill((15, 23, 42))
        top = max(cost.values()) or 1.0
        for y in range(ROWS):
            for x in range(COLS):
                ch = MAP[y][x]
                rect = (x * CELL, y * CELL, CELL - 1, CELL - 1)
                if ch == "#":
                    pygame.draw.rect(screen, (71, 85, 105), rect)
                    continue
                if show_heat and (x, y) in cost:
                    shade = max(0, min(255, int(40 + 180 * (1 - cost[(x, y)] / top))))
                    pygame.draw.rect(screen, (shade // 3, shade // 2, shade), rect)
                else:
                    pygame.draw.rect(screen, (92, 64, 51) if ch == "~" else (30, 41, 59), rect)
                if show_arrows and (x, y) in field:
                    c = cell_center((x, y))
                    pygame.draw.line(screen, (100, 116, 139), c, c + field[(x, y)] * 11, 2)
        pygame.draw.circle(screen, (248, 113, 113), cell_center(goal), 10, 3)
        for unit in units:
            pygame.draw.circle(screen, (250, 204, 21), unit.pos, UNIT_RADIUS)
        hud = f"arrived {arrived(units, goal)}/{len(units)}   field rebuilds {rebuilds}"
        screen.blit(font.render(hud, True, (226, 232, 240)), (8, 6))
        pygame.display.flip()

    pygame.quit()
    print(f"goal {goal}; field rebuilds: {rebuilds}")
    print(f"units near the goal: {arrived(units, goal)} of {len(units)}")


if __name__ == "__main__":
    main()

📓 Learning Journal

📝 Summary

A* is only as good as its heuristic. An admissible heuristic never overestimates and guarantees an optimal path; a consistent one also guarantees that a cell's first pop already carries its cheapest cost. Pick the heuristic that matches your moves (Manhattan for 4-way, octile for 8-way with √2 diagonals) and compare against Dijkstra when in doubt. Weighted A* makes the greedy trade on purpose, with a cost bound of w times optimal. Jump Point Search prunes symmetric paths on uniform-cost grids only. Flow fields flip the problem around: one search from the goal gives every cell an arrow, and any number of units can follow it with the steering from the previous lesson.

🔭 Looking Ahead

Your agents can now move well and find their way. Next they need to decide what to do. In Behavior Trees you build the decision structure many modern games use for NPCs, with priorities that can interrupt long-running actions.

❓ Common Questions

🎯 Quick Quiz

🌟 Going Further

• Count the savings: add the closed set's size to the Crowd Commander HUD by computing a per-unit A* to the same goal, and compare it with the one field build.
• Moving goal: make the goal follow the mouse, and rebuild the field only when the goal changes cell. Count rebuilds per second.
• Roads: add a road terrain costing 0.5. Then fix the heuristic in the warm-up so it stays admissible (scale it by the cheapest step cost).
• Read the paper: Harabor and Grastien, Online Graph Pruning for Pathfinding on Grid Maps (AAAI 2011), and Amit Patel's heuristics notes.
• Coming up in Game Dev III: Advanced: Real-time Strategy moves armies with octile A* and the pathfinding hygiene from this lesson.