srenblad/nalgebra
1
0
Fork 0
forked from M-Labs/nalgebra
Code Pull Requests Activity

Make Isometry, Unit/Quaternion, Rotation, Similarity, and UnitComplex partially compatible with AoSoA.

Browse Source
This commit is contained in:
sebcrozet 2020-03-21 23:22:55 +01:00
parent f8cd26cfa9
commit cbcf4d7c27
24 changed files with 570 additions and 368 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/geometry/abstract_rotation.rs
View File

@ -1,6 +1,6 @@
use crate::allocator::Allocator; use crate::allocator::Allocator;
use crate::geometry::{Rotation, UnitComplex, UnitQuaternion}; 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; use simba::scalar::ClosedMul;
@ -18,8 +18,10 @@ pub trait AbstractRotation<N: Scalar, D: DimName>: PartialEq + ClosedMul + Clone
where DefaultAllocator: Allocator<N, D>; where DefaultAllocator: Allocator<N, D>;
} }
impl<N: RealField, D: DimName> AbstractRotation<N, D> for Rotation<N, D> impl<N: SimdRealField, D: DimName> AbstractRotation<N, D> for Rotation<N, D>
where DefaultAllocator: Allocator<N, D, D> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, D, D>,
{ {
#[inline] #[inline]
fn identity() -> Self { fn identity() -> Self {
@ -61,7 +63,9 @@ where DefaultAllocator: Allocator<N, D, D>
} }
} }
impl<N: RealField> AbstractRotation<N, U3> for UnitQuaternion<N> { impl<N: SimdRealField> AbstractRotation<N, U3> for UnitQuaternion<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn identity() -> Self { fn identity() -> Self {
Self::identity() Self::identity()
@ -98,7 +102,9 @@ impl<N: RealField> AbstractRotation<N, U3> for UnitQuaternion<N> {
} }
} }
impl<N: RealField> AbstractRotation<N, U2> for UnitComplex<N> { impl<N: SimdRealField> AbstractRotation<N, U2> for UnitComplex<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn identity() -> Self { fn identity() -> Self {
Self::identity() Self::identity()

41
src/geometry/isometry.rs
View File

@ -12,6 +12,7 @@ use serde::{Deserialize, Serialize};
use abomonation::Abomonation; use abomonation::Abomonation;
use simba::scalar::{RealField, SubsetOf}; use simba::scalar::{RealField, SubsetOf};
use simba::simd::SimdRealField;
use crate::base::allocator::Allocator; use crate::base::allocator::Allocator;
use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1}; use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1};
@ -35,26 +36,19 @@ use crate::geometry::{AbstractRotation, Point, Translation};
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
Owned<N, D>: Deserialize<'de>")) Owned<N, D>: Deserialize<'de>"))
)] )]
pub struct Isometry<N: RealField, D: DimName, R> pub struct Isometry<N: SimdRealField, D: DimName, R>
where DefaultAllocator: Allocator<N, D> where DefaultAllocator: Allocator<N, D>
{ {
/// The pure rotational part of this isometry. /// The pure rotational part of this isometry.
pub rotation: R, pub rotation: R,
/// The pure translational part of this isometry. /// The pure translational part of this isometry.
pub translation: Translation<N, D>, pub translation: Translation<N, D>,
// One dummy private field just to prevent explicit construction.
#[cfg_attr(
feature = "serde-serialize",
serde(skip_serializing, skip_deserializing)
)]
_noconstruct: PhantomData<N>,
} }
#[cfg(feature = "abomonation-serialize")] #[cfg(feature = "abomonation-serialize")]
impl<N, D, R> Abomonation for Isometry<N, D, R> impl<N, D, R> Abomonation for Isometry<N, D, R>
where where
N: RealField, N: SimdRealField,
D: DimName, D: DimName,
R: Abomonation, R: Abomonation,
Translation<N, D>: Abomonation, Translation<N, D>: Abomonation,
@ -76,7 +70,7 @@ where
} }
} }
impl<N: RealField + hash::Hash, D: DimName + hash::Hash, R: hash::Hash> hash::Hash impl<N: SimdRealField + hash::Hash, D: DimName + hash::Hash, R: hash::Hash> hash::Hash
for Isometry<N, D, R> for Isometry<N, D, R>
where where
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
@ -88,15 +82,19 @@ where
} }
} }
impl<N: RealField, D: DimName + Copy, R: AbstractRotation<N, D> + Copy> Copy for Isometry<N, D, R> impl<N: SimdRealField, D: DimName + Copy, R: AbstractRotation<N, D> + Copy> Copy
for Isometry<N, D, R>
where where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
Owned<N, D>: Copy, Owned<N, D>: Copy,
{ {
} }
impl<N: RealField, D: DimName, R: AbstractRotation<N, D> + Clone> Clone for Isometry<N, D, R> impl<N: SimdRealField, D: DimName, R: AbstractRotation<N, D> + Clone> Clone for Isometry<N, D, R>
where DefaultAllocator: Allocator<N, D> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, D>,
{ {
#[inline] #[inline]
fn clone(&self) -> Self { fn clone(&self) -> Self {
@ -104,8 +102,10 @@ where DefaultAllocator: Allocator<N, D>
} }
} }
impl<N: RealField, D: DimName, R: AbstractRotation<N, D>> Isometry<N, D, R> impl<N: SimdRealField, D: DimName, R: AbstractRotation<N, D>> Isometry<N, D, R>
where DefaultAllocator: Allocator<N, D> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, D>,
{ {
/// Creates a new isometry from its rotational and translational parts. /// Creates a new isometry from its rotational and translational parts.
/// ///
@ -124,9 +124,8 @@ where DefaultAllocator: Allocator<N, D>
#[inline] #[inline]
pub fn from_parts(translation: Translation<N, D>, rotation: R) -> Self { pub fn from_parts(translation: Translation<N, D>, rotation: R) -> Self {
Self { Self {
rotation: rotation, rotation,
translation: translation, translation,
_noconstruct: PhantomData,
} }
} }
@ -352,7 +351,7 @@ where DefaultAllocator: Allocator<N, D>
// and makes it hard to use it, e.g., for Transform × Isometry implementation. // 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 // 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). // explicit struct construction is prevented by the dummy ZST field).
impl<N: RealField, D: DimName, R> Isometry<N, D, R> impl<N: SimdRealField, D: DimName, R> Isometry<N, D, R>
where DefaultAllocator: Allocator<N, D> where DefaultAllocator: Allocator<N, D>
{ {
/// Converts this isometry into its equivalent homogeneous transformation matrix. /// Converts this isometry into its equivalent homogeneous transformation matrix.
@ -385,14 +384,14 @@ where DefaultAllocator: Allocator<N, D>
} }
} }
impl<N: RealField, D: DimName, R> Eq for Isometry<N, D, R> impl<N: SimdRealField, D: DimName, R> Eq for Isometry<N, D, R>
where where
R: AbstractRotation<N, D> + Eq, R: AbstractRotation<N, D> + Eq,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
{ {
} }
impl<N: RealField, D: DimName, R> PartialEq for Isometry<N, D, R> impl<N: SimdRealField, D: DimName, R> PartialEq for Isometry<N, D, R>
where where
R: AbstractRotation<N, D> + PartialEq, R: AbstractRotation<N, D> + PartialEq,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,

27
src/geometry/isometry_construction.rs
View File

@ -8,6 +8,7 @@ use rand::distributions::{Distribution, Standard};
use rand::Rng; use rand::Rng;
use simba::scalar::RealField; use simba::scalar::RealField;
use simba::simd::SimdRealField;
use crate::base::allocator::Allocator; use crate::base::allocator::Allocator;
use crate::base::dimension::{DimName, U2, U3}; use crate::base::dimension::{DimName, U2, U3};
@ -18,8 +19,10 @@ use crate::geometry::{
Translation2, Translation3, UnitComplex, UnitQuaternion, Translation2, Translation3, UnitComplex, UnitQuaternion,
}; };
impl<N: RealField, D: DimName, R: AbstractRotation<N, D>> Isometry<N, D, R> impl<N: SimdRealField, D: DimName, R: AbstractRotation<N, D>> Isometry<N, D, R>
where DefaultAllocator: Allocator<N, D> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, D>,
{ {
/// Creates a new identity isometry. /// Creates a new identity isometry.
/// ///
@ -64,8 +67,10 @@ where DefaultAllocator: Allocator<N, D>
} }
} }
impl<N: RealField, D: DimName, R: AbstractRotation<N, D>> One for Isometry<N, D, R> impl<N: SimdRealField, D: DimName, R: AbstractRotation<N, D>> One for Isometry<N, D, R>
where DefaultAllocator: Allocator<N, D> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, D>,
{ {
/// Creates a new identity isometry. /// Creates a new identity isometry.
#[inline] #[inline]
@ -89,7 +94,8 @@ where
#[cfg(feature = "arbitrary")] #[cfg(feature = "arbitrary")]
impl<N, D: DimName, R> Arbitrary for Isometry<N, D, R> impl<N, D: DimName, R> Arbitrary for Isometry<N, D, R>
where where
N: RealField + Arbitrary + Send, N: SimdRealField + Arbitrary + Send,
N::Element: SimdRealField,
R: AbstractRotation<N, D> + Arbitrary + Send, R: AbstractRotation<N, D> + Arbitrary + Send,
Owned<N, D>: Send, Owned<N, D>: Send,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
@ -107,7 +113,9 @@ where
*/ */
// 2D rotation. // 2D rotation.
impl<N: RealField> Isometry<N, U2, Rotation2<N>> { impl<N: SimdRealField> Isometry<N, U2, Rotation2<N>>
where N::Element: SimdRealField
{
/// Creates a new 2D isometry from a translation and a rotation angle. /// Creates a new 2D isometry from a translation and a rotation angle.
/// ///
/// Its rotational part is represented as a 2x2 rotation matrix. /// Its rotational part is represented as a 2x2 rotation matrix.
@ -142,7 +150,9 @@ impl<N: RealField> Isometry<N, U2, Rotation2<N>> {
} }
} }
impl<N: RealField> Isometry<N, U2, UnitComplex<N>> { impl<N: SimdRealField> Isometry<N, U2, UnitComplex<N>>
where N::Element: SimdRealField
{
/// Creates a new 2D isometry from a translation and a rotation angle. /// Creates a new 2D isometry from a translation and a rotation angle.
/// ///
/// Its rotational part is represented as an unit complex number. /// Its rotational part is represented as an unit complex number.
@ -180,7 +190,8 @@ impl<N: RealField> Isometry<N, U2, UnitComplex<N>> {
// 3D rotation. // 3D rotation.
macro_rules! isometry_construction_impl( macro_rules! isometry_construction_impl(
($RotId: ident < $($RotParams: ident),*>, $RRDim: ty, $RCDim: ty) => { ($RotId: ident < $($RotParams: ident),*>, $RRDim: ty, $RCDim: ty) => {
impl<N: RealField> Isometry<N, U3, $RotId<$($RotParams),*>> { impl<N: SimdRealField> Isometry<N, U3, $RotId<$($RotParams),*>>
where N::Element: SimdRealField {
/// Creates a new isometry from a translation and a rotation axis-angle. /// Creates a new isometry from a translation and a rotation axis-angle.
/// ///
/// # Example /// # Example

3
src/geometry/isometry_conversion.rs
View File

@ -1,4 +1,5 @@
use simba::scalar::{RealField, SubsetOf, SupersetOf}; use simba::scalar::{RealField, SubsetOf, SupersetOf};
use simba::simd::SimdRealField;
use crate::base::allocator::Allocator; use crate::base::allocator::Allocator;
use crate::base::dimension::{DimMin, DimName, DimNameAdd, DimNameSum, U1}; use crate::base::dimension::{DimMin, DimName, DimNameAdd, DimNameSum, U1};
@ -150,7 +151,7 @@ where
} }
} }
impl<N: RealField, D: DimName, R> From<Isometry<N, D, R>> for MatrixN<N, DimNameSum<D, U1>> impl<N: SimdRealField, D: DimName, R> From<Isometry<N, D, R>> for MatrixN<N, DimNameSum<D, U1>>
where where
D: DimNameAdd<U1>, D: DimNameAdd<U1>,
R: SubsetOf<MatrixN<N, DimNameSum<D, U1>>>, R: SubsetOf<MatrixN<N, DimNameSum<D, U1>>>,

31
src/geometry/isometry_ops.rs
View File

@ -1,7 +1,8 @@
use num::{One, Zero}; use num::{One, Zero};
use std::ops::{Div, DivAssign, Mul, MulAssign}; 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::allocator::Allocator;
use crate::base::dimension::{DimName, U1, U3, U4}; use crate::base::dimension::{DimName, U1, U3, U4};
@ -65,8 +66,9 @@ macro_rules! isometry_binop_impl(
($Op: ident, $op: ident; ($Op: ident, $op: ident;
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
$action: expr; $($lives: tt),*) => { $action: expr; $($lives: tt),*) => {
impl<$($lives ,)* N: RealField, D: DimName, R> $Op<$Rhs> for $Lhs impl<$($lives ,)* N: SimdRealField, D: DimName, R> $Op<$Rhs> for $Lhs
where R: AbstractRotation<N, D>, where N::Element: SimdRealField,
R: AbstractRotation<N, D>,
DefaultAllocator: Allocator<N, D> { DefaultAllocator: Allocator<N, D> {
type Output = $Output; type Output = $Output;
@ -112,8 +114,9 @@ macro_rules! isometry_binop_assign_impl_all(
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty;
[val] => $action_val: expr; [val] => $action_val: expr;
[ref] => $action_ref: expr;) => { [ref] => $action_ref: expr;) => {
impl<N: RealField, D: DimName, R> $OpAssign<$Rhs> for $Lhs impl<N: SimdRealField, D: DimName, R> $OpAssign<$Rhs> for $Lhs
where R: AbstractRotation<N, D>, where N::Element: SimdRealField,
R: AbstractRotation<N, D>,
DefaultAllocator: Allocator<N, D> { DefaultAllocator: Allocator<N, D> {
#[inline] #[inline]
fn $op_assign(&mut $lhs, $rhs: $Rhs) { 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 impl<'b, N: SimdRealField, D: DimName, R> $OpAssign<&'b $Rhs> for $Lhs
where R: AbstractRotation<N, D>, where N::Element: SimdRealField,
R: AbstractRotation<N, D>,
DefaultAllocator: Allocator<N, D> { DefaultAllocator: Allocator<N, D> {
#[inline] #[inline]
fn $op_assign(&mut $lhs, $rhs: &'b $Rhs) { fn $op_assign(&mut $lhs, $rhs: &'b $Rhs) {
@ -191,7 +195,7 @@ isometry_binop_assign_impl_all!(
// Isometry ×= R // Isometry ×= R
// Isometry ÷= R // Isometry ÷= R
md_assign_impl_all!( 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; (D, U1), (D, D) for D: DimName;
self: Isometry<N, D, Rotation<N, D>>, rhs: Rotation<N, D>; self: Isometry<N, D, Rotation<N, D>>, rhs: Rotation<N, D>;
[val] => self.rotation *= rhs; [val] => self.rotation *= rhs;
@ -199,7 +203,7 @@ md_assign_impl_all!(
); );
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; (D, U1), (D, D) for D: DimName;
self: Isometry<N, D, Rotation<N, D>>, rhs: Rotation<N, D>; self: Isometry<N, D, Rotation<N, D>>, rhs: Rotation<N, D>;
// FIXME: don't invert explicitly? // FIXME: don't invert explicitly?
@ -208,7 +212,7 @@ md_assign_impl_all!(
); );
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; (U3, U3), (U3, U3) for;
self: Isometry<N, U3, UnitQuaternion<N>>, rhs: UnitQuaternion<N>; self: Isometry<N, U3, UnitQuaternion<N>>, rhs: UnitQuaternion<N>;
[val] => self.rotation *= rhs; [val] => self.rotation *= rhs;
@ -216,7 +220,7 @@ md_assign_impl_all!(
); );
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; (U3, U3), (U3, U3) for;
self: Isometry<N, U3, UnitQuaternion<N>>, rhs: UnitQuaternion<N>; self: Isometry<N, U3, UnitQuaternion<N>>, rhs: UnitQuaternion<N>;
// FIXME: don't invert explicitly? // 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),*; ($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(for $Dims: ident: $DimsBound: ident),*;
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
$action: expr; $($lives: tt),*) => { $action: expr; $($lives: tt),*) => {
impl<$($lives ,)* N: RealField $(, $Dims: $DimsBound)*> $Op<$Rhs> for $Lhs impl<$($lives ,)* N: SimdRealField $(, $Dims: $DimsBound)*> $Op<$Rhs> for $Lhs
where DefaultAllocator: Allocator<N, $R1, $C1> + where N::Element: SimdRealField,
DefaultAllocator: Allocator<N, $R1, $C1> +
Allocator<N, $R2, $C2> { Allocator<N, $R2, $C2> {
type Output = $Output; type Output = $Output;

10
src/geometry/isometry_simba.rs
View File

@ -3,16 +3,16 @@ use simba::simd::SimdValue;
use crate::base::allocator::Allocator; use crate::base::allocator::Allocator;
use crate::base::dimension::DimName; use crate::base::dimension::DimName;
use crate::base::{DefaultAllocator, Scalar}; use crate::base::{DefaultAllocator, Scalar};
use crate::RealField; use crate::SimdRealField;
use crate::geometry::{AbstractRotation, Isometry, Translation}; use crate::geometry::{AbstractRotation, Isometry, Translation};
impl<N: RealField, D: DimName, R> SimdValue for Isometry<N, D, R> impl<N: SimdRealField, D: DimName, R> SimdValue for Isometry<N, D, R>
where where
N::Element: Scalar, N::Element: SimdRealField,
R: SimdValue<SimdBool = N::SimdBool> + AbstractRotation<N, D>, R: SimdValue<SimdBool = N::SimdBool> + AbstractRotation<N, D>,
R::Element: AbstractRotation<N, D>, R::Element: AbstractRotation<N::Element, D>,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D> + Allocator<N::Element, D>,
{ {
type Element = Isometry<N::Element, D, R::Element>; type Element = Isometry<N::Element, D, R::Element>;
type SimdBool = N::SimdBool; type SimdBool = N::SimdBool;

9
src/geometry/op_macros.rs
View File

@ -85,7 +85,7 @@ macro_rules! md_impl_all(
macro_rules! md_assign_impl( macro_rules! md_assign_impl(
( (
// Operator, operator method, and scalar bounds. // 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. // Storage dimensions, and dimension bounds.
($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident $(<$($BoundParam: ty),*>)*),* ($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident $(<$($BoundParam: ty),*>)*),*
// [Optional] Extra allocator bounds. // [Optional] Extra allocator bounds.
@ -96,6 +96,7 @@ macro_rules! md_assign_impl(
$action: expr; $($lives: tt),*) => { $action: expr; $($lives: tt),*) => {
impl<$($lives ,)* N $(, $Dims: $DimsBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs impl<$($lives ,)* N $(, $Dims: $DimsBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs
where N: Scalar + Zero + One + ClosedAdd + ClosedMul $($(+ $ScalarBounds)*)*, where N: Scalar + Zero + One + ClosedAdd + ClosedMul $($(+ $ScalarBounds)*)*,
$(N::Element: $ElementBounds,)*
DefaultAllocator: Allocator<N, $R1, $C1> + DefaultAllocator: Allocator<N, $R1, $C1> +
Allocator<N, $R2, $C2>, Allocator<N, $R2, $C2>,
$( $ConstraintType: $ConstraintBound $(<$( $ConstraintBoundParams $( = $EqBound )*),*>)* ),* $( $ConstraintType: $ConstraintBound $(<$( $ConstraintBoundParams $( = $EqBound )*),*>)* ),*
@ -113,7 +114,7 @@ macro_rules! md_assign_impl(
macro_rules! md_assign_impl_all( macro_rules! md_assign_impl_all(
( (
// Operator, operator method, and scalar bounds. // 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. // Storage dimensions, and dimension bounds.
($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident $(<$($BoundParam: ty),*>)*),* ($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident $(<$($BoundParam: ty),*>)*),*
// [Optional] Extra allocator bounds. // [Optional] Extra allocator bounds.
@ -124,14 +125,14 @@ macro_rules! md_assign_impl_all(
[val] => $action_val: expr; [val] => $action_val: expr;
[ref] => $action_ref: expr;) => { [ref] => $action_ref: expr;) => {
md_assign_impl!( 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),*>)*),* ($R1, $C1),($R2, $C2) for $($Dims: $DimsBound $(<$($BoundParam),*>)*),*
$(where $ConstraintType: $ConstraintBound $(<$($ConstraintBoundParams $( = $EqBound )*),*>)*)*; $(where $ConstraintType: $ConstraintBound $(<$($ConstraintBoundParams $( = $EqBound )*),*>)*)*;
$lhs: $Lhs, $rhs: $Rhs; $lhs: $Lhs, $rhs: $Rhs;
$action_val; ); $action_val; );
md_assign_impl!( 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),*>)*),* ($R1, $C1),($R2, $C2) for $($Dims: $DimsBound $(<$($BoundParam),*>)*),*
$(where $ConstraintType: $ConstraintBound $(<$($ConstraintBoundParams $( = $EqBound )*),*>)*)*; $(where $ConstraintType: $ConstraintBound $(<$($ConstraintBoundParams $( = $EqBound )*),*>)*)*;
$lhs: $Lhs, $rhs: &'b $Rhs; $lhs: $Lhs, $rhs: &'b $Rhs;

157
src/geometry/quaternion.rs
View File

@ -14,11 +14,13 @@ use serde::{Deserialize, Deserializer, Serialize, Serializer};
use abomonation::Abomonation; use abomonation::Abomonation;
use simba::scalar::RealField; 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::dimension::{U1, U3, U4};
use crate::base::storage::{CStride, RStride}; 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}; use crate::geometry::{Point3, Rotation};
@ -26,7 +28,7 @@ use crate::geometry::{Point3, Rotation};
/// that may be used as a rotation. /// that may be used as a rotation.
#[repr(C)] #[repr(C)]
#[derive(Debug)] #[derive(Debug)]
pub struct Quaternion<N: SimdRealField> { pub struct Quaternion<N: Scalar + SimdValue> {
/// This quaternion as a 4D vector of coordinates in the `[ x, y, z, w ]` storage order. /// This quaternion as a 4D vector of coordinates in the `[ x, y, z, w ]` storage order.
pub coords: Vector4<N>, pub coords: Vector4<N>,
} }
@ -48,9 +50,11 @@ where Vector4<N>: Abomonation
} }
} }
impl<N: SimdRealField + Eq> Eq for Quaternion<N> {} impl<N: SimdRealField + Eq> Eq for Quaternion<N> where N::Element: SimdRealField {}
impl<N: SimdRealField> PartialEq for Quaternion<N> { impl<N: SimdRealField> PartialEq for Quaternion<N>
where N::Element: SimdRealField
{
fn eq(&self, rhs: &Self) -> bool { fn eq(&self, rhs: &Self) -> bool {
self.coords == rhs.coords || self.coords == rhs.coords ||
// Account for the double-covering of S², i.e. q = -q // Account for the double-covering of S², i.e. q = -q
@ -95,7 +99,9 @@ where Owned<N, U4>: Deserialize<'a>
} }
} }
impl<N: SimdRealField> Quaternion<N> { impl<N: SimdRealField> Quaternion<N>
where N::Element: SimdRealField
{
/// Moves this unit quaternion into one that owns its data. /// Moves this unit quaternion into one that owns its data.
#[inline] #[inline]
#[deprecated(note = "This method is a no-op and will be removed in a future release.")] #[deprecated(note = "This method is a no-op and will be removed in a future release.")]
@ -282,7 +288,9 @@ impl<N: SimdRealField> Quaternion<N> {
} }
} }
impl<N: RealField> Quaternion<N> { impl<N: SimdRealField> Quaternion<N>
where N::Element: SimdRealField
{
/// Inverts this quaternion if it is not zero. /// Inverts this quaternion if it is not zero.
/// ///
/// # Example /// # Example
@ -303,7 +311,8 @@ impl<N: RealField> Quaternion<N> {
/// ``` /// ```
#[inline] #[inline]
#[must_use = "Did you mean to use try_inverse_mut()?"] #[must_use = "Did you mean to use try_inverse_mut()?"]
pub fn try_inverse(&self) -> Option<Self> { pub fn try_inverse(&self) -> Option<Self>
where N: RealField {
let mut res = self.clone(); let mut res = self.clone();
if res.try_inverse_mut() { if res.try_inverse_mut() {
@ -313,6 +322,14 @@ impl<N: RealField> Quaternion<N> {
} }
} }
#[inline]
#[must_use = "Did you mean to use try_inverse_mut()?"]
pub fn simd_try_inverse(&self) -> SimdOption<Self> {
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). /// Calculates the inner product (also known as the dot product).
/// See "Foundations of Game Engine Development, Volume 1: Mathematics" by Lengyel /// See "Foundations of Game Engine Development, Volume 1: Mathematics" by Lengyel
/// Formula 4.89. /// Formula 4.89.
@ -365,7 +382,8 @@ impl<N: RealField> Quaternion<N> {
/// assert_relative_eq!(expected, result, epsilon = 1.0e-5); /// assert_relative_eq!(expected, result, epsilon = 1.0e-5);
/// ``` /// ```
#[inline] #[inline]
pub fn project(&self, other: &Self) -> Option<Self> { pub fn project(&self, other: &Self) -> Option<Self>
where N: RealField {
self.inner(other).right_div(other) self.inner(other).right_div(other)
} }
@ -384,7 +402,8 @@ impl<N: RealField> Quaternion<N> {
/// assert_relative_eq!(expected, result, epsilon = 1.0e-5); /// assert_relative_eq!(expected, result, epsilon = 1.0e-5);
/// ``` /// ```
#[inline] #[inline]
pub fn reject(&self, other: &Self) -> Option<Self> { pub fn reject(&self, other: &Self) -> Option<Self>
where N: RealField {
self.outer(other).right_div(other) self.outer(other).right_div(other)
} }
@ -403,7 +422,8 @@ impl<N: RealField> Quaternion<N> {
/// assert_eq!(half_ang, f32::consts::FRAC_PI_2); /// assert_eq!(half_ang, f32::consts::FRAC_PI_2);
/// assert_eq!(axis, Some(Vector3::x_axis())); /// assert_eq!(axis, Some(Vector3::x_axis()));
/// ``` /// ```
pub fn polar_decomposition(&self) -> (N, N, Option<Unit<Vector3<N>>>) { pub fn polar_decomposition(&self) -> (N, N, Option<Unit<Vector3<N>>>)
where N: RealField {
if let Some((q, n)) = Unit::try_new_and_get(*self, N::zero()) { 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()) { if let Some(axis) = Unit::try_new(self.vector().clone_owned(), N::zero()) {
let angle = q.angle() / crate::convert(2.0f64); let angle = q.angle() / crate::convert(2.0f64);
@ -432,7 +452,7 @@ impl<N: RealField> Quaternion<N> {
let v = self.vector(); let v = self.vector();
let s = self.scalar(); 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. /// Compute the exponential of a quaternion.
@ -446,7 +466,7 @@ impl<N: RealField> Quaternion<N> {
/// ``` /// ```
#[inline] #[inline]
pub fn exp(&self) -> Self { 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 /// Compute the exponential of a quaternion. Returns the identity if the vector part of this quaternion
@ -467,16 +487,17 @@ impl<N: RealField> Quaternion<N> {
pub fn exp_eps(&self, eps: N) -> Self { pub fn exp_eps(&self, eps: N) -> Self {
let v = self.vector(); let v = self.vector();
let nn = v.norm_squared(); 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::from_parts(w_exp * n.simd_cos(), nv)
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)
}
} }
/// Raise the quaternion to a given floating power. /// Raise the quaternion to a given floating power.
@ -560,17 +581,11 @@ impl<N: RealField> Quaternion<N> {
/// assert!(!q.try_inverse_mut()); /// assert!(!q.try_inverse_mut());
/// ``` /// ```
#[inline] #[inline]
pub fn try_inverse_mut(&mut self) -> bool { pub fn try_inverse_mut(&mut self) -> N::SimdBool {
let norm_squared = self.norm_squared(); let norm_squared = self.norm_squared();
let ge = norm_squared.simd_ge(N::simd_default_epsilon());
if relative_eq!(&norm_squared, &N::zero()) { *self = ge.if_else(|| self.conjugate() / norm_squared, || *self);
false ge
} else {
self.conjugate_mut();
self.coords /= norm_squared;
true
}
} }
/// Normalizes this quaternion. /// Normalizes this quaternion.
@ -607,6 +622,8 @@ impl<N: RealField> Quaternion<N> {
} }
/// Check if the quaternion is pure. /// Check if the quaternion is pure.
///
/// A quaternion is pure if it has no real part (`self.w == 0.0`).
#[inline] #[inline]
pub fn is_pure(&self) -> bool { pub fn is_pure(&self) -> bool {
self.w.is_zero() self.w.is_zero()
@ -622,7 +639,8 @@ impl<N: RealField> Quaternion<N> {
/// ///
/// Calculates B<sup>-1</sup> * A where A = self, B = other. /// Calculates B<sup>-1</sup> * A where A = self, B = other.
#[inline] #[inline]
pub fn left_div(&self, other: &Self) -> Option<Self> { pub fn left_div(&self, other: &Self) -> Option<Self>
where N: RealField {
other.try_inverse().map(|inv| inv * self) other.try_inverse().map(|inv| inv * self)
} }
@ -641,7 +659,8 @@ impl<N: RealField> Quaternion<N> {
/// assert_relative_eq!(expected, result, epsilon = 1.0e-7); /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
/// ``` /// ```
#[inline] #[inline]
pub fn right_div(&self, other: &Self) -> Option<Self> { pub fn right_div(&self, other: &Self) -> Option<Self>
where N: RealField {
other.try_inverse().map(|inv| self * inv) other.try_inverse().map(|inv| self * inv)
} }
@ -659,8 +678,8 @@ impl<N: RealField> Quaternion<N> {
#[inline] #[inline]
pub fn cos(&self) -> Self { pub fn cos(&self) -> Self {
let z = self.imag().magnitude(); let z = self.imag().magnitude();
let w = -self.w.sin() * z.sinhc(); let w = -self.w.simd_sin() * z.simd_sinhc();
Self::from_parts(self.w.cos() * z.cosh(), self.imag() * w) Self::from_parts(self.w.simd_cos() * z.simd_cosh(), self.imag() * w)
} }
/// Calculates the quaternionic arccosinus. /// Calculates the quaternionic arccosinus.
@ -697,8 +716,8 @@ impl<N: RealField> Quaternion<N> {
#[inline] #[inline]
pub fn sin(&self) -> Self { pub fn sin(&self) -> Self {
let z = self.imag().magnitude(); let z = self.imag().magnitude();
let w = self.w.cos() * z.sinhc(); let w = self.w.simd_cos() * z.simd_sinhc();
Self::from_parts(self.w.sin() * z.cosh(), self.imag() * w) Self::from_parts(self.w.simd_sin() * z.simd_cosh(), self.imag() * w)
} }
/// Calculates the quaternionic arcsinus. /// Calculates the quaternionic arcsinus.
@ -733,7 +752,8 @@ impl<N: RealField> Quaternion<N> {
/// assert_relative_eq!(expected, result, epsilon = 1.0e-7); /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
/// ``` /// ```
#[inline] #[inline]
pub fn tan(&self) -> Self { pub fn tan(&self) -> Self
where N: RealField {
self.sin().right_div(&self.cos()).unwrap() self.sin().right_div(&self.cos()).unwrap()
} }
@ -748,7 +768,8 @@ impl<N: RealField> Quaternion<N> {
/// assert_relative_eq!(input, result, epsilon = 1.0e-7); /// assert_relative_eq!(input, result, epsilon = 1.0e-7);
/// ``` /// ```
#[inline] #[inline]
pub fn atan(&self) -> Self { pub fn atan(&self) -> Self
where N: RealField {
let u = Self::from_imag(self.imag().normalize()); let u = Self::from_imag(self.imag().normalize());
let num = u + self; let num = u + self;
let den = u - self; let den = u - self;
@ -835,7 +856,8 @@ impl<N: RealField> Quaternion<N> {
/// assert_relative_eq!(expected, result, epsilon = 1.0e-7); /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
/// ``` /// ```
#[inline] #[inline]
pub fn tanh(&self) -> Self { pub fn tanh(&self) -> Self
where N: RealField {
self.sinh().right_div(&self.cosh()).unwrap() self.sinh().right_div(&self.cosh()).unwrap()
} }
@ -944,25 +966,9 @@ impl<N: SimdRealField> Normed for Quaternion<N> {
} }
} }
impl<N: RealField> UnitQuaternion<N> { impl<N: SimdRealField> UnitQuaternion<N>
/// Moves this unit quaternion into one that owns its data. where N::Element: SimdRealField
#[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
}
/// The rotation angle in [0; pi] of this unit quaternion. /// The rotation angle in [0; pi] of this unit quaternion.
/// ///
/// # Example /// # Example
@ -974,8 +980,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// ``` /// ```
#[inline] #[inline]
pub fn angle(&self) -> N { pub fn angle(&self) -> N {
let w = self.quaternion().scalar().abs(); let w = self.quaternion().scalar().simd_abs();
self.quaternion().imag().norm().atan2(w) * crate::convert(2.0f64) self.quaternion().imag().norm().simd_atan2(w) * crate::convert(2.0f64)
} }
/// The underlying quaternion. /// The underlying quaternion.
@ -1113,7 +1119,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// assert_eq!(q.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0)); /// assert_eq!(q.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0));
/// ``` /// ```
#[inline] #[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()) self.try_slerp(other, t, N::default_epsilon())
.expect("Quaternion slerp: ambiguous configuration.") .expect("Quaternion slerp: ambiguous configuration.")
} }
@ -1129,7 +1136,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// * `epsilon`: the value below which the sinus of the angle separating both quaternion /// * `epsilon`: the value below which the sinus of the angle separating both quaternion
/// must be to return `None`. /// must be to return `None`.
#[inline] #[inline]
pub fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option<Self> { pub fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option<Self>
where N: RealField {
let coords = if self.coords.dot(&other.coords) < N::zero() { let coords = if self.coords.dot(&other.coords) < N::zero() {
Unit::new_unchecked(self.coords).try_slerp( Unit::new_unchecked(self.coords).try_slerp(
&Unit::new_unchecked(-other.coords), &Unit::new_unchecked(-other.coords),
@ -1185,7 +1193,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// assert!(rot.axis().is_none()); /// assert!(rot.axis().is_none());
/// ``` /// ```
#[inline] #[inline]
pub fn axis(&self) -> Option<Unit<Vector3<N>>> { pub fn axis(&self) -> Option<Unit<Vector3<N>>>
where N: RealField {
let v = if self.quaternion().scalar() >= N::zero() { let v = if self.quaternion().scalar() >= N::zero() {
self.as_ref().vector().clone_owned() self.as_ref().vector().clone_owned()
} else { } else {
@ -1206,7 +1215,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// assert_relative_eq!(rot.scaled_axis(), axisangle, epsilon = 1.0e-6); /// assert_relative_eq!(rot.scaled_axis(), axisangle, epsilon = 1.0e-6);
/// ``` /// ```
#[inline] #[inline]
pub fn scaled_axis(&self) -> Vector3<N> { pub fn scaled_axis(&self) -> Vector3<N>
where N: RealField {
if let Some(axis) = self.axis() { if let Some(axis) = self.axis() {
axis.into_inner() * self.angle() axis.into_inner() * self.angle()
} else { } else {
@ -1231,7 +1241,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// assert!(rot.axis_angle().is_none()); /// assert!(rot.axis_angle().is_none());
/// ``` /// ```
#[inline] #[inline]
pub fn axis_angle(&self) -> Option<(Unit<Vector3<N>>, N)> { pub fn axis_angle(&self) -> Option<(Unit<Vector3<N>>, N)>
where N: RealField {
self.axis().map(|axis| (axis, self.angle())) self.axis().map(|axis| (axis, self.angle()))
} }
@ -1258,7 +1269,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// assert_relative_eq!(q.ln().vector().into_owned(), axisangle, epsilon = 1.0e-6); /// assert_relative_eq!(q.ln().vector().into_owned(), axisangle, epsilon = 1.0e-6);
/// ``` /// ```
#[inline] #[inline]
pub fn ln(&self) -> Quaternion<N> { pub fn ln(&self) -> Quaternion<N>
where N: RealField {
if let Some(v) = self.axis() { if let Some(v) = self.axis() {
Quaternion::from_imag(v.into_inner() * self.angle()) Quaternion::from_imag(v.into_inner() * self.angle())
} else { } else {
@ -1283,7 +1295,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// assert_eq!(pow.angle(), 2.4); /// assert_eq!(pow.angle(), 2.4);
/// ``` /// ```
#[inline] #[inline]
pub fn powf(&self, n: N) -> Self { pub fn powf(&self, n: N) -> Self
where N: RealField {
if let Some(v) = self.axis() { if let Some(v) = self.axis() {
Self::from_axis_angle(&v, self.angle() * n) Self::from_axis_angle(&v, self.angle() * n)
} else { } else {
@ -1343,7 +1356,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// The angles are produced in the form (roll, pitch, yaw). /// The angles are produced in the form (roll, pitch, yaw).
#[inline] #[inline]
#[deprecated(note = "This is renamed to use `.euler_angles()`.")] #[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() self.euler_angles()
} }
@ -1362,7 +1376,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6); /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6);
/// ``` /// ```
#[inline] #[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() self.to_rotation_matrix().euler_angles()
} }

154
src/geometry/quaternion_construction.rs
View File

@ -10,6 +10,7 @@ use rand::distributions::{Distribution, OpenClosed01, Standard};
use rand::Rng; use rand::Rng;
use simba::scalar::RealField; use simba::scalar::RealField;
use simba::simd::SimdBool;
use crate::base::dimension::U3; use crate::base::dimension::U3;
use crate::base::storage::Storage; use crate::base::storage::Storage;
@ -96,7 +97,9 @@ impl<N: SimdRealField> Quaternion<N> {
} }
// FIXME: merge with the previous block. // FIXME: merge with the previous block.
impl<N: RealField> Quaternion<N> { impl<N: SimdRealField> Quaternion<N>
where N::Element: SimdRealField
{
/// Creates a new quaternion from its polar decomposition. /// Creates a new quaternion from its polar decomposition.
/// ///
/// Note that `axis` is assumed to be a unit vector. /// Note that `axis` is assumed to be a unit vector.
@ -109,14 +112,18 @@ impl<N: RealField> Quaternion<N> {
} }
} }
impl<N: SimdRealField> One for Quaternion<N> { impl<N: SimdRealField> One for Quaternion<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn one() -> Self { fn one() -> Self {
Self::identity() Self::identity()
} }
} }
impl<N: SimdRealField> Zero for Quaternion<N> { impl<N: SimdRealField> Zero for Quaternion<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn zero() -> Self { fn zero() -> Self {
Self::from(Vector4::zero()) Self::from(Vector4::zero())
@ -152,7 +159,9 @@ where Owned<N, U4>: Send
} }
} }
impl<N: RealField> UnitQuaternion<N> { impl<N: SimdRealField> UnitQuaternion<N>
where N::Element: SimdRealField
{
/// The rotation identity. /// The rotation identity.
/// ///
/// # Example /// # Example
@ -199,7 +208,7 @@ impl<N: RealField> UnitQuaternion<N> {
#[inline] #[inline]
pub fn from_axis_angle<SB>(axis: &Unit<Vector<N, U3, SB>>, angle: N) -> Self pub fn from_axis_angle<SB>(axis: &Unit<Vector<N, U3, SB>>, angle: N) -> Self
where SB: Storage<N, U3> { where SB: Storage<N, U3> {
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); let q = Quaternion::from_parts(cang, axis.as_ref() * sang);
Self::new_unchecked(q) Self::new_unchecked(q)
@ -229,9 +238,9 @@ impl<N: RealField> UnitQuaternion<N> {
/// ``` /// ```
#[inline] #[inline]
pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self { pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self {
let (sr, cr) = (roll * 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)).sin_cos(); let (sp, cp) = (pitch * crate::convert(0.5f64)).simd_sin_cos();
let (sy, cy) = (yaw * crate::convert(0.5f64)).sin_cos(); let (sy, cy) = (yaw * crate::convert(0.5f64)).simd_sin_cos();
let q = Quaternion::new( let q = Quaternion::new(
cr * cp * cy + sr * sp * sy, cr * cp * cy + sr * sp * sy,
@ -262,46 +271,58 @@ impl<N: RealField> UnitQuaternion<N> {
// Robust matrix to quaternion transformation. // Robust matrix to quaternion transformation.
// See https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion // See https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion
let tr = rotmat[(0, 0)] + rotmat[(1, 1)] + rotmat[(2, 2)]; let tr = rotmat[(0, 0)] + rotmat[(1, 1)] + rotmat[(2, 2)];
let res;
let _0_25: N = crate::convert(0.25); let _0_25: N = crate::convert(0.25);
if tr > N::zero() { let res = tr.simd_gt(N::zero()).if_else3(
let denom = (tr + N::one()).sqrt() * crate::convert(2.0); || {
res = Quaternion::new( let denom = (tr + N::one()).simd_sqrt() * crate::convert(2.0);
_0_25 * denom, Quaternion::new(
(rotmat[(2, 1)] - rotmat[(1, 2)]) / denom, _0_25 * denom,
(rotmat[(0, 2)] - rotmat[(2, 0)]) / denom, (rotmat[(2, 1)] - rotmat[(1, 2)]) / denom,
(rotmat[(1, 0)] - rotmat[(0, 1)]) / 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[(0, 0)].simd_gt(rotmat[(1, 1)]) & rotmat[(0, 0)].simd_gt(rotmat[(2, 2)]),
(rotmat[(2, 1)] - rotmat[(1, 2)]) / denom, || {
_0_25 * denom, let denom = (N::one() + rotmat[(0, 0)] - rotmat[(1, 1)] - rotmat[(2, 2)])
(rotmat[(0, 1)] + rotmat[(1, 0)]) / denom, .simd_sqrt()
(rotmat[(0, 2)] + rotmat[(2, 0)]) / denom, * crate::convert(2.0);
); Quaternion::new(
} else if rotmat[(1, 1)] > rotmat[(2, 2)] { (rotmat[(2, 1)] - rotmat[(1, 2)]) / denom,
let denom = (N::one() + rotmat[(1, 1)] - rotmat[(0, 0)] - rotmat[(2, 2)]).sqrt() _0_25 * denom,
* crate::convert(2.0); (rotmat[(0, 1)] + rotmat[(1, 0)]) / denom,
res = Quaternion::new( (rotmat[(0, 2)] + rotmat[(2, 0)]) / denom,
(rotmat[(0, 2)] - rotmat[(2, 0)]) / denom, )
(rotmat[(0, 1)] + rotmat[(1, 0)]) / denom, },
_0_25 * denom, ),
(rotmat[(1, 2)] + rotmat[(2, 1)]) / denom, (
); || rotmat[(1, 1)].simd_gt(rotmat[(2, 2)]),
} else { || {
let denom = (N::one() + rotmat[(2, 2)] - rotmat[(0, 0)] - rotmat[(1, 1)]).sqrt() let denom = (N::one() + rotmat[(1, 1)] - rotmat[(0, 0)] - rotmat[(2, 2)])
* crate::convert(2.0); .simd_sqrt()
res = Quaternion::new( * crate::convert(2.0);
(rotmat[(1, 0)] - rotmat[(0, 1)]) / denom, Quaternion::new(
(rotmat[(0, 2)] + rotmat[(2, 0)]) / denom, (rotmat[(0, 2)] - rotmat[(2, 0)]) / denom,
(rotmat[(1, 2)] + rotmat[(2, 1)]) / denom, (rotmat[(0, 1)] + rotmat[(1, 0)]) / denom,
_0_25 * 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) Self::new_unchecked(res)
} }
@ -311,7 +332,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// This is an iterative method. See `.from_matrix_eps` to provide mover /// This is an iterative method. See `.from_matrix_eps` to provide mover
/// convergence parameters and starting solution. /// convergence parameters and starting solution.
/// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al. /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
pub fn from_matrix(m: &Matrix3<N>) -> Self { pub fn from_matrix(m: &Matrix3<N>) -> Self
where N: RealField {
Rotation3::from_matrix(m).into() Rotation3::from_matrix(m).into()
} }
@ -327,7 +349,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close /// * `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 /// to the actual solution is provided. Can be set to `UnitQuaternion::identity()` if no other
/// guesses come to mind. /// guesses come to mind.
pub fn from_matrix_eps(m: &Matrix3<N>, eps: N, max_iter: usize, guess: Self) -> Self { pub fn from_matrix_eps(m: &Matrix3<N>, eps: N, max_iter: usize, guess: Self) -> Self
where N: RealField {
let guess = Rotation3::from(guess); let guess = Rotation3::from(guess);
Rotation3::from_matrix_eps(m, eps, max_iter, guess).into() Rotation3::from_matrix_eps(m, eps, max_iter, guess).into()
} }
@ -348,6 +371,7 @@ impl<N: RealField> UnitQuaternion<N> {
#[inline] #[inline]
pub fn rotation_between<SB, SC>(a: &Vector<N, U3, SB>, b: &Vector<N, U3, SC>) -> Option<Self> pub fn rotation_between<SB, SC>(a: &Vector<N, U3, SB>, b: &Vector<N, U3, SC>) -> Option<Self>
where where
N: RealField,
SB: Storage<N, U3>, SB: Storage<N, U3>,
SC: Storage<N, U3>, SC: Storage<N, U3>,
{ {
@ -375,6 +399,7 @@ impl<N: RealField> UnitQuaternion<N> {
s: N, s: N,
) -> Option<Self> ) -> Option<Self>
where where
N: RealField,
SB: Storage<N, U3>, SB: Storage<N, U3>,
SC: Storage<N, U3>, SC: Storage<N, U3>,
{ {
@ -408,6 +433,7 @@ impl<N: RealField> UnitQuaternion<N> {
b: &Unit<Vector<N, U3, SC>>, b: &Unit<Vector<N, U3, SC>>,
) -> Option<Self> ) -> Option<Self>
where where
N: RealField,
SB: Storage<N, U3>, SB: Storage<N, U3>,
SC: Storage<N, U3>, SC: Storage<N, U3>,
{ {
@ -435,6 +461,7 @@ impl<N: RealField> UnitQuaternion<N> {
s: N, s: N,
) -> Option<Self> ) -> Option<Self>
where where
N: RealField,
SB: Storage<N, U3>, SB: Storage<N, U3>,
SC: Storage<N, U3>, SC: Storage<N, U3>,
{ {
@ -706,7 +733,8 @@ impl<N: RealField> UnitQuaternion<N> {
/// assert_relative_eq!(euler_angles_mean.0, 0.0, epsilon = 1.0e-7) /// assert_relative_eq!(euler_angles_mean.0, 0.0, epsilon = 1.0e-7)
/// ``` /// ```
#[inline] #[inline]
pub fn mean_of(unit_quaternions: impl IntoIterator<Item = Self>) -> Self { pub fn mean_of(unit_quaternions: impl IntoIterator<Item = Self>) -> Self
where N: RealField {
let quaternions_matrix: Matrix4<N> = unit_quaternions let quaternions_matrix: Matrix4<N> = unit_quaternions
.into_iter() .into_iter()
.map(|q| q.as_vector() * q.as_vector().transpose()) .map(|q| q.as_vector() * q.as_vector().transpose())
@ -734,15 +762,19 @@ impl<N: RealField> UnitQuaternion<N> {
} }
} }
impl<N: RealField> One for UnitQuaternion<N> { impl<N: SimdRealField> One for UnitQuaternion<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn one() -> Self { fn one() -> Self {
Self::identity() Self::identity()
} }
} }
impl<N: RealField> Distribution<UnitQuaternion<N>> for Standard impl<N: SimdRealField> Distribution<UnitQuaternion<N>> for Standard
where OpenClosed01: Distribution<N> where
N::Element: SimdRealField,
OpenClosed01: Distribution<N>,
{ {
/// Generate a uniformly distributed random rotation quaternion. /// Generate a uniformly distributed random rotation quaternion.
#[inline] #[inline]
@ -753,20 +785,20 @@ where OpenClosed01: Distribution<N>
let x0 = rng.sample(OpenClosed01); let x0 = rng.sample(OpenClosed01);
let x1 = rng.sample(OpenClosed01); let x1 = rng.sample(OpenClosed01);
let x2 = rng.sample(OpenClosed01); let x2 = rng.sample(OpenClosed01);
let theta1 = N::two_pi() * x1; let theta1 = N::simd_two_pi() * x1;
let theta2 = N::two_pi() * x2; let theta2 = N::simd_two_pi() * x2;
let s1 = theta1.sin(); let s1 = theta1.simd_sin();
let c1 = theta1.cos(); let c1 = theta1.simd_cos();
let s2 = theta2.sin(); let s2 = theta2.simd_sin();
let c2 = theta2.cos(); let c2 = theta2.simd_cos();
let r1 = (N::one() - x0).sqrt(); let r1 = (N::one() - x0).simd_sqrt();
let r2 = x0.sqrt(); let r2 = x0.simd_sqrt();
Unit::new_unchecked(Quaternion::new(s1 * r1, c1 * r1, s2 * r2, c2 * r2)) Unit::new_unchecked(Quaternion::new(s1 * r1, c1 * r1, s2 * r2, c2 * r2))
} }
} }
#[cfg(feature = "arbitrary")] #[cfg(feature = "arbitrary")]
impl<N: RealField + Arbitrary> Arbitrary for UnitQuaternion<N> impl<N: SimdRealField + Arbitrary> Arbitrary for UnitQuaternion<N>
where where
Owned<N, U4>: Send, Owned<N, U4>: Send,
Owned<N, U3>: Send, Owned<N, U3>: Send,

28
src/geometry/quaternion_conversion.rs
View File

@ -1,13 +1,13 @@
use num::Zero; use num::Zero;
use simba::scalar::{RealField, SubsetOf, SupersetOf}; use simba::scalar::{RealField, SubsetOf, SupersetOf};
use simba::simd::SimdRealField; use simba::simd::{SimdRealField, SimdValue};
#[cfg(feature = "mint")] #[cfg(feature = "mint")]
use mint; use mint;
use crate::base::dimension::U3; use crate::base::dimension::U3;
use crate::base::{Matrix3, Matrix4, Vector4}; use crate::base::{Matrix3, Matrix4, Scalar, Vector4};
use crate::geometry::{ use crate::geometry::{
AbstractRotation, Isometry, Quaternion, Rotation, Rotation3, Similarity, SuperTCategoryOf, AbstractRotation, Isometry, Quaternion, Rotation, Rotation3, Similarity, SuperTCategoryOf,
TAffine, Transform, Translation, UnitQuaternion, TAffine, Transform, Translation, UnitQuaternion,
@ -184,14 +184,14 @@ impl<N1: RealField, N2: RealField + SupersetOf<N1>> SubsetOf<Matrix4<N2>> for Un
} }
#[cfg(feature = "mint")] #[cfg(feature = "mint")]
impl<N: RealField> From<mint::Quaternion<N>> for Quaternion<N> { impl<N: SimdRealField> From<mint::Quaternion<N>> for Quaternion<N> {
fn from(q: mint::Quaternion<N>) -> Self { fn from(q: mint::Quaternion<N>) -> Self {
Self::new(q.s, q.v.x, q.v.y, q.v.z) Self::new(q.s, q.v.x, q.v.y, q.v.z)
} }
} }
#[cfg(feature = "mint")] #[cfg(feature = "mint")]
impl<N: RealField> Into<mint::Quaternion<N>> for Quaternion<N> { impl<N: SimdRealField> Into<mint::Quaternion<N>> for Quaternion<N> {
fn into(self) -> mint::Quaternion<N> { fn into(self) -> mint::Quaternion<N> {
mint::Quaternion { mint::Quaternion {
v: mint::Vector3 { v: mint::Vector3 {
@ -205,7 +205,7 @@ impl<N: RealField> Into<mint::Quaternion<N>> for Quaternion<N> {
} }
#[cfg(feature = "mint")] #[cfg(feature = "mint")]
impl<N: RealField> Into<mint::Quaternion<N>> for UnitQuaternion<N> { impl<N: SimdRealField> Into<mint::Quaternion<N>> for UnitQuaternion<N> {
fn into(self) -> mint::Quaternion<N> { fn into(self) -> mint::Quaternion<N> {
mint::Quaternion { mint::Quaternion {
v: mint::Vector3 { v: mint::Vector3 {
@ -218,35 +218,43 @@ impl<N: RealField> Into<mint::Quaternion<N>> for UnitQuaternion<N> {
} }
} }
impl<N: RealField> From<UnitQuaternion<N>> for Matrix4<N> { impl<N: SimdRealField> From<UnitQuaternion<N>> for Matrix4<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn from(q: UnitQuaternion<N>) -> Self { fn from(q: UnitQuaternion<N>) -> Self {
q.to_homogeneous() q.to_homogeneous()
} }
} }
impl<N: RealField> From<UnitQuaternion<N>> for Rotation3<N> { impl<N: SimdRealField> From<UnitQuaternion<N>> for Rotation3<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn from(q: UnitQuaternion<N>) -> Self { fn from(q: UnitQuaternion<N>) -> Self {
q.to_rotation_matrix() q.to_rotation_matrix()
} }
} }
impl<N: RealField> From<Rotation3<N>> for UnitQuaternion<N> { impl<N: SimdRealField> From<Rotation3<N>> for UnitQuaternion<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn from(q: Rotation3<N>) -> Self { fn from(q: Rotation3<N>) -> Self {
Self::from_rotation_matrix(&q) Self::from_rotation_matrix(&q)
} }
} }
impl<N: RealField> From<UnitQuaternion<N>> for Matrix3<N> { impl<N: SimdRealField> From<UnitQuaternion<N>> for Matrix3<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn from(q: UnitQuaternion<N>) -> Self { fn from(q: UnitQuaternion<N>) -> Self {
q.to_rotation_matrix().into_inner() q.to_rotation_matrix().into_inner()
} }
} }
impl<N: SimdRealField> From<Vector4<N>> for Quaternion<N> { impl<N: Scalar + SimdValue> From<Vector4<N>> for Quaternion<N> {
#[inline] #[inline]
fn from(coords: Vector4<N>) -> Self { fn from(coords: Vector4<N>) -> Self {
Self { coords } Self { coords }

23
src/geometry/quaternion_ops.rs
View File

@ -85,7 +85,8 @@ macro_rules! quaternion_op_impl(
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty $(=> $VDimA: ty, $VDimB: ty)*; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty $(=> $VDimA: ty, $VDimB: ty)*;
$action: expr; $($lives: tt),*) => { $action: expr; $($lives: tt),*) => {
impl<$($lives ,)* N: SimdRealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs impl<$($lives ,)* N: SimdRealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs
where DefaultAllocator: Allocator<N, $LhsRDim, $LhsCDim> + where N::Element: SimdRealField,
DefaultAllocator: Allocator<N, $LhsRDim, $LhsCDim> +
Allocator<N, $RhsRDim, $RhsCDim> { Allocator<N, $RhsRDim, $RhsCDim> {
type Output = $Result; type Output = $Result;
@ -489,7 +490,8 @@ Unit::new_unchecked(self * rhs.into_inner());
macro_rules! scalar_op_impl( macro_rules! scalar_op_impl(
($($Op: ident, $op: ident, $OpAssign: ident, $op_assign: ident);* $(;)*) => {$( ($($Op: ident, $op: ident, $OpAssign: ident, $op_assign: ident);* $(;)*) => {$(
impl<N: SimdRealField> $Op<N> for Quaternion<N> { impl<N: SimdRealField> $Op<N> for Quaternion<N>
where N::Element: SimdRealField {
type Output = Quaternion<N>; type Output = Quaternion<N>;
#[inline] #[inline]
@ -498,7 +500,8 @@ macro_rules! scalar_op_impl(
} }
} }
impl<'a, N: SimdRealField> $Op<N> for &'a Quaternion<N> { impl<'a, N: SimdRealField> $Op<N> for &'a Quaternion<N>
where N::Element: SimdRealField {
type Output = Quaternion<N>; type Output = Quaternion<N>;
#[inline] #[inline]
@ -507,7 +510,8 @@ macro_rules! scalar_op_impl(
} }
} }
impl<N: SimdRealField> $OpAssign<N> for Quaternion<N> { impl<N: SimdRealField> $OpAssign<N> for Quaternion<N>
where N::Element: SimdRealField {
#[inline] #[inline]
fn $op_assign(&mut self, n: N) { fn $op_assign(&mut self, n: N) {
@ -546,7 +550,9 @@ macro_rules! left_scalar_mul_impl(
left_scalar_mul_impl!(f32, f64); left_scalar_mul_impl!(f32, f64);
impl<N: SimdRealField> Neg for Quaternion<N> { impl<N: SimdRealField> Neg for Quaternion<N>
where N::Element: SimdRealField
{
type Output = Quaternion<N>; type Output = Quaternion<N>;
#[inline] #[inline]
@ -555,7 +561,9 @@ impl<N: SimdRealField> Neg for Quaternion<N> {
} }
} }
impl<'a, N: SimdRealField> Neg for &'a Quaternion<N> { impl<'a, N: SimdRealField> Neg for &'a Quaternion<N>
where N::Element: SimdRealField
{
type Output = Quaternion<N>; type Output = Quaternion<N>;
#[inline] #[inline]
@ -570,7 +578,8 @@ macro_rules! quaternion_op_impl(
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty $(=> $VDimA: ty, $VDimB: ty)*; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty $(=> $VDimA: ty, $VDimB: ty)*;
$action: expr; $($lives: tt),*) => { $action: expr; $($lives: tt),*) => {
impl<$($lives ,)* N: SimdRealField> $OpAssign<$Rhs> for $Lhs impl<$($lives ,)* N: SimdRealField> $OpAssign<$Rhs> for $Lhs
where DefaultAllocator: Allocator<N, $LhsRDim, $LhsCDim> + where N::Element: SimdRealField,
DefaultAllocator: Allocator<N, $LhsRDim, $LhsCDim> +
Allocator<N, $RhsRDim, $RhsCDim> { Allocator<N, $RhsRDim, $RhsCDim> {
#[inline] #[inline]

2
src/geometry/quaternion_simba.rs
View File

@ -4,7 +4,7 @@ use crate::base::Vector4;
use crate::geometry::Quaternion; use crate::geometry::Quaternion;
use crate::{RealField, Scalar}; use crate::{RealField, Scalar};
impl<N: RealField> SimdValue for Quaternion<N> impl<N: Scalar + SimdValue> SimdValue for Quaternion<N>
where N::Element: Scalar where N::Element: Scalar
{ {
type Element = Quaternion<N::Element>; type Element = Quaternion<N::Element>;

7
src/geometry/rotation.rs
View File

@ -15,6 +15,7 @@ use crate::base::storage::Owned;
use abomonation::Abomonation; use abomonation::Abomonation;
use simba::scalar::RealField; use simba::scalar::RealField;
use simba::simd::SimdRealField;
use crate::base::allocator::Allocator; use crate::base::allocator::Allocator;
use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1}; use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1};
@ -354,8 +355,10 @@ where DefaultAllocator: Allocator<N, D, D>
} }
} }
impl<N: RealField, D: DimName> Rotation<N, D> impl<N: SimdRealField, D: DimName> Rotation<N, D>
where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{ {
/// Rotate the given point. /// Rotate the given point.
/// ///

141
src/geometry/rotation_specialization.rs
View File

@ -7,6 +7,7 @@ use num::Zero;
use rand::distributions::{Distribution, OpenClosed01, Standard}; use rand::distributions::{Distribution, OpenClosed01, Standard};
use rand::Rng; use rand::Rng;
use simba::scalar::RealField; use simba::scalar::RealField;
use simba::simd::{SimdBool, SimdRealField};
use std::ops::Neg; use std::ops::Neg;
use crate::base::dimension::{U1, U2, U3}; use crate::base::dimension::{U1, U2, U3};
@ -20,7 +21,7 @@ use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion};
* 2D Rotation matrix. * 2D Rotation matrix.
* *
*/ */
impl<N: RealField> Rotation2<N> { impl<N: SimdRealField> Rotation2<N> {
/// Builds a 2 dimensional rotation matrix from an angle in radian. /// Builds a 2 dimensional rotation matrix from an angle in radian.
/// ///
/// # Example /// # Example
@ -34,7 +35,7 @@ impl<N: RealField> Rotation2<N> {
/// assert_relative_eq!(rot * Point2::new(3.0, 4.0), Point2::new(-4.0, 3.0)); /// assert_relative_eq!(rot * Point2::new(3.0, 4.0), Point2::new(-4.0, 3.0));
/// ``` /// ```
pub fn new(angle: N) -> Self { 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)) Self::from_matrix_unchecked(Matrix2::new(coa, -sia, sia, coa))
} }
@ -53,7 +54,8 @@ impl<N: RealField> Rotation2<N> {
/// This is an iterative method. See `.from_matrix_eps` to provide mover /// This is an iterative method. See `.from_matrix_eps` to provide mover
/// convergence parameters and starting solution. /// convergence parameters and starting solution.
/// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al. /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
pub fn from_matrix(m: &Matrix2<N>) -> Self { pub fn from_matrix(m: &Matrix2<N>) -> Self
where N: RealField {
Self::from_matrix_eps(m, N::default_epsilon(), 0, Self::identity()) Self::from_matrix_eps(m, N::default_epsilon(), 0, Self::identity())
} }
@ -69,7 +71,8 @@ impl<N: RealField> Rotation2<N> {
/// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close /// * `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 /// to the actual solution is provided. Can be set to `Rotation2::identity()` if no other
/// guesses come to mind. /// guesses come to mind.
pub fn from_matrix_eps(m: &Matrix2<N>, eps: N, mut max_iter: usize, guess: Self) -> Self { pub fn from_matrix_eps(m: &Matrix2<N>, eps: N, mut max_iter: usize, guess: Self) -> Self
where N: RealField {
if max_iter == 0 { if max_iter == 0 {
max_iter = usize::max_value(); max_iter = usize::max_value();
} }
@ -108,6 +111,7 @@ impl<N: RealField> Rotation2<N> {
#[inline] #[inline]
pub fn rotation_between<SB, SC>(a: &Vector<N, U2, SB>, b: &Vector<N, U2, SC>) -> Self pub fn rotation_between<SB, SC>(a: &Vector<N, U2, SB>, b: &Vector<N, U2, SC>) -> Self
where where
N: RealField,
SB: Storage<N, U2>, SB: Storage<N, U2>,
SC: Storage<N, U2>, SC: Storage<N, U2>,
{ {
@ -135,6 +139,7 @@ impl<N: RealField> Rotation2<N> {
s: N, s: N,
) -> Self ) -> Self
where where
N: RealField,
SB: Storage<N, U2>, SB: Storage<N, U2>,
SC: Storage<N, U2>, SC: Storage<N, U2>,
{ {
@ -152,7 +157,7 @@ impl<N: RealField> Rotation2<N> {
/// ``` /// ```
#[inline] #[inline]
pub fn angle(&self) -> N { 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. /// The rotation angle needed to make `self` and `other` coincide.
@ -193,7 +198,8 @@ impl<N: RealField> Rotation2<N> {
/// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated /// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated
/// computations might cause the matrix from progressively not being orthonormal anymore. /// computations might cause the matrix from progressively not being orthonormal anymore.
#[inline] #[inline]
pub fn renormalize(&mut self) { pub fn renormalize(&mut self)
where N: RealField {
let mut c = UnitComplex::from(*self); let mut c = UnitComplex::from(*self);
let _ = c.renormalize(); let _ = c.renormalize();
@ -226,19 +232,23 @@ impl<N: RealField> Rotation2<N> {
} }
} }
impl<N: RealField> Distribution<Rotation2<N>> for Standard impl<N: SimdRealField> Distribution<Rotation2<N>> for Standard
where OpenClosed01: Distribution<N> where
N::Element: SimdRealField,
OpenClosed01: Distribution<N>,
{ {
/// Generate a uniformly distributed random rotation. /// Generate a uniformly distributed random rotation.
#[inline] #[inline]
fn sample<'a, R: Rng + ?Sized>(&self, rng: &'a mut R) -> Rotation2<N> { fn sample<'a, R: Rng + ?Sized>(&self, rng: &'a mut R) -> Rotation2<N> {
Rotation2::new(rng.sample(OpenClosed01) * N::two_pi()) Rotation2::new(rng.sample(OpenClosed01) * N::simd_two_pi())
} }
} }
#[cfg(feature = "arbitrary")] #[cfg(feature = "arbitrary")]
impl<N: RealField + Arbitrary> Arbitrary for Rotation2<N> impl<N: SimdRealField + Arbitrary> Arbitrary for Rotation2<N>
where Owned<N, U2, U2>: Send where
N::Element: SimdRealField,
Owned<N, U2, U2>: Send,
{ {
#[inline] #[inline]
fn arbitrary<G: Gen>(g: &mut G) -> Self { fn arbitrary<G: Gen>(g: &mut G) -> Self {
@ -251,7 +261,9 @@ where Owned<N, U2, U2>: Send
* 3D Rotation matrix. * 3D Rotation matrix.
* *
*/ */
impl<N: RealField> Rotation3<N> { impl<N: SimdRealField> Rotation3<N>
where N::Element: SimdRealField
{
/// Builds a 3 dimensional rotation matrix from an axis and an angle. /// Builds a 3 dimensional rotation matrix from an axis and an angle.
/// ///
/// # Arguments /// # Arguments
@ -286,7 +298,8 @@ impl<N: RealField> Rotation3<N> {
/// This is an iterative method. See `.from_matrix_eps` to provide mover /// This is an iterative method. See `.from_matrix_eps` to provide mover
/// convergence parameters and starting solution. /// convergence parameters and starting solution.
/// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al. /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
pub fn from_matrix(m: &Matrix3<N>) -> Self { pub fn from_matrix(m: &Matrix3<N>) -> Self
where N: RealField {
Self::from_matrix_eps(m, N::default_epsilon(), 0, Self::identity()) Self::from_matrix_eps(m, N::default_epsilon(), 0, Self::identity())
} }
@ -302,7 +315,8 @@ impl<N: RealField> Rotation3<N> {
/// * `guess`: a guess of the solution. Convergence will be significantly faster if an initial solution close /// * `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 /// to the actual solution is provided. Can be set to `Rotation3::identity()` if no other
/// guesses come to mind. /// guesses come to mind.
pub fn from_matrix_eps(m: &Matrix3<N>, eps: N, mut max_iter: usize, guess: Self) -> Self { pub fn from_matrix_eps(m: &Matrix3<N>, eps: N, mut max_iter: usize, guess: Self) -> Self
where N: RealField {
if max_iter == 0 { if max_iter == 0 {
max_iter = usize::max_value(); max_iter = usize::max_value();
} }
@ -378,30 +392,31 @@ impl<N: RealField> Rotation3<N> {
/// ``` /// ```
pub fn from_axis_angle<SB>(axis: &Unit<Vector<N, U3, SB>>, angle: N) -> Self pub fn from_axis_angle<SB>(axis: &Unit<Vector<N, U3, SB>>, angle: N) -> Self
where SB: Storage<N, U3> { where SB: Storage<N, U3> {
if angle.is_zero() { angle.simd_ne(N::zero()).if_else(
Self::identity() || {
} else { let ux = axis.as_ref()[0];
let ux = axis.as_ref()[0]; let uy = axis.as_ref()[1];
let uy = axis.as_ref()[1]; let uz = axis.as_ref()[2];
let uz = axis.as_ref()[2]; let sqx = ux * ux;
let sqx = ux * ux; let sqy = uy * uy;
let sqy = uy * uy; let sqz = uz * uz;
let sqz = uz * uz; let (sin, cos) = angle.simd_sin_cos();
let (sin, cos) = angle.sin_cos(); let one_m_cos = N::one() - cos;
let one_m_cos = N::one() - cos;
Self::from_matrix_unchecked(MatrixN::<N, U3>::new( Self::from_matrix_unchecked(MatrixN::<N, U3>::new(
sqx + (N::one() - sqx) * cos, sqx + (N::one() - sqx) * cos,
ux * uy * one_m_cos - uz * sin, ux * uy * one_m_cos - uz * sin,
ux * uz * one_m_cos + uy * sin, ux * uz * one_m_cos + uy * sin,
ux * uy * one_m_cos + uz * sin, ux * uy * one_m_cos + uz * sin,
sqy + (N::one() - sqy) * cos, sqy + (N::one() - sqy) * cos,
uy * uz * one_m_cos - ux * sin, uy * uz * one_m_cos - ux * sin,
ux * uz * one_m_cos - uy * sin, ux * uz * one_m_cos - uy * sin,
uy * uz * one_m_cos + ux * sin, uy * uz * one_m_cos + ux * sin,
sqz + (N::one() - sqz) * cos, sqz + (N::one() - sqz) * cos,
)) ))
} },
|| Self::identity(),
)
} }
/// Creates a new rotation from Euler angles. /// Creates a new rotation from Euler angles.
@ -419,9 +434,9 @@ impl<N: RealField> Rotation3<N> {
/// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6); /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6);
/// ``` /// ```
pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self { pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self {
let (sr, cr) = roll.sin_cos(); let (sr, cr) = roll.simd_sin_cos();
let (sp, cp) = pitch.sin_cos(); let (sp, cp) = pitch.simd_sin_cos();
let (sy, cy) = yaw.sin_cos(); let (sy, cy) = yaw.simd_sin_cos();
Self::from_matrix_unchecked(MatrixN::<N, U3>::new( Self::from_matrix_unchecked(MatrixN::<N, U3>::new(
cy * cp, cy * cp,
@ -440,7 +455,8 @@ impl<N: RealField> Rotation3<N> {
/// ///
/// The angles are produced in the form (roll, pitch, yaw). /// The angles are produced in the form (roll, pitch, yaw).
#[deprecated(note = "This is renamed to use `.euler_angles()`.")] #[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() self.euler_angles()
} }
@ -458,7 +474,8 @@ impl<N: RealField> Rotation3<N> {
/// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6); /// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6);
/// assert_relative_eq!(euler.2, 0.3, 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 // 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 // https://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.371.6578
if self[(2, 0)].abs() < N::one() { if self[(2, 0)].abs() < N::one() {
@ -480,7 +497,8 @@ impl<N: RealField> Rotation3<N> {
/// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated /// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated
/// computations might cause the matrix from progressively not being orthonormal anymore. /// computations might cause the matrix from progressively not being orthonormal anymore.
#[inline] #[inline]
pub fn renormalize(&mut self) { pub fn renormalize(&mut self)
where N: RealField {
let mut c = UnitQuaternion::from(*self); let mut c = UnitQuaternion::from(*self);
let _ = c.renormalize(); let _ = c.renormalize();
@ -612,6 +630,7 @@ impl<N: RealField> Rotation3<N> {
#[inline] #[inline]
pub fn rotation_between<SB, SC>(a: &Vector<N, U3, SB>, b: &Vector<N, U3, SC>) -> Option<Self> pub fn rotation_between<SB, SC>(a: &Vector<N, U3, SB>, b: &Vector<N, U3, SC>) -> Option<Self>
where where
N: RealField,
SB: Storage<N, U3>, SB: Storage<N, U3>,
SC: Storage<N, U3>, SC: Storage<N, U3>,
{ {
@ -639,6 +658,7 @@ impl<N: RealField> Rotation3<N> {
n: N, n: N,
) -> Option<Self> ) -> Option<Self>
where where
N: RealField,
SB: Storage<N, U3>, SB: Storage<N, U3>,
SC: Storage<N, U3>, SC: Storage<N, U3>,
{ {
@ -677,7 +697,7 @@ impl<N: RealField> Rotation3<N> {
pub fn angle(&self) -> N { pub fn angle(&self) -> N {
((self.matrix()[(0, 0)] + self.matrix()[(1, 1)] + self.matrix()[(2, 2)] - N::one()) ((self.matrix()[(0, 0)] + self.matrix()[(1, 1)] + self.matrix()[(2, 2)] - N::one())
/ crate::convert(2.0)) / crate::convert(2.0))
.acos() .simd_acos()
} }
/// The rotation axis. Returns `None` if the rotation angle is zero or PI. /// The rotation axis. Returns `None` if the rotation angle is zero or PI.
@ -696,7 +716,8 @@ impl<N: RealField> Rotation3<N> {
/// assert!(rot.axis().is_none()); /// assert!(rot.axis().is_none());
/// ``` /// ```
#[inline] #[inline]
pub fn axis(&self) -> Option<Unit<Vector3<N>>> { pub fn axis(&self) -> Option<Unit<Vector3<N>>>
where N: RealField {
let axis = VectorN::<N, U3>::new( let axis = VectorN::<N, U3>::new(
self.matrix()[(2, 1)] - self.matrix()[(1, 2)], self.matrix()[(2, 1)] - self.matrix()[(1, 2)],
self.matrix()[(0, 2)] - self.matrix()[(2, 0)], self.matrix()[(0, 2)] - self.matrix()[(2, 0)],
@ -717,7 +738,8 @@ impl<N: RealField> Rotation3<N> {
/// assert_relative_eq!(rot.scaled_axis(), axisangle, epsilon = 1.0e-6); /// assert_relative_eq!(rot.scaled_axis(), axisangle, epsilon = 1.0e-6);
/// ``` /// ```
#[inline] #[inline]
pub fn scaled_axis(&self) -> Vector3<N> { pub fn scaled_axis(&self) -> Vector3<N>
where N: RealField {
if let Some(axis) = self.axis() { if let Some(axis) = self.axis() {
axis.into_inner() * self.angle() axis.into_inner() * self.angle()
} else { } else {
@ -745,7 +767,8 @@ impl<N: RealField> Rotation3<N> {
/// assert!(rot.axis_angle().is_none()); /// assert!(rot.axis_angle().is_none());
/// ``` /// ```
#[inline] #[inline]
pub fn axis_angle(&self) -> Option<(Unit<Vector3<N>>, N)> { pub fn axis_angle(&self) -> Option<(Unit<Vector3<N>>, N)>
where N: RealField {
if let Some(axis) = self.axis() { if let Some(axis) = self.axis() {
Some((axis, self.angle())) Some((axis, self.angle()))
} else { } else {
@ -801,7 +824,8 @@ impl<N: RealField> Rotation3<N> {
/// assert_eq!(pow.angle(), 2.4); /// assert_eq!(pow.angle(), 2.4);
/// ``` /// ```
#[inline] #[inline]
pub fn powf(&self, n: N) -> Self { pub fn powf(&self, n: N) -> Self
where N: RealField {
if let Some(axis) = self.axis() { if let Some(axis) = self.axis() {
Self::from_axis_angle(&axis, self.angle() * n) Self::from_axis_angle(&axis, self.angle() * n)
} else if self.matrix()[(0, 0)] < N::zero() { } else if self.matrix()[(0, 0)] < N::zero() {
@ -813,8 +837,10 @@ impl<N: RealField> Rotation3<N> {
} }
} }
impl<N: RealField> Distribution<Rotation3<N>> for Standard impl<N: SimdRealField> Distribution<Rotation3<N>> for Standard
where OpenClosed01: Distribution<N> where
N::Element: SimdRealField,
OpenClosed01: Distribution<N>,
{ {
/// Generate a uniformly distributed random rotation. /// Generate a uniformly distributed random rotation.
#[inline] #[inline]
@ -824,8 +850,8 @@ where OpenClosed01: Distribution<N>
// In D. Kirk, editor, Graphics Gems III, pages 117-120. Academic, New York, 1992. // In D. Kirk, editor, Graphics Gems III, pages 117-120. Academic, New York, 1992.
// Compute a random rotation around Z // Compute a random rotation around Z
let theta = N::two_pi() * rng.sample(OpenClosed01); let theta = N::simd_two_pi() * rng.sample(OpenClosed01);
let (ts, tc) = theta.sin_cos(); let (ts, tc) = theta.simd_sin_cos();
let a = MatrixN::<N, U3>::new( let a = MatrixN::<N, U3>::new(
tc, tc,
ts, ts,
@ -839,11 +865,11 @@ where OpenClosed01: Distribution<N>
); );
// Compute a random rotation *of* Z // 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 z = rng.sample(OpenClosed01);
let (ps, pc) = phi.sin_cos(); let (ps, pc) = phi.simd_sin_cos();
let sqrt_z = z.sqrt(); let sqrt_z = z.simd_sqrt();
let v = Vector3::new(pc * sqrt_z, ps * sqrt_z, (N::one() - z).sqrt()); let v = Vector3::new(pc * sqrt_z, ps * sqrt_z, (N::one() - z).simd_sqrt());
let mut b = v * v.transpose(); let mut b = v * v.transpose();
b += b; b += b;
b -= MatrixN::<N, U3>::identity(); b -= MatrixN::<N, U3>::identity();
@ -853,8 +879,9 @@ where OpenClosed01: Distribution<N>
} }
#[cfg(feature = "arbitrary")] #[cfg(feature = "arbitrary")]
impl<N: RealField + Arbitrary> Arbitrary for Rotation3<N> impl<N: SimdRealField + Arbitrary> Arbitrary for Rotation3<N>
where where
N::Element: SimdRealField,
Owned<N, U3, U3>: Send, Owned<N, U3, U3>: Send,
Owned<N, U3>: Send, Owned<N, U3>: Send,
{ {

41
src/geometry/similarity.rs
View File

@ -11,6 +11,7 @@ use serde::{Deserialize, Serialize};
use abomonation::Abomonation; use abomonation::Abomonation;
use simba::scalar::{RealField, SubsetOf}; use simba::scalar::{RealField, SubsetOf};
use simba::simd::SimdRealField;
use crate::base::allocator::Allocator; use crate::base::allocator::Allocator;
use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1}; use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1};
@ -36,7 +37,7 @@ use crate::geometry::{AbstractRotation, Isometry, Point, Translation};
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
Owned<N, D>: Deserialize<'de>")) Owned<N, D>: Deserialize<'de>"))
)] )]
pub struct Similarity<N: RealField, D: DimName, R> pub struct Similarity<N: SimdRealField, D: DimName, R>
where DefaultAllocator: Allocator<N, D> where DefaultAllocator: Allocator<N, D>
{ {
/// The part of this similarity that does not include the scaling factor. /// The part of this similarity that does not include the scaling factor.
@ -45,7 +46,7 @@ where DefaultAllocator: Allocator<N, D>
} }
#[cfg(feature = "abomonation-serialize")] #[cfg(feature = "abomonation-serialize")]
impl<N: RealField, D: DimName, R> Abomonation for Similarity<N, D, R> impl<N: SimdRealField, D: DimName, R> Abomonation for Similarity<N, D, R>
where where
Isometry<N, D, R>: Abomonation, Isometry<N, D, R>: Abomonation,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
@ -63,7 +64,7 @@ where
} }
} }
impl<N: RealField + hash::Hash, D: DimName + hash::Hash, R: hash::Hash> hash::Hash impl<N: SimdRealField + hash::Hash, D: DimName + hash::Hash, R: hash::Hash> hash::Hash
for Similarity<N, D, R> for Similarity<N, D, R>
where where
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
@ -75,15 +76,19 @@ where
} }
} }
impl<N: RealField, D: DimName + Copy, R: AbstractRotation<N, D> + Copy> Copy for Similarity<N, D, R> impl<N: SimdRealField, D: DimName + Copy, R: AbstractRotation<N, D> + Copy> Copy
for Similarity<N, D, R>
where where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
Owned<N, D>: Copy, Owned<N, D>: Copy,
{ {
} }
impl<N: RealField, D: DimName, R: AbstractRotation<N, D> + Clone> Clone for Similarity<N, D, R> impl<N: SimdRealField, D: DimName, R: AbstractRotation<N, D> + Clone> Clone for Similarity<N, D, R>
where DefaultAllocator: Allocator<N, D> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, D>,
{ {
#[inline] #[inline]
fn clone(&self) -> Self { fn clone(&self) -> Self {
@ -91,8 +96,9 @@ where DefaultAllocator: Allocator<N, D>
} }
} }
impl<N: RealField, D: DimName, R> Similarity<N, D, R> impl<N: SimdRealField, D: DimName, R> Similarity<N, D, R>
where where
N::Element: SimdRealField,
R: AbstractRotation<N, D>, R: AbstractRotation<N, D>,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
{ {
@ -105,10 +111,7 @@ where
/// Creates a new similarity from its rotational and translational parts. /// Creates a new similarity from its rotational and translational parts.
#[inline] #[inline]
pub fn from_isometry(isometry: Isometry<N, D, R>, scaling: N) -> Self { pub fn from_isometry(isometry: Isometry<N, D, R>, scaling: N) -> Self {
assert!( assert!(!scaling.is_zero(), "The scaling factor must not be zero.");
!relative_eq!(scaling, N::zero()),
"The scaling factor must not be zero."
);
Self { isometry, scaling } Self { isometry, scaling }
} }
@ -140,7 +143,7 @@ where
#[inline] #[inline]
pub fn set_scaling(&mut self, scaling: N) { pub fn set_scaling(&mut self, scaling: N) {
assert!( assert!(
!relative_eq!(scaling, N::zero()), !scaling.is_zero(),
"The similarity scaling factor must not be zero." "The similarity scaling factor must not be zero."
); );
@ -158,7 +161,7 @@ where
#[must_use = "Did you mean to use prepend_scaling_mut()?"] #[must_use = "Did you mean to use prepend_scaling_mut()?"]
pub fn prepend_scaling(&self, scaling: N) -> Self { pub fn prepend_scaling(&self, scaling: N) -> Self {
assert!( assert!(
!relative_eq!(scaling, N::zero()), !scaling.is_zero(),
"The similarity scaling factor must not be zero." "The similarity scaling factor must not be zero."
); );
@ -170,7 +173,7 @@ where
#[must_use = "Did you mean to use append_scaling_mut()?"] #[must_use = "Did you mean to use append_scaling_mut()?"]
pub fn append_scaling(&self, scaling: N) -> Self { pub fn append_scaling(&self, scaling: N) -> Self {
assert!( assert!(
!relative_eq!(scaling, N::zero()), !scaling.is_zero(),
"The similarity scaling factor must not be zero." "The similarity scaling factor must not be zero."
); );
@ -185,7 +188,7 @@ where
#[inline] #[inline]
pub fn prepend_scaling_mut(&mut self, scaling: N) { pub fn prepend_scaling_mut(&mut self, scaling: N) {
assert!( assert!(
!relative_eq!(scaling, N::zero()), !scaling.is_zero(),
"The similarity scaling factor must not be zero." "The similarity scaling factor must not be zero."
); );
@ -196,7 +199,7 @@ where
#[inline] #[inline]
pub fn append_scaling_mut(&mut self, scaling: N) { pub fn append_scaling_mut(&mut self, scaling: N) {
assert!( assert!(
!relative_eq!(scaling, N::zero()), !scaling.is_zero(),
"The similarity scaling factor must not be 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. // 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 // 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). // explicit struct construction is prevented by the private scaling factor).
impl<N: RealField, D: DimName, R> Similarity<N, D, R> impl<N: SimdRealField, D: DimName, R> Similarity<N, D, R>
where DefaultAllocator: Allocator<N, D> where DefaultAllocator: Allocator<N, D>
{ {
/// Converts this similarity into its equivalent homogeneous transformation matrix. /// Converts this similarity into its equivalent homogeneous transformation matrix.
@ -337,14 +340,14 @@ where DefaultAllocator: Allocator<N, D>
} }
} }
impl<N: RealField, D: DimName, R> Eq for Similarity<N, D, R> impl<N: SimdRealField, D: DimName, R> Eq for Similarity<N, D, R>
where where
R: AbstractRotation<N, D> + Eq, R: AbstractRotation<N, D> + Eq,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
{ {
} }
impl<N: RealField, D: DimName, R> PartialEq for Similarity<N, D, R> impl<N: SimdRealField, D: DimName, R> PartialEq for Similarity<N, D, R>
where where
R: AbstractRotation<N, D> + PartialEq, R: AbstractRotation<N, D> + PartialEq,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,

31
src/geometry/similarity_construction.rs
View File

@ -8,6 +8,7 @@ use rand::distributions::{Distribution, Standard};
use rand::Rng; use rand::Rng;
use simba::scalar::RealField; use simba::scalar::RealField;
use simba::simd::SimdRealField;
use crate::base::allocator::Allocator; use crate::base::allocator::Allocator;
use crate::base::dimension::{DimName, U2, U3}; use crate::base::dimension::{DimName, U2, U3};
@ -18,8 +19,9 @@ use crate::geometry::{
UnitComplex, UnitQuaternion, UnitComplex, UnitQuaternion,
}; };
impl<N: RealField, D: DimName, R> Similarity<N, D, R> impl<N: SimdRealField, D: DimName, R> Similarity<N, D, R>
where where
N::Element: SimdRealField,
R: AbstractRotation<N, D>, R: AbstractRotation<N, D>,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
{ {
@ -44,8 +46,9 @@ where
} }
} }
impl<N: RealField, D: DimName, R> One for Similarity<N, D, R> impl<N: SimdRealField, D: DimName, R> One for Similarity<N, D, R>
where where
N::Element: SimdRealField,
R: AbstractRotation<N, D>, R: AbstractRotation<N, D>,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
{ {
@ -73,8 +76,9 @@ where
} }
} }
impl<N: RealField, D: DimName, R> Similarity<N, D, R> impl<N: SimdRealField, D: DimName, R> Similarity<N, D, R>
where where
N::Element: SimdRealField,
R: AbstractRotation<N, D>, R: AbstractRotation<N, D>,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
{ {
@ -103,7 +107,8 @@ where
#[cfg(feature = "arbitrary")] #[cfg(feature = "arbitrary")]
impl<N, D: DimName, R> Arbitrary for Similarity<N, D, R> impl<N, D: DimName, R> Arbitrary for Similarity<N, D, R>
where where
N: RealField + Arbitrary + Send, N: SimdRealField + Arbitrary + Send,
N::Element: SimdRealField,
R: AbstractRotation<N, D> + Arbitrary + Send, R: AbstractRotation<N, D> + Arbitrary + Send,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
Owned<N, D>: Send, Owned<N, D>: Send,
@ -111,7 +116,7 @@ where
#[inline] #[inline]
fn arbitrary<G: Gen>(rng: &mut G) -> Self { fn arbitrary<G: Gen>(rng: &mut G) -> Self {
let mut s = Arbitrary::arbitrary(rng); let mut s = Arbitrary::arbitrary(rng);
while relative_eq!(s, N::zero()) { while s.is_zero() {
s = Arbitrary::arbitrary(rng) s = Arbitrary::arbitrary(rng)
} }
@ -125,8 +130,10 @@ where
* *
*/ */
// 2D rotation. // 2D similarity.
impl<N: RealField> Similarity<N, U2, Rotation2<N>> { impl<N: SimdRealField> Similarity<N, U2, Rotation2<N>>
where N::Element: SimdRealField
{
/// Creates a new similarity from a translation, a rotation, and an uniform scaling factor. /// Creates a new similarity from a translation, a rotation, and an uniform scaling factor.
/// ///
/// # Example /// # Example
@ -149,7 +156,9 @@ impl<N: RealField> Similarity<N, U2, Rotation2<N>> {
} }
} }
impl<N: RealField> Similarity<N, U2, UnitComplex<N>> { impl<N: SimdRealField> Similarity<N, U2, UnitComplex<N>>
where N::Element: SimdRealField
{
/// Creates a new similarity from a translation and a rotation angle. /// Creates a new similarity from a translation and a rotation angle.
/// ///
/// # Example /// # Example
@ -175,7 +184,8 @@ impl<N: RealField> Similarity<N, U2, UnitComplex<N>> {
// 3D rotation. // 3D rotation.
macro_rules! similarity_construction_impl( macro_rules! similarity_construction_impl(
($Rot: ty) => { ($Rot: ty) => {
impl<N: RealField> Similarity<N, U3, $Rot> { impl<N: SimdRealField> Similarity<N, U3, $Rot>
where N::Element: SimdRealField {
/// Creates a new similarity from a translation, rotation axis-angle, and scaling /// Creates a new similarity from a translation, rotation axis-angle, and scaling
/// factor. /// 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); /// assert_relative_eq!(sim * vec, Vector3::new(18.0, 15.0, -12.0), epsilon = 1.0e-5);
/// ``` /// ```
#[inline] #[inline]
pub fn new(translation: Vector3<N>, axisangle: Vector3<N>, scaling: N) -> Self { pub fn new(translation: Vector3<N>, axisangle: Vector3<N>, scaling: N) -> Self
{
Self::from_isometry(Isometry::<_, U3, $Rot>::new(translation, axisangle), scaling) Self::from_isometry(Isometry::<_, U3, $Rot>::new(translation, axisangle), scaling)
} }

3
src/geometry/similarity_conversion.rs
View File

@ -1,4 +1,5 @@
use simba::scalar::{RealField, SubsetOf, SupersetOf}; use simba::scalar::{RealField, SubsetOf, SupersetOf};
use simba::simd::SimdRealField;
use crate::base::allocator::Allocator; use crate::base::allocator::Allocator;
use crate::base::dimension::{DimMin, DimName, DimNameAdd, DimNameSum, U1}; use crate::base::dimension::{DimMin, DimName, DimNameAdd, DimNameSum, U1};
@ -168,7 +169,7 @@ where
} }
} }
impl<N: RealField, D: DimName, R> From<Similarity<N, D, R>> for MatrixN<N, DimNameSum<D, U1>> impl<N: SimdRealField, D: DimName, R> From<Similarity<N, D, R>> for MatrixN<N, DimNameSum<D, U1>>
where where
D: DimNameAdd<U1>, D: DimNameAdd<U1>,
R: SubsetOf<MatrixN<N, DimNameSum<D, U1>>>, R: SubsetOf<MatrixN<N, DimNameSum<D, U1>>>,

32
src/geometry/similarity_ops.rs
View File

@ -1,7 +1,8 @@
use num::{One, Zero}; use num::{One, Zero};
use std::ops::{Div, DivAssign, Mul, MulAssign}; 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::allocator::Allocator;
use crate::base::dimension::{DimName, U1, U3, U4}; use crate::base::dimension::{DimName, U1, U3, U4};
@ -68,8 +69,9 @@ macro_rules! similarity_binop_impl(
($Op: ident, $op: ident; ($Op: ident, $op: ident;
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
$action: expr; $($lives: tt),*) => { $action: expr; $($lives: tt),*) => {
impl<$($lives ,)* N: RealField, D: DimName, R> $Op<$Rhs> for $Lhs impl<$($lives ,)* N: SimdRealField, D: DimName, R> $Op<$Rhs> for $Lhs
where R: AbstractRotation<N, D>, where N::Element: SimdRealField,
R: AbstractRotation<N, D>,
DefaultAllocator: Allocator<N, D> { DefaultAllocator: Allocator<N, D> {
type Output = $Output; type Output = $Output;
@ -115,8 +117,9 @@ macro_rules! similarity_binop_assign_impl_all(
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty;
[val] => $action_val: expr; [val] => $action_val: expr;
[ref] => $action_ref: expr;) => { [ref] => $action_ref: expr;) => {
impl<N: RealField, D: DimName, R> $OpAssign<$Rhs> for $Lhs impl<N: SimdRealField, D: DimName, R> $OpAssign<$Rhs> for $Lhs
where R: AbstractRotation<N, D>, where N::Element: SimdRealField,
R: AbstractRotation<N, D>,
DefaultAllocator: Allocator<N, D> { DefaultAllocator: Allocator<N, D> {
#[inline] #[inline]
fn $op_assign(&mut $lhs, $rhs: $Rhs) { 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 impl<'b, N: SimdRealField, D: DimName, R> $OpAssign<&'b $Rhs> for $Lhs
where R: AbstractRotation<N, D>, where N::Element: SimdRealField,
R: AbstractRotation<N, D>,
DefaultAllocator: Allocator<N, D> { DefaultAllocator: Allocator<N, D> {
#[inline] #[inline]
fn $op_assign(&mut $lhs, $rhs: &'b $Rhs) { fn $op_assign(&mut $lhs, $rhs: &'b $Rhs) {
@ -161,6 +165,7 @@ similarity_binop_impl_all!(
// Similarity ×= Translation // Similarity ×= Translation
similarity_binop_assign_impl_all!( similarity_binop_assign_impl_all!(
MulAssign, mul_assign; MulAssign, mul_assign;
self: Similarity<N, D, R>, rhs: Translation<N, D>; self: Similarity<N, D, R>, rhs: Translation<N, D>;
[val] => *self *= &rhs; [val] => *self *= &rhs;
@ -214,7 +219,7 @@ similarity_binop_assign_impl_all!(
// Similarity ×= R // Similarity ×= R
// Similarity ÷= R // Similarity ÷= R
md_assign_impl_all!( 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; (D, U1), (D, D) for D: DimName;
self: Similarity<N, D, Rotation<N, D>>, rhs: Rotation<N, D>; self: Similarity<N, D, Rotation<N, D>>, rhs: Rotation<N, D>;
[val] => self.isometry.rotation *= rhs; [val] => self.isometry.rotation *= rhs;
@ -222,7 +227,7 @@ md_assign_impl_all!(
); );
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; (D, U1), (D, D) for D: DimName;
self: Similarity<N, D, Rotation<N, D>>, rhs: Rotation<N, D>; self: Similarity<N, D, Rotation<N, D>>, rhs: Rotation<N, D>;
// FIXME: don't invert explicitly? // FIXME: don't invert explicitly?
@ -231,7 +236,7 @@ md_assign_impl_all!(
); );
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; (U3, U3), (U3, U3) for;
self: Similarity<N, U3, UnitQuaternion<N>>, rhs: UnitQuaternion<N>; self: Similarity<N, U3, UnitQuaternion<N>>, rhs: UnitQuaternion<N>;
[val] => self.isometry.rotation *= rhs; [val] => self.isometry.rotation *= rhs;
@ -239,7 +244,7 @@ md_assign_impl_all!(
); );
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; (U3, U3), (U3, U3) for;
self: Similarity<N, U3, UnitQuaternion<N>>, rhs: UnitQuaternion<N>; self: Similarity<N, U3, UnitQuaternion<N>>, rhs: UnitQuaternion<N>;
// FIXME: don't invert explicitly? // 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),*; ($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(for $Dims: ident: $DimsBound: ident),*;
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
$action: expr; $($lives: tt),*) => { $action: expr; $($lives: tt),*) => {
impl<$($lives ,)* N: RealField $(, $Dims: $DimsBound)*> $Op<$Rhs> for $Lhs impl<$($lives ,)* N: SimdRealField $(, $Dims: $DimsBound)*> $Op<$Rhs> for $Lhs
where DefaultAllocator: Allocator<N, $R1, $C1> + where N::Element: SimdRealField,
DefaultAllocator: Allocator<N, $R1, $C1> +
Allocator<N, $R2, $C2> { Allocator<N, $R2, $C2> {
type Output = $Output; type Output = $Output;

11
src/geometry/similarity_simba.rs
View File

@ -1,18 +1,17 @@
use simba::simd::SimdValue; use simba::simd::{SimdRealField, SimdValue};
use crate::base::allocator::Allocator; use crate::base::allocator::Allocator;
use crate::base::dimension::DimName; use crate::base::dimension::DimName;
use crate::base::{DefaultAllocator, Scalar}; use crate::base::{DefaultAllocator, Scalar};
use crate::RealField;
use crate::geometry::{AbstractRotation, Isometry, Similarity}; use crate::geometry::{AbstractRotation, Isometry, Similarity};
impl<N: RealField, D: DimName, R> SimdValue for Similarity<N, D, R> impl<N: SimdRealField, D: DimName, R> SimdValue for Similarity<N, D, R>
where where
N::Element: Scalar, N::Element: SimdRealField,
R: SimdValue<SimdBool = N::SimdBool> + AbstractRotation<N, D>, R: SimdValue<SimdBool = N::SimdBool> + AbstractRotation<N, D>,
R::Element: AbstractRotation<N, D>, R::Element: AbstractRotation<N::Element, D>,
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D> + Allocator<N::Element, D>,
{ {
type Element = Similarity<N::Element, D, R::Element>; type Element = Similarity<N::Element, D, R::Element>;
type SimdBool = N::SimdBool; type SimdBool = N::SimdBool;

9
src/geometry/unit_complex.rs
View File

@ -40,7 +40,9 @@ impl<N: SimdRealField> Normed for Complex<N> {
} }
} }
impl<N: RealField> UnitComplex<N> { impl<N: SimdRealField> UnitComplex<N>
where N::Element: SimdRealField
{
/// The rotation angle in `]-pi; pi]` of this unit complex number. /// The rotation angle in `]-pi; pi]` of this unit complex number.
/// ///
/// # Example /// # Example
@ -51,7 +53,7 @@ impl<N: RealField> UnitComplex<N> {
/// ``` /// ```
#[inline] #[inline]
pub fn angle(&self) -> N { pub fn angle(&self) -> N {
self.im.atan2(self.re) self.im.simd_atan2(self.re)
} }
/// The sine of the rotation angle. /// The sine of the rotation angle.
@ -97,7 +99,8 @@ impl<N: RealField> UnitComplex<N> {
/// the `.angle()` method instead is more common. /// the `.angle()` method instead is more common.
/// Returns `None` if the angle is zero. /// Returns `None` if the angle is zero.
#[inline] #[inline]
pub fn axis_angle(&self) -> Option<(Unit<Vector1<N>>, N)> { pub fn axis_angle(&self) -> Option<(Unit<Vector1<N>>, N)>
where N: RealField {
let ang = self.angle(); let ang = self.angle();
if ang.is_zero() { if ang.is_zero() {

35
src/geometry/unit_complex_construction.rs
View File

@ -11,8 +11,11 @@ use crate::base::storage::Storage;
use crate::base::{Matrix2, Unit, Vector}; use crate::base::{Matrix2, Unit, Vector};
use crate::geometry::{Rotation2, UnitComplex}; use crate::geometry::{Rotation2, UnitComplex};
use simba::scalar::RealField; use simba::scalar::RealField;
use simba::simd::SimdRealField;
impl<N: RealField> UnitComplex<N> { impl<N: SimdRealField> UnitComplex<N>
where N::Element: SimdRealField
{
/// The unit complex number multiplicative identity. /// The unit complex number multiplicative identity.
/// ///
/// # Example /// # Example
@ -43,7 +46,7 @@ impl<N: RealField> UnitComplex<N> {
/// ``` /// ```
#[inline] #[inline]
pub fn new(angle: N) -> Self { 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) Self::from_cos_sin_unchecked(cos, sin)
} }
@ -110,7 +113,7 @@ impl<N: RealField> UnitComplex<N> {
/// The input complex number will be normalized. Returns the norm of the complex number as well. /// The input complex number will be normalized. Returns the norm of the complex number as well.
#[inline] #[inline]
pub fn from_complex_and_get(q: Complex<N>) -> (Self, N) { pub fn from_complex_and_get(q: Complex<N>) -> (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) (Self::new_unchecked(q / norm), norm)
} }
@ -134,7 +137,8 @@ impl<N: RealField> UnitComplex<N> {
/// This is an iterative method. See `.from_matrix_eps` to provide mover /// This is an iterative method. See `.from_matrix_eps` to provide mover
/// convergence parameters and starting solution. /// convergence parameters and starting solution.
/// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al. /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
pub fn from_matrix(m: &Matrix2<N>) -> Self { pub fn from_matrix(m: &Matrix2<N>) -> Self
where N: RealField {
Rotation2::from_matrix(m).into() Rotation2::from_matrix(m).into()
} }
@ -150,7 +154,8 @@ impl<N: RealField> UnitComplex<N> {
/// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close /// * `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 /// to the actual solution is provided. Can be set to `UnitQuaternion::identity()` if no other
/// guesses come to mind. /// guesses come to mind.
pub fn from_matrix_eps(m: &Matrix2<N>, eps: N, max_iter: usize, guess: Self) -> Self { pub fn from_matrix_eps(m: &Matrix2<N>, eps: N, max_iter: usize, guess: Self) -> Self
where N: RealField {
let guess = Rotation2::from(guess); let guess = Rotation2::from(guess);
Rotation2::from_matrix_eps(m, eps, max_iter, guess).into() Rotation2::from_matrix_eps(m, eps, max_iter, guess).into()
} }
@ -171,6 +176,7 @@ impl<N: RealField> UnitComplex<N> {
#[inline] #[inline]
pub fn rotation_between<SB, SC>(a: &Vector<N, U2, SB>, b: &Vector<N, U2, SC>) -> Self pub fn rotation_between<SB, SC>(a: &Vector<N, U2, SB>, b: &Vector<N, U2, SC>) -> Self
where where
N: RealField,
SB: Storage<N, U2>, SB: Storage<N, U2>,
SC: Storage<N, U2>, SC: Storage<N, U2>,
{ {
@ -198,6 +204,7 @@ impl<N: RealField> UnitComplex<N> {
s: N, s: N,
) -> Self ) -> Self
where where
N: RealField,
SB: Storage<N, U2>, SB: Storage<N, U2>,
SC: Storage<N, U2>, SC: Storage<N, U2>,
{ {
@ -264,29 +271,35 @@ impl<N: RealField> UnitComplex<N> {
let sang = na.perp(&nb); let sang = na.perp(&nb);
let cang = na.dot(&nb); let cang = na.dot(&nb);
Self::from_angle(sang.atan2(cang) * s) Self::from_angle(sang.simd_atan2(cang) * s)
} }
} }
impl<N: RealField> One for UnitComplex<N> { impl<N: SimdRealField> One for UnitComplex<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn one() -> Self { fn one() -> Self {
Self::identity() Self::identity()
} }
} }
impl<N: RealField> Distribution<UnitComplex<N>> for Standard impl<N: SimdRealField> Distribution<UnitComplex<N>> for Standard
where OpenClosed01: Distribution<N> where
N::Element: SimdRealField,
OpenClosed01: Distribution<N>,
{ {
/// Generate a uniformly distributed random `UnitComplex`. /// Generate a uniformly distributed random `UnitComplex`.
#[inline] #[inline]
fn sample<'a, R: Rng + ?Sized>(&self, rng: &mut R) -> UnitComplex<N> { fn sample<'a, R: Rng + ?Sized>(&self, rng: &mut R) -> UnitComplex<N> {
UnitComplex::from_angle(rng.sample(OpenClosed01) * N::two_pi()) UnitComplex::from_angle(rng.sample(OpenClosed01) * N::simd_two_pi())
} }
} }
#[cfg(feature = "arbitrary")] #[cfg(feature = "arbitrary")]
impl<N: RealField + Arbitrary> Arbitrary for UnitComplex<N> { impl<N: SimdRealField + Arbitrary> Arbitrary for UnitComplex<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn arbitrary<G: Gen>(g: &mut G) -> Self { fn arbitrary<G: Gen>(g: &mut G) -> Self {
UnitComplex::from_angle(N::arbitrary(g)) UnitComplex::from_angle(N::arbitrary(g))

17
src/geometry/unit_complex_conversion.rs
View File

@ -2,6 +2,7 @@ use num::Zero;
use num_complex::Complex; use num_complex::Complex;
use simba::scalar::{RealField, SubsetOf, SupersetOf}; use simba::scalar::{RealField, SubsetOf, SupersetOf};
use simba::simd::SimdRealField;
use crate::base::dimension::U2; use crate::base::dimension::U2;
use crate::base::{Matrix2, Matrix3}; use crate::base::{Matrix2, Matrix3};
@ -153,28 +154,36 @@ impl<N1: RealField, N2: RealField + SupersetOf<N1>> SubsetOf<Matrix3<N2>> for Un
} }
} }
impl<N: RealField> From<UnitComplex<N>> for Rotation2<N> { impl<N: SimdRealField> From<UnitComplex<N>> for Rotation2<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn from(q: UnitComplex<N>) -> Self { fn from(q: UnitComplex<N>) -> Self {
q.to_rotation_matrix() q.to_rotation_matrix()
} }
} }
impl<N: RealField> From<Rotation2<N>> for UnitComplex<N> { impl<N: SimdRealField> From<Rotation2<N>> for UnitComplex<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn from(q: Rotation2<N>) -> Self { fn from(q: Rotation2<N>) -> Self {
Self::from_rotation_matrix(&q) Self::from_rotation_matrix(&q)
} }
} }
impl<N: RealField> From<UnitComplex<N>> for Matrix3<N> { impl<N: SimdRealField> From<UnitComplex<N>> for Matrix3<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn from(q: UnitComplex<N>) -> Matrix3<N> { fn from(q: UnitComplex<N>) -> Matrix3<N> {
q.to_homogeneous() q.to_homogeneous()
} }
} }
impl<N: RealField> From<UnitComplex<N>> for Matrix2<N> { impl<N: SimdRealField> From<UnitComplex<N>> for Matrix2<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn from(q: UnitComplex<N>) -> Self { fn from(q: UnitComplex<N>) -> Self {
q.to_rotation_matrix().into_inner() q.to_rotation_matrix().into_inner()

104
src/geometry/unit_complex_ops.rs
View File

@ -6,6 +6,7 @@ use crate::base::storage::Storage;
use crate::base::{DefaultAllocator, Unit, Vector, Vector2}; use crate::base::{DefaultAllocator, Unit, Vector, Vector2};
use crate::geometry::{Isometry, Point2, Rotation, Similarity, Translation, UnitComplex}; use crate::geometry::{Isometry, Point2, Rotation, Similarity, Translation, UnitComplex};
use simba::scalar::RealField; use simba::scalar::RealField;
use simba::simd::SimdRealField;
/* /*
* This file provides: * This file provides:
@ -44,7 +45,7 @@ use simba::scalar::RealField;
*/ */
// UnitComplex × UnitComplex // UnitComplex × UnitComplex
impl<N: RealField> Mul<Self> for UnitComplex<N> { impl<N: SimdRealField> Mul<Self> for UnitComplex<N> {
type Output = Self; type Output = Self;
#[inline] #[inline]
@ -53,7 +54,9 @@ impl<N: RealField> Mul<Self> for UnitComplex<N> {
} }
} }
impl<'a, N: RealField> Mul<UnitComplex<N>> for &'a UnitComplex<N> { impl<'a, N: SimdRealField> Mul<UnitComplex<N>> for &'a UnitComplex<N>
where N::Element: SimdRealField
{
type Output = UnitComplex<N>; type Output = UnitComplex<N>;
#[inline] #[inline]
@ -62,26 +65,32 @@ impl<'a, N: RealField> Mul<UnitComplex<N>> for &'a UnitComplex<N> {
} }
} }
impl<'b, N: RealField> Mul<&'b UnitComplex<N>> for UnitComplex<N> { impl<'b, N: SimdRealField> Mul<&'b UnitComplex<N>> for UnitComplex<N>
where N::Element: SimdRealField
{
type Output = Self; type Output = Self;
#[inline] #[inline]
fn mul(self, rhs: &'b UnitComplex<N>) -> Self::Output { fn mul(self, rhs: &'b UnitComplex<N>) -> 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<N>> for &'a UnitComplex<N> { impl<'a, 'b, N: SimdRealField> Mul<&'b UnitComplex<N>> for &'a UnitComplex<N>
where N::Element: SimdRealField
{
type Output = UnitComplex<N>; type Output = UnitComplex<N>;
#[inline] #[inline]
fn mul(self, rhs: &'b UnitComplex<N>) -> Self::Output { fn mul(self, rhs: &'b UnitComplex<N>) -> Self::Output {
Unit::new_unchecked(self.complex() * rhs.complex()) Unit::new_unchecked(self.complex() * rhs.as_ref())
} }
} }
// UnitComplex ÷ UnitComplex // UnitComplex ÷ UnitComplex
impl<N: RealField> Div<Self> for UnitComplex<N> { impl<N: SimdRealField> Div<Self> for UnitComplex<N>
where N::Element: SimdRealField
{
type Output = Self; type Output = Self;
#[inline] #[inline]
@ -90,7 +99,9 @@ impl<N: RealField> Div<Self> for UnitComplex<N> {
} }
} }
impl<'a, N: RealField> Div<UnitComplex<N>> for &'a UnitComplex<N> { impl<'a, N: SimdRealField> Div<UnitComplex<N>> for &'a UnitComplex<N>
where N::Element: SimdRealField
{
type Output = UnitComplex<N>; type Output = UnitComplex<N>;
#[inline] #[inline]
@ -99,7 +110,9 @@ impl<'a, N: RealField> Div<UnitComplex<N>> for &'a UnitComplex<N> {
} }
} }
impl<'b, N: RealField> Div<&'b UnitComplex<N>> for UnitComplex<N> { impl<'b, N: SimdRealField> Div<&'b UnitComplex<N>> for UnitComplex<N>
where N::Element: SimdRealField
{
type Output = Self; type Output = Self;
#[inline] #[inline]
@ -108,7 +121,9 @@ impl<'b, N: RealField> Div<&'b UnitComplex<N>> for UnitComplex<N> {
} }
} }
impl<'a, 'b, N: RealField> Div<&'b UnitComplex<N>> for &'a UnitComplex<N> { impl<'a, 'b, N: SimdRealField> Div<&'b UnitComplex<N>> for &'a UnitComplex<N>
where N::Element: SimdRealField
{
type Output = UnitComplex<N>; type Output = UnitComplex<N>;
#[inline] #[inline]
@ -122,8 +137,9 @@ macro_rules! complex_op_impl(
($RDim: ident, $CDim: ident) $(for $Storage: ident: $StoragesBound: ident $(<$($BoundParam: ty),*>)*),*; ($RDim: ident, $CDim: ident) $(for $Storage: ident: $StoragesBound: ident $(<$($BoundParam: ty),*>)*),*;
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty;
$action: expr; $($lives: tt),*) => { $action: expr; $($lives: tt),*) => {
impl<$($lives ,)* N: RealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs impl<$($lives ,)* N: SimdRealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs
where DefaultAllocator: Allocator<N, $RDim, $CDim> { where N::Element: SimdRealField,
DefaultAllocator: Allocator<N, $RDim, $CDim> {
type Output = $Result; type Output = $Result;
#[inline] #[inline]
@ -300,14 +316,18 @@ complex_op_impl_all!(
); );
// UnitComplex ×= UnitComplex // UnitComplex ×= UnitComplex
impl<N: RealField> MulAssign<UnitComplex<N>> for UnitComplex<N> { impl<N: SimdRealField> MulAssign<UnitComplex<N>> for UnitComplex<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn mul_assign(&mut self, rhs: UnitComplex<N>) { fn mul_assign(&mut self, rhs: UnitComplex<N>) {
*self = &*self * rhs *self = &*self * rhs
} }
} }
impl<'b, N: RealField> MulAssign<&'b UnitComplex<N>> for UnitComplex<N> { impl<'b, N: SimdRealField> MulAssign<&'b UnitComplex<N>> for UnitComplex<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn mul_assign(&mut self, rhs: &'b UnitComplex<N>) { fn mul_assign(&mut self, rhs: &'b UnitComplex<N>) {
*self = &*self * rhs *self = &*self * rhs
@ -315,14 +335,18 @@ impl<'b, N: RealField> MulAssign<&'b UnitComplex<N>> for UnitComplex<N> {
} }
// UnitComplex /= UnitComplex // UnitComplex /= UnitComplex
impl<N: RealField> DivAssign<UnitComplex<N>> for UnitComplex<N> { impl<N: SimdRealField> DivAssign<UnitComplex<N>> for UnitComplex<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn div_assign(&mut self, rhs: UnitComplex<N>) { fn div_assign(&mut self, rhs: UnitComplex<N>) {
*self = &*self / rhs *self = &*self / rhs
} }
} }
impl<'b, N: RealField> DivAssign<&'b UnitComplex<N>> for UnitComplex<N> { impl<'b, N: SimdRealField> DivAssign<&'b UnitComplex<N>> for UnitComplex<N>
where N::Element: SimdRealField
{
#[inline] #[inline]
fn div_assign(&mut self, rhs: &'b UnitComplex<N>) { fn div_assign(&mut self, rhs: &'b UnitComplex<N>) {
*self = &*self / rhs *self = &*self / rhs
@ -330,8 +354,10 @@ impl<'b, N: RealField> DivAssign<&'b UnitComplex<N>> for UnitComplex<N> {
} }
// UnitComplex ×= Rotation // UnitComplex ×= Rotation
impl<N: RealField> MulAssign<Rotation<N, U2>> for UnitComplex<N> impl<N: SimdRealField> MulAssign<Rotation<N, U2>> for UnitComplex<N>
where DefaultAllocator: Allocator<N, U2, U2> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, U2, U2>,
{ {
#[inline] #[inline]
fn mul_assign(&mut self, rhs: Rotation<N, U2>) { fn mul_assign(&mut self, rhs: Rotation<N, U2>) {
@ -339,8 +365,10 @@ where DefaultAllocator: Allocator<N, U2, U2>
} }
} }
impl<'b, N: RealField> MulAssign<&'b Rotation<N, U2>> for UnitComplex<N> impl<'b, N: SimdRealField> MulAssign<&'b Rotation<N, U2>> for UnitComplex<N>
where DefaultAllocator: Allocator<N, U2, U2> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, U2, U2>,
{ {
#[inline] #[inline]
fn mul_assign(&mut self, rhs: &'b Rotation<N, U2>) { fn mul_assign(&mut self, rhs: &'b Rotation<N, U2>) {
@ -349,8 +377,10 @@ where DefaultAllocator: Allocator<N, U2, U2>
} }
// UnitComplex ÷= Rotation // UnitComplex ÷= Rotation
impl<N: RealField> DivAssign<Rotation<N, U2>> for UnitComplex<N> impl<N: SimdRealField> DivAssign<Rotation<N, U2>> for UnitComplex<N>
where DefaultAllocator: Allocator<N, U2, U2> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, U2, U2>,
{ {
#[inline] #[inline]
fn div_assign(&mut self, rhs: Rotation<N, U2>) { fn div_assign(&mut self, rhs: Rotation<N, U2>) {
@ -358,8 +388,10 @@ where DefaultAllocator: Allocator<N, U2, U2>
} }
} }
impl<'b, N: RealField> DivAssign<&'b Rotation<N, U2>> for UnitComplex<N> impl<'b, N: SimdRealField> DivAssign<&'b Rotation<N, U2>> for UnitComplex<N>
where DefaultAllocator: Allocator<N, U2, U2> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, U2, U2>,
{ {
#[inline] #[inline]
fn div_assign(&mut self, rhs: &'b Rotation<N, U2>) { fn div_assign(&mut self, rhs: &'b Rotation<N, U2>) {
@ -368,8 +400,10 @@ where DefaultAllocator: Allocator<N, U2, U2>
} }
// Rotation ×= UnitComplex // Rotation ×= UnitComplex
impl<N: RealField> MulAssign<UnitComplex<N>> for Rotation<N, U2> impl<N: SimdRealField> MulAssign<UnitComplex<N>> for Rotation<N, U2>
where DefaultAllocator: Allocator<N, U2, U2> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, U2, U2>,
{ {
#[inline] #[inline]
fn mul_assign(&mut self, rhs: UnitComplex<N>) { fn mul_assign(&mut self, rhs: UnitComplex<N>) {
@ -377,8 +411,10 @@ where DefaultAllocator: Allocator<N, U2, U2>
} }
} }
impl<'b, N: RealField> MulAssign<&'b UnitComplex<N>> for Rotation<N, U2> impl<'b, N: SimdRealField> MulAssign<&'b UnitComplex<N>> for Rotation<N, U2>
where DefaultAllocator: Allocator<N, U2, U2> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, U2, U2>,
{ {
#[inline] #[inline]
fn mul_assign(&mut self, rhs: &'b UnitComplex<N>) { fn mul_assign(&mut self, rhs: &'b UnitComplex<N>) {
@ -387,8 +423,10 @@ where DefaultAllocator: Allocator<N, U2, U2>
} }
// Rotation ÷= UnitComplex // Rotation ÷= UnitComplex
impl<N: RealField> DivAssign<UnitComplex<N>> for Rotation<N, U2> impl<N: SimdRealField> DivAssign<UnitComplex<N>> for Rotation<N, U2>
where DefaultAllocator: Allocator<N, U2, U2> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, U2, U2>,
{ {
#[inline] #[inline]
fn div_assign(&mut self, rhs: UnitComplex<N>) { fn div_assign(&mut self, rhs: UnitComplex<N>) {
@ -396,8 +434,10 @@ where DefaultAllocator: Allocator<N, U2, U2>
} }
} }
impl<'b, N: RealField> DivAssign<&'b UnitComplex<N>> for Rotation<N, U2> impl<'b, N: SimdRealField> DivAssign<&'b UnitComplex<N>> for Rotation<N, U2>
where DefaultAllocator: Allocator<N, U2, U2> where
N::Element: SimdRealField,
DefaultAllocator: Allocator<N, U2, U2>,
{ {
#[inline] #[inline]
fn div_assign(&mut self, rhs: &'b UnitComplex<N>) { fn div_assign(&mut self, rhs: &'b UnitComplex<N>) {

6
src/geometry/unit_complex_simba.rs
View File

@ -4,10 +4,10 @@ use std::ops::Deref;
use crate::base::{Scalar, Unit}; use crate::base::{Scalar, Unit};
use crate::geometry::UnitComplex; use crate::geometry::UnitComplex;
use crate::RealField; use crate::SimdRealField;
impl<N: RealField> SimdValue for UnitComplex<N> impl<N: SimdRealField> SimdValue for UnitComplex<N>
where N::Element: Scalar where N::Element: SimdRealField
{ {
type Element = UnitComplex<N::Element>; type Element = UnitComplex<N::Element>;
type SimdBool = N::SimdBool; type SimdBool = N::SimdBool;