feat(physics): add angular dynamics, sequential impulse solver, sleep system, BVH improvements, CCD, and ray extensions
- Angular velocity integration with diagonal inertia tensor (sphere/box/capsule) - Angular impulse in collision solver (torque from off-center contacts) - Sequential impulse solver with configurable iterations (default 4) - Sleep/island system: bodies sleep after velocity threshold timeout, wake on collision - Ray vs triangle intersection (Moller-Trumbore algorithm) - raycast_all returning all hits sorted by distance - BVH query_pairs replaced N^2 brute force with recursive tree traversal - BVH query_ray for accelerated raycasting - BVH refit for incremental AABB updates - Swept sphere vs AABB continuous collision detection (CCD) - Updated lib.rs exports for all new public APIs Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -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");
|
||||
}
|
||||
}
|
||||
|
||||
118
crates/voltex_physics/src/ccd.rs
Normal file
118
crates/voltex_physics/src/ccd.rs
Normal file
@@ -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<f32> {
|
||||
// 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());
|
||||
}
|
||||
}
|
||||
@@ -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::<Transform, RigidBody>()
|
||||
.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::<RigidBody>(entity) {
|
||||
rb.velocity = new_velocity;
|
||||
}
|
||||
if let Some(t) = world.get_mut::<Transform>(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::<RigidBody>()
|
||||
.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::<RigidBody>(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::<Transform>(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::<Transform>(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::<RigidBody>(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::<Transform>(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::<RigidBody>(e).unwrap();
|
||||
assert!(!rb.is_sleeping, "fast-moving body should not sleep");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -72,6 +72,58 @@ pub fn raycast(world: &World, ray: &Ray, max_dist: f32) -> Option<RayHit> {
|
||||
closest
|
||||
}
|
||||
|
||||
/// Cast a ray and return ALL hits sorted by distance.
|
||||
pub fn raycast_all(world: &World, ray: &Ray, max_dist: f32) -> Vec<RayHit> {
|
||||
let entities: Vec<(Entity, Vec3, Collider)> = world
|
||||
.query2::<Transform, Collider>()
|
||||
.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();
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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::<RigidBody>(contact.entity_a).copied();
|
||||
let rb_b = world.get::<RigidBody>(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::<RigidBody>(contact.entity_a).copied();
|
||||
let rb_b = world.get::<RigidBody>(contact.entity_b).copied();
|
||||
let col_a = world.get::<Collider>(contact.entity_a).copied();
|
||||
let col_b = world.get::<Collider>(contact.entity_b).copied();
|
||||
let pos_a = world.get::<Transform>(contact.entity_a).map(|t| t.position);
|
||||
let pos_b = world.get::<Transform>(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::<RigidBody>(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::<Transform>(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<Entity> = contacts
|
||||
.iter()
|
||||
.flat_map(|c| {
|
||||
let mut entities = Vec::new();
|
||||
if let Some(rb) = world.get::<RigidBody>(c.entity_a) {
|
||||
if rb.is_sleeping { entities.push(c.entity_a); }
|
||||
}
|
||||
if let Some(rb) = world.get::<RigidBody>(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::<RigidBody>(entity) {
|
||||
rb.velocity = rb.velocity + dv;
|
||||
}
|
||||
}
|
||||
|
||||
for (entity, dp) in position_changes {
|
||||
if let Some(t) = world.get_mut::<Transform>(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::<Transform, RigidBody, Collider>()
|
||||
.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::<Transform, Collider>()
|
||||
.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::<Transform>(entity) {
|
||||
t.position = safe_pos;
|
||||
}
|
||||
if let Some(rb) = world.get_mut::<RigidBody>(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::<RigidBody>(a).unwrap().velocity;
|
||||
let vb = world.get::<RigidBody>(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::<RigidBody>(ball).unwrap();
|
||||
let floor_rb = world.get::<RigidBody>(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::<Transform>(a).unwrap().position;
|
||||
let pb = world.get::<Transform>(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::<RigidBody>(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::<Transform>(a).unwrap().position;
|
||||
let pb = world.get::<Transform>(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::<RigidBody>(a).unwrap();
|
||||
let rb_b_after = world.get::<RigidBody>(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::<Transform>(*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::<RigidBody>(a).unwrap();
|
||||
assert!(!rb_a_after.is_sleeping, "body should wake on collision");
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user