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