diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs index 6c36d4eed..5177394b5 100644 --- a/src/geometry/quaternion.rs +++ b/src/geometry/quaternion.rs @@ -516,7 +516,13 @@ where #[inline] #[must_use] pub fn exp(&self) -> Self { - self.exp_eps(T::simd_default_epsilon()) + let v = self.vector(); + let v_norm = v.norm(); + let exp = self.scalar().simd_exp(); + Self::from_parts( + exp.clone() * v_norm.clone().simd_cos(), + v * exp * v_norm.simd_sinc(), + ) } /// Compute the exponential of a quaternion. Returns the identity if the vector part of this quaternion @@ -1323,10 +1329,14 @@ where where T: RealField, { - match self.axis() { - Some(axis) => axis.into_inner() * self.angle(), - None => Vector3::zero(), - } + let angle = self.angle(); + let vec = if self.scalar() >= T::zero() { + self.imag() + } else { + -self.imag() + }; + let two: T = crate::convert(2.0); + vec * two.clone() / (angle / two).simd_sinc() } /// The rotation axis and angle in (0, pi] of this unit quaternion. diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index a9a8aec1b..d153ff694 100644 --- a/src/geometry/rotation_specialization.rs +++ b/src/geometry/rotation_specialization.rs @@ -3,8 +3,6 @@ use crate::base::storage::Owned; #[cfg(feature = "arbitrary")] use quickcheck::{Arbitrary, Gen}; -use num::Zero; - #[cfg(feature = "rand-no-std")] use rand::{ Rng, @@ -332,9 +330,7 @@ where /// assert_eq!(Rotation3::new(Vector3::::zeros()), Rotation3::identity()); /// ``` pub fn new>(axisangle: Vector) -> Self { - let axisangle = axisangle.into_owned(); - let (axis, angle) = Unit::new_and_get(axisangle); - Self::from_axis_angle(&axis, angle) + UnitQuaternion::new(axisangle).to_rotation_matrix() } /// Builds a 3D rotation matrix from an axis scaled by the rotation angle. @@ -823,13 +819,11 @@ impl Rotation3 { /// ``` #[inline] #[must_use] - pub fn angle(&self) -> T { - ((self.matrix()[(0, 0)].clone() - + self.matrix()[(1, 1)].clone() - + self.matrix()[(2, 2)].clone() - - T::one()) - / crate::convert(2.0)) - .simd_acos() + pub fn angle(&self) -> T + where + T: RealField, + { + UnitQuaternion::from_rotation_matrix(&self).angle() } /// The rotation axis. Returns `None` if the rotation angle is zero or PI. @@ -853,14 +847,7 @@ impl Rotation3 { where T: RealField, { - let rotmat = self.matrix(); - let axis = SVector::::new( - rotmat[(2, 1)].clone() - rotmat[(1, 2)].clone(), - rotmat[(0, 2)].clone() - rotmat[(2, 0)].clone(), - rotmat[(1, 0)].clone() - rotmat[(0, 1)].clone(), - ); - - Unit::try_new(axis, T::default_epsilon()) + Unit::try_new(self.scaled_axis(), T::default_epsilon()) } /// The rotation axis multiplied by the rotation angle. @@ -879,10 +866,7 @@ impl Rotation3 { where T: RealField, { - match self.axis() { - Some(axis) => axis.into_inner() * self.angle(), - None => Vector::zero(), - } + UnitQuaternion::from_rotation_matrix(&self).scaled_axis() } /// The rotation axis and angle in (0, pi] of this rotation matrix. @@ -910,7 +894,9 @@ impl Rotation3 { where T: RealField, { - self.axis().map(|axis| (axis, self.angle())) + let scaled_axis = self.scaled_axis(); + let axis = Unit::try_new(scaled_axis.clone(), T::default_epsilon()); + axis.map(|axis| (axis, scaled_axis.norm())) } /// The rotation angle needed to make `self` and `other` coincide. @@ -927,7 +913,7 @@ impl Rotation3 { #[must_use] pub fn angle_to(&self, other: &Self) -> T where - T::Element: SimdRealField, + T: RealField, { self.rotation_to(other).angle() } diff --git a/tests/geometry/quaternion.rs b/tests/geometry/quaternion.rs index ff7eff6cb..27c79a2a9 100644 --- a/tests/geometry/quaternion.rs +++ b/tests/geometry/quaternion.rs @@ -1,7 +1,7 @@ #![cfg(feature = "proptest-support")] #![allow(non_snake_case)] -use na::{Unit, UnitQuaternion}; +use na::{Unit, UnitQuaternion, Vector3}; use crate::proptest::*; use proptest::{prop_assert, proptest}; @@ -264,3 +264,16 @@ proptest!( && uqMuv == &uq * uv) } ); + +#[test] +fn unit_quaternion_from_small_scaled_axis_computations_stability() { + let scaled_axis = Vector3::new(5.0e-20, 1e-16, -1.0e-17); + let q = UnitQuaternion::new(scaled_axis); + assert_relative_eq!(q.scaled_axis(), scaled_axis, max_relative = 1e-16, epsilon = 0.0); + assert_relative_eq!(q.angle(), scaled_axis.norm(), max_relative = 1e-16, epsilon = 0.0); + + let (roll, pitch, yaw) = q.euler_angles(); + assert_relative_eq!(roll, scaled_axis.x, max_relative=1e-14, epsilon = 0.0); + assert_relative_eq!(pitch, scaled_axis.y, max_relative=1e-14, epsilon = 0.0); + assert_relative_eq!(yaw, scaled_axis.z, max_relative=1e-14, epsilon = 0.0); +} diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs index f461558aa..6a4262ac3 100644 --- a/tests/geometry/rotation.rs +++ b/tests/geometry/rotation.rs @@ -1,5 +1,5 @@ use na::{ - Matrix3, Quaternion, RealField, Rotation3, UnitQuaternion, UnitVector3, Vector2, Vector3, + Matrix3, Quaternion, RealField, Rotation3, UnitQuaternion, UnitVector3, Vector2, Vector3, Unit }; use std::f64::consts::PI; @@ -357,3 +357,34 @@ mod proptest_tests { } } + +#[test] +fn small_scale_axis_no_precision_loss() { + let scaled_axis = Vector3::new(-1.0e-25, 1.0e-16, 2.0e-18); + let r = Rotation3::new(scaled_axis); + assert_relative_eq!(r.scaled_axis(), scaled_axis, max_relative = 1e-6, epsilon = 1e-30); +} + +#[test] +fn get_axis_for_pi_angle_rotation_issue_1382() { + let axis = Unit::new_normalize(Vector3::new(1., 2., 1.)); + let r = Rotation3::from_axis_angle(&axis, PI); + assert_relative_eq!(r.axis().unwrap(), axis, max_relative=1.0e-16); +} + +#[test] +fn angle_for_tiny_rotation_computed_as_multiplication_is_not_nan() { + let roll = 0.8448984061042941; + let pitch = -1.4629885349276224; + let yaw = -2.6163871512855312; + let r1 = Rotation3::from_euler_angles(roll, pitch, yaw); + let r2 = Rotation3::from_euler_angles( + roll * (1.0 + 1e-16), + pitch * (1.0 - 2e-16), + yaw * (1.0 + 1e-16), + ); + let r = r1 * r2.inverse(); + let angle: f64 = r.angle(); + assert!(!angle.is_nan()); + assert!(angle < 1e-15); +}