euclid/
rotation.rs

Help
1// Copyright 2013 The Servo Project Developers. See the COPYRIGHT
2// file at the top-level directory of this distribution.
3//
4// Licensed under the Apache License, Version 2.0 <LICENSE-APACHE or
5// http://www.apache.org/licenses/LICENSE-2.0> or the MIT license
6// <LICENSE-MIT or http://opensource.org/licenses/MIT>, at your
7// option. This file may not be copied, modified, or distributed
8// except according to those terms.
9
10use crate::approxeq::ApproxEq;
11use crate::trig::Trig;
12use crate::{point2, point3, vec3, Angle, Point2D, Point3D, Vector2D, Vector3D};
13use crate::{Transform2D, Transform3D, UnknownUnit};
14use core::cmp::{Eq, PartialEq};
15use core::fmt;
16use core::hash::Hash;
17use core::marker::PhantomData;
18use core::ops::{Add, Mul, Neg, Sub};
19use num_traits::{Float, NumCast, One, Zero};
20#[cfg(feature = "serde")]
21use serde::{Deserialize, Serialize};
22
23/// A transform that can represent rotations in 2d, represented as an angle in radians.
24#[repr(C)]
25#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
26#[cfg_attr(
27    feature = "serde",
28    serde(bound(
29        serialize = "T: serde::Serialize",
30        deserialize = "T: serde::Deserialize<'de>"
31    ))
32)]
33pub struct Rotation2D<T, Src, Dst> {
34    /// Angle in radians
35    pub angle: T,
36    #[doc(hidden)]
37    pub _unit: PhantomData<(Src, Dst)>,
38}
39
40impl<T: Copy, Src, Dst> Copy for Rotation2D<T, Src, Dst> {}
41
42impl<T: Clone, Src, Dst> Clone for Rotation2D<T, Src, Dst> {
43    fn clone(&self) -> Self {
44        Rotation2D {
45            angle: self.angle.clone(),
46            _unit: PhantomData,
47        }
48    }
49}
50
51impl<T, Src, Dst> Eq for Rotation2D<T, Src, Dst> where T: Eq {}
52
53impl<T, Src, Dst> PartialEq for Rotation2D<T, Src, Dst>
54where
55    T: PartialEq,
56{
57    fn eq(&self, other: &Self) -> bool {
58        self.angle == other.angle
59    }
60}
61
62impl<T, Src, Dst> Hash for Rotation2D<T, Src, Dst>
63where
64    T: Hash,
65{
66    fn hash<H: core::hash::Hasher>(&self, h: &mut H) {
67        self.angle.hash(h);
68    }
69}
70
71impl<T, Src, Dst> Rotation2D<T, Src, Dst> {
72    /// Creates a rotation from an angle in radians.
73    #[inline]
74    pub fn new(angle: Angle<T>) -> Self {
75        Rotation2D {
76            angle: angle.radians,
77            _unit: PhantomData,
78        }
79    }
80
81    /// Creates a rotation from an angle in radians.
82    pub fn radians(angle: T) -> Self {
83        Self::new(Angle::radians(angle))
84    }
85
86    /// Creates the identity rotation.
87    #[inline]
88    pub fn identity() -> Self
89    where
90        T: Zero,
91    {
92        Self::radians(T::zero())
93    }
94}
95
96impl<T: Copy, Src, Dst> Rotation2D<T, Src, Dst> {
97    /// Cast the unit, preserving the numeric value.
98    ///
99    /// # Example
100    ///
101    /// ```rust
102    /// # use euclid::Rotation2D;
103    /// enum Local {}
104    /// enum World {}
105    ///
106    /// enum Local2 {}
107    /// enum World2 {}
108    ///
109    /// let to_world: Rotation2D<_, Local, World> = Rotation2D::radians(42);
110    ///
111    /// assert_eq!(to_world.angle, to_world.cast_unit::<Local2, World2>().angle);
112    /// ```
113    #[inline]
114    pub fn cast_unit<Src2, Dst2>(&self) -> Rotation2D<T, Src2, Dst2> {
115        Rotation2D {
116            angle: self.angle,
117            _unit: PhantomData,
118        }
119    }
120
121    /// Drop the units, preserving only the numeric value.
122    ///
123    /// # Example
124    ///
125    /// ```rust
126    /// # use euclid::Rotation2D;
127    /// enum Local {}
128    /// enum World {}
129    ///
130    /// let to_world: Rotation2D<_, Local, World> = Rotation2D::radians(42);
131    ///
132    /// assert_eq!(to_world.angle, to_world.to_untyped().angle);
133    /// ```
134    #[inline]
135    pub fn to_untyped(&self) -> Rotation2D<T, UnknownUnit, UnknownUnit> {
136        self.cast_unit()
137    }
138
139    /// Tag a unitless value with units.
140    ///
141    /// # Example
142    ///
143    /// ```rust
144    /// # use euclid::Rotation2D;
145    /// use euclid::UnknownUnit;
146    /// enum Local {}
147    /// enum World {}
148    ///
149    /// let rot: Rotation2D<_, UnknownUnit, UnknownUnit> = Rotation2D::radians(42);
150    ///
151    /// assert_eq!(rot.angle, Rotation2D::<_, Local, World>::from_untyped(&rot).angle);
152    /// ```
153    #[inline]
154    pub fn from_untyped(r: &Rotation2D<T, UnknownUnit, UnknownUnit>) -> Self {
155        r.cast_unit()
156    }
157}
158
159impl<T, Src, Dst> Rotation2D<T, Src, Dst>
160where
161    T: Copy,
162{
163    /// Returns self.angle as a strongly typed `Angle<T>`.
164    pub fn get_angle(&self) -> Angle<T> {
165        Angle::radians(self.angle)
166    }
167}
168
169impl<T: Float, Src, Dst> Rotation2D<T, Src, Dst> {
170    /// Creates a 3d rotation (around the z axis) from this 2d rotation.
171    #[inline]
172    pub fn to_3d(&self) -> Rotation3D<T, Src, Dst> {
173        Rotation3D::around_z(self.get_angle())
174    }
175
176    /// Returns the inverse of this rotation.
177    #[inline]
178    pub fn inverse(&self) -> Rotation2D<T, Dst, Src> {
179        Rotation2D::radians(-self.angle)
180    }
181
182    /// Returns a rotation representing the other rotation followed by this rotation.
183    #[inline]
184    pub fn then<NewSrc>(
185        &self,
186        other: &Rotation2D<T, NewSrc, Src>,
187    ) -> Rotation2D<T, NewSrc, Dst> {
188        Rotation2D::radians(self.angle + other.angle)
189    }
190
191    /// Returns the given 2d point transformed by this rotation.
192    ///
193    /// The input point must be use the unit Src, and the returned point has the unit Dst.
194    #[inline]
195    pub fn transform_point(&self, point: Point2D<T, Src>) -> Point2D<T, Dst> {
196        let (sin, cos) = Float::sin_cos(self.angle);
197        point2(point.x * cos - point.y * sin, point.y * cos + point.x * sin)
198    }
199
200    /// Returns the given 2d vector transformed by this rotation.
201    ///
202    /// The input point must be use the unit Src, and the returned point has the unit Dst.
203    #[inline]
204    pub fn transform_vector(&self, vector: Vector2D<T, Src>) -> Vector2D<T, Dst> {
205        self.transform_point(vector.to_point()).to_vector()
206    }
207}
208
209impl<T, Src, Dst> Rotation2D<T, Src, Dst>
210where
211    T: Copy + Add<Output = T> + Sub<Output = T> + Mul<Output = T> + Zero + Trig,
212{
213    /// Returns the matrix representation of this rotation.
214    #[inline]
215    pub fn to_transform(&self) -> Transform2D<T, Src, Dst> {
216        Transform2D::rotation(self.get_angle())
217    }
218}
219
220/// A transform that can represent rotations in 3d, represented as a quaternion.
221///
222/// Most methods expect the quaternion to be normalized.
223/// When in doubt, use `unit_quaternion` instead of `quaternion` to create
224/// a rotation as the former will ensure that its result is normalized.
225///
226/// Some people use the `x, y, z, w` (or `w, x, y, z`) notations. The equivalence is
227/// as follows: `x -> i`, `y -> j`, `z -> k`, `w -> r`.
228/// The memory layout of this type corresponds to the `x, y, z, w` notation
229#[repr(C)]
230#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
231#[cfg_attr(
232    feature = "serde",
233    serde(bound(
234        serialize = "T: serde::Serialize",
235        deserialize = "T: serde::Deserialize<'de>"
236    ))
237)]
238pub struct Rotation3D<T, Src, Dst> {
239    /// Component multiplied by the imaginary number `i`.
240    pub i: T,
241    /// Component multiplied by the imaginary number `j`.
242    pub j: T,
243    /// Component multiplied by the imaginary number `k`.
244    pub k: T,
245    /// The real part.
246    pub r: T,
247    #[doc(hidden)]
248    pub _unit: PhantomData<(Src, Dst)>,
249}
250
251impl<T: Copy, Src, Dst> Copy for Rotation3D<T, Src, Dst> {}
252
253impl<T: Clone, Src, Dst> Clone for Rotation3D<T, Src, Dst> {
254    fn clone(&self) -> Self {
255        Rotation3D {
256            i: self.i.clone(),
257            j: self.j.clone(),
258            k: self.k.clone(),
259            r: self.r.clone(),
260            _unit: PhantomData,
261        }
262    }
263}
264
265impl<T, Src, Dst> Eq for Rotation3D<T, Src, Dst> where T: Eq {}
266
267impl<T, Src, Dst> PartialEq for Rotation3D<T, Src, Dst>
268where
269    T: PartialEq,
270{
271    fn eq(&self, other: &Self) -> bool {
272        self.i == other.i && self.j == other.j && self.k == other.k && self.r == other.r
273    }
274}
275
276impl<T, Src, Dst> Hash for Rotation3D<T, Src, Dst>
277where
278    T: Hash,
279{
280    fn hash<H: core::hash::Hasher>(&self, h: &mut H) {
281        self.i.hash(h);
282        self.j.hash(h);
283        self.k.hash(h);
284        self.r.hash(h);
285    }
286}
287
288impl<T, Src, Dst> Rotation3D<T, Src, Dst> {
289    /// Creates a rotation around from a quaternion representation.
290    ///
291    /// The parameters are a, b, c and r compose the quaternion `a*i + b*j + c*k + r`
292    /// where `a`, `b` and `c` describe the vector part and the last parameter `r` is
293    /// the real part.
294    ///
295    /// The resulting quaternion is not necessarily normalized. See [`unit_quaternion`].
296    ///
297    /// [`unit_quaternion`]: #method.unit_quaternion
298    #[inline]
299    pub fn quaternion(a: T, b: T, c: T, r: T) -> Self {
300        Rotation3D {
301            i: a,
302            j: b,
303            k: c,
304            r,
305            _unit: PhantomData,
306        }
307    }
308
309    /// Creates the identity rotation.
310    #[inline]
311    pub fn identity() -> Self
312    where
313        T: Zero + One,
314    {
315        Self::quaternion(T::zero(), T::zero(), T::zero(), T::one())
316    }
317}
318
319impl<T, Src, Dst> Rotation3D<T, Src, Dst>
320where
321    T: Copy,
322{
323    /// Returns the vector part (i, j, k) of this quaternion.
324    #[inline]
325    pub fn vector_part(&self) -> Vector3D<T, UnknownUnit> {
326        vec3(self.i, self.j, self.k)
327    }
328
329    /// Cast the unit, preserving the numeric value.
330    ///
331    /// # Example
332    ///
333    /// ```rust
334    /// # use euclid::Rotation3D;
335    /// enum Local {}
336    /// enum World {}
337    ///
338    /// enum Local2 {}
339    /// enum World2 {}
340    ///
341    /// let to_world: Rotation3D<_, Local, World> = Rotation3D::quaternion(1, 2, 3, 4);
342    ///
343    /// assert_eq!(to_world.i, to_world.cast_unit::<Local2, World2>().i);
344    /// assert_eq!(to_world.j, to_world.cast_unit::<Local2, World2>().j);
345    /// assert_eq!(to_world.k, to_world.cast_unit::<Local2, World2>().k);
346    /// assert_eq!(to_world.r, to_world.cast_unit::<Local2, World2>().r);
347    /// ```
348    #[inline]
349    pub fn cast_unit<Src2, Dst2>(&self) -> Rotation3D<T, Src2, Dst2> {
350        Rotation3D {
351            i: self.i,
352            j: self.j,
353            k: self.k,
354            r: self.r,
355            _unit: PhantomData,
356        }
357    }
358
359    /// Drop the units, preserving only the numeric value.
360    ///
361    /// # Example
362    ///
363    /// ```rust
364    /// # use euclid::Rotation3D;
365    /// enum Local {}
366    /// enum World {}
367    ///
368    /// let to_world: Rotation3D<_, Local, World> = Rotation3D::quaternion(1, 2, 3, 4);
369    ///
370    /// assert_eq!(to_world.i, to_world.to_untyped().i);
371    /// assert_eq!(to_world.j, to_world.to_untyped().j);
372    /// assert_eq!(to_world.k, to_world.to_untyped().k);
373    /// assert_eq!(to_world.r, to_world.to_untyped().r);
374    /// ```
375    #[inline]
376    pub fn to_untyped(&self) -> Rotation3D<T, UnknownUnit, UnknownUnit> {
377        self.cast_unit()
378    }
379
380    /// Tag a unitless value with units.
381    ///
382    /// # Example
383    ///
384    /// ```rust
385    /// # use euclid::Rotation3D;
386    /// use euclid::UnknownUnit;
387    /// enum Local {}
388    /// enum World {}
389    ///
390    /// let rot: Rotation3D<_, UnknownUnit, UnknownUnit> = Rotation3D::quaternion(1, 2, 3, 4);
391    ///
392    /// assert_eq!(rot.i, Rotation3D::<_, Local, World>::from_untyped(&rot).i);
393    /// assert_eq!(rot.j, Rotation3D::<_, Local, World>::from_untyped(&rot).j);
394    /// assert_eq!(rot.k, Rotation3D::<_, Local, World>::from_untyped(&rot).k);
395    /// assert_eq!(rot.r, Rotation3D::<_, Local, World>::from_untyped(&rot).r);
396    /// ```
397    #[inline]
398    pub fn from_untyped(r: &Rotation3D<T, UnknownUnit, UnknownUnit>) -> Self {
399        r.cast_unit()
400    }
401}
402
403impl<T, Src, Dst> Rotation3D<T, Src, Dst>
404where
405    T: Float,
406{
407    /// Creates a rotation around from a quaternion representation and normalizes it.
408    ///
409    /// The parameters are a, b, c and r compose the quaternion `a*i + b*j + c*k + r`
410    /// before normalization, where `a`, `b` and `c` describe the vector part and the
411    /// last parameter `r` is the real part.
412    #[inline]
413    pub fn unit_quaternion(i: T, j: T, k: T, r: T) -> Self {
414        Self::quaternion(i, j, k, r).normalize()
415    }
416
417    /// Creates a rotation around a given axis.
418    pub fn around_axis(axis: Vector3D<T, Src>, angle: Angle<T>) -> Self {
419        let axis = axis.normalize();
420        let two = T::one() + T::one();
421        let (sin, cos) = Angle::sin_cos(angle / two);
422        Self::quaternion(axis.x * sin, axis.y * sin, axis.z * sin, cos)
423    }
424
425    /// Creates a rotation around the x axis.
426    pub fn around_x(angle: Angle<T>) -> Self {
427        let zero = Zero::zero();
428        let two = T::one() + T::one();
429        let (sin, cos) = Angle::sin_cos(angle / two);
430        Self::quaternion(sin, zero, zero, cos)
431    }
432
433    /// Creates a rotation around the y axis.
434    pub fn around_y(angle: Angle<T>) -> Self {
435        let zero = Zero::zero();
436        let two = T::one() + T::one();
437        let (sin, cos) = Angle::sin_cos(angle / two);
438        Self::quaternion(zero, sin, zero, cos)
439    }
440
441    /// Creates a rotation around the z axis.
442    pub fn around_z(angle: Angle<T>) -> Self {
443        let zero = Zero::zero();
444        let two = T::one() + T::one();
445        let (sin, cos) = Angle::sin_cos(angle / two);
446        Self::quaternion(zero, zero, sin, cos)
447    }
448
449    /// Creates a rotation from Euler angles.
450    ///
451    /// The rotations are applied in roll then pitch then yaw order.
452    ///
453    ///  - Roll (also called bank) is a rotation around the x axis.
454    ///  - Pitch (also called bearing) is a rotation around the y axis.
455    ///  - Yaw (also called heading) is a rotation around the z axis.
456    pub fn euler(roll: Angle<T>, pitch: Angle<T>, yaw: Angle<T>) -> Self {
457        let half = T::one() / (T::one() + T::one());
458
459        let (sy, cy) = Float::sin_cos(half * yaw.get());
460        let (sp, cp) = Float::sin_cos(half * pitch.get());
461        let (sr, cr) = Float::sin_cos(half * roll.get());
462
463        Self::quaternion(
464            cy * sr * cp - sy * cr * sp,
465            cy * cr * sp + sy * sr * cp,
466            sy * cr * cp - cy * sr * sp,
467            cy * cr * cp + sy * sr * sp,
468        )
469    }
470
471    /// Returns the inverse of this rotation.
472    #[inline]
473    pub fn inverse(&self) -> Rotation3D<T, Dst, Src> {
474        Rotation3D::quaternion(-self.i, -self.j, -self.k, self.r)
475    }
476
477    /// Computes the norm of this quaternion.
478    #[inline]
479    pub fn norm(&self) -> T {
480        self.square_norm().sqrt()
481    }
482
483    /// Computes the squared norm of this quaternion.
484    #[inline]
485    pub fn square_norm(&self) -> T {
486        self.i * self.i + self.j * self.j + self.k * self.k + self.r * self.r
487    }
488
489    /// Returns a [unit quaternion] from this one.
490    ///
491    /// [unit quaternion]: https://en.wikipedia.org/wiki/Quaternion#Unit_quaternion
492    #[inline]
493    pub fn normalize(&self) -> Self {
494        self.mul(T::one() / self.norm())
495    }
496
497    /// Returns `true` if [norm] of this quaternion is (approximately) one.
498    ///
499    /// [norm]: #method.norm
500    #[inline]
501    pub fn is_normalized(&self) -> bool
502    where
503        T: ApproxEq<T>,
504    {
505        let eps = NumCast::from(1.0e-5).unwrap();
506        self.square_norm().approx_eq_eps(&T::one(), &eps)
507    }
508
509    /// Spherical linear interpolation between this rotation and another rotation.
510    ///
511    /// `t` is expected to be between zero and one.
512    pub fn slerp(&self, other: &Self, t: T) -> Self
513    where
514        T: ApproxEq<T>,
515    {
516        debug_assert!(self.is_normalized());
517        debug_assert!(other.is_normalized());
518
519        let r1 = *self;
520        let mut r2 = *other;
521
522        let mut dot = r1.i * r2.i + r1.j * r2.j + r1.k * r2.k + r1.r * r2.r;
523
524        let one = T::one();
525
526        if dot.approx_eq(&T::one()) {
527            // If the inputs are too close, linearly interpolate to avoid precision issues.
528            return r1.lerp(&r2, t);
529        }
530
531        // If the dot product is negative, the quaternions
532        // have opposite handed-ness and slerp won't take
533        // the shorter path. Fix by reversing one quaternion.
534        if dot < T::zero() {
535            r2 = r2.mul(-T::one());
536            dot = -dot;
537        }
538
539        // For robustness, stay within the domain of acos.
540        dot = Float::min(dot, one);
541
542        // Angle between r1 and the result.
543        let theta = Float::acos(dot) * t;
544
545        // r1 and r3 form an orthonormal basis.
546        let r3 = r2.sub(r1.mul(dot)).normalize();
547        let (sin, cos) = Float::sin_cos(theta);
548        r1.mul(cos).add(r3.mul(sin))
549    }
550
551    /// Basic Linear interpolation between this rotation and another rotation.
552    #[inline]
553    pub fn lerp(&self, other: &Self, t: T) -> Self {
554        let one_t = T::one() - t;
555        self.mul(one_t).add(other.mul(t)).normalize()
556    }
557
558    /// Returns the given 3d point transformed by this rotation.
559    ///
560    /// The input point must be use the unit Src, and the returned point has the unit Dst.
561    pub fn transform_point3d(&self, point: Point3D<T, Src>) -> Point3D<T, Dst>
562    where
563        T: ApproxEq<T>,
564    {
565        debug_assert!(self.is_normalized());
566
567        let two = T::one() + T::one();
568        let cross = self.vector_part().cross(point.to_vector().to_untyped()) * two;
569
570        point3(
571            point.x + self.r * cross.x + self.j * cross.z - self.k * cross.y,
572            point.y + self.r * cross.y + self.k * cross.x - self.i * cross.z,
573            point.z + self.r * cross.z + self.i * cross.y - self.j * cross.x,
574        )
575    }
576
577    /// Returns the given 2d point transformed by this rotation then projected on the xy plane.
578    ///
579    /// The input point must be use the unit Src, and the returned point has the unit Dst.
580    #[inline]
581    pub fn transform_point2d(&self, point: Point2D<T, Src>) -> Point2D<T, Dst>
582    where
583        T: ApproxEq<T>,
584    {
585        self.transform_point3d(point.to_3d()).xy()
586    }
587
588    /// Returns the given 3d vector transformed by this rotation.
589    ///
590    /// The input vector must be use the unit Src, and the returned point has the unit Dst.
591    #[inline]
592    pub fn transform_vector3d(&self, vector: Vector3D<T, Src>) -> Vector3D<T, Dst>
593    where
594        T: ApproxEq<T>,
595    {
596        self.transform_point3d(vector.to_point()).to_vector()
597    }
598
599    /// Returns the given 2d vector transformed by this rotation then projected on the xy plane.
600    ///
601    /// The input vector must be use the unit Src, and the returned point has the unit Dst.
602    #[inline]
603    pub fn transform_vector2d(&self, vector: Vector2D<T, Src>) -> Vector2D<T, Dst>
604    where
605        T: ApproxEq<T>,
606    {
607        self.transform_vector3d(vector.to_3d()).xy()
608    }
609
610    /// Returns the matrix representation of this rotation.
611    #[inline]
612    pub fn to_transform(&self) -> Transform3D<T, Src, Dst>
613    where
614        T: ApproxEq<T>,
615    {
616        debug_assert!(self.is_normalized());
617
618        let i2 = self.i + self.i;
619        let j2 = self.j + self.j;
620        let k2 = self.k + self.k;
621        let ii = self.i * i2;
622        let ij = self.i * j2;
623        let ik = self.i * k2;
624        let jj = self.j * j2;
625        let jk = self.j * k2;
626        let kk = self.k * k2;
627        let ri = self.r * i2;
628        let rj = self.r * j2;
629        let rk = self.r * k2;
630
631        let one = T::one();
632        let zero = T::zero();
633
634        let m11 = one - (jj + kk);
635        let m12 = ij + rk;
636        let m13 = ik - rj;
637
638        let m21 = ij - rk;
639        let m22 = one - (ii + kk);
640        let m23 = jk + ri;
641
642        let m31 = ik + rj;
643        let m32 = jk - ri;
644        let m33 = one - (ii + jj);
645
646        Transform3D::new(
647            m11, m12, m13, zero,
648            m21, m22, m23, zero,
649            m31, m32, m33, zero,
650            zero, zero, zero, one,
651        )
652    }
653
654    /// Returns a rotation representing this rotation followed by the other rotation.
655    #[inline]
656    pub fn then<NewDst>(
657        &self,
658        other: &Rotation3D<T, Dst, NewDst>,
659    ) -> Rotation3D<T, Src, NewDst>
660    where
661        T: ApproxEq<T>,
662    {
663        debug_assert!(self.is_normalized());
664        Rotation3D::quaternion(
665            other.i * self.r + other.r * self.i + other.j * self.k - other.k * self.j,
666            other.j * self.r + other.r * self.j + other.k * self.i - other.i * self.k,
667            other.k * self.r + other.r * self.k + other.i * self.j - other.j * self.i,
668            other.r * self.r - other.i * self.i - other.j * self.j - other.k * self.k,
669        )
670    }
671
672    // add, sub and mul are used internally for intermediate computation but aren't public
673    // because they don't carry real semantic meanings (I think?).
674
675    #[inline]
676    fn add(&self, other: Self) -> Self {
677        Self::quaternion(
678            self.i + other.i,
679            self.j + other.j,
680            self.k + other.k,
681            self.r + other.r,
682        )
683    }
684
685    #[inline]
686    fn sub(&self, other: Self) -> Self {
687        Self::quaternion(
688            self.i - other.i,
689            self.j - other.j,
690            self.k - other.k,
691            self.r - other.r,
692        )
693    }
694
695    #[inline]
696    fn mul(&self, factor: T) -> Self {
697        Self::quaternion(
698            self.i * factor,
699            self.j * factor,
700            self.k * factor,
701            self.r * factor,
702        )
703    }
704}
705
706impl<T: fmt::Debug, Src, Dst> fmt::Debug for Rotation3D<T, Src, Dst> {
707    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
708        write!(
709            f,
710            "Quat({:?}*i + {:?}*j + {:?}*k + {:?})",
711            self.i, self.j, self.k, self.r
712        )
713    }
714}
715
716impl<T, Src, Dst> ApproxEq<T> for Rotation3D<T, Src, Dst>
717where
718    T: Copy + Neg<Output = T> + ApproxEq<T>,
719{
720    fn approx_epsilon() -> T {
721        T::approx_epsilon()
722    }
723
724    fn approx_eq_eps(&self, other: &Self, eps: &T) -> bool {
725        (self.i.approx_eq_eps(&other.i, eps)
726            && self.j.approx_eq_eps(&other.j, eps)
727            && self.k.approx_eq_eps(&other.k, eps)
728            && self.r.approx_eq_eps(&other.r, eps))
729            || (self.i.approx_eq_eps(&-other.i, eps)
730                && self.j.approx_eq_eps(&-other.j, eps)
731                && self.k.approx_eq_eps(&-other.k, eps)
732                && self.r.approx_eq_eps(&-other.r, eps))
733    }
734}
735
736#[test]
737fn simple_rotation_2d() {
738    use crate::default::Rotation2D;
739    use core::f32::consts::{FRAC_PI_2, PI};
740
741    let ri = Rotation2D::identity();
742    let r90 = Rotation2D::radians(FRAC_PI_2);
743    let rm90 = Rotation2D::radians(-FRAC_PI_2);
744    let r180 = Rotation2D::radians(PI);
745
746    assert!(ri
747        .transform_point(point2(1.0, 2.0))
748        .approx_eq(&point2(1.0, 2.0)));
749    assert!(r90
750        .transform_point(point2(1.0, 2.0))
751        .approx_eq(&point2(-2.0, 1.0)));
752    assert!(rm90
753        .transform_point(point2(1.0, 2.0))
754        .approx_eq(&point2(2.0, -1.0)));
755    assert!(r180
756        .transform_point(point2(1.0, 2.0))
757        .approx_eq(&point2(-1.0, -2.0)));
758
759    assert!(r90
760        .inverse()
761        .inverse()
762        .transform_point(point2(1.0, 2.0))
763        .approx_eq(&r90.transform_point(point2(1.0, 2.0))));
764}
765
766#[test]
767fn simple_rotation_3d_in_2d() {
768    use crate::default::Rotation3D;
769    use core::f32::consts::{FRAC_PI_2, PI};
770
771    let ri = Rotation3D::identity();
772    let r90 = Rotation3D::around_z(Angle::radians(FRAC_PI_2));
773    let rm90 = Rotation3D::around_z(Angle::radians(-FRAC_PI_2));
774    let r180 = Rotation3D::around_z(Angle::radians(PI));
775
776    assert!(ri
777        .transform_point2d(point2(1.0, 2.0))
778        .approx_eq(&point2(1.0, 2.0)));
779    assert!(r90
780        .transform_point2d(point2(1.0, 2.0))
781        .approx_eq(&point2(-2.0, 1.0)));
782    assert!(rm90
783        .transform_point2d(point2(1.0, 2.0))
784        .approx_eq(&point2(2.0, -1.0)));
785    assert!(r180
786        .transform_point2d(point2(1.0, 2.0))
787        .approx_eq(&point2(-1.0, -2.0)));
788
789    assert!(r90
790        .inverse()
791        .inverse()
792        .transform_point2d(point2(1.0, 2.0))
793        .approx_eq(&r90.transform_point2d(point2(1.0, 2.0))));
794}
795
796#[test]
797fn pre_post() {
798    use crate::default::Rotation3D;
799    use core::f32::consts::FRAC_PI_2;
800
801    let r1 = Rotation3D::around_x(Angle::radians(FRAC_PI_2));
802    let r2 = Rotation3D::around_y(Angle::radians(FRAC_PI_2));
803    let r3 = Rotation3D::around_z(Angle::radians(FRAC_PI_2));
804
805    let t1 = r1.to_transform();
806    let t2 = r2.to_transform();
807    let t3 = r3.to_transform();
808
809    let p = point3(1.0, 2.0, 3.0);
810
811    // Check that the order of transformations is correct (corresponds to what
812    // we do in Transform3D).
813    let p1 = r1.then(&r2).then(&r3).transform_point3d(p);
814    let p2 = t1
815        .then(&t2)
816        .then(&t3)
817        .transform_point3d(p);
818
819    assert!(p1.approx_eq(&p2.unwrap()));
820
821    // Check that changing the order indeed matters.
822    let p3 = t3
823        .then(&t1)
824        .then(&t2)
825        .transform_point3d(p);
826    assert!(!p1.approx_eq(&p3.unwrap()));
827}
828
829#[test]
830fn to_transform3d() {
831    use crate::default::Rotation3D;
832
833    use core::f32::consts::{FRAC_PI_2, PI};
834    let rotations = [
835        Rotation3D::identity(),
836        Rotation3D::around_x(Angle::radians(FRAC_PI_2)),
837        Rotation3D::around_x(Angle::radians(-FRAC_PI_2)),
838        Rotation3D::around_x(Angle::radians(PI)),
839        Rotation3D::around_y(Angle::radians(FRAC_PI_2)),
840        Rotation3D::around_y(Angle::radians(-FRAC_PI_2)),
841        Rotation3D::around_y(Angle::radians(PI)),
842        Rotation3D::around_z(Angle::radians(FRAC_PI_2)),
843        Rotation3D::around_z(Angle::radians(-FRAC_PI_2)),
844        Rotation3D::around_z(Angle::radians(PI)),
845    ];
846
847    let points = [
848        point3(0.0, 0.0, 0.0),
849        point3(1.0, 2.0, 3.0),
850        point3(-5.0, 3.0, -1.0),
851        point3(-0.5, -1.0, 1.5),
852    ];
853
854    for rotation in &rotations {
855        for &point in &points {
856            let p1 = rotation.transform_point3d(point);
857            let p2 = rotation.to_transform().transform_point3d(point);
858            assert!(p1.approx_eq(&p2.unwrap()));
859        }
860    }
861}
862
863#[test]
864fn slerp() {
865    use crate::default::Rotation3D;
866
867    let q1 = Rotation3D::quaternion(1.0, 0.0, 0.0, 0.0);
868    let q2 = Rotation3D::quaternion(0.0, 1.0, 0.0, 0.0);
869    let q3 = Rotation3D::quaternion(0.0, 0.0, -1.0, 0.0);
870
871    // The values below can be obtained with a python program:
872    // import numpy
873    // import quaternion
874    // q1 = numpy.quaternion(1, 0, 0, 0)
875    // q2 = numpy.quaternion(0, 1, 0, 0)
876    // quaternion.slerp_evaluate(q1, q2, 0.2)
877
878    assert!(q1.slerp(&q2, 0.0).approx_eq(&q1));
879    assert!(q1.slerp(&q2, 0.2).approx_eq(&Rotation3D::quaternion(
880        0.951056516295154,
881        0.309016994374947,
882        0.0,
883        0.0
884    )));
885    assert!(q1.slerp(&q2, 0.4).approx_eq(&Rotation3D::quaternion(
886        0.809016994374947,
887        0.587785252292473,
888        0.0,
889        0.0
890    )));
891    assert!(q1.slerp(&q2, 0.6).approx_eq(&Rotation3D::quaternion(
892        0.587785252292473,
893        0.809016994374947,
894        0.0,
895        0.0
896    )));
897    assert!(q1.slerp(&q2, 0.8).approx_eq(&Rotation3D::quaternion(
898        0.309016994374947,
899        0.951056516295154,
900        0.0,
901        0.0
902    )));
903    assert!(q1.slerp(&q2, 1.0).approx_eq(&q2));
904
905    assert!(q1.slerp(&q3, 0.0).approx_eq(&q1));
906    assert!(q1.slerp(&q3, 0.2).approx_eq(&Rotation3D::quaternion(
907        0.951056516295154,
908        0.0,
909        -0.309016994374947,
910        0.0
911    )));
912    assert!(q1.slerp(&q3, 0.4).approx_eq(&Rotation3D::quaternion(
913        0.809016994374947,
914        0.0,
915        -0.587785252292473,
916        0.0
917    )));
918    assert!(q1.slerp(&q3, 0.6).approx_eq(&Rotation3D::quaternion(
919        0.587785252292473,
920        0.0,
921        -0.809016994374947,
922        0.0
923    )));
924    assert!(q1.slerp(&q3, 0.8).approx_eq(&Rotation3D::quaternion(
925        0.309016994374947,
926        0.0,
927        -0.951056516295154,
928        0.0
929    )));
930    assert!(q1.slerp(&q3, 1.0).approx_eq(&q3));
931}
932
933#[test]
934fn around_axis() {
935    use crate::default::Rotation3D;
936    use core::f32::consts::{FRAC_PI_2, PI};
937
938    // Two sort of trivial cases:
939    let r1 = Rotation3D::around_axis(vec3(1.0, 1.0, 0.0), Angle::radians(PI));
940    let r2 = Rotation3D::around_axis(vec3(1.0, 1.0, 0.0), Angle::radians(FRAC_PI_2));
941    assert!(r1
942        .transform_point3d(point3(1.0, 2.0, 0.0))
943        .approx_eq(&point3(2.0, 1.0, 0.0)));
944    assert!(r2
945        .transform_point3d(point3(1.0, 0.0, 0.0))
946        .approx_eq(&point3(0.5, 0.5, -0.5.sqrt())));
947
948    // A more arbitrary test (made up with numpy):
949    let r3 = Rotation3D::around_axis(vec3(0.5, 1.0, 2.0), Angle::radians(2.291288));
950    assert!(r3
951        .transform_point3d(point3(1.0, 0.0, 0.0))
952        .approx_eq(&point3(-0.58071821, 0.81401868, -0.01182979)));
953}
954
955#[test]
956fn from_euler() {
957    use crate::default::Rotation3D;
958    use core::f32::consts::FRAC_PI_2;
959
960    // First test simple separate yaw pitch and roll rotations, because it is easy to come
961    // up with the corresponding quaternion.
962    // Since several quaternions can represent the same transformation we compare the result
963    // of transforming a point rather than the values of each quaternions.
964    let p = point3(1.0, 2.0, 3.0);
965
966    let angle = Angle::radians(FRAC_PI_2);
967    let zero = Angle::radians(0.0);
968
969    // roll
970    let roll_re = Rotation3D::euler(angle, zero, zero);
971    let roll_rq = Rotation3D::around_x(angle);
972    let roll_pe = roll_re.transform_point3d(p);
973    let roll_pq = roll_rq.transform_point3d(p);
974
975    // pitch
976    let pitch_re = Rotation3D::euler(zero, angle, zero);
977    let pitch_rq = Rotation3D::around_y(angle);
978    let pitch_pe = pitch_re.transform_point3d(p);
979    let pitch_pq = pitch_rq.transform_point3d(p);
980
981    // yaw
982    let yaw_re = Rotation3D::euler(zero, zero, angle);
983    let yaw_rq = Rotation3D::around_z(angle);
984    let yaw_pe = yaw_re.transform_point3d(p);
985    let yaw_pq = yaw_rq.transform_point3d(p);
986
987    assert!(roll_pe.approx_eq(&roll_pq));
988    assert!(pitch_pe.approx_eq(&pitch_pq));
989    assert!(yaw_pe.approx_eq(&yaw_pq));
990
991    // Now check that the yaw pitch and roll transformations when combined are applied in
992    // the proper order: roll -> pitch -> yaw.
993    let ypr_e = Rotation3D::euler(angle, angle, angle);
994    let ypr_q = roll_rq.then(&pitch_rq).then(&yaw_rq);
995    let ypr_pe = ypr_e.transform_point3d(p);
996    let ypr_pq = ypr_q.transform_point3d(p);
997
998    assert!(ypr_pe.approx_eq(&ypr_pq));
999}
euclid/rotation.rs

euclid/
rotation.rs
