Created
August 2, 2019 08:25
-
-
Save qiaoxu123/948030bf5695e9cd719c6dcdfcdd7df5 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// A C++ Program to implement A* Search Algorithm | |
#include<bits/stdc++.h> | |
using namespace std; | |
#define ROW 9 | |
#define COL 10 | |
// Creating a shortcut for int, int pair type | |
typedef pair<int, int> Pair; | |
// Creating a shortcut for pair<int, pair<int, int>> type | |
typedef pair<double, pair<int, int>> pPair; | |
// A structure to hold the neccesary parameters | |
struct cell | |
{ | |
// Row and Column index of its parent | |
// Note that 0 <= i <= ROW-1 & 0 <= j <= COL-1 | |
int parent_i, parent_j; | |
// f = g + h | |
double f, g, h; | |
}; | |
// A Utility Function to check whether given cell (row, col) | |
// is a valid cell or not. | |
bool isValid(int row, int col) | |
{ | |
// Returns true if row number and column number | |
// is in range | |
return (row >= 0) && (row < ROW) && | |
(col >= 0) && (col < COL); | |
} | |
// A Utility Function to check whether the given cell is | |
// blocked or not | |
bool isUnBlocked(int grid[][COL], int row, int col) | |
{ | |
// Returns true if the cell is not blocked else false | |
if (grid[row][col] == 1) | |
return (true); | |
else | |
return (false); | |
} | |
// A Utility Function to check whether destination cell has | |
// been reached or not | |
bool isDestination(int row, int col, Pair dest) | |
{ | |
if (row == dest.first && col == dest.second) | |
return (true); | |
else | |
return (false); | |
} | |
// A Utility Function to calculate the 'h' heuristics. | |
double calculateHValue(int row, int col, Pair dest) | |
{ | |
// Return using the distance formula | |
return ((double)sqrt ((row-dest.first)*(row-dest.first) | |
+ (col-dest.second)*(col-dest.second))); | |
} | |
// A Utility Function to trace the path from the source | |
// to destination | |
void tracePath(cell cellDetails[][COL], Pair dest) | |
{ | |
printf ("\nThe Path is "); | |
int row = dest.first; | |
int col = dest.second; | |
stack<Pair> Path; | |
while (!(cellDetails[row][col].parent_i == row | |
&& cellDetails[row][col].parent_j == col )) | |
{ | |
Path.push (make_pair (row, col)); | |
int temp_row = cellDetails[row][col].parent_i; | |
int temp_col = cellDetails[row][col].parent_j; | |
row = temp_row; | |
col = temp_col; | |
} | |
Path.push (make_pair (row, col)); | |
while (!Path.empty()) | |
{ | |
pair<int,int> p = Path.top(); | |
Path.pop(); | |
printf("-> (%d,%d) ",p.first,p.second); | |
} | |
return; | |
} | |
// A Function to find the shortest path between | |
// a given source cell to a destination cell according | |
// to A* Search Algorithm | |
void aStarSearch(int grid[][COL], Pair src, Pair dest) | |
{ | |
// If the source is out of range | |
if (isValid (src.first, src.second) == false) | |
{ | |
printf ("Source is invalid\n"); | |
return; | |
} | |
// If the destination is out of range | |
if (isValid (dest.first, dest.second) == false) | |
{ | |
printf ("Destination is invalid\n"); | |
return; | |
} | |
// Either the source or the destination is blocked | |
if (isUnBlocked(grid, src.first, src.second) == false || | |
isUnBlocked(grid, dest.first, dest.second) == false) | |
{ | |
printf ("Source or the destination is blocked\n"); | |
return; | |
} | |
// If the destination cell is the same as source cell | |
if (isDestination(src.first, src.second, dest) == true) | |
{ | |
printf ("We are already at the destination\n"); | |
return; | |
} | |
// Create a closed list and initialise it to false which means | |
// that no cell has been included yet | |
// This closed list is implemented as a boolean 2D array | |
bool closedList[ROW][COL]; | |
memset(closedList, false, sizeof (closedList)); | |
// Declare a 2D array of structure to hold the details | |
//of that cell | |
cell cellDetails[ROW][COL]; | |
int i, j; | |
for (i=0; i<ROW; i++) | |
{ | |
for (j=0; j<COL; j++) | |
{ | |
cellDetails[i][j].f = FLT_MAX; | |
cellDetails[i][j].g = FLT_MAX; | |
cellDetails[i][j].h = FLT_MAX; | |
cellDetails[i][j].parent_i = -1; | |
cellDetails[i][j].parent_j = -1; | |
} | |
} | |
// Initialising the parameters of the starting node | |
i = src.first, j = src.second; | |
cellDetails[i][j].f = 0.0; | |
cellDetails[i][j].g = 0.0; | |
cellDetails[i][j].h = 0.0; | |
cellDetails[i][j].parent_i = i; | |
cellDetails[i][j].parent_j = j; | |
/* | |
Create an open list having information as- | |
<f, <i, j>> | |
where f = g + h, | |
and i, j are the row and column index of that cell | |
Note that 0 <= i <= ROW-1 & 0 <= j <= COL-1 | |
This open list is implenented as a set of pair of pair.*/ | |
set<pPair> openList; | |
// Put the starting cell on the open list and set its | |
// 'f' as 0 | |
openList.insert(make_pair (0.0, make_pair (i, j))); | |
// We set this boolean value as false as initially | |
// the destination is not reached. | |
bool foundDest = false; | |
while (!openList.empty()) | |
{ | |
pPair p = *openList.begin(); | |
// Remove this vertex from the open list | |
openList.erase(openList.begin()); | |
// Add this vertex to the closed list | |
i = p.second.first; | |
j = p.second.second; | |
closedList[i][j] = true; | |
/* | |
Generating all the 8 successor of this cell | |
N.W N N.E | |
\ | / | |
\ | / | |
W----Cell----E | |
/ | \ | |
/ | \ | |
S.W S S.E | |
Cell-->Popped Cell (i, j) | |
N --> North (i-1, j) | |
S --> South (i+1, j) | |
E --> East (i, j+1) | |
W --> West (i, j-1) | |
N.E--> North-East (i-1, j+1) | |
N.W--> North-West (i-1, j-1) | |
S.E--> South-East (i+1, j+1) | |
S.W--> South-West (i+1, j-1)*/ | |
// To store the 'g', 'h' and 'f' of the 8 successors | |
double gNew, hNew, fNew; | |
//----------- 1st Successor (North) ------------ | |
// Only process this cell if this is a valid one | |
if (isValid(i-1, j) == true) | |
{ | |
// If the destination cell is the same as the | |
// current successor | |
if (isDestination(i-1, j, dest) == true) | |
{ | |
// Set the Parent of the destination cell | |
cellDetails[i-1][j].parent_i = i; | |
cellDetails[i-1][j].parent_j = j; | |
printf ("The destination cell is found\n"); | |
tracePath (cellDetails, dest); | |
foundDest = true; | |
return; | |
} | |
// If the successor is already on the closed | |
// list or if it is blocked, then ignore it. | |
// Else do the following | |
else if (closedList[i-1][j] == false && | |
isUnBlocked(grid, i-1, j) == true) | |
{ | |
gNew = cellDetails[i][j].g + 1.0; | |
hNew = calculateHValue (i-1, j, dest); | |
fNew = gNew + hNew; | |
// 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 cell | |
// OR | |
// If it is on the open list already, check | |
// to see if this path to that square is better, | |
// using 'f' cost as the measure. | |
if (cellDetails[i-1][j].f == FLT_MAX || | |
cellDetails[i-1][j].f > fNew) | |
{ | |
openList.insert( make_pair(fNew, | |
make_pair(i-1, j))); | |
// Update the details of this cell | |
cellDetails[i-1][j].f = fNew; | |
cellDetails[i-1][j].g = gNew; | |
cellDetails[i-1][j].h = hNew; | |
cellDetails[i-1][j].parent_i = i; | |
cellDetails[i-1][j].parent_j = j; | |
} | |
} | |
} | |
//----------- 2nd Successor (South) ------------ | |
// Only process this cell if this is a valid one | |
if (isValid(i+1, j) == true) | |
{ | |
// If the destination cell is the same as the | |
// current successor | |
if (isDestination(i+1, j, dest) == true) | |
{ | |
// Set the Parent of the destination cell | |
cellDetails[i+1][j].parent_i = i; | |
cellDetails[i+1][j].parent_j = j; | |
printf("The destination cell is found\n"); | |
tracePath(cellDetails, dest); | |
foundDest = true; | |
return; | |
} | |
// If the successor is already on the closed | |
// list or if it is blocked, then ignore it. | |
// Else do the following | |
else if (closedList[i+1][j] == false && | |
isUnBlocked(grid, i+1, j) == true) | |
{ | |
gNew = cellDetails[i][j].g + 1.0; | |
hNew = calculateHValue(i+1, j, dest); | |
fNew = gNew + hNew; | |
// 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 cell | |
// OR | |
// If it is on the open list already, check | |
// to see if this path to that square is better, | |
// using 'f' cost as the measure. | |
if (cellDetails[i+1][j].f == FLT_MAX || | |
cellDetails[i+1][j].f > fNew) | |
{ | |
openList.insert( make_pair (fNew, make_pair (i+1, j))); | |
// Update the details of this cell | |
cellDetails[i+1][j].f = fNew; | |
cellDetails[i+1][j].g = gNew; | |
cellDetails[i+1][j].h = hNew; | |
cellDetails[i+1][j].parent_i = i; | |
cellDetails[i+1][j].parent_j = j; | |
} | |
} | |
} | |
//----------- 3rd Successor (East) ------------ | |
// Only process this cell if this is a valid one | |
if (isValid (i, j+1) == true) | |
{ | |
// If the destination cell is the same as the | |
// current successor | |
if (isDestination(i, j+1, dest) == true) | |
{ | |
// Set the Parent of the destination cell | |
cellDetails[i][j+1].parent_i = i; | |
cellDetails[i][j+1].parent_j = j; | |
printf("The destination cell is found\n"); | |
tracePath(cellDetails, dest); | |
foundDest = true; | |
return; | |
} | |
// If the successor is already on the closed | |
// list or if it is blocked, then ignore it. | |
// Else do the following | |
else if (closedList[i][j+1] == false && | |
isUnBlocked (grid, i, j+1) == true) | |
{ | |
gNew = cellDetails[i][j].g + 1.0; | |
hNew = calculateHValue (i, j+1, dest); | |
fNew = gNew + hNew; | |
// 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 cell | |
// OR | |
// If it is on the open list already, check | |
// to see if this path to that square is better, | |
// using 'f' cost as the measure. | |
if (cellDetails[i][j+1].f == FLT_MAX || | |
cellDetails[i][j+1].f > fNew) | |
{ | |
openList.insert( make_pair(fNew, | |
make_pair (i, j+1))); | |
// Update the details of this cell | |
cellDetails[i][j+1].f = fNew; | |
cellDetails[i][j+1].g = gNew; | |
cellDetails[i][j+1].h = hNew; | |
cellDetails[i][j+1].parent_i = i; | |
cellDetails[i][j+1].parent_j = j; | |
} | |
} | |
} | |
//----------- 4th Successor (West) ------------ | |
// Only process this cell if this is a valid one | |
if (isValid(i, j-1) == true) | |
{ | |
// If the destination cell is the same as the | |
// current successor | |
if (isDestination(i, j-1, dest) == true) | |
{ | |
// Set the Parent of the destination cell | |
cellDetails[i][j-1].parent_i = i; | |
cellDetails[i][j-1].parent_j = j; | |
printf("The destination cell is found\n"); | |
tracePath(cellDetails, dest); | |
foundDest = true; | |
return; | |
} | |
// If the successor is already on the closed | |
// list or if it is blocked, then ignore it. | |
// Else do the following | |
else if (closedList[i][j-1] == false && | |
isUnBlocked(grid, i, j-1) == true) | |
{ | |
gNew = cellDetails[i][j].g + 1.0; | |
hNew = calculateHValue(i, j-1, dest); | |
fNew = gNew + hNew; | |
// 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 cell | |
// OR | |
// If it is on the open list already, check | |
// to see if this path to that square is better, | |
// using 'f' cost as the measure. | |
if (cellDetails[i][j-1].f == FLT_MAX || | |
cellDetails[i][j-1].f > fNew) | |
{ | |
openList.insert( make_pair (fNew, | |
make_pair (i, j-1))); | |
// Update the details of this cell | |
cellDetails[i][j-1].f = fNew; | |
cellDetails[i][j-1].g = gNew; | |
cellDetails[i][j-1].h = hNew; | |
cellDetails[i][j-1].parent_i = i; | |
cellDetails[i][j-1].parent_j = j; | |
} | |
} | |
} | |
//----------- 5th Successor (North-East) ------------ | |
// Only process this cell if this is a valid one | |
if (isValid(i-1, j+1) == true) | |
{ | |
// If the destination cell is the same as the | |
// current successor | |
if (isDestination(i-1, j+1, dest) == true) | |
{ | |
// Set the Parent of the destination cell | |
cellDetails[i-1][j+1].parent_i = i; | |
cellDetails[i-1][j+1].parent_j = j; | |
printf ("The destination cell is found\n"); | |
tracePath (cellDetails, dest); | |
foundDest = true; | |
return; | |
} | |
// If the successor is already on the closed | |
// list or if it is blocked, then ignore it. | |
// Else do the following | |
else if (closedList[i-1][j+1] == false && | |
isUnBlocked(grid, i-1, j+1) == true) | |
{ | |
gNew = cellDetails[i][j].g + 1.414; | |
hNew = calculateHValue(i-1, j+1, dest); | |
fNew = gNew + hNew; | |
// 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 cell | |
// OR | |
// If it is on the open list already, check | |
// to see if this path to that square is better, | |
// using 'f' cost as the measure. | |
if (cellDetails[i-1][j+1].f == FLT_MAX || | |
cellDetails[i-1][j+1].f > fNew) | |
{ | |
openList.insert( make_pair (fNew, | |
make_pair(i-1, j+1))); | |
// Update the details of this cell | |
cellDetails[i-1][j+1].f = fNew; | |
cellDetails[i-1][j+1].g = gNew; | |
cellDetails[i-1][j+1].h = hNew; | |
cellDetails[i-1][j+1].parent_i = i; | |
cellDetails[i-1][j+1].parent_j = j; | |
} | |
} | |
} | |
//----------- 6th Successor (North-West) ------------ | |
// Only process this cell if this is a valid one | |
if (isValid (i-1, j-1) == true) | |
{ | |
// If the destination cell is the same as the | |
// current successor | |
if (isDestination (i-1, j-1, dest) == true) | |
{ | |
// Set the Parent of the destination cell | |
cellDetails[i-1][j-1].parent_i = i; | |
cellDetails[i-1][j-1].parent_j = j; | |
printf ("The destination cell is found\n"); | |
tracePath (cellDetails, dest); | |
foundDest = true; | |
return; | |
} | |
// If the successor is already on the closed | |
// list or if it is blocked, then ignore it. | |
// Else do the following | |
else if (closedList[i-1][j-1] == false && | |
isUnBlocked(grid, i-1, j-1) == true) | |
{ | |
gNew = cellDetails[i][j].g + 1.414; | |
hNew = calculateHValue(i-1, j-1, dest); | |
fNew = gNew + hNew; | |
// 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 cell | |
// OR | |
// If it is on the open list already, check | |
// to see if this path to that square is better, | |
// using 'f' cost as the measure. | |
if (cellDetails[i-1][j-1].f == FLT_MAX || | |
cellDetails[i-1][j-1].f > fNew) | |
{ | |
openList.insert( make_pair (fNew, make_pair (i-1, j-1))); | |
// Update the details of this cell | |
cellDetails[i-1][j-1].f = fNew; | |
cellDetails[i-1][j-1].g = gNew; | |
cellDetails[i-1][j-1].h = hNew; | |
cellDetails[i-1][j-1].parent_i = i; | |
cellDetails[i-1][j-1].parent_j = j; | |
} | |
} | |
} | |
//----------- 7th Successor (South-East) ------------ | |
// Only process this cell if this is a valid one | |
if (isValid(i+1, j+1) == true) | |
{ | |
// If the destination cell is the same as the | |
// current successor | |
if (isDestination(i+1, j+1, dest) == true) | |
{ | |
// Set the Parent of the destination cell | |
cellDetails[i+1][j+1].parent_i = i; | |
cellDetails[i+1][j+1].parent_j = j; | |
printf ("The destination cell is found\n"); | |
tracePath (cellDetails, dest); | |
foundDest = true; | |
return; | |
} | |
// If the successor is already on the closed | |
// list or if it is blocked, then ignore it. | |
// Else do the following | |
else if (closedList[i+1][j+1] == false && | |
isUnBlocked(grid, i+1, j+1) == true) | |
{ | |
gNew = cellDetails[i][j].g + 1.414; | |
hNew = calculateHValue(i+1, j+1, dest); | |
fNew = gNew + hNew; | |
// 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 cell | |
// OR | |
// If it is on the open list already, check | |
// to see if this path to that square is better, | |
// using 'f' cost as the measure. | |
if (cellDetails[i+1][j+1].f == FLT_MAX || | |
cellDetails[i+1][j+1].f > fNew) | |
{ | |
openList.insert(make_pair(fNew, | |
make_pair (i+1, j+1))); | |
// Update the details of this cell | |
cellDetails[i+1][j+1].f = fNew; | |
cellDetails[i+1][j+1].g = gNew; | |
cellDetails[i+1][j+1].h = hNew; | |
cellDetails[i+1][j+1].parent_i = i; | |
cellDetails[i+1][j+1].parent_j = j; | |
} | |
} | |
} | |
//----------- 8th Successor (South-West) ------------ | |
// Only process this cell if this is a valid one | |
if (isValid (i+1, j-1) == true) | |
{ | |
// If the destination cell is the same as the | |
// current successor | |
if (isDestination(i+1, j-1, dest) == true) | |
{ | |
// Set the Parent of the destination cell | |
cellDetails[i+1][j-1].parent_i = i; | |
cellDetails[i+1][j-1].parent_j = j; | |
printf("The destination cell is found\n"); | |
tracePath(cellDetails, dest); | |
foundDest = true; | |
return; | |
} | |
// If the successor is already on the closed | |
// list or if it is blocked, then ignore it. | |
// Else do the following | |
else if (closedList[i+1][j-1] == false && | |
isUnBlocked(grid, i+1, j-1) == true) | |
{ | |
gNew = cellDetails[i][j].g + 1.414; | |
hNew = calculateHValue(i+1, j-1, dest); | |
fNew = gNew + hNew; | |
// 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 cell | |
// OR | |
// If it is on the open list already, check | |
// to see if this path to that square is better, | |
// using 'f' cost as the measure. | |
if (cellDetails[i+1][j-1].f == FLT_MAX || | |
cellDetails[i+1][j-1].f > fNew) | |
{ | |
openList.insert(make_pair(fNew, | |
make_pair(i+1, j-1))); | |
// Update the details of this cell | |
cellDetails[i+1][j-1].f = fNew; | |
cellDetails[i+1][j-1].g = gNew; | |
cellDetails[i+1][j-1].h = hNew; | |
cellDetails[i+1][j-1].parent_i = i; | |
cellDetails[i+1][j-1].parent_j = j; | |
} | |
} | |
} | |
} | |
// When the destination cell is not found and the open | |
// list is empty, then we conclude that we failed to | |
// reach the destiantion cell. This may happen when the | |
// there is no way to destination cell (due to blockages) | |
if (foundDest == false) | |
printf("Failed to find the Destination Cell\n"); | |
return; | |
} | |
// Driver program to test above function | |
int main() | |
{ | |
/* Description of the Grid- | |
1--> The cell is not blocked | |
0--> The cell is blocked */ | |
int grid[ROW][COL] = | |
{ | |
{ 1, 0, 1, 1, 1, 1, 0, 1, 1, 1 }, | |
{ 1, 1, 1, 0, 1, 1, 1, 0, 1, 1 }, | |
{ 1, 1, 1, 0, 1, 1, 0, 1, 0, 1 }, | |
{ 0, 0, 1, 0, 1, 0, 0, 0, 0, 1 }, | |
{ 1, 1, 1, 0, 1, 1, 1, 0, 1, 0 }, | |
{ 1, 0, 1, 1, 1, 1, 0, 1, 0, 0 }, | |
{ 1, 0, 0, 0, 0, 1, 0, 0, 0, 1 }, | |
{ 1, 0, 1, 1, 1, 1, 0, 1, 1, 1 }, | |
{ 1, 1, 1, 0, 0, 0, 1, 0, 0, 1 } | |
}; | |
// Source is the left-most bottom-most corner | |
Pair src = make_pair(8, 0); | |
// Destination is the left-most top-most corner | |
Pair dest = make_pair(0, 0); | |
aStarSearch(grid, src, dest); | |
return(0); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment