First working path finding.

1 files changed, 29 insertions, 1 deletions

diff --git a/src/path_finder.cpp b/src/path_finder.cpp
index 294f708..ab300dc 100644
--- a/src/path_finder.cpp
+++ b/src/path_finder.cpp
@@ -21,6 +21,9 @@ void path_finder_t::setup_nodes(v2f_t src_, v2f_t dst_)
 	src = src_;
 	dst = dst_;
 
+	tile_center = v2f_t(0.5, 0.5);
+	shortest_dist = INFINITY;
+
 	src_margin[0] = src - path_margin;
 	src_margin[1] = src + path_margin;
 	dst_margin[0] = dst - path_margin;
@@ -41,6 +44,7 @@ void path_finder_t::setup_nodes(v2f_t src_, v2f_t dst_)
 void path_finder_t::find_r(tile_index_t index, float dist)
 {
 	path_node_t *node;
+	float dist_to_dst;
 
 	node = nodes + index[1] * width + index[0];
 
@@ -49,8 +53,15 @@ void path_finder_t::find_r(tile_index_t index, float dist)
 	if (node->dist <= dist)
 		return;
 
+	path.push_back(index);
 	node->dist = dist;
 
+	dist_to_dst = (v2f_t(base + index) + tile_center - dst).len();
+	if (dist_to_dst < 1.0f && dist + dist_to_dst < shortest_dist) {
+		shortest_path = path;
+		shortest_dist = dist + dist_to_dst;
+	}
+
 	for (size_t i = 0; i < 8; i++) {
 		tile_index_t offset, next;
 
@@ -63,6 +74,8 @@ void path_finder_t::find_r(tile_index_t index, float dist)
 
 		find_r(next, dist + v2f_t(offset).len());
 	}
+
+	path.pop_back();
 }
 
 void path_finder_t::find(void)
@@ -77,7 +90,7 @@ void path_finder_t::find(void)
 		v2f_t offset;
 
 		next = start + path_offsets[i];
-		offset = src - v2f_t(base) - v2f_t(next);
+		offset = (src - v2f_t(base)) - v2f_t(next) - tile_center;
 
 		if (next[0] < 0 || next[1] < 0 ||
 		    next[0] >= (int64_t)width || next[1] >= (int64_t)height)
@@ -87,4 +100,19 @@ void path_finder_t::find(void)
 	}
 }
 
+bool path_finder_t::export_path(std::list<v2f_t> *list)
+{
+	if (!shortest_path.size())
+		return false;
+
+	list->clear();
+
+	for (tile_index_t &index : shortest_path)
+		list->push_back(v2f_t(index + base) + tile_center);
+
+	list->push_back(dst);
+
+	return true;
+}
+
 } // namespace world