Generalize A* and add adjacent/nearest-tile search

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

author
Marijn Besseling <njirambem@gmail.com> · 2026-10-24 09:11 UTC
commit
312e46fde3e5dec3abbbd3f49185435ae495bbe9
parent
43e7028759
tree
browse at this commit

1 file changed +107 -9

src/pathfinding.rs +107 -9

src/pathfinding.rs +107 -9

@@ -1,4 +1,7 @@
1 -use std::{cmp::Reverse, collections::BinaryHeap};
1 +use std::{
2 + cmp::Reverse,
3 + collections::{BinaryHeap, HashSet, VecDeque},
4 +};
2 5
3 6 use bevy::prelude::*;
4 7
@@ -20,11 +23,59 @@ const NEIGHBORS: [IVec2; 8] = [
20 23 ];
21 24
22 25 pub fn find_path(map: &WorldMap, start: UVec2, goal: UVec2) -> Option<Vec<UVec2>> {
23 - let start = start.as_ivec2();
26 + map.move_cost(goal.as_ivec2())?;
24 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();
25 77 map.move_cost(start)?;
26 - map.move_cost(goal)?;
27 - if start == goal {
78 + if is_goal(start) {
28 79 return Some(vec![]);
29 80 }
30 81
@@ -36,12 +87,12 @@ pub fn find_path(map: &WorldMap, start: UVec2, goal: UVec2) -> Option<Vec<UVec2>
36 87 let mut open = BinaryHeap::new();
37 88
38 89 cost_so_far[index(start)] = 0;
39 - open.push(Reverse((heuristic(start, goal), start.x, start.y)));
90 + open.push(Reverse((heuristic(start), start.x, start.y)));
40 91
41 92 while let Some(Reverse((_, x, y))) = open.pop() {
42 93 let current = IVec2::new(x, y);
43 - if current == goal {
44 - return Some(reconstruct(&came_from, index, start, goal));
94 + if is_goal(current) {
95 + return Some(reconstruct(&came_from, index, start, current));
45 96 }
46 97 let current_cost = cost_so_far[index(current)];
47 98 for step in NEIGHBORS {
@@ -61,14 +112,14 @@ pub fn find_path(map: &WorldMap, start: UVec2, goal: UVec2) -> Option<Vec<UVec2>
61 112 if new_cost < cost_so_far[index(next)] {
62 113 cost_so_far[index(next)] = new_cost;
63 114 came_from[index(next)] = Some(current);
64 - open.push(Reverse((new_cost + heuristic(next, goal), next.x, next.y)));
115 + open.push(Reverse((new_cost + heuristic(next), next.x, next.y)));
65 116 }
66 117 }
67 118 }
68 119 None
69 120 }
70 121
71 -fn heuristic(a: IVec2, b: IVec2) -> u32 {
122 +fn octile(a: IVec2, b: IVec2) -> u32 {
72 123 let d = (a - b).abs();
73 124 let (min, max) = (d.x.min(d.y) as u32, d.x.max(d.y) as u32);
74 125 (DIAGONAL * min + STRAIGHT * (max - min)) * MIN_TERRAIN_COST
@@ -214,6 +265,53 @@ mod tests {
214 265 }
215 266
216 267 #[test]
268 + fn path_adjacent_stops_next_to_target() {
269 + let map = map_from(&["......"]);
270 + let path = find_path_adjacent(&map, UVec2::new(0, 0), UVec2::new(5, 0)).unwrap();
271 + assert_eq!(path.last(), Some(&UVec2::new(4, 0)));
272 + }
273 +
274 + #[test]
275 + fn path_adjacent_works_for_impassable_targets() {
276 + let map = map_from(&["...#"]);
277 + let path = find_path_adjacent(&map, UVec2::new(0, 0), UVec2::new(3, 0)).unwrap();
278 + assert_eq!(path.last(), Some(&UVec2::new(2, 0)));
279 + }
280 +
281 + #[test]
282 + fn path_adjacent_from_neighbor_is_empty() {
283 + let map = map_from(&["..."]);
284 + let path = find_path_adjacent(&map, UVec2::new(1, 0), UVec2::new(2, 0));
285 + assert_eq!(path, Some(vec![]));
286 + }
287 +
288 + #[test]
289 + fn path_adjacent_to_enclosed_target_is_none() {
290 + let walled = map_from(&[
291 + "#####", //
292 + "#...#", "#####", ".....",
293 + ]);
294 + assert_eq!(
295 + find_path_adjacent(&walled, UVec2::new(0, 0), UVec2::new(2, 2)),
296 + None
297 + );
298 + }
299 +
300 + #[test]
301 + fn nearest_tile_finds_closest_match_by_walking_distance() {
302 + let map = map_from(&[
303 + ".#...", //
304 + ".#.#.", "...#.",
305 + ]);
306 + let found = nearest_tile(&map, UVec2::new(0, 0), 100, |p| p.x == 4);
307 + assert_eq!(found, Some(UVec2::new(4, 2)));
308 + assert_eq!(
309 + nearest_tile(&map, UVec2::new(0, 0), 100, |p| p.x == 9),
310 + None
311 + );
312 + }
313 +
314 + #[test]
217 315 fn finds_paths_across_a_generated_world() {
218 316 let map = crate::worldgen::generate(crate::worldgen::WORLD_SIZE, 1);
219 317 let region = crate::worldgen::largest_region(&map);