A* Pathfinding Algorithm

Given
 an 
n
 x 
n 
grid 
with 
a 
person
 and 
obstacles, 
how
 would 
you 
find 
a
 path 
for 
the
 person
 to
 a
 particular 
destination?
 
The 
person
 is 
permitted
 to
 move 
left,
right,
 up,
 and
 down.

Sample Input:

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

'p' represents the start position, 'o' a obstacle, d ' the destination.

Output: a optimal path from  'p' to 'd' 

C++ Sample Code (using A* method to find a optimal path):

#include <iostream>
#include <vector>
#include <unordered_set>
#include <set>
#include <string>
#include <cstdlib>
#include <climits>
using namespace std;

struct Point {
    int i, j;
    Point(int _i = -1, int _j = -1) : i(_i), j(_j) {}
};

int get_index(int cols, const Point& point) {
    return point.i * cols + point.j;
}

vector<Point> neighbor_points(vector<vector<char>>& grid, int index) {
    vector<Point> result;
    int rows = grid.size(), cols = grid[0].size();
    int i = index / cols, j = index % cols;
    int x[4], y[4];
    // 0: left, 1: right, 2: top, 3: bottom
    x[0] = i, x[1] = i, x[2] = i-1, x[3] = i+1;
    y[0] = j-1, y[1] = j+1, y[2] = j, y[3] = j;
    for (int k = 0; k < 4; k++) {
        if (x[k] >= 0 && x[k] < rows && y[k] >= 0 && y[k] < cols 
            && grid[x[k]][y[k]] != 'o') {
            result.push_back(Point(x[k], y[k]));
        }
    }
    return result;
}

int h_score_estimate(const Point& p1, const Point& p2) {
    return abs(p2.i - p1.i) + abs(p2.j - p1.j);
}

void print_path(int cols, int index, const vector<int>& parent) {
    if (index != -1) {
        print_path(cols, parent[index], parent);
        cout << "(" << index/cols << ", " << index%cols << ") ";
    }
}

class my_comp {
    const vector<int>& f_score;
public:
    my_comp(const vector<int>& f_scr) : f_score(f_scr) {}
    bool operator() (const int i, const int j) const {
        if (f_score[i] != f_score[j]) {
            return f_score[i] < f_score[j];
        } else return i < j;
    }
};
    
void A_star(vector<vector<char>>& grid, const Point& person, const Point& destination) {
    int rows = grid.size();
    int cols = grid[0].size();
    int size = rows * cols;
    
    // cost form the start point along best known path
    vector<int> g_score(size, INT_MAX);    
    // estimate total cost from the start point to the destination through a point 
    vector<int> f_score(size, INT_MAX);  
    
    vector<int> parent(size, -1);
    
    set<int, my_comp> open_set((my_comp(f_score))); // the set of node to be evaluated
    unordered_set<int> closed_set; // the set of node evaluated

    int psn_index = get_index(cols, person);
    int dst_index = get_index(cols, destination);

    g_score[psn_index] = 0;
    f_score[psn_index] = h_score_estimate(person, destination);
    open_set.insert(psn_index);
    
    while (!open_set.empty()) {
        // get the point with minimal f_score in open_set
        int cur_index = *(open_set.begin());
        //cout << "curr : (" << cur_index/cols << ", " << cur_index%cols << ")" << endl; 
        if (cur_index == dst_index) {
            cout << "destination found !!!" << endl;
            print_path(cols, dst_index, parent);
            cout << endl;
            return;
        }
        
        // switch the current point from open_set to close_set
        open_set.erase(cur_index);
        closed_set.insert(cur_index);

        auto neighbors = neighbor_points(grid, cur_index);
        //cout << "no. of neighbors : " << neighbors.size() << endl; 
        for (auto& neighbor : neighbors) {
            //cout << "nbr : (" << neighbor.i << ", " << neighbor.j << ")" << endl;
            int nbr_index = get_index(cols, neighbor);
            if (closed_set.count(nbr_index) != 0) continue;

            int tentative_g_score = g_score[cur_index] + 1 /*dist_between(current, neighbor)*/;
            if (open_set.count(nbr_index) == 0 || tentative_g_score < g_score[nbr_index]) {
                parent[nbr_index] = cur_index;
                g_score[nbr_index] = tentative_g_score;
                f_score[nbr_index] = g_score[nbr_index] + h_score_estimate(neighbor, destination);
                if (open_set.count(nbr_index) != 0) {
                    open_set.erase(nbr_index);
                }
                open_set.insert(nbr_index);
            }
        }
    }
    cout << "destination not found !!!" << endl;
}

int main() {
    vector<vector<char>> grid;
    Point person, destination;
    string line;
    int i = 0;
    while (getline(cin, line)) {
        vector<char> row;
        for (int j = 0; j < line.size(); j++) {
            if (line[j] == 'p') {
                person = Point(i, j);
            } else if (line[j] == 'd') {
                destination = Point(i, j);
            }
            row.push_back(line[j]);
        }
        //cout << line << endl;
        grid.push_back(row);
        i++;
    }

    A_star(grid, person, destination);
    return 0;
}

The output of the program with the sample input above:

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) 

 

The terminologies used in A* algorithm are described on Wikipedia (https://en.wikipedia.org/wiki/A*_search_algorithm). 

Summary of the A* Method (From http://www.policyalmanac.org/games/aStarTutorial.htm)

1) Add the starting square (or node) to the open list (open_set in my code).

2) Repeat the following:

a) Look for the lowest F (f_score) cost square on the open list. We refer to this as the current square.

b) Switch it to the closed list (close_set).

c) For each of the 8 squares adjacent to this current square …  (In the problem above, we just consider at most 4 neighbors)

• If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.           

• If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square. 

• If it is on the open list already, check to see if this path to that square is better, using G cost as the measure. A lower G cost means that this is a better path. If so, change the parent of the square to the current square, and recalculate the G and F scores of the square. If you are keeping your open list sorted by F score, you may need to resort the list to account for the change.

d) Stop when you:

• Add the target square to the closed list, in which case the path has been found, or
• Fail to find the target square, and the open list is empty. In this case, there is no path.   

3) Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path.