From 6eab8f5175a57b29ce5091e88f6fb3f6b6735a8d Mon Sep 17 00:00:00 2001
From: Joshua Smith <jsmith62831@gmail.com>
Date: Mon, 28 Mar 2022 16:07:11 -0500
Subject: [PATCH] added considerations for 180deg rotations in Rotation::powf

---
 src/geometry/rotation_specialization.rs | 26 +++++---
 tests/geometry/rotation.rs              | 79 +++++++++++++++++++++----
 2 files changed, 84 insertions(+), 21 deletions(-)

diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs
index 07e5189a..d3104c36 100644
--- a/src/geometry/rotation_specialization.rs
+++ b/src/geometry/rotation_specialization.rs
@@ -1065,19 +1065,21 @@ impl<T:RealField, const D: usize> Rotation<T,D>
         // println!("q:{}d:{:.3}", q, d);
 
         //go down the diagonal and pow every block
-        for i in 0..(D-1) {
+        let mut i = 0;
+        while i < 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() {
+            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() ||
 
-                // println!("{}", i);
+                //for +-180 deg rotations
+                d[(i,i)]<T::zero() && d[(i+1,i+1)]<T::zero()
+            {
 
-                //convert to a complex num and take the arg()
+                //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);
-
-                // println!("{}", angle);
+                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();
@@ -1089,6 +1091,12 @@ impl<T:RealField, const D: usize> Rotation<T,D>
                 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;
             }
 
         }
diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs
index 7c14c4bc..a823418d 100644
--- a/tests/geometry/rotation.rs
+++ b/tests/geometry/rotation.rs
@@ -39,30 +39,41 @@ mod proptest_tests {
     use crate::proptest::*;
     use proptest::{prop_assert, prop_assert_eq, proptest};
 
+    //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($($v1:ident in $vec1:ident(), $v2:ident in $vec2:ident()),*);
+            fn $powf_rot_n:ident($($v:ident in $vec:ident()),*);
         )*) => {
             proptest!{$(
 
                 #[test]
                 fn $powf_rot_n(
-                    $($v1 in $vec1(), $v2 in $vec2(),)*
+                    $($v in $vec(),)*
                     pow in PROPTEST_F64
                 ) {
 
                     use nalgebra::*;
 
-                    //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.
-                    let mut bivectors = [
-                        //Since we start with vector pairs, each bivector is guaranteed to be simple
-                        $($v1.transpose().kronecker(&$v2) - $v2.transpose().kronecker(&$v1)),*
-                    ];
+                    let mut bivectors = gen_rotation_planes!($($v),*);
 
                     //condition the bivectors
                     for b in &mut bivectors {
@@ -88,7 +99,7 @@ mod proptest_tests {
                 }
 
             )*}
-        }
+        };
     }
 
     gen_powf_rotation_test!(
@@ -101,6 +112,50 @@ mod proptest_tests {
         );
     );
 
+    proptest! {
+
+        #[test]
+        fn powf_180deg_rotation_4d(v1 in vector4(), v2 in vector4(), v3 in vector4(), v4 in vector4()) {
+
+            use nalgebra::*;
+            use std::f64::consts::PI;
+
+            let [b1,b2] = gen_rotation_planes!(v1,v2,v3,v4);
+
+            if let (Some((b1,a1)), Some((b2,a2))) = (
+                Unit::try_new_and_get(b1,0.0), Unit::try_new_and_get(b2,0.0)
+            ) {
+
+                let (b1, b2) = (b1.into_inner(), b2.into_inner());
+                {
+                    let b = a1*b1 + PI*b2;
+                    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));
+                }
+
+                {
+                    let b = PI*b1 + a2*b2;
+                    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));
+                }
+
+                {
+                    let b = PI*b1 + PI*b2;
+                    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));
+                }
+
+            }
+
+
+
+        }
+
+    }
+
     proptest! {
         /*
          *