Merge pull request #1101 from timethy/dev

Fix from_matrix when argument is already a valid rotation matrix
This commit is contained in:
Sébastien Crozet 2022-07-27 22:22:27 +02:00 committed by GitHub
commit 1e9e1ba46d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 90 additions and 4 deletions

View File

@ -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;
}
}

View File

@ -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(