From 7aadbcf21d2feafbe282fcdcc6b6469bf60dc859 Mon Sep 17 00:00:00 2001
From: Tim Taubner <tt@daedalean.ai>
Date: Wed, 27 Jul 2022 11:31:34 +0200
Subject: [PATCH] From_rotation_matrix: Use the larger of eps.sqrt() or eps*eps
 as disturbance. Add tests for eps > 1.0

---
 src/geometry/quaternion_construction.rs |  1 -
 src/geometry/rotation_specialization.rs |  7 +++--
 tests/geometry/rotation.rs              | 40 ++++++++++++++++++-------
 3 files changed, 34 insertions(+), 14 deletions(-)

diff --git a/src/geometry/quaternion_construction.rs b/src/geometry/quaternion_construction.rs
index 18f7a65c..6de21bd5 100644
--- a/src/geometry/quaternion_construction.rs
+++ b/src/geometry/quaternion_construction.rs
@@ -410,7 +410,6 @@ where
     /// This is an iterative method. See `.from_matrix_eps` to provide mover
     /// convergence parameters and starting solution.
     /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
-    #[cfg(feature = "rand-no-std")]
     pub fn from_matrix(m: &Matrix3<T>) -> Self
     where
         T: RealField,
diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs
index f00d8cd7..fc2c9b93 100644
--- a/src/geometry/rotation_specialization.rs
+++ b/src/geometry/rotation_specialization.rs
@@ -735,6 +735,8 @@ where
             max_iter = usize::MAX;
         }
 
+        // Using sqrt(eps) ensures we perturb with something larger than eps; clamp to eps to handle the case of eps > 1.0
+        let eps_disturbance = eps.clone().sqrt().max(eps.clone() * eps.clone());
         let mut perturbation_axes = Vector3::x_axis();
         let mut rot = guess.into_inner();
 
@@ -758,10 +760,9 @@ where
 
                 // Perturb until the new norm is significantly different
                 loop {
-                    perturbed *= Rotation3::from_axis_angle(&perturbation_axes, T::frac_pi_8());
+                    perturbed *= Rotation3::from_axis_angle(&perturbation_axes, eps_disturbance.clone());
                     new_norm_squared = (m - &perturbed).norm_squared();
-
-                    if relative_ne!(norm_squared, new_norm_squared) {
+                    if abs_diff_ne!(norm_squared, new_norm_squared, epsilon = T::default_epsilon()) {
                         break;
                     }
                 }
diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs
index 883d4c0b..f5a77b54 100644
--- a/tests/geometry/rotation.rs
+++ b/tests/geometry/rotation.rs
@@ -32,31 +32,51 @@ fn from_rotation_matrix() {
         &Rotation3::from_axis_angle(&UnitVector3::new_unchecked(Vector3::new(1.0, 0.0, 0.0)), PI),
         epsilon = 0.001
     );
-    // Test that issue 628 is fixed
-    let m_628 = nalgebra::Matrix3::<f64>::new(-1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0);
+    // Test that issue 627 is fixed
+    let m_627 = Matrix3::<f64>::new(-1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0);
     assert_relative_ne!(
         identity,
-        nalgebra::Rotation3::from_matrix(&m_628),
+        Rotation3::from_matrix(&m_627),
         epsilon = 0.01
     );
     assert_relative_eq!(
-        nalgebra::Rotation3::from_matrix_unchecked(m_628.clone()),
-        nalgebra::Rotation3::from_matrix(&m_628),
+        Rotation3::from_matrix_unchecked(m_627.clone()),
+        Rotation3::from_matrix(&m_627),
         epsilon = 0.001
     );
-
     // Test that issue 1078 is fixed
-    let m_1078 = nalgebra::Matrix3::<f64>::new(0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0);
+    let m_1078 = Matrix3::<f64>::new(0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0);
     assert_relative_ne!(
         identity,
-        nalgebra::Rotation3::from_matrix(&m_1078),
+        Rotation3::from_matrix(&m_1078),
         epsilon = 0.01
     );
     assert_relative_eq!(
-        nalgebra::Rotation3::from_matrix_unchecked(m_1078.clone()),
-        nalgebra::Rotation3::from_matrix(&m_1078),
+        Rotation3::from_matrix_unchecked(m_1078.clone()),
+        Rotation3::from_matrix(&m_1078),
         epsilon = 0.001
     );
+    // Additional test cases for eps >= 1.0
+    assert_relative_ne!(
+        identity,
+        Rotation3::from_matrix_eps(&m_627, 1.2, 0, Rotation3::identity()),
+        epsilon = 0.6
+    );
+    assert_relative_eq!(
+        Rotation3::from_matrix_unchecked(m_627.clone()),
+        Rotation3::from_matrix_eps(&m_627, 1.2, 0, Rotation3::identity()),
+        epsilon = 0.6
+    );
+    assert_relative_ne!(
+        identity,
+        Rotation3::from_matrix_eps(&m_1078, 1.0, 0, Rotation3::identity()),
+        epsilon = 0.1
+    );
+    assert_relative_eq!(
+        Rotation3::from_matrix_unchecked(m_1078.clone()),
+        Rotation3::from_matrix_eps(&m_1078, 1.0, 0, Rotation3::identity()),
+        epsilon = 0.1
+    );
 }
 
 #[test]