From_rotation_matrix: Use the larger of eps.sqrt() or eps*eps as disturbance. Add tests for eps > 1.0
This commit is contained in:
parent
18a8a30671
commit
7aadbcf21d
|
@ -410,7 +410,6 @@ 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,
|
||||
|
|
|
@ -735,6 +735,8 @@ where
|
|||
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();
|
||||
|
||||
|
@ -758,10 +760,9 @@ where
|
|||
|
||||
// Perturb until the new norm is significantly different
|
||||
loop {
|
||||
perturbed *= Rotation3::from_axis_angle(&perturbation_axes, T::frac_pi_8());
|
||||
perturbed *= Rotation3::from_axis_angle(&perturbation_axes, eps_disturbance.clone());
|
||||
new_norm_squared = (m - &perturbed).norm_squared();
|
||||
|
||||
if relative_ne!(norm_squared, new_norm_squared) {
|
||||
if abs_diff_ne!(norm_squared, new_norm_squared, epsilon = T::default_epsilon()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -32,31 +32,51 @@ fn from_rotation_matrix() {
|
|||
&Rotation3::from_axis_angle(&UnitVector3::new_unchecked(Vector3::new(1.0, 0.0, 0.0)), PI),
|
||||
epsilon = 0.001
|
||||
);
|
||||
// Test that issue 628 is fixed
|
||||
let m_628 = nalgebra::Matrix3::<f64>::new(-1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0);
|
||||
// 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,
|
||||
nalgebra::Rotation3::from_matrix(&m_628),
|
||||
Rotation3::from_matrix(&m_627),
|
||||
epsilon = 0.01
|
||||
);
|
||||
assert_relative_eq!(
|
||||
nalgebra::Rotation3::from_matrix_unchecked(m_628.clone()),
|
||||
nalgebra::Rotation3::from_matrix(&m_628),
|
||||
Rotation3::from_matrix_unchecked(m_627.clone()),
|
||||
Rotation3::from_matrix(&m_627),
|
||||
epsilon = 0.001
|
||||
);
|
||||
|
||||
// Test that issue 1078 is fixed
|
||||
let m_1078 = nalgebra::Matrix3::<f64>::new(0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0);
|
||||
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,
|
||||
nalgebra::Rotation3::from_matrix(&m_1078),
|
||||
Rotation3::from_matrix(&m_1078),
|
||||
epsilon = 0.01
|
||||
);
|
||||
assert_relative_eq!(
|
||||
nalgebra::Rotation3::from_matrix_unchecked(m_1078.clone()),
|
||||
nalgebra::Rotation3::from_matrix(&m_1078),
|
||||
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]
|
||||
|
|
Loading…
Reference in New Issue