diff --git a/src/geometry/abstract_rotation.rs b/src/geometry/abstract_rotation.rs index 5563b6a7..6cadb44e 100644 --- a/src/geometry/abstract_rotation.rs +++ b/src/geometry/abstract_rotation.rs @@ -1,6 +1,6 @@ use crate::allocator::Allocator; use crate::geometry::{Rotation, UnitComplex, UnitQuaternion}; -use crate::{DefaultAllocator, DimName, Point, RealField, Scalar, VectorN, U2, U3}; +use crate::{DefaultAllocator, DimName, Point, Scalar, SimdRealField, VectorN, U2, U3}; use simba::scalar::ClosedMul; @@ -18,8 +18,10 @@ pub trait AbstractRotation: PartialEq + ClosedMul + Clone where DefaultAllocator: Allocator; } -impl AbstractRotation for Rotation -where DefaultAllocator: Allocator +impl AbstractRotation for Rotation +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn identity() -> Self { @@ -61,7 +63,9 @@ where DefaultAllocator: Allocator } } -impl AbstractRotation for UnitQuaternion { +impl AbstractRotation for UnitQuaternion +where N::Element: SimdRealField +{ #[inline] fn identity() -> Self { Self::identity() @@ -98,7 +102,9 @@ impl AbstractRotation for UnitQuaternion { } } -impl AbstractRotation for UnitComplex { +impl AbstractRotation for UnitComplex +where N::Element: SimdRealField +{ #[inline] fn identity() -> Self { Self::identity() diff --git a/src/geometry/isometry.rs b/src/geometry/isometry.rs index 309a3877..c6a0ab1a 100755 --- a/src/geometry/isometry.rs +++ b/src/geometry/isometry.rs @@ -12,6 +12,7 @@ use serde::{Deserialize, Serialize}; use abomonation::Abomonation; use simba::scalar::{RealField, SubsetOf}; +use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1}; @@ -35,26 +36,19 @@ use crate::geometry::{AbstractRotation, Point, Translation}; DefaultAllocator: Allocator, Owned: Deserialize<'de>")) )] -pub struct Isometry +pub struct Isometry where DefaultAllocator: Allocator { /// The pure rotational part of this isometry. pub rotation: R, /// The pure translational part of this isometry. pub translation: Translation, - - // One dummy private field just to prevent explicit construction. - #[cfg_attr( - feature = "serde-serialize", - serde(skip_serializing, skip_deserializing) - )] - _noconstruct: PhantomData, } #[cfg(feature = "abomonation-serialize")] impl Abomonation for Isometry where - N: RealField, + N: SimdRealField, D: DimName, R: Abomonation, Translation: Abomonation, @@ -76,7 +70,7 @@ where } } -impl hash::Hash +impl hash::Hash for Isometry where DefaultAllocator: Allocator, @@ -88,15 +82,19 @@ where } } -impl + Copy> Copy for Isometry +impl + Copy> Copy + for Isometry where + N::Element: SimdRealField, DefaultAllocator: Allocator, Owned: Copy, { } -impl + Clone> Clone for Isometry -where DefaultAllocator: Allocator +impl + Clone> Clone for Isometry +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn clone(&self) -> Self { @@ -104,8 +102,10 @@ where DefaultAllocator: Allocator } } -impl> Isometry -where DefaultAllocator: Allocator +impl> Isometry +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { /// Creates a new isometry from its rotational and translational parts. /// @@ -124,9 +124,8 @@ where DefaultAllocator: Allocator #[inline] pub fn from_parts(translation: Translation, rotation: R) -> Self { Self { - rotation: rotation, - translation: translation, - _noconstruct: PhantomData, + rotation, + translation, } } @@ -352,7 +351,7 @@ where DefaultAllocator: Allocator // and makes it hard to use it, e.g., for Transform × Isometry implementation. // This is OK since all constructors of the isometry enforce the Rotation bound already (and // explicit struct construction is prevented by the dummy ZST field). -impl Isometry +impl Isometry where DefaultAllocator: Allocator { /// Converts this isometry into its equivalent homogeneous transformation matrix. @@ -385,14 +384,14 @@ where DefaultAllocator: Allocator } } -impl Eq for Isometry +impl Eq for Isometry where R: AbstractRotation + Eq, DefaultAllocator: Allocator, { } -impl PartialEq for Isometry +impl PartialEq for Isometry where R: AbstractRotation + PartialEq, DefaultAllocator: Allocator, diff --git a/src/geometry/isometry_construction.rs b/src/geometry/isometry_construction.rs index c6f5f3f1..fc055b93 100644 --- a/src/geometry/isometry_construction.rs +++ b/src/geometry/isometry_construction.rs @@ -8,6 +8,7 @@ use rand::distributions::{Distribution, Standard}; use rand::Rng; use simba::scalar::RealField; +use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimName, U2, U3}; @@ -18,8 +19,10 @@ use crate::geometry::{ Translation2, Translation3, UnitComplex, UnitQuaternion, }; -impl> Isometry -where DefaultAllocator: Allocator +impl> Isometry +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { /// Creates a new identity isometry. /// @@ -64,8 +67,10 @@ where DefaultAllocator: Allocator } } -impl> One for Isometry -where DefaultAllocator: Allocator +impl> One for Isometry +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { /// Creates a new identity isometry. #[inline] @@ -89,7 +94,8 @@ where #[cfg(feature = "arbitrary")] impl Arbitrary for Isometry where - N: RealField + Arbitrary + Send, + N: SimdRealField + Arbitrary + Send, + N::Element: SimdRealField, R: AbstractRotation + Arbitrary + Send, Owned: Send, DefaultAllocator: Allocator, @@ -107,7 +113,9 @@ where */ // 2D rotation. -impl Isometry> { +impl Isometry> +where N::Element: SimdRealField +{ /// Creates a new 2D isometry from a translation and a rotation angle. /// /// Its rotational part is represented as a 2x2 rotation matrix. @@ -142,7 +150,9 @@ impl Isometry> { } } -impl Isometry> { +impl Isometry> +where N::Element: SimdRealField +{ /// Creates a new 2D isometry from a translation and a rotation angle. /// /// Its rotational part is represented as an unit complex number. @@ -180,7 +190,8 @@ impl Isometry> { // 3D rotation. macro_rules! isometry_construction_impl( ($RotId: ident < $($RotParams: ident),*>, $RRDim: ty, $RCDim: ty) => { - impl Isometry> { + impl Isometry> + where N::Element: SimdRealField { /// Creates a new isometry from a translation and a rotation axis-angle. /// /// # Example diff --git a/src/geometry/isometry_conversion.rs b/src/geometry/isometry_conversion.rs index 87711675..28ec469d 100644 --- a/src/geometry/isometry_conversion.rs +++ b/src/geometry/isometry_conversion.rs @@ -1,4 +1,5 @@ use simba::scalar::{RealField, SubsetOf, SupersetOf}; +use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimMin, DimName, DimNameAdd, DimNameSum, U1}; @@ -150,7 +151,7 @@ where } } -impl From> for MatrixN> +impl From> for MatrixN> where D: DimNameAdd, R: SubsetOf>>, diff --git a/src/geometry/isometry_ops.rs b/src/geometry/isometry_ops.rs index cf23e376..4c0b5860 100644 --- a/src/geometry/isometry_ops.rs +++ b/src/geometry/isometry_ops.rs @@ -1,7 +1,8 @@ use num::{One, Zero}; use std::ops::{Div, DivAssign, Mul, MulAssign}; -use simba::scalar::{ClosedAdd, ClosedMul, RealField}; +use simba::scalar::{ClosedAdd, ClosedMul}; +use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimName, U1, U3, U4}; @@ -65,8 +66,9 @@ macro_rules! isometry_binop_impl( ($Op: ident, $op: ident; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty; $action: expr; $($lives: tt),*) => { - impl<$($lives ,)* N: RealField, D: DimName, R> $Op<$Rhs> for $Lhs - where R: AbstractRotation, + impl<$($lives ,)* N: SimdRealField, D: DimName, R> $Op<$Rhs> for $Lhs + where N::Element: SimdRealField, + R: AbstractRotation, DefaultAllocator: Allocator { type Output = $Output; @@ -112,8 +114,9 @@ macro_rules! isometry_binop_assign_impl_all( $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty; [val] => $action_val: expr; [ref] => $action_ref: expr;) => { - impl $OpAssign<$Rhs> for $Lhs - where R: AbstractRotation, + impl $OpAssign<$Rhs> for $Lhs + where N::Element: SimdRealField, + R: AbstractRotation, DefaultAllocator: Allocator { #[inline] fn $op_assign(&mut $lhs, $rhs: $Rhs) { @@ -121,8 +124,9 @@ macro_rules! isometry_binop_assign_impl_all( } } - impl<'b, N: RealField, D: DimName, R> $OpAssign<&'b $Rhs> for $Lhs - where R: AbstractRotation, + impl<'b, N: SimdRealField, D: DimName, R> $OpAssign<&'b $Rhs> for $Lhs + where N::Element: SimdRealField, + R: AbstractRotation, DefaultAllocator: Allocator { #[inline] fn $op_assign(&mut $lhs, $rhs: &'b $Rhs) { @@ -191,7 +195,7 @@ isometry_binop_assign_impl_all!( // Isometry ×= R // Isometry ÷= R md_assign_impl_all!( - MulAssign, mul_assign where N: RealField; + MulAssign, mul_assign where N: SimdRealField for N::Element: SimdRealField; (D, U1), (D, D) for D: DimName; self: Isometry>, rhs: Rotation; [val] => self.rotation *= rhs; @@ -199,7 +203,7 @@ md_assign_impl_all!( ); md_assign_impl_all!( - DivAssign, div_assign where N: RealField; + DivAssign, div_assign where N: SimdRealField for N::Element: SimdRealField; (D, U1), (D, D) for D: DimName; self: Isometry>, rhs: Rotation; // FIXME: don't invert explicitly? @@ -208,7 +212,7 @@ md_assign_impl_all!( ); md_assign_impl_all!( - MulAssign, mul_assign where N: RealField; + MulAssign, mul_assign where N: SimdRealField for N::Element: SimdRealField; (U3, U3), (U3, U3) for; self: Isometry>, rhs: UnitQuaternion; [val] => self.rotation *= rhs; @@ -216,7 +220,7 @@ md_assign_impl_all!( ); md_assign_impl_all!( - DivAssign, div_assign where N: RealField; + DivAssign, div_assign where N: SimdRealField for N::Element: SimdRealField; (U3, U3), (U3, U3) for; self: Isometry>, rhs: UnitQuaternion; // FIXME: don't invert explicitly? @@ -286,8 +290,9 @@ macro_rules! isometry_from_composition_impl( ($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(for $Dims: ident: $DimsBound: ident),*; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty; $action: expr; $($lives: tt),*) => { - impl<$($lives ,)* N: RealField $(, $Dims: $DimsBound)*> $Op<$Rhs> for $Lhs - where DefaultAllocator: Allocator + + impl<$($lives ,)* N: SimdRealField $(, $Dims: $DimsBound)*> $Op<$Rhs> for $Lhs + where N::Element: SimdRealField, + DefaultAllocator: Allocator + Allocator { type Output = $Output; diff --git a/src/geometry/isometry_simba.rs b/src/geometry/isometry_simba.rs index 262cdf20..a3515305 100755 --- a/src/geometry/isometry_simba.rs +++ b/src/geometry/isometry_simba.rs @@ -3,16 +3,16 @@ use simba::simd::SimdValue; use crate::base::allocator::Allocator; use crate::base::dimension::DimName; use crate::base::{DefaultAllocator, Scalar}; -use crate::RealField; +use crate::SimdRealField; use crate::geometry::{AbstractRotation, Isometry, Translation}; -impl SimdValue for Isometry +impl SimdValue for Isometry where - N::Element: Scalar, + N::Element: SimdRealField, R: SimdValue + AbstractRotation, - R::Element: AbstractRotation, - DefaultAllocator: Allocator, + R::Element: AbstractRotation, + DefaultAllocator: Allocator + Allocator, { type Element = Isometry; type SimdBool = N::SimdBool; diff --git a/src/geometry/op_macros.rs b/src/geometry/op_macros.rs index 86d46f96..19d348f4 100644 --- a/src/geometry/op_macros.rs +++ b/src/geometry/op_macros.rs @@ -85,7 +85,7 @@ macro_rules! md_impl_all( macro_rules! md_assign_impl( ( // Operator, operator method, and scalar bounds. - $Op: ident, $op: ident $(where N: $($ScalarBounds: ident),*)*; + $Op: ident, $op: ident $(where N: $($ScalarBounds: ident),*)* $(for N::Element: $ElementBounds: ident)*; // Storage dimensions, and dimension bounds. ($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident $(<$($BoundParam: ty),*>)*),* // [Optional] Extra allocator bounds. @@ -96,6 +96,7 @@ macro_rules! md_assign_impl( $action: expr; $($lives: tt),*) => { impl<$($lives ,)* N $(, $Dims: $DimsBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs where N: Scalar + Zero + One + ClosedAdd + ClosedMul $($(+ $ScalarBounds)*)*, + $(N::Element: $ElementBounds,)* DefaultAllocator: Allocator + Allocator, $( $ConstraintType: $ConstraintBound $(<$( $ConstraintBoundParams $( = $EqBound )*),*>)* ),* @@ -113,7 +114,7 @@ macro_rules! md_assign_impl( macro_rules! md_assign_impl_all( ( // Operator, operator method, and scalar bounds. - $Op: ident, $op: ident $(where N: $($ScalarBounds: ident),*)*; + $Op: ident, $op: ident $(where N: $($ScalarBounds: ident),*)* $(for N::Element: $($ElementBounds: ident),*)*; // Storage dimensions, and dimension bounds. ($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident $(<$($BoundParam: ty),*>)*),* // [Optional] Extra allocator bounds. @@ -124,14 +125,14 @@ macro_rules! md_assign_impl_all( [val] => $action_val: expr; [ref] => $action_ref: expr;) => { md_assign_impl!( - $Op, $op $(where N: $($ScalarBounds),*)*; + $Op, $op $(where N: $($ScalarBounds),*)* $(for N::Element: $($ElementBounds),*)*; ($R1, $C1),($R2, $C2) for $($Dims: $DimsBound $(<$($BoundParam),*>)*),* $(where $ConstraintType: $ConstraintBound $(<$($ConstraintBoundParams $( = $EqBound )*),*>)*)*; $lhs: $Lhs, $rhs: $Rhs; $action_val; ); md_assign_impl!( - $Op, $op $(where N: $($ScalarBounds),*)*; + $Op, $op $(where N: $($ScalarBounds),*)* $(for N::Element: $($ElementBounds),*)*; ($R1, $C1),($R2, $C2) for $($Dims: $DimsBound $(<$($BoundParam),*>)*),* $(where $ConstraintType: $ConstraintBound $(<$($ConstraintBoundParams $( = $EqBound )*),*>)*)*; $lhs: $Lhs, $rhs: &'b $Rhs; diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs index 297e9922..57dc41fc 100755 --- a/src/geometry/quaternion.rs +++ b/src/geometry/quaternion.rs @@ -14,11 +14,13 @@ use serde::{Deserialize, Deserializer, Serialize, Serializer}; use abomonation::Abomonation; use simba::scalar::RealField; -use simba::simd::SimdRealField; +use simba::simd::{SimdBool, SimdOption, SimdRealField, SimdValue}; use crate::base::dimension::{U1, U3, U4}; use crate::base::storage::{CStride, RStride}; -use crate::base::{Matrix3, Matrix4, MatrixSlice, MatrixSliceMut, Normed, Unit, Vector3, Vector4}; +use crate::base::{ + Matrix3, Matrix4, MatrixSlice, MatrixSliceMut, Normed, Scalar, Unit, Vector3, Vector4, +}; use crate::geometry::{Point3, Rotation}; @@ -26,7 +28,7 @@ use crate::geometry::{Point3, Rotation}; /// that may be used as a rotation. #[repr(C)] #[derive(Debug)] -pub struct Quaternion { +pub struct Quaternion { /// This quaternion as a 4D vector of coordinates in the `[ x, y, z, w ]` storage order. pub coords: Vector4, } @@ -48,9 +50,11 @@ where Vector4: Abomonation } } -impl Eq for Quaternion {} +impl Eq for Quaternion where N::Element: SimdRealField {} -impl PartialEq for Quaternion { +impl PartialEq for Quaternion +where N::Element: SimdRealField +{ fn eq(&self, rhs: &Self) -> bool { self.coords == rhs.coords || // Account for the double-covering of S², i.e. q = -q @@ -95,7 +99,9 @@ where Owned: Deserialize<'a> } } -impl Quaternion { +impl Quaternion +where N::Element: SimdRealField +{ /// Moves this unit quaternion into one that owns its data. #[inline] #[deprecated(note = "This method is a no-op and will be removed in a future release.")] @@ -282,7 +288,9 @@ impl Quaternion { } } -impl Quaternion { +impl Quaternion +where N::Element: SimdRealField +{ /// Inverts this quaternion if it is not zero. /// /// # Example @@ -303,7 +311,8 @@ impl Quaternion { /// ``` #[inline] #[must_use = "Did you mean to use try_inverse_mut()?"] - pub fn try_inverse(&self) -> Option { + pub fn try_inverse(&self) -> Option + where N: RealField { let mut res = self.clone(); if res.try_inverse_mut() { @@ -313,6 +322,14 @@ impl Quaternion { } } + #[inline] + #[must_use = "Did you mean to use try_inverse_mut()?"] + pub fn simd_try_inverse(&self) -> SimdOption { + let norm_squared = self.norm_squared(); + let ge = norm_squared.simd_ge(N::simd_default_epsilon()); + SimdOption::new(self.conjugate() / norm_squared, ge) + } + /// Calculates the inner product (also known as the dot product). /// See "Foundations of Game Engine Development, Volume 1: Mathematics" by Lengyel /// Formula 4.89. @@ -365,7 +382,8 @@ impl Quaternion { /// assert_relative_eq!(expected, result, epsilon = 1.0e-5); /// ``` #[inline] - pub fn project(&self, other: &Self) -> Option { + pub fn project(&self, other: &Self) -> Option + where N: RealField { self.inner(other).right_div(other) } @@ -384,7 +402,8 @@ impl Quaternion { /// assert_relative_eq!(expected, result, epsilon = 1.0e-5); /// ``` #[inline] - pub fn reject(&self, other: &Self) -> Option { + pub fn reject(&self, other: &Self) -> Option + where N: RealField { self.outer(other).right_div(other) } @@ -403,7 +422,8 @@ impl Quaternion { /// assert_eq!(half_ang, f32::consts::FRAC_PI_2); /// assert_eq!(axis, Some(Vector3::x_axis())); /// ``` - pub fn polar_decomposition(&self) -> (N, N, Option>>) { + pub fn polar_decomposition(&self) -> (N, N, Option>>) + where N: RealField { if let Some((q, n)) = Unit::try_new_and_get(*self, N::zero()) { if let Some(axis) = Unit::try_new(self.vector().clone_owned(), N::zero()) { let angle = q.angle() / crate::convert(2.0f64); @@ -432,7 +452,7 @@ impl Quaternion { let v = self.vector(); let s = self.scalar(); - Self::from_parts(n.ln(), v.normalize() * (s / n).acos()) + Self::from_parts(n.simd_ln(), v.normalize() * (s / n).simd_acos()) } /// Compute the exponential of a quaternion. @@ -446,7 +466,7 @@ impl Quaternion { /// ``` #[inline] pub fn exp(&self) -> Self { - self.exp_eps(N::default_epsilon()) + self.exp_eps(N::simd_default_epsilon()) } /// Compute the exponential of a quaternion. Returns the identity if the vector part of this quaternion @@ -467,16 +487,17 @@ impl Quaternion { pub fn exp_eps(&self, eps: N) -> Self { let v = self.vector(); let nn = v.norm_squared(); + let le = nn.simd_le(eps * eps); + le.if_else( + || Self::identity(), + || { + let w_exp = self.scalar().simd_exp(); + let n = nn.simd_sqrt(); + let nv = v * (w_exp * n.simd_sin() / n); - if nn <= eps * eps { - Self::identity() - } else { - let w_exp = self.scalar().exp(); - let n = nn.sqrt(); - let nv = v * (w_exp * n.sin() / n); - - Self::from_parts(w_exp * n.cos(), nv) - } + Self::from_parts(w_exp * n.simd_cos(), nv) + }, + ) } /// Raise the quaternion to a given floating power. @@ -560,17 +581,11 @@ impl Quaternion { /// assert!(!q.try_inverse_mut()); /// ``` #[inline] - pub fn try_inverse_mut(&mut self) -> bool { + pub fn try_inverse_mut(&mut self) -> N::SimdBool { let norm_squared = self.norm_squared(); - - if relative_eq!(&norm_squared, &N::zero()) { - false - } else { - self.conjugate_mut(); - self.coords /= norm_squared; - - true - } + let ge = norm_squared.simd_ge(N::simd_default_epsilon()); + *self = ge.if_else(|| self.conjugate() / norm_squared, || *self); + ge } /// Normalizes this quaternion. @@ -607,6 +622,8 @@ impl Quaternion { } /// Check if the quaternion is pure. + /// + /// A quaternion is pure if it has no real part (`self.w == 0.0`). #[inline] pub fn is_pure(&self) -> bool { self.w.is_zero() @@ -622,7 +639,8 @@ impl Quaternion { /// /// Calculates B-1 * A where A = self, B = other. #[inline] - pub fn left_div(&self, other: &Self) -> Option { + pub fn left_div(&self, other: &Self) -> Option + where N: RealField { other.try_inverse().map(|inv| inv * self) } @@ -641,7 +659,8 @@ impl Quaternion { /// assert_relative_eq!(expected, result, epsilon = 1.0e-7); /// ``` #[inline] - pub fn right_div(&self, other: &Self) -> Option { + pub fn right_div(&self, other: &Self) -> Option + where N: RealField { other.try_inverse().map(|inv| self * inv) } @@ -659,8 +678,8 @@ impl Quaternion { #[inline] pub fn cos(&self) -> Self { let z = self.imag().magnitude(); - let w = -self.w.sin() * z.sinhc(); - Self::from_parts(self.w.cos() * z.cosh(), self.imag() * w) + let w = -self.w.simd_sin() * z.simd_sinhc(); + Self::from_parts(self.w.simd_cos() * z.simd_cosh(), self.imag() * w) } /// Calculates the quaternionic arccosinus. @@ -697,8 +716,8 @@ impl Quaternion { #[inline] pub fn sin(&self) -> Self { let z = self.imag().magnitude(); - let w = self.w.cos() * z.sinhc(); - Self::from_parts(self.w.sin() * z.cosh(), self.imag() * w) + let w = self.w.simd_cos() * z.simd_sinhc(); + Self::from_parts(self.w.simd_sin() * z.simd_cosh(), self.imag() * w) } /// Calculates the quaternionic arcsinus. @@ -733,7 +752,8 @@ impl Quaternion { /// assert_relative_eq!(expected, result, epsilon = 1.0e-7); /// ``` #[inline] - pub fn tan(&self) -> Self { + pub fn tan(&self) -> Self + where N: RealField { self.sin().right_div(&self.cos()).unwrap() } @@ -748,7 +768,8 @@ impl Quaternion { /// assert_relative_eq!(input, result, epsilon = 1.0e-7); /// ``` #[inline] - pub fn atan(&self) -> Self { + pub fn atan(&self) -> Self + where N: RealField { let u = Self::from_imag(self.imag().normalize()); let num = u + self; let den = u - self; @@ -835,7 +856,8 @@ impl Quaternion { /// assert_relative_eq!(expected, result, epsilon = 1.0e-7); /// ``` #[inline] - pub fn tanh(&self) -> Self { + pub fn tanh(&self) -> Self + where N: RealField { self.sinh().right_div(&self.cosh()).unwrap() } @@ -944,25 +966,9 @@ impl Normed for Quaternion { } } -impl UnitQuaternion { - /// Moves this unit quaternion into one that owns its data. - #[inline] - #[deprecated( - note = "This method is unnecessary and will be removed in a future release. Use `.clone()` instead." - )] - pub fn into_owned(self) -> Self { - self - } - - /// Clones this unit quaternion into one that owns its data. - #[inline] - #[deprecated( - note = "This method is unnecessary and will be removed in a future release. Use `.clone()` instead." - )] - pub fn clone_owned(&self) -> Self { - *self - } - +impl UnitQuaternion +where N::Element: SimdRealField +{ /// The rotation angle in [0; pi] of this unit quaternion. /// /// # Example @@ -974,8 +980,8 @@ impl UnitQuaternion { /// ``` #[inline] pub fn angle(&self) -> N { - let w = self.quaternion().scalar().abs(); - self.quaternion().imag().norm().atan2(w) * crate::convert(2.0f64) + let w = self.quaternion().scalar().simd_abs(); + self.quaternion().imag().norm().simd_atan2(w) * crate::convert(2.0f64) } /// The underlying quaternion. @@ -1113,7 +1119,8 @@ impl UnitQuaternion { /// assert_eq!(q.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0)); /// ``` #[inline] - pub fn slerp(&self, other: &Self, t: N) -> Self { + pub fn slerp(&self, other: &Self, t: N) -> Self + where N: RealField { self.try_slerp(other, t, N::default_epsilon()) .expect("Quaternion slerp: ambiguous configuration.") } @@ -1129,7 +1136,8 @@ impl UnitQuaternion { /// * `epsilon`: the value below which the sinus of the angle separating both quaternion /// must be to return `None`. #[inline] - pub fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option { + pub fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option + where N: RealField { let coords = if self.coords.dot(&other.coords) < N::zero() { Unit::new_unchecked(self.coords).try_slerp( &Unit::new_unchecked(-other.coords), @@ -1185,7 +1193,8 @@ impl UnitQuaternion { /// assert!(rot.axis().is_none()); /// ``` #[inline] - pub fn axis(&self) -> Option>> { + pub fn axis(&self) -> Option>> + where N: RealField { let v = if self.quaternion().scalar() >= N::zero() { self.as_ref().vector().clone_owned() } else { @@ -1206,7 +1215,8 @@ impl UnitQuaternion { /// assert_relative_eq!(rot.scaled_axis(), axisangle, epsilon = 1.0e-6); /// ``` #[inline] - pub fn scaled_axis(&self) -> Vector3 { + pub fn scaled_axis(&self) -> Vector3 + where N: RealField { if let Some(axis) = self.axis() { axis.into_inner() * self.angle() } else { @@ -1231,7 +1241,8 @@ impl UnitQuaternion { /// assert!(rot.axis_angle().is_none()); /// ``` #[inline] - pub fn axis_angle(&self) -> Option<(Unit>, N)> { + pub fn axis_angle(&self) -> Option<(Unit>, N)> + where N: RealField { self.axis().map(|axis| (axis, self.angle())) } @@ -1258,7 +1269,8 @@ impl UnitQuaternion { /// assert_relative_eq!(q.ln().vector().into_owned(), axisangle, epsilon = 1.0e-6); /// ``` #[inline] - pub fn ln(&self) -> Quaternion { + pub fn ln(&self) -> Quaternion + where N: RealField { if let Some(v) = self.axis() { Quaternion::from_imag(v.into_inner() * self.angle()) } else { @@ -1283,7 +1295,8 @@ impl UnitQuaternion { /// assert_eq!(pow.angle(), 2.4); /// ``` #[inline] - pub fn powf(&self, n: N) -> Self { + pub fn powf(&self, n: N) -> Self + where N: RealField { if let Some(v) = self.axis() { Self::from_axis_angle(&v, self.angle() * n) } else { @@ -1343,7 +1356,8 @@ impl UnitQuaternion { /// The angles are produced in the form (roll, pitch, yaw). #[inline] #[deprecated(note = "This is renamed to use `.euler_angles()`.")] - pub fn to_euler_angles(&self) -> (N, N, N) { + pub fn to_euler_angles(&self) -> (N, N, N) + where N: RealField { self.euler_angles() } @@ -1362,7 +1376,8 @@ impl UnitQuaternion { /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6); /// ``` #[inline] - pub fn euler_angles(&self) -> (N, N, N) { + pub fn euler_angles(&self) -> (N, N, N) + where N: RealField { self.to_rotation_matrix().euler_angles() } diff --git a/src/geometry/quaternion_construction.rs b/src/geometry/quaternion_construction.rs index fb08dd83..ef1e99cb 100644 --- a/src/geometry/quaternion_construction.rs +++ b/src/geometry/quaternion_construction.rs @@ -10,6 +10,7 @@ use rand::distributions::{Distribution, OpenClosed01, Standard}; use rand::Rng; use simba::scalar::RealField; +use simba::simd::SimdBool; use crate::base::dimension::U3; use crate::base::storage::Storage; @@ -96,7 +97,9 @@ impl Quaternion { } // FIXME: merge with the previous block. -impl Quaternion { +impl Quaternion +where N::Element: SimdRealField +{ /// Creates a new quaternion from its polar decomposition. /// /// Note that `axis` is assumed to be a unit vector. @@ -109,14 +112,18 @@ impl Quaternion { } } -impl One for Quaternion { +impl One for Quaternion +where N::Element: SimdRealField +{ #[inline] fn one() -> Self { Self::identity() } } -impl Zero for Quaternion { +impl Zero for Quaternion +where N::Element: SimdRealField +{ #[inline] fn zero() -> Self { Self::from(Vector4::zero()) @@ -152,7 +159,9 @@ where Owned: Send } } -impl UnitQuaternion { +impl UnitQuaternion +where N::Element: SimdRealField +{ /// The rotation identity. /// /// # Example @@ -199,7 +208,7 @@ impl UnitQuaternion { #[inline] pub fn from_axis_angle(axis: &Unit>, angle: N) -> Self where SB: Storage { - let (sang, cang) = (angle / crate::convert(2.0f64)).sin_cos(); + let (sang, cang) = (angle / crate::convert(2.0f64)).simd_sin_cos(); let q = Quaternion::from_parts(cang, axis.as_ref() * sang); Self::new_unchecked(q) @@ -229,9 +238,9 @@ impl UnitQuaternion { /// ``` #[inline] pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self { - let (sr, cr) = (roll * crate::convert(0.5f64)).sin_cos(); - let (sp, cp) = (pitch * crate::convert(0.5f64)).sin_cos(); - let (sy, cy) = (yaw * crate::convert(0.5f64)).sin_cos(); + let (sr, cr) = (roll * crate::convert(0.5f64)).simd_sin_cos(); + let (sp, cp) = (pitch * crate::convert(0.5f64)).simd_sin_cos(); + let (sy, cy) = (yaw * crate::convert(0.5f64)).simd_sin_cos(); let q = Quaternion::new( cr * cp * cy + sr * sp * sy, @@ -262,46 +271,58 @@ impl UnitQuaternion { // Robust matrix to quaternion transformation. // See https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion let tr = rotmat[(0, 0)] + rotmat[(1, 1)] + rotmat[(2, 2)]; - let res; - let _0_25: N = crate::convert(0.25); - if tr > N::zero() { - let denom = (tr + N::one()).sqrt() * crate::convert(2.0); - res = Quaternion::new( - _0_25 * denom, - (rotmat[(2, 1)] - rotmat[(1, 2)]) / denom, - (rotmat[(0, 2)] - rotmat[(2, 0)]) / denom, - (rotmat[(1, 0)] - rotmat[(0, 1)]) / denom, - ); - } else if rotmat[(0, 0)] > rotmat[(1, 1)] && rotmat[(0, 0)] > rotmat[(2, 2)] { - let denom = (N::one() + rotmat[(0, 0)] - rotmat[(1, 1)] - rotmat[(2, 2)]).sqrt() - * crate::convert(2.0); - res = Quaternion::new( - (rotmat[(2, 1)] - rotmat[(1, 2)]) / denom, - _0_25 * denom, - (rotmat[(0, 1)] + rotmat[(1, 0)]) / denom, - (rotmat[(0, 2)] + rotmat[(2, 0)]) / denom, - ); - } else if rotmat[(1, 1)] > rotmat[(2, 2)] { - let denom = (N::one() + rotmat[(1, 1)] - rotmat[(0, 0)] - rotmat[(2, 2)]).sqrt() - * crate::convert(2.0); - res = Quaternion::new( - (rotmat[(0, 2)] - rotmat[(2, 0)]) / denom, - (rotmat[(0, 1)] + rotmat[(1, 0)]) / denom, - _0_25 * denom, - (rotmat[(1, 2)] + rotmat[(2, 1)]) / denom, - ); - } else { - let denom = (N::one() + rotmat[(2, 2)] - rotmat[(0, 0)] - rotmat[(1, 1)]).sqrt() - * crate::convert(2.0); - res = Quaternion::new( - (rotmat[(1, 0)] - rotmat[(0, 1)]) / denom, - (rotmat[(0, 2)] + rotmat[(2, 0)]) / denom, - (rotmat[(1, 2)] + rotmat[(2, 1)]) / denom, - _0_25 * denom, - ); - } + let res = tr.simd_gt(N::zero()).if_else3( + || { + let denom = (tr + N::one()).simd_sqrt() * crate::convert(2.0); + Quaternion::new( + _0_25 * denom, + (rotmat[(2, 1)] - rotmat[(1, 2)]) / denom, + (rotmat[(0, 2)] - rotmat[(2, 0)]) / denom, + (rotmat[(1, 0)] - rotmat[(0, 1)]) / denom, + ) + }, + ( + || rotmat[(0, 0)].simd_gt(rotmat[(1, 1)]) & rotmat[(0, 0)].simd_gt(rotmat[(2, 2)]), + || { + let denom = (N::one() + rotmat[(0, 0)] - rotmat[(1, 1)] - rotmat[(2, 2)]) + .simd_sqrt() + * crate::convert(2.0); + Quaternion::new( + (rotmat[(2, 1)] - rotmat[(1, 2)]) / denom, + _0_25 * denom, + (rotmat[(0, 1)] + rotmat[(1, 0)]) / denom, + (rotmat[(0, 2)] + rotmat[(2, 0)]) / denom, + ) + }, + ), + ( + || rotmat[(1, 1)].simd_gt(rotmat[(2, 2)]), + || { + let denom = (N::one() + rotmat[(1, 1)] - rotmat[(0, 0)] - rotmat[(2, 2)]) + .simd_sqrt() + * crate::convert(2.0); + Quaternion::new( + (rotmat[(0, 2)] - rotmat[(2, 0)]) / denom, + (rotmat[(0, 1)] + rotmat[(1, 0)]) / denom, + _0_25 * denom, + (rotmat[(1, 2)] + rotmat[(2, 1)]) / denom, + ) + }, + ), + || { + let denom = (N::one() + rotmat[(2, 2)] - rotmat[(0, 0)] - rotmat[(1, 1)]) + .simd_sqrt() + * crate::convert(2.0); + Quaternion::new( + (rotmat[(1, 0)] - rotmat[(0, 1)]) / denom, + (rotmat[(0, 2)] + rotmat[(2, 0)]) / denom, + (rotmat[(1, 2)] + rotmat[(2, 1)]) / denom, + _0_25 * denom, + ) + }, + ); Self::new_unchecked(res) } @@ -311,7 +332,8 @@ impl UnitQuaternion { /// This is an iterative method. See `.from_matrix_eps` to provide mover /// convergence parameters and starting solution. /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al. - pub fn from_matrix(m: &Matrix3) -> Self { + pub fn from_matrix(m: &Matrix3) -> Self + where N: RealField { Rotation3::from_matrix(m).into() } @@ -327,7 +349,8 @@ impl UnitQuaternion { /// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close /// to the actual solution is provided. Can be set to `UnitQuaternion::identity()` if no other /// guesses come to mind. - pub fn from_matrix_eps(m: &Matrix3, eps: N, max_iter: usize, guess: Self) -> Self { + pub fn from_matrix_eps(m: &Matrix3, eps: N, max_iter: usize, guess: Self) -> Self + where N: RealField { let guess = Rotation3::from(guess); Rotation3::from_matrix_eps(m, eps, max_iter, guess).into() } @@ -348,6 +371,7 @@ impl UnitQuaternion { #[inline] pub fn rotation_between(a: &Vector, b: &Vector) -> Option where + N: RealField, SB: Storage, SC: Storage, { @@ -375,6 +399,7 @@ impl UnitQuaternion { s: N, ) -> Option where + N: RealField, SB: Storage, SC: Storage, { @@ -408,6 +433,7 @@ impl UnitQuaternion { b: &Unit>, ) -> Option where + N: RealField, SB: Storage, SC: Storage, { @@ -435,6 +461,7 @@ impl UnitQuaternion { s: N, ) -> Option where + N: RealField, SB: Storage, SC: Storage, { @@ -706,7 +733,8 @@ impl UnitQuaternion { /// assert_relative_eq!(euler_angles_mean.0, 0.0, epsilon = 1.0e-7) /// ``` #[inline] - pub fn mean_of(unit_quaternions: impl IntoIterator) -> Self { + pub fn mean_of(unit_quaternions: impl IntoIterator) -> Self + where N: RealField { let quaternions_matrix: Matrix4 = unit_quaternions .into_iter() .map(|q| q.as_vector() * q.as_vector().transpose()) @@ -734,15 +762,19 @@ impl UnitQuaternion { } } -impl One for UnitQuaternion { +impl One for UnitQuaternion +where N::Element: SimdRealField +{ #[inline] fn one() -> Self { Self::identity() } } -impl Distribution> for Standard -where OpenClosed01: Distribution +impl Distribution> for Standard +where + N::Element: SimdRealField, + OpenClosed01: Distribution, { /// Generate a uniformly distributed random rotation quaternion. #[inline] @@ -753,20 +785,20 @@ where OpenClosed01: Distribution let x0 = rng.sample(OpenClosed01); let x1 = rng.sample(OpenClosed01); let x2 = rng.sample(OpenClosed01); - let theta1 = N::two_pi() * x1; - let theta2 = N::two_pi() * x2; - let s1 = theta1.sin(); - let c1 = theta1.cos(); - let s2 = theta2.sin(); - let c2 = theta2.cos(); - let r1 = (N::one() - x0).sqrt(); - let r2 = x0.sqrt(); + let theta1 = N::simd_two_pi() * x1; + let theta2 = N::simd_two_pi() * x2; + let s1 = theta1.simd_sin(); + let c1 = theta1.simd_cos(); + let s2 = theta2.simd_sin(); + let c2 = theta2.simd_cos(); + let r1 = (N::one() - x0).simd_sqrt(); + let r2 = x0.simd_sqrt(); Unit::new_unchecked(Quaternion::new(s1 * r1, c1 * r1, s2 * r2, c2 * r2)) } } #[cfg(feature = "arbitrary")] -impl Arbitrary for UnitQuaternion +impl Arbitrary for UnitQuaternion where Owned: Send, Owned: Send, diff --git a/src/geometry/quaternion_conversion.rs b/src/geometry/quaternion_conversion.rs index d79a7cdc..d9cc6c4a 100644 --- a/src/geometry/quaternion_conversion.rs +++ b/src/geometry/quaternion_conversion.rs @@ -1,13 +1,13 @@ use num::Zero; use simba::scalar::{RealField, SubsetOf, SupersetOf}; -use simba::simd::SimdRealField; +use simba::simd::{SimdRealField, SimdValue}; #[cfg(feature = "mint")] use mint; use crate::base::dimension::U3; -use crate::base::{Matrix3, Matrix4, Vector4}; +use crate::base::{Matrix3, Matrix4, Scalar, Vector4}; use crate::geometry::{ AbstractRotation, Isometry, Quaternion, Rotation, Rotation3, Similarity, SuperTCategoryOf, TAffine, Transform, Translation, UnitQuaternion, @@ -184,14 +184,14 @@ impl> SubsetOf> for Un } #[cfg(feature = "mint")] -impl From> for Quaternion { +impl From> for Quaternion { fn from(q: mint::Quaternion) -> Self { Self::new(q.s, q.v.x, q.v.y, q.v.z) } } #[cfg(feature = "mint")] -impl Into> for Quaternion { +impl Into> for Quaternion { fn into(self) -> mint::Quaternion { mint::Quaternion { v: mint::Vector3 { @@ -205,7 +205,7 @@ impl Into> for Quaternion { } #[cfg(feature = "mint")] -impl Into> for UnitQuaternion { +impl Into> for UnitQuaternion { fn into(self) -> mint::Quaternion { mint::Quaternion { v: mint::Vector3 { @@ -218,35 +218,43 @@ impl Into> for UnitQuaternion { } } -impl From> for Matrix4 { +impl From> for Matrix4 +where N::Element: SimdRealField +{ #[inline] fn from(q: UnitQuaternion) -> Self { q.to_homogeneous() } } -impl From> for Rotation3 { +impl From> for Rotation3 +where N::Element: SimdRealField +{ #[inline] fn from(q: UnitQuaternion) -> Self { q.to_rotation_matrix() } } -impl From> for UnitQuaternion { +impl From> for UnitQuaternion +where N::Element: SimdRealField +{ #[inline] fn from(q: Rotation3) -> Self { Self::from_rotation_matrix(&q) } } -impl From> for Matrix3 { +impl From> for Matrix3 +where N::Element: SimdRealField +{ #[inline] fn from(q: UnitQuaternion) -> Self { q.to_rotation_matrix().into_inner() } } -impl From> for Quaternion { +impl From> for Quaternion { #[inline] fn from(coords: Vector4) -> Self { Self { coords } diff --git a/src/geometry/quaternion_ops.rs b/src/geometry/quaternion_ops.rs index 62e14828..0929c6bd 100644 --- a/src/geometry/quaternion_ops.rs +++ b/src/geometry/quaternion_ops.rs @@ -85,7 +85,8 @@ macro_rules! quaternion_op_impl( $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty $(=> $VDimA: ty, $VDimB: ty)*; $action: expr; $($lives: tt),*) => { impl<$($lives ,)* N: SimdRealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs - where DefaultAllocator: Allocator + + where N::Element: SimdRealField, + DefaultAllocator: Allocator + Allocator { type Output = $Result; @@ -489,7 +490,8 @@ Unit::new_unchecked(self * rhs.into_inner()); macro_rules! scalar_op_impl( ($($Op: ident, $op: ident, $OpAssign: ident, $op_assign: ident);* $(;)*) => {$( - impl $Op for Quaternion { + impl $Op for Quaternion + where N::Element: SimdRealField { type Output = Quaternion; #[inline] @@ -498,7 +500,8 @@ macro_rules! scalar_op_impl( } } - impl<'a, N: SimdRealField> $Op for &'a Quaternion { + impl<'a, N: SimdRealField> $Op for &'a Quaternion + where N::Element: SimdRealField { type Output = Quaternion; #[inline] @@ -507,7 +510,8 @@ macro_rules! scalar_op_impl( } } - impl $OpAssign for Quaternion { + impl $OpAssign for Quaternion + where N::Element: SimdRealField { #[inline] fn $op_assign(&mut self, n: N) { @@ -546,7 +550,9 @@ macro_rules! left_scalar_mul_impl( left_scalar_mul_impl!(f32, f64); -impl Neg for Quaternion { +impl Neg for Quaternion +where N::Element: SimdRealField +{ type Output = Quaternion; #[inline] @@ -555,7 +561,9 @@ impl Neg for Quaternion { } } -impl<'a, N: SimdRealField> Neg for &'a Quaternion { +impl<'a, N: SimdRealField> Neg for &'a Quaternion +where N::Element: SimdRealField +{ type Output = Quaternion; #[inline] @@ -570,7 +578,8 @@ macro_rules! quaternion_op_impl( $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty $(=> $VDimA: ty, $VDimB: ty)*; $action: expr; $($lives: tt),*) => { impl<$($lives ,)* N: SimdRealField> $OpAssign<$Rhs> for $Lhs - where DefaultAllocator: Allocator + + where N::Element: SimdRealField, + DefaultAllocator: Allocator + Allocator { #[inline] diff --git a/src/geometry/quaternion_simba.rs b/src/geometry/quaternion_simba.rs index 6eed073e..5bd3e2b5 100755 --- a/src/geometry/quaternion_simba.rs +++ b/src/geometry/quaternion_simba.rs @@ -4,7 +4,7 @@ use crate::base::Vector4; use crate::geometry::Quaternion; use crate::{RealField, Scalar}; -impl SimdValue for Quaternion +impl SimdValue for Quaternion where N::Element: Scalar { type Element = Quaternion; diff --git a/src/geometry/rotation.rs b/src/geometry/rotation.rs index 0717895f..eb393a01 100755 --- a/src/geometry/rotation.rs +++ b/src/geometry/rotation.rs @@ -15,6 +15,7 @@ use crate::base::storage::Owned; use abomonation::Abomonation; use simba::scalar::RealField; +use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1}; @@ -354,8 +355,10 @@ where DefaultAllocator: Allocator } } -impl Rotation -where DefaultAllocator: Allocator + Allocator +impl Rotation +where + N::Element: SimdRealField, + DefaultAllocator: Allocator + Allocator, { /// Rotate the given point. /// diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index f72a07d5..399106cd 100644 --- a/src/geometry/rotation_specialization.rs +++ b/src/geometry/rotation_specialization.rs @@ -7,6 +7,7 @@ use num::Zero; use rand::distributions::{Distribution, OpenClosed01, Standard}; use rand::Rng; use simba::scalar::RealField; +use simba::simd::{SimdBool, SimdRealField}; use std::ops::Neg; use crate::base::dimension::{U1, U2, U3}; @@ -20,7 +21,7 @@ use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion}; * 2D Rotation matrix. * */ -impl Rotation2 { +impl Rotation2 { /// Builds a 2 dimensional rotation matrix from an angle in radian. /// /// # Example @@ -34,7 +35,7 @@ impl Rotation2 { /// assert_relative_eq!(rot * Point2::new(3.0, 4.0), Point2::new(-4.0, 3.0)); /// ``` pub fn new(angle: N) -> Self { - let (sia, coa) = angle.sin_cos(); + let (sia, coa) = angle.simd_sin_cos(); Self::from_matrix_unchecked(Matrix2::new(coa, -sia, sia, coa)) } @@ -53,7 +54,8 @@ impl Rotation2 { /// This is an iterative method. See `.from_matrix_eps` to provide mover /// convergence parameters and starting solution. /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al. - pub fn from_matrix(m: &Matrix2) -> Self { + pub fn from_matrix(m: &Matrix2) -> Self + where N: RealField { Self::from_matrix_eps(m, N::default_epsilon(), 0, Self::identity()) } @@ -69,7 +71,8 @@ impl Rotation2 { /// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close /// to the actual solution is provided. Can be set to `Rotation2::identity()` if no other /// guesses come to mind. - pub fn from_matrix_eps(m: &Matrix2, eps: N, mut max_iter: usize, guess: Self) -> Self { + pub fn from_matrix_eps(m: &Matrix2, eps: N, mut max_iter: usize, guess: Self) -> Self + where N: RealField { if max_iter == 0 { max_iter = usize::max_value(); } @@ -108,6 +111,7 @@ impl Rotation2 { #[inline] pub fn rotation_between(a: &Vector, b: &Vector) -> Self where + N: RealField, SB: Storage, SC: Storage, { @@ -135,6 +139,7 @@ impl Rotation2 { s: N, ) -> Self where + N: RealField, SB: Storage, SC: Storage, { @@ -152,7 +157,7 @@ impl Rotation2 { /// ``` #[inline] pub fn angle(&self) -> N { - self.matrix()[(1, 0)].atan2(self.matrix()[(0, 0)]) + self.matrix()[(1, 0)].simd_atan2(self.matrix()[(0, 0)]) } /// The rotation angle needed to make `self` and `other` coincide. @@ -193,7 +198,8 @@ impl Rotation2 { /// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated /// computations might cause the matrix from progressively not being orthonormal anymore. #[inline] - pub fn renormalize(&mut self) { + pub fn renormalize(&mut self) + where N: RealField { let mut c = UnitComplex::from(*self); let _ = c.renormalize(); @@ -226,19 +232,23 @@ impl Rotation2 { } } -impl Distribution> for Standard -where OpenClosed01: Distribution +impl Distribution> for Standard +where + N::Element: SimdRealField, + OpenClosed01: Distribution, { /// Generate a uniformly distributed random rotation. #[inline] fn sample<'a, R: Rng + ?Sized>(&self, rng: &'a mut R) -> Rotation2 { - Rotation2::new(rng.sample(OpenClosed01) * N::two_pi()) + Rotation2::new(rng.sample(OpenClosed01) * N::simd_two_pi()) } } #[cfg(feature = "arbitrary")] -impl Arbitrary for Rotation2 -where Owned: Send +impl Arbitrary for Rotation2 +where + N::Element: SimdRealField, + Owned: Send, { #[inline] fn arbitrary(g: &mut G) -> Self { @@ -251,7 +261,9 @@ where Owned: Send * 3D Rotation matrix. * */ -impl Rotation3 { +impl Rotation3 +where N::Element: SimdRealField +{ /// Builds a 3 dimensional rotation matrix from an axis and an angle. /// /// # Arguments @@ -286,7 +298,8 @@ impl Rotation3 { /// This is an iterative method. See `.from_matrix_eps` to provide mover /// convergence parameters and starting solution. /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al. - pub fn from_matrix(m: &Matrix3) -> Self { + pub fn from_matrix(m: &Matrix3) -> Self + where N: RealField { Self::from_matrix_eps(m, N::default_epsilon(), 0, Self::identity()) } @@ -302,7 +315,8 @@ impl Rotation3 { /// * `guess`: a guess of the solution. Convergence will be significantly faster if an initial solution close /// to the actual solution is provided. Can be set to `Rotation3::identity()` if no other /// guesses come to mind. - pub fn from_matrix_eps(m: &Matrix3, eps: N, mut max_iter: usize, guess: Self) -> Self { + pub fn from_matrix_eps(m: &Matrix3, eps: N, mut max_iter: usize, guess: Self) -> Self + where N: RealField { if max_iter == 0 { max_iter = usize::max_value(); } @@ -378,30 +392,31 @@ impl Rotation3 { /// ``` pub fn from_axis_angle(axis: &Unit>, angle: N) -> Self where SB: Storage { - if angle.is_zero() { - Self::identity() - } else { - let ux = axis.as_ref()[0]; - let uy = axis.as_ref()[1]; - let uz = axis.as_ref()[2]; - let sqx = ux * ux; - let sqy = uy * uy; - let sqz = uz * uz; - let (sin, cos) = angle.sin_cos(); - let one_m_cos = N::one() - cos; + angle.simd_ne(N::zero()).if_else( + || { + let ux = axis.as_ref()[0]; + let uy = axis.as_ref()[1]; + let uz = axis.as_ref()[2]; + let sqx = ux * ux; + let sqy = uy * uy; + let sqz = uz * uz; + let (sin, cos) = angle.simd_sin_cos(); + let one_m_cos = N::one() - cos; - Self::from_matrix_unchecked(MatrixN::::new( - sqx + (N::one() - sqx) * cos, - ux * uy * one_m_cos - uz * sin, - ux * uz * one_m_cos + uy * sin, - ux * uy * one_m_cos + uz * sin, - sqy + (N::one() - sqy) * cos, - uy * uz * one_m_cos - ux * sin, - ux * uz * one_m_cos - uy * sin, - uy * uz * one_m_cos + ux * sin, - sqz + (N::one() - sqz) * cos, - )) - } + Self::from_matrix_unchecked(MatrixN::::new( + sqx + (N::one() - sqx) * cos, + ux * uy * one_m_cos - uz * sin, + ux * uz * one_m_cos + uy * sin, + ux * uy * one_m_cos + uz * sin, + sqy + (N::one() - sqy) * cos, + uy * uz * one_m_cos - ux * sin, + ux * uz * one_m_cos - uy * sin, + uy * uz * one_m_cos + ux * sin, + sqz + (N::one() - sqz) * cos, + )) + }, + || Self::identity(), + ) } /// Creates a new rotation from Euler angles. @@ -419,9 +434,9 @@ impl Rotation3 { /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6); /// ``` pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self { - let (sr, cr) = roll.sin_cos(); - let (sp, cp) = pitch.sin_cos(); - let (sy, cy) = yaw.sin_cos(); + let (sr, cr) = roll.simd_sin_cos(); + let (sp, cp) = pitch.simd_sin_cos(); + let (sy, cy) = yaw.simd_sin_cos(); Self::from_matrix_unchecked(MatrixN::::new( cy * cp, @@ -440,7 +455,8 @@ impl Rotation3 { /// /// The angles are produced in the form (roll, pitch, yaw). #[deprecated(note = "This is renamed to use `.euler_angles()`.")] - pub fn to_euler_angles(&self) -> (N, N, N) { + pub fn to_euler_angles(&self) -> (N, N, N) + where N: RealField { self.euler_angles() } @@ -458,7 +474,8 @@ impl Rotation3 { /// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6); /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6); /// ``` - pub fn euler_angles(&self) -> (N, N, N) { + pub fn euler_angles(&self) -> (N, N, N) + where N: RealField { // Implementation informed by "Computing Euler angles from a rotation matrix", by Gregory G. Slabaugh // https://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.371.6578 if self[(2, 0)].abs() < N::one() { @@ -480,7 +497,8 @@ impl Rotation3 { /// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated /// computations might cause the matrix from progressively not being orthonormal anymore. #[inline] - pub fn renormalize(&mut self) { + pub fn renormalize(&mut self) + where N: RealField { let mut c = UnitQuaternion::from(*self); let _ = c.renormalize(); @@ -612,6 +630,7 @@ impl Rotation3 { #[inline] pub fn rotation_between(a: &Vector, b: &Vector) -> Option where + N: RealField, SB: Storage, SC: Storage, { @@ -639,6 +658,7 @@ impl Rotation3 { n: N, ) -> Option where + N: RealField, SB: Storage, SC: Storage, { @@ -677,7 +697,7 @@ impl Rotation3 { pub fn angle(&self) -> N { ((self.matrix()[(0, 0)] + self.matrix()[(1, 1)] + self.matrix()[(2, 2)] - N::one()) / crate::convert(2.0)) - .acos() + .simd_acos() } /// The rotation axis. Returns `None` if the rotation angle is zero or PI. @@ -696,7 +716,8 @@ impl Rotation3 { /// assert!(rot.axis().is_none()); /// ``` #[inline] - pub fn axis(&self) -> Option>> { + pub fn axis(&self) -> Option>> + where N: RealField { let axis = VectorN::::new( self.matrix()[(2, 1)] - self.matrix()[(1, 2)], self.matrix()[(0, 2)] - self.matrix()[(2, 0)], @@ -717,7 +738,8 @@ impl Rotation3 { /// assert_relative_eq!(rot.scaled_axis(), axisangle, epsilon = 1.0e-6); /// ``` #[inline] - pub fn scaled_axis(&self) -> Vector3 { + pub fn scaled_axis(&self) -> Vector3 + where N: RealField { if let Some(axis) = self.axis() { axis.into_inner() * self.angle() } else { @@ -745,7 +767,8 @@ impl Rotation3 { /// assert!(rot.axis_angle().is_none()); /// ``` #[inline] - pub fn axis_angle(&self) -> Option<(Unit>, N)> { + pub fn axis_angle(&self) -> Option<(Unit>, N)> + where N: RealField { if let Some(axis) = self.axis() { Some((axis, self.angle())) } else { @@ -801,7 +824,8 @@ impl Rotation3 { /// assert_eq!(pow.angle(), 2.4); /// ``` #[inline] - pub fn powf(&self, n: N) -> Self { + pub fn powf(&self, n: N) -> Self + where N: RealField { if let Some(axis) = self.axis() { Self::from_axis_angle(&axis, self.angle() * n) } else if self.matrix()[(0, 0)] < N::zero() { @@ -813,8 +837,10 @@ impl Rotation3 { } } -impl Distribution> for Standard -where OpenClosed01: Distribution +impl Distribution> for Standard +where + N::Element: SimdRealField, + OpenClosed01: Distribution, { /// Generate a uniformly distributed random rotation. #[inline] @@ -824,8 +850,8 @@ where OpenClosed01: Distribution // In D. Kirk, editor, Graphics Gems III, pages 117-120. Academic, New York, 1992. // Compute a random rotation around Z - let theta = N::two_pi() * rng.sample(OpenClosed01); - let (ts, tc) = theta.sin_cos(); + let theta = N::simd_two_pi() * rng.sample(OpenClosed01); + let (ts, tc) = theta.simd_sin_cos(); let a = MatrixN::::new( tc, ts, @@ -839,11 +865,11 @@ where OpenClosed01: Distribution ); // Compute a random rotation *of* Z - let phi = N::two_pi() * rng.sample(OpenClosed01); + let phi = N::simd_two_pi() * rng.sample(OpenClosed01); let z = rng.sample(OpenClosed01); - let (ps, pc) = phi.sin_cos(); - let sqrt_z = z.sqrt(); - let v = Vector3::new(pc * sqrt_z, ps * sqrt_z, (N::one() - z).sqrt()); + let (ps, pc) = phi.simd_sin_cos(); + let sqrt_z = z.simd_sqrt(); + let v = Vector3::new(pc * sqrt_z, ps * sqrt_z, (N::one() - z).simd_sqrt()); let mut b = v * v.transpose(); b += b; b -= MatrixN::::identity(); @@ -853,8 +879,9 @@ where OpenClosed01: Distribution } #[cfg(feature = "arbitrary")] -impl Arbitrary for Rotation3 +impl Arbitrary for Rotation3 where + N::Element: SimdRealField, Owned: Send, Owned: Send, { diff --git a/src/geometry/similarity.rs b/src/geometry/similarity.rs index 6bfd7a93..5260c927 100755 --- a/src/geometry/similarity.rs +++ b/src/geometry/similarity.rs @@ -11,6 +11,7 @@ use serde::{Deserialize, Serialize}; use abomonation::Abomonation; use simba::scalar::{RealField, SubsetOf}; +use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1}; @@ -36,7 +37,7 @@ use crate::geometry::{AbstractRotation, Isometry, Point, Translation}; DefaultAllocator: Allocator, Owned: Deserialize<'de>")) )] -pub struct Similarity +pub struct Similarity where DefaultAllocator: Allocator { /// The part of this similarity that does not include the scaling factor. @@ -45,7 +46,7 @@ where DefaultAllocator: Allocator } #[cfg(feature = "abomonation-serialize")] -impl Abomonation for Similarity +impl Abomonation for Similarity where Isometry: Abomonation, DefaultAllocator: Allocator, @@ -63,7 +64,7 @@ where } } -impl hash::Hash +impl hash::Hash for Similarity where DefaultAllocator: Allocator, @@ -75,15 +76,19 @@ where } } -impl + Copy> Copy for Similarity +impl + Copy> Copy + for Similarity where + N::Element: SimdRealField, DefaultAllocator: Allocator, Owned: Copy, { } -impl + Clone> Clone for Similarity -where DefaultAllocator: Allocator +impl + Clone> Clone for Similarity +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn clone(&self) -> Self { @@ -91,8 +96,9 @@ where DefaultAllocator: Allocator } } -impl Similarity +impl Similarity where + N::Element: SimdRealField, R: AbstractRotation, DefaultAllocator: Allocator, { @@ -105,10 +111,7 @@ where /// Creates a new similarity from its rotational and translational parts. #[inline] pub fn from_isometry(isometry: Isometry, scaling: N) -> Self { - assert!( - !relative_eq!(scaling, N::zero()), - "The scaling factor must not be zero." - ); + assert!(!scaling.is_zero(), "The scaling factor must not be zero."); Self { isometry, scaling } } @@ -140,7 +143,7 @@ where #[inline] pub fn set_scaling(&mut self, scaling: N) { assert!( - !relative_eq!(scaling, N::zero()), + !scaling.is_zero(), "The similarity scaling factor must not be zero." ); @@ -158,7 +161,7 @@ where #[must_use = "Did you mean to use prepend_scaling_mut()?"] pub fn prepend_scaling(&self, scaling: N) -> Self { assert!( - !relative_eq!(scaling, N::zero()), + !scaling.is_zero(), "The similarity scaling factor must not be zero." ); @@ -170,7 +173,7 @@ where #[must_use = "Did you mean to use append_scaling_mut()?"] pub fn append_scaling(&self, scaling: N) -> Self { assert!( - !relative_eq!(scaling, N::zero()), + !scaling.is_zero(), "The similarity scaling factor must not be zero." ); @@ -185,7 +188,7 @@ where #[inline] pub fn prepend_scaling_mut(&mut self, scaling: N) { assert!( - !relative_eq!(scaling, N::zero()), + !scaling.is_zero(), "The similarity scaling factor must not be zero." ); @@ -196,7 +199,7 @@ where #[inline] pub fn append_scaling_mut(&mut self, scaling: N) { assert!( - !relative_eq!(scaling, N::zero()), + !scaling.is_zero(), "The similarity scaling factor must not be zero." ); @@ -316,7 +319,7 @@ where // and makes it harder to use it, e.g., for Transform × Isometry implementation. // This is OK since all constructors of the isometry enforce the Rotation bound already (and // explicit struct construction is prevented by the private scaling factor). -impl Similarity +impl Similarity where DefaultAllocator: Allocator { /// Converts this similarity into its equivalent homogeneous transformation matrix. @@ -337,14 +340,14 @@ where DefaultAllocator: Allocator } } -impl Eq for Similarity +impl Eq for Similarity where R: AbstractRotation + Eq, DefaultAllocator: Allocator, { } -impl PartialEq for Similarity +impl PartialEq for Similarity where R: AbstractRotation + PartialEq, DefaultAllocator: Allocator, diff --git a/src/geometry/similarity_construction.rs b/src/geometry/similarity_construction.rs index f71f60dc..60e6d0f0 100644 --- a/src/geometry/similarity_construction.rs +++ b/src/geometry/similarity_construction.rs @@ -8,6 +8,7 @@ use rand::distributions::{Distribution, Standard}; use rand::Rng; use simba::scalar::RealField; +use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimName, U2, U3}; @@ -18,8 +19,9 @@ use crate::geometry::{ UnitComplex, UnitQuaternion, }; -impl Similarity +impl Similarity where + N::Element: SimdRealField, R: AbstractRotation, DefaultAllocator: Allocator, { @@ -44,8 +46,9 @@ where } } -impl One for Similarity +impl One for Similarity where + N::Element: SimdRealField, R: AbstractRotation, DefaultAllocator: Allocator, { @@ -73,8 +76,9 @@ where } } -impl Similarity +impl Similarity where + N::Element: SimdRealField, R: AbstractRotation, DefaultAllocator: Allocator, { @@ -103,7 +107,8 @@ where #[cfg(feature = "arbitrary")] impl Arbitrary for Similarity where - N: RealField + Arbitrary + Send, + N: SimdRealField + Arbitrary + Send, + N::Element: SimdRealField, R: AbstractRotation + Arbitrary + Send, DefaultAllocator: Allocator, Owned: Send, @@ -111,7 +116,7 @@ where #[inline] fn arbitrary(rng: &mut G) -> Self { let mut s = Arbitrary::arbitrary(rng); - while relative_eq!(s, N::zero()) { + while s.is_zero() { s = Arbitrary::arbitrary(rng) } @@ -125,8 +130,10 @@ where * */ -// 2D rotation. -impl Similarity> { +// 2D similarity. +impl Similarity> +where N::Element: SimdRealField +{ /// Creates a new similarity from a translation, a rotation, and an uniform scaling factor. /// /// # Example @@ -149,7 +156,9 @@ impl Similarity> { } } -impl Similarity> { +impl Similarity> +where N::Element: SimdRealField +{ /// Creates a new similarity from a translation and a rotation angle. /// /// # Example @@ -175,7 +184,8 @@ impl Similarity> { // 3D rotation. macro_rules! similarity_construction_impl( ($Rot: ty) => { - impl Similarity { + impl Similarity + where N::Element: SimdRealField { /// Creates a new similarity from a translation, rotation axis-angle, and scaling /// factor. /// @@ -202,7 +212,8 @@ macro_rules! similarity_construction_impl( /// assert_relative_eq!(sim * vec, Vector3::new(18.0, 15.0, -12.0), epsilon = 1.0e-5); /// ``` #[inline] - pub fn new(translation: Vector3, axisangle: Vector3, scaling: N) -> Self { + pub fn new(translation: Vector3, axisangle: Vector3, scaling: N) -> Self + { Self::from_isometry(Isometry::<_, U3, $Rot>::new(translation, axisangle), scaling) } diff --git a/src/geometry/similarity_conversion.rs b/src/geometry/similarity_conversion.rs index 6b0adb27..032deff2 100644 --- a/src/geometry/similarity_conversion.rs +++ b/src/geometry/similarity_conversion.rs @@ -1,4 +1,5 @@ use simba::scalar::{RealField, SubsetOf, SupersetOf}; +use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimMin, DimName, DimNameAdd, DimNameSum, U1}; @@ -168,7 +169,7 @@ where } } -impl From> for MatrixN> +impl From> for MatrixN> where D: DimNameAdd, R: SubsetOf>>, diff --git a/src/geometry/similarity_ops.rs b/src/geometry/similarity_ops.rs index f34b13b9..f98063dc 100644 --- a/src/geometry/similarity_ops.rs +++ b/src/geometry/similarity_ops.rs @@ -1,7 +1,8 @@ use num::{One, Zero}; use std::ops::{Div, DivAssign, Mul, MulAssign}; -use simba::scalar::{ClosedAdd, ClosedMul, RealField}; +use simba::scalar::{ClosedAdd, ClosedMul}; +use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimName, U1, U3, U4}; @@ -68,8 +69,9 @@ macro_rules! similarity_binop_impl( ($Op: ident, $op: ident; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty; $action: expr; $($lives: tt),*) => { - impl<$($lives ,)* N: RealField, D: DimName, R> $Op<$Rhs> for $Lhs - where R: AbstractRotation, + impl<$($lives ,)* N: SimdRealField, D: DimName, R> $Op<$Rhs> for $Lhs + where N::Element: SimdRealField, + R: AbstractRotation, DefaultAllocator: Allocator { type Output = $Output; @@ -115,8 +117,9 @@ macro_rules! similarity_binop_assign_impl_all( $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty; [val] => $action_val: expr; [ref] => $action_ref: expr;) => { - impl $OpAssign<$Rhs> for $Lhs - where R: AbstractRotation, + impl $OpAssign<$Rhs> for $Lhs + where N::Element: SimdRealField, + R: AbstractRotation, DefaultAllocator: Allocator { #[inline] fn $op_assign(&mut $lhs, $rhs: $Rhs) { @@ -124,8 +127,9 @@ macro_rules! similarity_binop_assign_impl_all( } } - impl<'b, N: RealField, D: DimName, R> $OpAssign<&'b $Rhs> for $Lhs - where R: AbstractRotation, + impl<'b, N: SimdRealField, D: DimName, R> $OpAssign<&'b $Rhs> for $Lhs + where N::Element: SimdRealField, + R: AbstractRotation, DefaultAllocator: Allocator { #[inline] fn $op_assign(&mut $lhs, $rhs: &'b $Rhs) { @@ -161,6 +165,7 @@ similarity_binop_impl_all!( // Similarity ×= Translation similarity_binop_assign_impl_all!( + MulAssign, mul_assign; self: Similarity, rhs: Translation; [val] => *self *= &rhs; @@ -214,7 +219,7 @@ similarity_binop_assign_impl_all!( // Similarity ×= R // Similarity ÷= R md_assign_impl_all!( - MulAssign, mul_assign where N: RealField; + MulAssign, mul_assign where N: SimdRealField for N::Element: SimdRealField; (D, U1), (D, D) for D: DimName; self: Similarity>, rhs: Rotation; [val] => self.isometry.rotation *= rhs; @@ -222,7 +227,7 @@ md_assign_impl_all!( ); md_assign_impl_all!( - DivAssign, div_assign where N: RealField; + DivAssign, div_assign where N: SimdRealField for N::Element: SimdRealField; (D, U1), (D, D) for D: DimName; self: Similarity>, rhs: Rotation; // FIXME: don't invert explicitly? @@ -231,7 +236,7 @@ md_assign_impl_all!( ); md_assign_impl_all!( - MulAssign, mul_assign where N: RealField; + MulAssign, mul_assign where N: SimdRealField for N::Element: SimdRealField; (U3, U3), (U3, U3) for; self: Similarity>, rhs: UnitQuaternion; [val] => self.isometry.rotation *= rhs; @@ -239,7 +244,7 @@ md_assign_impl_all!( ); md_assign_impl_all!( - DivAssign, div_assign where N: RealField; + DivAssign, div_assign where N: SimdRealField for N::Element: SimdRealField; (U3, U3), (U3, U3) for; self: Similarity>, rhs: UnitQuaternion; // FIXME: don't invert explicitly? @@ -368,8 +373,9 @@ macro_rules! similarity_from_composition_impl( ($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(for $Dims: ident: $DimsBound: ident),*; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty; $action: expr; $($lives: tt),*) => { - impl<$($lives ,)* N: RealField $(, $Dims: $DimsBound)*> $Op<$Rhs> for $Lhs - where DefaultAllocator: Allocator + + impl<$($lives ,)* N: SimdRealField $(, $Dims: $DimsBound)*> $Op<$Rhs> for $Lhs + where N::Element: SimdRealField, + DefaultAllocator: Allocator + Allocator { type Output = $Output; diff --git a/src/geometry/similarity_simba.rs b/src/geometry/similarity_simba.rs index e05ec213..3db56b74 100755 --- a/src/geometry/similarity_simba.rs +++ b/src/geometry/similarity_simba.rs @@ -1,18 +1,17 @@ -use simba::simd::SimdValue; +use simba::simd::{SimdRealField, SimdValue}; use crate::base::allocator::Allocator; use crate::base::dimension::DimName; use crate::base::{DefaultAllocator, Scalar}; -use crate::RealField; use crate::geometry::{AbstractRotation, Isometry, Similarity}; -impl SimdValue for Similarity +impl SimdValue for Similarity where - N::Element: Scalar, + N::Element: SimdRealField, R: SimdValue + AbstractRotation, - R::Element: AbstractRotation, - DefaultAllocator: Allocator, + R::Element: AbstractRotation, + DefaultAllocator: Allocator + Allocator, { type Element = Similarity; type SimdBool = N::SimdBool; diff --git a/src/geometry/unit_complex.rs b/src/geometry/unit_complex.rs index 2ed88378..7b51391b 100755 --- a/src/geometry/unit_complex.rs +++ b/src/geometry/unit_complex.rs @@ -40,7 +40,9 @@ impl Normed for Complex { } } -impl UnitComplex { +impl UnitComplex +where N::Element: SimdRealField +{ /// The rotation angle in `]-pi; pi]` of this unit complex number. /// /// # Example @@ -51,7 +53,7 @@ impl UnitComplex { /// ``` #[inline] pub fn angle(&self) -> N { - self.im.atan2(self.re) + self.im.simd_atan2(self.re) } /// The sine of the rotation angle. @@ -97,7 +99,8 @@ impl UnitComplex { /// the `.angle()` method instead is more common. /// Returns `None` if the angle is zero. #[inline] - pub fn axis_angle(&self) -> Option<(Unit>, N)> { + pub fn axis_angle(&self) -> Option<(Unit>, N)> + where N: RealField { let ang = self.angle(); if ang.is_zero() { diff --git a/src/geometry/unit_complex_construction.rs b/src/geometry/unit_complex_construction.rs index ce28ea74..a65498c4 100644 --- a/src/geometry/unit_complex_construction.rs +++ b/src/geometry/unit_complex_construction.rs @@ -11,8 +11,11 @@ use crate::base::storage::Storage; use crate::base::{Matrix2, Unit, Vector}; use crate::geometry::{Rotation2, UnitComplex}; use simba::scalar::RealField; +use simba::simd::SimdRealField; -impl UnitComplex { +impl UnitComplex +where N::Element: SimdRealField +{ /// The unit complex number multiplicative identity. /// /// # Example @@ -43,7 +46,7 @@ impl UnitComplex { /// ``` #[inline] pub fn new(angle: N) -> Self { - let (sin, cos) = angle.sin_cos(); + let (sin, cos) = angle.simd_sin_cos(); Self::from_cos_sin_unchecked(cos, sin) } @@ -110,7 +113,7 @@ impl UnitComplex { /// The input complex number will be normalized. Returns the norm of the complex number as well. #[inline] pub fn from_complex_and_get(q: Complex) -> (Self, N) { - let norm = (q.im * q.im + q.re * q.re).sqrt(); + let norm = (q.im * q.im + q.re * q.re).simd_sqrt(); (Self::new_unchecked(q / norm), norm) } @@ -134,7 +137,8 @@ impl UnitComplex { /// This is an iterative method. See `.from_matrix_eps` to provide mover /// convergence parameters and starting solution. /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al. - pub fn from_matrix(m: &Matrix2) -> Self { + pub fn from_matrix(m: &Matrix2) -> Self + where N: RealField { Rotation2::from_matrix(m).into() } @@ -150,7 +154,8 @@ impl UnitComplex { /// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close /// to the actual solution is provided. Can be set to `UnitQuaternion::identity()` if no other /// guesses come to mind. - pub fn from_matrix_eps(m: &Matrix2, eps: N, max_iter: usize, guess: Self) -> Self { + pub fn from_matrix_eps(m: &Matrix2, eps: N, max_iter: usize, guess: Self) -> Self + where N: RealField { let guess = Rotation2::from(guess); Rotation2::from_matrix_eps(m, eps, max_iter, guess).into() } @@ -171,6 +176,7 @@ impl UnitComplex { #[inline] pub fn rotation_between(a: &Vector, b: &Vector) -> Self where + N: RealField, SB: Storage, SC: Storage, { @@ -198,6 +204,7 @@ impl UnitComplex { s: N, ) -> Self where + N: RealField, SB: Storage, SC: Storage, { @@ -264,29 +271,35 @@ impl UnitComplex { let sang = na.perp(&nb); let cang = na.dot(&nb); - Self::from_angle(sang.atan2(cang) * s) + Self::from_angle(sang.simd_atan2(cang) * s) } } -impl One for UnitComplex { +impl One for UnitComplex +where N::Element: SimdRealField +{ #[inline] fn one() -> Self { Self::identity() } } -impl Distribution> for Standard -where OpenClosed01: Distribution +impl Distribution> for Standard +where + N::Element: SimdRealField, + OpenClosed01: Distribution, { /// Generate a uniformly distributed random `UnitComplex`. #[inline] fn sample<'a, R: Rng + ?Sized>(&self, rng: &mut R) -> UnitComplex { - UnitComplex::from_angle(rng.sample(OpenClosed01) * N::two_pi()) + UnitComplex::from_angle(rng.sample(OpenClosed01) * N::simd_two_pi()) } } #[cfg(feature = "arbitrary")] -impl Arbitrary for UnitComplex { +impl Arbitrary for UnitComplex +where N::Element: SimdRealField +{ #[inline] fn arbitrary(g: &mut G) -> Self { UnitComplex::from_angle(N::arbitrary(g)) diff --git a/src/geometry/unit_complex_conversion.rs b/src/geometry/unit_complex_conversion.rs index bd442ee7..4a9402cb 100644 --- a/src/geometry/unit_complex_conversion.rs +++ b/src/geometry/unit_complex_conversion.rs @@ -2,6 +2,7 @@ use num::Zero; use num_complex::Complex; use simba::scalar::{RealField, SubsetOf, SupersetOf}; +use simba::simd::SimdRealField; use crate::base::dimension::U2; use crate::base::{Matrix2, Matrix3}; @@ -153,28 +154,36 @@ impl> SubsetOf> for Un } } -impl From> for Rotation2 { +impl From> for Rotation2 +where N::Element: SimdRealField +{ #[inline] fn from(q: UnitComplex) -> Self { q.to_rotation_matrix() } } -impl From> for UnitComplex { +impl From> for UnitComplex +where N::Element: SimdRealField +{ #[inline] fn from(q: Rotation2) -> Self { Self::from_rotation_matrix(&q) } } -impl From> for Matrix3 { +impl From> for Matrix3 +where N::Element: SimdRealField +{ #[inline] fn from(q: UnitComplex) -> Matrix3 { q.to_homogeneous() } } -impl From> for Matrix2 { +impl From> for Matrix2 +where N::Element: SimdRealField +{ #[inline] fn from(q: UnitComplex) -> Self { q.to_rotation_matrix().into_inner() diff --git a/src/geometry/unit_complex_ops.rs b/src/geometry/unit_complex_ops.rs index 43c97cd9..d10d323e 100644 --- a/src/geometry/unit_complex_ops.rs +++ b/src/geometry/unit_complex_ops.rs @@ -6,6 +6,7 @@ use crate::base::storage::Storage; use crate::base::{DefaultAllocator, Unit, Vector, Vector2}; use crate::geometry::{Isometry, Point2, Rotation, Similarity, Translation, UnitComplex}; use simba::scalar::RealField; +use simba::simd::SimdRealField; /* * This file provides: @@ -44,7 +45,7 @@ use simba::scalar::RealField; */ // UnitComplex × UnitComplex -impl Mul for UnitComplex { +impl Mul for UnitComplex { type Output = Self; #[inline] @@ -53,7 +54,9 @@ impl Mul for UnitComplex { } } -impl<'a, N: RealField> Mul> for &'a UnitComplex { +impl<'a, N: SimdRealField> Mul> for &'a UnitComplex +where N::Element: SimdRealField +{ type Output = UnitComplex; #[inline] @@ -62,26 +65,32 @@ impl<'a, N: RealField> Mul> for &'a UnitComplex { } } -impl<'b, N: RealField> Mul<&'b UnitComplex> for UnitComplex { +impl<'b, N: SimdRealField> Mul<&'b UnitComplex> for UnitComplex +where N::Element: SimdRealField +{ type Output = Self; #[inline] fn mul(self, rhs: &'b UnitComplex) -> Self::Output { - Unit::new_unchecked(self.into_inner() * rhs.complex()) + Unit::new_unchecked(self.into_inner() * rhs.as_ref()) } } -impl<'a, 'b, N: RealField> Mul<&'b UnitComplex> for &'a UnitComplex { +impl<'a, 'b, N: SimdRealField> Mul<&'b UnitComplex> for &'a UnitComplex +where N::Element: SimdRealField +{ type Output = UnitComplex; #[inline] fn mul(self, rhs: &'b UnitComplex) -> Self::Output { - Unit::new_unchecked(self.complex() * rhs.complex()) + Unit::new_unchecked(self.complex() * rhs.as_ref()) } } // UnitComplex ÷ UnitComplex -impl Div for UnitComplex { +impl Div for UnitComplex +where N::Element: SimdRealField +{ type Output = Self; #[inline] @@ -90,7 +99,9 @@ impl Div for UnitComplex { } } -impl<'a, N: RealField> Div> for &'a UnitComplex { +impl<'a, N: SimdRealField> Div> for &'a UnitComplex +where N::Element: SimdRealField +{ type Output = UnitComplex; #[inline] @@ -99,7 +110,9 @@ impl<'a, N: RealField> Div> for &'a UnitComplex { } } -impl<'b, N: RealField> Div<&'b UnitComplex> for UnitComplex { +impl<'b, N: SimdRealField> Div<&'b UnitComplex> for UnitComplex +where N::Element: SimdRealField +{ type Output = Self; #[inline] @@ -108,7 +121,9 @@ impl<'b, N: RealField> Div<&'b UnitComplex> for UnitComplex { } } -impl<'a, 'b, N: RealField> Div<&'b UnitComplex> for &'a UnitComplex { +impl<'a, 'b, N: SimdRealField> Div<&'b UnitComplex> for &'a UnitComplex +where N::Element: SimdRealField +{ type Output = UnitComplex; #[inline] @@ -122,8 +137,9 @@ macro_rules! complex_op_impl( ($RDim: ident, $CDim: ident) $(for $Storage: ident: $StoragesBound: ident $(<$($BoundParam: ty),*>)*),*; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty; $action: expr; $($lives: tt),*) => { - impl<$($lives ,)* N: RealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs - where DefaultAllocator: Allocator { + impl<$($lives ,)* N: SimdRealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs + where N::Element: SimdRealField, + DefaultAllocator: Allocator { type Output = $Result; #[inline] @@ -300,14 +316,18 @@ complex_op_impl_all!( ); // UnitComplex ×= UnitComplex -impl MulAssign> for UnitComplex { +impl MulAssign> for UnitComplex +where N::Element: SimdRealField +{ #[inline] fn mul_assign(&mut self, rhs: UnitComplex) { *self = &*self * rhs } } -impl<'b, N: RealField> MulAssign<&'b UnitComplex> for UnitComplex { +impl<'b, N: SimdRealField> MulAssign<&'b UnitComplex> for UnitComplex +where N::Element: SimdRealField +{ #[inline] fn mul_assign(&mut self, rhs: &'b UnitComplex) { *self = &*self * rhs @@ -315,14 +335,18 @@ impl<'b, N: RealField> MulAssign<&'b UnitComplex> for UnitComplex { } // UnitComplex /= UnitComplex -impl DivAssign> for UnitComplex { +impl DivAssign> for UnitComplex +where N::Element: SimdRealField +{ #[inline] fn div_assign(&mut self, rhs: UnitComplex) { *self = &*self / rhs } } -impl<'b, N: RealField> DivAssign<&'b UnitComplex> for UnitComplex { +impl<'b, N: SimdRealField> DivAssign<&'b UnitComplex> for UnitComplex +where N::Element: SimdRealField +{ #[inline] fn div_assign(&mut self, rhs: &'b UnitComplex) { *self = &*self / rhs @@ -330,8 +354,10 @@ impl<'b, N: RealField> DivAssign<&'b UnitComplex> for UnitComplex { } // UnitComplex ×= Rotation -impl MulAssign> for UnitComplex -where DefaultAllocator: Allocator +impl MulAssign> for UnitComplex +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn mul_assign(&mut self, rhs: Rotation) { @@ -339,8 +365,10 @@ where DefaultAllocator: Allocator } } -impl<'b, N: RealField> MulAssign<&'b Rotation> for UnitComplex -where DefaultAllocator: Allocator +impl<'b, N: SimdRealField> MulAssign<&'b Rotation> for UnitComplex +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn mul_assign(&mut self, rhs: &'b Rotation) { @@ -349,8 +377,10 @@ where DefaultAllocator: Allocator } // UnitComplex ÷= Rotation -impl DivAssign> for UnitComplex -where DefaultAllocator: Allocator +impl DivAssign> for UnitComplex +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn div_assign(&mut self, rhs: Rotation) { @@ -358,8 +388,10 @@ where DefaultAllocator: Allocator } } -impl<'b, N: RealField> DivAssign<&'b Rotation> for UnitComplex -where DefaultAllocator: Allocator +impl<'b, N: SimdRealField> DivAssign<&'b Rotation> for UnitComplex +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn div_assign(&mut self, rhs: &'b Rotation) { @@ -368,8 +400,10 @@ where DefaultAllocator: Allocator } // Rotation ×= UnitComplex -impl MulAssign> for Rotation -where DefaultAllocator: Allocator +impl MulAssign> for Rotation +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn mul_assign(&mut self, rhs: UnitComplex) { @@ -377,8 +411,10 @@ where DefaultAllocator: Allocator } } -impl<'b, N: RealField> MulAssign<&'b UnitComplex> for Rotation -where DefaultAllocator: Allocator +impl<'b, N: SimdRealField> MulAssign<&'b UnitComplex> for Rotation +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn mul_assign(&mut self, rhs: &'b UnitComplex) { @@ -387,8 +423,10 @@ where DefaultAllocator: Allocator } // Rotation ÷= UnitComplex -impl DivAssign> for Rotation -where DefaultAllocator: Allocator +impl DivAssign> for Rotation +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn div_assign(&mut self, rhs: UnitComplex) { @@ -396,8 +434,10 @@ where DefaultAllocator: Allocator } } -impl<'b, N: RealField> DivAssign<&'b UnitComplex> for Rotation -where DefaultAllocator: Allocator +impl<'b, N: SimdRealField> DivAssign<&'b UnitComplex> for Rotation +where + N::Element: SimdRealField, + DefaultAllocator: Allocator, { #[inline] fn div_assign(&mut self, rhs: &'b UnitComplex) { diff --git a/src/geometry/unit_complex_simba.rs b/src/geometry/unit_complex_simba.rs index 4784fa15..e1c9de34 100755 --- a/src/geometry/unit_complex_simba.rs +++ b/src/geometry/unit_complex_simba.rs @@ -4,10 +4,10 @@ use std::ops::Deref; use crate::base::{Scalar, Unit}; use crate::geometry::UnitComplex; -use crate::RealField; +use crate::SimdRealField; -impl SimdValue for UnitComplex -where N::Element: Scalar +impl SimdValue for UnitComplex +where N::Element: SimdRealField { type Element = UnitComplex; type SimdBool = N::SimdBool;