A Star (A*) Path Finding C++
Tags: astar, a, pathfinding
Published: 2018-03-22 00:36:50

---
title: A Star (A*) Path Finding C++
published: true
description: A star tutorial
tags: astar, a*, path finding
---
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.

![A Star](https://thepracticaldev.s3.amazonaws.com/i/5t0o6cjblypt3g990xl9.PNG)

###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.
```cpp
#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.
```cpp
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.
```cpp
	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.
```cpp
	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.
```cpp
	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
```cpp
	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.
```cpp
	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.
```cpp
			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.](https://dev.to/jansonsa/pokmon-or-not-pokmon-5dn6)
And check out just the code [here.](https://github.coventry.ac.uk/jansonsa/D1/blob/master/pokemonGUI/pokemonGUI/Cordinate.h)