macaw/
dual_quat.rs

1//! Dual scalar and dual quaternion implementation.
2//!
3//! References:
4//! - <https://users.cs.utah.edu/~ladislav/kavan07skinning/kavan07skinning.pdf>
5//! - <https://faculty.sites.iastate.edu/jia/files/inline-files/dual-quaternion.pdf>
6//! - <https://stackoverflow.com/questions/23174899/properly-normalizing-a-dual-quaternion>
7//! - <http://wscg.zcu.cz/wscg2012/short/a29-full.pdf>
8//! - <https://borodust.github.io/public/shared/paper_dual-quats.pdf>
9//! - <https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/dual-quaternion.pdf>
10
11use crate::IsoTransform;
12use crate::Quat;
13use crate::Vec3;
14use crate::Vec4;
15use crate::Vec4Swizzles;
16
17#[cfg(target_arch = "spirv")]
18use num_traits::Float;
19
20/// A dual scalar, with a real and dual part
21#[derive(Clone, Copy, PartialEq)]
22#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
23#[cfg_attr(feature = "speedy", derive(speedy::Writable, speedy::Readable))]
24#[cfg_attr(
25    feature = "bytemuck",
26    derive(bytemuck::NoUninit, bytemuck::AnyBitPattern)
27)]
28#[repr(C)]
29pub struct DualScalar {
30    /// The coefficient of the real part
31    pub real: f32,
32    /// The coefficient of the dual part
33    pub dual: f32,
34}
35
36impl DualScalar {
37    /// Defined only for dual numbers with a positive real part.
38    #[inline]
39    pub fn sqrt(self) -> Self {
40        let real_sqrt = self.real.sqrt();
41
42        let dual = self.dual / 2.0 * real_sqrt;
43
44        Self {
45            real: real_sqrt,
46            dual,
47        }
48    }
49
50    /// More efficient than `.sqrt().inverse()`. Defined only for dual numbers with positive,
51    /// non-zero real part.
52    #[inline]
53    pub fn inverse_sqrt(self) -> Self {
54        let real_sqrt = self.real.sqrt();
55
56        Self {
57            real: 1.0 / real_sqrt,
58            dual: -self.dual / (2.0 * self.real * real_sqrt),
59        }
60    }
61
62    /// Gives the inverse of `self` such that the inverse multiplied with `self` will equal
63    /// `(1.0, 0.0)`.
64    #[inline]
65    pub fn inverse(self) -> Self {
66        let real_inv = 1.0 / self.real;
67        Self {
68            real: real_inv,
69            dual: -self.dual * real_inv * real_inv,
70        }
71    }
72}
73
74impl core::ops::Mul for DualScalar {
75    type Output = Self;
76    #[inline]
77    fn mul(self, rhs: Self) -> Self::Output {
78        Self {
79            real: self.real * rhs.real,
80            dual: self.real * rhs.dual + self.dual * rhs.real,
81        }
82    }
83}
84
85impl core::ops::Add for DualScalar {
86    type Output = Self;
87    #[inline]
88    fn add(self, rhs: Self) -> Self::Output {
89        Self {
90            real: self.real + rhs.real,
91            dual: self.dual + rhs.dual,
92        }
93    }
94}
95
96impl core::ops::Sub for DualScalar {
97    type Output = Self;
98    #[inline]
99    fn sub(self, rhs: Self) -> Self::Output {
100        Self {
101            real: self.real - rhs.real,
102            dual: self.dual - rhs.dual,
103        }
104    }
105}
106
107impl core::ops::Mul<f32> for DualScalar {
108    type Output = Self;
109    #[inline]
110    fn mul(self, rhs: f32) -> Self::Output {
111        Self {
112            real: self.real * rhs,
113            dual: self.dual * rhs,
114        }
115    }
116}
117
118/// Represents a rigid body transformation which can be thought of as a "screw" motion
119/// which is the combination of a translation along a vector and a rotation around that vector.
120///
121/// Represents the same kind of transformation as an `IsoTransform` but interpolates and transforms
122/// in a way that preserves volume.
123#[derive(Clone, Copy, PartialEq)]
124#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
125#[cfg_attr(feature = "speedy", derive(speedy::Writable, speedy::Readable))]
126#[cfg_attr(
127    feature = "bytemuck",
128    derive(bytemuck::NoUninit, bytemuck::AnyBitPattern)
129)]
130#[repr(C, align(16))]
131pub struct DualQuat {
132    /// The real quaternion part, i.e. the rotator
133    pub real: Quat,
134    /// The dual quaternion part, i.e. the translator
135    pub dual: Quat,
136}
137
138impl DualQuat {
139    const fn _assert_repr() {
140        let _: [(); core::mem::size_of::<Self>()] = [(); 32];
141        let _: [(); core::mem::align_of::<Self>()] = [(); 16];
142    }
143
144    /// The identity transform: doesn't transform at all. Like multiplying with `1`.
145    pub const IDENTITY: Self = Self {
146        real: Quat::IDENTITY,
147        dual: Quat::from_xyzw(0.0, 0.0, 0.0, 0.0),
148    };
149
150    /// Not a valid quaternion in and of itself. All values filled with 0s.
151    /// Can be useful when blending a set of transforms.
152    pub const ZERO: Self = Self {
153        real: Quat::from_xyzw(0.0, 0.0, 0.0, 0.0),
154        dual: Quat::from_xyzw(0.0, 0.0, 0.0, 0.0),
155    };
156
157    /// Create a dual quaternion from an [`IsoTransform`]
158    #[inline]
159    pub fn from_iso_transform(iso_transform: IsoTransform) -> Self {
160        Self::from_rotation_translation(iso_transform.rotation, iso_transform.translation())
161    }
162
163    /// Create a dual quaternion that rotates and then translates by the specified amount.
164    /// `rotation` is assumed to be normalized.
165    #[inline]
166    pub fn from_rotation_translation(rotation: Quat, translation: Vec3) -> Self {
167        Self {
168            real: rotation,
169            dual: Quat::from_vec4((translation * 0.5).extend(0.0)) * rotation,
170        }
171    }
172
173    /// A pure translation without any rotation.
174    #[inline]
175    pub fn from_translation(translation: Vec3) -> Self {
176        Self {
177            real: Quat::IDENTITY,
178            dual: Quat::from_vec4((translation * 0.5).extend(0.0)),
179        }
180    }
181
182    /// A pure rotation without any translation.
183    #[inline]
184    pub fn from_quat(rotation: Quat) -> Self {
185        Self {
186            real: rotation,
187            dual: Quat::from_xyzw(0.0, 0.0, 0.0, 0.0),
188        }
189    }
190
191    /// Returns this transform decomposed into `(rotation, translation)`. *Assumes `self` is
192    /// already normalized.* If not, consider
193    /// using [`normalize_to_rotation_translation`][Self::normalize_to_rotation_translation]
194    ///
195    /// You can then apply this to a point by doing `rotation.mul_vec3(point) + translation`.
196    #[inline]
197    pub fn to_rotation_translation(self) -> (Quat, Vec3) {
198        let translation = 2.0 * Vec4::from(self.dual * self.real.conjugate()).xyz();
199        (self.real, translation)
200    }
201
202    /// Quaternion conjugate of this dual quaternion, which is given `q = (real + dual)`,
203    /// `q* = (real* + dual*)`. This form of conjugate is also the inverse as long as this
204    /// dual quaternion is unit.
205    #[inline]
206    pub fn conjugate(self) -> Self {
207        Self {
208            real: self.real.conjugate(),
209            dual: self.dual.conjugate(),
210        }
211    }
212
213    /// Gives the inverse of this dual quaternion such that `self * self.inverse() = identity`.
214    ///
215    /// This function should only be used if self is unit, i.e if `self.is_normalized()` is true.
216    /// Will panic in debug builds if it is not normalized.
217    #[inline]
218    pub fn inverse(self) -> Self {
219        debug_assert!(self.is_normalized());
220        self.conjugate()
221    }
222
223    /// Gives the norm squared of the dual quaternion.
224    ///
225    /// `self` is normalized if `real = 1.0` and `dual = 0.0`.
226    #[inline]
227    pub fn norm_squared(self) -> DualScalar {
228        // https://math.stackexchange.com/questions/4609827/dual-quaternion-norm-expression-from-skinning-with-dual-quaternions-paper
229        // https://users.cs.utah.edu/~ladislav/kavan07skinning/kavan07skinning.pdf
230        let real_vec = Vec4::from(self.real);
231        let dual_vec = Vec4::from(self.dual);
232        let dot = real_vec.dot(dual_vec);
233
234        DualScalar {
235            real: self.real.length_squared(),
236            dual: 2.0 * dot,
237        }
238    }
239
240    /// Gives the norm of the dual quaternion.
241    ///
242    /// `self` is normalized if `real = 1.0` and `dual = 0.0`.
243    #[inline]
244    pub fn norm(self) -> DualScalar {
245        // https://math.stackexchange.com/questions/4609827/dual-quaternion-norm-expression-from-skinning-with-dual-quaternions-paper
246        // https://users.cs.utah.edu/~ladislav/kavan07skinning/kavan07skinning.pdf
247        let real_mag = self.real.length();
248        let real_vec = Vec4::from(self.real);
249        let dual_vec = Vec4::from(self.dual);
250        let dot = real_vec.dot(dual_vec);
251
252        DualScalar {
253            real: real_mag,
254            dual: dot / real_mag,
255        }
256    }
257
258    /// Whether `self` is normalized, i.e. a unit dual quaternion.
259    #[inline]
260    pub fn is_normalized(self) -> bool {
261        let norm_sq = self.norm_squared();
262
263        let eps = 1e-4; // same as glam chose...
264        (norm_sq.real - 1.0).abs() < eps && norm_sq.dual.abs() < eps
265    }
266
267    /// Normalizes `self` to make it a unit dual quaternion.
268    ///
269    /// If you will immediately apply the normalized dual quat to transform a point/vector, consider
270    /// using [`normalize_to_rotation_translation`][Self::normalize_to_rotation_translation]
271    /// instead.
272    #[inline]
273    pub fn normalize_full(self) -> Self {
274        // combine norm + inverse of norm into single calculation to optimize slightly.
275        // see https://users.cs.utah.edu/~ladislav/kavan07skinning/kavan07skinning.pdf section 3.4
276        //
277        // norm = (norm(real), (real dot dual) / norm(real))
278        //
279        // inv = (1.0 / real, -dual / real^2)
280        //
281        // inverse norm = (1.0 / norm(real), -(real dot dual) / (norm(real) * normsq(real)))
282
283        let real_normsq = self.real.length_squared();
284        let real_norm = real_normsq.sqrt();
285        let real_vec = Vec4::from(self.real);
286        let dual_vec = Vec4::from(self.dual);
287        let dot = real_vec.dot(dual_vec);
288
289        let normalizer = DualScalar {
290            real: 1.0 / real_norm,
291            dual: -dot / (real_norm * real_normsq),
292        };
293
294        normalizer * self
295    }
296
297    /// Normalize `self` and then extract its rotation and translation components which can be
298    /// applied to a point by doing `rotation.mul_vec3(point) + translation`
299    ///
300    /// This will be faster than `self.normalize().to_rotation_translation()` as we can use
301    /// the full expansion of the operation to cancel out some calculations that would
302    /// otherwise need to be performed. For justification of this, see
303    /// <https://users.cs.utah.edu/~ladislav/kavan07skinning/kavan07skinning.pdf> particularly
304    /// equation (4) and section 3.4.
305    ///
306    /// You can then apply this to a point by doing `(rotation * point + translation)`
307    #[inline]
308    pub fn normalize_to_rotation_translation(mut self) -> (Quat, Vec3) {
309        let real_norm_inv = self.real.length_recip();
310
311        self.real = self.real * real_norm_inv;
312        self.dual = self.dual * real_norm_inv;
313        self.to_rotation_translation()
314    }
315
316    /// Whether `self` is approximately equal to `other` with `max_abs_diff` error
317    #[inline]
318    pub fn abs_diff_eq(self, other: Self, max_abs_diff: f32) -> bool {
319        self.real.abs_diff_eq(other.real, max_abs_diff)
320            && self.dual.abs_diff_eq(other.dual, max_abs_diff)
321    }
322
323    /// An optimized form of `self * DualQuat::from_translation(trans)`
324    /// (note the order!)
325    #[inline]
326    pub fn right_mul_translation(mut self, trans: Vec3) -> Self {
327        self.dual.w -=
328            0.5 * (self.real.x * trans.x + self.real.y * trans.y + self.real.z * trans.z);
329        self.dual.x +=
330            0.5 * (self.real.w * trans.x + self.real.y * trans.z - self.real.z * trans.y);
331        self.dual.y +=
332            0.5 * (self.real.w * trans.y + self.real.z * trans.x - self.real.x * trans.z);
333        self.dual.z +=
334            0.5 * (self.real.w * trans.z + self.real.x * trans.y - self.real.y * trans.x);
335        self
336    }
337}
338
339impl core::ops::Mul for DualQuat {
340    type Output = Self;
341
342    #[inline]
343    fn mul(self, rhs: Self) -> Self {
344        let Self { real: q0, dual: q1 } = self;
345        let Self { real: q2, dual: q3 } = rhs;
346        let real = q0 * q2;
347        let dual = q0 * q3 + q1 * q2;
348        Self { real, dual }
349    }
350}
351
352impl core::ops::Add for DualQuat {
353    type Output = Self;
354
355    #[inline]
356    fn add(self, rhs: Self) -> Self {
357        Self {
358            real: self.real + rhs.real,
359            dual: self.dual + rhs.dual,
360        }
361    }
362}
363
364impl core::ops::AddAssign for DualQuat {
365    #[inline]
366    fn add_assign(&mut self, rhs: Self) {
367        self.real = self.real + rhs.real;
368        self.dual = self.dual + rhs.dual;
369    }
370}
371
372impl core::ops::Sub for DualQuat {
373    type Output = Self;
374
375    #[inline]
376    fn sub(self, rhs: Self) -> Self {
377        Self {
378            real: self.real - rhs.real,
379            dual: self.dual - rhs.dual,
380        }
381    }
382}
383
384impl core::ops::SubAssign for DualQuat {
385    #[inline]
386    fn sub_assign(&mut self, rhs: Self) {
387        self.real = self.real - rhs.real;
388        self.dual = self.dual - rhs.dual;
389    }
390}
391
392impl core::ops::Mul<f32> for DualQuat {
393    type Output = Self;
394
395    #[inline]
396    fn mul(self, rhs: f32) -> Self::Output {
397        Self {
398            real: self.real * rhs,
399            dual: self.dual * rhs,
400        }
401    }
402}
403
404impl core::ops::Mul<DualQuat> for f32 {
405    type Output = DualQuat;
406
407    #[inline]
408    fn mul(self, rhs: DualQuat) -> Self::Output {
409        rhs * self
410    }
411}
412
413impl core::ops::Mul<DualQuat> for DualScalar {
414    type Output = DualQuat;
415
416    #[inline]
417    fn mul(self, rhs: DualQuat) -> Self::Output {
418        DualQuat {
419            real: rhs.real * self.real,
420            dual: (rhs.dual * self.real) + (rhs.real * self.dual),
421        }
422    }
423}
424
425impl core::ops::Mul<DualScalar> for DualQuat {
426    type Output = Self;
427
428    #[inline]
429    fn mul(self, rhs: DualScalar) -> Self::Output {
430        rhs * self
431    }
432}
433
434#[cfg(feature = "std")]
435impl core::fmt::Debug for DualQuat {
436    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
437        let (rot, translation) = self.to_rotation_translation();
438        let (axis, angle) = rot.to_axis_angle();
439        f.debug_struct("DualQuat")
440            .field(
441                "translation",
442                &format!("[{} {} {}]", translation.x, translation.y, translation.z),
443            )
444            .field(
445                "rotation",
446                &format!(
447                    "{:.1}° around [{} {} {}]",
448                    angle.to_degrees(),
449                    axis[0],
450                    axis[1],
451                    axis[2],
452                ),
453            )
454            .field("real(raw)", &self.real)
455            .field("dual(raw)", &self.dual)
456            .finish()
457    }
458}
459
460#[cfg(test)]
461mod test {
462    use super::*;
463
464    fn approx_eq_dualquat(a: DualQuat, b: DualQuat) -> bool {
465        let max_abs_diff = 1e-6;
466        a.abs_diff_eq(b, max_abs_diff)
467    }
468
469    macro_rules! assert_approx_eq_dualquat {
470        ($a: expr, $b: expr) => {
471            assert!(approx_eq_dualquat($a, $b), "{:#?} != {:#?}", $a, $b,);
472        };
473    }
474
475    #[test]
476    fn test_inverse() {
477        let transform = DualQuat::from_rotation_translation(
478            Quat::from_rotation_y(std::f32::consts::PI),
479            Vec3::ONE,
480        );
481        let identity = transform * transform.inverse();
482        assert_approx_eq_dualquat!(identity, DualQuat::IDENTITY);
483
484        let transform = DualQuat::from_rotation_translation(
485            Quat::from_axis_angle(Vec3::ONE.normalize(), 1.234),
486            Vec3::new(1.0, 2.0, 3.0),
487        );
488        let identity = transform * transform.inverse();
489        assert_approx_eq_dualquat!(identity, DualQuat::IDENTITY);
490    }
491}
macaw/dual_quat.rs

macaw/
dual_quat.rs
