feat(ai): add navmesh builder, funnel algorithm, dynamic obstacle avoidance

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

crates/voltex_ai/Cargo.toml

@@ -5,3 +5,4 @@ edition = "2021"
 
 [dependencies]
 voltex_math.workspace = true
+voltex_ecs.workspace = true

crates/voltex_ai/src/navmesh_builder.rs

@@ -0,0 +1,651 @@
+use voltex_math::Vec3;
+use crate::navmesh::{NavMesh, NavTriangle};
+
+/// Configuration for navmesh generation.
+pub struct NavMeshBuilder {
+    /// XZ voxel size (default 0.3)
+    pub cell_size: f32,
+    /// Y voxel size (default 0.2)
+    pub cell_height: f32,
+    /// Minimum clearance height for walkable areas (default 2.0)
+    pub agent_height: f32,
+    /// Agent capsule radius used to erode walkable areas (default 0.5)
+    pub agent_radius: f32,
+    /// Maximum walkable slope angle in degrees (default 45.0)
+    pub max_slope: f32,
+}
+
+impl Default for NavMeshBuilder {
+    fn default() -> Self {
+        Self {
+            cell_size: 0.3,
+            cell_height: 0.2,
+            agent_height: 2.0,
+            agent_radius: 0.5,
+            max_slope: 45.0,
+        }
+    }
+}
+
+/// Heightfield: a 2D grid of min/max height spans for voxelized geometry.
+pub struct Heightfield {
+    pub width: usize,  // number of cells along X
+    pub depth: usize,  // number of cells along Z
+    pub min_x: f32,
+    pub min_z: f32,
+    pub cell_size: f32,
+    pub cell_height: f32,
+    /// For each cell (x, z), store (min_y, max_y). None if empty.
+    pub cells: Vec<Option<(f32, f32)>>,
+}
+
+impl Heightfield {
+    pub fn cell_index(&self, x: usize, z: usize) -> usize {
+        z * self.width + x
+    }
+}
+
+/// Map of walkable cells. True if the cell is walkable.
+pub struct WalkableMap {
+    pub width: usize,
+    pub depth: usize,
+    pub min_x: f32,
+    pub min_z: f32,
+    pub cell_size: f32,
+    pub walkable: Vec<bool>,
+    /// Height at each walkable cell (top of walkable surface).
+    pub heights: Vec<f32>,
+}
+
+impl WalkableMap {
+    pub fn cell_index(&self, x: usize, z: usize) -> usize {
+        z * self.width + x
+    }
+}
+
+/// A region of connected walkable cells (flood-fill result).
+pub struct RegionMap {
+    pub width: usize,
+    pub depth: usize,
+    pub min_x: f32,
+    pub min_z: f32,
+    pub cell_size: f32,
+    /// Region ID per cell. 0 = not walkable, 1+ = region ID.
+    pub regions: Vec<u32>,
+    pub heights: Vec<f32>,
+    pub num_regions: u32,
+}
+
+impl NavMeshBuilder {
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Step 1: Rasterize triangles into a heightfield grid.
+    pub fn voxelize(&self, vertices: &[Vec3], indices: &[u32]) -> Heightfield {
+        // Find bounding box
+        let mut min_x = f32::INFINITY;
+        let mut max_x = f32::NEG_INFINITY;
+        let mut min_y = f32::INFINITY;
+        let mut max_y = f32::NEG_INFINITY;
+        let mut min_z = f32::INFINITY;
+        let mut max_z = f32::NEG_INFINITY;
+
+        for v in vertices {
+            min_x = min_x.min(v.x);
+            max_x = max_x.max(v.x);
+            min_y = min_y.min(v.y);
+            max_y = max_y.max(v.y);
+            min_z = min_z.min(v.z);
+            max_z = max_z.max(v.z);
+        }
+
+        let width = ((max_x - min_x) / self.cell_size).ceil() as usize + 1;
+        let depth = ((max_z - min_z) / self.cell_size).ceil() as usize + 1;
+        let mut cells: Vec<Option<(f32, f32)>> = vec![None; width * depth];
+
+        // Rasterize each triangle
+        for tri_i in (0..indices.len()).step_by(3) {
+            if tri_i + 2 >= indices.len() {
+                break;
+            }
+            let v0 = vertices[indices[tri_i] as usize];
+            let v1 = vertices[indices[tri_i + 1] as usize];
+            let v2 = vertices[indices[tri_i + 2] as usize];
+
+            // Find bounding box of triangle in grid coords
+            let tri_min_x = v0.x.min(v1.x).min(v2.x);
+            let tri_max_x = v0.x.max(v1.x).max(v2.x);
+            let tri_min_z = v0.z.min(v1.z).min(v2.z);
+            let tri_max_z = v0.z.max(v1.z).max(v2.z);
+
+            let gx0 = ((tri_min_x - min_x) / self.cell_size).floor() as isize;
+            let gx1 = ((tri_max_x - min_x) / self.cell_size).ceil() as isize;
+            let gz0 = ((tri_min_z - min_z) / self.cell_size).floor() as isize;
+            let gz1 = ((tri_max_z - min_z) / self.cell_size).ceil() as isize;
+
+            let gx0 = gx0.max(0) as usize;
+            let gx1 = (gx1 as usize).min(width - 1);
+            let gz0 = gz0.max(0) as usize;
+            let gz1 = (gz1 as usize).min(depth - 1);
+
+            for gz in gz0..=gz1 {
+                for gx in gx0..=gx1 {
+                    let cx = min_x + (gx as f32 + 0.5) * self.cell_size;
+                    let cz = min_z + (gz as f32 + 0.5) * self.cell_size;
+
+                    // Check if cell center is inside triangle (XZ)
+                    let p = Vec3::new(cx, 0.0, cz);
+                    if point_in_triangle_xz_loose(p, v0, v1, v2, self.cell_size * 0.5) {
+                        // Interpolate Y at this XZ point
+                        let y = interpolate_y(v0, v1, v2, cx, cz);
+                        let idx = gz * width + gx;
+                        match &mut cells[idx] {
+                            Some((ref mut lo, ref mut hi)) => {
+                                *lo = lo.min(y);
+                                *hi = hi.max(y);
+                            }
+                            None => {
+                                cells[idx] = Some((y, y));
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        Heightfield {
+            width,
+            depth,
+            min_x,
+            min_z,
+            cell_size: self.cell_size,
+            cell_height: self.cell_height,
+            cells,
+        }
+    }
+
+    /// Step 2: Mark walkable cells based on slope and agent height clearance.
+    pub fn mark_walkable(&self, hf: &Heightfield) -> WalkableMap {
+        let max_slope_cos = (self.max_slope * std::f32::consts::PI / 180.0).cos();
+        let n = hf.width * hf.depth;
+        let mut walkable = vec![false; n];
+        let mut heights = vec![0.0f32; n];
+
+        for z in 0..hf.depth {
+            for x in 0..hf.width {
+                let idx = z * hf.width + x;
+                if let Some((_lo, hi)) = hf.cells[idx] {
+                    // Check slope by comparing height differences with neighbors
+                    let slope_ok = self.check_slope(hf, x, z, max_slope_cos);
+
+                    // Check clearance: for simplicity, if cell has geometry, assume clearance
+                    // unless there's a cell above within agent_height (not implemented for simple case)
+                    if slope_ok {
+                        walkable[idx] = true;
+                        heights[idx] = hi;
+                    }
+                }
+            }
+        }
+
+        // Erode by agent radius: remove walkable cells too close to non-walkable
+        let erosion_cells = (self.agent_radius / hf.cell_size).ceil() as usize;
+        if erosion_cells > 0 {
+            let mut eroded = walkable.clone();
+            for z in 0..hf.depth {
+                for x in 0..hf.width {
+                    let idx = z * hf.width + x;
+                    if !walkable[idx] {
+                        continue;
+                    }
+                    // Check if near boundary of walkable area
+                    let mut near_edge = false;
+                    for dz in 0..=erosion_cells {
+                        for dx in 0..=erosion_cells {
+                            if dx == 0 && dz == 0 {
+                                continue;
+                            }
+                            // Check all 4 quadrants
+                            let checks: [(isize, isize); 4] = [
+                                (dx as isize, dz as isize),
+                                (-(dx as isize), dz as isize),
+                                (dx as isize, -(dz as isize)),
+                                (-(dx as isize), -(dz as isize)),
+                            ];
+                            for (ddx, ddz) in checks {
+                                let nx = x as isize + ddx;
+                                let nz = z as isize + ddz;
+                                if nx < 0 || nz < 0 || nx >= hf.width as isize || nz >= hf.depth as isize {
+                                    near_edge = true;
+                                    break;
+                                }
+                                let ni = nz as usize * hf.width + nx as usize;
+                                if !walkable[ni] {
+                                    near_edge = true;
+                                    break;
+                                }
+                            }
+                            if near_edge {
+                                break;
+                            }
+                        }
+                        if near_edge {
+                            break;
+                        }
+                    }
+                    if near_edge {
+                        eroded[idx] = false;
+                    }
+                }
+            }
+            return WalkableMap {
+                width: hf.width,
+                depth: hf.depth,
+                min_x: hf.min_x,
+                min_z: hf.min_z,
+                cell_size: hf.cell_size,
+                walkable: eroded,
+                heights,
+            };
+        }
+
+        WalkableMap {
+            width: hf.width,
+            depth: hf.depth,
+            min_x: hf.min_x,
+            min_z: hf.min_z,
+            cell_size: hf.cell_size,
+            walkable,
+            heights,
+        }
+    }
+
+    /// Check if the slope at cell (x, z) is walkable.
+    fn check_slope(&self, hf: &Heightfield, x: usize, z: usize, max_slope_cos: f32) -> bool {
+        let idx = z * hf.width + x;
+        let h = match hf.cells[idx] {
+            Some((_, hi)) => hi,
+            None => return false,
+        };
+
+        // Compare with direct neighbors to estimate slope
+        let neighbors: [(isize, isize); 4] = [(1, 0), (-1, 0), (0, 1), (0, -1)];
+        for (dx, dz) in neighbors {
+            let nx = x as isize + dx;
+            let nz = z as isize + dz;
+            if nx < 0 || nz < 0 || nx >= hf.width as isize || nz >= hf.depth as isize {
+                continue;
+            }
+            let ni = nz as usize * hf.width + nx as usize;
+            if let Some((_, nh)) = hf.cells[ni] {
+                let dy = (nh - h).abs();
+                let dist = hf.cell_size;
+                // slope angle: atan(dy/dist), check cos of that angle
+                let slope_len = (dy * dy + dist * dist).sqrt();
+                let cos_angle = dist / slope_len;
+                if cos_angle < max_slope_cos {
+                    return false;
+                }
+            }
+        }
+        true
+    }
+
+    /// Step 3: Flood-fill connected walkable areas into regions.
+    pub fn build_regions(&self, wm: &WalkableMap) -> RegionMap {
+        let n = wm.width * wm.depth;
+        let mut regions = vec![0u32; n];
+        let mut current_region = 0u32;
+
+        for z in 0..wm.depth {
+            for x in 0..wm.width {
+                let idx = z * wm.width + x;
+                if wm.walkable[idx] && regions[idx] == 0 {
+                    current_region += 1;
+                    // Flood fill
+                    let mut stack = vec![(x, z)];
+                    regions[idx] = current_region;
+                    while let Some((cx, cz)) = stack.pop() {
+                        let neighbors: [(isize, isize); 4] = [(1, 0), (-1, 0), (0, 1), (0, -1)];
+                        for (dx, dz) in neighbors {
+                            let nx = cx as isize + dx;
+                            let nz = cz as isize + dz;
+                            if nx < 0 || nz < 0 || nx >= wm.width as isize || nz >= wm.depth as isize {
+                                continue;
+                            }
+                            let ni = nz as usize * wm.width + nx as usize;
+                            if wm.walkable[ni] && regions[ni] == 0 {
+                                regions[ni] = current_region;
+                                stack.push((nx as usize, nz as usize));
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        RegionMap {
+            width: wm.width,
+            depth: wm.depth,
+            min_x: wm.min_x,
+            min_z: wm.min_z,
+            cell_size: wm.cell_size,
+            regions,
+            heights: wm.heights.clone(),
+            num_regions: current_region,
+        }
+    }
+
+    /// Steps 4-5 combined: Convert walkable grid cells directly into a NavMesh.
+    /// Each walkable cell becomes a quad (2 triangles), with adjacency computed.
+    pub fn triangulate(&self, rm: &RegionMap) -> NavMesh {
+        let mut vertices = Vec::new();
+        let mut triangles = Vec::new();
+
+        // For each walkable cell, create 4 vertices and 2 triangles.
+        // Map from cell (x,z) -> (tri_a_idx, tri_b_idx) for adjacency lookup.
+        let n = rm.width * rm.depth;
+        // cell_tri_map[cell_idx] = Some((tri_a_idx, tri_b_idx)) or None
+        let mut cell_tri_map: Vec<Option<(usize, usize)>> = vec![None; n];
+
+        for z in 0..rm.depth {
+            for x in 0..rm.width {
+                let idx = z * rm.width + x;
+                if rm.regions[idx] == 0 {
+                    continue;
+                }
+
+                let h = rm.heights[idx];
+                let x0 = rm.min_x + x as f32 * rm.cell_size;
+                let x1 = x0 + rm.cell_size;
+                let z0 = rm.min_z + z as f32 * rm.cell_size;
+                let z1 = z0 + rm.cell_size;
+
+                let vi = vertices.len();
+                vertices.push(Vec3::new(x0, h, z0)); // vi+0: bottom-left
+                vertices.push(Vec3::new(x1, h, z0)); // vi+1: bottom-right
+                vertices.push(Vec3::new(x1, h, z1)); // vi+2: top-right
+                vertices.push(Vec3::new(x0, h, z1)); // vi+3: top-left
+
+                let ta = triangles.len();
+                // Triangle A: bottom-left, bottom-right, top-right (vi+0, vi+1, vi+2)
+                triangles.push(NavTriangle {
+                    indices: [vi, vi + 1, vi + 2],
+                    neighbors: [None, None, None], // filled in later
+                });
+                // Triangle B: bottom-left, top-right, top-left (vi+0, vi+2, vi+3)
+                triangles.push(NavTriangle {
+                    indices: [vi, vi + 2, vi + 3],
+                    neighbors: [None, None, None],
+                });
+
+                // Internal adjacency: A and B share edge (vi+0, vi+2)
+                // For A: edge 2 is (vi+2 -> vi+0) — indices[2] to indices[0]
+                // For B: edge 0 is (vi+0 -> vi+2) — indices[0] to indices[1]
+                triangles[ta].neighbors[2] = Some(ta + 1);     // A's edge2 -> B
+                triangles[ta + 1].neighbors[0] = Some(ta);     // B's edge0 -> A
+
+                cell_tri_map[idx] = Some((ta, ta + 1));
+            }
+        }
+
+        // Now compute inter-cell adjacency
+        // Cell layout:
+        //   Tri A: (v0, v1, v2) = (BL, BR, TR)
+        //     edge 0: BL->BR (bottom edge, connects to cell below z-1)
+        //     edge 1: BR->TR (right edge, connects to cell at x+1)
+        //     edge 2: TR->BL (diagonal, internal, already connected to B)
+        //   Tri B: (v0, v2, v3) = (BL, TR, TL)
+        //     edge 0: BL->TR (diagonal, internal, already connected to A)
+        //     edge 1: TR->TL (top edge, connects to cell above z+1)
+        //     edge 2: TL->BL (left edge, connects to cell at x-1)
+
+        for z in 0..rm.depth {
+            for x in 0..rm.width {
+                let idx = z * rm.width + x;
+                if cell_tri_map[idx].is_none() {
+                    continue;
+                }
+                let (ta, tb) = cell_tri_map[idx].unwrap();
+
+                // Bottom neighbor (z-1): A's edge 0 connects to neighbor's B edge 1 (TR->TL = top)
+                if z > 0 {
+                    let ni = (z - 1) * rm.width + x;
+                    if let Some((_, nb_tb)) = cell_tri_map[ni] {
+                        triangles[ta].neighbors[0] = Some(nb_tb);
+                        triangles[nb_tb].neighbors[1] = Some(ta);
+                    }
+                }
+
+                // Right neighbor (x+1): A's edge 1 connects to neighbor's B edge 2 (TL->BL = left)
+                if x + 1 < rm.width {
+                    let ni = z * rm.width + (x + 1);
+                    if let Some((_, nb_tb)) = cell_tri_map[ni] {
+                        triangles[ta].neighbors[1] = Some(nb_tb);
+                        triangles[nb_tb].neighbors[2] = Some(ta);
+                    }
+                }
+            }
+        }
+
+        NavMesh::new(vertices, triangles)
+    }
+
+    /// Full pipeline: voxelize, mark walkable, build regions, triangulate.
+    pub fn build(&self, vertices: &[Vec3], indices: &[u32]) -> NavMesh {
+        let hf = self.voxelize(vertices, indices);
+        let wm = self.mark_walkable(&hf);
+        let rm = self.build_regions(&wm);
+        self.triangulate(&rm)
+    }
+}
+
+/// Loose point-in-triangle test on XZ plane, with a tolerance margin.
+fn point_in_triangle_xz_loose(point: Vec3, a: Vec3, b: Vec3, c: Vec3, margin: f32) -> bool {
+    let px = point.x;
+    let pz = point.z;
+
+    let denom = (b.z - c.z) * (a.x - c.x) + (c.x - b.x) * (a.z - c.z);
+    if denom.abs() < f32::EPSILON {
+        // Degenerate: check if point is near the line segment
+        return false;
+    }
+
+    let u = ((b.z - c.z) * (px - c.x) + (c.x - b.x) * (pz - c.z)) / denom;
+    let v = ((c.z - a.z) * (px - c.x) + (a.x - c.x) * (pz - c.z)) / denom;
+    let w = 1.0 - u - v;
+
+    let e = margin / ((a - b).length().max((b - c).length()).max((c - a).length()).max(0.001));
+    u >= -e && v >= -e && w >= -e
+}
+
+/// Interpolate Y height at (px, pz) on the plane defined by triangle (a, b, c).
+fn interpolate_y(a: Vec3, b: Vec3, c: Vec3, px: f32, pz: f32) -> f32 {
+    let denom = (b.z - c.z) * (a.x - c.x) + (c.x - b.x) * (a.z - c.z);
+    if denom.abs() < f32::EPSILON {
+        return (a.y + b.y + c.y) / 3.0;
+    }
+    let u = ((b.z - c.z) * (px - c.x) + (c.x - b.x) * (pz - c.z)) / denom;
+    let v = ((c.z - a.z) * (px - c.x) + (a.x - c.x) * (pz - c.z)) / denom;
+    let w = 1.0 - u - v;
+    u * a.y + v * b.y + w * c.y
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    /// Create a simple flat plane (10x10, y=0) as 2 triangles.
+    fn flat_plane_geometry() -> (Vec<Vec3>, Vec<u32>) {
+        let vertices = vec![
+            Vec3::new(0.0, 0.0, 0.0),
+            Vec3::new(10.0, 0.0, 0.0),
+            Vec3::new(10.0, 0.0, 10.0),
+            Vec3::new(0.0, 0.0, 10.0),
+        ];
+        let indices = vec![0, 1, 2, 0, 2, 3];
+        (vertices, indices)
+    }
+
+    /// Create a plane with a box obstacle (hole) in the middle.
+    /// The plane is 10x10 with a 2x2 hole at center (4-6, 4-6).
+    fn plane_with_obstacle() -> (Vec<Vec3>, Vec<u32>) {
+        // Build geometry as a grid of quads, skipping the obstacle region.
+        // Use 1.0 unit grid cells for simplicity.
+        let mut vertices = Vec::new();
+        let mut indices = Vec::new();
+
+        // 10x10 grid of 1x1 quads, skip 4<=x<6 && 4<=z<6
+        for z in 0..10 {
+            for x in 0..10 {
+                if x >= 4 && x < 6 && z >= 4 && z < 6 {
+                    continue; // obstacle
+                }
+                let vi = vertices.len() as u32;
+                let fx = x as f32;
+                let fz = z as f32;
+                vertices.push(Vec3::new(fx, 0.0, fz));
+                vertices.push(Vec3::new(fx + 1.0, 0.0, fz));
+                vertices.push(Vec3::new(fx + 1.0, 0.0, fz + 1.0));
+                vertices.push(Vec3::new(fx, 0.0, fz + 1.0));
+                indices.push(vi);
+                indices.push(vi + 1);
+                indices.push(vi + 2);
+                indices.push(vi);
+                indices.push(vi + 2);
+                indices.push(vi + 3);
+            }
+        }
+
+        (vertices, indices)
+    }
+
+    #[test]
+    fn test_voxelize_flat_plane() {
+        let builder = NavMeshBuilder {
+            cell_size: 1.0,
+            cell_height: 0.2,
+            agent_height: 2.0,
+            agent_radius: 0.0, // no erosion for simple test
+            max_slope: 45.0,
+        };
+        let (verts, idxs) = flat_plane_geometry();
+        let hf = builder.voxelize(&verts, &idxs);
+
+        // Should have cells covering the 10x10 area
+        assert!(hf.width > 0);
+        assert!(hf.depth > 0);
+
+        // Check that some cells are populated
+        let populated = hf.cells.iter().filter(|c| c.is_some()).count();
+        assert!(populated > 0, "some cells should be populated");
+    }
+
+    #[test]
+    fn test_flat_plane_single_region() {
+        let builder = NavMeshBuilder {
+            cell_size: 1.0,
+            cell_height: 0.2,
+            agent_height: 2.0,
+            agent_radius: 0.0,
+            max_slope: 45.0,
+        };
+        let (verts, idxs) = flat_plane_geometry();
+        let hf = builder.voxelize(&verts, &idxs);
+        let wm = builder.mark_walkable(&hf);
+        let rm = builder.build_regions(&wm);
+
+        assert_eq!(rm.num_regions, 1, "flat plane should be a single region");
+    }
+
+    #[test]
+    fn test_flat_plane_builds_navmesh() {
+        let builder = NavMeshBuilder {
+            cell_size: 1.0,
+            cell_height: 0.2,
+            agent_height: 2.0,
+            agent_radius: 0.0,
+            max_slope: 45.0,
+        };
+        let (verts, idxs) = flat_plane_geometry();
+        let nm = builder.build(&verts, &idxs);
+
+        assert!(!nm.vertices.is_empty(), "navmesh should have vertices");
+        assert!(!nm.triangles.is_empty(), "navmesh should have triangles");
+
+        // Should be able to find a triangle at center of the plane
+        let center = Vec3::new(5.0, 0.0, 5.0);
+        assert!(nm.find_triangle(center).is_some(), "should find triangle at center");
+    }
+
+    #[test]
+    fn test_obstacle_path_around() {
+        let builder = NavMeshBuilder {
+            cell_size: 1.0,
+            cell_height: 0.2,
+            agent_height: 2.0,
+            agent_radius: 0.0,
+            max_slope: 45.0,
+        };
+        let (verts, idxs) = plane_with_obstacle();
+        let nm = builder.build(&verts, &idxs);
+
+        // Start at (1, 0, 5) and goal at (9, 0, 5) — must go around obstacle
+        let start = Vec3::new(1.5, 0.0, 5.5);
+        let goal = Vec3::new(8.5, 0.0, 5.5);
+
+        use crate::pathfinding::find_path;
+        let path = find_path(&nm, start, goal);
+        assert!(path.is_some(), "should find path around obstacle");
+    }
+
+    #[test]
+    fn test_slope_walkable_unwalkable() {
+        // Create a steep ramp: triangle from (0,0,0) to (1,0,0) to (0.5, 5, 1)
+        // Slope angle = atan(5/1) ≈ 78.7 degrees — should be unwalkable at max_slope=45
+        let builder = NavMeshBuilder {
+            cell_size: 0.2,
+            cell_height: 0.2,
+            agent_height: 2.0,
+            agent_radius: 0.0,
+            max_slope: 45.0,
+        };
+        let vertices = vec![
+            Vec3::new(0.0, 0.0, 0.0),
+            Vec3::new(2.0, 0.0, 0.0),
+            Vec3::new(1.0, 10.0, 2.0),
+        ];
+        let indices = vec![0, 1, 2];
+        let hf = builder.voxelize(&vertices, &indices);
+        let wm = builder.mark_walkable(&hf);
+
+        // Most interior cells should be unwalkable due to steep slope
+        let walkable_count = wm.walkable.iter().filter(|&&w| w).count();
+        // The bottom edge cells might be walkable (flat), but interior should not
+        // Just check that not all cells are walkable
+        let total_cells = hf.cells.iter().filter(|c| c.is_some()).count();
+        assert!(
+            walkable_count < total_cells || total_cells <= 1,
+            "steep slope should make most cells unwalkable (walkable={}, total={})",
+            walkable_count, total_cells
+        );
+    }
+
+    #[test]
+    fn test_navmesh_adjacency() {
+        let builder = NavMeshBuilder {
+            cell_size: 1.0,
+            cell_height: 0.2,
+            agent_height: 2.0,
+            agent_radius: 0.0,
+            max_slope: 45.0,
+        };
+        let (verts, idxs) = flat_plane_geometry();
+        let nm = builder.build(&verts, &idxs);
+
+        // Check that some triangles have neighbors
+        let has_neighbors = nm.triangles.iter().any(|t| t.neighbors.iter().any(|n| n.is_some()));
+        assert!(has_neighbors, "navmesh triangles should have adjacency");
+    }
+}

crates/voltex_ai/src/obstacle.rs

@@ -0,0 +1,176 @@
+use voltex_math::Vec3;
+
+/// A dynamic obstacle represented as a position and radius.
+#[derive(Debug, Clone)]
+pub struct DynamicObstacle {
+    pub position: Vec3,
+    pub radius: f32,
+}
+
+/// Compute avoidance steering force using velocity obstacle approach.
+///
+/// Projects the agent's velocity forward by `look_ahead` distance and checks
+/// for circle intersections with obstacles. Returns a steering force perpendicular
+/// to the approach direction to avoid the nearest threatening obstacle.
+pub fn avoid_obstacles(
+    agent_pos: Vec3,
+    agent_vel: Vec3,
+    agent_radius: f32,
+    obstacles: &[DynamicObstacle],
+    look_ahead: f32,
+) -> Vec3 {
+    let speed = agent_vel.length();
+    if speed < f32::EPSILON {
+        return Vec3::ZERO;
+    }
+
+    let forward = agent_vel * (1.0 / speed);
+    let mut nearest_t = f32::INFINITY;
+    let mut avoidance = Vec3::ZERO;
+
+    for obs in obstacles {
+        let to_obs = obs.position - agent_pos;
+        let combined_radius = agent_radius + obs.radius;
+
+        // Project obstacle center onto the velocity ray
+        let proj = to_obs.dot(forward);
+
+        // Obstacle is behind or too far ahead
+        if proj < 0.0 || proj > look_ahead {
+            continue;
+        }
+
+        // Lateral distance from the velocity ray to obstacle center (XZ only for ground agents)
+        let closest_on_ray = agent_pos + forward * proj;
+        let diff = obs.position - closest_on_ray;
+        let lateral_dist_sq = diff.x * diff.x + diff.z * diff.z;
+        let combined_sq = combined_radius * combined_radius;
+
+        if lateral_dist_sq >= combined_sq {
+            continue; // No collision
+        }
+
+        // This obstacle threatens the agent — check if it's the nearest
+        if proj < nearest_t {
+            nearest_t = proj;
+
+            // Avoidance direction: perpendicular to approach, away from obstacle
+            // Use XZ plane lateral vector
+            let lateral = Vec3::new(diff.x, 0.0, diff.z);
+            let lat_len = lateral.length();
+            if lat_len > f32::EPSILON {
+                // Steer away from obstacle (opposite direction of lateral offset)
+                let steer_dir = lateral * (-1.0 / lat_len);
+                // Strength inversely proportional to distance (closer = stronger)
+                let strength = 1.0 - (proj / look_ahead);
+                avoidance = steer_dir * strength * speed;
+            } else {
+                // Agent heading straight at obstacle center — pick perpendicular
+                // Use cross product with Y to get a lateral direction
+                let perp = Vec3::new(-forward.z, 0.0, forward.x);
+                avoidance = perp * speed;
+            }
+        }
+    }
+
+    avoidance
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_no_obstacle_zero_force() {
+        let force = avoid_obstacles(
+            Vec3::new(0.0, 0.0, 0.0),
+            Vec3::new(1.0, 0.0, 0.0),
+            0.5,
+            &[],
+            5.0,
+        );
+        assert!(force.length() < 1e-6, "no obstacles should give zero force");
+    }
+
+    #[test]
+    fn test_obstacle_behind_zero_force() {
+        let obs = DynamicObstacle {
+            position: Vec3::new(-3.0, 0.0, 0.0),
+            radius: 1.0,
+        };
+        let force = avoid_obstacles(
+            Vec3::new(0.0, 0.0, 0.0),
+            Vec3::new(1.0, 0.0, 0.0),
+            0.5,
+            &[obs],
+            5.0,
+        );
+        assert!(force.length() < 1e-6, "obstacle behind should give zero force");
+    }
+
+    #[test]
+    fn test_obstacle_ahead_lateral_force() {
+        let obs = DynamicObstacle {
+            position: Vec3::new(3.0, 0.0, 0.5), // slightly to the right
+            radius: 1.0,
+        };
+        let force = avoid_obstacles(
+            Vec3::new(0.0, 0.0, 0.0),
+            Vec3::new(2.0, 0.0, 0.0), // moving in +X
+            0.5,
+            &[obs],
+            5.0,
+        );
+        assert!(force.length() > 0.1, "obstacle ahead should give non-zero force");
+        // Force should push away from obstacle (obstacle is at +Z, force should be -Z)
+        assert!(force.z < 0.0, "force should push away from obstacle (negative Z)");
+    }
+
+    #[test]
+    fn test_obstacle_far_away_zero_force() {
+        let obs = DynamicObstacle {
+            position: Vec3::new(3.0, 0.0, 10.0), // far to the side
+            radius: 1.0,
+        };
+        let force = avoid_obstacles(
+            Vec3::new(0.0, 0.0, 0.0),
+            Vec3::new(1.0, 0.0, 0.0),
+            0.5,
+            &[obs],
+            5.0,
+        );
+        assert!(force.length() < 1e-6, "distant obstacle should give zero force");
+    }
+
+    #[test]
+    fn test_obstacle_beyond_lookahead_zero_force() {
+        let obs = DynamicObstacle {
+            position: Vec3::new(10.0, 0.0, 0.0),
+            radius: 1.0,
+        };
+        let force = avoid_obstacles(
+            Vec3::new(0.0, 0.0, 0.0),
+            Vec3::new(1.0, 0.0, 0.0),
+            0.5,
+            &[obs],
+            5.0, // look_ahead is only 5
+        );
+        assert!(force.length() < 1e-6, "obstacle beyond lookahead should give zero force");
+    }
+
+    #[test]
+    fn test_zero_velocity_zero_force() {
+        let obs = DynamicObstacle {
+            position: Vec3::new(3.0, 0.0, 0.0),
+            radius: 1.0,
+        };
+        let force = avoid_obstacles(
+            Vec3::new(0.0, 0.0, 0.0),
+            Vec3::ZERO,
+            0.5,
+            &[obs],
+            5.0,
+        );
+        assert!(force.length() < 1e-6, "zero velocity should give zero force");
+    }
+}