static_math/
dual_quaternion.rs

1//-------------------------------------------------------------------------
2// @file dual_quaternion.rs
3//
4// @date 05/17/21 10:01:13
5// @author Martin Noblia
6// @email mnoblia@disroot.org
7//
8// @brief
9//
10// @detail
11//
12// Licence MIT:
13// Copyright <2021> <Martin Noblia>
14//
15// Permission is hereby granted, free of charge, to any person obtaining a copy
16// of this software and associated documentation files (the "Software"), to deal
17// in the Software without restriction, including without limitation the rights
18// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
19// copies of the Software, and to permit persons to whom the Software is
20// furnished to do so, subject to the following conditions:
21//
22// The above copyright notice and this permission notice shall be included in
23// all copies or substantial portions of the Software.  THE SOFTWARE IS PROVIDED
24// "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
25// LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
26// PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
27// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
28// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
29// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
30//--------------------------------------------------------------------------
31use core::fmt;
32use crate::quaternion::Quaternion;
33use crate::vector4::V4;
34use crate::matrix4x4::M44;
35use crate::matrix3x3::M33;
36use crate::vector3::V3;
37use crate::utils::{nearly_zero, nearly_equal};
38use crate::transformations::{get_parts_raw, homogeneous_from_rotation};
39use core::ops::{Mul, Add, Sub, Neg, Div};
40use num::{Num, Float, Signed, Zero, One};
41use num::traits::FloatConst;
42
43// TODO(elsuizo:2021-05-17): maybe we need a flag to the norm
44#[derive(Copy, Debug, Clone, PartialEq)]
45pub struct DualQuaternion<T> {
46    /// the real part
47    q_real: Quaternion<T>,
48    /// the dual part
49    q_dual: Quaternion<T>,
50    /// normalized flag
51    normalized: bool
52}
53
54impl<T> DualQuaternion<T> {
55    pub const fn new(q_real: Quaternion<T>, q_dual: Quaternion<T>) -> Self {
56        Self{q_real, q_dual, normalized: false}
57    }
58}
59
60impl<T: Num + Copy> DualQuaternion<T> {
61    /// Get the real Quaternion part
62    pub fn real(&self) -> Quaternion<T> {
63        self.q_real
64    }
65
66    /// Get the dual Quaternion part
67    pub fn dual(&self) -> Quaternion<T> {
68        self.q_dual
69    }
70
71    /// Create a pure rotation transformation from a given Quaternion
72    pub fn new_from_rotation(r: &Quaternion<T>) -> Self {
73        Self{q_real: *r, q_dual: Quaternion::zero(), normalized: true}
74    }
75
76    /// Create a Dual Quaternion that represent a point when the real part is a unit and the dual
77    /// part is a pure Quaternion
78    pub fn new_from_point(v: &V3<T>) -> Self {
79        Self{q_real: Quaternion::one(), q_dual: Quaternion::new_imag(v), normalized: true}
80    }
81
82}
83
84// dq + dq
85impl<T: Num + Copy> Add for DualQuaternion<T> {
86    type Output = Self;
87    fn add(self, rhs: Self) -> Self {
88        Self::new(self.q_real + rhs.q_real, self.q_dual + rhs.q_dual)
89    }
90}
91
92// dq - dq
93impl<T: Num + Copy> Sub for DualQuaternion<T> {
94    type Output = Self;
95    fn sub(self, rhs: Self) -> Self {
96        Self::new(self.q_real - rhs.q_real, self.q_dual - rhs.q_dual)
97    }
98}
99
100// -dq
101impl<T: Num + Copy + Signed> Neg for DualQuaternion<T> {
102    type Output = Self;
103    #[inline]
104    fn neg(self) -> Self {
105        Self::new(-self.q_real, -self.q_dual)
106    }
107}
108
109// dq * dq
110impl<T: Num + Copy> Mul for DualQuaternion<T> {
111    type Output = Self;
112
113    #[inline(always)]
114    fn mul(self, rhs: Self) -> Self::Output {
115        let q_real = self.q_real * rhs.q_real;
116        let q_dual = self.q_real * rhs.q_dual + self.q_dual * rhs.q_real;
117        Self::new(q_real, q_dual)
118    }
119}
120
121// dq * constant
122impl<T: Num + Copy> Mul<T> for DualQuaternion<T> {
123    type Output = Self;
124    fn mul(self, rhs: T) -> Self::Output {
125        Self::new(self.q_real * rhs, self.q_dual * rhs)
126    }
127}
128
129// dq / dq
130impl<T: Float + Signed> Div for DualQuaternion<T> {
131    type Output = Self;
132
133    fn div(self, rhs: Self) -> Self::Output {
134        let rhs_real_sqr = rhs.q_real * rhs.q_real;
135        let prod_real    = self.q_real * rhs.q_real / rhs_real_sqr;
136        let prod_dual    = (rhs.q_real * self.q_dual - self.q_real * rhs.q_dual) / rhs_real_sqr;
137        Self::new(prod_real, prod_dual)
138    }
139}
140
141impl<T: Float + Signed> DualQuaternion<T> {
142    /// Convert the `DualQuaternion` to a homogeneus transformation matrix `M44<Float>` in SE(3)
143    pub fn to_homogeneous(&self) -> M44<T> {
144        let (r, p) = self.to_rotation_translation();
145        homogeneous_from_rotation(&r, &p)
146    }
147
148    /// Calculate the inverse of the `DualQuaternion`
149    pub fn inverse(&self) -> Self {
150        if self.normalized {
151            self.conj()
152        } else {
153            let q_real_inv = self.q_real.inverse().expect("the zero Quaternion cannot be inverted");
154            Self::new(q_real_inv, -q_real_inv * self.q_dual * q_real_inv)
155        }
156    }
157
158    /// Get the screw parameters from this `DualQuaternion` the implementation follow the following
159    /// paper: "Dual Quaternions" by Yan-Bin Jia
160    ///
161    /// Output:
162    /// l: V3<Float> a unit 3d vector that represent one of the plucker coordinates
163    /// m: V3<Float> a vector in 3d that represent the moment of the line l and his norm represent
164    /// the distance from the origin to the line
165    /// theta: Float the amount of rotation around the screw axis l
166    /// d: Float the amount of translation along the screw axis l
167    ///
168    pub fn get_screw_parameters(&self) -> (V3<T>, V3<T>, T, T) {
169        let one = T::one();
170        let two = one + one;
171        let half = one / two;
172        let theta = self.real().get_angle();
173        let t = self.get_translation();
174
175        if !nearly_zero(theta) {
176            let l = self.real().imag() / T::sin(theta / two);
177            let d = t * l;
178            let cot = one / T::tan(theta / two);
179            let m = (t.cross(l) + (t - l * d) * cot) * half;
180            (l, m, theta, d)
181        } else {
182            let d = t.norm2();
183            let mut l = V3::zero();
184            if !nearly_zero(d) {
185                l = t / d;
186            }
187            let inf = T::infinity();
188            let m = V3::new_from(inf, inf, inf);
189            (l, m, theta, d)
190        }
191    }
192
193    // TODO(elsuizo:2021-08-04): change the name parameters to a more descriptives(no single
194    // letters)
195    // TODO(elsuizo:2021-05-23): maybe here we need to validate the norm of l
196    /// Create a `DualQuaternion` from the screw parameters
197    ///
198    /// Function arguments:
199    /// `l`: a unit vector that represent the screw axis
200    ///
201    /// `m`: screw axis moment that its perpendicular to l (m * l = 0) and the norm represent the
202    /// actual moment
203    ///
204    /// `theta`: screw angle, represent the amount of rotation around the screw axis
205    ///
206    /// `d`: screw displacement, represent the amount of displacement along the screw axis
207    ///
208    pub fn new_from_screw_parameters(l: &V3<T>, m: &V3<T>, theta: T, d: T) -> Self {
209        let one = T::one();
210        let two = one + one;
211        let (sin, cos) = (theta / two).sin_cos();
212        let q_real = Quaternion::new(cos, *l * sin);
213        let q_dual = Quaternion::new(sin * (-d / two), *m * sin + *l * (d / two) * cos);
214        Self::new(q_real, q_dual)
215    }
216
217    // NOTE(elsuizo:2021-05-23): the multiplication looks funky because we dont do products with
218    // constants to the lef size of the multiplications
219
220    /// Compute the power of the `DualQuaternion`
221    ///
222    /// Function arguments:
223    /// `exponent`: Float the exponent to raise the power
224    ///
225    /// Output: Self^(exponent)
226    ///
227    pub fn pow(&self, exponent: T) -> Self {
228        let one = T::one();
229        let two = one + one;
230        let theta = two * T::acos(self.real().real());
231        let (s, c) = (theta / two).sin_cos();
232        if nearly_zero(theta) {
233            Self::new_from_point(&(self.get_translation() * exponent))
234        } else {
235            let s0 = self.real().imag() / s;
236            let d  = self.dual().real() * -two / s;
237            let se = (self.dual().imag() - s0 * (d / two) * c) / s;
238
239            let (s_exp, c_exp) = (exponent * theta / two).sin_cos();
240            let q_real = Quaternion::new(c_exp, s0 * s_exp);
241            let q_dual = Quaternion::new(s_exp * -exponent * d / two, s0 * c_exp * exponent * d / two + se * s_exp);
242            Self::new(q_real, q_dual)
243        }
244    }
245    // TODO(elsuizo:2021-05-23): maybe this clone is not necesary...
246
247    /// Screw Linear Interpolation: is an extension of the spherical linear interpolation (SLERP)
248    /// over Quaternions. It performs a screw motion with constant rotation and translation speeds
249    /// from `begin` to `end`
250    ///
251    /// Function arguments:
252    /// `begin`: `DualQuaternion<Float>` the first "point" for the interpolation
253    /// `end`: `DualQuaternion<Float>` the second "point" for the interpolation
254    /// `tau`: Float in [0, 1] representing how far along and around the screw axis to interpolate
255    ///
256    pub fn screw_lerp(begin: &Self, end: &Self, tau: T) -> Self {
257        let one = T::one();
258        let mut start = *begin;
259        // TODO(elsuizo:2021-05-23): this is from the python implementation that refers to a paper
260        // that "ensure we always find closest solution, See Kavan and Zara 2005"
261        if (start.real() * end.real()).real() < T::zero() {
262            start.q_real = begin.real() * -one;
263        }
264        start * (start.inverse() * *end).pow(tau)
265    }
266
267    // TODO(elsuizo:2021-05-24): warning this could be wrong...
268    pub fn log(&self) -> Option<Self> {
269        if self.normalized {
270            let one = T::one();
271            let two = one + one;
272            let half = one / two;
273            let (axis, angle) = self.real().axis_angle();
274            let q_real = Quaternion::new_imag(&(axis.expect("there is no axis") * half * angle));
275            let q_dual = Quaternion::new_imag(&(self.get_translation() * half));
276            Some(DualQuaternion::new(q_real, q_dual))
277        } else {
278            None
279        }
280    }
281}
282
283impl<T: Float> DualQuaternion<T> {
284
285    /// Create a `DualQuaternion` from an array that encodes a Quaternion and a 3d vecto (V3<Float>)
286    pub fn new_from_array(array: [T; 7]) -> Self {
287        let one = T::one();
288        let half = one / (one + one);
289        let v_q = V4::new_from(array[0], array[1], array[2], array[3]);
290        let q_real = Quaternion::new_from_vec(&v_q);
291        let v_d = V3::new_from(array[4], array[5], array[6]);
292        let q_dual = Quaternion::new_imag(&v_d) * half * q_real;
293        Self::new(q_real, q_dual)
294    }
295
296    /// Create a `DualQuaternion` that represent a pure translation transformation
297    pub fn new_from_translation(t: &V3<T>) -> Self {
298        let one = T::one();
299        let two = one + one;
300        Self{q_real: Quaternion::one(), q_dual: Quaternion::new_imag(&V3::new_from(t[0]/two, t[1]/two, t[2]/two)), normalized: true}
301    }
302
303    /// Create a new DualQuaternion from a rotation(Quaternion) and translation(V3)
304    pub fn new_from_rot_trans(rot: &Quaternion<T>, translation: &V3<T>) -> Self {
305        let one = T::one();
306        let half = one / (one + one);
307        let q_real = rot.normalize().expect("the quaternion it can't be zero!!!");
308        let q_dual = (Quaternion::new_imag(translation) * half) * q_real;
309        Self{q_real, q_dual, normalized: true}
310    }
311
312    pub fn is_normalized(&self) -> bool {
313        if self.normalized {
314            true
315        } else {
316            if nearly_zero(self.real().norm2()) {
317                return true
318            }
319            let check1 = nearly_equal(self.real().norm2(), T::one(), T::epsilon());
320            let dual_norm = self.dual() / self.real().norm2();
321            let check2 = nearly_equal(dual_norm.real(), self.dual().real(), T::epsilon()) &&
322                         nearly_equal(dual_norm.imag()[0], self.dual().imag()[0], T::epsilon()) &&
323                         nearly_equal(dual_norm.imag()[1], self.dual().imag()[1], T::epsilon()) &&
324                         nearly_equal(dual_norm.imag()[2], self.dual().imag()[2], T::epsilon());
325            check1 && check2
326        }
327    }
328
329}
330
331impl<T: Float + FloatConst + core::iter::Sum> DualQuaternion<T> {
332
333    /// Normalize the `DualQuaternion` only if necessary
334    pub fn normalize(&self) -> Option<Self> {
335        if self.normalized {
336            Some(*self)
337        } else {
338            let norm_q_real = self.q_real.norm2();
339            if !nearly_zero(norm_q_real) {
340                let mut result = Self::one();
341                result.q_real = self.q_real / norm_q_real;
342                result.q_dual = self.q_dual / norm_q_real;
343                result.normalized = true;
344                Some(result)
345            } else {
346                None
347            }
348        }
349    }
350
351    // TODO(elsuizo:2021-05-18): maybe here is better with the function `get_parts` that get the
352    // Quaternion from euler angles
353    /// Create a new `DualQuaternion` from a homogeneous transformation matrix in SE(3)
354    pub fn new_from_homogeneous(t: &M44<T>) -> Self {
355        let (rot, p) = get_parts_raw(t);
356        let q = Quaternion::from_rotation(&rot).normalize().expect("the quaternion it can't be zero!!!");
357        Self::new_from_rot_trans(&q, &p)
358    }
359
360    /// Create a `DualQuaternion` that represent a rotation pure transformation
361    pub fn new_from_rotation_matrix(m: &M33<T>) -> Self {
362        Self{q_real: Quaternion::from_rotation(m), q_dual: Quaternion::zero(), normalized: true}
363    }
364}
365
366impl<T: Num + Copy + Signed> DualQuaternion<T> {
367    /// Calculate the conjugate of the `DualQuaternion`
368    pub fn conj(&self) -> Self {
369        Self::new(self.q_real.conj(), self.q_dual.conj())
370    }
371
372    /// Calculate the dual number conjugation of the `DualQuaternion`
373    pub fn conj_as_dual(&self) -> Self {
374        Self::new(self.q_real, -self.q_dual)
375    }
376
377    /// Calculate the combined(as a dual number and Quaternion) conjugation of the `DualQuaternion`
378    pub fn conj_combined(&self) -> Self {
379        Self::new(self.q_real.conj(), -self.q_dual.conj())
380    }
381
382    /// Calculate the norm of the DualQuaternion
383    pub fn norm(&self) -> Self {
384        *self * self.conj()
385    }
386
387    /// Get the underlying rotation and translation from the `DualQuaternion`
388    pub fn to_rotation_translation(&self) -> (M33<T>, V3<T>) {
389        let r = self.real().to_rotation();
390        let t = self.get_translation();
391        (r, t)
392    }
393
394    /// Get the underlying translation from the `DualQuaternion`
395    pub fn get_translation(&self) -> V3<T> {
396        let one = T::one();
397        let two = one + one;
398        ((self.dual() * two) * self.real().conj()).imag()
399    }
400
401    /// Get the underlying Quaternion and translation from the `DualQuaternion`
402    pub fn to_quaternion_translation(&self) -> (Quaternion<T>, V3<T>) {
403        let one = T::one();
404        let two = one + one;
405        let q = self.real();
406        let t = (self.dual() * two) * self.real().conj();
407        (q, t.imag())
408    }
409
410    /// Transform the given point in 3D with the transformation that represent this
411    /// `DualQuaternion` like a homogeneous transformation in SE(3)
412    pub fn transform_point(&self, point: &V3<T>) -> V3<T> {
413        let dq_point = Self::new_from_point(point);
414        (*self * dq_point * self.conj()).dual().imag()
415    }
416}
417
418impl<T: Num + Copy> DualQuaternion<T> {
419
420    /// Construct a new DualQuaternion from two V4
421    pub fn new_from_vecs(q_real: &V4<T>, q_dual: &V4<T>) -> Self {
422        Self::new(Quaternion::new_from_vec(q_real), Quaternion::new_from_vec(q_dual))
423    }
424
425    /// construct a zero DualQuaternion
426    pub fn zero() -> Self {
427        <DualQuaternion<T> as Zero>::zero()
428    }
429
430    /// construct a unit DualQuaternion
431    pub fn one() -> DualQuaternion<T> {
432        <DualQuaternion<T> as One>::one()
433    }
434}
435
436// create the zero DualQuaternion
437impl<T: Num + Copy> Zero for DualQuaternion<T> {
438    fn zero() -> Self {
439        Self::new(Quaternion::zero(), Quaternion::zero())
440    }
441
442    fn is_zero(&self) -> bool {
443        *self == DualQuaternion::zero()
444    }
445}
446
447// create the unit DualQuaternion
448impl<T: Num + Copy> One for DualQuaternion<T> {
449    /// Create an identity DualQuaternion
450    fn one() -> Self {
451        Self{q_real: Quaternion::one(), q_dual: Quaternion::zero(), normalized: true}
452    }
453}
454
455// TODO(elsuizo:2021-05-17): this could be better...
456//-------------------------------------------------------------------------
457//                      Display for DualQuaternion
458//-------------------------------------------------------------------------
459impl<T: Num + fmt::Display> fmt::Display for DualQuaternion<T> {
460    fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result {
461        write!(dest, "real: {}\ndual: {}", self.q_real, self.q_dual)
462    }
463}
464
465//-------------------------------------------------------------------------
466//                        tests
467//-------------------------------------------------------------------------
468#[cfg(test)]
469mod test_dual_quaternion {
470    use crate::dual_quaternion::DualQuaternion;
471    use crate::quaternion::Quaternion;
472    use crate::vector3::V3;
473    use crate::vector4::V4;
474    use crate::matrix3x3::M33;
475    use crate::matrix4x4::M44;
476    use crate::m44_new;
477    use crate::utils::{nearly_equal, nearly_zero, compare_vecs, compare_dual_quaternions};
478    use crate::transformations::{homogeneous_from_quaternion, euler_to_rotation};
479    const EPS: f32 = 1e-4;
480
481    #[test]
482    fn unity_product_dual_quaternion_test() {
483        let unit = DualQuaternion::one();
484        let expected: DualQuaternion<f32> = DualQuaternion::new_from_translation(&V3::z_axis());
485        let result = unit * expected;
486        assert!(compare_dual_quaternions(result, expected, EPS))
487    }
488
489    #[test]
490    fn dual_quaternion_creation_tests() {
491        // create a new DualQuaternion that represents a translation pure transformation
492        let dq: DualQuaternion<f32> = DualQuaternion::new_from_translation(&V3::z_axis());
493        // get the vector of translation and the rotation matrix
494        let result = dq.to_rotation_translation();
495        let result2 = dq.to_quaternion_translation();
496        let expected1 = V3::z_axis();
497
498        assert_eq!(
499            &result.1[..],
500            &expected1[..],
501            "\nExpected\n{:?}\nfound\n{:?}",
502            &result.1[..],
503            &expected1[..]
504        );
505
506        assert_eq!(
507            &result2.1[..],
508            &expected1[..],
509            "\nExpected\n{:?}\nfound\n{:?}",
510            &result2.1[..],
511            &expected1[..]
512        );
513
514        let expected2 = M33::identity();
515        assert!(compare_vecs(&result.0.as_vec(), &expected2.as_vec(), EPS));
516
517        let expected3 = Quaternion::one();
518        assert!(nearly_equal(result2.0.real(), expected3.real(), EPS));
519        assert!(nearly_equal(result2.0.imag()[0], expected3.imag()[0], EPS));
520        assert!(nearly_equal(result2.0.imag()[1], expected3.imag()[1], EPS));
521        assert!(nearly_equal(result2.0.imag()[2], expected3.imag()[2], EPS));
522    }
523
524    #[test]
525    fn dual_quaternion_test_norm() {
526        // let q = Quaternion::rotation(1.78, &V3::x_axis());
527        // let dq = DualQuaternion::new_from_rot_trans(&q, &V3::y_axis());
528        let dq = DualQuaternion::new_from_vecs(&V4::new_from(1.0, 2.0, 3.0, 4.0), &V4::new_from(5.0, 6.0, 7.0, 8.0));
529        let one:DualQuaternion<f32> = DualQuaternion::one();
530        let normalized = dq.normalize().unwrap();
531        assert!(normalized.is_normalized());
532        assert!(one.is_normalized());
533        assert!(compare_dual_quaternions(one, one.normalize().unwrap(), EPS))
534    }
535
536    // NOTE(elsuizo:2021-05-18): homogeneous ---> DualQuaternion ---> homogeneous
537    #[test]
538    fn homogeneous_conversions() {
539        let q  = Quaternion::from_euler_angles(10f32.to_radians(), 10f32.to_radians(), 10f32.to_radians());
540        let expected = homogeneous_from_quaternion(&q, &V3::new_from(1.0, 2.0, 3.0));
541        let result = DualQuaternion::new_from_homogeneous(&expected).to_homogeneous();
542
543        assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
544    }
545
546    #[test]
547    fn conjugate_product_test() {
548        let q1  = Quaternion::from_euler_angles(10f32.to_radians(), 10f32.to_radians(), 10f32.to_radians());
549        let q2  = Quaternion::from_euler_angles(30f32.to_radians(), 10f32.to_radians(), 30f32.to_radians());
550        let dq1 = DualQuaternion::new_from_rot_trans(&q1, &V3::x_axis());
551        let dq2 = DualQuaternion::new_from_rot_trans(&q2, &V3::z_axis());
552
553        let result = (dq1 * dq2).conj();
554        let expected = dq2.conj() * dq1.conj();
555
556        assert!(compare_dual_quaternions(result, expected, EPS));
557    }
558
559    // NOTE(elsuizo:2021-05-19): rot_pure * traslation_pure == normal_dual
560    #[test]
561    fn combined_transformations_tests() {
562        let rot = euler_to_rotation(10f32.to_radians(), 10f32.to_radians(), 10f32.to_radians(), None);
563        let q  = Quaternion::from_euler_angles(10f32.to_radians(), 10f32.to_radians(), 10f32.to_radians());
564
565        let t_pure = DualQuaternion::new_from_translation(&V3::x_axis());
566        let r_pure = DualQuaternion::new_from_rotation(&q);
567
568        let expected = DualQuaternion::new_from_rot_trans(&q, &V3::x_axis());
569        let result1 = t_pure * r_pure;
570
571        let r_pure2 = DualQuaternion::new_from_rotation_matrix(&rot);
572        let result2 = t_pure * r_pure2;
573
574        assert!(compare_dual_quaternions(result1, expected, EPS));
575        assert!(compare_dual_quaternions(result2, expected, EPS));
576    }
577
578    // NOTE(elsuizo:2021-05-20): if the DualQuaternion is not normalized the accumulate rounding
579    // error increase and we need a EPS = 1e-6
580    #[test]
581    fn dual_quaternion_inverse_test() {
582        // let dq = DualQuaternion::new_from_vecs(&V4::new_from(1.0, 2.0, 3.0, 4.0), &V4::new_from(5.0, 6.0, 7.0, 8.0));
583        let q  = Quaternion::from_euler_angles(10f32.to_radians(), 10f32.to_radians(), 10f32.to_radians());
584        let dq = DualQuaternion::new_from_rot_trans(&q, &V3::x_axis());
585        let result = dq.inverse() * dq;
586        let expected = DualQuaternion::one();
587        assert!(compare_dual_quaternions(result, expected, EPS));
588    }
589
590    // NOTE(elsuizo:2021-05-20): the inverse of the transformation represented by a DualQuaternion
591    // is a simple conjugation!!!
592    #[test]
593    fn dual_quaternion_transformation_inverse() {
594        let t_1_2 = m44_new!(0.0, 1.0, 0.0, 2.0;
595                            -1.0, 0.0, 0.0, 4.0;
596                             0.0, 0.0, 1.0, 6.0;
597                             0.0, 0.0, 0.0, 1.0);
598
599        let t_2_1 = m44_new!(0.0, -1.0,  0.0,  4.0;
600                             1.0,  0.0,  0.0, -2.0;
601                             0.0,  0.0,  1.0, -6.0;
602                             0.0,  0.0,  0.0,  1.0);
603
604        let dq_1_2 = DualQuaternion::new_from_homogeneous(&t_1_2);
605        let dq_2_1 = DualQuaternion::new_from_homogeneous(&t_2_1);
606
607        assert!(compare_vecs(&dq_2_1.to_homogeneous().as_vec(), &dq_1_2.inverse().to_homogeneous().as_vec(), EPS));
608        assert!(compare_vecs(&dq_1_2.to_homogeneous().as_vec(), &dq_2_1.inverse().to_homogeneous().as_vec(), EPS));
609    }
610
611    #[test]
612    fn dual_quaternion_transform_point_test() {
613        let q  = Quaternion::from_euler_angles(10f32.to_radians(), 10f32.to_radians(), 10f32.to_radians());
614        let t = homogeneous_from_quaternion(&q, &V3::new_from(1.0, 2.0, 3.0));
615
616        let p = V4::new_from(1.0, 2.0, 3.0, 0.0);
617        let expected = t * p;
618
619        let p = V3::new_from(1.0, 2.0, 3.0);
620        let dq = DualQuaternion::new_from_homogeneous(&t);
621        let result = dq.transform_point(&p);
622
623        // NOTE(elsuizo:2021-05-22): the transform point with the DualQuaternion should be the same
624        // that the with the homogeneus transformation(the first three elements)
625        assert!(nearly_equal(result[0], expected[0], EPS));
626        assert!(nearly_equal(result[1], expected[1], EPS));
627        assert!(nearly_equal(result[2], expected[2], EPS));
628        assert!(nearly_zero(expected[3]));
629    }
630
631    #[test]
632    fn dual_quaternion_get_screw_params_translation_test() {
633        use num::Float;
634        let trans = &V3::new_from(10.0, 3.7, 7.3);
635        let t_pure = DualQuaternion::new_from_translation(&trans);
636        let (l_result, m_result, theta_result, d_result) = t_pure.get_screw_parameters();
637        let inf = f32::infinity();
638        // NOTE(elsuizo:2021-05-22): l should be unit vector
639        assert!(nearly_equal(l_result.norm2(), 1.0, EPS));
640        // NOTE(elsuizo:2021-05-22): we use inf to signaling that m is tooo far
641        assert!(compare_vecs(&*m_result, &*V3::new_from(inf, inf, inf), EPS));
642        // NOTE(elsuizo:2021-05-22): theta should be zero because no rotation in the transformation
643        assert!(nearly_zero(theta_result));
644        // NOTE(elsuizo:2021-05-22): d should be finite
645        assert!(d_result.is_finite());
646        // NOTE(elsuizo:2021-05-22): the amount of distance along the screw axis should be the norm
647        // of the translation vector because this is a translation pure transformation
648        assert!(nearly_equal(d_result, trans.norm2(), EPS))
649    }
650
651    #[test]
652    fn dual_quaternion_screw_params_rotation_test() {
653        let q  = Quaternion::from_euler_angles(73f32.to_radians(), 10f32.to_radians(), 10f32.to_radians());
654        let r_pure = DualQuaternion::new_from_rotation(&q);
655        let (l_result, m_result, theta_result, d_result) = r_pure.get_screw_parameters();
656        // NOTE(elsuizo:2021-05-22): l should be a unit vector
657        assert!(nearly_equal(l_result.norm2(), 1.0, EPS));
658        // NOTE(elsuizo:2021-05-22): m should be zero because the moment should be zero
659        assert!(compare_vecs(&*m_result, &*V3::zeros(), EPS));
660        // NOTE(elsuizo:2021-05-22): theta should be equal to the angle of rotation that represent
661        // the Quaternion transformation
662        assert!(nearly_equal(theta_result, q.get_angle(), EPS));
663        // NOTE(elsuizo:2021-05-22): d should be zero because this is a pure rotation
664        // transformation
665        assert!(nearly_zero(d_result));
666    }
667
668    // NOTE(elsuizo:2021-05-22): rotate around the z axis 45 degrees and translate along the z axis
669    // 7 units, this example the l vector should be z_hat, d should be 7, theta should be 45
670    // degrees and m should be zero because the moment is zero
671    // like a screw in the floor that rotate 45 degrees and go 7 units up :)
672    #[test]
673    fn dual_quternion_screw_paras_full() {
674        let v = V3::z_axis() * 45f32.to_radians();
675        let q = Quaternion::rotation_norm_encoded(&v);
676        let trans = V3::new_from(0.0, 0.0, 7.0);
677        let dq_full = DualQuaternion::new_from_rot_trans(&q, &trans);
678        let (l, m, theta, d) = dq_full.get_screw_parameters();
679        assert!(compare_vecs(&*l, &*V3::z_axis(), EPS));
680        assert!(compare_vecs(&*m, &*V3::zeros(), EPS));
681        assert!(nearly_equal(theta, 45f32.to_radians(), EPS));
682        assert!(nearly_equal(d, 7.0, EPS));
683    }
684
685    #[test]
686    fn dual_quaternion_screw_params_test() {
687        let v = V3::z_axis() * 45f32.to_radians();
688        let q = Quaternion::rotation_norm_encoded(&v);
689        let trans = V3::new_from(0.0, 0.0, 7.0);
690        let dq_full = DualQuaternion::new_from_rot_trans(&q, &trans);
691        let (l, m, theta, d) = dq_full.get_screw_parameters();
692        let result = DualQuaternion::new_from_screw_parameters(&l, &m, theta, d);
693
694        assert!(compare_dual_quaternions(dq_full, result, EPS));
695    }
696
697    #[test]
698    fn dual_quaternion_pow_test() {
699        let q  = Quaternion::from_euler_angles(73f32.to_radians(), 10f32.to_radians(), 10f32.to_radians());
700        let trans = V3::new_from(0.0, 0.0, 7.0);
701        let dq_full = DualQuaternion::new_from_rot_trans(&q, &trans);
702        let expected = dq_full * dq_full * dq_full;
703        let result = dq_full.pow(3.0);
704
705        assert!(compare_dual_quaternions(expected, result, EPS));
706    }
707
708    #[test]
709    fn dual_quaternion_interpolation_test() {
710        let taus = [0.0, 0.37, 0.73, 1.0];
711        let q  = Quaternion::from_euler_angles(73f32.to_radians(), 10f32.to_radians(), 10f32.to_radians());
712        let trans = V3::new_from(0.0, 0.0, 7.0);
713        let dq = DualQuaternion::new_from_rot_trans(&q, &trans).normalize().unwrap();
714        let (l, m, theta, d) = dq.get_screw_parameters();
715
716        for tau in &taus {
717            let result = DualQuaternion::screw_lerp(&DualQuaternion::one(), &dq, *tau);
718            let expected = DualQuaternion::new_from_screw_parameters(&l, &m, *tau * theta, *tau * d);
719            assert!(compare_dual_quaternions(expected, result, EPS));
720        }
721
722    }
723}
static_math/dual_quaternion.rs

static_math/
dual_quaternion.rs
