diff --git a/src/geometry/rotation_interpolation.rs b/src/geometry/rotation_interpolation.rs index fb3927a0..4f9a919a 100644 --- a/src/geometry/rotation_interpolation.rs +++ b/src/geometry/rotation_interpolation.rs @@ -21,7 +21,7 @@ impl Rotation2 { /// ``` #[inline] #[must_use] - pub fn slerp(&self, other: &Self, t: T) -> Self + fn slerp_2d(&self, other: &Self, t: T) -> Self where T::Element: SimdRealField, { @@ -51,7 +51,7 @@ impl Rotation3 { /// ``` #[inline] #[must_use] - pub fn slerp(&self, other: &Self, t: T) -> Self + fn slerp_3d(&self, other: &Self, t: T) -> Self where T: RealField, { @@ -82,25 +82,6 @@ impl Rotation3 { } } -impl Rotation { - - #[warn(missing_docs)] - pub fn basis_rot(i:usize, j:usize, angle:T) -> Self { - - let mut m = SMatrix::identity(); - if i==j { return Self::from_matrix_unchecked(m); } - - let (s,c) = angle.simd_sin_cos(); - m[(i,i)] = c.clone(); - m[(i,j)] = -s.clone(); - m[(j,i)] = s; - m[(j,j)] = c; - - Self::from_matrix_unchecked(m) - - } -} - impl Rotation where Const: DimSub, ArrayStorage: Storage,Const>, @@ -109,59 +90,44 @@ impl Rotation where Allocator,U1>> { + //FIXME: merging slerp for Rotation2 and Rotation3 into this raises the trait bounds + //from SimdRealField to RealField + #[inline] + #[must_use] #[warn(missing_docs)] - pub fn general_slerp(&self, other: &Self, t:T) -> Self { - self * (self/other).general_pow(t) - } + pub fn slerp(&self, other: &Self, t:T) -> Self { - #[warn(missing_docs)] - pub fn general_pow(self, t:T) -> Self { - if D<=1 { return self; } + use std::mem::transmute; - println!("r:{}", self); - println!("{}", self.clone().into_inner().hessenberg().unpack_h()); + //The best option here would be to use #[feature(specialization)], but until + //that's stabilized, this is the best we can do. Theoretically, the compiler should + //pretty thoroughly optimize away all the excess checks and conversions + match D { - //taking the (real) schur form is guaranteed to produce a block-diagonal matrix - //where each block is either a 1 (if there's no rotation in that axis) or a 2x2 - //rotation matrix in a particular plane - let schur = self.into_inner().schur(); - let (q, mut d) = schur.unpack(); + 0 => self.clone(), - // println!("q:{}d:{:.3}", q, d); + //FIXME: this doesn't really work in 1D since we can't interp between -1 and 1 + 1 => self.clone(), - //go down the diagonal and pow every block - for i in 0..(D-1) { - - //we've found a 2x2 block! - //NOTE: the impl of the schur decomposition always sets the inferior diagonal to 0 - if !d[(i+1,i)].is_zero() { - - // println!("{}", i); - - //convert to a complex num and take the arg() - let (c, s) = (d[(i,i)].clone(), d[(i+1,i)].clone()); - let angle = s.atan2(c); - - // println!("{}", angle); - - //scale the arg and exponentiate back - let angle2 = angle * t.clone(); - let (s2, c2) = angle2.sin_cos(); - - //convert back into a rot block - d[(i, i )] = c2.clone(); - d[(i, i+1)] = -s2.clone(); - d[(i+1,i )] = s2; - d[(i+1,i+1)] = c2; - - } + //NOTE: Not pretty, but without refactoring the API, this is the best we can do + //NOTE: This is safe because we directly check the dimension first + 2 => unsafe { + let (self2d, other2d) = ( + transmute::<&Self,&Rotation2>(self), + transmute::<&Self,&Rotation2>(other), + ); + transmute::<&Rotation2,&Self>(&self2d.slerp_2d(other2d, t)).clone() + }, + 3 => unsafe { + let (self3d, other3d) = ( + transmute::<&Self,&Rotation3>(self), + transmute::<&Self,&Rotation3>(other), + ); + transmute::<&Rotation3,&Self>(&self3d.slerp_3d(other3d, t)).clone() + }, + _ => self * (self/other).powf(t) } - // println!("d:{:.3}", d); - - let qt = q.transpose(); //avoids an extra clone - - Self::from_matrix_unchecked(q * d * qt) } diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index 41405c87..07e5189a 100644 --- a/src/geometry/rotation_specialization.rs +++ b/src/geometry/rotation_specialization.rs @@ -15,11 +15,13 @@ use simba::scalar::RealField; use simba::simd::{SimdBool, SimdRealField}; use std::ops::Neg; -use crate::base::dimension::{U1, U2, U3}; +use crate::base::dimension::{Const, U1, U2, U3, DimSub, DimDiff}; +use crate::base::allocator::Allocator; use crate::base::storage::Storage; -use crate::base::{Matrix2, Matrix3, SMatrix, SVector, Unit, Vector, Vector1, Vector2, Vector3}; +use crate::base::{ Matrix2, Matrix3, SMatrix, SVector, Unit, Vector, Vector1, Vector2, Vector3}; +use crate::base::{ArrayStorage, DefaultAllocator}; -use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion}; +use crate::geometry::{Rotation, Rotation2, Rotation3, UnitComplex, UnitQuaternion}; /* * @@ -217,7 +219,7 @@ impl Rotation2 { /// ``` #[inline] #[must_use] - pub fn powf(&self, n: T) -> Self { + fn powf_2d(&self, n: T) -> Self { Self::new(self.angle() * n) } } @@ -676,7 +678,7 @@ where /// ``` #[inline] #[must_use] - pub fn powf(&self, n: T) -> Self + fn powf_3d(&self, n: T) -> Self where T: RealField, { @@ -1004,3 +1006,98 @@ where Self::new(SVector::arbitrary(g)) } } + +impl Rotation +{ + + //FIXME: merging powf for Rotation2 into this raises the trait bounds from SimdRealField to RealField + #[warn(missing_docs)] + pub fn powf(&self, t: T) -> Self where + Const: DimSub, + ArrayStorage: Storage,Const>, + DefaultAllocator: Allocator,Const,Buffer=ArrayStorage> + Allocator> + + Allocator,DimDiff,U1>> + + Allocator,U1>> + { + use std::mem::*; + + //The best option here would be to use #[feature(specialization)], but until + //that's stabilized, this is the best we can do. Theoretically, the compiler should + //pretty thoroughly optimize away all the excess checks and conversions + match D { + + 0 => self.clone(), + 1 => self.clone(), + + //NOTE: Not pretty, but without refactoring the API, this is the best we can do + //NOTE: This is safe because we directly check the dimension first + 2 => unsafe { + let r2d = transmute::<&Self,&Rotation2>(self).powf_2d(t); + transmute::<&Rotation2,&Self>(&r2d).clone() + }, + 3 => unsafe { + let r3d = transmute::<&Self,&Rotation3>(self).powf_3d(t); + transmute::<&Rotation3,&Self>(&r3d).clone() + }, + + _ => self.clone().general_pow(t) + } + } + + fn general_pow(self, t:T) -> Self where + Const: DimSub, + ArrayStorage: Storage,Const>, + DefaultAllocator: Allocator,Const,Buffer=ArrayStorage> + Allocator> + + Allocator,DimDiff,U1>> + + Allocator,U1>> + { + if D<=1 { return self; } + + // println!("r:{}", self); + // println!("{}", self.clone().into_inner().hessenberg().unpack_h()); + + //taking the (real) schur form is guaranteed to produce a block-diagonal matrix + //where each block is either a 1 (if there's no rotation in that axis) or a 2x2 + //rotation matrix in a particular plane + let schur = self.into_inner().schur(); + let (q, mut d) = schur.unpack(); + + // println!("q:{}d:{:.3}", q, d); + + //go down the diagonal and pow every block + for i in 0..(D-1) { + + //we've found a 2x2 block! + //NOTE: the impl of the schur decomposition always sets the inferior diagonal to 0 + if !d[(i+1,i)].is_zero() { + + // println!("{}", i); + + //convert to a complex num and take the arg() + let (c, s) = (d[(i,i)].clone(), d[(i+1,i)].clone()); + let angle = s.atan2(c); + + // println!("{}", angle); + + //scale the arg and exponentiate back + let angle2 = angle * t.clone(); + let (s2, c2) = angle2.sin_cos(); + + //convert back into a rot block + d[(i, i )] = c2.clone(); + d[(i, i+1)] = -s2.clone(); + d[(i+1,i )] = s2; + d[(i+1,i+1)] = c2; + + } + + } + // println!("d:{:.3}", d); + + let qt = q.transpose(); //avoids an extra clone + + Self::from_matrix_unchecked(q * d * qt) + + } + +} diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs index 3d2ef996..7c14c4bc 100644 --- a/tests/geometry/rotation.rs +++ b/tests/geometry/rotation.rs @@ -52,7 +52,6 @@ mod proptest_tests { ) { use nalgebra::*; - use num_traits::Zero; //make an orthonormal basis let mut basis = [$($v1, $v2),*]; @@ -81,7 +80,7 @@ mod proptest_tests { bivector += bivectors[i]; } - let r1 = Rotation::from_matrix_unchecked(bivector.exp()).general_pow(pow); + let r1 = Rotation::from_matrix_unchecked(bivector.exp()).powf(pow); let r2 = Rotation::from_matrix_unchecked((bivector * pow).exp()); prop_assert!(relative_eq!(r1, r2, epsilon=1e-7));