src/pathfinding.rs 12.2 K · 393 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 index = |p: UVec2| (p.y * map.size.x + p.x) as usize;
52 let mut seen = vec![false; (map.size.x * map.size.y) as usize];
53 let mut seen_count = 1;
54 if let Some(cell) = seen.get_mut(index(start)) {
55 *cell = true;
56 }
57 let mut queue = VecDeque::from([start]);
58 while let Some(pos) = queue.pop_front() {
59 if is_target(pos) {
60 return Some(pos);
61 }
62 if seen_count >= max_tiles {
63 continue;
64 }
65 for step in NEIGHBORS {
66 let next = pos.as_ivec2() + step;
67 if map.move_cost(next).is_some() && !seen[index(next.as_uvec2())] {
68 seen[index(next.as_uvec2())] = true;
69 seen_count += 1;
70 queue.push_back(next.as_uvec2());
71 }
72 }
73 }
74 None
75 }
76
77 pub fn search_reach(max_tiles: usize) -> u32 {
78 (max_tiles as f32).sqrt().ceil() as u32
79 }
80
81 pub fn nearest_of(
82 map: &WorldMap,
83 start: UVec2,
84 max_tiles: usize,
85 targets: &HashSet<UVec2>,
86 ) -> Option<UVec2> {
87 let reach = search_reach(max_tiles) as i32;
88 let in_reach = |p: &&UVec2| (p.as_ivec2() - start.as_ivec2()).abs().max_element() <= reach;
89 if !targets.iter().any(|p| in_reach(&p)) {
90 return None;
91 }
92 nearest_tile(map, start, max_tiles, |p| targets.contains(&p))
93 }
94
95 fn astar(
96 map: &WorldMap,
97 start: UVec2,
98 is_goal: impl Fn(IVec2) -> bool,
99 heuristic: impl Fn(IVec2) -> u32,
100 ) -> Option<Vec<UVec2>> {
101 let start = start.as_ivec2();
102 if !map.in_bounds(start) {
103 return None;
104 }
105 if is_goal(start) {
106 return Some(vec![]);
107 }
108
109 let width = map.size.x as usize;
110 let index = |p: IVec2| p.y as usize * width + p.x as usize;
111 let tiles = map.size.element_product() as usize;
112 let mut cost_so_far = vec![u32::MAX; tiles];
113 let mut came_from = vec![None::<IVec2>; tiles];
114 let mut open = BinaryHeap::new();
115
116 cost_so_far[index(start)] = 0;
117 open.push(Reverse((heuristic(start), start.x, start.y)));
118
119 while let Some(Reverse((_, x, y))) = open.pop() {
120 let current = IVec2::new(x, y);
121 if is_goal(current) {
122 return Some(reconstruct(&came_from, index, start, current));
123 }
124 let current_cost = cost_so_far[index(current)];
125 for step in NEIGHBORS {
126 let next = current + step;
127 let Some(terrain_cost) = map.step_cost(current, next) else {
128 continue;
129 };
130 let diagonal = step.x != 0 && step.y != 0;
131 if diagonal
132 && (map.move_cost(current + IVec2::new(step.x, 0)).is_none()
133 || map.move_cost(current + IVec2::new(0, step.y)).is_none())
134 {
135 continue;
136 }
137 let base = if diagonal { DIAGONAL } else { STRAIGHT };
138 let new_cost = current_cost + base * terrain_cost;
139 if new_cost < cost_so_far[index(next)] {
140 cost_so_far[index(next)] = new_cost;
141 came_from[index(next)] = Some(current);
142 open.push(Reverse((new_cost + heuristic(next), next.x, next.y)));
143 }
144 }
145 }
146 None
147 }
148
149 fn octile(a: IVec2, b: IVec2) -> u32 {
150 let d = (a - b).abs();
151 let (min, max) = (d.x.min(d.y) as u32, d.x.max(d.y) as u32);
152 (DIAGONAL * min + STRAIGHT * (max - min)) * MIN_TERRAIN_COST
153 }
154
155 fn reconstruct(
156 came_from: &[Option<IVec2>],
157 index: impl Fn(IVec2) -> usize,
158 start: IVec2,
159 goal: IVec2,
160 ) -> Vec<UVec2> {
161 let mut path = vec![goal.as_uvec2()];
162 let mut current = goal;
163 while let Some(prev) = came_from[index(current)] {
164 if prev == start {
165 break;
166 }
167 path.push(prev.as_uvec2());
168 current = prev;
169 }
170 path.reverse();
171 path
172 }
173
174 #[cfg(test)]
175 mod tests {
176 use super::*;
177 use crate::world::Terrain;
178
179 // Idle colonists look for food and cooking input several times per game minute. A stack far
180 // beyond what the flood fill can cover made every one of those searches visit the full tile
181 // budget and fail, which dominated frame time at high speeds.
182 #[test]
183 fn target_searches_skip_targets_beyond_their_reach() {
184 let map = WorldMap::filled(UVec2::splat(300), Terrain::Grass);
185 let start = UVec2::splat(10);
186 let max_tiles = 400;
187 let near = start + UVec2::new(search_reach(max_tiles) / 4, 0);
188 let far = start + UVec2::new(search_reach(max_tiles) + 1, 0);
189 assert_eq!(
190 nearest_of(&map, start, max_tiles, &HashSet::from([far])),
191 None
192 );
193 assert_eq!(
194 nearest_of(&map, start, max_tiles, &HashSet::from([far, near])),
195 Some(near)
196 );
197 }
198
199 fn map_from(rows: &[&str]) -> WorldMap {
200 let size = UVec2::new(rows[0].len() as u32, rows.len() as u32);
201 let mut map = WorldMap::filled(size, Terrain::Grass);
202 for (row, line) in rows.iter().enumerate() {
203 let y = size.y - 1 - row as u32;
204 for (x, c) in line.chars().enumerate() {
205 let terrain = match c {
206 '#' => Terrain::Stone,
207 '~' => Terrain::ShallowWater,
208 _ => Terrain::Grass,
209 };
210 map.set(UVec2::new(x as u32, y), terrain);
211 }
212 }
213 map
214 }
215
216 fn is_connected(start: UVec2, path: &[UVec2]) -> bool {
217 std::iter::once(start)
218 .chain(path.iter().copied())
219 .collect::<Vec<_>>()
220 .windows(2)
221 .all(|w| (w[0].as_ivec2() - w[1].as_ivec2()).abs().max_element() == 1)
222 }
223
224 #[test]
225 fn path_excludes_start_and_ends_at_goal() {
226 let map = map_from(&["....."]);
227 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
228 assert_eq!(
229 path,
230 vec![
231 UVec2::new(1, 0),
232 UVec2::new(2, 0),
233 UVec2::new(3, 0),
234 UVec2::new(4, 0)
235 ]
236 );
237 }
238
239 #[test]
240 fn path_to_self_is_empty() {
241 let map = map_from(&["..."]);
242 assert_eq!(find_path(&map, UVec2::ZERO, UVec2::ZERO), Some(vec![]));
243 }
244
245 #[test]
246 fn path_goes_around_walls() {
247 let map = map_from(&[
248 ".....", //
249 ".###.", ".#.#.", "...#.",
250 ]);
251 let start = UVec2::new(2, 1);
252 let goal = UVec2::new(4, 0);
253 let path = find_path(&map, start, goal).unwrap();
254 assert!(is_connected(start, &path));
255 assert_eq!(path.last(), Some(&goal));
256 assert!(path.iter().all(|p| map.move_cost(p.as_ivec2()).is_some()));
257 }
258
259 #[test]
260 fn unreachable_or_impassable_goal_has_no_path() {
261 let map = map_from(&[
262 "..#..", //
263 "..#..",
264 ]);
265 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)), None);
266 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(2, 0)), None);
267 }
268
269 // Colonists move in straight lines between tile centers, so a diagonal step between two
270 // blocked orthogonal neighbours would visually clip through the corners of walls.
271 #[test]
272 fn diagonal_steps_never_cut_wall_corners() {
273 let map = map_from(&[
274 "#.", //
275 ".#",
276 ]);
277 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(1, 1)), None);
278 let map = map_from(&[
279 "..", //
280 "#.",
281 ]);
282 let path = find_path(&map, UVec2::new(0, 1), UVec2::new(1, 0)).unwrap();
283 assert_eq!(path, vec![UVec2::new(1, 1), UVec2::new(1, 0)]);
284 }
285
286 #[test]
287 fn prefers_detour_over_slow_terrain() {
288 let map = map_from(&[
289 ".....", //
290 "..~..",
291 ]);
292 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
293 assert!(!path.contains(&UVec2::new(2, 0)));
294 }
295
296 #[test]
297 fn walks_around_a_hill_rather_than_over_it() {
298 let mut map = map_from(&[
299 ".....", //
300 ".....",
301 ]);
302 map.set_height(UVec2::new(2, 0), 10.0);
303 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
304 assert!(!path.contains(&UVec2::new(2, 0)));
305 }
306
307 #[test]
308 fn detours_along_paths_when_they_are_faster() {
309 let mut map = map_from(&[
310 "..........", //
311 "..........",
312 ]);
313 for x in 0..10 {
314 map.set(UVec2::new(x, 1), Terrain::Paved);
315 }
316 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(9, 0)).unwrap();
317 assert!(path.iter().filter(|p| p.y == 1).count() >= 6);
318 }
319
320 // A wall can go up on a tile a colonist is standing on (or just about to enter); routes must
321 // still start from that tile so nobody gets permanently stuck inside a wall.
322 #[test]
323 fn colonists_can_walk_out_of_a_blocked_tile() {
324 let mut map = map_from(&["....."]);
325 map.set_wall(UVec2::new(2, 0), true);
326 let path = find_path(&map, UVec2::new(2, 0), UVec2::new(4, 0)).unwrap();
327 assert_eq!(path, vec![UVec2::new(3, 0), UVec2::new(4, 0)]);
328 }
329
330 #[test]
331 fn crosses_slow_terrain_when_it_is_the_only_way() {
332 let map = map_from(&["..~.."]);
333 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
334 assert!(path.contains(&UVec2::new(2, 0)));
335 }
336
337 #[test]
338 fn path_adjacent_stops_next_to_target() {
339 let map = map_from(&["......"]);
340 let path = find_path_adjacent(&map, UVec2::new(0, 0), UVec2::new(5, 0)).unwrap();
341 assert_eq!(path.last(), Some(&UVec2::new(4, 0)));
342 }
343
344 #[test]
345 fn path_adjacent_works_for_impassable_targets() {
346 let map = map_from(&["...#"]);
347 let path = find_path_adjacent(&map, UVec2::new(0, 0), UVec2::new(3, 0)).unwrap();
348 assert_eq!(path.last(), Some(&UVec2::new(2, 0)));
349 }
350
351 #[test]
352 fn path_adjacent_from_neighbor_is_empty() {
353 let map = map_from(&["..."]);
354 let path = find_path_adjacent(&map, UVec2::new(1, 0), UVec2::new(2, 0));
355 assert_eq!(path, Some(vec![]));
356 }
357
358 #[test]
359 fn path_adjacent_to_enclosed_target_is_none() {
360 let walled = map_from(&[
361 "#####", //
362 "#...#", "#####", ".....",
363 ]);
364 assert_eq!(
365 find_path_adjacent(&walled, UVec2::new(0, 0), UVec2::new(2, 2)),
366 None
367 );
368 }
369
370 #[test]
371 fn nearest_tile_finds_closest_match_by_walking_distance() {
372 let map = map_from(&[
373 ".#...", //
374 ".#.#.", "...#.",
375 ]);
376 let found = nearest_tile(&map, UVec2::new(0, 0), 100, |p| p.x == 4);
377 assert_eq!(found, Some(UVec2::new(4, 2)));
378 assert_eq!(
379 nearest_tile(&map, UVec2::new(0, 0), 100, |p| p.x == 9),
380 None
381 );
382 }
383
384 #[test]
385 fn finds_paths_across_a_generated_world() {
386 let map = crate::worldgen::generate(crate::worldgen::WORLD_SIZE, 1);
387 let region = crate::worldgen::largest_region(&map);
388 let (start, goal) = (region[0], region[region.len() - 1]);
389 let path = find_path(&map, start, goal).unwrap();
390 assert!(is_connected(start, &path));
391 assert_eq!(path.last(), Some(&goal));
392 }
393 }