11.1 Pathfinding (A*, Navmesh)

This chapter answers one very concrete question: a character standing at point A needs to walk to point B, without walking through walls, and without the trip looking robotic. Earlier chapters gave you the raw material for this — graphs (a set of nodes connected by edges), breadth-first search (BFS), and the priority queue you built by hand for Dijkstra's algorithm. This chapter puts all three to work building A* (said "A-star"), the pathfinding algorithm almost every game uses, and then shows how the same idea scales from a small grid up to Unity's NavMesh system used in full 3D worlds.

Every section below follows the same shape: a small piece of runnable C# code, its real output (or a worked trace when there is no console to print to), and then a plain explanation of what happened and why.

1. The problem: getting from A to B without walking into walls

Pathfinding is the job of finding a route through a game level from a start position to a goal position that does not pass through anything solid, and ideally costs as little as possible (shortest distance, least time, or lowest "danger" — whatever your game cares about). It shows up everywhere: an enemy walking to the player, a companion following you through a dungeon, a whole crowd of NPCs crossing a city square, or a unit in a strategy game moving to a clicked tile.

Here is a small top-down room. S is the start, G is the goal, and # is a wall:

A straight line from S to G would cut straight through the wall — not allowed. The character has to find some route that only crosses open (.) cells. That sounds simple to a human staring at the picture, but a computer sees none of this shape; it only sees a big block of numbers. Turning "the shape of the level" into something an algorithm can search is the first real problem, and it is exactly the graph-representation problem from the data structures chapter, applied to space instead of, say, a social network or a city map.

It is worth separating two things that sound similar but are not: pathfinding is global planning — figuring out, ahead of time, a whole route across the level using a map of the world. Obstacle avoidance (or local steering) is reactive — swerving around a wall or another character you are about to bump into, using only what is right in front of you, with no map at all. A good game character needs both: pathfinding to pick a sensible overall route, and steering to react to things that were not on the map (another moving character, a barrel someone just knocked over). This chapter is entirely about the first one; Unity's NavMeshAgent, covered later, actually does a bit of both for you.

2. Representing the world as a graph: the grid

Every pathfinding algorithm you already know — BFS, Dijkstra — works on a graph: a set of nodes connected by edges. To use them here, we need to turn the level into one. The simplest, most common way to start is a grid: chop the walkable area into equal-sized square cells, make each open cell a node, and connect each cell to its neighbors with an edge.

The choice between 4-directional and 8-directional movement changes the shape of every path: 4-directional paths only ever look like staircases; 8-directional paths can cut diagonally and look more natural, but need a different edge cost (a diagonal step covers more real distance than an orthogonal one) and a rule for corners (see the tip below). This chapter builds the 4-directional case first because it keeps the numbers simple to trace by hand; Exercise 2 asks you to extend it to 8 directions.

In code, the simplest grid is just a 2D array of booleans, one per cell, saying whether it is walkable:

public class GridMap
{
    public readonly bool[,] Walkable;
    public readonly int Width, Height;

    public GridMap(bool[,] walkable)
    {
        Walkable = walkable;
        Width = walkable.GetLength(0);
        Height = walkable.GetLength(1);
    }

    public bool IsWalkable(int x, int y)
    {
        if (x < 0 || y < 0 || x >= Width || y >= Height) return false;
        return Walkable[x, y];
    }
}

One thing worth saying clearly now, before it causes confusion later: the (x, y) here is a grid coordinate — column and row index into the array — not a Unity world-space position. In this chapter's diagrams, y = 0 is the top row and y grows downward, purely because that is easier to read on a printed page. Mapping a grid cell to an actual 3D position in the scene (multiply by a cell size, add a grid origin) is a separate, small piece of code you write once when you place tiles — it is not part of the search algorithm itself.

Let's build the room from Section 1 as a GridMap and check a few cells:

bool[,] walkable = new bool[7, 5];
for (int x = 0; x < 7; x++)
    for (int y = 0; y < 5; y++)
        walkable[x, y] = true;

// carve the wall: column x = 3, rows y = 0..3 (row 4 is left open as a gap)
walkable[3, 0] = false;
walkable[3, 1] = false;
walkable[3, 2] = false;
walkable[3, 3] = false;

var map = new GridMap(walkable);

Console.WriteLine("IsWalkable(3, 2) -> " + map.IsWalkable(3, 2));
Console.WriteLine("IsWalkable(3, 4) -> " + map.IsWalkable(3, 4));
Console.WriteLine("IsWalkable(6, 2) -> " + map.IsWalkable(6, 2));
Console.WriteLine("IsWalkable(-1, 2) -> " + map.IsWalkable(-1, 2));

Output:

IsWalkable(3, 2) -> False
IsWalkable(3, 4) -> True
IsWalkable(6, 2) -> True
IsWalkable(-1, 2) -> False

Cell (3, 2) is inside the wall, so it is unwalkable. Cell (3, 4) is the one gap we left open in the wall — this is the only place a path can cross from the left side of the room to the right side, and it is going to matter a lot in a few sections. (-1, 2) is outside the array entirely, so IsWalkable returns false instead of crashing — always bounds-check before indexing, exactly like the array chapter warned you about.

3. Recap: BFS finds the shortest path when every step costs the same

Quick recap from the data structures chapter: BFS (breadth-first search) explores a graph one "ring" at a time — first every node one step from the start, then every node two steps away, and so on — using a plain FIFO queue. Because it always finishes an entire ring before starting the next one, the first time it reaches any node is guaranteed to be by the shortest possible number of edges (hops). That guarantee only holds when every edge is worth exactly the same — BFS has no idea that one edge might be "worse" than another, because a plain graph does not even have a concept of edge weight.

Here is that BFS, plus two small helpers we will reuse for the rest of this chapter, written generically over grid coordinates:

static IEnumerable<(int x, int y)> Neighbors((int x, int y) cell, bool[,] walkable)
{
    (int dx, int dy)[] dirs = { (1, 0), (-1, 0), (0, 1), (0, -1) };
    foreach (var (dx, dy) in dirs)
    {
        int nx = cell.x + dx, ny = cell.y + dy;
        if (nx >= 0 && ny >= 0 && nx < walkable.GetLength(0) && ny < walkable.GetLength(1) && walkable[nx, ny])
            yield return (nx, ny);
    }
}

static List<(int x, int y)> ReconstructPath(Dictionary<(int x, int y), (int x, int y)> cameFrom,
                                             (int x, int y) start, (int x, int y) goal)
{
    var path = new List<(int x, int y)> { goal };
    var current = goal;
    while (current != start)
    {
        current = cameFrom[current];
        path.Add(current);
    }
    path.Reverse();
    return path;
}

static List<(int x, int y)> BFS((int x, int y) start, (int x, int y) goal, bool[,] walkable)
{
    var queue = new Queue<(int x, int y)>();
    var cameFrom = new Dictionary<(int x, int y), (int x, int y)>();
    var visited = new HashSet<(int x, int y)> { start };
    queue.Enqueue(start);

    while (queue.Count > 0)
    {
        var current = queue.Dequeue();
        if (current.Equals(goal)) break;

        foreach (var next in Neighbors(current, walkable))
        {
            if (visited.Contains(next)) continue;
            visited.Add(next);
            cameFrom[next] = current;
            queue.Enqueue(next);
        }
    }
    return ReconstructPath(cameFrom, start, goal);
}

Now a tiny 3x2 patch of ground with one bad tile in it — ~ is mud, everything else is normal ground:

bool[,] walkable = { { true, true }, { true, true }, { true, true } }; // 3 columns x 2 rows, all walkable

var path = BFS((0, 0), (2, 0), walkable);
Console.WriteLine(string.Join(" -> ", path));

Output:

(0, 0) -> (1, 0) -> (2, 0)

BFS takes the straight line right through the mud, because that is the path with the fewest hops (2), and hops are the only thing BFS can see. If we add up the real cost of that route by hand — the cost of entering each cell after the start — it is 5 (mud) + 1 (goal) = 6. BFS has no way to know that, and no way to prefer a longer-but-cheaper route. That is exactly the gap Dijkstra's algorithm fills.

4. Recap: Dijkstra's algorithm and the priority queue

Dijkstra's algorithm solves the version of the problem BFS cannot: shortest total cost, when edges are allowed to cost different amounts. The change from BFS is small but important: swap the FIFO queue for a priority queue (a queue that always hands you the item with the smallest priority value first, instead of the item that has waited longest — you built one of these on a binary heap in the data structures chapter), and use each node's best-known total cost so far as its priority.

Here is that priority queue again, as a compact binary min-heap — the same shape you built before, ready to reuse for the rest of this chapter:

public class PriorityQueue<TItem>
{
    private readonly List<(TItem item, float priority)> heap = new List<(TItem item, float priority)>();

    public int Count => heap.Count;

    public void Enqueue(TItem item, float priority)
    {
        heap.Add((item, priority));
        int i = heap.Count - 1;
        while (i > 0)
        {
            int parent = (i - 1) / 2;
            if (heap[parent].priority <= heap[i].priority) break;
            (heap[parent], heap[i]) = (heap[i], heap[parent]); // swap
            i = parent;
        }
    }

    public TItem Dequeue()
    {
        TItem best = heap[0].item;
        int last = heap.Count - 1;
        heap[0] = heap[last];
        heap.RemoveAt(last);

        int i = 0;
        while (true)
        {
            int left = i * 2 + 1;
            int right = i * 2 + 2;
            int smallest = i;
            if (left < heap.Count && heap[left].priority < heap[smallest].priority) smallest = left;
            if (right < heap.Count && heap[right].priority < heap[smallest].priority) smallest = right;
            if (smallest == i) break;
            (heap[smallest], heap[i]) = (heap[i], heap[smallest]); // swap
            i = smallest;
        }
        return best;
    }
}

static List<(int x, int y)> Dijkstra((int x, int y) start, (int x, int y) goal, bool[,] walkable, int[,] cost)
{
    var open = new PriorityQueue<(int x, int y)>();
    var cameFrom = new Dictionary<(int x, int y), (int x, int y)>();
    var bestCost = new Dictionary<(int x, int y), int> { [start] = 0 };
    open.Enqueue(start, 0);

    while (open.Count > 0)
    {
        var current = open.Dequeue();
        if (current.Equals(goal)) break;

        foreach (var next in Neighbors(current, walkable))
        {
            int newCost = bestCost[current] + cost[next.x, next.y];
            if (!bestCost.ContainsKey(next) || newCost < bestCost[next])
            {
                bestCost[next] = newCost;
                cameFrom[next] = current;
                open.Enqueue(next, newCost);
            }
        }
    }
    return ReconstructPath(cameFrom, start, goal);
}

Run it on the same mud patch. Tracing it by hand, step by step (pop the cheapest node, look at its neighbors):

pop (0,0) cost 0   -> push (1,0) cost 5, push (0,1) cost 1
pop (0,1) cost 1   -> push (1,1) cost 2
pop (1,1) cost 2   -> push (2,1) cost 3
pop (2,1) cost 3   -> push (2,0) cost 4
pop (2,0) cost 4   -> this is the goal, stop

path: (0,0) -> (0,1) -> (1,1) -> (2,1) -> (2,0), total cost 4

Dijkstra finds a route that takes four hops instead of BFS's two — but its real cost is 4, cheaper than the mud route's 6. Notice the mud cell (1, 0) was pushed onto the open set early (at cost 5) and just sat there, never popped, because something cheaper always came along first. That is the whole idea of Dijkstra: always expand the cheapest-so-far option next, and you can never be fooled into finalizing an expensive node before a cheap one.

Dijkstra always finds the cheapest path, and that is exactly the guarantee we want for pathfinding. Its one real weakness is that it has no idea where the goal is. It expands the cheapest unexplored node everywhere, equally in every direction, even straight away from the goal, until it happens to reach it. On a big level that is a lot of wasted work. That waste is exactly what A* fixes.

5. A* = Dijkstra guided by a heuristic

Dijkstra ranks nodes in the open set purely by g — the real, known cost from the start to that node. A* ranks them by g plus an estimate of how far that node still is from the goal, called the heuristic and written h. The value A* actually sorts by is:

A node with a low g but a huge h (cheap to reach, but pointing far away from the goal) gets pushed to the back of the queue. A node with a slightly higher g but a small h (heading straight at the goal) gets tried first. The search still keeps track of real cost exactly like Dijkstra — nothing about correctness changes — but the order it explores nodes in gets bent toward the goal.

This single idea — reuse Dijkstra exactly, but sort the open set by g + h instead of just g — is the entire trick behind A*. Everything else in this chapter is either "how do you pick a good h" or "how do you use A* on something other than a grid."

6. Heuristics: Manhattan, Euclidean, and admissibility

A heuristic is just a function: give it a node and the goal, get back a guessed distance. Two common ones:

public static float ManhattanDistance(Vector2Int a, Vector2Int b)
{
    return Mathf.Abs(a.x - b.x) + Mathf.Abs(a.y - b.y);
}

public static float EuclideanDistance(Vector2Int a, Vector2Int b)
{
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return Mathf.Sqrt(dx * dx + dy * dy);
}

Vector2Int a = new Vector2Int(0, 0);
Vector2Int b = new Vector2Int(3, 4);
Debug.Log(ManhattanDistance(a, b));
Debug.Log(EuclideanDistance(a, b));

Output:

7
5

Manhattan distance (|dx| + |dy|) is the distance if you can only move along grid axes — the name comes from walking city blocks, where you cannot cut diagonally through a building. Euclidean distance (straight-line, Pythagorean) is the distance "as the crow flies." Euclidean is never larger than Manhattan for the same two points — a straight line is always the shortest way to connect them.

Picking a heuristic is not just a style choice — it has to match how your agent is allowed to move, because of a property called admissibility: a heuristic is admissible if it never overestimates the true remaining cost, for every node. This matters because A*'s optimality guarantee (that it finds the cheapest possible path, exactly like Dijkstra) depends on it.

On a 4-directional grid (no diagonals), Manhattan distance is always admissible: it is exactly the true cost when nothing is in the way, and walls can only make the true path longer, never shorter — so the guess never overestimates. But if you switch to 8-directional movement, Manhattan distance becomes inadmissible: a diagonal move covers one unit of dx and one unit of dy in a single step, so the true cheapest path can beat what Manhattan predicts, and the guess can overestimate. In that case Euclidean (or a diagonal-aware distance) is the right choice instead.

7. The A* algorithm step by step

A* keeps four pieces of bookkeeping while it searches:

• open set — a priority queue of nodes still waiting to be examined, ordered by f = g + h. Starts with just the start node.
• gScore — a dictionary of the best known real cost from the start to each node seen so far.
• cameFrom — a dictionary recording, for each node, which neighbor we most cheaply arrived from. This is what lets us rebuild the actual path at the end.
• closed set — nodes we have already fully expanded, so we do not waste time reprocessing them.

The key line to notice is the neighbor update: A* does not just push every neighbor blindly. It only updates a neighbor's cameFrom and gScore when it has found a cheaper way to reach it than any way found before. A node can be pushed into the open set more than once, with different priorities, if a cheaper route to it is discovered later — the closed set is what stops us from wastefully re-expanding a node whose cheapest route is already settled.

8. C# implementation: A* on a grid

Time to write the real thing, as it would look in a Unity project, using Vector2Int instead of tuples:

using System.Collections.Generic;
using UnityEngine;

public class AStarPathfinder
{
    private readonly GridMap map;

    private static readonly Vector2Int[] Directions =
    {
        new Vector2Int(0, -1), // up a row  (toward y = 0)
        new Vector2Int(0, 1),  // down a row
        new Vector2Int(-1, 0), // left a column
        new Vector2Int(1, 0),  // right a column
    };

    public AStarPathfinder(GridMap map)
    {
        this.map = map;
    }

    public List<Vector2Int> FindPath(Vector2Int start, Vector2Int goal)
    {
        var open = new PriorityQueue<Vector2Int>();
        var cameFrom = new Dictionary<Vector2Int, Vector2Int>();
        var gScore = new Dictionary<Vector2Int, float> { [start] = 0f };
        var closed = new HashSet<Vector2Int>();

        open.Enqueue(start, Heuristic(start, goal));

        while (open.Count > 0)
        {
            Vector2Int current = open.Dequeue();
            if (current == goal)
                return ReconstructPath(cameFrom, current);

            if (!closed.Add(current)) continue; // already fully expanded, skip

            foreach (var (neighbor, moveCost) in GetNeighbors(current))
            {
                float tentativeG = gScore[current] + moveCost;
                if (!gScore.ContainsKey(neighbor) || tentativeG < gScore[neighbor])
                {
                    cameFrom[neighbor] = current;
                    gScore[neighbor] = tentativeG;
                    float f = tentativeG + Heuristic(neighbor, goal);
                    open.Enqueue(neighbor, f);
                }
            }
        }
        return null; // open set emptied without reaching the goal: no path exists
    }

    private float Heuristic(Vector2Int a, Vector2Int b)
    {
        return Mathf.Abs(a.x - b.x) + Mathf.Abs(a.y - b.y); // Manhattan: matches 4-directional movement
    }

    private IEnumerable<(Vector2Int, float)> GetNeighbors(Vector2Int cell)
    {
        foreach (var d in Directions)
        {
            Vector2Int next = cell + d;
            if (map.IsWalkable(next.x, next.y))
                yield return (next, 1f);
        }
    }

    private List<Vector2Int> ReconstructPath(Dictionary<Vector2Int, Vector2Int> cameFrom, Vector2Int current)
    {
        var path = new List<Vector2Int> { current };
        while (cameFrom.TryGetValue(current, out var prev))
        {
            current = prev;
            path.Add(current);
        }
        path.Reverse();
        return path;
    }
}

Run it on the room from Section 1:

bool[,] walkable = new bool[7, 5];
for (int x = 0; x < 7; x++)
    for (int y = 0; y < 5; y++)
        walkable[x, y] = true;

walkable[3, 0] = false;
walkable[3, 1] = false;
walkable[3, 2] = false;
walkable[3, 3] = false;

var map = new GridMap(walkable);
var pathfinder = new AStarPathfinder(map);
List<Vector2Int> path = pathfinder.FindPath(new Vector2Int(0, 2), new Vector2Int(6, 2));

Debug.Log("length: " + (path.Count - 1) + " steps");
Debug.Log(string.Join(" -> ", path));

Output:

length: 10 steps
(0, 2) -> (1, 2) -> (2, 2) -> (2, 3) -> (2, 4) -> (3, 4) -> (4, 4) -> (4, 3) -> (4, 2) -> (5, 2) -> (6, 2)

Ten steps, routed through the one gap in the wall at (3, 4). (Your exact list of cells can come out slightly different depending on the order GetNeighbors checks directions in, if there happen to be several equally short routes — but the length, 10, is guaranteed to be the true minimum, and we will check that by hand in the next section.)

9. Tracing A*: why it expands fewer nodes than Dijkstra

Let's check that 10 is really the minimum, and see exactly what A*'s heuristic buys us. First, the real cost to reach every open cell in the room from S — this is g, the same number Dijkstra (or plain BFS, since every edge costs 1 here) would compute for each cell if it explored the whole map:

The goal's real cost is 10 — matching the path length A* returned. Good. Now add the heuristic: f = g + h, where h is Manhattan distance to (6, 2):

Here is the payoff. Dijkstra orders purely by g, so before it can be sure it has reached the goal at cost 10, it must fully settle every cell with g <= 10 — that is 28 of the 31 open cells, almost the entire room. That includes cells like (4, 0) (g = 10) and (4, 1) (g = 9) — a dead-end pocket in the top-right that is not on any shortest path at all, but Dijkstra has no way to know that, so it explores there anyway.

A* never bothers with that dead end. Look at their f values: (4, 0) has f = 14 and (4, 1) has f = 12 — both worse than the goal's f = 10. Since A*'s priority queue always tries the smallest f first, the goal gets popped before either of those cells ever would be. The heuristic correctly flags them as "far from the goal," even though they happen to be cheap to reach from the start, and A* skips them entirely.

That is the entire performance argument for A* in one example: same guarantee of finding the cheapest path as Dijkstra, but the heuristic quietly prunes away whole regions of the search that could not possibly lead to a better answer.

10. Path smoothing: string-pulling so agents don't zigzag

The 10-step path A* found is correct, but it looks awful in motion: it is a staircase of individual grid steps, because 4-directional movement can only go straight or turn 90 degrees.

Path smoothing, also called string-pulling, fixes this after the fact: pretend you are pulling a piece of string taut between the start and the goal, threaded through the grid cells the path passed through — the string naturally goes straight wherever nothing is in the way, and only bends at corners it actually has to go around. The algorithm: starting from the first waypoint, look as far ahead as possible for a later waypoint you have a clear, unobstructed straight line to; jump straight to the farthest one you can see; repeat from there.

public static List<Vector2Int> SmoothPath(GridMap map, List<Vector2Int> path)
{
    if (path.Count < 3) return path;

    var smoothed = new List<Vector2Int> { path[0] };
    int current = 0;

    while (current < path.Count - 1)
    {
        int farthest = current + 1;
        for (int test = current + 2; test < path.Count; test++)
        {
            if (HasLineOfSight(map, path[current], path[test]))
                farthest = test;
        }
        smoothed.Add(path[farthest]);
        current = farthest;
    }
    return smoothed;
}

private static bool HasLineOfSight(GridMap map, Vector2Int a, Vector2Int b)
{
    int steps = Mathf.Max(Mathf.Abs(b.x - a.x), Mathf.Abs(b.y - a.y)) * 4;
    for (int i = 0; i <= steps; i++)
    {
        float t = (float)i / steps;
        int x = Mathf.RoundToInt(Mathf.Lerp(a.x, b.x, t));
        int y = Mathf.RoundToInt(Mathf.Lerp(a.y, b.y, t));
        if (!map.IsWalkable(x, y)) return false;
    }
    return true;
}

HasLineOfSight samples points along the straight line from a to b and checks every sampled cell is walkable — a simple stand-in for a proper raycast. Trace it on our 10-step path: from S = (0, 2), the straight line to (4, 4) clips directly through the wall cell (3, 3) (you can see it on the diagram above — that line would cut the corner of the wall), so line of sight there is blocked. But the line to (3, 4) — the gap — stays clear the entire way, since it only touches column x = 3 at the very end, exactly at the open gap. So the farthest visible point from S turns out to be (3, 4).

From (3, 4), every remaining waypoint is on the far side of the wall already, and nothing else blocks the room, so the straight line all the way to G = (6, 2) is completely clear.

Output:

Before smoothing: 11 waypoints
After smoothing:  3 waypoints -> (0, 2), (3, 4), (6, 2)

Eleven waypoints collapse into three, and an agent walking this new path takes two straight lines instead of ten little staircase steps. This is why almost no shipped game moves a character exactly along the raw output of a grid search — smoothing (or a search that never produces a grid-locked path in the first place, which is exactly what navmeshes give you, coming up next) is what makes the motion look intentional instead of robotic.

11. Navmesh: walkable polygons for big 3D worlds

A grid works well for a game like a roguelike or a tile-based strategy game, but it struggles for a big, open, continuous 3D world. Three reasons:

• Memory and search size. A fine enough grid to look smooth over a large open-world map can mean millions of cells, most of them sitting in the middle of wide-open, featureless ground that did not need to be subdivided at all.
• Shape. Real level geometry has slopes, curved walls, and rooms at odd angles. A grid forces everything into square cells aligned to fixed axes, no matter what angle the actual walls are at.
• Precision where it is not needed, and not enough where it is. A single grid cell size is either too coarse for a tight doorway or wastefully fine for an open field — a grid cannot easily be coarse in one place and fine in another.

The alternative used by essentially every modern 3D game is a navmesh (navigation mesh): instead of covering the level in uniform square cells, cover only the walkable surface — floors, ramps, bridges — with a mesh of connected, usually convex, polygons. A big open room might be two or three large polygons. A narrow doorway gets its own small polygon between them. The mesh adapts to the shape of the level instead of forcing the level into a fixed grid.

Here is the important idea: A* does not care whether its nodes are grid cells or navmesh polygons. It only ever needs three things from a graph — a way to list a node's neighbors, a cost for each edge, and a heuristic to the goal. On a navmesh, the nodes are polygons (or their centers), and an edge exists between two polygons that share a border (usually called a portal), with a cost of roughly the distance between their centers. Swap that graph in, and every line of the A* algorithm from Section 7 runs completely unchanged.

Once A* finds a sequence of polygons, walking straight through the center of each one would still zigzag. The exact, geometrically correct version of the string-pulling idea from Section 10 — called the funnel algorithm — pulls a tight, exact path through the shared portal edges of that polygon sequence, without the sampling approximation our grid version needed, because the exact edges of each polygon are already known.

Two names come up constantly in this space: Recast is an open-source library that takes a level's raw 3D collision geometry, voxelizes it (breaks it into a temporary 3D grid, similar in spirit to Section 2's grid, purely as an intermediate step), and automatically generates a walkable navmesh from it — figuring out which surfaces are flat and wide enough to walk on, how far from ledges and steep slopes to stay, and so on. Detour is the companion runtime library that answers pathfinding queries (A* plus the funnel algorithm) against a navmesh that Recast already built. Unity's own navigation system is built on the same ideas.

In Unity, you rarely touch Recast or Detour directly. Instead:

• You mark level geometry (floors, ramps, walls) as Navigation Static, and bake a navmesh for the scene (Window > AI > Navigation, or the NavMeshSurface component) — this is Unity running Recast-like generation for you, offline, once.
• You attach a NavMeshAgent component to any character that should move along the navmesh.

using UnityEngine;
using UnityEngine.AI;

public class EnemyChaser : MonoBehaviour
{
    public Transform target;
    private NavMeshAgent agent;

    void Awake()
    {
        agent = GetComponent<NavMeshAgent>();
    }

    void Update()
    {
        agent.SetDestination(target.position);
    }
}

That single call, SetDestination, hides this entire chapter behind it: Unity runs A* on the baked navmesh, runs the funnel algorithm to get a tight, smooth corridor, and then moves the agent along it frame by frame — while also doing a bit of local steering (Section 1's other kind of avoidance) so agents with NavMeshAgent attached gently steer around each other instead of walking through one another. Everything earlier in this chapter is what that one line is actually doing underneath.

12. Hierarchical pathfinding for huge maps

A* on a navmesh is fast, but "fast" still has limits: a huge open-world map can have tens of thousands of polygons, and a real-time strategy game might need fresh paths for hundreds of units every single frame. Running full A* across the entire map for every one of those requests gets expensive.

The fix mirrors something you may have already seen elsewhere: solve the problem coarsely first, then refine only where it matters. Hierarchical pathfinding groups the map into clusters (chunks of nearby nodes), and builds a small, separate abstract graph where each cluster is just one node, connected to neighboring clusters that it shares a valid crossing point with.

This is the same idea as choosing a route on a highway map before worrying about which exact street to turn onto — you decide the sequence of cities first, on a map with almost nothing on it, and only load the detailed street map for the cities you are actually going to pass through. The abstract graph is tiny compared to the full map, so step 1 is nearly free, and step 2 only ever runs detailed search over a handful of small clusters instead of the whole level.

Because the level geometry is usually static, the connections between clusters (which pairs of clusters touch, and where) can be precomputed once, offline, exactly like a navmesh bake. Only the actual per-agent path request has to happen at runtime, and it is now cheap enough to run for hundreds of units without dropping frames. This is the standard approach behind pathfinding in large-scale RTS games and open-world crowds — it is less about a new algorithm and more about running the A* you already know at two different zoom levels.

13. Greedy best-first search: fast, but it can walk you the long way

Everything up to here has been C# you would drop straight into a Unity project. The next several sections dig deeper into the algorithms themselves, and for that it helps to have programs you can compile and run in a couple of seconds with nothing but a C++ compiler — no engine, no scene. So the demos below are self-contained C++ (clang++ -std=c++17 file.cpp && ./a.out). The logic is identical to the C# you have already seen; only the surface syntax differs.

Section 4 sorted the open set by g (Dijkstra); Section 5 sorted it by g + h (A*). There is an obvious third option sitting right between them: sort by h alone. That algorithm is called greedy best-first search, and it is worth building once, because seeing exactly how it fails is the clearest way to understand what the g term in A* is actually protecting you from.

Greedy best-first always expands whichever open node looks closest to the goal, ignoring entirely how much it already cost to get there. That makes it fast — it charges straight at the goal — but it has no way to notice when "straight at the goal" has led it down a long detour. Here is all three on one small level, counting both the path length and how many cells each one expands:

#include <cstdio>
#include <cstdlib>
#include <vector>
#include <string>
#include <queue>
#include <tuple>
#include <unordered_set>
#include <unordered_map>
using namespace std;

// A small level. '#' is a wall, '.' is open, S start, G goal.
vector<string> level = {
    "S.......",
    "####....",
    "........",
    "...##..#",
    ".#.##.#.",
    "..#....G",
};

int W, H;
bool Walkable(int x, int y) {
    if (x < 0 || y < 0 || x >= W || y >= H) return false;
    return level[y][x] != '#';
}
int Manhattan(int ax, int ay, int bx, int by) {
    return abs(ax - bx) + abs(ay - by);
}

enum Mode { DIJKSTRA, GREEDY, ASTAR };

// Returns the path length in steps; writes the number of expanded cells to *expanded.
int Search(Mode mode, int sx, int sy, int gx, int gy, int *expanded) {
    auto id = [&](int x, int y) { return y * W + x; };
    // priority queue of (priority, insertionOrder, cellId), smallest priority first
    priority_queue<tuple<int,int,int>, vector<tuple<int,int,int>>, greater<>> open;
    unordered_map<int,int> gScore, cameFrom;
    unordered_set<int> closed;
    int order = 0;
    *expanded = 0;

    gScore[id(sx,sy)] = 0;
    int startPriority = (mode == DIJKSTRA) ? 0 : Manhattan(sx, sy, gx, gy);
    open.push({startPriority, order++, id(sx,sy)});

    int dx[4] = {1,-1,0,0}, dy[4] = {0,0,1,-1};
    while (!open.empty()) {
        auto [pri, ord, k] = open.top(); open.pop();
        int cx = k % W, cy = k / W;
        if (cx == gx && cy == gy) {           // reached the goal
            int len = 0, c = k;
            while (c != id(sx,sy)) { c = cameFrom[c]; len++; }
            return len;
        }
        if (closed.count(k)) continue;
        closed.insert(k);
        (*expanded)++;
        for (int i = 0; i < 4; i++) {
            int nx = cx + dx[i], ny = cy + dy[i];
            if (!Walkable(nx, ny)) continue;
            int nk = id(nx, ny), ng = gScore[k] + 1;
            // GREEDY commits to the first route it discovers to each cell;
            // DIJKSTRA / ASTAR keep the cheapest g and may relax it later.
            if (mode == GREEDY) {
                if (gScore.count(nk)) continue;      // already discovered: keep first cameFrom
                gScore[nk] = ng; cameFrom[nk] = k;
                open.push({Manhattan(nx,ny,gx,gy), order++, nk});
            } else {
                if (!gScore.count(nk) || ng < gScore[nk]) {
                    gScore[nk] = ng; cameFrom[nk] = k;
                    int h = Manhattan(nx, ny, gx, gy);
                    int f = (mode == DIJKSTRA) ? ng : ng + h;
                    open.push({f, order++, nk});
                }
            }
        }
    }
    return -1;
}

int main() {
    H = level.size(); W = level[0].size();
    int sx=0, sy=0, gx=0, gy=0;
    for (int y = 0; y < H; y++)
        for (int x = 0; x < W; x++) {
            if (level[y][x] == 'S') { sx = x; sy = y; }
            if (level[y][x] == 'G') { gx = x; gy = y; }
        }
    const char* names[3] = {"Dijkstra (g only) ", "Greedy   (h only) ", "A*       (g + h)  "};
    Mode modes[3] = {DIJKSTRA, GREEDY, ASTAR};
    for (int m = 0; m < 3; m++) {
        int expanded;
        int len = Search(modes[m], sx, sy, gx, gy, &expanded);
        printf("%s  path = %2d steps   cells expanded = %d\n", names[m], len, expanded);
    }
    return 0;
}

Output:

Dijkstra (g only)   path = 12 steps   cells expanded = 30
Greedy   (h only)   path = 16 steps   cells expanded = 16
A*       (g + h)    path = 12 steps   cells expanded = 21

Read those three lines carefully, because each pair of numbers tells a different half of the story. Dijkstra and A* both return the true shortest path, 12 steps — but A* gets there while expanding only 21 cells to Dijkstra's 30, because the heuristic pulls it toward the goal instead of spreading out evenly in all directions. Greedy expands the fewest cells of all, just 16 — it really is the cheapest search — but the path it returns is 16 steps, a third longer than necessary. It rushed toward the goal, committed to a corridor that turned out to be a detour, and because it never looks at g it had no way to tell it was making the trip longer.

That is the whole job of the g term in f = g + h: it keeps the search honest about distance already travelled, so A* can abandon a route that looked promising but is turning out expensive. Drop g and you get speed at the price of correctness — that is greedy best-first in one sentence.

14. Consistency: the property beyond admissibility

Section 6 said an admissible heuristic (one that never overestimates) is what A* needs to return the cheapest path. That is true for the textbook version of A* — the one willing to re-examine a node it already settled. But look again at the real implementation in Section 8: once a node comes out of the open set it goes into closed, and the very next line, if (!closed.Add(current)) continue;, makes sure it is never expanded again. That optimization is what keeps A* fast. It is also only safe under a property stronger than admissibility, called consistency (or monotonicity).

Consistency is the triangle inequality applied to your heuristic: stepping from n to a neighbour n' may lower your estimate of the remaining distance by at most the cost of that step. If one step could make h drop by more than the step actually costs, the heuristic is inconsistent. Every consistent heuristic is automatically admissible, but not the reverse — and that gap is exactly where the closed-set optimization can bite.

Here is why it matters, in one fact: with a consistent heuristic, the first time A* pops a node its g is already the cheapest possible, so nailing it shut is safe and every node is expanded at most once. With a merely-admissible-but-inconsistent heuristic that guarantee is gone — A* can pop a node, close it, and only later discover a cheaper way to reach it, which the Section 8 code refuses to propagate. This four-node graph makes it happen:

#include <cstdio>
#include <vector>
#include <queue>
#include <tuple>
#include <climits>
#include <unordered_set>
using namespace std;

// Four nodes: S=0, B=1, C=2, G=3.  Directed edges (from, to, cost).
const int S=0, B=1, C=2, G=3, N=4;
const char* name[N] = {"S","B","C","G"};
vector<tuple<int,int,int>> edges = { {S,B,3}, {S,C,1}, {C,B,1}, {B,G,2} };

// An admissible but INCONSISTENT heuristic (guessed distance to goal G).
int h[N] = { 3, 0, 3, 0 };   // h[S]=3, h[B]=0, h[C]=3, h[G]=0

vector<vector<pair<int,int>>> adj;

int dijkstra() {                      // true optimal S -> G
    vector<int> d(N, INT_MAX); d[S]=0;
    priority_queue<pair<int,int>, vector<pair<int,int>>, greater<>> pq; pq.push({0,S});
    while(!pq.empty()){ auto[dd,u]=pq.top(); pq.pop(); if(dd>d[u]) continue;
        for(auto&e:adj[u]) if(d[u]+e.second<d[e.first]){ d[e.first]=d[u]+e.second; pq.push({d[e.first],e.first}); } }
    return d[G];
}

// Section 8's logic: once a node is popped it is closed and never reopened.
int astarClosedSet() {
    vector<int> g(N, INT_MAX); g[S]=0;
    unordered_set<int> closed;
    priority_queue<tuple<int,int,int>, vector<tuple<int,int,int>>, greater<>> open;
    int ord=0; open.push({g[S]+h[S], ord++, S});
    while(!open.empty()){
        auto[f,o,u]=open.top(); open.pop();
        if(u==G) return g[G];
        if(closed.count(u)) continue;
        closed.insert(u);
        for(auto&e:adj[u]){ int v=e.first, ng=g[u]+e.second;
            if(ng<g[v]){ g[v]=ng; open.push({ng+h[v], ord++, v}); } }
    }
    return -1;
}

// The fix: allow a closed node to be reopened when a cheaper g is found.
int astarReopening() {
    vector<int> g(N, INT_MAX); g[S]=0;
    unordered_set<int> closed;
    priority_queue<tuple<int,int,int>, vector<tuple<int,int,int>>, greater<>> open;
    int ord=0; open.push({g[S]+h[S], ord++, S});
    while(!open.empty()){
        auto[f,o,u]=open.top(); open.pop();
        if(u==G) return g[G];
        closed.erase(u);                 // may be re-expanded if a better route arrives
        for(auto&e:adj[u]){ int v=e.first, ng=g[u]+e.second;
            if(ng<g[v]){ g[v]=ng; open.push({ng+h[v], ord++, v}); } }
    }
    return -1;
}

int main(){
    adj.assign(N,{});
    for(auto&e:edges) adj[get<0>(e)].push_back({get<1>(e), get<2>(e)});

    printf("consistency check  h(u) <= cost(u,v) + h(v)  for every edge:\n");
    for(auto&e:edges){ int u=get<0>(e), v=get<1>(e), c=get<2>(e);
        bool ok = h[u] <= c + h[v];
        printf("  %s->%s cost %d :  h(%s)=%d  vs  cost+h(%s)=%d   %s\n",
               name[u],name[v],c, name[u],h[u], name[v], c+h[v], ok?"ok":"VIOLATED");
    }
    printf("\noptimal cost (Dijkstra)          = %d\n", dijkstra());
    printf("A* with closed set, no reopening = %d   <- Section 8's code\n", astarClosedSet());
    printf("A* that reopens closed nodes     = %d\n", astarReopening());
    return 0;
}

Output:

consistency check  h(u) <= cost(u,v) + h(v)  for every edge:
  S->B cost 3 :  h(S)=3  vs  cost+h(B)=3   ok
  S->C cost 1 :  h(S)=3  vs  cost+h(C)=4   ok
  C->B cost 1 :  h(C)=3  vs  cost+h(B)=1   VIOLATED
  B->G cost 2 :  h(B)=0  vs  cost+h(G)=2   ok

optimal cost (Dijkstra)          = 4
A* with closed set, no reopening = 5   <- Section 8's code
A* that reopens closed nodes     = 4

The true cheapest route is S -> C -> B -> G, costing 1 + 1 + 2 = 4. Section 8's fast A* returns 5. Walk the trace: from S it discovers B at g = 3 (edge S->B) and C at g = 1 (edge S->C). Because h(B) = 0, node B's priority is f = 3, lower than C's f = 1 + 3 = 4 — so B is popped and closed first, and expanding it reaches the goal at g = 5. Only afterwards is C popped, and C finds a cheaper way to B (g = 2 via C->B). But B is already closed, so that improvement is written into gScore and then thrown away — it never propagates on to the goal.

The single culprit is the edge C->B, the one flagged VIOLATED: h(C) = 3, but stepping to B costs only 1 and leaves an estimate of h(B) = 0 — the guess dropped by 3 for a step worth 1. That is the inconsistency, and one violated edge is enough to make the fast version of A* return a path that is not the cheapest.

There are two ways to stay safe, and the first is the reassuring one. (1) Use a consistent heuristic. The standard geometric heuristics from Section 6 — Manhattan, Euclidean, Chebyshev, octile — are all consistent, as long as your edge costs are the real distances between cells, because each step can change the heuristic by at most that step's cost. That is precisely why the Section 8 grid code is correct in practice and why you almost never meet this bug on a plain grid. (2) When you genuinely need an inconsistent heuristic — hand-tuned estimates, learned heuristics, or the maximum of several heuristics, all of which show up in advanced pathfinding — allow closed nodes to be reopened, as astarReopening does with closed.erase(u). It restores the correct answer (back to 4) at the cost of occasionally expanding a node more than once.

15. Tie-breaking: taming the flood of equal f-costs

On a uniform grid a huge number of cells end up sharing the same f. Over open ground every shortest route from S to G costs the same, so every cell on every one of those routes carries one identical f value. When A* has no rule for breaking those ties, it expands them in whatever incidental order they happened to land in the heap — which means it fans out across the entire tied region before it commits to the goal. Watch what that costs on a completely open 15x15 grid, corner to corner. The only change from the Section 13 harness is that the priority becomes a triple (f, tieKey, insertionOrder), and tieKey is filled three different ways:

// The heap compares tuples left to right, so tieKey is consulted ONLY when two
// cells have the same f. It never changes the path length, just the search order.
int f = ng + Manhattan(nx, ny, gx, gy);
int tieKey = 0;                              // NONE: ties fall back to insertion order

if (mode == LOWER_H)                         // prefer the cell with the smaller h (closer to goal)
    tieKey = Manhattan(nx, ny, gx, gy);

else if (mode == CROSS) {                    // prefer cells near the straight S->G line
    int dx1 = nx - gx, dy1 = ny - gy;
    int dx2 = sx - gx, dy2 = sy - gy;
    tieKey = abs(dx1 * dy2 - dx2 * dy1);     // |cross product|: distance-ish from that line
}

open.push({f, tieKey, order++, id(nx, ny)});

Output:

no tie-break (FIFO)      path = 28 steps   cells expanded = 224
tie-break: prefer low h   path = 28 steps   cells expanded = 28
tie-break: cross product   path = 28 steps   cells expanded = 41

All three find a shortest 28-step path — tie-breaking never changes the length, only which of the equally-good routes you get and how hard you worked for it. But with no tie rule, A* expands 224 of the grid's 225 cells: essentially the whole board, because every cell was tied and got its turn. Preferring the smaller h on a tie collapses that to 28 — it walks almost straight to the goal, since on open ground the lower-h neighbour is always the one heading right at it. The cross-product rule, which nudges the search toward the straight line from start to goal, expands 41 and produces the most centred, natural-looking path of the three.

Both rules are cheap: prefer-low-h is one extra number, already computed, and cross product is a single 2D cross product measuring roughly how far a cell sits from the straight S-to-G line, so ties resolve in favour of cells hugging that line. Kept as a strict secondary key — only ever consulted when f is exactly equal — neither one can change the path's cost, so A* stays optimal and simply does far less work.

16. Weighted A*: trading a little optimality for a lot of speed

Sometimes even a well-tuned A* explores more than you can afford — hundreds of agents, a huge map, a tight frame budget. Weighted A* buys speed with a single knob. Instead of f = g + h it sorts by f = g + w*h for a weight w >= 1. Inflating h makes the search greedier: it leans harder toward the goal and expands fewer nodes. The price is optimality — but a bounded price, which is what makes the trick usable. Here it is on a map with obstacles, at four weights:

// The only change from ordinary A* is the w in the priority. Everything else -
// the level, Walkable, Manhattan, the closed set - is exactly the Section 13 harness.
int WeightedAStar(double w, int sx,int sy,int gx,int gy, int *expanded) {
    auto id = [&](int x,int y){ return y*W+x; };
    priority_queue<tuple<double,int,int>, vector<tuple<double,int,int>>, greater<>> open;
    unordered_map<int,int> g, cameFrom;
    unordered_set<int> closed;
    int ord=0; *expanded=0;
    g[id(sx,sy)] = 0;
    open.push({ w*Manhattan(sx,sy,gx,gy), ord++, id(sx,sy) });
    int dx[4]={1,-1,0,0}, dy[4]={0,0,1,-1};
    while(!open.empty()){
        auto [f,o,k] = open.top(); open.pop();
        int cx=k%W, cy=k/W;
        if(cx==gx && cy==gy){ int len=0,c=k; while(c!=id(sx,sy)){c=cameFrom[c];len++;} return len; }
        if(closed.count(k)) continue;
        closed.insert(k); (*expanded)++;
        for(int i=0;i<4;i++){
            int nx=cx+dx[i], ny=cy+dy[i];
            if(!Walkable(nx,ny)) continue;
            int nk=id(nx,ny), ng=g[k]+1;
            if(!g.count(nk) || ng<g[nk]){
                g[nk]=ng; cameFrom[nk]=k;
                open.push({ ng + w*Manhattan(nx,ny,gx,gy), ord++, nk });   // g + w*h
            }
        }
    }
    return -1;
}

// driver: run the same query at several weights
for (double w : {1.0, 1.5, 2.0, 4.0}) {
    int e; int cost = WeightedAStar(w, sx,sy,gx,gy,&e);
    printf("w = %.1f :  path cost = %d   cells expanded = %d\n", w, cost, e);
}

Output:

w = 1.0 :  path cost = 36   cells expanded = 101   (optimal)
w = 1.5 :  path cost = 36   cells expanded = 48
w = 2.0 :  path cost = 38   cells expanded = 44
w = 4.0 :  path cost = 38   cells expanded = 43

Look at the jump from w = 1.0 to w = 1.5: the path is still the optimal 36, but the search collapsed from 101 cells to 48 — less than half the work, for free, on this map. Push to w = 2.0 and the path lengthens slightly to 38 (about 6% over optimal) while the search shrinks a little further. Past that the returns diminish; as w heads toward infinity, weighted A* degenerates into the greedy best-first search from Section 13.

The guarantee that makes this safe to ship: weighted A* never returns a path more than w times the optimal cost. With w = 1.5 you are promised a route at most 50% longer than the best possible — and in practice, as here, you usually land far inside that bound. For a game that means you can turn w up until the search is cheap enough for your frame budget, knowing the worst case is a slightly longer route, never a broken or ugly one.

17. Waypoint graphs: the third way to represent a level

Section 2 built a grid; Section 11 built a navmesh. There is a third representation, older than the navmesh and still in use, that sits between them: the waypoint graph. Instead of covering the walkable area (grid cells, navmesh polygons), you scatter a handful of waypoints — single points — at the places that matter (doorways, corners, cover spots, room centres) and connect two waypoints with an edge whenever an agent can walk in a clear straight line between them.

The payoff is that the graph is tiny: a level that would be thousands of grid cells might be a few dozen waypoints, so A* over it is nearly free. That is exactly why waypoint graphs were the standard in older 3D games — the hand-placed node networks in engines like Quake and Source — where running A* on a full grid of a 3D level was too expensive, so designers dropped nodes in by hand. The idea survives today for coarse, high-level routing ("which rooms do I pass through"), often as the top layer of the hierarchical scheme from Section 12.

The costs are the mirror image of the benefits. Someone — a designer or an offline tool — has to place the waypoints, and the agent may only travel along the edges you gave it, so paths are only as good as the graph. Sparse waypoints produce paths that visibly snap between fixed points and follow none of the real geometry in between. A navmesh sidesteps both problems by covering the whole walkable surface automatically, which is why it, not the waypoint graph, became the default for continuous 3D worlds. But the underlying search never changed: A* does not care — grid cell, navmesh polygon, or hand-placed waypoint, it is all just "nodes, neighbours, edge costs, and a heuristic," the same four things from Section 7.

18. The funnel algorithm: exact string-pulling on a navmesh

Sections 10 and 11 both named the funnel algorithm as the navmesh-native way to straighten a path, but never showed it. It is worth seeing properly, because it is how every navmesh path you have watched an agent walk actually got its shape.

Recall the setup from Section 11: A* on a navmesh returns a sequence of polygons to cross, and the gates between them — the shared edges, or portals — are known exactly. Walking through the centre of each polygon would zigzag. Section 10's grid smoothing fixed zigzags by sampling a straight line and hoping it stayed walkable; on a navmesh we can do better, because the portal edges hand us the exact corridor. The funnel algorithm pulls a string taut through that corridor, geometrically, with no sampling.

The idea is a moving funnel: an apex (the last point the path is committed to) plus two feeler lines running from the apex to the left and right endpoints of the portal currently under consideration. As you advance portal by portal, each new endpoint can only ever narrow the funnel — pull the left feeler rightward or the right feeler leftward. Two things can happen:

• The new endpoint keeps the funnel a valid wedge (left feeler still left of right feeler): tighten that side and move on.
• The feelers cross over — one side has been pulled past the other. That crossing point is a corner the path is physically forced to bend around: lock it in as the new apex, emit it as a path corner, and restart the funnel from there.

Here is Mikko Mononen's compact "Simple Stupid Funnel Algorithm," the version most navmesh code is based on, run on an L-shaped corridor that turns a corner:

#include <cstdio>
#include <cmath>
#include <vector>
using namespace std;

struct V { float x, y; };

// twice the signed area of triangle a,b,c. >0 = c is left of a->b, <0 = right.
float triArea2(V a, V b, V c){ return (b.x-a.x)*(c.y-a.y) - (c.x-a.x)*(b.y-a.y); }
bool vEqual(V a, V b){ float dx=a.x-b.x, dy=a.y-b.y; return dx*dx+dy*dy < 1e-6f; }

// portals: portal[i] = (left endpoint, right endpoint).
// portal[0] is (start,start); the last is (goal,goal).
vector<V> stringPull(vector<pair<V,V>> portal){
    vector<V> pts;
    V apex = portal[0].first, left = portal[0].first, right = portal[0].second;
    int apexIdx=0, leftIdx=0, rightIdx=0;
    pts.push_back(apex);
    for(int i=1;i<(int)portal.size();){
        V pl = portal[i].first, pr = portal[i].second;
        // tighten right side
        if(triArea2(apex,right,pr) <= 0.0f){
            if(vEqual(apex,right) || triArea2(apex,left,pr) > 0.0f){
                right = pr; rightIdx = i;                  // narrow the funnel
            } else {                                       // right crossed left: lock in left as a corner
                pts.push_back(left);
                apex = left; apexIdx = leftIdx;
                left = apex; right = apex; leftIdx = apexIdx; rightIdx = apexIdx;
                i = apexIdx + 1; continue;
            }
        }
        // tighten left side
        if(triArea2(apex,left,pl) >= 0.0f){
            if(vEqual(apex,left) || triArea2(apex,right,pl) < 0.0f){
                left = pl; leftIdx = i;                    // narrow the funnel
            } else {                                       // left crossed right: lock in right as a corner
                pts.push_back(right);
                apex = right; apexIdx = rightIdx;
                left = apex; right = apex; leftIdx = apexIdx; rightIdx = apexIdx;
                i = apexIdx + 1; continue;
            }
        }
        i++;
    }
    if(pts.empty() || !vEqual(pts.back(), portal.back().first))
        pts.push_back(portal.back().first);
    return pts;
}

int main(){
    // An L-shaped corridor of width 2. It runs right (+x), then turns up (+y).
    // The inside corner of the bend is at (2,2); the tight path should cut across it.
    // Each gate is (outer-wall endpoint, inner-wall endpoint), wound consistently.
    V start{0.5f, 1.0f}, goal{3.0f, 5.5f};
    vector<pair<V,V>> portals = {
        { start, start },
        { {1,0}, {1,2} },     // vertical gate at x=1
        { {2,0}, {2,2} },     // vertical gate at x=2  (inner endpoint = the corner)
        { {4,3}, {2,3} },     // horizontal gate at y=3
        { {4,4}, {2,4} },     // horizontal gate at y=4
        { goal, goal },
    };
    vector<V> path = stringPull(portals);
    printf("tight path through the corridor (%d corners):\n", (int)path.size());
    for(auto p: path) printf("  (%.1f, %.1f)\n", p.x, p.y);
    return 0;
}

Output:

tight path through the corridor (3 corners):
  (0.5, 1.0)
  (2.0, 2.0)
  (3.0, 5.5)

The corridor runs right, then turns upward; its inside corner is at (2, 2). Out of the four gates the path crosses, the funnel emits exactly one interior corner — (2, 2) — and returns a two-segment route from start to that corner to goal. That is the tight, exact path: it hugs the one corner it must and goes dead straight everywhere else, which is precisely what makes navmesh movement look natural without any of the per-cell smoothing a grid needs. The triArea2 function doing all the work is just a 2D cross product; its sign tells you whether a point lies to the left or right of a feeler line.

19. When the map changes: replanning, and why you repath on a timer

Everything so far assumed the map holds still while you search. Real games do not: doors open and close, a bridge is destroyed, a crate is shoved into a corridor, and above all the target moves — a guard chasing the player is aiming at a spot the player has already left by the time the path is computed. A path is a snapshot of a world that has already changed. So the real question is not "how do I find a path" but "how often, and how, do I find a new one." Recomputing every frame is the tempting answer, and it is wrong on two separate counts.

Cost. A* is not free, and "every frame, every agent" multiplies its cost by your frame rate and your crowd size at once. Here is a chaser planning toward a wandering target for 150 frames, measured three ways — replanning every frame, replanning on a fixed timer, and replanning only when the target has drifted more than a few cells from where it was last planned to. astarCost(...) is the Section 13 A*, returning how many cells it expanded:

int lastPlanX = tx, lastPlanY = ty;   // where the target was at the last threshold-replan

for (int f = 0; f < frames; f++) {
    targetRandomWalk(&tx, &ty);        // target drifts one cell

    // Policy A: replan every frame
    callsEvery++;  expEvery  += astarCost(chaserX, chaserY, tx, ty);

    // Policy B: replan on a timer (every 10 frames)
    if (f % 10 == 0) { callsTimer++; expTimer += astarCost(chaserX, chaserY, tx, ty); }

    // Policy C: replan only when the target strayed > 3 cells from the last plan
    if (manhattan(tx, ty, lastPlanX, lastPlanY) > 3) {
        callsThresh++; expThresh += astarCost(chaserX, chaserY, tx, ty);
        lastPlanX = tx; lastPlanY = ty;
    }
}

Output:

over 150 frames, planning cost (A* calls / total cells expanded):
  every frame          : 150 calls,  19564 cells
  timer (every 10)      :  15 calls,   2020 cells
  threshold (moved > 3) :   9 calls,    984 cells

Repathing on a timer does one-tenth the work; repathing only when the target actually moved enough to matter does about one-twentieth — 984 expanded cells against nearly twenty thousand — for a chase the player cannot tell apart from the every-frame version.

Jitter. The subtler reason is that constant replanning makes agents twitch. When two routes are nearly equal in cost, a tiny change — the target stepping one cell, an obstacle shifting slightly — flips A* from one route to the other and back. The agent, dutifully following whichever path is current, visibly jerks between them, and can even stall, unable to commit to moving because it keeps re-deciding. Repathing on a timer (say every 0.2 to 0.5 seconds) or only on a real trigger gives each path time to actually be followed, which reads as far more deliberate than a perfect path recomputed sixty times a second.

So the standard pattern is event-driven or throttled replanning: repath when the target moves past a threshold, when the current path gets blocked, or when a timer fires — not on a fixed every-frame schedule. In Unity:

using UnityEngine;
using UnityEngine.AI;

public class ThrottledChaser : MonoBehaviour
{
    public Transform target;
    public float repathInterval = 0.3f;   // at most one repath every 0.3s ...
    public float moveThreshold = 1.5f;    // ... and only if the target moved this far

    private NavMeshAgent agent;
    private Vector3 lastTargetPos;
    private float timer;

    void Awake() { agent = GetComponent<NavMeshAgent>(); }

    void Update()
    {
        timer += Time.deltaTime;
        if (timer < repathInterval) return;                       // not time yet

        Vector3 delta = target.position - lastTargetPos;
        if (delta.sqrMagnitude < moveThreshold * moveThreshold)
            return;                                               // target hasn't moved enough

        timer = 0f;
        lastTargetPos = target.position;
        agent.SetDestination(target.position);                    // the one expensive call
    }
}

Reusing the last search: incremental replanning. When the world changes only a little, throwing away the whole previous search and starting over is wasteful — most of it is still valid. A family of algorithms, the best-known being D* Lite (and its cousin LPA*, Lifelong Planning A*), exploits this: they keep the search from last time and repair only the part affected by the change, instead of replanning from scratch. When a single door slams shut in a large map, D* Lite fixes the handful of nodes near that door and reuses everything else, which is dramatically cheaper than a fresh A* — the reason it is a staple in robotics and in games with constantly-shifting costs. The idea to carry away: changing a little of the world should cost only a little to react to.

20. Following the path: steering and local avoidance (RVO/ORCA)

A path is a list of points. It is not motion. The last piece of the pipeline — the one the player actually watches — turns that list into second-by-second movement and handles the things the path could not know about. It splits cleanly into two jobs that work at different scales.

Path following is the local control that walks an agent along the waypoints. The simplest follower steers toward the current waypoint, and once within a small arrival radius, advances to the next:

using System.Collections.Generic;
using UnityEngine;

public class PathFollower : MonoBehaviour
{
    public float speed = 4f;
    public float arriveRadius = 0.2f;     // "close enough" to a waypoint
    public float slowRadius = 1.5f;       // start easing off near the final point

    private List<Vector3> path;
    private int index;

    public void SetPath(List<Vector3> newPath) { path = newPath; index = 0; }

    void Update()
    {
        if (path == null || index >= path.Count) return;

        Vector3 toTarget = path[index] - transform.position;
        float distance = toTarget.magnitude;

        if (distance < arriveRadius) { index++; return; }   // reached it; aim at the next next frame

        // ease down only on the final leg, so the agent doesn't overshoot the goal
        float wantSpeed = (index == path.Count - 1)
            ? speed * Mathf.Clamp01(distance / slowRadius)
            : speed;

        transform.position += toTarget.normalized * wantSpeed * Time.deltaTime;
    }
}

That is deliberately minimal, but it already shows the shape of the problem: the raw path gives you where to go; the follower decides how — turn rate, arrival slowdown, and (in a fuller version) looking a little ahead on the path so the agent rounds corners smoothly instead of stopping dead on every waypoint. Getting this layer right is most of what makes movement feel good, and it is entirely separate from the search that produced the path.

Local avoidance is the other job: keeping agents from walking through each other and through moving obstacles the path never accounted for. This is not a pathfinding problem — you cannot afford to run A* against a crowd every frame, and even if you could, the crowd has moved by the time you finish. Instead each agent reasons directly about velocities. The core question: given where my neighbour is and how it is moving, which of my own candidate velocities will run me into it soon? That set of "bad" velocities is the velocity obstacle. Here is its heart — the time until two moving discs first touch:

#include <cstdio>
#include <cmath>
using namespace std;

struct Vec { float x, y; };
float dot(Vec a, Vec b){ return a.x*b.x + a.y*b.y; }
Vec  sub(Vec a, Vec b){ return {a.x-b.x, a.y-b.y}; }

// Time until agent A (moving vA) first touches agent B (at pB, moving vB), within horizon tau.
// Returns -1 if they never get within the combined radius R inside [0, tau].
float timeToCollision(Vec pA, Vec vA, Vec pB, Vec vB, float R, float tau){
    Vec p = sub(pB, pA);          // B relative to A
    Vec u = sub(vB, vA);          // relative velocity
    float a = dot(u,u);
    float b = 2*dot(p,u);
    float c = dot(p,p) - R*R;
    if(a < 1e-9f) return (c <= 0.0f) ? 0.0f : -1.0f;    // no relative motion
    float disc = b*b - 4*a*c;
    if(disc < 0) return -1.0f;                           // paths never close within R
    float t = (-b - sqrtf(disc)) / (2*a);               // earliest contact
    if(t < 0) t = (-b + sqrtf(disc)) / (2*a);
    if(t < 0 || t > tau) return -1.0f;
    return t;
}

int main(){
    Vec pA{0,0}, pB{6,0};       // B is 6 units to the right ...
    Vec vB{-1,0};               // ... and walking straight toward A
    float R = 1.0f, tau = 5.0f; // combined radius 0.5+0.5, look 5 seconds ahead
    Vec cand[4] = { {1,0}, {1,0.5f}, {1,1}, {0,0} };
    const char* label[4] = { "straight at B (1.0, 0.0)", "veer up a little (1.0, 0.5)",
                             "veer up more   (1.0, 1.0)", "stop           (0.0, 0.0)" };
    for(int i=0;i<4;i++){
        float t = timeToCollision(pA, cand[i], pB, vB, R, tau);
        if(t < 0) printf("A velocity %-28s -> SAFE (no collision within %.0fs)\n", label[i], tau);
        else      printf("A velocity %-28s -> COLLISION at t = %.2fs\n", label[i], t);
    }
    return 0;
}

Output:

A velocity straight at B (1.0, 0.0)     -> COLLISION at t = 2.50s
A velocity veer up a little (1.0, 0.5)  -> SAFE (no collision within 5s)
A velocity veer up more   (1.0, 1.0)    -> SAFE (no collision within 5s)
A velocity stop           (0.0, 0.0)    -> COLLISION at t = 5.00s

Two agents are closing head-on. Driving straight at each other collides in 2.5 seconds; veering aside even slightly — (1, 0.5) — is completely safe; and, tellingly, stopping does not help, because the other agent keeps coming (contact at t = 5). The colliding velocities form a cone, the velocity obstacle; the safe move is the velocity closest to "toward my next waypoint" that lies outside that cone. Compute it every frame against nearby neighbours and agents flow around one another smoothly.

The refinement that makes this work in a crowd is reciprocity. If both agents assume the other will hold course and each dodges the full amount, they overcorrect; next frame they see no conflict, revert, and collide again — an endless oscillating dance. RVO (Reciprocal Velocity Obstacles) fixes it by having each agent take half the avoidance, trusting the other to take the other half. ORCA (Optimal Reciprocal Collision Avoidance) is the modern, faster formulation of the same idea, expressed as linear constraints; it is what sits inside Unity's NavMeshAgent avoidance and most crowd systems. Global pathfinding picks the corridor; reciprocal local avoidance negotiates the last metre.

21. Glossary

• Pathfinding — finding a route from a start position to a goal that avoids obstacles, usually at the lowest possible cost.
• Obstacle avoidance / steering — short-range, reactive dodging of things not accounted for in the planned path.
• Graph — a set of nodes connected by edges; the general structure every search algorithm in this chapter runs on.
• Grid — the simplest graph for pathfinding: equal-sized cells as nodes, connected to their neighbors.
• BFS (breadth-first search) — explores a graph ring by ring using a FIFO queue; finds the shortest path by hop count when every edge costs the same.
• Dijkstra's algorithm — explores nodes in order of cheapest known cost first, using a priority queue; finds the shortest path by total cost on weighted graphs.
• Priority queue — a queue that always returns its lowest-priority item first, typically backed by a binary heap.
• A* — Dijkstra's algorithm with an added heuristic, so it searches in the goal's general direction instead of uniformly outward.
• Heuristic (h) — an estimate of the remaining cost from a node to the goal.
• g-cost — the real, known cost from the start to a node.
• f-cost — g + h; the value A*'s open set is sorted by.
• Admissible heuristic — a heuristic that never overestimates the true remaining cost; required for A* to guarantee the cheapest path.
• Manhattan distance — |dx| + |dy|; matches 4-directional grid movement.
• Euclidean distance — straight-line distance; matches free or diagonal movement.
• Open set — nodes discovered but not yet fully expanded.
• Closed set — nodes already fully expanded, so they are not reprocessed.
• came-from map — records, per node, which neighbor it was most cheaply reached from; used to rebuild the final path.
• String-pulling / path smoothing — collapsing a zigzag path into straight segments using line-of-sight checks.
• Navmesh — a mesh of connected walkable polygons covering a level's surface, used instead of a grid in most 3D games.
• Portal — a shared edge between two adjacent navmesh polygons, used as a graph edge for pathfinding.
• Funnel algorithm — the exact, navmesh-native version of string-pulling, threading a tight path through a sequence of portals.
• Recast — an open-source library that generates a navmesh from raw level geometry.
• Detour — the companion runtime library that runs pathfinding queries against a navmesh Recast built.
• NavMeshAgent — Unity's component for moving a character along a baked navmesh, including basic local avoidance.
• Hierarchical pathfinding — searching a coarse, cluster-level graph first, then refining the path only inside the clusters it passes through.
• Greedy best-first search — orders the open set by h alone; very fast, but not guaranteed to find the cheapest path.
• Consistency / monotonicity — a heuristic where h(n) <= cost(n, n') + h(n') for every edge; stronger than admissibility, and what makes A*'s closed-set optimization safe.
• Tie-breaking — a secondary rule for ordering open-set nodes that share an f-cost, sharply reducing how many nodes A* expands.
• Weighted A* — sorts by g + w*h with w > 1; faster than A* and guaranteed to return a path no worse than w times the optimal cost.
• Waypoint graph — a level represented as hand- or tool-placed points connected by clear straight-line edges.
• Replanning / repathing — computing a fresh path when the world or the target changes; done on a timer or a trigger, not every frame.
• D* Lite / LPA* — incremental search algorithms that repair the previous path around a change instead of replanning from scratch.
• Path following — the per-frame steering that moves an agent along a computed path (arrival, look-ahead, turn rate).
• Local avoidance — reactive, per-frame steering that keeps agents from colliding with each other and with moving obstacles; distinct from global pathfinding.
• Velocity obstacle — the set of an agent's velocities that would lead to a collision with a neighbour within a time horizon.
• RVO / ORCA — reciprocal local-avoidance methods where each agent takes a share of the avoidance; ORCA is the formulation used in Unity's agent avoidance.

22. Exercises

private static readonly (int dx, int dy, float cost)[] Directions8 =
{
    (0, -1, 1f), (0, 1, 1f), (-1, 0, 1f), (1, 0, 1f),                 // orthogonal
    (-1, -1, 1.41421356f), (1, -1, 1.41421356f),                      // diagonals
    (-1, 1, 1.41421356f), (1, 1, 1.41421356f),
};

private IEnumerable<(Vector2Int, float)> GetNeighbors(Vector2Int cell)
{
    foreach (var (dx, dy, cost) in Directions8)
    {
        Vector2Int next = cell + new Vector2Int(dx, dy);
        if (!map.IsWalkable(next.x, next.y)) continue;

        bool isDiagonal = dx != 0 && dy != 0;
        if (isDiagonal)
        {
            // block cutting a corner: both adjacent orthogonal cells must also be open
            bool sideA = map.IsWalkable(cell.x + dx, cell.y);
            bool sideB = map.IsWalkable(cell.x, cell.y + dy);
            if (!sideA || !sideB) continue;
        }

        yield return (next, cost);
    }
}

private float Heuristic(Vector2Int a, Vector2Int b)
{
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return Mathf.Sqrt(dx * dx + dy * dy); // Euclidean: matches free 8-directional movement
}