A* Algorithm in C++

astar.cpp

#include <bits/stdc++.h>

using namespace std;
typedef long long ll;
typedef vector<vector<ll> > matrix;

map<ll, float> cost2d(const matrix &conn, const ll goal) {
  ll h = conn.size();
  ll w = conn[0].size();
  float x = goal % w;
  float y = (goal-x)/w;
  map<ll, float> costs;
  for (ll j=0; j < h; ++j) {
    for (ll k=0; k < w; ++k) {
      //costs[j*w+k] = abs(x-k) + abs(y-j); // Manhattan
      costs[j*w+k] = sqrt((x-k) * (x-k) + (y-j) * (y-j)); // L2
    }
  }
  return costs;
}

map<ll, float> cost(const matrix &conn, const ll goal) {
  ll num_of_nodes = conn.size();
  map<ll, float> costs;
  stack<ll> s;
  s.push(goal);
  ll acc = 0;
  while (!s.empty()) {
    ll cur = s.top(); s.pop();
    acc += 1;

    for (ll j=0; j < num_of_nodes; ++j) {
      if (conn[cur][j] > 0 && costs[j] == 0) {
        costs[j] = acc;
        s.push(j);
      }
    }
  }
  return costs;
}

list<ll> open_candidates2d(const matrix &conn, const ll cur) {
  ll h = conn.size();
  ll w = conn[0].size();
  ll x = cur % w;
  ll y = (cur-x)/w;
  list<ll> candidates;
  if (0 < x && conn[y][x-1]) candidates.push_back(y*w + (x-1));
  if (0 < y && conn[y-1][x]) candidates.push_back((y-1)*w + x);
  if (x < w-1 && conn[y][x+1]) candidates.push_back(y*w+(x+1));
  if (y < h-1 && conn[y+1][x]) candidates.push_back((y+1)*w+x);
  return candidates;
}

list<ll> open_candidates(const matrix &conn, const ll cur) {
  ll num_of_nodes = conn.size();
  list<ll> candidates;
  for (ll j=0; j < num_of_nodes; ++j) {
    if (conn[cur][j] > 0) {
      candidates.push_front(j);
    }
  }
  return candidates;
}

list<ll> reconstruct_path(const vector<ll> pass, ll cur) {
  list<ll> total_path = {};
  while (cur != -1) {
    total_path.push_front(cur);
    cur = pass[cur];
  }
  return total_path;
}

list<ll> astar(const matrix &conn, const ll start, const ll goal, bool two_dimension = false) {
  ll num_of_nodes = two_dimension ? conn.size()*conn[0].size() : conn.size();
  auto heu = two_dimension ? cost2d(conn, goal) : cost(conn, goal);
  map<ll, float> score, acc; for (ll j=0; j < num_of_nodes; ++j) score[j] = acc[j] = 1000000.;
  acc[start] = 0.; score[start] = heu[start];
  vector<int> opened;
  opened.push_back(start);
  unordered_map<ll, bool> closed; closed[start] = true;
  vector<ll> pass(num_of_nodes, -1);
  cout << "Start(" << num_of_nodes << " nodes)" << endl;
  if (two_dimension) {
    ll w = conn[0].size();
    ll xs = start % w; ll ys = (start-xs)/w; ll xg = goal % w; ll yg = (goal-xg)/w;
    cout << "(" << xs << "," << ys << ") -> (" << xg << "," << yg << ")" << endl;
  } else {
    cout << start << " -> " << goal << endl;;
  }
  while (!opened.empty()) {
    int cur;
    sort(opened.rbegin(), opened.rend(), [&](ll l, ll r) {
          return score[l] < score[r];
        });
    cur = opened.back(); opened.pop_back();
    if (cur == goal) break;
    closed[cur] = true;
    auto candidates = two_dimension ? open_candidates2d(conn, cur) : open_candidates(conn, cur);
    for (auto node : candidates) {
      if (!closed[node]) {
        if (find(opened.begin(), opened.end(), node) == opened.end()) {
          opened.push_back(node);
          pass[node] = cur;
        } else if (acc[cur] + (two_dimension?1:conn[cur][node]) >= acc[node]) {
          continue;
        }
        pass[node] = cur;
        acc[node] = acc[cur]+(two_dimension?1:conn[cur][node]);
        score[node] = acc[node] + heu[node];
      }
    }
  }
  return reconstruct_path(pass, goal);
}

//// for Graph
// N M S G
// A_1 B_1
// A_2 B_2
// ...
// A_M B_M

//// for 2D
// W H 0 0
// X_S Y_S X_G Y_G
// ....#.
// ...##.
// ..###.
// .####.
// ......

int main() {
  ll N, M, S, G; cin >> N >> M >> S >> G;
  if (S == G) { // 2D
    ll xs, ys, xg, yg; cin >> xs >> ys >> xg >> yg;
    matrix conn(M, vector<ll>(N, 1));
    for (ll y=0; y < M; ++y) {
      string row; cin >> row;
      for (ll x=0; x < N; ++x) {
        if (row[x] == '#') {
          conn[y][x] = 0;
        }
      }
    }
    cout << "2D" << endl;
    auto path = astar(conn, ys*N+xs, yg*N+xg, true);
    for (ll p : path) {
      ll x = p % N;
      ll y = (p-x)/N;
      conn[y][x] = 2;
    }
    for (ll j=0; j < M; ++j) {
      for (ll k=0; k < N; ++k) {
        cout << (conn[j][k] == 0 ? '#' : (conn[j][k] == 1 ? '.' : 'o'));
      }
      cout << endl;
    }
  } else {
    matrix conn(N, vector<ll>(N, -1));
    for (ll j=0; j < M; ++j) {
      ll A, B, C; cin >> A >> B >> C;
      conn[A][B] = conn[B][A] = C;
    }
    auto path = astar(conn, S, G);
    for (ll p : path) {
      cout << p << " ";
    }
    cout << endl;
  }
  return 0;
}