From 7375a691e232bb59033980dc09c7179bc16e377f Mon Sep 17 00:00:00 2001 From: Bruce Mitchener Date: Sun, 5 May 2024 16:41:38 +0630 Subject: Fix some typos. (#620) --- src/utils.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/utils.rs') diff --git a/src/utils.rs b/src/utils.rs index 6521b0f..5025744 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -647,7 +647,7 @@ impl SimdAngularInertia for SdpMatrix3 { } // This is an RAII structure that enables flushing denormal numbers -// to zero, and automatically reseting previous flags once it is dropped. +// to zero, and automatically resetting previous flags once it is dropped. #[derive(Clone, Debug, PartialEq, Eq)] pub(crate) struct FlushToZeroDenormalsAreZeroFlags { original_flags: u32, -- cgit From 62379de9ecc81fb42b7c2a0d2b8e3e1b02d63f38 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 25 May 2024 10:36:34 +0200 Subject: feat: add simple inverse-kinematics solver for multibodies (#632) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat: add a simple jacobian-based inverse-kinematics implementation for multibodies * feat: add 2d inverse kinematics example * feat: make forward_kinematics auto-fix the root’s degrees of freedom * feat: add 3d inverse kinematics example * chore: update changelog * chore: clippy fixes * chore: more clippy fixes * fix tests --- src/utils.rs | 163 +---------------------------------------------------------- 1 file changed, 2 insertions(+), 161 deletions(-) (limited to 'src/utils.rs') diff --git a/src/utils.rs b/src/utils.rs index 5025744..102c85f 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,8 +1,8 @@ //! Miscellaneous utilities. use na::{ - Matrix1, Matrix2, Matrix3, Point2, Point3, RowVector2, Scalar, SimdRealField, UnitComplex, - UnitQuaternion, Vector1, Vector2, Vector3, + Matrix1, Matrix2, Matrix3, RowVector2, Scalar, SimdRealField, UnitComplex, UnitQuaternion, + Vector1, Vector2, Vector3, }; use num::Zero; use simba::simd::SimdValue; @@ -90,35 +90,6 @@ impl SimdSign for SimdReal { } } -pub(crate) trait SimdComponent: Sized { - type Element; - - fn min_component(self) -> Self::Element; - fn max_component(self) -> Self::Element; -} - -impl SimdComponent for Real { - type Element = Real; - - fn min_component(self) -> Self::Element { - self - } - fn max_component(self) -> Self::Element { - self - } -} - -impl SimdComponent for SimdReal { - type Element = Real; - - fn min_component(self) -> Self::Element { - self.simd_horizontal_min() - } - fn max_component(self) -> Self::Element { - self.simd_horizontal_max() - } -} - /// Trait to compute the orthonormal basis of a vector. pub trait SimdBasis: Sized { /// The type of the array of orthonormal vectors. @@ -166,89 +137,6 @@ impl> SimdBasis for Vector3 { } } -pub(crate) trait SimdVec: Sized { - type Element; - - fn horizontal_inf(&self) -> Self::Element; - fn horizontal_sup(&self) -> Self::Element; -} - -impl SimdVec for Vector2 -where - N::Element: Scalar, -{ - type Element = Vector2; - - fn horizontal_inf(&self) -> Self::Element { - Vector2::new(self.x.min_component(), self.y.min_component()) - } - - fn horizontal_sup(&self) -> Self::Element { - Vector2::new(self.x.max_component(), self.y.max_component()) - } -} - -impl SimdVec for Point2 -where - N::Element: Scalar, -{ - type Element = Point2; - - fn horizontal_inf(&self) -> Self::Element { - Point2::new(self.x.min_component(), self.y.min_component()) - } - - fn horizontal_sup(&self) -> Self::Element { - Point2::new(self.x.max_component(), self.y.max_component()) - } -} - -impl SimdVec for Vector3 -where - N::Element: Scalar, -{ - type Element = Vector3; - - fn horizontal_inf(&self) -> Self::Element { - Vector3::new( - self.x.min_component(), - self.y.min_component(), - self.z.min_component(), - ) - } - - fn horizontal_sup(&self) -> Self::Element { - Vector3::new( - self.x.max_component(), - self.y.max_component(), - self.z.max_component(), - ) - } -} - -impl SimdVec for Point3 -where - N::Element: Scalar, -{ - type Element = Point3; - - fn horizontal_inf(&self) -> Self::Element { - Point3::new( - self.x.min_component(), - self.y.min_component(), - self.z.min_component(), - ) - } - - fn horizontal_sup(&self) -> Self::Element { - Point3::new( - self.x.max_component(), - self.y.max_component(), - self.z.max_component(), - ) - } -} - pub(crate) trait SimdCrossMatrix: Sized { type CrossMat; type CrossMatTr; @@ -463,28 +351,21 @@ impl SimdQuat for UnitQuaternion { pub(crate) trait SimdAngularInertia { type AngVector; - type LinVector; type AngMatrix; fn inverse(&self) -> Self; - fn transform_lin_vector(&self, pt: Self::LinVector) -> Self::LinVector; fn transform_vector(&self, pt: Self::AngVector) -> Self::AngVector; fn squared(&self) -> Self; - fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix; fn into_matrix(self) -> Self::AngMatrix; } impl SimdAngularInertia for N { type AngVector = N; - type LinVector = Vector2; type AngMatrix = N; fn inverse(&self) -> Self { simd_inv(*self) } - fn transform_lin_vector(&self, pt: Vector2) -> Vector2 { - pt * *self - } fn transform_vector(&self, pt: N) -> N { pt * *self } @@ -493,10 +374,6 @@ impl SimdAngularInertia for N { *self * *self } - fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix { - *mat * *self - } - fn into_matrix(self) -> Self::AngMatrix { self } @@ -504,7 +381,6 @@ impl SimdAngularInertia for N { impl SimdAngularInertia for SdpMatrix3 { type AngVector = Vector3; - type LinVector = Vector3; type AngMatrix = Matrix3; fn inverse(&self) -> Self { @@ -540,10 +416,6 @@ impl SimdAngularInertia for SdpMatrix3 { } } - fn transform_lin_vector(&self, v: Vector3) -> Vector3 { - self.transform_vector(v) - } - fn transform_vector(&self, v: Vector3) -> Vector3 { let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z; let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z; @@ -559,16 +431,10 @@ impl SimdAngularInertia for SdpMatrix3 { self.m13, self.m23, self.m33, ) } - - #[rustfmt::skip] - fn transform_matrix(&self, m: &Matrix3) -> Matrix3 { - *self * *m - } } impl SimdAngularInertia for SdpMatrix3 { type AngVector = Vector3; - type LinVector = Vector3; type AngMatrix = Matrix3; fn inverse(&self) -> Self { @@ -593,10 +459,6 @@ impl SimdAngularInertia for SdpMatrix3 { } } - fn transform_lin_vector(&self, v: Vector3) -> Vector3 { - self.transform_vector(v) - } - fn transform_vector(&self, v: Vector3) -> Vector3 { let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z; let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z; @@ -623,27 +485,6 @@ impl SimdAngularInertia for SdpMatrix3 { self.m13, self.m23, self.m33, ) } - - #[rustfmt::skip] - fn transform_matrix(&self, m: &Matrix3) -> Matrix3 { - let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31; - let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31; - let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31; - - let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32; - let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32; - let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32; - - let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33; - let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33; - let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33; - - Matrix3::new( - x0, x1, x2, - y0, y1, y2, - z0, z1, z2, - ) - } } // This is an RAII structure that enables flushing denormal numbers -- cgit