euclid/
rotation.rs

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

euclid/
rotation.rs
