From 2b1374c596957ac8cabe085859be3b823a1ba0c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 19 Apr 2022 18:57:40 +0200 Subject: [PATCH] First round deleting the component sets. --- Cargo.toml | 10 +- src/data/component_set.rs | 123 ------ src/data/mod.rs | 2 - src/dynamics/ccd/ccd_solver.rs | 70 +--- src/dynamics/island_manager.rs | 48 +-- .../joint/impulse_joint/impulse_joint_set.rs | 40 +- .../joint/multibody_joint/multibody.rs | 89 ++-- .../multibody_joint/multibody_joint_set.rs | 35 +- src/dynamics/rigid_body.rs | 390 ++++++++---------- src/dynamics/rigid_body_components.rs | 39 +- src/dynamics/rigid_body_set.rs | 68 +-- src/dynamics/solver/categorization.rs | 7 +- .../solver/generic_velocity_constraint.rs | 16 +- .../generic_velocity_ground_constraint.rs | 15 +- src/dynamics/solver/interaction_groups.rs | 31 +- src/dynamics/solver/island_solver.rs | 22 +- .../joint_constraint/joint_constraint.rs | 49 +-- src/dynamics/solver/parallel_island_solver.rs | 17 +- .../solver/parallel_solver_constraints.rs | 31 +- .../solver/parallel_velocity_solver.rs | 15 +- src/dynamics/solver/solver_constraints.rs | 175 +++----- src/dynamics/solver/velocity_constraint.rs | 15 +- .../solver/velocity_constraint_wide.rs | 11 +- .../solver/velocity_ground_constraint.rs | 15 +- .../solver/velocity_ground_constraint_wide.rs | 11 +- src/dynamics/solver/velocity_solver.rs | 17 +- .../broad_phase_multi_sap/broad_phase.rs | 34 +- src/geometry/collider.rs | 183 ++++---- src/geometry/collider_set.rs | 64 +-- src/geometry/narrow_phase.rs | 142 ++----- src/pipeline/collision_pipeline.rs | 66 +-- src/pipeline/physics_hooks.rs | 53 +-- src/pipeline/physics_pipeline.rs | 139 ++----- src/pipeline/query_pipeline.rs | 229 +++------- src/pipeline/user_changes.rs | 98 ++--- src_testbed/ui.rs | 2 +- 36 files changed, 722 insertions(+), 1649 deletions(-) delete mode 100644 src/data/component_set.rs diff --git a/Cargo.toml b/Cargo.toml index 042d9b2..c307641 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -8,11 +8,11 @@ resolver = "2" #simba = { path = "../simba" } #kiss3d = { path = "../kiss3d" } -#parry2d = { path = "../parry/crates/parry2d" } -#parry3d = { path = "../parry/crates/parry3d" } -#parry2d-f64 = { path = "../parry/crates/parry2d-f64" } -#parry3d-f64 = { path = "../parry/crates/parry3d-f64" } -#nalgebra = { path = "../nalgebra" } +parry2d = { path = "../parry/crates/parry2d" } +parry3d = { path = "../parry/crates/parry3d" } +parry2d-f64 = { path = "../parry/crates/parry2d-f64" } +parry3d-f64 = { path = "../parry/crates/parry3d-f64" } +# nalgebra = { path = "../nalgebra" } #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } diff --git a/src/data/component_set.rs b/src/data/component_set.rs deleted file mode 100644 index 6e0461c..0000000 --- a/src/data/component_set.rs +++ /dev/null @@ -1,123 +0,0 @@ -use crate::data::Index; - -// TODO ECS: use this to handle optional components properly. -// pub trait OptionalComponentSet { -// fn get(&self, handle: Index) -> Option<&T>; -// } - -/// A set of optional elements of type `T`. -pub trait ComponentSetOption: Sync { - /// Get the element associated to the given `handle`, if there is one. - fn get(&self, handle: Index) -> Option<&T>; -} - -/// A set of elements of type `T`. -pub trait ComponentSet: ComponentSetOption { - /// The estimated number of elements in this set. - /// - /// This value is typically used for preallocating some arrays for - /// better performances. - 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. - /// Iterate through all the elements on this set. - fn for_each(&self, f: impl FnMut(Index, &T)); - /// Get the element associated to the given `handle`. - fn index(&self, handle: Index) -> &T { - self.get(handle).unwrap() - } -} - -/// A set of mutable elements of type `T`. -pub trait ComponentSetMut: ComponentSet { - /// Applies the given closure to the element associated to the given `handle`. - /// - /// Return `None` if the element doesn't exist. - fn map_mut_internal( - &mut self, - handle: crate::data::Index, - f: impl FnOnce(&mut T) -> Result, - ) -> Option; - - /// Set the value of this element. - fn set_internal(&mut self, handle: crate::data::Index, val: T); -} - -/// Helper trait to address multiple elements at once. -pub trait BundleSet<'a, T> { - /// Access multiple elements from this set. - 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 f20e433..6d9e2ce 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -2,10 +2,8 @@ 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 bd3b20b..8fc5a7f 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,11 +1,11 @@ use super::TOIEntry; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; -use crate::dynamics::{IslandManager, RigidBodyColliders, RigidBodyForces}; use crate::dynamics::{ - RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, + IslandManager, RigidBodyCcd, RigidBodyColliders, RigidBodyForces, RigidBodyHandle, + RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyVelocity, }; use crate::geometry::{ - ColliderParent, ColliderPosition, ColliderShape, ColliderType, CollisionEvent, NarrowPhase, + ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, CollisionEvent, + NarrowPhase, }; use crate::math::Real; use crate::parry::utils::SortedPair; @@ -57,13 +57,7 @@ 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 Bodies, impacts: &PredictedImpacts) - where - Bodies: ComponentSet - + ComponentSetMut - + ComponentSet - + ComponentSet, - { + pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) { match impacts { PredictedImpacts::Impacts(tois) => { for (handle, toi) in tois { @@ -93,18 +87,13 @@ 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, islands: &IslandManager, - bodies: &mut Bodies, + bodies: &mut RigidBodySet, dt: Real, include_forces: bool, - ) -> bool - where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSet, - { + ) -> bool { let mut ccd_active = false; // println!("Checking CCD activation"); @@ -128,27 +117,14 @@ impl CCDSolver { } /// 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, islands: &IslandManager, - bodies: &Bodies, - colliders: &Colliders, + bodies: &RigidBodySet, + colliders: &ColliderSet, narrow_phase: &NarrowPhase, - ) -> Option - where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - Colliders: ComponentSetOption - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) -> Option { // Update the query pipeline. self.query_pipeline.update_with_mode( islands, @@ -266,29 +242,15 @@ 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, islands: &IslandManager, - bodies: &Bodies, - colliders: &Colliders, + bodies: &RigidBodySet, + colliders: &ColliderSet, narrow_phase: &NarrowPhase, events: &dyn EventHandler, - ) -> PredictedImpacts - where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - Colliders: ComponentSetOption - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) -> PredictedImpacts { let mut frozen = HashMap::<_, Real>::default(); let mut all_toi = BinaryHeap::new(); let mut pairs_seen = HashMap::default(); diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 06f3820..0cb92e9 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -1,9 +1,8 @@ -use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; use crate::dynamics::{ ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle, - RigidBodyIds, RigidBodyType, RigidBodyVelocity, + RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity, }; -use crate::geometry::{ColliderParent, NarrowPhase}; +use crate::geometry::{ColliderSet, NarrowPhase}; use crate::math::Real; use crate::utils::WDot; @@ -40,10 +39,7 @@ impl IslandManager { } /// Update this data-structure after one or multiple rigid-bodies have been removed for `bodies`. - pub fn cleanup_removed_rigid_bodies( - &mut self, - bodies: &mut impl ComponentSetMut, - ) { + pub fn cleanup_removed_rigid_bodies(&mut self, bodies: &mut RigidBodySet) { let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; for active_set in &mut active_sets { @@ -69,7 +65,7 @@ impl IslandManager { &mut self, removed_handle: RigidBodyHandle, removed_ids: &RigidBodyIds, - bodies: &mut impl ComponentSetMut, + bodies: &mut RigidBodySet, ) { let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; @@ -77,10 +73,11 @@ impl IslandManager { if active_set.get(removed_ids.active_set_id) == Some(&removed_handle) { active_set.swap_remove(removed_ids.active_set_id); - if let Some(replacement) = active_set.get(removed_ids.active_set_id) { - bodies.map_mut_internal(replacement.0, |ids| { - ids.active_set_id = removed_ids.active_set_id; - }); + if let Some(replacement) = active_set + .get(removed_ids.active_set_id) + .and_then(|h| bodies.get_mut_internal(*h)) + { + replacement.ids.active_set_id = removed_ids.active_set_id; } } } @@ -90,17 +87,11 @@ impl IslandManager { /// /// If `strong` is `true` then it is assured that the rigid-body will /// remain awake during multiple subsequent timesteps. - pub fn wake_up(&mut self, bodies: &mut Bodies, handle: RigidBodyHandle, strong: bool) - where - Bodies: ComponentSetMut - + ComponentSetOption - + ComponentSetMut, - { + pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) { // NOTE: the use an Option here because there are many legitimate cases (like when // deleting a joint attached to an already-removed body) where we could be // attempting to wake-up a rigid-body that has already been deleted. - let rb_type: Option = bodies.get(handle.0).copied(); - if rb_type == Some(RigidBodyType::Dynamic) { + if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) { bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { activation.wake_up(strong) }); @@ -141,23 +132,16 @@ impl IslandManager { self.active_islands[island_id]..self.active_islands[island_id + 1] } - pub(crate) fn update_active_set_with_contacts( + pub(crate) fn update_active_set_with_contacts( &mut self, dt: Real, - bodies: &mut Bodies, - colliders: &Colliders, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, narrow_phase: &NarrowPhase, impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, min_island_size: usize, - ) where - Bodies: ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet, - Colliders: ComponentSetOption, - { + ) { assert!( min_island_size > 0, "The minimum island size must be at least 1." @@ -203,7 +187,7 @@ impl IslandManager { #[inline(always)] fn push_contacting_bodies( rb_colliders: &RigidBodyColliders, - colliders: &impl ComponentSetOption, + colliders: &ColliderSet, narrow_phase: &NarrowPhase, stack: &mut Vec, ) { diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 448b87d..6b6d980 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -2,9 +2,11 @@ use super::ImpulseJoint; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex}; use crate::data::arena::Arena; -use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut}; -use crate::dynamics::{GenericJoint, RigidBodyHandle}; -use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType}; +use crate::data::Coarena; +use crate::dynamics::{ + GenericJoint, IslandManager, RigidBodyActivation, RigidBodyHandle, RigidBodyIds, RigidBodySet, + RigidBodyType, +}; /// The unique identifier of a joint added to the joint set. /// The unique identifier of a collider added to a collider set. @@ -215,16 +217,12 @@ impl ImpulseJointSet { /// Retrieve all the impulse_joints happening between two active bodies. // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions. - pub(crate) fn select_active_interactions( + pub(crate) fn select_active_interactions( &self, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, out: &mut Vec>, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for out_island in &mut out[..islands.num_islands()] { out_island.clear(); } @@ -263,18 +261,13 @@ impl ImpulseJointSet { /// /// If `wake_up` is set to `true`, then the bodies attached to this joint will be /// automatically woken up. - pub fn remove( + pub fn remove( &mut self, handle: ImpulseJointHandle, islands: &mut IslandManager, - bodies: &mut Bodies, + bodies: &mut RigidBodySet, wake_up: bool, - ) -> Option - where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - { + ) -> Option { let id = self.joint_ids.remove(handle.0)?; let endpoints = self.joint_graph.graph.edge_endpoints(id)?; @@ -302,17 +295,12 @@ impl ImpulseJointSet { /// The provided rigid-body handle is not required to identify a rigid-body that /// is still contained by the `bodies` component set. /// Returns the (now invalid) handles of the removed impulse_joints. - pub fn remove_joints_attached_to_rigid_body( + pub fn remove_joints_attached_to_rigid_body( &mut self, handle: RigidBodyHandle, islands: &mut IslandManager, - bodies: &mut Bodies, - ) -> Vec - where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - { + bodies: &mut RigidBodySet, + ) -> Vec { let mut deleted = vec![]; if let Some(deleted_id) = self diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index c4cc85f..5bd790a 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1,10 +1,8 @@ use super::multibody_link::{MultibodyLink, MultibodyLinkVec}; use super::multibody_workspace::MultibodyWorkspace; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; -use crate::dynamics::solver::AnyJointVelocityConstraint; use crate::dynamics::{ - IntegrationParameters, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, - RigidBodyType, RigidBodyVelocity, + solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyForces, RigidBodyHandle, + RigidBodyMassProps, RigidBodySet, RigidBodyType, RigidBodyVelocity, }; #[cfg(feature = "dim3")] use crate::math::Matrix; @@ -369,12 +367,7 @@ impl Multibody { .extend((0..num_jacobians).map(|_| Jacobian::zeros(0))); } - pub(crate) fn update_acceleration(&mut self, bodies: &Bodies) - where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + pub(crate) fn update_acceleration(&mut self, bodies: &RigidBodySet) { if self.ndofs == 0 { return; // Nothing to do. } @@ -452,10 +445,7 @@ impl Multibody { } /// Computes the constant terms of the dynamics. - pub(crate) fn update_dynamics(&mut self, dt: Real, bodies: &mut Bodies) - where - Bodies: ComponentSetMut + ComponentSet, - { + pub(crate) fn update_dynamics(&mut self, dt: Real, bodies: &mut RigidBodySet) { /* * Compute velocities. * NOTE: this is needed for kinematic bodies too. @@ -545,10 +535,7 @@ impl Multibody { } } - fn update_inertias(&mut self, dt: Real, bodies: &Bodies) - where - Bodies: ComponentSet + ComponentSet, - { + fn update_inertias(&mut self, dt: Real, bodies: &RigidBodySet) { if self.ndofs == 0 { return; // Nothing to do. } @@ -790,17 +777,11 @@ impl Multibody { } } - pub(crate) fn update_root_type(&mut self, bodies: &mut Bodies) - where - Bodies: ComponentSet + ComponentSet, - { - let rb_type: Option<&RigidBodyType> = bodies.get(self.links[0].rigid_body.0); - if let Some(rb_type) = rb_type { - let rb_pos: &RigidBodyPosition = bodies.index(self.links[0].rigid_body.0); - - if rb_type.is_dynamic() != self.root_is_dynamic { - if rb_type.is_dynamic() { - let free_joint = MultibodyJoint::free(rb_pos.position); + pub(crate) fn update_root_type(&mut self, bodies: &mut RigidBodySet) { + if let Some(rb) = bodies.get(self.links[0].rigid_body) { + if rb.is_dynamic() != self.root_is_dynamic { + if rb.is_dynamic() { + let free_joint = MultibodyJoint::free(*rb.position()); let prev_root_ndofs = self.links[0].joint().ndofs(); self.links[0].joint = free_joint; self.links[0].assembly_id = 0; @@ -819,7 +800,7 @@ impl Multibody { assert!(self.damping.len() >= SPATIAL_DIM); assert!(self.accelerations.len() >= SPATIAL_DIM); - let fixed_joint = MultibodyJoint::fixed(rb_pos.position); + let fixed_joint = MultibodyJoint::fixed(*rb.position()); let prev_root_ndofs = self.links[0].joint().ndofs(); self.links[0].joint = fixed_joint; self.links[0].assembly_id = 0; @@ -844,39 +825,31 @@ impl Multibody { } } - self.root_is_dynamic = rb_type.is_dynamic(); + self.root_is_dynamic = rb.is_dynamic(); } // Make sure the positions are properly set to match the rigid-body’s. if self.links[0].joint.data.locked_axes.is_empty() { - self.links[0].joint.set_free_pos(rb_pos.position); + self.links[0].joint.set_free_pos(*rb.position()); } else { - self.links[0].joint.data.local_frame1 = rb_pos.position; + self.links[0].joint.data.local_frame1 = *rb.position(); } } } /// Apply forward-kinematics to this multibody and its related rigid-bodies. - pub fn forward_kinematics(&mut self, bodies: &mut Bodies, update_mass_props: bool) - where - Bodies: ComponentSet - + ComponentSetMut - + ComponentSetMut, - { + pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) { // Special case for the root, which has no parent. { let link = &mut self.links[0]; link.local_to_parent = link.joint.body_to_parent(); link.local_to_world = link.local_to_parent; - bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| { - rb_pos.next_position = link.local_to_world; - }); - - if update_mass_props { - bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| { - mprops.update_world_mass_properties(&link.local_to_world) - }); + if let Some(rb) = bodies.get_mut_internal(link.rigid_body) { + rb.pos.next_position = link.local_to_world; + if update_mass_props { + rb.mprops.update_world_mass_properties(&link.local_to_world); + } } } @@ -888,32 +861,30 @@ impl Multibody { link.local_to_world = parent_link.local_to_world * link.local_to_parent; { - let parent_rb_mprops: &RigidBodyMassProps = bodies.index(parent_link.rigid_body.0); - let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0); - let c0 = parent_link.local_to_world * parent_rb_mprops.local_mprops.local_com; + let parent_rb = &bodies[parent_link.rigid_body]; + let link_rb = &bodies[link.rigid_body]; + let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com; let c2 = link.local_to_world * Point::from(link.joint.data.local_frame2.translation.vector); - let c3 = link.local_to_world * rb_mprops.local_mprops.local_com; + let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com; link.shift02 = c2 - c0; link.shift23 = c3 - c2; } - bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| { - rb_pos.next_position = link.local_to_world; - }); + let link_rb = bodies.index_mut_internal(link.rigid_body); + link_rb.pos.next_position = link.local_to_world; - let rb_type: &RigidBodyType = bodies.index(link.rigid_body.0); assert_eq!( - *rb_type, + link_rb.body_type, RigidBodyType::Dynamic, "A rigid-body that is not at the root of a multibody must be dynamic." ); if update_mass_props { - bodies.map_mut_internal(link.rigid_body.0, |rb_mprops: &mut RigidBodyMassProps| { - rb_mprops.update_world_mass_properties(&link.local_to_world) - }); + link_rb + .mprops + .update_world_mass_properties(&link.local_to_world); } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 0365062..748530f 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -1,8 +1,7 @@ -use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index}; +use crate::data::{Arena, Coarena, Index}; use crate::dynamics::joint::MultibodyLink; use crate::dynamics::{ - GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle, - RigidBodyIds, RigidBodyType, + GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyHandle, RigidBodySet, }; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex}; use crate::parry::partitioning::IndexedData; @@ -163,17 +162,13 @@ impl MultibodyJointSet { } /// Removes an multibody_joint from this set. - pub fn remove( + pub fn remove( &mut self, handle: MultibodyJointHandle, islands: &mut IslandManager, - bodies: &mut Bodies, + bodies: &mut RigidBodySet, wake_up: bool, - ) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - { + ) { if let Some(removed) = self.rb2mb.get(handle.0).copied() { let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); @@ -216,17 +211,13 @@ impl MultibodyJointSet { } /// Removes all the multibody_joints from the multibody the given rigid-body is part of. - pub fn remove_multibody_articulations( + pub fn remove_multibody_articulations( &mut self, handle: RigidBodyHandle, islands: &mut IslandManager, - bodies: &mut Bodies, + bodies: &mut RigidBodySet, wake_up: bool, - ) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - { + ) { if let Some(removed) = self.rb2mb.get(handle.0).copied() { // Remove the multibody. let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); @@ -248,16 +239,12 @@ impl MultibodyJointSet { } /// Removes all the multibody joints attached to a rigid-body. - pub fn remove_joints_attached_to_rigid_body( + pub fn remove_joints_attached_to_rigid_body( &mut self, rb_to_remove: RigidBodyHandle, islands: &mut IslandManager, - bodies: &mut Bodies, - ) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - { + bodies: &mut RigidBodySet, + ) { // TODO: optimize this. if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() { let mut articulations_to_remove = vec![]; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index d4746ba..cf52c1f 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -17,21 +17,21 @@ use num::Zero; /// To create a new rigid-body, use the `RigidBodyBuilder` structure. #[derive(Debug, Clone)] pub struct RigidBody { - pub(crate) rb_pos: RigidBodyPosition, - pub(crate) rb_mprops: RigidBodyMassProps, - pub(crate) rb_vels: RigidBodyVelocity, - pub(crate) rb_damping: RigidBodyDamping, - pub(crate) rb_forces: RigidBodyForces, - pub(crate) rb_ccd: RigidBodyCcd, - pub(crate) rb_ids: RigidBodyIds, - pub(crate) rb_colliders: RigidBodyColliders, + pub(crate) pos: RigidBodyPosition, + pub(crate) mprops: RigidBodyMassProps, + pub(crate) vels: RigidBodyVelocity, + pub(crate) damping: RigidBodyDamping, + pub(crate) forces: RigidBodyForces, + pub(crate) ccd: RigidBodyCcd, + pub(crate) ids: RigidBodyIds, + pub(crate) colliders: RigidBodyColliders, /// Whether or not this rigid-body is sleeping. - pub(crate) rb_activation: RigidBodyActivation, + pub(crate) activation: RigidBodyActivation, pub(crate) changes: RigidBodyChanges, /// The status of the body, governing how it is affected by external forces. - pub(crate) rb_type: RigidBodyType, + pub(crate) body_type: RigidBodyType, /// The dominance group this rigid-body is part of. - pub(crate) rb_dominance: RigidBodyDominance, + pub(crate) dominance: RigidBodyDominance, /// User-defined data associated to this rigid-body. pub user_data: u128, } @@ -45,75 +45,75 @@ impl Default for RigidBody { impl RigidBody { fn new() -> Self { Self { - rb_pos: RigidBodyPosition::default(), - rb_mprops: RigidBodyMassProps::default(), - rb_vels: RigidBodyVelocity::default(), - rb_damping: RigidBodyDamping::default(), - rb_forces: RigidBodyForces::default(), - rb_ccd: RigidBodyCcd::default(), - rb_ids: RigidBodyIds::default(), - rb_colliders: RigidBodyColliders::default(), - rb_activation: RigidBodyActivation::active(), + pos: RigidBodyPosition::default(), + mprops: RigidBodyMassProps::default(), + vels: RigidBodyVelocity::default(), + damping: RigidBodyDamping::default(), + forces: RigidBodyForces::default(), + ccd: RigidBodyCcd::default(), + ids: RigidBodyIds::default(), + colliders: RigidBodyColliders::default(), + activation: RigidBodyActivation::active(), changes: RigidBodyChanges::all(), - rb_type: RigidBodyType::Dynamic, - rb_dominance: RigidBodyDominance::default(), + body_type: RigidBodyType::Dynamic, + dominance: RigidBodyDominance::default(), user_data: 0, } } pub(crate) fn reset_internal_references(&mut self) { - self.rb_colliders.0 = Vec::new(); - self.rb_ids = Default::default(); + self.colliders.0 = Vec::new(); + self.ids = Default::default(); } /// The activation status of this rigid-body. pub fn activation(&self) -> &RigidBodyActivation { - &self.rb_activation + &self.activation } /// Mutable reference to the activation status of this rigid-body. pub fn activation_mut(&mut self) -> &mut RigidBodyActivation { self.changes |= RigidBodyChanges::SLEEP; - &mut self.rb_activation + &mut self.activation } /// The linear damping coefficient of this rigid-body. #[inline] pub fn linear_damping(&self) -> Real { - self.rb_damping.linear_damping + self.damping.linear_damping } /// Sets the linear damping coefficient of this rigid-body. #[inline] pub fn set_linear_damping(&mut self, damping: Real) { - self.rb_damping.linear_damping = damping; + self.damping.linear_damping = damping; } /// The angular damping coefficient of this rigid-body. #[inline] pub fn angular_damping(&self) -> Real { - self.rb_damping.angular_damping + self.damping.angular_damping } /// Sets the angular damping coefficient of this rigid-body. #[inline] pub fn set_angular_damping(&mut self, damping: Real) { - self.rb_damping.angular_damping = damping + self.damping.angular_damping = damping } /// The type of this rigid-body. pub fn body_type(&self) -> RigidBodyType { - self.rb_type + self.body_type } /// Sets the type of this rigid-body. pub fn set_body_type(&mut self, status: RigidBodyType) { - if status != self.rb_type { + if status != self.body_type { self.changes.insert(RigidBodyChanges::TYPE); - self.rb_type = status; + self.body_type = status; if status == RigidBodyType::Fixed { - self.rb_vels = RigidBodyVelocity::zero(); + self.vels = RigidBodyVelocity::zero(); } } } @@ -121,7 +121,7 @@ impl RigidBody { /// The mass properties of this rigid-body. #[inline] pub fn mass_properties(&self) -> &MassProperties { - &self.rb_mprops.local_mprops + &self.mprops.local_mprops } /// The dominance group of this rigid-body. @@ -130,18 +130,18 @@ impl RigidBody { /// rigid-bodies. #[inline] pub fn effective_dominance_group(&self) -> i16 { - self.rb_dominance.effective_group(&self.rb_type) + self.dominance.effective_group(&self.body_type) } /// Sets the axes along which this rigid-body cannot translate or rotate. #[inline] pub fn set_locked_axes(&mut self, locked_axes: LockedAxes, wake_up: bool) { - if locked_axes != self.rb_mprops.flags { + if locked_axes != self.mprops.flags { if self.is_dynamic() && wake_up { self.wake_up(true); } - self.rb_mprops.flags = locked_axes; + self.mprops.flags = locked_axes; self.update_world_mass_properties(); } } @@ -149,20 +149,14 @@ impl RigidBody { #[inline] /// Locks or unlocks all the rotations of this rigid-body. pub fn lock_rotations(&mut self, locked: bool, wake_up: bool) { - if !self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED) { + if !self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED) { if self.is_dynamic() && wake_up { self.wake_up(true); } - self.rb_mprops - .flags - .set(LockedAxes::ROTATION_LOCKED_X, locked); - self.rb_mprops - .flags - .set(LockedAxes::ROTATION_LOCKED_Y, locked); - self.rb_mprops - .flags - .set(LockedAxes::ROTATION_LOCKED_Z, locked); + self.mprops.flags.set(LockedAxes::ROTATION_LOCKED_X, locked); + self.mprops.flags.set(LockedAxes::ROTATION_LOCKED_Y, locked); + self.mprops.flags.set(LockedAxes::ROTATION_LOCKED_Z, locked); self.update_world_mass_properties(); } } @@ -176,21 +170,21 @@ impl RigidBody { allow_rotations_z: bool, wake_up: bool, ) { - if self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) != !allow_rotations_x - || self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) != !allow_rotations_y - || self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) != !allow_rotations_z + if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) != !allow_rotations_x + || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) != !allow_rotations_y + || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) != !allow_rotations_z { if self.is_dynamic() && wake_up { self.wake_up(true); } - self.rb_mprops + self.mprops .flags .set(LockedAxes::ROTATION_LOCKED_X, !allow_rotations_x); - self.rb_mprops + self.mprops .flags .set(LockedAxes::ROTATION_LOCKED_Y, !allow_rotations_y); - self.rb_mprops + self.mprops .flags .set(LockedAxes::ROTATION_LOCKED_Z, !allow_rotations_z); self.update_world_mass_properties(); @@ -200,16 +194,12 @@ impl RigidBody { #[inline] /// Locks or unlocks all the rotations of this rigid-body. pub fn lock_translations(&mut self, locked: bool, wake_up: bool) { - if !self - .rb_mprops - .flags - .contains(LockedAxes::TRANSLATION_LOCKED) - { + if !self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED) { if self.is_dynamic() && wake_up { self.wake_up(true); } - self.rb_mprops + self.mprops .flags .set(LockedAxes::TRANSLATION_LOCKED, locked); self.update_world_mass_properties(); @@ -226,36 +216,16 @@ impl RigidBody { wake_up: bool, ) { #[cfg(feature = "dim2")] - if self - .rb_mprops - .flags - .contains(LockedAxes::TRANSLATION_LOCKED_X) - == !allow_translation_x - && self - .rb_mprops - .flags - .contains(LockedAxes::TRANSLATION_LOCKED_Y) - == !allow_translation_y + if self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) == !allow_translation_x + && self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) == !allow_translation_y { // Nothing to change. return; } #[cfg(feature = "dim3")] - if self - .rb_mprops - .flags - .contains(LockedAxes::TRANSLATION_LOCKED_X) - == !allow_translation_x - && self - .rb_mprops - .flags - .contains(LockedAxes::TRANSLATION_LOCKED_Y) - == !allow_translation_y - && self - .rb_mprops - .flags - .contains(LockedAxes::TRANSLATION_LOCKED_Z) - == !allow_translation_z + if self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) == !allow_translation_x + && self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) == !allow_translation_y + && self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Z) == !allow_translation_z { // Nothing to change. return; @@ -265,14 +235,14 @@ impl RigidBody { self.wake_up(true); } - self.rb_mprops + self.mprops .flags .set(LockedAxes::TRANSLATION_LOCKED_X, !allow_translation_x); - self.rb_mprops + self.mprops .flags .set(LockedAxes::TRANSLATION_LOCKED_Y, !allow_translation_y); #[cfg(feature = "dim3")] - self.rb_mprops + self.mprops .flags .set(LockedAxes::TRANSLATION_LOCKED_Z, !allow_translation_z); self.update_world_mass_properties(); @@ -281,7 +251,7 @@ impl RigidBody { /// Are the translations of this rigid-body locked? #[cfg(feature = "dim2")] pub fn is_translation_locked(&self) -> bool { - self.rb_mprops + self.mprops .flags .contains(LockedAxes::TRANSLATION_LOCKED_X | LockedAxes::TRANSLATION_LOCKED_Y) } @@ -289,24 +259,22 @@ impl RigidBody { /// Are the translations of this rigid-body locked? #[cfg(feature = "dim3")] pub fn is_translation_locked(&self) -> bool { - self.rb_mprops - .flags - .contains(LockedAxes::TRANSLATION_LOCKED) + self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED) } /// Are the rotations of this rigid-body locked? #[cfg(feature = "dim2")] pub fn is_rotation_locked(&self) -> bool { - self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) + self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) } /// Returns `true` for each rotational degrees of freedom locked on this rigid-body. #[cfg(feature = "dim3")] pub fn is_rotation_locked(&self) -> [bool; 3] { [ - self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X), - self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y), - self.rb_mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z), + self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X), + self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y), + self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z), ] } @@ -314,12 +282,12 @@ impl RigidBody { /// /// CCD prevents tunneling, but may still allow limited interpenetration of colliders. pub fn enable_ccd(&mut self, enabled: bool) { - self.rb_ccd.ccd_enabled = enabled; + self.ccd.ccd_enabled = enabled; } /// Is CCD (continous collision-detection) enabled for this rigid-body? pub fn is_ccd_enabled(&self) -> bool { - self.rb_ccd.ccd_enabled + self.ccd.ccd_enabled } // This is different from `is_ccd_enabled`. This checks that CCD @@ -334,7 +302,7 @@ impl RigidBody { /// checks if CCD is allowed to run for this rigid-body or if /// it is completely disabled (independently from its velocity). pub fn is_ccd_active(&self) -> bool { - self.rb_ccd.ccd_active + self.ccd.ccd_active } /// Sets the rigid-body's initial mass properties. @@ -343,47 +311,47 @@ impl RigidBody { /// put to sleep because it did not move for a while. #[inline] pub fn set_mass_properties(&mut self, props: MassProperties, wake_up: bool) { - if self.rb_mprops.local_mprops != props { + if self.mprops.local_mprops != props { if self.is_dynamic() && wake_up { self.wake_up(true); } - self.rb_mprops.local_mprops = props; + self.mprops.local_mprops = props; self.update_world_mass_properties(); } } /// The handles of colliders attached to this rigid body. pub fn colliders(&self) -> &[ColliderHandle] { - &self.rb_colliders.0[..] + &self.colliders.0[..] } /// Is this rigid body dynamic? /// /// A dynamic body can move freely and is affected by forces. pub fn is_dynamic(&self) -> bool { - self.rb_type == RigidBodyType::Dynamic + self.body_type == RigidBodyType::Dynamic } /// Is this rigid body kinematic? /// /// A kinematic body can move freely but is not affected by forces. pub fn is_kinematic(&self) -> bool { - self.rb_type.is_kinematic() + self.body_type.is_kinematic() } /// Is this rigid body fixed? /// /// A fixed body cannot move and is not affected by forces. pub fn is_fixed(&self) -> bool { - self.rb_type == RigidBodyType::Fixed + self.body_type == RigidBodyType::Fixed } /// The mass of this rigid body. /// /// Returns zero if this rigid body has an infinite mass. pub fn mass(&self) -> Real { - self.rb_mprops.local_mprops.mass() + self.mprops.local_mprops.mass() } /// The predicted position of this rigid-body. @@ -392,36 +360,36 @@ impl RigidBody { /// method and is used for estimating the kinematic body velocity at the next timestep. /// For non-kinematic bodies, this value is currently unspecified. pub fn next_position(&self) -> &Isometry { - &self.rb_pos.next_position + &self.pos.next_position } /// The scale factor applied to the gravity affecting this rigid-body. pub fn gravity_scale(&self) -> Real { - self.rb_forces.gravity_scale + self.forces.gravity_scale } /// Sets the gravity scale facter for this rigid-body. pub fn set_gravity_scale(&mut self, scale: Real, wake_up: bool) { - if self.rb_forces.gravity_scale != scale { - if wake_up && self.rb_activation.sleeping { + if self.forces.gravity_scale != scale { + if wake_up && self.activation.sleeping { self.changes.insert(RigidBodyChanges::SLEEP); - self.rb_activation.sleeping = false; + self.activation.sleeping = false; } - self.rb_forces.gravity_scale = scale; + self.forces.gravity_scale = scale; } } /// The dominance group of this rigid-body. pub fn dominance_group(&self) -> i8 { - self.rb_dominance.0 + self.dominance.0 } /// The dominance group of this rigid-body. pub fn set_dominance_group(&mut self, dominance: i8) { - if self.rb_dominance.0 != dominance { + if self.dominance.0 != dominance { self.changes.insert(RigidBodyChanges::DOMINANCE); - self.rb_dominance.0 = dominance + self.dominance.0 = dominance } } @@ -435,11 +403,11 @@ impl RigidBody { co_shape: &ColliderShape, co_mprops: &ColliderMassProps, ) { - self.rb_colliders.attach_collider( + self.colliders.attach_collider( &mut self.changes, - &mut self.rb_ccd, - &mut self.rb_mprops, - &self.rb_pos, + &mut self.ccd, + &mut self.mprops, + &self.pos, co_handle, co_pos, co_parent, @@ -450,14 +418,14 @@ impl RigidBody { /// Removes a collider from this rigid-body. pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) { - if let Some(i) = self.rb_colliders.0.iter().position(|e| *e == handle) { + if let Some(i) = self.colliders.0.iter().position(|e| *e == handle) { self.changes.set(RigidBodyChanges::COLLIDERS, true); - self.rb_colliders.0.swap_remove(i); + self.colliders.0.swap_remove(i); let mass_properties = coll .mass_properties() .transform_by(coll.position_wrt_parent().unwrap()); - self.rb_mprops.local_mprops -= mass_properties; + self.mprops.local_mprops -= mass_properties; self.update_world_mass_properties(); } } @@ -468,8 +436,8 @@ impl RigidBody { /// it is waken up. It can be woken manually with `self.wake_up` or automatically due to /// external forces like contacts. pub fn sleep(&mut self) { - self.rb_activation.sleep(); - self.rb_vels = RigidBodyVelocity::zero(); + self.activation.sleep(); + self.vels = RigidBodyVelocity::zero(); } /// Wakes up this rigid body if it is sleeping. @@ -477,11 +445,11 @@ impl RigidBody { /// If `strong` is `true` then it is assured that the rigid-body will /// remain awake during multiple subsequent timesteps. pub fn wake_up(&mut self, strong: bool) { - if self.rb_activation.sleeping { + if self.activation.sleeping { self.changes.insert(RigidBodyChanges::SLEEP); } - self.rb_activation.wake_up(strong); + self.activation.wake_up(strong); } /// Is this rigid body sleeping? @@ -490,29 +458,29 @@ impl RigidBody { // - return false for fixed bodies. // - return true for non-sleeping dynamic bodies. // - return true only for kinematic bodies with non-zero velocity? - self.rb_activation.sleeping + self.activation.sleeping } /// Is the velocity of this body not zero? pub fn is_moving(&self) -> bool { - !self.rb_vels.linvel.is_zero() || !self.rb_vels.angvel.is_zero() + !self.vels.linvel.is_zero() || !self.vels.angvel.is_zero() } /// The linear velocity of this rigid-body. pub fn linvel(&self) -> &Vector { - &self.rb_vels.linvel + &self.vels.linvel } /// The angular velocity of this rigid-body. #[cfg(feature = "dim2")] pub fn angvel(&self) -> Real { - self.rb_vels.angvel + self.vels.angvel } /// The angular velocity of this rigid-body. #[cfg(feature = "dim3")] pub fn angvel(&self) -> &Vector { - &self.rb_vels.angvel + &self.vels.angvel } /// The linear velocity of this rigid-body. @@ -520,16 +488,16 @@ impl RigidBody { /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. pub fn set_linvel(&mut self, linvel: Vector, wake_up: bool) { - if self.rb_vels.linvel != linvel { - match self.rb_type { + if self.vels.linvel != linvel { + match self.body_type { RigidBodyType::Dynamic => { - self.rb_vels.linvel = linvel; + self.vels.linvel = linvel; if wake_up { self.wake_up(true) } } RigidBodyType::KinematicVelocityBased => { - self.rb_vels.linvel = linvel; + self.vels.linvel = linvel; } RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {} } @@ -542,16 +510,16 @@ impl RigidBody { /// put to sleep because it did not move for a while. #[cfg(feature = "dim2")] pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) { - if self.rb_vels.angvel != angvel { - match self.rb_type { + if self.vels.angvel != angvel { + match self.body_type { RigidBodyType::Dynamic => { - self.rb_vels.angvel = angvel; + self.vels.angvel = angvel; if wake_up { self.wake_up(true) } } RigidBodyType::KinematicVelocityBased => { - self.rb_vels.angvel = angvel; + self.vels.angvel = angvel; } RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {} } @@ -564,16 +532,16 @@ impl RigidBody { /// put to sleep because it did not move for a while. #[cfg(feature = "dim3")] pub fn set_angvel(&mut self, angvel: Vector, wake_up: bool) { - if self.rb_vels.angvel != angvel { - match self.rb_type { + if self.vels.angvel != angvel { + match self.body_type { RigidBodyType::Dynamic => { - self.rb_vels.angvel = angvel; + self.vels.angvel = angvel; if wake_up { self.wake_up(true) } } RigidBodyType::KinematicVelocityBased => { - self.rb_vels.angvel = angvel; + self.vels.angvel = angvel; } RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {} } @@ -583,24 +551,24 @@ impl RigidBody { /// The world-space position of this rigid-body. #[inline] pub fn position(&self) -> &Isometry { - &self.rb_pos.position + &self.pos.position } /// The translational part of this rigid-body's position. #[inline] pub fn translation(&self) -> &Vector { - &self.rb_pos.position.translation.vector + &self.pos.position.translation.vector } /// Sets the translational part of this rigid-body's position. #[inline] pub fn set_translation(&mut self, translation: Vector, wake_up: bool) { - if self.rb_pos.position.translation.vector != translation - || self.rb_pos.next_position.translation.vector != translation + if self.pos.position.translation.vector != translation + || self.pos.next_position.translation.vector != translation { self.changes.insert(RigidBodyChanges::POSITION); - self.rb_pos.position.translation.vector = translation; - self.rb_pos.next_position.translation.vector = translation; + self.pos.position.translation.vector = translation; + self.pos.next_position.translation.vector = translation; // TODO: Do we really need to check that the body isn't dynamic? if wake_up && self.is_dynamic() { @@ -612,7 +580,7 @@ impl RigidBody { /// The rotational part of this rigid-body's position. #[inline] pub fn rotation(&self) -> &Rotation { - &self.rb_pos.position.rotation + &self.pos.position.rotation } /// Sets the rotational part of this rigid-body's position. @@ -620,12 +588,10 @@ impl RigidBody { pub fn set_rotation(&mut self, rotation: AngVector, wake_up: bool) { let rotation = Rotation::new(rotation); - if self.rb_pos.position.rotation != rotation - || self.rb_pos.next_position.rotation != rotation - { + if self.pos.position.rotation != rotation || self.pos.next_position.rotation != rotation { self.changes.insert(RigidBodyChanges::POSITION); - self.rb_pos.position.rotation = rotation; - self.rb_pos.next_position.rotation = rotation; + self.pos.position.rotation = rotation; + self.pos.next_position.rotation = rotation; // TODO: Do we really need to check that the body isn't dynamic? if wake_up && self.is_dynamic() { @@ -644,10 +610,10 @@ impl RigidBody { /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. pub fn set_position(&mut self, pos: Isometry, wake_up: bool) { - if self.rb_pos.position != pos || self.rb_pos.next_position != pos { + if self.pos.position != pos || self.pos.next_position != pos { self.changes.insert(RigidBodyChanges::POSITION); - self.rb_pos.position = pos; - self.rb_pos.next_position = pos; + self.pos.position = pos; + self.pos.next_position = pos; // TODO: Do we really need to check that the body isn't dynamic? if wake_up && self.is_dynamic() { @@ -659,38 +625,33 @@ impl RigidBody { /// If this rigid body is kinematic, sets its future translation after the next timestep integration. pub fn set_next_kinematic_rotation(&mut self, rotation: AngVector) { if self.is_kinematic() { - self.rb_pos.next_position.rotation = Rotation::new(rotation); + self.pos.next_position.rotation = Rotation::new(rotation); } } /// If this rigid body is kinematic, sets its future orientation after the next timestep integration. pub fn set_next_kinematic_translation(&mut self, translation: Vector) { if self.is_kinematic() { - self.rb_pos.next_position.translation = translation.into(); + self.pos.next_position.translation = translation.into(); } } /// If this rigid body is kinematic, sets its future position after the next timestep integration. pub fn set_next_kinematic_position(&mut self, pos: Isometry) { if self.is_kinematic() { - self.rb_pos.next_position = pos; + self.pos.next_position = pos; } } /// Predicts the next position of this rigid-body, by integrating its velocity and forces /// by a time of `dt`. pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry { - self.rb_pos.integrate_forces_and_velocities( - dt, - &self.rb_forces, - &self.rb_vels, - &self.rb_mprops, - ) + self.pos + .integrate_forces_and_velocities(dt, &self.forces, &self.vels, &self.mprops) } pub(crate) fn update_world_mass_properties(&mut self) { - self.rb_mprops - .update_world_mass_properties(&self.rb_pos.position); + self.mprops.update_world_mass_properties(&self.pos.position); } } @@ -698,8 +659,8 @@ impl RigidBody { impl RigidBody { /// Resets to zero all the constant (linear) forces manually applied to this rigid-body. pub fn reset_forces(&mut self, wake_up: bool) { - if !self.rb_forces.user_force.is_zero() { - self.rb_forces.user_force = na::zero(); + if !self.forces.user_force.is_zero() { + self.forces.user_force = na::zero(); if wake_up { self.wake_up(true); @@ -709,8 +670,8 @@ impl RigidBody { /// Resets to zero all the constant torques manually applied to this rigid-body. pub fn reset_torques(&mut self, wake_up: bool) { - if !self.rb_forces.user_torque.is_zero() { - self.rb_forces.user_torque = na::zero(); + if !self.forces.user_torque.is_zero() { + self.forces.user_torque = na::zero(); if wake_up { self.wake_up(true); @@ -723,8 +684,8 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. pub fn add_force(&mut self, force: Vector, wake_up: bool) { if !force.is_zero() { - if self.rb_type == RigidBodyType::Dynamic { - self.rb_forces.user_force += force; + if self.body_type == RigidBodyType::Dynamic { + self.forces.user_force += force; if wake_up { self.wake_up(true); @@ -739,8 +700,8 @@ impl RigidBody { #[cfg(feature = "dim2")] pub fn add_torque(&mut self, torque: Real, wake_up: bool) { if !torque.is_zero() { - if self.rb_type == RigidBodyType::Dynamic { - self.rb_forces.user_torque += torque; + if self.body_type == RigidBodyType::Dynamic { + self.forces.user_torque += torque; if wake_up { self.wake_up(true); @@ -755,8 +716,8 @@ impl RigidBody { #[cfg(feature = "dim3")] pub fn add_torque(&mut self, torque: Vector, wake_up: bool) { if !torque.is_zero() { - if self.rb_type == RigidBodyType::Dynamic { - self.rb_forces.user_torque += torque; + if self.body_type == RigidBodyType::Dynamic { + self.forces.user_torque += torque; if wake_up { self.wake_up(true); @@ -770,9 +731,9 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. pub fn add_force_at_point(&mut self, force: Vector, point: Point, wake_up: bool) { if !force.is_zero() { - if self.rb_type == RigidBodyType::Dynamic { - self.rb_forces.user_force += force; - self.rb_forces.user_torque += (point - self.rb_mprops.world_com).gcross(force); + if self.body_type == RigidBodyType::Dynamic { + self.forces.user_force += force; + self.forces.user_torque += (point - self.mprops.world_com).gcross(force); if wake_up { self.wake_up(true); @@ -788,8 +749,8 @@ impl RigidBody { /// The impulse is applied right away, changing the linear velocity. /// This does nothing on non-dynamic bodies. pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { - if !impulse.is_zero() && self.rb_type == RigidBodyType::Dynamic { - self.rb_vels.linvel += impulse.component_mul(&self.rb_mprops.effective_inv_mass); + if !impulse.is_zero() && self.body_type == RigidBodyType::Dynamic { + self.vels.linvel += impulse.component_mul(&self.mprops.effective_inv_mass); if wake_up { self.wake_up(true); @@ -802,9 +763,9 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim2")] pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) { - if !torque_impulse.is_zero() && self.rb_type == RigidBodyType::Dynamic { - self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt - * (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse); + if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic { + self.vels.angvel += self.mprops.effective_world_inv_inertia_sqrt + * (self.mprops.effective_world_inv_inertia_sqrt * torque_impulse); if wake_up { self.wake_up(true); @@ -817,9 +778,9 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim3")] pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { - if !torque_impulse.is_zero() && self.rb_type == RigidBodyType::Dynamic { - self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt - * (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse); + if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic { + self.vels.angvel += self.mprops.effective_world_inv_inertia_sqrt + * (self.mprops.effective_world_inv_inertia_sqrt * torque_impulse); if wake_up { self.wake_up(true); @@ -836,7 +797,7 @@ impl RigidBody { point: Point, wake_up: bool, ) { - let torque_impulse = (point - self.rb_mprops.world_com).gcross(impulse); + let torque_impulse = (point - self.mprops.world_com).gcross(impulse); self.apply_impulse(impulse, wake_up); self.apply_torque_impulse(torque_impulse, wake_up); } @@ -845,28 +806,27 @@ impl RigidBody { impl RigidBody { /// The velocity of the given world-space point on this rigid-body. pub fn velocity_at_point(&self, point: &Point) -> Vector { - self.rb_vels - .velocity_at_point(point, &self.rb_mprops.world_com) + self.vels.velocity_at_point(point, &self.mprops.world_com) } /// The kinetic energy of this body. pub fn kinetic_energy(&self) -> Real { - self.rb_vels.kinetic_energy(&self.rb_mprops) + self.vels.kinetic_energy(&self.mprops) } /// The potential energy of this body in a gravity field. pub fn gravitational_potential_energy(&self, dt: Real, gravity: Vector) -> Real { let world_com = self - .rb_mprops + .mprops .local_mprops - .world_com(&self.rb_pos.position) + .world_com(&self.pos.position) .coords; // Project position back along velocity vector one half-step (leap-frog) // to sync up the potential energy with the kinetic energy: - let world_com = world_com - self.rb_vels.linvel * (dt / 2.0); + let world_com = world_com - self.vels.linvel * (dt / 2.0); - -self.mass() * self.rb_forces.gravity_scale * gravity.dot(&world_com) + -self.mass() * self.forces.gravity_scale * gravity.dot(&world_com) } } @@ -886,7 +846,7 @@ pub struct RigidBodyBuilder { pub linear_damping: Real, /// Damping factor for gradually slowing down the angular motion of the rigid-body, `0.0` by default. pub angular_damping: Real, - rb_type: RigidBodyType, + body_type: RigidBodyType, mprops_flags: LockedAxes, /// The additional mass properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information. pub additional_mass_properties: MassProperties, @@ -906,7 +866,7 @@ pub struct RigidBodyBuilder { impl RigidBodyBuilder { /// Initialize a new builder for a rigid body which is either fixed, dynamic, or kinematic. - pub fn new(rb_type: RigidBodyType) -> Self { + pub fn new(body_type: RigidBodyType) -> Self { Self { position: Isometry::identity(), linvel: Vector::zeros(), @@ -914,7 +874,7 @@ impl RigidBodyBuilder { gravity_scale: 1.0, linear_damping: 0.0, angular_damping: 0.0, - rb_type, + body_type, mprops_flags: LockedAxes::empty(), additional_mass_properties: MassProperties::zero(), can_sleep: true, @@ -1191,23 +1151,23 @@ impl RigidBodyBuilder { /// Build a new rigid-body with the parameters configured with this builder. pub fn build(&self) -> RigidBody { let mut rb = RigidBody::new(); - rb.rb_pos.next_position = self.position; // FIXME: compute the correct value? - rb.rb_pos.position = self.position; - rb.rb_vels.linvel = self.linvel; - rb.rb_vels.angvel = self.angvel; - rb.rb_type = self.rb_type; + rb.pos.next_position = self.position; // FIXME: compute the correct value? + rb.pos.position = self.position; + rb.vels.linvel = self.linvel; + rb.vels.angvel = self.angvel; + rb.body_type = self.body_type; rb.user_data = self.user_data; if self.additional_mass_properties != MassProperties::default() { - rb.rb_mprops.additional_local_mprops = Some(Box::new(self.additional_mass_properties)); - rb.rb_mprops.local_mprops = self.additional_mass_properties; + rb.mprops.additional_local_mprops = Some(Box::new(self.additional_mass_properties)); + rb.mprops.local_mprops = self.additional_mass_properties; } - rb.rb_mprops.flags = self.mprops_flags; - rb.rb_damping.linear_damping = self.linear_damping; - rb.rb_damping.angular_damping = self.angular_damping; - rb.rb_forces.gravity_scale = self.gravity_scale; - rb.rb_dominance = RigidBodyDominance(self.dominance_group); + rb.mprops.flags = self.mprops_flags; + rb.damping.linear_damping = self.linear_damping; + rb.damping.angular_damping = self.angular_damping; + rb.forces.gravity_scale = self.gravity_scale; + rb.dominance = RigidBodyDominance(self.dominance_group); rb.enable_ccd(self.ccd_enabled); if self.can_sleep && self.sleeping { @@ -1215,8 +1175,8 @@ impl RigidBodyBuilder { } if !self.can_sleep { - rb.rb_activation.linear_threshold = -1.0; - rb.rb_activation.angular_threshold = -1.0; + rb.activation.linear_threshold = -1.0; + rb.activation.angular_threshold = -1.0; } rb diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index b8f88ef..c7655e0 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -1,8 +1,7 @@ -use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; use crate::dynamics::MassProperties; use crate::geometry::{ ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, - ColliderShape, + ColliderSet, ColliderShape, }; use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, @@ -295,16 +294,12 @@ impl RigidBodyMassProps { } /// Recompute the mass-properties of this rigid-bodies based on its currentely attached colliders. - pub fn recompute_mass_properties_from_colliders( + pub fn recompute_mass_properties_from_colliders( &mut self, - colliders: &Colliders, + colliders: &ColliderSet, attached_colliders: &RigidBodyColliders, position: &Isometry, - ) where - Colliders: ComponentSetOption - + ComponentSet - + ComponentSet, - { + ) { self.local_mprops = self .additional_local_mprops .as_ref() @@ -312,14 +307,14 @@ impl RigidBodyMassProps { .unwrap_or_else(MassProperties::default); for handle in &attached_colliders.0 { - let co_parent: Option<&ColliderParent> = colliders.get(handle.0); - if let Some(co_parent) = co_parent { - let (co_mprops, co_shape): (&ColliderMassProps, &ColliderShape) = - colliders.index_bundle(handle.0); - let to_add = co_mprops - .mass_properties(&**co_shape) - .transform_by(&co_parent.pos_wrt_parent); - self.local_mprops += to_add; + if let Some(co) = colliders.get(handle) { + if let Some(co_parent) = co.parent { + let to_add = co + .mprops + .mass_properties(&**co.shape) + .transform_by(&co_parent.pos_wrt_parent); + self.local_mprops += to_add; + } } } @@ -892,16 +887,12 @@ impl RigidBodyColliders { } /// Update the positions of all the colliders attached to this rigid-body. - pub fn update_positions( + pub fn update_positions( &self, - colliders: &mut Colliders, + colliders: &mut ColliderSet, modified_colliders: &mut Vec, parent_pos: &Isometry, - ) where - Colliders: ComponentSetMut - + ComponentSetMut - + ComponentSetOption, - { + ) { for handle in &self.0 { // NOTE: the ColliderParent component must exist if we enter this method. let co_parent: &ColliderParent = colliders diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index a75d96d..62a6a54 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -1,11 +1,6 @@ -use crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption}; +use crate::data::Arena; use crate::dynamics::{ - ImpulseJointSet, RigidBody, RigidBodyCcd, RigidBodyChanges, RigidBodyDamping, RigidBodyForces, - RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, -}; -use crate::dynamics::{ - IslandManager, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, - RigidBodyHandle, RigidBodyType, + ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBody, RigidBodyChanges, RigidBodyHandle, }; use crate::geometry::ColliderSet; use std::ops::{Index, IndexMut}; @@ -39,59 +34,6 @@ pub struct RigidBodySet { pub(crate) modified_bodies: Vec, } -macro_rules! impl_field_component_set( - ($T: ty, $field: ident) => { - impl ComponentSetOption<$T> for RigidBodySet { - fn get(&self, handle: crate::data::Index) -> Option<&$T> { - self.get(RigidBodyHandle(handle)).map(|b| &b.$field) - } - } - - impl ComponentSet<$T> for RigidBodySet { - fn size_hint(&self) -> usize { - self.len() - } - - #[inline(always)] - fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) { - for (handle, body) in self.bodies.iter() { - f(handle, &body.$field) - } - } - } - - impl ComponentSetMut<$T> for RigidBodySet { - fn set_internal(&mut self, handle: crate::data::Index, val: $T) { - if let Some(rb) = self.get_mut_internal(RigidBodyHandle(handle)) { - rb.$field = val; - } - } - - #[inline(always)] - fn map_mut_internal( - &mut self, - handle: crate::data::Index, - f: impl FnOnce(&mut $T) -> Result, - ) -> Option { - self.get_mut_internal(RigidBodyHandle(handle)).map(|rb| f(&mut rb.$field)) - } - } - } -); - -impl_field_component_set!(RigidBodyPosition, rb_pos); -impl_field_component_set!(RigidBodyMassProps, rb_mprops); -impl_field_component_set!(RigidBodyVelocity, rb_vels); -impl_field_component_set!(RigidBodyDamping, rb_damping); -impl_field_component_set!(RigidBodyForces, rb_forces); -impl_field_component_set!(RigidBodyCcd, rb_ccd); -impl_field_component_set!(RigidBodyIds, rb_ids); -impl_field_component_set!(RigidBodyType, rb_type); -impl_field_component_set!(RigidBodyActivation, rb_activation); -impl_field_component_set!(RigidBodyColliders, rb_colliders); -impl_field_component_set!(RigidBodyDominance, rb_dominance); -impl_field_component_set!(RigidBodyChanges, changes); - impl RigidBodySet { /// Create a new empty set of rigid bodies. pub fn new() -> Self { @@ -147,7 +89,7 @@ impl RigidBodySet { /* * Update active sets. */ - islands.rigid_body_removed(handle, &rb.rb_ids, self); + islands.rigid_body_removed(handle, &rb.ids, self); /* * Remove colliders attached to this rigid-body. @@ -233,6 +175,10 @@ impl RigidBodySet { self.bodies.get_mut(handle.0) } + pub(crate) fn index_mut_internal(&mut self, handle: RigidBodyHandle) -> &mut RigidBody { + &mut self.bodies[handle.0] + } + // Just a very long name instead of `.get_mut` to make sure // this is really the method we wanted to use instead of `get_mut_internal`. pub(crate) fn get_mut_internal_with_modification_tracking( diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 0083388..06ba340 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -1,9 +1,8 @@ -use crate::data::ComponentSet; -use crate::dynamics::{JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyType}; +use crate::dynamics::{JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub(crate) fn categorize_contacts( - _bodies: &impl ComponentSet, // Unused but useful to simplify the parallel code. + _bodies: &RigidBodySet, // Unused but useful to simplify the parallel code. multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], @@ -40,7 +39,7 @@ pub(crate) fn categorize_contacts( } pub(crate) fn categorize_joints( - bodies: &impl ComponentSet, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, impulse_joints: &[JointGraphEdge], joint_indices: &[JointIndex], diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index de09a21..1be34bc 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -1,8 +1,7 @@ -use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::solver::{GenericRhs, VelocityConstraint}; use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType, - RigidBodyVelocity, + IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet, + RigidBodyType, RigidBodyVelocity, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; @@ -27,22 +26,17 @@ pub(crate) struct GenericVelocityConstraint { } impl GenericVelocityConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, out_constraints: &mut Vec, jacobians: &mut DVector, jacobian_id: &mut usize, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let inv_dt = params.inv_dt(); let erp_inv_dt = params.erp_inv_dt(); diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index 76e2e51..d953e6f 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -1,8 +1,6 @@ -use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::solver::VelocityGroundConstraint; use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType, - RigidBodyVelocity, + IntegrationParameters, MultibodyJointSet, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS}; @@ -25,22 +23,17 @@ pub(crate) struct GenericVelocityGroundConstraint { } impl GenericVelocityGroundConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, out_constraints: &mut Vec, jacobians: &mut DVector, jacobian_id: &mut usize, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let inv_dt = params.inv_dt(); let erp_inv_dt = params.erp_inv_dt(); diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 2bcbacc..951b77f 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -1,5 +1,4 @@ -use crate::data::ComponentSet; -use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds}; +use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] @@ -62,17 +61,15 @@ impl ParallelInteractionGroups { self.groups.len() - 1 } - pub fn group_interactions( + pub fn group_interactions( &mut self, island_id: usize, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, interactions: &[Interaction], interaction_indices: &[usize], - ) where - Bodies: ComponentSet + ComponentSet, - { + ) { let num_island_bodies = islands.active_island(island_id).len(); self.bodies_color.clear(); self.interaction_indices.clear(); @@ -217,7 +214,7 @@ impl InteractionGroups { &mut self, _island_id: usize, _islands: &IslandManager, - _bodies: &impl ComponentSet, + _bodies: &RigidBodySet, _interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], ) { @@ -226,16 +223,14 @@ impl InteractionGroups { } #[cfg(feature = "simd-is-enabled")] - pub fn group_joints( + pub fn group_joints( &mut self, island_id: usize, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], - ) where - Bodies: ComponentSet + ComponentSet, - { + ) { // TODO: right now, we only sort based on the axes locked by the joint. // We could also take motors and limits into account in the future (most of // the SIMD constraints generation for motors and limits is already implemented). @@ -376,7 +371,7 @@ impl InteractionGroups { &mut self, _island_id: usize, _islands: &IslandManager, - _bodies: &impl ComponentSet, + _bodies: &RigidBodySet, _interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], ) { @@ -385,16 +380,14 @@ impl InteractionGroups { } #[cfg(feature = "simd-is-enabled")] - pub fn group_manifolds( + pub fn group_manifolds( &mut self, island_id: usize, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], - ) where - Bodies: ComponentSet + ComponentSet, - { + ) { // Note: each bit of a body mask indicates what bucket already contains // a constraints involving this body. // TODO: currently, this is a bit overconservative because when a bucket diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 90c8d90..edd17f8 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -1,14 +1,10 @@ use super::VelocitySolver; use crate::counters::Counters; -use crate::data::{ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints, }; -use crate::dynamics::{ - IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces, - RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, -}; -use crate::dynamics::{IslandManager, RigidBodyVelocity}; +use crate::dynamics::IslandManager; +use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::prelude::MultibodyJointSet; @@ -33,27 +29,19 @@ impl IslandSolver { } } - pub fn init_and_solve( + pub fn init_and_solve( &mut self, island_id: usize, counters: &mut Counters, params: &IntegrationParameters, islands: &IslandManager, - bodies: &mut Bodies, + bodies: &mut RigidBodySet, manifolds: &mut [&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], impulse_joints: &mut [JointGraphEdge], joint_indices: &[JointIndex], multibody_joints: &mut MultibodyJointSet, - ) where - Bodies: ComponentSet - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { // Init the solver id for multibody_joints. // We need that for building the constraints. let mut solver_id = 0; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index e1150d5..a46744d 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -1,4 +1,3 @@ -use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{ JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint, }; @@ -8,7 +7,7 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds, - RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, + RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity, }; use crate::math::{Real, SPATIAL_DIM}; use crate::prelude::MultibodyJointSet; @@ -51,22 +50,17 @@ impl AnyJointVelocityConstraint { (num_constraints, num_constraints) } - pub fn from_joint( + pub fn from_joint( params: &IntegrationParameters, joint_id: JointIndex, joint: &ImpulseJoint, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, j_id: &mut usize, jacobians: &mut DVector, out: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let local_frame1 = joint.data.local_frame1; let local_frame2 = joint.data.local_frame2; let rb1: ( @@ -184,19 +178,14 @@ impl AnyJointVelocityConstraint { } #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint( + pub fn from_wide_joint( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], impulse_joints: [&ImpulseJoint; SIMD_WIDTH], - bodies: &Bodies, + bodies: &RigidBodySet, out: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let rbs1: ( [&RigidBodyPosition; SIMD_WIDTH], [&RigidBodyVelocity; SIMD_WIDTH], @@ -274,23 +263,17 @@ impl AnyJointVelocityConstraint { } } - pub fn from_joint_ground( + pub fn from_joint_ground( params: &IntegrationParameters, joint_id: JointIndex, joint: &ImpulseJoint, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, j_id: &mut usize, jacobians: &mut DVector, out: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let mut handle1 = joint.body1; let mut handle2 = joint.body2; let status2: &RigidBodyType = bodies.index(handle2.0); @@ -408,20 +391,14 @@ impl AnyJointVelocityConstraint { } #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint_ground( + pub fn from_wide_joint_ground( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], impulse_joints: [&ImpulseJoint; SIMD_WIDTH], - bodies: &Bodies, + bodies: &RigidBodySet, out: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let mut handles1 = gather![|ii| impulse_joints[ii].body1]; let mut handles2 = gather![|ii| impulse_joints[ii].body2]; let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 61b3fcb..261c627 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -2,7 +2,6 @@ use std::sync::atomic::{AtomicUsize, Ordering}; use rayon::Scope; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ AnyJointVelocityConstraint, AnyVelocityConstraint, ParallelSolverConstraints, }; @@ -162,27 +161,19 @@ impl ParallelIslandSolver { } } - pub fn init_and_solve<'s, Bodies>( + pub fn init_and_solve<'s>( &'s mut self, scope: &Scope<'s>, island_id: usize, islands: &'s IslandManager, params: &'s IntegrationParameters, - bodies: &'s mut Bodies, + bodies: &'s mut RigidBodySet, manifolds: &'s mut Vec<&'s mut ContactManifold>, manifold_indices: &'s [ContactManifoldIndex], impulse_joints: &'s mut Vec, joint_indices: &[JointIndex], multibodies: &mut MultibodyJointSet, - ) where - Bodies: ComponentSet - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let num_threads = rayon::current_num_threads(); let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island? self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here? @@ -288,7 +279,7 @@ impl ParallelIslandSolver { // Transmute *mut -> &mut let velocity_solver: &mut ParallelVelocitySolver = unsafe { std::mem::transmute(velocity_solver.load(Ordering::Relaxed)) }; - let bodies: &mut Bodies = + let bodies: &mut RigidBodySet = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; let multibodies: &mut MultibodyJointSet = unsafe { std::mem::transmute(multibodies.load(Ordering::Relaxed)) }; diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs index aadee48..d883494 100644 --- a/src/dynamics/solver/parallel_solver_constraints.rs +++ b/src/dynamics/solver/parallel_solver_constraints.rs @@ -1,6 +1,5 @@ use super::ParallelInteractionGroups; use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext}; -use crate::data::ComponentSet; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint; use crate::dynamics::solver::{ @@ -88,16 +87,16 @@ macro_rules! impl_init_constraints_group { $num_active_constraints_and_jacobian_lines: path, $empty_velocity_constraint: expr $(, $weight: ident)*) => { impl ParallelSolverConstraints<$VelocityConstraint> { - pub fn init_constraint_groups( + pub fn init_constraint_groups( &mut self, island_id: usize, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, interactions: &mut [$Interaction], interaction_groups: &ParallelInteractionGroups, j_id: &mut usize, - ) where Bodies: ComponentSet + ComponentSet { + ) { let mut total_num_constraints = 0; let num_groups = interaction_groups.num_groups(); @@ -316,20 +315,14 @@ impl_init_constraints_group!( ); impl ParallelSolverConstraints { - pub fn fill_constraints( + pub fn fill_constraints( &mut self, thread: &ThreadContext, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, manifolds_all: &[&mut ContactManifold], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let descs = &self.constraint_descs; crate::concurrent_loop! { @@ -372,20 +365,14 @@ impl ParallelSolverConstraints { } impl ParallelSolverConstraints { - pub fn fill_constraints( + pub fn fill_constraints( &mut self, thread: &ThreadContext, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, joints_all: &[JointGraphEdge], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let descs = &self.constraint_descs; crate::concurrent_loop! { diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index b913eef..5b062e0 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -1,6 +1,5 @@ use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext}; use crate::concurrent_loop; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::{ solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, @@ -26,27 +25,19 @@ impl ParallelVelocitySolver { } } - pub fn solve( + pub fn solve( &mut self, thread: &ThreadContext, params: &IntegrationParameters, island_id: usize, islands: &IslandManager, - bodies: &mut Bodies, + bodies: &mut RigidBodySet, multibodies: &mut MultibodyJointSet, manifolds_all: &mut [&mut ContactManifold], joints_all: &mut [JointGraphEdge], contact_constraints: &mut ParallelSolverConstraints, joint_constraints: &mut ParallelSolverConstraints, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet, - { + ) { let mut start_index = thread .solve_interaction_index .fetch_add(thread.batch_size, Ordering::SeqCst); diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 081ea9c..4006a85 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -3,15 +3,13 @@ use super::{ }; #[cfg(feature = "simd-is-enabled")] use super::{WVelocityConstraint, WVelocityGroundConstraint}; -use crate::data::ComponentSet; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::generic_velocity_ground_constraint::GenericVelocityGroundConstraint; use crate::dynamics::solver::GenericVelocityConstraint; use crate::dynamics::{ - solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, - MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, + solver::AnyVelocityConstraint, IntegrationParameters, IslandManager, JointGraphEdge, + JointIndex, MultibodyJointSet, RigidBodySet, }; -use crate::dynamics::{IslandManager, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::Real; #[cfg(feature = "simd-is-enabled")] @@ -55,17 +53,15 @@ impl SolverConstraints { } impl SolverConstraints { - pub fn init_constraint_groups( + pub fn init_constraint_groups( &mut self, island_id: usize, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - ) where - Bodies: ComponentSet + ComponentSet, - { + ) { self.not_ground_interactions.clear(); self.ground_interactions.clear(); self.generic_not_ground_interactions.clear(); @@ -109,22 +105,16 @@ impl SolverConstraints { // .append(&mut self.ground_interaction_groups.grouped_interactions); } - pub fn init( + pub fn init( &mut self, island_id: usize, params: &IntegrationParameters, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { self.velocity_constraints.clear(); self.init_constraint_groups( @@ -165,17 +155,12 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_constraints( + fn compute_grouped_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifolds_i in self .interaction_groups .grouped_interactions @@ -194,17 +179,12 @@ impl SolverConstraints { } } - fn compute_nongrouped_constraints( + fn compute_nongrouped_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifold_i in &self.interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; VelocityConstraint::generate( @@ -218,20 +198,14 @@ impl SolverConstraints { } } - fn compute_generic_constraints( + fn compute_generic_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, manifolds_all: &[&mut ContactManifold], jacobian_id: &mut usize, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifold_i in &self.generic_not_ground_interactions { let manifold = &manifolds_all[*manifold_i]; GenericVelocityConstraint::generate( @@ -248,20 +222,14 @@ impl SolverConstraints { } } - fn compute_generic_ground_constraints( + fn compute_generic_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, manifolds_all: &[&mut ContactManifold], jacobian_id: &mut usize, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifold_i in &self.generic_ground_interactions { let manifold = &manifolds_all[*manifold_i]; GenericVelocityGroundConstraint::generate( @@ -279,17 +247,12 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_ground_constraints( + fn compute_grouped_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifolds_i in self .ground_interaction_groups .grouped_interactions @@ -308,17 +271,12 @@ impl SolverConstraints { } } - fn compute_nongrouped_ground_constraints( + fn compute_nongrouped_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifold_i in &self.ground_interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; VelocityGroundConstraint::generate( @@ -334,22 +292,16 @@ impl SolverConstraints { } impl SolverConstraints { - pub fn init( + pub fn init( &mut self, island_id: usize, params: &IntegrationParameters, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, impulse_joints: &[JointGraphEdge], joint_constraint_indices: &[JointIndex], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { // Generate constraints for impulse_joints. self.not_ground_interactions.clear(); self.ground_interactions.clear(); @@ -464,19 +416,13 @@ impl SolverConstraints { } } - fn compute_nongrouped_joint_ground_constraints( + fn compute_nongrouped_joint_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, joints_all: &[JointGraphEdge], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let mut j_id = 0; for joint_i in &self.ground_interaction_groups.nongrouped_interactions { let joint = &joints_all[*joint_i].weight; @@ -495,18 +441,12 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_joint_ground_constraints( + fn compute_grouped_joint_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, joints_all: &[JointGraphEdge], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for joints_i in self .ground_interaction_groups .grouped_interactions @@ -525,19 +465,14 @@ impl SolverConstraints { } } - fn compute_nongrouped_joint_constraints( + fn compute_nongrouped_joint_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, joints_all: &[JointGraphEdge], j_id: &mut usize, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for joint_i in &self.interaction_groups.nongrouped_interactions { let joint = &joints_all[*joint_i].weight; AnyJointVelocityConstraint::from_joint( @@ -554,19 +489,14 @@ impl SolverConstraints { } } - fn compute_generic_joint_constraints( + fn compute_generic_joint_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, joints_all: &[JointGraphEdge], j_id: &mut usize, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for joint_i in &self.generic_not_ground_interactions { let joint = &joints_all[*joint_i].weight; AnyJointVelocityConstraint::from_joint( @@ -583,20 +513,14 @@ impl SolverConstraints { } } - fn compute_generic_ground_joint_constraints( + fn compute_generic_ground_joint_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, joints_all: &[JointGraphEdge], j_id: &mut usize, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for joint_i in &self.generic_ground_interactions { let joint = &joints_all[*joint_i].weight; AnyJointVelocityConstraint::from_joint_ground( @@ -614,17 +538,12 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_joint_constraints( + fn compute_grouped_joint_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, joints_all: &[JointGraphEdge], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for joints_i in self .interaction_groups .grouped_interactions diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index dc079d4..c854136 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -1,10 +1,11 @@ -use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::solver::{ GenericVelocityConstraint, GenericVelocityGroundConstraint, VelocityGroundConstraint, }; #[cfg(feature = "simd-is-enabled")] use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; -use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; +use crate::dynamics::{ + IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, +}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot}; @@ -144,18 +145,14 @@ impl VelocityConstraint { ) } - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, - bodies: &Bodies, + bodies: &RigidBodySet, out_constraints: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { assert_eq!(manifold.data.relative_dominance, 0); let inv_dt = params.inv_dt(); diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 0c85755..81c27fd 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -1,7 +1,6 @@ use super::{ AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart, }; -use crate::data::ComponentSet; use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ @@ -30,18 +29,14 @@ pub(crate) struct WVelocityConstraint { } impl WVelocityConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: [ContactManifoldIndex; SIMD_WIDTH], manifolds: [&ContactManifold; SIMD_WIDTH], - bodies: &Bodies, + bodies: &RigidBodySet, out_constraints: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for ii in 0..SIMD_WIDTH { assert_eq!(manifolds[ii].data.relative_dominance, 0); } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 93da6dc..0acc604 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -7,8 +7,9 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::WBasis; use crate::utils::{self, WAngularInertia, WCross, WDot}; -use crate::data::{BundleSet, ComponentSet}; -use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; +use crate::dynamics::{ + IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, +}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[derive(Copy, Clone, Debug)] @@ -27,18 +28,14 @@ pub(crate) struct VelocityGroundConstraint { } impl VelocityGroundConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, - bodies: &Bodies, + bodies: &RigidBodySet, out_constraints: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let inv_dt = params.inv_dt(); let erp_inv_dt = params.erp_inv_dt(); diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 1889901..4b3d429 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -2,7 +2,6 @@ use super::{ AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart, }; -use crate::data::ComponentSet; use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ @@ -29,18 +28,14 @@ pub(crate) struct WVelocityGroundConstraint { } impl WVelocityGroundConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: [ContactManifoldIndex; SIMD_WIDTH], manifolds: [&ContactManifold; SIMD_WIDTH], - bodies: &Bodies, + bodies: &RigidBodySet, out_constraints: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let inv_dt = SimdReal::splat(params.inv_dt()); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 66d7caf..a0d494e 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,9 +1,8 @@ use super::AnyJointVelocityConstraint; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping, - RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, + RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyVelocity, }; use crate::geometry::ContactManifold; @@ -24,12 +23,12 @@ impl VelocitySolver { } } - pub fn solve( + pub fn solve( &mut self, island_id: usize, params: &IntegrationParameters, islands: &IslandManager, - bodies: &mut Bodies, + bodies: &mut RigidBodySet, multibodies: &mut MultibodyJointSet, manifolds_all: &mut [&mut ContactManifold], joints_all: &mut [JointGraphEdge], @@ -37,15 +36,7 @@ impl VelocitySolver { generic_contact_jacobians: &DVector, joint_constraints: &mut [AnyJointVelocityConstraint], generic_joint_jacobians: &DVector, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet, - { + ) { let cfm_factor = params.cfm_factor(); self.mj_lambdas.clear(); self.mj_lambdas diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs index e70affb..7afc671 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -3,15 +3,14 @@ use super::{ }; use crate::geometry::broad_phase_multi_sap::SAPProxyIndex; use crate::geometry::{ - ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderShape, + ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet, + ColliderShape, }; use crate::math::Real; use crate::utils::IndexMut2; use parry::bounding_volume::BoundingVolume; use parry::utils::hashmap::HashMap; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; - /// A broad-phase combining a Hierarchical Grid and Sweep-and-Prune. /// /// The basic Sweep-and-Prune (SAP) algorithm has one significant flaws: @@ -435,19 +434,14 @@ impl BroadPhase { } /// Updates the broad-phase, taking into account the new collider positions. - pub fn update( + pub fn update( &mut self, prediction_distance: Real, - colliders: &mut Colliders, + colliders: &mut ColliderSet, modified_colliders: &[ColliderHandle], removed_colliders: &[ColliderHandle], events: &mut Vec, - ) where - Colliders: ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { // Phase 1: pre-delete the collisions that have been deleted. self.handle_removed_colliders(removed_colliders); @@ -457,30 +451,22 @@ impl BroadPhase { for handle in modified_colliders { // NOTE: we use `get` because the collider may no longer // exist if it has been removed. - let co_changes: Option<&ColliderChanges> = colliders.get(handle.0); - - if let Some(co_changes) = co_changes { - let (co_bf_data, co_pos, co_shape): ( - &ColliderBroadPhaseData, - &ColliderPosition, - &ColliderShape, - ) = colliders.index_bundle(handle.0); - - if !co_changes.needs_broad_phase_update() { + if let Some(co) = colliders.get(*handle) { + if !co.changes.needs_broad_phase_update() { continue; } - let mut new_proxy_id = co_bf_data.proxy_index; + let mut new_proxy_id = co.bf_data.proxy_index; if self.handle_modified_collider( prediction_distance, *handle, &mut new_proxy_id, - (co_pos, co_shape, co_changes), + (&co.pos, &co.shape, &co.changes), ) { need_region_propagation = true; } - if co_bf_data.proxy_index != new_proxy_id { + if co.bf_data.proxy_index != new_proxy_id { self.colliders_proxy_ids.insert(*handle, new_proxy_id); // Make sure we have the new proxy index in case diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 3113e5e..8d9b005 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -17,118 +17,118 @@ use parry::shape::Shape; /// /// To build a new collider, use the `ColliderBuilder` structure. pub struct Collider { - pub(crate) co_type: ColliderType, - pub(crate) co_shape: ColliderShape, - pub(crate) co_mprops: ColliderMassProps, - pub(crate) co_changes: ColliderChanges, - pub(crate) co_parent: Option, - pub(crate) co_pos: ColliderPosition, - pub(crate) co_material: ColliderMaterial, - pub(crate) co_flags: ColliderFlags, - pub(crate) co_bf_data: ColliderBroadPhaseData, + pub(crate) coll_type: ColliderType, + pub(crate) shape: ColliderShape, + pub(crate) mprops: ColliderMassProps, + pub(crate) changes: ColliderChanges, + pub(crate) parent: Option, + pub(crate) pos: ColliderPosition, + pub(crate) material: ColliderMaterial, + pub(crate) flags: ColliderFlags, + pub(crate) bf_data: ColliderBroadPhaseData, /// User-defined data associated to this collider. pub user_data: u128, } impl Collider { pub(crate) fn reset_internal_references(&mut self) { - self.co_bf_data.proxy_index = crate::INVALID_U32; - self.co_changes = ColliderChanges::all(); + self.bf_data.proxy_index = crate::INVALID_U32; + self.changes = ColliderChanges::all(); } /// The rigid body this collider is attached to. pub fn parent(&self) -> Option { - self.co_parent.map(|parent| parent.handle) + self.parent.map(|parent| parent.handle) } /// Is this collider a sensor? pub fn is_sensor(&self) -> bool { - self.co_type.is_sensor() + self.coll_type.is_sensor() } /// The physics hooks enabled for this collider. pub fn active_hooks(&self) -> ActiveHooks { - self.co_flags.active_hooks + self.flags.active_hooks } /// Sets the physics hooks enabled for this collider. pub fn set_active_hooks(&mut self, active_hooks: ActiveHooks) { - self.co_flags.active_hooks = active_hooks; + self.flags.active_hooks = active_hooks; } /// The events enabled for this collider. pub fn active_events(&self) -> ActiveEvents { - self.co_flags.active_events + self.flags.active_events } /// Sets the events enabled for this collider. pub fn set_active_events(&mut self, active_events: ActiveEvents) { - self.co_flags.active_events = active_events; + self.flags.active_events = active_events; } /// The collision types enabled for this collider. pub fn active_collision_types(&self) -> ActiveCollisionTypes { - self.co_flags.active_collision_types + self.flags.active_collision_types } /// Sets the collision types enabled for this collider. pub fn set_active_collision_types(&mut self, active_collision_types: ActiveCollisionTypes) { - self.co_flags.active_collision_types = active_collision_types; + self.flags.active_collision_types = active_collision_types; } /// The friction coefficient of this collider. pub fn friction(&self) -> Real { - self.co_material.friction + self.material.friction } /// Sets the friction coefficient of this collider. pub fn set_friction(&mut self, coefficient: Real) { - self.co_material.friction = coefficient + self.material.friction = coefficient } /// The combine rule used by this collider to combine its friction /// coefficient with the friction coefficient of the other collider it /// is in contact with. pub fn friction_combine_rule(&self) -> CoefficientCombineRule { - self.co_material.friction_combine_rule + self.material.friction_combine_rule } /// Sets the combine rule used by this collider to combine its friction /// coefficient with the friction coefficient of the other collider it /// is in contact with. pub fn set_friction_combine_rule(&mut self, rule: CoefficientCombineRule) { - self.co_material.friction_combine_rule = rule; + self.material.friction_combine_rule = rule; } /// The restitution coefficient of this collider. pub fn restitution(&self) -> Real { - self.co_material.restitution + self.material.restitution } /// Sets the restitution coefficient of this collider. pub fn set_restitution(&mut self, coefficient: Real) { - self.co_material.restitution = coefficient + self.material.restitution = coefficient } /// The combine rule used by this collider to combine its restitution /// coefficient with the restitution coefficient of the other collider it /// is in contact with. pub fn restitution_combine_rule(&self) -> CoefficientCombineRule { - self.co_material.restitution_combine_rule + self.material.restitution_combine_rule } /// Sets the combine rule used by this collider to combine its restitution /// coefficient with the restitution coefficient of the other collider it /// is in contact with. pub fn set_restitution_combine_rule(&mut self, rule: CoefficientCombineRule) { - self.co_material.restitution_combine_rule = rule; + self.material.restitution_combine_rule = rule; } /// Sets whether or not this is a sensor collider. pub fn set_sensor(&mut self, is_sensor: bool) { if is_sensor != self.is_sensor() { - self.co_changes.insert(ColliderChanges::TYPE); - self.co_type = if is_sensor { + self.changes.insert(ColliderChanges::TYPE); + self.coll_type = if is_sensor { ColliderType::Sensor } else { ColliderType::Solid @@ -138,55 +138,55 @@ impl Collider { /// Sets the translational part of this collider's position. pub fn set_translation(&mut self, translation: Vector) { - self.co_changes.insert(ColliderChanges::POSITION); - self.co_pos.0.translation.vector = translation; + self.changes.insert(ColliderChanges::POSITION); + self.pos.0.translation.vector = translation; } /// Sets the rotational part of this collider's position. pub fn set_rotation(&mut self, rotation: AngVector) { - self.co_changes.insert(ColliderChanges::POSITION); - self.co_pos.0.rotation = Rotation::new(rotation); + self.changes.insert(ColliderChanges::POSITION); + self.pos.0.rotation = Rotation::new(rotation); } /// Sets the position of this collider. pub fn set_position(&mut self, position: Isometry) { - self.co_changes.insert(ColliderChanges::POSITION); - self.co_pos.0 = position; + self.changes.insert(ColliderChanges::POSITION); + self.pos.0 = position; } /// The world-space position of this collider. pub fn position(&self) -> &Isometry { - &self.co_pos + &self.pos } /// The translational part of this collider's position. pub fn translation(&self) -> &Vector { - &self.co_pos.0.translation.vector + &self.pos.0.translation.vector } /// The rotational part of this collider's position. pub fn rotation(&self) -> &Rotation { - &self.co_pos.0.rotation + &self.pos.0.rotation } /// The position of this collider wrt the body it is attached to. pub fn position_wrt_parent(&self) -> Option<&Isometry> { - self.co_parent.as_ref().map(|p| &p.pos_wrt_parent) + self.parent.as_ref().map(|p| &p.pos_wrt_parent) } /// Sets the translational part of this collider's translation relative to its parent rigid-body. pub fn set_translation_wrt_parent(&mut self, translation: Vector) { - if let Some(co_parent) = self.co_parent.as_mut() { - self.co_changes.insert(ColliderChanges::PARENT); - co_parent.pos_wrt_parent.translation.vector = translation; + if let Some(parent) = self.parent.as_mut() { + self.changes.insert(ColliderChanges::PARENT); + parent.pos_wrt_parent.translation.vector = translation; } } /// Sets the rotational part of this collider's rotaiton relative to its parent rigid-body. pub fn set_rotation_wrt_parent(&mut self, rotation: AngVector) { - if let Some(co_parent) = self.co_parent.as_mut() { - self.co_changes.insert(ColliderChanges::PARENT); - co_parent.pos_wrt_parent.rotation = Rotation::new(rotation); + if let Some(parent) = self.parent.as_mut() { + self.changes.insert(ColliderChanges::PARENT); + parent.pos_wrt_parent.rotation = Rotation::new(rotation); } } @@ -194,46 +194,46 @@ impl Collider { /// /// Does nothing if the collider is not attached to a rigid-body. pub fn set_position_wrt_parent(&mut self, pos_wrt_parent: Isometry) { - if let Some(co_parent) = self.co_parent.as_mut() { - self.co_changes.insert(ColliderChanges::PARENT); - co_parent.pos_wrt_parent = pos_wrt_parent; + if let Some(parent) = self.parent.as_mut() { + self.changes.insert(ColliderChanges::PARENT); + parent.pos_wrt_parent = pos_wrt_parent; } } /// The collision groups used by this collider. pub fn collision_groups(&self) -> InteractionGroups { - self.co_flags.collision_groups + self.flags.collision_groups } /// Sets the collision groups of this collider. pub fn set_collision_groups(&mut self, groups: InteractionGroups) { - if self.co_flags.collision_groups != groups { - self.co_changes.insert(ColliderChanges::GROUPS); - self.co_flags.collision_groups = groups; + if self.flags.collision_groups != groups { + self.changes.insert(ColliderChanges::GROUPS); + self.flags.collision_groups = groups; } } /// The solver groups used by this collider. pub fn solver_groups(&self) -> InteractionGroups { - self.co_flags.solver_groups + self.flags.solver_groups } /// Sets the solver groups of this collider. pub fn set_solver_groups(&mut self, groups: InteractionGroups) { - if self.co_flags.solver_groups != groups { - self.co_changes.insert(ColliderChanges::GROUPS); - self.co_flags.solver_groups = groups; + if self.flags.solver_groups != groups { + self.changes.insert(ColliderChanges::GROUPS); + self.flags.solver_groups = groups; } } /// The material (friction and restitution properties) of this collider. pub fn material(&self) -> &ColliderMaterial { - &self.co_material + &self.material } /// The density of this collider, if set. pub fn density(&self) -> Option { - match &self.co_mprops { + match &self.mprops { ColliderMassProps::Density(density) => Some(*density), ColliderMassProps::MassProperties(_) => None, } @@ -241,7 +241,7 @@ impl Collider { /// The geometric shape of this collider. pub fn shape(&self) -> &dyn Shape { - self.co_shape.as_ref() + self.shape.as_ref() } /// A mutable reference to the geometric shape of this collider. @@ -250,37 +250,36 @@ impl Collider { /// cloned first so that `self` contains a unique copy of that /// shape that you can modify. pub fn shape_mut(&mut self) -> &mut dyn Shape { - self.co_changes.insert(ColliderChanges::SHAPE); - self.co_shape.make_mut() + self.changes.insert(ColliderChanges::SHAPE); + self.shape.make_mut() } /// Sets the shape of this collider. pub fn set_shape(&mut self, shape: SharedShape) { - self.co_changes.insert(ColliderChanges::SHAPE); - self.co_shape = shape; + self.changes.insert(ColliderChanges::SHAPE); + self.shape = shape; } /// Retrieve the SharedShape. Also see the `shape()` function pub fn shared_shape(&self) -> &SharedShape { - &self.co_shape + &self.shape } /// Compute the axis-aligned bounding box of this collider. pub fn compute_aabb(&self) -> AABB { - self.co_shape.compute_aabb(&self.co_pos) + self.shape.compute_aabb(&self.pos) } /// Compute the axis-aligned bounding box of this collider moving from its current position /// to the given `next_position` pub fn compute_swept_aabb(&self, next_position: &Isometry) -> AABB { - self.co_shape - .compute_swept_aabb(&self.co_pos, next_position) + self.shape.compute_swept_aabb(&self.pos, next_position) } /// Compute the local-space mass properties of this collider. pub fn mass_properties(&self) -> MassProperties { - match &self.co_mprops { - ColliderMassProps::Density(density) => self.co_shape.mass_properties(*density), + match &self.mprops { + ColliderMassProps::Density(density) => self.shape.mass_properties(*density), ColliderMassProps::MassProperties(mass_properties) => **mass_properties, } } @@ -726,18 +725,17 @@ impl ColliderBuilder { /// Builds a new collider attached to the given rigid-body. pub fn build(&self) -> Collider { - let (co_changes, co_pos, co_bf_data, co_shape, co_type, co_material, co_flags, co_mprops) = - self.components(); + let (changes, pos, bf_data, shape, coll_type, material, flags, mprops) = self.components(); Collider { - co_shape, - co_mprops, - co_material, - co_parent: None, - co_changes, - co_pos, - co_bf_data, - co_flags, - co_type, + shape, + mprops, + material, + parent: None, + changes, + pos, + bf_data, + flags, + coll_type, user_data: self.user_data, } } @@ -763,39 +761,32 @@ impl ColliderBuilder { ColliderMassProps::Density(density) }; - let co_shape = self.shape.clone(); - let co_mprops = mass_info; - let co_material = ColliderMaterial { + let shape = self.shape.clone(); + let mprops = mass_info; + let material = ColliderMaterial { friction: self.friction, restitution: self.restitution, friction_combine_rule: self.friction_combine_rule, restitution_combine_rule: self.restitution_combine_rule, }; - let co_flags = ColliderFlags { + let flags = ColliderFlags { collision_groups: self.collision_groups, solver_groups: self.solver_groups, active_collision_types: self.active_collision_types, active_hooks: self.active_hooks, active_events: self.active_events, }; - let co_changes = ColliderChanges::all(); - let co_pos = ColliderPosition(self.position); - let co_bf_data = ColliderBroadPhaseData::default(); - let co_type = if self.is_sensor { + let changes = ColliderChanges::all(); + let pos = ColliderPosition(self.position); + let bf_data = ColliderBroadPhaseData::default(); + let coll_type = if self.is_sensor { ColliderType::Sensor } else { ColliderType::Solid }; ( - co_changes, - co_pos, - co_bf_data, - co_shape, - co_type, - co_material, - co_flags, - co_mprops, + changes, pos, bf_data, shape, coll_type, material, flags, mprops, ) } } diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index a25fd1e..de182e7 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -1,11 +1,6 @@ use crate::data::arena::Arena; -use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption}; use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet}; -use crate::geometry::{ - Collider, ColliderBroadPhaseData, ColliderFlags, ColliderMassProps, ColliderMaterial, - ColliderParent, ColliderPosition, ColliderShape, ColliderType, -}; -use crate::geometry::{ColliderChanges, ColliderHandle}; +use crate::geometry::{Collider, ColliderChanges, ColliderHandle, ColliderParent}; use crate::math::Isometry; use std::ops::{Index, IndexMut}; @@ -18,63 +13,6 @@ pub struct ColliderSet { pub(crate) removed_colliders: Vec, } -macro_rules! impl_field_component_set( - ($T: ty, $field: ident) => { - impl ComponentSetOption<$T> for ColliderSet { - fn get(&self, handle: crate::data::Index) -> Option<&$T> { - self.get(ColliderHandle(handle)).map(|b| &b.$field) - } - } - - impl ComponentSet<$T> for ColliderSet { - fn size_hint(&self) -> usize { - self.len() - } - - #[inline(always)] - fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) { - for (handle, body) in self.colliders.iter() { - f(handle, &body.$field) - } - } - } - - impl ComponentSetMut<$T> for ColliderSet { - fn set_internal(&mut self, handle: crate::data::Index, val: $T) { - if let Some(rb) = self.get_mut_internal(ColliderHandle(handle)) { - rb.$field = val; - } - } - - #[inline(always)] - fn map_mut_internal( - &mut self, - handle: crate::data::Index, - f: impl FnOnce(&mut $T) -> Result, - ) -> Option { - self.get_mut_internal(ColliderHandle(handle)).map(|rb| f(&mut rb.$field)) - } - } - } -); - -impl_field_component_set!(ColliderType, co_type); -impl_field_component_set!(ColliderShape, co_shape); -impl_field_component_set!(ColliderMassProps, co_mprops); -impl_field_component_set!(ColliderChanges, co_changes); -impl_field_component_set!(ColliderPosition, co_pos); -impl_field_component_set!(ColliderMaterial, co_material); -impl_field_component_set!(ColliderFlags, co_flags); -impl_field_component_set!(ColliderBroadPhaseData, co_bf_data); - -impl ComponentSetOption for ColliderSet { - #[inline(always)] - fn get(&self, handle: crate::data::Index) -> Option<&ColliderParent> { - self.get(ColliderHandle(handle)) - .and_then(|b| b.co_parent.as_ref()) - } -} - impl ColliderSet { /// Create a new empty set of colliders. pub fn new() -> Self { diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index d737bfc..26bddf1 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -1,16 +1,16 @@ #[cfg(feature = "parallel")] use rayon::prelude::*; -use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut, ComponentSetOption}; -use crate::dynamics::CoefficientCombineRule; +use crate::data::Coarena; use crate::dynamics::{ - IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds, RigidBodyType, + CoefficientCombineRule, IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds, + RigidBodySet, RigidBodyType, }; use crate::geometry::{ BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderMaterial, - ColliderPair, ColliderParent, ColliderPosition, ColliderShape, ColliderType, CollisionEvent, - ContactData, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph, - IntersectionPair, SolverContact, SolverFlags, + ColliderPair, ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, + CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair, + InteractionGraph, IntersectionPair, SolverContact, SolverFlags, }; use crate::math::{Real, Vector}; use crate::pipeline::{ @@ -250,23 +250,15 @@ impl NarrowPhase { // } /// Maintain the narrow-phase internal state by taking collider removal into account. - pub fn handle_user_changes( + pub fn handle_user_changes( &mut self, mut islands: Option<&mut IslandManager>, modified_colliders: &[ColliderHandle], removed_colliders: &[ColliderHandle], - colliders: &mut Colliders, - bodies: &mut Bodies, + colliders: &mut ColliderSet, + bodies: &mut RigidBodySet, events: &dyn EventHandler, - ) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - Colliders: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSetOption, - { + ) { // TODO: avoid these hash-maps. // They are necessary to handle the swap-remove done internally // by the contact/intersection graphs when a node is removed. @@ -305,22 +297,17 @@ impl NarrowPhase { self.handle_modified_colliders(islands, modified_colliders, colliders, bodies, events); } - pub(crate) fn remove_collider( + pub(crate) fn remove_collider( &mut self, intersection_graph_id: ColliderGraphIndex, contact_graph_id: ColliderGraphIndex, mut islands: Option<&mut IslandManager>, - colliders: &mut Colliders, - bodies: &mut Bodies, + colliders: &mut ColliderSet, + bodies: &mut RigidBodySet, prox_id_remap: &mut HashMap, contact_id_remap: &mut HashMap, events: &dyn EventHandler, - ) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - Colliders: ComponentSetOption, - { + ) { // Wake up every body in contact with the deleted collider and generate Stopped collision events. if let Some(islands) = islands.as_deref_mut() { for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) { @@ -379,22 +366,14 @@ impl NarrowPhase { } } - pub(crate) fn handle_modified_colliders( + pub(crate) fn handle_modified_colliders( &mut self, mut islands: Option<&mut IslandManager>, modified_colliders: &[ColliderHandle], - colliders: &Colliders, - bodies: &mut Bodies, + colliders: &ColliderSet, + bodies: &mut RigidBodySet, events: &dyn EventHandler, - ) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - Colliders: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSetOption, - { + ) { let mut pairs_to_remove = vec![]; for handle in modified_colliders { @@ -496,22 +475,15 @@ impl NarrowPhase { } } - fn remove_pair( + fn remove_pair( &mut self, islands: Option<&mut IslandManager>, - colliders: &Colliders, - bodies: &mut Bodies, + colliders: &ColliderSet, + bodies: &mut RigidBodySet, pair: &ColliderPair, events: &dyn EventHandler, mode: PairRemovalMode, - ) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut, - Colliders: ComponentSet - + ComponentSet - + ComponentSetOption, - { + ) { let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0); let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0); @@ -582,10 +554,7 @@ impl NarrowPhase { } } - fn add_pair(&mut self, colliders: &Colliders, pair: &ColliderPair) - where - Colliders: ComponentSet + ComponentSetOption, - { + fn add_pair(&mut self, colliders: &ColliderSet, pair: &ColliderPair) { let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0); let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0); @@ -666,21 +635,14 @@ impl NarrowPhase { } } - pub(crate) fn register_pairs( + pub(crate) fn register_pairs( &mut self, mut islands: Option<&mut IslandManager>, - colliders: &Colliders, - bodies: &mut Bodies, + colliders: &ColliderSet, + bodies: &mut RigidBodySet, broad_phase_events: &[BroadPhasePairEvent], events: &dyn EventHandler, - ) where - Bodies: ComponentSetMut - + ComponentSetMut - + ComponentSet, - Colliders: ComponentSet - + ComponentSet - + ComponentSetOption, - { + ) { for event in broad_phase_events { match event { BroadPhasePairEvent::AddPair(pair) => { @@ -700,24 +662,14 @@ impl NarrowPhase { } } - pub(crate) fn compute_intersections( + pub(crate) fn compute_intersections( &mut self, - bodies: &Bodies, - colliders: &Colliders, + bodies: &RigidBodySet, + colliders: &ColliderSet, modified_colliders: &[ColliderHandle], - hooks: &dyn PhysicsHooks, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - Colliders: ComponentSet - + ComponentSetOption - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { if modified_colliders.is_empty() { return; } @@ -824,25 +776,15 @@ impl NarrowPhase { }); } - pub(crate) fn compute_contacts( + pub(crate) fn compute_contacts( &mut self, prediction_distance: Real, - bodies: &Bodies, - colliders: &Colliders, + bodies: &RigidBodySet, + colliders: &ColliderSet, modified_colliders: &[ColliderHandle], - hooks: &dyn PhysicsHooks, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - Colliders: ComponentSet - + ComponentSetOption - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { if modified_colliders.is_empty() { return; } @@ -1057,17 +999,13 @@ impl NarrowPhase { /// Retrieve all the interactions with at least one contact point, happening between two active bodies. // NOTE: this is very similar to the code from ImpulseJointSet::select_active_interactions. - pub(crate) fn select_active_contacts<'a, Bodies>( + pub(crate) fn select_active_contacts<'a>( &'a mut self, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, out_manifolds: &mut Vec<&'a mut ContactManifold>, out: &mut Vec>, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for out_island in &mut out[..islands.num_islands()] { out_island.clear(); } diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 009dfa9..48a11e7 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,14 +1,8 @@ //! Physics pipeline structures. -use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption}; -use crate::dynamics::{ - RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle, - RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, -}; +use crate::dynamics::RigidBodyHandle; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderFlags, - ColliderHandle, ColliderMassProps, ColliderMaterial, ColliderPair, ColliderParent, - ColliderPosition, ColliderShape, ColliderType, NarrowPhase, + BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase, }; use crate::math::Real; use crate::pipeline::{EventHandler, PhysicsHooks}; @@ -48,32 +42,19 @@ impl CollisionPipeline { } } - fn detect_collisions( + fn detect_collisions( &mut self, prediction_distance: Real, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, - bodies: &mut Bodies, - colliders: &mut Colliders, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, modified_colliders: &[ColliderHandle], removed_colliders: &[ColliderHandle], - hooks: &dyn PhysicsHooks, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, handle_user_changes: bool, - ) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut - + ComponentSet, - Colliders: ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSetOption - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { // Update broad-phase. self.broad_phase_events.clear(); self.broadphase_collider_pairs.clear(); @@ -112,7 +93,7 @@ impl CollisionPipeline { fn clear_modified_colliders( &mut self, - colliders: &mut impl ComponentSetMut, + colliders: &mut ColliderSet, modified_colliders: &mut Vec, ) { for handle in modified_colliders.drain(..) { @@ -129,7 +110,7 @@ impl CollisionPipeline { narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - hooks: &dyn PhysicsHooks, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { let mut modified_bodies = bodies.take_modified(); @@ -151,38 +132,19 @@ impl CollisionPipeline { } /// Executes one step of the collision detection. - pub fn step_generic( + pub fn step_generic( &mut self, prediction_distance: Real, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, - bodies: &mut Bodies, - colliders: &mut Colliders, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, modified_bodies: &mut Vec, modified_colliders: &mut Vec, removed_colliders: &mut Vec, - hooks: &dyn PhysicsHooks, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, - ) where - Bodies: ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet, - Colliders: ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSetOption - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { super::user_changes::handle_user_changes_to_colliders( bodies, colliders, diff --git a/src/pipeline/physics_hooks.rs b/src/pipeline/physics_hooks.rs index 68a05d1..4024237 100644 --- a/src/pipeline/physics_hooks.rs +++ b/src/pipeline/physics_hooks.rs @@ -1,14 +1,14 @@ -use crate::dynamics::RigidBodyHandle; -use crate::geometry::{ColliderHandle, ContactManifold, SolverContact, SolverFlags}; +use crate::dynamics::{RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, SolverContact, SolverFlags}; use crate::math::{Real, Vector}; use na::ComplexField; /// Context given to custom collision filters to filter-out collisions. -pub struct PairFilterContext<'a, Bodies, Colliders> { +pub struct PairFilterContext<'a> { /// The set of rigid-bodies. - pub bodies: &'a Bodies, + pub bodies: &'a RigidBodySet, /// The set of colliders. - pub colliders: &'a Colliders, + pub colliders: &'a ColliderSet, /// The handle of the first collider involved in the potential collision. pub collider1: ColliderHandle, /// The handle of the first collider involved in the potential collision. @@ -20,11 +20,11 @@ pub struct PairFilterContext<'a, Bodies, Colliders> { } /// Context given to custom contact modifiers to modify the contacts seen by the constraints solver. -pub struct ContactModificationContext<'a, Bodies, Colliders> { +pub struct ContactModificationContext<'a> { /// The set of rigid-bodies. - pub bodies: &'a Bodies, + pub bodies: &'a RigidBodySet, /// The set of colliders. - pub colliders: &'a Colliders, + pub colliders: &'a ColliderSet, /// The handle of the first collider involved in the potential collision. pub collider1: ColliderHandle, /// The handle of the first collider involved in the potential collision. @@ -45,7 +45,7 @@ pub struct ContactModificationContext<'a, Bodies, Colliders> { pub user_data: &'a mut u32, } -impl<'a, Bodies, Colliders> ContactModificationContext<'a, Bodies, Colliders> { +impl<'a> ContactModificationContext<'a> { /// Helper function to update `self` to emulate a oneway-platform. /// /// The "oneway" behavior will only allow contacts between two colliders @@ -139,28 +139,24 @@ impl Default for ActiveHooks { // not having Send+Sync isn't a problem. /// User-defined functions called by the physics engines during one timestep in order to customize its behavior. #[cfg(target_arch = "wasm32")] -pub trait PhysicsHooks { +pub trait PhysicsHooks { /// Applies the contact pair filter. - fn filter_contact_pair( - &self, - _context: &PairFilterContext, - ) -> Option { + fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option { None } /// Applies the intersection pair filter. - fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool { + fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool { false } /// Modifies the set of contacts seen by the constraints solver. - fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) { - } + fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {} } /// User-defined functions called by the physics engines during one timestep in order to customize its behavior. #[cfg(not(target_arch = "wasm32"))] -pub trait PhysicsHooks: Send + Sync { +pub trait PhysicsHooks: Send + Sync { /// Applies the contact pair filter. /// /// Note that this method will only be called if at least one of the colliders @@ -185,10 +181,7 @@ pub trait PhysicsHooks: Send + Sync { /// will be taken into account by the constraints solver. If this returns /// `Some(SolverFlags::empty())` then the constraints solver will ignore these /// contacts. - fn filter_contact_pair( - &self, - _context: &PairFilterContext, - ) -> Option { + fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option { Some(SolverFlags::COMPUTE_IMPULSES) } @@ -212,7 +205,7 @@ pub trait PhysicsHooks: Send + Sync { /// not compute any intersection information for it. /// If this return `true` then the narrow-phase will compute intersection /// information for this pair. - fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool { + fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool { true } @@ -241,21 +234,17 @@ pub trait PhysicsHooks: Send + Sync { /// as 0 and can be modified in `context.user_data`. /// /// The world-space contact normal can be modified in `context.normal`. - fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) { - } + fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {} } -impl PhysicsHooks for () { - fn filter_contact_pair( - &self, - _context: &PairFilterContext, - ) -> Option { +impl PhysicsHooks for () { + fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option { Some(SolverFlags::default()) } - fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool { + fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool { true } - fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {} + fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {} } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 2d90c4b..ffcb25f 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -1,21 +1,18 @@ //! Physics pipeline structures. use crate::counters::Counters; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; #[cfg(not(feature = "parallel"))] use crate::dynamics::IslandSolver; use crate::dynamics::{ CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, - RigidBodyActivation, RigidBodyCcd, RigidBodyChanges, RigidBodyColliders, RigidBodyDamping, - RigidBodyDominance, RigidBodyForces, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, - RigidBodyPosition, RigidBodyType, RigidBodyVelocity, + RigidBodyColliders, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, + RigidBodyType, RigidBodyVelocity, }; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderFlags, - ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition, - ColliderShape, ColliderType, ContactManifoldIndex, NarrowPhase, ColliderMassProps + BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, + ContactManifoldIndex, NarrowPhase, }; use crate::math::{Real, Vector}; use crate::pipeline::{EventHandler, PhysicsHooks}; @@ -71,7 +68,7 @@ impl PhysicsPipeline { fn clear_modified_colliders( &mut self, - colliders: &mut impl ComponentSetMut, + colliders: &mut ColliderSet, modified_colliders: &mut Vec, ) { for handle in modified_colliders.drain(..) { @@ -79,33 +76,20 @@ impl PhysicsPipeline { } } - fn detect_collisions( + fn detect_collisions( &mut self, integration_parameters: &IntegrationParameters, islands: &mut IslandManager, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, - bodies: &mut Bodies, - colliders: &mut Colliders, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, modified_colliders: &[ColliderHandle], removed_colliders: &[ColliderHandle], - hooks: &dyn PhysicsHooks, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, handle_user_changes: bool, - ) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut - + ComponentSet, - Colliders: ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSetOption - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { self.counters.stages.collision_detection_time.resume(); self.counters.cd.broad_phase_time.resume(); @@ -155,28 +139,17 @@ impl PhysicsPipeline { self.counters.stages.collision_detection_time.pause(); } - fn build_islands_and_solve_velocity_constraints( + fn build_islands_and_solve_velocity_constraints( &mut self, gravity: &Vector, integration_parameters: &IntegrationParameters, islands: &mut IslandManager, narrow_phase: &mut NarrowPhase, - bodies: &mut Bodies, - colliders: &mut Colliders, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, - ) where - Bodies: ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet, - Colliders: ComponentSetOption, - { + ) { self.counters.stages.island_construction_time.resume(); islands.update_active_set_with_contacts( integration_parameters.dt, @@ -285,7 +258,7 @@ impl PhysicsPipeline { .par_iter_mut() .enumerate() .for_each(|(island_id, solver)| { - let bodies: &mut Bodies = + let bodies: &mut RigidBodySet = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; let manifolds: &mut Vec<&mut ContactManifold> = unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) }; @@ -313,28 +286,16 @@ impl PhysicsPipeline { self.counters.stages.solver_time.pause(); } - fn run_ccd_motion_clamping( + fn run_ccd_motion_clamping( &mut self, integration_parameters: &IntegrationParameters, islands: &IslandManager, - bodies: &mut Bodies, - colliders: &mut Colliders, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, narrow_phase: &NarrowPhase, ccd_solver: &mut CCDSolver, events: &dyn EventHandler, - ) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - Colliders: ComponentSetOption - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { self.counters.ccd.toi_computation_time.start(); // Handle CCD let impacts = ccd_solver.predict_impacts_at_next_positions( @@ -349,22 +310,13 @@ impl PhysicsPipeline { self.counters.ccd.toi_computation_time.pause(); } - fn advance_to_final_positions( + fn advance_to_final_positions( &mut self, islands: &IslandManager, - bodies: &mut Bodies, - colliders: &mut Colliders, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, modified_colliders: &mut Vec, - ) where - Bodies: ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet, - Colliders: ComponentSetMut - + ComponentSetMut - + ComponentSetOption, - { + ) { // Set the rigid-bodies and kinematic bodies to their final position. for handle in islands.iter_active_bodies() { bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| { @@ -377,17 +329,12 @@ impl PhysicsPipeline { } } - fn interpolate_kinematic_velocities( + fn interpolate_kinematic_velocities( &mut self, integration_parameters: &IntegrationParameters, islands: &IslandManager, - bodies: &mut Bodies, - ) where - Bodies: ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet, - { + bodies: &mut RigidBodySet, + ) { // Update kinematic bodies velocities. // TODO: what is the best place for this? It should at least be // located before the island computation because we test the velocity @@ -440,7 +387,7 @@ impl PhysicsPipeline { impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, ccd_solver: &mut CCDSolver, - hooks: &dyn PhysicsHooks, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { let mut modified_bodies = bodies.take_modified(); @@ -467,46 +414,24 @@ impl PhysicsPipeline { } /// Executes one timestep of the physics simulation. - pub fn step_generic( + pub fn step_generic( &mut self, gravity: &Vector, integration_parameters: &IntegrationParameters, islands: &mut IslandManager, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, - bodies: &mut Bodies, - colliders: &mut Colliders, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, modified_bodies: &mut Vec, modified_colliders: &mut Vec, removed_colliders: &mut Vec, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, ccd_solver: &mut CCDSolver, - hooks: &dyn PhysicsHooks, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, - ) where - Bodies: ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - Colliders: ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSetOption - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { self.counters.reset(); self.counters.step_started(); diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index a5565cd..584989e 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,4 +1,3 @@ -use crate::data::{BundleSet, ComponentSet, ComponentSetOption}; use crate::dynamics::{ IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, @@ -40,9 +39,9 @@ pub struct QueryPipeline { dilation_factor: Real, } -struct QueryPipelineAsCompositeShape<'a, Colliders> { +struct QueryPipelineAsCompositeShape<'a> { query_pipeline: &'a QueryPipeline, - colliders: &'a Colliders, + colliders: &'a ColliderSet, query_groups: InteractionGroups, filter: Option<&'a dyn Fn(ColliderHandle) -> bool>, } @@ -63,12 +62,7 @@ pub enum QueryPipelineMode { }, } -impl<'a, Colliders> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a, Colliders> -where - // TODO ECS: make everything optional but the shape? - Colliders: - ComponentSet + ComponentSet + ComponentSet, -{ +impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { type PartShape = dyn Shape; type PartId = ColliderHandle; @@ -77,15 +71,11 @@ where shape_id: Self::PartId, mut f: impl FnMut(Option<&Isometry>, &Self::PartShape), ) { - let co_flags: Option<&ColliderFlags> = self.colliders.get(shape_id.0); - - if let Some(co_flags) = co_flags { - if co_flags.collision_groups.test(self.query_groups) + if let Some(co) = self.colliders.get(shape_id) { + if co.flags.collision_groups.test(self.query_groups) && self.filter.map(|f| f(shape_id)).unwrap_or(true) { - let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = - self.colliders.index_bundle(shape_id.0); - f(Some(co_pos), &**co_shape) + f(Some(co.pos), &**co.shape) } } } @@ -115,12 +105,12 @@ impl QueryPipeline { Self::with_query_dispatcher(DefaultQueryDispatcher) } - fn as_composite_shape<'a, Colliders>( + fn as_composite_shape<'a>( &'a self, - colliders: &'a Colliders, + colliders: &'a ColliderSet, query_groups: InteractionGroups, filter: Option<&'a dyn Fn(ColliderHandle) -> bool>, - ) -> QueryPipelineAsCompositeShape<'a, Colliders> { + ) -> QueryPipelineAsCompositeShape<'a> { QueryPipelineAsCompositeShape { query_pipeline: self, colliders, @@ -162,21 +152,12 @@ impl QueryPipeline { } /// Update the acceleration structure on the query pipeline. - pub fn update_generic( + pub fn update_generic( &mut self, islands: &IslandManager, - bodies: &Bodies, - colliders: &Colliders, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - Colliders: ComponentSet - + ComponentSet - + ComponentSetOption, - { + bodies: &RigidBodySet, + colliders: &ColliderSet, + ) { self.update_with_mode( islands, bodies, @@ -186,40 +167,22 @@ impl QueryPipeline { } /// Update the acceleration structure on the query pipeline. - pub fn update_with_mode( + pub fn update_with_mode( &mut self, islands: &IslandManager, - bodies: &Bodies, - colliders: &Colliders, + bodies: &RigidBodySet, + colliders: &ColliderSet, mode: QueryPipelineMode, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - Colliders: ComponentSet - + ComponentSet - + ComponentSetOption, - { - struct DataGenerator<'a, Bs, Cs> { - bodies: &'a Bs, - colliders: &'a Cs, + ) { + struct DataGenerator<'a> { + bodies: &'a RigidBodySet, + colliders: &'a ColliderSet, mode: QueryPipelineMode, } - impl<'a, Bs, Cs> QBVHDataGenerator for DataGenerator<'a, Bs, Cs> - where - Bs: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - Cs: ComponentSet - + ComponentSet - + ComponentSetOption, - { + impl<'a> QBVHDataGenerator for DataGenerator<'a> { fn size_hint(&self) -> usize { - ComponentSet::::size_hint(self.colliders) + self.colliders.len() } #[inline(always)] @@ -313,16 +276,13 @@ impl QueryPipeline { QueryPipelineMode::SweepTestWithNextPosition => { self.qbvh.update( |handle| { - let co_parent: Option<&ColliderParent> = colliders.get(handle.0); - let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = - colliders.index_bundle(handle.0); - - if let Some(co_parent) = co_parent { - let rb_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0); - let next_position = rb_pos.next_position * co_parent.pos_wrt_parent; - co_shape.compute_swept_aabb(&co_pos, &next_position) + let co = &colliders[handle]; + if let Some(parent) = co.parent { + let rb_pos: &RigidBodyPosition = bodies.index(co.parent.handle.0); + let next_position = rb_pos.next_position * co.parent.pos_wrt_parent; + co.shape.compute_swept_aabb(&co.pos, &next_position) } else { - co_shape.compute_aabb(&co_pos) + co.shape.compute_aabb(&co.pos) } }, self.dilation_factor, @@ -331,25 +291,17 @@ impl QueryPipeline { QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { self.qbvh.update( |handle| { - let co_parent: Option<&ColliderParent> = colliders.get(handle.0); - let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = - colliders.index_bundle(handle.0); + let co = &colliders[handle]; + if let Some(parent) = co.parent { + let rb = &bodies[parent.handle]; + let predicted_pos = rb + .pos + .integrate_forces_and_velocities(dt, rb.forces, rb.vels, rb.mprops); - if let Some(co_parent) = co_parent { - let (rb_pos, vels, forces, mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyForces, - &RigidBodyMassProps, - ) = bodies.index_bundle(co_parent.handle.0); - - let predicted_pos = - rb_pos.integrate_forces_and_velocities(dt, forces, vels, mprops); - - let next_position = predicted_pos * co_parent.pos_wrt_parent; - co_shape.compute_swept_aabb(&co_pos, &next_position) + let next_position = predicted_pos * parent.pos_wrt_parent; + co.shape.compute_swept_aabb(&co.pos, &next_position) } else { - co_shape.compute_aabb(&co_pos) + co.shape.compute_aabb(&co.pos) } }, self.dilation_factor, @@ -373,20 +325,15 @@ impl QueryPipeline { /// * `filter`: a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn cast_ray( + pub fn cast_ray( &self, - colliders: &Colliders, + colliders: &ColliderSet, ray: &Ray, max_toi: Real, solid: bool, query_groups: InteractionGroups, filter: Option<&dyn Fn(ColliderHandle) -> bool>, - ) -> Option<(ColliderHandle, Real)> - where - Colliders: ComponentSet - + ComponentSet - + ComponentSet, - { + ) -> Option<(ColliderHandle, Real)> { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid); @@ -409,20 +356,15 @@ impl QueryPipeline { /// * `filter`: a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn cast_ray_and_get_normal( + pub fn cast_ray_and_get_normal( &self, - colliders: &Colliders, + colliders: &ColliderSet, ray: &Ray, max_toi: Real, solid: bool, query_groups: InteractionGroups, filter: Option<&dyn Fn(ColliderHandle) -> bool>, - ) -> Option<(ColliderHandle, RayIntersection)> - where - Colliders: ComponentSet - + ComponentSet - + ComponentSet, - { + ) -> Option<(ColliderHandle, RayIntersection)> { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new( &pipeline_shape, @@ -452,20 +394,16 @@ impl QueryPipeline { /// * `callback`: function executed on each collider for which a ray intersection has been found. /// There is no guarantees on the order the results will be yielded. If this callback returns `false`, /// this method will exit early, ignore any further raycast. - pub fn intersections_with_ray<'a, Colliders>( + pub fn intersections_with_ray<'a, ColliderSet>( &self, - colliders: &'a Colliders, + colliders: &'a ColliderSet, ray: &Ray, max_toi: Real, solid: bool, query_groups: InteractionGroups, filter: Option<&dyn Fn(ColliderHandle) -> bool>, mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool, - ) where - Colliders: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let mut leaf_callback = &mut |handle: &ColliderHandle| { let co_shape: Option<&ColliderShape> = colliders.get(handle.0); if let Some(co_shape) = co_shape { @@ -499,19 +437,14 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn intersection_with_shape( + pub fn intersection_with_shape( &self, - colliders: &Colliders, + colliders: &ColliderSet, shape_pos: &Isometry, shape: &dyn Shape, query_groups: InteractionGroups, filter: Option<&dyn Fn(ColliderHandle) -> bool>, - ) -> Option - where - Colliders: ComponentSet - + ComponentSet - + ComponentSet, - { + ) -> Option { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = IntersectionCompositeShapeShapeBestFirstVisitor::new( &*self.query_dispatcher, @@ -540,19 +473,14 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn project_point( + pub fn project_point( &self, - colliders: &Colliders, + colliders: &ColliderSet, point: &Point, solid: bool, query_groups: InteractionGroups, filter: Option<&dyn Fn(ColliderHandle) -> bool>, - ) -> Option<(ColliderHandle, PointProjection)> - where - Colliders: ComponentSet - + ComponentSet - + ComponentSet, - { + ) -> Option<(ColliderHandle, PointProjection)> { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid); @@ -574,18 +502,14 @@ impl QueryPipeline { /// is either `None` or returns `true`. /// * `callback` - A function called with each collider with a shape /// containing the `point`. - pub fn intersections_with_point<'a, Colliders>( + pub fn intersections_with_point<'a, ColliderSet>( &self, - colliders: &'a Colliders, + colliders: &'a ColliderSet, point: &Point, query_groups: InteractionGroups, filter: Option<&dyn Fn(ColliderHandle) -> bool>, mut callback: impl FnMut(ColliderHandle) -> bool, - ) where - Colliders: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let mut leaf_callback = &mut |handle: &ColliderHandle| { let co_shape: Option<&ColliderShape> = colliders.get(handle.0); @@ -626,18 +550,13 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn project_point_and_get_feature( + pub fn project_point_and_get_feature( &self, - colliders: &Colliders, + colliders: &ColliderSet, point: &Point, query_groups: InteractionGroups, filter: Option<&dyn Fn(ColliderHandle) -> bool>, - ) -> Option<(ColliderHandle, PointProjection, FeatureId)> - where - Colliders: ComponentSet - + ComponentSet - + ComponentSet, - { + ) -> Option<(ColliderHandle, PointProjection, FeatureId)> { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false); @@ -674,21 +593,16 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn cast_shape<'a, Colliders>( + pub fn cast_shape<'a>( &self, - colliders: &'a Colliders, + colliders: &'a ColliderSet, shape_pos: &Isometry, shape_vel: &Vector, shape: &dyn Shape, max_toi: Real, query_groups: InteractionGroups, filter: Option<&dyn Fn(ColliderHandle) -> bool>, - ) -> Option<(ColliderHandle, TOI)> - where - Colliders: ComponentSet - + ComponentSet - + ComponentSet, - { + ) -> Option<(ColliderHandle, TOI)> { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new( &*self.query_dispatcher, @@ -724,9 +638,9 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn nonlinear_cast_shape( + pub fn nonlinear_cast_shape( &self, - colliders: &Colliders, + colliders: &ColliderSet, shape_motion: &NonlinearRigidMotion, shape: &dyn Shape, start_time: Real, @@ -734,12 +648,7 @@ impl QueryPipeline { stop_at_penetration: bool, query_groups: InteractionGroups, filter: Option<&dyn Fn(ColliderHandle) -> bool>, - ) -> Option<(ColliderHandle, TOI)> - where - Colliders: ComponentSet - + ComponentSet - + ComponentSet, - { + ) -> Option<(ColliderHandle, TOI)> { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let pipeline_motion = NonlinearRigidMotion::identity(); let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new( @@ -768,19 +677,15 @@ impl QueryPipeline { /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. /// * `callback` - A function called with the handles of each collider intersecting the `shape`. - pub fn intersections_with_shape<'a, Colliders>( + pub fn intersections_with_shape<'a, ColliderSet>( &self, - colliders: &'a Colliders, + colliders: &'a ColliderSet, shape_pos: &Isometry, shape: &dyn Shape, query_groups: InteractionGroups, filter: Option<&dyn Fn(ColliderHandle) -> bool>, mut callback: impl FnMut(ColliderHandle) -> bool, - ) where - Colliders: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let dispatcher = &*self.query_dispatcher; let inv_shape_pos = shape_pos.inverse(); diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index 2c03f1c..b999f0f 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -1,28 +1,15 @@ -use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; use crate::dynamics::{ - IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyHandle, - RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, -}; -use crate::geometry::{ - ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, - ColliderShape, + IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyHandle, RigidBodyIds, + RigidBodyPosition, RigidBodySet, RigidBodyType, }; +use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet}; use parry::utils::hashmap::HashMap; -pub(crate) fn handle_user_changes_to_colliders( - bodies: &mut Bodies, - colliders: &mut Colliders, +pub(crate) fn handle_user_changes_to_colliders( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, modified_colliders: &[ColliderHandle], -) where - Bodies: ComponentSet - + ComponentSet - + ComponentSetMut, - Colliders: ComponentSetMut - + ComponentSetMut - + ComponentSetOption - + ComponentSet - + ComponentSet, -{ +) { // TODO: avoid this hashmap? We could perhaps add a new flag to RigidBodyChanges to // indicated that the mass properties need to be recomputed? let mut mprops_to_update = HashMap::default(); @@ -30,25 +17,20 @@ pub(crate) fn handle_user_changes_to_colliders( for handle in modified_colliders { // NOTE: we use `get` because the collider may no longer // exist if it has been removed. - let co_changes: Option = colliders.get(handle.0).copied(); - - if let Some(co_changes) = co_changes { - if co_changes.contains(ColliderChanges::PARENT) { - let co_parent: Option<&ColliderParent> = colliders.get(handle.0); - - if let Some(co_parent) = co_parent { + if let Some(co) = colliders.get(*handle) { + if co.changes.contains(ColliderChanges::PARENT) { + if let Some(co_parent) = co.parent { let parent_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0); let new_pos = parent_pos.position * co_parent.pos_wrt_parent; - let new_changes = co_changes | ColliderChanges::POSITION; + let new_changes = co.changes | ColliderChanges::POSITION; colliders.set_internal(handle.0, ColliderPosition(new_pos)); colliders.set_internal(handle.0, new_changes); } } - if co_changes.contains(ColliderChanges::SHAPE) { - let co_parent: Option<&ColliderParent> = colliders.get(handle.0); - if let Some(co_parent) = co_parent { + if co.changes.contains(ColliderChanges::SHAPE) { + if let Some(co_parent) = co.parent { mprops_to_update.insert(co_parent.handle, ()); } } @@ -56,11 +38,10 @@ pub(crate) fn handle_user_changes_to_colliders( } for (to_update, _) in mprops_to_update { - let (rb_pos, rb_colliders): (&RigidBodyPosition, &RigidBodyColliders) = - bodies.index_bundle(to_update.0); - let position = rb_pos.position; + let rb = &bodies[to_update]; + let position = rb.position(); // FIXME: remove the clone once we remove the ComponentSets. - let attached_colliders = rb_colliders.clone(); + let attached_colliders = rb.colliders().clone(); bodies.map_mut_internal(to_update.0, |rb_mprops| { rb_mprops.recompute_mass_properties_from_colliders( @@ -72,23 +53,13 @@ pub(crate) fn handle_user_changes_to_colliders( } } -pub(crate) fn handle_user_changes_to_rigid_bodies( +pub(crate) fn handle_user_changes_to_rigid_bodies( mut islands: Option<&mut IslandManager>, - bodies: &mut Bodies, - colliders: &mut Colliders, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, modified_bodies: &[RigidBodyHandle], modified_colliders: &mut Vec, -) where - Bodies: ComponentSetMut - + ComponentSet - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet, - Colliders: ComponentSetMut - + ComponentSetMut - + ComponentSetOption, -{ +) { enum FinalAction { UpdateActiveKinematicSetId, UpdateActiveDynamicSetId, @@ -96,28 +67,23 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( for handle in modified_bodies { let mut final_action = None; - let changes: Option<&RigidBodyChanges> = bodies.get(handle.0); - if changes.is_none() { + if !bodies.contains(*handle) { // The body no longer exists. continue; } - let mut changes = *changes.unwrap(); - let mut ids: RigidBodyIds = *bodies.index(handle.0); - let mut activation: RigidBodyActivation = *bodies.index(handle.0); - let (status, rb_colliders, poss): ( - &RigidBodyType, - &RigidBodyColliders, - &RigidBodyPosition, - ) = bodies.index_bundle(handle.0); + let rb = &bodies[handle]; + let mut changes = rb.changes; + let mut ids: RigidBodyIds = rb.ids; + let mut activation: RigidBodyActivation = rb.activation; { // The body's status changed. We need to make sure // it is on the correct active set. if let Some(islands) = islands.as_deref_mut() { if changes.contains(RigidBodyChanges::TYPE) { - match status { + match rb.status { RigidBodyType::Dynamic => { // Remove from the active kinematic set if it was there. if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) { @@ -161,9 +127,10 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( if changes.contains(RigidBodyChanges::POSITION) || changes.contains(RigidBodyChanges::COLLIDERS) { - rb_colliders.update_positions(colliders, modified_colliders, &poss.position); + rb.colliders + .update_positions(colliders, modified_colliders, &rb.pos.position); - if status.is_kinematic() + if rb.is_kinematic() && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) { ids.active_set_id = islands.active_kinematic_set.len(); @@ -175,7 +142,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( // sleeping and if it is not already inside of the active set. if changes.contains(RigidBodyChanges::SLEEP) && !activation.sleeping // May happen if the body was put to sleep manually. - && status.is_dynamic() // Only dynamic bodies are in the active dynamic set. + && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set. && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle) { ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates. @@ -186,14 +153,15 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( if changes.contains(RigidBodyChanges::POSITION) || changes.contains(RigidBodyChanges::COLLIDERS) { - rb_colliders.update_positions(colliders, modified_colliders, &poss.position); + rb.colliders + .update_positions(colliders, modified_colliders, &rb.pos.position); } } if changes.contains(RigidBodyChanges::DOMINANCE) || changes.contains(RigidBodyChanges::TYPE) { - for handle in rb_colliders.0.iter() { + for handle in rb.colliders.0.iter() { colliders.map_mut_internal(handle.0, |co_changes: &mut ColliderChanges| { if !co_changes.contains(ColliderChanges::MODIFIED) { modified_colliders.push(*handle); diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index bed8f85..b026075 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -268,7 +268,7 @@ fn serialization_string(timestep_id: usize, physics: &PhysicsState) -> String { Hashes at frame: {} |_ Broad phase [{:.1}KB]: {} |_ Narrow phase [{:.1}KB]: {} -|_ Bodies [{:.1}KB]: {} +|_ &RigidBodySet [{:.1}KB]: {} |_ Colliders [{:.1}KB]: {} |_ Joints [{:.1}KB]: {}"#, serialization_time,