Numerically stable implementation of axis, angle and scaled_axis for Rotation3 and UnitQuaternion

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.

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::<f32>::zeros()), Rotation3::identity());
     /// ```
     pub fn new<SB: Storage<T, U3>>(axisangle: Vector<T, U3, SB>) -> 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<T: SimdRealField> Rotation3<T> {
     /// ```
     #[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<T: SimdRealField> Rotation3<T> {
     where
         T: RealField,
     {
-        let rotmat = self.matrix();
-        let axis = SVector::<T, 3>::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<T: SimdRealField> Rotation3<T> {
     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<T: SimdRealField> Rotation3<T> {
     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<T: SimdRealField> Rotation3<T> {
     #[must_use]
     pub fn angle_to(&self, other: &Self) -> T
     where
-        T::Element: SimdRealField,
+        T: RealField,
     {
         self.rotation_to(other).angle()
     }

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);
+}

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);
+}