Merge pull request #1101 from timethy/dev
Fix from_matrix when argument is already a valid rotation matrix
This commit is contained in:
commit
1e9e1ba46d
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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(
|
||||
|
|
Loading…
Reference in New Issue