diff --git a/crates/voltex_physics/src/collision.rs b/crates/voltex_physics/src/collision.rs new file mode 100644 index 0000000..2f270d5 --- /dev/null +++ b/crates/voltex_physics/src/collision.rs @@ -0,0 +1,181 @@ +use voltex_ecs::{World, Entity}; +use voltex_ecs::Transform; +use voltex_math::Vec3; + +use crate::collider::Collider; +use crate::contact::ContactPoint; +use crate::bvh::BvhTree; +use crate::narrow; + +pub fn detect_collisions(world: &World) -> Vec { + // 1. Gather entities with Transform + Collider + let pairs_data: Vec<(Entity, Vec3, Collider)> = world + .query2::() + .into_iter() + .map(|(e, t, c)| (e, t.position, *c)) + .collect(); + + if pairs_data.len() < 2 { + return Vec::new(); + } + + // 2. Build AABBs + let entries: Vec<(Entity, voltex_math::AABB)> = pairs_data + .iter() + .map(|(e, pos, col)| (*e, col.aabb(*pos))) + .collect(); + + // 3. Broad phase + let bvh = BvhTree::build(&entries); + let broad_pairs = bvh.query_pairs(); + + // 4. Narrow phase + let mut contacts = Vec::new(); + + let lookup = |entity: Entity| -> Option<(Vec3, Collider)> { + pairs_data.iter().find(|(e, _, _)| *e == entity).map(|(_, p, c)| (*p, *c)) + }; + + for (ea, eb) in broad_pairs { + let (pos_a, col_a) = match lookup(ea) { Some(v) => v, None => continue }; + let (pos_b, col_b) = match lookup(eb) { Some(v) => v, None => continue }; + + let result = match (&col_a, &col_b) { + (Collider::Sphere { radius: ra }, Collider::Sphere { radius: rb }) => { + narrow::sphere_vs_sphere(pos_a, *ra, pos_b, *rb) + } + (Collider::Sphere { radius }, Collider::Box { half_extents }) => { + narrow::sphere_vs_box(pos_a, *radius, pos_b, *half_extents) + } + (Collider::Box { half_extents }, Collider::Sphere { radius }) => { + narrow::sphere_vs_box(pos_b, *radius, pos_a, *half_extents) + .map(|(n, d, pa, pb)| (-n, d, pb, pa)) + } + (Collider::Box { half_extents: ha }, Collider::Box { half_extents: hb }) => { + narrow::box_vs_box(pos_a, *ha, pos_b, *hb) + } + }; + + if let Some((normal, depth, point_on_a, point_on_b)) = result { + contacts.push(ContactPoint { + entity_a: ea, + entity_b: eb, + normal, + depth, + point_on_a, + point_on_b, + }); + } + } + + contacts +} + +#[cfg(test)] +mod tests { + use super::*; + use voltex_ecs::World; + use voltex_ecs::Transform; + use voltex_math::Vec3; + use crate::Collider; + + #[test] + fn test_no_colliders() { + let world = World::new(); + let contacts = detect_collisions(&world); + assert!(contacts.is_empty()); + } + + #[test] + fn test_single_entity() { + let mut world = World::new(); + let e = world.spawn(); + world.add(e, Transform::from_position(Vec3::ZERO)); + world.add(e, Collider::Sphere { radius: 1.0 }); + let contacts = detect_collisions(&world); + assert!(contacts.is_empty()); + } + + #[test] + fn test_two_spheres_colliding() { + let mut world = World::new(); + let a = world.spawn(); + world.add(a, Transform::from_position(Vec3::ZERO)); + world.add(a, Collider::Sphere { radius: 1.0 }); + + let b = world.spawn(); + world.add(b, Transform::from_position(Vec3::new(1.5, 0.0, 0.0))); + world.add(b, Collider::Sphere { radius: 1.0 }); + + let contacts = detect_collisions(&world); + assert_eq!(contacts.len(), 1); + assert!((contacts[0].depth - 0.5).abs() < 1e-5); + } + + #[test] + fn test_two_spheres_separated() { + let mut world = World::new(); + let a = world.spawn(); + world.add(a, Transform::from_position(Vec3::ZERO)); + world.add(a, Collider::Sphere { radius: 1.0 }); + + let b = world.spawn(); + world.add(b, Transform::from_position(Vec3::new(10.0, 0.0, 0.0))); + world.add(b, Collider::Sphere { radius: 1.0 }); + + let contacts = detect_collisions(&world); + assert!(contacts.is_empty()); + } + + #[test] + fn test_sphere_vs_box_collision() { + let mut world = World::new(); + let a = world.spawn(); + world.add(a, Transform::from_position(Vec3::ZERO)); + world.add(a, Collider::Sphere { radius: 1.0 }); + + let b = world.spawn(); + world.add(b, Transform::from_position(Vec3::new(1.5, 0.0, 0.0))); + world.add(b, Collider::Box { half_extents: Vec3::ONE }); + + let contacts = detect_collisions(&world); + assert_eq!(contacts.len(), 1); + assert!(contacts[0].depth > 0.0); + } + + #[test] + fn test_box_vs_box_collision() { + let mut world = World::new(); + let a = world.spawn(); + world.add(a, Transform::from_position(Vec3::ZERO)); + world.add(a, Collider::Box { half_extents: Vec3::ONE }); + + let b = world.spawn(); + world.add(b, Transform::from_position(Vec3::new(1.5, 0.0, 0.0))); + world.add(b, Collider::Box { half_extents: Vec3::ONE }); + + let contacts = detect_collisions(&world); + assert_eq!(contacts.len(), 1); + assert!((contacts[0].depth - 0.5).abs() < 1e-5); + } + + #[test] + fn test_three_entities_mixed() { + let mut world = World::new(); + + let a = world.spawn(); + world.add(a, Transform::from_position(Vec3::ZERO)); + world.add(a, Collider::Sphere { radius: 1.0 }); + + let b = world.spawn(); + world.add(b, Transform::from_position(Vec3::new(1.5, 0.0, 0.0))); + world.add(b, Collider::Sphere { radius: 1.0 }); + + let c = world.spawn(); + world.add(c, Transform::from_position(Vec3::new(100.0, 0.0, 0.0))); + world.add(c, Collider::Box { half_extents: Vec3::ONE }); + + let contacts = detect_collisions(&world); + assert_eq!(contacts.len(), 1); + } +} diff --git a/crates/voltex_physics/src/lib.rs b/crates/voltex_physics/src/lib.rs index 10d66eb..1d34893 100644 --- a/crates/voltex_physics/src/lib.rs +++ b/crates/voltex_physics/src/lib.rs @@ -2,7 +2,9 @@ pub mod bvh; pub mod collider; pub mod contact; pub mod narrow; +pub mod collision; pub use bvh::BvhTree; pub use collider::Collider; pub use contact::ContactPoint; +pub use collision::detect_collisions;