Start from random rotation in from_matrix to prevent issues when calling from_matrix on rotation matrices

This commit is contained in:
Tim Taubner 2022-03-31 17:28:31 +02:00 committed by Sébastien Crozet
parent 7b0c9d64a0
commit f9aa2d76aa
3 changed files with 25 additions and 5 deletions

View File

@ -410,9 +410,11 @@ where
/// This is an iterative method. See `.from_matrix_eps` to provide mover /// This is an iterative method. See `.from_matrix_eps` to provide mover
/// convergence parameters and starting solution. /// convergence parameters and starting solution.
/// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al. /// 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 pub fn from_matrix(m: &Matrix3<T>) -> Self
where where
T: RealField, T: RealField + Scalar,
Standard: Distribution<Rotation3<T>>,
{ {
Rotation3::from_matrix(m).into() Rotation3::from_matrix(m).into()
} }

View File

@ -706,11 +706,15 @@ where
/// This is an iterative method. See `.from_matrix_eps` to provide mover /// This is an iterative method. See `.from_matrix_eps` to provide mover
/// convergence parameters and starting solution. /// convergence parameters and starting solution.
/// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al. /// 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 pub fn from_matrix(m: &Matrix3<T>) -> Self
where 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`. /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
@ -730,7 +734,7 @@ where
T: RealField, T: RealField,
{ {
if max_iter == 0 { if max_iter == 0 {
max_iter = usize::max_value(); max_iter = usize::MAX;
} }
let mut rot = guess.into_inner(); let mut rot = guess.into_inner();

View File

@ -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] #[test]
fn angle_2() { fn angle_2() {
@ -16,6 +17,19 @@ fn angle_3() {
assert_eq!(a.angle(&b), 0.0); 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] #[test]
fn quaternion_euler_angles_issue_494() { fn quaternion_euler_angles_issue_494() {
let quat = UnitQuaternion::from_quaternion(Quaternion::new( let quat = UnitQuaternion::from_quaternion(Quaternion::new(