PathFinder.cpp

#include <vector>
#include <algorithm>

#include "PathFinder.hpp"

namespace pft
{

    std::vector <sf::Vector2i>
    PathFinder::aStar(sf::Vector2i from, sf::Vector2i to, TileGrid& grid, sf::RenderWindow& window)
    {
        std::vector <sf::Vector2i> path;

        std::vector <Node*> openList;
        std::vector <Node*> closedList;

        auto begin = new Node(grid, from);
        auto end   = new Node(grid, to  );
        
        Node* current = nullptr;
        Node* child   = nullptr;

        int tries = 0;

        bool pressed = false;

        // adding the point A to the open list
        begin->setOpen(true);
        openList.push_back(begin);

        grid.setPoint(*begin, TileGrid::OPEN);

        while (tries == 0 || (*current != *end && tries <= 5000))
        {
            if (current != nullptr)
                grid.setPoint(*current, TileGrid::CLOSED);

            if (openList.empty())
                return path;

            for (auto it = openList.begin(); it != openList.end(); ++it)
            {
                // picking an open node with the smallest F score
                if (it == openList.begin() || (*it)->getFScore() <= current->getFScore())
                {
                    current = *it;
                }
            }

            if (*current == *end)
                break;

            // the current node is now checked so it can be marked as closed
            current->setOpen(false);
            openList.erase(std::find(openList.begin(), openList.end(), current));

            current->setClosed(true);
            closedList.push_back(current);

            // checking all of the nodes surrounding the current one
            for (int y = -1; y <= 1; ++y)
            {
                if (grid.getTile(sf::Vector2i(current->x, current->y+y)) == TileGrid::OBSTACLE)
                    continue;
                for (int x = -1; x <= 1; ++x)
                {
                    if (grid.getTile(sf::Vector2i(current->x+x, current->y)) == TileGrid::OBSTACLE)
                        continue;
                    if (x != 0 || y != 0) // ignoring the node in the middle, because this is the current node
                    {
                        bool exists = false;
                        for (auto it = openList.begin(); it != openList.end(); ++it)
                        {
                            if ((*it)->x == current->x+x && (*it)->y == current->y+y)
                            {
                                child = *it;
                                exists = true;
                                break;
                            }
                        }
                        if (!exists)
                        for (auto it = closedList.begin(); it != closedList.end(); ++it)
                        {
                            if ((*it)->x == current->x+x && (*it)->y == current->y+y)
                            {
                                child = *it;
                                exists = true;
                                break;
                            }
                        }
                        if (!exists)
                            child = new Node(grid, sf::Vector2i(current->x + x, current->y + y));

                        if (child->isWalkable() && !child->isClosed())
                        {
                            if (child->isOpen()) // if the node is already marked as open
                            {
                                if (child->getGScore() > child->getGScore(current)) // checking if it is better to walk through
                                {                                                   // the current node
                                    child->setParent(current);
                                    child->calculateScores(end);
                                }
                            }
                            else // if the child node is not marked as open
                            {
                                child->setParent(current);   // then connect it to the current node,
                                child->setOpen(true);        // set it open
                                child->calculateScores(end); // and calculate its scores

                                openList.push_back(child);

                                grid.setPoint(*child, TileGrid::OPEN);
                            }
                        }
                        else if (!exists)
                            delete child;
                    }
                }
            }
            ++tries;
            
            grid.setPoint(*current, TileGrid::CURRENT);

#if 0
#if 0
            while (true)
            {
                if (sf::Keyboard::isKeyPressed(sf::Keyboard::Space))
                {
                    if (!pressed)
                    {
                        pressed = true;
                        break;
                    }
                }
                else
                {
                    pressed = false;
                }
            }
#endif
            sf::sleep(sf::seconds(0.1f));
            window.clear();
            window.draw(grid);
            window.display();
#endif
        }

        for (auto it = openList.begin(); it != openList.end(); ++it)
        {
            (*it)->setOpen(false);
        }
        for (auto it = closedList.begin(); it != closedList.end(); ++it)
        {
            (*it)->setClosed(false);
        }

        while (current->getParent() != nullptr && *current != *begin)
        {
            path.push_back(sf::Vector2i(current->x, current->y));
            
            auto temp = current;
            current = temp->getParent();
            delete temp;
        }

        openList.clear();
        closedList.clear();

        return path;
    }

}

PathFinder.hpp

#ifndef PATHFINDER_HPP
#define PATHFINDER_HPP

#include <SFML/System/Vector2.hpp>
#include <cmath>
#include <vector>

#include "TileGrid.hpp"

namespace pft
{

    class PathFinder
    {
    private: class Node : public sf::Vector2i
             {
             private: Node* m_parent;

                      bool  m_walkable;
                      bool  m_open;
                      bool  m_closed;

                      int   m_FScore;
                      int   m_GScore;
                      int   m_HScore;

             public:  bool isWalkable() const;
                      bool isOpen    () const;
                      bool isClosed  () const;

                      Node* getParent() const;

                      int getFScore(     ) const;
                      int getGScore(     ) const;
                      int getGScore(Node*) const;
                      int getHScore(     ) const;
                      int getHScore(Node*) const;

                      void calculateScores(Node*);

                      void setWalkable(bool);
                      void setOpen    (bool);
                      void setClosed  (bool);

                      void setParent(Node*);

                      bool operator==(Node&);
                      bool operator!=(Node&);

                      Node();
                      Node(TileGrid&, sf::Vector2i);
             };

    public:  static std::vector <sf::Vector2i>
             aStar(sf::Vector2i from, sf::Vector2i to, TileGrid&, sf::RenderWindow&);
    };

}

#endif // PATHFINDER_HPP

PathFinder_Node.cpp

#include <cmath>

#include "PathFinder.hpp"

namespace pft
{

// PUBLIC

    PathFinder::Node::Node() :
      m_parent  (nullptr),
      m_open    (false),
      m_closed  (false),
      m_FScore  (0),
      m_GScore  (0),
      m_HScore  (0) {}

    PathFinder::Node::Node(TileGrid& grid, sf::Vector2i position) :
      m_parent  (nullptr),
      m_walkable(grid.getTile(position) != TileGrid::OBSTACLE),
      m_open    (false),
      m_closed  (false),
      m_FScore  (0),
      m_GScore  (0),
      m_HScore  (0)
    {
        this->x = position.x;
        this->y = position.y;
    }

    bool
    PathFinder::Node::isWalkable() const
    {
        return m_walkable;
    }

    bool
    PathFinder::Node::isOpen() const
    {
        return m_open;
    }

    bool
    PathFinder::Node::isClosed() const
    {
        return m_closed;
    }

    PathFinder::Node*
    PathFinder::Node::getParent() const
    {
        return m_parent;
    }

    int
    PathFinder::Node::getFScore() const
    {
        return m_FScore;
    }

    int
    PathFinder::Node::getGScore() const
    {
        return m_GScore;
    }

    int
    PathFinder::Node::getGScore(PathFinder::Node* neighbor) const
    {
        return neighbor->getGScore() + ((this->x == neighbor->x || this->y == neighbor->y) ? 10 : 14);
    }

    int
    PathFinder::Node::getHScore() const
    {
        return m_HScore;
    }

    int
    PathFinder::Node::getHScore(PathFinder::Node* other) const
    {
        return 10 * (std::abs(this->x - other->x) + std::abs(this->y - other->y));
    }

    void
    PathFinder::Node::calculateScores(PathFinder::Node* goal)
    {
        m_GScore = getGScore(m_parent);
        m_HScore = getHScore(goal);
        m_FScore = m_GScore + m_HScore;
    }

    void
    PathFinder::Node::setWalkable(bool b)
    {
        m_walkable = b;
    }

    void
    PathFinder::Node::setOpen(bool b)
    {
        m_open = b;
    }

    void
    PathFinder::Node::setClosed(bool b)
    {
        m_closed = b;
    }

    void
    PathFinder::Node::setParent(Node* parent)
    {
        m_parent = parent;
    }

    bool
    PathFinder::Node::operator==(PathFinder::Node& right)
    {
        return x == right.x && y == right.y;
    }

    bool
    PathFinder::Node::operator!=(PathFinder::Node& right)
    {
        return x != right.x || y != right.y;
    }

}