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

Fix mass property update when adding a collider.

Browse Source
This commit is contained in:
Sébastien Crozet
2020-09-01 17:05:24 +02:00
parent 9622827dc6
commit 2f2a073ce4
7 changed files with 127 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
src/dynamics/mass_properties.rs
View File

@@ -24,59 +24,6 @@ pub struct MassProperties {
pub principal_inertia_local_frame: Rotation<f32>,
}
impl approx::AbsDiffEq for MassProperties {
type Epsilon = f32;
fn default_epsilon() -> Self::Epsilon {
f32::default_epsilon()
}
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
self.local_com.abs_diff_eq(&other.local_com, epsilon)
&& self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon)
&& self
.inv_principal_inertia_sqrt
.abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon)
// && self
// .principal_inertia_local_frame
// .abs_diff_eq(&other.principal_inertia_local_frame, epsilon)
}
}
impl approx::RelativeEq for MassProperties {
fn default_max_relative() -> Self::Epsilon {
f32::default_max_relative()
}
fn relative_eq(
&self,
other: &Self,
epsilon: Self::Epsilon,
max_relative: Self::Epsilon,
) -> bool {
#[cfg(feature = "dim2")]
let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq(
&other.inv_principal_inertia_sqrt,
epsilon,
max_relative,
);
#[cfg(feature = "dim3")]
let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq(
&other.reconstruct_inverse_inertia_matrix(),
epsilon,
max_relative,
);
inertia_is_ok
&& self
.local_com
.relative_eq(&other.local_com, epsilon, max_relative)
&& self
.inv_mass
.relative_eq(&other.inv_mass, epsilon, max_relative)
}
}
impl MassProperties {
#[cfg(feature = "dim2")]
pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self {
@@ -190,6 +137,19 @@ impl MassProperties {
Matrix3::zeros()
}
}
/// Transform each element of the mass properties.
pub fn transform_by(&self, m: &Isometry<f32>) -> Self {
// NOTE: we don't apply the parallel axis theorem here
// because the center of mass is also transformed.
Self {
local_com: m * self.local_com,
inv_mass: self.inv_mass,
inv_principal_inertia_sqrt: self.inv_principal_inertia_sqrt,
#[cfg(feature = "dim3")]
principal_inertia_local_frame: m.rotation * self.principal_inertia_local_frame,
}
}
}
impl Zero for MassProperties {
@@ -331,6 +291,59 @@ impl AddAssign<MassProperties> for MassProperties {
}
}
impl approx::AbsDiffEq for MassProperties {
type Epsilon = f32;
fn default_epsilon() -> Self::Epsilon {
f32::default_epsilon()
}
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
self.local_com.abs_diff_eq(&other.local_com, epsilon)
&& self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon)
&& self
.inv_principal_inertia_sqrt
.abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon)
// && self
// .principal_inertia_local_frame
// .abs_diff_eq(&other.principal_inertia_local_frame, epsilon)
}
}
impl approx::RelativeEq for MassProperties {
fn default_max_relative() -> Self::Epsilon {
f32::default_max_relative()
}
fn relative_eq(
&self,
other: &Self,
epsilon: Self::Epsilon,
max_relative: Self::Epsilon,
) -> bool {
#[cfg(feature = "dim2")]
let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq(
&other.inv_principal_inertia_sqrt,
epsilon,
max_relative,
);
#[cfg(feature = "dim3")]
let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq(
&other.reconstruct_inverse_inertia_matrix(),
epsilon,
max_relative,
);
inertia_is_ok
&& self
.local_com
.relative_eq(&other.local_com, epsilon, max_relative)
&& self
.inv_mass
.relative_eq(&other.inv_mass, epsilon, max_relative)
}
}
#[cfg(test)]
mod test {
use super::MassProperties;

10
src/dynamics/rigid_body.rs
View File

@@ -139,7 +139,9 @@ impl RigidBody {
/// Adds a collider to this rigid-body.
pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
let mass_properties = coll.mass_properties();
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
self.colliders.push(handle);
self.mass_properties += mass_properties;
self.update_world_mass_properties();
@@ -149,7 +151,9 @@ impl RigidBody {
pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
if let Some(i) = self.colliders.iter().position(|e| *e == handle) {
self.colliders.swap_remove(i);
let mass_properties = coll.mass_properties();
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
self.mass_properties -= mass_properties;
self.update_world_mass_properties();
}
@@ -189,7 +193,7 @@ impl RigidBody {
}
fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses.
let com = &self.position * self.mass_properties.local_com;
let shift = Translation::from(com.coords);
shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
}

1
src/geometry/collider.rs
View File

@@ -150,6 +150,7 @@ impl Collider {
}
/// The position of this collider expressed in the local-space of the rigid-body it is attached to.
#[deprecated(note = "use `.position_wrt_parent()` instead.")]
pub fn delta(&self) -> &Isometry<f32> {
&self.delta
}

4
src/geometry/contact.rs
View File

@@ -360,8 +360,8 @@ impl ContactManifold {
pair,
(subshape1, subshape2),
BodyPair::new(coll1.parent, coll2.parent),
*coll1.delta(),
*coll2.delta(),
*coll1.position_wrt_parent(),
*coll2.position_wrt_parent(),
(coll1.friction + coll2.friction) * 0.5,
(coll1.restitution + coll2.restitution) * 0.5,
)