From 2b1374c596957ac8cabe085859be3b823a1ba0c6 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 19 Apr 2022 18:57:40 +0200 Subject: First round deleting the component sets. --- 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 +-- src/dynamics/joint/multibody_joint/multibody.rs | 89 ++--- .../joint/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 +- src/dynamics/solver/generic_velocity_constraint.rs | 16 +- .../solver/generic_velocity_ground_constraint.rs | 15 +- src/dynamics/solver/interaction_groups.rs | 31 +- src/dynamics/solver/island_solver.rs | 22 +- .../solver/joint_constraint/joint_constraint.rs | 49 +-- src/dynamics/solver/parallel_island_solver.rs | 17 +- src/dynamics/solver/parallel_solver_constraints.rs | 31 +- src/dynamics/solver/parallel_velocity_solver.rs | 15 +- src/dynamics/solver/solver_constraints.rs | 175 +++------ src/dynamics/solver/velocity_constraint.rs | 15 +- src/dynamics/solver/velocity_constraint_wide.rs | 11 +- src/dynamics/solver/velocity_ground_constraint.rs | 15 +- .../solver/velocity_ground_constraint_wide.rs | 11 +- src/dynamics/solver/velocity_solver.rs | 17 +- src/geometry/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 | 231 ++++-------- src/pipeline/user_changes.rs | 98 ++---- 34 files changed, 717 insertions(+), 1644 deletions(-) delete mode 100644 src/data/component_set.rs (limited to 'src') 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