1 files changed, 80 insertions, 0 deletions
diff --git a/DijkstraPathfinder.cpp b/DijkstraPathfinder.cpp
index 1603094..95291b3 100644
--- a/DijkstraPathfinder.cpp
+++ b/DijkstraPathfinder.cpp
@@ -1,3 +1,83 @@
+#include <algorithm>
+
 #include "DijkstraPathfinder.h"
+#include "Museum.h"
+
+using namespace std;
+
+const DijkstraPathfinder::Path & DijkstraPathfinder::get_path() {
+	return this->solution;
+}
+
+void DijkstraPathfinder::clear() {
+	Pathfinder::clear();
+	this->solution.clear();
+	this->map.clear();
+}
+
+void DijkstraPathfinder::find_between(const XY & start, const XY & end) {
+	this->clear();
+	this->end = end;
+
+	this->explore(start);
+
+	// no solution
+	if (!this->is_visited(end)) return;
+
+	XY pos = end;
+	while (pos != start) {
+		solution.push_front(pos);
+		pos = this->map[pos].parent;
+	}
+}
+
+DijkstraPathfinder::Node & DijkstraPathfinder::map_get(const XY & pos) {
+	if (!this->map.contains(pos))
+		this->map[pos] = Node();
+	return this->map[pos];
+}
+
+void DijkstraPathfinder::explore(const XY & here, unsigned int cost) {
+	if (this->is_visited(here)) return;
+
+	PathfindingContext & ctx = this->museum.pathfinding;
+	unsigned int end_cost = this->map_get(this->end).cost;
+
+	vector<pair<unsigned int, XY>> neighbors;
+	vector<XY> offsets = {
+		{ 0, 1 },
+		{ 1, 0 },
+		{ 0, -1 },
+		{ -1, 0 },
+	};
+	for (const XY & offset : offsets) {
+		const XY neighbor = here + offset;
+		if (ctx.empty_point(neighbor)) continue;
+		
+		Tile & neighbor_tile = this->museum.canvas.get_tile(neighbor);
+		unsigned int neighbor_cost = ctx.get_weight(neighbor_tile.data.type);
+		unsigned int step_cost = cost + neighbor_cost;
+
+		if (step_cost > end_cost) continue;
+
+		neighbors.push_back({ step_cost, neighbor });
+
+		Node & neighbor_node = this->map_get(neighbor);
+		if (step_cost < neighbor_node.cost) {
+			neighbor_node.parent = here;
+			neighbor_node.cost = step_cost;
+		}
+	}
+
+	this->set_visited(here);
+
+	// sort by cost ascending
+	sort(neighbors.begin(), neighbors.end(), [] (auto & left, auto & right) {
+		return left.first < right.first;
+	});
 
+	for (pair<unsigned int, XY> & neighbor : neighbors) {
+		this->explore(neighbor.second, neighbor.first);
+	}
+}