src/pathfinding.rs 19.5 K · 601 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 can_step(map: &WorldMap, from: IVec2, step: IVec2) -> bool {
46 let next = from + step;
47 map.move_cost(next).is_some()
48 && (step.x == 0
49 || step.y == 0
50 || (map.move_cost(from + IVec2::new(step.x, 0)).is_some()
51 && map.move_cost(from + IVec2::new(0, step.y)).is_some()))
52 }
53
54 pub fn nearest_tile(
55 map: &WorldMap,
56 start: UVec2,
57 max_tiles: usize,
58 mut is_target: impl FnMut(UVec2) -> bool,
59 ) -> Option<UVec2> {
60 let index = |p: UVec2| (p.y * map.size.x + p.x) as usize;
61 let mut seen = vec![false; (map.size.x * map.size.y) as usize];
62 let mut seen_count = 1;
63 if let Some(cell) = seen.get_mut(index(start)) {
64 *cell = true;
65 }
66 let mut queue = VecDeque::from([start]);
67 while let Some(pos) = queue.pop_front() {
68 if is_target(pos) {
69 return Some(pos);
70 }
71 if seen_count >= max_tiles {
72 continue;
73 }
74 for step in NEIGHBORS {
75 let next = pos.as_ivec2() + step;
76 if can_step(map, pos.as_ivec2(), step) && !seen[index(next.as_uvec2())] {
77 seen[index(next.as_uvec2())] = true;
78 seen_count += 1;
79 queue.push_back(next.as_uvec2());
80 }
81 }
82 }
83 None
84 }
85
86 #[derive(Default, Debug, Clone)]
87 pub struct Reach {
88 width: u32,
89 seen: Vec<bool>,
90 }
91
92 #[derive(Resource, Default, Debug, Clone)]
93 pub struct ColonyReach(pub Option<Reach>);
94
95 impl ColonyReach {
96 pub fn allows(&self, tile: UVec2) -> bool {
97 self.0.as_ref().is_none_or(|reach| reach.contains(tile))
98 }
99
100 pub fn allows_near(&self, tile: UVec2) -> bool {
101 self.0.as_ref().is_none_or(|reach| reach.near(tile))
102 }
103
104 pub fn nearest_unbounded(&self, from: UVec2, targets: &HashSet<UVec2>) -> Option<UVec2> {
105 targets
106 .iter()
107 .filter(|t| self.allows(**t))
108 .min_by_key(|t| {
109 let d = t.as_ivec2() - from.as_ivec2();
110 (d.length_squared(), t.y, t.x)
111 })
112 .copied()
113 }
114
115 pub fn nearest(
116 &self,
117 map: &WorldMap,
118 from: UVec2,
119 max_tiles: usize,
120 targets: &HashSet<UVec2>,
121 ) -> Option<UVec2> {
122 match &self.0 {
123 Some(reach) if reach.contains(from) => targets
124 .iter()
125 .filter(|t| reach.contains(**t))
126 .min_by_key(|t| {
127 let d = t.as_ivec2() - from.as_ivec2();
128 (d.length_squared(), t.y, t.x)
129 })
130 .copied(),
131 _ => nearest_of(map, from, max_tiles, targets),
132 }
133 }
134 }
135
136 impl Reach {
137 pub fn flood(map: &WorldMap, start: UVec2) -> Self {
138 let width = map.size.x;
139 let mut seen = vec![false; map.size.element_product() as usize];
140 let index = |p: UVec2| (p.y * width + p.x) as usize;
141 if map.move_cost(start.as_ivec2()).is_some() {
142 seen[index(start)] = true;
143 let mut queue = VecDeque::from([start]);
144 while let Some(pos) = queue.pop_front() {
145 for step in NEIGHBORS {
146 let next = pos.as_ivec2() + step;
147 if can_step(map, pos.as_ivec2(), step) && !seen[index(next.as_uvec2())] {
148 seen[index(next.as_uvec2())] = true;
149 queue.push_back(next.as_uvec2());
150 }
151 }
152 }
153 }
154 Self { width, seen }
155 }
156
157 pub fn contains(&self, p: UVec2) -> bool {
158 p.x < self.width
159 && self
160 .seen
161 .get((p.y * self.width + p.x) as usize)
162 .copied()
163 .unwrap_or(false)
164 }
165
166 pub fn near(&self, p: UVec2) -> bool {
167 (-1..=1).any(|dy| {
168 (-1..=1).any(|dx| {
169 let q = p.as_ivec2() + IVec2::new(dx, dy);
170 q.min_element() >= 0 && self.contains(q.as_uvec2())
171 })
172 })
173 }
174 }
175
176 pub fn search_reach(max_tiles: usize) -> u32 {
177 ((max_tiles as f32).sqrt() / 2.0).ceil() as u32
178 }
179
180 pub fn nearest_of(
181 map: &WorldMap,
182 start: UVec2,
183 max_tiles: usize,
184 targets: &HashSet<UVec2>,
185 ) -> Option<UVec2> {
186 let reach = search_reach(max_tiles) as i32;
187 let in_reach = |p: &&UVec2| (p.as_ivec2() - start.as_ivec2()).abs().max_element() <= reach;
188 if !targets.iter().any(|p| in_reach(&p)) {
189 return None;
190 }
191 nearest_tile(map, start, max_tiles, |p| targets.contains(&p))
192 }
193
194 fn octile(a: IVec2, b: IVec2) -> u32 {
195 let d = (a - b).abs();
196 let (min, max) = (d.x.min(d.y) as u32, d.x.max(d.y) as u32);
197 (DIAGONAL * min + STRAIGHT * (max - min)) * MIN_TERRAIN_COST
198 }
199
200 #[derive(Default)]
201 struct Scratch {
202 generation: u32,
203 stamp: Vec<u32>,
204 cost: Vec<u32>,
205 from: Vec<IVec2>,
206 open: BinaryHeap<Reverse<(u32, i32, i32)>>,
207 }
208
209 thread_local! {
210 static SCRATCH: std::cell::RefCell<Scratch> = std::cell::RefCell::new(Scratch::default());
211 }
212
213 fn astar(
214 map: &WorldMap,
215 start: UVec2,
216 is_goal: impl Fn(IVec2) -> bool,
217 heuristic: impl Fn(IVec2) -> u32,
218 ) -> Option<Vec<UVec2>> {
219 astar_limited(map, start, is_goal, heuristic, usize::MAX)
220 }
221
222 fn astar_limited(
223 map: &WorldMap,
224 start: UVec2,
225 is_goal: impl Fn(IVec2) -> bool,
226 heuristic: impl Fn(IVec2) -> u32,
227 max_expanded: usize,
228 ) -> Option<Vec<UVec2>> {
229 let start = start.as_ivec2();
230 if !map.in_bounds(start) {
231 return None;
232 }
233 if is_goal(start) {
234 return Some(vec![]);
235 }
236 SCRATCH.with_borrow_mut(|scratch| {
237 let tiles = map.size.element_product() as usize;
238 if scratch.stamp.len() != tiles {
239 *scratch = Scratch {
240 stamp: vec![0; tiles],
241 cost: vec![0; tiles],
242 from: vec![IVec2::ZERO; tiles],
243 ..default()
244 };
245 }
246 scratch.generation = scratch.generation.wrapping_add(1);
247 if scratch.generation == 0 {
248 scratch.stamp.fill(0);
249 scratch.generation = 1;
250 }
251 let Scratch {
252 generation,
253 stamp,
254 cost,
255 from,
256 open,
257 } = scratch;
258 let generation = *generation;
259 let width = map.size.x as usize;
260 let index = |p: IVec2| p.y as usize * width + p.x as usize;
261 open.clear();
262 stamp[index(start)] = generation;
263 cost[index(start)] = 0;
264 from[index(start)] = start;
265 open.push(Reverse((heuristic(start), start.x, start.y)));
266 let mut expanded = 0;
267 while let Some(Reverse((_, x, y))) = open.pop() {
268 let current = IVec2::new(x, y);
269 if is_goal(current) {
270 let mut path = vec![current.as_uvec2()];
271 let mut at = current;
272 while from[index(at)] != start {
273 at = from[index(at)];
274 path.push(at.as_uvec2());
275 }
276 path.reverse();
277 return Some(path);
278 }
279 expanded += 1;
280 if expanded > max_expanded {
281 return None;
282 }
283 let current_cost = cost[index(current)];
284 for step in NEIGHBORS {
285 let next = current + step;
286 if !can_step(map, current, step) {
287 continue;
288 }
289 let Some(terrain_cost) = map.step_cost(current, next) else {
290 continue;
291 };
292 let diagonal = step.x != 0 && step.y != 0;
293 let base = if diagonal { DIAGONAL } else { STRAIGHT };
294 let new_cost = current_cost + base * terrain_cost;
295 let i = index(next);
296 if stamp[i] != generation || new_cost < cost[i] {
297 stamp[i] = generation;
298 cost[i] = new_cost;
299 from[i] = current;
300 open.push(Reverse((new_cost + heuristic(next), next.x, next.y)));
301 }
302 }
303 }
304 None
305 })
306 }
307
308 pub const WANDER_SEARCH: usize = 800;
309
310 pub fn find_nearby_path(
311 map: &WorldMap,
312 start: UVec2,
313 goal: UVec2,
314 max_expanded: usize,
315 ) -> Option<Vec<UVec2>> {
316 map.move_cost(goal.as_ivec2())?;
317 let goal = goal.as_ivec2();
318 astar_limited(map, start, |p| p == goal, |p| octile(p, goal), max_expanded)
319 }
320
321 #[cfg(test)]
322 mod tests {
323 use super::*;
324 use crate::world::Terrain;
325
326 // Inside the colony's flooded reach, the nearest reachable target by distance stands in for
327 // a fresh flood fill per search; with 40+ colonists those floods dominated job choice.
328 #[test]
329 fn colony_reach_picks_the_nearest_reachable_target() {
330 let mut map = WorldMap::filled(UVec2::splat(30), Terrain::Grass);
331 for y in 0..30 {
332 map.set(UVec2::new(10, y), Terrain::DeepWater);
333 }
334 let reach = ColonyReach(Some(Reach::flood(&map, UVec2::new(5, 5))));
335 let across = UVec2::new(12, 5);
336 let far = UVec2::new(5, 20);
337 let targets = HashSet::from([across, far]);
338 assert_eq!(
339 reach.nearest(&map, UVec2::new(5, 5), 100, &targets),
340 Some(far)
341 );
342 let elsewhere = UVec2::new(20, 5);
343 assert_eq!(reach.nearest(&map, elsewhere, 1000, &targets), Some(across));
344 }
345
346 // Wandering picks random nearby goals; one walled off by a river or cliff made A* search the
347 // whole connected region before failing. Short trips cap the search instead.
348 #[test]
349 fn nearby_paths_give_up_early_on_unreachable_goals() {
350 let mut map = WorldMap::filled(UVec2::splat(100), Terrain::Grass);
351 for y in 0..100 {
352 map.set(UVec2::new(50, y), Terrain::DeepWater);
353 }
354 let start = UVec2::new(45, 50);
355 assert!(find_nearby_path(&map, start, UVec2::new(40, 52), 200).is_some());
356 assert_eq!(find_nearby_path(&map, start, UVec2::new(55, 50), 200), None);
357 assert_eq!(find_path(&map, start, UVec2::new(55, 50)), None);
358 }
359
360 // Target searches must use the same no-corner-cutting rule as A*. A stack on a tile touching
361 // the colony only diagonally between two rocks was "found" by the search, A* then explored
362 // the whole map before failing, and the job was picked again at once: ~10 ms, many times a
363 // second, which made Max speed crawl on a 50-day colony.
364 #[test]
365 fn searches_never_pick_targets_that_paths_cannot_reach() {
366 let mut map = WorldMap::filled(UVec2::splat(6), Terrain::Grass);
367 for y in 3..6 {
368 for x in 0..6 {
369 map.set(UVec2::new(x, y), Terrain::Stone);
370 }
371 }
372 map.set(UVec2::new(2, 3), Terrain::Grass);
373 map.set(UVec2::new(3, 4), Terrain::Gravel);
374 let pocket = UVec2::new(3, 4);
375 assert!(map.move_cost(IVec2::new(2, 3)).is_some());
376 assert_eq!(find_path(&map, UVec2::ZERO, pocket), None);
377 let targets = HashSet::from([pocket]);
378 assert_eq!(nearest_of(&map, UVec2::ZERO, 100, &targets), None);
379 }
380
381 // Idle colonists look for food and cooking input several times per game minute. A stack far
382 // beyond what the flood fill can cover made every one of those searches visit the full tile
383 // budget and fail, which dominated frame time at high speeds. The reach is a radius: the
384 // flood fills a square, so its half-width (not its width, as it once was) bounds it.
385 #[test]
386 fn target_searches_skip_targets_beyond_their_reach() {
387 let map = WorldMap::filled(UVec2::splat(300), Terrain::Grass);
388 let start = UVec2::splat(10);
389 let max_tiles = 400;
390 let near = start + UVec2::new(search_reach(max_tiles) / 4, 0);
391 let far = start + UVec2::new(search_reach(max_tiles) + 1, 0);
392 assert_eq!(
393 nearest_of(&map, start, max_tiles, &HashSet::from([far])),
394 None
395 );
396 assert_eq!(
397 nearest_of(&map, start, max_tiles, &HashSet::from([far, near])),
398 Some(near)
399 );
400 let edge = start + UVec2::new(search_reach(max_tiles) - 1, 0);
401 assert_eq!(
402 nearest_of(&map, start, max_tiles, &HashSet::from([edge])),
403 Some(edge)
404 );
405 }
406
407 fn map_from(rows: &[&str]) -> WorldMap {
408 let size = UVec2::new(rows[0].len() as u32, rows.len() as u32);
409 let mut map = WorldMap::filled(size, Terrain::Grass);
410 for (row, line) in rows.iter().enumerate() {
411 let y = size.y - 1 - row as u32;
412 for (x, c) in line.chars().enumerate() {
413 let terrain = match c {
414 '#' => Terrain::Stone,
415 '~' => Terrain::ShallowWater,
416 _ => Terrain::Grass,
417 };
418 map.set(UVec2::new(x as u32, y), terrain);
419 }
420 }
421 map
422 }
423
424 fn is_connected(start: UVec2, path: &[UVec2]) -> bool {
425 std::iter::once(start)
426 .chain(path.iter().copied())
427 .collect::<Vec<_>>()
428 .windows(2)
429 .all(|w| (w[0].as_ivec2() - w[1].as_ivec2()).abs().max_element() == 1)
430 }
431
432 #[test]
433 fn path_excludes_start_and_ends_at_goal() {
434 let map = map_from(&["....."]);
435 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
436 assert_eq!(
437 path,
438 vec![
439 UVec2::new(1, 0),
440 UVec2::new(2, 0),
441 UVec2::new(3, 0),
442 UVec2::new(4, 0)
443 ]
444 );
445 }
446
447 #[test]
448 fn path_to_self_is_empty() {
449 let map = map_from(&["..."]);
450 assert_eq!(find_path(&map, UVec2::ZERO, UVec2::ZERO), Some(vec![]));
451 }
452
453 #[test]
454 fn path_goes_around_walls() {
455 let map = map_from(&[
456 ".....", //
457 ".###.", ".#.#.", "...#.",
458 ]);
459 let start = UVec2::new(2, 1);
460 let goal = UVec2::new(4, 0);
461 let path = find_path(&map, start, goal).unwrap();
462 assert!(is_connected(start, &path));
463 assert_eq!(path.last(), Some(&goal));
464 assert!(path.iter().all(|p| map.move_cost(p.as_ivec2()).is_some()));
465 }
466
467 #[test]
468 fn unreachable_or_impassable_goal_has_no_path() {
469 let map = map_from(&[
470 "..#..", //
471 "..#..",
472 ]);
473 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)), None);
474 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(2, 0)), None);
475 }
476
477 // Colonists move in straight lines between tile centers, so a diagonal step between two
478 // blocked orthogonal neighbours would visually clip through the corners of walls.
479 #[test]
480 fn diagonal_steps_never_cut_wall_corners() {
481 let map = map_from(&[
482 "#.", //
483 ".#",
484 ]);
485 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(1, 1)), None);
486 let map = map_from(&[
487 "..", //
488 "#.",
489 ]);
490 let path = find_path(&map, UVec2::new(0, 1), UVec2::new(1, 0)).unwrap();
491 assert_eq!(path, vec![UVec2::new(1, 1), UVec2::new(1, 0)]);
492 }
493
494 #[test]
495 fn prefers_detour_over_slow_terrain() {
496 let map = map_from(&[
497 ".....", //
498 "..~..",
499 ]);
500 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
501 assert!(!path.contains(&UVec2::new(2, 0)));
502 }
503
504 #[test]
505 fn walks_around_a_hill_rather_than_over_it() {
506 let mut map = map_from(&[
507 ".....", //
508 ".....",
509 ]);
510 map.set_height(UVec2::new(2, 0), 10.0);
511 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
512 assert!(!path.contains(&UVec2::new(2, 0)));
513 }
514
515 #[test]
516 fn detours_along_paths_when_they_are_faster() {
517 let mut map = map_from(&[
518 "..........", //
519 "..........",
520 ]);
521 for x in 0..10 {
522 map.set(UVec2::new(x, 1), Terrain::Paved);
523 }
524 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(9, 0)).unwrap();
525 assert!(path.iter().filter(|p| p.y == 1).count() >= 6);
526 }
527
528 // A wall can go up on a tile a colonist is standing on (or just about to enter); routes must
529 // still start from that tile so nobody gets permanently stuck inside a wall.
530 #[test]
531 fn colonists_can_walk_out_of_a_blocked_tile() {
532 let mut map = map_from(&["....."]);
533 map.set_wall(UVec2::new(2, 0), true);
534 let path = find_path(&map, UVec2::new(2, 0), UVec2::new(4, 0)).unwrap();
535 assert_eq!(path, vec![UVec2::new(3, 0), UVec2::new(4, 0)]);
536 }
537
538 #[test]
539 fn crosses_slow_terrain_when_it_is_the_only_way() {
540 let map = map_from(&["..~.."]);
541 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
542 assert!(path.contains(&UVec2::new(2, 0)));
543 }
544
545 #[test]
546 fn path_adjacent_stops_next_to_target() {
547 let map = map_from(&["......"]);
548 let path = find_path_adjacent(&map, UVec2::new(0, 0), UVec2::new(5, 0)).unwrap();
549 assert_eq!(path.last(), Some(&UVec2::new(4, 0)));
550 }
551
552 #[test]
553 fn path_adjacent_works_for_impassable_targets() {
554 let map = map_from(&["...#"]);
555 let path = find_path_adjacent(&map, UVec2::new(0, 0), UVec2::new(3, 0)).unwrap();
556 assert_eq!(path.last(), Some(&UVec2::new(2, 0)));
557 }
558
559 #[test]
560 fn path_adjacent_from_neighbor_is_empty() {
561 let map = map_from(&["..."]);
562 let path = find_path_adjacent(&map, UVec2::new(1, 0), UVec2::new(2, 0));
563 assert_eq!(path, Some(vec![]));
564 }
565
566 #[test]
567 fn path_adjacent_to_enclosed_target_is_none() {
568 let walled = map_from(&[
569 "#####", //
570 "#...#", "#####", ".....",
571 ]);
572 assert_eq!(
573 find_path_adjacent(&walled, UVec2::new(0, 0), UVec2::new(2, 2)),
574 None
575 );
576 }
577
578 #[test]
579 fn nearest_tile_finds_closest_match_by_walking_distance() {
580 let map = map_from(&[
581 ".#...", //
582 ".#.#.", "...#.",
583 ]);
584 let found = nearest_tile(&map, UVec2::new(0, 0), 100, |p| p.x == 4);
585 assert_eq!(found, Some(UVec2::new(4, 2)));
586 assert_eq!(
587 nearest_tile(&map, UVec2::new(0, 0), 100, |p| p.x == 9),
588 None
589 );
590 }
591
592 #[test]
593 fn finds_paths_across_a_generated_world() {
594 let map = crate::worldgen::generate(crate::worldgen::WORLD_SIZE, 1);
595 let region = crate::worldgen::largest_region(&map);
596 let (start, goal) = (region[0], region[region.len() - 1]);
597 let path = find_path(&map, start, goal).unwrap();
598 assert!(is_connected(start, &path));
599 assert_eq!(path.last(), Some(&goal));
600 }
601 }