From e52057b8a589ece1403480c7a31db7a7f22d4f2d Mon Sep 17 00:00:00 2001
From: Joshua Smith <jsmith62831@gmail.com>
Date: Thu, 24 Mar 2022 12:34:04 -0500
Subject: [PATCH] added powf proptest for 4d rotations

---
 src/geometry/rotation_interpolation.rs |  9 +++
 tests/geometry/rotation.rs             | 76 +++++++++++++++++++++++++-
 2 files changed, 84 insertions(+), 1 deletion(-)

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<T:RealField, const D: usize> Rotation<T,D> 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<T:RealField, const D: usize> Rotation<T,D> 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<T:RealField, const D: usize> Rotation<T,D> 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<T> = Rotation<T,4>;
+
+            //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)
+            );
+
+
+        }
+
+
+
     }
 }