From 5d0fc9d8d1c9d0f7f962b3b626025b1454eeb0f6 Mon Sep 17 00:00:00 2001 From: tolelom <98kimsungmin@naver.com> Date: Wed, 25 Mar 2026 14:18:45 +0900 Subject: [PATCH] feat(ai): add A* pathfinding on NavMesh triangle graph Implements find_path() using A* over triangle adjacency with XZ heuristic. Path starts at the given start point, passes through intermediate triangle centers, and ends at the goal point. Returns None when points are outside the mesh or no connection exists. 5 passing tests. Co-Authored-By: Claude Sonnet 4.6 --- crates/voltex_ai/src/pathfinding.rs | 241 ++++++++++++++++++++++++++++ 1 file changed, 241 insertions(+) create mode 100644 crates/voltex_ai/src/pathfinding.rs diff --git a/crates/voltex_ai/src/pathfinding.rs b/crates/voltex_ai/src/pathfinding.rs new file mode 100644 index 0000000..96bb081 --- /dev/null +++ b/crates/voltex_ai/src/pathfinding.rs @@ -0,0 +1,241 @@ +use std::collections::BinaryHeap; +use std::cmp::Ordering; +use voltex_math::Vec3; +use crate::navmesh::NavMesh; + +/// Node for A* priority queue (min-heap by f_cost, then g_cost). +#[derive(Debug, Clone)] +struct AStarNode { + tri_idx: usize, + g_cost: f32, + f_cost: f32, + parent: Option, +} + +impl PartialEq for AStarNode { + fn eq(&self, other: &Self) -> bool { + self.f_cost == other.f_cost && self.g_cost == other.g_cost + } +} + +impl Eq for AStarNode {} + +// BinaryHeap is a max-heap; we negate costs to get min-heap behavior. +impl PartialOrd for AStarNode { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} + +impl Ord for AStarNode { + fn cmp(&self, other: &Self) -> Ordering { + // Reverse ordering: lower f_cost = higher priority + other.f_cost.partial_cmp(&self.f_cost) + .unwrap_or(Ordering::Equal) + .then_with(|| other.g_cost.partial_cmp(&self.g_cost).unwrap_or(Ordering::Equal)) + } +} + +/// XZ distance between two Vec3 points (ignoring Y). +pub fn distance_xz(a: Vec3, b: Vec3) -> f32 { + let dx = a.x - b.x; + let dz = a.z - b.z; + (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> { + 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]); + } + + 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); + + let mut open = BinaryHeap::new(); + open.push(AStarNode { + tri_idx: start_tri, + g_cost: 0.0, + f_cost: h, + parent: None, + }); + + while let Some(node) = open.pop() { + let idx = node.tri_idx; + + if visited[idx] { + continue; + } + visited[idx] = true; + parents[idx] = node.parent; + + if idx == goal_tri { + // Reconstruct path + let mut tri_path = Vec::new(); + let mut cur = idx; + loop { + tri_path.push(cur); + match parents[cur] { + Some(p) => cur = p, + None => break, + } + } + 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); + } + + let tri = &navmesh.triangles[idx]; + let current_center = navmesh.triangle_center(idx); + + for neighbor_opt in &tri.neighbors { + if let Some(nb_idx) = *neighbor_opt { + if visited[nb_idx] { + continue; + } + let nb_center = navmesh.triangle_center(nb_idx); + let tentative_g = g_costs[idx] + distance_xz(current_center, nb_center); + if tentative_g < g_costs[nb_idx] { + g_costs[nb_idx] = tentative_g; + let h_nb = distance_xz(nb_center, goal_center); + open.push(AStarNode { + tri_idx: nb_idx, + g_cost: tentative_g, + f_cost: tentative_g + h_nb, + parent: Some(idx), + }); + } + } + } + } + + None // No path found +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::navmesh::{NavMesh, NavTriangle}; + + /// Build a 3-triangle strip along Z axis: + /// v0=(0,0,0), v1=(2,0,0), v2=(0,0,2), v3=(2,0,2), v4=(0,0,4), v5=(2,0,4) + /// Tri0: v0,v1,v2 — Tri1: v1,v3,v2 — Tri2: v2,v3,v4... wait, need a strip + /// + /// Simpler strip: + /// Tri0: (0,0,0),(2,0,0),(1,0,2) center ~(1,0,0.67) + /// Tri1: (2,0,0),(3,0,2),(1,0,2) center ~(2,0,1.33) + /// Tri2: (1,0,2),(3,0,2),(2,0,4) center ~(2,0,2.67) + fn make_strip() -> NavMesh { + let vertices = vec![ + Vec3::new(0.0, 0.0, 0.0), // 0 + Vec3::new(2.0, 0.0, 0.0), // 1 + Vec3::new(1.0, 0.0, 2.0), // 2 + Vec3::new(3.0, 0.0, 2.0), // 3 + Vec3::new(2.0, 0.0, 4.0), // 4 + ]; + let triangles = vec![ + NavTriangle { indices: [0, 1, 2], neighbors: [None, Some(1), None] }, + NavTriangle { indices: [1, 3, 2], neighbors: [None, Some(2), Some(0)] }, + NavTriangle { indices: [2, 3, 4], neighbors: [Some(1), None, None] }, + ]; + NavMesh::new(vertices, triangles) + } + + fn make_disconnected() -> NavMesh { + // Two separate triangles not connected + let vertices = vec![ + Vec3::new(0.0, 0.0, 0.0), + Vec3::new(2.0, 0.0, 0.0), + Vec3::new(1.0, 0.0, 2.0), + Vec3::new(10.0, 0.0, 10.0), + Vec3::new(12.0, 0.0, 10.0), + Vec3::new(11.0, 0.0, 12.0), + ]; + let triangles = vec![ + NavTriangle { indices: [0, 1, 2], neighbors: [None, None, None] }, + NavTriangle { indices: [3, 4, 5], neighbors: [None, None, None] }, + ]; + NavMesh::new(vertices, triangles) + } + + #[test] + fn test_path_same_triangle() { + 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 path = find_path(&nm, start, goal).expect("should find path"); + assert_eq!(path.len(), 2); + assert_eq!(path[0], start); + assert_eq!(path[path.len() - 1], goal); + } + + #[test] + fn test_path_adjacent_triangles() { + let nm = make_strip(); + // start in tri0, goal in tri1 + let start = Vec3::new(0.8, 0.0, 0.5); + let goal = Vec3::new(2.5, 0.0, 1.5); + let path = find_path(&nm, start, goal).expect("should find path"); + assert!(path.len() >= 2); + assert_eq!(path[0], start); + assert_eq!(path[path.len() - 1], goal); + } + + #[test] + fn test_path_three_triangle_strip() { + let nm = make_strip(); + // start in tri0, goal in tri2 + let start = Vec3::new(0.8, 0.0, 0.5); + let goal = Vec3::new(2.0, 0.0, 3.5); + let path = find_path(&nm, start, goal).expect("should find path"); + assert!(path.len() >= 3, "path through 3 triangles should have at least 3 points"); + assert_eq!(path[0], start); + assert_eq!(path[path.len() - 1], goal); + } + + #[test] + fn test_path_outside_returns_none() { + let nm = make_strip(); + // start is outside the mesh + let start = Vec3::new(100.0, 0.0, 100.0); + let goal = Vec3::new(0.8, 0.0, 0.5); + let result = find_path(&nm, start, goal); + assert!(result.is_none()); + } + + #[test] + fn test_path_disconnected_returns_none() { + let nm = make_disconnected(); + let start = Vec3::new(0.8, 0.0, 0.5); + let goal = Vec3::new(11.0, 0.0, 10.5); + let result = find_path(&nm, start, goal); + assert!(result.is_none()); + } +}