C++: A* search with a Manhattan-distance heuristic
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.
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";
}