diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index b57eeb53..304f5ee5 100644 --- a/src/geometry/rotation_specialization.rs +++ b/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) -> Self where - T: RealField + crate::Scalar, - Standard: Distribution>, + T: RealField, { // 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 = rand::thread_rng().gen(); - Self::from_matrix_eps(m, T::default_epsilon(), 0, random_rotation) + Self::from_matrix_eps(m, T::default_epsilon(), 0, Rotation3::identity()) } /// 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,7 +750,25 @@ where if let Some((axis, angle)) = Unit::try_new_and_get(axisangle, eps.clone()) { rot = Rotation3::from_axis_angle(&axis, angle) * rot; } else { - break; + // 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; } }