1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
|
#include <algorithm>
#include <queue>
#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->map_get(start).distance = 0;
this->dijkstra(start);
if (!this->is_visited(end)) {
printf("Dijkstra: no solution found\n");
return;
}
XY pos = end;
int steps = 0;
while (pos != start) {
solution.push_front(pos);
pos = this->map_get(pos).parent;
steps++;
}
printf("Dijkstra: solution found (%d steps, %u time)\n", steps, this->map_get(end).distance);
}
DijkstraPathfinder::Node & DijkstraPathfinder::map_get(const XY & pos) {
if (!this->map.contains(pos))
this->map[pos] = Node();
return this->map[pos];
}
void DijkstraPathfinder::dijkstra(const XY & start) {
PathfindingContext & ctx = this->museum.pathfinding;
const vector<XY> offsets = {
{ 0, 1 },
{ 1, 0 },
{ 0, -1 },
{ -1, 0 },
};
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,
});
}
}
}
|