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

4
benchmarks2d/joint_ball2.rs
View File

@@ -42,7 +42,7 @@ pub fn init_world(testbed: &mut Testbed) {
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); let parent_handle = *body_handles.last().unwrap();
let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]); let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
// Horizontal joint. // Horizontal joint.
@@ -50,7 +50,7 @@ pub fn init_world(testbed: &mut Testbed) {
let parent_index = body_handles.len() - numi; let parent_index = body_handles.len() - numi;
let parent_handle = body_handles[parent_index]; let parent_handle = body_handles[parent_index];
let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]); let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
body_handles.push(child_handle); body_handles.push(child_handle);

4
benchmarks2d/joint_fixed2.rs
View File

@@ -48,7 +48,7 @@ pub fn init_world(testbed: &mut Testbed) {
let parent_handle = *body_handles.last().unwrap(); let parent_handle = *body_handles.last().unwrap();
let joint = FixedJointBuilder::new() let joint = FixedJointBuilder::new()
.local_frame2(Isometry::translation(0.0, shift)); .local_frame2(Isometry::translation(0.0, shift));
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
// Horizontal joint. // Horizontal joint.
@@ -57,7 +57,7 @@ pub fn init_world(testbed: &mut Testbed) {
let parent_handle = body_handles[parent_index]; let parent_handle = body_handles[parent_index];
let joint = FixedJointBuilder::new() let joint = FixedJointBuilder::new()
.local_frame2(Isometry::translation(-shift, 0.0)); .local_frame2(Isometry::translation(-shift, 0.0));
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
body_handles.push(child_handle); body_handles.push(child_handle);

2
benchmarks2d/joint_prismatic2.rs
View File

@@ -46,7 +46,7 @@ pub fn init_world(testbed: &mut Testbed) {
let prism = PrismaticJointBuilder::new(axis) let prism = PrismaticJointBuilder::new(axis)
.local_anchor2(point![0.0, shift]) .local_anchor2(point![0.0, shift])
.limits([-1.5, 1.5]); .limits([-1.5, 1.5]);
impulse_joints.insert(curr_parent, curr_child, prism); impulse_joints.insert(curr_parent, curr_child, prism, true);
curr_parent = curr_child; curr_parent = curr_child;
} }

4
benchmarks3d/joint_ball3.rs
View File

@@ -37,7 +37,7 @@ pub fn init_world(testbed: &mut Testbed) {
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); let parent_handle = *body_handles.last().unwrap();
let joint = SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]); let joint = SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
// Horizontal joint. // Horizontal joint.
@@ -45,7 +45,7 @@ pub fn init_world(testbed: &mut Testbed) {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; let parent_handle = body_handles[parent_index];
let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
body_handles.push(child_handle); body_handles.push(child_handle);

4
benchmarks3d/joint_fixed3.rs
View File

@@ -54,7 +54,7 @@ pub fn init_world(testbed: &mut Testbed) {
let parent_handle = *body_handles.last().unwrap(); let parent_handle = *body_handles.last().unwrap();
let joint = let joint =
FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]); FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
// Horizontal joint. // Horizontal joint.
@@ -63,7 +63,7 @@ pub fn init_world(testbed: &mut Testbed) {
let parent_handle = body_handles[parent_index]; let parent_handle = body_handles[parent_index];
let joint = let joint =
FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
body_handles.push(child_handle); body_handles.push(child_handle);

2
benchmarks3d/joint_prismatic3.rs
View File

@@ -45,7 +45,7 @@ pub fn init_world(testbed: &mut Testbed) {
let prism = PrismaticJointBuilder::new(axis) let prism = PrismaticJointBuilder::new(axis)
.local_anchor2(point![0.0, 0.0, -shift]) .local_anchor2(point![0.0, 0.0, -shift])
.limits([-2.0, 0.0]); .limits([-2.0, 0.0]);
impulse_joints.insert(curr_parent, curr_child, prism); impulse_joints.insert(curr_parent, curr_child, prism, true);
curr_parent = curr_child; curr_parent = curr_child;
} }

8
benchmarks3d/joint_revolute3.rs
View File

@@ -55,10 +55,10 @@ pub fn init_world(testbed: &mut Testbed) {
RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]), RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]),
]; ];
impulse_joints.insert(curr_parent, handles[0], revs[0]); impulse_joints.insert(curr_parent, handles[0], revs[0], true);
impulse_joints.insert(handles[0], handles[1], revs[1]); impulse_joints.insert(handles[0], handles[1], revs[1], true);
impulse_joints.insert(handles[1], handles[2], revs[2]); impulse_joints.insert(handles[1], handles[2], revs[2], true);
impulse_joints.insert(handles[2], handles[3], revs[3]); impulse_joints.insert(handles[2], handles[3], revs[3], true);
curr_parent = handles[3]; curr_parent = handles[3];
} }

4
examples2d/joints2.rs
View File

@@ -45,7 +45,7 @@ pub fn init_world(testbed: &mut Testbed) {
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); let parent_handle = *body_handles.last().unwrap();
let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]); let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
// Horizontal joint. // Horizontal joint.
@@ -53,7 +53,7 @@ pub fn init_world(testbed: &mut Testbed) {
let parent_index = body_handles.len() - numi; let parent_index = body_handles.len() - numi;
let parent_handle = body_handles[parent_index]; let parent_handle = body_handles[parent_index];
let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]); let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
body_handles.push(child_handle); body_handles.push(child_handle);

4
examples3d/debug_articulations3.rs
View File

@@ -39,7 +39,7 @@ fn create_ball_articulations(
let parent_handle = *body_handles.last().unwrap(); let parent_handle = *body_handles.last().unwrap();
let joint = let joint =
SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]); SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
multibody_joints.insert(parent_handle, child_handle, joint); multibody_joints.insert(parent_handle, child_handle, joint, true);
} }
// Horizontal multibody_joint. // Horizontal multibody_joint.
@@ -52,7 +52,7 @@ fn create_ball_articulations(
// let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); // let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
// let joint = // let joint =
// RevoluteJoint::new(Vector::x_axis()).local_anchor2(point![-shift, 0.0, 0.0]); // RevoluteJoint::new(Vector::x_axis()).local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
body_handles.push(child_handle); body_handles.push(child_handle);

44
examples3d/debug_excentric_boxes3.rs Normal file
View File

@@ -0,0 +1,44 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let ground_size = 100.1;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, handle, &mut bodies);
// Build the dynamic box rigid body.
let (mut vtx, idx) = Cuboid::new(vector![1.0, 1.0, 1.0]).to_trimesh();
vtx.iter_mut()
.for_each(|pt| *pt += vector![100.0, 100.0, 100.0]);
let shape = SharedShape::convex_mesh(vtx, &idx).unwrap();
for _ in 0..2 {
let rigid_body = RigidBodyBuilder::dynamic()
.translation(vector![-100.0, -100.0 + 10.0, -100.0])
.can_sleep(false);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::new(shape.clone());
colliders.insert_with_parent(collider, handle, &mut bodies);
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
}

2
examples3d/debug_prismatic3.rs
View File

@@ -36,7 +36,7 @@ fn prismatic_repro(
let prismatic = PrismaticJointBuilder::new(Vector::y_axis()) let prismatic = PrismaticJointBuilder::new(Vector::y_axis())
.local_anchor1(point![pos.x, pos.y, pos.z]) .local_anchor1(point![pos.x, pos.y, pos.z])
.motor_position(0.0, stiffness, damping); .motor_position(0.0, stiffness, damping);
impulse_joints.insert(box_rb, wheel_rb, prismatic); impulse_joints.insert(box_rb, wheel_rb, prismatic, true);
} }
// put a small box under one of the wheels // put a small box under one of the wheels

64
examples3d/joints3.rs
View File

@@ -24,9 +24,9 @@ fn create_coupled_joints(
.coupled_axes(JointAxesMask::Y | JointAxesMask::Z); .coupled_axes(JointAxesMask::Y | JointAxesMask::Z);
if use_articulations { if use_articulations {
multibody_joints.insert(ground, body1, joint1); multibody_joints.insert(ground, body1, joint1, true);
} else { } else {
impulse_joints.insert(ground, body1, joint1); impulse_joints.insert(ground, body1, joint1, true);
} }
} }
@@ -66,9 +66,9 @@ fn create_prismatic_joints(
.limits([-2.0, 2.0]); .limits([-2.0, 2.0]);
if use_articulations { if use_articulations {
multibody_joints.insert(curr_parent, curr_child, prism); multibody_joints.insert(curr_parent, curr_child, prism, true);
} else { } else {
impulse_joints.insert(curr_parent, curr_child, prism); impulse_joints.insert(curr_parent, curr_child, prism, true);
} }
curr_parent = curr_child; curr_parent = curr_child;
} }
@@ -130,9 +130,9 @@ fn create_actuated_prismatic_joints(
} }
if use_articulations { if use_articulations {
multibody_joints.insert(curr_parent, curr_child, prism); multibody_joints.insert(curr_parent, curr_child, prism, true);
} else { } else {
impulse_joints.insert(curr_parent, curr_child, prism); impulse_joints.insert(curr_parent, curr_child, prism, true);
} }
curr_parent = curr_child; curr_parent = curr_child;
@@ -185,15 +185,15 @@ fn create_revolute_joints(
]; ];
if use_articulations { if use_articulations {
multibody_joints.insert(curr_parent, handles[0], revs[0]); multibody_joints.insert(curr_parent, handles[0], revs[0], true);
multibody_joints.insert(handles[0], handles[1], revs[1]); multibody_joints.insert(handles[0], handles[1], revs[1], true);
multibody_joints.insert(handles[1], handles[2], revs[2]); multibody_joints.insert(handles[1], handles[2], revs[2], true);
multibody_joints.insert(handles[2], handles[3], revs[3]); multibody_joints.insert(handles[2], handles[3], revs[3], true);
} else { } else {
impulse_joints.insert(curr_parent, handles[0], revs[0]); impulse_joints.insert(curr_parent, handles[0], revs[0], true);
impulse_joints.insert(handles[0], handles[1], revs[1]); impulse_joints.insert(handles[0], handles[1], revs[1], true);
impulse_joints.insert(handles[1], handles[2], revs[2]); impulse_joints.insert(handles[1], handles[2], revs[2], true);
impulse_joints.insert(handles[2], handles[3], revs[3]); impulse_joints.insert(handles[2], handles[3], revs[3], true);
} }
curr_parent = handles[3]; curr_parent = handles[3];
@@ -228,9 +228,9 @@ fn create_revolute_joints_with_limits(
// .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z); // .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z);
if use_articulations { if use_articulations {
multibody_joints.insert(ground, platform1, joint1); multibody_joints.insert(ground, platform1, joint1, true);
} else { } else {
impulse_joints.insert(ground, platform1, joint1); impulse_joints.insert(ground, platform1, joint1, true);
} }
let joint2 = RevoluteJointBuilder::new(z) let joint2 = RevoluteJointBuilder::new(z)
@@ -247,9 +247,9 @@ fn create_revolute_joints_with_limits(
// .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z); // .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z);
if use_articulations { if use_articulations {
multibody_joints.insert(platform1, platform2, joint2); multibody_joints.insert(platform1, platform2, joint2, true);
} else { } else {
impulse_joints.insert(platform1, platform2, joint2); impulse_joints.insert(platform1, platform2, joint2, true);
} }
// Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits. // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits.
@@ -315,9 +315,9 @@ fn create_fixed_joints(
let joint = FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]); let joint = FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]);
if use_articulations { if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint); multibody_joints.insert(parent_handle, child_handle, joint, true);
} else { } else {
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
} }
@@ -326,7 +326,7 @@ fn create_fixed_joints(
let parent_index = body_handles.len() - 1; let parent_index = body_handles.len() - 1;
let parent_handle = body_handles[parent_index]; let parent_handle = body_handles[parent_index];
let joint = FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); let joint = FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
body_handles.push(child_handle); body_handles.push(child_handle);
@@ -374,9 +374,9 @@ fn create_spherical_joints(
SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]); SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
if use_articulations { if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint); multibody_joints.insert(parent_handle, child_handle, joint, true);
} else { } else {
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
} }
@@ -385,7 +385,7 @@ fn create_spherical_joints(
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; let parent_handle = body_handles[parent_index];
let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
body_handles.push(child_handle); body_handles.push(child_handle);
@@ -426,11 +426,11 @@ fn create_spherical_joints_with_limits(
.limits(JointAxis::Y, [-0.3, 0.3]); .limits(JointAxis::Y, [-0.3, 0.3]);
if use_articulations { if use_articulations {
multibody_joints.insert(ground, ball1, joint1); multibody_joints.insert(ground, ball1, joint1, true);
multibody_joints.insert(ball1, ball2, joint2); multibody_joints.insert(ball1, ball2, joint2, true);
} else { } else {
impulse_joints.insert(ground, ball1, joint1); impulse_joints.insert(ground, ball1, joint1, true);
impulse_joints.insert(ball1, ball2, joint2); impulse_joints.insert(ball1, ball2, joint2, true);
} }
} }
@@ -493,9 +493,9 @@ fn create_actuated_revolute_joints(
} }
if use_articulations { if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint); multibody_joints.insert(parent_handle, child_handle, joint, true);
} else { } else {
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
} }
@@ -559,9 +559,9 @@ fn create_actuated_spherical_joints(
} }
if use_articulations { if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint); multibody_joints.insert(parent_handle, child_handle, joint, true);
} else { } else {
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint, true);
} }
} }

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

@@ -42,6 +42,7 @@ pub struct ImpulseJointSet {
rb_graph_ids: Coarena<RigidBodyGraphIndex>, rb_graph_ids: Coarena<RigidBodyGraphIndex>,
joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph. joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
joint_graph: InteractionGraph<RigidBodyHandle, ImpulseJoint>, 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 { impl ImpulseJointSet {
@@ -51,6 +52,7 @@ impl ImpulseJointSet {
rb_graph_ids: Coarena::new(), rb_graph_ids: Coarena::new(),
joint_ids: Arena::new(), joint_ids: Arena::new(),
joint_graph: InteractionGraph::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. /// 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( pub fn insert(
&mut self, &mut self,
body1: RigidBodyHandle, body1: RigidBodyHandle,
body2: RigidBodyHandle, body2: RigidBodyHandle,
data: impl Into<GenericJoint>, data: impl Into<GenericJoint>,
wake_up: bool,
) -> ImpulseJointHandle { ) -> ImpulseJointHandle {
let data = data.into(); let data = data.into();
let handle = self.joint_ids.insert(0.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); 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) ImpulseJointHandle(handle)
} }
@@ -257,23 +269,16 @@ impl ImpulseJointSet {
/// ///
/// If `wake_up` is set to `true`, then the bodies attached to this joint will be /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
/// automatically woken up. /// automatically woken up.
pub fn remove( pub fn remove(&mut self, handle: ImpulseJointHandle, wake_up: bool) -> Option<ImpulseJoint> {
&mut self,
handle: ImpulseJointHandle,
islands: &mut IslandManager,
bodies: &mut RigidBodySet,
wake_up: bool,
) -> Option<ImpulseJoint> {
let id = self.joint_ids.remove(handle.0)?; let id = self.joint_ids.remove(handle.0)?;
let endpoints = self.joint_graph.graph.edge_endpoints(id)?; let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
if wake_up { if wake_up {
// Wake-up the bodies attached to this joint.
if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) { 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) { 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( pub fn remove_joints_attached_to_rigid_body(
&mut self, &mut self,
handle: RigidBodyHandle, handle: RigidBodyHandle,
islands: &mut IslandManager,
bodies: &mut RigidBodySet,
) -> Vec<ImpulseJointHandle> { ) -> Vec<ImpulseJointHandle> {
let mut deleted = vec![]; let mut deleted = vec![];
@@ -324,8 +327,8 @@ impl ImpulseJointSet {
} }
// Wake up the attached bodies. // Wake up the attached bodies.
islands.wake_up(bodies, h1, true); self.to_wake_up.push(h1);
islands.wake_up(bodies, h2, true); self.to_wake_up.push(h2);
} }
if let Some(other) = self.joint_graph.remove_node(deleted_id) { 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::geometry::{InteractionGraph, RigidBodyGraphIndex};
use crate::parry::partitioning::IndexedData; use crate::parry::partitioning::IndexedData;
use crate::prelude::RigidBody;
/// The unique handle of an multibody_joint added to a `MultibodyJointSet`. /// The unique handle of an multibody_joint added to a `MultibodyJointSet`.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] #[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 // 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. // that any more in the future when we improve our island builder.
pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>, pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>,
pub(crate) to_wake_up: Vec<RigidBodyHandle>,
} }
impl MultibodyJointSet { impl MultibodyJointSet {
@@ -93,6 +95,7 @@ impl MultibodyJointSet {
multibodies: Arena::new(), multibodies: Arena::new(),
rb2mb: Coarena::new(), rb2mb: Coarena::new(),
connectivity_graph: InteractionGraph::new(), connectivity_graph: InteractionGraph::new(),
to_wake_up: vec![],
} }
} }
@@ -113,6 +116,7 @@ impl MultibodyJointSet {
body1: RigidBodyHandle, body1: RigidBodyHandle,
body2: RigidBodyHandle, body2: RigidBodyHandle,
data: impl Into<GenericJoint>, data: impl Into<GenericJoint>,
wake_up: bool,
) -> Option<MultibodyJointHandle> { ) -> Option<MultibodyJointHandle> {
let data = data.into(); let data = data.into();
let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| { 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)); 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, // Because each rigid-body can only have one parent link,
// we can use the second rigid-body’s handle as the multibody_joint’s // we can use the second rigid-body’s handle as the multibody_joint’s
// handle. // handle.
@@ -162,13 +171,7 @@ impl MultibodyJointSet {
} }
/// Removes an multibody_joint from this set. /// Removes an multibody_joint from this set.
pub fn remove( pub fn remove(&mut self, handle: MultibodyJointHandle, wake_up: bool) {
&mut self,
handle: MultibodyJointHandle,
islands: &mut IslandManager,
bodies: &mut RigidBodySet,
wake_up: bool,
) {
if let Some(removed) = self.rb2mb.get(handle.0).copied() { if let Some(removed) = self.rb2mb.get(handle.0).copied() {
let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
@@ -181,8 +184,8 @@ impl MultibodyJointSet {
); );
if wake_up { if wake_up {
islands.wake_up(bodies, RigidBodyHandle(handle.0), true); self.to_wake_up.push(RigidBodyHandle(handle.0));
islands.wake_up(bodies, parent_rb, true); self.to_wake_up.push(parent_rb);
} }
// TODO: remove the node if it no longer has any attached edges? // 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. /// 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, wake_up: bool) {
&mut self,
handle: RigidBodyHandle,
islands: &mut IslandManager,
bodies: &mut RigidBodySet,
wake_up: bool,
) {
if let Some(removed) = self.rb2mb.get(handle.0).copied() { if let Some(removed) = self.rb2mb.get(handle.0).copied() {
// Remove the multibody. // Remove the multibody.
let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
@@ -225,7 +222,7 @@ impl MultibodyJointSet {
let rb_handle = link.rigid_body; let rb_handle = link.rigid_body;
if wake_up { 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. // 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. /// 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) {
&mut self,
rb_to_remove: RigidBodyHandle,
islands: &mut IslandManager,
bodies: &mut RigidBodySet,
) {
// TODO: optimize this. // TODO: optimize this.
if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() { if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() {
let mut articulations_to_remove = vec![]; 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. // There is a multibody_joint handle is equal to the second rigid-body’s handle.
articulations_to_remove.push(MultibodyJointHandle(rb2.0)); articulations_to_remove.push(MultibodyJointHandle(rb2.0));
islands.wake_up(bodies, rb1, true); self.to_wake_up.push(rb1);
islands.wake_up(bodies, rb2, true); self.to_wake_up.push(rb2);
} }
for articulation_handle in articulations_to_remove { 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. * Remove impulse_joints attached to this rigid-body.
*/ */
impulse_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, islands, self); multibody_joints.remove_joints_attached_to_rigid_body(handle);
Some(rb) Some(rb)
} }

24
src_testbed/testbed.rs
View File

@@ -691,12 +691,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
.collect(); .collect();
let num_to_delete = (impulse_joints.len() / 10).max(1); let num_to_delete = (impulse_joints.len() / 10).max(1);
for to_delete in &impulse_joints[..num_to_delete] { for to_delete in &impulse_joints[..num_to_delete] {
self.harness.physics.impulse_joints.remove( self.harness.physics.impulse_joints.remove(*to_delete, true);
*to_delete,
&mut self.harness.physics.islands,
&mut self.harness.physics.bodies,
true,
);
} }
} }
KeyCode::A => { KeyCode::A => {
@@ -710,12 +705,10 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
.collect(); .collect();
let num_to_delete = (multibody_joints.len() / 10).max(1); let num_to_delete = (multibody_joints.len() / 10).max(1);
for to_delete in &multibody_joints[..num_to_delete] { for to_delete in &multibody_joints[..num_to_delete] {
self.harness.physics.multibody_joints.remove( self.harness
*to_delete, .physics
&mut self.harness.physics.islands, .multibody_joints
&mut self.harness.physics.bodies, .remove(*to_delete, true);
true,
);
} }
} }
KeyCode::M => { KeyCode::M => {
@@ -731,12 +724,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
self.harness self.harness
.physics .physics
.multibody_joints .multibody_joints
.remove_multibody_articulations( .remove_multibody_articulations(to_delete, true);
to_delete,
&mut self.harness.physics.islands,
&mut self.harness.physics.bodies,
true,
);
} }
} }
_ => {} _ => {}