polyscope_render/
dual_quat.rs

1//! Minimal dual quaternion implementation for rigid body interpolation.
2//!
3//! A dual quaternion encodes a rigid body transformation (rotation + translation)
4//! as a pair of quaternions `(real, dual)`. Used for smooth camera flight animation
5//! matching C++ Polyscope's `glm::dualquat` interpolation.
6
7use glam::{Quat, Vec3};
8
9/// A dual quaternion representing a rigid body transformation.
10///
11/// `real` encodes the rotation, `dual` encodes the translation as:
12/// `dual = 0.5 * Quat(t.x, t.y, t.z, 0) * real`
13#[derive(Debug, Clone, Copy)]
14pub struct DualQuat {
15    /// Rotation part (unit quaternion).
16    pub real: Quat,
17    /// Translation-encoding part.
18    pub dual: Quat,
19}
20
21impl DualQuat {
22    /// Creates a dual quaternion from a rotation quaternion and translation vector.
23    #[must_use]
24    pub fn from_rotation_translation(rot: Quat, translation: Vec3) -> Self {
25        let real = rot.normalize();
26        // dual = 0.5 * pure_quat(t) * real
27        let t_quat = Quat::from_xyzw(translation.x, translation.y, translation.z, 0.0);
28        let dual = (t_quat * real) * 0.5;
29        Self { real, dual }
30    }
31
32    /// Extracts the rotation quaternion and translation vector.
33    #[must_use]
34    pub fn to_rotation_translation(&self) -> (Quat, Vec3) {
35        let rot = self.real.normalize();
36        // t_quat = 2 * dual * conjugate(real)
37        let t_quat = (self.dual * rot.conjugate()) * 2.0;
38        (rot, Vec3::new(t_quat.x, t_quat.y, t_quat.z))
39    }
40
41    /// Dual linear blend (DLB) interpolation between two dual quaternions.
42    ///
43    /// Matches GLM's `glm::lerp` for dual quaternions: component-wise lerp
44    /// of both real and dual parts, then normalize. Produces smooth rigid
45    /// body interpolation with slight arc motion.
46    #[must_use]
47    pub fn lerp(a: &Self, b: &Self, t: f32) -> Self {
48        // Ensure shortest path: flip b if dot product is negative
49        let (b_real, b_dual) = if a.real.dot(b.real) < 0.0 {
50            (-b.real, -b.dual)
51        } else {
52            (b.real, b.dual)
53        };
54
55        let real = a.real * (1.0 - t) + b_real * t;
56        let dual = a.dual * (1.0 - t) + b_dual * t;
57
58        Self { real, dual }.normalize()
59    }
60
61    /// Normalizes the dual quaternion.
62    ///
63    /// Normalizes the real part to unit length, then makes the dual part
64    /// orthogonal to the real part (removes any non-rigid component).
65    #[must_use]
66    pub fn normalize(&self) -> Self {
67        let norm = self.real.length();
68        if norm < 1e-10 {
69            return *self;
70        }
71        let inv_norm = 1.0 / norm;
72        let real = self.real * inv_norm;
73        let dual = self.dual * inv_norm;
74        // Remove any component of dual parallel to real
75        let dual = dual - real * real.dot(dual);
76        Self { real, dual }
77    }
78}
79
80#[cfg(test)]
81mod tests {
82    use super::*;
83    use std::f32::consts::FRAC_PI_2;
84
85    #[test]
86    fn test_identity_round_trip() {
87        let rot = Quat::IDENTITY;
88        let t = Vec3::new(1.0, 2.0, 3.0);
89        let dq = DualQuat::from_rotation_translation(rot, t);
90        let (rot_out, t_out) = dq.to_rotation_translation();
91        assert!((rot_out.dot(rot) - 1.0).abs() < 1e-5);
92        assert!((t_out - t).length() < 1e-5);
93    }
94
95    #[test]
96    fn test_rotated_round_trip() {
97        let rot = Quat::from_axis_angle(Vec3::Y, FRAC_PI_2);
98        let t = Vec3::new(5.0, -3.0, 1.0);
99        let dq = DualQuat::from_rotation_translation(rot, t);
100        let (rot_out, t_out) = dq.to_rotation_translation();
101        assert!((rot_out.dot(rot).abs() - 1.0).abs() < 1e-5);
102        assert!((t_out - t).length() < 1e-5);
103    }
104
105    #[test]
106    fn test_lerp_endpoints() {
107        let a = DualQuat::from_rotation_translation(Quat::IDENTITY, Vec3::ZERO);
108        let rot_b = Quat::from_axis_angle(Vec3::Y, FRAC_PI_2);
109        let t_b = Vec3::new(10.0, 0.0, 0.0);
110        let b = DualQuat::from_rotation_translation(rot_b, t_b);
111
112        // t=0 should give a
113        let at0 = DualQuat::lerp(&a, &b, 0.0);
114        let (r0, t0) = at0.to_rotation_translation();
115        assert!((r0.dot(Quat::IDENTITY).abs() - 1.0).abs() < 1e-4);
116        assert!(t0.length() < 1e-4);
117
118        // t=1 should give b
119        let at1 = DualQuat::lerp(&a, &b, 1.0);
120        let (r1, t1) = at1.to_rotation_translation();
121        assert!((r1.dot(rot_b).abs() - 1.0).abs() < 1e-4);
122        assert!((t1 - t_b).length() < 1e-3);
123    }
124
125    #[test]
126    fn test_lerp_midpoint() {
127        let a = DualQuat::from_rotation_translation(Quat::IDENTITY, Vec3::ZERO);
128        let b = DualQuat::from_rotation_translation(Quat::IDENTITY, Vec3::new(10.0, 0.0, 0.0));
129
130        let mid = DualQuat::lerp(&a, &b, 0.5);
131        let (_, t_mid) = mid.to_rotation_translation();
132        assert!((t_mid - Vec3::new(5.0, 0.0, 0.0)).length() < 1e-4);
133    }
134}
polyscope_render/dual_quat.rs

polyscope_render/
dual_quat.rs
