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