fix dijkstra but efficient this time

1 files changed, 41 insertions, 28 deletions

diff --git a/DijkstraPathfinder.cpp b/DijkstraPathfinder.cpp
index 2fb07f5..8d86235 100644
--- a/DijkstraPathfinder.cpp
+++ b/DijkstraPathfinder.cpp
@@ -1,4 +1,5 @@
 #include <algorithm>
+#include <queue>
 
 #include "DijkstraPathfinder.h"
 #include "Museum.h"
@@ -38,42 +39,54 @@ DijkstraPathfinder::Node & DijkstraPathfinder::map_get(const XY & pos) {
 	return this->map[pos];
 }
 
-void DijkstraPathfinder::dijkstra(const XY & here) {
-	this->set_visited(here);
+void DijkstraPathfinder::dijkstra(const XY & start) {
 	PathfindingContext & ctx = this->museum.pathfinding;
-	unsigned int dist_end = this->map_get(this->end).distance;
 
-	vector<pair<unsigned int, XY>> neighbors;
-	vector<XY> offsets = {
+	const 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 dist_neighbor = this->map_get(here).distance + neighbor_cost;
-
-		if (dist_neighbor >= dist_end) continue;
-
-		Node & neighbor_node = this->map_get(neighbor);
-		if (dist_neighbor >= neighbor_node.distance) continue;
-
-		neighbor_node.parent = here;
-		neighbor_node.distance = dist_neighbor;
-		neighbors.push_back({ dist_neighbor, neighbor });
-	}
 
-	// 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->dijkstra(neighbor.second);
+	auto compare = [] (Neighbor a, Neighbor b) -> bool { return a.distance > b.distance; };
+	priority_queue<Neighbor, vector<Neighbor>, decltype(compare)> pq;
+
+	pq.push({ 0, start });
+	this->map_get(start).distance = 0;
+
+	while (!pq.empty()) {
+		XY here = pq.top().position;
+		unsigned int distance = pq.top().distance;
+		pq.pop();
+
+		if (this->is_visited(here)) continue;
+		this->set_visited(here);
+
+		if (distance > this->map_get(here).distance) continue;
+
+		unsigned int dist_end = this->map_get(this->end).distance;
+
+		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 dist_neighbor = distance + ctx.get_weight(neighbor_tile.data.type);
+
+			if (dist_neighbor >= dist_end) continue;
+
+			Node & neighbor_node = this->map_get(neighbor);
+			if (dist_neighbor >= neighbor_node.distance) continue;
+
+			neighbor_node.parent = here;
+			neighbor_node.distance = dist_neighbor;
+
+			pq.push({
+				.distance = dist_neighbor,
+				.position = neighbor,
+			});
+		}
 	}
 }