C++: A* search with a Manhattan-distance heuristic

Before you start

C++ priority_queue, tuples, and grids.

Guide a shortest-path search with an estimate of the remaining distance.
Predict the sample output and explain the state changes.
Complete the practice task and check the boundary cases.

The concept

A* orders its frontier by f = g + h, where g is the cost so far and h is a heuristic estimate of the remaining cost. When h never overestimates the true remaining cost, it is called admissible, and A* returns a shortest path. On a grid with four-direction moves, the Manhattan distance is admissible.

A priority queue holds the frontier, with the smallest f first. This version records the best known cost for each cell and skips stale queue entries. It reports the path length and how many cells were expanded, which shows the benefit of the heuristic compared with breadth-first search.

Code example

C++

#include <cstdlib>
#include <iostream>
#include <queue>
#include <string>
#include <tuple>
#include <vector>

int main() {
  std::vector<std::string> grid = {
      "S..#.",
      ".#...",
      ".#.#.",
      "....G",
  };
  int rows = 4, cols = 5;
  int goal_r = 3, goal_c = 4;
  auto h = [&](int r, int c) { return std::abs(r - goal_r) + std::abs(c - goal_c); };

  std::vector<std::vector<int>> best(rows, std::vector<int>(cols, 1000));
  using Item = std::tuple<int, int, int, int, int>;  // f, h, g, row, col
  std::priority_queue<Item, std::vector<Item>, std::greater<Item>> open;
  best[0][0] = 0;
  open.emplace(h(0, 0), h(0, 0), 0, 0, 0);

  int expanded = 0;
  int dr[] = {1, -1, 0, 0};
  int dc[] = {0, 0, 1, -1};
  while (!open.empty()) {
    auto [f, hv, g, r, c] = open.top();
    open.pop();
    if (g > best[r][c]) {
      continue;
    }
    ++expanded;
    if (r == goal_r && c == goal_c) {
      std::cout << "path length " << g << ", expanded " << expanded << " cells\n";
      return 0;
    }
    for (int k = 0; k < 4; ++k) {
      int nr = r + dr[k], nc = c + dc[k];
      if (nr < 0 || nr >= rows || nc < 0 || nc >= cols || grid[nr][nc] == '#') {
        continue;
      }
      if (g + 1 < best[nr][nc]) {
        best[nr][nc] = g + 1;
        open.emplace(g + 1 + h(nr, nc), h(nr, nc), g + 1, nr, nc);
      }
    }
  }
  std::cout << "no path\n";
}

Output

path length 7, expanded 8 cells

Example details

Follow the code

Open the walkthrough to inspect the example one step at a time.

Show guided walkthrough

Guided walkthroughReady

1#include <cstdlib>

2#include <iostream>

3#include <queue>

4#include <string>

5#include <tuple>

6#include <vector>

8int main() {

9 std::vector<std::string> grid = {

10 "S..#.",

11 ".#...",

12 ".#.#.",

13 "....G",

14 };

15 int rows = 4, cols = 5;

16 int goal_r = 3, goal_c = 4;

17 auto h = [&](int r, int c) { return std::abs(r - goal_r) + std::abs(c - goal_c); };

19 std::vector<std::vector<int>> best(rows, std::vector<int>(cols, 1000));

20 using Item = std::tuple<int, int, int, int, int>; // f, h, g, row, col

21 std::priority_queue<Item, std::vector<Item>, std::greater<Item>> open;

22 best[0][0] = 0;

23 open.emplace(h(0, 0), h(0, 0), 0, 0, 0);

25 int expanded = 0;

26 int dr[] = {1, -1, 0, 0};

27 int dc[] = {0, 0, 1, -1};

28 while (!open.empty()) {

29 auto [f, hv, g, r, c] = open.top();

30 open.pop();

31 if (g > best[r][c]) {

32 continue;

33 }

34 ++expanded;

35 if (r == goal_r && c == goal_c) {

36 std::cout << "path length " << g << ", expanded " << expanded << " cells\n";

37 return 0;

38 }

39 for (int k = 0; k < 4; ++k) {

40 int nr = r + dr[k], nc = c + dc[k];

41 if (nr < 0 || nr >= rows || nc < 0 || nc >= cols || grid[nr][nc] == '#') {

42 continue;

43 }

44 if (g + 1 < best[nr][nc]) {

45 best[nr][nc] = g + 1;

46 open.emplace(g + 1 + h(nr, nc), h(nr, nc), g + 1, nr, nc);

47 }

48 }

49 }

50 std::cout << "no path\n";

51}

Follow the example

Start to inspect the source, values, and output. These prepared steps explain the published sample; grouped loop steps describe multiple iterations.

View the complete dry-run table

Prepared trace of the published program. Some rows group repeated iterations.
What happens	State after step	Output so far
Manhattan distance never overestimates when moves are four-directional with cost 1.	`—`	—
Start with f = 0 + 7 for the start cell. Ties in f are broken in favour of the smaller h, the cell nearer the goal.	`open = {(f=7, h=7, g=0, (0,0))}`	—
Skip entries that were superseded by a cheaper route to the same cell.	`open = {(f=7, h=7, g=0, (0,0))}`	—
The goal is popped with g = 7, the shortest possible, after expanding only 8 cells, exactly the cells of the path, out of the 16 open cells.	`g = 7`	path length 7, expanded 8 cells

Behind the scenes

How C++ processes this code

The C++ compiler produces a native executable. Execution enters main, where objects and function calls perform the work.

Cost and common mistakes

Worst case exponential in general; on a grid with a perfect heuristic it expands about the cells on the shortest path.

If h overestimates, A* may return a path that is not the shortest. Admissibility is what guarantees optimality.
Using the heuristic h = 0 turns A* into Dijkstra's algorithm. A better h expands fewer nodes.
Diagonal moves need a different heuristic, such as the Chebyshev or octile distance.

Try it yourself

What heuristic makes A* behave like breadth-first search?

Reveal answer and reasoning

h = 0 for every cell; then f equals g and cells are expanded in order of distance from the start.