From 1e614db22744b77f982728c11685b85797727173 Mon Sep 17 00:00:00 2001 From: adamnemecek Date: Mon, 18 Mar 2019 01:08:42 -0700 Subject: [PATCH] Quaternionic division + refactoring (#563) --- src/base/unit.rs | 2 +- src/geometry/quaternion.rs | 49 ++++++++++++++++--------- src/geometry/quaternion_construction.rs | 8 +++- src/geometry/quaternion_ops.rs | 4 +- src/linalg/inverse.rs | 6 +-- 5 files changed, 45 insertions(+), 24 deletions(-) diff --git a/src/base/unit.rs b/src/base/unit.rs index 3d0a9c03..20f15956 100644 --- a/src/base/unit.rs +++ b/src/base/unit.rs @@ -225,7 +225,7 @@ impl Neg for Unit { #[inline] fn neg(self) -> Self::Output { - Unit::new_unchecked(-self.value) + Self::Output::new_unchecked(-self.value) } } diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs index 8a56412b..cb7d252c 100755 --- a/src/geometry/quaternion.rs +++ b/src/geometry/quaternion.rs @@ -528,7 +528,7 @@ impl Quaternion { /// Check if the quaternion is pure. #[inline] pub fn is_pure(&self) -> bool { - self.w == N::zero() + self.w.is_zero() } /// Convert quaternion to pure quaternion. @@ -537,6 +537,33 @@ impl Quaternion { Self::from_imag(self.imag()) } + /// Left quaternionic division. + /// + /// Calculates B-1 * A where A = self, B = other. + #[inline] + pub fn left_div(&self, other: &Self) -> Option { + other.try_inverse().map(|inv| inv * self) + } + + /// Right quaternionic division. + /// + /// Calculates A * B-1 where A = self, B = other. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::Quaternion; + /// let a = Quaternion::new(0.0, 1.0, 2.0, 3.0); + /// let b = Quaternion::new(0.0, 5.0, 2.0, 1.0); + /// let result = a.right_div(&b).unwrap(); + /// let expected = Quaternion::new(0.4, 0.13333333333333336, -0.4666666666666667, 0.26666666666666666); + /// assert_relative_eq!(expected, result, epsilon = 1.0e-7); + /// ``` + #[inline] + pub fn right_div(&self, other: &Self) -> Option { + other.try_inverse().map(|inv| self * inv) + } + /// Calculates the quaternionic cosinus. /// /// # Example @@ -626,11 +653,7 @@ impl Quaternion { /// ``` #[inline] pub fn tan(&self) -> Self { - let s = self.sin(); - let c = self.cos(); - - let ci = c.try_inverse().unwrap(); - s * ci + self.sin().right_div(&self.cos()).unwrap() } /// Calculates the quaternionic arctangent. @@ -648,7 +671,7 @@ impl Quaternion { let u = Self::from_imag(self.imag().normalize()); let num = u + self; let den = u - self; - let fr = num * den.try_inverse().unwrap(); + let fr = num.right_div(&den).unwrap(); let ln = fr.ln(); (u.half()) * ln } @@ -732,11 +755,7 @@ impl Quaternion { /// ``` #[inline] pub fn tanh(&self) -> Self { - let s = self.sinh(); - let c = self.cosh(); - - let ci = c.try_inverse().unwrap(); - s * ci + self.sinh().right_div(&self.cosh()).unwrap() } /// Calculates the hyperbolic quaternionic arctangent. @@ -1096,11 +1115,7 @@ impl UnitQuaternion { /// ``` #[inline] pub fn axis_angle(&self) -> Option<(Unit>, N)> { - if let Some(axis) = self.axis() { - Some((axis, self.angle())) - } else { - None - } + self.axis().map(|axis| (axis, self.angle())) } /// Compute the exponential of a quaternion. diff --git a/src/geometry/quaternion_construction.rs b/src/geometry/quaternion_construction.rs index 8eb16b1b..4b7976bd 100644 --- a/src/geometry/quaternion_construction.rs +++ b/src/geometry/quaternion_construction.rs @@ -71,6 +71,12 @@ impl Quaternion { Self::new(scalar, vector[0], vector[1], vector[2]) } + /// Constructs a real quaternion. + #[inline] + pub fn from_real(r: N) -> Self { + Self::from_parts(r, Vector3::zero()) + } + /// Creates a new quaternion from its polar decomposition. /// /// Note that `axis` is assumed to be a unit vector. @@ -95,7 +101,7 @@ impl Quaternion { /// ``` #[inline] pub fn identity() -> Self { - Self::from_parts(N::one(), Vector3::zero()) + Self::from_real(N::one()) } } diff --git a/src/geometry/quaternion_ops.rs b/src/geometry/quaternion_ops.rs index 2ed72453..d43f104f 100644 --- a/src/geometry/quaternion_ops.rs +++ b/src/geometry/quaternion_ops.rs @@ -552,7 +552,7 @@ impl Neg for Quaternion { #[inline] fn neg(self) -> Self::Output { - Quaternion::from(-self.coords) + Self::Output::from(-self.coords) } } @@ -561,7 +561,7 @@ impl<'a, N: Real> Neg for &'a Quaternion { #[inline] fn neg(self) -> Self::Output { - Quaternion::from(-&self.coords) + Self::Output::from(-&self.coords) } } diff --git a/src/linalg/inverse.rs b/src/linalg/inverse.rs index 5748900f..69f1463d 100644 --- a/src/linalg/inverse.rs +++ b/src/linalg/inverse.rs @@ -36,7 +36,7 @@ impl> SquareMatrix { 0 => true, 1 => { let determinant = self.get_unchecked((0, 0)).clone(); - if determinant == N::zero() { + if determinant.is_zero() { false } else { *self.get_unchecked_mut((0, 0)) = N::one() / determinant; @@ -51,7 +51,7 @@ impl> SquareMatrix { let determinant = m11 * m22 - m21 * m12; - if determinant == N::zero() { + if determinant.is_zero() { false } else { *self.get_unchecked_mut((0, 0)) = m22 / determinant; @@ -83,7 +83,7 @@ impl> SquareMatrix { let determinant = m11 * minor_m12_m23 - m12 * minor_m11_m23 + m13 * minor_m11_m22; - if determinant == N::zero() { + if determinant.is_zero() { false } else { *self.get_unchecked_mut((0, 0)) = minor_m12_m23 / determinant;