diff options
Diffstat (limited to 'src/dynamics')
17 files changed, 286 insertions, 1056 deletions
diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs deleted file mode 100644 index a36ceb5..0000000 --- a/src/dynamics/mass_properties.rs +++ /dev/null @@ -1,442 +0,0 @@ -use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector}; -use crate::utils; -use num::Zero; -use std::ops::{Add, AddAssign, Sub, SubAssign}; -#[cfg(feature = "dim3")] -use {na::Matrix3, std::ops::MulAssign}; - -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// The local mass properties of a rigid-body. -pub struct MassProperties { - /// The center of mass of a rigid-body expressed in its local-space. - pub local_com: Point<f32>, - /// The inverse of the mass of a rigid-body. - /// - /// If this is zero, the rigid-body is assumed to have infinite mass. - pub inv_mass: f32, - /// The inverse of the principal angular inertia of the rigid-body. - /// - /// Components set to zero are assumed to be infinite along the corresponding principal axis. - pub inv_principal_inertia_sqrt: AngVector<f32>, - #[cfg(feature = "dim3")] - /// The principal vectors of the local angular inertia tensor of the rigid-body. - pub principal_inertia_local_frame: Rotation<f32>, -} - -impl MassProperties { - /// Initializes the mass properties with the given center-of-mass, mass, and angular inertia. - /// - /// The center-of-mass is specified in the local-space of the rigid-body. - #[cfg(feature = "dim2")] - pub fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self { - let inv_mass = utils::inv(mass); - let inv_principal_inertia_sqrt = utils::inv(principal_inertia.sqrt()); - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - } - } - - /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia. - /// - /// The center-of-mass is specified in the local-space of the rigid-body. - /// The principal angular inertia are the angular inertia along the coordinate axes in the local-space - /// of the rigid-body. - #[cfg(feature = "dim3")] - pub fn new(local_com: Point<f32>, mass: f32, principal_inertia: AngVector<f32>) -> Self { - Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity()) - } - - /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia. - /// - /// The center-of-mass is specified in the local-space of the rigid-body. - /// The principal angular inertia are the angular inertia along the coordinate axes defined by - /// the `principal_inertia_local_frame` expressed in the local-space of the rigid-body. - #[cfg(feature = "dim3")] - pub fn with_principal_inertia_frame( - local_com: Point<f32>, - mass: f32, - principal_inertia: AngVector<f32>, - principal_inertia_local_frame: Rotation<f32>, - ) -> Self { - let inv_mass = utils::inv(mass); - let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - principal_inertia_local_frame, - } - } - - /// The world-space center of mass of the rigid-body. - pub fn world_com(&self, pos: &Isometry<f32>) -> Point<f32> { - pos * self.local_com - } - - #[cfg(feature = "dim2")] - /// The world-space inverse angular inertia tensor of the rigid-body. - pub fn world_inv_inertia_sqrt(&self, _rot: &Rotation<f32>) -> AngularInertia<f32> { - self.inv_principal_inertia_sqrt - } - - #[cfg(feature = "dim3")] - /// The world-space inverse angular inertia tensor of the rigid-body. - pub fn world_inv_inertia_sqrt(&self, rot: &Rotation<f32>) -> AngularInertia<f32> { - if !self.inv_principal_inertia_sqrt.is_zero() { - let mut lhs = (rot * self.principal_inertia_local_frame) - .to_rotation_matrix() - .into_inner(); - let rhs = lhs.transpose(); - lhs.column_mut(0) - .mul_assign(self.inv_principal_inertia_sqrt.x); - lhs.column_mut(1) - .mul_assign(self.inv_principal_inertia_sqrt.y); - lhs.column_mut(2) - .mul_assign(self.inv_principal_inertia_sqrt.z); - let inertia = lhs * rhs; - AngularInertia::from_sdp_matrix(inertia) - } else { - AngularInertia::zero() - } - } - - #[cfg(feature = "dim3")] - /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes. - pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> { - 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 axes. - pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> { - let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e)); - self.principal_inertia_local_frame.to_rotation_matrix() - * Matrix3::from_diagonal(&principal_inertia) - * self - .principal_inertia_local_frame - .inverse() - .to_rotation_matrix() - } - - #[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; - i + shift.norm_squared() * mass - } else { - 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 diag = shift.norm_squared(); - let diagm = Matrix3::from_diagonal_element(diag); - matrix + (diagm + shift * shift.transpose()) * mass - } else { - matrix - } - } - - /// 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 { - fn zero() -> Self { - Self { - inv_mass: 0.0, - inv_principal_inertia_sqrt: na::zero(), - #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), - local_com: Point::origin(), - } - } - - fn is_zero(&self) -> bool { - *self == Self::zero() - } -} - -impl Sub<MassProperties> 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_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())); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - principal_inertia_local_frame, - } - } -} - -impl SubAssign<MassProperties> for MassProperties { - fn sub_assign(&mut self, rhs: MassProperties) { - *self = *self - rhs - } -} - -impl Add<MassProperties> for MassProperties { - type Output = Self; - - #[cfg(feature = "dim2")] - fn add(self, other: MassProperties) -> Self { - if self.is_zero() { - return other; - } else if 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 inv_principal_inertia_sqrt = utils::inv(inertia.sqrt()); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - } - } - - #[cfg(feature = "dim3")] - fn add(self, other: MassProperties) -> Self { - if self.is_zero() { - return other; - } else if 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_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())); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - principal_inertia_local_frame, - } - } -} - -impl AddAssign<MassProperties> for MassProperties { - fn add_assign(&mut self, rhs: MassProperties) { - *self = *self + rhs - } -} - -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 { - #[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) - } -} - -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; - 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. - 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 - ); - } -} diff --git a/src/dynamics/mass_properties_ball.rs b/src/dynamics/mass_properties_ball.rs deleted file mode 100644 index ac5790a..0000000 --- a/src/dynamics/mass_properties_ball.rs +++ /dev/null @@ -1,30 +0,0 @@ -use crate::dynamics::MassProperties; -#[cfg(feature = "dim3")] -use crate::math::Vector; -use crate::math::{Point, PrincipalAngularInertia}; - -impl MassProperties { - pub(crate) fn ball_volume_unit_angular_inertia( - radius: f32, - ) -> (f32, PrincipalAngularInertia<f32>) { - #[cfg(feature = "dim2")] - { - let volume = std::f32::consts::PI * radius * radius; - let i = radius * radius / 2.0; - (volume, i) - } - #[cfg(feature = "dim3")] - { - let volume = std::f32::consts::PI * radius * radius * radius * 4.0 / 3.0; - let i = radius * radius * 2.0 / 5.0; - - (volume, Vector::repeat(i)) - } - } - - pub(crate) fn from_ball(density: f32, radius: f32) -> Self { - let (vol, unit_i) = Self::ball_volume_unit_angular_inertia(radius); - let mass = vol * density; - Self::new(Point::origin(), mass, unit_i * mass) - } -} diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs deleted file mode 100644 index 3b1b214..0000000 --- a/src/dynamics/mass_properties_capsule.rs +++ /dev/null @@ -1,39 +0,0 @@ -use crate::dynamics::MassProperties; -#[cfg(feature = "dim3")] -use crate::geometry::Capsule; -use crate::math::Point; - -impl MassProperties { - pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self { - let half_height = (b - a).norm() / 2.0; - let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); - let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius); - let cap_vol = cyl_vol + ball_vol; - let cap_mass = cap_vol * density; - let mut cap_unit_i = cyl_unit_i + ball_unit_i; - let local_com = na::center(&a, &b); - - #[cfg(feature = "dim2")] - { - let h = half_height * 2.0; - let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; - cap_unit_i += extra; - Self::new(local_com, cap_mass, cap_unit_i * cap_mass) - } - - #[cfg(feature = "dim3")] - { - let h = half_height * 2.0; - let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; - cap_unit_i.x += extra; - cap_unit_i.z += extra; - let local_frame = Capsule::new(a, b, radius).rotation_wrt_y(); - Self::with_principal_inertia_frame( - local_com, - cap_mass, - cap_unit_i * cap_mass, - local_frame, - ) - } - } -} diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs deleted file mode 100644 index 12f831f..0000000 --- a/src/dynamics/mass_properties_cone.rs +++ /dev/null @@ -1,29 +0,0 @@ -use crate::dynamics::MassProperties; -use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; - -impl MassProperties { - pub(crate) fn cone_y_volume_unit_inertia( - half_height: f32, - radius: f32, - ) -> (f32, PrincipalAngularInertia<f32>) { - let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0; - let sq_radius = radius * radius; - let sq_height = half_height * half_height * 4.0; - let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0; - let principal = sq_radius * 3.0 / 10.0; - - (volume, Vector::new(off_principal, principal, off_principal)) - } - - pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self { - let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius); - let cyl_mass = cyl_vol * density; - - Self::with_principal_inertia_frame( - Point::new(0.0, -half_height / 2.0, 0.0), - cyl_mass, - cyl_unit_i * cyl_mass, - Rotation::identity(), - ) - } -} diff --git a/src/dynamics/mass_properties_cuboid.rs b/src/dynamics/mass_properties_cuboid.rs deleted file mode 100644 index 2d870cf..0000000 --- a/src/dynamics/mass_properties_cuboid.rs +++ /dev/null @@ -1,33 +0,0 @@ -use crate::dynamics::MassProperties; -use crate::math::{Point, PrincipalAngularInertia, Vector}; - -impl MassProperties { - pub(crate) fn cuboid_volume_unit_inertia( - half_extents: Vector<f32>, - ) -> (f32, PrincipalAngularInertia<f32>) { - #[cfg(feature = "dim2")] - { - let volume = half_extents.x * half_extents.y * 4.0; - let ix = (half_extents.x * half_extents.x) / 3.0; - let iy = (half_extents.y * half_extents.y) / 3.0; - - (volume, ix + iy) - } - - #[cfg(feature = "dim3")] - { - let volume = half_extents.x * half_extents.y * half_extents.z * 8.0; - let ix = (half_extents.x * half_extents.x) / 3.0; - let iy = (half_extents.y * half_extents.y) / 3.0; - let iz = (half_extents.z * half_extents.z) / 3.0; - - (volume, Vector::new(iy + iz, ix + iz, ix + iy)) - } - } - - pub(crate) fn from_cuboid(density: f32, half_extents: Vector<f32>) -> Self { - let (vol, unit_i) = Self::cuboid_volume_unit_inertia(half_extents); - let mass = vol * density; - Self::new(Point::origin(), mass, unit_i * mass) - } -} diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs deleted file mode 100644 index 7c8054a..0000000 --- a/src/dynamics/mass_properties_cylinder.rs +++ /dev/null @@ -1,40 +0,0 @@ -use crate::dynamics::MassProperties; -#[cfg(feature = "dim3")] -use crate::math::{Point, Rotation}; -use crate::math::{PrincipalAngularInertia, Vector}; - -impl MassProperties { - pub(crate) fn cylinder_y_volume_unit_inertia( - half_height: f32, - radius: f32, - ) -> (f32, PrincipalAngularInertia<f32>) { - #[cfg(feature = "dim2")] - { - Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) - } - - #[cfg(feature = "dim3")] - { - let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; - let sq_radius = radius * radius; - let sq_height = half_height * half_height * 4.0; - let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; - - let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); - (volume, inertia) - } - } - - #[cfg(feature = "dim3")] - pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self { - let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); - let cyl_mass = cyl_vol * density; - - Self::with_principal_inertia_frame( - Point::origin(), - cyl_mass, - cyl_unit_i * cyl_mass, - Rotation::identity(), - ) - } -} diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs deleted file mode 100644 index 8b0b811..0000000 --- a/src/dynamics/mass_properties_polygon.rs +++ /dev/null @@ -1,146 +0,0 @@ -#![allow(dead_code)] // TODO: remove this - -use crate::dynamics::MassProperties; -use crate::math::Point; - -impl MassProperties { - pub(crate) fn from_polygon(density: f32, vertices: &[Point<f32>]) -> MassProperties { - let (area, com) = convex_polygon_area_and_center_of_mass(vertices); - - if area == 0.0 { - return MassProperties::new(com, 0.0, 0.0); - } - - let mut itot = 0.0; - let factor = 1.0 / 6.0; - - let mut iterpeek = vertices.iter().peekable(); - let firstelement = *iterpeek.peek().unwrap(); // store first element to close the cycle in the end with unwrap_or - while let Some(elem) = iterpeek.next() { - let area = triangle_area(&com, elem, iterpeek.peek().unwrap_or(&firstelement)); - - // algorithm adapted from Box2D - let e1 = *elem - com; - let e2 = **(iterpeek.peek().unwrap_or(&firstelement)) - com; - - let ex1 = e1[0]; - let ey1 = e1[1]; - let ex2 = e2[0]; - let ey2 = e2[1]; - - let intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2; - let inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2; - - let ipart = factor * (intx2 + inty2); - - itot += ipart * area; - } - - Self::new(com, area * density, itot * density) - } -} - -fn convex_polygon_area_and_center_of_mass(convex_polygon: &[Point<f32>]) -> (f32, Point<f32>) { - let geometric_center = convex_polygon - .iter() - .fold(Point::origin(), |e1, e2| e1 + e2.coords) - / convex_polygon.len() as f32; - let mut res = Point::origin(); - let mut areasum = 0.0; - - let mut iterpeek = convex_polygon.iter().peekable(); - let firstelement = *iterpeek.peek().unwrap(); // Stores first element to close the cycle in the end with unwrap_or. - while let Some(elem) = iterpeek.next() { - let (a, b, c) = ( - elem, - iterpeek.peek().unwrap_or(&firstelement), - &geometric_center, - ); - let area = triangle_area(a, b, c); - let center = (a.coords + b.coords + c.coords) / 3.0; - - res += center * area; - areasum += area; - } - - if areasum == 0.0 { - (areasum, geometric_center) - } else { - (areasum, res / areasum) - } -} - -pub fn triangle_area(pa: &Point<f32>, pb: &Point<f32>, pc: &Point<f32>) -> f32 { - // Kahan's formula. - let a = na::distance(pa, pb); - let b = na::distance(pb, pc); - let c = na::distance(pc, pa); - - let (c, b, a) = sort3(&a, &b, &c); - let a = *a; - let b = *b; - let c = *c; - - let sqr = (a + (b + c)) * (c - (a - b)) * (c + (a - b)) * (a + (b - c)); - - sqr.sqrt() * 0.25 -} - -/// Sorts a set of three values in increasing order. -#[inline] -pub fn sort3<'a>(a: &'a f32, b: &'a f32, c: &'a f32) -> (&'a f32, &'a f32, &'a f32) { - let a_b = *a > *b; - let a_c = *a > *c; - let b_c = *b > *c; - - let sa; - let sb; - let sc; - - // Sort the three values. - // FIXME: move this to the utilities? - if a_b { - // a > b - if a_c { - // a > c - sc = a; - - if b_c { - // b > c - sa = c; - sb = b; - } else { - // b <= c - sa = b; - sb = c; - } - } else { - // a <= c - sa = b; - sb = a; - sc = c; - } - } else { - // a < b - if !a_c { - // a <= c - sa = a; - - if b_c { - // b > c - sb = c; - sc = b; - } else { - sb = b; - sc = c; - } - } else { - // a > c - sa = c; - sb = a; - sc = b; - } - } - - (sa, sb, sc) -} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 6967904..28f149a 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -7,9 +7,9 @@ pub use self::joint::RevoluteJoint; pub use self::joint::{ BallJoint, FixedJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint, }; -pub use self::mass_properties::MassProperties; pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder}; pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet}; +pub use buckler::shape::MassProperties; // #[cfg(not(feature = "parallel"))] pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::rigid_body::RigidBodyChanges; @@ -20,15 +20,6 @@ pub(crate) use self::solver::ParallelIslandSolver; mod integration_parameters; mod joint; -mod mass_properties; -mod mass_properties_ball; -mod mass_properties_capsule; -#[cfg(feature = "dim3")] -mod mass_properties_cone; -mod mass_properties_cuboid; -mod mass_properties_cylinder; -#[cfg(feature = "dim2")] -mod mass_properties_polygon; mod rigid_body; mod rigid_body_set; mod solver; diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index c552d57..f1ca4b6 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -1,7 +1,7 @@ use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdFloat, SIMD_WIDTH}; +use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -10,17 +10,17 @@ pub(crate) struct WBallPositionConstraint { position1: [usize; SIMD_WIDTH], position2: [usize; SIMD_WIDTH], - local_com1: Point<SimdFloat>, - local_com2: Point<SimdFloat>, + local_com1: Point<SimdReal>, + local_com2: Point<SimdReal>, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, - ii1: AngularInertia<SimdFloat>, - ii2: AngularInertia<SimdFloat>, + ii1: AngularInertia<SimdReal>, + ii2: AngularInertia<SimdReal>, - local_anchor1: Point<SimdFloat>, - local_anchor2: Point<SimdFloat>, + local_anchor1: Point<SimdReal>, + local_anchor2: Point<SimdReal>, } impl WBallPositionConstraint { @@ -31,13 +31,13 @@ impl WBallPositionConstraint { ) -> Self { let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]); let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); - let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1 = AngularInertia::<SimdFloat>::from( + let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1 = AngularInertia::<SimdReal>::from( array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH |