diff --git a/src/geometry/rotation_interpolation.rs b/src/geometry/rotation_interpolation.rs index 28b1fb80..c6d17ec5 100644 --- a/src/geometry/rotation_interpolation.rs +++ b/src/geometry/rotation_interpolation.rs @@ -118,12 +118,16 @@ impl Rotation where pub fn general_pow(self, t:T) -> Self { if D<=1 { return self; } + println!("r:{}", self); + //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) { @@ -131,10 +135,14 @@ impl Rotation where //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(); @@ -148,6 +156,7 @@ impl Rotation where } } + println!("d:{:.3}", d); let qt = q.transpose(); //avoids an extra clone diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs index 9a29772e..9eac2ff5 100644 --- a/tests/geometry/rotation.rs +++ b/tests/geometry/rotation.rs @@ -32,7 +32,7 @@ fn quaternion_euler_angles_issue_494() { #[cfg(feature = "proptest-support")] mod proptest_tests { - use na::{self, Rotation2, Rotation3, Unit}; + use na::{self, Rotation, Rotation2, Rotation3, Unit}; use simba::scalar::RealField; use std::f64; @@ -229,5 +229,79 @@ mod proptest_tests { prop_assert_eq!(r, Rotation3::identity()) } } + + // macro_rules! gen_pof_rotation_test { + // ($( + // fn $powf_rot_n:ident($($v1:ident in $vec1:ident(), $v2:ident in $vec2:ident()),*); + // )*) => {$ + // + // #[test] + // fn $powf_rot_n( + // $($v1 in $vec1(), $v2 in $vec2(),)* + // pow in PROPTEST_F64 + // ) { + // + // } + // + // )*} + // } + + #[test] + fn powf_rotation_4( + v1 in vector4(), v2 in vector4(), + v3 in vector4(), v4 in vector4(), + pow in PROPTEST_F64 + ) { + + use nalgebra::*; + use num_traits::Zero; + + type Rotation4 = Rotation; + + //make an orthonormal basis + let mut basis = [v1,v2,v3,v4]; + Vector::orthonormalize(&mut basis); + let [v1,v2,v3,v4] = basis; + + //"wedge" the vectors to make two 2-blades representing two rotation planes + //since we start with vector pairs, each bivector is guaranteed to be simple + let mut b1 = v1.transpose().kronecker(&v2) - v2.transpose().kronecker(&v1); + let mut b2 = v3.transpose().kronecker(&v4) - v4.transpose().kronecker(&v3); + + //condition b1 + if let Some((unit, norm)) = Unit::try_new_and_get(b1, 0.0) { + //every component is duplicated once, so there's an extra factor or sqrt(2) in the norm + //and wrap angle into the correct range + let mut angle = norm / 2.0f64.sqrt(); + angle = na::wrap(angle, -f64::pi(), f64::pi()); + b1 = unit.into_inner() * angle * 2.0f64.sqrt(); + } + + //condition b2 + if let Some((unit, norm)) = Unit::try_new_and_get(b2, 0.0) { + let mut angle = norm / 2.0f64.sqrt(); + angle = na::wrap(angle, -f64::pi(), f64::pi()); + b2 = unit.into_inner() * angle * 2.0f64.sqrt(); + } + + let bivector = b1+b2; + + println!("b:{:.3}", bivector); + + let r1 = Rotation4::from_matrix_unchecked(bivector.exp()); + let r2 = Rotation4::from_matrix_unchecked((bivector * pow).exp()); + + // println!("{}{}", r1, r2); + // println!("{}", r1.general_pow(pow)); + + prop_assert!( + relative_eq!(r1.general_pow(pow), r2, epsilon=1e-7) + ); + + + } + + + } }