repame_nav/
bake.rs

1//! Navmesh bake from engine geometry.
2//!
3//! Godot bakes a `NavigationMesh` from render geometry; here the source is the
4//! engine's own collision input. A caller implements [`Source2d`] or
5//! [`Source3d`] (any [`repame_physics::VoxelQuery`] is a `Source3d` for free),
6//! so a bake needs no renderer and is fully testable.
7//!
8//! The bake is a heightfield build: one walkable column becomes one quad
9//! region whose corners sit on the sampled surface, so a ramp produces a
10//! tilted quad and the funnel follows the slope. Regions are not merged, which
11//! keeps shared edges exact and adjacency unambiguous.
12
13use crate::mesh::NavMesh;
14use crate::NavError;
15use glam::{Vec2, Vec3};
16use std::collections::HashMap;
17
18/// Unit horizontal size of one voxel column, in world units.
19pub const VOXEL: f32 = 1.0;
20
21/// 2D solidity sampler: a cell is either solid or free.
22pub trait Source2d {
23    /// Is the 2D cell solid?
24    fn is_solid(&self, cell: [i32; 2]) -> bool;
25}
26
27/// 3D collision sampler: solidity plus the top surface height behind a cell
28/// (ramps live behind [`Source3d::surface_top`]).
29pub trait Source3d {
30    /// Is the 3D cell solid?
31    fn is_solid(&self, cell: [i32; 3]) -> bool;
32    /// Top surface height of `cell` at world (`wx`, `wz`), or `None` in air.
33    fn surface_top(&self, cell: [i32; 3], wx: f32, wz: f32) -> Option<f32>;
34}
35
36impl<Q: repame_physics::VoxelQuery> Source3d for Q {
37    fn is_solid(&self, cell: [i32; 3]) -> bool {
38        repame_physics::VoxelQuery::is_solid(self, cell)
39    }
40    fn surface_top(&self, cell: [i32; 3], wx: f32, wz: f32) -> Option<f32> {
41        repame_physics::VoxelQuery::surface_top(self, cell, wx, wz)
42    }
43}
44
45/// 2D bake parameters. `min` is inclusive, `max` exclusive.
46#[derive(Clone, Copy, Debug)]
47pub struct Bake2d {
48    /// First cell column, inclusive.
49    pub min: [i32; 2],
50    /// One past the last cell column.
51    pub max: [i32; 2],
52    /// World size of one grid cell.
53    pub cell_size: f32,
54    /// Agent radius. Walkable cells closer than this to a solid cell are
55    /// dropped, so agents do not hug walls.
56    pub agent_radius: f32,
57}
58
59impl Default for Bake2d {
60    fn default() -> Self {
61        Self {
62            min: [0, 0],
63            max: [0, 0],
64            cell_size: 1.0,
65            agent_radius: 0.4,
66        }
67    }
68}
69
70/// 3D bake parameters. `min` is inclusive, `max` exclusive; a voxel is one
71/// world unit.
72#[derive(Clone, Copy, Debug)]
73pub struct Bake3d {
74    /// First cell, inclusive.
75    pub min: [i32; 3],
76    /// One past the last cell.
77    pub max: [i32; 3],
78    /// Vertical clearance a walkable column must have above its surface.
79    pub agent_height: f32,
80    /// Agent radius; walkable columns nearer than this to a non-walkable
81    /// column are dropped.
82    pub agent_radius: f32,
83    /// Steepest walkable slope, in degrees.
84    pub max_slope_deg: f32,
85}
86
87impl Default for Bake3d {
88    fn default() -> Self {
89        Self {
90            min: [0, 0, 0],
91            max: [0, 0, 0],
92            agent_height: 1.8,
93            agent_radius: 0.4,
94            max_slope_deg: 45.0,
95        }
96    }
97}
98
99/// Bake a flat 2D navmesh of unit squares from a solidity sampler.
100///
101/// The 3D voxel world can drive this too by sampling one height layer, but a
102/// dedicated 2D source keeps the crate usable without a voxel world.
103pub fn bake_2d<S: Source2d>(source: &S, config: &Bake2d) -> Result<NavMesh<Vec2>, NavError> {
104    if config.max[0] <= config.min[0] || config.max[1] <= config.min[1] {
105        return Err(NavError::EmptyRegion);
106    }
107    if config.cell_size <= 0.0 {
108        return Err(NavError::InvalidConfig);
109    }
110    let radius_cells = (config.agent_radius / config.cell_size).ceil() as i32;
111    let walkable = |x: i32, z: i32| -> bool {
112        if source.is_solid([x, z]) {
113            return false;
114        }
115        for dx in -radius_cells..=radius_cells {
116            for dz in -radius_cells..=radius_cells {
117                if dx * dx + dz * dz <= radius_cells * radius_cells && source.is_solid([x + dx, z + dz])
118                {
119                    return false;
120                }
121            }
122        }
123        true
124    };
125
126    let mut vertices: Vec<Vec2> = Vec::new();
127    let mut corners: HashMap<(i32, i32), u32> = HashMap::new();
128    let mut polygons: Vec<Vec<u32>> = Vec::new();
129    let scale = config.cell_size;
130    let vertex = |x: i32, z: i32, vertices: &mut Vec<Vec2>, corners: &mut HashMap<(i32, i32), u32>| {
131        *corners.entry((x, z)).or_insert_with(|| {
132            vertices.push(Vec2::new(x as f32 * scale, z as f32 * scale));
133            (vertices.len() - 1) as u32
134        })
135    };
136
137    for x in config.min[0]..config.max[0] {
138        for z in config.min[1]..config.max[1] {
139            if !walkable(x, z) {
140                continue;
141            }
142            let c0 = vertex(x, z, &mut vertices, &mut corners);
143            let c1 = vertex(x + 1, z, &mut vertices, &mut corners);
144            let c2 = vertex(x + 1, z + 1, &mut vertices, &mut corners);
145            let c3 = vertex(x, z + 1, &mut vertices, &mut corners);
146            polygons.push(vec![c0, c1, c2, c3]);
147        }
148    }
149    if polygons.is_empty() {
150        return Err(NavError::NoWalkableArea);
151    }
152    NavMesh::from_polygons(vertices, polygons)
153}
154
155/// Bake a heightfield 3D navmesh from a collision sampler.
156pub fn bake_3d<S: Source3d>(source: &S, config: &Bake3d) -> Result<NavMesh<Vec3>, NavError> {
157    if config.max[0] <= config.min[0] || config.max[1] <= config.min[1] || config.max[2] <= config.min[2]
158    {
159        return Err(NavError::EmptyRegion);
160    }
161    let radius_cells = (config.agent_radius / VOXEL).ceil() as i32;
162    let margin = radius_cells + 1;
163    let clearance_cells = config.agent_height.ceil() as i32;
164
165    let scan_min = [config.min[0] - margin, config.min[2] - margin];
166    let scan_max = [config.max[0] + margin, config.max[2] + margin];
167    let width = (scan_max[0] - scan_min[0] + 1) as usize;
168    let height = (scan_max[1] - scan_min[1] + 1) as usize;
169
170    let mut ground: Vec<Option<[i32; 3]>> = vec![None; width * height];
171    let index = |x: i32, z: i32| -> usize {
172        ((x - scan_min[0]) as usize) + ((z - scan_min[1]) as usize) * width
173    };
174
175    for x in scan_min[0]..=scan_max[0] {
176        for z in scan_min[1]..=scan_max[1] {
177            let mut found = None;
178            for y in (config.min[1]..config.max[1]).rev() {
179                if !source.is_solid([x, y, z]) {
180                    continue;
181                }
182                let mut clear = true;
183                for k in 1..=clearance_cells {
184                    if source.is_solid([x, y + k, z]) {
185                        clear = false;
186                        break;
187                    }
188                }
189                if clear {
190                    found = Some([x, y, z]);
191                    break;
192                }
193            }
194            ground[index(x, z)] = found;
195        }
196    }
197
198    let column = |x: i32, z: i32| -> Option<[i32; 3]> {
199        if x < scan_min[0] || z < scan_min[1] || x > scan_max[0] || z > scan_max[1] {
200            None
201        } else {
202            ground[index(x, z)]
203        }
204    };
205    let corner_height = |x: i32, z: i32| -> Option<f32> {
206        column(x, z)
207            .map(|cell| source.surface_top(cell, x as f32, z as f32).unwrap_or((cell[1] + 1) as f32))
208    };
209    let erode = |x: i32, z: i32| -> bool {
210        if column(x, z).is_none() {
211            return false;
212        }
213        for dx in -radius_cells..=radius_cells {
214            for dz in -radius_cells..=radius_cells {
215                if dx * dx + dz * dz <= radius_cells * radius_cells && column(x + dx, z + dz).is_none() {
216                    return false;
217                }
218            }
219        }
220        true
221    };
222
223    let cos_limit = config.max_slope_deg.to_radians().cos();
224    let mut vertices: Vec<Vec3> = Vec::new();
225    let mut corners: HashMap<(i32, i32), u32> = HashMap::new();
226    let mut polygons: Vec<Vec<u32>> = Vec::new();
227
228    for x in config.min[0]..config.max[0] {
229        for z in config.min[2]..config.max[2] {
230            if !erode(x, z) {
231                continue;
232            }
233            let (Some(h00), Some(h10), Some(h11), Some(h01)) = (
234                corner_height(x, z),
235                corner_height(x + 1, z),
236                corner_height(x + 1, z + 1),
237                corner_height(x, z + 1),
238            ) else {
239                continue;
240            };
241            let p00 = Vec3::new(x as f32, h00, z as f32);
242            let p10 = Vec3::new((x + 1) as f32, h10, z as f32);
243            let p11 = Vec3::new((x + 1) as f32, h11, (z + 1) as f32);
244            let p01 = Vec3::new(x as f32, h01, (z + 1) as f32);
245            let normal = (p10 - p00).cross(p11 - p00) + (p11 - p00).cross(p01 - p00);
246            let length = normal.length();
247            if length <= 1e-6 {
248                continue;
249            }
250            if normal.y.abs() / length < cos_limit {
251                continue;
252            }
253            let key = |x: i32, z: i32, vertices: &mut Vec<Vec3>, corners: &mut HashMap<(i32, i32), u32>| {
254                *corners.entry((x, z)).or_insert_with(|| {
255                    let height = corner_height(x, z).unwrap_or(0.0);
256                    vertices.push(Vec3::new(x as f32, height, z as f32));
257                    (vertices.len() - 1) as u32
258                })
259            };
260            let c0 = key(x, z, &mut vertices, &mut corners);
261            let c1 = key(x + 1, z, &mut vertices, &mut corners);
262            let c2 = key(x + 1, z + 1, &mut vertices, &mut corners);
263            let c3 = key(x, z + 1, &mut vertices, &mut corners);
264            polygons.push(vec![c0, c1, c2, c3]);
265        }
266    }
267    if polygons.is_empty() {
268        return Err(NavError::NoWalkableArea);
269    }
270    NavMesh::from_polygons(vertices, polygons)
271}
repame_nav/bake.rs

repame_nav/
bake.rs
