src/pathfinding.rs 10.6 K · 348 lines · raw · history

1 use std::{
2 cmp::Reverse,
3 collections::{BinaryHeap, HashSet, VecDeque},
4 };
5
6 use bevy::prelude::*;
7
8 use crate::world::{CHEAPEST_MOVE_COST, WorldMap};
9
10 const STRAIGHT: u32 = 10;
11 const DIAGONAL: u32 = 14;
12 const MIN_TERRAIN_COST: u32 = CHEAPEST_MOVE_COST;
13
14 const NEIGHBORS: [IVec2; 8] = [
15 IVec2::new(1, 0),
16 IVec2::new(-1, 0),
17 IVec2::new(0, 1),
18 IVec2::new(0, -1),
19 IVec2::new(1, 1),
20 IVec2::new(1, -1),
21 IVec2::new(-1, 1),
22 IVec2::new(-1, -1),
23 ];
24
25 pub fn find_path(map: &WorldMap, start: UVec2, goal: UVec2) -> Option<Vec<UVec2>> {
26 map.move_cost(goal.as_ivec2())?;
27 let goal = goal.as_ivec2();
28 astar(map, start, |p| p == goal, |p| octile(p, goal))
29 }
30
31 pub fn find_path_adjacent(map: &WorldMap, start: UVec2, target: UVec2) -> Option<Vec<UVec2>> {
32 let target = target.as_ivec2();
33 astar(
34 map,
35 start,
36 |p| is_adjacent(p, target),
37 |p| octile(p, target).saturating_sub(DIAGONAL * MIN_TERRAIN_COST),
38 )
39 }
40
41 pub fn is_adjacent(a: IVec2, b: IVec2) -> bool {
42 a != b && (a - b).abs().max_element() == 1
43 }
44
45 pub fn nearest_tile(
46 map: &WorldMap,
47 start: UVec2,
48 max_tiles: usize,
49 mut is_target: impl FnMut(UVec2) -> bool,
50 ) -> Option<UVec2> {
51 let mut seen = HashSet::from([start]);
52 let mut queue = VecDeque::from([start]);
53 while let Some(pos) = queue.pop_front() {
54 if is_target(pos) {
55 return Some(pos);
56 }
57 if seen.len() >= max_tiles {
58 continue;
59 }
60 for step in NEIGHBORS {
61 let next = pos.as_ivec2() + step;
62 if map.move_cost(next).is_some() && seen.insert(next.as_uvec2()) {
63 queue.push_back(next.as_uvec2());
64 }
65 }
66 }
67 None
68 }
69
70 fn astar(
71 map: &WorldMap,
72 start: UVec2,
73 is_goal: impl Fn(IVec2) -> bool,
74 heuristic: impl Fn(IVec2) -> u32,
75 ) -> Option<Vec<UVec2>> {
76 let start = start.as_ivec2();
77 if !map.in_bounds(start) {
78 return None;
79 }
80 if is_goal(start) {
81 return Some(vec![]);
82 }
83
84 let width = map.size.x as usize;
85 let index = |p: IVec2| p.y as usize * width + p.x as usize;
86 let tiles = map.size.element_product() as usize;
87 let mut cost_so_far = vec![u32::MAX; tiles];
88 let mut came_from = vec![None::<IVec2>; tiles];
89 let mut open = BinaryHeap::new();
90
91 cost_so_far[index(start)] = 0;
92 open.push(Reverse((heuristic(start), start.x, start.y)));
93
94 while let Some(Reverse((_, x, y))) = open.pop() {
95 let current = IVec2::new(x, y);
96 if is_goal(current) {
97 return Some(reconstruct(&came_from, index, start, current));
98 }
99 let current_cost = cost_so_far[index(current)];
100 for step in NEIGHBORS {
101 let next = current + step;
102 let Some(terrain_cost) = map.step_cost(current, next) else {
103 continue;
104 };
105 let diagonal = step.x != 0 && step.y != 0;
106 if diagonal
107 && (map.move_cost(current + IVec2::new(step.x, 0)).is_none()
108 || map.move_cost(current + IVec2::new(0, step.y)).is_none())
109 {
110 continue;
111 }
112 let base = if diagonal { DIAGONAL } else { STRAIGHT };
113 let new_cost = current_cost + base * terrain_cost;
114 if new_cost < cost_so_far[index(next)] {
115 cost_so_far[index(next)] = new_cost;
116 came_from[index(next)] = Some(current);
117 open.push(Reverse((new_cost + heuristic(next), next.x, next.y)));
118 }
119 }
120 }
121 None
122 }
123
124 fn octile(a: IVec2, b: IVec2) -> u32 {
125 let d = (a - b).abs();
126 let (min, max) = (d.x.min(d.y) as u32, d.x.max(d.y) as u32);
127 (DIAGONAL * min + STRAIGHT * (max - min)) * MIN_TERRAIN_COST
128 }
129
130 fn reconstruct(
131 came_from: &[Option<IVec2>],
132 index: impl Fn(IVec2) -> usize,
133 start: IVec2,
134 goal: IVec2,
135 ) -> Vec<UVec2> {
136 let mut path = vec![goal.as_uvec2()];
137 let mut current = goal;
138 while let Some(prev) = came_from[index(current)] {
139 if prev == start {
140 break;
141 }
142 path.push(prev.as_uvec2());
143 current = prev;
144 }
145 path.reverse();
146 path
147 }
148
149 #[cfg(test)]
150 mod tests {
151 use super::*;
152 use crate::world::Terrain;
153
154 fn map_from(rows: &[&str]) -> WorldMap {
155 let size = UVec2::new(rows[0].len() as u32, rows.len() as u32);
156 let mut map = WorldMap::filled(size, Terrain::Grass);
157 for (row, line) in rows.iter().enumerate() {
158 let y = size.y - 1 - row as u32;
159 for (x, c) in line.chars().enumerate() {
160 let terrain = match c {
161 '#' => Terrain::Stone,
162 '~' => Terrain::ShallowWater,
163 _ => Terrain::Grass,
164 };
165 map.set(UVec2::new(x as u32, y), terrain);
166 }
167 }
168 map
169 }
170
171 fn is_connected(start: UVec2, path: &[UVec2]) -> bool {
172 std::iter::once(start)
173 .chain(path.iter().copied())
174 .collect::<Vec<_>>()
175 .windows(2)
176 .all(|w| (w[0].as_ivec2() - w[1].as_ivec2()).abs().max_element() == 1)
177 }
178
179 #[test]
180 fn path_excludes_start_and_ends_at_goal() {
181 let map = map_from(&["....."]);
182 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
183 assert_eq!(
184 path,
185 vec![
186 UVec2::new(1, 0),
187 UVec2::new(2, 0),
188 UVec2::new(3, 0),
189 UVec2::new(4, 0)
190 ]
191 );
192 }
193
194 #[test]
195 fn path_to_self_is_empty() {
196 let map = map_from(&["..."]);
197 assert_eq!(find_path(&map, UVec2::ZERO, UVec2::ZERO), Some(vec![]));
198 }
199
200 #[test]
201 fn path_goes_around_walls() {
202 let map = map_from(&[
203 ".....", //
204 ".###.", ".#.#.", "...#.",
205 ]);
206 let start = UVec2::new(2, 1);
207 let goal = UVec2::new(4, 0);
208 let path = find_path(&map, start, goal).unwrap();
209 assert!(is_connected(start, &path));
210 assert_eq!(path.last(), Some(&goal));
211 assert!(path.iter().all(|p| map.move_cost(p.as_ivec2()).is_some()));
212 }
213
214 #[test]
215 fn unreachable_or_impassable_goal_has_no_path() {
216 let map = map_from(&[
217 "..#..", //
218 "..#..",
219 ]);
220 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)), None);
221 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(2, 0)), None);
222 }
223
224 // Colonists move in straight lines between tile centers, so a diagonal step between two
225 // blocked orthogonal neighbours would visually clip through the corners of walls.
226 #[test]
227 fn diagonal_steps_never_cut_wall_corners() {
228 let map = map_from(&[
229 "#.", //
230 ".#",
231 ]);
232 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(1, 1)), None);
233 let map = map_from(&[
234 "..", //
235 "#.",
236 ]);
237 let path = find_path(&map, UVec2::new(0, 1), UVec2::new(1, 0)).unwrap();
238 assert_eq!(path, vec![UVec2::new(1, 1), UVec2::new(1, 0)]);
239 }
240
241 #[test]
242 fn prefers_detour_over_slow_terrain() {
243 let map = map_from(&[
244 ".....", //
245 "..~..",
246 ]);
247 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
248 assert!(!path.contains(&UVec2::new(2, 0)));
249 }
250
251 #[test]
252 fn walks_around_a_hill_rather_than_over_it() {
253 let mut map = map_from(&[
254 ".....", //
255 ".....",
256 ]);
257 map.set_height(UVec2::new(2, 0), 10.0);
258 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
259 assert!(!path.contains(&UVec2::new(2, 0)));
260 }
261
262 #[test]
263 fn detours_along_paths_when_they_are_faster() {
264 let mut map = map_from(&[
265 "..........", //
266 "..........",
267 ]);
268 for x in 0..10 {
269 map.set(UVec2::new(x, 1), Terrain::Paved);
270 }
271 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(9, 0)).unwrap();
272 assert!(path.iter().filter(|p| p.y == 1).count() >= 6);
273 }
274
275 // A wall can go up on a tile a colonist is standing on (or just about to enter); routes must
276 // still start from that tile so nobody gets permanently stuck inside a wall.
277 #[test]
278 fn colonists_can_walk_out_of_a_blocked_tile() {
279 let mut map = map_from(&["....."]);
280 map.set_wall(UVec2::new(2, 0), true);
281 let path = find_path(&map, UVec2::new(2, 0), UVec2::new(4, 0)).unwrap();
282 assert_eq!(path, vec![UVec2::new(3, 0), UVec2::new(4, 0)]);
283 }
284
285 #[test]
286 fn crosses_slow_terrain_when_it_is_the_only_way() {
287 let map = map_from(&["..~.."]);
288 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
289 assert!(path.contains(&UVec2::new(2, 0)));
290 }
291
292 #[test]
293 fn path_adjacent_stops_next_to_target() {
294 let map = map_from(&["......"]);
295 let path = find_path_adjacent(&map, UVec2::new(0, 0), UVec2::new(5, 0)).unwrap();
296 assert_eq!(path.last(), Some(&UVec2::new(4, 0)));
297 }
298
299 #[test]
300 fn path_adjacent_works_for_impassable_targets() {
301 let map = map_from(&["...#"]);
302 let path = find_path_adjacent(&map, UVec2::new(0, 0), UVec2::new(3, 0)).unwrap();
303 assert_eq!(path.last(), Some(&UVec2::new(2, 0)));
304 }
305
306 #[test]
307 fn path_adjacent_from_neighbor_is_empty() {
308 let map = map_from(&["..."]);
309 let path = find_path_adjacent(&map, UVec2::new(1, 0), UVec2::new(2, 0));
310 assert_eq!(path, Some(vec![]));
311 }
312
313 #[test]
314 fn path_adjacent_to_enclosed_target_is_none() {
315 let walled = map_from(&[
316 "#####", //
317 "#...#", "#####", ".....",
318 ]);
319 assert_eq!(
320 find_path_adjacent(&walled, UVec2::new(0, 0), UVec2::new(2, 2)),
321 None
322 );
323 }
324
325 #[test]
326 fn nearest_tile_finds_closest_match_by_walking_distance() {
327 let map = map_from(&[
328 ".#...", //
329 ".#.#.", "...#.",
330 ]);
331 let found = nearest_tile(&map, UVec2::new(0, 0), 100, |p| p.x == 4);
332 assert_eq!(found, Some(UVec2::new(4, 2)));
333 assert_eq!(
334 nearest_tile(&map, UVec2::new(0, 0), 100, |p| p.x == 9),
335 None
336 );
337 }
338
339 #[test]
340 fn finds_paths_across_a_generated_world() {
341 let map = crate::worldgen::generate(crate::worldgen::WORLD_SIZE, 1);
342 let region = crate::worldgen::largest_region(&map);
343 let (start, goal) = (region[0], region[region.len() - 1]);
344 let path = find_path(&map, start, goal).unwrap();
345 assert!(is_connected(start, &path));
346 assert_eq!(path.last(), Some(&goal));
347 }
348 }