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