Improve path finding.

The path finder will avoid paths that would intersect obstacles.

6 files changed, 116 insertions, 12 deletions

diff --git a/src/common.hpp b/src/common.hpp
index b7ce218..6d4c7c2 100644
--- a/src/common.hpp
+++ b/src/common.hpp
@@ -236,6 +236,10 @@ namespace render {
 	class state_t {
 		sf::RenderWindow *window;
 		double now;
+
+		void drender_rect(rectf_t rect, sf::Color color);
+		void drender_text(rectf_t rect, std::string str);
+		void drender_entity(world::entity_t *ent);
 	public:
 
 		state_t(sf::RenderWindow *window_);
diff --git a/src/game.cpp b/src/game.cpp
index 91cf4d2..a2f4bbc 100644
--- a/src/game.cpp
+++ b/src/game.cpp
@@ -116,7 +116,7 @@ void state_t::start(void)
 	human.size[1] = v2f_t(+0.4f, +0.4f);
 	human.render_size[0] = v2f_t(-0.5f, -1.0f);
 	human.render_size[1] = v2f_t(+0.5f, +0.5f);
-	human.place(&world, v2f_t(0.5f, 0.5f));
+	human.place(&world, v2f_t(2.5f, -3.5f));
 
 	human2.size[0] = v2f_t(-0.4f, -0.4f);
 	human2.size[1] = v2f_t(+0.4f, +0.4f);
diff --git a/src/math.hpp b/src/math.hpp
index 938241f..8c93a76 100644
--- a/src/math.hpp
+++ b/src/math.hpp
@@ -265,6 +265,11 @@ public:
 		return v[i];
 	}
 
+	vec_t<T, N> dims(void) const
+	{
+		return v[1] - v[0];
+	}
+
 	T dim(size_t i) const
 	{
 		return v[1][i] - v[0][i];
@@ -282,6 +287,11 @@ public:
 		return r;
 	}
 
+	vec_t<T, N> center(void) const
+	{
+		return v[0] / 2 + v[1] / 2;
+	}
+
 	friend bool operator&&(const rect_t<T, N> &a, const rect_t<T, N> &b)
 	{
 		for (size_t i = 0; i < N; i++)
@@ -308,6 +318,18 @@ public:
 		stream << "(" << r[0] << " x " << r[1] << ")";
 		return stream;
 	}
+
+	// Compatibility with SFML
+
+	rect_t(sf::FloatRect b)
+	{
+		static_assert(N == 2, "conversion of sf::FloatRect to rect_t with N != 2");
+
+		v[0][0] = b.left;
+		v[0][1] = b.top;
+		v[1][0] = b.left + b.width;
+		v[1][1] = b.top + b.height;
+	}
 };
 
 // Shorthands
diff --git a/src/path_finder.cpp b/src/path_finder.cpp
index aeded09..6d9540c 100644
--- a/src/path_finder.cpp
+++ b/src/path_finder.cpp
@@ -3,9 +3,12 @@
 namespace world {
 
 static const v2f_t path_margin(5, 5);
-static const tile_index_t path_offsets[8] = {
+/*static const tile_index_t path_offsets[8] = {
 	{+1, 0}, {+1, +1}, {0, +1}, {-1, +1},
 	{-1, 0}, {-1, -1}, {0, -1}, {+1, -1}
+};*/
+static const tile_index_t path_offsets[4] = {
+	{+1, 0}, {0, +1}, {-1, 0}, {0, -1}
 };
 
 path_finder_t::~path_finder_t()
@@ -38,8 +41,10 @@ void path_finder_t::setup_nodes(v2f_t src_, v2f_t dst_, cflags_t cflags_)
 	height = end[1] - base[1] + 1;
 
 	nodes = new path_node_t[width * height];
-	for (size_t i = 0; i < width * height; i++)
+	for (size_t i = 0; i < width * height; i++) {
+		nodes[i].accessible = true;
 		nodes[i].dist = INFINITY;
+	}
 }
 
 void path_finder_t::eliminate_nodes(rectf_t bounds)
@@ -47,8 +52,20 @@ void path_finder_t::eliminate_nodes(rectf_t bounds)
 	rect_t<coord_t, 2> index_bounds;
 	tile_index_t index;
 
-	index_bounds[0] = tile_index_t(bounds[0].floor()) - base;
-	index_bounds[1] = tile_index_t(bounds[1].ceil()) - base;
+	bounds[0] -= tile_center;
+	bounds[1] -= tile_center;
+
+	index_bounds[0] = tile_index_t(bounds[0].ceil()) - base;
+	index_bounds[1] = tile_index_t(bounds[1].floor()) - base;
+
+	if (index_bounds[0][0] < 0)
+		index_bounds[0][0] = 0;
+	if (index_bounds[0][1] < 0)
+		index_bounds[0][1] = 0;
+	if (index_bounds[1][0] >= (coord_t)width)
+		index_bounds[1][0] = width - 1;
+	if (index_bounds[1][1] >= (coord_t)height)
+		index_bounds[1][1] = height - 1;
 
 	for (index[1] = index_bounds[0][1]; index[1] <= index_bounds[1][1]; index[1]++)
 	for (index[0] = index_bounds[0][0]; index[0] <= index_bounds[1][0]; index[0]++) {
@@ -80,7 +97,7 @@ void path_finder_t::find_r(tile_index_t index, float dist, float limit)
 		shortest_dist = dist + dist_to_dst;
 	}
 
-	for (size_t i = 0; i < 8; i++) {
+	for (size_t i = 0; i < 4; i++) {
 		tile_index_t offset, next;
 
 		offset = path_offsets[i];
@@ -112,7 +129,7 @@ bool path_finder_t::find(void)
 	start = tile_index_at(src) - base;
 	nodes[start[1] * width + start[0]].accessible = false;
 
-	for (size_t i = 0; i < 8; i++) {
+	for (size_t i = 0; i < 4; i++) {
 		tile_index_t next;
 		v2f_t offset;
 
diff --git a/src/render.cpp b/src/render.cpp
index 7b38989..9fb1625 100644
--- a/src/render.cpp
+++ b/src/render.cpp
@@ -69,6 +69,53 @@ void state_t::end_frame(void)
 	window->display();
 }
 
+void state_t::drender_rect(rectf_t rect, sf::Color color)
+{
+	sf::Color fill;
+
+
+	fill = sf::Color(color.r, color.g, color.b, 50);
+
+	wot_rect.setSize(rect.dims());
+	wot_rect.setPosition(rect[0]);
+	wot_rect.setFillColor(fill);
+	wot_rect.setOutlineThickness(0.01);
+	wot_rect.setOutlineColor(color);
+	window->draw(wot_rect);
+}
+
+void state_t::drender_text(rectf_t rect, std::string str)
+{
+	sf::Text text(str, font, 20);
+	sf::FloatRect text_rect;
+	v2f_t text_size, x;
+
+	text.setScale(0.006f, 0.006f);
+	text_rect = text.getGlobalBounds();
+	text_size = v2f_t(text_rect.width, text_rect.height);
+	x = rect.center() - text_size / 2;
+
+	text.setPosition(x + v2f_t(0.01f, 0.01f));
+	text.setFillColor(sf::Color::Black);
+	window->draw(text);
+
+	text.setPosition(x);
+	text.setFillColor(sf::Color::White);
+	window->draw(text);
+}
+
+void state_t::drender_entity(world::entity_t *ent)
+{
+	std::stringstream ss;
+
+	drender_rect(ent->render_bounds, sf::Color::Red);
+	drender_rect(ent->cmodel.bounds, sf::Color::Yellow);
+
+	ss << (void*)ent << "\n";
+	ss << "CF=" << ent->cmodel.cflags;
+	drender_text(ent->render_bounds, ss.str());
+}
+
 void state_t::render(game::state_t *game)
 {
 	sf::Vector2u size = window->getSize();
@@ -96,8 +143,10 @@ void state_t::render(game::state_t *game)
 			return x->render_bounds[1][0] < y->render_bounds[1][0];
 		});
 
-	for (world::entity_t *ent : ents)
+	for (world::entity_t *ent : ents) {
 		ent->render_to(this);
+		drender_entity(ent);
+	}
 
 	for (world::world_t::debug_t &debug : game->world.debug) {
 		sf::Text text(debug.text, font, 20);
diff --git a/src/world.cpp b/src/world.cpp
index ab3ef92..778e463 100644
--- a/src/world.cpp
+++ b/src/world.cpp
@@ -85,16 +85,27 @@ bool world_t::find_path(v2f_t src, v2f_t dst, cmodel_t *cmodel, entity_t *ignore
 {
 	path_finder_t finder;
 	rectf_t bounds;
+	v2f_t cmodel_dims;
+	bool found;
 
 	finder.setup_nodes(src, dst, cmodel->cflags);
 
+	cmodel_dims = cmodel->bounds.dims();
+
 	for (size_t y = 0; y < finder.height; y++)
 	for (size_t x = 0; x < finder.width; x++) {
-		path_node_t *node = finder.nodes + y * finder.width + x;
 		tile_index_t index;
+		rectf_t combined;
 
 		index = finder.base + tile_index_t(x, y);
-		node->accessible = get_tile(index)->type == TILE_DIRT;
+
+		if (get_tile(index)->type == TILE_DIRT)
+			continue;
+
+		combined[0] = v2f_t(index)  - cmodel_dims / 2;
+		combined[1] = v2f_t(index) + v2f_t(1.0f, 1.0f) + cmodel_dims / 2;
+
+		finder.eliminate_nodes(combined);
 	}
 
 	bounds = rectf_t(src, dst).norm();
@@ -103,8 +114,7 @@ bool world_t::find_path(v2f_t src, v2f_t dst, cmodel_t *cmodel, entity_t *ignore
 		if (ent != ignore)
 			finder.eliminate_nodes(ent->cmodel.bounds);
 
-	if (!finder.find())
-		return false;
+	found = finder.find();
 
 	debug.clear();
 
@@ -122,6 +132,8 @@ bool world_t::find_path(v2f_t src, v2f_t dst, cmodel_t *cmodel, entity_t *ignore
 		debug.push_back((debug_t){finder.base + tile_index_t(x, y), ss.str()});
 	}
 
+	if (!found)
+		return false;
 
 	finder.export_path(path);
 	return true;