# A* Algorithm implementation

This is an implementation of A* algorithm, that I will use in my game.

#pragma once

#include <SFML\Graphics.hpp>
#include <algorithm>
#include <iostream>

#include <vector>
#include <list>
#include <set>

using namespace std;

class Field;

//Comparer using by set to allow priority
struct FieldComparer
{
bool operator()(const Field *, const Field *) const;
};

using FieldSet = set<Field*, FieldComparer>;
using FieldContainer = vector<Field*>;

//Contains info about field, buildings builded on it and type of terrain
class Field
{
private:
sf::Vector2i mapPosition{ 0, 0 };

unsigned hCost{ 0 }; //to goal
unsigned gCost{ 0 }; //to start

Field *  parent;

bool     isWalkable { true };
bool     isPathPart { false };
public:
void SetParent(Field&);
Field * GetParent() const;

unsigned GetFCost() const; //sum of hCost and gCost
unsigned GetHCost() const;
unsigned GetGCost() const;

bool     IsWalkable() const;
bool     IsPathPart() const;

void SetHCost(unsigned);
void SetGCost(unsigned);

void SetWalkable(bool);
void SetAsPartOfPath(bool);

sf::Vector2i GetMapPosition() const;

//compares positions
bool operator == (const Field& other);

Field(sf::Vector2i mapPosition, bool isWalkable)
: mapPosition(mapPosition), isWalkable(isWalkable) {}
};

//Contains fields and describes them
class Map
{
private:
sf::Vector2u mapSize;
Field *** fields; //two dimensional array of fields gives the fastest access
public:

sf::Vector2u GetMapSize() const;
Field *** GetFields();

Map(sf::Vector2u);
Map() {}
};

//Searching patch after giving a specified map
class PathFinder
{
private:
//Calculate score between two fields
unsigned CalcScore(Field&, Field&) const;

//Get neighbours of field in specified map
FieldContainer GetNeighbours(Field&, Map&) const;
public:

//Find path that have the lowest cost, from a to b in map
FieldContainer FindPath(Map&, Field&, Field&);

//Reconstruct path using pointers to parent
FieldContainer ReconstructPath(Field*, Field*) const;
};


#include "Navigation.hpp"

#pragma region Field

void Field::SetParent(Field & parent) { this->parent = &parent; }
Field * Field::GetParent() const { return parent; }

unsigned Field::GetFCost() const { return hCost + gCost; }
unsigned Field::GetHCost() const { return hCost; }
unsigned Field::GetGCost() const { return gCost; }

bool Field::IsWalkable()   const { return isWalkable; }
bool Field::IsPathPart()   const { return isPathPart; }

void Field::SetHCost(unsigned value) { hCost = value; }
void Field::SetGCost(unsigned value) { gCost = value; }

void Field::SetWalkable(bool isWalkable)     { this->isWalkable = isWalkable; }
void Field::SetAsPartOfPath(bool isPathPart) { this->isPathPart = isPathPart; }

sf::Vector2i Field::GetMapPosition() const { return mapPosition; }
bool Field::operator == (const Field& other)
{
return this->mapPosition == other.GetMapPosition();
}

#pragma endregion Field

#pragma region Map

sf::Vector2u Map::GetMapSize() const { return mapSize; }
Field *** Map::GetFields() { return fields; }
Map::Map(sf::Vector2u mapSize) : mapSize(mapSize)
{
//initialize map
fields = new Field**[mapSize.x];

//initialize all fields
for (unsigned x = 0; x < mapSize.x; x++)
{
fields[x] = new Field*[mapSize.y];

for (unsigned y = 0; y < mapSize.y; y++)
{
fields[x][y] = new Field({static_cast<int>(x), static_cast<int>(y)},
{
(!(y == 3 && x >= 1) || (x == 5 && y < 4))
});
}
}
}

#pragma endregion Map

#pragma region PathFinder

bool FieldComparer::operator()(const Field * l, const Field * r) const
{
return l->GetFCost() <  r->GetFCost() ||   //another field has smaller fcost
l->GetFCost() == r->GetFCost() &&   //or fcost equals, and checked field is nearer to goal than current field
l->GetHCost() <  r->GetHCost();

}

unsigned PathFinder::CalcScore(Field & a, Field & b) const
{
sf::Vector2u dst
{
static_cast<unsigned>(abs(b.GetMapPosition().x - a.GetMapPosition().x)),
static_cast<unsigned>(abs(b.GetMapPosition().y - a.GetMapPosition().y))
};

return (dst.x > dst.y ? 14 * dst.y + 10 * (dst.x - dst.y) :
14 * dst.x + 10 * (dst.y - dst.x));
}

FieldContainer PathFinder::GetNeighbours(Field & f, Map & m) const
{
FieldContainer neighbours{};

//cout << "checking neighbours for field: { " << f.GetMapPosition().x << ", " << f.GetMapPosition().y << " }\n";
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
int xPos = f.GetMapPosition().x + x;
int yPos = f.GetMapPosition().y + y;

if (x == 0 && y == 0) //dont check the same field
continue;

//check that field is in the map
bool isInTheMap = (xPos >= 0 && yPos >= 0 && xPos < m.GetMapSize().x && yPos < m.GetMapSize().y);

if (isInTheMap)
{
neighbours.push_back(m.GetFields()[xPos][yPos]);
}
}
}

return neighbours;
}

FieldContainer PathFinder::FindPath(Map& map, Field& a, Field& b)
{
FieldSet open = {};   //not expanded fields
FieldSet closed = {}; //expanded fields

a.SetHCost(CalcScore(a, b)); //calculate h cost for start field, gcost equals 0
open.insert(&a);             //add start field to open vector

while (!open.empty()) //while we have unexpanded fields in open set
{
Field * current = *open.begin(); //set current field

//if current checked field is our goal field
if (*current == b)
{
return
ReconstructPath(&a, current); //return reversed path
}

closed.insert(current); //end of checking current field, add it to closed vector...
open.erase(open.find(current)); //set solution

//get neighbours of current field
for (auto f : GetNeighbours(*current, map))
{
//continue if f is unavailable
if (closed.find(f) != closed.end() || !f->IsWalkable())
{
continue;
}

//calculate tentative g cost, based on current cost and direction changed
unsigned tentativeGCost = current->GetGCost() + (current->GetMapPosition().x != f->GetMapPosition().x && current->GetMapPosition().y != f->GetMapPosition().y ? 14 : 10);

bool fieldIsNotInOpenSet = open.find(f) == open.end();
if (tentativeGCost < f->GetGCost() || fieldIsNotInOpenSet)
{
f->SetGCost(tentativeGCost);
f->SetHCost(CalcScore(*f, b));
f->SetParent(*current);

if (fieldIsNotInOpenSet)
{
open.insert(f);
}
}
}
}
return {}; //no path anaviable
}

FieldContainer PathFinder::ReconstructPath(Field * a, Field * current) const
{
FieldContainer totalPath { current };

while (!(current == a))
{
totalPath.push_back(current);
current->SetAsPartOfPath(true);
current = current->GetParent();
}

std::reverse(totalPath.begin(), totalPath.end()); //reverse the path
}

#pragma endregion PathFinder


• Do you see there any bad practises?
• What I can do better?
• Is this implemetation fast?
• Are containers, that I chose, the best for that algorithm? I mean: set for open and closed list, vectors for everything else?

The most important thing for me is of course speed. Now this code for map 1500x1500 needs ~27ms to find path (in release mode). I think that is quite good result, but I will use it probably for big maps, with different types of fields that haves different costs so I want to be sure.

EDIT

Now I am thinking that container representing closed fields don't need to be sorted, so I think it could be unordered_set.

# AFTER REFACTORING

This is modified code, where I took the @ratchet freak and @Ben Steffan advices. Now code for 150x150 map is about 10ms faster, that is 1/5 so it's pretty good result.

#pragma once

#include <SFML\Graphics.hpp>
#include <algorithm>
#include <iostream>

#include <vector>
#include <list>
#include <set>

class Field;

//Comparer using by set to allow priority
struct FieldComparer
{
bool operator()(const Field *, const Field *) const;
};

using FieldSet = std::set<Field*, FieldComparer>;
using FieldContainer = std::vector<Field*>;
using FieldsVector = std::vector<std::vector<Field>>;

//Contains info about field, buildings builded on it and type of terrain
class Field
{
private:
sf::Vector2i mapPosition{ 0, 0 };

unsigned hCost{ 0 }; //to goal
unsigned gCost{ 0 }; //to start

Field *  parent;

bool     isWalkable { true };
bool     isPathPart { false };
bool     isClosed   { false };
public:
void SetParent(Field&);
Field * GetParent() const;

unsigned GetFCost() const; //sum of hCost and gCost
unsigned GetHCost() const;
unsigned GetGCost() const;

bool     IsWalkable() const;
bool     IsPathPart() const;
bool     IsClosed() const;

void SetHCost(unsigned);
void SetGCost(unsigned);

void SetWalkable(bool);
void SetAsPartOfPath(bool);
void SetClosedStatus(bool);

sf::Vector2i GetMapPosition() const;

//compares positions
bool operator == (const Field& other);

Field(sf::Vector2i mapPosition, bool isWalkable)
: mapPosition(mapPosition), isWalkable(isWalkable) {}

Field() = default;
};

//Contains fields and describes them
class Map
{
private:
sf::Vector2u mapSize;
FieldsVector fields; //two dimensional array of fields gives the fastest access
public:

sf::Vector2u GetMapSize() const;
FieldsVector & GetFields();

Map(sf::Vector2u);
Map() {}

~Map();
};

//Searching patch after giving a specified map
class PathFinder
{
private:
//Calculate score between two fields
unsigned CalcScore(Field&, Field&) const;

//Get neighbours of field in specified map
FieldContainer GetNeighbours(Field&, Map&) const;
public:

//Find path that have the lowest cost, from a to b in map
FieldContainer FindPath(Map&, Field&, Field&);

//Reconstruct path using pointers to parent
FieldContainer ReconstructPath(Field*, Field*) const;
};


#include "Navigation.hpp"

#pragma region Field

void Field::SetParent(Field & parent) { this->parent = &parent; }
Field * Field::GetParent() const { return parent; }

unsigned Field::GetFCost() const { return hCost + gCost; }
unsigned Field::GetHCost() const { return hCost; }
unsigned Field::GetGCost() const { return gCost; }

bool Field::IsWalkable()   const { return isWalkable; }
bool Field::IsPathPart()   const { return isPathPart; }
bool Field::IsClosed()     const { return isClosed;   }

void Field::SetHCost(unsigned value) { hCost = value; }
void Field::SetGCost(unsigned value) { gCost = value; }

void Field::SetWalkable(bool isWalkable)     { this->isWalkable = isWalkable; }
void Field::SetAsPartOfPath(bool isPathPart) { this->isPathPart = isPathPart; }
void Field::SetClosedStatus(bool isClosed)   { this->isClosed = isClosed;     }

sf::Vector2i Field::GetMapPosition() const { return mapPosition; }
bool Field::operator == (const Field& other)
{
return this->mapPosition == other.GetMapPosition();
}

#pragma endregion Field

#pragma region Map

sf::Vector2u Map::GetMapSize() const { return mapSize; }
FieldsVector & Map::GetFields() { return fields; }
Map::Map(sf::Vector2u mapSize) : mapSize(mapSize)
{
//initialize map
fields = {};

//initialize all fields
for (unsigned x = 0; x < mapSize.x; x++)
{
fields.push_back({});

for (unsigned y = 0; y < mapSize.y; y++)
{
fields[x].push_back({ {static_cast<int>(x), static_cast<int>(y)},
{
(!(y == 3 && x >= 1) || (x == 5 && y < 4))
} });
}
}
}

Map::~Map() {}

#pragma endregion Map

#pragma region PathFinder

bool FieldComparer::operator()(const Field * l, const Field * r) const
{
return l->GetFCost() <  r->GetFCost() ||   //another field has smaller fcost
l->GetFCost() == r->GetFCost() &&   //or fcost equals, and checked field is nearer to goal than current field
l->GetHCost() <  r->GetHCost();

}

unsigned PathFinder::CalcScore(Field & a, Field & b) const
{
sf::Vector2u dst
{
static_cast<unsigned>(abs(b.GetMapPosition().x - a.GetMapPosition().x)),
static_cast<unsigned>(abs(b.GetMapPosition().y - a.GetMapPosition().y))
};

return (dst.x > dst.y ? 14 * dst.y + 10 * (dst.x - dst.y) :
14 * dst.x + 10 * (dst.y - dst.x));
}

FieldContainer PathFinder::GetNeighbours(Field & f, Map & m) const
{
FieldContainer neighbours{};

//cout << "checking neighbours for field: { " << f.GetMapPosition().x << ", " << f.GetMapPosition().y << " }\n";
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
int xPos = f.GetMapPosition().x + x;
int yPos = f.GetMapPosition().y + y;

if (x == 0 && y == 0) //dont check the same field
continue;

//check that field is in the map
bool isInTheMap = (xPos >= 0 && yPos >= 0 && xPos < m.GetMapSize().x && yPos < m.GetMapSize().y);

if (isInTheMap)
{
neighbours.push_back(&m.GetFields()[xPos][yPos]);
}
}
}

return neighbours;
}

FieldContainer PathFinder::FindPath(Map& map, Field& a, Field& b)
{
FieldSet open = {};   //not expanded fields

a.SetHCost(CalcScore(a, b)); //calculate h cost for start field, gcost equals 0
open.insert(&a);             //add start field to open vector

while (!open.empty()) //while we have unexpanded fields in open set
{
auto currIt = open.begin();
Field * current = *currIt; //set current field

//if current checked field is our goal field
if (*current == b)
{
return
ReconstructPath(&a, current); //return reversed path
}

current->SetClosedStatus(true); //end of checking current field, change closed status
open.erase(currIt); //set solution

//get neighbours of current field
for (auto f : GetNeighbours(*current, map))
{
//continue if f is unavailable
if (f->IsClosed() || !f->IsWalkable())
{
continue;
}

//calculate tentative g cost, based on current cost and direction changed
unsigned tentativeGCost = current->GetGCost() + (current->GetMapPosition().x != f->GetMapPosition().x && current->GetMapPosition().y != f->GetMapPosition().y ? 14 : 10);

bool fieldIsNotInOpenSet = open.find(f) == open.end();
if (tentativeGCost < f->GetGCost() || fieldIsNotInOpenSet)
{
if (!fieldIsNotInOpenSet)
{
open.erase(open.find(f));
}

f->SetGCost(tentativeGCost);
f->SetHCost(CalcScore(*f, b));
f->SetParent(*current);

open.insert(f);
}
}
}
return {}; //no path anaviable
}

FieldContainer PathFinder::ReconstructPath(Field * a, Field * current) const
{
FieldContainer totalPath { current };

while (!(current == a))
{
totalPath.push_back(current);
current->SetAsPartOfPath(true);
current = current->GetParent();
}

std::reverse(totalPath.begin(), totalPath.end()); //reverse the path
}

#pragma endregion PathFinder

• You're using an order set = binary tree for your open set. This gives better performance than a vector, but you'd still get better performance using the proper data structure, a priority queue. You can replace the closed-set completely with a node property called hasBeenVisited Jul 13 '17 at 18:17
• @BlueRaja-DannyPflughoeft Could you tell me why in your opinion priority queue will be better? I think that your idea with property in node is pretty good. :) Jul 13 '17 at 18:22
• @Michal: Because that's the data-structure used by the algorithm. You've emulated a priority queue using a binary search tree, but a proper PQ using an array-based heap will certainly be more efficient. Jul 13 '17 at 18:39
• It's quite likely mapping the 2D-map to a 1D-sequence is more performant than using 2 coordinates and an additional indirection. That miniscule additional calculation can often be optimized out, and is likely far cheaper than a random memory-access. Jul 13 '17 at 19:08
• If you want to show us your code after factoring in the reviews, you have two choices: 1. Post a review where you mention anything else you found deserves changing, and the final code. 2. Ask for a follow-up-review with a new question, and link them. What you should never do is change the question. Jul 13 '17 at 23:28

1. Do not use using namespace std;, especially not in a header. It can introduce subtle bugs.
2. You Map class has no destructor and is leaking memory. Everything that is allocated through new has to be deleted again (or, in the case of arrays, delete[]d).
3. //two dimensional array of fields gives the fastest access makes no sense because your array is not two dimensional (and I doubt that a two dimensional array would perform better than a one dimensional, but you are free to prove me wrong). If this is meant as a hint for you to improve the implementation later on, you should probably mark it as such (e.g. using the well known //TODO comment).
4. Comments like //check that field is in the map are superfluous when followed by lines such as bool isInTheMap = ..., because the variable name makes its purpose sufficiently clear. Generally, using comments is good, but they can also be overused (or written badly); using them effectively is key (and also not easy).
5. You should generally avoid using #pragma, at least when the code is supposed to be portable. In this case #pragma region is not portable at all (it is VS only) and generally also hints at bad code separation. If you think your file is unreadable without it, you should split it up into multiple.
• Good points especially for Map destructor. I know that declaring using namespaces is bad practise, really idk why I used it in the header, miss that. For me, array of arrays is two dimensional array :D. Is is possible to create real dynamic two dimensional array? Jul 13 '17 at 19:36
• @Michael You could have a std::vector of std::vectors. Also, I didn't comment on it in my answer, but you tend to have one layer of pointer indirection too much (i.e. Field** would suffice). Jul 13 '17 at 19:41
• I don't think that I have one layer too much, because fields is array of arrays of pointers, so it needs to be like that :). I don't think that using vector here is good idea, arrays are simply and fast, vectors additionally have so many methods that here are not needed. Jul 13 '17 at 19:45
• @Michael Methods you do not use have no overhead. Arrays are simple, but dangerous (you forgetting to free your memory is the best example of that). fields does not need to be an array of arrays of pointers, array of elements is enough. Adding another pointer adds you nothing but indirection (your elements live on the heap anyway). Jul 13 '17 at 19:47
• Yes, I am not freeing memory, because it is not the part of code where I will be doing this, take a look to FindPath method, it returns a vector<Field*>, vector of pointers, deleting them before returning path is not good idea, so thanks but I remember. I operate on pointers because they are fasters. Jul 13 '17 at 19:56

Don't use raw pointers and prefer keeping the Field by value in Map. Each dereference into cold cache is 200 cycles dropped on the floor, you force the cpu to do 2 of them each time to get a Field. If you instead have a single std::vector with size mapSize*mapSize indexed with y*mapSize+x (you can overload operator[] to retain the interface) you will get faster random access.

Whether a Field is closed can be kept by adding a bool closed member field into Field defaulted to false.

open.erase(open.find(current)); is equivalent to open.erase(open.begin()); and you got begin() just a little bit earlier. You may as well have just kept it temporarily.

When updating a Field you should remove the Field from the open set and reinsert it. The std::set you use won't work correctly when you update it's values in place and invalidate relative ordering.

if (tentativeGCost < f->GetGCost() || fieldIsNotInOpenSet)
{
if (!fieldIsNotInOpenSet)
{
open.erase(open.find(f));
}
f->SetGCost(tentativeGCost);
f->SetHCost(CalcScore(*f, b));
f->SetParent(*current);

open.insert(f);
}