A Star (A*) Path Finding C++ - DEV Community !

" 12/25/18, 1(54 PM

A Star (A*) Path Finding C++

Andris Jansons

Mar 22

A* (A star) path finding algorithm is an extension of the famous Dijkstra's

path finding algorithm, which is more efficient, but occasionally doesn't
actually find the best route, but just a good enough route.

The A* algorithm implementation

Let's take in account that our map is 1000x500 pixels divided in 50x25
blocks. I am using Magic Numbers to make it as widely usable as possible.

#define X_MAX 1000

#define X_STEP 20
#define Y_MAX 500
#define Y_STEP 20

Then I make a new structure to represent each Node (square) on the map.
Since later on I am putting this structure in to a set, I first need to overload

https://dev.to/jansonsa/a-star-a-path-finding-c-4a4h Page 1 of 7
A Star (A*) Path Finding C++ - DEV Community ! " 12/25/18, 1(54 PM

the operator < to tell the set which value we compare, since it is an ordered
sequence.

struct Node
{
int y;
int x;
int parentX;
int parentY;
float gCost;
float hCost;
float fCost;
};

inline bool operator < (const Node& lhs, const Node& rhs)
{//We need to overload "<" to put our struct into a set
return lhs.fCost < rhs.fCost;
}

Then we make Our Class and inside we use these functions:

First we need a Boolean to check whether a node(square) is valid - not an

obstacle and not out of bounds.

static bool isValid(int x, int y) { //If our Node is an obstacle it is not val
int id = x + y * (X_MAX / X_STEP);
if (world.obstacles.count(id) == 0) {
if (x < 0 || y < 0 || x >= (X_MAX / X_STEP) || y >= (Y_MAX / Y_STEP
return false;
}
return true;
}
return false;
}

We also need a Boolean to check if a node is our destination.

static bool isDestination(int x, int y, Node dest) {

if (x == dest.x && y == dest.y) {
return true;

https://dev.to/jansonsa/a-star-a-path-finding-c-4a4h Page 2 of 7
A Star (A*) Path Finding C++ - DEV Community ! " 12/25/18, 1(54 PM

}
return false;
}

This is where it starts to differ from Dijkstra's. We use H cost and F cost so
we first check nodes that move towards our destination and only then the
ones that move away from it.

static double calculateH(int x, int y, Node dest) {

double H = (sqrt((x - dest.x)*(x - dest.x)
+ (y - dest.y)*(y - dest.y)));
return H;
}

Now the algorithm itself

static vector<Node> aStar(Node player, Node dest) {

vector<Node> empty;
if (isValid(dest.x, dest.y) == false) {
cout << "Destination is an obstacle" << endl;
return empty;
//Destination is invalid
}
if (isDestination(player.x, player.y, dest)) {
cout << "You are the destination" << endl;
return empty;
//You clicked on yourself
}
bool closedList[(X_MAX / X_STEP)][(Y_MAX / Y_STEP)];

//Initialize whole map

//Node allMap[50][25];
array<array < Node, (Y_MAX / Y_STEP)>, (X_MAX / X_STEP)> allMap;
for (int x = 0; x < (X_MAX / X_STEP); x++) {
for (int y = 0; y < (Y_MAX / Y_STEP); y++) {
allMap[x][y].fCost = FLT_MAX;
allMap[x][y].gCost = FLT_MAX;
allMap[x][y].hCost = FLT_MAX;
allMap[x][y].parentX = -1;
allMap[x][y].parentY = -1;
allMap[x][y].x = x;

https://dev.to/jansonsa/a-star-a-path-finding-c-4a4h Page 3 of 7
A Star (A*) Path Finding C++ - DEV Community ! " 12/25/18, 1(54 PM

allMap[x][y].y = y;

closedList[x][y] = false;
}
}

//Initialize our starting list

int x = player.x;
int y = player.y;
allMap[x][y].fCost = 0.0;
allMap[x][y].gCost = 0.0;
allMap[x][y].hCost = 0.0;
allMap[x][y].parentX = x;
allMap[x][y].parentY = y;

vector<Node> openList;
openList.emplace_back(allMap[x][y]);
bool destinationFound = false;

while (!openList.empty()&&openList.size()<(X_MAX / X_STEP)*(Y_MAX /

Node node;
do {
//This do-while loop could be replaced with extracting the first
//element from a set, but you'd have to make the openList a set.
//To be completely honest, I don't remember the reason why I do
//it with a vector, but for now it's still an option, although
//not as good as a set performance wise.
float temp = FLT_MAX;
vector<Node>::iterator itNode;
for (vector<Node>::iterator it = openList.begin();
it != openList.end(); it = next(it)) {
Node n = *it;
if (n.fCost < temp) {
temp = n.fCost;
itNode = it;
}
}
node = *itNode;
openList.erase(itNode);
} while (isValid(node.x, node.y) == false);

x = node.x;
y = node.y;
closedList[x][y] = true;

https://dev.to/jansonsa/a-star-a-path-finding-c-4a4h Page 4 of 7
A Star (A*) Path Finding C++ - DEV Community ! " 12/25/18, 1(54 PM

//For each neighbour starting from North-West to South-East

for (int newX = -1; newX <= 1; newX++) {
for (int newY = -1; newY <= 1; newY++) {
double gNew, hNew, fNew;
if (isValid(x + newX, y + newY)) {
if (isDestination(x + newX, y + newY, dest))
{
//Destination found - make path
allMap[x + newX][y + newY].parentX = x;
allMap[x + newX][y + newY].parentY = y;
destinationFound = true;
return makePath(allMap, dest);
}
else if (closedList[x + newX][y + newY] == false)
{
gNew = node.gCost + 1.0;
hNew = calculateH(x + newX, y + newY, dest);
fNew = gNew + hNew;
// Check if this path is better than the one already p
if (allMap[x + newX][y + newY].fCost == FLT_MAX
allMap[x + newX][y + newY].fCost > fNew)
{
// Update the details of this neighbour node
allMap[x + newX][y + newY].fCost = fNew;
allMap[x + newX][y + newY].gCost = gNew;
allMap[x + newX][y + newY].hCost = hNew;
allMap[x + newX][y + newY].parentX = x;
allMap[x + newX][y + newY].parentY = y;
openList.emplace_back(allMap[x + newX][y +
}
}
}
}
}
}
if (destinationFound == false) {
cout << "Destination not found" << endl;
return empty;
}
}

You might notice that we're still missing one function makePath. That is
where we follow each parent starting from our destination node till we

https://dev.to/jansonsa/a-star-a-path-finding-c-4a4h Page 5 of 7
A Star (A*) Path Finding C++ - DEV Community ! " 12/25/18, 1(54 PM

reach our starting point while putting them inside a vector, we're later
going to pass to aStar and from there to anyplace else we might find it
useful.

static vector<Node> makePath(array<array<Node, (Y_MAX / Y_STEP)>, (X_MAX

try {
cout << "Found a path" << endl;
int x = dest.x;
int y = dest.y;
stack<Node> path;
vector<Node> usablePath;

while (!(map[x][y].parentX == x && map[x][y].parentY == y)

&& map[x][y].x != -1 && map[x][y].y != -1)
{
path.push(map[x][y]);
int tempX = map[x][y].parentX;
int tempY = map[x][y].parentY;
x = tempX;
y = tempY;

}
path.push(map[x][y]);

while (!path.empty()) {
Node top = path.top();
path.pop();
usablePath.emplace_back(top);
}
return usablePath;
}
catch(const exception& e){
cout << e.what() << endl;
}
}

To actually use the algorithm:

First we create a node for starting position as well as one for the
destination and then run the algorithm.

https://dev.to/jansonsa/a-star-a-path-finding-c-4a4h Page 6 of 7
A Star (A*) Path Finding C++ - DEV Community ! " 12/25/18, 1(54 PM

Node player;
player.x = X / X_STEP;
player.y = Y / Y_STEP;

Node destination;
destination.x = e->X / X_STEP;
destination.y = e->Y / Y_STEP;

for (Node node : YourClass::aStar(player, destination)) {

//Your code here

Check out my pokemon game where I use this algorithm here.

And check out just the code here.

https://dev.to/jansonsa/a-star-a-path-finding-c-4a4h Page 7 of 7