From f9aa2d76aab6ed1dc2c31f485f20d6557cc86233 Mon Sep 17 00:00:00 2001
From: Tim Taubner <tt@daedalean.ai>
Date: Thu, 31 Mar 2022 17:28:31 +0200
Subject: [PATCH] Start from random rotation in from_matrix to prevent issues
 when calling from_matrix on rotation matrices

---
 src/geometry/quaternion_construction.rs |  4 +++-
 src/geometry/rotation_specialization.rs | 10 +++++++---
 tests/geometry/rotation.rs              | 16 +++++++++++++++-
 3 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/src/geometry/quaternion_construction.rs b/src/geometry/quaternion_construction.rs
index 6de21bd5..dbd1edbc 100644
--- a/src/geometry/quaternion_construction.rs
+++ b/src/geometry/quaternion_construction.rs
@@ -410,9 +410,11 @@ 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,
+        T: RealField + Scalar,
+        Standard: Distribution<Rotation3<T>>,
     {
         Rotation3::from_matrix(m).into()
     }
diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs
index 41405c87..b57eeb53 100644
--- a/src/geometry/rotation_specialization.rs
+++ b/src/geometry/rotation_specialization.rs
@@ -706,11 +706,15 @@ 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,
+        T: RealField + crate::Scalar,
+        Standard: Distribution<Rotation3<T>>,
     {
-        Self::from_matrix_eps(m, T::default_epsilon(), 0, Self::identity())
+        // Starting from a random rotation has almost zero likelihood to end up in a maximum if `m` is already a rotation matrix
+        let random_rotation: Rotation3<T> = rand::thread_rng().gen();
+        Self::from_matrix_eps(m, T::default_epsilon(), 0, random_rotation)
     }
 
     /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
@@ -730,7 +734,7 @@ where
         T: RealField,
     {
         if max_iter == 0 {
-            max_iter = usize::max_value();
+            max_iter = usize::MAX;
         }
 
         let mut rot = guess.into_inner();
diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs
index 84bba676..61af83d9 100644
--- a/tests/geometry/rotation.rs
+++ b/tests/geometry/rotation.rs
@@ -1,4 +1,5 @@
-use na::{Quaternion, RealField, UnitQuaternion, Vector2, Vector3};
+use std::f64::consts::PI;
+use na::{Matrix3, Quaternion, RealField, Rotation3, UnitQuaternion, UnitVector3, Vector2, Vector3};
 
 #[test]
 fn angle_2() {
@@ -16,6 +17,19 @@ fn angle_3() {
     assert_eq!(a.angle(&b), 0.0);
 }
 
+#[test]
+fn from_rotation_matrix() {
+    // Test degenerate case when from_matrix_eps gets stuck in maximum
+    let identity = Rotation3::from_matrix(&Matrix3::new(
+        1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0,
+    ));
+    assert_relative_eq!(identity, &Rotation3::identity(), epsilon = 0.001);
+    let rotated_z = Rotation3::from_matrix(&Matrix3::new(
+        1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, -1.0,
+    ));
+    assert_relative_eq!(rotated_z, &Rotation3::from_axis_angle(&UnitVector3::new_unchecked(Vector3::new(1.0, 0.0, 0.0)), PI), epsilon = 0.001);
+}
+
 #[test]
 fn quaternion_euler_angles_issue_494() {
     let quat = UnitQuaternion::from_quaternion(Quaternion::new(