C++ implementation of the A* pathfinding algorithm

A lot of the code I show here is simply setting the environment and doesn't really affect the algorithm itself, so please don't get scared away by the amount of code. The algorithm itself is showed at the bottom. If you don't want to go through all of the code, feel free to mainly look at the A* algorithm itself and only reference to the rest if you aren't sure what a method does based on its name (I'm here to learn, so tell me if a method has a bad name).

I start by creating an instance of a Map class, setting the size of the map in the constructor (number of rows and columns). Then, I create a Tile object for each place in the map. Then, I set a vector of neighbours to each tile:

Map::Map(int rows, int columns) :
_rows(rows),
_columns(columns)
{
//Create tiles
int tilesAmount = rows * columns;
Tile** tiles = new Tile*[tilesAmount];

for (int id = 0; id < tilesAmount; id++) {
tiles[id] = new Tile(id, Tile::TerrainAvailability::ALL, this);
}

this->_tiles = tiles;

//Set neighbours to each tile
for (int id = 0; id < tilesAmount; id++) {
this->setNeighbours(id, tiles);
}
}

void Map::setNeighbours(int id, Tile** tiles) {
std::vector<Tile*> neighbours;
int row = idToRow(id);
int column = idToColumn(id);

//Loop through all tiles around the current tile
for (int r = -1; r <= 1; r++) {
for (int c = -1; c <= 1; c++) {

//Skip iterating through itself
if (r == 0 && c == 0) {
continue;
}

//If the tile is at the edge, skip tiles that would be out of bounds
if ((row == 0 && r == -1) ||
(row == (this->_rows - 1) && r == 1) ||
(column == 0 && c == -1) ||
(column == (this->_columns - 1) && c == 1)) {
continue;
}

//Add this tile to the neighbours vector
neighbours.push_back(tiles[positionToId(row + r, column + c)]);
}
}

//Add the vector to the current tile
tiles[id]->setNeighbours(neighbours);
}

Tile::TerrainAvailability is an enum that says whether the tile is an obstacle or not. When I create the tiles, I set them all to available (not obstacles). I have another method that can set them to obstacles, but that's not important here.

I want to include diagonal movement, therefore each tile can have up to 8 neighbours.

I store the 2D map in a 1D array where the id of each tile corresponds with the array index. However, it's more convenient to work with tile position as a row and a column, so I have 3 functions in the Map class to convert id to rows and columns, and vice versa:

int Map::idToRow(int id) {
return (id / this->_columns);
}

int Map::idToColumn(int id) {
return (id % this->_columns);
}

int Map::positionToId(int row, int column) {
return (column + (row * this->_columns));
}

This is how I construct the Tile objects:

Tile::Tile(int id, TerrainAvailability type, Map* mapP) :
_id(id),
_type(type),
_mapP(mapP)
{
this->_wasVisited = false;
this->_G = INT_MAX;             //Not infinity, but close enough
this->_H = INT_MAX;             //Not infinity, but close enough
Tile* _parentP = nullptr;
}

The Tile class has a bunch of getters and setters, and 2 methods important for the algorithm. Method calculateH calculates the distance between the tile calling the method and the tile provided in the argument (the desired destination of the path):

int Tile::calculateH(Tile* endTile) {
//Get difference in rows and columns between the tiles
int rowDiff = abs(_mapP->idToRow(this->_id) - _mapP->idToRow(endTile->getId()));
int columnDiff = abs(_mapP->idToColumn(this->_id) - _mapP->idToColumn(endTile->getId()));

//Determine which one is smaller
int smaller = std::min(rowDiff, columnDiff);

//Calculate diagonal distance by taking all units common to both differences (rows and columns)
//and translating them into the distance, where 1 diagonal has a distance of 14
//(assuming the length of 1 tile is 10, then 14 is an approximation of sqrt(2 * 10))
int distance = smaller * 14;

//By subtracting the smaller value from both differences, one value will become 0 and
//the other one will respond to the amount of straight paths.
distance += (rowDiff + columnDiff - 2 * smaller) * 10;

return distance;
}

Method isNeighbourDiagonal returns true if the tile calling the method and the neighbour tile provided in the argument are diagonal, otherwise returns false.

bool Tile::isNeighbourDiagonal(Tile* neighbour) {
//Get difference in rows and columns between the tiles
int rowDiff = abs(_mapP->idToRow(this->_id) - _mapP->idToRow(neighbour->getId()));
int columnDiff = abs(_mapP->idToColumn(this->_id) - _mapP->idToColumn(neighbour->getId()));

//If the difference in rows and columns is 1, return true. Otherwise return false.
return ((rowDiff == 1 && columnDiff == 1) ? true : false);
}

Finally, this is the A* implementation:

void Pathfinder::A_Star(Tile* start, Tile* end) {

//Create a vector of open tiles
/*
Open tiles are the tiles which are candidates to be visited.
Once they are visited, they are removed from the vector.
*/
std::vector<Tile*> openTiles;

//Set up the start tile
start->setG(0);
start->setH(start->calculateH(end));

//Add the start tile to openTiles
openTiles.push_back(start);

//Begin the loop
bool pathFound = false;
Tile* currentTile = nullptr;

while (!pathFound) {

//Choose the most suitable tile to visit
if (openTiles.size() != 0) {
//Sort the vector, tile with the lowest cost is at the end
std::sort(openTiles.begin(), openTiles.end(), compareTilesF);
currentTile = openTiles[openTiles.size() - 1];
}
else {
//Out of open tiles, path not found
break;
}

//Remove the pointer to the current tile from the openTiles vector, as I'm about to visit the tile
openTiles.pop_back();

//Set the current tile as visited
currentTile->setWasVisited(true);

//Analyze neighbours
std::vector<Tile*>* neighbours = currentTile->getNeighboursP();
for (int i = 0; i < neighbours->size(); i++) {

//If the neighbour tile was already checked, skip it.
//If the unit cannot move through the neighbour tile, also skip it.

if (!(*neighbours)[i]->getWasVisited() && (*neighbours)[i]->getType() == Tile::TerrainAvailability::ALL) {

//This can happen multiple times per tile

//Set G value
/*
I first need to check if the neighbour tile is diagonal or not.
If it's diagonal, I would add 14 to the current G, otherwise 10.
I only change the G value if the new value would be smaller than
the current one.
*/

int G_increase = currentTile->isNeighbourDiagonal((*neighbours)[i]) ? 14 : 10;

if (currentTile->getG() + G_increase < (*neighbours)[i]->getG()) {
(*neighbours)[i]->setG(currentTile->getG() + G_increase);

//Set the parent
//Only if the new G is smaller than the previous G
(*neighbours)[i]->setParentP(currentTile);

}

//This can only happen once per tile
if ((*neighbours)[i]->getH() == INT_MAX) {
//Set H value
int H = (*neighbours)[i]->calculateH(this->_mapP->getTilesP()[end->getId()]);
(*neighbours)[i]->setH(H);

if (H == 0) {
pathFound = true;
}

//Add this tile to the vector of open tiles
openTiles.push_back((*neighbours)[i]);
}
}
}
}
}

bool Pathfinder::compareTilesF(Tile* a, Tile* b) {
//Sorts pointers to Tile objects based on their F values in descending order
return (a->getF() > b->getF());
}

That's all the code. I tried running it on an 80x100 map with some obstacles, and the algorithm seemed to take a bit too long (0.5 seconds), as I want to use this in a game with lots of units. I don't want the game to freeze for half a second every time the player commands a unit to move. Of course, for straight forward paths the time is almost instant, but if I select an unreachable end (which wouldn't be uncommon), then 0.5 seconds is the time is takes.

I'd like to know if the algorithm can be improved in terms of efficiency. I will be thankful even for pieces of advice about setting the environment, not just the algorithm itself, but the stuff setting the environment only happens once, therefore isn't as important as the A* loop which happens many times (correct me if I'm wrong).

I noticed that if I don't include the sorting (ignoring the fact that the algorithm wouldn't work), it takes considerably less time - about 1/4 of the current time (about 0.14 seconds). That makes me think whether it's possible to somehow make the sorting more efficient.

Map definition:

class Map {
public:

Map();          //Won't be used

/* Map
Initializes terrain with all tiles available to all units.
*/
Map(int rows, int columns);

~Map();

Changes values of _terrain to new values defined in this function.
*/

/* int idToRow
Returns the row based on the id
*/
int idToRow(int id);

/* int idToColumn
Returns the column based on the id
*/
int idToColumn(int id);

/* int positionToId
Returns the id based on the row and column
*/
int positionToId(int row, int column);

//Getters
int getRows();
int getColumns();
Tile** getTilesP();

private:
//VARIABLES
int _rows;
int _columns;

/* Tile** _tiles
Pointer to an array of pointers to instances of Tile object.
_tiles[i] points to a tile with id i.
Example: _tiles points to a tile with id 20.
*/
Tile** _tiles;

//METHODS
/* void setNeighbours(Tile* tile, Tile** tiles)
Sets neighbours of tiles[id] (id = arg1) as pointers to other tiles from array tiles (arg2)
*/
void setNeighbours(int id, Tile** tiles);

};

loadTestMap creates some obstacles on the map. I'm not posting the definition as it would be too long. The rest of the methods/variables should be self-explanatory.

Tile definition:

class Tile {
public:
enum TerrainAvailability {
ALL,
AIR,
NONE
};

Tile();         //Won't be used

/* Tile
Creates the object and initializes _id, _type, _mapP.
*/
Tile(int id, TerrainAvailability type, Map* mapP);

/* int calculateH
Calculates H (the distance between this tile and the end tile).
*/
int calculateH(Tile* endTile);

/* bool isNeighbourDiagonal
Returns true if the neighbour tile is touching this tile by a corner.
Returns false if the neighbour tile is touching this tile by an edge.
*/
bool isNeighbourDiagonal(Tile* neighbour);

/* void reset
Resets all member variables to their default state.
*/
void reset();

//Setters
void setType(TerrainAvailability type);
void setNeighbours(std::vector<Tile*> neighbours);
void setWasVisited(bool wasVisited);
void setG(int G);
void setH(int H);
void setParentP(Tile* parentP);

//Getters
int getId();
TerrainAvailability getType();
std::vector<Tile*>* getNeighboursP();
bool getWasVisited();
int getG();
int getH();
int getF();
Tile* getParentP();

private:
int _id;
TerrainAvailability _type;
Map* _mapP;                         //Pointer to the map object, allows the use of utility functions
//(switching between columns and rows and id)
std::vector<Tile*> _neighbours;     //Vector holding pointers to neighbour tiles
bool _wasVisited;
int _G;                             //Distance from start
int _H;                             //Minimal distance to end
//int _F doesn't have to be stored, as I can get it by summing up _G and _H
Tile* _parentP;

};

The only non-self-explanatory methods here are reset and getF. Reset is used after the algorithm finishes.

void Tile::reset() {
this->_wasVisited = false;
this->_G = INT_MAX;             //Not infinity, but close enough
this->_H = INT_MAX;             //Not infinity, but close enough
Tile* _parentP = nullptr;
}

int Tile::getF() {
return (this->_G + this->_H);
}

Pathfinder definition:

public:
Pathfinder();        //Won't be used

/* Pathfinder
Creates the object and initializes _mapP
*/
Pathfinder(Map* mapP);

void A_Star(Tile* start, Tile* end);

private:
Map* _mapP;

//Methods
static bool compareTilesF(Tile* a, Tile* b);
};

I removed some code that doesn't contribute to the problem at all but if you want to just copy the code and play with it, clone it from my GitHub.

The GitHub version includes some SDL2 libraries and draws the map on the screen, so you'll need to get those libraries. Or you can comment out all the code that requires libraries, as the pathfinder doesn't need them (but it won't draw the map on the screen, obviously).

• Have you tried doing a search on this site? You made the same mistake that I see in nearly every A* implementation post on this site: you used the wrong data structures. – BlueRaja - Danny Pflughoeft Jul 9 '18 at 15:32
• @TobySpeight I posted the header files for the classes you mentioned. I didn't include all the stuff because some of it didn't have anything to do with the problem. Is it okay like this, or should I also include the .cpp files? It feels redundant when the important methods are already showed and the rest are usually getters/setters. – Peacetoletov Jul 9 '18 at 15:56
• @BlueRaja-DannyPflughoeft Thanks, I haven't actually searched here. I asked on SO and there they told me to go and ask here. I'll definitely do a search here now. – Peacetoletov Jul 9 '18 at 15:57

int _G;                             //Distance from start
int _H;                             //Minimal distance to end

Writing identifiers with a leading underscore is a bad idea, and is generally dissuaded as a style. Note that it is legal for member names if (and only if) the next character is not a capital letter or another underscore.

Don’t write this-> before member names when you access them. The member names are in the scope of the member function bodies.

Tile::Tile(int id, TerrainAvailability type, Map* mapP) :
_id(id),
_type(type),
_mapP(mapP)
{
this->_wasVisited = false;
this->_G = INT_MAX;             //Not infinity, but close enough
this->_H = INT_MAX;             //Not infinity, but close enough
Tile* _parentP = nullptr;
}

more generally, use initialization for members in the constructor, not assignment in the body of the constructor. You are using a mixture of both, and I don’t see what is different betweeen the two sets of members you are treating differently.

Those can probably go inline in the class definition as default initializers, so you you don’t need to do wasVisited, G, and H here at all.

The ones that are initializers are using ye olde syntax. Use uniform initialization syntax for initializing with values.

INT_MAX is a C library macro. Prefer using the C++ library version, and being type-safe. If you change G from an int to something else in the class, you’ll have to track down all such places that assume the type is int.

I don’t see any use of const, and it seems normal that get⋯ functions should be const. So you are probably leaving it off where it should be used.

I noticed that if I don't include the sorting (ignoring the fact that the algorithm wouldn't work), it takes considerably less time - about 1/4 of the current time (about 0.14 seconds). That makes me think whether it's possible to somehow make the sorting more efficient.

That is well noted, and it is the performance problem of your code, you shouldn't sort but use a priority queue (of some sort). Like Dijkstra's algorithm A* is limited by the effectiveness of finding the next candidate.

But opposing Dijkstra A* doesn't have to evaluate all nodes (in all cases) so a full sort is overkill, one of the O(log n) priority heaps will do the work for you more efficient.