diff --git a/Cargo.toml b/Cargo.toml index 6d8c3c3..cf6a253 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -21,25 +21,36 @@ default = ["std", "2d", "3d", "bevy_reflect"] 3d = ["bevy_math/alloc"] # Enable reflection using `bevy_reflect`. -bevy_reflect = ["dep:bevy_reflect", "bevy_math/bevy_reflect"] +bevy_reflect = [ + "dep:bevy_reflect", + "bevy_math/bevy_reflect", + "glam_matrix_extensions/bevy_reflect", +] # Enable data serialization/deserialization using `serde`. -serialize = ["dep:serde", "bevy_math/serialize"] +serialize = ["dep:serde", "bevy_math/serialize", "glam_matrix_extensions/serde"] # Enable `approx` to approximate floating point equality comparisons and assertions for types. -approx = ["dep:approx", "bevy_math/approx"] +approx = ["dep:approx", "bevy_math/approx", "glam_matrix_extensions/approx"] # Enable libm mathematical functions for glam types to ensure consistent outputs # across platforms at the cost of losing hardware-level optimization using intrinsics. -libm = ["bevy_math/libm"] +libm = ["bevy_math/libm", "glam_matrix_extensions/libm"] + +# Enable libm mathematical functions as a fallback for no_std environments. +# Can be overridden with std feature. +nostd-libm = ["bevy_math/nostd-libm", "glam_matrix_extensions/nostd-libm"] # Enable the Rust standard library. -std = ["bevy_math/std"] +std = ["bevy_math/std", "glam_matrix_extensions/std"] [dependencies] # Math approx = { version = "0.5", optional = true } bevy_math = { version = "0.16", default-features = false } +glam_matrix_extensions = { git = "https://github.com/Jondolf/glam_matrix_extensions", default-features = false, features = [ + "f32", +] } # Serialization bevy_reflect = { version = "0.16", default-features = false, optional = true } diff --git a/src/dim3/angular_inertia.rs b/src/dim3/angular_inertia.rs index 7955b24..47eb1e3 100644 --- a/src/dim3/angular_inertia.rs +++ b/src/dim3/angular_inertia.rs @@ -1,13 +1,10 @@ use core::ops::*; -use crate::MatExt; use bevy_math::{Mat3, Quat, Vec3}; #[cfg(all(feature = "bevy_reflect", feature = "serialize"))] use bevy_reflect::{ReflectDeserialize, ReflectSerialize}; +use glam_matrix_extensions::{MatConversionError, SymmetricEigen3, SymmetricMat3}; -use super::SymmetricEigen3; - -// TODO: Add errors for asymmetric and non-positive-semidefinite matrices. /// An error returned for an invalid [`AngularInertiaTensor`] in 3D. #[derive(Clone, Copy, Debug, PartialEq)] pub enum AngularInertiaTensorError { @@ -17,8 +14,6 @@ pub enum AngularInertiaTensorError { Nan, } -// TODO: The matrix should be symmetric and positive-semidefinite. -// We could add a custom `SymmetricMat3` type to enforce symmetricity and reduce memory usage. /// The 3x3 [angular inertia] tensor of a 3D object, representing resistance to angular acceleration. /// /// The [inertia tensor] is a [symmetric], [positive-semidefinite] matrix that describes the moment of inertia @@ -45,10 +40,10 @@ pub enum AngularInertiaTensorError { reflect(Serialize, Deserialize) )] #[doc(alias = "MomentOfInertiaTensor")] -pub struct AngularInertiaTensor(Mat3); +pub struct AngularInertiaTensor(SymmetricMat3); impl Deref for AngularInertiaTensor { - type Target = Mat3; + type Target = SymmetricMat3; #[inline] fn deref(&self) -> &Self::Target { @@ -58,13 +53,13 @@ impl Deref for AngularInertiaTensor { impl AngularInertiaTensor { /// Zero angular inertia. - pub const ZERO: Self = Self(Mat3::ZERO); + pub const ZERO: Self = Self(SymmetricMat3::ZERO); /// An angular inertia tensor with a principal angular inertia of `1.0` along the diagonal. - pub const IDENTITY: Self = Self(Mat3::IDENTITY); + pub const IDENTITY: Self = Self(SymmetricMat3::IDENTITY); /// Infinite angular inertia. - pub const INFINITY: Self = Self(Mat3::from_diagonal(Vec3::INFINITY)); + pub const INFINITY: Self = Self(SymmetricMat3::from_diagonal(Vec3::INFINITY)); /// Creates a new [`AngularInertiaTensor`] from the given principal angular inertia. /// @@ -84,7 +79,7 @@ impl AngularInertiaTensor { "principal angular inertia must be positive or zero for all axes" ); - Self(Mat3::from_diagonal(principal_angular_inertia)) + Self(SymmetricMat3::from_diagonal(principal_angular_inertia)) } /// Tries to create a new [`AngularInertiaTensor`] from the given principal angular inertia. @@ -104,7 +99,9 @@ impl AngularInertiaTensor { } else if principal_angular_inertia.is_nan() { Err(AngularInertiaTensorError::Nan) } else { - Ok(Self(Mat3::from_diagonal(principal_angular_inertia))) + Ok(Self(SymmetricMat3::from_diagonal( + principal_angular_inertia, + ))) } } @@ -126,11 +123,11 @@ impl AngularInertiaTensor { "principal angular inertia must be positive or zero for all axes" ); - Self( + Self(SymmetricMat3::from_mat3_unchecked( Mat3::from_quat(orientation) * Mat3::from_diagonal(principal_angular_inertia) * Mat3::from_quat(orientation.inverse()), - ) + )) } /// Tries to create a new [`AngularInertiaTensor`] from the given principal angular inertia @@ -153,67 +150,96 @@ impl AngularInertiaTensor { } else if principal_angular_inertia.is_nan() { Err(AngularInertiaTensorError::Nan) } else { - Ok(Self( + Ok(Self(SymmetricMat3::from_mat3_unchecked( Mat3::from_quat(orientation) * Mat3::from_diagonal(principal_angular_inertia) * Mat3::from_quat(orientation.inverse()), - )) + ))) } } - /// Creates a new [`AngularInertiaTensor`] from the given angular inertia [tensor]. + /// Creates a new [`AngularInertiaTensor`] from the given angular inertia [tensor] + /// represented as a [`SymmetricMat3`]. /// - /// The tensor should be [symmetric] and [positive-semidefinite], but this is *not* checked. + /// The tensor should be [positive-semidefinite], but this is *not* checked. /// /// [tensor]: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor - /// [symmetric]: https://en.wikipedia.org/wiki/Symmetric_matrix /// [positive-semidefinite]: https://en.wikipedia.org/wiki/Definite_matrix #[inline] + #[must_use] #[doc(alias = "from_tensor")] - pub fn from_mat3(mat: Mat3) -> Self { + pub const fn from_symmetric_mat3(mat: SymmetricMat3) -> Self { Self(mat) } - /// Returns the angular inertia tensor as a [`Mat3`]. + /// Tries to create a new [`AngularInertiaTensor`] from the given angular inertia [tensor] + /// represented as a [`Mat3`]. + /// + /// The tensor should be [positive-semidefinite], but this is *not* checked. + /// + /// [tensor]: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor + /// [positive-semidefinite]: https://en.wikipedia.org/wiki/Definite_matrix + /// + /// # Errors + /// + /// Returns a [`MatConversionError`] if the given matrix is not symmetric. + #[inline] + pub fn try_from_mat3(mat: Mat3) -> Result { + SymmetricMat3::try_from_mat3(mat).map(Self) + } + + /// Creates a new [`AngularInertiaTensor`] from the given angular inertia [tensor] + /// represented as a [`Mat3`]. + /// + /// Only the lower left triangle of the matrix is used. No check is performed to ensure + /// that the given matrix is truly symmetric or positive-semidefinite. + #[inline] + #[must_use] + pub const fn from_mat3_unchecked(mat: Mat3) -> Self { + Self(SymmetricMat3::from_mat3_unchecked(mat)) + } + + /// Returns the angular inertia tensor as a [`SymmetricMat3`]. /// /// Equivalent to [`value`](AngularInertiaTensor::value). - #[doc(alias = "as_tensor")] #[inline] - pub fn as_mat3(&self) -> Mat3 { + #[doc(alias = "as_tensor")] + pub fn as_symmetric_mat3(&self) -> SymmetricMat3 { self.0 } - /// Returns a mutable reference to the [`Mat3`] stored in `self`. - /// - /// Note that this allows making changes that could make the angular inertia tensor invalid - /// (non-symmetric or non-positive-semidefinite). + /// Returns a mutable reference to the [`SymmetricMat3`] stored in `self`. /// /// Equivalent to [`value_mut`](AngularInertiaTensor::value_mut). - #[doc(alias = "as_tensor_mut")] #[inline] - pub fn as_mat3_mut(&mut self) -> &mut Mat3 { + #[doc(alias = "as_tensor_mut")] + pub fn as_symmetric_mat3_mut(&mut self) -> &mut SymmetricMat3 { &mut self.0 } - /// Returns the angular inertia tensor as a [`Mat3`]. + /// Returns the angular inertia tensor as a [`SymmetricMat3`]. /// - /// Equivalent to [`as_mat3`](AngularInertiaTensor::as_mat3). + /// Equivalent to [`as_symmetric_mat3`](AngularInertiaTensor::as_symmetric_mat3). #[inline] - pub fn value(self) -> Mat3 { + pub fn value(self) -> SymmetricMat3 { self.0 } - /// Returns a mutable reference to the [`Mat3`] stored in `self`. - /// - /// Note that this allows making changes that could make the angular inertia tensor invalid - /// (non-symmetric or non-positive-semidefinite). + /// Returns a mutable reference to the [`SymmetricMat3`] stored in `self`. /// - /// Equivalent to [`as_mat3_mut`](AngularInertiaTensor::as_mat3_mut). + /// Equivalent to [`as_symmetric_mat3_mut`](AngularInertiaTensor::as_symmetric_mat3_mut). #[inline] - pub fn value_mut(&mut self) -> &mut Mat3 { + pub fn value_mut(&mut self) -> &mut SymmetricMat3 { &mut self.0 } + /// Returns the angular inertia tensor as a [`Mat3`]. + #[inline] + #[doc(alias = "to_tensor")] + pub fn to_mat3(&self) -> Mat3 { + self.0.to_mat3() + } + /// Returns the inverse of the angular inertia tensor. #[inline] pub fn inverse(self) -> Self { @@ -261,7 +287,7 @@ impl AngularInertiaTensor { #[inline] pub fn rotated(self, rotation: Quat) -> Self { let rot_mat3 = Mat3::from_quat(rotation); - Self::from_mat3((rot_mat3 * self.0) * rot_mat3.transpose()) + Self::from_mat3_unchecked((rot_mat3 * self.0) * rot_mat3.transpose()) } /// Computes the angular inertia tensor shifted by the given offset, taking into account the given mass. @@ -273,27 +299,43 @@ impl AngularInertiaTensor { let diagonal_mat = Mat3::from_diagonal(Vec3::splat(diagonal_element)); let offset_outer_product = Mat3::from_cols(offset * offset.x, offset * offset.y, offset * offset.z); - Self::from_mat3(self.0 + mass * (diagonal_mat - offset_outer_product)) + Self::from_mat3_unchecked(self.0 + mass * (diagonal_mat - offset_outer_product)) } else { self } } } -impl From for AngularInertiaTensor { +impl From for AngularInertiaTensor { #[inline] - fn from(angular_inertia: Mat3) -> Self { - Self::from_mat3(angular_inertia) + fn from(angular_inertia: SymmetricMat3) -> Self { + Self::from_symmetric_mat3(angular_inertia) } } -impl From for Mat3 { +impl TryFrom for AngularInertiaTensor { + type Error = MatConversionError; + + #[inline] + fn try_from(angular_inertia: Mat3) -> Result { + Self::try_from_mat3(angular_inertia) + } +} + +impl From for SymmetricMat3 { #[inline] fn from(angular_inertia: AngularInertiaTensor) -> Self { angular_inertia.0 } } +impl From for Mat3 { + #[inline] + fn from(angular_inertia: AngularInertiaTensor) -> Self { + angular_inertia.0.to_mat3() + } +} + impl TryFrom for AngularInertiaTensor { type Error = AngularInertiaTensorError; @@ -376,7 +418,7 @@ impl approx::AbsDiffEq for AngularInertiaTensor { f32::EPSILON } fn abs_diff_eq(&self, other: &Self, epsilon: f32) -> bool { - self.0.abs_diff_eq(other.0, epsilon) + self.0.abs_diff_eq(&other.0, epsilon) } } diff --git a/src/dim3/eigen3.rs b/src/dim3/eigen3.rs deleted file mode 100644 index 228529c..0000000 --- a/src/dim3/eigen3.rs +++ /dev/null @@ -1,326 +0,0 @@ -// The eigensolver is a Rust adaptation, with modifications, of the pseudocode and approach described in -// "A Robust Eigensolver for 3 x 3 Symmetric Matrices" by David Eberly, Geometric Tools, Redmond WA 98052. -// https://www.geometrictools.com/Documentation/RobustEigenSymmetric3x3.pdf - -use bevy_math::{ops, FloatPow, Mat3, Vec3, Vec3Swizzles}; - -/// The [eigen decomposition] of a [symmetric] 3x3 matrix. -/// -/// [eigen decomposition]: https://en.wikipedia.org/wiki/Eigendecomposition_of_a_matrix -/// [symmetric]: https://en.wikipedia.org/wiki/Symmetric_matrix -#[derive(Clone, Copy, Debug, PartialEq)] -#[cfg_attr(feature = "bevy_reflect", derive(bevy_reflect::Reflect))] -#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] -pub struct SymmetricEigen3 { - /// The eigenvalues of the symmetric 3x3 matrix. - /// - /// These should be in ascending order `eigen1 <= eigen2 <= eigen3`. - pub eigenvalues: Vec3, - /// The three eigenvectors of the symmetric 3x3 matrix. - /// Each eigenvector should be unit length and orthogonal to the other eigenvectors. - /// - /// The eigenvectors are ordered to correspond to the eigenvalues. For example, - /// `eigenvectors.x_axis` corresponds to `eigenvalues.x`. - pub eigenvectors: Mat3, -} - -impl SymmetricEigen3 { - /// Computes the eigen decomposition of the given symmetric 3x3 matrix. - /// - /// The eigenvalues are returned in ascending order `eigen1 <= eigen2 <= eigen3`. - /// This can be reversed with the [`reverse`](Self::reverse) method. - pub fn new(mat: Mat3) -> Self { - let (mut eigenvalues, is_diagonal) = Self::eigenvalues(mat); - - if is_diagonal { - // The matrix is already diagonal. Sort the eigenvalues in ascending order, - // ordering the eigenvectors accordingly, and return early. - let mut eigenvectors = Mat3::IDENTITY; - if eigenvalues[0] > eigenvalues[1] { - core::mem::swap(&mut eigenvalues.x, &mut eigenvalues.y); - core::mem::swap(&mut eigenvectors.x_axis, &mut eigenvectors.y_axis); - } - if eigenvalues[1] > eigenvalues[2] { - core::mem::swap(&mut eigenvalues.y, &mut eigenvalues.z); - core::mem::swap(&mut eigenvectors.y_axis, &mut eigenvectors.z_axis); - } - return Self { - eigenvalues, - eigenvectors, - }; - } - - // Compute the eigenvectors corresponding to the eigenvalues. - let eigenvector1 = Self::eigenvector1(mat, eigenvalues.x); - let eigenvector2 = Self::eigenvector2(mat, eigenvector1, eigenvalues.y); - let eigenvector3 = Self::eigenvector3(eigenvector1, eigenvector2); - - Self { - eigenvalues, - eigenvectors: Mat3::from_cols(eigenvector1, eigenvector2, eigenvector3), - } - } - - /// Reverses the order of the eigenvalues and their corresponding eigenvectors. - pub fn reverse(&self) -> Self { - Self { - eigenvalues: self.eigenvalues.zyx(), - eigenvectors: Mat3::from_cols( - self.eigenvectors.z_axis, - self.eigenvectors.y_axis, - self.eigenvectors.x_axis, - ), - } - } - - /// Computes the eigenvalues of a symmetric 3x3 matrix, also returning whether the input matrix is diagonal. - /// - /// If the matrix is already diagonal, the eigenvalues are returned as is without reordering. - /// Otherwise, the eigenvalues are computed and returned in ascending order - /// such that `eigen1 <= eigen2 <= eigen3`. - pub fn eigenvalues(mat: Mat3) -> (Vec3, bool) { - // Reference: https://en.wikipedia.org/wiki/Eigenvalue_algorithm#Symmetric_3%C3%973_matrices - - let p1 = mat.y_axis.x.squared() + mat.z_axis.x.squared() + mat.z_axis.y.squared(); - - if p1 == 0.0 { - // The matrix is diagonal. - return (Vec3::new(mat.x_axis.x, mat.y_axis.y, mat.z_axis.z), true); - } - - let q = (mat.x_axis.x + mat.y_axis.y + mat.z_axis.z) / 3.0; - let p2 = (mat.x_axis.x - q).squared() - + (mat.y_axis.y - q).squared() - + (mat.z_axis.z - q).squared() - + 2.0 * p1; - let p = ops::sqrt(p2 / 6.0); - let mat_b = 1.0 / p * (mat - q * Mat3::IDENTITY); - let r = mat_b.determinant() / 2.0; - - // r should be in the [-1, 1] range for a symmetric matrix, - // but computation error can leave it slightly outside this range. - let phi = if r <= -1.0 { - core::f32::consts::FRAC_PI_3 - } else if r >= 1.0 { - 0.0 - } else { - ops::acos(r) / 3.0 - }; - - // The eigenvalues satisfy eigen3 <= eigen2 <= eigen1 - let eigen1 = q + 2.0 * p * ops::cos(phi); - let eigen3 = q + 2.0 * p * ops::cos(phi + 2.0 * core::f32::consts::FRAC_PI_3); - let eigen2 = 3.0 * q - eigen1 - eigen3; // trace(mat) = eigen1 + eigen2 + eigen3 - (Vec3::new(eigen3, eigen2, eigen1), false) - } - - // TODO: Fall back to QL when the eigenvalue precision is poor. - /// Computes the unit-length eigenvector corresponding to the `eigenvalue1` of `mat` that was - /// computed from the root of a cubic polynomial with a multiplicity of 1. - /// - /// If the other two eigenvalues are well separated, this method can be used for computing - /// all three eigenvectors. However, to avoid numerical issues when eigenvalues are close to - /// each other, it's recommended to use the `eigenvector2` method for the second eigenvector. - /// - /// The third eigenvector can be computed as the cross product of the first two. - pub fn eigenvector1(mat: Mat3, eigenvalue1: f32) -> Vec3 { - let cols = mat - Mat3::from_diagonal(Vec3::splat(eigenvalue1)); - let c0xc1 = cols.x_axis.cross(cols.y_axis); - let c0xc2 = cols.x_axis.cross(cols.z_axis); - let c1xc2 = cols.y_axis.cross(cols.z_axis); - let d0 = c0xc1.length_squared(); - let d1 = c0xc2.length_squared(); - let d2 = c1xc2.length_squared(); - - let mut d_max = d0; - let mut i_max = 0; - - if d1 > d_max { - d_max = d1; - i_max = 1; - } - if d2 > d_max { - i_max = 2; - } - if i_max == 0 { - c0xc1 / ops::sqrt(d0) - } else if i_max == 1 { - c0xc2 / ops::sqrt(d1) - } else { - c1xc2 / ops::sqrt(d2) - } - } - - /// Computes the unit-length eigenvector corresponding to the `eigenvalue2` of `mat` that was - /// computed from the root of a cubic polynomial with a potential multiplicity of 2. - /// - /// The third eigenvector can be computed as the cross product of the first two. - pub fn eigenvector2(mat: Mat3, eigenvector1: Vec3, eigenvalue2: f32) -> Vec3 { - // Compute right-handed orthonormal set { U, V, W }, where W is eigenvector1. - let (u, v) = eigenvector1.any_orthonormal_pair(); - - // The unit-length eigenvector is E = x0 * U + x1 * V. We need to compute x0 and x1. - // - // Define the symmetrix 2x2 matrix M = J^T * (mat - eigenvalue2 * I), where J = [U V] - // and I is a 3x3 identity matrix. This means that E = J * X, where X is a column vector - // with rows x0 and x1. The 3x3 linear system (mat - eigenvalue2 * I) * E = 0 reduces to - // the 2x2 linear system M * X = 0. - // - // When eigenvalue2 != eigenvalue3, M has rank 1 and is not the zero matrix. - // Otherwise, it has rank 0, and it is the zero matrix. - - let au = mat * u; - let av = mat * v; - - let mut m00 = u.dot(au) - eigenvalue2; - let mut m01 = u.dot(av); - let mut m11 = v.dot(av) - eigenvalue2; - let (abs_m00, abs_m01, abs_m11) = (ops::abs(m00), ops::abs(m01), ops::abs(m11)); - - if abs_m00 >= abs_m11 { - let max_abs_component = abs_m00.max(abs_m01); - if max_abs_component > 0.0 { - if abs_m00 >= abs_m01 { - // m00 is the largest component of the row. - // Factor it out for normalization and discard to avoid underflow or overflow. - m01 /= m00; - m00 = 1.0 / ops::sqrt(1.0 + m01 * m01); - m01 *= m00; - } else { - // m01 is the largest component of the row. - // Factor it out for normalization and discard to avoid underflow or overflow. - m00 /= m01; - m01 = 1.0 / ops::sqrt(1.0 + m00 * m00); - m00 *= m01; - } - return m01 * u - m00 * v; - } - } else { - let max_abs_component = abs_m11.max(abs_m01); - if max_abs_component > 0.0 { - if abs_m11 >= abs_m01 { - // m11 is the largest component of the row. - // Factor it out for normalization and discard to avoid underflow or overflow. - m01 /= m11; - m11 = 1.0 / ops::sqrt(1.0 + m01 * m01); - m01 *= m11; - } else { - // m01 is the largest component of the row. - // Factor it out for normalization and discard to avoid underflow or overflow. - m11 /= m01; - m01 = 1.0 / ops::sqrt(1.0 + m11 * m11); - m11 *= m01; - } - return m11 * u - m01 * v; - } - } - - // M is the zero matrix, any unit-length solution suffices. - u - } - - /// Computes the third eigenvector as the cross product of the first two. - /// If the given eigenvectors are valid, the returned vector should be unit length. - pub fn eigenvector3(eigenvector1: Vec3, eigenvector2: Vec3) -> Vec3 { - eigenvector1.cross(eigenvector2) - } -} - -#[cfg(test)] -mod test { - use super::SymmetricEigen3; - use approx::assert_relative_eq; - use bevy_math::{Mat3, Vec3}; - use rand::{Rng, SeedableRng}; - - #[test] - fn eigen_3x3() { - let mat = Mat3::from_cols_array_2d(&[[2.0, 7.0, 8.0], [7.0, 6.0, 3.0], [8.0, 3.0, 0.0]]); - let eigen = SymmetricEigen3::new(mat); - - assert_relative_eq!( - eigen.eigenvalues, - Vec3::new(-7.605, 0.577, 15.028), - epsilon = 0.001 - ); - assert_relative_eq!( - Mat3::from_cols( - eigen.eigenvectors.x_axis.abs(), - eigen.eigenvectors.y_axis.abs(), - eigen.eigenvectors.z_axis.abs() - ), - Mat3::from_cols( - Vec3::new(-1.075, 0.333, 1.0).normalize().abs(), - Vec3::new(0.542, -1.253, 1.0).normalize().abs(), - Vec3::new(1.359, 1.386, 1.0).normalize().abs() - ), - epsilon = 0.001 - ); - } - - #[test] - fn eigen_3x3_diagonal() { - let mat = Mat3::from_cols_array_2d(&[[2.0, 0.0, 0.0], [0.0, 5.0, 0.0], [0.0, 0.0, 3.0]]); - let eigen = SymmetricEigen3::new(mat); - - assert_eq!(eigen.eigenvalues, Vec3::new(2.0, 3.0, 5.0)); - assert_eq!( - Mat3::from_cols( - eigen.eigenvectors.x_axis.normalize().abs(), - eigen.eigenvectors.y_axis.normalize().abs(), - eigen.eigenvectors.z_axis.normalize().abs() - ), - Mat3::from_cols_array_2d(&[[1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, 1.0, 0.0]]) - ); - } - - #[test] - fn eigen_3x3_reconstruction() { - let mut rng = rand_chacha::ChaCha8Rng::from_seed(Default::default()); - - // Generate random symmetric matrices and verify that the eigen decomposition is correct. - for _ in 0..10_000 { - let eigenvalues = Vec3::new( - rng.gen_range(0.1..100.0), - rng.gen_range(0.1..100.0), - rng.gen_range(0.1..100.0), - ); - let eigenvectors = Mat3::from_cols( - Vec3::new( - rng.gen_range(-1.0..1.0), - rng.gen_range(-1.0..1.0), - rng.gen_range(-1.0..1.0), - ) - .normalize(), - Vec3::new( - rng.gen_range(-1.0..1.0), - rng.gen_range(-1.0..1.0), - rng.gen_range(-1.0..1.0), - ) - .normalize(), - Vec3::new( - rng.gen_range(-1.0..1.0), - rng.gen_range(-1.0..1.0), - rng.gen_range(-1.0..1.0), - ) - .normalize(), - ); - - // Construct the symmetric matrix from the eigenvalues and eigenvectors. - let mat1 = eigenvectors * Mat3::from_diagonal(eigenvalues) * eigenvectors.transpose(); - - // Compute the eigen decomposition of the constructed matrix. - let eigen = SymmetricEigen3::new(mat1); - - // Reconstruct the matrix from the computed eigenvalues and eigenvectors. - let mat2 = eigen.eigenvectors - * Mat3::from_diagonal(eigen.eigenvalues) - * eigen.eigenvectors.transpose(); - - // The reconstructed matrix should be close to the original matrix. - // Note: The precision depends on how large the eigenvalues are. - // Larger eigenvalues can lead to larger absolute error. - assert_relative_eq!(mat1, mat2, epsilon = 1e-2); - } - } -} diff --git a/src/dim3/impls.rs b/src/dim3/impls.rs index aee48fe..1b51a29 100644 --- a/src/dim3/impls.rs +++ b/src/dim3/impls.rs @@ -6,8 +6,9 @@ use bevy_math::{ BoxedPolyline3d, Capsule3d, Cone, ConicalFrustum, Cuboid, Cylinder, Line3d, Measured3d, Plane3d, Polyline3d, Segment3d, Sphere, Torus, }, - FloatPow, Mat3, Quat, Vec3, + FloatPow, Quat, Vec3, }; +use glam_matrix_extensions::SymmetricMat3; impl ComputeMassProperties3d for Sphere { #[inline] @@ -548,9 +549,8 @@ impl ComputeMassProperties3d for Tetrahedron { + x4 * y4 * 2.0) * 0.05; - AngularInertiaTensor::from_mat3(Mat3::from_cols_array(&[ - a0, -b1, -c1, -b1, b0, -a1, -c1, -a1, c0, - ])) + // TODO: Are -b1 and -c1 flipped here? + AngularInertiaTensor::from_symmetric_mat3(SymmetricMat3::new(a0, -b1, -c1, b0, -a1, c0)) } #[inline] diff --git a/src/dim3/mod.rs b/src/dim3/mod.rs index d17c32e..caa80bc 100644 --- a/src/dim3/mod.rs +++ b/src/dim3/mod.rs @@ -10,9 +10,6 @@ pub use angular_inertia::{AngularInertiaTensor, AngularInertiaTensorError}; /// [`ComputeMassProperties3d`] implementations for 3D geometric primitives. mod impls; -mod eigen3; -pub use eigen3::SymmetricEigen3; - use crate::RecipOrZero; /// A trait for computing [`MassProperties3d`] for 3D objects. @@ -260,7 +257,7 @@ impl MassProperties3d { lhs.y_axis *= self.principal_angular_inertia.y; lhs.z_axis *= self.principal_angular_inertia.z; - AngularInertiaTensor::from_mat3(lhs * rhs) + AngularInertiaTensor::from_mat3_unchecked(lhs * rhs) } /// Returns the mass properties transformed by the given [isometry]. @@ -330,7 +327,9 @@ impl core::ops::Add for MassProperties3d { // Compute the new principal angular inertia, taking the new center of mass into account. let i1 = self.shifted_angular_inertia_tensor(new_center_of_mass - self.center_of_mass); let i2 = other.shifted_angular_inertia_tensor(new_center_of_mass - other.center_of_mass); - let new_angular_inertia = AngularInertiaTensor::from_mat3(i1.as_mat3() + i2.as_mat3()); + let new_angular_inertia = AngularInertiaTensor::from_symmetric_mat3( + i1.as_symmetric_mat3() + i2.as_symmetric_mat3(), + ); Self::new_with_angular_inertia_tensor(new_mass, new_angular_inertia, new_center_of_mass) } @@ -372,7 +371,9 @@ impl core::ops::Sub for MassProperties3d { // Compute the new principal angular inertia, taking the new center of mass into account. let i1 = self.shifted_angular_inertia_tensor(new_center_of_mass - self.center_of_mass); let i2 = other.shifted_angular_inertia_tensor(new_center_of_mass - other.center_of_mass); - let new_angular_inertia = AngularInertiaTensor::from_mat3(i1.as_mat3() - i2.as_mat3()); + let new_angular_inertia = AngularInertiaTensor::from_symmetric_mat3( + i1.as_symmetric_mat3() - i2.as_symmetric_mat3(), + ); Self::new_with_angular_inertia_tensor(new_mass, new_angular_inertia, new_center_of_mass) } diff --git a/src/lib.rs b/src/lib.rs index 8180114..1c4e5f2 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -149,6 +149,6 @@ pub use dim2::{ComputeMassProperties2d, MassProperties2d}; #[cfg(feature = "3d")] pub use dim3::{ AngularInertiaTensor, AngularInertiaTensorError, ComputeMassProperties3d, MassProperties3d, - SymmetricEigen3, }; -pub use math_ext::{MatExt, RecipOrZero}; +pub use glam_matrix_extensions::{Mat3Ext, MatConversionError, SquareMatExt, SymmetricMat3}; +pub use math_ext::RecipOrZero; diff --git a/src/math_ext.rs b/src/math_ext.rs index 1a8b24a..2f8b715 100644 --- a/src/math_ext.rs +++ b/src/math_ext.rs @@ -66,54 +66,3 @@ impl RecipOrZero for DVec3 { ) } } - -/// An extension trait for matrix types. -pub trait MatExt { - /// Computes the inverse of `self` if `self` is not zero, - /// and returns zero otherwise to avoid division by zero. - fn inverse_or_zero(self) -> Self; -} - -impl MatExt for Mat2 { - #[inline] - fn inverse_or_zero(self) -> Self { - if self.determinant() == 0.0 { - Self::ZERO - } else { - self.inverse() - } - } -} - -impl MatExt for DMat2 { - #[inline] - fn inverse_or_zero(self) -> Self { - if self.determinant() == 0.0 { - Self::ZERO - } else { - self.inverse() - } - } -} - -impl MatExt for Mat3 { - #[inline] - fn inverse_or_zero(self) -> Self { - if self.determinant() == 0.0 { - Self::ZERO - } else { - self.inverse() - } - } -} - -impl MatExt for DMat3 { - #[inline] - fn inverse_or_zero(self) -> Self { - if self.determinant() == 0.0 { - Self::ZERO - } else { - self.inverse() - } - } -}