Merge pull request #1101 from timethy/dev

Fix from_matrix when argument is already a valid rotation matrix

src/geometry/rotation_specialization.rs

@@ -17,7 +17,9 @@ 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, UnitVector3, Vector, Vector1, Vector2, Vector3,
+};
 
 use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion};
 
@@ -730,9 +732,12 @@ where
         T: RealField,
     {
         if max_iter == 0 {
-            max_iter = usize::max_value();
+            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();
 
         for _ in 0..max_iter {
@@ -748,7 +753,33 @@ 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, eps_disturbance.clone());
+                    new_norm_squared = (m - &perturbed).norm_squared();
+                    if abs_diff_ne!(
+                        norm_squared,
+                        new_norm_squared,
+                        epsilon = T::default_epsilon()
+                    ) {
+                        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(perturbation_axes.yzx());
+                rot = perturbed;
             }
         }
 

tests/geometry/rotation.rs

@@ -1,4 +1,7 @@
-use na::{Quaternion, RealField, UnitQuaternion, Vector2, Vector3};
+use na::{
+    Matrix3, Quaternion, RealField, Rotation3, UnitQuaternion, UnitVector3, Vector2, Vector3,
+};
+use std::f64::consts::PI;
 
 #[test]
 fn angle_2() {
@@ -16,6 +19,58 @@ fn angle_3() {
     assert_eq!(a.angle(&b), 0.0);
 }
 
+#[test]
+fn from_rotation_matrix() {
+    // Test degenerate case when from_matrix gets stuck in Identity rotation
+    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 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, Rotation3::from_matrix(&m_627), epsilon = 0.01);
+    assert_relative_eq!(
+        Rotation3::from_matrix_unchecked(m_627.clone()),
+        Rotation3::from_matrix(&m_627),
+        epsilon = 0.001
+    );
+    // Test that issue 1078 is fixed
+    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, Rotation3::from_matrix(&m_1078), epsilon = 0.01);
+    assert_relative_eq!(
+        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]
 fn quaternion_euler_angles_issue_494() {
     let quat = UnitQuaternion::from_quaternion(Quaternion::new(