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
/// 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,
T: RealField + Scalar,
Standard: Distribution<Rotation3<T>>,
{
Rotation3::from_matrix(m).into()
}

View File

@ -706,11 +706,15 @@ 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,
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`.
@ -730,7 +734,7 @@ where
T: RealField,
{
if max_iter == 0 {
max_iter = usize::max_value();
max_iter = usize::MAX;
}
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]
fn angle_2() {
@ -16,6 +17,19 @@ fn angle_3() {
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]
fn quaternion_euler_angles_issue_494() {
let quat = UnitQuaternion::from_quaternion(Quaternion::new(