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

Add the ability to disable contacts between two rigid-bodies attached by joints

Browse Source
This commit is contained in:
Sébastien Crozet
2022-06-23 16:23:39 +02:00
parent cd0be8c076
commit 5063f3bb4f
12 changed files with 197 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/dynamics/integration_parameters.rs
View File

@@ -134,7 +134,7 @@ impl IntegrationParameters {
1.0 / (1.0 + cfm_coeff)
}
/// The CFM (constranits force mixing) coefficient applied to all joints for constraints regularization
/// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization
pub fn joint_cfm_coeff(&self) -> Real {
// Compute CFM assuming a critically damped spring multiplied by the damping ratio.
let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0;

23
src/dynamics/joint/fixed_joint.rs
View File

@@ -6,7 +6,8 @@ use crate::math::{Isometry, Point, Real};
#[repr(transparent)]
/// A fixed joint, locks all relative motion between two bodies.
pub struct FixedJoint {
data: GenericJoint,
/// The underlying joint data.
pub data: GenericJoint,
}
impl Default for FixedJoint {
@@ -23,6 +24,17 @@ impl FixedJoint {
Self { data }
}
/// Are contacts between the attached rigid-bodies enabled?
pub fn contacts_enabled(&self) -> bool {
self.data.contacts_enabled
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
self.data.set_contacts_enabled(enabled);
self
}
/// The joint’s frame, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_frame1(&self) -> &Isometry<Real> {
@@ -81,7 +93,7 @@ impl Into<GenericJoint> for FixedJoint {
/// Create fixed joints using the builder pattern.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq, Default)]
pub struct FixedJointBuilder(FixedJoint);
pub struct FixedJointBuilder(pub FixedJoint);
impl FixedJointBuilder {
/// Creates a new builder for fixed joints.
@@ -89,6 +101,13 @@ impl FixedJointBuilder {
Self(FixedJoint::new())
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
#[must_use]
pub fn contacts_enabled(mut self, enabled: bool) -> Self {
self.0.set_contacts_enabled(enabled);
self
}
/// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {

26
src/dynamics/joint/generic_joint.rs
View File

@@ -206,6 +206,8 @@ pub struct GenericJoint {
///
/// Note that the mostor must also be explicitly enabled by the `motors` bitmask.
pub motors: [JointMotor; SPATIAL_DIM],
/// Are contacts between the attached rigid-bodies enabled?
pub contacts_enabled: bool,
}
impl Default for GenericJoint {
@@ -219,6 +221,7 @@ impl Default for GenericJoint {
coupled_axes: JointAxesMask::empty(),
limits: [JointLimits::default(); SPATIAL_DIM],
motors: [JointMotor::default(); SPATIAL_DIM],
contacts_enabled: true,
}
}
}
@@ -275,6 +278,17 @@ impl GenericJoint {
self
}
/// Are contacts between the attached rigid-bodies enabled?
pub fn contacts_enabled(&self) -> bool {
self.contacts_enabled
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
self.contacts_enabled = enabled;
self
}
/// The principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_axis1(&self) -> UnitVector<Real> {
@@ -481,8 +495,9 @@ impl GenericJoint {
}
/// Create generic joints using the builder pattern.
#[derive(Copy, Clone, Debug)]
pub struct GenericJointBuilder(GenericJoint);
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct GenericJointBuilder(pub GenericJoint);
impl GenericJointBuilder {
/// Creates a new generic joint builder.
@@ -498,6 +513,13 @@ impl GenericJointBuilder {
self
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
#[must_use]
pub fn contacts_enabled(mut self, enabled: bool) -> Self {
self.0.contacts_enabled = enabled;
self
}
/// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
#[must_use]
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {

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

@@ -71,6 +71,20 @@ impl ImpulseJointSet {
&self.joint_graph
}
/// Iterates through all the joints between two rigid-bodies.
pub fn joints_between<'a>(
&'a self,
body1: RigidBodyHandle,
body2: RigidBodyHandle,
) -> impl Iterator<Item = (ImpulseJointHandle, &'a ImpulseJoint)> {
self.rb_graph_ids
.get(body1.0)
.zip(self.rb_graph_ids.get(body2.0))
.into_iter()
.flat_map(move |(id1, id2)| self.joint_graph.interaction_pair(*id1, *id2).into_iter())
.map(|inter| (inter.2.handle, inter.2))
}
/// Iterates through all the impulse joints attached to the given rigid-body.
pub fn attached_joints<'a>(
&'a self,

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

@@ -321,6 +321,41 @@ impl MultibodyJointSet {
))
}
/// Returns the the joint between two rigid-bodies (if it exists).
pub fn joint_between(
&self,
rb1: RigidBodyHandle,
rb2: RigidBodyHandle,
) -> Option<(MultibodyJointHandle, &Multibody, &MultibodyLink)> {
let id1 = self.rb2mb.get(rb1.0)?;
let id2 = self.rb2mb.get(rb2.0)?;
// Both bodies must be part of the same multibody.
if id1.multibody != id2.multibody {
return None;
}
let mb = self.multibodies.get(id1.multibody.0)?;
// NOTE: if there is a joint between these two bodies, then
// one of the bodies must be the parent of the other.
let link1 = mb.link(id1.id)?;
let parent1 = link1.parent_id()?;
if parent1 == id2.id {
Some((MultibodyJointHandle(rb1.0), mb, &link1))
} else {
let link2 = mb.link(id2.id)?;
let parent2 = link2.parent_id()?;
if parent2 == id1.id {
Some((MultibodyJointHandle(rb2.0), mb, &link2))
} else {
None
}
}
}
/// Iterates through all the joints attached to the given rigid-body.
pub fn attached_joints(
&self,

24
src/dynamics/joint/prismatic_joint.rs
View File

@@ -9,7 +9,8 @@ use super::{JointLimits, JointMotor};
#[repr(transparent)]
/// A prismatic joint, locks all relative motion between two bodies except for translation along the joint’s principal axis.
pub struct PrismaticJoint {
data: GenericJoint,
/// The underlying joint data.
pub data: GenericJoint,
}
impl PrismaticJoint {
@@ -29,6 +30,17 @@ impl PrismaticJoint {
&self.data
}
/// Are contacts between the attached rigid-bodies enabled?
pub fn contacts_enabled(&self) -> bool {
self.data.contacts_enabled
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
self.data.set_contacts_enabled(enabled);
self
}
/// The joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
@@ -149,8 +161,9 @@ impl Into<GenericJoint> for PrismaticJoint {
/// Create prismatic joints using the builder pattern.
///
/// A prismatic joint locks all relative motion except for translations along the joint’s principal axis.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct PrismaticJointBuilder(PrismaticJoint);
pub struct PrismaticJointBuilder(pub PrismaticJoint);
impl PrismaticJointBuilder {
/// Creates a new builder for prismatic joints.
@@ -160,6 +173,13 @@ impl PrismaticJointBuilder {
Self(PrismaticJoint::new(axis))
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
#[must_use]
pub fn contacts_enabled(mut self, enabled: bool) -> Self {
self.0.set_contacts_enabled(enabled);
self
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {

23
src/dynamics/joint/revolute_joint.rs
View File

@@ -10,7 +10,8 @@ use crate::math::UnitVector;
#[repr(transparent)]
/// A revolute joint, locks all relative motion except for rotation along the joint’s principal axis.
pub struct RevoluteJoint {
data: GenericJoint,
/// The underlying joint data.
pub data: GenericJoint,
}
impl RevoluteJoint {
@@ -38,6 +39,17 @@ impl RevoluteJoint {
&self.data
}
/// Are contacts between the attached rigid-bodies enabled?
pub fn contacts_enabled(&self) -> bool {
self.data.contacts_enabled
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
self.data.set_contacts_enabled(enabled);
self
}
/// The joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
@@ -136,7 +148,7 @@ impl Into<GenericJoint> for RevoluteJoint {
/// A revolute joint locks all relative motion except for rotations along the joint’s principal axis.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct RevoluteJointBuilder(RevoluteJoint);
pub struct RevoluteJointBuilder(pub RevoluteJoint);
impl RevoluteJointBuilder {
/// Creates a new revolute joint builder.
@@ -153,6 +165,13 @@ impl RevoluteJointBuilder {
Self(RevoluteJoint::new(axis))
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
#[must_use]
pub fn contacts_enabled(mut self, enabled: bool) -> Self {
self.0.set_contacts_enabled(enabled);
self
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {

23
src/dynamics/joint/spherical_joint.rs
View File

@@ -9,7 +9,8 @@ use super::JointLimits;
#[repr(transparent)]
/// A spherical joint, locks all relative translations between two bodies.
pub struct SphericalJoint {
data: GenericJoint,
/// The underlying joint data.
pub data: GenericJoint,
}
impl Default for SphericalJoint {
@@ -30,6 +31,17 @@ impl SphericalJoint {
&self.data
}
/// Are contacts between the attached rigid-bodies enabled?
pub fn contacts_enabled(&self) -> bool {
self.data.contacts_enabled
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
self.data.set_contacts_enabled(enabled);
self
}
/// The joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
@@ -132,7 +144,7 @@ impl Into<GenericJoint> for SphericalJoint {
/// Create spherical joints using the builder pattern.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct SphericalJointBuilder(SphericalJoint);
pub struct SphericalJointBuilder(pub SphericalJoint);
impl Default for SphericalJointBuilder {
fn default() -> Self {
@@ -146,6 +158,13 @@ impl SphericalJointBuilder {
Self(SphericalJoint::new())
}
/// Sets whether contacts between the attached rigid-bodies are enabled.
#[must_use]
pub fn contacts_enabled(mut self, enabled: bool) -> Self {
self.0.set_contacts_enabled(enabled);
self
}
/// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {

2
src/dynamics/rigid_body.rs
View File

@@ -20,7 +20,7 @@ pub struct RigidBody {
pub(crate) pos: RigidBodyPosition,
pub(crate) mprops: RigidBodyMassProps,
// NOTE: we need this so that the CCD can use the actual velocities obtained
// by the velocity solver with bias. If we switch to intepolation, we
// by the velocity solver with bias. If we switch to interpolation, we
// should remove this field.
pub(crate) integrated_vels: RigidBodyVelocity,
pub(crate) vels: RigidBodyVelocity,

28
src/geometry/narrow_phase.rs
View File

@@ -4,7 +4,8 @@ use rayon::prelude::*;
use crate::data::graph::EdgeIndex;
use crate::data::Coarena;
use crate::dynamics::{
CoefficientCombineRule, IslandManager, RigidBodyDominance, RigidBodySet, RigidBodyType,
CoefficientCombineRule, ImpulseJointSet, IslandManager, RigidBodyDominance, RigidBodySet,
RigidBodyType,
};
use crate::geometry::{
BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair,
@@ -16,7 +17,7 @@ use crate::pipeline::{
ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext,
PhysicsHooks,
};
use crate::prelude::CollisionEventFlags;
use crate::prelude::{CollisionEventFlags, MultibodyJointSet};
use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
use parry::utils::IsometryOpt;
use std::collections::HashMap;
@@ -774,6 +775,8 @@ impl NarrowPhase {
prediction_distance: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
modified_colliders: &[ColliderHandle],
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
@@ -812,6 +815,27 @@ impl NarrowPhase {
rb_type2 = bodies[co_parent2.handle].body_type;
}
// Deal with contacts disabled between bodies attached by joints.
if let (Some(co_parent1), Some(co_parent2)) = (&co1.parent, &co2.parent) {
for (_, joint) in
impulse_joints.joints_between(co_parent1.handle, co_parent2.handle)
{
if !joint.data.contacts_enabled {
pair.clear();
break 'emit_events;
}
}
if let Some((_, _, mb_link)) =
multibody_joints.joint_between(co_parent1.handle, co_parent2.handle)
{
if !mb_link.joint.data.contacts_enabled {
pair.clear();
break 'emit_events;
}
}
}
// Filter based on the rigid-body types.
if !co1.flags.active_collision_types.test(rb_type1, rb_type2)
&& !co2.flags.active_collision_types.test(rb_type1, rb_type2)

3
src/pipeline/collision_pipeline.rs
View File

@@ -1,5 +1,6 @@
//! Physics pipeline structures.
use crate::dynamics::{ImpulseJointSet, MultibodyJointSet};
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase,
};
@@ -81,6 +82,8 @@ impl CollisionPipeline {
prediction_distance,
bodies,
colliders,
&ImpulseJointSet::new(),
&MultibodyJointSet::new(),
modified_colliders,
hooks,
events,

8
src/pipeline/physics_pipeline.rs
View File

@@ -85,6 +85,8 @@ impl PhysicsPipeline {
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
hooks: &dyn PhysicsHooks,
@@ -130,6 +132,8 @@ impl PhysicsPipeline {
integration_parameters.prediction_distance,
bodies,
colliders,
impulse_joints,
multibody_joints,
modified_colliders,
hooks,
events,
@@ -449,6 +453,8 @@ impl PhysicsPipeline {
narrow_phase,
bodies,
colliders,
impulse_joints,
multibody_joints,
&modified_colliders[..],
&mut removed_colliders,
hooks,
@@ -574,6 +580,8 @@ impl PhysicsPipeline {
narrow_phase,
bodies,
colliders,
impulse_joints,
multibody_joints,
&mut modified_colliders,
&mut removed_colliders,
hooks,