diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index 41405c87..c9197fd6 100644 --- a/src/geometry/rotation_specialization.rs +++ b/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; } } diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs index 84bba676..2e8d2482 100644 --- a/tests/geometry/rotation.rs +++ b/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::::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::::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(