1#include <bits/stdc++.h>
2using namespace std;
3
4// Manhattan heuristic.
5int h(pair<int,int> a, pair<int,int> b) {
6 return abs(a.first - b.first) + abs(a.second - b.second);
7}
8
9vector<pair<int,int>> aStar(vector<vector<int>>& grid,
10 pair<int,int> start,
11 pair<int,int> goal) {
12 int R = grid.size(), C = grid[0].size();
13 map<pair<int,int>, int> g;
14 map<pair<int,int>, pair<int,int>> par;
15 priority_queue<tuple<int,int,int,int>,
16 vector<tuple<int,int,int,int>>,
17 greater<>> pq;
18 g[start] = 0; par[start] = {-1, -1};
19 pq.push({h(start, goal), 0, start.first, start.second});
20 int dr[] = {1,-1,0,0}, dc[] = {0,0,1,-1};
21 while (!pq.empty()) {
22 auto [f, gc, r, c] = pq.top(); pq.pop();
23 if (make_pair(r, c) == goal) break;
24 if (gc > g[{r, c}]) continue;
25 for (int k = 0; k < 4; ++k) {
26 int nr = r + dr[k], nc = c + dc[k];
27 if (nr < 0 || nc < 0 || nr >= R || nc >= C || grid[nr][nc] == 1) continue;
28 int ng = gc + 1;
29 if (!g.count({nr, nc}) || ng < g[{nr, nc}]) {
30 g[{nr, nc}] = ng; par[{nr, nc}] = {r, c};
31 pq.push({ng + h({nr, nc}, goal), ng, nr, nc});
32 }
33 }
34 }
35 vector<pair<int,int>> path;
36 if (!par.count(goal)) return path;
37 for (auto cur = goal; cur.first != -1; cur = par[cur]) path.push_back(cur);
38 reverse(path.begin(), path.end());
39 return path;
40}
