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

ImpulseJointSet::get_mut option to wake up connected bodies (#716)

Browse Source
This commit is contained in:
Thierry Berger
2024-09-13 10:48:56 +02:00
committed by GitHub
parent 04058a111d
commit c714ff81f2
5 changed files with 51 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,3 +1,5 @@
use parry::utils::hashmap::HashMap;
use super::ImpulseJoint;
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
@@ -40,9 +42,11 @@ pub(crate) type JointGraphEdge = crate::data::graph::Edge<ImpulseJoint>;
/// A set of impulse_joints that can be handled by a physics `World`.
pub struct ImpulseJointSet {
rb_graph_ids: Coarena<RigidBodyGraphIndex>,
joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
/// Map joint handles to edge ids on the graph.
joint_ids: Arena<TemporaryInteractionIndex>,
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.
/// A set of rigid-body handles to wake-up during the next timestep.
pub(crate) to_wake_up: HashMap<RigidBodyHandle, ()>,
}
impl ImpulseJointSet {
@@ -52,7 +56,7 @@ impl ImpulseJointSet {
rb_graph_ids: Coarena::new(),
joint_ids: Arena::new(),
joint_graph: InteractionGraph::new(),
to_wake_up: vec![],
to_wake_up: HashMap::default(),
}
}
@@ -145,9 +149,20 @@ impl ImpulseJointSet {
}
/// Gets a mutable reference to the joint with the given handle.
pub fn get_mut(&mut self, handle: ImpulseJointHandle) -> Option<&mut ImpulseJoint> {
pub fn get_mut(
&mut self,
handle: ImpulseJointHandle,
wake_up_connected_bodies: bool,
) -> Option<&mut ImpulseJoint> {
let id = self.joint_ids.get(handle.0)?;
self.joint_graph.graph.edge_weight_mut(*id)
let joint = self.joint_graph.graph.edge_weight_mut(*id);
if wake_up_connected_bodies {
if let Some(joint) = &joint {
self.to_wake_up.insert(joint.body1, ());
self.to_wake_up.insert(joint.body2, ());
}
}
joint
}
/// Gets the joint with the given handle without a known generation.
@@ -269,8 +284,8 @@ 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);
self.to_wake_up.insert(body1, ());
self.to_wake_up.insert(body2, ());
}
ImpulseJointHandle(handle)
@@ -320,10 +335,10 @@ impl ImpulseJointSet {
if wake_up {
if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) {
self.to_wake_up.push(*rb_handle);
self.to_wake_up.insert(*rb_handle, ());
}
if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) {
self.to_wake_up.push(*rb_handle);
self.to_wake_up.insert(*rb_handle, ());
}
}
@@ -372,8 +387,8 @@ impl ImpulseJointSet {
}
// Wake up the attached bodies.
self.to_wake_up.push(h1);
self.to_wake_up.push(h2);
self.to_wake_up.insert(h1, ());
self.to_wake_up.insert(h2, ());
}
if let Some(other) = self.joint_graph.remove_node(deleted_id) {

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

@@ -1,3 +1,5 @@
use parry::utils::hashmap::HashMap;
use crate::data::{Arena, Coarena, Index};
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::{GenericJoint, Multibody, MultibodyJoint, RigidBodyHandle};
@@ -94,7 +96,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>,
pub(crate) to_wake_up: HashMap<RigidBodyHandle, ()>,
}
impl MultibodyJointSet {
@@ -104,7 +106,7 @@ impl MultibodyJointSet {
multibodies: Arena::new(),
rb2mb: Coarena::new(),
connectivity_graph: InteractionGraph::new(),
to_wake_up: vec![],
to_wake_up: HashMap::default(),
}
}
@@ -200,8 +202,8 @@ impl MultibodyJointSet {
multibody1.append(mb2, link1.id, MultibodyJoint::new(data.into(), kinematic));
if wake_up {
self.to_wake_up.push(body1);
self.to_wake_up.push(body2);
self.to_wake_up.insert(body1, ());
self.to_wake_up.insert(body2, ());
}
// Because each rigid-body can only have one parent link,
@@ -223,8 +225,8 @@ impl MultibodyJointSet {
.remove_edge(parent_graph_id, removed.graph_id);
if wake_up {
self.to_wake_up.push(RigidBodyHandle(handle.0));
self.to_wake_up.push(parent_rb);
self.to_wake_up.insert(RigidBodyHandle(handle.0), ());
self.to_wake_up.insert(parent_rb, ());
}
// TODO: remove the node if it no longer has any attached edges?
@@ -265,7 +267,7 @@ impl MultibodyJointSet {
let rb_handle = link.rigid_body;
if wake_up {
self.to_wake_up.push(rb_handle);
self.to_wake_up.insert(rb_handle, ());
}
// Remove the rigid-body <-> multibody mapping for this link.
@@ -290,8 +292,8 @@ impl MultibodyJointSet {
// There is a multibody_joint handle is equal to the second rigid-body’s handle.
articulations_to_remove.push(MultibodyJointHandle(rb2.0));
self.to_wake_up.push(rb1);
self.to_wake_up.push(rb2);
self.to_wake_up.insert(rb1, ());
self.to_wake_up.insert(rb2, ());
}
for articulation_handle in articulations_to_remove {