............
....oo..d...
....o.......
.p..oooo....
............

  1 #include <iostream>
  2 #include <vector>
  3 #include <unordered_set>
  4 #include <set>
  5 #include <string>
  6 #include <cstdlib>
  7 #include <climits>
  8 using namespace std;
  9 
 10 struct Point {
 11     int i, j;
 12     Point(int _i = -1, int _j = -1) : i(_i), j(_j) {}
 13 };
 14 
 15 int get_index(int cols, const Point& point) {
 16     return point.i * cols + point.j;
 17 }
 18 
 19 vector<Point> neighbor_points(vector<vector<char>>& grid, int index) {
 20     vector<Point> result;
 21     int rows = grid.size(), cols = grid[0].size();
 22     int i = index / cols, j = index % cols;
 23     int x[4], y[4];
 24     // 0: left, 1: right, 2: top, 3: bottom
 25     x[0] = i, x[1] = i, x[2] = i-1, x[3] = i+1;
 26     y[0] = j-1, y[1] = j+1, y[2] = j, y[3] = j;
 27     for (int k = 0; k < 4; k++) {
 28         if (x[k] >= 0 && x[k] < rows && y[k] >= 0 && y[k] < cols 
 29             && grid[x[k]][y[k]] != ‘o‘) {
 30             result.push_back(Point(x[k], y[k]));
 31         }
 32     }
 33     return result;
 34 }
 35 
 36 int h_score_estimate(const Point& p1, const Point& p2) {
 37     return abs(p2.i - p1.i) + abs(p2.j - p1.j);
 38 }
 39 
 40 void print_path(int cols, int index, const vector<int>& parent) {
 41     if (index != -1) {
 42         print_path(cols, parent[index], parent);
 43         cout << "(" << index/cols << ", " << index%cols << ") ";
 44     }
 45 }
 46 
 47 class my_comp {
 48     const vector<int>& f_score;
 49 public:
 50     my_comp(const vector<int>& f_scr) : f_score(f_scr) {}
 51     bool operator() (const int i, const int j) const {
 52         if (f_score[i] != f_score[j]) {
 53             return f_score[i] < f_score[j];
 54         } else return i < j;
 55     }
 56 };
 57     
 58 void A_star(vector<vector<char>>& grid, const Point& person, const Point& destination) {
 59     int rows = grid.size();
 60     int cols = grid[0].size();
 61     int size = rows * cols;
 62     
 63     // cost form the start point along best known path
 64     vector<int> g_score(size, INT_MAX);    
 65     // estimate total cost from the start point to the destination through a point 
 66     vector<int> f_score(size, INT_MAX);  
 67     
 68     vector<int> parent(size, -1);
 69     
 70     set<int, my_comp> open_set((my_comp(f_score))); // the set of node to be evaluated
 71     unordered_set<int> closed_set; // the set of node evaluated
 72 
 73     int psn_index = get_index(cols, person);
 74     int dst_index = get_index(cols, destination);
 75 
 76     g_score[psn_index] = 0;
 77     f_score[psn_index] = h_score_estimate(person, destination);
 78     open_set.insert(psn_index);
 79     
 80     while (!open_set.empty()) {
 81         // get the point with minimal f_score in open_set
 82         int cur_index = *(open_set.begin());
 83         //cout << "curr : (" << cur_index/cols << ", " << cur_index%cols << ")" << endl; 
 84         if (cur_index == dst_index) {
 85             cout << "destination found !!!" << endl;
 86             print_path(cols, dst_index, parent);
 87             cout << endl;
 88             return;
 89         }
 90         
 91         // switch the current point from open_set to close_set
 92         open_set.erase(cur_index);
 93         closed_set.insert(cur_index);
 94 
 95         auto neighbors = neighbor_points(grid, cur_index);
 96         //cout << "no. of neighbors : " << neighbors.size() << endl; 
 97         for (auto& neighbor : neighbors) {
 98             //cout << "nbr : (" << neighbor.i << ", " << neighbor.j << ")" << endl;
 99             int nbr_index = get_index(cols, neighbor);
100             if (closed_set.count(nbr_index) != 0) continue;
101 
102             int tentative_g_score = g_score[cur_index] + 1 /*dist_between(current, neighbor)*/;
103             if (open_set.count(nbr_index) == 0 || tentative_g_score < g_score[nbr_index]) {
104                 parent[nbr_index] = cur_index;
105                 g_score[nbr_index] = tentative_g_score;
106                 f_score[nbr_index] = g_score[nbr_index] + h_score_estimate(neighbor, destination);
107                 if (open_set.count(nbr_index) != 0) {
108                     open_set.erase(nbr_index);
109                 }
110                 open_set.insert(nbr_index);
111             }
112         }
113     }
114     cout << "destination not found !!!" << endl;
115 }
116 
117 int main() {
118     vector<vector<char>> grid;
119     Point person, destination;
120     string line;
121     int i = 0;
122     while (getline(cin, line)) {
123         vector<char> row;
124         for (int j = 0; j < line.size(); j++) {
125             if (line[j] == ‘p‘) {
126                 person = Point(i, j);
127             } else if (line[j] == ‘d‘) {
128                 destination = Point(i, j);
129             }
130             row.push_back(line[j]);
131         }
132         //cout << line << endl;
133         grid.push_back(row);
134         i++;
135     }
136 
137     A_star(grid, person, destination);
138     return 0;
139 }

destination found !!!
(3, 1) (2, 1) (1, 1) (1, 2) (1, 3) (0, 3) (0, 4) (0, 5) (0, 6) (0, 7) (0, 8) (1, 8)