How to store a complete path that a priority queue follows while performing A* search - c++

Question

I have been give a problem in which I am provided with user-entered matrix (rows and columns). User will also provide Start State (row and column) and the Goal State.
The job is to use A* search to find the path from the start node to the goal node.
A sample matrix is provided below,
0 0 0 0 0 0 0 0 0 0
0 0 0 1 1 0 0 0 0 G
0 0 0 1 1 0 0 0 1 1
0 0 0 0 0 0 0 0 0 0
0 1 1 0 0 0 0 0 0 1
0 0 0 0 0 0 0 0 1 0
0 0 0 0 0 1 1 0 0 0
0 0 0 0 0 1 1 0 0 0
0 1 0 1 0 1 1 0 0 0
0 1 0 1 0 1 1 0 0 0
0 0 0 0 0 0 0 0 0 0
S 0 0 0 0 0 0 0 0 0
0 1 0 0 0 1 1 0 0 0
0 0 0 0 0 1 1 0 0 0
where "S" is the start state, and "G" is the goal state. 0 are the states, in which you can move to and 1 are the obstacles in the grid, you can't move to them.
There are 3 actions allowed.
Up one cell (cost is 1)
right one cell (cost is 3)
diagonally up towards the right (cost is 2)
To solve this problem, I used Manhattan's Distance as my heuristic function and calculated the heuristic values for all my states.... They looked something like this (for the grid specified above)
10 9 8 7 6 5 4 3 2 1
9 8 7 6 5 4 3 2 1 0
10 9 8 7 6 5 4 3 2 1
11 10 9 8 7 6 5 4 3 2
12 11 10 9 8 7 6 5 4 3
13 12 11 10 9 8 7 6 5 4
14 13 12 11 10 9 8 7 6 5
15 14 13 12 11 10 9 8 7 6
16 15 14 13 12 11 10 9 8 7
17 16 15 14 13 12 11 10 9 8
18 17 16 15 14 13 12 11 10 9
19 18 17 16 15 14 13 12 11 10
20 19 18 17 16 15 14 13 12 11
21 20 19 18 17 16 15 14 13 12
Now, this is my code for A* search
void A_search()
{
priority_queue<node, vector<node>, CompareCost>q; // Priority Queue is used.
// node contains 4 elements... 1. "Heuristic" value, 2. Index row, 3. Index Col, 4. Actual Cost until this point
q.push(node(heuristic[srow][scol], srow, scol, 0)); // srow, scol is start state. 0 is Actual Cost
while (!q.empty())
{
node temp = q.top();
path_cost = temp.cost; // path_cost is global variable, which stores actual cost until this point
path[temp.i][temp.j] = true; // Boolean array, which tells the path followed so far.
q.pop();
if (temp.i == grow && temp.j == gcol) // If goal state is found, we break out of the loop
break;
if (temp.i > 0) // Checking for rows above the current state.
{
if (arr[temp.i - 1][temp.j] != 1) // Checking if index above current state is obstacle or not
{
q.push(node(heuristic[temp.i - 1][temp.j] + (temp.cost+1), temp.i - 1, temp.j, temp.cost + 1)); // pushing the above index into queue
}
if (temp.j - 1 < cols)
{
if (arr[temp.i - 1][temp.j + 1] != 1) // Diagonal Index checking
{
q.push(node(heuristic[temp.i - 1][temp.j + 1] + (temp.cost + 2), temp.i - 1, temp.j + 1, temp.cost + 2));
}
}
}
if (temp.j - 1 < cols) // Horizontal Index... Checking if column has exceeded the total cols or not
{
if (arr[temp.i][temp.j + 1] != 1) // Obstacle check for horizontal index
{
q.push(node(heuristic[temp.i][temp.j + 1] + (temp.cost + 3), temp.i, temp.j + 1, temp.cost + 3));
}
}
}
}
And this is the result I get after running this algorithm (Please note that # represents the path taken by the program... I am simply using a boolean 2D array to check which nodes are being visited by Priority Queue. For those indexes only, I am printing # and rest of the grid remains the same)
0 0 0 0 0 # # # # #
# # # 1 1 # # # # G
# # # 1 1 # # # 1 1
# # 0 # # # # # 0 0
# 1 1 # # # # 0 0 1
# # # # # # # 0 1 0
# # # # # 1 1 0 0 0
# # # # 0 1 1 0 0 0
# 1 # 1 0 1 1 0 0 0
# 1 # 1 0 1 1 0 0 0
# # 0 0 0 0 0 0 0 0
S 0 0 0 0 0 0 0 0 0
0 1 0 0 0 1 1 0 0 0
0 0 0 0 0 1 1 0 0 0
Path Cost: 21
Now, the problem, as evident from the output, is that it is storing every index that gets visited (because heuristic values have very low difference for all the indexes, that is why, almost every node is being visited.. However, ultimately, A* search finds the best path, and that can be seen from "Path Cost: 21" which is the actual cost of the optimal path)
I believe that my algorithm is correct, considering the path cost but what I want now is store also the path of the optimal path.
For this, I want to keep a record of all the indexes (row and column) that are visited by one path.
For example, my path starts from
Row 11, Col 0
Then "optimal paths" goes to,
Row 10, Col 1 -> When I push these nodes into queue, I want to store "11, 0" as well. So that, I can know what path this node has taken previously to reach this state.
Following the same, then it will go to,
Row 9, Col 2 -> So, this node should also store both "11, 0" and "10, 1" in it, hence keeping record of the path it has taken so far.
And this goes on, until the "goal" node.
But I can't seem to find a way to implement this thing, something that keeps track of all the path every node has taken. In this way, I can easily avoid the problem I am facing (I will simply print the path the "goal node" took to reach that point, ignoring all the other nodes which were visited unnecessarily)
Can anyone help me in trying to find a logic for this?
Also, just to be clear, my class node and CompareCost have this implementation,
class node
{
public:
int i, j, heuristic, cost;
node() { i = j = heuristic = cost = 0; }
node(int heuristic, int i, int j, int cost) :i(i), j(j), heuristic(heuristic), cost(cost) {}
};
struct CompareCost {
bool operator()(node const& p1, node const& p2)
{
return p1.heuristic > p2.heuristic;
}
};
I am guessing that I need to store something extra in my "class node" but I can't seem to figure out the exact thing.

Answer 1

Construct your node like a linked list:
class node
{
public:
int i, j, cost;
node* next;
}
Add a method to the class to display the full path:
void ShowPath()
{
Node* temp = this;
do
{
if (temp != NULL)
{
std::cout << "(" << temp->i << ", " << temp->j << ")";
temp = temp->next;
}
} while (temp != NULL);
}
Last, modify A_search() so that it returns the new node definition. You can then call ShowPath() on the return value.