src/pathfinding.rs 14.8 K · 465 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 pub struct Reach {
87 width: u32,
88 seen: Vec<bool>,
89 }
90
91 impl Reach {
92 pub fn flood(map: &WorldMap, start: UVec2) -> Self {
93 let width = map.size.x;
94 let mut seen = vec![false; map.size.element_product() as usize];
95 let index = |p: UVec2| (p.y * width + p.x) as usize;
96 if map.move_cost(start.as_ivec2()).is_some() {
97 seen[index(start)] = true;
98 let mut queue = VecDeque::from([start]);
99 while let Some(pos) = queue.pop_front() {
100 for step in NEIGHBORS {
101 let next = pos.as_ivec2() + step;
102 if can_step(map, pos.as_ivec2(), step) && !seen[index(next.as_uvec2())] {
103 seen[index(next.as_uvec2())] = true;
104 queue.push_back(next.as_uvec2());
105 }
106 }
107 }
108 }
109 Self { width, seen }
110 }
111
112 pub fn contains(&self, p: UVec2) -> bool {
113 p.x < self.width
114 && self
115 .seen
116 .get((p.y * self.width + p.x) as usize)
117 .copied()
118 .unwrap_or(false)
119 }
120
121 pub fn near(&self, p: UVec2) -> bool {
122 (-1..=1).any(|dy| {
123 (-1..=1).any(|dx| {
124 let q = p.as_ivec2() + IVec2::new(dx, dy);
125 q.min_element() >= 0 && self.contains(q.as_uvec2())
126 })
127 })
128 }
129 }
130
131 pub fn search_reach(max_tiles: usize) -> u32 {
132 (max_tiles as f32).sqrt().ceil() as u32
133 }
134
135 pub fn nearest_of(
136 map: &WorldMap,
137 start: UVec2,
138 max_tiles: usize,
139 targets: &HashSet<UVec2>,
140 ) -> Option<UVec2> {
141 let reach = search_reach(max_tiles) as i32;
142 let in_reach = |p: &&UVec2| (p.as_ivec2() - start.as_ivec2()).abs().max_element() <= reach;
143 if !targets.iter().any(|p| in_reach(&p)) {
144 return None;
145 }
146 nearest_tile(map, start, max_tiles, |p| targets.contains(&p))
147 }
148
149 fn astar(
150 map: &WorldMap,
151 start: UVec2,
152 is_goal: impl Fn(IVec2) -> bool,
153 heuristic: impl Fn(IVec2) -> u32,
154 ) -> Option<Vec<UVec2>> {
155 let start = start.as_ivec2();
156 if !map.in_bounds(start) {
157 return None;
158 }
159 if is_goal(start) {
160 return Some(vec![]);
161 }
162
163 let width = map.size.x as usize;
164 let index = |p: IVec2| p.y as usize * width + p.x as usize;
165 let tiles = map.size.element_product() as usize;
166 let mut cost_so_far = vec![u32::MAX; tiles];
167 let mut came_from = vec![None::<IVec2>; tiles];
168 let mut open = BinaryHeap::new();
169
170 cost_so_far[index(start)] = 0;
171 open.push(Reverse((heuristic(start), start.x, start.y)));
172
173 while let Some(Reverse((_, x, y))) = open.pop() {
174 let current = IVec2::new(x, y);
175 if is_goal(current) {
176 return Some(reconstruct(&came_from, index, start, current));
177 }
178 let current_cost = cost_so_far[index(current)];
179 for step in NEIGHBORS {
180 let next = current + step;
181 if !can_step(map, current, step) {
182 continue;
183 }
184 let Some(terrain_cost) = map.step_cost(current, next) else {
185 continue;
186 };
187 let diagonal = step.x != 0 && step.y != 0;
188 let base = if diagonal { DIAGONAL } else { STRAIGHT };
189 let new_cost = current_cost + base * terrain_cost;
190 if new_cost < cost_so_far[index(next)] {
191 cost_so_far[index(next)] = new_cost;
192 came_from[index(next)] = Some(current);
193 open.push(Reverse((new_cost + heuristic(next), next.x, next.y)));
194 }
195 }
196 }
197 None
198 }
199
200 fn octile(a: IVec2, b: IVec2) -> u32 {
201 let d = (a - b).abs();
202 let (min, max) = (d.x.min(d.y) as u32, d.x.max(d.y) as u32);
203 (DIAGONAL * min + STRAIGHT * (max - min)) * MIN_TERRAIN_COST
204 }
205
206 fn reconstruct(
207 came_from: &[Option<IVec2>],
208 index: impl Fn(IVec2) -> usize,
209 start: IVec2,
210 goal: IVec2,
211 ) -> Vec<UVec2> {
212 let mut path = vec![goal.as_uvec2()];
213 let mut current = goal;
214 while let Some(prev) = came_from[index(current)] {
215 if prev == start {
216 break;
217 }
218 path.push(prev.as_uvec2());
219 current = prev;
220 }
221 path.reverse();
222 path
223 }
224
225 #[cfg(test)]
226 mod tests {
227 use super::*;
228 use crate::world::Terrain;
229
230 // Target searches must use the same no-corner-cutting rule as A*. A stack on a tile touching
231 // the colony only diagonally between two rocks was "found" by the search, A* then explored
232 // the whole map before failing, and the job was picked again at once: ~10 ms, many times a
233 // second, which made Max speed crawl on a 50-day colony.
234 #[test]
235 fn searches_never_pick_targets_that_paths_cannot_reach() {
236 let mut map = WorldMap::filled(UVec2::splat(6), Terrain::Grass);
237 for y in 3..6 {
238 for x in 0..6 {
239 map.set(UVec2::new(x, y), Terrain::Stone);
240 }
241 }
242 map.set(UVec2::new(2, 3), Terrain::Grass);
243 map.set(UVec2::new(3, 4), Terrain::Gravel);
244 let pocket = UVec2::new(3, 4);
245 assert!(map.move_cost(IVec2::new(2, 3)).is_some());
246 assert_eq!(find_path(&map, UVec2::ZERO, pocket), None);
247 let targets = HashSet::from([pocket]);
248 assert_eq!(nearest_of(&map, UVec2::ZERO, 100, &targets), None);
249 }
250
251 // Idle colonists look for food and cooking input several times per game minute. A stack far
252 // beyond what the flood fill can cover made every one of those searches visit the full tile
253 // budget and fail, which dominated frame time at high speeds.
254 #[test]
255 fn target_searches_skip_targets_beyond_their_reach() {
256 let map = WorldMap::filled(UVec2::splat(300), Terrain::Grass);
257 let start = UVec2::splat(10);
258 let max_tiles = 400;
259 let near = start + UVec2::new(search_reach(max_tiles) / 4, 0);
260 let far = start + UVec2::new(search_reach(max_tiles) + 1, 0);
261 assert_eq!(
262 nearest_of(&map, start, max_tiles, &HashSet::from([far])),
263 None
264 );
265 assert_eq!(
266 nearest_of(&map, start, max_tiles, &HashSet::from([far, near])),
267 Some(near)
268 );
269 }
270
271 fn map_from(rows: &[&str]) -> WorldMap {
272 let size = UVec2::new(rows[0].len() as u32, rows.len() as u32);
273 let mut map = WorldMap::filled(size, Terrain::Grass);
274 for (row, line) in rows.iter().enumerate() {
275 let y = size.y - 1 - row as u32;
276 for (x, c) in line.chars().enumerate() {
277 let terrain = match c {
278 '#' => Terrain::Stone,
279 '~' => Terrain::ShallowWater,
280 _ => Terrain::Grass,
281 };
282 map.set(UVec2::new(x as u32, y), terrain);
283 }
284 }
285 map
286 }
287
288 fn is_connected(start: UVec2, path: &[UVec2]) -> bool {
289 std::iter::once(start)
290 .chain(path.iter().copied())
291 .collect::<Vec<_>>()
292 .windows(2)
293 .all(|w| (w[0].as_ivec2() - w[1].as_ivec2()).abs().max_element() == 1)
294 }
295
296 #[test]
297 fn path_excludes_start_and_ends_at_goal() {
298 let map = map_from(&["....."]);
299 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
300 assert_eq!(
301 path,
302 vec![
303 UVec2::new(1, 0),
304 UVec2::new(2, 0),
305 UVec2::new(3, 0),
306 UVec2::new(4, 0)
307 ]
308 );
309 }
310
311 #[test]
312 fn path_to_self_is_empty() {
313 let map = map_from(&["..."]);
314 assert_eq!(find_path(&map, UVec2::ZERO, UVec2::ZERO), Some(vec![]));
315 }
316
317 #[test]
318 fn path_goes_around_walls() {
319 let map = map_from(&[
320 ".....", //
321 ".###.", ".#.#.", "...#.",
322 ]);
323 let start = UVec2::new(2, 1);
324 let goal = UVec2::new(4, 0);
325 let path = find_path(&map, start, goal).unwrap();
326 assert!(is_connected(start, &path));
327 assert_eq!(path.last(), Some(&goal));
328 assert!(path.iter().all(|p| map.move_cost(p.as_ivec2()).is_some()));
329 }
330
331 #[test]
332 fn unreachable_or_impassable_goal_has_no_path() {
333 let map = map_from(&[
334 "..#..", //
335 "..#..",
336 ]);
337 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)), None);
338 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(2, 0)), None);
339 }
340
341 // Colonists move in straight lines between tile centers, so a diagonal step between two
342 // blocked orthogonal neighbours would visually clip through the corners of walls.
343 #[test]
344 fn diagonal_steps_never_cut_wall_corners() {
345 let map = map_from(&[
346 "#.", //
347 ".#",
348 ]);
349 assert_eq!(find_path(&map, UVec2::new(0, 0), UVec2::new(1, 1)), None);
350 let map = map_from(&[
351 "..", //
352 "#.",
353 ]);
354 let path = find_path(&map, UVec2::new(0, 1), UVec2::new(1, 0)).unwrap();
355 assert_eq!(path, vec![UVec2::new(1, 1), UVec2::new(1, 0)]);
356 }
357
358 #[test]
359 fn prefers_detour_over_slow_terrain() {
360 let map = map_from(&[
361 ".....", //
362 "..~..",
363 ]);
364 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
365 assert!(!path.contains(&UVec2::new(2, 0)));
366 }
367
368 #[test]
369 fn walks_around_a_hill_rather_than_over_it() {
370 let mut map = map_from(&[
371 ".....", //
372 ".....",
373 ]);
374 map.set_height(UVec2::new(2, 0), 10.0);
375 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
376 assert!(!path.contains(&UVec2::new(2, 0)));
377 }
378
379 #[test]
380 fn detours_along_paths_when_they_are_faster() {
381 let mut map = map_from(&[
382 "..........", //
383 "..........",
384 ]);
385 for x in 0..10 {
386 map.set(UVec2::new(x, 1), Terrain::Paved);
387 }
388 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(9, 0)).unwrap();
389 assert!(path.iter().filter(|p| p.y == 1).count() >= 6);
390 }
391
392 // A wall can go up on a tile a colonist is standing on (or just about to enter); routes must
393 // still start from that tile so nobody gets permanently stuck inside a wall.
394 #[test]
395 fn colonists_can_walk_out_of_a_blocked_tile() {
396 let mut map = map_from(&["....."]);
397 map.set_wall(UVec2::new(2, 0), true);
398 let path = find_path(&map, UVec2::new(2, 0), UVec2::new(4, 0)).unwrap();
399 assert_eq!(path, vec![UVec2::new(3, 0), UVec2::new(4, 0)]);
400 }
401
402 #[test]
403 fn crosses_slow_terrain_when_it_is_the_only_way() {
404 let map = map_from(&["..~.."]);
405 let path = find_path(&map, UVec2::new(0, 0), UVec2::new(4, 0)).unwrap();
406 assert!(path.contains(&UVec2::new(2, 0)));
407 }
408
409 #[test]
410 fn path_adjacent_stops_next_to_target() {
411 let map = map_from(&["......"]);
412 let path = find_path_adjacent(&map, UVec2::new(0, 0), UVec2::new(5, 0)).unwrap();
413 assert_eq!(path.last(), Some(&UVec2::new(4, 0)));
414 }
415
416 #[test]
417 fn path_adjacent_works_for_impassable_targets() {
418 let map = map_from(&["...#"]);
419 let path = find_path_adjacent(&map, UVec2::new(0, 0), UVec2::new(3, 0)).unwrap();
420 assert_eq!(path.last(), Some(&UVec2::new(2, 0)));
421 }
422
423 #[test]
424 fn path_adjacent_from_neighbor_is_empty() {
425 let map = map_from(&["..."]);
426 let path = find_path_adjacent(&map, UVec2::new(1, 0), UVec2::new(2, 0));
427 assert_eq!(path, Some(vec![]));
428 }
429
430 #[test]
431 fn path_adjacent_to_enclosed_target_is_none() {
432 let walled = map_from(&[
433 "#####", //
434 "#...#", "#####", ".....",
435 ]);
436 assert_eq!(
437 find_path_adjacent(&walled, UVec2::new(0, 0), UVec2::new(2, 2)),
438 None
439 );
440 }
441
442 #[test]
443 fn nearest_tile_finds_closest_match_by_walking_distance() {
444 let map = map_from(&[
445 ".#...", //
446 ".#.#.", "...#.",
447 ]);
448 let found = nearest_tile(&map, UVec2::new(0, 0), 100, |p| p.x == 4);
449 assert_eq!(found, Some(UVec2::new(4, 2)));
450 assert_eq!(
451 nearest_tile(&map, UVec2::new(0, 0), 100, |p| p.x == 9),
452 None
453 );
454 }
455
456 #[test]
457 fn finds_paths_across_a_generated_world() {
458 let map = crate::worldgen::generate(crate::worldgen::WORLD_SIZE, 1);
459 let region = crate::worldgen::largest_region(&map);
460 let (start, goal) = (region[0], region[region.len() - 1]);
461 let path = find_path(&map, start, goal).unwrap();
462 assert!(is_connected(start, &path));
463 assert_eq!(path.last(), Some(&goal));
464 }
465 }