nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add the option to automatically wake-up rigid-bodies a new joint is attached to

Browse Source
This commit is contained in:
Sébastien Crozet
2022-05-30 18:24:46 +02:00
parent 6ce26f3818
commit ab8833f275
16 changed files with 138 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -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) {

44
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -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);
}
}
}

4
src/dynamics/rigid_body_set.rs
View File

@@ -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)
}