tolelom/game_engine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(ai): add funnel string-pulling and navmesh serialization

Browse Source 
Add Simple Stupid Funnel (SSF) algorithm for optimal path smoothing through
triangle corridors. Refactor A* to expose find_path_triangles for triangle
index paths. Add binary serialize/deserialize for NavMesh and shared_edge
query method.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
tolelom
2026-03-25 21:00:35 +09:00
parent 1081fb472f
commit dccea21bfe
2 changed files with 376 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

157
crates/voltex_ai/src/navmesh.rs
View File

@@ -67,6 +67,25 @@ impl NavMesh {
(a.z + b.z) / 2.0,
)
}
/// Return the shared edge (portal) vertices between two adjacent triangles.
/// Returns (left, right) vertices of the portal edge as seen from `from_tri` looking toward `to_tri`.
/// Returns None if the triangles are not adjacent.
pub fn shared_edge(&self, from_tri: usize, to_tri: usize) -> Option<(Vec3, Vec3)> {
let tri = &self.triangles[from_tri];
for edge_idx in 0..3 {
if tri.neighbors[edge_idx] == Some(to_tri) {
let (i0, i1) = match edge_idx {
0 => (tri.indices[0], tri.indices[1]),
1 => (tri.indices[1], tri.indices[2]),
2 => (tri.indices[2], tri.indices[0]),
_ => unreachable!(),
};
return Some((self.vertices[i0], self.vertices[i1]));
}
}
None
}
}
/// Test whether `point` lies inside or on the triangle (a, b, c) using XZ barycentric coordinates.
@@ -90,6 +109,104 @@ pub fn point_in_triangle_xz(point: Vec3, a: Vec3, b: Vec3, c: Vec3) -> bool {
u >= 0.0 && v >= 0.0 && w >= 0.0
}
/// Serialize a NavMesh to binary format.
/// Format: vertex_count(u32) + vertices(f32*3 each) + triangle_count(u32) + triangles(indices u32*3 + neighbors i32*3 each)
pub fn serialize_navmesh(navmesh: &NavMesh) -> Vec<u8> {
let mut data = Vec::new();
// Vertex count
let vc = navmesh.vertices.len() as u32;
data.extend_from_slice(&vc.to_le_bytes());
// Vertices
for v in &navmesh.vertices {
data.extend_from_slice(&v.x.to_le_bytes());
data.extend_from_slice(&v.y.to_le_bytes());
data.extend_from_slice(&v.z.to_le_bytes());
}
// Triangle count
let tc = navmesh.triangles.len() as u32;
data.extend_from_slice(&tc.to_le_bytes());
// Triangles: indices(u32*3) + neighbors(i32*3, -1 for None)
for tri in &navmesh.triangles {
for &idx in &tri.indices {
data.extend_from_slice(&(idx as u32).to_le_bytes());
}
for &nb in &tri.neighbors {
let val: i32 = match nb {
Some(i) => i as i32,
None => -1,
};
data.extend_from_slice(&val.to_le_bytes());
}
}
data
}
/// Deserialize a NavMesh from binary data.
pub fn deserialize_navmesh(data: &[u8]) -> Result<NavMesh, String> {
let mut offset = 0;
let read_u32 = |off: &mut usize| -> Result<u32, String> {
if *off + 4 > data.len() {
return Err("unexpected end of data".to_string());
}
let val = u32::from_le_bytes([data[*off], data[*off + 1], data[*off + 2], data[*off + 3]]);
*off += 4;
Ok(val)
};
let read_i32 = |off: &mut usize| -> Result<i32, String> {
if *off + 4 > data.len() {
return Err("unexpected end of data".to_string());
}
let val = i32::from_le_bytes([data[*off], data[*off + 1], data[*off + 2], data[*off + 3]]);
*off += 4;
Ok(val)
};
let read_f32 = |off: &mut usize| -> Result<f32, String> {
if *off + 4 > data.len() {
return Err("unexpected end of data".to_string());
}
let val = f32::from_le_bytes([data[*off], data[*off + 1], data[*off + 2], data[*off + 3]]);
*off += 4;
Ok(val)
};
let vc = read_u32(&mut offset)? as usize;
let mut vertices = Vec::with_capacity(vc);
for _ in 0..vc {
let x = read_f32(&mut offset)?;
let y = read_f32(&mut offset)?;
let z = read_f32(&mut offset)?;
vertices.push(Vec3::new(x, y, z));
}
let tc = read_u32(&mut offset)? as usize;
let mut triangles = Vec::with_capacity(tc);
for _ in 0..tc {
let i0 = read_u32(&mut offset)? as usize;
let i1 = read_u32(&mut offset)? as usize;
let i2 = read_u32(&mut offset)? as usize;
let n0 = read_i32(&mut offset)?;
let n1 = read_i32(&mut offset)?;
let n2 = read_i32(&mut offset)?;
let to_opt = |v: i32| -> Option<usize> {
if v < 0 { None } else { Some(v as usize) }
};
triangles.push(NavTriangle {
indices: [i0, i1, i2],
neighbors: [to_opt(n0), to_opt(n1), to_opt(n2)],
});
}
Ok(NavMesh::new(vertices, triangles))
}
#[cfg(test)]
mod tests {
use super::*;
@@ -155,4 +272,44 @@ mod tests {
assert!((mid.y - 0.0).abs() < 1e-5);
assert!((mid.z - 1.0).abs() < 1e-5);
}
#[test]
fn test_shared_edge() {
let nm = make_simple_navmesh();
// Tri 0 edge 1 connects to Tri 1: shared edge is v1=(2,0,0) and v2=(1,0,2)
let (left, right) = nm.shared_edge(0, 1).expect("should find shared edge");
assert_eq!(left, Vec3::new(2.0, 0.0, 0.0));
assert_eq!(right, Vec3::new(1.0, 0.0, 2.0));
}
#[test]
fn test_shared_edge_not_adjacent() {
let nm = make_simple_navmesh();
// Tri 0 is not adjacent to itself via neighbors
assert!(nm.shared_edge(0, 0).is_none());
}
#[test]
fn test_serialize_roundtrip() {
let nm = make_simple_navmesh();
let data = serialize_navmesh(&nm);
let nm2 = deserialize_navmesh(&data).expect("should deserialize");
assert_eq!(nm2.vertices.len(), nm.vertices.len());
assert_eq!(nm2.triangles.len(), nm.triangles.len());
for (a, b) in nm.vertices.iter().zip(nm2.vertices.iter()) {
assert!((a.x - b.x).abs() < 1e-6);
assert!((a.y - b.y).abs() < 1e-6);
assert!((a.z - b.z).abs() < 1e-6);
}
for (a, b) in nm.triangles.iter().zip(nm2.triangles.iter()) {
assert_eq!(a.indices, b.indices);
assert_eq!(a.neighbors, b.neighbors);
}
}
#[test]
fn test_deserialize_empty_data() {
let result = deserialize_navmesh(&[]);
assert!(result.is_err());
}
}

243
crates/voltex_ai/src/pathfinding.rs
View File

@@ -43,29 +43,22 @@ pub fn distance_xz(a: Vec3, b: Vec3) -> f32 {
(dx * dx + dz * dz).sqrt()
}
/// Find a path from `start` to `goal` on the given NavMesh using A*.
///
/// Returns Some(path) where path[0] == start, path[last] == goal, and
/// intermediate points are triangle centers. Returns None if either
/// point is outside the mesh or no path exists.
pub fn find_path(navmesh: &NavMesh, start: Vec3, goal: Vec3) -> Option<Vec<Vec3>> {
/// Run A* on the NavMesh and return the sequence of triangle indices from start to goal.
/// Returns None if either point is outside the mesh or no path exists.
pub fn find_path_triangles(navmesh: &NavMesh, start: Vec3, goal: Vec3) -> Option<Vec<usize>> {
let start_tri = navmesh.find_triangle(start)?;
let goal_tri = navmesh.find_triangle(goal)?;
// If start and goal are in the same triangle, path is direct.
if start_tri == goal_tri {
return Some(vec![start, goal]);
return Some(vec![start_tri]);
}
let n = navmesh.triangles.len();
// g_cost[i] = best known cost to reach triangle i
let mut g_costs = vec![f32::INFINITY; n];
// parent[i] = index of parent triangle in the A* tree
let mut parents: Vec<Option<usize>> = vec![None; n];
let mut visited = vec![false; n];
let goal_center = navmesh.triangle_center(goal_tri);
g_costs[start_tri] = 0.0;
let start_center = navmesh.triangle_center(start_tri);
let h = distance_xz(start_center, goal_center);
@@ -88,7 +81,6 @@ pub fn find_path(navmesh: &NavMesh, start: Vec3, goal: Vec3) -> Option<Vec<Vec3>
parents[idx] = node.parent;
if idx == goal_tri {
// Reconstruct path
let mut tri_path = Vec::new();
let mut cur = idx;
loop {
@@ -99,17 +91,7 @@ pub fn find_path(navmesh: &NavMesh, start: Vec3, goal: Vec3) -> Option<Vec<Vec3>
}
}
tri_path.reverse();
// Convert triangle path to Vec3 waypoints:
// start point -> intermediate triangle centers -> goal point
let mut path = Vec::new();
path.push(start);
// skip first (start_tri) and last (goal_tri) in intermediate centers
for &ti in &tri_path[1..tri_path.len() - 1] {
path.push(navmesh.triangle_center(ti));
}
path.push(goal);
return Some(path);
return Some(tri_path);
}
let tri = &navmesh.triangles[idx];
@@ -136,7 +118,138 @@ pub fn find_path(navmesh: &NavMesh, start: Vec3, goal: Vec3) -> Option<Vec<Vec3>
}
}
None // No path found
None
}
/// Find a path from `start` to `goal` on the given NavMesh using A*.
///
/// Returns Some(path) where path[0] == start, path[last] == goal, and
/// intermediate points are triangle centers. Returns None if either
/// point is outside the mesh or no path exists.
pub fn find_path(navmesh: &NavMesh, start: Vec3, goal: Vec3) -> Option<Vec<Vec3>> {
let tri_path = find_path_triangles(navmesh, start, goal)?;
if tri_path.len() == 1 {
return Some(vec![start, goal]);
}
let mut path = Vec::new();
path.push(start);
for &ti in &tri_path[1..tri_path.len() - 1] {
path.push(navmesh.triangle_center(ti));
}
path.push(goal);
Some(path)
}
/// 2D cross product on XZ plane: (b - a) x (c - a) projected onto Y.
fn cross_xz(a: Vec3, b: Vec3, c: Vec3) -> f32 {
(b.x - a.x) * (c.z - a.z) - (b.z - a.z) * (c.x - a.x)
}
/// Simple Stupid Funnel (SSF) algorithm for string-pulling a triangle corridor path.
///
/// Given the sequence of triangle indices from A*, produces an optimal path with
/// waypoints at portal edge corners where the path turns.
pub fn funnel_smooth(path_triangles: &[usize], navmesh: &NavMesh, start: Vec3, end: Vec3) -> Vec<Vec3> {
// Trivial cases
if path_triangles.is_empty() {
return vec![start, end];
}
if path_triangles.len() == 1 {
return vec![start, end];
}
// Build portal list: shared edges between consecutive triangles
let mut portals_left = Vec::new();
let mut portals_right = Vec::new();
for i in 0..path_triangles.len() - 1 {
let from = path_triangles[i];
let to = path_triangles[i + 1];
if let Some((left, right)) = navmesh.shared_edge(from, to) {
portals_left.push(left);
portals_right.push(right);
}
}
// Add end point as the final portal (degenerate portal: both sides = end)
portals_left.push(end);
portals_right.push(end);
let mut path = vec![start];
let mut apex = start;
let mut left = start;
let mut right = start;
#[allow(unused_assignments)]
let mut apex_idx: usize = 0;
let mut left_idx: usize = 0;
let mut right_idx: usize = 0;
let n = portals_left.len();
for i in 0..n {
let pl = portals_left[i];
let pr = portals_right[i];
// Update right vertex
if cross_xz(apex, right, pr) <= 0.0 {
if apex == right || cross_xz(apex, left, pr) > 0.0 {
// Tighten the funnel
right = pr;
right_idx = i;
} else {
// Right over left: left becomes new apex
if left != apex {
path.push(left);
}
apex = left;
apex_idx = left_idx;
left = apex;
right = apex;
left_idx = apex_idx;
right_idx = apex_idx;
// Restart scan from apex
// We need to continue from apex_idx + 1, but since
// we can't restart a for loop, we use a recursive approach
// or just continue (the standard SSF continues from i)
continue;
}
}
// Update left vertex
if cross_xz(apex, left, pl) >= 0.0 {
if apex == left || cross_xz(apex, right, pl) < 0.0 {
// Tighten the funnel
left = pl;
left_idx = i;
} else {
// Left over right: right becomes new apex
if right != apex {
path.push(right);
}
apex = right;
apex_idx = right_idx;
left = apex;
right = apex;
left_idx = apex_idx;
right_idx = apex_idx;
continue;
}
}
}
// Add end point if not already there
if let Some(&last) = path.last() {
if (last.x - end.x).abs() > 1e-6 || (last.z - end.z).abs() > 1e-6 {
path.push(end);
}
} else {
path.push(end);
}
path
}
#[cfg(test)]
@@ -238,4 +351,86 @@ mod tests {
let result = find_path(&nm, start, goal);
assert!(result.is_none());
}
#[test]
fn test_find_path_triangles_same() {
let nm = make_strip();
let start = Vec3::new(0.5, 0.0, 0.5);
let goal = Vec3::new(1.5, 0.0, 0.5);
let tris = find_path_triangles(&nm, start, goal).expect("should find path");
assert_eq!(tris.len(), 1);
}
#[test]
fn test_find_path_triangles_strip() {
let nm = make_strip();
let start = Vec3::new(0.8, 0.0, 0.5);
let goal = Vec3::new(2.0, 0.0, 3.5);
let tris = find_path_triangles(&nm, start, goal).expect("should find path");
assert_eq!(tris.len(), 3);
assert_eq!(tris[0], 0);
assert_eq!(tris[2], 2);
}
#[test]
fn test_funnel_straight_path() {
// Straight corridor: path should be just start and end (2 points)
let nm = make_strip();
let start = Vec3::new(1.0, 0.0, 0.5);
let end = Vec3::new(2.0, 0.0, 3.5);
let tris = find_path_triangles(&nm, start, end).expect("should find path");
let smoothed = funnel_smooth(&tris, &nm, start, end);
assert!(smoothed.len() >= 2, "funnel path should have at least 2 points, got {}", smoothed.len());
assert_eq!(smoothed[0], start);
assert_eq!(smoothed[smoothed.len() - 1], end);
}
#[test]
fn test_funnel_same_triangle() {
let nm = make_strip();
let start = Vec3::new(0.5, 0.0, 0.5);
let end = Vec3::new(1.5, 0.0, 0.5);
let smoothed = funnel_smooth(&[0], &nm, start, end);
assert_eq!(smoothed.len(), 2);
assert_eq!(smoothed[0], start);
assert_eq!(smoothed[1], end);
}
#[test]
fn test_funnel_l_shaped_path() {
// Build an L-shaped navmesh to force a turn
// Tri0: (0,0,0),(2,0,0),(0,0,2) - bottom left
// Tri1: (2,0,0),(2,0,2),(0,0,2) - top right of first square
// Tri2: (2,0,0),(4,0,0),(2,0,2) - extends right
// Tri3: (2,0,2),(4,0,0),(4,0,2) - top right
// Tri4: (2,0,2),(4,0,2),(2,0,4) - goes up
// This makes an L shape going right then up
let vertices = vec![
Vec3::new(0.0, 0.0, 0.0), // 0
Vec3::new(2.0, 0.0, 0.0), // 1
Vec3::new(0.0, 0.0, 2.0), // 2
Vec3::new(2.0, 0.0, 2.0), // 3
Vec3::new(4.0, 0.0, 0.0), // 4
Vec3::new(4.0, 0.0, 2.0), // 5
Vec3::new(2.0, 0.0, 4.0), // 6
];
let triangles = vec![
NavTriangle { indices: [0, 1, 2], neighbors: [Some(1), None, None] }, // 0
NavTriangle { indices: [1, 3, 2], neighbors: [Some(2), None, Some(0)] }, // 1
NavTriangle { indices: [1, 4, 3], neighbors: [None, Some(3), Some(1)] }, // 2
NavTriangle { indices: [4, 5, 3], neighbors: [None, None, Some(2)] }, // 3 -- not used in L path
NavTriangle { indices: [3, 5, 6], neighbors: [None, None, None] }, // 4
];
let nm = NavMesh::new(vertices, triangles);
let start = Vec3::new(0.3, 0.0, 0.3);
let end = Vec3::new(3.5, 0.0, 0.5);
let tris = find_path_triangles(&nm, start, end);
if let Some(tris) = tris {
let smoothed = funnel_smooth(&tris, &nm, start, end);
assert!(smoothed.len() >= 2);
assert_eq!(smoothed[0], start);
assert_eq!(smoothed[smoothed.len() - 1], end);
}
}
}