diff options
Diffstat (limited to 'src/dynamics')
| -rw-r--r-- | src/dynamics/island_manager.rs | 2 | ||||
| -rw-r--r-- | src/dynamics/joint/impulse_joint/impulse_joint_set.rs | 14 | ||||
| -rw-r--r-- | src/dynamics/joint/multibody_joint/multibody_joint_set.rs | 25 |
3 files changed, 29 insertions, 12 deletions
diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 246b50e..1295f14 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -256,7 +256,7 @@ impl IslandManager { // in contact or joined with this collider. push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); - for inter in impulse_joints.joints_with(handle) { + for inter in impulse_joints.attached_joints(handle) { let other = crate::utils::select_other((inter.0, inter.1), handle); self.stack.push(other); } diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 1cd177d..0309cff 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -69,15 +69,23 @@ impl ImpulseJointSet { &self.joint_graph } - /// Iterates through all the impulse_joints attached to the given rigid-body. - pub fn joints_with<'a>( + /// Iterates through all the impulse joints attached to the given rigid-body. + pub fn attached_joints<'a>( &'a self, body: RigidBodyHandle, - ) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyHandle, &'a ImpulseJoint)> { + ) -> impl Iterator< + Item = ( + RigidBodyHandle, + RigidBodyHandle, + ImpulseJointHandle, + &'a ImpulseJoint, + ), + > { self.rb_graph_ids .get(body.0) .into_iter() .flat_map(move |id| self.joint_graph.interactions_with(*id)) + .map(|inter| (inter.0, inter.1, inter.2.handle, inter.2)) } /// Is the given joint handle valid? diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 06dff5d..50a9438 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -252,14 +252,8 @@ impl MultibodyJointSet { .connectivity_graph .interactions_with(link_to_remove.graph_id) { - // There is an multibody_joint that we need to remove between these two bodies. - // If this is an outbound edge, then the multibody_joint’s handle is equal to the - // second body handle. - if rb1 == rb_to_remove { - articulations_to_remove.push(MultibodyJointHandle(rb2.0)); - } else { - articulations_to_remove.push(MultibodyJointHandle(rb1.0)); - } + // 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); @@ -338,6 +332,21 @@ impl MultibodyJointSet { )) } + /// Iterates through all the joints attached to the given rigid-body. + pub fn attached_joints( + &self, + rb: RigidBodyHandle, + ) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyHandle, MultibodyJointHandle)> + '_ { + self.rb2mb + .get(rb.0) + .into_iter() + .flat_map(move |link| self.connectivity_graph.interactions_with(link.graph_id)) + .map(|inter| { + // NOTE: the joint handle is always equal to the handle of the second rigid-body. + (inter.0, inter.1, MultibodyJointHandle(inter.1 .0)) + }) + } + /// Iterate through the handles of all the rigid-bodies attached to this rigid-body /// by an multibody_joint. pub fn attached_bodies<'a>( |