From c32da78f2a6014c491aa3e975fb83ddb7c80610e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Apr 2021 17:59:25 +0200 Subject: Split rigid-bodies and colliders into multiple components --- CHANGELOG.md | 5 + benchmarks2d/balls2.rs | 6 +- benchmarks2d/joint_ball2.rs | 6 +- benchmarks2d/joint_fixed2.rs | 6 +- benchmarks3d/balls3.rs | 6 +- benchmarks3d/joint_ball3.rs | 6 +- benchmarks3d/joint_fixed3.rs | 6 +- build/rapier2d-f64/Cargo.toml | 3 +- build/rapier2d/Cargo.toml | 3 +- build/rapier3d-f64/Cargo.toml | 3 +- build/rapier3d/Cargo.toml | 3 +- examples2d/add_remove2.rs | 9 +- examples2d/joints2.rs | 6 +- examples2d/one_way_platforms2.rs | 33 +- examples3d/debug_add_remove_collider3.rs | 7 +- examples3d/debug_dynamic_collider_add3.rs | 4 +- examples3d/fountain3.rs | 9 +- examples3d/joints3.rs | 20 +- examples3d/one_way_platforms3.rs | 43 +- examples3d/platform3.rs | 56 +- src/data/arena.rs | 87 ++- src/data/coarena.rs | 30 +- src/data/component_set.rs | 106 +++ src/data/mod.rs | 3 + src/dynamics/ccd/ccd_solver.rs | 444 ++++++++----- src/dynamics/ccd/toi_entry.rs | 112 +++- src/dynamics/coefficient_combine_rule.rs | 14 +- src/dynamics/island_manager.rs | 344 ++++++++++ src/dynamics/joint/joint_set.rs | 116 ++-- src/dynamics/mod.rs | 17 +- src/dynamics/rigid_body.rs | 728 ++++++++------------- src/dynamics/rigid_body_components.rs | 659 +++++++++++++++++++ src/dynamics/rigid_body_set.rs | 658 +++---------------- src/dynamics/solver/categorization.rs | 13 +- src/dynamics/solver/interaction_groups.rs | 96 ++- src/dynamics/solver/island_solver.rs | 92 ++- .../joint_constraint/ball_position_constraint.rs | 56 +- .../ball_position_constraint_wide.rs | 83 ++- .../joint_constraint/ball_velocity_constraint.rs | 114 ++-- .../ball_velocity_constraint_wide.rs | 190 +++--- .../joint_constraint/fixed_position_constraint.rs | 48 +- .../fixed_position_constraint_wide.rs | 34 +- .../joint_constraint/fixed_velocity_constraint.rs | 86 ++- .../fixed_velocity_constraint_wide.rs | 196 +++--- .../generic_position_constraint.rs | 2 +- .../generic_position_constraint_wide.rs | 13 +- .../generic_velocity_constraint.rs | 16 +- .../generic_velocity_constraint_wide.rs | 154 +++-- .../solver/joint_constraint/joint_constraint.rs | 139 ++-- .../joint_constraint/joint_position_constraint.rs | 99 ++- .../prismatic_position_constraint.rs | 42 +- .../prismatic_position_constraint_wide.rs | 34 +- .../prismatic_velocity_constraint.rs | 138 ++-- .../prismatic_velocity_constraint_wide.rs | 296 +++++---- .../revolute_position_constraint.rs | 60 +- .../revolute_position_constraint_wide.rs | 34 +- .../revolute_velocity_constraint.rs | 140 ++-- .../revolute_velocity_constraint_wide.rs | 258 ++++---- src/dynamics/solver/parallel_island_solver.rs | 39 +- src/dynamics/solver/parallel_solver_constraints.rs | 26 +- src/dynamics/solver/position_constraint.rs | 44 +- src/dynamics/solver/position_constraint_wide.rs | 69 +- src/dynamics/solver/position_ground_constraint.rs | 33 +- .../solver/position_ground_constraint_wide.rs | 54 +- src/dynamics/solver/position_solver.rs | 31 +- src/dynamics/solver/solver_constraints.rs | 152 +++-- src/dynamics/solver/velocity_constraint.rs | 60 +- src/dynamics/solver/velocity_constraint_wide.rs | 144 ++-- src/dynamics/solver/velocity_ground_constraint.rs | 56 +- .../solver/velocity_ground_constraint_wide.rs | 123 ++-- src/dynamics/solver/velocity_solver.rs | 51 +- src/geometry/broad_phase_multi_sap/broad_phase.rs | 220 ++++--- src/geometry/broad_phase_multi_sap/sap_layer.rs | 5 +- src/geometry/collider.rs | 305 ++++----- src/geometry/collider_components.rs | 220 +++++++ src/geometry/collider_set.rs | 277 +++----- src/geometry/contact_pair.rs | 22 +- src/geometry/mod.rs | 17 +- src/geometry/narrow_phase.rs | 593 +++++++++++------ src/lib.rs | 8 +- src/pipeline/collision_pipeline.rs | 67 +- src/pipeline/mod.rs | 1 + src/pipeline/physics_hooks.rs | 83 +-- src/pipeline/physics_pipeline.rs | 360 +++++++--- src/pipeline/query_pipeline.rs | 424 ++++++++---- src/pipeline/user_changes.rs | 156 +++++ src_testbed/box2d_backend.rs | 4 +- src_testbed/harness/mod.rs | 14 +- src_testbed/physics/mod.rs | 6 +- src_testbed/physx_backend.rs | 6 +- src_testbed/testbed.rs | 15 +- 91 files changed, 5996 insertions(+), 3680 deletions(-) create mode 100644 src/data/component_set.rs create mode 100644 src/dynamics/island_manager.rs create mode 100644 src/dynamics/rigid_body_components.rs create mode 100644 src/geometry/collider_components.rs create mode 100644 src/pipeline/user_changes.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index 61bce74..2d3dcf9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,8 @@ +## v0.9.0 + +### Modified +- Renamed `BodyStatus` to `RigidBodyType`. + ## v0.8.0 ### Modified - Switch to nalgebra 0.26. diff --git a/benchmarks2d/balls2.rs b/benchmarks2d/balls2.rs index 6791505..9a28195 100644 --- a/benchmarks2d/balls2.rs +++ b/benchmarks2d/balls2.rs @@ -1,5 +1,5 @@ use na::Point2; -use rapier2d::dynamics::{BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier2d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed2d::Testbed; @@ -42,9 +42,9 @@ pub fn init_world(testbed: &mut Testbed) { let y = j as f32 * shifty + centery; let status = if j == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; // Build the rigid body. diff --git a/benchmarks2d/joint_ball2.rs b/benchmarks2d/joint_ball2.rs index ecfdc47..35423e7 100644 --- a/benchmarks2d/joint_ball2.rs +++ b/benchmarks2d/joint_ball2.rs @@ -1,5 +1,5 @@ use na::Point2; -use rapier2d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::dynamics::{BallJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier2d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed2d::Testbed; @@ -28,9 +28,9 @@ pub fn init_world(testbed: &mut Testbed) { let fi = i as f32; let status = if k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/benchmarks2d/joint_fixed2.rs b/benchmarks2d/joint_fixed2.rs index 32a219e..861912c 100644 --- a/benchmarks2d/joint_fixed2.rs +++ b/benchmarks2d/joint_fixed2.rs @@ -1,5 +1,5 @@ use na::{Isometry2, Point2}; -use rapier2d::dynamics::{BodyStatus, FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::dynamics::{FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier2d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed2d::Testbed; @@ -33,9 +33,9 @@ pub fn init_world(testbed: &mut Testbed) { let fi = i as f32; let status = if k == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/benchmarks3d/balls3.rs b/benchmarks3d/balls3.rs index bdbe75e..1f0f1df 100644 --- a/benchmarks3d/balls3.rs +++ b/benchmarks3d/balls3.rs @@ -1,5 +1,5 @@ use na::Point3; -use rapier3d::dynamics::{BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -30,9 +30,9 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift - centerz; let status = if j == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let density = 0.477; diff --git a/benchmarks3d/joint_ball3.rs b/benchmarks3d/joint_ball3.rs index fee32a2..12b62a9 100644 --- a/benchmarks3d/joint_ball3.rs +++ b/benchmarks3d/joint_ball3.rs @@ -1,5 +1,5 @@ use na::Point3; -use rapier3d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::dynamics::{BallJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -23,9 +23,9 @@ pub fn init_world(testbed: &mut Testbed) { let fi = i as f32; let status = if i == 0 && (k % 4 == 0 || k == num - 1) { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/benchmarks3d/joint_fixed3.rs b/benchmarks3d/joint_fixed3.rs index c8912e7..9839e38 100644 --- a/benchmarks3d/joint_fixed3.rs +++ b/benchmarks3d/joint_fixed3.rs @@ -1,5 +1,5 @@ use na::{Isometry3, Point3}; -use rapier3d::dynamics::{BodyStatus, FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::dynamics::{FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -36,9 +36,9 @@ pub fn init_world(testbed: &mut Testbed) { // a joint between these. let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/build/rapier2d-f64/Cargo.toml b/build/rapier2d-f64/Cargo.toml index 3314825..d244d9b 100644 --- a/build/rapier2d-f64/Cargo.toml +++ b/build/rapier2d-f64/Cargo.toml @@ -16,9 +16,10 @@ edition = "2018" maintenance = { status = "actively-developed" } [features] -default = [ "dim2", "f64" ] +default = [ "dim2", "f64", "default-sets" ] dim2 = [ ] f64 = [ ] +default-sets = [ ] parallel = [ "rayon" ] simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 790bb94..a149bd9 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -16,9 +16,10 @@ edition = "2018" maintenance = { status = "actively-developed" } [features] -default = [ "dim2", "f32" ] +default = [ "dim2", "f32", "default-sets" ] dim2 = [ ] f32 = [ ] +default-sets = [ ] parallel = [ "rayon" ] simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] diff --git a/build/rapier3d-f64/Cargo.toml b/build/rapier3d-f64/Cargo.toml index cb4f03c..4b6519c 100644 --- a/build/rapier3d-f64/Cargo.toml +++ b/build/rapier3d-f64/Cargo.toml @@ -16,9 +16,10 @@ edition = "2018" maintenance = { status = "actively-developed" } [features] -default = [ "dim3", "f64" ] +default = [ "dim3", "f64", "default-sets" ] dim3 = [ ] f64 = [ ] +default-sets = [ ] parallel = [ "rayon" ] simd-stable = [ "parry3d-f64/simd-stable", "simba/wide", "simd-is-enabled" ] simd-nightly = [ "parry3d-f64/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index e797ea5..8a3877b 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -16,9 +16,10 @@ edition = "2018" maintenance = { status = "actively-developed" } [features] -default = [ "dim3", "f32" ] +default = [ "dim3", "f32", "default-sets" ] dim3 = [ ] f32 = [ ] +default-sets = [ ] parallel = [ "rayon" ] simd-stable = [ "parry3d/simd-stable", "simba/wide", "simd-is-enabled" ] simd-nightly = [ "parry3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] diff --git a/examples2d/add_remove2.rs b/examples2d/add_remove2.rs index 0aeffbe..826d5c9 100644 --- a/examples2d/add_remove2.rs +++ b/examples2d/add_remove2.rs @@ -31,9 +31,12 @@ pub fn init_world(testbed: &mut Testbed) { .map(|e| e.0) .collect(); for handle in to_remove { - physics - .bodies - .remove(handle, &mut physics.colliders, &mut physics.joints); + physics.bodies.remove( + handle, + &mut physics.islands, + &mut physics.colliders, + &mut physics.joints, + ); if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) { graphics.remove_body_nodes(*window, handle); diff --git a/examples2d/joints2.rs b/examples2d/joints2.rs index d1a1942..4d7d060 100644 --- a/examples2d/joints2.rs +++ b/examples2d/joints2.rs @@ -1,5 +1,5 @@ use na::Point2; -use rapier2d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::dynamics::{BallJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier2d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed2d::Testbed; @@ -31,9 +31,9 @@ pub fn init_world(testbed: &mut Testbed) { let fi = i as f32; let status = if i == 0 && k == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index fc3acb1..90607f7 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -9,12 +9,15 @@ struct OneWayPlatformHook { platform2: ColliderHandle, } -impl PhysicsHooks for OneWayPlatformHook { +impl PhysicsHooks for OneWayPlatformHook { fn active_hooks(&self) -> PhysicsHooksFlags { PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS } - fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { + fn modify_solver_contacts( + &self, + context: &mut ContactModificationContext, + ) { // The allowed normal for the first platform is its local +y axis, and the // allowed normal for the second platform is its local -y axis. // @@ -29,16 +32,16 @@ impl PhysicsHooks for OneWayPlatformHook { // - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y. let mut allowed_local_n1 = Vector2::zeros(); - if context.collider_handle1 == self.platform1 { + if context.collider1 == self.platform1 { allowed_local_n1 = Vector2::y(); - } else if context.collider_handle2 == self.platform1 { + } else if context.collider2 == self.platform1 { // Flip the allowed direction. allowed_local_n1 = -Vector2::y(); } - if context.collider_handle1 == self.platform2 { + if context.collider1 == self.platform2 { allowed_local_n1 = -Vector2::y(); - } else if context.collider_handle2 == self.platform2 { + } else if context.collider2 == self.platform2 { // Flip the allowed direction. allowed_local_n1 = Vector2::y(); } @@ -47,13 +50,12 @@ impl PhysicsHooks for OneWayPlatformHook { context.update_as_oneway_platform(&allowed_local_n1, 0.1); // Set the surface velocity of the accepted contacts. - let tangent_velocity = if context.collider_handle1 == self.platform1 - || context.collider_handle2 == self.platform2 - { - -12.0 - } else { - 12.0 - }; + let tangent_velocity = + if context.collider1 == self.platform1 || context.collider2 == self.platform2 { + -12.0 + } else { + 12.0 + }; for contact in context.solver_contacts.iter_mut() { contact.tangent_velocity.x = tangent_velocity; @@ -115,13 +117,14 @@ pub fn init_world(testbed: &mut Testbed) { } } - physics.bodies.foreach_active_dynamic_body_mut(|_, body| { + for handle in physics.islands.active_dynamic_bodies() { + let body = &mut physics.bodies[*handle]; if body.position().translation.y > 1.0 { body.set_gravity_scale(1.0, false); } else if body.position().translation.y < -1.0 { body.set_gravity_scale(-1.0, false); } - }); + } }); /* diff --git a/examples3d/debug_add_remove_collider3.rs b/examples3d/debug_add_remove_collider3.rs index f353162..9aab754 100644 --- a/examples3d/debug_add_remove_collider3.rs +++ b/examples3d/debug_add_remove_collider3.rs @@ -38,7 +38,12 @@ pub fn init_world(testbed: &mut Testbed) { // Remove then re-add the ground collider. let coll = physics .colliders - .remove(ground_collider_handle, &mut physics.bodies, true) + .remove( + ground_collider_handle, + &mut physics.islands, + &mut physics.bodies, + true, + ) .unwrap(); ground_collider_handle = physics .colliders diff --git a/examples3d/debug_dynamic_collider_add3.rs b/examples3d/debug_dynamic_collider_add3.rs index bd7205e..61c1482 100644 --- a/examples3d/debug_dynamic_collider_add3.rs +++ b/examples3d/debug_dynamic_collider_add3.rs @@ -79,7 +79,9 @@ pub fn init_world(testbed: &mut Testbed) { step = snapped_frame; for handle in &extra_colliders { - physics.colliders.remove(*handle, &mut physics.bodies, true); + physics + .colliders + .remove(*handle, &mut physics.islands, &mut physics.bodies, true); } extra_colliders.clear(); diff --git a/examples3d/fountain3.rs b/examples3d/fountain3.rs index 6c80562..08e1dfe 100644 --- a/examples3d/fountain3.rs +++ b/examples3d/fountain3.rs @@ -61,9 +61,12 @@ pub fn init_world(testbed: &mut Testbed) { let num_to_remove = to_remove.len() - MAX_NUMBER_OF_BODIES; for (handle, _) in &to_remove[..num_to_remove] { - physics - .bodies - .remove(*handle, &mut physics.colliders, &mut physics.joints); + physics.bodies.remove( + *handle, + &mut physics.islands, + &mut physics.colliders, + &mut physics.joints, + ); if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) { graphics.remove_body_nodes(window, *handle); diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index 6ba9461..5b082fb 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -1,7 +1,7 @@ use na::{Isometry3, Point3, Unit, UnitQuaternion, Vector3}; use rapier3d::dynamics::{ - BallJoint, BodyStatus, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder, - RigidBodyHandle, RigidBodySet, + BallJoint, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder, + RigidBodyHandle, RigidBodySet, RigidBodyType, }; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -203,9 +203,9 @@ fn create_fixed_joints( // fixed bodies. Because physx will crash if we add // a joint between these. let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) @@ -258,9 +258,9 @@ fn create_ball_joints( let fi = i as f32; let status = if i == 0 && (k % 4 == 0 || k == num - 1) { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) @@ -317,9 +317,9 @@ fn create_actuated_revolute_joints( // fixed bodies. Because physx will crash if we add // a joint between these. let status = if i == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let shifty = (i >= 1) as u32 as f32 * -2.0; @@ -378,9 +378,9 @@ fn create_actuated_ball_joints( // fixed bodies. Because physx will crash if we add // a joint between these. let status = if i == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index d117a5b..f5a2f1b 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -9,36 +9,39 @@ struct OneWayPlatformHook { platform2: ColliderHandle, } -impl PhysicsHooks for OneWayPlatformHook { +impl PhysicsHooks for OneWayPlatformHook { fn active_hooks(&self) -> PhysicsHooksFlags { PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS } - fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { + fn modify_solver_contacts( + &self, + context: &mut ContactModificationContext, + ) { // The allowed normal for the first platform is its local +y axis, and the // allowed normal for the second platform is its local -y axis. // // Now we have to be careful because the `manifold.local_n1` normal points // toward the outside of the shape of `context.co1`. So we need to flip the - // allowed normal direction if the platform is in `context.collider_handle2`. + // allowed normal direction if the platform is in `context.collider2`. // // Therefore: - // - If context.collider_handle1 == self.platform1 then the allowed normal is +y. - // - If context.collider_handle2 == self.platform1 then the allowed normal is -y. - // - If context.collider_handle1 == self.platform2 then its allowed normal +y needs to be flipped to -y. - // - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y. + // - If context.collider1 == self.platform1 then the allowed normal is +y. + // - If context.collider2 == self.platform1 then the allowed normal is -y. + // - If context.collider1 == self.platform2 then its allowed normal +y needs to be flipped to -y. + // - If context.collider2 == self.platform2 then the allowed normal -y needs to be flipped to +y. let mut allowed_local_n1 = Vector3::zeros(); - if context.collider_handle1 == self.platform1 { + if context.collider1 == self.platform1 { allowed_local_n1 = Vector3::y(); - } else if context.collider_handle2 == self.platform1 { + } else if context.collider2 == self.platform1 { // Flip the allowed direction. allowed_local_n1 = -Vector3::y(); } - if context.collider_handle1 == self.platform2 { + if context.collider1 == self.platform2 { allowed_local_n1 = -Vector3::y(); - } else if context.collider_handle2 == self.platform2 { + } else if context.collider2 == self.platform2 { // Flip the allowed direction. allowed_local_n1 = Vector3::y(); } @@ -47,13 +50,12 @@ impl PhysicsHooks for OneWayPlatformHook { context.update_as_oneway_platform(&allowed_local_n1, 0.1); // Set the surface velocity of the accepted contacts. - let tangent_velocity = if context.collider_handle1 == self.platform1 - || context.collider_handle2 == self.platform2 - { - -12.0 - } else { - 12.0 - }; + let tangent_velocity = + if context.collider1 == self.platform1 || context.collider2 == self.platform2 { + -12.0 + } else { + 12.0 + }; for contact in context.solver_contacts.iter_mut() { contact.tangent_velocity.z = tangent_velocity; @@ -115,13 +117,14 @@ pub fn init_world(testbed: &mut Testbed) { } } - physics.bodies.foreach_active_dynamic_body_mut(|_, body| { + for handle in physics.islands.active_dynamic_bodies() { + let body = physics.bodies.get_mut(*handle).unwrap(); if body.position().translation.y > 1.0 { body.set_gravity_scale(1.0, false); } else if body.position().translation.y < -1.0 { body.set_gravity_scale(-1.0, false); } - }); + } }); /* diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs index 7b5c986..cbe5b15 100644 --- a/examples3d/platform3.rs +++ b/examples3d/platform3.rs @@ -1,4 +1,4 @@ -use na::Point3; +use na::{Isometry3, Point3, UnitQuaternion, Vector3}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -14,7 +14,7 @@ pub fn init_world(testbed: &mut Testbed) { /* * Ground. */ - let ground_size = 10.0; + let ground_size = 20.0; let ground_height = 0.1; let rigid_body = RigidBodyBuilder::new_static() @@ -27,25 +27,28 @@ pub fn init_world(testbed: &mut Testbed) { /* * Create the boxes */ - let num = 6; + let num = 1; let rad = 0.2; let shift = rad * 2.0; let centerx = shift * num as f32 / 2.0; - let centery = shift / 2.0 + 3.04; + let centery = shift / 2.0; let centerz = shift * num as f32 / 2.0; - for i in 0usize..num { + for i in 0usize..20 { for j in 0usize..num { for k in 0usize..num { - let x = i as f32 * shift - centerx; + let x = i as f32 * (shift + rad / 4.0) - centerx; let y = j as f32 * shift + centery; let z = k as f32 * shift - centerz; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(x, y + rad, z) + .ccd_enabled(true) + .build(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad * 2.0, rad).build(); colliders.insert(collider, handle, &mut bodies); } } @@ -55,11 +58,16 @@ pub fn init_world(testbed: &mut Testbed) { * Setup a kinematic rigid body. */ let platform_body = RigidBodyBuilder::new_kinematic() - .translation(0.0, 1.5 + 0.8, -10.0 * rad) + .translation(0.2, 0.4, -40.0 * rad) .build(); let platform_handle = bodies.insert(platform_body); - let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0).build(); - colliders.insert(collider, platform_handle, &mut bodies); + let collider1 = ColliderBuilder::cuboid(rad * 5.0, rad * 2.0, rad * 10.0).build(); + let collider2 = ColliderBuilder::cuboid(rad * 5.0, rad * 2.0, rad * 10.0) + .position_wrt_parent(Isometry3::translation(0.0, rad * 2.1, 0.0)) + .build(); + colliders.insert(collider1, platform_handle, &mut bodies); + colliders.insert(collider2, platform_handle, &mut bodies); + testbed.set_body_color(platform_handle, Point3::new(1.0, 1.0, 0.0)); /* * Setup a callback to control the platform. @@ -67,23 +75,27 @@ pub fn init_world(testbed: &mut Testbed) { let mut count = 0; testbed.add_callback(move |_, _, physics, _, run_state| { count += 1; - if count % 100 > 50 { - return; - } + // if count % 100 > 50 { + // return; + // } if let Some(platform) = physics.bodies.get_mut(platform_handle) { let mut next_pos = *platform.position(); let dt = 0.016; - next_pos.translation.vector.y += (run_state.time * 5.0).sin() * dt; - next_pos.translation.vector.z += run_state.time.sin() * 5.0 * dt; + // next_pos.translation.vector.y += (run_state.time * 5.0).sin() * dt; + // next_pos.translation.vector.z += run_state.time.sin() * 5.0 * dt; - if next_pos.translation.vector.z >= rad * 10.0 { - next_pos.translation.vector.z -= dt; - } - if next_pos.translation.vector.z <= -rad * 10.0 { - next_pos.translation.vector.z += dt; - } + let drot = UnitQuaternion::new(Vector3::y() * 0.01); + next_pos.rotation = drot * next_pos.rotation; + next_pos.translation.vector += next_pos.rotation * Vector3::z() * 0.1; + + // if next_pos.translation.vector.z >= rad * 10.0 { + // next_pos.translation.vector.z -= dt; + // } + // if next_pos.translation.vector.z <= -rad * 10.0 { + // next_pos.translation.vector.z += dt; + // } platform.set_next_kinematic_position(next_pos); } diff --git a/src/data/arena.rs b/src/data/arena.rs index 9d057b8..bc7176d 100644 --- a/src/data/arena.rs +++ b/src/data/arena.rs @@ -19,16 +19,16 @@ use std::vec; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct Arena { items: Vec>, - generation: u64, - free_list_head: Option, + generation: u32, + free_list_head: Option, len: usize, } #[derive(Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] enum Entry { - Free { next_free: Option }, - Occupied { generation: u64, value: T }, + Free { next_free: Option }, + Occupied { generation: u32, value: T }, } /// An index (and generation) into an `Arena`. @@ -48,17 +48,17 @@ enum Entry { #[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct Index { - index: usize, - generation: u64, + index: u32, + generation: u32, } impl IndexedData for Index { fn default() -> Self { - Self::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64) + Self::from_raw_parts(crate::INVALID_U32, crate::INVALID_U32) } fn index(&self) -> usize { - self.into_raw_parts().0 + self.into_raw_parts().0 as usize } } @@ -70,7 +70,7 @@ impl Index { /// /// Providing arbitrary values will lead to malformed indices and ultimately /// panics. - pub fn from_raw_parts(a: usize, b: u64) -> Index { + pub fn from_raw_parts(a: u32, b: u32) -> Index { Index { index: a, generation: b, @@ -84,7 +84,7 @@ impl Index { /// `Index` like `pub struct MyIdentifier(Index);`. However, for external /// types whose definition you can't customize, but which you can construct /// instances of, this method can be useful. - pub fn into_raw_parts(self) -> (usize, u64) { + pub fn into_raw_parts(self) -> (u32, u32) { (self.index, self.generation) } } @@ -161,7 +161,7 @@ impl Arena { pub fn clear(&mut self) { self.items.clear(); - let end = self.items.capacity(); + let end = self.items.capacity() as u32; self.items.extend((0..end).map(|i| { if i == end - 1 { Entry::Free { next_free: None } @@ -206,7 +206,7 @@ impl Arena { match self.try_alloc_next_index() { None => Err(value), Some(index) => { - self.items[index.index] = Entry::Occupied { + self.items[index.index as usize] = Entry::Occupied { generation: self.generation, value, }; @@ -247,7 +247,7 @@ impl Arena { match self.try_alloc_next_index() { None => Err(create), Some(index) => { - self.items[index.index] = Entry::Occupied { + self.items[index.index as usize] = Entry::Occupied { generation: self.generation, value: create(index), }; @@ -260,13 +260,13 @@ impl Arena { fn try_alloc_next_index(&mut self) -> Option { match self.free_list_head { None => None, - Some(i) => match self.items[i] { + Some(i) => match self.items[i as usize] { Entry::Occupied { .. } => panic!("corrupt free list"), Entry::Free { next_free } => { self.free_list_head = next_free; self.len += 1; Some(Index { - index: i, + index: i as u32, generation: self.generation, }) } @@ -355,14 +355,14 @@ impl Arena { /// assert_eq!(arena.remove(idx), None); /// ``` pub fn remove(&mut self, i: Index) -> Option { - if i.index >= self.items.len() { + if i.index >= self.items.len() as u32 { return None; } - match self.items[i.index] { + match self.items[i.index as usize] { Entry::Occupied { generation, .. } if i.generation == generation => { let entry = mem::replace( - &mut self.items[i.index], + &mut self.items[i.index as usize], Entry::Free { next_free: self.free_list_head, }, @@ -402,8 +402,8 @@ impl Arena { /// assert!(crew_members.next().is_none()); /// ``` pub fn retain(&mut self, mut predicate: impl FnMut(Index, &mut T) -> bool) { - for i in 0..self.capacity() { - let remove = match &mut self.items[i] { + for i in 0..self.capacity() as u32 { + let remove = match &mut self.items[i as usize] { Entry::Occupied { generation, value } => { let index = Index { index: i, @@ -462,7 +462,7 @@ impl Arena { /// assert!(arena.get(idx).is_none()); /// ``` pub fn get(&self, i: Index) -> Option<&T> { - match self.items.get(i.index) { + match self.items.get(i.index as usize) { Some(Entry::Occupied { generation, value }) if *generation == i.generation => { Some(value) } @@ -488,7 +488,7 @@ impl Arena { /// assert!(arena.get_mut(idx).is_none()); /// ``` pub fn get_mut(&mut self, i: Index) -> Option<&mut T> { - match self.items.get_mut(i.index) { + match self.items.get_mut(i.index as usize) { Some(Entry::Occupied { generation, value }) if *generation == i.generation => { Some(value) } @@ -526,7 +526,7 @@ impl Arena { /// assert_eq!(arena[idx2], 4); /// ``` pub fn get2_mut(&mut self, i1: Index, i2: Index) -> (Option<&mut T>, Option<&mut T>) { - let len = self.items.len(); + let len = self.items.len() as u32; if i1.index == i2.index { assert!(i1.generation != i2.generation); @@ -544,11 +544,13 @@ impl Arena { } let (raw_item1, raw_item2) = { - let (xs, ys) = self.items.split_at_mut(cmp::max(i1.index, i2.index)); + let (xs, ys) = self + .items + .split_at_mut(cmp::max(i1.index, i2.index) as usize); if i1.index < i2.index { - (&mut xs[i1.index], &mut ys[0]) + (&mut xs[i1.index as usize], &mut ys[0]) } else { - (&mut ys[0], &mut xs[i2.index]) + (&mut ys[0], &mut xs[i2.index as usize]) } }; @@ -666,11 +668,11 @@ impl Arena { } } else { Entry::Free { - next_free: Some(i + 1), + next_free: Some(i as u32 + 1), } } })); - self.free_list_head = Some(start); + self.free_list_head = Some(start as u32); } /// Iterate over shared references to the elements in this arena. @@ -774,7 +776,7 @@ impl Arena { value, Index { generation: *generation, - index: i, + index: i as u32, }, )), _ => None, @@ -797,7 +799,7 @@ impl Arena { value, Index { generation: *generation, - index: i, + index: i as u32, }, )), _ => None, @@ -941,7 +943,10 @@ impl<'a, T> Iterator for Iter<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -970,7 +975,10 @@ impl<'a, T> DoubleEndedIterator for Iter<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -1039,7 +1047,10 @@ impl<'a, T> Iterator for IterMut<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -1068,7 +1079,10 @@ impl<'a, T> DoubleEndedIterator for IterMut<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -1126,7 +1140,10 @@ impl<'a, T> Iterator for Drain<'a, T> { match self.inner.next() { Some((_, Entry::Free { .. })) => continue, Some((index, Entry::Occupied { generation, value })) => { - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => return None, diff --git a/src/data/coarena.rs b/src/data/coarena.rs index c25cc55..cd64910 100644 --- a/src/data/coarena.rs +++ b/src/data/coarena.rs @@ -4,7 +4,7 @@ use crate::data::arena::Index; #[derive(Clone, Debug)] /// A container for data associated to item existing into another Arena. pub struct Coarena { - data: Vec<(u64, T)>, + data: Vec<(u32, T)>, } impl Coarena { @@ -17,7 +17,7 @@ impl Coarena { pub fn get(&self, index: Index) -> Option<&T> { let (i, g) = index.into_raw_parts(); self.data - .get(i) + .get(i as usize) .and_then(|(gg, t)| if g == *gg { Some(t) } else { None }) } @@ -25,7 +25,7 @@ impl Coarena { pub fn get_mut(&mut self, index: Index) -> Option<&mut T> { let (i, g) = index.into_raw_parts(); self.data - .get_mut(i) + .get_mut(i as usize) .and_then(|(gg, t)| if g == *gg { Some(t) } else { None }) } @@ -36,11 +36,11 @@ impl Coarena { { let (i1, g1) = a.into_raw_parts(); - if self.data.len() <= i1 { - self.data.resize(i1 + 1, (u32::MAX as u64, T::default())); + if self.data.len() <= i1 as usize { + self.data.resize(i1 as usize + 1, (u32::MAX, T::default())); } - self.data[i1] = (g1, value); + self.data[i1 as usize] = (g1, value); } /// Ensure that elements at the two given indices exist in this coarena, and return their reference. @@ -56,20 +56,22 @@ impl Coarena { assert_ne!(i1, i2, "Cannot index the same object twice."); let (elt1, elt2) = if i1 > i2 { - if self.data.len() <= i1 { - self.data.resize(i1 + 1, (u32::MAX as u64, default.clone())); + if self.data.len() <= i1 as usize { + self.data + .resize(i1 as usize + 1, (u32::MAX, default.clone())); } - let (left, right) = self.data.split_at_mut(i1); - (&mut right[0], &mut left[i2]) + let (left, right) = self.data.split_at_mut(i1 as usize); + (&mut right[0], &mut left[i2 as usize]) } else { // i2 > i1 - if self.data.len() <= i2 { - self.data.resize(i2 + 1, (u32::MAX as u64, default.clone())); + if self.data.len() <= i2 as usize { + self.data + .resize(i2 as usize + 1, (u32::MAX, default.clone())); } - let (left, right) = self.data.split_at_mut(i2); - (&mut left[i1], &mut right[0]) + let (left, right) = self.data.split_at_mut(i2 as usize); + (&mut left[i1 as usize], &mut right[0]) }; if elt1.0 != g1 { diff --git a/src/data/component_set.rs b/src/data/component_set.rs new file mode 100644 index 0000000..76e076a --- /dev/null +++ b/src/data/component_set.rs @@ -0,0 +1,106 @@ +use crate::data::Index; + +// TODO ECS: use this to handle optional components properly. +// pub trait OptionalComponentSet { +// fn get(&self, handle: Index) -> Option<&T>; +// } + +pub trait ComponentSetOption { + fn get(&self, handle: Index) -> Option<&T>; +} + +pub trait ComponentSet: ComponentSetOption { + fn size_hint(&self) -> usize; + // TODO ECS: remove this, its only needed by the query pipeline update + // which should only take the modified colliders into account. + fn for_each(&self, f: impl FnMut(Index, &T)); + fn index(&self, handle: Index) -> &T { + self.get(handle).unwrap() + } +} + +pub trait ComponentSetMut: ComponentSet { + fn map_mut_internal( + &mut self, + handle: crate::data::Index, + f: impl FnOnce(&mut T) -> Result, + ) -> Option; + fn set_internal(&mut self, handle: crate::data::Index, val: T); +} + +pub trait BundleSet<'a, T> { + fn index_bundle(&'a self, handle: Index) -> T; +} + +impl<'a, T, A, B> BundleSet<'a, (&'a A, &'a B)> for T +where + T: ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B) { + (self.index(handle), self.index(handle)) + } +} + +impl<'a, T, A, B, C> BundleSet<'a, (&'a A, &'a B, &'a C)> for T +where + T: ComponentSet + ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C) { + (self.index(handle), self.index(handle), self.index(handle)) + } +} + +impl<'a, T, A, B, C, D> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D)> for T +where + T: ComponentSet + ComponentSet + ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D) { + ( + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + ) + } +} + +impl<'a, T, A, B, C, D, E> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E)> for T +where + T: ComponentSet + ComponentSet + ComponentSet + ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E) { + ( + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + ) + } +} + +impl<'a, T, A, B, C, D, E, F> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F)> for T +where + T: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F) { + ( + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + ) + } +} diff --git a/src/data/mod.rs b/src/data/mod.rs index 7c49314..f20e433 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -1,8 +1,11 @@ //! Data structures modified with guaranteed deterministic behavior after deserialization. +pub use self::arena::{Arena, Index}; pub use self::coarena::Coarena; +pub use self::component_set::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; pub mod arena; mod coarena; +mod component_set; pub(crate) mod graph; pub mod pubsub; diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index ff463c7..6afd860 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,6 +1,12 @@ use super::TOIEntry; -use crate::dynamics::{RigidBodyHandle, RigidBodySet}; -use crate::geometry::{ColliderSet, IntersectionEvent, NarrowPhase}; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; +use crate::dynamics::{IslandManager, RigidBodyColliders, RigidBodyForces}; +use crate::dynamics::{ + RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, +}; +use crate::geometry::{ + ColliderParent, ColliderPosition, ColliderShape, ColliderType, IntersectionEvent, NarrowPhase, +}; use crate::math::Real; use crate::parry::utils::SortedPair; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; @@ -44,19 +50,34 @@ impl CCDSolver { /// Apply motion-clamping to the bodies affected by the given `impacts`. /// /// The `impacts` should be the result of a previous call to `self.predict_next_impacts`. - pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) { + pub fn clamp_motions(&self, dt: Real, bodies: &mut Bodies, impacts: &PredictedImpacts) + where + Bodies: ComponentSet + + ComponentSetMut + + ComponentSet + + ComponentSet, + { match impacts { PredictedImpacts::Impacts(tois) => { // println!("Num to clamp: {}", tois.len()); for (handle, toi) in tois { - if let Some(body) = bodies.get_mut_internal(*handle) { - let min_toi = (body.ccd_thickness - * 0.15 - * crate::utils::inv(body.max_point_velocity())) - .min(dt); - // println!("Min toi: {}, Toi: {}", min_toi, toi); - body.integrate_next_position(toi.max(min_toi)); - } + let (rb_poss, vels, ccd, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyCcd, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); + let local_com = &mprops.mass_properties.local_com; + + let min_toi = (ccd.ccd_thickness + * 0.15 + * crate::utils::inv(ccd.max_point_velocity(vels))) + .min(dt); + // println!("Min toi: {}, Toi: {}", min_toi, toi); + let new_pos = vels.integrate(toi.max(min_toi), &rb_poss.position, &local_com); + bodies.map_mut_internal(handle.0, |rb_poss| { + rb_poss.next_position = new_pos; + }); } } _ => {} @@ -66,34 +87,64 @@ impl CCDSolver { /// Updates the set of bodies that needs CCD to be resolved. /// /// Returns `true` if any rigid-body must have CCD resolved. - pub fn update_ccd_active_flags( + pub fn update_ccd_active_flags( &self, - bodies: &mut RigidBodySet, + islands: &IslandManager, + bodies: &mut Bodies, dt: Real, include_forces: bool, - ) -> bool { + ) -> bool + where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSet, + { let mut ccd_active = false; // println!("Checking CCD activation"); - bodies.foreach_active_dynamic_body_mut_internal(|_, body| { - body.update_ccd_active_flag(dt, include_forces); - // println!("CCD is active: {}, for {:?}", ccd_active, handle); - ccd_active = ccd_active || body.is_ccd_active(); - }); + for handle in islands.active_dynamic_bodies() { + let (ccd, vels, forces): (&RigidBodyCcd, &RigidBodyVelocity, &RigidBodyForces) = + bodies.index_bundle(handle.0); + + if ccd.ccd_enabled { + let forces = if include_forces { Some(forces) } else { None }; + let moving_fast = ccd.is_moving_fast(dt, vels, forces); + + bodies.map_mut_internal(handle.0, |ccd| { + ccd.ccd_active = moving_fast; + }); + + ccd_active = ccd_active || moving_fast; + } + } ccd_active } /// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider. - pub fn find_first_impact( + pub fn find_first_impact( &mut self, dt: Real, - bodies: &RigidBodySet, - colliders: &ColliderSet, + islands: &IslandManager, + bodies: &Bodies, + colliders: &Colliders, narrow_phase: &NarrowPhase, - ) -> Option { + ) -> Option + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { // Update the query pipeline. self.query_pipeline.update_with_mode( + islands, bodies, colliders, QueryPipelineMode::SweepTestWithPredictedPosition { dt }, @@ -102,19 +153,37 @@ impl CCDSolver { let mut pairs_seen = HashMap::default(); let mut min_toi = dt; - for (_, rb1) in bodies.iter_active_dynamic() { - if rb1.is_ccd_active() { - let predicted_body_pos1 = rb1.predict_position_using_velocity_and_forces(dt); - - for ch1 in &rb1.colliders { - let co1 = &colliders[*ch1]; - - if co1.is_sensor() { + for handle in islands.active_dynamic_bodies() { + let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); + + if rb_ccd1.ccd_active { + let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyMassProps, + &RigidBodyColliders, + ) = bodies.index_bundle(handle.0); + + let predicted_body_pos1 = + rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1); + + for ch1 in &rb_colliders1.0 { + let co_parent1: &ColliderParent = colliders + .get(ch1.0) + .expect("Could not find the ColliderParent component."); + let (co_shape1, co_pos1, co_type1): ( + &ColliderShape, + &ColliderPosition, + &ColliderType, + ) = colliders.index_bundle(ch1.0); + + if co_type1.is_sensor() { continue; // Ignore sensors. } - let aabb1 = - co1.compute_swept_aabb(&(predicted_body_pos1 * co1.position_wrt_parent())); + let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent; + let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { @@ -130,12 +199,17 @@ impl CCDSolver { ) .is_none() { - let c1 = colliders.get(*ch1).unwrap(); - let c2 = colliders.get(*ch2).unwrap(); - let bh1 = c1.parent(); - let bh2 = c2.parent(); + let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); + let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); + let c1: (_, _, _) = colliders.index_bundle(ch1.0); + let c2: (_, _, _) = colliders.index_bundle(ch2.0); + let co_type1: &ColliderType = colliders.index(ch1.0); + let co_type2: &ColliderType = colliders.index(ch1.0); - if bh1 == bh2 || (c1.is_sensor() || c2.is_sensor()) { + let bh1 = co_parent1.map(|p| p.handle); + let bh2 = co_parent2.map(|p| p.handle); + + if bh1 == bh2 || (co_type1.is_sensor() || co_type2.is_sensor()) { // Ignore self-intersection and sensors. return true; } @@ -146,16 +220,15 @@ impl CCDSolver { .map(|c| c.1.dist) .unwrap_or(0.0); - let b1 = bodies.get(bh1).unwrap(); - let b2 = bodies.get(bh2).unwrap(); + let b2 = bh2.map(|h| bodies.index_bundle(h.0)); if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), *ch1, *ch2, - c1, - c2, - b1, + (c1.0, c1.1, c1.2, co_parent1), + (c2.0, c2.1, c2.2, co_parent2), + Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), b2, None, None, @@ -181,14 +254,27 @@ impl CCDSolver { } /// Outputs the set of bodies as well as their first time-of-impact event. - pub fn predict_impacts_at_next_positions( + pub fn predict_impacts_at_next_positions( &mut self, dt: Real, - bodies: &RigidBodySet, - colliders: &ColliderSet, + islands: &IslandManager, + bodies: &Bodies, + colliders: &Colliders, narrow_phase: &NarrowPhase, events: &dyn EventHandler, - ) -> PredictedImpacts { + ) -> PredictedImpacts + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { let mut frozen = HashMap::<_, Real>::default(); let mut all_toi = BinaryHeap::new(); let mut pairs_seen = HashMap::default(); @@ -196,6 +282,7 @@ impl CCDSolver { // Update the query pipeline. self.query_pipeline.update_with_mode( + islands, bodies, colliders, QueryPipelineMode::SweepTestWithNextPosition, @@ -207,71 +294,94 @@ impl CCDSolver { * */ // TODO: don't iterate through all the colliders. - for (ch1, co1) in colliders.iter() { - let rb1 = &bodies[co1.parent()]; - if rb1.is_ccd_active() { - let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent())); - - self.query_pipeline - .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { - if ch1 == *ch2 { - // Ignore self-intersection. - return true; - } - - if pairs_seen - .insert( - SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), - (), - ) - .is_none() - { - let c1 = colliders.get(ch1).unwrap(); - let c2 = colliders.get(*ch2).unwrap(); - let bh1 = c1.parent(); - let bh2 = c2.parent(); + for handle in islands.active_dynamic_bodies() { + let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); + + if rb_ccd1.ccd_active { + let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyMassProps, + &RigidBodyColliders, + ) = bodies.index_bundle(handle.0); + + let predicted_body_pos1 = + rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1); + + for ch1 in &rb_colliders1.0 { + let co_parent1: &ColliderParent = colliders + .get(ch1.0) + .expect("Could not find the ColliderParent component."); + let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) = + colliders.index_bundle(ch1.0); + + let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent; + let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1); - if bh1 == bh2 { + self.query_pipeline + .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { + if *ch1 == *ch2 { // Ignore self-intersection. return true; } - let b1 = bodies.get(bh1).unwrap(); - let b2 = bodies.get(bh2).unwrap(); - - let smallest_dist = narrow_phase - .contact_pair(ch1, *ch2) - .and_then(|p| p.find_deepest_contact()) - .map(|c| c.1.dist) - .unwrap_or(0.0); - - if let Some(toi) = TOIEntry::try_from_colliders( - self.query_pipeline.query_dispatcher(), - ch1, - *ch2, - c1, - c2, - b1, - b2, - None, - None, - 0.0, - // NOTE: we use dt here only once we know that - // there is at least one TOI before dt. - min_overstep, - smallest_dist, - ) { - if toi.toi > dt { - min_overstep = min_overstep.min(toi.toi); -