1 files changed, 90 insertions, 0 deletions
diff --git a/src/path_finder.cpp b/src/path_finder.cpp
new file mode 100644
index 0000000..294f708
--- /dev/null
+++ b/src/path_finder.cpp
@@ -0,0 +1,90 @@
+#include "common.hpp"
+
+namespace world {
+
+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}
+};
+
+path_finder_t::~path_finder_t()
+{
+	delete nodes;
+}
+
+void path_finder_t::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 path_finder_t::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 path_finder_t::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());
+	}
+}
+
+} // namespace world