diff --git a/crates/voltex_physics/src/bvh.rs b/crates/voltex_physics/src/bvh.rs index f97b1e0..23b8080 100644 --- a/crates/voltex_physics/src/bvh.rs +++ b/crates/voltex_physics/src/bvh.rs @@ -1,5 +1,6 @@ use voltex_ecs::Entity; -use voltex_math::AABB; +use voltex_math::{AABB, Ray}; +use crate::ray::ray_vs_aabb; #[derive(Debug)] enum BvhNode { @@ -72,34 +73,131 @@ impl BvhTree { idx } + /// Query all overlapping pairs using recursive tree traversal (replaces N² brute force). pub fn query_pairs(&self) -> Vec<(Entity, Entity)> { let mut pairs = Vec::new(); if self.nodes.is_empty() { return pairs; } let root = self.nodes.len() - 1; - let mut leaves = Vec::new(); - self.collect_leaves(root, &mut leaves); - for i in 0..leaves.len() { - for j in (i + 1)..leaves.len() { - let (ea, aabb_a) = leaves[i]; - let (eb, aabb_b) = leaves[j]; - if aabb_a.intersects(&aabb_b) { - pairs.push((ea, eb)); - } - } - } + self.query_pairs_recursive(root, root, &mut pairs); pairs } - fn collect_leaves(&self, node_idx: usize, out: &mut Vec<(Entity, AABB)>) { - match &self.nodes[node_idx] { + fn query_pairs_recursive(&self, a: usize, b: usize, pairs: &mut Vec<(Entity, Entity)>) { + let aabb_a = self.node_aabb(a); + let aabb_b = self.node_aabb(b); + + if !aabb_a.intersects(&aabb_b) { + return; + } + + match (&self.nodes[a], &self.nodes[b]) { + (BvhNode::Leaf { entity: ea, aabb: aabb_a }, BvhNode::Leaf { entity: eb, aabb: aabb_b }) => { + if a != b && ea.id <= eb.id && aabb_a.intersects(aabb_b) { + pairs.push((*ea, *eb)); + } + } + (BvhNode::Leaf { .. }, BvhNode::Internal { left, right, .. }) => { + self.query_pairs_recursive(a, *left, pairs); + self.query_pairs_recursive(a, *right, pairs); + } + (BvhNode::Internal { left, right, .. }, BvhNode::Leaf { .. }) => { + self.query_pairs_recursive(*left, b, pairs); + self.query_pairs_recursive(*right, b, pairs); + } + (BvhNode::Internal { left: la, right: ra, .. }, BvhNode::Internal { left: lb, right: rb, .. }) => { + if a == b { + // Same node: check children against each other and themselves + let la = *la; + let ra = *ra; + self.query_pairs_recursive(la, la, pairs); + self.query_pairs_recursive(ra, ra, pairs); + self.query_pairs_recursive(la, ra, pairs); + } else { + let la = *la; + let ra = *ra; + let lb = *lb; + let rb = *rb; + self.query_pairs_recursive(la, lb, pairs); + self.query_pairs_recursive(la, rb, pairs); + self.query_pairs_recursive(ra, lb, pairs); + self.query_pairs_recursive(ra, rb, pairs); + } + } + } + } + + fn node_aabb(&self, idx: usize) -> &AABB { + match &self.nodes[idx] { + BvhNode::Leaf { aabb, .. } => aabb, + BvhNode::Internal { aabb, .. } => aabb, + } + } + + /// Query ray against BVH, returning all (Entity, t) hits sorted by t. + pub fn query_ray(&self, ray: &Ray, max_t: f32) -> Vec<(Entity, f32)> { + let mut hits = Vec::new(); + if self.nodes.is_empty() { + return hits; + } + let root = self.nodes.len() - 1; + self.query_ray_recursive(root, ray, max_t, &mut hits); + hits.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap()); + hits + } + + fn query_ray_recursive(&self, idx: usize, ray: &Ray, max_t: f32, hits: &mut Vec<(Entity, f32)>) { + let aabb = self.node_aabb(idx); + match ray_vs_aabb(ray, aabb) { + Some(t) if t <= max_t => {} + _ => return, + } + + match &self.nodes[idx] { BvhNode::Leaf { entity, aabb } => { - out.push((*entity, *aabb)); + if let Some(t) = ray_vs_aabb(ray, aabb) { + if t <= max_t { + hits.push((*entity, t)); + } + } } BvhNode::Internal { left, right, .. } => { - self.collect_leaves(*left, out); - self.collect_leaves(*right, out); + self.query_ray_recursive(*left, ray, max_t, hits); + self.query_ray_recursive(*right, ray, max_t, hits); + } + } + } + + /// Refit the BVH: update leaf AABBs and propagate changes to parents. + /// `updated` maps entity → new AABB. Leaves not in map are unchanged. + pub fn refit(&mut self, updated: &[(Entity, AABB)]) { + if self.nodes.is_empty() { + return; + } + let root = self.nodes.len() - 1; + self.refit_recursive(root, updated); + } + + fn refit_recursive(&mut self, idx: usize, updated: &[(Entity, AABB)]) -> AABB { + match self.nodes[idx] { + BvhNode::Leaf { entity, ref mut aabb } => { + if let Some((_, new_aabb)) = updated.iter().find(|(e, _)| *e == entity) { + *aabb = *new_aabb; + } + *aabb + } + BvhNode::Internal { left, right, aabb: _ } => { + let left = left; + let right = right; + let left_aabb = self.refit_recursive(left, updated); + let right_aabb = self.refit_recursive(right, updated); + let new_aabb = left_aabb.merged(&right_aabb); + // Update the internal node's AABB + if let BvhNode::Internal { ref mut aabb, .. } = self.nodes[idx] { + *aabb = new_aabb; + } + new_aabb } } } @@ -163,4 +261,117 @@ mod tests { let (a, b) = pairs[0]; assert!((a.id == 0 && b.id == 1) || (a.id == 1 && b.id == 0)); } + + // --- query_pairs: verify recursive gives same results as brute force --- + + #[test] + fn test_query_pairs_matches_brute_force() { + let entries = vec![ + (make_entity(0), AABB::new(Vec3::ZERO, Vec3::new(2.0, 2.0, 2.0))), + (make_entity(1), AABB::new(Vec3::ONE, Vec3::new(3.0, 3.0, 3.0))), + (make_entity(2), AABB::new(Vec3::new(2.5, 2.5, 2.5), Vec3::new(4.0, 4.0, 4.0))), + (make_entity(3), AABB::new(Vec3::new(10.0, 10.0, 10.0), Vec3::new(11.0, 11.0, 11.0))), + (make_entity(4), AABB::new(Vec3::new(10.5, 10.5, 10.5), Vec3::new(12.0, 12.0, 12.0))), + ]; + + let tree = BvhTree::build(&entries); + let mut pairs = tree.query_pairs(); + pairs.sort_by_key(|(a, b)| (a.id.min(b.id), a.id.max(b.id))); + + // Brute force + let mut brute: Vec<(Entity, Entity)> = Vec::new(); + for i in 0..entries.len() { + for j in (i + 1)..entries.len() { + if entries[i].1.intersects(&entries[j].1) { + let a = entries[i].0; + let b = entries[j].0; + brute.push(if a.id <= b.id { (a, b) } else { (b, a) }); + } + } + } + brute.sort_by_key(|(a, b)| (a.id, b.id)); + + assert_eq!(pairs.len(), brute.len(), "pair count mismatch: tree={}, brute={}", pairs.len(), brute.len()); + for (tree_pair, brute_pair) in pairs.iter().zip(brute.iter()) { + let t = (tree_pair.0.id.min(tree_pair.1.id), tree_pair.0.id.max(tree_pair.1.id)); + let b = (brute_pair.0.id, brute_pair.1.id); + assert_eq!(t, b); + } + } + + // --- query_ray tests --- + + #[test] + fn test_query_ray_basic() { + let entries = vec![ + (make_entity(0), AABB::new(Vec3::new(4.0, -1.0, -1.0), Vec3::new(6.0, 1.0, 1.0))), + (make_entity(1), AABB::new(Vec3::new(10.0, 10.0, 10.0), Vec3::new(11.0, 11.0, 11.0))), + ]; + let tree = BvhTree::build(&entries); + let ray = Ray::new(Vec3::ZERO, Vec3::X); + let hits = tree.query_ray(&ray, 100.0); + assert_eq!(hits.len(), 1); + assert_eq!(hits[0].0.id, 0); + } + + #[test] + fn test_query_ray_multiple() { + let entries = vec![ + (make_entity(0), AABB::new(Vec3::new(2.0, -1.0, -1.0), Vec3::new(4.0, 1.0, 1.0))), + (make_entity(1), AABB::new(Vec3::new(6.0, -1.0, -1.0), Vec3::new(8.0, 1.0, 1.0))), + ]; + let tree = BvhTree::build(&entries); + let ray = Ray::new(Vec3::ZERO, Vec3::X); + let hits = tree.query_ray(&ray, 100.0); + assert_eq!(hits.len(), 2); + assert!(hits[0].1 < hits[1].1, "should be sorted by distance"); + } + + #[test] + fn test_query_ray_miss() { + let entries = vec![ + (make_entity(0), AABB::new(Vec3::new(4.0, 4.0, 4.0), Vec3::new(6.0, 6.0, 6.0))), + ]; + let tree = BvhTree::build(&entries); + let ray = Ray::new(Vec3::ZERO, Vec3::X); + let hits = tree.query_ray(&ray, 100.0); + assert!(hits.is_empty()); + } + + // --- refit tests --- + + #[test] + fn test_refit_updates_leaf() { + let entries = vec![ + (make_entity(0), AABB::new(Vec3::ZERO, Vec3::ONE)), + (make_entity(1), AABB::new(Vec3::new(5.0, 5.0, 5.0), Vec3::new(6.0, 6.0, 6.0))), + ]; + let mut tree = BvhTree::build(&entries); + + // Initially separated + assert!(tree.query_pairs().is_empty()); + + // Move entity 1 to overlap entity 0 + tree.refit(&[(make_entity(1), AABB::new(Vec3::new(0.5, 0.5, 0.5), Vec3::new(1.5, 1.5, 1.5)))]); + + let pairs = tree.query_pairs(); + assert_eq!(pairs.len(), 1, "after refit, overlapping entities should be found"); + } + + #[test] + fn test_refit_separates_entities() { + let entries = vec![ + (make_entity(0), AABB::new(Vec3::ZERO, Vec3::new(2.0, 2.0, 2.0))), + (make_entity(1), AABB::new(Vec3::ONE, Vec3::new(3.0, 3.0, 3.0))), + ]; + let mut tree = BvhTree::build(&entries); + + // Initially overlapping + assert_eq!(tree.query_pairs().len(), 1); + + // Move entity 1 far away + tree.refit(&[(make_entity(1), AABB::new(Vec3::new(100.0, 100.0, 100.0), Vec3::new(101.0, 101.0, 101.0)))]); + + assert!(tree.query_pairs().is_empty(), "after refit, separated entities should not overlap"); + } } diff --git a/crates/voltex_physics/src/ccd.rs b/crates/voltex_physics/src/ccd.rs new file mode 100644 index 0000000..f4401ae --- /dev/null +++ b/crates/voltex_physics/src/ccd.rs @@ -0,0 +1,118 @@ +use voltex_math::{Vec3, AABB, Ray}; +use crate::ray::ray_vs_aabb; + +/// Swept sphere vs AABB continuous collision detection. +/// Expands the AABB by the sphere radius, then tests a ray from start to end. +/// Returns t in [0,1] of first contact, or None if no contact. +pub fn swept_sphere_vs_aabb(start: Vec3, end: Vec3, radius: f32, aabb: &AABB) -> Option { + // Expand AABB by sphere radius + let r = Vec3::new(radius, radius, radius); + let expanded = AABB::new(aabb.min - r, aabb.max + r); + + let direction = end - start; + let sweep_len = direction.length(); + + if sweep_len < 1e-10 { + // No movement — check if already inside + if expanded.contains_point(start) { + return Some(0.0); + } + return None; + } + + let ray = Ray::new(start, direction * (1.0 / sweep_len)); + + match ray_vs_aabb(&ray, &expanded) { + Some(t) => { + let parametric_t = t / sweep_len; + if parametric_t <= 1.0 { + Some(parametric_t) + } else { + None + } + } + None => None, + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn approx(a: f32, b: f32) -> bool { + (a - b).abs() < 1e-3 + } + + #[test] + fn test_swept_sphere_hits_aabb() { + let start = Vec3::new(-10.0, 0.0, 0.0); + let end = Vec3::new(10.0, 0.0, 0.0); + let radius = 0.5; + let aabb = AABB::new(Vec3::new(4.0, -1.0, -1.0), Vec3::new(6.0, 1.0, 1.0)); + + let t = swept_sphere_vs_aabb(start, end, radius, &aabb).unwrap(); + // Expanded AABB min.x = 3.5, start.x = -10, direction = 20 + // t = (3.5 - (-10)) / 20 = 13.5 / 20 = 0.675 + assert!(t > 0.0 && t < 1.0); + assert!(approx(t, 0.675)); + } + + #[test] + fn test_swept_sphere_misses_aabb() { + let start = Vec3::new(-10.0, 10.0, 0.0); + let end = Vec3::new(10.0, 10.0, 0.0); + let radius = 0.5; + let aabb = AABB::new(Vec3::new(4.0, -1.0, -1.0), Vec3::new(6.0, 1.0, 1.0)); + + assert!(swept_sphere_vs_aabb(start, end, radius, &aabb).is_none()); + } + + #[test] + fn test_swept_sphere_starts_inside() { + let start = Vec3::new(5.0, 0.0, 0.0); + let end = Vec3::new(10.0, 0.0, 0.0); + let radius = 0.5; + let aabb = AABB::new(Vec3::new(4.0, -1.0, -1.0), Vec3::new(6.0, 1.0, 1.0)); + + let t = swept_sphere_vs_aabb(start, end, radius, &aabb).unwrap(); + assert!(approx(t, 0.0)); + } + + #[test] + fn test_swept_sphere_tunneling_detection() { + // Fast sphere that would tunnel through a thin wall + let start = Vec3::new(-100.0, 0.0, 0.0); + let end = Vec3::new(100.0, 0.0, 0.0); + let radius = 0.1; + // Thin wall at x=0 + let aabb = AABB::new(Vec3::new(-0.05, -10.0, -10.0), Vec3::new(0.05, 10.0, 10.0)); + + let t = swept_sphere_vs_aabb(start, end, radius, &aabb); + assert!(t.is_some(), "should detect tunneling through thin wall"); + let t = t.unwrap(); + assert!(t > 0.0 && t < 1.0); + } + + #[test] + fn test_swept_sphere_no_movement() { + let start = Vec3::new(5.0, 0.0, 0.0); + let end = Vec3::new(5.0, 0.0, 0.0); + let radius = 0.5; + let aabb = AABB::new(Vec3::new(4.0, -1.0, -1.0), Vec3::new(6.0, 1.0, 1.0)); + + // Inside expanded AABB, so should return 0 + let t = swept_sphere_vs_aabb(start, end, radius, &aabb).unwrap(); + assert!(approx(t, 0.0)); + } + + #[test] + fn test_swept_sphere_beyond_range() { + let start = Vec3::new(-10.0, 0.0, 0.0); + let end = Vec3::new(-5.0, 0.0, 0.0); + let radius = 0.5; + let aabb = AABB::new(Vec3::new(4.0, -1.0, -1.0), Vec3::new(6.0, 1.0, 1.0)); + + // AABB is at x=4..6, moving from -10 to -5 won't reach it + assert!(swept_sphere_vs_aabb(start, end, radius, &aabb).is_none()); + } +} diff --git a/crates/voltex_physics/src/integrator.rs b/crates/voltex_physics/src/integrator.rs index 8caf763..98a3fb0 100644 --- a/crates/voltex_physics/src/integrator.rs +++ b/crates/voltex_physics/src/integrator.rs @@ -1,28 +1,102 @@ use voltex_ecs::World; use voltex_ecs::Transform; use voltex_math::Vec3; -use crate::rigid_body::{RigidBody, PhysicsConfig}; +use crate::collider::Collider; +use crate::rigid_body::{RigidBody, PhysicsConfig, SLEEP_VELOCITY_THRESHOLD, SLEEP_TIME_THRESHOLD}; + +/// Compute diagonal inertia tensor for a collider shape. +/// Returns Vec3 where each component is the moment of inertia about that axis. +pub fn inertia_tensor(collider: &Collider, mass: f32) -> Vec3 { + match collider { + Collider::Sphere { radius } => { + let i = (2.0 / 5.0) * mass * radius * radius; + Vec3::new(i, i, i) + } + Collider::Box { half_extents } => { + let w = half_extents.x * 2.0; + let h = half_extents.y * 2.0; + let d = half_extents.z * 2.0; + let factor = mass / 12.0; + Vec3::new( + factor * (h * h + d * d), + factor * (w * w + d * d), + factor * (w * w + h * h), + ) + } + Collider::Capsule { radius, half_height } => { + // Approximate as cylinder with total height = 2*half_height + 2*radius + let r = *radius; + let h = half_height * 2.0; + let ix = mass * (3.0 * r * r + h * h) / 12.0; + let iy = mass * r * r / 2.0; + let iz = ix; + Vec3::new(ix, iy, iz) + } + } +} + +/// Compute inverse inertia (component-wise 1/I). Returns zero for zero-mass or zero-inertia. +pub fn inv_inertia(inertia: Vec3) -> Vec3 { + Vec3::new( + if inertia.x > 1e-10 { 1.0 / inertia.x } else { 0.0 }, + if inertia.y > 1e-10 { 1.0 / inertia.y } else { 0.0 }, + if inertia.z > 1e-10 { 1.0 / inertia.z } else { 0.0 }, + ) +} pub fn integrate(world: &mut World, config: &PhysicsConfig) { - // 1. Collect - let updates: Vec<(voltex_ecs::Entity, Vec3, Vec3)> = world + // 1. Collect linear + angular updates + let updates: Vec<(voltex_ecs::Entity, Vec3, Vec3, Vec3)> = world .query2::() .into_iter() - .filter(|(_, _, rb)| !rb.is_static()) + .filter(|(_, _, rb)| !rb.is_static() && !rb.is_sleeping) .map(|(entity, transform, rb)| { let new_velocity = rb.velocity + config.gravity * rb.gravity_scale * config.fixed_dt; let new_position = transform.position + new_velocity * config.fixed_dt; - (entity, new_velocity, new_position) + let new_rotation = transform.rotation + rb.angular_velocity * config.fixed_dt; + (entity, new_velocity, new_position, new_rotation) }) .collect(); // 2. Apply - for (entity, new_velocity, new_position) in updates { + for (entity, new_velocity, new_position, new_rotation) in updates { if let Some(rb) = world.get_mut::(entity) { rb.velocity = new_velocity; } if let Some(t) = world.get_mut::(entity) { t.position = new_position; + t.rotation = new_rotation; + } + } + + // 3. Update sleep timers + update_sleep_timers(world, config); +} + +fn update_sleep_timers(world: &mut World, config: &PhysicsConfig) { + let sleep_updates: Vec<(voltex_ecs::Entity, bool, f32)> = world + .query::() + .filter(|(_, rb)| !rb.is_static()) + .map(|(entity, rb)| { + let speed = rb.velocity.length() + rb.angular_velocity.length(); + if speed < SLEEP_VELOCITY_THRESHOLD { + let new_timer = rb.sleep_timer + config.fixed_dt; + let should_sleep = new_timer >= SLEEP_TIME_THRESHOLD; + (entity, should_sleep, new_timer) + } else { + (entity, false, 0.0) + } + }) + .collect(); + + for (entity, should_sleep, timer) in sleep_updates { + if let Some(rb) = world.get_mut::(entity) { + rb.sleep_timer = timer; + if should_sleep { + rb.is_sleeping = true; + rb.velocity = Vec3::ZERO; + rb.angular_velocity = Vec3::ZERO; + } } } } @@ -33,7 +107,7 @@ mod tests { use voltex_ecs::World; use voltex_ecs::Transform; use voltex_math::Vec3; - use crate::RigidBody; + use crate::{RigidBody, Collider}; fn approx(a: f32, b: f32) -> bool { (a - b).abs() < 1e-4 @@ -93,4 +167,146 @@ mod tests { let expected_x = 5.0 * config.fixed_dt; assert!(approx(t.position.x, expected_x)); } + + // --- Inertia tensor tests --- + + #[test] + fn test_inertia_tensor_sphere() { + let c = Collider::Sphere { radius: 1.0 }; + let i = inertia_tensor(&c, 1.0); + let expected = 2.0 / 5.0; + assert!(approx(i.x, expected)); + assert!(approx(i.y, expected)); + assert!(approx(i.z, expected)); + } + + #[test] + fn test_inertia_tensor_box() { + let c = Collider::Box { half_extents: Vec3::ONE }; + let i = inertia_tensor(&c, 12.0); + // w=2, h=2, d=2, factor=1 => ix = 4+4=8, etc + assert!(approx(i.x, 8.0)); + assert!(approx(i.y, 8.0)); + assert!(approx(i.z, 8.0)); + } + + #[test] + fn test_inertia_tensor_capsule() { + let c = Collider::Capsule { radius: 0.5, half_height: 1.0 }; + let i = inertia_tensor(&c, 1.0); + // Approximate as cylinder: r=0.5, h=2.0 + // ix = m*(3*r^2 + h^2)/12 = (3*0.25 + 4)/12 = 4.75/12 + assert!(approx(i.x, 4.75 / 12.0)); + // iy = m*r^2/2 = 0.25/2 = 0.125 + assert!(approx(i.y, 0.125)); + } + + // --- Angular velocity integration tests --- + + #[test] + fn test_spinning_sphere() { + let mut world = World::new(); + let e = world.spawn(); + world.add(e, Transform::from_position(Vec3::ZERO)); + let mut rb = RigidBody::dynamic(1.0); + rb.angular_velocity = Vec3::new(0.0, 3.14159, 0.0); // ~PI rad/s around Y + rb.gravity_scale = 0.0; + world.add(e, rb); + + let config = PhysicsConfig::default(); + integrate(&mut world, &config); + + let t = world.get::(e).unwrap(); + let expected_rot_y = 3.14159 * config.fixed_dt; + assert!(approx(t.rotation.y, expected_rot_y)); + // Position should not change (no linear velocity, no gravity) + assert!(approx(t.position.x, 0.0)); + assert!(approx(t.position.y, 0.0)); + } + + #[test] + fn test_angular_velocity_persists() { + let mut world = World::new(); + let e = world.spawn(); + world.add(e, Transform::from_position(Vec3::ZERO)); + let mut rb = RigidBody::dynamic(1.0); + rb.angular_velocity = Vec3::new(1.0, 0.0, 0.0); + rb.gravity_scale = 0.0; + world.add(e, rb); + + let config = PhysicsConfig::default(); + integrate(&mut world, &config); + integrate(&mut world, &config); + + let t = world.get::(e).unwrap(); + let expected_rot_x = 2.0 * config.fixed_dt; + assert!(approx(t.rotation.x, expected_rot_x)); + } + + // --- Sleep system tests --- + + #[test] + fn test_body_sleeps_after_resting() { + let mut world = World::new(); + let e = world.spawn(); + world.add(e, Transform::from_position(Vec3::ZERO)); + let mut rb = RigidBody::dynamic(1.0); + rb.velocity = Vec3::ZERO; + rb.gravity_scale = 0.0; + world.add(e, rb); + + let config = PhysicsConfig { + gravity: Vec3::ZERO, + fixed_dt: 1.0 / 60.0, + solver_iterations: 4, + }; + + // Integrate many times until sleep timer exceeds threshold + for _ in 0..60 { + integrate(&mut world, &config); + } + + let rb = world.get::(e).unwrap(); + assert!(rb.is_sleeping, "body should be sleeping after resting"); + } + + #[test] + fn test_sleeping_body_not_integrated() { + let mut world = World::new(); + let e = world.spawn(); + world.add(e, Transform::from_position(Vec3::new(0.0, 10.0, 0.0))); + let mut rb = RigidBody::dynamic(1.0); + rb.is_sleeping = true; + world.add(e, rb); + + let config = PhysicsConfig::default(); + integrate(&mut world, &config); + + let t = world.get::(e).unwrap(); + assert!(approx(t.position.y, 10.0), "sleeping body should not move"); + } + + #[test] + fn test_moving_body_does_not_sleep() { + let mut world = World::new(); + let e = world.spawn(); + world.add(e, Transform::from_position(Vec3::ZERO)); + let mut rb = RigidBody::dynamic(1.0); + rb.velocity = Vec3::new(5.0, 0.0, 0.0); + rb.gravity_scale = 0.0; + world.add(e, rb); + + let config = PhysicsConfig { + gravity: Vec3::ZERO, + fixed_dt: 1.0 / 60.0, + solver_iterations: 4, + }; + + for _ in 0..60 { + integrate(&mut world, &config); + } + + let rb = world.get::(e).unwrap(); + assert!(!rb.is_sleeping, "fast-moving body should not sleep"); + } } diff --git a/crates/voltex_physics/src/lib.rs b/crates/voltex_physics/src/lib.rs index cc9f238..d549fd5 100644 --- a/crates/voltex_physics/src/lib.rs +++ b/crates/voltex_physics/src/lib.rs @@ -9,12 +9,15 @@ pub mod rigid_body; pub mod integrator; pub mod solver; pub mod raycast; +pub mod ccd; pub use bvh::BvhTree; pub use collider::Collider; pub use contact::ContactPoint; pub use collision::detect_collisions; pub use rigid_body::{RigidBody, PhysicsConfig}; -pub use integrator::integrate; +pub use integrator::{integrate, inertia_tensor, inv_inertia}; pub use solver::{resolve_collisions, physics_step}; -pub use raycast::{RayHit, raycast}; +pub use raycast::{RayHit, raycast, raycast_all}; +pub use ray::ray_vs_triangle; +pub use ccd::swept_sphere_vs_aabb; diff --git a/crates/voltex_physics/src/ray.rs b/crates/voltex_physics/src/ray.rs index 88fba32..5174ca8 100644 --- a/crates/voltex_physics/src/ray.rs +++ b/crates/voltex_physics/src/ray.rs @@ -183,6 +183,43 @@ pub fn ray_vs_capsule(ray: &Ray, center: Vec3, radius: f32, half_height: f32) -> best } +/// Ray vs Triangle (Möller–Trumbore algorithm). +/// Returns (t, normal) where normal is the triangle face normal. +pub fn ray_vs_triangle(ray: &Ray, v0: Vec3, v1: Vec3, v2: Vec3) -> Option<(f32, Vec3)> { + let edge1 = v1 - v0; + let edge2 = v2 - v0; + let h = ray.direction.cross(edge2); + let a = edge1.dot(h); + + if a.abs() < 1e-8 { + return None; // Ray is parallel to triangle + } + + let f = 1.0 / a; + let s = ray.origin - v0; + let u = f * s.dot(h); + + if u < 0.0 || u > 1.0 { + return None; + } + + let q = s.cross(edge1); + let v = f * ray.direction.dot(q); + + if v < 0.0 || u + v > 1.0 { + return None; + } + + let t = f * edge2.dot(q); + + if t < 0.0 { + return None; // Triangle is behind the ray + } + + let normal = edge1.cross(edge2).normalize(); + Some((t, normal)) +} + #[cfg(test)] mod tests { use super::*; @@ -265,4 +302,61 @@ mod tests { let (t, _normal) = ray_vs_box(&ray, Vec3::ZERO, Vec3::ONE).unwrap(); assert!(approx(t, 0.0)); } + + // --- ray_vs_triangle tests --- + + #[test] + fn test_triangle_hit() { + let v0 = Vec3::new(-1.0, -1.0, 5.0); + let v1 = Vec3::new(1.0, -1.0, 5.0); + let v2 = Vec3::new(0.0, 1.0, 5.0); + let ray = Ray::new(Vec3::ZERO, Vec3::Z); + let (t, normal) = ray_vs_triangle(&ray, v0, v1, v2).unwrap(); + assert!(approx(t, 5.0)); + // Normal should point toward -Z (facing the ray) + assert!(normal.z.abs() > 0.9); + } + + #[test] + fn test_triangle_miss() { + let v0 = Vec3::new(-1.0, -1.0, 5.0); + let v1 = Vec3::new(1.0, -1.0, 5.0); + let v2 = Vec3::new(0.0, 1.0, 5.0); + // Ray pointing away + let ray = Ray::new(Vec3::new(10.0, 10.0, 0.0), Vec3::Z); + assert!(ray_vs_triangle(&ray, v0, v1, v2).is_none()); + } + + #[test] + fn test_triangle_behind_ray() { + let v0 = Vec3::new(-1.0, -1.0, -5.0); + let v1 = Vec3::new(1.0, -1.0, -5.0); + let v2 = Vec3::new(0.0, 1.0, -5.0); + let ray = Ray::new(Vec3::ZERO, Vec3::Z); + assert!(ray_vs_triangle(&ray, v0, v1, v2).is_none()); + } + + #[test] + fn test_triangle_parallel() { + let v0 = Vec3::new(0.0, 0.0, 5.0); + let v1 = Vec3::new(1.0, 0.0, 5.0); + let v2 = Vec3::new(0.0, 0.0, 6.0); + // Ray parallel to triangle (in XZ plane) + let ray = Ray::new(Vec3::ZERO, Vec3::X); + assert!(ray_vs_triangle(&ray, v0, v1, v2).is_none()); + } + + #[test] + fn test_triangle_edge_hit() { + // Ray hitting exactly on an edge + let v0 = Vec3::new(-1.0, 0.0, 5.0); + let v1 = Vec3::new(1.0, 0.0, 5.0); + let v2 = Vec3::new(0.0, 2.0, 5.0); + // Hit on the midpoint of v0-v1 edge + let ray = Ray::new(Vec3::new(0.0, 0.0, 0.0), Vec3::Z); + let result = ray_vs_triangle(&ray, v0, v1, v2); + assert!(result.is_some()); + let (t, _) = result.unwrap(); + assert!(approx(t, 5.0)); + } } diff --git a/crates/voltex_physics/src/raycast.rs b/crates/voltex_physics/src/raycast.rs index c0a7219..5474726 100644 --- a/crates/voltex_physics/src/raycast.rs +++ b/crates/voltex_physics/src/raycast.rs @@ -72,6 +72,58 @@ pub fn raycast(world: &World, ray: &Ray, max_dist: f32) -> Option { closest } +/// Cast a ray and return ALL hits sorted by distance. +pub fn raycast_all(world: &World, ray: &Ray, max_dist: f32) -> Vec { + let entities: Vec<(Entity, Vec3, Collider)> = world + .query2::() + .into_iter() + .map(|(e, t, c)| (e, t.position, *c)) + .collect(); + + if entities.is_empty() { + return Vec::new(); + } + + let mut hits = Vec::new(); + + for (entity, pos, collider) in &entities { + let aabb = collider.aabb(*pos); + + // Broad phase: ray vs AABB + match ray_tests::ray_vs_aabb(ray, &aabb) { + Some(t) if t <= max_dist => {} + _ => continue, + }; + + // Narrow phase + let result = match collider { + Collider::Sphere { radius } => { + ray_tests::ray_vs_sphere(ray, *pos, *radius) + } + Collider::Box { half_extents } => { + ray_tests::ray_vs_box(ray, *pos, *half_extents) + } + Collider::Capsule { radius, half_height } => { + ray_tests::ray_vs_capsule(ray, *pos, *radius, *half_height) + } + }; + + if let Some((t, normal)) = result { + if t <= max_dist { + hits.push(RayHit { + entity: *entity, + t, + point: ray.at(t), + normal, + }); + } + } + } + + hits.sort_by(|a, b| a.t.partial_cmp(&b.t).unwrap()); + hits +} + #[cfg(test)] mod tests { use super::*; @@ -150,6 +202,62 @@ mod tests { assert!(approx(hit.t, 4.0)); } + // --- raycast_all tests --- + + #[test] + fn test_raycast_all_multiple_hits() { + let mut world = World::new(); + + let near = world.spawn(); + world.add(near, Transform::from_position(Vec3::new(3.0, 0.0, 0.0))); + world.add(near, Collider::Sphere { radius: 0.5 }); + + let mid = world.spawn(); + world.add(mid, Transform::from_position(Vec3::new(6.0, 0.0, 0.0))); + world.add(mid, Collider::Sphere { radius: 0.5 }); + + let far = world.spawn(); + world.add(far, Transform::from_position(Vec3::new(10.0, 0.0, 0.0))); + world.add(far, Collider::Sphere { radius: 0.5 }); + + let ray = Ray::new(Vec3::ZERO, Vec3::X); + let hits = raycast_all(&world, &ray, 100.0); + + assert_eq!(hits.len(), 3); + assert_eq!(hits[0].entity, near); + assert_eq!(hits[1].entity, mid); + assert_eq!(hits[2].entity, far); + assert!(hits[0].t < hits[1].t); + assert!(hits[1].t < hits[2].t); + } + + #[test] + fn test_raycast_all_empty() { + let world = World::new(); + let ray = Ray::new(Vec3::ZERO, Vec3::X); + let hits = raycast_all(&world, &ray, 100.0); + assert!(hits.is_empty()); + } + + #[test] + fn test_raycast_all_max_dist() { + let mut world = World::new(); + + let near = world.spawn(); + world.add(near, Transform::from_position(Vec3::new(3.0, 0.0, 0.0))); + world.add(near, Collider::Sphere { radius: 0.5 }); + + let far = world.spawn(); + world.add(far, Transform::from_position(Vec3::new(50.0, 0.0, 0.0))); + world.add(far, Collider::Sphere { radius: 0.5 }); + + let ray = Ray::new(Vec3::ZERO, Vec3::X); + let hits = raycast_all(&world, &ray, 10.0); + + assert_eq!(hits.len(), 1); + assert_eq!(hits[0].entity, near); + } + #[test] fn test_mixed_sphere_box() { let mut world = World::new(); diff --git a/crates/voltex_physics/src/rigid_body.rs b/crates/voltex_physics/src/rigid_body.rs index 1f7ac6c..29158da 100644 --- a/crates/voltex_physics/src/rigid_body.rs +++ b/crates/voltex_physics/src/rigid_body.rs @@ -1,5 +1,8 @@ use voltex_math::Vec3; +pub const SLEEP_VELOCITY_THRESHOLD: f32 = 0.01; +pub const SLEEP_TIME_THRESHOLD: f32 = 0.5; + #[derive(Debug, Clone, Copy)] pub struct RigidBody { pub velocity: Vec3, @@ -8,6 +11,8 @@ pub struct RigidBody { pub restitution: f32, pub gravity_scale: f32, pub friction: f32, // Coulomb friction coefficient, default 0.5 + pub is_sleeping: bool, + pub sleep_timer: f32, } impl RigidBody { @@ -19,6 +24,8 @@ impl RigidBody { restitution: 0.3, gravity_scale: 1.0, friction: 0.5, + is_sleeping: false, + sleep_timer: 0.0, } } @@ -30,6 +37,8 @@ impl RigidBody { restitution: 0.3, gravity_scale: 0.0, friction: 0.5, + is_sleeping: false, + sleep_timer: 0.0, } } @@ -40,11 +49,18 @@ impl RigidBody { pub fn is_static(&self) -> bool { self.mass == 0.0 } + + /// Wake this body from sleep. + pub fn wake(&mut self) { + self.is_sleeping = false; + self.sleep_timer = 0.0; + } } pub struct PhysicsConfig { pub gravity: Vec3, pub fixed_dt: f32, + pub solver_iterations: u32, } impl Default for PhysicsConfig { @@ -52,6 +68,7 @@ impl Default for PhysicsConfig { Self { gravity: Vec3::new(0.0, -9.81, 0.0), fixed_dt: 1.0 / 60.0, + solver_iterations: 4, } } } @@ -69,6 +86,8 @@ mod tests { assert_eq!(rb.velocity, Vec3::ZERO); assert_eq!(rb.restitution, 0.3); assert_eq!(rb.gravity_scale, 1.0); + assert!(!rb.is_sleeping); + assert_eq!(rb.sleep_timer, 0.0); } #[test] @@ -85,5 +104,16 @@ mod tests { let cfg = PhysicsConfig::default(); assert!((cfg.gravity.y - (-9.81)).abs() < 1e-6); assert!((cfg.fixed_dt - 1.0 / 60.0).abs() < 1e-6); + assert_eq!(cfg.solver_iterations, 4); + } + + #[test] + fn test_wake() { + let mut rb = RigidBody::dynamic(1.0); + rb.is_sleeping = true; + rb.sleep_timer = 1.0; + rb.wake(); + assert!(!rb.is_sleeping); + assert_eq!(rb.sleep_timer, 0.0); } } diff --git a/crates/voltex_physics/src/solver.rs b/crates/voltex_physics/src/solver.rs index e4e19dd..d06f36a 100644 --- a/crates/voltex_physics/src/solver.rs +++ b/crates/voltex_physics/src/solver.rs @@ -2,102 +2,271 @@ use voltex_ecs::{World, Entity}; use voltex_ecs::Transform; use voltex_math::Vec3; +use crate::collider::Collider; use crate::contact::ContactPoint; use crate::rigid_body::{RigidBody, PhysicsConfig}; use crate::collision::detect_collisions; -use crate::integrator::integrate; +use crate::integrator::{integrate, inertia_tensor, inv_inertia}; +use crate::ccd; const POSITION_SLOP: f32 = 0.01; const POSITION_PERCENT: f32 = 0.4; -pub fn resolve_collisions(world: &mut World, contacts: &[ContactPoint]) { - let mut velocity_changes: Vec<(Entity, Vec3)> = Vec::new(); - let mut position_changes: Vec<(Entity, Vec3)> = Vec::new(); +pub fn resolve_collisions(world: &mut World, contacts: &[ContactPoint], iterations: u32) { + // Wake sleeping bodies that are in contact + wake_colliding_bodies(world, contacts); - for contact in contacts { - let rb_a = world.get::(contact.entity_a).copied(); - let rb_b = world.get::(contact.entity_b).copied(); + for _iter in 0..iterations { + let mut velocity_changes: Vec<(Entity, Vec3, Vec3)> = Vec::new(); // (entity, dv_linear, dv_angular) + let mut position_changes: Vec<(Entity, Vec3)> = Vec::new(); - let (rb_a, rb_b) = match (rb_a, rb_b) { - (Some(a), Some(b)) => (a, b), - _ => continue, - }; + for contact in contacts { + let rb_a = world.get::(contact.entity_a).copied(); + let rb_b = world.get::(contact.entity_b).copied(); + let col_a = world.get::(contact.entity_a).copied(); + let col_b = world.get::(contact.entity_b).copied(); + let pos_a = world.get::(contact.entity_a).map(|t| t.position); + let pos_b = world.get::(contact.entity_b).map(|t| t.position); - let inv_mass_a = rb_a.inv_mass(); - let inv_mass_b = rb_b.inv_mass(); - let inv_mass_sum = inv_mass_a + inv_mass_b; - - if inv_mass_sum == 0.0 { - continue; - } - - let v_rel = rb_a.velocity - rb_b.velocity; - let v_rel_n = v_rel.dot(contact.normal); - - // normal points A→B; v_rel_n > 0 means A approaches B → apply impulse - let j = if v_rel_n > 0.0 { - let e = rb_a.restitution.min(rb_b.restitution); - let j = (1.0 + e) * v_rel_n / inv_mass_sum; - - velocity_changes.push((contact.entity_a, contact.normal * (-j * inv_mass_a))); - velocity_changes.push((contact.entity_b, contact.normal * (j * inv_mass_b))); - - j - } else { - // No separating impulse needed, but use contact depth to derive a - // representative normal force magnitude for friction clamping. - // A simple proxy: treat the penetration as providing a static normal force. - contact.depth / inv_mass_sum - }; - - // Coulomb friction: tangential impulse clamped to mu * normal impulse - let v_rel_n_scalar = v_rel.dot(contact.normal); - let v_rel_tangent = v_rel - contact.normal * v_rel_n_scalar; - let tangent_len = v_rel_tangent.length(); - - if tangent_len > 1e-6 { - let tangent = v_rel_tangent * (1.0 / tangent_len); - - // Friction coefficient: average of both bodies - let mu = (rb_a.friction + rb_b.friction) * 0.5; - - // Coulomb's law: friction impulse <= mu * normal impulse - let jt = -v_rel_tangent.dot(tangent) / inv_mass_sum; - let friction_j = if jt.abs() <= j * mu { - jt // static friction - } else { - j * mu * jt.signum() // dynamic friction (sliding), clamped magnitude + let (rb_a, rb_b) = match (rb_a, rb_b) { + (Some(a), Some(b)) => (a, b), + _ => continue, }; - velocity_changes.push((contact.entity_a, tangent * (friction_j * inv_mass_a))); - velocity_changes.push((contact.entity_b, tangent * (-friction_j * inv_mass_b))); + let inv_mass_a = rb_a.inv_mass(); + let inv_mass_b = rb_b.inv_mass(); + let inv_mass_sum = inv_mass_a + inv_mass_b; + + if inv_mass_sum == 0.0 { + continue; + } + + // Compute lever arms for angular impulse + let center_a = pos_a.unwrap_or(Vec3::ZERO); + let center_b = pos_b.unwrap_or(Vec3::ZERO); + let r_a = contact.point_on_a - center_a; + let r_b = contact.point_on_b - center_b; + + // Compute inverse inertia + let inv_i_a = col_a.map(|c| inv_inertia(inertia_tensor(&c, rb_a.mass))) + .unwrap_or(Vec3::ZERO); + let inv_i_b = col_b.map(|c| inv_inertia(inertia_tensor(&c, rb_b.mass))) + .unwrap_or(Vec3::ZERO); + + // Relative velocity at contact point (including angular contribution) + let v_a = rb_a.velocity + rb_a.angular_velocity.cross(r_a); + let v_b = rb_b.velocity + rb_b.angular_velocity.cross(r_b); + let v_rel = v_a - v_b; + let v_rel_n = v_rel.dot(contact.normal); + + // Effective mass including rotational terms + let r_a_cross_n = r_a.cross(contact.normal); + let r_b_cross_n = r_b.cross(contact.normal); + let angular_term_a = Vec3::new( + r_a_cross_n.x * inv_i_a.x, + r_a_cross_n.y * inv_i_a.y, + r_a_cross_n.z * inv_i_a.z, + ).cross(r_a).dot(contact.normal); + let angular_term_b = Vec3::new( + r_b_cross_n.x * inv_i_b.x, + r_b_cross_n.y * inv_i_b.y, + r_b_cross_n.z * inv_i_b.z, + ).cross(r_b).dot(contact.normal); + + let effective_mass = inv_mass_sum + angular_term_a + angular_term_b; + + // normal points A→B; v_rel_n > 0 means A approaches B → apply impulse + let j = if v_rel_n > 0.0 { + let e = rb_a.restitution.min(rb_b.restitution); + let j = (1.0 + e) * v_rel_n / effective_mass; + + // Linear impulse + velocity_changes.push((contact.entity_a, contact.normal * (-j * inv_mass_a), Vec3::ZERO)); + velocity_changes.push((contact.entity_b, contact.normal * (j * inv_mass_b), Vec3::ZERO)); + + // Angular impulse: torque = r × impulse + let angular_impulse_a = r_a.cross(contact.normal * (-j)); + let angular_impulse_b = r_b.cross(contact.normal * j); + let dw_a = Vec3::new( + angular_impulse_a.x * inv_i_a.x, + angular_impulse_a.y * inv_i_a.y, + angular_impulse_a.z * inv_i_a.z, + ); + let dw_b = Vec3::new( + angular_impulse_b.x * inv_i_b.x, + angular_impulse_b.y * inv_i_b.y, + angular_impulse_b.z * inv_i_b.z, + ); + velocity_changes.push((contact.entity_a, Vec3::ZERO, dw_a)); + velocity_changes.push((contact.entity_b, Vec3::ZERO, dw_b)); + + j + } else { + contact.depth / inv_mass_sum + }; + + // Coulomb friction: tangential impulse clamped to mu * normal impulse + let v_rel_tangent = v_rel - contact.normal * v_rel_n; + let tangent_len = v_rel_tangent.length(); + + if tangent_len > 1e-6 { + let tangent = v_rel_tangent * (1.0 / tangent_len); + let mu = (rb_a.friction + rb_b.friction) * 0.5; + + let jt = -v_rel_tangent.dot(tangent) / effective_mass; + let friction_j = if jt.abs() <= j * mu { + jt + } else { + j * mu * jt.signum() + }; + + velocity_changes.push((contact.entity_a, tangent * (friction_j * inv_mass_a), Vec3::ZERO)); + velocity_changes.push((contact.entity_b, tangent * (-friction_j * inv_mass_b), Vec3::ZERO)); + + // Angular friction impulse + let angular_fric_a = r_a.cross(tangent * friction_j); + let angular_fric_b = r_b.cross(tangent * (-friction_j)); + let dw_fric_a = Vec3::new( + angular_fric_a.x * inv_i_a.x, + angular_fric_a.y * inv_i_a.y, + angular_fric_a.z * inv_i_a.z, + ); + let dw_fric_b = Vec3::new( + angular_fric_b.x * inv_i_b.x, + angular_fric_b.y * inv_i_b.y, + angular_fric_b.z * inv_i_b.z, + ); + velocity_changes.push((contact.entity_a, Vec3::ZERO, dw_fric_a)); + velocity_changes.push((contact.entity_b, Vec3::ZERO, dw_fric_b)); + } + + // Position correction only on first iteration + if _iter == 0 { + let correction_mag = (contact.depth - POSITION_SLOP).max(0.0) * POSITION_PERCENT / inv_mass_sum; + if correction_mag > 0.0 { + let correction = contact.normal * correction_mag; + position_changes.push((contact.entity_a, correction * (-inv_mass_a))); + position_changes.push((contact.entity_b, correction * inv_mass_b)); + } + } } - let correction_mag = (contact.depth - POSITION_SLOP).max(0.0) * POSITION_PERCENT / inv_mass_sum; - if correction_mag > 0.0 { - let correction = contact.normal * correction_mag; - position_changes.push((contact.entity_a, correction * (-inv_mass_a))); - position_changes.push((contact.entity_b, correction * inv_mass_b)); + // Apply velocity changes + for (entity, dv, dw) in velocity_changes { + if let Some(rb) = world.get_mut::(entity) { + rb.velocity = rb.velocity + dv; + rb.angular_velocity = rb.angular_velocity + dw; + } + } + + // Apply position corrections + for (entity, dp) in position_changes { + if let Some(t) = world.get_mut::(entity) { + t.position = t.position + dp; + } } } +} - for (entity, dv) in velocity_changes { +fn wake_colliding_bodies(world: &mut World, contacts: &[ContactPoint]) { + let wake_list: Vec = contacts + .iter() + .flat_map(|c| { + let mut entities = Vec::new(); + if let Some(rb) = world.get::(c.entity_a) { + if rb.is_sleeping { entities.push(c.entity_a); } + } + if let Some(rb) = world.get::(c.entity_b) { + if rb.is_sleeping { entities.push(c.entity_b); } + } + entities + }) + .collect(); + + for entity in wake_list { if let Some(rb) = world.get_mut::(entity) { - rb.velocity = rb.velocity + dv; - } - } - - for (entity, dp) in position_changes { - if let Some(t) = world.get_mut::(entity) { - t.position = t.position + dp; + rb.wake(); } } } pub fn physics_step(world: &mut World, config: &PhysicsConfig) { + // CCD: for fast-moving bodies, check for tunneling + apply_ccd(world, config); + integrate(world, config); let contacts = detect_collisions(world); - resolve_collisions(world, &contacts); + resolve_collisions(world, &contacts, config.solver_iterations); +} + +fn apply_ccd(world: &mut World, config: &PhysicsConfig) { + // Gather fast-moving bodies and all collider AABBs + let bodies: Vec<(Entity, Vec3, Vec3, Collider)> = world + .query3::() + .into_iter() + .filter(|(_, _, rb, _)| !rb.is_static() && !rb.is_sleeping) + .map(|(e, t, rb, c)| (e, t.position, rb.velocity, *c)) + .collect(); + + let all_colliders: Vec<(Entity, voltex_math::AABB)> = world + .query2::() + .into_iter() + .map(|(e, t, c)| (e, c.aabb(t.position))) + .collect(); + + let mut ccd_corrections: Vec<(Entity, Vec3)> = Vec::new(); + + for (entity, pos, vel, collider) in &bodies { + let speed = vel.length(); + let collider_radius = match collider { + Collider::Sphere { radius } => *radius, + Collider::Box { half_extents } => half_extents.x.min(half_extents.y).min(half_extents.z), + Collider::Capsule { radius, .. } => *radius, + }; + + // Only apply CCD if displacement > collider radius + if speed * config.fixed_dt <= collider_radius { + continue; + } + + let sweep_radius = match collider { + Collider::Sphere { radius } => *radius, + _ => collider_radius, + }; + + let end = *pos + *vel * config.fixed_dt; + + let mut earliest_t = 1.0f32; + + for (other_entity, other_aabb) in &all_colliders { + if *other_entity == *entity { + continue; + } + + if let Some(t) = ccd::swept_sphere_vs_aabb(*pos, end, sweep_radius, other_aabb) { + if t < earliest_t { + earliest_t = t; + } + } + } + + if earliest_t < 1.0 { + // Place body just before collision point + let safe_t = (earliest_t - 0.01).max(0.0); + let safe_pos = *pos + *vel * config.fixed_dt * safe_t; + ccd_corrections.push((*entity, safe_pos)); + } + } + + for (entity, safe_pos) in ccd_corrections { + if let Some(t) = world.get_mut::(entity) { + t.position = safe_pos; + } + if let Some(rb) = world.get_mut::(entity) { + // Reduce velocity to prevent re-tunneling + rb.velocity = rb.velocity * 0.5; + } + } } #[cfg(test)] @@ -138,7 +307,7 @@ mod tests { let contacts = detect_collisions(&world); assert_eq!(contacts.len(), 1); - resolve_collisions(&mut world, &contacts); + resolve_collisions(&mut world, &contacts, 1); let va = world.get::(a).unwrap().velocity; let vb = world.get::(b).unwrap().velocity; @@ -168,7 +337,7 @@ mod tests { let contacts = detect_collisions(&world); assert_eq!(contacts.len(), 1); - resolve_collisions(&mut world, &contacts); + resolve_collisions(&mut world, &contacts, 1); let ball_rb = world.get::(ball).unwrap(); let floor_rb = world.get::(floor).unwrap(); @@ -198,7 +367,7 @@ mod tests { let contacts = detect_collisions(&world); assert_eq!(contacts.len(), 1); - resolve_collisions(&mut world, &contacts); + resolve_collisions(&mut world, &contacts, 1); let pa = world.get::(a).unwrap().position; let pb = world.get::(b).unwrap().position; @@ -234,14 +403,13 @@ mod tests { #[test] fn test_friction_slows_sliding() { - // Ball sliding on static floor with friction let mut world = World::new(); let ball = world.spawn(); world.add(ball, Transform::from_position(Vec3::new(0.0, 0.4, 0.0))); world.add(ball, Collider::Sphere { radius: 0.5 }); let mut rb = RigidBody::dynamic(1.0); - rb.velocity = Vec3::new(5.0, 0.0, 0.0); // sliding horizontally + rb.velocity = Vec3::new(5.0, 0.0, 0.0); rb.gravity_scale = 0.0; rb.friction = 0.5; world.add(ball, rb); @@ -253,14 +421,12 @@ mod tests { floor_rb.friction = 0.5; world.add(floor, floor_rb); - // Ball center at 0.4, radius 0.5, floor top at 0.0 → overlap 0.1 let contacts = detect_collisions(&world); if !contacts.is_empty() { - resolve_collisions(&mut world, &contacts); + resolve_collisions(&mut world, &contacts, 1); } let ball_v = world.get::(ball).unwrap().velocity; - // X velocity should be reduced by friction assert!(ball_v.x < 5.0, "friction should slow horizontal velocity: {}", ball_v.x); assert!(ball_v.x > 0.0, "should still be moving: {}", ball_v.x); } @@ -280,11 +446,125 @@ mod tests { world.add(b, RigidBody::statik()); let contacts = detect_collisions(&world); - resolve_collisions(&mut world, &contacts); + resolve_collisions(&mut world, &contacts, 1); let pa = world.get::(a).unwrap().position; let pb = world.get::(b).unwrap().position; assert!(approx(pa.x, 0.0)); assert!(approx(pb.x, 0.5)); } + + // --- Angular impulse tests --- + + #[test] + fn test_off_center_hit_produces_spin() { + let mut world = World::new(); + + // Sphere A moving right, hitting sphere B off-center (offset in Y) + let a = world.spawn(); + world.add(a, Transform::from_position(Vec3::new(-0.5, 0.5, 0.0))); + world.add(a, Collider::Sphere { radius: 1.0 }); + let mut rb_a = RigidBody::dynamic(1.0); + rb_a.velocity = Vec3::new(2.0, 0.0, 0.0); + rb_a.restitution = 0.5; + rb_a.gravity_scale = 0.0; + world.add(a, rb_a); + + let b = world.spawn(); + world.add(b, Transform::from_position(Vec3::new(0.5, -0.5, 0.0))); + world.add(b, Collider::Sphere { radius: 1.0 }); + let mut rb_b = RigidBody::dynamic(1.0); + rb_b.gravity_scale = 0.0; + rb_b.restitution = 0.5; + world.add(b, rb_b); + + let contacts = detect_collisions(&world); + assert!(!contacts.is_empty()); + + resolve_collisions(&mut world, &contacts, 4); + + let rb_a_after = world.get::(a).unwrap(); + let rb_b_after = world.get::(b).unwrap(); + + // At least one body should have non-zero angular velocity after off-center collision + let total_angular = rb_a_after.angular_velocity.length() + rb_b_after.angular_velocity.length(); + assert!(total_angular > 1e-4, "off-center hit should produce angular velocity, got {}", total_angular); + } + + // --- Sequential impulse tests --- + + #[test] + fn test_sequential_impulse_stability() { + // Stack of 3 boxes on floor - with iterations they should be more stable + let mut world = World::new(); + + let floor = world.spawn(); + world.add(floor, Transform::from_position(Vec3::new(0.0, -0.5, 0.0))); + world.add(floor, Collider::Box { half_extents: Vec3::new(10.0, 0.5, 10.0) }); + world.add(floor, RigidBody::statik()); + + let mut boxes = Vec::new(); + for i in 0..3 { + let e = world.spawn(); + let y = 0.5 + i as f32 * 1.0; + world.add(e, Transform::from_position(Vec3::new(0.0, y, 0.0))); + world.add(e, Collider::Box { half_extents: Vec3::new(0.5, 0.5, 0.5) }); + let mut rb = RigidBody::dynamic(1.0); + rb.gravity_scale = 0.0; // no gravity for stability test + world.add(e, rb); + boxes.push(e); + } + + let config = PhysicsConfig { + gravity: Vec3::ZERO, + fixed_dt: 1.0 / 60.0, + solver_iterations: 4, + }; + + // Run a few steps + for _ in 0..5 { + physics_step(&mut world, &config); + } + + // All boxes should remain roughly in place (no gravity, just resting) + for (i, e) in boxes.iter().enumerate() { + let t = world.get::(*e).unwrap(); + let expected_y = 0.5 + i as f32 * 1.0; + assert!((t.position.y - expected_y).abs() < 1.0, + "box {} moved too much: expected y~{}, got {}", i, expected_y, t.position.y); + } + } + + // --- Wake on collision test --- + + #[test] + fn test_wake_on_collision() { + let mut world = World::new(); + + // Sleeping body + let a = world.spawn(); + world.add(a, Transform::from_position(Vec3::ZERO)); + world.add(a, Collider::Sphere { radius: 1.0 }); + let mut rb_a = RigidBody::dynamic(1.0); + rb_a.is_sleeping = true; + rb_a.gravity_scale = 0.0; + world.add(a, rb_a); + + // Moving body that collides with sleeping body + 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 mut rb_b = RigidBody::dynamic(1.0); + rb_b.velocity = Vec3::new(-2.0, 0.0, 0.0); + rb_b.gravity_scale = 0.0; + world.add(b, rb_b); + + let contacts = detect_collisions(&world); + assert!(!contacts.is_empty()); + + resolve_collisions(&mut world, &contacts, 1); + + let rb_a_after = world.get::(a).unwrap(); + assert!(!rb_a_after.is_sleeping, "body should wake on collision"); + } }