From 74694315a69101a5e65ee6a685721b1756419a7d Mon Sep 17 00:00:00 2001 From: tolelom <98kimsungmin@naver.com> Date: Wed, 25 Mar 2026 10:14:05 +0900 Subject: [PATCH] feat(physics): add narrow phase collision detection (sphere-sphere, sphere-box, box-box) Co-Authored-By: Claude Sonnet 4.6 --- crates/voltex_physics/src/lib.rs | 1 + crates/voltex_physics/src/narrow.rs | 236 ++++++++++++++++++++++++++++ 2 files changed, 237 insertions(+) create mode 100644 crates/voltex_physics/src/narrow.rs diff --git a/crates/voltex_physics/src/lib.rs b/crates/voltex_physics/src/lib.rs index 6d58942..42c8190 100644 --- a/crates/voltex_physics/src/lib.rs +++ b/crates/voltex_physics/src/lib.rs @@ -1,5 +1,6 @@ pub mod collider; pub mod contact; +pub mod narrow; pub use collider::Collider; pub use contact::ContactPoint; diff --git a/crates/voltex_physics/src/narrow.rs b/crates/voltex_physics/src/narrow.rs new file mode 100644 index 0000000..92d166c --- /dev/null +++ b/crates/voltex_physics/src/narrow.rs @@ -0,0 +1,236 @@ +use voltex_math::Vec3; + +/// Returns (normal A→B, depth, point_on_a, point_on_b) or None if no collision. +pub fn sphere_vs_sphere( + pos_a: Vec3, radius_a: f32, + pos_b: Vec3, radius_b: f32, +) -> Option<(Vec3, f32, Vec3, Vec3)> { + let diff = pos_b - pos_a; + let dist_sq = diff.length_squared(); + let sum_r = radius_a + radius_b; + + if dist_sq > sum_r * sum_r { + return None; + } + + let dist = dist_sq.sqrt(); + let normal = if dist > 1e-8 { + diff * (1.0 / dist) + } else { + Vec3::Y + }; + + let depth = sum_r - dist; + let point_on_a = pos_a + normal * radius_a; + let point_on_b = pos_b - normal * radius_b; + + Some((normal, depth, point_on_a, point_on_b)) +} + +pub fn sphere_vs_box( + sphere_pos: Vec3, radius: f32, + box_pos: Vec3, half_extents: Vec3, +) -> Option<(Vec3, f32, Vec3, Vec3)> { + let bmin = box_pos - half_extents; + let bmax = box_pos + half_extents; + + let closest = Vec3::new( + sphere_pos.x.clamp(bmin.x, bmax.x), + sphere_pos.y.clamp(bmin.y, bmax.y), + sphere_pos.z.clamp(bmin.z, bmax.z), + ); + + let diff = sphere_pos - closest; + let dist_sq = diff.length_squared(); + + // Sphere center outside box + if dist_sq > 1e-8 { + let dist = dist_sq.sqrt(); + if dist > radius { + return None; + } + let normal = diff * (-1.0 / dist); // sphere→box direction + let depth = radius - dist; + let point_on_a = sphere_pos + normal * radius; // sphere surface toward box + let point_on_b = closest; + return Some((normal, depth, point_on_a, point_on_b)); + } + + // Sphere center inside box — find nearest face + let dx_min = sphere_pos.x - bmin.x; + let dx_max = bmax.x - sphere_pos.x; + let dy_min = sphere_pos.y - bmin.y; + let dy_max = bmax.y - sphere_pos.y; + let dz_min = sphere_pos.z - bmin.z; + let dz_max = bmax.z - sphere_pos.z; + + let mut min_dist = dx_min; + let mut normal = Vec3::new(-1.0, 0.0, 0.0); + let mut closest_face = Vec3::new(bmin.x, sphere_pos.y, sphere_pos.z); + + if dx_max < min_dist { + min_dist = dx_max; + normal = Vec3::new(1.0, 0.0, 0.0); + closest_face = Vec3::new(bmax.x, sphere_pos.y, sphere_pos.z); + } + if dy_min < min_dist { + min_dist = dy_min; + normal = Vec3::new(0.0, -1.0, 0.0); + closest_face = Vec3::new(sphere_pos.x, bmin.y, sphere_pos.z); + } + if dy_max < min_dist { + min_dist = dy_max; + normal = Vec3::new(0.0, 1.0, 0.0); + closest_face = Vec3::new(sphere_pos.x, bmax.y, sphere_pos.z); + } + if dz_min < min_dist { + min_dist = dz_min; + normal = Vec3::new(0.0, 0.0, -1.0); + closest_face = Vec3::new(sphere_pos.x, sphere_pos.y, bmin.z); + } + if dz_max < min_dist { + min_dist = dz_max; + normal = Vec3::new(0.0, 0.0, 1.0); + closest_face = Vec3::new(sphere_pos.x, sphere_pos.y, bmax.z); + } + + let depth = min_dist + radius; + let point_on_a = sphere_pos + normal * radius; + let point_on_b = closest_face; + + Some((normal, depth, point_on_a, point_on_b)) +} + +pub fn box_vs_box( + pos_a: Vec3, half_a: Vec3, + pos_b: Vec3, half_b: Vec3, +) -> Option<(Vec3, f32, Vec3, Vec3)> { + let diff = pos_b - pos_a; + + let overlap_x = (half_a.x + half_b.x) - diff.x.abs(); + if overlap_x < 0.0 { return None; } + + let overlap_y = (half_a.y + half_b.y) - diff.y.abs(); + if overlap_y < 0.0 { return None; } + + let overlap_z = (half_a.z + half_b.z) - diff.z.abs(); + if overlap_z < 0.0 { return None; } + + let (normal, depth) = if overlap_x <= overlap_y && overlap_x <= overlap_z { + let sign = if diff.x >= 0.0 { 1.0 } else { -1.0 }; + (Vec3::new(sign, 0.0, 0.0), overlap_x) + } else if overlap_y <= overlap_z { + let sign = if diff.y >= 0.0 { 1.0 } else { -1.0 }; + (Vec3::new(0.0, sign, 0.0), overlap_y) + } else { + let sign = if diff.z >= 0.0 { 1.0 } else { -1.0 }; + (Vec3::new(0.0, 0.0, sign), overlap_z) + }; + + let point_on_a = pos_a + Vec3::new( + normal.x * half_a.x, + normal.y * half_a.y, + normal.z * half_a.z, + ); + let point_on_b = pos_b - Vec3::new( + normal.x * half_b.x, + normal.y * half_b.y, + normal.z * half_b.z, + ); + + Some((normal, depth, point_on_a, point_on_b)) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn approx(a: f32, b: f32) -> bool { (a - b).abs() < 1e-5 } + fn approx_vec(a: Vec3, b: Vec3) -> bool { approx(a.x, b.x) && approx(a.y, b.y) && approx(a.z, b.z) } + + // sphere_vs_sphere tests + #[test] + fn test_sphere_sphere_separated() { + let r = sphere_vs_sphere(Vec3::ZERO, 1.0, Vec3::new(5.0, 0.0, 0.0), 1.0); + assert!(r.is_none()); + } + + #[test] + fn test_sphere_sphere_overlapping() { + let r = sphere_vs_sphere(Vec3::ZERO, 1.0, Vec3::new(1.5, 0.0, 0.0), 1.0); + let (normal, depth, pa, pb) = r.unwrap(); + assert!(approx_vec(normal, Vec3::X)); + assert!(approx(depth, 0.5)); + assert!(approx_vec(pa, Vec3::new(1.0, 0.0, 0.0))); + assert!(approx_vec(pb, Vec3::new(0.5, 0.0, 0.0))); + } + + #[test] + fn test_sphere_sphere_touching() { + let r = sphere_vs_sphere(Vec3::ZERO, 1.0, Vec3::new(2.0, 0.0, 0.0), 1.0); + let (normal, depth, _pa, _pb) = r.unwrap(); + assert!(approx_vec(normal, Vec3::X)); + assert!(approx(depth, 0.0)); + } + + #[test] + fn test_sphere_sphere_coincident() { + let r = sphere_vs_sphere(Vec3::ZERO, 1.0, Vec3::ZERO, 1.0); + let (_normal, depth, _pa, _pb) = r.unwrap(); + assert!(approx(depth, 2.0)); + } + + // sphere_vs_box tests + #[test] + fn test_sphere_box_separated() { + let r = sphere_vs_box(Vec3::new(5.0, 0.0, 0.0), 1.0, Vec3::ZERO, Vec3::ONE); + assert!(r.is_none()); + } + + #[test] + fn test_sphere_box_face_overlap() { + let r = sphere_vs_box(Vec3::new(1.5, 0.0, 0.0), 1.0, Vec3::ZERO, Vec3::ONE); + let (normal, depth, _pa, pb) = r.unwrap(); + assert!(approx(normal.x, -1.0)); + assert!(approx(depth, 0.5)); + assert!(approx(pb.x, 1.0)); + } + + #[test] + fn test_sphere_box_center_inside() { + let r = sphere_vs_box(Vec3::ZERO, 0.5, Vec3::ZERO, Vec3::ONE); + assert!(r.is_some()); + let (_normal, depth, _pa, _pb) = r.unwrap(); + assert!(depth > 0.0); + } + + // box_vs_box tests + #[test] + fn test_box_box_separated() { + let r = box_vs_box(Vec3::ZERO, Vec3::ONE, Vec3::new(5.0, 0.0, 0.0), Vec3::ONE); + assert!(r.is_none()); + } + + #[test] + fn test_box_box_overlapping() { + let r = box_vs_box(Vec3::ZERO, Vec3::ONE, Vec3::new(1.5, 0.0, 0.0), Vec3::ONE); + let (normal, depth, _pa, _pb) = r.unwrap(); + assert!(approx_vec(normal, Vec3::X)); + assert!(approx(depth, 0.5)); + } + + #[test] + fn test_box_box_touching() { + let r = box_vs_box(Vec3::ZERO, Vec3::ONE, Vec3::new(2.0, 0.0, 0.0), Vec3::ONE); + let (_normal, depth, _pa, _pb) = r.unwrap(); + assert!(approx(depth, 0.0)); + } + + #[test] + fn test_box_box_y_axis() { + let r = box_vs_box(Vec3::ZERO, Vec3::ONE, Vec3::new(0.0, 1.5, 0.0), Vec3::ONE); + let (normal, depth, _pa, _pb) = r.unwrap(); + assert!(approx_vec(normal, Vec3::Y)); + assert!(approx(depth, 0.5)); + } +}