803 Advanced

803-a-star-pathfinding — A* Pathfinding

Functional Programming

Tutorial Video

Text description (accessibility)

This video demonstrates the "803-a-star-pathfinding — A* Pathfinding" functional Rust example. Difficulty level: Advanced. Key concepts covered: Functional Programming. A* (Hart, Nilsson, Raphael, 1968) is the standard pathfinding algorithm in games, robotics, and navigation systems. Key difference from OCaml: 1. **Priority queue**: Rust's `BinaryHeap<Reverse<...>>` is efficient; OCaml's balanced BST

Tutorial

The Problem

A* (Hart, Nilsson, Raphael, 1968) is the standard pathfinding algorithm in games, robotics, and navigation systems. It improves on Dijkstra by adding a heuristic that guides the search toward the goal, dramatically reducing explored nodes. The Manhattan distance heuristic is admissible (never overestimates) on grids, guaranteeing optimal paths. Used in every major game engine (Unity NavMesh, Unreal AI), Google Maps, and robot motion planning.

🎯 Learning Outcomes

• Implement A* using an open set (BinaryHeap<Reverse<(f_score, position)>>) with came_from map

• Use the Manhattan distance |dx| + |dy| as an admissible heuristic for grid movement

• Track g_score (cost from start) and f_score = g_score + h(goal) per node

• Reconstruct the path by walking the came_from map backward from goal to start

• Understand why admissible heuristics guarantee optimal paths

Code Example

#![allow(clippy::all)]
//! # A* Pathfinding
use std::cmp::Reverse;
use std::collections::{BinaryHeap, HashMap};

pub fn astar(
    start: (i32, i32),
    goal: (i32, i32),
    obstacles: &[(i32, i32)],
) -> Option<Vec<(i32, i32)>> {
    let obs: std::collections::HashSet<_> = obstacles.iter().copied().collect();
    let heuristic = |p: (i32, i32)| ((p.0 - goal.0).abs() + (p.1 - goal.1).abs()) as usize;
    let dirs = [(0, 1), (0, -1), (1, 0), (-1, 0)];

    let mut open = BinaryHeap::new();
    let mut g_score = HashMap::new();
    let mut came_from = HashMap::new();

    g_score.insert(start, 0usize);
    open.push(Reverse((heuristic(start), start)));

    while let Some(Reverse((_, current))) = open.pop() {
        if current == goal {
            let mut path = vec![current];
            let mut c = current;
            while let Some(&p) = came_from.get(&c) {
                path.push(p);
                c = p;
            }
            path.reverse();
            return Some(path);
        }

        for &(dx, dy) in &dirs {
            let next = (current.0 + dx, current.1 + dy);
            if obs.contains(&next) {
                continue;
            }
            let new_g = g_score[&current] + 1;
            if !g_score.contains_key(&next) || new_g < g_score[&next] {
                g_score.insert(next, new_g);
                came_from.insert(next, current);
                open.push(Reverse((new_g + heuristic(next), next)));
            }
        }
    }
    None
}

#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    fn test_astar() {
        let path = astar((0, 0), (2, 2), &[(1, 1)]);
        assert!(path.is_some());
    }
}

(* A* Pathfinding on a grid — Manhattan heuristic *)
(* grid: '.' = open, '#' = wall, 'S' = start, 'E' = end *)

module PQ = Set.Make(struct
  type t = int * int * (int * int)  (* (f, g, pos) *)
  let compare (f1,g1,(r1,c1)) (f2,g2,(r2,c2)) =
    let cf = compare f1 f2 in if cf <> 0 then cf
    else let cg = compare g1 g2 in if cg <> 0 then cg
    else compare (r1,c1) (r2,c2)
end)

let a_star grid start goal =
  let rows = Array.length grid in
  let cols = Array.length grid.(0) in
  let inf  = max_int / 2 in
  let g_cost = Array.init rows (fun _ -> Array.make cols inf) in
  let came  = Array.init rows (fun _ -> Array.make cols None) in
  let (sr, sc) = start and (er, ec) = goal in
  let h r c = abs (r - er) + abs (c - ec) in
  g_cost.(sr).(sc) <- 0;
  let open_set = ref (PQ.singleton (h sr sc, 0, start)) in
  let found    = ref false in
  while not (PQ.is_empty !open_set) && not !found do
    let (_, g, (r, c)) = PQ.min_elt !open_set in
    open_set := PQ.remove (PQ.min_elt !open_set) !open_set;
    if g > g_cost.(r).(c) then ()  (* stale entry *)
    else if (r,c) = goal then found := true
    else List.iter (fun (dr, dc) ->
      let nr = r + dr and nc = c + dc in
      if nr >= 0 && nr < rows && nc >= 0 && nc < cols
         && grid.(nr).(nc) <> '#' then begin
        let ng = g + 1 in
        if ng < g_cost.(nr).(nc) then begin
          g_cost.(nr).(nc) <- ng;
          came.(nr).(nc)   <- Some (r, c);
          open_set := PQ.add (ng + h nr nc, ng, (nr, nc)) !open_set
        end
      end
    ) [(-1,0);(1,0);(0,-1);(0,1)]
  done;
  if not !found then None
  else begin
    let path = ref [] in
    let pos  = ref goal in
    while !pos <> start do
      path := !pos :: !path;
      let (r,c) = !pos in
      pos := match came.(r).(c) with Some p -> p | None -> start
    done;
    path := start :: !path;
    Some !path
  end

let () =
  let grid = [|
    [|'.';'.';'.';'#';'.'|];
    [|'.';'#';'.';'#';'.'|];
    [|'.';'#';'.';'.';'.'|];
    [|'.';'.';'#';'#';'.'|];
    [|'#';'.';'.';'.';'.'|];
  |] in
  match a_star grid (0,0) (4,4) with
  | None   -> Printf.printf "No path found\n"
  | Some p ->
    Printf.printf "Path length: %d\n" (List.length p - 1);
    Printf.printf "Path: %s\n"
      (String.concat " -> " (List.map (fun (r,c) -> Printf.sprintf "(%d,%d)" r c) p))

Key Differences

Priority queue: Rust's BinaryHeap<Reverse<...>> is efficient; OCaml's balanced BST-based Set has higher constant factors but equivalent asymptotic complexity.

Heuristic pluggability: Rust's closure let heuristic = |p| ... makes it easy to swap heuristics; OCaml uses first-class functions similarly.

Tuple comparison: Rust's Reverse((f_score, pos)) sorts by f_score first due to tuple lexicographic order; OCaml's custom Set.compare must be explicit.

Game use: Unity's NavMesh, Unreal's PathFollowingComponent, and Godot's NavigationAgent all use A* variants internally; Rust game engines use the same algorithm.

OCaml Approach

OCaml implements A* using Map.t for g_score and came_from, and Set.t as the open set with a custom ordering by f_score. OCaml's Hashtbl provides faster mutable alternatives. The OcamlAI library and various game frameworks use OCaml A* implementations. The path reconstruction uses List.rev on the accumulated path.

Full Source

#![allow(clippy::all)]
//! # A* Pathfinding
use std::cmp::Reverse;
use std::collections::{BinaryHeap, HashMap};

pub fn astar(
    start: (i32, i32),
    goal: (i32, i32),
    obstacles: &[(i32, i32)],
) -> Option<Vec<(i32, i32)>> {
    let obs: std::collections::HashSet<_> = obstacles.iter().copied().collect();
    let heuristic = |p: (i32, i32)| ((p.0 - goal.0).abs() + (p.1 - goal.1).abs()) as usize;
    let dirs = [(0, 1), (0, -1), (1, 0), (-1, 0)];

    let mut open = BinaryHeap::new();
    let mut g_score = HashMap::new();
    let mut came_from = HashMap::new();

    g_score.insert(start, 0usize);
    open.push(Reverse((heuristic(start), start)));

    while let Some(Reverse((_, current))) = open.pop() {
        if current == goal {
            let mut path = vec![current];
            let mut c = current;
            while let Some(&p) = came_from.get(&c) {
                path.push(p);
                c = p;
            }
            path.reverse();
            return Some(path);
        }

        for &(dx, dy) in &dirs {
            let next = (current.0 + dx, current.1 + dy);
            if obs.contains(&next) {
                continue;
            }
            let new_g = g_score[&current] + 1;
            if !g_score.contains_key(&next) || new_g < g_score[&next] {
                g_score.insert(next, new_g);
                came_from.insert(next, current);
                open.push(Reverse((new_g + heuristic(next), next)));
            }
        }
    }
    None
}

#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    fn test_astar() {
        let path = astar((0, 0), (2, 2), &[(1, 1)]);
        assert!(path.is_some());
    }
}

(* A* Pathfinding on a grid — Manhattan heuristic *)
(* grid: '.' = open, '#' = wall, 'S' = start, 'E' = end *)

module PQ = Set.Make(struct
  type t = int * int * (int * int)  (* (f, g, pos) *)
  let compare (f1,g1,(r1,c1)) (f2,g2,(r2,c2)) =
    let cf = compare f1 f2 in if cf <> 0 then cf
    else let cg = compare g1 g2 in if cg <> 0 then cg
    else compare (r1,c1) (r2,c2)
end)

let a_star grid start goal =
  let rows = Array.length grid in
  let cols = Array.length grid.(0) in
  let inf  = max_int / 2 in
  let g_cost = Array.init rows (fun _ -> Array.make cols inf) in
  let came  = Array.init rows (fun _ -> Array.make cols None) in
  let (sr, sc) = start and (er, ec) = goal in
  let h r c = abs (r - er) + abs (c - ec) in
  g_cost.(sr).(sc) <- 0;
  let open_set = ref (PQ.singleton (h sr sc, 0, start)) in
  let found    = ref false in
  while not (PQ.is_empty !open_set) && not !found do
    let (_, g, (r, c)) = PQ.min_elt !open_set in
    open_set := PQ.remove (PQ.min_elt !open_set) !open_set;
    if g > g_cost.(r).(c) then ()  (* stale entry *)
    else if (r,c) = goal then found := true
    else List.iter (fun (dr, dc) ->
      let nr = r + dr and nc = c + dc in
      if nr >= 0 && nr < rows && nc >= 0 && nc < cols
         && grid.(nr).(nc) <> '#' then begin
        let ng = g + 1 in
        if ng < g_cost.(nr).(nc) then begin
          g_cost.(nr).(nc) <- ng;
          came.(nr).(nc)   <- Some (r, c);
          open_set := PQ.add (ng + h nr nc, ng, (nr, nc)) !open_set
        end
      end
    ) [(-1,0);(1,0);(0,-1);(0,1)]
  done;
  if not !found then None
  else begin
    let path = ref [] in
    let pos  = ref goal in
    while !pos <> start do
      path := !pos :: !path;
      let (r,c) = !pos in
      pos := match came.(r).(c) with Some p -> p | None -> start
    done;
    path := start :: !path;
    Some !path
  end

let () =
  let grid = [|
    [|'.';'.';'.';'#';'.'|];
    [|'.';'#';'.';'#';'.'|];
    [|'.';'#';'.';'.';'.'|];
    [|'.';'.';'#';'#';'.'|];
    [|'#';'.';'.';'.';'.'|];
  |] in
  match a_star grid (0,0) (4,4) with
  | None   -> Printf.printf "No path found\n"
  | Some p ->
    Printf.printf "Path length: %d\n" (List.length p - 1);
    Printf.printf "Path: %s\n"
      (String.concat " -> " (List.map (fun (r,c) -> Printf.sprintf "(%d,%d)" r c) p))

✓ Tests Rust test suite

#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    fn test_astar() {
        let path = astar((0, 0), (2, 2), &[(1, 1)]);
        assert!(path.is_some());
    }
}

Deep Comparison

OCaml vs Rust: A Star Pathfinding

Overview

See the example.rs and example.ml files for detailed implementations.

Key Differences

Aspect	OCaml	Rust
Type system	Hindley-Milner	Ownership + traits
Memory	GC	Zero-cost abstractions
Mutability	Explicit ref	mut keyword
Error handling	Option/Result	Result<T, E>

See README.md for detailed comparison.

Exercises

Add diagonal movement (8 directions) with a cost of √2 for diagonals, and switch the heuristic to Euclidean distance.

Implement weighted A* (multiply heuristic by a weight w > 1) to trade optimality for speed. Compare path quality and nodes expanded for w = 1, 1.5, 2.0.

Add a max_steps limit to A* and return the partial path if the goal is not reached within the step budget — useful for real-time game AI.

Open Source Repos

functional-rust

View the source for this example on GitHub — OCaml and Rust side by side in the repo.

Rust