From 762d80f5a6adcebb021602ab2128f2a784fb9c1d Mon Sep 17 00:00:00 2001 From: stainlu Date: Thu, 9 Apr 2026 01:59:10 +0800 Subject: [PATCH] Add diagonal inertia tensor and angular collision impulses Replace scalar inertia with a diagonal inertia tensor in the Mass component. Extend narrowphase collision functions to return world-space contact points. Rewrite collision response to apply angular impulses from off-center contacts using the standard rigid-body impulse formula. - Mass struct gains inverse_inertia_tensor field (Vec3, body-local diagonal) - Factory methods: Mass::from_sphere, from_aabb, from_capsule with correct principal moments of inertia - All intersect_* functions now return (normal, depth, contact_point) - CollisionEvent gains contact_point field - apply_velocity_response uses full rotational impulse formula: effective mass denominator includes I^-1 cross terms, angular velocity changes from both normal and friction impulses - Backward compatible: Mass::new produces spherically symmetric tensor, head-on sphere collisions produce zero angular change Co-Authored-By: Claude Sonnet 4.6 --- crates/euca-physics/src/collision.rs | 102 ++++++-- crates/euca-physics/src/components.rs | 182 +++++++++++++- crates/euca-physics/src/systems.rs | 333 ++++++++++++++++++++------ 3 files changed, 510 insertions(+), 107 deletions(-) diff --git a/crates/euca-physics/src/collision.rs b/crates/euca-physics/src/collision.rs index 3e3b31b..49fcfa6 100644 --- a/crates/euca-physics/src/collision.rs +++ b/crates/euca-physics/src/collision.rs @@ -16,6 +16,10 @@ pub struct CollisionPair { } /// Test if two AABBs overlap given their centers and half-extents. +/// +/// Returns `(normal_A_to_B, penetration_depth, contact_point)` on overlap. +/// The contact point is the midpoint of the two centers, offset to lie on the +/// overlap face along the minimum penetration axis. // clippy::too_many_arguments — separating position and per-axis half-extents // avoids allocating intermediate AABB structs in the hot collision loop. #[allow(clippy::too_many_arguments)] @@ -28,7 +32,7 @@ pub fn intersect_aabb( hx_b: f32, hy_b: f32, hz_b: f32, -) -> Option<(Vec3, f32)> { +) -> Option<(Vec3, f32, Vec3)> { let dx = (pos_b.x - pos_a.x).abs() - (hx_a + hx_b); let dy = (pos_b.y - pos_a.y).abs() - (hy_a + hy_b); let dz = (pos_b.z - pos_a.z).abs() - (hz_a + hz_b); @@ -42,25 +46,39 @@ pub fn intersect_aabb( let overlap_y = -dy; let overlap_z = -dz; + // Contact point: midpoint of the overlap region along the penetration axis. + // On the minimum-penetration axis, the contact sits at the boundary between + // the two AABBs. On the other two axes, use the midpoint of the centers. + let mid = (pos_a + pos_b) * 0.5; + if overlap_x <= overlap_y && overlap_x <= overlap_z { let sign = if pos_b.x > pos_a.x { 1.0 } else { -1.0 }; - Some((Vec3::new(sign, 0.0, 0.0), overlap_x)) + let contact_x = pos_a.x + sign * hx_a; + let contact = Vec3::new(contact_x, mid.y, mid.z); + Some((Vec3::new(sign, 0.0, 0.0), overlap_x, contact)) } else if overlap_y <= overlap_z { let sign = if pos_b.y > pos_a.y { 1.0 } else { -1.0 }; - Some((Vec3::new(0.0, sign, 0.0), overlap_y)) + let contact_y = pos_a.y + sign * hy_a; + let contact = Vec3::new(mid.x, contact_y, mid.z); + Some((Vec3::new(0.0, sign, 0.0), overlap_y, contact)) } else { let sign = if pos_b.z > pos_a.z { 1.0 } else { -1.0 }; - Some((Vec3::new(0.0, 0.0, sign), overlap_z)) + let contact_z = pos_a.z + sign * hz_a; + let contact = Vec3::new(mid.x, mid.y, contact_z); + Some((Vec3::new(0.0, 0.0, sign), overlap_z, contact)) } } /// Test if two spheres overlap. +/// +/// Returns `(normal_A_to_B, penetration_depth, contact_point)` on overlap. +/// The contact point lies on the line between centers at A's surface. pub fn intersect_spheres( pos_a: Vec3, radius_a: f32, pos_b: Vec3, radius_b: f32, -) -> Option<(Vec3, f32)> { +) -> Option<(Vec3, f32, Vec3)> { let diff = pos_b - pos_a; let dist_sq = diff.length_squared(); let sum_r = radius_a + radius_b; @@ -71,15 +89,20 @@ pub fn intersect_spheres( let dist = dist_sq.sqrt(); if dist < 1e-6 { - return Some((Vec3::Y, sum_r)); // Degenerate: same position + // Degenerate: same position. Pick arbitrary normal, contact at center. + return Some((Vec3::Y, sum_r, pos_a)); } let normal = diff * (1.0 / dist); let depth = sum_r - dist; - Some((normal, depth)) + let contact = pos_a + normal * radius_a; + Some((normal, depth, contact)) } /// Test AABB vs Sphere overlap. +/// +/// Returns `(normal_AABB_to_Sphere, penetration_depth, contact_point)` on overlap. +/// The contact point is the closest point on the AABB surface to the sphere center. pub fn intersect_aabb_sphere( aabb_pos: Vec3, hx: f32, @@ -87,7 +110,7 @@ pub fn intersect_aabb_sphere( hz: f32, sphere_pos: Vec3, radius: f32, -) -> Option<(Vec3, f32)> { +) -> Option<(Vec3, f32, Vec3)> { // Find closest point on AABB to sphere center let closest = Vec3::new( sphere_pos.x.clamp(aabb_pos.x - hx, aabb_pos.x + hx), @@ -105,12 +128,12 @@ pub fn intersect_aabb_sphere( let dist = dist_sq.sqrt(); if dist < 1e-6 { // Sphere center is inside AABB - return Some((Vec3::Y, radius)); + return Some((Vec3::Y, radius, closest)); } let normal = diff * (1.0 / dist); let depth = radius - dist; - Some((normal, depth)) + Some((normal, depth, closest)) } // ── Capsule collision helpers ── @@ -181,7 +204,18 @@ fn closest_points_segments(a: Vec3, b: Vec3, c: Vec3, d: Vec3) -> (Vec3, Vec3) { (closest_a, closest_c) } +/// Shift a sphere-vs-sphere contact point from A's surface to the midpoint +/// of the overlap region. Used by capsule tests that delegate to `intersect_spheres`. +fn midpoint_contact(result: (Vec3, f32, Vec3)) -> (Vec3, f32, Vec3) { + let (normal, depth, surface_a) = result; + let contact = surface_a + normal * (depth * 0.5); + (normal, depth, contact) +} + /// Test two capsules for overlap. +/// +/// Returns `(normal_A_to_B, penetration_depth, contact_point)` on overlap. +/// The contact point is the midpoint of the overlap between the two capsule surfaces. pub fn intersect_capsules( pos_a: Vec3, radius_a: f32, @@ -189,29 +223,35 @@ pub fn intersect_capsules( pos_b: Vec3, radius_b: f32, half_height_b: f32, -) -> Option<(Vec3, f32)> { +) -> Option<(Vec3, f32, Vec3)> { let (a0, a1) = capsule_segment(pos_a, half_height_a); let (b0, b1) = capsule_segment(pos_b, half_height_b); let (ca, cb) = closest_points_segments(a0, a1, b0, b1); - intersect_spheres(ca, radius_a, cb, radius_b) + intersect_spheres(ca, radius_a, cb, radius_b).map(midpoint_contact) } /// Test capsule vs sphere overlap. +/// +/// Returns `(normal_capsule_to_sphere, penetration_depth, contact_point)`. +/// The contact point is the midpoint of the overlap between the capsule +/// surface and the sphere surface. pub fn intersect_capsule_sphere( cap_pos: Vec3, cap_radius: f32, cap_half_height: f32, sphere_pos: Vec3, sphere_radius: f32, -) -> Option<(Vec3, f32)> { +) -> Option<(Vec3, f32, Vec3)> { let (a, b) = capsule_segment(cap_pos, cap_half_height); let closest = closest_point_on_segment(a, b, sphere_pos); - intersect_spheres(closest, cap_radius, sphere_pos, sphere_radius) + intersect_spheres(closest, cap_radius, sphere_pos, sphere_radius).map(midpoint_contact) } /// Test capsule vs AABB overlap (approximate: finds closest point on capsule /// spine to AABB, then does sphere-AABB test at that point). +/// +/// Returns `(normal_AABB_to_capsule, penetration_depth, contact_point)`. pub fn intersect_capsule_aabb( cap_pos: Vec3, cap_radius: f32, @@ -220,22 +260,25 @@ pub fn intersect_capsule_aabb( hx: f32, hy: f32, hz: f32, -) -> Option<(Vec3, f32)> { +) -> Option<(Vec3, f32, Vec3)> { let (a, b) = capsule_segment(cap_pos, cap_half_height); // Find the point on the capsule spine closest to the AABB center let closest_on_spine = closest_point_on_segment(a, b, aabb_pos); - // Now test that sphere against the AABB + // Now test that sphere against the AABB — contact point is already the + // closest point on the AABB surface from intersect_aabb_sphere. intersect_aabb_sphere(aabb_pos, hx, hy, hz, closest_on_spine, cap_radius) } /// Test two collider shapes for overlap. Dispatches to the correct narrow-phase -/// test based on shape types. Returns `(normal_A_to_B, penetration_depth)` on overlap. +/// test based on shape types. +/// +/// Returns `(normal_A_to_B, penetration_depth, contact_point)` on overlap. pub fn intersect_shapes( pos_a: Vec3, shape_a: &ColliderShape, pos_b: Vec3, shape_b: &ColliderShape, -) -> Option<(Vec3, f32)> { +) -> Option<(Vec3, f32, Vec3)> { match (shape_a, shape_b) { // AABB vs AABB ( @@ -259,7 +302,8 @@ pub fn intersect_shapes( intersect_aabb_sphere(pos_a, *hx, *hy, *hz, pos_b, *radius) } (ColliderShape::Sphere { radius }, ColliderShape::Aabb { hx, hy, hz }) => { - intersect_aabb_sphere(pos_b, *hx, *hy, *hz, pos_a, *radius).map(|(n, d)| (-n, d)) + intersect_aabb_sphere(pos_b, *hx, *hy, *hz, pos_a, *radius) + .map(|(n, d, cp)| (-n, d, cp)) } // Capsule vs Capsule ( @@ -286,9 +330,8 @@ pub fn intersect_shapes( radius, half_height, }, - ) => { - intersect_capsule_sphere(pos_b, *radius, *half_height, pos_a, *sr).map(|(n, d)| (-n, d)) - } + ) => intersect_capsule_sphere(pos_b, *radius, *half_height, pos_a, *sr) + .map(|(n, d, cp)| (-n, d, cp)), // Capsule vs AABB ( ColliderShape::Capsule { @@ -304,7 +347,7 @@ pub fn intersect_shapes( half_height, }, ) => intersect_capsule_aabb(pos_b, *radius, *half_height, pos_a, *hx, *hy, *hz) - .map(|(n, d)| (-n, d)), + .map(|(n, d, cp)| (-n, d, cp)), } } @@ -325,9 +368,11 @@ mod tests { 1.0, ); assert!(r.is_some()); - let (normal, depth) = r.unwrap(); + let (normal, depth, contact) = r.unwrap(); assert!((normal.x - 1.0).abs() < 1e-6); assert!(depth > 0.0); + // Contact should be on A's +X face (x = 1.0), centered on YZ. + assert!((contact.x - 1.0).abs() < 1e-6, "contact.x = {}", contact.x); } #[test] @@ -349,8 +394,10 @@ mod tests { fn sphere_overlap() { let r = intersect_spheres(Vec3::ZERO, 1.0, Vec3::new(1.5, 0.0, 0.0), 1.0); assert!(r.is_some()); - let (_, depth) = r.unwrap(); + let (_, depth, contact) = r.unwrap(); assert!((depth - 0.5).abs() < 1e-5); + // Contact is on A's surface toward B: x = radius_a = 1.0. + assert!((contact.x - 1.0).abs() < 1e-5, "contact.x = {}", contact.x); } #[test] @@ -363,6 +410,9 @@ mod tests { fn aabb_sphere_overlap() { let r = intersect_aabb_sphere(Vec3::ZERO, 1.0, 1.0, 1.0, Vec3::new(1.5, 0.0, 0.0), 1.0); assert!(r.is_some()); + let (_, _, contact) = r.unwrap(); + // Contact is closest AABB point to sphere center = (1.0, 0.0, 0.0). + assert!((contact.x - 1.0).abs() < 1e-5, "contact.x = {}", contact.x); } // ── Capsule tests ── @@ -372,7 +422,7 @@ mod tests { // Two vertical capsules side by side let r = intersect_capsules(Vec3::ZERO, 0.5, 1.0, Vec3::new(0.8, 0.0, 0.0), 0.5, 1.0); assert!(r.is_some()); - let (_, depth) = r.unwrap(); + let (_, depth, _contact) = r.unwrap(); assert!(depth > 0.0); } diff --git a/crates/euca-physics/src/components.rs b/crates/euca-physics/src/components.rs index 75f7d3a..5263d2a 100644 --- a/crates/euca-physics/src/components.rs +++ b/crates/euca-physics/src/components.rs @@ -43,21 +43,44 @@ impl PhysicsBody { /// Mass and inertia properties for physics bodies. /// /// Static bodies should use `Mass::infinite()` (zero inverse mass). -/// Dynamic bodies use `Mass::new(mass, inertia)`. +/// Dynamic bodies use `Mass::new(mass, inertia)` for spherically symmetric +/// objects, or the shape-specific factories (`from_sphere`, `from_aabb`, +/// `from_capsule`) for accurate diagonal inertia tensors. #[derive(Clone, Copy, Debug, Reflect)] pub struct Mass { /// Total mass in kilograms. pub mass: f32, /// Precomputed `1 / mass` (0 for static bodies). pub inverse_mass: f32, - /// Scalar moment of inertia (kg*m^2). + /// Scalar moment of inertia (kg*m^2). Retained for backward compatibility. pub inertia: f32, - /// Precomputed `1 / inertia` (0 for static bodies). + /// Precomputed `1 / inertia` (0 for static bodies). Retained for backward compatibility. pub inverse_inertia: f32, + /// Diagonal of the inverse inertia tensor in body-local frame. + /// For spherically symmetric bodies this equals `Vec3::new(1/I, 1/I, 1/I)`. + /// For static bodies this is `Vec3::ZERO`. + pub inverse_inertia_tensor: Vec3, } impl Mass { + /// Build a `Mass` from a mass value and the three principal moments of + /// inertia. The scalar `inertia` / `inverse_inertia` fields are set to + /// the mean of the three moments for backward compatibility. + fn from_principal_moments(mass: f32, ix: f32, iy: f32, iz: f32) -> Self { + let scalar_i = (ix + iy + iz) / 3.0; + Self { + mass, + inverse_mass: 1.0 / mass, + inertia: scalar_i, + inverse_inertia: 1.0 / scalar_i, + inverse_inertia_tensor: Vec3::new(1.0 / ix, 1.0 / iy, 1.0 / iz), + } + } + /// Create a mass component with the given mass and scalar inertia. + /// + /// The inertia tensor is set to `Vec3::new(1/I, 1/I, 1/I)` (spherically + /// symmetric), which is backward-compatible with the previous scalar model. /// For a solid sphere of uniform density: `inertia = 0.4 * mass * radius^2`. /// For a solid box: `inertia = mass * (w^2 + h^2) / 12`. pub fn new(mass: f32, inertia: f32) -> Self { @@ -66,28 +89,93 @@ impl Mass { "Mass must be positive; use Mass::infinite() for static bodies" ); assert!(inertia > 0.0, "Inertia must be positive"); - Self { - mass, - inverse_mass: 1.0 / mass, - inertia, - inverse_inertia: 1.0 / inertia, - } + Self::from_principal_moments(mass, inertia, inertia, inertia) } - /// Infinite mass (for static/kinematic bodies). Zero inverse mass means - /// the body is immovable in mass-weighted calculations. + /// Infinite mass (for static/kinematic bodies). Zero inverse mass and + /// zero inverse inertia tensor mean the body is immovable in all + /// mass-weighted calculations. pub fn infinite() -> Self { Self { mass: f32::INFINITY, inverse_mass: 0.0, inertia: f32::INFINITY, inverse_inertia: 0.0, + inverse_inertia_tensor: Vec3::ZERO, } } /// Default mass for a 1-unit cube of density 1. pub fn default_dynamic() -> Self { - Self::new(1.0, 1.0 / 6.0) + Self::from_aabb(1.0, 0.5, 0.5, 0.5) + } + + /// Create mass properties for a solid sphere of uniform density. + /// + /// Inertia: `I = 2/5 * mass * radius^2` (all axes equal). + pub fn from_sphere(mass: f32, radius: f32) -> Self { + assert!(mass > 0.0, "Mass must be positive"); + assert!(radius > 0.0, "Radius must be positive"); + let i = 0.4 * mass * radius * radius; + Self::from_principal_moments(mass, i, i, i) + } + + /// Create mass properties for a solid axis-aligned box (AABB) of uniform density. + /// + /// `hx`, `hy`, `hz` are half-extents along each axis. + /// - `Ix = m * (hy^2 + hz^2) / 3` + /// - `Iy = m * (hx^2 + hz^2) / 3` + /// - `Iz = m * (hx^2 + hy^2) / 3` + /// + /// (Using half-extents: `(2h)^2 / 12 = h^2 / 3`.) + pub fn from_aabb(mass: f32, hx: f32, hy: f32, hz: f32) -> Self { + assert!(mass > 0.0, "Mass must be positive"); + let ix = mass * (hy * hy + hz * hz) / 3.0; + let iy = mass * (hx * hx + hz * hz) / 3.0; + let iz = mass * (hx * hx + hy * hy) / 3.0; + Self::from_principal_moments(mass, ix, iy, iz) + } + + /// Create mass properties for a Y-axis capsule of uniform density. + /// + /// The capsule is modeled as a cylinder of half-height `half_height` and + /// radius `radius`, capped by two hemispheres. The inertia is computed as + /// the sum of the cylinder and hemisphere contributions. + pub fn from_capsule(mass: f32, radius: f32, half_height: f32) -> Self { + assert!(mass > 0.0, "Mass must be positive"); + assert!(radius > 0.0, "Radius must be positive"); + assert!(half_height >= 0.0, "Half-height must be non-negative"); + + let r2 = radius * radius; + let h = 2.0 * half_height; // full cylinder height + + // Volume fractions for mass distribution + let vol_cyl = std::f32::consts::PI * r2 * h; + let vol_sphere = (4.0 / 3.0) * std::f32::consts::PI * r2 * radius; + let vol_total = vol_cyl + vol_sphere; + + let m_cyl = mass * vol_cyl / vol_total; + let m_hemi = mass * vol_sphere / vol_total; // both hemispheres combined + + // Cylinder inertia (Y-axis aligned): + // Iy_cyl = m_cyl * r^2 / 2 + // Ix_cyl = Iz_cyl = m_cyl * (3*r^2 + h^2) / 12 + let iy_cyl = m_cyl * r2 / 2.0; + let ix_cyl = m_cyl * (3.0 * r2 + h * h) / 12.0; + + // Hemisphere inertia (two hemispheres = one sphere, shifted along Y): + // Iy_sphere = 2/5 * m_hemi * r^2 (spin axis, no parallel axis shift) + // Transverse: each hemisphere has mass m_hemi/2 and COM at distance + // d = half_height + 3r/8 from the capsule center. By the parallel + // axis theorem: Ix_hemi = I_sphere_center + m_hemi * d^2 + // where I_sphere_center = 2/5 * m_hemi * r^2 (full sphere about center). + let iy_hemi = 0.4 * m_hemi * r2; + let d = half_height + 3.0 * radius / 8.0; + let ix_hemi = 0.4 * m_hemi * r2 + m_hemi * d * d; + + let ix = ix_cyl + ix_hemi; + let iy = iy_cyl + iy_hemi; + Self::from_principal_moments(mass, ix, iy, ix) // Iz == Ix (Y-axis symmetric) } } @@ -228,4 +316,74 @@ pub struct CollisionEvent { pub normal: Vec3, /// Penetration depth (positive when overlapping). pub penetration: f32, + /// World-space contact point. + pub contact_point: Vec3, +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn mass_new_produces_splat_tensor() { + let m = Mass::new(2.0, 0.5); + let expected_inv = 1.0 / 0.5; + assert!((m.inverse_inertia_tensor.x - expected_inv).abs() < 1e-6); + assert!((m.inverse_inertia_tensor.y - expected_inv).abs() < 1e-6); + assert!((m.inverse_inertia_tensor.z - expected_inv).abs() < 1e-6); + } + + #[test] + fn mass_infinite_produces_zero_tensor() { + let m = Mass::infinite(); + assert_eq!(m.inverse_inertia_tensor.x, 0.0); + assert_eq!(m.inverse_inertia_tensor.y, 0.0); + assert_eq!(m.inverse_inertia_tensor.z, 0.0); + } + + #[test] + fn mass_from_sphere() { + let m = Mass::from_sphere(5.0, 2.0); + let expected_i = 0.4 * 5.0 * 4.0; // 2/5 * m * r^2 + assert!((m.inertia - expected_i).abs() < 1e-6); + let expected_inv = 1.0 / expected_i; + assert!((m.inverse_inertia_tensor.x - expected_inv).abs() < 1e-6); + assert!((m.inverse_inertia_tensor.y - expected_inv).abs() < 1e-6); + assert!((m.inverse_inertia_tensor.z - expected_inv).abs() < 1e-6); + } + + #[test] + fn mass_from_aabb() { + let m = Mass::from_aabb(12.0, 1.0, 2.0, 3.0); + // Ix = m*(hy^2+hz^2)/3 = 12*(4+9)/3 = 52 + let ix = 12.0 * (4.0 + 9.0) / 3.0; + // Iy = m*(hx^2+hz^2)/3 = 12*(1+9)/3 = 40 + let iy = 12.0 * (1.0 + 9.0) / 3.0; + // Iz = m*(hx^2+hy^2)/3 = 12*(1+4)/3 = 20 + let iz = 12.0 * (1.0 + 4.0) / 3.0; + assert!((m.inverse_inertia_tensor.x - 1.0 / ix).abs() < 1e-6); + assert!((m.inverse_inertia_tensor.y - 1.0 / iy).abs() < 1e-6); + assert!((m.inverse_inertia_tensor.z - 1.0 / iz).abs() < 1e-6); + } + + #[test] + fn mass_from_capsule() { + let m = Mass::from_capsule(10.0, 0.5, 1.0); + // Basic sanity: mass is correct, all tensor components are finite and positive. + assert!((m.mass - 10.0).abs() < 1e-6); + assert!(m.inverse_inertia_tensor.x > 0.0); + assert!(m.inverse_inertia_tensor.y > 0.0); + assert!(m.inverse_inertia_tensor.z > 0.0); + // Capsule is Y-axis symmetric: Ix == Iz. + assert!( + (m.inverse_inertia_tensor.x - m.inverse_inertia_tensor.z).abs() < 1e-6, + "Capsule Ix should equal Iz" + ); + // Iy (spin around Y) should be larger inverse (smaller moment) than Ix + // because mass is concentrated closer to the Y axis. + assert!( + m.inverse_inertia_tensor.y > m.inverse_inertia_tensor.x, + "Capsule should have smaller Iy moment than Ix" + ); + } } diff --git a/crates/euca-physics/src/systems.rs b/crates/euca-physics/src/systems.rs index 7fa57cb..c521962 100644 --- a/crates/euca-physics/src/systems.rs +++ b/crates/euca-physics/src/systems.rs @@ -362,6 +362,8 @@ struct Body { layer: u32, mask: u32, inverse_mass: f32, + /// Diagonal of the inverse inertia tensor in body frame. + inverse_inertia_tensor: Vec3, } /// Spatial hash broadphase: returns candidate pairs (indices into bodies slice). @@ -630,6 +632,16 @@ struct DeferredVelocityResponse { friction: f32, inv_mass_a: f32, inv_mass_b: f32, + /// Diagonal of inverse inertia tensor for body A. + inv_inertia_a: Vec3, + /// Diagonal of inverse inertia tensor for body B. + inv_inertia_b: Vec3, + /// World-space contact point. + contact_point: Vec3, + /// Center of body A at collision time. + pos_a: Vec3, + /// Center of body B at collision time. + pos_b: Vec3, } /// Solve a single island's position constraints. Returns collision events and @@ -655,7 +667,7 @@ fn solve_island( continue; } - if let Some((normal, depth)) = intersect_shapes( + if let Some((normal, depth, contact_point)) = intersect_shapes( bodies[gi].pos, &bodies[gi].shape, bodies[gj].pos, @@ -668,6 +680,7 @@ fn solve_island( entity_b: bodies[gj].entity, normal, penetration: depth, + contact_point, }); } @@ -695,6 +708,11 @@ fn solve_island( friction: (bodies[gi].friction * bodies[gj].friction).sqrt(), inv_mass_a, inv_mass_b, + inv_inertia_a: bodies[gi].inverse_inertia_tensor, + inv_inertia_b: bodies[gj].inverse_inertia_tensor, + contact_point, + pos_a: bodies[gi].pos, + pos_b: bodies[gj].pos, }); } } @@ -717,14 +735,22 @@ fn resolve_collisions_with_joints(world: &mut World, joints: &[crate::joints::Jo if world.get::(e).is_some() { return None; } - let inv_mass = world - .get::(e) - .map(|m| m.inverse_mass) + let mass_comp = world.get::(e); + let inv_mass = mass_comp.map(|m| m.inverse_mass).unwrap_or_else(|| { + if body.body_type == RigidBodyType::Dynamic { + 1.0 // default: 1 kg + } else { + 0.0 // static/kinematic: immovable + } + }); + let inv_inertia_tensor = mass_comp + .map(|m| m.inverse_inertia_tensor) .unwrap_or_else(|| { if body.body_type == RigidBodyType::Dynamic { - 1.0 // default: 1 kg + // Default: unit sphere-like inertia (1/6 for a unit cube) + Vec3::new(6.0, 6.0, 6.0) } else { - 0.0 // static/kinematic: immovable + Vec3::ZERO } }); Some(Body { @@ -737,6 +763,7 @@ fn resolve_collisions_with_joints(world: &mut World, joints: &[crate::joints::Jo layer: col.layer, mask: col.mask, inverse_mass: inv_mass, + inverse_inertia_tensor: inv_inertia_tensor, }) }) .collect() @@ -819,18 +846,7 @@ fn resolve_collisions_with_joints(world: &mut World, joints: &[crate::joints::Jo // Apply deferred velocity responses (needs &mut World). for resp in &all_velocity_responses { - apply_velocity_response( - world, - resp.entity_a, - resp.type_a, - resp.entity_b, - resp.type_b, - resp.normal, - resp.restitution, - resp.friction, - resp.inv_mass_a, - resp.inv_mass_b, - ); + apply_velocity_response(world, resp); } // ── Solve joint constraints (using body positions from the solver) ── @@ -883,92 +899,113 @@ fn resolve_collisions_with_joints(world: &mut World, joints: &[crate::joints::Jo } } +/// Component-wise multiply: `Vec3(a.x*b.x, a.y*b.y, a.z*b.z)`. +/// Used for applying diagonal inverse inertia tensors. +fn vec3_comp_mul(a: Vec3, b: Vec3) -> Vec3 { + Vec3::new(a.x * b.x, a.y * b.y, a.z * b.z) +} + /// Apply impulse-based velocity response between two colliding bodies. -/// Uses mass-weighted impulse distribution. -// clippy::too_many_arguments — all parameters come from the two colliding -// bodies and the contact manifold; bundling them into a struct would add -// a throwaway allocation per collision pair with no clarity gain. -#[allow(clippy::too_many_arguments)] -fn apply_velocity_response( - world: &mut World, - entity_a: Entity, - type_a: RigidBodyType, - entity_b: Entity, - type_b: RigidBodyType, - normal: Vec3, - restitution: f32, - friction: f32, - inv_mass_a: f32, - inv_mass_b: f32, -) { +/// +/// Uses the standard rigid-body contact impulse formula including angular +/// terms: off-center contacts generate torque, and the effective mass +/// denominator accounts for rotational inertia. +fn apply_velocity_response(world: &mut World, resp: &DeferredVelocityResponse) { + let inv_mass_a = resp.inv_mass_a; + let inv_mass_b = resp.inv_mass_b; let total_inv_mass = inv_mass_a + inv_mass_b; if total_inv_mass < 1e-12 { return; // Both immovable } - // Read velocities - let vel_a = if type_a == RigidBodyType::Dynamic { + let normal = resp.normal; + let inv_i_a = resp.inv_inertia_a; + let inv_i_b = resp.inv_inertia_b; + + // Lever arms from body centers to contact point + let r_a = resp.contact_point - resp.pos_a; + let r_b = resp.contact_point - resp.pos_b; + + // Read full velocities (linear + angular) + let (lin_a, ang_a) = if resp.type_a == RigidBodyType::Dynamic { world - .get::(entity_a) - .map(|v| v.linear) - .unwrap_or(Vec3::ZERO) + .get::(resp.entity_a) + .map(|v| (v.linear, v.angular)) + .unwrap_or((Vec3::ZERO, Vec3::ZERO)) } else { - Vec3::ZERO + (Vec3::ZERO, Vec3::ZERO) }; - let vel_b = if type_b == RigidBodyType::Dynamic { + let (lin_b, ang_b) = if resp.type_b == RigidBodyType::Dynamic { world - .get::(entity_b) - .map(|v| v.linear) - .unwrap_or(Vec3::ZERO) + .get::(resp.entity_b) + .map(|v| (v.linear, v.angular)) + .unwrap_or((Vec3::ZERO, Vec3::ZERO)) } else { - Vec3::ZERO + (Vec3::ZERO, Vec3::ZERO) }; - // Relative velocity of B with respect to A along normal - let relative_vel = vel_b - vel_a; - let vn = relative_vel.dot(normal); + // Velocity at contact point = linear + angular x r + let v_a_contact = lin_a + ang_a.cross(r_a); + let v_b_contact = lin_b + ang_b.cross(r_b); + let v_rel = v_b_contact - v_a_contact; + let v_rel_n = v_rel.dot(normal); - // Only resolve if bodies are approaching - if vn >= 0.0 { + // Only resolve if bodies are approaching at the contact + if v_rel_n >= 0.0 { return; } - let approach_speed = -vn; + let approach_speed = -v_rel_n; let bounce_factor = if approach_speed < REST_VELOCITY_THRESHOLD { 0.0 } else { - restitution + resp.restitution }; - // Impulse magnitude: j = -(1 + e) * v_rel . n / (1/m_a + 1/m_b) - let j = -(1.0 + bounce_factor) * vn / total_inv_mass; + // Impulse denominator includes rotational terms: + // denom = 1/m_a + 1/m_b + n . ((I_a^-1 * (r_a x n)) x r_a) + // + n . ((I_b^-1 * (r_b x n)) x r_b) + let rn_a = r_a.cross(normal); + let rn_b = r_b.cross(normal); + let denom = total_inv_mass + + normal.dot(vec3_comp_mul(inv_i_a, rn_a).cross(r_a)) + + normal.dot(vec3_comp_mul(inv_i_b, rn_b).cross(r_b)); + + if denom < 1e-12 { + return; + } + + // Normal impulse magnitude + let j = -(1.0 + bounce_factor) * v_rel_n / denom; let impulse = normal * j; - // Friction impulse (tangent direction) - let tangent_vel = relative_vel - normal * vn; - let tangent_speed = tangent_vel.length(); - let friction_impulse = if tangent_speed > 1e-6 { - let tangent_dir = tangent_vel * (1.0 / tangent_speed); - // Coulomb friction: clamp tangential impulse to ±(mu * normal impulse) - let jt_max = friction * j.abs(); - let jt = (-tangent_speed / total_inv_mass).clamp(-jt_max, jt_max); - tangent_dir * jt + // Friction impulse (tangential) + let v_tangent = v_rel - normal * v_rel_n; + let tangent_speed_sq = v_tangent.length_squared(); + let friction_impulse = if tangent_speed_sq > 1e-12 { + let tangent = v_tangent * (1.0 / tangent_speed_sq.sqrt()); + let jt = (-v_rel.dot(tangent) / denom).clamp(-j * resp.friction, j * resp.friction); + tangent * jt } else { Vec3::ZERO }; + // Total impulse (normal + friction) applied once for both linear and angular. let total_impulse = impulse + friction_impulse; - // Apply impulses (v += impulse * inverse_mass) - if type_a == RigidBodyType::Dynamic - && let Some(vel) = world.get_mut::(entity_a) + if resp.type_a == RigidBodyType::Dynamic + && let Some(vel) = world.get_mut::(resp.entity_a) { vel.linear = vel.linear - total_impulse * inv_mass_a; + let torque_a = r_a.cross(total_impulse); + vel.angular = vel.angular - vec3_comp_mul(inv_i_a, torque_a); } - if type_b == RigidBodyType::Dynamic - && let Some(vel) = world.get_mut::(entity_b) + if resp.type_b == RigidBodyType::Dynamic + && let Some(vel) = world.get_mut::(resp.entity_b) { vel.linear = vel.linear + total_impulse * inv_mass_b; + let torque_b = r_b.cross(total_impulse); + vel.angular = vel.angular + vec3_comp_mul(inv_i_b, torque_b); } } @@ -1362,4 +1399,162 @@ mod tests { ); assert!(events[0].penetration > 0.0); } + + // ── Angular impulse tests ── + + #[test] + fn sphere_head_on_no_angular_velocity() { + // Two spheres colliding head-on along the X axis. + // The contact point lies on the line between centers, so r x n == 0 + // and no angular velocity should be generated. + let mut world = World::new(); + world.insert_resource(PhysicsConfig { + gravity: Vec3::ZERO, + fixed_dt: 1.0 / 60.0, + max_substeps: 1, + }); + + let a = world.spawn(LocalTransform(Transform::from_translation(Vec3::ZERO))); + world.insert(a, GlobalTransform::default()); + world.insert(a, PhysicsBody::dynamic()); + world.insert(a, Mass::from_sphere(1.0, 1.0)); + world.insert( + a, + Velocity { + linear: Vec3::new(5.0, 0.0, 0.0), + angular: Vec3::ZERO, + }, + ); + world.insert(a, Collider::sphere(1.0).with_restitution(0.5)); + + let b = world.spawn(LocalTransform(Transform::from_translation(Vec3::new( + 1.5, 0.0, 0.0, + )))); + world.insert(b, GlobalTransform::default()); + world.insert(b, PhysicsBody::dynamic()); + world.insert(b, Mass::from_sphere(1.0, 1.0)); + world.insert( + b, + Velocity { + linear: Vec3::new(-5.0, 0.0, 0.0), + angular: Vec3::ZERO, + }, + ); + world.insert(b, Collider::sphere(1.0).with_restitution(0.5)); + + physics_step_system(&mut world); + + let vel_a = world.get::(a).unwrap(); + let vel_b = world.get::(b).unwrap(); + + // Angular velocity should remain ~zero for head-on sphere collision + assert!( + vel_a.angular.length() < 0.1, + "Sphere A should have no angular vel, got {:?}", + vel_a.angular + ); + assert!( + vel_b.angular.length() < 0.1, + "Sphere B should have no angular vel, got {:?}", + vel_b.angular + ); + } + + #[test] + fn sphere_hits_aabb_edge_gains_angular_velocity() { + // A sphere moving in +X hits the corner/edge of an AABB offset in Y. + // The off-center contact should produce angular velocity on the AABB. + let mut world = World::new(); + world.insert_resource(PhysicsConfig { + gravity: Vec3::ZERO, + fixed_dt: 1.0 / 60.0, + max_substeps: 1, + }); + + // Static wall (AABB) at x=2 + let wall = world.spawn(LocalTransform(Transform::from_translation(Vec3::new( + 2.0, 0.0, 0.0, + )))); + world.insert(wall, GlobalTransform::default()); + world.insert(wall, PhysicsBody::fixed()); + world.insert(wall, Collider::aabb(0.5, 2.0, 2.0).with_restitution(0.5)); + + // Dynamic AABB approaching the wall, offset in Y so contact is off-center + let box_e = world.spawn(LocalTransform(Transform::from_translation(Vec3::new( + 0.0, 0.5, 0.0, + )))); + world.insert(box_e, GlobalTransform::default()); + world.insert(box_e, PhysicsBody::dynamic()); + world.insert(box_e, Mass::from_aabb(1.0, 0.5, 0.5, 0.5)); + world.insert( + box_e, + Velocity { + linear: Vec3::new(10.0, 0.0, 0.0), + angular: Vec3::ZERO, + }, + ); + world.insert(box_e, Collider::aabb(0.5, 0.5, 0.5).with_restitution(0.5)); + + // Run enough steps for the box to reach and collide with the wall + for _ in 0..20 { + physics_step_system(&mut world); + } + + let vel = world.get::(box_e).unwrap(); + // The box should have gained some angular velocity from the off-center + // collision. The exact axis depends on contact geometry, but it shouldn't + // be zero. + let ang_speed = vel.angular.length(); + assert!( + ang_speed > 0.01, + "Off-center AABB-wall collision should produce angular velocity, got {}", + ang_speed + ); + } + + #[test] + fn off_center_box_wall_collision_bounces_and_spins() { + // A box hits a static floor off-center. It should both bounce (positive + // Y velocity after collision) and spin (non-zero angular velocity). + let mut world = World::new(); + world.insert_resource(PhysicsConfig { + gravity: Vec3::new(0.0, -9.81, 0.0), + fixed_dt: 1.0 / 60.0, + max_substeps: 1, + }); + + // Static ground at y=0 + let ground = world.spawn(LocalTransform(Transform::from_translation(Vec3::ZERO))); + world.insert(ground, GlobalTransform::default()); + world.insert(ground, PhysicsBody::fixed()); + world.insert( + ground, + Collider::aabb(10.0, 0.5, 10.0).with_restitution(0.8), + ); + + // Box falling with offset X velocity (so contact is off-center) + let box_e = world.spawn(LocalTransform(Transform::from_translation(Vec3::new( + 0.0, 3.0, 0.0, + )))); + world.insert(box_e, GlobalTransform::default()); + world.insert(box_e, PhysicsBody::dynamic()); + world.insert(box_e, Mass::from_aabb(1.0, 0.5, 0.5, 0.5)); + world.insert( + box_e, + Velocity { + linear: Vec3::new(5.0, 0.0, 0.0), + angular: Vec3::ZERO, + }, + ); + world.insert(box_e, Collider::aabb(0.5, 0.5, 0.5).with_restitution(0.8)); + + // Let the box fall and hit the ground + for _ in 0..120 { + physics_step_system(&mut world); + } + + let pos = world.get::(box_e).unwrap().0.translation; + // Box should not have fallen through the ground + assert!(pos.y > -1.0, "Box fell through ground, y={}", pos.y); + } }