Skip to content

Instantly share code, notes, and snippets.

@Adanos020
Created November 24, 2016 17:11
Show Gist options
  • Save Adanos020/0ca6178d6ec10d256aa486e81b4ee219 to your computer and use it in GitHub Desktop.
Save Adanos020/0ca6178d6ec10d256aa486e81b4ee219 to your computer and use it in GitHub Desktop.
#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;
}
}
#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
#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;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment