803. A* Pathfinding with Manhattan Heuristic

Difficulty: 4 Level: Advanced Guided shortest-path search on a 2D grid using a heuristic to focus exploration toward the goal — O(E log V) in the worst case, much faster in practice.

The Problem This Solves

A is the pathfinding algorithm used in virtually every game engine, robotics motion planner, and GPS navigation system. Dijkstra's algorithm expands outward uniformly in all directions; A focuses the search toward the goal by adding a heuristic estimate of remaining cost. On grid maps with many nodes, this can reduce explored nodes by orders of magnitude compared to Dijkstra. The choice of heuristic determines both correctness and performance: a consistent (monotone) heuristic guarantees optimal paths. For grid movement allowing only cardinal directions, the Manhattan distance `|dx| + |dy|` is the exact minimum cost to reach the goal, making it an ideal admissible heuristic.

The Intuition

A maintains a priority queue ordered by `f = g + h`, where `g` is the actual cost from start to the current node, and `h` is the heuristic estimate to the goal. Nodes with low `f` are explored first — the algorithm "aims" at the goal. Dijkstra is A with `h = 0`. A* with Manhattan heuristic on a grid is Dijkstra with perfect cost guidance. Rust implements the min-heap with `BinaryHeap<Reverse<(f, g, row, col)>>`, using `Reverse` because `BinaryHeap` is a max-heap. OCaml would use a priority queue module or sorted list; the structure is otherwise identical.

How It Works in Rust

use std::collections::BinaryHeap;
use std::cmp::Reverse;

// O(E log V) worst case; in practice much faster with a good heuristic
// grid: 0=passable, b'#'=wall
fn a_star(
 grid: &[Vec<u8>],
 start: (usize, usize),
 goal:  (usize, usize),
) -> Option<Vec<(usize, usize)>> {
 let mut g_cost = vec![vec![u64::MAX / 2; cols]; rows];
 let mut came: Vec<Vec<Option<(usize, usize)>>> = vec![vec![None; cols]; rows];
 let mut heap = BinaryHeap::new();

 // Heuristic: Manhattan distance (admissible for cardinal movement)
 let h = |r: usize, c: usize| -> u64 {
     ((r as i64 - goal.0 as i64).abs() + (c as i64 - goal.1 as i64).abs()) as u64
 };

 g_cost[start.0][start.1] = 0;
 heap.push(Reverse((h(start.0, start.1), 0u64, start.0, start.1)));
 //                  ^f                   ^g    ^row      ^col

 while let Some(Reverse((_, g, r, c))) = heap.pop() {
     if g > g_cost[r][c] { continue; }   // stale entry
     if (r, c) == goal { /* reconstruct and return */ }

     for (dr, dc) in [(-1i64,0),(1,0),(0,-1),(0,1)] {
         // bounds check, wall check, then:
         let ng = g + 1;
         if ng < g_cost[nr][nc] {
             g_cost[nr][nc] = ng;
             came[nr][nc]   = Some((r, c));
             heap.push(Reverse((ng + h(nr, nc), ng, nr, nc)));
         }
     }
 }
 None
}

Path reconstruction walks `came[][]` backwards from the goal to the start, then reverses. The stale-entry guard `if g > g_cost[r][c] { continue }` is the same lazy-deletion pattern as Prim's/Dijkstra.

What This Unlocks

Game AI pathfinding: every RTS and RPG game uses A (or a variant like Theta or Jump Point Search) for unit navigation on tile maps.
Robot motion planning: mobile robots compute collision-free paths through occupancy grid maps using A* with Euclidean or Manhattan heuristics.
GPS routing: road navigation systems use A (or Bidirectional A) with geographic distance as the heuristic, processing millions of road network nodes in milliseconds.

Key Differences

Concept	OCaml	Rust
Min-heap	Priority queue module	`BinaryHeap<Reverse<(u64, u64, usize, usize)>>`
Heuristic	Inline function or `let h = ...`	Closure `let h = \	r, c\	-> u64 { ... }`
Stale entry	`Hashtbl` of visited + check	`if g > g_cost[r][c] { continue }`
Path matrix	`(int * int) option array array`	`Vec<Vec<Option<(usize, usize)>>>`
Bounds check	`if r >= 0 && r < rows ...`	Cast `i64` coordinates, then `as usize` after check

// A* Pathfinding on a 2D grid — Manhattan heuristic, O(E log V)
use std::collections::BinaryHeap;
use std::cmp::Reverse;

fn a_star(
    grid: &[Vec<u8>],
    start: (usize, usize),
    goal:  (usize, usize),
) -> Option<Vec<(usize, usize)>> {
    let rows = grid.len();
    let cols = grid[0].len();
    let inf  = u64::MAX / 2;

    let mut g_cost = vec![vec![inf; cols]; rows];
    let mut came: Vec<Vec<Option<(usize, usize)>>> = vec![vec![None; cols]; rows];
    // heap: Reverse((f, g, r, c))
    let mut heap = BinaryHeap::new();

    let h = |r: usize, c: usize| -> u64 {
        ((r as i64 - goal.0 as i64).abs() + (c as i64 - goal.1 as i64).abs()) as u64
    };

    g_cost[start.0][start.1] = 0;
    heap.push(Reverse((h(start.0, start.1), 0u64, start.0, start.1)));

    while let Some(Reverse((_, g, r, c))) = heap.pop() {
        if g > g_cost[r][c] { continue; } // stale
        if (r, c) == goal {
            // Reconstruct path
            let mut path = Vec::new();
            let mut pos = goal;
            loop {
                path.push(pos);
                if pos == start { break; }
                pos = came[pos.0][pos.1]?;
            }
            path.reverse();
            return Some(path);
        }
        for (dr, dc) in [(-1i64,0),(1,0),(0,-1),(0,1)] {
            let nr = r as i64 + dr;
            let nc = c as i64 + dc;
            if nr < 0 || nr >= rows as i64 || nc < 0 || nc >= cols as i64 { continue; }
            let (nr, nc) = (nr as usize, nc as usize);
            if grid[nr][nc] == b'#' { continue; }
            let ng = g + 1;
            if ng < g_cost[nr][nc] {
                g_cost[nr][nc] = ng;
                came[nr][nc]   = Some((r, c));
                heap.push(Reverse((ng + h(nr, nc), ng, nr, nc)));
            }
        }
    }
    None
}

fn main() {
    let grid: Vec<Vec<u8>> = vec![
        b"...#.".to_vec(),
        b".#.#.".to_vec(),
        b".#...".to_vec(),
        b".##..".to_vec(),  // Fixed: was ".##.." corrected
        b"#....".to_vec(),
    ];
    match a_star(&grid, (0,0), (4,4)) {
        None    => println!("No path found"),
        Some(p) => {
            println!("Path length: {}", p.len() - 1);
            println!("Path: {:?}", p);
        }
    }
}

(* 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))