# A Star (A*) Path Finding C++

### Andris Jansons ・4 min read

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 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 valid
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;
}
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;
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 / Y_STEP)) {
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;
//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 present
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 + newY]);
}
}
}
}
}
}
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 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 / X_STEP)> map, Node dest) {
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.

```
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.

Thank you for sharing your dive into the A* algorithm :)

I always like to think about and discuss algorithms and unfortunately don't find enough time during my regular work.

I'd just like to add one note to what you said: A* will always give the best route (given its constraints) and is like a Dijkstra where the cost function reruns the same value for each estimate. It's important though that the estimated cost has to be equal to or larger than the actual path. This usually is guaranteed by the Manhatten distance, but can bite you in a game where you can teleport (i.e. move to steps in the opposite direction and then skip 10 steps, this will kill the heuristic and can lead to pseudo-optimal paths).

But apart from that: thanks for sharing this (maybe I am repeating myself here ;) )

Nice comment but with one minor issue. A-star's(tree version) optimality is guaranteed when using an admissible heuristic, which means the h value never overestimates the actual cost for any node. So, it is important that the estimate has to be equal to or smaller than the actual cost. With no heuristic function given, that's equivalent to h(x)=0 for all x, it will downgrade to Dijkstra.

So the A* algorithm is asking for "some" information about the distance to the goal, but not too aggressive information.

But the author's code is actually a graph version of A*(because there is a closed list maintained), so admissibility is not enough in some situation to guarantee optimality. We need to ensure another property of the heuristic: consistency, which means h(x) <= cost(x,y) + h(y) should be true for all node x.

That is an interesting situation I hadn't thought about before.

A* usually is good enough, but it has these situations where it's just too Greedy, that it misses better options. That is exactly why there are multiple different algorithms out there to explore their trade-offs. This is why developers should work shoulder to shoulder with their level designers, to anticipate these situations and make the better choice.

Either way, a lovely comment Philip, hope to hear from you more!

Hey, what is "world.obstacles.count(id)"?? Where does world and obstacle get defined and what type is it?

Hi, "obstacles" is an std::set object. I'm using .count() to check if the corresponding tile is an obstacle or not. "world" is just a class where I keep it

how we can use this world, can you please out?

please what's FLT_MAX ??

FLT_MAX stands for maximum floating point value. Since we want to initialize every node with a cost of infinity, FLT_MAX is the closest we can get to infinity.

But it's undefined here in your code ,, where i should define it ?

and could u send me your email , because i have some question for a small university project

you can include cfloat lib

You can use a binary heap for the open set makes it way faster. Also you can check my A* implementation on Laser Maze project, in the folder named Pathfinder if you like :)

I'm surprised no one mentioned this but your algorithm is whack. you need to give the diagonal movements a higher movement cost.

if(newX == newY){

gNew = node.gCost + 1.414;

} else {

gNew = node.gCost + 1.0;

}