repame_nav/
lib.rs

1//! Spatial navigation for Repame: a heightfield navmesh you bake from the
2//! engine's own collision input, A* path queries over the region graph,
3//! funnel (string-pull) corridors, and path-following agents with reciprocal
4//! velocity obstacle avoidance. Both 2D ([`glam::Vec2`]) and 3D
5//! ([`glam::Vec3`]) meshes share one implementation.
6//!
7//! # Pipeline
8//! 1. Implement [`Source2d`] or [`Source3d`] (any
9//!    [`repame_physics::VoxelQuery`] is a [`Source3d`]) and call [`bake_2d`]
10//!    or [`bake_3d`] to get a [`NavMesh`].
11//! 2. Query with [`NavMesh::find_path`], or wrap the mesh in a [`NavMap`] to
12//!    layer dynamic [`NavObstacle`]s and off-mesh [`NavLink`]s on top.
13//! 3. Drive a [`NavigationAgent`] with the returned path.
14//!
15//! # Determinism
16//! Bakes emit regions in row-major cell order and deduplicate shared corners,
17//! so vertex numbering is fixed. Adjacency and path results never iterate a
18//! hash map; A* breaks cost ties by polygon index and the funnel is
19//! order-stable. Obstacles and links are read in insertion order, and the
20//! avoidance resolver samples a fixed fan around sorted neighbours. Within one
21//! binary, the same inputs give bit-identical output. Cross-target float
22//! equality (native libm vs wasm, notably `sqrt`/`sin_cos`) is not claimed,
23//! the same caveat as [`repame_sim`]: hash a trace and diff to prove it.
24//!
25//! ```rust
26//! use repame_nav::{bake_2d, Bake2d, Source2d};
27//! use glam::Vec2;
28//!
29//! struct Open;
30//! impl Source2d for Open {
31//!     fn is_solid(&self, _cell: [i32; 2]) -> bool { false }
32//! }
33//!
34//! let mesh = bake_2d(&Open, &Bake2d { min: [0, 0], max: [4, 4], cell_size: 1.0, agent_radius: 0.0 })
35//!     .expect("open field bakes");
36//! let path = mesh.find_path(Vec2::new(0.5, 0.5), Vec2::new(3.5, 3.5)).expect("connected");
37//! assert_eq!(path.len(), 2);
38//! ```
39
40mod agent;
41mod avoid;
42mod bake;
43mod funnel;
44mod map;
45mod mesh;
46mod path;
47mod vec;
48
49pub use agent::{AgentNeighbor, NavigationAgent};
50pub use avoid::{resolve, AvoidanceConfig, Neighbor2};
51pub use bake::{bake_2d, bake_3d, Bake2d, Bake3d, Source2d, Source3d, VOXEL};
52pub use funnel::funnel;
53pub use map::{NavLink, NavMap, NavObstacle};
54pub use mesh::{NavMesh, Neighbor, Polygon};
55pub use vec::NavVec;
56
57/// A navigation setup or query refused to build.
58#[derive(Clone, Copy, Debug, PartialEq, Eq)]
59pub enum NavError {
60    /// The bake region is empty or inverted.
61    EmptyRegion,
62    /// A polygon index pointed outside the vertex buffer.
63    BadIndex,
64    /// A polygon had fewer than three vertices.
65    DegeneratePolygon,
66    /// The region held no walkable cell after erosion.
67    NoWalkableArea,
68    /// A scale or radius was not positive.
69    InvalidConfig,
70}
71
72impl core::fmt::Display for NavError {
73    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
74        let message = match self {
75            NavError::EmptyRegion => "navigation bake region is empty",
76            NavError::BadIndex => "polygon index is out of range",
77            NavError::DegeneratePolygon => "polygon has fewer than three vertices",
78            NavError::NoWalkableArea => "no walkable area after agent-radius erosion",
79            NavError::InvalidConfig => "navigation configuration is invalid",
80        };
81        f.write_str(message)
82    }
83}
84
85impl std::error::Error for NavError {}
86
87#[cfg(test)]
88mod tests {
89    use super::*;
90    use glam::{Vec2, Vec3};
91
92    struct Grid {
93        solid: Vec<[i32; 2]>,
94    }
95
96    impl Source2d for Grid {
97        fn is_solid(&self, cell: [i32; 2]) -> bool {
98            self.solid.contains(&cell)
99        }
100    }
101
102    fn walled_corridor() -> Grid {
103        Grid { solid: vec![[2, 0], [2, 1]] }
104    }
105
106    fn corridor() -> NavMesh<Vec2> {
107        bake_2d(
108            &walled_corridor(),
109            &Bake2d { min: [0, 0], max: [5, 3], cell_size: 1.0, agent_radius: 0.0 },
110        )
111        .expect("corridor bakes")
112    }
113
114    fn path_length(path: &[Vec2]) -> f32 {
115        path.windows(2).map(|w| w[0].distance(w[1])).sum()
116    }
117
118    #[test]
119    fn two_d_path_hugs_the_wall_corners() {
120        let mesh = corridor();
121        let path = mesh.find_path(Vec2::new(0.5, 1.5), Vec2::new(4.5, 1.5)).expect("connected");
122        assert_eq!(path.len(), 4, "start, two corners, end: {path:?}");
123        assert!((path[1].x - 2.0).abs() < 1e-4 && (path[1].y - 2.0).abs() < 1e-4, "corner {}", path[1]);
124        assert!((path[2].x - 3.0).abs() < 1e-4 && (path[2].y - 2.0).abs() < 1e-4, "corner {}", path[2]);
125        let expected = 2.0 * 2.5f32.sqrt() + 1.0;
126        assert!(
127            (path_length(&path) - expected).abs() < 1e-3,
128            "length {} expected {expected}",
129            path_length(&path)
130        );
131    }
132
133    #[test]
134    fn path_is_deterministic() {
135        let mesh = corridor();
136        let from = Vec2::new(0.5, 0.75);
137        let to = Vec2::new(4.5, 2.25);
138        let first = mesh.find_path(from, to);
139        let second = mesh.find_path(from, to);
140        assert!(first.is_some());
141        assert_eq!(first, second, "same query must give the identical path");
142    }
143
144    #[test]
145    fn obstacle_carves_the_only_gap() {
146        let mesh = corridor();
147        let mut map = NavMap::new(mesh);
148        let from = Vec2::new(0.5, 1.5);
149        let to = Vec2::new(4.5, 1.5);
150        assert!(map.find_path(from, to).is_some(), "open gap");
151        map.add_obstacle(NavObstacle::new(vec![[2.0, 2.0], [3.0, 2.0], [3.0, 3.0], [2.0, 3.0]]));
152        assert!(map.find_path(from, to).is_none(), "carved gap blocks the route");
153    }
154
155    #[test]
156    fn off_mesh_link_shortens_the_route() {
157        let mesh = corridor();
158        let from = Vec2::new(0.5, 1.5);
159        let to = Vec2::new(4.5, 1.5);
160        let detour = path_length(&mesh.find_path(from, to).expect("connected"));
161
162        let mut map = NavMap::new(mesh);
163        map.add_link(NavLink::new(Vec2::new(1.5, 1.5), Vec2::new(3.5, 1.5)));
164        let direct = path_length(&map.find_path(from, to).expect("linked route"));
165        assert!((direct - 4.0).abs() < 1e-3, "link path length {direct}");
166        assert!(direct < detour, "link {direct} must beat detour {detour}");
167    }
168
169    struct Ramp;
170
171    impl Source3d for Ramp {
172        fn is_solid(&self, cell: [i32; 3]) -> bool {
173            cell[1] <= 0 && (-4..=8).contains(&cell[0]) && (-4..=8).contains(&cell[2])
174        }
175        fn surface_top(&self, cell: [i32; 3], wx: f32, _wz: f32) -> Option<f32> {
176            (cell[1] == 0).then_some(0.5 * wx)
177        }
178    }
179
180    #[test]
181    fn three_d_path_follows_a_slope() {
182        let mesh = bake_3d(
183            &Ramp,
184            &Bake3d { min: [0, 0, 0], max: [4, 1, 2], ..Bake3d::default() },
185        )
186        .expect("ramp bakes");
187        let from = Vec3::new(0.5, 0.0, 0.5);
188        let to = Vec3::new(3.5, 0.0, 0.5);
189        let path = mesh.find_path(from, to).expect("connected");
190        assert!((path[0].y - 0.25).abs() < 1e-3, "start lifted to the surface: {}", path[0].y);
191        assert!((path[path.len() - 1].y - 1.75).abs() < 1e-3, "end lifted: {}", path[path.len() - 1].y);
192        let length: f32 = path.windows(2).map(|w| w[0].distance(w[1])).sum();
193        let expected = (3.0f32 * 3.0 + 1.5 * 1.5).sqrt();
194        assert!((length - expected).abs() < 0.02, "slope length {length} expected {expected}");
195    }
196
197    #[test]
198    fn agent_seeks_and_arrives() {
199        let mesh = corridor();
200        let mut agent = NavigationAgent::<Vec2>::new(2.0);
201        agent.arrival_radius = 0.25;
202        agent.set_path(mesh.find_path(Vec2::new(0.5, 1.5), Vec2::new(4.5, 1.5)).expect("connected"));
203
204        let mut position = Vec2::new(0.5, 1.5);
205        for _ in 0..600 {
206            let velocity = agent.desired_velocity(position);
207            if velocity == Vec2::ZERO {
208                break;
209            }
210            position += velocity * (1.0 / 60.0);
211        }
212        assert!(agent.is_finished(position), "agent stopped at {position}");
213        assert!(position.distance(Vec2::new(4.5, 1.5)) <= agent.arrival_radius + 1e-3);
214    }
215
216    #[test]
217    fn avoidance_is_deterministic_and_diverges_from_desired() {
218        let neighbor = [AgentNeighbor {
219            position: Vec2::new(1.0, 0.0),
220            velocity: Vec2::new(-1.0, 0.0),
221            radius: 0.4,
222        }];
223        let mut agent = NavigationAgent::<Vec2>::new(1.0);
224        agent.avoidance_enabled = true;
225        agent.desired_velocity = Vec2::new(1.0, 0.0);
226        let first = agent.avoidance_velocity(Vec2::ZERO, &neighbor);
227        let second = agent.avoidance_velocity(Vec2::ZERO, &neighbor);
228        assert_eq!(first, second, "avoidance must be deterministic");
229        assert!(first.y.abs() > 1e-3 || first.x < 1.0, "avoidance steers off a head-on course: {first}");
230    }
231}
repame_nav/lib.rs

repame_nav/
lib.rs
