Start working on path finding.

1 files changed, 142 insertions, 3 deletions

diff --git a/src/world.cpp b/src/world.cpp
index d2fc2bb..818c024 100644
--- a/src/world.cpp
+++ b/src/world.cpp
@@ -1,11 +1,17 @@
 #include "common.hpp"
+#include <stack>
+#include <sstream>
 
 namespace world {
 
 sector_index_t sector_index_at(v2f_t x)
 {
-	return sector_index_t(floor(x[0] / SECTOR_SIZE),
-	                      floor(x[1] / SECTOR_SIZE));
+	return sector_index_t((x / SECTOR_SIZE).floor());
+}
+
+tile_index_t tile_index_at(v2f_t x)
+{
+	return sector_index_t(x.floor());
 }
 
 world_t::world_t(void)
@@ -45,7 +51,7 @@ void world_t::generate(sector_t *sector, sector_index_t index)
 	sector->index = index;
 
 	sector->bounds.v[0] = (v2f_t)index * SECTOR_SIZE;
-	sector->bounds.v[1].broadcast(SECTOR_SIZE);
+	sector->bounds.v[1] = sector->bounds.v[0] + v2f_t(SECTOR_SIZE, SECTOR_SIZE);
 
 	std::cout << "generating " << index << "\n";
 
@@ -58,6 +64,139 @@ void world_t::generate(sector_t *sector, sector_index_t index)
 	sector->empty = false;
 }
 
+typedef struct {
+	bool accessible;
+	float dist;
+} path_node_t;
+
+static const v2f_t path_margin(5, 5);
+static const tile_index_t path_offsets[8] = {
+	{+1, 0}, {+1, +1}, {0, +1}, {-1, +1},
+	{-1, 0}, {-1, -1}, {0, -1}, {+1, -1}
+};
+
+class path_finder_t {
+public:
+	v2f_t src, dst;
+	path_node_t *nodes;
+	size_t width, height;
+	tile_index_t base;
+	std::stack<v2f_t> current_path, shortest_path;
+
+	~path_finder_t()
+	{
+		delete nodes;
+	}
+
+	void setup_nodes(v2f_t src_, v2f_t dst_)
+	{
+		rectf_t src_margin, dst_margin, bounds;
+		tile_index_t end;
+
+		src = src_;
+		dst = dst_;
+
+		src_margin[0] = src - path_margin;
+		src_margin[1] = src + path_margin;
+		dst_margin[0] = dst - path_margin;
+		dst_margin[1] = dst + path_margin;
+		bounds = src_margin + dst_margin;
+
+		base = tile_index_at(bounds[0]);
+		end = tile_index_at(bounds[1]);
+
+		width = end[0] - base[0] + 1;
+		height = end[1] - base[1] + 1;
+
+		nodes = new path_node_t[width * height];
+		for (size_t i = 0; i < width * height; i++)
+			nodes[i].dist = INFINITY;
+	}
+
+	void find_r(tile_index_t index, float dist)
+	{
+		path_node_t *node;
+
+		node = nodes + index[1] * width + index[0];
+
+		if (!node->accessible)
+			return;
+		if (node->dist <= dist)
+			return;
+
+		node->dist = dist;
+
+		for (size_t i = 0; i < 8; i++) {
+			tile_index_t offset, next;
+
+			offset = path_offsets[i];
+			next = index + offset;
+
+			if (next[0] < 0 || next[1] < 0 ||
+			    next[0] >= (int64_t)width || next[1] >= (int64_t)height)
+				continue;
+
+			find_r(next, dist + v2f_t(offset).len());
+		}
+	}
+
+	void find(void)
+	{
+		tile_index_t start;
+
+		start = tile_index_at(src) - base;
+		nodes[start[1] * width + start[0]].accessible = false;
+
+		for (size_t i = 0; i < 8; i++) {
+			tile_index_t next;
+			v2f_t offset;
+
+			next = start + path_offsets[i];
+			offset = src - v2f_t(base) - v2f_t(next);
+
+			if (next[0] < 0 || next[1] < 0 ||
+			    next[0] >= (int64_t)width || next[1] >= (int64_t)height)
+				continue;
+
+			find_r(next, v2f_t(offset).len());
+		}
+	}
+};
+
+bool world_t::find_path(v2f_t src, v2f_t dst, rectf_t size,
+                        std::list<v2f_t> *path)
+{
+	path_finder_t finder;
+
+	finder.setup_nodes(src, dst);
+
+	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;
+
+		index = finder.base + tile_index_t(x, y);
+		node->accessible = (get_tile(index)->type >= 1);
+	}
+
+	finder.find();
+
+	debug.clear();
+
+	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;
+		std::stringstream ss;
+
+		ss << finder.base + tile_index_t(x, y) << "\n";
+		ss << node->dist;
+
+		debug.push_back((debug_t){finder.base + tile_index_t(x, y), ss.str()});
+	}
+
+	return false;
+}
+
 sector_t *world_t::get_sector(sector_index_t index)
 {
 	sector_t *sector;