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

Merge pull request #72 from dimforge/parallel_axis_inertia_bug

Browse Source 
Fix a bug when applying the parallel axis theorem to the angular inertia matrix is incorrect if the mass is zero.
This commit is contained in:
Sébastien Crozet
2020-12-02 11:26:45 +01:00
committed by GitHub
parent 2c0872954d bde828cdab
commit c2b1fa19f8
1 changed files with 41 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/dynamics/mass_properties.rs
View File

@@ -129,25 +129,27 @@ impl MassProperties {
#[cfg(feature = "dim2")]
pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector<f32>) -> f32 {
let i = utils::inv(self.inv_principal_inertia_sqrt * self.inv_principal_inertia_sqrt);
if self.inv_mass != 0.0 {
let mass = 1.0 / self.inv_mass;
let i = utils::inv(self.inv_principal_inertia_sqrt * self.inv_principal_inertia_sqrt);
i + shift.norm_squared() * mass
} else {
0.0
i
}
}
#[cfg(feature = "dim3")]
pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector<f32>) -> Matrix3<f32> {
let matrix = self.reconstruct_inertia_matrix();
if self.inv_mass != 0.0 {
let mass = 1.0 / self.inv_mass;
let matrix = self.reconstruct_inertia_matrix();
let diag = shift.norm_squared();
let diagm = Matrix3::from_diagonal_element(diag);
matrix + (diagm + shift * shift.transpose()) * mass
} else {
Matrix3::zeros()
matrix
}
}
@@ -371,9 +373,44 @@ impl approx::RelativeEq for MassProperties {
mod test {
use super::MassProperties;
use crate::geometry::ColliderBuilder;
use crate::math::{Point, Rotation, Vector};
use approx::assert_relative_eq;
use num::Zero;
#[test]
fn mass_properties_add_partial_zero() {
let m1 = MassProperties {
local_com: Point::origin(),
inv_mass: 2.0,
inv_principal_inertia_sqrt: na::zero(),
#[cfg(feature = "dim3")]
principal_inertia_local_frame: Rotation::identity(),
};
let m2 = MassProperties {
local_com: Point::origin(),
inv_mass: 0.0,
#[cfg(feature = "dim2")]
inv_principal_inertia_sqrt: 1.0,
#[cfg(feature = "dim3")]
inv_principal_inertia_sqrt: Vector::new(1.0, 2.0, 3.0),
#[cfg(feature = "dim3")]
principal_inertia_local_frame: Rotation::identity(),
};
let result = MassProperties {
local_com: Point::origin(),
inv_mass: 2.0,
#[cfg(feature = "dim2")]
inv_principal_inertia_sqrt: 1.0,
#[cfg(feature = "dim3")]
inv_principal_inertia_sqrt: Vector::new(1.0, 2.0, 3.0),
#[cfg(feature = "dim3")]
principal_inertia_local_frame: Rotation::identity(),
};
assert_eq!(m1 + m2, result);
assert_eq!(m2 + m1, result);
}
#[test]
fn mass_properties_add_sub() {
// Check that addition and subtraction of mass properties behave as expected.