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