A* Algorithm implementation

Question

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

---

Navigation.hpp

#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;
};

---

Navigation.cpp

#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
 return totalPath;
 }
#pragma endregion PathFinder

---

I have a few questions about this code:

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

Navigation.hpp

#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;
};

Navigation.cpp

#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
 return totalPath;
 }
#pragma endregion PathFinder

• BlueRaja - Danny Pflughoeft: 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
• Michael: @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. :)
• BlueRaja - Danny Pflughoeft: @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.
• Deduplicator: 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.
• Deduplicator: 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.

Answer 1

Some general advice:

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.

• Michael: 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?
• Ben Steffan: @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).
• Michael: 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.
• Ben Steffan: @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).
• Michael: 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.

Answer 2

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);
}