From dccea21bfe2f4e23e8ac7b786a7d36deff3a0540 Mon Sep 17 00:00:00 2001 From: tolelom <98kimsungmin@naver.com> Date: Wed, 25 Mar 2026 21:00:35 +0900 Subject: [PATCH] feat(ai): add funnel string-pulling and navmesh serialization 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) --- crates/voltex_ai/src/navmesh.rs | 157 ++++++++++++++++++ crates/voltex_ai/src/pathfinding.rs | 243 +++++++++++++++++++++++++--- 2 files changed, 376 insertions(+), 24 deletions(-) diff --git a/crates/voltex_ai/src/navmesh.rs b/crates/voltex_ai/src/navmesh.rs index 3bcbcba..1be8f7d 100644 --- a/crates/voltex_ai/src/navmesh.rs +++ b/crates/voltex_ai/src/navmesh.rs @@ -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 { + 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 { + let mut offset = 0; + + let read_u32 = |off: &mut usize| -> Result { + 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 { + 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 { + 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 { + 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()); + } } diff --git a/crates/voltex_ai/src/pathfinding.rs b/crates/voltex_ai/src/pathfinding.rs index 96bb081..cb8ceeb 100644 --- a/crates/voltex_ai/src/pathfinding.rs +++ b/crates/voltex_ai/src/pathfinding.rs @@ -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> { +/// 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> { 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> = 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 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 } } 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 } } - 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> { + 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 { + // 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); + } + } }