From 28ae2434fb4cc168e23fad1526da18fb5d9acf8a Mon Sep 17 00:00:00 2001 From: Matthew Rodusek <7519129+bitwizeshift@users.noreply.github.com> Date: Mon, 30 Sep 2024 14:23:27 +0000 Subject: [PATCH] Add initial implementation of `DualQuaternion` Dual-Quaternions help compress translations and rotations into a single representation. This is useful for skinning and other animation techniques where you want to interpolate between different poses. This provides an initial implementation with some basic tests for accuracy. However, it is also unclear whether this is correctly done -- so this might need to be revisited. --- alloy/src/math.rs | 4 + alloy/src/math/dual_quaternion.rs | 267 +++++++++++++++++++++++++ alloy/src/math/dual_quaternion.test.rs | 140 +++++++++++++ 3 files changed, 411 insertions(+) create mode 100644 alloy/src/math/dual_quaternion.rs create mode 100644 alloy/src/math/dual_quaternion.test.rs diff --git a/alloy/src/math.rs b/alloy/src/math.rs index 85cee93..e387e60 100644 --- a/alloy/src/math.rs +++ b/alloy/src/math.rs @@ -5,6 +5,7 @@ pub mod simd; pub mod vec; mod angle; +mod dual_quaternion; mod euler; mod interpolate; mod quaternion; @@ -18,5 +19,8 @@ pub use interpolate::*; #[doc(inline)] pub use quaternion::*; +#[doc(inline)] +pub use dual_quaternion::*; + #[doc(inline)] pub use euler::*; diff --git a/alloy/src/math/dual_quaternion.rs b/alloy/src/math/dual_quaternion.rs new file mode 100644 index 0000000..56b643f --- /dev/null +++ b/alloy/src/math/dual_quaternion.rs @@ -0,0 +1,267 @@ +use std::ops; + +use crate::math::vec::Vec3; +use crate::math::{Angle, Quaternion}; + +use super::mat::Matrix4; +use super::vec::Vector3; + +/// A dual-quaternion represents both translations and rotations in a combined +/// format. +#[repr(C)] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct DualQuaternion { + real: Quaternion, + dual: Quaternion, +} + +// Construction + +impl DualQuaternion { + /// Creates a new dual quaternion from a real and dual quaternion. + /// + /// # Parameters + /// + /// * `real` - The real part of the dual quaternion. + /// * `dual` - The dual part of the dual quaternion. + #[inline(always)] + #[must_use] + pub const fn new(real: Quaternion, dual: Quaternion) -> Self { + Self { real, dual } + } + + /// Constructs a new dual quaternion with the identity rotation and no translation. + #[inline(always)] + #[must_use] + pub const fn identity() -> Self { + Self::new(Quaternion::identity(), Quaternion::ZERO) + } + + /// Constructs this dual quaternion from a rotation quaternion. + /// + /// # Parameters + /// + /// * `rotation` - The rotation quaternion to construct the dual quaternion from. + #[must_use] + pub const fn from_rotation(rotation: Quaternion) -> Self { + Self::new(rotation, Quaternion::ZERO) + } + + /// Constructs this dual quaternion from the yaw angle + /// + /// # Parameters + /// + /// * `angle` - The yaw angle. + #[inline(always)] + #[must_use] + pub fn from_yaw(angle: A) -> Self { + Self::from_rotation(Quaternion::from_yaw(angle)) + } + + /// Constructs this dual quaternion from the pitch angle + /// + /// # Parameters + /// + /// * `angle` - The pitch angle. + #[inline(always)] + #[must_use] + pub fn from_pitch(angle: A) -> Self { + Self::from_rotation(Quaternion::from_pitch(angle)) + } + + /// Constructs this dual quaternion from the roll angle + /// + /// # Parameters + /// + /// * `angle` - The roll angle. + #[inline(always)] + #[must_use] + pub fn from_roll(angle: A) -> Self { + Self::from_rotation(Quaternion::from_roll(angle)) + } + + /// Constructs this dual quaternion from a translation vector. + /// + /// # Parameters + /// + /// * `translation` - The translation vector to construct the dual quaternion from. + pub fn from_translation(translation: &Vec3) -> Self { + Self::new( + Quaternion::identity(), + Quaternion::new( + 0.0, + 0.5 * translation.x(), + 0.5 * translation.y(), + 0.5 * translation.z(), + ), + ) + } +} + +// Conversions +impl DualQuaternion { + /// Returns the transform matrix representation of the quaternion. + #[must_use] + pub fn to_matrix4(&self) -> Matrix4 { + let translation = self.translation(); + let mut matrix4 = self.real.to_matrix4(); + matrix4[0][3] = translation.x(); + matrix4[1][3] = translation.y(); + matrix4[2][3] = translation.z(); + matrix4 + } +} + +// Properties +impl DualQuaternion { + /// Returns the real part of the dual quaternion. + #[must_use] + #[inline(always)] + pub const fn real(&self) -> &Quaternion { + &self.real + } + + /// Returns the dual part of the dual quaternion. + #[must_use] + #[inline(always)] + pub const fn dual(&self) -> &Quaternion { + &self.dual + } + + /// Returns the translation component in local-space. + #[must_use] + pub fn translation(&self) -> Vector3 { + let dual = Quaternion::new( + self.dual.w() * 2.0, + self.dual.i() * 2.0, + self.dual.j() * 2.0, + self.dual.k() * 2.0, + ); + + let translation_quat = dual * self.real.conjugate(); + + Vector3::new( + translation_quat.i(), + translation_quat.j(), + translation_quat.k(), + ) + } + + /// Returns the translation of the dual quaternion in world-space. + #[must_use] + pub fn world_translation(&self) -> Vector3 { + Vector3::new( + self.dual.i() * 2.0, + self.dual.j() * 2.0, + self.dual.k() * 2.0, + ) + } + + /// Returns the rotation component of the dual quaternion. + #[must_use] + #[inline(always)] + pub const fn rotation(&self) -> &Quaternion { + &self.real + } + + /// Returns a normalized DualQuaternion. + #[must_use] + #[inline(always)] + pub fn normalized(&self) -> DualQuaternion { + let norm = self.real.norm(); + Self { + real: &self.real / norm, + dual: &self.dual / norm, + } + } +} + +// Modifiers +impl DualQuaternion { + /// Normalizes the dual quaternion. + pub fn normalize(&mut self) { + let norm = self.real.norm(); + self.real /= norm; + self.dual /= norm; + } + + /// Translates the dual quaternion by the given vector in world space. + /// + /// # Parameters + /// + /// * `translation` - The translation vector to apply to the dual quaternion. + pub fn translate_world(&mut self, translation: &Vec3) { + self.dual += &Quaternion::new( + 0.0, + 0.5 * translation.x(), + 0.5 * translation.y(), + 0.5 * translation.z(), + ); + } + + /// Translates the dual quaternion by the given vector in local space. + /// + /// This will translate relative to the orientation of the dual quaternion. + /// + /// # Parameters + /// + /// * `translation` - The translation vector to apply to the dual quaternion. + pub fn translate(&mut self, translation: &Vec3) { + self.dual += &(Quaternion::new( + 0.0, + 0.5 * translation.x(), + 0.5 * translation.y(), + 0.5 * translation.z(), + ) * self.real); + } + + /// Rotates the dual quaternion by the given angle. + pub fn rotate(&mut self, rotation: &Quaternion) { + self.real = rotation * self.real; + self.dual = rotation * self.dual; + } + + /// Rotates the dual quaternion by the given angle around the x-axis. + #[inline(always)] + pub fn rotate_yaw(&mut self, angle: A) { + self.rotate(&Quaternion::from_yaw(angle)); + } + + /// Rotates the dual quaternion by the given angle around the y-axis. + #[inline(always)] + pub fn rotate_pitch(&mut self, angle: A) { + self.rotate(&Quaternion::from_pitch(angle)); + } + + /// Rotates the dual quaternion by the given angle around the z-axis. + #[inline(always)] + pub fn rotate_roll(&mut self, angle: A) { + self.rotate(&Quaternion::from_roll(angle)); + } +} + +impl ops::Mul for DualQuaternion { + type Output = Self; + + fn mul(self, rhs: Self) -> Self { + Self { + real: self.real * rhs.real, + dual: (self.real * rhs.dual) + (self.dual * rhs.real), + } + } +} + +impl ops::Add for DualQuaternion { + type Output = Self; + + fn add(self, rhs: Self) -> Self { + Self { + real: self.real + rhs.real, + dual: self.dual + rhs.dual, + } + } +} + +#[cfg(test)] +#[path = "dual_quaternion.test.rs"] +mod test; diff --git a/alloy/src/math/dual_quaternion.test.rs b/alloy/src/math/dual_quaternion.test.rs new file mode 100644 index 0000000..6df50db --- /dev/null +++ b/alloy/src/math/dual_quaternion.test.rs @@ -0,0 +1,140 @@ +use crate::cmp::AlmostEq; +use crate::math::vec::Vector3; +use crate::math::{Degree, DualQuaternion, Quaternion}; + +#[test] +fn from_translation() { + let translation = DualQuaternion::from_translation(&Vector3::new(1.0, 2.0, 3.0)); + let expected = DualQuaternion::new(Quaternion::identity(), Quaternion::new(0.0, 0.5, 1.0, 1.5)); + + assert_eq!(translation, expected); +} + +#[test] +fn translation() { + let translation = DualQuaternion::from_translation(&Vector3::new(1.0, 2.0, 3.0)); + let expected = Vector3::new(1.0, 2.0, 3.0); + + assert_eq!(translation.translation(), expected); +} + +#[test] +fn translate() { + struct TestCase { + rotation: Quaternion, + translation: Vector3, + } + + let test_cases = [ + TestCase { + rotation: Quaternion::identity(), + translation: Vector3::new(1.0, 2.0, 3.0), + }, + TestCase { + rotation: Quaternion::from_euler_angles( + Degree::new(90.0), + Degree::new(0.0), + Degree::new(0.0), + ), + translation: Vector3::new(0.0, 0.0, 1.0), + }, + TestCase { + rotation: Quaternion::from_euler_angles( + Degree::new(0.0), + Degree::new(90.0), + Degree::new(0.0), + ), + translation: Vector3::new(0.0, 1.0, 0.0), + }, + TestCase { + rotation: Quaternion::from_euler_angles( + Degree::new(0.0), + Degree::new(0.0), + Degree::new(90.0), + ), + translation: Vector3::new(1.0, 0.0, 0.0), + }, + ]; + + for test in test_cases { + let mut dual_quaternion = DualQuaternion::from_rotation(test.rotation); + + dual_quaternion.translate(&test.translation); + let translation = dual_quaternion.translation(); + let rotation = dual_quaternion.rotation(); + let _world = dual_quaternion.world_translation(); + + assert!( + translation.almost_eq(&test.translation), + "{} ~= {}", + translation, + test.translation + ); + assert!( + rotation.almost_eq(&test.rotation), + "{} ~= {}", + rotation, + test.rotation + ) + } +} + +#[test] +fn translate_world() { + struct TestCase { + rotation: Quaternion, + translation: Vector3, + } + + let test_cases = [ + TestCase { + rotation: Quaternion::identity(), + translation: Vector3::new(1.0, 2.0, 3.0), + }, + TestCase { + rotation: Quaternion::from_euler_angles( + Degree::new(90.0), + Degree::new(0.0), + Degree::new(0.0), + ), + translation: Vector3::new(0.0, 0.0, 1.0), + }, + TestCase { + rotation: Quaternion::from_euler_angles( + Degree::new(0.0), + Degree::new(90.0), + Degree::new(0.0), + ), + translation: Vector3::new(0.0, 1.0, 0.0), + }, + TestCase { + rotation: Quaternion::from_euler_angles( + Degree::new(0.0), + Degree::new(0.0), + Degree::new(90.0), + ), + translation: Vector3::new(1.0, 0.0, 0.0), + }, + ]; + + for test in test_cases { + let mut dual_quaternion = DualQuaternion::from_rotation(test.rotation); + + dual_quaternion.translate_world(&test.translation); + let translation = dual_quaternion.world_translation(); + let rotation = dual_quaternion.rotation(); + + assert!( + translation.almost_eq(&test.translation), + "{} ~= {}", + translation, + test.translation + ); + assert!( + rotation.almost_eq(&test.rotation), + "{} ~= {}", + rotation, + test.rotation + ) + } +}