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/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");
+    }
+}