From df2156ffd02ea1b8c86e86f1d68c5e4e915e6d98 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 31 Aug 2020 17:58:14 +0200 Subject: Allow the removal of a collider. --- src/dynamics/mass_properties.rs | 165 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 164 insertions(+), 1 deletion(-) (limited to 'src/dynamics/mass_properties.rs') diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index cc2979c..8affe0a 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -1,7 +1,7 @@ use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector}; use crate::utils; use num::Zero; -use std::ops::{Add, AddAssign}; +use std::ops::{Add, AddAssign, Sub, SubAssign}; #[cfg(feature = "dim3")] use {na::Matrix3, std::ops::MulAssign}; @@ -24,6 +24,59 @@ pub struct MassProperties { pub principal_inertia_local_frame: Rotation, } +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, mass: f32, principal_inertia: f32) -> Self { @@ -90,6 +143,18 @@ impl MassProperties { } } + #[cfg(feature = "dim3")] + /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii. + pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3 { + let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e); + self.principal_inertia_local_frame.to_rotation_matrix() + * Matrix3::from_diagonal(&inv_principal_inertia) + * self + .principal_inertia_local_frame + .inverse() + .to_rotation_matrix() + } + #[cfg(feature = "dim3")] /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii. pub fn reconstruct_inertia_matrix(&self) -> Matrix3 { @@ -143,6 +208,67 @@ impl Zero for MassProperties { } } +impl Sub for MassProperties { + type Output = Self; + + #[cfg(feature = "dim2")] + fn sub(self, other: MassProperties) -> Self { + if self.is_zero() || other.is_zero() { + return self; + } + + let m1 = utils::inv(self.inv_mass); + let m2 = utils::inv(other.inv_mass); + let inv_mass = utils::inv(m1 - m2); + + let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; + let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); + let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); + let inertia = i1 - i2; + // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. + let inv_principal_inertia_sqrt = utils::inv(inertia.max(0.0).sqrt()); + + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + } + } + + #[cfg(feature = "dim3")] + fn sub(self, other: MassProperties) -> Self { + if self.is_zero() || other.is_zero() { + return self; + } + + let m1 = utils::inv(self.inv_mass); + let m2 = utils::inv(other.inv_mass); + let inv_mass = utils::inv(m1 - m2); + let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; + let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); + let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); + let inertia = i1 - i2; + let eigen = inertia.symmetric_eigen(); + let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors); + let principal_inertia = eigen.eigenvalues; + // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. + let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.max(0.0).sqrt())); + + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + principal_inertia_local_frame, + } + } +} + +impl SubAssign for MassProperties { + fn sub_assign(&mut self, rhs: MassProperties) { + *self = *self - rhs + } +} + impl Add for MassProperties { type Output = Self; @@ -204,3 +330,40 @@ impl AddAssign for MassProperties { *self = *self + rhs } } + +#[cfg(test)] +mod test { + use super::MassProperties; + use crate::geometry::ColliderBuilder; + use approx::assert_relative_eq; + use num::Zero; + + #[test] + fn mass_properties_add_sub() { + // Check that addition and subtraction of mass properties behave as expected. + let c1 = ColliderBuilder::capsule_x(1.0, 2.0).build(); + let c2 = ColliderBuilder::capsule_y(3.0, 4.0).build(); + let c3 = ColliderBuilder::ball(5.0).build(); + + let m1 = c1.mass_properties(); + let m2 = c2.mass_properties(); + let m3 = c3.mass_properties(); + let m1m2m3 = m1 + m2 + m3; + + assert_relative_eq!(m1 + m2, m2 + m1, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6); + assert_relative_eq!( + m1m2m3 - m1 - m2 - m3, + MassProperties::zero(), + epsilon = 1.0e-6 + ); + } +} -- cgit From 2f2a073ce47eaa17f44d88b9dc6cc56362c374e2 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 1 Sep 2020 17:05:24 +0200 Subject: Fix mass property update when adding a collider. --- src/dynamics/mass_properties.rs | 119 ++++++++++++++++++++++------------------ 1 file changed, 66 insertions(+), 53 deletions(-) (limited to 'src/dynamics/mass_properties.rs') diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index 8affe0a..f2fce4b 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -24,59 +24,6 @@ pub struct MassProperties { pub principal_inertia_local_frame: Rotation, } -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, 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) -> 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 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; -- cgit From fc0b3bf39484029d956055943b38bb55ab2c5791 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 1 Sep 2020 17:35:32 +0200 Subject: Mass properties: add a max number of iterations for the local-frame rotation computation. --- src/dynamics/mass_properties.rs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'src/dynamics/mass_properties.rs') diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index f2fce4b..586eaaf 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -209,7 +209,8 @@ impl Sub for MassProperties { let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); let inertia = i1 - i2; let eigen = inertia.symmetric_eigen(); - let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors); + let principal_inertia_local_frame = + Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one()); let principal_inertia = eigen.eigenvalues; // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.max(0.0).sqrt())); @@ -272,7 +273,8 @@ impl Add for MassProperties { let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); let inertia = i1 + i2; let eigen = inertia.symmetric_eigen(); - let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors); + let principal_inertia_local_frame = + Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one()); let principal_inertia = eigen.eigenvalues; let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); -- cgit From 939e569491ca81d8deaca6f13119490c439fc9bf Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 1 Sep 2020 17:47:21 +0200 Subject: Take local inertial frame into accound for abs comparison of MassProperties. --- src/dynamics/mass_properties.rs | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) (limited to 'src/dynamics/mass_properties.rs') diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index 586eaaf..22e7da5 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -300,14 +300,22 @@ impl approx::AbsDiffEq for MassProperties { } fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { - self.local_com.abs_diff_eq(&other.local_com, epsilon) + #[cfg(feature = "dim2")] + let inertia_is_ok = self + .inv_principal_inertia_sqrt + .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon); + + #[cfg(feature = "dim3")] + let inertia_is_ok = self + .reconstruct_inverse_inertia_matrix() + .abs_diff_eq(&other.reconstruct_inverse_inertia_matrix(), epsilon); + + inertia_is_ok + && 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) } } -- cgit