src/pathfinding.rs 6.9 K · 225 lines · raw · history

1 use std::{cmp::Reverse, collections::BinaryHeap};
2
3 use bevy::prelude::*;
4
5 use crate::world::WorldMap;
6
7 const STRAIGHT: u32 = 10;
8 const DIAGONAL: u32 = 14;
9 const MIN_TERRAIN_COST: u32 = 100;
10
11 const NEIGHBORS: [IVec2; 8] = [
12 IVec2::new(1, 0),
13 IVec2::new(-1, 0),
14 IVec2::new(0, 1),
15 IVec2::new(0, -1),
16 IVec2::new(1, 1),
17 IVec2::new(1, -1),
18 IVec2::new(-1, 1),
19 IVec2::new(-1, -1),
20 ];
21
22 pub fn find_path(map: &WorldMap, start: UVec2, goal: UVec2) -> Option<Vec<UVec2>> {
23 let start = start.as_ivec2();
24 let goal = goal.as_ivec2();
25 map.move_cost(start)?;
26 map.move_cost(goal)?;
27 if start == goal {
28 return Some(vec![]);
29 }
30
31 let width = map.size.x as usize;
32 let index = |p: IVec2| p.y as usize * width + p.x as usize;
33 let tiles = map.size.element_product() as usize;
34 let mut cost_so_far = vec![u32::MAX; tiles];
35 let mut came_from = vec![None::<IVec2>; tiles];
36 let mut open = BinaryHeap::new();
37
38 cost_so_far[index(start)] = 0;
39 open.push(Reverse((heuristic(start, goal), start.x, start.y)));
40
41 while let Some(Reverse((_, x, y))) = open.pop() {
42 let current = IVec2::new(x, y);
43 if current == goal {
44 return Some(reconstruct(&came_from, index, start, goal));
45 }
46 let current_cost = cost_so_far[index(current)];
47 for step in NEIGHBORS {
48 let next = current + step;
49 let Some(terrain_cost) = map.step_cost(current, next) else {
50 continue;
51 };
52 let diagonal = step.x != 0 && step.y != 0;
53 if diagonal
54 && (map.move_cost(current + IVec2::new(step.x, 0)).is_none()
55 || map.move_cost(current + IVec2::new(0, step.y)).is_none())
56 {
57 continue;
58 }
59 let base = if diagonal { DIAGONAL } else { STRAIGHT };
60 let new_cost = current_cost + base * terrain_cost;
61 if new_cost < cost_so_far[index(next)] {
62 cost_so_far[index(next)] = new_cost;
63 came_from[index(next)] = Some(current);
64 open.push(Reverse((new_cost + heuristic(next, goal), next.x, next.y)));
65 }
66 }
67 }
68 None
69 }
70
71 fn heuristic(a: IVec2, b: IVec2) -> u32 {
72 let d = (a - b).abs();
73 let (min, max) = (d.x.min(d.y) as u32, d.x.max(d.y) as u32);
74 (DIAGONAL * min + STRAIGHT * (max - min)) * MIN_TERRAIN_COST
75 }
76
77 fn reconstruct(
78 came_from: &[Option<IVec2>],
79 index: impl Fn(IVec2) -> usize,
80 start: IVec2,
81 goal: IVec2,
82 ) -> Vec<UVec2> {
83 let mut path = vec![goal.as_uvec2()];
84 let mut current = goal;
85 while let Some(prev) = came_from[index(current)] {
86 if prev == start {
87 break;
88 }
89 path.push(prev.as_uvec2());
90 current = prev;
91 }
92 path.reverse();
93 path
94 }
95
96 #[cfg(test)]
97 mod tests {
98 use super::*;
99 use crate::world::Terrain;
100
101 fn map_from(rows: &[&str]) -> WorldMap {
102 let size = UVec2::new(rows[0].len() as u32, rows.len() as u32);
103 let mut map = WorldMap::filled(size, Terrain::Grass);
104 for (row, line) in rows.iter().enumerate() {
105 let y = size.y - 1 - row as u32;
106 for (x, c) in line.chars().enumerate() {
107 let terrain = match c {
108 '#' => Terrain::Stone,
109 '~' => Terrain::ShallowWater,
110 _ => Terrain::Grass,
111 };
112 map.set(UVec2::new(x as u32, y), terrain);
113 }
114 }
115 map
116 }
117
118 fn is_connected(start: UVec2, path: &[UVec2]) -> bool {
119 std::iter::once(start)
120 .chain(path.iter().copied())
121 .collect::<Vec<_>>()
122 .windows(2)
123 .all(|w| (w[0].as_ivec2() - w[1].as_ivec2()).abs().max_element() == 1)
124 }
125
126 #[test]
127 fn path_excludes_start_and_ends_at_goal() {
128 let map = map_from(&["....."]);
129 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
130 assert_eq!(
131 path,
132 vec![
133 UVec2::new(1, 0),
134 UVec2::new(2, 0),
135 UVec2::new(3, 0),
136 UVec2::new(4, 0)
137 ]
138 );
139 }
140
141 #[test]
142 fn path_to_self_is_empty() {
143 let map = map_from(&["..."]);
144 assert_eq!(find_path(&map, UVec2::ZERO, UVec2::ZERO), Some(vec![]));
145 }
146
147 #[test]
148 fn path_goes_around_walls() {
149 let map = map_from(&[
150 ".....", //
151 ".###.", ".#.#.", "...#.",
152 ]);
153 let start = UVec2::new(2, 1);
154 let goal = UVec2::new(4, 0);
155 let path = find_path(&map, start, goal).unwrap();
156 assert!(is_connected(start, &path));
157 assert_eq!(path.last(), Some(&goal));
158 assert!(path.iter().all(|p| map.move_cost(p.as_ivec2()).is_some()));
159 }
160
161 #[test]
162 fn unreachable_or_impassable_goal_has_no_path() {
163 let map = map_from(&[
164 "..#..", //
165 "..#..",
166 ]);
167 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)), None);
168 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(2, 0)), None);
169 }
170
171 // Colonists move in straight lines between tile centers, so a diagonal step between two
172 // blocked orthogonal neighbours would visually clip through the corners of walls.
173 #[test]
174 fn diagonal_steps_never_cut_wall_corners() {
175 let map = map_from(&[
176 "#.", //
177 ".#",
178 ]);
179 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(1, 1)), None);
180 let map = map_from(&[
181 "..", //
182 "#.",
183 ]);
184 let path = find_path(&map, UVec2::new(0, 1), UVec2::new(1, 0)).unwrap();
185 assert_eq!(path, vec![UVec2::new(1, 1), UVec2::new(1, 0)]);
186 }
187
188 #[test]
189 fn prefers_detour_over_slow_terrain() {
190 let map = map_from(&[
191 ".....", //
192 "..~..",
193 ]);
194 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
195 assert!(!path.contains(&UVec2::new(2, 0)));
196 }
197
198 #[test]
199 fn walks_around_a_hill_rather_than_over_it() {
200 let mut map = map_from(&[
201 ".....", //
202 ".....",
203 ]);
204 map.set_height(UVec2::new(2, 0), 10.0);
205 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
206 assert!(!path.contains(&UVec2::new(2, 0)));
207 }
208
209 #[test]
210 fn crosses_slow_terrain_when_it_is_the_only_way() {
211 let map = map_from(&["..~.."]);
212 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
213 assert!(path.contains(&UVec2::new(2, 0)));
214 }
215
216 #[test]
217 fn finds_paths_across_a_generated_world() {
218 let map = crate::worldgen::generate(crate::worldgen::WORLD_SIZE, 1);
219 let region = crate::worldgen::largest_region(&map);
220 let (start, goal) = (region[0], region[region.len() - 1]);
221 let path = find_path(&map, start, goal).unwrap();
222 assert!(is_connected(start, &path));
223 assert_eq!(path.last(), Some(&goal));
224 }
225 }