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

Fix mass-properties update after collider change

Browse Source
This commit is contained in:
Sébastien Crozet
2022-04-16 11:54:03 +02:00
committed by Sébastien Crozet
parent 775c45e9ff
commit ee679427cd
8 changed files with 118 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -76,6 +76,12 @@ bitflags::bitflags! {
}
}
impl Default for JointAxesMask {
fn default() -> Self {
Self::empty()
}
}
/// Identifiers of degrees of freedoms of a joint.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
@@ -230,7 +236,8 @@ impl GenericJoint {
self.limit_axes.is_empty() && self.motor_axes.is_empty()
}
fn complete_ang_frame(axis: UnitVector<Real>) -> Rotation<Real> {
#[doc(hidden)]
pub fn complete_ang_frame(axis: UnitVector<Real>) -> Rotation<Real> {
let basis = axis.orthonormal_basis();
#[cfg(feature = "dim2")]

21
src/dynamics/rigid_body.rs
View File

@@ -383,7 +383,7 @@ impl RigidBody {
///
/// Returns zero if this rigid body has an infinite mass.
pub fn mass(&self) -> Real {
utils::inv(self.rb_mprops.local_mprops.inv_mass)
self.rb_mprops.local_mprops.mass()
}
/// The predicted position of this rigid-body.
@@ -889,7 +889,7 @@ pub struct RigidBodyBuilder {
rb_type: RigidBodyType,
mprops_flags: LockedAxes,
/// The additional mass properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information.
pub mass_properties: MassProperties,
pub additional_mass_properties: MassProperties,
/// Whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
pub can_sleep: bool,
/// Whether or not the rigid-body is to be created asleep.
@@ -916,7 +916,7 @@ impl RigidBodyBuilder {
angular_damping: 0.0,
rb_type,
mprops_flags: LockedAxes::empty(),
mass_properties: MassProperties::zero(),
additional_mass_properties: MassProperties::zero(),
can_sleep: true,
sleeping: false,
ccd_enabled: false,
@@ -1008,7 +1008,7 @@ impl RigidBodyBuilder {
/// mass properties of your rigid-body, don't attach colliders to it, or
/// only attach colliders with densities equal to zero.
pub fn additional_mass_properties(mut self, props: MassProperties) -> Self {
self.mass_properties = props;
self.additional_mass_properties = props;
self
}
@@ -1072,7 +1072,7 @@ impl RigidBodyBuilder {
/// equal to the sum of this additional mass and the mass computed from the colliders
/// (with non-zero densities) attached to this rigid-body.
pub fn additional_mass(mut self, mass: Real) -> Self {
self.mass_properties.set_mass(mass, false);
self.additional_mass_properties.set_mass(mass, false);
self
}
@@ -1093,7 +1093,7 @@ impl RigidBodyBuilder {
/// computed from the colliders (with non-zero densities) attached to this rigid-body.
#[cfg(feature = "dim2")]
pub fn additional_principal_angular_inertia(mut self, inertia: Real) -> Self {
self.mass_properties.inv_principal_inertia_sqrt =
self.additional_mass_properties.inv_principal_inertia_sqrt =
utils::inv(ComplexField::sqrt(inertia.max(0.0)));
self
}
@@ -1119,7 +1119,7 @@ impl RigidBodyBuilder {
/// computed from the colliders (with non-zero densities) attached to this rigid-body.
#[cfg(feature = "dim3")]
pub fn additional_principal_angular_inertia(mut self, inertia: AngVector<Real>) -> Self {
self.mass_properties.inv_principal_inertia_sqrt =
self.additional_mass_properties.inv_principal_inertia_sqrt =
inertia.map(|e| utils::inv(ComplexField::sqrt(e.max(0.0))));
self
}
@@ -1197,7 +1197,12 @@ impl RigidBodyBuilder {
rb.rb_vels.angvel = self.angvel;
rb.rb_type = self.rb_type;
rb.user_data = self.user_data;
rb.rb_mprops.local_mprops = self.mass_properties;
if self.additional_mass_properties != MassProperties::default() {
rb.rb_mprops.additional_local_mprops = Some(Box::new(self.additional_mass_properties));
rb.rb_mprops.local_mprops = self.additional_mass_properties;
}
rb.rb_mprops.flags = self.mprops_flags;
rb.rb_damping.linear_damping = self.linear_damping;
rb.rb_damping.angular_damping = self.angular_damping;

38
src/dynamics/rigid_body_components.rs
View File

@@ -1,4 +1,4 @@
use crate::data::{ComponentSetMut, ComponentSetOption};
use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::MassProperties;
use crate::geometry::{
ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition,
@@ -225,13 +225,14 @@ bitflags::bitflags! {
// TODO: split this into "LocalMassProps" and `WorldMassProps"?
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy, PartialEq)]
#[derive(Clone, Debug, PartialEq)]
/// The mass properties of this rigid-bodies.
pub struct RigidBodyMassProps {
/// Flags for locking rotation and translation.
pub flags: LockedAxes,
/// The local mass properties of the rigid-body.
pub local_mprops: MassProperties,
pub additional_local_mprops: Option<Box<MassProperties>>,
/// The world-space center of mass of the rigid-body.
pub world_com: Point<Real>,
/// The inverse mass taking into account translation locking.
@@ -246,6 +247,7 @@ impl Default for RigidBodyMassProps {
Self {
flags: LockedAxes::empty(),
local_mprops: MassProperties::zero(),
additional_local_mprops: None,
world_com: Point::origin(),
effective_inv_mass: Vector::zero(),
effective_world_inv_inertia_sqrt: AngularInertia::zero(),
@@ -292,6 +294,38 @@ impl RigidBodyMassProps {
self.effective_world_inv_inertia_sqrt.squared().inverse()
}
/// Recompute the mass-properties of this rigid-bodies based on its currentely attached colliders.
pub fn recompute_mass_properties_from_colliders<Colliders>(
&mut self,
colliders: &Colliders,
attached_colliders: &RigidBodyColliders,
position: &Isometry<Real>,
) where
Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderMassProps>
+ ComponentSet<ColliderShape>,
{
self.local_mprops = self
.additional_local_mprops
.as_ref()
.map(|mprops| **mprops)
.unwrap_or_else(MassProperties::default);
for handle in &attached_colliders.0 {
let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
if let Some(co_parent) = co_parent {
let (co_mprops, co_shape): (&ColliderMassProps, &ColliderShape) =
colliders.index_bundle(handle.0);
let to_add = co_mprops
.mass_properties(&**co_shape)
.transform_by(&co_parent.pos_wrt_parent);
self.local_mprops += to_add;
}
}
self.update_world_mass_properties(position);
}
/// Update the world-space mass properties of `self`, taking into account the new position.
pub fn update_world_mass_properties(&mut self, position: &Isometry<Real>) {
self.world_com = self.local_mprops.world_com(&position);

8
src/dynamics/solver/interaction_groups.rs
View File

@@ -431,14 +431,14 @@ impl InteractionGroups {
let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0);
(*data.0, data.1.active_set_offset)
} else {
(RigidBodyType::Fixed, 0)
(RigidBodyType::Fixed, usize::MAX)
};
let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
{
let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0);
(*data.0, data.1.active_set_offset)
} else {
(RigidBodyType::Fixed, 0)
(RigidBodyType::Fixed, usize::MAX)
};
let is_fixed1 = !status1.is_dynamic();
@@ -451,7 +451,9 @@ impl InteractionGroups {
let i1 = active_set_offset1;
let i2 = active_set_offset2;
let conflicts = self.body_masks[i1] | self.body_masks[i2];
let mask1 = if !is_fixed1 { self.body_masks[i1] } else { 0 };
let mask2 = if !is_fixed2 { self.body_masks[i2] } else { 0 };
let conflicts = mask1 | mask2;
let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;