From 440fc74785fbbeb6a0a42c3e482e4df0e8831ed3 Mon Sep 17 00:00:00 2001 From: Nikolay Mayorov Date: Sat, 28 Mar 2026 03:41:44 +0300 Subject: [PATCH 1/6] ENH: Robust non-singular implementation of Quaternion::exp and UnitQuaternion::scaled_axis --- src/geometry/quaternion.rs | 31 ++++++++++++++++++++++++------- tests/geometry/quaternion.rs | 9 ++++++++- 2 files changed, 32 insertions(+), 8 deletions(-) diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs index 6c36d4eed..ff4ef3d7d 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 @@ -968,8 +974,8 @@ impl> AbsDiffEq for Quaternion { #[inline] fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { self.as_vector().abs_diff_eq(other.as_vector(), epsilon.clone()) || - // Account for the double-covering of S², i.e. q = -q - self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.abs_diff_eq(&-b.clone(), epsilon.clone())) + // Account for the double-covering of S², i.e. q = -q + self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.abs_diff_eq(&-b.clone(), epsilon.clone())) } } @@ -1323,10 +1329,21 @@ where where T: RealField, { - match self.axis() { - Some(axis) => axis.into_inner() * self.angle(), - None => Vector3::zero(), - } + let angle = self.angle(); + let scaling = if angle < crate::convert(1.0e-3) { + let a0: T = crate::convert(2.0); + let a2: T = crate::convert(1.0 / 12.0); + let a4: T = crate::convert(7.0 / 2880.0); + a0 + a2 * angle.clone().simd_powi(2) + a4 * angle.simd_powi(4) + } else { + angle.clone() / (crate::convert::(0.5) * angle).simd_sin() + }; + let vec = if self.scalar() >= T::zero() { + self.imag() + } else { + -self.imag() + }; + vec * scaling } /// The rotation axis and angle in (0, pi] of this unit quaternion. diff --git a/tests/geometry/quaternion.rs b/tests/geometry/quaternion.rs index ff7eff6cb..aacc9e9e7 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,10 @@ proptest!( && uqMuv == &uq * uv) } ); + +#[test] +fn unit_quaternion_small_scaled_axis() { + 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, epsilon = 1.0e-10); +} From 1946d0d89ba992a0ab341a20796be77a5e6815f7 Mon Sep 17 00:00:00 2001 From: Nikolay Mayorov Date: Sat, 28 Mar 2026 12:17:09 +0300 Subject: [PATCH 2/6] ENH: Robust implementations of Rotation::{angle, axis, scaled_axis} --- src/geometry/rotation_specialization.rs | 38 ++++++++----------------- tests/geometry/rotation.rs | 18 ++++++++++-- 2 files changed, 27 insertions(+), 29 deletions(-) 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/rotation.rs b/tests/geometry/rotation.rs index f461558aa..5f01cb751 100644 --- a/tests/geometry/rotation.rs +++ b/tests/geometry/rotation.rs @@ -1,6 +1,4 @@ -use na::{ - Matrix3, Quaternion, RealField, Rotation3, UnitQuaternion, UnitVector3, Vector2, Vector3, -}; +use na::{Matrix3, Quaternion, RealField, Rotation3, Unit, UnitQuaternion, UnitVector3, Vector2, Vector3}; use std::f64::consts::PI; #[test] @@ -357,3 +355,17 @@ 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); +} From b8582218a86b9bdee80c82c2884597fa6e952659 Mon Sep 17 00:00:00 2001 From: Nikolay Mayorov Date: Sat, 28 Mar 2026 12:23:47 +0300 Subject: [PATCH 3/6] ENH: Cleaner implementation of UnitQuaternion::scaled_axis --- src/geometry/quaternion.rs | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs index ff4ef3d7d..b1a641a01 100644 --- a/src/geometry/quaternion.rs +++ b/src/geometry/quaternion.rs @@ -1330,20 +1330,13 @@ where T: RealField, { let angle = self.angle(); - let scaling = if angle < crate::convert(1.0e-3) { - let a0: T = crate::convert(2.0); - let a2: T = crate::convert(1.0 / 12.0); - let a4: T = crate::convert(7.0 / 2880.0); - a0 + a2 * angle.clone().simd_powi(2) + a4 * angle.simd_powi(4) - } else { - angle.clone() / (crate::convert::(0.5) * angle).simd_sin() - }; let vec = if self.scalar() >= T::zero() { self.imag() } else { -self.imag() }; - vec * scaling + 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. From d42eb79a5fb194ca182c91ac52330c7c923136f4 Mon Sep 17 00:00:00 2001 From: Nikolay Mayorov Date: Sat, 28 Mar 2026 12:33:47 +0300 Subject: [PATCH 4/6] MNT: Rollback unnecessary diffs --- src/geometry/quaternion.rs | 4 ++-- tests/geometry/rotation.rs | 4 +++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs index b1a641a01..5177394b5 100644 --- a/src/geometry/quaternion.rs +++ b/src/geometry/quaternion.rs @@ -974,8 +974,8 @@ impl> AbsDiffEq for Quaternion { #[inline] fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { self.as_vector().abs_diff_eq(other.as_vector(), epsilon.clone()) || - // Account for the double-covering of S², i.e. q = -q - self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.abs_diff_eq(&-b.clone(), epsilon.clone())) + // Account for the double-covering of S², i.e. q = -q + self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.abs_diff_eq(&-b.clone(), epsilon.clone())) } } diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs index 5f01cb751..5648fbd99 100644 --- a/tests/geometry/rotation.rs +++ b/tests/geometry/rotation.rs @@ -1,4 +1,6 @@ -use na::{Matrix3, Quaternion, RealField, Rotation3, Unit, UnitQuaternion, UnitVector3, Vector2, Vector3}; +use na::{ + Matrix3, Quaternion, RealField, Rotation3, UnitQuaternion, UnitVector3, Vector2, Vector3, Unit +}; use std::f64::consts::PI; #[test] From 938310cfbd3a28782400a8d3ceca0673435b3d8e Mon Sep 17 00:00:00 2001 From: Nikolay Mayorov Date: Sat, 28 Mar 2026 13:13:48 +0300 Subject: [PATCH 5/6] TST: Proper test on unit quaterion from small scaled axis --- tests/geometry/quaternion.rs | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/tests/geometry/quaternion.rs b/tests/geometry/quaternion.rs index aacc9e9e7..27c79a2a9 100644 --- a/tests/geometry/quaternion.rs +++ b/tests/geometry/quaternion.rs @@ -266,8 +266,14 @@ proptest!( ); #[test] -fn unit_quaternion_small_scaled_axis() { +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, epsilon = 1.0e-10); + 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); } From 3cae1ddd241c9c394a6e70fe1593124d45a0a68b Mon Sep 17 00:00:00 2001 From: Nikolay Mayorov Date: Sun, 5 Apr 2026 12:38:56 +0300 Subject: [PATCH 6/6] TST: Test angle computation from Rotation3 for tiny rotation computed as multiplication --- tests/geometry/rotation.rs | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs index 5648fbd99..6a4262ac3 100644 --- a/tests/geometry/rotation.rs +++ b/tests/geometry/rotation.rs @@ -371,3 +371,20 @@ fn get_axis_for_pi_angle_rotation_issue_1382() { 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); +}