Generalized slerp() and powf() for Rotation<T,N>

src/geometry/rotation_interpolation.rs

@@ -1,4 +1,7 @@
-use crate::{RealField, Rotation2, Rotation3, SimdRealField, UnitComplex, UnitQuaternion};
+use crate::{Allocator, ArrayStorage, Const, DefaultAllocator, DimDiff, DimSub, Storage, U1};
+use crate::{
+    RealField, Rotation, Rotation2, Rotation3, SimdRealField, UnitComplex, UnitQuaternion,
+};
 
 /// # Interpolation
 impl<T: SimdRealField> Rotation2<T> {
@@ -19,7 +22,7 @@ impl<T: SimdRealField> Rotation2<T> {
     /// ```
     #[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,
     {
@@ -49,7 +52,7 @@ impl<T: SimdRealField> Rotation3<T> {
     /// ```
     #[inline]
     #[must_use]
-    pub fn slerp(&self, other: &Self, t: T) -> Self
+    fn slerp_3d(&self, other: &Self, t: T) -> Self
     where
         T: RealField,
     {
@@ -79,3 +82,67 @@ impl<T: SimdRealField> Rotation3<T> {
         q1.try_slerp(&q2, t, epsilon).map(|q| q.into())
     }
 }
+
+impl<T: RealField, const D: usize> Rotation<T, D>
+where
+    Const<D>: DimSub<U1>,
+    ArrayStorage<T, D, D>: Storage<T, Const<D>, Const<D>>,
+    DefaultAllocator: Allocator<T, Const<D>, Const<D>, Buffer = ArrayStorage<T, D, D>>
+        + Allocator<T, Const<D>>
+        + Allocator<T, Const<D>, DimDiff<Const<D>, U1>>
+        + Allocator<T, DimDiff<Const<D>, U1>>,
+{
+    ///
+    /// Computes the spherical linear interpolation between two general rotations.
+    ///
+    /// # Examples:
+    ///
+    /// ```
+    /// # use nalgebra::geometry::Rotation3;
+    ///
+    /// let q1 = Rotation3::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0);
+    /// let q2 = Rotation3::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0);
+    ///
+    /// let q = q1.slerp(&q2, 1.0 / 3.0);
+    ///
+    /// assert_eq!(q.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0));
+    /// ```
+    ///
+    //FIXME: merging slerp for Rotation2 and Rotation3 into this raises the trait bounds
+    //from SimdRealField to RealField
+    #[inline]
+    #[must_use]
+    pub fn slerp(&self, other: &Self, t: T) -> Self {
+        use std::mem::transmute;
+
+        //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(),
+
+            //FIXME: this doesn't really work in 1D since we can't interp between -1 and 1
+            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 (self2d, other2d) = (
+                    transmute::<&Self, &Rotation2<T>>(self),
+                    transmute::<&Self, &Rotation2<T>>(other),
+                );
+                transmute::<&Rotation2<T>, &Self>(&self2d.slerp_2d(other2d, t)).clone()
+            },
+            3 => unsafe {
+                let (self3d, other3d) = (
+                    transmute::<&Self, &Rotation3<T>>(self),
+                    transmute::<&Self, &Rotation3<T>>(other),
+                );
+                transmute::<&Rotation3<T>, &Self>(&self3d.slerp_3d(other3d, t)).clone()
+            },
+
+            //the multiplication order matters here
+            _ => (other / self).powf(t) * self,
+        }
+    }
+}

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::allocator::Allocator;
+use crate::base::dimension::{Const, DimDiff, DimSub, U1, U2, U3};
 use crate::base::storage::Storage;
+use crate::base::{ArrayStorage, DefaultAllocator};
 use crate::base::{Matrix2, Matrix3, SMatrix, SVector, Unit, Vector, Vector1, Vector2, Vector3};
 
-use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion};
+use crate::geometry::{Rotation, Rotation2, Rotation3, UnitComplex, UnitQuaternion};
 
 /*
  *
@@ -217,7 +219,7 @@ impl<T: SimdRealField> Rotation2<T> {
     /// ```
     #[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,119 @@ where
         Self::new(SVector::arbitrary(g))
     }
 }
+
+impl<T: RealField, const D: usize> Rotation<T, D> {
+    ///
+    /// Raise the rotation to a given floating power, i.e., returns the rotation with the same
+    /// axis as `self` and an angle equal to `self.angle()` multiplied by `n`.
+    ///
+    /// # Example
+    /// ```
+    /// # #[macro_use] extern crate approx;
+    /// # use nalgebra::{Rotation3, Vector3, Unit};
+    ///
+    /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
+    /// let angle = 1.2;
+    /// let rot = Rotation3::from_axis_angle(&axis, angle);
+    /// let pow = rot.powf(2.0);
+    ///
+    /// assert_relative_eq!(pow.axis().unwrap(), axis, epsilon = 1.0e-6);
+    /// assert_eq!(pow.angle(), 2.4);
+    /// ```
+    //FIXME: merging powf for Rotation2 into this raises the trait bounds from SimdRealField to RealField
+    pub fn powf(&self, t: T) -> Self
+    where
+        Const<D>: DimSub<U1>,
+        ArrayStorage<T, D, D>: Storage<T, Const<D>, Const<D>>,
+        DefaultAllocator: Allocator<T, Const<D>, Const<D>, Buffer = ArrayStorage<T, D, D>>
+            + Allocator<T, Const<D>>
+            + Allocator<T, Const<D>, DimDiff<Const<D>, U1>>
+            + Allocator<T, DimDiff<Const<D>, 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<T>>(self).powf_2d(t);
+                transmute::<&Rotation2<T>, &Self>(&r2d).clone()
+            },
+            3 => unsafe {
+                let r3d = transmute::<&Self, &Rotation3<T>>(self).powf_3d(t);
+                transmute::<&Rotation3<T>, &Self>(&r3d).clone()
+            },
+
+            _ => self.clone().general_pow(t),
+        }
+    }
+
+    fn general_pow(self, t: T) -> Self
+    where
+        Const<D>: DimSub<U1>,
+        ArrayStorage<T, D, D>: Storage<T, Const<D>, Const<D>>,
+        DefaultAllocator: Allocator<T, Const<D>, Const<D>, Buffer = ArrayStorage<T, D, D>>
+            + Allocator<T, Const<D>>
+            + Allocator<T, Const<D>, DimDiff<Const<D>, U1>>
+            + Allocator<T, DimDiff<Const<D>, 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
+        let mut i = 0;
+        while i < D - 1 {
+            if
+            //For most 2x2 blocks
+            //NOTE: we use strict equality since `nalgebra`'s schur decomp sets the infradiagonal to zero
+            !d[(i+1,i)].is_zero() ||
+
+                //for +-180 deg rotations
+                d[(i,i)]<T::zero() && d[(i+1,i+1)]<T::zero()
+            {
+                //convert to a complex num and find the arg()
+                let (c, s) = (d[(i, i)].clone(), d[(i + 1, i)].clone());
+                let angle = s.atan2(c); //for +-180deg rots, this implicitely takes the +180 branch
+
+                //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;
+
+                //increase by 2 so we don't accidentally misinterpret the
+                //next line as a 180deg rotation
+                i += 2;
+            } else {
+                i += 1;
+            }
+        }
+        // println!("d:{:.3}", d);
+
+        let qt = q.transpose(); //avoids an extra clone
+
+        Self::from_matrix_unchecked(q * d * qt)
+    }
+}

tests/geometry/rotation.rs

@@ -32,7 +32,8 @@ fn quaternion_euler_angles_issue_494() {
 
 #[cfg(feature = "proptest-support")]
 mod proptest_tests {
-    use na::{self, Rotation2, Rotation3, Unit};
+    use na::{self, Matrix, Rotation, Rotation2, Rotation3, SMatrix, Unit, Vector};
+    use num_traits::Zero;
     use simba::scalar::RealField;
     use std::f64;
 
@@ -229,5 +230,133 @@ mod proptest_tests {
                 prop_assert_eq!(r, Rotation3::identity())
             }
         }
+
+    }
+
+    //creates N rotation planes and angles
+    macro_rules! gen_rotation_planes {
+        ($($v1:ident, $v2:ident),*) => {
+            {
+                //make an orthonormal basis
+                let mut basis = [$($v1, $v2),*];
+                Vector::orthonormalize(&mut basis);
+                let [$($v1, $v2),*] = basis;
+
+                //"wedge" the vectors to make an arrary 2-blades representing rotation planes.
+                [
+                    //Since we start with vector pairs, each bivector is guaranteed to be simple
+                    $($v1.transpose().kronecker(&$v2) - $v2.transpose().kronecker(&$v1)),*
+                ]
+            }
+
+        };
     }
+
+    macro_rules! gen_powf_rotation_test {
+        ($(
+            fn $powf_rot_n:ident($($v:ident in $vec:ident()),*);
+        )*) => {
+            proptest!{$(
+
+                #[test]
+                fn $powf_rot_n(
+                    $($v in $vec(),)*
+                    pow in PROPTEST_F64
+                ) {
+
+                    //"wedge" the vectors to make an arrary 2-blades representing rotation planes.
+                    let mut bivectors = gen_rotation_planes!($($v),*);
+
+                    //condition the bivectors
+                    for b in &mut bivectors {
+                        if let Some((unit, norm)) = Unit::try_new_and_get(*b, 0.0) {
+                            //every component is duplicated once, so there's an extra factor of
+                            //sqrt(2) in the norm
+                            let mut angle = norm / 2.0f64.sqrt();
+                            angle = na::wrap(angle, -f64::pi(), f64::pi());
+                            *b = unit.into_inner() * angle * 2.0f64.sqrt();
+                        }
+                    }
+
+                    let mut bivector = bivectors[0].clone();
+                    for i in 1..bivectors.len() {
+                        bivector += bivectors[i];
+                    }
+
+                    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));
+
+                }
+
+            )*}
+        };
+    }
+
+    macro_rules! gen_powf_180deg_rotation_test {
+        ($(
+            fn $powf_rot_n:ident($($v:ident in $vec:ident()),*);
+        )*) => {$(
+            proptest! {
+
+                #[test]
+                fn $powf_rot_n($($v in $vec(),)*) {
+
+                    use std::f64::consts::PI;
+
+                    //an array of tuples with the unit plane and angle
+                    let plane_angles = gen_rotation_planes!($($v),*).iter().map(
+                        |b| Unit::try_new_and_get(*b,0.0).map_or_else(
+                            || (Matrix::zero(), 0.0),
+                            |(b,a)| (b.into_inner(), a)
+                        )
+                    ).collect::<Vec<_>>();
+
+                    //loop over every choice of between the original angle and swapping to 180 deg
+                    let n = plane_angles.len();
+                    for mask in 0..(1<<n) {
+
+                        let mut b = SMatrix::zero();
+                        for i in 0..n {
+                            let (bi, ai) = plane_angles[i];
+                            if mask & (1<<i) != 0 {
+                                b += PI*bi;
+                            } else {
+                                b += ai*bi;
+                            }
+                        }
+
+                        //makes sure that e^(B/2)^2 == e^B
+                        let r1 = Rotation::from_matrix_unchecked(b.exp());
+                        let r2 = Rotation::from_matrix_unchecked((b/2.0).exp());
+                        prop_assert!(relative_eq!(r1.powf(0.5), r2, epsilon=1e-7));
+
+                    }
+                }
+            }
+        )*}
+    }
+
+    gen_powf_rotation_test!(
+        fn powf_rotation_4(v1 in vector4(), v2 in vector4(), v3 in vector4(), v4 in vector4());
+        fn powf_rotation_5(v1 in vector5(), v2 in vector5(), v3 in vector5(), v4 in vector5());
+        fn powf_rotation_6(
+            v1 in vector6(), v2 in vector6(),
+            v3 in vector6(), v4 in vector6(),
+            v5 in vector6(), v6 in vector6()
+        );
+    );
+
+    gen_powf_180deg_rotation_test!(
+        fn powf_180deg_rotation_2(v1 in vector2(), v2 in vector2());
+        fn powf_180deg_rotation_3(v1 in vector3(), v2 in vector3());
+        fn powf_180deg_rotation_4(v1 in vector4(), v2 in vector4(), v3 in vector4(), v4 in vector4());
+        fn powf_180deg_rotation_5(v1 in vector5(), v2 in vector5(), v3 in vector5(), v4 in vector5());
+        fn powf_180deg_rotation_6(
+            v1 in vector6(), v2 in vector6(),
+            v3 in vector6(), v4 in vector6(),
+            v5 in vector6(), v6 in vector6()
+        );
+    );
 }