diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/dynamics/joint/impulse_joint/impulse_joint_set.rs | 31 | ||||
| -rw-r--r-- | src/dynamics/joint/multibody_joint/multibody_joint_set.rs | 44 | ||||
| -rw-r--r-- | src/dynamics/rigid_body_set.rs | 4 |
3 files changed, 37 insertions, 42 deletions
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 0309cff..c4ec734 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -42,6 +42,7 @@ pub struct ImpulseJointSet { rb_graph_ids: Coarena<RigidBodyGraphIndex>, joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph. joint_graph: InteractionGraph<RigidBodyHandle, ImpulseJoint>, + pub(crate) to_wake_up: Vec<RigidBodyHandle>, // A set of rigid-body handles to wake-up during the next timestep. } impl ImpulseJointSet { @@ -51,6 +52,7 @@ impl ImpulseJointSet { rb_graph_ids: Coarena::new(), joint_ids: Arena::new(), joint_graph: InteractionGraph::new(), + to_wake_up: vec![], } } @@ -180,11 +182,15 @@ impl ImpulseJointSet { } /// Inserts a new joint into this set and retrieve its handle. + /// + /// If `wake_up` is set to `true`, then the bodies attached to this joint will be + /// automatically woken up during the next timestep. pub fn insert( &mut self, body1: RigidBodyHandle, body2: RigidBodyHandle, data: impl Into<GenericJoint>, + wake_up: bool, ) -> ImpulseJointHandle { let data = data.into(); let handle = self.joint_ids.insert(0.into()); @@ -217,6 +223,12 @@ impl ImpulseJointSet { } self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint); + + if wake_up { + self.to_wake_up.push(body1); + self.to_wake_up.push(body2); + } + ImpulseJointHandle(handle) } @@ -257,23 +269,16 @@ impl ImpulseJointSet { /// /// If `wake_up` is set to `true`, then the bodies attached to this joint will be /// automatically woken up. - pub fn remove( - &mut self, - handle: ImpulseJointHandle, - islands: &mut IslandManager, - bodies: &mut RigidBodySet, - wake_up: bool, - ) -> Option<ImpulseJoint> { + pub fn remove(&mut self, handle: ImpulseJointHandle, wake_up: bool) -> Option<ImpulseJoint> { let id = self.joint_ids.remove(handle.0)?; let endpoints = self.joint_graph.graph.edge_endpoints(id)?; if wake_up { - // Wake-up the bodies attached to this joint. if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) { - islands.wake_up(bodies, *rb_handle, true); + self.to_wake_up.push(*rb_handle); } if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) { - islands.wake_up(bodies, *rb_handle, true); + self.to_wake_up.push(*rb_handle); } } @@ -294,8 +299,6 @@ impl ImpulseJointSet { pub fn remove_joints_attached_to_rigid_body( &mut self, handle: RigidBodyHandle, - islands: &mut IslandManager, - bodies: &mut RigidBodySet, ) -> Vec<ImpulseJointHandle> { let mut deleted = vec![]; @@ -324,8 +327,8 @@ impl ImpulseJointSet { } // Wake up the attached bodies. - islands.wake_up(bodies, h1, true); - islands.wake_up(bodies, h2, true); + self.to_wake_up.push(h1); + self.to_wake_up.push(h2); } if let Some(other) = self.joint_graph.remove_node(deleted_id) { diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 50a9438..fd2fa67 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -5,6 +5,7 @@ use crate::dynamics::{ }; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex}; use crate::parry::partitioning::IndexedData; +use crate::prelude::RigidBody; /// The unique handle of an multibody_joint added to a `MultibodyJointSet`. #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] @@ -84,6 +85,7 @@ pub struct MultibodyJointSet { // NOTE: this is mostly for the island extraction. So perhaps we won’t need // that any more in the future when we improve our island builder. pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>, + pub(crate) to_wake_up: Vec<RigidBodyHandle>, } impl MultibodyJointSet { @@ -93,6 +95,7 @@ impl MultibodyJointSet { multibodies: Arena::new(), rb2mb: Coarena::new(), connectivity_graph: InteractionGraph::new(), + to_wake_up: vec![], } } @@ -113,6 +116,7 @@ impl MultibodyJointSet { body1: RigidBodyHandle, body2: RigidBodyHandle, data: impl Into<GenericJoint>, + wake_up: bool, ) -> Option<MultibodyJointHandle> { let data = data.into(); let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| { @@ -155,6 +159,11 @@ impl MultibodyJointSet { multibody1.append(mb2, link1.id, MultibodyJoint::new(data)); + if wake_up { + self.to_wake_up.push(body1); + self.to_wake_up.push(body2); + } + // Because each rigid-body can only have one parent link, // we can use the second rigid-body’s handle as the multibody_joint’s // handle. @@ -162,13 +171,7 @@ impl MultibodyJointSet { } /// Removes an multibody_joint from this set. - pub fn remove( - &mut self, - handle: MultibodyJointHandle, - islands: &mut IslandManager, - bodies: &mut RigidBodySet, - wake_up: bool, - ) { + pub fn remove(&mut self, handle: MultibodyJointHandle, wake_up: bool) { if let Some(removed) = self.rb2mb.get(handle.0).copied() { let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); @@ -181,8 +184,8 @@ impl MultibodyJointSet { ); if wake_up { - islands.wake_up(bodies, RigidBodyHandle(handle.0), true); - islands.wake_up(bodies, parent_rb, true); + self.to_wake_up.push(RigidBodyHandle(handle.0)); + self.to_wake_up.push(parent_rb); } // TODO: remove the node if it no longer has any attached edges? @@ -211,13 +214,7 @@ impl MultibodyJointSet { } /// Removes all the multibody_joints from the multibody the given rigid-body is part of. - pub fn remove_multibody_articulations( - &mut self, - handle: RigidBodyHandle, - islands: &mut IslandManager, - bodies: &mut RigidBodySet, - wake_up: bool, - ) { + pub fn remove_multibody_articulations(&mut self, handle: RigidBodyHandle, wake_up: bool) { if let Some(removed) = self.rb2mb.get(handle.0).copied() { // Remove the multibody. let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); @@ -225,7 +222,7 @@ impl MultibodyJointSet { let rb_handle = link.rigid_body; if wake_up { - islands.wake_up(bodies, rb_handle, true); + self.to_wake_up.push(rb_handle); } // Remove the rigid-body <-> multibody mapping for this link. @@ -239,12 +236,7 @@ impl MultibodyJointSet { } /// Removes all the multibody joints attached to a rigid-body. - pub fn remove_joints_attached_to_rigid_body( - &mut self, - rb_to_remove: RigidBodyHandle, - islands: &mut IslandManager, - bodies: &mut RigidBodySet, - ) { + pub fn remove_joints_attached_to_rigid_body(&mut self, rb_to_remove: RigidBodyHandle) { // TODO: optimize this. if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() { let mut articulations_to_remove = vec![]; @@ -255,12 +247,12 @@ impl MultibodyJointSet { // There is a multibody_joint handle is equal to the second rigid-body’s handle. articulations_to_remove.push(MultibodyJointHandle(rb2.0)); - islands.wake_up(bodies, rb1, true); - islands.wake_up(bodies, rb2, true); + self.to_wake_up.push(rb1); + self.to_wake_up.push(rb2); } for articulation_handle in articulations_to_remove { - self.remove(articulation_handle, islands, bodies, true); + self.remove(articulation_handle, true); } } } diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 62a6a54..cd8f30a 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -109,8 +109,8 @@ impl RigidBodySet { /* * Remove impulse_joints attached to this rigid-body. */ - impulse_joints.remove_joints_attached_to_rigid_body(handle, islands, self); - multibody_joints.remove_joints_attached_to_rigid_body(handle, islands, self); + impulse_joints.remove_joints_attached_to_rigid_body(handle); + multibody_joints.remove_joints_attached_to_rigid_body(handle); Some(rb) } |