Commit 51de4a31 authored by louiz’'s avatar louiz’

Simplify and improve the pathfinding

parent 80afc91f
...@@ -27,8 +27,8 @@ Map::~Map() ...@@ -27,8 +27,8 @@ Map::~Map()
std::vector<Layer*>::iterator it; std::vector<Layer*>::iterator it;
for (it = this->layers.begin(); it < this->layers.end(); ++it) for (it = this->layers.begin(); it < this->layers.end(); ++it)
delete *it; delete *it;
if (walking_map) if (this->walking_map)
delete[] walking_map; delete[] this->walking_map;
} }
bool Map::load_from_file(const std::string& map_name) bool Map::load_from_file(const std::string& map_name)
...@@ -130,8 +130,7 @@ bool Map::get_layer_level(boost::property_tree::ptree& tree, unsigned int& level ...@@ -130,8 +130,7 @@ bool Map::get_layer_level(boost::property_tree::ptree& tree, unsigned int& level
log_error("No properties for the layer"); log_error("No properties for the layer");
return false; return false;
} }
BOOST_FOREACH(boost::property_tree::ptree::value_type &property, for (const boost::property_tree::ptree::value_type &property: tree.get_child("properties"))
tree.get_child("properties"))
if ((property.first == "property") && if ((property.first == "property") &&
(property.second.get<std::string>("<xmlattr>.name", "") == "level")) (property.second.get<std::string>("<xmlattr>.name", "") == "level"))
{ {
...@@ -222,10 +221,6 @@ void Map::generate_walking_map() ...@@ -222,10 +221,6 @@ void Map::generate_walking_map()
} }
else else
{ {
if (level != 0)
{
log_debug("max level for cell " << i << "=" << level);
}
const unsigned int tile_value = this->layers[level]->get_cell(i); const unsigned int tile_value = this->layers[level]->get_cell(i);
if (tile_value == 0) if (tile_value == 0)
this->walking_map[i].value = ~0; this->walking_map[i].value = ~0;
...@@ -256,10 +251,6 @@ int Map::get_max_level_for_cell(const unsigned int cell) const ...@@ -256,10 +251,6 @@ int Map::get_max_level_for_cell(const unsigned int cell) const
TileHeights Map::get_cell_heights(const unsigned int col, const unsigned int row) const TileHeights Map::get_cell_heights(const unsigned int col, const unsigned int row) const
{ {
assert(col >= 0);
assert(row >= 0);
assert(col < this->get_width_in_tiles());
assert(row < this->get_height_in_tiles() * 2);
std::size_t index = (this->get_width_in_tiles() * row) + col; std::size_t index = (this->get_width_in_tiles() * row) + col;
return this->walking_map[index]; return this->walking_map[index];
} }
...@@ -303,12 +294,10 @@ CellPath Map::do_astar(const Cell start, const Cell goal) ...@@ -303,12 +294,10 @@ CellPath Map::do_astar(const Cell start, const Cell goal)
{ {
current = open.front(); current = open.front();
if (current.index == end) if (current.index == end)
{ return reconstruct_path(came_from, current.index);
return reconstruct_path(came_from, current.index); open.erase(open.begin());
}
open.pop_front();
closed.push_back(current.index); closed.push_back(current.index);
auto neighbours = this->get_neighbour_cells(current.index); auto neighbours = this->get_neighbour_walkable_cells(current.index);
for (const CellIndex& neighbour: neighbours) for (const CellIndex& neighbour: neighbours)
{ {
if (is_in_set(neighbour, closed) == true) if (is_in_set(neighbour, closed) == true)
...@@ -319,16 +308,12 @@ CellPath Map::do_astar(const Cell start, const Cell goal) ...@@ -319,16 +308,12 @@ CellPath Map::do_astar(const Cell start, const Cell goal)
insert_node(open, neighbour, tentative_g, tentative_g); insert_node(open, neighbour, tentative_g, tentative_g);
came_from[neighbour] = current.index; came_from[neighbour] = current.index;
} }
else
{
log_error("Is not better than the one already opened");
}
} }
} }
return {}; return {};
} }
std::vector<CellIndex> Map::get_neighbour_cells(const CellIndex index) std::vector<CellIndex> Map::get_neighbour_walkable_cells(const CellIndex index)
{ {
const Cell cell = this->index_to_cell(index); const Cell cell = this->index_to_cell(index);
...@@ -338,6 +323,7 @@ std::vector<CellIndex> Map::get_neighbour_cells(const CellIndex index) ...@@ -338,6 +323,7 @@ std::vector<CellIndex> Map::get_neighbour_cells(const CellIndex index)
const unsigned short y = std::get<1>(cell); const unsigned short y = std::get<1>(cell);
std::vector<CellIndex> res; std::vector<CellIndex> res;
res.reserve(4);
if (y != 0 && x != 0) if (y != 0 && x != 0)
{ // Try top left { // Try top left
CellIndex neighbour; CellIndex neighbour;
...@@ -393,6 +379,82 @@ std::vector<CellIndex> Map::get_neighbour_cells(const CellIndex index) ...@@ -393,6 +379,82 @@ std::vector<CellIndex> Map::get_neighbour_cells(const CellIndex index)
return res; return res;
} }
bool Map::is_cell_in_neighbour_lower_cells(const CellIndex index, const CellIndex near)
{
const Cell cell = this->index_to_cell(index);
const TileHeights heights = this->get_cell_heights(index);
const unsigned short x = std::get<0>(cell);
const unsigned short y = std::get<1>(cell);
if (x != this->get_width_in_tiles() - 1 && y != this->get_height_in_tiles() - 1)
{ // Bottom right
CellIndex neighbour;
if (y % 2 == 0)
neighbour = this->cell_to_index(std::make_tuple(x, y + 1));
else
neighbour = this->cell_to_index(std::make_tuple(x + 1, y + 1));
if (neighbour == near)
{
const TileHeights neighbour_heights = this->get_cell_heights(neighbour);
if (heights.corners.right >= neighbour_heights.corners.top &&
heights.corners.bottom >= neighbour_heights.corners.left)
return true;
}
}
if (x != 0 && y != this->get_width_in_tiles() - 1)
{ // Bottom left
CellIndex neighbour;
if (y % 2 == 0)
neighbour = this->cell_to_index(std::make_tuple(x - 1, y + 1));
else
neighbour = this->cell_to_index(std::make_tuple(x, y + 1));
if (neighbour == near)
{
const TileHeights neighbour_heights = this->get_cell_heights(neighbour);
if (heights.corners.left >= neighbour_heights.corners.top &&
heights.corners.bottom >= neighbour_heights.corners.right)
return true;
}
}
if (// res.size() == 2 &&
y != this->get_height_in_tiles() - 2)
{ // Bottom
CellIndex neighbour = this->cell_to_index(std::make_tuple(x, y + 2));
if (neighbour == near)
{
const TileHeights neighbour_heights = this->get_cell_heights(neighbour);
if (heights.corners.bottom >= neighbour_heights.corners.top)
return true;
}
}
if (x != this->get_width_in_tiles() - 1)
{ // Right
CellIndex neighbour = this->cell_to_index(std::make_tuple(x + 1, y));
if (neighbour == near)
{
const TileHeights neighbour_heights = this->get_cell_heights(neighbour);
if (heights.corners.right >= neighbour_heights.corners.left)
return true;
}
}
if (x != 0)
{ // Left
CellIndex neighbour = this->cell_to_index(std::make_tuple(x - 1, y));
if (neighbour == near)
{
const TileHeights neighbour_heights = this->get_cell_heights(neighbour);
if (heights.corners.left >= neighbour_heights.corners.right)
return true;
}
}
return false;
}
int heuristic() int heuristic()
{ {
return 0; return 0;
...@@ -427,13 +489,7 @@ void insert_node(AStarNodes& nodes, CellIndex index, int g, int f) ...@@ -427,13 +489,7 @@ void insert_node(AStarNodes& nodes, CellIndex index, int g, int f)
bool is_in_set(std::size_t index, const std::vector<CellIndex>& nodes) bool is_in_set(std::size_t index, const std::vector<CellIndex>& nodes)
{ {
std::vector<std::size_t>::const_iterator it; return std::find(nodes.begin(), nodes.end(), index) != nodes.end();
for (it = nodes.begin(); it != nodes.end(); ++it)
{
if ((*it) == index)
return true;
}
return false;
} }
bool is_better_than_previously_open(const CellIndex index, const int score, const AStarNodes& open) bool is_better_than_previously_open(const CellIndex index, const int score, const AStarNodes& open)
...@@ -460,8 +516,10 @@ CellPath reconstruct_path(const std::map<std::size_t, std::size_t>& came_from, ...@@ -460,8 +516,10 @@ CellPath reconstruct_path(const std::map<std::size_t, std::size_t>& came_from,
it = came_from.find(end); it = came_from.find(end);
while (it != came_from.end()) while (it != came_from.end())
{ {
res.push_back((*it).second); auto next = came_from.find((*it).second);
it = came_from.find((*it).second); if (next != came_from.end())
res.push_back((*it).second);
it = next;
} }
return res; return res;
} }
......
...@@ -100,9 +100,19 @@ public: ...@@ -100,9 +100,19 @@ public:
* Returns the neighbour cells of the given one if it's walkable to it. * Returns the neighbour cells of the given one if it's walkable to it.
* It's walkable if the two adjacent and corresponding heights are equal. * It's walkable if the two adjacent and corresponding heights are equal.
* If one of them is different, it's not walkable. The return vector can * If one of them is different, it's not walkable. The return vector can
* contain 2, 3 or 4 values. * contain between 0 and 4 values.
*/ */
std::vector<CellIndex> get_neighbour_cells(const CellIndex); std::vector<CellIndex> get_neighbour_walkable_cells(const CellIndex);
/**
* Returns the neighbour cells that are considered to “partially” hide the
* given one.
*
* A neighbour cell “a” can hide a cell “c” if a has a bigger column
* (biggest value on the y axis), and one of its adjacent corner from “a”
* is higher than “c”.
* The vector may contain between 0 and 3 values.
*/
bool is_cell_in_neighbour_lower_cells(const CellIndex, const CellIndex);
/** /**
* Convert a cell from one representation to the other * Convert a cell from one representation to the other
...@@ -156,7 +166,7 @@ struct AStarNode ...@@ -156,7 +166,7 @@ struct AStarNode
int f; int f;
}; };
using AStarNodes = std::list<AStarNode>; using AStarNodes = std::vector<AStarNode>;
/** /**
* Insert a node in the given list, in the correct position as to have the * Insert a node in the given list, in the correct position as to have the
......
...@@ -38,7 +38,7 @@ void Mobility::follow_path(Path& path, World* world, Location* location) ...@@ -38,7 +38,7 @@ void Mobility::follow_path(Path& path, World* world, Location* location)
{ {
// Movement is too short to have something != 0 because of the Fix16 precision. // Movement is too short to have something != 0 because of the Fix16 precision.
// We consider to be at the goal. // We consider to be at the goal.
path.pop_front(); path.erase(path.begin());
return ; return ;
} }
......
...@@ -8,8 +8,8 @@ ...@@ -8,8 +8,8 @@
# define __PATH_HPP__ # define __PATH_HPP__
#include <world/position.hpp> #include <world/position.hpp>
#include <list> #include <vector>
using Path = std::list<Position>; using Path = std::vector<Position>;
#endif // __PATH_HPP__ #endif // __PATH_HPP__
...@@ -107,6 +107,9 @@ Path World::calculate_path(Position endpos, Location* location) ...@@ -107,6 +107,9 @@ Path World::calculate_path(Position endpos, Location* location)
{ {
log_debug("Calculating path for entity. Starting at position: " << location->position() << " ending at pos: " << endpos); log_debug("Calculating path for entity. Starting at position: " << location->position() << " ending at pos: " << endpos);
if (this->can_walk_in_straight_line(location->position(), endpos, location->get_width()) == true)
return {endpos};
CellPath cell_path = this->map.do_astar(this->get_cell_at_position(location->position()), CellPath cell_path = this->map.do_astar(this->get_cell_at_position(location->position()),
this->get_cell_at_position(endpos)); this->get_cell_at_position(endpos));
...@@ -118,9 +121,123 @@ Path World::calculate_path(Position endpos, Location* location) ...@@ -118,9 +121,123 @@ Path World::calculate_path(Position endpos, Location* location)
return {}; return {};
} }
Path World::smooth_path(CellPath path,
Position& start, const Position& end, const Fix16 width) const
{
Path res;
Position current_position = start;
CellPath::reverse_iterator next_cell = path.rbegin();
while (next_cell != path.rend())
{
next_cell = this->get_farthest_walkable_cell(current_position, path, next_cell, width);
current_position = this->cell_center(*next_cell);
next_cell++;
if (next_cell == path.rend())
res.push_back(end);
else
res.push_back(current_position);
}
return res;
}
CellPath::reverse_iterator World::get_farthest_walkable_cell(const Position& pos,
const CellPath& path,
const CellPath::reverse_iterator& current_cell,
const Fix16 width) const
{
auto res = current_cell;
auto it = current_cell + 1;
while (it != path.rend())
{
if (this->can_walk_in_straight_line(pos, this->cell_center(*it), width))
res = it;
it++;
}
return res;
}
Position World::cell_center(const CellIndex index) const
{
Cell cell = this->map.index_to_cell(index);
const auto x = std::get<0>(cell);
const auto y = std::get<1>(cell);
Position res{(x + 1) * CELL_SIZE,
(y + 1) * HALF_CELL_SIZE};
if (y % 2 == 0)
res.x -= HALF_CELL_SIZE;
return res;
}
bool World::can_walk_in_straight_line(const Position& start, const Position& end, const Fix16 width) const
{
static const Fix16 step = 25;
Vec2 forward(end - start);
forward.set_length(width);
Position pstart = start + forward.perpendicular1();
Position pend = end + forward.perpendicular1();
if (this->has_a_line_of_sight(pstart, pend, step) == false)
return false;
pstart = start + forward.perpendicular2();
pend = end + forward.perpendicular2();
if (this->has_a_line_of_sight(pstart, pend, step) == false)
return false;
return true;
}
bool World::has_a_line_of_sight(const Position& start, const Position& end,
const Fix16 step) const
{
// The point to move along the line to check if everything is walkable.
Position pointer(start);
// A vector with a length of 'step' in the direction of the end position
// We use this vector to move the pointer along the line.
Vec2 forward(end - start);
forward.set_length(step);
// the cell position the pointer is at.
unsigned short prevx, prevy;
unsigned short x, y;
std::tie(prevx, prevy) = this->get_cell_at_position(pointer);
// Move the pointer until we reach the destination, or an obstacle.
Fix16 travelled_distance = 0;
const Fix16 distance_to_travel = Position::distance(start, end);
while (travelled_distance < distance_to_travel)
{
// Move the pointer forward.
pointer += forward;
travelled_distance += step;
std::tie(x, y) = this->get_cell_at_position(pointer);
// If the cell changed, we check if we can walk from the previous to
// the current one. If not, it's not walkable and we return false
if ((prevx != x) || (prevy != y))
{
if (this->can_traverse_cell(prevx, prevy, x, y) == false)
// we hit an obstabcle.
return false;
}
prevy = y;
prevx = x;
}
std::tie(x, y) = this->get_cell_at_position(end);
if (((prevx != x) || (prevy != y)) && this->can_traverse_cell(prevx, prevy, x, y) == false)
{
return false;
}
return true;
}
Entity* World::do_new_entity(const EntityType type, const Position& pos, const uint16_t team_value) Entity* World::do_new_entity(const EntityType type, const Position& pos, const uint16_t team_value)
{ {
auto entity = this->entity_factory.make_entity(type); auto entity = this->entity_factory.make_entity(this, type);
Location* location = entity->get<Location>(); Location* location = entity->get<Location>();
assert(location); assert(location);
location->position() = pos; location->position() = pos;
...@@ -341,145 +458,6 @@ Fix16 World::get_position_height(const Position& pos) const ...@@ -341,145 +458,6 @@ Fix16 World::get_position_height(const Position& pos) const
return hx + (hy - hy2); return hx + (hy - hy2);
} }
Path World::smooth_path(CellPath path,
Position& start, const Position& end, const Fix16 width) const
{
Path res;
Position current_pos(start);
Position next_pos;
do
{
next_pos = this->get_next_path_position(path, current_pos, end, width);
res.push_back(next_pos);
current_pos = next_pos;
} while (next_pos != end);
return res;
}
Position World::get_next_path_position(CellPath& path,
const Position& current, const Position& goal,
const Fix16 unit_width) const
{
const Fix16 step = 2;
// First check if the whole path is just a simple straight line.
if (this->can_walk_in_straight_line(current, goal, step, unit_width) == true)
return goal;
Position temp_goal(0, 0);
Position prev_pos(current);
bool first = true;
while (true)
{
auto rit = path.rbegin();
CellIndex cell = *rit;
for (const auto& c: path)
log_debug(this->map.index_to_cell(c));
if (path.empty())
return goal;
if (path.size() == 1)
temp_goal = goal;
else
{
CellIndex next_cell = *(rit + 1);
temp_goal = this->get_nearest_corner(current, cell, next_cell, unit_width);
}
if (!first && this->can_walk_in_straight_line(current, temp_goal,
step, unit_width) == false)
return prev_pos;
prev_pos = temp_goal;
path.pop_back();
first = false;
}
}
bool World::can_walk_in_straight_line(const Position& start, const Position& end, const Fix16 step, const Fix16 width) const
{
Vec2 forward(end - start);
// TODO, use the real width value
forward.set_length(width);
Position start1 = start + forward.perpendicular1();
Position end1 = end + forward.perpendicular1();
Position start2 = start + forward.perpendicular2();
Position end2 = end + forward.perpendicular2();
log_error("can_walk_in_straight_line : start = " << start << ", end=" << end << " width=" << width);
log_error("From " << start1 << " to " << end1 << ". And from " << start2 << " to " << end2);
if (this->has_a_line_of_sight(start1, end1, step) == false)
{
log_debug("no1");
return false;
}
if (this->has_a_line_of_sight(start2, end2, step) == false)
{
log_debug("no2");
return false;
}
// if (this->has_a_line_of_sight(start, end, step) == false)
// {
// log_debug("no");
// return false;
// }
log_debug("yes");
return true;
}
bool World::has_a_line_of_sight(const Position& start, const Position& end,
const Fix16 step) const
{
log_debug("has_a_line_of_sight: " << start << " and " << end);
// The point to move along the line to check if everything is walkable.
Position pointer(start);
// A vector with a length of 'step' in the direction of the end position
// We use this vector to move the pointor along the line.
Vec2 forward(end - start);
forward.set_length(step);
log_debug("Forward: " << forward);
// the cell position the pointer is at.
unsigned short prevx, prevy;
unsigned short x, y;
std::tie(prevx, prevy) = this->get_cell_at_position(pointer);
// Move the pointer until we reach the destination, or an obstacle.
log_debug("Distance between start and end of path: "<< std::to_string(Position::distance(pointer, end)));
while (Position::distance(pointer, end) >= step)
{
// Move the pointer forward.
pointer += forward;
std::tie(x, y) = this->get_cell_at_position(pointer);
// If the cell changed, we check if we can walk from the previous to
// the current one. If not, it's not walkable and we return false
if ((prevx != x) || (prevy != y))
{
if (this->can_traverse_cell(prevx, prevy, x, y) == false)
// we hit an obstabcle.
{
return false;
}
}
prevy = y;
prevx = x;
}
std::tie(x, y) = this->get_cell_at_position(end);
if (((prevx != x) || (prevy != y)) && this->can_traverse_cell(prevx, prevy, x, y) == false)
{
return false;
}
return true;
}
bool World::can_be_seen_by_team(const Position& position, const uint16_t team) bool World::can_be_seen_by_team(const Position& position, const uint16_t team)
{ {
auto res = auto res =
...@@ -505,202 +483,72 @@ bool World::can_be_seen_by_team(const Position& position, const uint16_t team) ...@@ -505,202 +483,72 @@ bool World::can_be_seen_by_team(const Position& position, const uint16_t team)
bool World::can_traverse_cell(const short x, const short y, bool World::can_traverse_cell(const short x, const short y,
const short x2, const short y2) const const short x2, const short y2) const
{ {
log_debug("can_traverse_cell: " << x << ":" << y << " " << x2 << ":" << y2);
if (y == y2 && x == x2) if (y == y2 && x == x2)
{ return true;
log_debug("same");
return true;
}
// TODO in the case of a diagonal move, we currently return true. Do a
// more correct thing checking if (for example if we go right-down) we can
// go right THEN down OR down THEN right. If one of those is possible,
// return true, otherwise return false.
TileHeights heights = this->map.get_cell_heights(x, y); TileHeights heights = this->map.get_cell_heights(x, y);
TileHeights neighbour_heights = this->map.get_cell_heights(x2, y2); TileHeights neighbour_heights = this->map.get_cell_heights(x2, y2);
// move up-right
if ((y % 2 == 0 && y2 == y - 1 && x2 == x) || if ((y % 2 == 0 && y2 == y - 1 && x2 == x) ||
(y % 2 == 1 && y2 == y - 1 && x2 == x + 1)) (y % 2 == 1 && y2 == y - 1 && x2 == x + 1))