ferravex_play/
navigation.rs

1//! Deterministic weighted-grid A* and small 2D steering helpers.
2use std::cmp::Ordering;
3use std::collections::BinaryHeap;
4
5#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
6pub struct Cell {
7    pub x: u32,
8    pub y: u32,
9}
10#[derive(Clone, Copy, Debug, PartialEq, Eq)]
11pub enum NavigationError {
12    InvalidDimensions,
13    OutOfBounds,
14    BlockedEndpoint,
15    NoPath,
16    InvalidCost,
17}
18#[derive(Clone, Copy, Debug, PartialEq, Eq)]
19pub enum Connectivity {
20    Four,
21    Eight,
22}
23
24pub struct Grid {
25    width: u32,
26    height: u32,
27    costs: Vec<u16>, // zero is impassable; positive values multiply edge cost.
28}
29impl Grid {
30    pub fn new(width: u32, height: u32) -> Result<Self, NavigationError> {
31        let count = width
32            .checked_mul(height)
33            .filter(|n| *n > 0 && *n <= 4_000_000)
34            .ok_or(NavigationError::InvalidDimensions)?;
35        Ok(Self {
36            width,
37            height,
38            costs: vec![1; count as usize],
39        })
40    }
41    pub fn width(&self) -> u32 {
42        self.width
43    }
44    pub fn height(&self) -> u32 {
45        self.height
46    }
47    pub fn set_blocked(&mut self, cell: Cell, blocked: bool) -> Result<(), NavigationError> {
48        let index = self.index(cell)?;
49        self.costs[index] = if blocked { 0 } else { 1 };
50        Ok(())
51    }
52    pub fn set_cost(&mut self, cell: Cell, cost: u16) -> Result<(), NavigationError> {
53        if cost == 0 {
54            return Err(NavigationError::InvalidCost);
55        }
56        let index = self.index(cell)?;
57        self.costs[index] = cost;
58        Ok(())
59    }
60    pub fn walkable(&self, cell: Cell) -> bool {
61        self.index(cell).is_ok_and(|index| self.costs[index] != 0)
62    }
63    pub fn path(
64        &self,
65        start: Cell,
66        end: Cell,
67        connectivity: Connectivity,
68    ) -> Result<Vec<Cell>, NavigationError> {
69        let start_index = self.index(start)?;
70        let end_index = self.index(end)?;
71        if !self.walkable(start) || !self.walkable(end) {
72            return Err(NavigationError::BlockedEndpoint);
73        }
74        let mut distance = vec![u64::MAX; self.costs.len()];
75        let mut parent = vec![usize::MAX; self.costs.len()];
76        let mut open = BinaryHeap::new();
77        distance[start_index] = 0;
78        open.push(Visit {
79            index: start_index,
80            cost: 0,
81            estimate: heuristic(start, end, connectivity),
82        });
83        while let Some(visit) = open.pop() {
84            if visit.cost != distance[visit.index] {
85                continue;
86            }
87            if visit.index == end_index {
88                let mut route = Vec::new();
89                let mut index = end_index;
90                loop {
91                    route.push(self.cell(index));
92                    if index == start_index {
93                        break;
94                    }
95                    index = parent[index];
96                }
97                route.reverse();
98                return Ok(route);
99            }
100            let current = self.cell(visit.index);
101            for (dx, dy) in [
102                (-1_i64, 0_i64),
103                (0, -1),
104                (1, 0),
105                (0, 1),
106                (-1, -1),
107                (1, -1),
108                (1, 1),
109                (-1, 1),
110            ] {
111                let diagonal = dx != 0 && dy != 0;
112                if diagonal && connectivity == Connectivity::Four {
113                    continue;
114                }
115                let (x, y) = (i64::from(current.x) + dx, i64::from(current.y) + dy);
116                if x < 0 || y < 0 || x >= i64::from(self.width) || y >= i64::from(self.height) {
117                    continue;
118                }
119                let next = Cell {
120                    x: x as u32,
121                    y: y as u32,
122                };
123                if !self.walkable(next) {
124                    continue;
125                }
126                if diagonal
127                    && (!self.walkable(Cell {
128                        x: next.x,
129                        y: current.y,
130                    }) || !self.walkable(Cell {
131                        x: current.x,
132                        y: next.y,
133                    }))
134                {
135                    continue;
136                }
137                let index = self.index(next)?;
138                let cost =
139                    visit.cost + u64::from(self.costs[index]) * if diagonal { 14 } else { 10 };
140                if cost < distance[index] {
141                    distance[index] = cost;
142                    parent[index] = visit.index;
143                    open.push(Visit {
144                        index,
145                        cost,
146                        estimate: cost + heuristic(next, end, connectivity),
147                    });
148                }
149            }
150        }
151        Err(NavigationError::NoPath)
152    }
153    fn index(&self, cell: Cell) -> Result<usize, NavigationError> {
154        if cell.x < self.width && cell.y < self.height {
155            Ok((cell.y * self.width + cell.x) as usize)
156        } else {
157            Err(NavigationError::OutOfBounds)
158        }
159    }
160    fn cell(&self, index: usize) -> Cell {
161        Cell {
162            x: index as u32 % self.width,
163            y: index as u32 / self.width,
164        }
165    }
166}
167#[derive(Clone, Copy, Eq, PartialEq)]
168struct Visit {
169    index: usize,
170    cost: u64,
171    estimate: u64,
172}
173impl Ord for Visit {
174    fn cmp(&self, other: &Self) -> Ordering {
175        other
176            .estimate
177            .cmp(&self.estimate)
178            .then_with(|| other.cost.cmp(&self.cost))
179            .then_with(|| other.index.cmp(&self.index))
180    }
181}
182impl PartialOrd for Visit {
183    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
184        Some(self.cmp(other))
185    }
186}
187fn heuristic(a: Cell, b: Cell, connectivity: Connectivity) -> u64 {
188    let dx = u64::from(a.x.abs_diff(b.x));
189    let dy = u64::from(a.y.abs_diff(b.y));
190    if connectivity == Connectivity::Four {
191        (dx + dy) * 10
192    } else {
193        dx.min(dy) * 14 + dx.abs_diff(dy) * 10
194    }
195}
196/// Desired velocity toward a target, slowing smoothly within the arrival radius.
197/// Invalid or non-positive parameters produce a stopped agent rather than NaNs.
198pub fn arrive(position: [f32; 2], target: [f32; 2], max_speed: f32, slow_radius: f32) -> [f32; 2] {
199    if !position.into_iter().chain(target).all(f32::is_finite)
200        || !max_speed.is_finite()
201        || !slow_radius.is_finite()
202        || max_speed <= 0.0
203        || slow_radius <= 0.0
204    {
205        return [0.0; 2];
206    }
207    let d = [target[0] - position[0], target[1] - position[1]];
208    let length = d[0].hypot(d[1]);
209    if length <= f32::EPSILON || !length.is_finite() {
210        return [0.0; 2];
211    }
212    let scale = max_speed * (length / slow_radius).min(1.0) / length;
213    [d[0] * scale, d[1] * scale]
214}
215/// Repulsion from nearby peers, capped to max_speed; excludes coincident agents.
216pub fn separation(position: [f32; 2], peers: &[[f32; 2]], radius: f32, max_speed: f32) -> [f32; 2] {
217    if !radius.is_finite() || radius <= 0.0 || !max_speed.is_finite() || max_speed <= 0.0 {
218        return [0.0; 2];
219    }
220    let mut sum = [0.0; 2];
221    for peer in peers {
222        let d = [position[0] - peer[0], position[1] - peer[1]];
223        let length = d[0].hypot(d[1]);
224        if length.is_finite() && length > 0.0001 && length < radius {
225            let weight = (1.0 - length / radius) / length;
226            sum[0] += d[0] * weight;
227            sum[1] += d[1] * weight;
228        }
229    }
230    arrive([0.0; 2], sum, max_speed, 1.0)
231}
232
233#[cfg(test)]
234mod tests {
235    use super::*;
236    fn c(x: u32, y: u32) -> Cell {
237        Cell { x, y }
238    }
239    #[test]
240    fn detours_around_walls_and_respects_costs() {
241        let mut grid = Grid::new(5, 3).unwrap();
242        grid.set_blocked(c(2, 1), true).unwrap();
243        let path = grid.path(c(0, 1), c(4, 1), Connectivity::Four).unwrap();
244        assert_eq!(path.len(), 7);
245        assert!(path.iter().all(|cell| grid.walkable(*cell)));
246        grid.set_blocked(c(2, 1), false).unwrap();
247        grid.set_cost(c(2, 1), 50).unwrap();
248        assert!(
249            !grid
250                .path(c(0, 1), c(4, 1), Connectivity::Four)
251                .unwrap()
252                .contains(&c(2, 1))
253        );
254    }
255    #[test]
256    fn diagonal_moves_do_not_cut_blocked_corners() {
257        let mut grid = Grid::new(2, 2).unwrap();
258        grid.set_blocked(c(1, 0), true).unwrap();
259        grid.set_blocked(c(0, 1), true).unwrap();
260        assert_eq!(
261            grid.path(c(0, 0), c(1, 1), Connectivity::Eight),
262            Err(NavigationError::NoPath)
263        );
264        grid.set_blocked(c(1, 0), false).unwrap();
265        assert_eq!(
266            grid.path(c(0, 0), c(1, 1), Connectivity::Eight)
267                .unwrap()
268                .len(),
269            3
270        );
271    }
272    #[test]
273    fn boundaries_empty_routes_and_invalid_dimensions() {
274        assert!(matches!(
275            Grid::new(0, 3),
276            Err(NavigationError::InvalidDimensions)
277        ));
278        let grid = Grid::new(3, 3).unwrap();
279        assert_eq!(
280            grid.path(c(1, 1), c(1, 1), Connectivity::Four).unwrap(),
281            vec![c(1, 1)]
282        );
283        assert_eq!(
284            grid.path(c(5, 0), c(0, 0), Connectivity::Four),
285            Err(NavigationError::OutOfBounds)
286        );
287        assert_eq!(
288            grid.path(c(0, 0), c(2, 2), Connectivity::Eight)
289                .unwrap()
290                .len(),
291            3
292        );
293    }
294    #[test]
295    fn steering_stops_at_target_and_slows_on_arrival() {
296        assert_eq!(arrive([1.0, 0.0], [1.0, 0.0], 4.0, 2.0), [0.0; 2]);
297        assert_eq!(arrive([0.0; 2], [1.0, 0.0], 4.0, 2.0), [2.0, 0.0]);
298        assert_eq!(arrive([0.0; 2], [f32::NAN, 0.0], 4.0, 2.0), [0.0; 2]);
299        assert!(separation([0.0; 2], &[[1.0, 0.0]], 2.0, 4.0)[0] < 0.0);
300    }
301}
ferravex_play/navigation.rs

ferravex_play/
navigation.rs
