Perturbations to check for convergence into maximum.

src/geometry/rotation_specialization.rs

@@ -17,7 +17,7 @@ use std::ops::Neg;
 
 use crate::base::dimension::{U1, U2, U3};
 use crate::base::storage::Storage;
-use crate::base::{Matrix2, Matrix3, SMatrix, SVector, Unit, Vector, Vector1, Vector2, Vector3};
+use crate::base::{Matrix2, Matrix3, SMatrix, SVector, Unit, Vector, Vector1, Vector2, Vector3, UnitVector3};
 
 use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion};
 
@@ -706,15 +706,12 @@ 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 + crate::Scalar,
+        T: RealField,
-        Standard: Distribution<Rotation3<T>>,
     {
         // 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, Rotation3::identity())
-        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`.
@@ -737,6 +734,7 @@ where
             max_iter = usize::MAX;
         }
 
+        let mut perturbation_axes = UnitVector3::new_unchecked(Vector3::new(T::one(), T::zero(), T::zero()));
         let mut rot = guess.into_inner();
 
         for _ in 0..max_iter {
@@ -752,9 +750,27 @@ where
             if let Some((axis, angle)) = Unit::try_new_and_get(axisangle, eps.clone()) {
                 rot = Rotation3::from_axis_angle(&axis, angle) * rot;
             } else {
+                // Check if stuck in a maximum w.r.t. the norm (m - rot).norm()
+                let mut perturbed = rot.clone();
+                let norm_squared = (m - &rot).norm_squared();
+                let mut new_norm_squared: T;
+                // Perturb until the new norm is significantly different
+                loop {
+                    perturbed *= Rotation3::from_axis_angle(&perturbation_axes, T::frac_pi_8());
+                    new_norm_squared = (m - &perturbed).norm_squared();
+                    if relative_ne!(norm_squared, new_norm_squared) {
                         break;
                     }
                 }
+                // If new norm is larger, it's a minimum
+                if norm_squared < new_norm_squared {
+                    break;
+                }
+                // If not, continue from perturbed rotation, but use a different axes for the next perturbation
+                perturbation_axes = UnitVector3::new_unchecked(Vector3::new(perturbation_axes.y.clone(), perturbation_axes.z.clone(), perturbation_axes.x.clone()));
+                rot = perturbed;
+            }
+        }
 
         Self::from_matrix_unchecked(rot)
     }