Skip to content

Instantly share code, notes, and snippets.

@MORTAL2000

MORTAL2000/test00.cpp

Last active July 9, 2016 21:26
Show Gist options
  • Save MORTAL2000/e26c072bf1e730bde93235da95308d77 to your computer and use it in GitHub Desktop.
Save MORTAL2000/e26c072bf1e730bde93235da95308d77 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
test00.cpp
#include <SFML/Graphics.hpp>
#include <iostream>
#include <iomanip>
#include <unordered_map>
#include <unordered_set>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <functional>
#include <algorithm>
namespace std
{
template <>
struct hash<sf::Vector2<int> >
{
inline size_t operator()(const sf::Vector2<int>& location) const
{
return location.x * 1812433253 + location.y;
}
};
}
template<typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> loc)
{
out << '(' << loc.x << ',' << loc.y << ')';
return out;
}
namespace sf
{
template<typename T >
bool operator> (const Vector2<T> &left, const Vector2<T> &right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template<typename T >
bool operator< (const Vector2<T> &left, const Vector2<T> &right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
}
class Grid : public sf::Drawable
{
using PQElement = std::pair<sf::Vector2i, double>;
public:
Grid(int width_, int height_)
: width(width_)
, height(height_)
, grid(width_ * height_)
{
const auto tile_size = sf::Vector2f(20, 20);
int i=0;
for(auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
tile.setFillColor(sf::Color::White);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
}
}
void add_wall(int x1, int y1, int x2, int y2)
{
// if(!in_bounds(sf::Vector2i(x1+1, y1+1)) || !in_bounds(sf::Vector2i(x2+1, y2+1)))
// {
// std::cout << "Square::add_wall: invalid parameters: "
// << sf::Vector2i(x1, y1) << ' '
// << sf::Vector2i(x2, y2) <<"\n";
// return;
// }
for (int x = x1; x < x2; ++x)
{
for (int y = y1; y < y2; ++y)
{
walls.insert(sf::Vector2i { x, y });
grid[x + width * y].setFillColor(sf::Color(139,137,137));
}
}
}
void breadth_first_search(sf::Vector2i start)
{
came_from.clear();
std::queue<sf::Vector2i> frontier;
frontier.push(start);
came_from[start] = start;
while (!frontier.empty())
{
auto current = frontier.front();
frontier.pop();
for (auto next : neighbors(current))
{
if (!came_from.count(next))
{
frontier.push(next);
came_from[next] = current;
}
}
}
begin = came_from.begin();
}
void breadth_first_search(sf::Vector2i start, sf::Vector2i goal)
{
std::queue<sf::Vector2i> frontier;
frontier.push(start);
came_from[start] = start;
while (!frontier.empty())
{
auto current = frontier.front();
frontier.pop();
if (current == goal) {
begin = came_from.begin();
reconstruct_path(start, goal);
return;
}
for (auto next : neighbors(current))
{
if (!came_from.count(next))
{
frontier.push(next);
came_from[next] = current;
}
}
}
}
void dijkstra_search(sf::Vector2i start, sf::Vector2i goal)
{
std::priority_queue<PQElement, std::vector<PQElement>, std::greater<PQElement>> frontier;
frontier.emplace(start, 0);
came_from[start] = start;
cost_so_far[start] = 0;
while (!frontier.empty())
{
auto current = frontier.top().first;
frontier.pop();
if (current == goal) {
begin = came_from.begin();
reconstruct_path(start, goal);
return;
}
for (auto next : neighbors(current))
{
double new_cost = cost_so_far[current] + cost(current, next);
if (!cost_so_far.count(next) || new_cost < cost_so_far[next])
{
cost_so_far[next] = new_cost;
came_from[next] = current;
frontier.emplace(next, new_cost);
}
}
}
}
void a_star_search(sf::Vector2i start, sf::Vector2i goal)
{
std::priority_queue<PQElement, std::vector<PQElement>, std::greater<PQElement>> frontier;
frontier.emplace(start, 0);
came_from[start] = start;
cost_so_far[start] = 0;
while (!frontier.empty())
{
auto current = frontier.top().first;
frontier.pop();
if (current == goal) {
begin = came_from.begin();
reconstruct_path(start, goal);
return;
}
for (auto next : neighbors(current))
{
double new_cost = cost_so_far[current] + cost(current, next);
if (!cost_so_far.count(next) || new_cost < cost_so_far[next])
{
cost_so_far[next] = new_cost;
double priority = new_cost + heuristic(next, goal);
frontier.emplace(next, priority);
came_from[next] = current;
}
}
}
}
bool updateSearch(sf::Time dt)
{
if (timer <= sf::Time::Zero)
{
timer += sf::seconds(0.0125f);//sf::seconds(0.25f);
if(begin == came_from.end()){timer = sf::Time::Zero; return false;}
if ( begin->second != start && begin->second != goal ){
//grid[begin->second.x + width * begin->second.y].setFillColor(sf::Color(127,255,212));
grid[begin->second.x + width * begin->second.y].setFillColor(sf::Color(102,205,170));
}
begin++;
}else timer -= dt;
return true;
}
bool updatePath(sf::Time dt)
{
if (timer <= sf::Time::Zero)
{
timer += sf::seconds(0.0125f);//sf::seconds(0.25f);
if(vbegin == path.end()) {timer = sf::Time::Zero; return false;}
if ( *vbegin != start && *vbegin != goal )
grid[vbegin->x + width * vbegin->y].setFillColor(sf::Color::Yellow);
vbegin++;
}else timer -= dt;
return true;
}
private:
inline bool in_bounds(sf::Vector2i id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
inline bool passable(sf::Vector2i id) const
{
return !walls.count(id);
}
std::vector<sf::Vector2i> neighbors(sf::Vector2i id) const
{
static const std::array<sf::Vector2i, 4> DIRS{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1}
}};
std::vector<sf::Vector2i> results;
for (auto dir : DIRS)
{
sf::Vector2i next(id + dir);
if (in_bounds(next) && passable(next))
{
results.emplace_back(next);
}
}
if ((id.x + id.y) % 2 == 0)
{
// aesthetic improvement on square grids
std::reverse(results.begin(), results.end());
}
return results;
}
double cost(sf::Vector2i from_node, sf::Vector2i to_node) const
{
return forests.count(to_node) ? 5 : 1;
}
inline double heuristic(sf::Vector2i a, sf::Vector2i b)
{
return abs(a.x - b.x) + abs(a.y - b.y);
}
void reconstruct_path(sf::Vector2i start_, sf::Vector2i goal_)
{
start = start_;
goal = goal_;
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
auto current = goal;
path.push_back(current);
while (current != start) {
current = came_from[current];
path.push_back(current);
}
std::reverse(path.begin(), path.end());
vbegin = path.begin();
}
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
for(const auto& tile : grid)
{
target.draw(tile);
}
}
private:
int width, height;
std::vector<sf::RectangleShape> grid;
std::unordered_set<sf::Vector2i> walls;
std::unordered_map<sf::Vector2i, double> cost_so_far;
std::unordered_map<sf::Vector2i, sf::Vector2i> came_from;
std::unordered_map<sf::Vector2i, sf::Vector2i>::iterator begin;
std::unordered_set<sf::Vector2i> forests;
std::vector<sf::Vector2i> path;
std::vector<sf::Vector2i>::iterator vbegin;
sf::Time timer = sf::Time::Zero;
sf::Vector2i start{};
sf::Vector2i goal{};
};
Grid make_diagram1()
{
Grid grid(30, 15);//(45, 30);
grid.add_wall(3, 3, 5, 12);
grid.add_wall(13, 4, 15, 15);
grid.add_wall(21, 0, 23, 7);
grid.add_wall(23, 5, 26, 7);
return grid;
}
Grid make_diagram4()
{
Grid grid(10, 10);
grid.add_wall(1, 7, 4, 9);
// grid.forests = std::unordered_set<sf::Vector2i> {
// {3, 4}, {3, 5}, {4, 1}, {4, 2},
// {4, 3}, {4, 4}, {4, 5}, {4, 6},
// {4, 7}, {4, 8}, {5, 1}, {5, 2},
// {5, 3}, {5, 4}, {5, 5}, {5, 6},
// {5, 7}, {5, 8}, {6, 2}, {6, 3},
// {6, 4}, {6, 5}, {6, 6}, {6, 7},
// {7, 3}, {7, 4}, {7, 5}
// };
return grid;
}
int main()
{
sf::RenderWindow window({30*20, 15*20}/*{45*20, 30*20}*/, "algorithm");
Grid grid = make_diagram1();
//grid.breadth_first_search(sf::Vector2i(7, 8), sf::Vector2i{17, 13});
//grid.dijkstra_search(sf::Vector2i(7, 8), sf::Vector2i{17, 13});
grid.a_star_search(sf::Vector2i(7, 8), sf::Vector2i{17, 13});
sf::Clock clock;
const sf::Time TimePerFrame = sf::seconds(1.f / 60.f);
sf::Time timeSinceLastUpdate = sf::Time::Zero;
while(window.isOpen())
{
sf::Time dt = clock.restart();
timeSinceLastUpdate += dt;
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
while (timeSinceLastUpdate > TimePerFrame)
{
timeSinceLastUpdate -= TimePerFrame;
if(!grid.updateSearch(TimePerFrame))
grid.updatePath(TimePerFrame);
}
window.clear();
window.draw(grid);
window.display();
}
}
Raw
test01.cpp
#include <SFML/Graphics.hpp>
#include <SFML/System.hpp>
#include <iostream>
#include <iomanip>
#include <unordered_map>
#include <unordered_set>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <functional>
#include <algorithm>
namespace std
{
template <>
struct hash<sf::Vector2<int> >
{
inline size_t operator()(const sf::Vector2<int>& v) const
{
return v.x + 1812433253 * v.y;
}
};
}
template<typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y << ')';
return out;
}
namespace sf
{
template<typename T >
bool operator> (const Vector2<T> &left, const Vector2<T> &right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template<typename T >
bool operator< (const Vector2<T> &left, const Vector2<T> &right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
}
class Grid
{
using pairs = std::pair<sf::Vector2i, double>;
public:
Grid(int width_, int height_, int block_size, sf::RenderWindow& window)
: width(width_)
, height(height_)
, grid(width_ * height_)
, window(window)
{
const auto tile_size = sf::Vector2f(block_size, block_size);
int i=0;
for(auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
tile.setFillColor(sf::Color::White);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
}
}
void add_wall(int x1, int y1, int x2, int y2)
{
// if(!in_bounds(sf::Vector2i(x1+1, y1+1)) || !in_bounds(sf::Vector2i(x2+1, y2+1)))
// {
// std::cout << "Square::add_wall: invalid parameters: "
// << sf::Vector2i(x1, y1) << ' '
// << sf::Vector2i(x2, y2) <<"\n";
// return;
// }
for (int x = x1; x < x2; ++x)
{
for (int y = y1; y < y2; ++y)
{
walls.insert({ x, y });
grid[x + width * y].setFillColor({139,137,137});
}
}
}
void breadth_first_search(sf::Vector2i start)
{
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
std::queue<sf::Vector2i> frontier;
frontier.push(start);
came_from[start] = start;
while (!frontier.empty())
{
sf::sleep(sf::seconds(0.5f));
auto current = frontier.front();
frontier.pop();
if(current != start && current != goal)
grid[current.x + width * current.y].setFillColor({102,205,170});
for (auto next : neighbors(current))
{
if (!came_from.count(next))
{
frontier.push(next);
came_from[next] = current;
if(next != start && next != goal)
grid[next.x + width * next.y].setFillColor({127,255,212});
}
}
}
}
void breadth_first_search(sf::Vector2i start, sf::Vector2i goal)
{
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
std::queue<sf::Vector2i> frontier;
frontier.push(start);
came_from[start] = start;
while (!frontier.empty())
{
sf::sleep(sf::seconds(0.5f));
auto current = frontier.front();
frontier.pop();
if(current != start && current != goal)
grid[current.x + width * current.y].setFillColor({102,205,170});
if (current == goal) {
reconstruct_path(start, goal);
return;
}
for (auto next : neighbors(current))
{
if (!came_from.count(next))
{
frontier.push(next);
came_from[next] = current;
if(next != start && next != goal)
grid[next.x + width * next.y].setFillColor({127,255,212});
}
}
draw();
}
}
void dijkstra_search(sf::Vector2i start, sf::Vector2i goal)
{
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
std::priority_queue<pairs, std::vector<pairs>, std::greater<pairs>> frontier;
frontier.emplace(start, 0);
came_from[start] = start;
cost_so_far[start] = 0;
while (!frontier.empty())
{
sf::sleep(sf::seconds(0.5f));
auto current = frontier.top().first;
frontier.pop();
if(current != start && current != goal)
grid[current.x + width * current.y].setFillColor({102,205,170});
if (current == goal) {
reconstruct_path(start, goal);
return;
}
for (auto next : neighbors(current))
{
double new_cost = cost_so_far[current] + cost(current, next);
if (!cost_so_far.count(next) || new_cost < cost_so_far[next])
{
cost_so_far[next] = new_cost;
came_from[next] = current;
frontier.emplace(next, new_cost);
if(next != start && next != goal)
grid[next.x + width * next.y].setFillColor({127,255,212});
}
}
draw();
}
}
void a_star_search(sf::Vector2i start, sf::Vector2i goal)
{
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
std::priority_queue<pairs, std::vector<pairs>, std::greater<pairs>> frontier;
frontier.emplace(start, 0);
came_from[start] = start;
cost_so_far[start] = 0;
while (!frontier.empty())
{
sf::sleep(sf::seconds(0.5f));
auto current = frontier.top().first;
frontier.pop();
if(current != start && current != goal)
grid[current.x + width * current.y].setFillColor({102,205,170});
if (current == goal) {
reconstruct_path(start, goal);
return;
}
for (auto next : neighbors(current))
{
std::cout << next << ' ';
double new_cost = cost_so_far[current] + cost(current, next);
if (!cost_so_far.count(next) || new_cost < cost_so_far[next])
{
cost_so_far[next] = new_cost;
double priority = new_cost + heuristic(next, goal);
frontier.emplace(next, priority);
came_from[next] = current;
if(next != start && next != goal)
grid[next.x + width * next.y].setFillColor({127,255,212});
}
}
std::cout << '\n';
draw();
}
}
bool updatePath(sf::Time dt)
{
if (timer <= sf::Time::Zero)
{
timer += sf::seconds(0.0125f);//sf::seconds(0.25f);
if(vbegin == path.end()) {timer = sf::Time::Zero; return false;}
if ( *vbegin != start && *vbegin != goal )
grid[vbegin->x + width * vbegin->y].setFillColor(sf::Color::Yellow);
vbegin++;
}else timer -= dt;
return true;
}
void draw() const
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for(const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
std::unordered_set<sf::Vector2i> forests;
private:
inline bool in_bounds(sf::Vector2i id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
inline bool passable(sf::Vector2i id) const
{
return !walls.count(id);
}
std::vector<sf::Vector2i> neighbors(sf::Vector2i id) const
{
static const std::array<sf::Vector2i, 4> DIRS{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
// { 1, 1},
// { 1, -1},
// {-1, 1},
// { 1, 1},
}};
std::vector<sf::Vector2i> results;
for (auto dir : DIRS)
{
sf::Vector2i next(id + dir);
if (in_bounds(next) && passable(next))
{
results.emplace_back(next);
}
}
if ((id.x + id.y) % 2 == 0)
{
// aesthetic improvement on square grids
//std::reverse(results.begin(), results.end());
}
return results;
}
double cost(sf::Vector2i from_node, sf::Vector2i to_node) const
{
return forests.count(to_node) ? 5 : 1;
}
inline double heuristic(sf::Vector2i a, sf::Vector2i b)
{
return abs(a.x - b.x) + abs(a.y - b.y);
}
void reconstruct_path(sf::Vector2i start_, sf::Vector2i goal_)
{
start = start_;
goal = goal_;
auto current = goal;
path.push_back(current);
while (current != start) {
current = came_from[current];
path.push_back(current);
}
std::reverse(path.begin(), path.end());
vbegin = path.begin();
}
private:
int width, height;
std::vector<sf::RectangleShape> grid;
std::unordered_set<sf::Vector2i> walls;
std::unordered_map<sf::Vector2i, double> cost_so_far;
std::unordered_map<sf::Vector2i, sf::Vector2i> came_from;
//std::unordered_set<sf::Vector2i> forests;
std::vector<sf::Vector2i> path;
std::vector<sf::Vector2i>::iterator vbegin;
sf::Time timer = sf::Time::Zero;
sf::Vector2i start{};
sf::Vector2i goal{};
sf::RenderWindow& window;
};
Grid make_diagram1(
sf::RenderWindow &window,
int width = 30,
int height = 15,
int block_size = 20)
{
Grid grid(width, height, block_size, window);//(45, 30);
grid.add_wall(3, 3, 5, 12);
grid.add_wall(13, 4, 15, 15);
grid.add_wall(21, 0, 23, 7);
grid.add_wall(23, 5, 26, 7);
return grid;
}
Grid make_diagram4(
sf::RenderWindow &window,
int width = 10,
int height = 10,
int block_size = 20)
{
Grid grid(width, height, block_size, window);
grid.add_wall(1, 7, 4, 9);
// grid.forests = std::unordered_set<sf::Vector2i> {
// {3, 4}, {3, 5}, {4, 1}, {4, 2},
// {4, 3}, {4, 4}, {4, 5}, {4, 6},
// {4, 7}, {4, 8}, {5, 1}, {5, 2},
// {5, 3}, {5, 4}, {5, 5}, {5, 6},
// {5, 7}, {5, 8}, {6, 2}, {6, 3},
// {6, 4}, {6, 5}, {6, 6}, {6, 7},
// {7, 3}, {7, 4}, {7, 5}
// };
return grid;
}
int main()
{
unsigned width = 10;
unsigned height = 10;
unsigned block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "algorithm");
Grid grid = make_diagram4(window);
//grid.breadth_first_search({1, 4}, {8, 5});
grid.dijkstra_search({1, 4}, {8, 5});
//grid.a_star_search({1, 4}, {8, 5});
sf::Clock clock;
const sf::Time TimePerFrame = sf::seconds(1.f / 60.f);
sf::Time timeSinceLastUpdate = sf::Time::Zero;
while(window.isOpen())
{
sf::Time dt = clock.restart();
timeSinceLastUpdate += dt;
while (timeSinceLastUpdate > TimePerFrame)
{
timeSinceLastUpdate -= TimePerFrame;
grid.updatePath(TimePerFrame);
}
grid.draw();
}
}
Raw
test02.cpp
#include <SFML/Graphics.hpp>
#include <SFML/System.hpp>
#include <iostream>
#include <iomanip>
#include <unordered_map>
#include <unordered_set>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <set>
#include <functional>
#include <algorithm>
namespace sf
{
template <typename T>
bool operator> (Vector2<T> const& left, Vector2<T> const& right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template <typename T>
bool operator< (Vector2<T> const& left, Vector2<T> const& right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
template <typename T>
Vector2<T> operator /(Vector2<T> const& left, Vector2<T> const& right)
{
T x = left.x / right.x;
T y = left.y / right.y;
return {x, y};
}
}
template<typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y << ')';
return out;
}
template<typename N>
class Graph
{
class Node;
using NodeHolder = std::set<Node>;
public:
using NodeVal = N;
using NodeRef = typename NodeHolder::iterator;
using Edges = std::vector<std::pair<NodeRef, int>>;
private:
class Node
{
N data;
mutable Edges outedge;
public:
Node(N const& data)
: data(data)
{}
void addEdge(NodeRef e, int cost) const
{
outedge.emplace_back(e, cost);
}
NodeVal const& value() const
{
return data;
}
Edges const& getEdges() const
{
return outedge;
}
friend bool operator<(Node const& lhs, Node const& rhs)
{
return lhs.data < rhs.data;
}
};
NodeHolder nodes;
public:
NodeRef addNode(N const& data)
{
auto const& result = nodes.emplace(data);
return result.first;
}
NodeRef getRef(N const& data)
{
auto const& found = nodes.find(data);
if(found == nodes.end())
throw std::runtime_error("Graph::getRef: Key not found!\n");
return found;
}
void addEdge(NodeRef src, NodeRef dst, int cost)
{
if (src != nodes.end() && dst != nodes.end()) {
src->addEdge(dst, cost);
}
}
Edges const& getEdges(N const& node) const
{
auto const& found = nodes.find(node);
if(found == nodes.end())
throw std::runtime_error("Graph::getEdges: node not found!\n");
return found;
}
};
template<typename Graph>
class Dijkstra
{
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
struct QueInfo: public std::tuple<NodeRef, int, std::vector<NodeRef>>
{
QueInfo(QueInfo const&) = default;
QueInfo(NodeRef const& data, int cost, std::vector<NodeRef> const& route)
: std::tuple<NodeRef, int, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator<(QueInfo const& lhs, QueInfo const& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
Graph const& graph;
sf::RenderWindow& window;
std::vector<sf::RectangleShape>& grid;
sf::Vector2i start, goal;
public:
Dijkstra(Graph const& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
sf::Vector2i start,
sf::Vector2i goal)
: graph(graph), window(window), grid(grid), start(start), goal(goal)
{}
std::vector<NodeRef> route(NodeRef const& src, NodeRef const& dst)
{
std::set<NodeVal> found;
std::priority_queue<QueInfo> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while(!frontier.empty())
{
sf::sleep(sf::seconds(0.0125f));
auto next = frontier.top();
frontier.pop();
auto const& current = std::get<0>(next);
if (found.find(current->value()) != found.end()) continue;
if(current->value() != start && current->value() != goal)
paint(current->value(),{102,205,170});
found.emplace(current->value());
auto const& result = std::get<2>(next);
if (current == dst) return result;
for(auto const& loop: current->getEdges())
{
if(loop.first->value() != start && loop.first->value() != goal)
paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second, result);
}
draw();
}
return {};
}
// debug drawing
void paint(sf::Vector2i coord, sf::Color color)
{
grid[coord.x + 10 * coord.y].setFillColor(color);
}
void draw()
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for(const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
};
class Grid : public sf::Drawable
{
public:
Grid(int width_,
int height_,
int block_size,
sf::Vector2i start,
sf::Vector2i goal,
sf::RenderWindow& window)
: width(width_), height(height_), grid(width_ * height_)
, start(start), goal(goal), graph()
, dijkstra(graph, window, grid, start, goal)
{
auto tile_size = sf::Vector2f(block_size, block_size);
int i=0;
for(auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
tile.setFillColor(sf::Color::White);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
graph.addNode(sf::Vector2i{position});
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
for(auto& tile : grid)
{
auto p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size) ;
for (auto next : neighbors(p))
graph.addEdge(graph.getRef(p), graph.getRef(next), 1);
}
auto const& result = dijkstra.route(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](auto d){return d->value();});
vbegin = path.begin();
}
bool updatePath(sf::Time dt)
{
if (timer <= sf::Time::Zero)
{
timer += sf::seconds(0.125f);//sf::seconds(0.25f);
if(vbegin == path.end()) {timer = sf::Time::Zero; return false;}
if ( *vbegin != start && *vbegin != goal )
grid[vbegin->x + width * vbegin->y].setFillColor(sf::Color::Yellow);
vbegin++;
}else timer -= dt;
return true;
}
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
for(const auto& tile : grid)
{
target.draw(tile);
}
}
private:
inline bool in_bounds(sf::Vector2i id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
std::vector<sf::Vector2i> neighbors(sf::Vector2i id) const
{
static const std::array<sf::Vector2i, 4> Directions{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
// {-1, 1},
// { 1, -1},
// {-1, -1},
// { 1, 1},
}};
std::vector<sf::Vector2i> results;
for (auto direction : Directions)
{
const auto& next(id + direction);
if (in_bounds(next))
{
results.emplace_back(next);
}
}
return results;
}
private:
int width, height;
std::vector<sf::RectangleShape> grid;
std::vector<sf::Vector2i> path;
// for animation
std::vector<sf::Vector2i>::iterator vbegin;
sf::Time timer = sf::Time::Zero;
sf::Vector2i start;
sf::Vector2i goal;
// Algorithm's engine
Graph<sf::Vector2i> graph;
Dijkstra<Graph<sf::Vector2i>> dijkstra;
};
int main()
{
try{
unsigned width = 10;
unsigned height = 10;
unsigned block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "algorithm");
Grid grid(width, height, block_size, {1, 4}, {8, 5}, window);
sf::Clock clock;
const sf::Time TimePerFrame = sf::seconds(1.f / 60.f);
sf::Time timeSinceLastUpdate = sf::Time::Zero;
while(window.isOpen())
{
sf::Time dt = clock.restart();
timeSinceLastUpdate += dt;
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
while (timeSinceLastUpdate > TimePerFrame)
{
timeSinceLastUpdate -= TimePerFrame;
grid.updatePath(TimePerFrame);
}
window.clear();
window.draw(grid);
window.display();
}
} catch(std::runtime_error& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
} catch(...)
{
std::cout << "Unknown Exception!\n";
return 1;
}
}
Raw
test03.cpp
#include <SFML/Graphics.hpp>
#include <SFML/System.hpp>
#include <iostream>
#include <iomanip>
#include <unordered_map>
#include <unordered_set>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <set>
#include <functional>
#include <algorithm>
namespace sf
{
template <typename T>
bool operator > (Vector2<T> const& left, Vector2<T> const& right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template <typename T>
bool operator < (Vector2<T> const& left, Vector2<T> const& right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
template <typename T>
Vector2<T> operator /(Vector2<T> const& left, Vector2<T> const& right)
{
T x = left.x / right.x;
T y = left.y / right.y;
return {x, y};
}
}
template <typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y << ')';
return out;
}
template <typename N>
class Graph
{
class Node;
using NodeHolder = std::set<Node>;
public:
using NodeVal = N;
using NodeRef = typename NodeHolder::iterator;
using Edges = std::vector<std::pair<NodeRef, int>>;
private:
class Node
{
N data;
mutable Edges outedge;
public:
Node(N const& data)
: data(data)
{}
void addEdge(NodeRef e, int cost) const
{
outedge.emplace_back(e, cost);
}
NodeVal const& value() const
{
return data;
}
Edges const& getEdges() const
{
return outedge;
}
friend bool operator<(Node const& lhs, Node const& rhs)
{
return lhs.data < rhs.data;
}
};
NodeHolder nodes;
public:
NodeRef addNode(N const& data)
{
auto const& result = nodes.emplace(data);
return result.first;
}
NodeRef getRef(N const& data)
{
auto const& found = nodes.find(data);
if(found == nodes.end())
throw std::runtime_error("Graph::getRef: Key not found!\n");
return found;
}
void addEdge(NodeRef src, NodeRef dst, int cost)
{
if (src != nodes.end() && dst != nodes.end()) {
src->addEdge(dst, cost);
}
}
Edges const& getEdges(N const& node) const
{
auto const& found = nodes.find(node);
if(found == nodes.end())
throw std::runtime_error("Graph::getEdges: node not found!\n");
return found;
}
};
template <typename Graph>
class Dijkstra
{
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
struct QueInfo: public std::tuple<NodeRef, int, std::vector<NodeRef>>
{
QueInfo(QueInfo const&) = default;
QueInfo(NodeRef const& data, int cost, std::vector<NodeRef> const& route)
: std::tuple<NodeRef, int, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator<(QueInfo const& lhs, QueInfo const& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
Graph const& graph;
sf::RenderWindow& window;
std::vector<sf::RectangleShape>& grid;
sf::Vector2i start, goal;
int width;
public:
Dijkstra(Graph const& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
sf::Vector2i start,
sf::Vector2i goal,
int width)
: graph(graph), window(window), grid(grid), start(start), goal(goal), width(width)
{}
std::vector<NodeRef> route(NodeRef const& src, NodeRef const& dst)
{
std::set<NodeVal> found;
std::priority_queue<QueInfo> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while(!frontier.empty())
{
sf::sleep(sf::seconds(0.0125f));
auto next = frontier.top();
frontier.pop();
auto const& current = std::get<0>(next);
if (found.find(current->value()) != found.end()) continue;
if(current->value() != start && current->value() != goal)
paint(current->value(),{102,205,170});
found.emplace(current->value());
auto const& result = std::get<2>(next);
if (current == dst) return result;
for(auto const& loop: current->getEdges())
{
if(loop.first->value() != start && loop.first->value() != goal)
paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second, result);
}
draw();
}
return {};
}
// debug drawing
void paint(sf::Vector2i coord, sf::Color color)
{
grid[coord.x + width * coord.y].setFillColor(color);
}
void draw()
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for(const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
};
template <typename Graph>
class Astar
{
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
struct QueInfo: public std::tuple<NodeRef, int, std::vector<NodeRef>>
{
QueInfo(QueInfo const&) = default;
QueInfo(NodeRef const& data, int cost, std::vector<NodeRef> const& route)
: std::tuple<NodeRef, int, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator<(QueInfo const& lhs, QueInfo const& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
Graph const& graph;
sf::RenderWindow& window;
std::vector<sf::RectangleShape>& grid;
sf::Vector2i start, goal;
int width;
public:
Astar(Graph const& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
sf::Vector2i start,
sf::Vector2i goal,
int width)
: graph(graph), window(window), grid(grid), start(start), goal(goal), width(width)
{}
std::vector<NodeRef> route(NodeRef const& src, NodeRef const& dst)
{
std::set<NodeVal> found;
std::priority_queue<QueInfo> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while(!frontier.empty())
{
//sf::sleep(sf::seconds(0.0125f));
auto next = frontier.top();
frontier.pop();
auto const& current = std::get<0>(next);
if (found.find(current->value()) != found.end()) continue;
if(current->value() != start && current->value() != goal)
paint(current->value(),{102,205,170});
found.emplace(current->value());
auto const& result = std::get<2>(next);
if (current == dst) return result;
for(auto const& loop: current->getEdges())
{
if(loop.first->value() != start && loop.first->value() != goal)
paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second + heuristic(loop.first->value(), goal), result);
}
draw();
}
return {};
}
inline int heuristic(sf::Vector2i a, sf::Vector2i b)
{
return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}
// debug drawing
void paint(sf::Vector2i coord, sf::Color color)
{
grid[coord.x + width * coord.y].setFillColor(color);
}
void draw()
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for(const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
};
class Grid : public sf::Drawable
{
public:
Grid(int width_,
int height_,
int block_size,
sf::Vector2i start,
sf::Vector2i goal,
sf::RenderWindow& window)
: width(width_), height(height_), grid(width_ * height_)
, start(start), goal(goal), graph()
, dijkstra(graph, window, grid, start, goal, width)
, astar(graph, window, grid, start, goal, width)
{
auto tile_size = sf::Vector2f(block_size, block_size);
int i=0;
for(auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
tile.setFillColor(sf::Color::White);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
graph.addNode(sf::Vector2i(position));
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
for(auto& tile : grid)
{
auto p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size) ;
for (auto next : neighbors(p))
graph.addEdge(graph.getRef(p), graph.getRef(next), 1);
}
// auto const& result = dijkstra.route(graph.getRef(start), graph.getRef(goal));
//
// std::transform(std::begin(result), std::end(result),
// std::back_inserter(path), [](auto d){return d->value();});
auto const& result = astar.route(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](auto d){return d->value();});
vbegin = path.begin();
}
bool updatePath(sf::Time dt)
{
if (timer <= sf::Time::Zero)
{
timer += sf::seconds(0.125f);//sf::seconds(0.25f);
if(vbegin == path.end()) {timer = sf::Time::Zero; return false;}
if ( *vbegin != start && *vbegin != goal )
grid[vbegin->x + width * vbegin->y].setFillColor(sf::Color::Yellow);
vbegin++;
}else timer -= dt;
return true;
}
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
for(const auto& tile : grid)
{
target.draw(tile);
}
}
private:
inline bool in_bounds(sf::Vector2i id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
std::vector<sf::Vector2i> neighbors(sf::Vector2i id) const
{
static const std::array<sf::Vector2i, 4> Directions{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
// {-1, 1},
// { 1, -1},
// {-1, -1},
// { 1, 1},
}};
std::vector<sf::Vector2i> results;
for (auto direction : Directions)
{
const auto& next(id + direction);
if (in_bounds(next))
{
results.emplace_back(next);
}
}
return results;
}
private:
int width, height;
std::vector<sf::RectangleShape> grid;
std::vector<sf::Vector2i> path;
// for animation
std::vector<sf::Vector2i>::iterator vbegin;
sf::Time timer = sf::Time::Zero;
sf::Vector2i start;
sf::Vector2i goal;
// Algorithm's engine
Graph<sf::Vector2i> graph;
Dijkstra<Graph<sf::Vector2i>> dijkstra;
Astar<Graph<sf::Vector2i>> astar;
};
int main()
{
try{
unsigned width = 45;//10;
unsigned height = 30;//10;
unsigned block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "algorithm");
//Grid grid(width, height, block_size, {1, 4}, {8, 5}, window);
Grid grid(width, height, block_size, {5, 20}, {40, 5}, window);
sf::Clock clock;
const sf::Time TimePerFrame = sf::seconds(1.f / 60.f);
sf::Time timeSinceLastUpdate = sf::Time::Zero;
while(window.isOpen())
{
sf::Time dt = clock.restart();
timeSinceLastUpdate += dt;
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
while (timeSinceLastUpdate > TimePerFrame)
{
timeSinceLastUpdate -= TimePerFrame;
grid.updatePath(TimePerFrame);
}
window.clear();
window.draw(grid);
window.display();
}
} catch(std::runtime_error& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
} catch(...)
{
std::cout << "Unknown Exception!\n";
return 1;
}
}
Raw
test04.cpp
#include <SFML/Graphics.hpp>
#include <SFML/System.hpp>
#include <iostream>
#include <iomanip>
#include <unordered_map>
#include <unordered_set>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <set>
#include <functional>
#include <algorithm>
namespace std
{
template <>
struct hash<sf::Vector2<int> >
{
inline size_t operator()(const sf::Vector2<int>& location) const
{
return location.x * 1812433253 + location.y;
}
};
}
namespace sf
{
template <typename T>
bool operator > (Vector2<T> const& left, Vector2<T> const& right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template <typename T>
bool operator < (Vector2<T> const& left, Vector2<T> const& right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
template <typename T>
Vector2<T> operator /(Vector2<T> const& left, Vector2<T> const& right)
{
T x = left.x / right.x;
T y = left.y / right.y;
return {x, y};
}
}
template <typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y << ')';
return out;
}
inline double heuristic(sf::Vector2i a, sf::Vector2i b)
{
// auto const& dx = b.x - a.x;//m_location[m_goal].x - m_location[u].x;
// auto const& dy = b.y - a.y;;
// return ::sqrt(dx * dx + dy * dy);
// return std::max(std::abs(a.x - b.x), std::abs(a.y - b.y));
static auto const D = 1;
auto const& dx = abs(a.x - b.x);
auto const& dy = abs(a.y - b.y);
return D * (dx + dy);
//return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}
//{
// assert(std::isless(0, epsilon)); // epsilon is a part of the whole quantity
// assert(std::isless(epsilon, 1));
//
// const auto delta = std::abs(a - b);
// const auto x = std::abs(a);
// const auto y = std::abs(b);
//
// // comparable generally and |a - b| < eps * (|a| + |b|) / 2
// return std::isless(epsilon * y, x)
// && std::isless(epsilon * x, y)
// && std::isless((delta + delta) / (x + y), epsilon);
//};
template <typename N>
class Graph
{
class Node;
using NodeHolder = std::set<Node>;
public:
using NodeVal = N;
using NodeRef = typename NodeHolder::iterator;
using Edges = std::vector<std::pair<NodeRef, double>>;
private:
class Node
{
N data;
mutable Edges outedge;
public:
Node(N const& data)
: data(data)
{}
void addEdge(NodeRef e, double cost) const
{
outedge.emplace_back(e, cost);
}
NodeVal const& value() const
{
return data;
}
Edges const& getEdges() const
{
return outedge;
}
friend bool operator<(Node const& lhs, Node const& rhs)
{
return lhs.data < rhs.data;
}
};
NodeHolder nodes;
public:
NodeRef addNode(N const& data)
{
auto const& result = nodes.emplace(data);
return result.first;
}
NodeRef getRef(N const& data)
{
auto const& found = nodes.find(data);
if(found == nodes.end())
throw std::runtime_error("Graph::getRef: Key not found!\n");
return found;
}
void addEdge(NodeRef src, NodeRef dst, double cost)
{
if (src != nodes.end() && dst != nodes.end()) {
src->addEdge(dst, cost);
}
}
Edges const& getEdges(N const& node) const
{
auto const& found = nodes.find(node);
if(found == nodes.end())
throw std::runtime_error("Graph::getEdges: node not found!\n");
return found;
}
};
template <typename Graph>
struct Base
{
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
struct QueInfo: public std::tuple<NodeRef, double, std::vector<NodeRef>>
{
QueInfo(QueInfo const&) = default;
QueInfo(NodeRef const& data, double cost, std::vector<NodeRef> const& route)
: std::tuple<NodeRef, double, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator<(QueInfo const& lhs, QueInfo const& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
Graph const& graph;
sf::RenderWindow& window;
std::vector<sf::RectangleShape>& grid;
int width;
Base(Graph const& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: graph(graph), window(window), grid(grid), width(width)
{}
// debug drawing
void paint(sf::Vector2i coord, sf::Color color)
{
grid[coord.x + width * coord.y].setFillColor(color);
}
void draw()
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for(const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
};
template <typename Graph>
class Dijkstra : Base<Graph>
{
using NodeRef = typename Base<Graph>::NodeRef;
using NodeVal = typename Base<Graph>::NodeVal;
public:
Dijkstra(Graph const& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: Base<Graph>(graph, window,grid, width)
{}
std::vector<NodeRef> route(NodeRef const& src, NodeRef const& dst)
{
std::set<NodeVal> found;
std::priority_queue<typename Base<Graph>::QueInfo> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while(!frontier.empty())
{
sf::sleep(sf::seconds(0.0125f));
auto next = frontier.top();
frontier.pop();
auto const& current = std::get<0>(next);
if (found.find(current->value()) != found.end()) continue;
if(current->value() != src->value() && current->value() != dst->value())
Base<Graph>::paint(current->value(),{102,205,170});
found.emplace(current->value());
auto const& result = std::get<2>(next);
if (current == dst) return result;
for(auto const& loop: current->getEdges())
{
if(loop.first->value() != src->value() && loop.first->value() != dst->value())
Base<Graph>::paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second, result);
}
Base<Graph>::draw();
}
return {};
}
};
template <typename Graph>
class Astar : Base<Graph>
{
using NodeRef = typename Base<Graph>::NodeRef;
using NodeVal = typename Base<Graph>::NodeVal;
public:
Astar(Graph const& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: Base<Graph>(graph, window,grid, width)
{}
std::vector<NodeRef> route(NodeRef const& src, NodeRef const& dst)
{
std::set<NodeVal> found;
std::priority_queue<typename Base<Graph>::QueInfo> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while(!frontier.empty())
{
//sf::sleep(sf::seconds(0.0125f));
auto next = frontier.top();
frontier.pop();
auto const& current = std::get<0>(next);
auto const& value = current->value();
if (found.find(value) != found.end()) continue;
if(value != src->value() && value != dst->value()) Base<Graph>::paint(value,{102,205,170});
found.emplace(value);
auto const& result = std::get<2>(next);
if (current == dst) return result;
// for(auto const& loop: current->getEdges())
// {
// auto const& value = std::get<0>(loop)->value();
// //if(value != start && value != goal) paint(value,{127,255,212});
// auto const& oldCost = std::get<1>(next);
// auto const& newCost = std::get<1>(loop);
// //std::cout << oldCost << ' ' << newCost << ' ' << heuristic(value, goal) <<'\n'; getchar();
// if(heuristic(std::get<0>(next)->value(), goal) > heuristic(value, goal))
// {
// if(value != start && value != goal) paint(value,{127,255,212});
// auto const& totalCost = std::get<1>(next) + std::get<1>(loop);// + heuristic(value, goal);
// frontier.emplace(std::get<0>(loop), totalCost, result);
// }
// }
for(auto const& loop: current->getEdges())
{
// dx1 = current.x - goal.x
// dy1 = current.y - goal.y
// dx2 = start.x - goal.x
// dy2 = start.y - goal.y
// cross = abs(dx1*dy2 - dx2*dy1)
// heuristic += cross*0.001
auto const& value = loop.first->value();
auto const& dx1 = value.x - dst->value().x;
auto const& dy1 = value.y - dst->value().y;
auto const& dx2 = src->value().x - dst->value().x;
auto const& dy2 = src->value().y - dst->value().y;
auto const& cross = abs(dx1*dy2 - dx2*dy1);
auto heuristics = heuristic(value, dst->value());
heuristics += cross*0.001;
//std::cout << heuristics << '\n';
//if(value != start && value != goal) paint(value,{127,255,212});
//auto const& oldCost = std::get<1>(next);
//auto const& newCost = std::get<1>(loop);
//std::cout << oldCost << ' ' << newCost << ' ' << heuristic(value, goal) <<'\n'; getchar();
//if(heuristic(std::get<0>(next)->value(), goal) > heuristic(loop.first->value(), goal))
{
if(value != src->value() && value != dst->value()) Base<Graph>::paint(value,{127,255,212});
//auto const& totalCost = std::get<1>(next) + loop.second;// + heuristic(value, goal);
frontier.emplace(std::get<0>(loop), loop.second + heuristics, result);
}
}
Base<Graph>::draw();
}
return {};
}
};
class Grid : public sf::Drawable
{
public:
Grid(int width_,
int height_,
int block_size,
sf::Vector2i start,
sf::Vector2i goal,
sf::RenderWindow& window)
: width(width_), height(height_), grid(width_ * height_)
, start(start), goal(goal), graph()
, dijkstra(graph, window, grid, width)
, astar(graph, window, grid, width)
{
auto tile_size = sf::Vector2f(block_size, block_size);
int i=0;
for(auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
//tile.setFillColor(sf::Color::White);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
graph.addNode(sf::Vector2i(position));
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
for(auto& tile : grid)
{
auto p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size) ;
for (auto next : neighbors(p))
graph.addEdge(graph.getRef(p), graph.getRef(next), 1);
}
// auto const& result = dijkstra.route(graph.getRef(start), graph.getRef(goal));
//
// std::transform(std::begin(result), std::end(result),
// std::back_inserter(path), [](auto d){return d->value();});
auto const& result = astar.route(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](auto d){return d->value();});
vbegin = path.begin();
}
void setPoint(sf::Vector2i s, sf::Vector2i g)
{
path.clear();
start = s;
goal = g;
reset();
auto const& result = astar.route(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](auto d){return d->value();});
vbegin = path.begin();
}
void reset()
{
for (auto& tile : grid)
{
tile.setFillColor(sf::Color::White);
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
}
//void setPath()
bool updatePath(sf::Time dt)
{
//if (timer <= sf::Time::Zero) {
timer += sf::seconds(0.125f);//sf::seconds(0.25f);
if(vbegin == path.end()) {timer = sf::Time::Zero; return false;}
if ( *vbegin != start && *vbegin != goal )
grid[vbegin->x + width * vbegin->y].setFillColor(sf::Color::Yellow);
vbegin++;
//}else timer -= dt;
return true;
}
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
for(const auto& tile : grid)
{
target.draw(tile);
}
}
private:
inline bool in_bounds(sf::Vector2i id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
std::vector<sf::Vector2i> neighbors(sf::Vector2i id) const
{
static const std::array<sf::Vector2i, 4> Directions{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
// {-1, 1},
// { 1, -1},
// {-1, -1},
// { 1, 1},
}};
std::vector<sf::Vector2i> results;
for (auto direction : Directions)
{
const auto& next(id + direction);
if (in_bounds(next))
{
results.emplace_back(next);
}
}
return results;
}
private:
int width, height;
std::vector<sf::RectangleShape> grid;
std::vector<sf::Vector2i> path;
// for animation
std::vector<sf::Vector2i>::iterator vbegin;
sf::Time timer = sf::Time::Zero;
sf::Vector2i start;
sf::Vector2i goal;
// Algorithm's engine
Graph<sf::Vector2i> graph;
Dijkstra<Graph<sf::Vector2i>> dijkstra;
Astar<Graph<sf::Vector2i>> astar;
};
int main()
{
try{
unsigned width = 45;//10;
unsigned height = 30;//10;
unsigned block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "algorithm");
//Grid grid(width, height, block_size, {1, 4}, {8, 5}, window);
Grid grid(width, height, block_size, {5, 20}, {40, 5}, window);
sf::Clock clock;
const sf::Time TimePerFrame = sf::seconds(1.f / 60.f);
sf::Time timeSinceLastUpdate = sf::Time::Zero;
while(window.isOpen())
{
sf::Time dt = clock.restart();
timeSinceLastUpdate += dt;
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
else if(event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::P)
grid.setPoint({10, 25},{35, 10});
}
while (timeSinceLastUpdate > TimePerFrame)
{
timeSinceLastUpdate -= TimePerFrame;
grid.updatePath(TimePerFrame);
}
window.clear();
window.draw(grid);
window.display();
}
} catch(std::runtime_error& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
} catch(...)
{
std::cout << "Unknown Exception!\n";
return 1;
}
}
Raw
test05.cpp
#include <SFML/Graphics.hpp>
#include <iostream>
#include <iomanip>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <set>
#include <functional>
#include <algorithm>
namespace sf
{
template <typename T>
bool operator > (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template <typename T>
bool operator < (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
template <typename T>
Vector2<T> operator / (const Vector2<T>& left, const Vector2<T>& right)
{
T x = left.x / right.x;
T y = left.y / right.y;
return {x, y};
}
}
template <typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y << ')';
return out;
}
inline double heuristic(const sf::Vector2i& a, const sf::Vector2i b)
{
//static const auto D = 1;
const auto& dx = std::abs(a.x - b.x);
const auto& dy = std::abs(a.y - b.y);
return /*D * */ (dx + dy);
}
template <typename N>
class Graph
{
class Node;
using NodeHolder = std::set<Node>;
public:
using NodeVal = N;
using NodeRef = typename NodeHolder::iterator;
using Edges = std::vector<std::pair<NodeRef, double>>;
public:
NodeRef addNode(const N& data)
{
auto const& result = nodes.emplace(data);
return result.first;
}
NodeRef getRef(const N& data)
{
const auto& found = nodes.find(data);
if(found == nodes.end())
throw std::runtime_error("Graph::getRef: Key not found!\n");
return found;
}
void addEdge(const NodeRef src, const NodeRef dst, double cost)
{
if (src != nodes.end() && dst != nodes.end()) {
src->addEdge(dst, cost);
}
}
const Edges& getEdges(const N& node) const
{
const auto& found = nodes.find(node);
if(found == nodes.end())
throw std::runtime_error("Graph::getEdges: node not found!\n");
return found;
}
private:
class Node
{
N data;
mutable Edges outedge;
public:
Node(const N& data) : data(data) {}
void addEdge(const NodeRef e, double cost) const
{
outedge.emplace_back(e, cost);
}
const NodeVal& value() const
{
return data;
}
const Edges& getEdges() const
{
return outedge;
}
friend bool operator < (Node const& lhs, Node const& rhs)
{
return lhs.data < rhs.data;
}
};
private:
NodeHolder nodes;
};
template <typename Graph>
class PathFinder
{
public:
PathFinder(const Graph& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: graph(graph), window(window), grid(grid), width(width)
{}
protected:
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
struct QueInfo : public std::tuple<NodeRef, double, std::vector<NodeRef>>
{
QueInfo(const QueInfo&) = default;
QueInfo(const NodeRef& data, double cost, const std::vector<NodeRef>& route)
: std::tuple<NodeRef, double, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator<(const QueInfo& lhs, const QueInfo& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
// debug drawing
void paint(const sf::Vector2i& coord, const sf::Color& color)
{
grid[coord.x + width * coord.y].setFillColor(color);
}
void draw()
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for(const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
private:
const Graph& graph;
sf::RenderWindow& window;
std::vector<sf::RectangleShape>& grid;
const int width;
};
template <typename Graph>
class Dijkstra : public PathFinder<Graph>
{
using NodeRef = typename PathFinder<Graph>::NodeRef;
using NodeVal = typename PathFinder<Graph>::NodeVal;
using QueInfo = typename PathFinder<Graph>::QueInfo;
public:
Dijkstra(const Graph& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: PathFinder<Graph>(graph, window,grid, width)
{}
std::vector<NodeRef> route(const NodeRef& src, const NodeRef& dst) // const
{
std::set<NodeVal> found;
std::priority_queue<QueInfo> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst)
PathFinder<Graph>::paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
if( loop.first != src && loop.first != dst)
PathFinder<Graph>::paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second, result);
}
PathFinder<Graph>::draw();
}
return {};
}
};
template <typename Graph>
class Astar : public PathFinder<Graph>
{
using NodeRef = typename PathFinder<Graph>::NodeRef;
using NodeVal = typename PathFinder<Graph>::NodeVal;
using QueInfo = typename PathFinder<Graph>::QueInfo;
public:
Astar(const Graph& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: PathFinder<Graph>(graph, window,grid, width)
{}
std::vector<NodeRef> route(const NodeRef& src, const NodeRef& dst) //const
{
std::set<NodeVal> found;
std::priority_queue<QueInfo> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
const auto& start = src->value();
const auto& goal = dst->value();
while(!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if(current != src && current != dst)
PathFinder<Graph>::paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for(const auto& loop: current->getEdges())
{
const auto& value = loop.first->value();
const auto& d1 = value - goal;
const auto& d2 = start - goal;
const auto& cross = std::abs(d1.x*d2.y - d2.x*d1.y);
auto heuristics = heuristic(value, goal);
heuristics += cross*0.001;
if(loop.first != src && loop.first != dst)
PathFinder<Graph>::paint(value,{127,255,212});
frontier.emplace(loop.first, loop.second + heuristics, result);
}
PathFinder<Graph>::draw();
}
return {};
}
};
class Grid : public sf::Drawable
{
public:
Grid(int width,
int height,
int block_size,
sf::Vector2i start,
sf::Vector2i goal,
sf::RenderWindow& window)
: width(width), height(height), grid(width * height)
, start(start), goal(goal), graph()
, dijkstra(graph, window, grid, width)
, astar(graph, window, grid, width)
{
const auto& tile_size = sf::Vector2f(block_size, block_size);
int i=0;
for(auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
graph.addNode(sf::Vector2i(position));
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
for(auto& tile : grid)
{
const auto& p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size) ;
for (const auto& next : neighbors(p))
graph.addEdge(graph.getRef(p), graph.getRef(next), 1);
}
// auto const& result = dijkstra.route(graph.getRef(start), graph.getRef(goal));
//
// std::transform(std::begin(result), std::end(result),
// std::back_inserter(path), [](auto d){return d->value();});
auto const& result = astar.route(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](auto d){return d->value();});
vbegin = path.begin();
}
void setPoints(const sf::Vector2i& s, const sf::Vector2i& g)
{
path.clear();
start = s;
goal = g;
reset();
const auto& result = astar.route(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](auto d){return d->value();});
vbegin = path.begin();
}
void reset()
{
for (auto& tile : grid)
{
tile.setFillColor(sf::Color::White);
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
}
void updatePath()
{
if(vbegin == path.end()) return;
if ( *vbegin != start && *vbegin != goal )
grid[vbegin->x + width * vbegin->y].setFillColor(sf::Color::Yellow);
vbegin++;
}
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
for(const auto& tile : grid)
{
target.draw(tile);
}
}
private:
inline bool in_bounds(const sf::Vector2i& id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
std::vector<sf::Vector2i> neighbors(const sf::Vector2i& id) const
{
static const std::array<sf::Vector2i, 4> Directions{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
// {-1, 1},
// { 1, -1},
// {-1, -1},
// { 1, 1},
}};
std::vector<sf::Vector2i> results;
for (auto direction : Directions)
{
const auto& next(id + direction);
if (in_bounds(next))
{
results.emplace_back(next);
}
}
return results;
}
private:
int width, height;
std::vector<sf::RectangleShape> grid;
std::vector<sf::Vector2i> path;
// for animation
std::vector<sf::Vector2i>::iterator vbegin;
sf::Time timer = sf::Time::Zero;
sf::Vector2i start;
sf::Vector2i goal;
// Algorithm's engine
Graph<sf::Vector2i> graph;
Dijkstra<Graph<sf::Vector2i>> dijkstra;
Astar<Graph<sf::Vector2i>> astar;
};
int main()
{
try{
unsigned width = 45;//10;
unsigned height = 30;//10;
unsigned block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "algorithm");
//Grid grid(width, height, block_size, {1, 4}, {8, 5}, window);
Grid grid(width, height, block_size, {5, 20}, {40, 5}, window);
while(window.isOpen())
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
else if(event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::P)
grid.setPoints({10, 25},{35, 10});
}
grid.updatePath();
window.clear();
window.draw(grid);
window.display();
}
} catch(std::runtime_error& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
} catch(...)
{
std::cout << "Unknown Exception!\n";
return 1;
}
}
Raw
test06.cpp
#include <SFML/Graphics.hpp>
#include <iostream>
#include <iomanip>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <set>
#include <algorithm>
namespace sf
{
template <typename T>
bool operator > (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template <typename T>
bool operator < (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
template <typename T>
Vector2<T> operator / (const Vector2<T>& left, const Vector2<T>& right)
{
T x = left.x / right.x;
T y = left.y / right.y;
return {x, y};
}
}
template <typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y << ')';
return out;
}
inline double heuristic(const sf::Vector2i& a, const sf::Vector2i b)
{
//static const auto D = 1;
const auto& dx = std::abs(a.x - b.x);
const auto& dy = std::abs(a.y - b.y);
return /*D * */ (dx + dy);
}
template <typename N>
class Graph
{
class Node;
using NodeHolder = std::set<Node>;
public:
using NodeVal = N;
using NodeRef = typename NodeHolder::iterator;
using Edges = std::vector<std::pair<NodeRef, double>>;
public:
NodeRef addNode(const N& data)
{
const auto& result = nodes.emplace(data);
return result.first;
}
NodeRef getRef(const N& data)
{
const auto& found = nodes.find(data);
if (found == nodes.end())
throw std::runtime_error("Graph::getRef: Key not found!\n");
return found;
}
void addEdge(const NodeRef src, const NodeRef dst, double cost)
{
if (src != nodes.end() && dst != nodes.end())
src->addEdge(dst, cost);
}
private:
class Node
{
N data;
mutable Edges outedge;
public:
Node(const N& data) : data(data) {}
void addEdge(const NodeRef e, double cost) const
{
outedge.emplace_back(e, cost);
}
const NodeVal& value() const
{
return data;
}
const Edges& getEdges() const
{
return outedge;
}
friend bool operator < (Node const& lhs, Node const& rhs)
{
return lhs.data < rhs.data;
}
};
private:
NodeHolder nodes;
};
template <typename Graph>
class PathFinder
{
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
struct QueueHelper : public std::tuple<NodeRef, double, std::vector<NodeRef>>
{
QueueHelper(const QueueHelper&) = default;
QueueHelper(const NodeRef& data, double cost, const std::vector<NodeRef>& route)
: std::tuple<NodeRef, double, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator < (const QueueHelper& lhs, const QueueHelper& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
public:
PathFinder(const Graph& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: graph(graph), window(window), grid(grid), width(width)
{}
std::vector<NodeRef> routeAstar(const NodeRef& src, const NodeRef& dst) //const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
const auto& start = src->value();
const auto& goal = dst->value();
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
const auto& value = loop.first->value();
const auto& d1 = value - goal;
const auto& d2 = start - goal;
const auto& cross = std::abs(d1.x*d2.y - d2.x*d1.y);
auto heuristics = heuristic(value, goal);
heuristics += cross*0.001;
if(loop.first != src && loop.first != dst)
paint(value,{127,255,212});
frontier.emplace(loop.first, loop.second + heuristics, result);
}
draw();
}
return {};
}
std::vector<NodeRef> routeDijkstra(const NodeRef& src, const NodeRef& dst) // const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
if( loop.first != src && loop.first != dst)
paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second, result);
}
draw();
}
return {};
}
private:
// debug drawing
void paint(const sf::Vector2i& coord, const sf::Color& color)
{
grid[coord.x + width * coord.y].setFillColor(color);
}
void draw()
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for (const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
private:
const Graph& graph;
sf::RenderWindow& window;
std::vector<sf::RectangleShape>& grid;
const int width;
};
class Grid : public sf::Drawable
{
public:
Grid(int width,
int height,
int block_size,
sf::Vector2i start,
sf::Vector2i goal,
sf::RenderWindow& window)
: width(width), height(height), grid(width * height)
, start(start), goal(goal), graph()
, pathFinder(graph, window, grid, width)
{
const auto& tile_size = sf::Vector2f(block_size, block_size);
int i=0;
for (auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
graph.addNode(sf::Vector2i(position));
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
for (auto& tile : grid)
{
const auto& p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size) ;
for (const auto& next : neighbors(p))
graph.addEdge(graph.getRef(p), graph.getRef(next), 1);
}
const auto& result = pathFinder.routeDijkstra(graph.getRef(start), graph.getRef(goal));
// const auto& result = pathFinder.routeAstar(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](const auto& d){return d->value();});
begin = path.begin();
}
void setPoints(const sf::Vector2i& s, const sf::Vector2i& g)
{
path.clear();
start = s;
goal = g;
reset();
const auto& result = pathFinder.routeAstar(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](const auto& d){return d->value();});
begin = path.begin();
}
void reset()
{
for (auto& tile : grid)
{
tile.setFillColor(sf::Color::White);
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
}
void updatePath()
{
if (begin == path.end()) return;
if ( *begin != start && *begin != goal )
grid[begin->x + width * begin->y].setFillColor(sf::Color::Yellow);
begin++;
}
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
for (const auto& tile : grid)
{
target.draw(tile);
}
}
private:
inline bool in_bounds(const sf::Vector2i& id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
std::vector<sf::Vector2i> neighbors(const sf::Vector2i& id) const
{
static const std::array<sf::Vector2i, 4> Directions{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
// {-1, 1},
// { 1, -1},
// {-1, -1},
// { 1, 1},
}};
std::vector<sf::Vector2i> results;
for (auto direction : Directions)
{
const auto& next(id + direction);
if (in_bounds(next))
{
results.emplace_back(next);
}
}
return results;
}
private:
const int width, height;
std::vector<sf::RectangleShape> grid;
std::vector<sf::Vector2i> path;
// for animation
std::vector<sf::Vector2i>::iterator begin;
sf::Time timer = sf::Time::Zero;
sf::Vector2i start;
sf::Vector2i goal;
// Algorithm's engine
Graph<sf::Vector2i> graph;
PathFinder<Graph<sf::Vector2i>> pathFinder;
};
int main()
{
try{
unsigned width = 45;
unsigned height = 30;
unsigned block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "algorithm");
Grid grid(width, height, block_size, {5, 20}, {40, 5}, window);
while (window.isOpen())
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
else if (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::P)
grid.setPoints({10, 25},{35, 10});
}
grid.updatePath();
window.clear();
window.draw(grid);
window.display();
}
} catch (std::runtime_error& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
} catch (...)
{
std::cout << "Unknown Exception!\n";
return 1;
}
}
Raw
test07.cpp
#include <SFML/Graphics.hpp>
#include <iostream>
#include <iomanip>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <set>
#include <algorithm>
#include <chrono>
#include <thread>
#include <future>
namespace sf
{
template <typename T>
bool operator > (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template <typename T>
bool operator < (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
template <typename T>
Vector2<T> operator / (const Vector2<T>& left, const Vector2<T>& right)
{
T x = left.x / right.x;
T y = left.y / right.y;
return {x, y};
}
//template <typename T>
bool operator > (const Color& left, const Color& right)
{
return std::tie(left.r, left.g, left.b) > std::tie(right.r, right.g, right.b);
}
//template <typename T>
bool operator < (const Color& left, const Color& right)
{
return std::tie(left.r, left.g, left.b) < std::tie(right.r, right.g, right.b);
}
}
template <typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y << ')';
return out;
}
inline double heuristic(const sf::Vector2i& a, const sf::Vector2i b)
{
//static const auto D = 1;
const auto& dx = std::abs(a.x - b.x);
const auto& dy = std::abs(a.y - b.y);
return /*D * */ (dx + dy);
}
template <typename N>
class Graph
{
class Node;
using NodeHolder = std::set<Node>;
public:
using NodeVal = N;
using NodeRef = typename NodeHolder::iterator;
using Edges = std::vector<std::pair<NodeRef, double>>;
public:
NodeRef addNode(const N& data)
{
const auto& result = nodes.emplace(data);
return result.first;
}
NodeRef getRef(const N& data)
{
const auto& found = nodes.find(data);
if (found == nodes.end())
throw std::runtime_error("Graph::getRef: Key not found!\n");
return found;
}
void addEdge(const NodeRef src, const NodeRef dst, double cost)
{
if (src != nodes.end() && dst != nodes.end())
src->addEdge(dst, cost);
}
void clear()
{
nodes.clear();
}
private:
class Node
{
N data;
mutable Edges outedge;
public:
Node(const N& data) : data(data) {}
void addEdge(const NodeRef e, double cost) const
{
outedge.emplace_back(e, cost);
}
const NodeVal& value() const
{
return data;
}
const Edges& getEdges() const
{
return outedge;
}
friend bool operator < (Node const& lhs, Node const& rhs)
{
return lhs.data < rhs.data;
}
};
private:
NodeHolder nodes;
};
template <typename Graph>
class PathFinder
{
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
struct QueueHelper : public std::tuple<NodeRef, double, std::vector<NodeRef>>
{
QueueHelper(const QueueHelper&) = default;
QueueHelper(const NodeRef& data, double cost, const std::vector<NodeRef>& route)
: std::tuple<NodeRef, double, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator < (const QueueHelper& lhs, const QueueHelper& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
public:
PathFinder(const Graph& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: graph(graph), window(window), grid(grid), width(width)
{}
std::vector<NodeRef> routeAstar(const NodeRef& src, const NodeRef& dst) //const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
const auto& start = src->value();
const auto& goal = dst->value();
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
const auto& value = loop.first->value();
const auto& d1 = value - goal;
const auto& d2 = start - goal;
const auto& cross = std::abs(d1.x*d2.y - d2.x*d1.y);
auto heuristics = heuristic(value, goal);
heuristics += cross*0.001;
if(loop.first != src && loop.first != dst)
paint(value,{127,255,212});
frontier.emplace(loop.first, loop.second + heuristics, result);
}
draw();
}
return {};
}
std::vector<NodeRef> routeDijkstra(const NodeRef& src, const NodeRef& dst) // const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
if( loop.first != src && loop.first != dst)
paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second, result);
}
draw();
}
return {};
}
private:
// debug drawing
void paint(const sf::Vector2i& coord, const sf::Color& color)
{
grid[coord.x + width * coord.y].setFillColor(color);
}
void draw()
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for (const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
private:
const Graph& graph;
sf::RenderWindow& window;
std::vector<sf::RectangleShape>& grid;
const int width;
};
class Grid : public sf::Drawable
{
public:
enum Algorithm
{
None = 0,
Dijkstra = 1 << 0,
Astar = 1 << 1,
};
public:
Grid(int width,
int height,
int block_size,
sf::Vector2i start,
sf::Vector2i goal,
sf::RenderWindow& window)
: width(width), height(height), tile_size(block_size, block_size), grid(width * height)
, start(start), goal(goal), algorithm(Algorithm::Dijkstra), graph()
, pathFinder(graph, window, grid, width)
{
int i=0;
for (auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
begin = path.end();
}
void setAlgorithmType(Algorithm a)
{
algorithm = a;
reset();
std::vector<Graph<sf::Vector2i>::NodeRef> result;
if (algorithm & Algorithm::Dijkstra)
result = pathFinder.routeDijkstra(graph.getRef(start), graph.getRef(goal));
else
result = pathFinder.routeAstar(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](const auto& d){return d->value();});
begin = path.begin();
}
void setPoints(const sf::Vector2i& s, const sf::Vector2i& g)
{
start = s;
goal = g;
reset();
}
void updatePath()
{
if (begin == path.end()) return;
if ( *begin != start && *begin != goal )
grid[begin->x + width * begin->y].setFillColor(sf::Color::Yellow);
begin++;
}
void addWall(const sf::Vector2i& position)
{
if(position == start || position == goal) return;
auto& tile = grid[position.x + width * position.y];
if (passable(position))tile.setFillColor(Gray);
else tile.setFillColor(sf::Color::White);
}
void movePoints(const sf::Vector2i& position)
{
//if(position != start && position != goal) return;
//if(position == start)
auto& tile = grid[position.x + width * position.y];
tile.setFillColor(sf::Color::Red);
}
private:
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
for (const auto& tile : grid)
{
target.draw(tile);
}
}
void reset()
{
path.clear();
begin = path.end();
for (auto& tile : grid)
{
if (tile.getFillColor() == Gray) continue;
tile.setFillColor(sf::Color::White);
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
graph.clear();
for (const auto& tile : grid)
{
const auto& p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size);
if (!passable(p)) continue;
graph.addNode(p);
}
for (const auto& tile : grid)
{
const auto& p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size);
if (!passable(p)) continue;
for (const auto& next : neighbors(p))
graph.addEdge(graph.getRef(p), graph.getRef(next), 1);
}
}
inline bool in_bounds(const sf::Vector2i& id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
inline bool passable(const sf::Vector2i& position) const
{
auto& tile = grid[position.x + width * position.y];
return tile.getFillColor() != Gray;
}
std::vector<sf::Vector2i> neighbors(const sf::Vector2i& id) const
{
static const std::array<sf::Vector2i, 4> Directions{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
}};
std::vector<sf::Vector2i> results;
for (auto direction : Directions)
{
const auto& next(id + direction);
if (in_bounds(next) && passable(next))
{
results.emplace_back(next);
}
}
return results;
}
private:
static const sf::Color Gray;
const int width, height;
sf::Vector2f tile_size;
std::vector<sf::RectangleShape> grid;
std::vector<sf::Vector2i> path;
// for animation
std::vector<sf::Vector2i>::iterator begin;
sf::Vector2i start;
sf::Vector2i goal;
// Algorithm's engine
Algorithm algorithm;
Graph<sf::Vector2i> graph;
PathFinder<Graph<sf::Vector2i>> pathFinder;
};
const sf::Color Grid::Gray = sf::Color{139,137,137};
int main()
{
try{
unsigned width = 45;
unsigned height = 30;
int block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "Path Finders - Algorithms");
window.setFramerateLimit(60);
Grid grid(width, height, block_size, {5, 20}, {40, 5}, window);
unsigned algorithm = Grid::Dijkstra;
const unsigned toggle = (Grid::Dijkstra ^ Grid::Astar);
unsigned onlyOnce = 1;
enum Mode
{
None = 0, // alway get back to edit
Start = 1 << 0,
Restart = 1 << 1,
ClearPath = 1 << 2,
ClearWall = 1 << 3,
};
unsigned mode = None;
while (window.isOpen())
{
sf::Event event;
while (window.pollEvent(event))
{
switch (event.type){
case sf::Event::Closed:window.close(); break;
case sf::Event::KeyPressed:
if (event.key.code == sf::Keyboard::Escape)
window.close();
else if (event.key.code == sf::Keyboard::P)
{
grid.setPoints({10, 25},{35, 10});
}
else if (event.key.code == sf::Keyboard::A)
{
onlyOnce ^= 1;
}
else if (event.key.code == sf::Keyboard::S)
{
mode = Start;
}
break;
case sf::Event::MouseButtonPressed:
{
const auto& position = sf::Mouse::getPosition(window);
auto coord = static_cast<sf::Vector2i>(window.mapPixelToCoords(position));
coord /= block_size;
grid.addWall(coord);
}
break;
case sf::Event::MouseButtonReleased:
break;
default:break;
};
}
// TODO: need algorithm https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
// if (sf::Mouse::isButtonPressed(sf::Mouse::Left))
// {
// sf::Vector2i position = sf::Mouse::getPosition(window);
// sf::Vector2i coord = static_cast<sf::Vector2i>(window.mapPixelToCoords(position));
// coord /= block_size;
// grid.movePoints(coord);
// grid.addWall(coord);
// }
if(mode & Mode::Start){
if ((algorithm & Grid::Dijkstra) && onlyOnce)
{
algorithm ^= toggle;
grid.setAlgorithmType(Grid::Dijkstra);
}
else if((algorithm & Grid::Astar) && !onlyOnce)
{
algorithm ^= toggle;
grid.setAlgorithmType(Grid::Astar);
}
//mode = None;
}
grid.updatePath();
window.clear();
window.draw(grid);
window.display();
}
} catch (std::runtime_error& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
} catch (...)
{
std::cout << "Unknown Exception!\n";
return 1;
}
}
Raw
test08.cpp
#include <SFML/Graphics.hpp>
#include <iostream>
#include <iomanip>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <set>
#include <algorithm>
namespace sf
{
template <typename T>
bool operator > (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template <typename T>
bool operator < (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
template <typename T>
Vector2<T> operator / (const Vector2<T>& left, const Vector2<T>& right)
{
if (right.x == 0 || right.y == 0 ) return left;
T x = left.x / right.x;
T y = left.y / right.y;
return {x, y};
}
}
template <typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y << ')';
return out;
}
inline double heuristic(const sf::Vector2i& a, const sf::Vector2i b)
{
//static const auto D = 1;
const auto& dx = std::abs(a.x - b.x);
const auto& dy = std::abs(a.y - b.y);
return /*D * */ (dx + dy);
}
template <typename N>
class Graph
{
class Node;
using NodeHolder = std::set<Node>;
public:
using NodeVal = N;
using NodeRef = typename NodeHolder::iterator;
using Edges = std::vector<std::pair<NodeRef, double>>;
public:
NodeRef addNode(const N& data)
{
const auto& result = nodes.emplace(data);
return result.first;
}
NodeRef getRef(const N& data)
{
const auto& found = nodes.find(data);
if (found == nodes.end())
throw std::runtime_error("Graph::getRef: Key not found!\n");
return found;
}
void addEdge(const NodeRef src, const NodeRef dst, double cost)
{
if (src != nodes.end() && dst != nodes.end())
src->addEdge(dst, cost);
}
void clear()
{
nodes.clear();
}
private:
class Node
{
N data;
mutable Edges outedge;
public:
Node(const N& data) : data(data) {}
void addEdge(const NodeRef e, double cost) const
{
outedge.emplace_back(e, cost);
}
const NodeVal& value() const
{
return data;
}
const Edges& getEdges() const
{
return outedge;
}
friend bool operator < (Node const& lhs, Node const& rhs)
{
return lhs.data < rhs.data;
}
};
private:
NodeHolder nodes;
};
template <typename Graph>
class PathFinder
{
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
struct QueueHelper : public std::tuple<NodeRef, double, std::vector<NodeRef>>
{
QueueHelper(const QueueHelper&) = default;
QueueHelper(const NodeRef& data, double cost, const std::vector<NodeRef>& route)
: std::tuple<NodeRef, double, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator < (const QueueHelper& lhs, const QueueHelper& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
public:
PathFinder(const Graph& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: graph(graph), window(window), grid(grid), width(width)
{}
std::vector<NodeRef> routeAstar(const NodeRef& src, const NodeRef& dst) //const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
const auto& start = src->value();
const auto& goal = dst->value();
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
const auto& value = loop.first->value();
const auto& d1 = value - goal;
const auto& d2 = start - goal;
const auto& cross = std::abs(d1.x*d2.y - d2.x*d1.y);
auto heuristics = heuristic(value, goal);
heuristics += cross*0.001;
if(loop.first != src && loop.first != dst)
paint(value,{127,255,212});
frontier.emplace(loop.first, loop.second + heuristics, result);
}
draw();
}
return {};
}
std::vector<NodeRef> routeDijkstra(const NodeRef& src, const NodeRef& dst) // const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
if( loop.first != src && loop.first != dst)
paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second, result);
}
draw();
}
return {};
}
private:
// debug drawing
void paint(const sf::Vector2i& coord, const sf::Color& color)
{
grid[coord.x + width * coord.y].setFillColor(color);
}
void draw()
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for (const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
private:
const Graph& graph;
sf::RenderWindow& window;
std::vector<sf::RectangleShape>& grid;
const int width;
};
class Grid : public sf::Drawable
{
public:
enum Algorithm
{
None = 0,
Dijkstra = 1 << 0,
Astar = 1 << 1,
};
public:
Grid(int width,
int height,
int block_size,
sf::Vector2i start,
sf::Vector2i goal,
sf::RenderWindow& window)
: width(width), height(height), tile_size(block_size, block_size), grid(width * height)
, start(start), goal(goal), algorithm(Algorithm::Dijkstra), graph()
, pathFinder(graph, window, grid, width)
{
int i=0;
for (auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
begin = path.end();
}
void setAlgorithmType(Algorithm a)
{
algorithm = a;
reset();
std::vector<Graph<sf::Vector2i>::NodeRef> result;
if (algorithm & Algorithm::Dijkstra)
result = pathFinder.routeDijkstra(graph.getRef(start), graph.getRef(goal));
else
result = pathFinder.routeAstar(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](const auto& d){return d->value();});
begin = path.begin();
}
void setPoints(const sf::Vector2i& s, const sf::Vector2i& g)
{
start = s;
goal = g;
reset();
}
void updatePath()
{
if (begin == path.end()) return;
if ( *begin != start && *begin != goal )
grid[begin->x + width * begin->y].setFillColor(sf::Color::Yellow);
begin++;
}
void addWall(const sf::Vector2i& position)
{
if(position == start || position == goal || !in_bounds(position)) return;
auto& tile = grid[position.x + width * position.y];
if (passable(position))tile.setFillColor(Gray);
else tile.setFillColor(sf::Color::White);
}
void movePoints(const sf::Vector2i& position)
{
//if(position != start && position != goal) return;
//if(position == start)
auto& tile = grid[position.x + width * position.y];
tile.setFillColor(sf::Color::Red);
}
private:
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
for (const auto& tile : grid)
{
target.draw(tile);
}
}
void reset()
{
path.clear();
begin = path.end();
for (auto& tile : grid)
{
if (tile.getFillColor() == Gray) continue;
tile.setFillColor(sf::Color::White);
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
graph.clear();
for (const auto& tile : grid)
{
const auto& p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size);
if (!passable(p)) continue;
graph.addNode(p);
}
for (const auto& tile : grid)
{
const auto& p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size);
if (!passable(p)) continue;
for (const auto& next : neighbors(p))
graph.addEdge(graph.getRef(p), graph.getRef(next), 1);
}
}
inline bool in_bounds(const sf::Vector2i& id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
inline bool passable(const sf::Vector2i& position) const
{
auto& tile = grid[position.x + width * position.y];
return tile.getFillColor() != Gray;
}
std::vector<sf::Vector2i> neighbors(const sf::Vector2i& id) const
{
static const std::array<sf::Vector2i, 4> Directions{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
}};
std::vector<sf::Vector2i> results;
for (auto direction : Directions)
{
const auto& next(id + direction);
if (in_bounds(next) && passable(next))
{
results.emplace_back(next);
}
}
return results;
}
private:
static const sf::Color Gray;
const int width, height;
const sf::Vector2f tile_size;
std::vector<sf::RectangleShape> grid;
std::vector<sf::Vector2i> path;
// for animation
std::vector<sf::Vector2i>::iterator begin;
sf::Vector2i start;
sf::Vector2i goal;
// Algorithm's engine
Algorithm algorithm;
Graph<sf::Vector2i> graph;
PathFinder<Graph<sf::Vector2i>> pathFinder;
};
const sf::Color Grid::Gray = sf::Color{139,137,137};
int main()
{
try{
unsigned width = 45;
unsigned height = 30;
int block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "Path Finders - Algorithms");
window.setFramerateLimit(60);
Grid grid(width, height, block_size, {5, 20}, {40, 5}, window);
unsigned algorithm = Grid::Dijkstra;
const unsigned toggle = (Grid::Dijkstra ^ Grid::Astar);
unsigned onlyOnce = 1;
enum State
{
None = 0, // always get back to edit state
Start = 1 << 0,
Restart = 1 << 1,
ClearPath = 1 << 2,
ClearWall = 1 << 3,
};
unsigned state = None;
bool mouse = false;
std::set<sf::Vector2i> positions;
while (window.isOpen())
{
sf::Event event;
while (window.pollEvent(event))
{
switch (event.type){
case sf::Event::Closed:window.close(); break;
case sf::Event::KeyPressed:
if (event.key.code == sf::Keyboard::Escape)
window.close();
else if (event.key.code == sf::Keyboard::P)
{
grid.setPoints({10, 25},{35, 10});
}
else if (event.key.code == sf::Keyboard::A)
{
onlyOnce ^= 1;
}
else if (event.key.code == sf::Keyboard::S)
{
state = Start;
}
break;
case sf::Event::MouseButtonPressed:
{
//if(!mouse){
const auto& position = sf::Mouse::getPosition(window);
auto coord = static_cast<sf::Vector2i>(window.mapPixelToCoords(position));
coord /= block_size;
grid.addWall(coord);
mouse = !mouse;
//}
}
break;
case sf::Event::MouseButtonReleased:
if (event.mouseButton.button == sf::Mouse::Left)
{
positions.clear();
mouse = !mouse;
}
break;
case sf::Event::MouseMoved:
{
if(!mouse) break;
const auto& position = sf::Mouse::getPosition(window);
auto coord = static_cast<sf::Vector2i>(window.mapPixelToCoords(position));
coord /= block_size;
positions.emplace(coord);
if(positions.size() > 1)
{
grid.addWall(*std::next(positions.begin()));
positions.clear();
}
}
break;
default:break;
};
}
if(state & State::Start)
{
if ((algorithm & Grid::Dijkstra) && onlyOnce)
{
algorithm ^= toggle;
grid.setAlgorithmType(Grid::Dijkstra);
}
else if((algorithm & Grid::Astar) && !onlyOnce)
{
algorithm ^= toggle;
grid.setAlgorithmType(Grid::Astar);
}
//state = None;
}
grid.updatePath();
window.clear();
window.draw(grid);
window.display();
}
} catch (std::runtime_error& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
} catch (...)
{
std::cout << "Unknown Exception!\n";
return 1;
}
}
Raw
test09.cpp
#include <SFML/Graphics.hpp>
#include <iostream>
#include <iomanip>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <set>
#include <algorithm>
#include <random>
namespace sf
{
template <typename T>
bool operator > (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template <typename T>
bool operator < (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
template <typename T>
Vector2<T> operator / (const Vector2<T>& left, const Vector2<T>& right)
{
if (right.x == 0 || right.y == 0 ) return left;
T x = left.x / right.x;
T y = left.y / right.y;
return {x, y};
}
}
//std::mt19937 engine{ std::random_device()() };
template <typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y << ')';
return out;
}
inline double heuristic(const sf::Vector2i& a, const sf::Vector2i b)
{
//static const auto D = 1;
const auto& dx = std::abs(a.x - b.x);
const auto& dy = std::abs(a.y - b.y);
return /*D * */ (dx + dy);
}
template <typename N>
class Graph
{
class Node;
using NodeHolder = std::set<Node>;
public:
using NodeVal = N;
using NodeRef = typename NodeHolder::iterator;
using Edges = std::vector<std::pair<NodeRef, double>>;
public:
NodeRef addNode(const N& data)
{
const auto& result = nodes.emplace(data);
return result.first;
}
NodeRef getRef(const N& data)
{
const auto& found = nodes.find(data);
if (found == nodes.end())
throw std::runtime_error("Graph::getRef: Key not found!\n");
return found;
}
void addEdge(const NodeRef src, const NodeRef dst, double cost)
{
if (src != nodes.end() && dst != nodes.end())
src->addEdge(dst, cost);
}
void clear()
{
nodes.clear();
}
private:
class Node
{
N data;
mutable Edges outedge;
public:
Node(const N& data) : data(data) {}
void addEdge(const NodeRef e, double cost) const
{
outedge.emplace_back(e, cost);
}
const NodeVal& value() const
{
return data;
}
const Edges& getEdges() const
{
return outedge;
}
friend bool operator < (Node const& lhs, Node const& rhs)
{
return lhs.data < rhs.data;
}
};
private:
NodeHolder nodes;
};
template <typename Graph>
class PathFinder
{
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
struct QueueHelper : public std::tuple<NodeRef, double, std::vector<NodeRef>>
{
QueueHelper(const QueueHelper&) = default;
QueueHelper(const NodeRef& data, double cost, const std::vector<NodeRef>& route)
: std::tuple<NodeRef, double, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator < (const QueueHelper& lhs, const QueueHelper& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
public:
PathFinder(const Graph& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: graph(graph), window(window), grid(grid), width(width)
{}
std::vector<NodeRef> routeAstar(const NodeRef& src, const NodeRef& dst) //const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
const auto& start = src->value();
const auto& goal = dst->value();
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
const auto& value = loop.first->value();
const auto& d1 = value - goal;
const auto& d2 = start - goal;
const auto& cross = std::abs(d1.x*d2.y - d2.x*d1.y);
auto heuristics = heuristic(value, goal);
heuristics += cross*0.001;
if(loop.first != src && loop.first != dst)
paint(value,{127,255,212});
frontier.emplace(loop.first, loop.second + heuristics, result);
}
draw();
}
return {};
}
std::vector<NodeRef> routeDijkstra(const NodeRef& src, const NodeRef& dst) // const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
if( loop.first != src && loop.first != dst)
paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second, result);
}
draw();
}
return {};
}
private:
// debug drawing
void paint(const sf::Vector2i& coord, const sf::Color& color)
{
grid[coord.x + width * coord.y].setFillColor(color);
}
void draw()
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for (const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
private:
const Graph& graph;
sf::RenderWindow& window;
std::vector<sf::RectangleShape>& grid;
const int width;
};
class Grid : public sf::Drawable
{
public:
enum Algorithm
{
None = 0,
Dijkstra = 1 << 0,
Astar = 1 << 1,
};
public:
Grid(int width,
int height,
int block_size,
sf::RenderWindow& window)
: width(width), height(height), tile_size(block_size, block_size), grid(width * height)
, start({}), goal({}), algorithm(Algorithm::Dijkstra), graph()
, pathFinder(graph, window, grid, width)
{
int i=0;
for (auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
}
std::random_device rd;
std::mt19937 gen(rd());
auto distH = std::uniform_int_distribution<>(1,height/2);
auto distW = std::uniform_int_distribution<>(width/4,width*2);
start.x = distW(gen)%width; start.y = distH(gen)%height;
goal.x = distW(gen)%width; goal.y = distH(gen)%height;
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
begin = path.end();
}
void setAlgorithmType(Algorithm a)
{
algorithm = a;
reset();
std::vector<Graph<sf::Vector2i>::NodeRef> result;
if (algorithm & Algorithm::Dijkstra)
result = pathFinder.routeDijkstra(graph.getRef(start), graph.getRef(goal));
else
result = pathFinder.routeAstar(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](const auto& d){return d->value();});
begin = path.begin();
}
void setPoints(const sf::Vector2i& s, const sf::Vector2i& g)
{
start = s;
goal = g;
reset();
}
void updatePath()
{
if (begin == path.end()) return;
if ( *begin != start && *begin != goal )
grid[begin->x + width * begin->y].setFillColor(sf::Color::Yellow);
begin++;
}
void movePoints(const sf::Vector2i& position)
{
if(!in_bounds(position) || !passable(position)) return;
//if(position == start)
auto& tile = grid[position.x + width * position.y];
//tile.setFillColor(sf::Color::White);
if (passable(position)) tile.setFillColor(sf::Color::Blue);
//else tile.setFillColor(sf::Color::White);
}
// mouse input
void addWall(const sf::Vector2i& position)
{
if(position == start || position == goal || !in_bounds(position)) return;
grid[position.x + width * position.y].setFillColor(Gray);
}
void removeWall(const sf::Vector2i& position)
{
if(position == start || position == goal || !in_bounds(position)) return;
grid[position.x + width * position.y].setFillColor(sf::Color::White);
}
// keyboards input
void clearPath()
{
path.clear();
begin = path.end();
for (auto& tile : grid) // keep wall
{
if (tile.getFillColor() == Gray) continue;
tile.setFillColor(sf::Color::White);
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
}
void clearWall()
{
for (auto& tile : grid)
{
if (tile.getFillColor() == Gray)
tile.setFillColor(sf::Color::White);
}
}
private:
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
for (const auto& tile : grid)
{
target.draw(tile);
}
}
void reset()
{
clearPath();
graph.clear();
for (const auto& tile : grid)
{
const auto& p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size);
if (!passable(p)) continue;
graph.addNode(p);
}
for (const auto& tile : grid)
{
const auto& p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size);
if (!passable(p)) continue;
for (const auto& next : neighbors(p))
graph.addEdge(graph.getRef(p), graph.getRef(next), 1);
}
}
inline bool in_bounds(const sf::Vector2i& id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
inline bool passable(const sf::Vector2i& position) const
{
auto& tile = grid[position.x + width * position.y];
return tile.getFillColor() != Gray;
}
std::vector<sf::Vector2i> neighbors(const sf::Vector2i& id) const
{
static const std::array<sf::Vector2i, 4> Directions{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
}};
std::vector<sf::Vector2i> results;
for (auto direction : Directions)
{
const auto& next(id + direction);
if (in_bounds(next) && passable(next))
{
results.emplace_back(next);
}
}
return results;
}
private:
static const sf::Color Gray;
const int width, height;
const sf::Vector2f tile_size;
std::vector<sf::RectangleShape> grid;
std::vector<sf::Vector2i> path;
// for animation
std::vector<sf::Vector2i>::iterator begin;
sf::Vector2i start;
sf::Vector2i goal;
// Algorithm's engine
Algorithm algorithm;
Graph<sf::Vector2i> graph;
PathFinder<Graph<sf::Vector2i>> pathFinder;
};
const sf::Color Grid::Gray = sf::Color{139,137,137};
int main()
{
try{
unsigned width = 45;
unsigned height = 30;
int block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "Path Finders - Algorithms");
window.setFramerateLimit(60);
Grid grid(width, height, block_size, window);
unsigned algorithm = Grid::Dijkstra;
const unsigned toggle = (Grid::Dijkstra ^ Grid::Astar);
enum State
{
None = 0, // always get back to edit state
Start = 1 << 0,
Restart = 1 << 1,
};
enum Input
{
AddWall = 1 << 0,
RemoveWall = 1 << 1,
MoveStart = 1 << 2,
MoveGoal = 1 << 3,
};
unsigned input = None;
unsigned state = None;
std::set<sf::Vector2i> positions;
while (window.isOpen())
{
sf::Event event;
while (window.pollEvent(event))
{
const auto& position = sf::Mouse::getPosition(window);
auto coord = static_cast<sf::Vector2i>(window.mapPixelToCoords(position));
coord /= block_size;
switch (event.type)
{
case sf::Event::Closed:window.close(); break;
case sf::Event::KeyPressed:
if (event.key.code == sf::Keyboard::Escape){
window.close();
}
else if (event.key.code == sf::Keyboard::P){
grid.clearPath();
}
else if (event.key.code == sf::Keyboard::R){
state = Restart;
}
else if (event.key.code == sf::Keyboard::S){
state = Start;
}
else if (event.key.code == sf::Keyboard::W){
grid.clearWall();
}
else if (event.key.code == sf::Keyboard::T){
algorithm ^= toggle;
}
break;
case sf::Event::MouseButtonPressed:
if (event.mouseButton.button == sf::Mouse::Left)
{
input = AddWall | MoveStart | MoveGoal;
if ((input & AddWall) && (input & (RemoveWall | MoveStart | MoveGoal))){
grid.addWall(coord);
}
if ((input & MoveStart) && (input & (AddWall | RemoveWall | MoveGoal))){
}
if ((input & MoveGoal) && (input & (AddWall | RemoveWall | MoveStart))){
}
}
if (event.mouseButton.button == sf::Mouse::Right)
{
input = RemoveWall;
if ((input & RemoveWall) && !(input & (AddWall | MoveStart | MoveGoal)))
grid.removeWall(coord);
}
break;
case sf::Event::MouseButtonReleased:
{
positions.clear();
input &= ~(AddWall | RemoveWall | MoveStart | MoveGoal);
}
break;
case sf::Event::MouseMoved:
if ((input & AddWall) && (input & (RemoveWall | MoveStart | MoveGoal))){
grid.addWall(coord);
}
if ((input & RemoveWall) && !(input & (AddWall | MoveStart | MoveGoal))){
grid.removeWall(coord);
}
if ((input & MoveStart) && (input & (AddWall | RemoveWall | MoveGoal))){
}
if ((input & MoveGoal) && (input & (AddWall | RemoveWall | MoveStart))){
}
break;
default:break;
};
}
if ((algorithm & Grid::Dijkstra) && (state & (Start | Restart))){
state &= ~(Start | Restart);
grid.setAlgorithmType(Grid::Dijkstra);
}
else if((algorithm & Grid::Astar) && (state & (Start | Restart))){
state &= ~(Start | Restart);
grid.setAlgorithmType(Grid::Astar);
}
grid.updatePath();
window.clear();
window.draw(grid);
window.display();
}
} catch (std::runtime_error& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
} catch (...)
{
std::cout << "Unknown Exception!\n";
return 1;
}
}
Raw
test10.cpp
#include <SFML/Graphics.hpp>
#include <iostream>
#include <iomanip>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <set>
#include <algorithm>
#include <random>
namespace sf
{
template <typename T>
bool operator > (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) > std::tie(right.x, right.y);
}
template <typename T>
bool operator < (const Vector2<T>& left, const Vector2<T>& right)
{
return std::tie(left.x, left.y) < std::tie(right.x, right.y);
}
template <typename T>
Vector2<T> operator / (const Vector2<T>& left, const Vector2<T>& right)
{
if (right.x == 0 || right.y == 0 ) return left;
T x = left.x / right.x;
T y = left.y / right.y;
return {x, y};
}
}
template <typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y << ')';
return out;
}
inline double heuristic(const sf::Vector2i& a, const sf::Vector2i b)
{
//static const auto D = 1;
const auto& dx = std::abs(a.x - b.x);
const auto& dy = std::abs(a.y - b.y);
return /*D * */ (dx + dy);
}
template <typename N>
class Graph
{
class Node;
using NodeHolder = std::set<Node>;
public:
using NodeVal = N;
using NodeRef = typename NodeHolder::iterator;
using Edges = std::vector<std::pair<NodeRef, double>>;
public:
NodeRef addNode(const N& data)
{
const auto& result = nodes.emplace(data);
return result.first;
}
NodeRef getRef(const N& data)
{
const auto& found = nodes.find(data);
if (found == nodes.end())
throw std::runtime_error("Graph::getRef: Key not found!\n");
return found;
}
void addEdge(const NodeRef src, const NodeRef dst, double cost)
{
if (src != nodes.end() && dst != nodes.end())
src->addEdge(dst, cost);
}
void clear()
{
nodes.clear();
}
private:
class Node
{
N data;
mutable Edges outedge;
public:
Node(const N& data) : data(data) {}
void addEdge(const NodeRef e, double cost) const
{
outedge.emplace_back(e, cost);
}
const NodeVal& value() const
{
return data;
}
const Edges& getEdges() const
{
return outedge;
}
friend bool operator < (Node const& lhs, Node const& rhs)
{
return lhs.data < rhs.data;
}
};
private:
NodeHolder nodes;
};
template <typename Graph>
class PathFinder
{
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
struct QueueHelper : public std::tuple<NodeRef, double, std::vector<NodeRef>>
{
QueueHelper(const QueueHelper&) = default;
QueueHelper(const NodeRef& data, double cost, const std::vector<NodeRef>& route)
: std::tuple<NodeRef, double, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator < (const QueueHelper& lhs, const QueueHelper& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
public:
PathFinder(const Graph& graph,
sf::RenderWindow& window,
std::vector<sf::RectangleShape>& grid,
int width)
: graph(graph), window(window), grid(grid), width(width)
{}
std::vector<NodeRef> routeAstar(const NodeRef& src, const NodeRef& dst) //const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
const auto& start = src->value();
const auto& goal = dst->value();
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
const auto& value = loop.first->value();
const auto& d1 = value - goal;
const auto& d2 = start - goal;
const auto& cross = std::abs(d1.x*d2.y - d2.x*d1.y);
auto heuristics = heuristic(value, goal);
heuristics += cross*0.001;
if(loop.first != src && loop.first != dst)
paint(value,{127,255,212});
frontier.emplace(loop.first, loop.second + heuristics, result);
}
draw();
}
return {};
}
std::vector<NodeRef> routeDijkstra(const NodeRef& src, const NodeRef& dst) // const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
if( loop.first != src && loop.first != dst)
paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second, result);
}
draw();
}
return {};
}
private:
// debug drawing
void paint(const sf::Vector2i& coord, const sf::Color& color)
{
grid[coord.x + width * coord.y].setFillColor(color);
}
void draw()
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
for (const auto& tile : grid)
{
window.draw(tile);
}
window.display();
}
private:
const Graph& graph;
sf::RenderWindow& window;
std::vector<sf::RectangleShape>& grid;
const int width;
};
class Grid : public sf::Drawable
{
public:
enum Algorithm
{
None = 0,
Dijkstra = 1 << 0,
Astar = 1 << 1,
};
public:
Grid(int width, int height, int block_size, sf::RenderWindow& window)
: width(width), height(height), tile_size(block_size, block_size)
, grid(width * height), start({}), goal({}), needUpdate(false)
, algorithm(Algorithm::Dijkstra), graph()
, pathFinder(graph, window, grid, width)
{
int i=0;
for (auto& tile : grid)
{
sf::Vector2f position(i % width, i / width); i++;
tile.setSize(tile_size);
tile.setOutlineColor(sf::Color(205,201,201));
tile.setOutlineThickness(-1);
tile.setPosition(position.x * tile_size.x, position.y * tile_size.y );
}
std::random_device rd;
std::mt19937 gen(rd());
auto distH = std::uniform_int_distribution<>(1,height/2);
auto distW = std::uniform_int_distribution<>(width/4,width*2);
start.x = distW(gen)%width; start.y = distH(gen)%height;
goal.x = distW(gen)%width; goal.y = distH(gen)%height;
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
begin = path.end();
}
void setAlgorithmType(Algorithm a)
{
algorithm = a;
reset();
std::vector<Graph<sf::Vector2i>::NodeRef> result;
if (algorithm & Algorithm::Dijkstra)
result = pathFinder.routeDijkstra(graph.getRef(start), graph.getRef(goal));
else
result = pathFinder.routeAstar(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](const auto& d){return d->value();});
begin = path.begin();
}
void update()
{
if(needUpdate){
for (auto& tile : grid) // keep wall
{
if (tile.getFillColor() == Gray) continue;
tile.setFillColor(sf::Color::White);
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
needUpdate = false;
}
if (begin == path.end()) return;
if ( *begin != start && *begin != goal )
grid[begin->x + width * begin->y].setFillColor(sf::Color::Yellow);
begin++;
}
// mouse input
bool moveStart(const sf::Vector2i& position)
{
if(!in_bounds(position) || !passable(position)|| position == goal) return false;
start = position;
needUpdate = true;
return needUpdate;
}
bool moveGoal(const sf::Vector2i& position)
{
if(!in_bounds(position) || !passable(position) || position == start) return false;
goal = position;
needUpdate = true;
return needUpdate;
}
bool addWall(const sf::Vector2i& position)
{
if(position == start || position == goal || !in_bounds(position)) return false;
grid[position.x + width * position.y].setFillColor(Gray);
return true;
}
void removeWall(const sf::Vector2i& position)
{
if(position == start || position == goal || !in_bounds(position)) return;
grid[position.x + width * position.y].setFillColor(sf::Color::White);
}
// keyboards input
void clearPath()
{
path.clear();
begin = path.end();
for (auto& tile : grid) // keep wall
{
if (tile.getFillColor() == Gray) continue;
tile.setFillColor(sf::Color::White);
}
grid[start.x + width * start.y].setFillColor(sf::Color::Green);
grid[goal.x + width * goal.y].setFillColor(sf::Color::Red);
}
void clearWall()
{
for (auto& tile : grid)
{
if (tile.getFillColor() == Gray)
tile.setFillColor(sf::Color::White);
}
}
private:
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
for (const auto& tile : grid)
{
target.draw(tile);
}
}
void reset()
{
clearPath();
graph.clear();
for (const auto& tile : grid)
{
const auto& p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size);
if (!passable(p)) continue;
graph.addNode(p);
}
for (const auto& tile : grid)
{
const auto& p = static_cast<sf::Vector2i>(tile.getPosition() / tile_size);
if (!passable(p)) continue;
for (const auto& next : neighbors(p))
graph.addEdge(graph.getRef(p), graph.getRef(next), 1);
}
}
inline bool in_bounds(const sf::Vector2i& id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
inline bool passable(const sf::Vector2i& position) const
{
auto& tile = grid[position.x + width * position.y];
return tile.getFillColor() != Gray;
}
std::vector<sf::Vector2i> neighbors(const sf::Vector2i& id) const
{
static const std::array<sf::Vector2i, 4> Directions{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
}};
std::vector<sf::Vector2i> results;
for (auto direction : Directions)
{
const auto& next(id + direction);
if (in_bounds(next) && passable(next))
{
results.emplace_back(next);
}
}
return results;
}
private:
static const sf::Color Gray;
const int width, height;
const sf::Vector2f tile_size;
std::vector<sf::RectangleShape> grid;
std::vector<sf::Vector2i> path;
// for animation
std::vector<sf::Vector2i>::iterator begin;
sf::Vector2i start;
sf::Vector2i goal;
bool needUpdate;
// Algorithm's engine
Algorithm algorithm;
Graph<sf::Vector2i> graph;
PathFinder<Graph<sf::Vector2i>> pathFinder;
};
const sf::Color Grid::Gray = sf::Color{139,137,137};
int main()
{
try{
unsigned width = 45;
unsigned height = 30;
int block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "Path Finders - Algorithms");
window.setFramerateLimit(60);
Grid grid(width, height, block_size, window);
unsigned algorithm = Grid::Dijkstra;
const unsigned toggle = (Grid::Dijkstra ^ Grid::Astar);
enum State
{
None = 0, // always get back to edit state
Start = 1 << 0,
Restart = 1 << 1,
};
enum Input
{
AddWall = 1 << 0,
RemoveWall = 1 << 1,
MoveStart = 1 << 2,
MoveGoal = 1 << 3,
};
unsigned input = None;
unsigned state = None;
while (window.isOpen())
{
sf::Event event;
while (window.pollEvent(event))
{
const auto& position = sf::Mouse::getPosition(window);
auto coord = static_cast<sf::Vector2i>(window.mapPixelToCoords(position));
coord /= block_size;
switch (event.type)
{
case sf::Event::Closed:window.close(); break;
case sf::Event::KeyPressed:
if (event.key.code == sf::Keyboard::Escape){
window.close();
}
else if (event.key.code == sf::Keyboard::P){
grid.clearPath();
}
else if (event.key.code == sf::Keyboard::R){
state = Restart;
}
else if (event.key.code == sf::Keyboard::S){
state = Start;
}
else if (event.key.code == sf::Keyboard::W){
grid.clearWall();
}
else if (event.key.code == sf::Keyboard::T){
algorithm ^= toggle;
}
break;
case sf::Event::MouseButtonPressed:
if (event.mouseButton.button == sf::Mouse::Left)
{
input = AddWall | MoveStart | MoveGoal;
if (input & AddWall){
if(grid.addWall(coord)){
input = AddWall;
}
}
if (input & MoveStart){
if(grid.moveStart(coord)){
input = MoveStart;
}
}
if (input & MoveGoal){
if(grid.moveGoal(coord)){
input = MoveGoal;
}
}
}
if (event.mouseButton.button == sf::Mouse::Right)
{
input = RemoveWall;
if (input & RemoveWall){
grid.removeWall(coord);
}
}
break;
case sf::Event::MouseButtonReleased:
{
input &= ~(AddWall | RemoveWall | MoveStart | MoveGoal);
}
break;
case sf::Event::MouseMoved:
if (input & AddWall){
grid.addWall(coord);
}
else if (input & RemoveWall){
grid.removeWall(coord);
}
else if (input & MoveStart){
grid.moveStart(coord);
}
else if (input & MoveGoal){
grid.moveGoal(coord);
}
break;
default:break;
};
}
if ((algorithm & Grid::Dijkstra) && (state & (Start | Restart))){
state &= ~(Start | Restart);
grid.setAlgorithmType(Grid::Dijkstra);
}
else if((algorithm & Grid::Astar) && (state & (Start | Restart))){
state &= ~(Start | Restart);
grid.setAlgorithmType(Grid::Astar);
}
grid.update();
window.clear();
window.draw(grid);
window.display();
}
} catch (std::runtime_error& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
} catch (...)
{
std::cout << "Unknown Exception!\n";
return 1;
}
}
Raw
test11.cpp
#include <SFML/Graphics.hpp>
#include <iostream>
#include <iomanip>
#include <array>
#include <vector>
#include <utility>
#include <queue>
#include <tuple>
#include <set>
#include <algorithm>
#include <random>
namespace sf
{
template <typename T>
inline bool operator < (const Vector2<T>& left, const Vector2<T>& right)
{
return (left.x < right.x) || ((left.x == right.x) && (left.y < right.y));
}
template <typename T>
inline bool operator > (const Vector2<T>& left, const Vector2<T>& right)
{
return (left.x > right.x) || ((left.x == right.x) && (left.y > right.y));
}
template <typename T>
inline Vector2<T> operator / (const Vector2<T>& left, const Vector2<T>& right)
{
if (right.x == 0 || right.y == 0 ) return left;
T x = left.x / right.x;
T y = left.y / right.y;
return {x, y};
}
}
template <typename T>
std::ostream& operator<<(std::ostream& out, sf::Vector2<T> v)
{
out << '(' << v.x << ',' << v.y <<')';
return out;
}
std::ostream& operator<<(std::ostream& out, sf::Color c)
{
out << '(' << (int)c.r << ',' << (int)c.b << ',' << (int)c.g<< ')';
return out;
}
template <typename T>
std::ostream& operator<<(std::ostream& out, sf::Rect<T> r)
{
out << '(' << r.left << ',' << r.top << ',' << r.width << ',' << r.height <<')';
return out;
}
inline double heuristic(const sf::Vector2i& a, const sf::Vector2i b)
{
//static const auto D = 1;
const auto& dx = std::abs(a.x - b.x);
const auto& dy = std::abs(a.y - b.y);
return /*D * */ (dx + dy);
}
template <typename N>
class Graph
{
class Node;
using NodeHolder = std::set<Node>;
public:
using NodeVal = N;
using NodeRef = typename NodeHolder::iterator;
using Edges = std::vector<std::pair<NodeRef, double>>;
public:
NodeRef addNode(const N& data)
{
const auto& result = nodes.emplace(data);
return result.first;
}
NodeRef getRef(const N& data)
{
const auto& found = nodes.find(data);
if (found == nodes.end())
throw std::runtime_error("Graph::getRef: Key not found!\n");
return found;
}
void addEdge(const NodeRef src, const NodeRef dst, double cost)
{
if (src != nodes.end() && dst != nodes.end())
src->addEdge(dst, cost);
}
void clear()
{
nodes.clear();
}
private:
class Node
{
N data;
mutable Edges outedge;
public:
Node(const N& data) : data(data) {}
void addEdge(const NodeRef e, double cost) const
{
outedge.emplace_back(e, cost);
}
const NodeVal& value() const
{
return data;
}
const Edges& getEdges() const
{
return outedge;
}
friend bool operator < (Node const& lhs, Node const& rhs)
{
return lhs.data < rhs.data;
}
};
private:
NodeHolder nodes;
};
typedef unsigned Vertexv;
class Graphv {
public:
typedef std::pair<Vertexv, Vertexv> Edge;
typedef std::set<Edge> Edges;
void addEdge(Vertexv u, Vertexv v) {
edges_.insert({u, v});
}
const Edges& edges() { return edges_; }
private:
Edges edges_;
};
struct cell
{
explicit cell(const sf::Vector2f& coord,unsigned size, const sf::Color& color)
{
quad[0].position = { coord.x * size, coord.y * size};
quad[1].position = {(coord.x + 1) * size, coord.y * size};
quad[2].position = {(coord.x + 1) * size, (coord.y + 1) * size};
quad[3].position = { coord.x * size, (coord.y + 1) * size};
quad[0].color = quad[1].color = quad[2].color = quad[3].color = color;
}
void setFillColor(const sf::Color& color)
{
quad[0].color = quad[1].color = quad[2].color = quad[3].color = color;
}
sf::Color getFillColor() const
{
return quad[0].color;
}
sf::Vector2i getPosition() const
{
return static_cast<sf::Vector2i>(quad[0].position);
}
friend std::ostream& operator<<(std::ostream& out, cell c)
{
out << c.quad[0].position << ' ' << c.quad[1].position << "\n "
<< c.quad[3].position << ' ' << c.quad[2].position << "\n "
<< c.quad[0].color << ' ' << c.quad[1].color << "\n "
<< c.quad[3].color << ' ' << c.quad[2].color << "\n";
return out;
}
std::array<sf::Vertex, 4> quad;
};
template <typename Graph>
class PathFinder
{
using NodeRef = typename Graph::NodeRef;
using NodeVal = typename Graph::NodeVal;
// TODO: tuple is damn slow, consider re-implement it.
// candidates: 1) create warper class for std::push_heap std::pop_heap
// 2) mimic boost implementation!! (trusted open source treasure) :)
struct QueueHelper : public std::tuple<NodeRef, double, std::vector<NodeRef>>
{
QueueHelper(const QueueHelper&) = default;
QueueHelper(const NodeRef& data, double cost, const std::vector<NodeRef>& route)
: std::tuple<NodeRef, double, std::vector<NodeRef>>(data, cost, route)
{
std::get<2>(*this).emplace_back(data);
}
friend bool operator < (const QueueHelper& lhs, const QueueHelper& rhs)
{
return std::get<1>(lhs) > std::get<1>(rhs);
}
};
public:
PathFinder(const Graph& graph,
sf::RenderWindow& window,
std::vector<cell>& grid,std::vector<sf::Vertex>& outlines,
int width)
: graph(graph), window(window), grid(grid), outlines(outlines),width(width)
{}
// TODO: A* looks like greedy FBS, consider re-implement it.
std::vector<NodeRef> routeAstar(const NodeRef& src, const NodeRef& dst) //const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
const auto& start = src->value();
const auto& goal = dst->value();
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
const auto& value = loop.first->value();
const auto& d1 = value - goal;
const auto& d2 = start - goal;
const auto& cross = std::abs(d1.x*d2.y - d2.x*d1.y);
auto heuristics = heuristic(value, goal);
heuristics += cross*0.001;
if(loop.first != src && loop.first != dst)
paint(value,{127,255,212});
frontier.emplace(loop.first, loop.second + heuristics, result);
}
draw();
}
return {};
}
std::vector<NodeRef> routeDijkstra(const NodeRef& src, const NodeRef& dst) // const
{
std::set<NodeVal> found;
std::priority_queue<QueueHelper> frontier;
frontier.emplace(src, 0, std::vector<NodeRef>{});
while (!frontier.empty())
{
auto next = frontier.top(); frontier.pop();
const auto& current = std::get<0>(next);
const auto& value = current->value();
if (found.find(value) != found.end()) continue;
if (current != src && current != dst) paint(value,{102,205,170});
found.emplace(value);
const auto& result = std::get<2>(next);
if (current == dst) return result;
for (const auto& loop: current->getEdges())
{
if( loop.first != src && loop.first != dst)
paint(loop.first->value(),{127,255,212});
frontier.emplace(loop.first, std::get<1>(next) + loop.second, result);
}
draw();
}
return {};
}
private:
// debug drawing
void paint(const sf::Vector2i& coord, const sf::Color& color)
{
//window.clear();
grid[coord.y + width * coord.x].setFillColor(color);
//window.draw(grid[coord.x + width * coord.y]);
//window.display();
}
void draw()
{
// TODO: 1) add observer pattern to handle input.
// 2) use vertex array to optimize drawing. [x]-done
// NOTE: SFML doesn't support in-place drawing.
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed
|| (event.type == sf::Event::KeyPressed
&& event.key.code == sf::Keyboard::Escape))
window.close();
}
window.clear();
window.draw(grid[0].quad.data(), (sizeof(cell)/sizeof(sf::Vertex))*grid.size(), sf::Quads);
window.draw(outlines.data(), outlines.size(), sf::Lines);
window.display();
}
private:
const Graph& graph;
sf::RenderWindow& window;
std::vector<cell>& grid;
std::vector<sf::Vertex>& outlines;
const int width;
};
std::vector<sf::Vertex>
make_outLines(unsigned width, unsigned height, float size, const sf::Color& color=sf::Color{139,137,137})
{
std::vector<sf::Vertex> vertices((width + height + 2) * 2);
static const auto e = 0.001f;
// make vertical lines
for (unsigned i = 0; i <= width; ++i) {
sf::Vertex* line = &vertices[i * 2];
line[0].position = {((i==0) ? i+e : i) * size, 0};
line[1].position = {((i==0) ? i+e : i) * size, height * size};
line[0].color = line[1].color = color;
}
// make horizontal lines
for (unsigned i = 0; i <= height; ++i) {
sf::Vertex* line = &vertices[((i+1 ) + width) * 2];
line[0].position = {0, ((i==height) ? i-e : i) * size};
line[1].position = {width * size, ((i==height) ? i-e : i) * size};
line[0].color = line[1].color = color;
}
return vertices;
}
class MapEditor final : public sf::Drawable
{
public:
enum Algorithm
{
None = 0,
Dijkstra = 1 << 0,
Astar = 1 << 1,
};
public:
explicit MapEditor(int width, int height, int block_size, sf::RenderWindow& window)
: width(width), height(height), tile_size(block_size, block_size)
, grid(), outlines(make_outLines(width, height, block_size))
, start({}), goal({}), needUpdate(false), algorithm(Algorithm::None), graph()
, pathFinder(graph, window, grid, outlines, height)
{
grid.reserve(width*height);
for (int i = 0; i < width; ++i)
for (int j = 0; j < height; ++j)
{
grid.emplace_back(sf::Vector2f(i, j), block_size, sf::Color::White);
}
std::random_device rd;
std::mt19937 gen(rd());
auto distH = std::uniform_int_distribution<>(1,height/2);
auto distW = std::uniform_int_distribution<>(width/4,width*2);
start.x = distW(gen)%width; start.y = distH(gen)%height;
goal.x = distW(gen)%width; goal.y = distH(gen)%height;
grid[start.y + height * start.x].setFillColor(sf::Color::Green);
grid[goal.y + height * goal.x].setFillColor(sf::Color::Red);
begin = path.end();
}
void setAlgorithmType(Algorithm a)
{
// if ((algorithm & a) && !needUpdate){
// clearPath();
// } else {
// algorithm = a;
// reset();
// }
algorithm = a;
reset();
std::vector<Graph<sf::Vector2i>::NodeRef> result;
if (algorithm & Algorithm::Dijkstra)
result = pathFinder.routeDijkstra(graph.getRef(start), graph.getRef(goal));
else
result = pathFinder.routeAstar(graph.getRef(start), graph.getRef(goal));
std::transform(std::begin(result), std::end(result),
std::back_inserter(path), [](const auto& d){return d->value();});
begin = path.begin();
}
void update()
{
if(needUpdate){
for (auto& tile : grid) // keep wall
{
if (tile.getFillColor() == Gray) continue;
tile.setFillColor(sf::Color::White);
}
grid[start.y + height * start.x].setFillColor(sf::Color::Green);
grid[goal.y + height * goal.x].setFillColor(sf::Color::Red);
needUpdate = false;
}
if (begin == path.end()) return;
if ( *begin != start && *begin != goal )
grid[begin->y + height * begin->x].setFillColor(sf::Color::Yellow);
begin++;
}
// mouse input
bool moveStart(const sf::Vector2i& position)
{
if(!in_bounds(position) || !passable(position)|| position == goal) return false;
start = position;
needUpdate = true;
return needUpdate;
}
bool moveGoal(const sf::Vector2i& position)
{
if(!in_bounds(position) || !passable(position) || position == start) return false;
goal = position;
needUpdate = true;
return needUpdate;
}
bool addWall(const sf::Vector2i& position)
{
if(position == start || position == goal || !in_bounds(position)) return false;
grid[position.y + height * position.x].setFillColor(Gray);
return true;
}
void removeWall(const sf::Vector2i& position)
{
if(position == start || position == goal || !in_bounds(position)) return;
grid[position.y + height * position.x].setFillColor(sf::Color::White);
needUpdate = true;
}
// keyboards input
void clearPath()
{
for (auto& tile : grid) // keep wall
{
if (tile.getFillColor() == Gray) continue;
tile.setFillColor(sf::Color::White);
}
grid[start.y + height * start.x].setFillColor(sf::Color::Green);
grid[goal.y + height * goal.x].setFillColor(sf::Color::Red);
}
void clearWall()
{
for (auto& tile : grid)
{
if (tile.getFillColor() == Gray)
tile.setFillColor(sf::Color::White);
}
needUpdate = true;
}
// void setColor(int x, int y, const sf::Color& color)
// {
// if(0 > x || x >= width || 0 > y || y >= height)
// throw std::out_of_range("Grid::setColor(): parameters out of range");
//
// grid[y + x * height].setFillColor(color);
// }
//
// sf::Color getColor(unsigned x, unsigned y) const
// {
// if(0 > x || x >= width || 0 > y || y >= height)
// throw std::out_of_range("Grid::getColor(): parameters out of range");
//
// return grid[y + x * height].getFillColor();
// }
//
// sf::Vector2i getPosition(unsigned x, unsigned y) const
// {
// return grid[y + x * height].getPosition();
// }
private:
void draw(sf::RenderTarget& target, sf::RenderStates) const
{
target.draw(grid[0].quad.data(), (sizeof(cell)/sizeof(sf::Vertex))*grid.size(), sf::Quads);
target.draw(outlines.data(), outlines.size(), sf::Lines);
}
void reset()
{
clearPath();
path.clear();
begin = path.end();
graph.clear();
for (const auto& tile : grid)
{
const auto& p = tile.getPosition() / tile_size;
if (!passable(p)) continue;
graph.addNode(p);
}
for (const auto& tile : grid)
{
const auto& p = tile.getPosition() / tile_size;
if (!passable(p)) continue;
for (const auto& next : neighbors(p))
graph.addEdge(graph.getRef(p), graph.getRef(next), 1);
}
}
inline bool in_bounds(const sf::Vector2i& id) const
{
return 0 <= id.x && id.x < width && 0 <= id.y && id.y < height;
}
inline bool passable(const sf::Vector2i& position) const
{
const auto& tile = grid[position.y + height * position.x];
return tile.getFillColor() != Gray;
}
std::vector<sf::Vector2i> neighbors(const sf::Vector2i& id) const
{
static const std::array<sf::Vector2i, 4> Directions{{
{ 1, 0},
{ 0, -1},
{-1, 0},
{ 0, 1},
}};
std::vector<sf::Vector2i> results;
for (const auto& direction : Directions)
{
const auto& next(id + direction);
if (in_bounds(next) && passable(next))
{
results.emplace_back(next);
}
}
return results;
}
private:
static const sf::Color Gray;
const int width, height;
const sf::Vector2i tile_size;
std::vector<cell> grid;
std::vector<sf::Vertex> outlines;
std::vector<sf::Vector2i> path;
// for animation
std::vector<sf::Vector2i>::iterator begin;
sf::Vector2i start;
sf::Vector2i goal;
bool needUpdate;
// Algorithm's engine
Algorithm algorithm;
Graph<sf::Vector2i> graph;
PathFinder<Graph<sf::Vector2i>> pathFinder;
};
const sf::Color MapEditor::Gray = sf::Color{139,137,137};
int main()
{
try{
unsigned width = 45;
unsigned height = 30;
int block_size = 20;
sf::RenderWindow window({width*block_size, height*block_size}, "Path Finders - Algorithms");
window.setFramerateLimit(60);
MapEditor editor(width, height, block_size, window);
unsigned algorithm = MapEditor::Dijkstra;
unsigned toggle = (MapEditor::Dijkstra ^ MapEditor::Astar);
enum Input
{
None = 0,
AddWall = 1 << 0,
RemoveWall = 1 << 1,
MoveStart = 1 << 2,
MoveGoal = 1 << 3,
};
unsigned input = None;
while (window.isOpen())
{
sf::Event event;
while (window.pollEvent(event))
{
const auto& position = sf::Mouse::getPosition(window);
auto coord = static_cast<sf::Vector2i>(window.mapPixelToCoords(position));
coord /= block_size;
switch (event.type)
{
case sf::Event::Closed:window.close(); break;
case sf::Event::KeyPressed:
if (event.key.code == sf::Keyboard::Escape){
window.close();
}
else if (event.key.code == sf::Keyboard::P){
editor.clearPath();
}
else if (event.key.code == sf::Keyboard::R){
editor.setAlgorithmType(static_cast<MapEditor::Algorithm>(algorithm));
}
else if (event.key.code == sf::Keyboard::S){
editor.setAlgorithmType(static_cast<MapEditor::Algorithm>(algorithm));
}
else if (event.key.code == sf::Keyboard::W){
editor.clearWall();
}
else if (event.key.code == sf::Keyboard::T){
algorithm ^= toggle;
}
break;
case sf::Event::MouseButtonPressed:
if (event.mouseButton.button == sf::Mouse::Left)
{
input = AddWall | MoveStart | MoveGoal;
if ((input & AddWall) && editor.addWall(coord)){
input = AddWall;
}
if ((input & MoveStart) && editor.moveStart(coord)){
input = MoveStart;
}
if ((input & MoveGoal) && editor.moveGoal(coord)){
input = MoveGoal;
}
}
if (event.mouseButton.button == sf::Mouse::Right)
{
input = RemoveWall;
if (input & RemoveWall){
editor.removeWall(coord);
}
}
break;
case sf::Event::MouseButtonReleased:
{
input &= ~(AddWall | RemoveWall | MoveStart | MoveGoal);
}
break;
case sf::Event::MouseMoved:
if (input & AddWall){
editor.addWall(coord);
}
else if (input & RemoveWall){
editor.removeWall(coord);
}
else if (input & MoveStart){
editor.moveStart(coord);
}
else if (input & MoveGoal){
editor.moveGoal(coord);
}
break;
default:break;
};
}
editor.update();
window.clear();
window.draw(editor);
window.display();
}
} catch (std::runtime_error& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
} catch(std::out_of_range& e)
{
std::cout << "Exception: " << e.what() << '\n';
return 1;
}catch (...)
{
std::cout << "Unknown Exception!\n";
return 1;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment