Skip to main content
added 8684 characters in body
Source Link
Michael
  • 305
  • 2
  • 8

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

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
edited tags
Link
200_success
  • 145.7k
  • 22
  • 191
  • 481
Source Link
Michael
  • 305
  • 2
  • 8

A* Algorithm implementation

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.