• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 // Generated from quat.rs.tera template. Edit the template, not the generated file.
2 
3 use crate::{
4     euler::{EulerRot, FromEuler, ToEuler},
5     f32::math,
6     wasm32::*,
7     DQuat, Mat3, Mat3A, Mat4, Vec2, Vec3, Vec3A, Vec4,
8 };
9 
10 use core::arch::wasm32::*;
11 
12 use core::fmt;
13 use core::iter::{Product, Sum};
14 use core::ops::{Add, Deref, DerefMut, Div, Mul, MulAssign, Neg, Sub};
15 
16 /// Creates a quaternion from `x`, `y`, `z` and `w` values.
17 ///
18 /// This should generally not be called manually unless you know what you are doing. Use
19 /// one of the other constructors instead such as `identity` or `from_axis_angle`.
20 #[inline]
21 #[must_use]
quat(x: f32, y: f32, z: f32, w: f32) -> Quat22 pub const fn quat(x: f32, y: f32, z: f32, w: f32) -> Quat {
23     Quat::from_xyzw(x, y, z, w)
24 }
25 
26 /// A quaternion representing an orientation.
27 ///
28 /// This quaternion is intended to be of unit length but may denormalize due to
29 /// floating point "error creep" which can occur when successive quaternion
30 /// operations are applied.
31 ///
32 /// SIMD vector types are used for storage on supported platforms.
33 ///
34 /// This type is 16 byte aligned.
35 #[derive(Clone, Copy)]
36 #[repr(transparent)]
37 pub struct Quat(pub(crate) v128);
38 
39 impl Quat {
40     /// All zeros.
41     const ZERO: Self = Self::from_array([0.0; 4]);
42 
43     /// The identity quaternion. Corresponds to no rotation.
44     pub const IDENTITY: Self = Self::from_xyzw(0.0, 0.0, 0.0, 1.0);
45 
46     /// All NANs.
47     pub const NAN: Self = Self::from_array([f32::NAN; 4]);
48 
49     /// Creates a new rotation quaternion.
50     ///
51     /// This should generally not be called manually unless you know what you are doing.
52     /// Use one of the other constructors instead such as `identity` or `from_axis_angle`.
53     ///
54     /// `from_xyzw` is mostly used by unit tests and `serde` deserialization.
55     ///
56     /// # Preconditions
57     ///
58     /// This function does not check if the input is normalized, it is up to the user to
59     /// provide normalized input or to normalized the resulting quaternion.
60     #[inline(always)]
61     #[must_use]
from_xyzw(x: f32, y: f32, z: f32, w: f32) -> Self62     pub const fn from_xyzw(x: f32, y: f32, z: f32, w: f32) -> Self {
63         Self(f32x4(x, y, z, w))
64     }
65 
66     /// Creates a rotation quaternion from an array.
67     ///
68     /// # Preconditions
69     ///
70     /// This function does not check if the input is normalized, it is up to the user to
71     /// provide normalized input or to normalized the resulting quaternion.
72     #[inline]
73     #[must_use]
from_array(a: [f32; 4]) -> Self74     pub const fn from_array(a: [f32; 4]) -> Self {
75         Self::from_xyzw(a[0], a[1], a[2], a[3])
76     }
77 
78     /// Creates a new rotation quaternion from a 4D vector.
79     ///
80     /// # Preconditions
81     ///
82     /// This function does not check if the input is normalized, it is up to the user to
83     /// provide normalized input or to normalized the resulting quaternion.
84     #[inline]
85     #[must_use]
from_vec4(v: Vec4) -> Self86     pub const fn from_vec4(v: Vec4) -> Self {
87         Self(v.0)
88     }
89 
90     /// Creates a rotation quaternion from a slice.
91     ///
92     /// # Preconditions
93     ///
94     /// This function does not check if the input is normalized, it is up to the user to
95     /// provide normalized input or to normalized the resulting quaternion.
96     ///
97     /// # Panics
98     ///
99     /// Panics if `slice` length is less than 4.
100     #[inline]
101     #[must_use]
from_slice(slice: &[f32]) -> Self102     pub fn from_slice(slice: &[f32]) -> Self {
103         Self::from_xyzw(slice[0], slice[1], slice[2], slice[3])
104     }
105 
106     /// Writes the quaternion to an unaligned slice.
107     ///
108     /// # Panics
109     ///
110     /// Panics if `slice` length is less than 4.
111     #[inline]
write_to_slice(self, slice: &mut [f32])112     pub fn write_to_slice(self, slice: &mut [f32]) {
113         slice[0] = self.x;
114         slice[1] = self.y;
115         slice[2] = self.z;
116         slice[3] = self.w;
117     }
118 
119     /// Create a quaternion for a normalized rotation `axis` and `angle` (in radians).
120     ///
121     /// The axis must be a unit vector.
122     ///
123     /// # Panics
124     ///
125     /// Will panic if `axis` is not normalized when `glam_assert` is enabled.
126     #[inline]
127     #[must_use]
from_axis_angle(axis: Vec3, angle: f32) -> Self128     pub fn from_axis_angle(axis: Vec3, angle: f32) -> Self {
129         glam_assert!(axis.is_normalized());
130         let (s, c) = math::sin_cos(angle * 0.5);
131         let v = axis * s;
132         Self::from_xyzw(v.x, v.y, v.z, c)
133     }
134 
135     /// Create a quaternion that rotates `v.length()` radians around `v.normalize()`.
136     ///
137     /// `from_scaled_axis(Vec3::ZERO)` results in the identity quaternion.
138     #[inline]
139     #[must_use]
from_scaled_axis(v: Vec3) -> Self140     pub fn from_scaled_axis(v: Vec3) -> Self {
141         let length = v.length();
142         if length == 0.0 {
143             Self::IDENTITY
144         } else {
145             Self::from_axis_angle(v / length, length)
146         }
147     }
148 
149     /// Creates a quaternion from the `angle` (in radians) around the x axis.
150     #[inline]
151     #[must_use]
from_rotation_x(angle: f32) -> Self152     pub fn from_rotation_x(angle: f32) -> Self {
153         let (s, c) = math::sin_cos(angle * 0.5);
154         Self::from_xyzw(s, 0.0, 0.0, c)
155     }
156 
157     /// Creates a quaternion from the `angle` (in radians) around the y axis.
158     #[inline]
159     #[must_use]
from_rotation_y(angle: f32) -> Self160     pub fn from_rotation_y(angle: f32) -> Self {
161         let (s, c) = math::sin_cos(angle * 0.5);
162         Self::from_xyzw(0.0, s, 0.0, c)
163     }
164 
165     /// Creates a quaternion from the `angle` (in radians) around the z axis.
166     #[inline]
167     #[must_use]
from_rotation_z(angle: f32) -> Self168     pub fn from_rotation_z(angle: f32) -> Self {
169         let (s, c) = math::sin_cos(angle * 0.5);
170         Self::from_xyzw(0.0, 0.0, s, c)
171     }
172 
173     /// Creates a quaternion from the given Euler rotation sequence and the angles (in radians).
174     #[inline]
175     #[must_use]
from_euler(euler: EulerRot, a: f32, b: f32, c: f32) -> Self176     pub fn from_euler(euler: EulerRot, a: f32, b: f32, c: f32) -> Self {
177         Self::from_euler_angles(euler, a, b, c)
178     }
179 
180     /// From the columns of a 3x3 rotation matrix.
181     ///
182     /// Note if the input axes contain scales, shears, or other non-rotation transformations then
183     /// the output of this function is ill-defined.
184     ///
185     /// # Panics
186     ///
187     /// Will panic if any axis is not normalized when `glam_assert` is enabled.
188     #[inline]
189     #[must_use]
from_rotation_axes(x_axis: Vec3, y_axis: Vec3, z_axis: Vec3) -> Self190     pub(crate) fn from_rotation_axes(x_axis: Vec3, y_axis: Vec3, z_axis: Vec3) -> Self {
191         glam_assert!(x_axis.is_normalized() && y_axis.is_normalized() && z_axis.is_normalized());
192         // Based on https://github.com/microsoft/DirectXMath `XMQuaternionRotationMatrix`
193         let (m00, m01, m02) = x_axis.into();
194         let (m10, m11, m12) = y_axis.into();
195         let (m20, m21, m22) = z_axis.into();
196         if m22 <= 0.0 {
197             // x^2 + y^2 >= z^2 + w^2
198             let dif10 = m11 - m00;
199             let omm22 = 1.0 - m22;
200             if dif10 <= 0.0 {
201                 // x^2 >= y^2
202                 let four_xsq = omm22 - dif10;
203                 let inv4x = 0.5 / math::sqrt(four_xsq);
204                 Self::from_xyzw(
205                     four_xsq * inv4x,
206                     (m01 + m10) * inv4x,
207                     (m02 + m20) * inv4x,
208                     (m12 - m21) * inv4x,
209                 )
210             } else {
211                 // y^2 >= x^2
212                 let four_ysq = omm22 + dif10;
213                 let inv4y = 0.5 / math::sqrt(four_ysq);
214                 Self::from_xyzw(
215                     (m01 + m10) * inv4y,
216                     four_ysq * inv4y,
217                     (m12 + m21) * inv4y,
218                     (m20 - m02) * inv4y,
219                 )
220             }
221         } else {
222             // z^2 + w^2 >= x^2 + y^2
223             let sum10 = m11 + m00;
224             let opm22 = 1.0 + m22;
225             if sum10 <= 0.0 {
226                 // z^2 >= w^2
227                 let four_zsq = opm22 - sum10;
228                 let inv4z = 0.5 / math::sqrt(four_zsq);
229                 Self::from_xyzw(
230                     (m02 + m20) * inv4z,
231                     (m12 + m21) * inv4z,
232                     four_zsq * inv4z,
233                     (m01 - m10) * inv4z,
234                 )
235             } else {
236                 // w^2 >= z^2
237                 let four_wsq = opm22 + sum10;
238                 let inv4w = 0.5 / math::sqrt(four_wsq);
239                 Self::from_xyzw(
240                     (m12 - m21) * inv4w,
241                     (m20 - m02) * inv4w,
242                     (m01 - m10) * inv4w,
243                     four_wsq * inv4w,
244                 )
245             }
246         }
247     }
248 
249     /// Creates a quaternion from a 3x3 rotation matrix.
250     ///
251     /// Note if the input matrix contain scales, shears, or other non-rotation transformations then
252     /// the resulting quaternion will be ill-defined.
253     ///
254     /// # Panics
255     ///
256     /// Will panic if any input matrix column is not normalized when `glam_assert` is enabled.
257     #[inline]
258     #[must_use]
from_mat3(mat: &Mat3) -> Self259     pub fn from_mat3(mat: &Mat3) -> Self {
260         Self::from_rotation_axes(mat.x_axis, mat.y_axis, mat.z_axis)
261     }
262 
263     /// Creates a quaternion from a 3x3 SIMD aligned rotation matrix.
264     ///
265     /// Note if the input matrix contain scales, shears, or other non-rotation transformations then
266     /// the resulting quaternion will be ill-defined.
267     ///
268     /// # Panics
269     ///
270     /// Will panic if any input matrix column is not normalized when `glam_assert` is enabled.
271     #[inline]
272     #[must_use]
from_mat3a(mat: &Mat3A) -> Self273     pub fn from_mat3a(mat: &Mat3A) -> Self {
274         Self::from_rotation_axes(mat.x_axis.into(), mat.y_axis.into(), mat.z_axis.into())
275     }
276 
277     /// Creates a quaternion from the upper 3x3 rotation matrix inside a homogeneous 4x4 matrix.
278     ///
279     /// Note if the upper 3x3 matrix contain scales, shears, or other non-rotation transformations
280     /// then the resulting quaternion will be ill-defined.
281     ///
282     /// # Panics
283     ///
284     /// Will panic if any column of the upper 3x3 rotation matrix is not normalized when
285     /// `glam_assert` is enabled.
286     #[inline]
287     #[must_use]
from_mat4(mat: &Mat4) -> Self288     pub fn from_mat4(mat: &Mat4) -> Self {
289         Self::from_rotation_axes(
290             mat.x_axis.truncate(),
291             mat.y_axis.truncate(),
292             mat.z_axis.truncate(),
293         )
294     }
295 
296     /// Gets the minimal rotation for transforming `from` to `to`.  The rotation is in the
297     /// plane spanned by the two vectors.  Will rotate at most 180 degrees.
298     ///
299     /// The inputs must be unit vectors.
300     ///
301     /// `from_rotation_arc(from, to) * from ≈ to`.
302     ///
303     /// For near-singular cases (from≈to and from≈-to) the current implementation
304     /// is only accurate to about 0.001 (for `f32`).
305     ///
306     /// # Panics
307     ///
308     /// Will panic if `from` or `to` are not normalized when `glam_assert` is enabled.
309     #[must_use]
from_rotation_arc(from: Vec3, to: Vec3) -> Self310     pub fn from_rotation_arc(from: Vec3, to: Vec3) -> Self {
311         glam_assert!(from.is_normalized());
312         glam_assert!(to.is_normalized());
313 
314         const ONE_MINUS_EPS: f32 = 1.0 - 2.0 * f32::EPSILON;
315         let dot = from.dot(to);
316         if dot > ONE_MINUS_EPS {
317             // 0° singularity: from ≈ to
318             Self::IDENTITY
319         } else if dot < -ONE_MINUS_EPS {
320             // 180° singularity: from ≈ -to
321             use core::f32::consts::PI; // half a turn = ��/2 = 180°
322             Self::from_axis_angle(from.any_orthonormal_vector(), PI)
323         } else {
324             let c = from.cross(to);
325             Self::from_xyzw(c.x, c.y, c.z, 1.0 + dot).normalize()
326         }
327     }
328 
329     /// Gets the minimal rotation for transforming `from` to either `to` or `-to`.  This means
330     /// that the resulting quaternion will rotate `from` so that it is colinear with `to`.
331     ///
332     /// The rotation is in the plane spanned by the two vectors.  Will rotate at most 90
333     /// degrees.
334     ///
335     /// The inputs must be unit vectors.
336     ///
337     /// `to.dot(from_rotation_arc_colinear(from, to) * from).abs() ≈ 1`.
338     ///
339     /// # Panics
340     ///
341     /// Will panic if `from` or `to` are not normalized when `glam_assert` is enabled.
342     #[inline]
343     #[must_use]
from_rotation_arc_colinear(from: Vec3, to: Vec3) -> Self344     pub fn from_rotation_arc_colinear(from: Vec3, to: Vec3) -> Self {
345         if from.dot(to) < 0.0 {
346             Self::from_rotation_arc(from, -to)
347         } else {
348             Self::from_rotation_arc(from, to)
349         }
350     }
351 
352     /// Gets the minimal rotation for transforming `from` to `to`.  The resulting rotation is
353     /// around the z axis. Will rotate at most 180 degrees.
354     ///
355     /// The inputs must be unit vectors.
356     ///
357     /// `from_rotation_arc_2d(from, to) * from ≈ to`.
358     ///
359     /// For near-singular cases (from≈to and from≈-to) the current implementation
360     /// is only accurate to about 0.001 (for `f32`).
361     ///
362     /// # Panics
363     ///
364     /// Will panic if `from` or `to` are not normalized when `glam_assert` is enabled.
365     #[must_use]
from_rotation_arc_2d(from: Vec2, to: Vec2) -> Self366     pub fn from_rotation_arc_2d(from: Vec2, to: Vec2) -> Self {
367         glam_assert!(from.is_normalized());
368         glam_assert!(to.is_normalized());
369 
370         const ONE_MINUS_EPSILON: f32 = 1.0 - 2.0 * f32::EPSILON;
371         let dot = from.dot(to);
372         if dot > ONE_MINUS_EPSILON {
373             // 0° singularity: from ≈ to
374             Self::IDENTITY
375         } else if dot < -ONE_MINUS_EPSILON {
376             // 180° singularity: from ≈ -to
377             const COS_FRAC_PI_2: f32 = 0.0;
378             const SIN_FRAC_PI_2: f32 = 1.0;
379             // rotation around z by PI radians
380             Self::from_xyzw(0.0, 0.0, SIN_FRAC_PI_2, COS_FRAC_PI_2)
381         } else {
382             // vector3 cross where z=0
383             let z = from.x * to.y - to.x * from.y;
384             let w = 1.0 + dot;
385             // calculate length with x=0 and y=0 to normalize
386             let len_rcp = 1.0 / math::sqrt(z * z + w * w);
387             Self::from_xyzw(0.0, 0.0, z * len_rcp, w * len_rcp)
388         }
389     }
390 
391     /// Creates a quaterion rotation from a facing direction and an up direction.
392     ///
393     /// For a left-handed view coordinate system with `+X=right`, `+Y=up` and `+Z=forward`.
394     ///
395     /// # Panics
396     ///
397     /// Will panic if `up` is not normalized when `glam_assert` is enabled.
398     #[inline]
399     #[must_use]
look_to_lh(dir: Vec3, up: Vec3) -> Self400     pub fn look_to_lh(dir: Vec3, up: Vec3) -> Self {
401         Self::look_to_rh(-dir, up)
402     }
403 
404     /// Creates a quaterion rotation from facing direction and an up direction.
405     ///
406     /// For a right-handed view coordinate system with `+X=right`, `+Y=up` and `+Z=back`.
407     ///
408     /// # Panics
409     ///
410     /// Will panic if `dir` and `up` are not normalized when `glam_assert` is enabled.
411     #[inline]
412     #[must_use]
look_to_rh(dir: Vec3, up: Vec3) -> Self413     pub fn look_to_rh(dir: Vec3, up: Vec3) -> Self {
414         glam_assert!(dir.is_normalized());
415         glam_assert!(up.is_normalized());
416         let f = dir;
417         let s = f.cross(up).normalize();
418         let u = s.cross(f);
419 
420         Self::from_rotation_axes(
421             Vec3::new(s.x, u.x, -f.x),
422             Vec3::new(s.y, u.y, -f.y),
423             Vec3::new(s.z, u.z, -f.z),
424         )
425     }
426 
427     /// Creates a left-handed view matrix using a camera position, a focal point, and an up
428     /// direction.
429     ///
430     /// For a left-handed view coordinate system with `+X=right`, `+Y=up` and `+Z=forward`.
431     ///
432     /// # Panics
433     ///
434     /// Will panic if `up` is not normalized when `glam_assert` is enabled.
435     #[inline]
436     #[must_use]
look_at_lh(eye: Vec3, center: Vec3, up: Vec3) -> Self437     pub fn look_at_lh(eye: Vec3, center: Vec3, up: Vec3) -> Self {
438         Self::look_to_lh(center.sub(eye).normalize(), up)
439     }
440 
441     /// Creates a right-handed view matrix using a camera position, an up direction, and a focal
442     /// point.
443     ///
444     /// For a right-handed view coordinate system with `+X=right`, `+Y=up` and `+Z=back`.
445     ///
446     /// # Panics
447     ///
448     /// Will panic if `up` is not normalized when `glam_assert` is enabled.
449     #[inline]
look_at_rh(eye: Vec3, center: Vec3, up: Vec3) -> Self450     pub fn look_at_rh(eye: Vec3, center: Vec3, up: Vec3) -> Self {
451         Self::look_to_rh(center.sub(eye).normalize(), up)
452     }
453 
454     /// Returns the rotation axis (normalized) and angle (in radians) of `self`.
455     #[inline]
456     #[must_use]
to_axis_angle(self) -> (Vec3, f32)457     pub fn to_axis_angle(self) -> (Vec3, f32) {
458         const EPSILON: f32 = 1.0e-8;
459         let v = Vec3::new(self.x, self.y, self.z);
460         let length = v.length();
461         if length >= EPSILON {
462             let angle = 2.0 * math::atan2(length, self.w);
463             let axis = v / length;
464             (axis, angle)
465         } else {
466             (Vec3::X, 0.0)
467         }
468     }
469 
470     /// Returns the rotation axis scaled by the rotation in radians.
471     #[inline]
472     #[must_use]
to_scaled_axis(self) -> Vec3473     pub fn to_scaled_axis(self) -> Vec3 {
474         let (axis, angle) = self.to_axis_angle();
475         axis * angle
476     }
477 
478     /// Returns the rotation angles for the given euler rotation sequence.
479     #[inline]
480     #[must_use]
to_euler(self, order: EulerRot) -> (f32, f32, f32)481     pub fn to_euler(self, order: EulerRot) -> (f32, f32, f32) {
482         self.to_euler_angles(order)
483     }
484 
485     /// `[x, y, z, w]`
486     #[inline]
487     #[must_use]
to_array(&self) -> [f32; 4]488     pub fn to_array(&self) -> [f32; 4] {
489         [self.x, self.y, self.z, self.w]
490     }
491 
492     /// Returns the vector part of the quaternion.
493     #[inline]
494     #[must_use]
xyz(self) -> Vec3495     pub fn xyz(self) -> Vec3 {
496         Vec3::new(self.x, self.y, self.z)
497     }
498 
499     /// Returns the quaternion conjugate of `self`. For a unit quaternion the
500     /// conjugate is also the inverse.
501     #[inline]
502     #[must_use]
conjugate(self) -> Self503     pub fn conjugate(self) -> Self {
504         const SIGN: v128 = v128_from_f32x4([-1.0, -1.0, -1.0, 1.0]);
505         Self(f32x4_mul(self.0, SIGN))
506     }
507 
508     /// Returns the inverse of a normalized quaternion.
509     ///
510     /// Typically quaternion inverse returns the conjugate of a normalized quaternion.
511     /// Because `self` is assumed to already be unit length this method *does not* normalize
512     /// before returning the conjugate.
513     ///
514     /// # Panics
515     ///
516     /// Will panic if `self` is not normalized when `glam_assert` is enabled.
517     #[inline]
518     #[must_use]
inverse(self) -> Self519     pub fn inverse(self) -> Self {
520         glam_assert!(self.is_normalized());
521         self.conjugate()
522     }
523 
524     /// Computes the dot product of `self` and `rhs`. The dot product is
525     /// equal to the cosine of the angle between two quaternion rotations.
526     #[inline]
527     #[must_use]
dot(self, rhs: Self) -> f32528     pub fn dot(self, rhs: Self) -> f32 {
529         Vec4::from(self).dot(Vec4::from(rhs))
530     }
531 
532     /// Computes the length of `self`.
533     #[doc(alias = "magnitude")]
534     #[inline]
535     #[must_use]
length(self) -> f32536     pub fn length(self) -> f32 {
537         Vec4::from(self).length()
538     }
539 
540     /// Computes the squared length of `self`.
541     ///
542     /// This is generally faster than `length()` as it avoids a square
543     /// root operation.
544     #[doc(alias = "magnitude2")]
545     #[inline]
546     #[must_use]
length_squared(self) -> f32547     pub fn length_squared(self) -> f32 {
548         Vec4::from(self).length_squared()
549     }
550 
551     /// Computes `1.0 / length()`.
552     ///
553     /// For valid results, `self` must _not_ be of length zero.
554     #[inline]
555     #[must_use]
length_recip(self) -> f32556     pub fn length_recip(self) -> f32 {
557         Vec4::from(self).length_recip()
558     }
559 
560     /// Returns `self` normalized to length 1.0.
561     ///
562     /// For valid results, `self` must _not_ be of length zero.
563     ///
564     /// Panics
565     ///
566     /// Will panic if `self` is zero length when `glam_assert` is enabled.
567     #[inline]
568     #[must_use]
normalize(self) -> Self569     pub fn normalize(self) -> Self {
570         Self::from_vec4(Vec4::from(self).normalize())
571     }
572 
573     /// Returns `true` if, and only if, all elements are finite.
574     /// If any element is either `NaN`, positive or negative infinity, this will return `false`.
575     #[inline]
576     #[must_use]
is_finite(self) -> bool577     pub fn is_finite(self) -> bool {
578         Vec4::from(self).is_finite()
579     }
580 
581     /// Returns `true` if any elements are `NAN`.
582     #[inline]
583     #[must_use]
is_nan(self) -> bool584     pub fn is_nan(self) -> bool {
585         Vec4::from(self).is_nan()
586     }
587 
588     /// Returns whether `self` of length `1.0` or not.
589     ///
590     /// Uses a precision threshold of `1e-6`.
591     #[inline]
592     #[must_use]
is_normalized(self) -> bool593     pub fn is_normalized(self) -> bool {
594         Vec4::from(self).is_normalized()
595     }
596 
597     #[inline]
598     #[must_use]
is_near_identity(self) -> bool599     pub fn is_near_identity(self) -> bool {
600         // Based on https://github.com/nfrechette/rtm `rtm::quat_near_identity`
601         let threshold_angle = 0.002_847_144_6;
602         // Because of floating point precision, we cannot represent very small rotations.
603         // The closest f32 to 1.0 that is not 1.0 itself yields:
604         // 0.99999994.acos() * 2.0  = 0.000690533954 rad
605         //
606         // An error threshold of 1.e-6 is used by default.
607         // (1.0 - 1.e-6).acos() * 2.0 = 0.00284714461 rad
608         // (1.0 - 1.e-7).acos() * 2.0 = 0.00097656250 rad
609         //
610         // We don't really care about the angle value itself, only if it's close to 0.
611         // This will happen whenever quat.w is close to 1.0.
612         // If the quat.w is close to -1.0, the angle will be near 2*PI which is close to
613         // a negative 0 rotation. By forcing quat.w to be positive, we'll end up with
614         // the shortest path.
615         let positive_w_angle = math::acos_approx(math::abs(self.w)) * 2.0;
616         positive_w_angle < threshold_angle
617     }
618 
619     /// Returns the angle (in radians) for the minimal rotation
620     /// for transforming this quaternion into another.
621     ///
622     /// Both quaternions must be normalized.
623     ///
624     /// # Panics
625     ///
626     /// Will panic if `self` or `rhs` are not normalized when `glam_assert` is enabled.
627     #[inline]
628     #[must_use]
angle_between(self, rhs: Self) -> f32629     pub fn angle_between(self, rhs: Self) -> f32 {
630         glam_assert!(self.is_normalized() && rhs.is_normalized());
631         math::acos_approx(math::abs(self.dot(rhs))) * 2.0
632     }
633 
634     /// Rotates towards `rhs` up to `max_angle` (in radians).
635     ///
636     /// When `max_angle` is `0.0`, the result will be equal to `self`. When `max_angle` is equal to
637     /// `self.angle_between(rhs)`, the result will be equal to `rhs`. If `max_angle` is negative,
638     /// rotates towards the exact opposite of `rhs`. Will not go past the target.
639     ///
640     /// Both quaternions must be normalized.
641     ///
642     /// # Panics
643     ///
644     /// Will panic if `self` or `rhs` are not normalized when `glam_assert` is enabled.
645     #[inline]
646     #[must_use]
rotate_towards(&self, rhs: Self, max_angle: f32) -> Self647     pub fn rotate_towards(&self, rhs: Self, max_angle: f32) -> Self {
648         glam_assert!(self.is_normalized() && rhs.is_normalized());
649         let angle = self.angle_between(rhs);
650         if angle <= 1e-4 {
651             return rhs;
652         }
653         let s = (max_angle / angle).clamp(-1.0, 1.0);
654         self.slerp(rhs, s)
655     }
656 
657     /// Returns true if the absolute difference of all elements between `self` and `rhs`
658     /// is less than or equal to `max_abs_diff`.
659     ///
660     /// This can be used to compare if two quaternions contain similar elements. It works
661     /// best when comparing with a known value. The `max_abs_diff` that should be used used
662     /// depends on the values being compared against.
663     ///
664     /// For more see
665     /// [comparing floating point numbers](https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/).
666     #[inline]
667     #[must_use]
abs_diff_eq(self, rhs: Self, max_abs_diff: f32) -> bool668     pub fn abs_diff_eq(self, rhs: Self, max_abs_diff: f32) -> bool {
669         Vec4::from(self).abs_diff_eq(Vec4::from(rhs), max_abs_diff)
670     }
671 
672     #[inline(always)]
673     #[must_use]
lerp_impl(self, end: Self, s: f32) -> Self674     fn lerp_impl(self, end: Self, s: f32) -> Self {
675         (self * (1.0 - s) + end * s).normalize()
676     }
677 
678     /// Performs a linear interpolation between `self` and `rhs` based on
679     /// the value `s`.
680     ///
681     /// When `s` is `0.0`, the result will be equal to `self`.  When `s`
682     /// is `1.0`, the result will be equal to `rhs`.
683     ///
684     /// # Panics
685     ///
686     /// Will panic if `self` or `end` are not normalized when `glam_assert` is enabled.
687     #[doc(alias = "mix")]
688     #[inline]
689     #[must_use]
lerp(self, end: Self, s: f32) -> Self690     pub fn lerp(self, end: Self, s: f32) -> Self {
691         glam_assert!(self.is_normalized());
692         glam_assert!(end.is_normalized());
693 
694         const NEG_ZERO: v128 = v128_from_f32x4([-0.0; 4]);
695         let dot = dot4_into_v128(self.0, end.0);
696         // Calculate the bias, if the dot product is positive or zero, there is no bias
697         // but if it is negative, we want to flip the 'end' rotation XYZW components
698         let bias = v128_and(dot, NEG_ZERO);
699         self.lerp_impl(Self(v128_xor(end.0, bias)), s)
700     }
701 
702     /// Performs a spherical linear interpolation between `self` and `end`
703     /// based on the value `s`.
704     ///
705     /// When `s` is `0.0`, the result will be equal to `self`.  When `s`
706     /// is `1.0`, the result will be equal to `end`.
707     ///
708     /// # Panics
709     ///
710     /// Will panic if `self` or `end` are not normalized when `glam_assert` is enabled.
711     #[inline]
712     #[must_use]
slerp(self, mut end: Self, s: f32) -> Self713     pub fn slerp(self, mut end: Self, s: f32) -> Self {
714         // http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
715         glam_assert!(self.is_normalized());
716         glam_assert!(end.is_normalized());
717 
718         // Note that a rotation can be represented by two quaternions: `q` and
719         // `-q`. The slerp path between `q` and `end` will be different from the
720         // path between `-q` and `end`. One path will take the long way around and
721         // one will take the short way. In order to correct for this, the `dot`
722         // product between `self` and `end` should be positive. If the `dot`
723         // product is negative, slerp between `self` and `-end`.
724         let mut dot = self.dot(end);
725         if dot < 0.0 {
726             end = -end;
727             dot = -dot;
728         }
729 
730         const DOT_THRESHOLD: f32 = 1.0 - f32::EPSILON;
731         if dot > DOT_THRESHOLD {
732             // if above threshold perform linear interpolation to avoid divide by zero
733             self.lerp_impl(end, s)
734         } else {
735             let theta = math::acos_approx(dot);
736 
737             let scale1 = math::sin(theta * (1.0 - s));
738             let scale2 = math::sin(theta * s);
739             let theta_sin = math::sin(theta);
740             ((self * scale1) + (end * scale2)) * (1.0 / theta_sin)
741         }
742     }
743 
744     /// Multiplies a quaternion and a 3D vector, returning the rotated vector.
745     ///
746     /// # Panics
747     ///
748     /// Will panic if `self` is not normalized when `glam_assert` is enabled.
749     #[inline]
750     #[must_use]
mul_vec3(self, rhs: Vec3) -> Vec3751     pub fn mul_vec3(self, rhs: Vec3) -> Vec3 {
752         glam_assert!(self.is_normalized());
753 
754         self.mul_vec3a(rhs.into()).into()
755     }
756 
757     /// Multiplies two quaternions. If they each represent a rotation, the result will
758     /// represent the combined rotation.
759     ///
760     /// Note that due to floating point rounding the result may not be perfectly normalized.
761     ///
762     /// # Panics
763     ///
764     /// Will panic if `self` or `rhs` are not normalized when `glam_assert` is enabled.
765     #[inline]
766     #[must_use]
mul_quat(self, rhs: Self) -> Self767     pub fn mul_quat(self, rhs: Self) -> Self {
768         let lhs = self.0;
769         let rhs = rhs.0;
770 
771         const CONTROL_WZYX: v128 = v128_from_f32x4([1.0, -1.0, 1.0, -1.0]);
772         const CONTROL_ZWXY: v128 = v128_from_f32x4([1.0, 1.0, -1.0, -1.0]);
773         const CONTROL_YXWZ: v128 = v128_from_f32x4([-1.0, 1.0, 1.0, -1.0]);
774 
775         let r_xxxx = i32x4_shuffle::<0, 0, 4, 4>(lhs, lhs);
776         let r_yyyy = i32x4_shuffle::<1, 1, 5, 5>(lhs, lhs);
777         let r_zzzz = i32x4_shuffle::<2, 2, 6, 6>(lhs, lhs);
778         let r_wwww = i32x4_shuffle::<3, 3, 7, 7>(lhs, lhs);
779 
780         let lxrw_lyrw_lzrw_lwrw = f32x4_mul(r_wwww, rhs);
781         let l_wzyx = i32x4_shuffle::<3, 2, 5, 4>(rhs, rhs);
782 
783         let lwrx_lzrx_lyrx_lxrx = f32x4_mul(r_xxxx, l_wzyx);
784         let l_zwxy = i32x4_shuffle::<1, 0, 7, 6>(l_wzyx, l_wzyx);
785 
786         let lwrx_nlzrx_lyrx_nlxrx = f32x4_mul(lwrx_lzrx_lyrx_lxrx, CONTROL_WZYX);
787 
788         let lzry_lwry_lxry_lyry = f32x4_mul(r_yyyy, l_zwxy);
789         let l_yxwz = i32x4_shuffle::<3, 2, 5, 4>(l_zwxy, l_zwxy);
790 
791         let lzry_lwry_nlxry_nlyry = f32x4_mul(lzry_lwry_lxry_lyry, CONTROL_ZWXY);
792 
793         let lyrz_lxrz_lwrz_lzrz = f32x4_mul(r_zzzz, l_yxwz);
794         let result0 = f32x4_add(lxrw_lyrw_lzrw_lwrw, lwrx_nlzrx_lyrx_nlxrx);
795 
796         let nlyrz_lxrz_lwrz_wlzrz = f32x4_mul(lyrz_lxrz_lwrz_lzrz, CONTROL_YXWZ);
797         let result1 = f32x4_add(lzry_lwry_nlxry_nlyry, nlyrz_lxrz_lwrz_wlzrz);
798 
799         Self(f32x4_add(result0, result1))
800     }
801 
802     /// Creates a quaternion from a 3x3 rotation matrix inside a 3D affine transform.
803     ///
804     /// Note if the input affine matrix contain scales, shears, or other non-rotation
805     /// transformations then the resulting quaternion will be ill-defined.
806     ///
807     /// # Panics
808     ///
809     /// Will panic if any input affine matrix column is not normalized when `glam_assert` is
810     /// enabled.
811     #[inline]
812     #[must_use]
from_affine3(a: &crate::Affine3A) -> Self813     pub fn from_affine3(a: &crate::Affine3A) -> Self {
814         #[allow(clippy::useless_conversion)]
815         Self::from_rotation_axes(
816             a.matrix3.x_axis.into(),
817             a.matrix3.y_axis.into(),
818             a.matrix3.z_axis.into(),
819         )
820     }
821 
822     /// Multiplies a quaternion and a 3D vector, returning the rotated vector.
823     #[inline]
824     #[must_use]
mul_vec3a(self, rhs: Vec3A) -> Vec3A825     pub fn mul_vec3a(self, rhs: Vec3A) -> Vec3A {
826         const TWO: v128 = v128_from_f32x4([2.0; 4]);
827         let w = i32x4_shuffle::<3, 3, 7, 7>(self.0, self.0);
828         let b = self.0;
829         let b2 = dot3_into_v128(b, b);
830         Vec3A(f32x4_add(
831             f32x4_add(
832                 f32x4_mul(rhs.0, f32x4_sub(f32x4_mul(w, w), b2)),
833                 f32x4_mul(b, f32x4_mul(dot3_into_v128(rhs.0, b), TWO)),
834             ),
835             f32x4_mul(Vec3A(b).cross(rhs).into(), f32x4_mul(w, TWO)),
836         ))
837     }
838 
839     #[inline]
840     #[must_use]
as_dquat(self) -> DQuat841     pub fn as_dquat(self) -> DQuat {
842         DQuat::from_xyzw(self.x as f64, self.y as f64, self.z as f64, self.w as f64)
843     }
844 }
845 
846 impl fmt::Debug for Quat {
fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result847     fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
848         fmt.debug_tuple(stringify!(Quat))
849             .field(&self.x)
850             .field(&self.y)
851             .field(&self.z)
852             .field(&self.w)
853             .finish()
854     }
855 }
856 
857 impl fmt::Display for Quat {
fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result858     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
859         if let Some(p) = f.precision() {
860             write!(
861                 f,
862                 "[{:.*}, {:.*}, {:.*}, {:.*}]",
863                 p, self.x, p, self.y, p, self.z, p, self.w
864             )
865         } else {
866             write!(f, "[{}, {}, {}, {}]", self.x, self.y, self.z, self.w)
867         }
868     }
869 }
870 
871 impl Add<Quat> for Quat {
872     type Output = Self;
873     /// Adds two quaternions.
874     ///
875     /// The sum is not guaranteed to be normalized.
876     ///
877     /// Note that addition is not the same as combining the rotations represented by the
878     /// two quaternions! That corresponds to multiplication.
879     #[inline]
add(self, rhs: Self) -> Self880     fn add(self, rhs: Self) -> Self {
881         Self::from_vec4(Vec4::from(self) + Vec4::from(rhs))
882     }
883 }
884 
885 impl Sub<Quat> for Quat {
886     type Output = Self;
887     /// Subtracts the `rhs` quaternion from `self`.
888     ///
889     /// The difference is not guaranteed to be normalized.
890     #[inline]
sub(self, rhs: Self) -> Self891     fn sub(self, rhs: Self) -> Self {
892         Self::from_vec4(Vec4::from(self) - Vec4::from(rhs))
893     }
894 }
895 
896 impl Mul<f32> for Quat {
897     type Output = Self;
898     /// Multiplies a quaternion by a scalar value.
899     ///
900     /// The product is not guaranteed to be normalized.
901     #[inline]
mul(self, rhs: f32) -> Self902     fn mul(self, rhs: f32) -> Self {
903         Self::from_vec4(Vec4::from(self) * rhs)
904     }
905 }
906 
907 impl Div<f32> for Quat {
908     type Output = Self;
909     /// Divides a quaternion by a scalar value.
910     /// The quotient is not guaranteed to be normalized.
911     #[inline]
div(self, rhs: f32) -> Self912     fn div(self, rhs: f32) -> Self {
913         Self::from_vec4(Vec4::from(self) / rhs)
914     }
915 }
916 
917 impl Mul<Quat> for Quat {
918     type Output = Self;
919     /// Multiplies two quaternions. If they each represent a rotation, the result will
920     /// represent the combined rotation.
921     ///
922     /// Note that due to floating point rounding the result may not be perfectly
923     /// normalized.
924     ///
925     /// # Panics
926     ///
927     /// Will panic if `self` or `rhs` are not normalized when `glam_assert` is enabled.
928     #[inline]
mul(self, rhs: Self) -> Self929     fn mul(self, rhs: Self) -> Self {
930         self.mul_quat(rhs)
931     }
932 }
933 
934 impl MulAssign<Quat> for Quat {
935     /// Multiplies two quaternions. If they each represent a rotation, the result will
936     /// represent the combined rotation.
937     ///
938     /// Note that due to floating point rounding the result may not be perfectly
939     /// normalized.
940     ///
941     /// # Panics
942     ///
943     /// Will panic if `self` or `rhs` are not normalized when `glam_assert` is enabled.
944     #[inline]
mul_assign(&mut self, rhs: Self)945     fn mul_assign(&mut self, rhs: Self) {
946         *self = self.mul_quat(rhs);
947     }
948 }
949 
950 impl Mul<Vec3> for Quat {
951     type Output = Vec3;
952     /// Multiplies a quaternion and a 3D vector, returning the rotated vector.
953     ///
954     /// # Panics
955     ///
956     /// Will panic if `self` is not normalized when `glam_assert` is enabled.
957     #[inline]
mul(self, rhs: Vec3) -> Self::Output958     fn mul(self, rhs: Vec3) -> Self::Output {
959         self.mul_vec3(rhs)
960     }
961 }
962 
963 impl Neg for Quat {
964     type Output = Self;
965     #[inline]
neg(self) -> Self966     fn neg(self) -> Self {
967         self * -1.0
968     }
969 }
970 
971 impl Default for Quat {
972     #[inline]
default() -> Self973     fn default() -> Self {
974         Self::IDENTITY
975     }
976 }
977 
978 impl PartialEq for Quat {
979     #[inline]
eq(&self, rhs: &Self) -> bool980     fn eq(&self, rhs: &Self) -> bool {
981         Vec4::from(*self).eq(&Vec4::from(*rhs))
982     }
983 }
984 
985 #[cfg(not(target_arch = "spirv"))]
986 impl AsRef<[f32; 4]> for Quat {
987     #[inline]
as_ref(&self) -> &[f32; 4]988     fn as_ref(&self) -> &[f32; 4] {
989         unsafe { &*(self as *const Self as *const [f32; 4]) }
990     }
991 }
992 
993 impl Sum<Self> for Quat {
sum<I>(iter: I) -> Self where I: Iterator<Item = Self>,994     fn sum<I>(iter: I) -> Self
995     where
996         I: Iterator<Item = Self>,
997     {
998         iter.fold(Self::ZERO, Self::add)
999     }
1000 }
1001 
1002 impl<'a> Sum<&'a Self> for Quat {
sum<I>(iter: I) -> Self where I: Iterator<Item = &'a Self>,1003     fn sum<I>(iter: I) -> Self
1004     where
1005         I: Iterator<Item = &'a Self>,
1006     {
1007         iter.fold(Self::ZERO, |a, &b| Self::add(a, b))
1008     }
1009 }
1010 
1011 impl Product for Quat {
product<I>(iter: I) -> Self where I: Iterator<Item = Self>,1012     fn product<I>(iter: I) -> Self
1013     where
1014         I: Iterator<Item = Self>,
1015     {
1016         iter.fold(Self::IDENTITY, Self::mul)
1017     }
1018 }
1019 
1020 impl<'a> Product<&'a Self> for Quat {
product<I>(iter: I) -> Self where I: Iterator<Item = &'a Self>,1021     fn product<I>(iter: I) -> Self
1022     where
1023         I: Iterator<Item = &'a Self>,
1024     {
1025         iter.fold(Self::IDENTITY, |a, &b| Self::mul(a, b))
1026     }
1027 }
1028 
1029 impl Mul<Vec3A> for Quat {
1030     type Output = Vec3A;
1031     #[inline]
mul(self, rhs: Vec3A) -> Self::Output1032     fn mul(self, rhs: Vec3A) -> Self::Output {
1033         self.mul_vec3a(rhs)
1034     }
1035 }
1036 
1037 impl From<Quat> for Vec4 {
1038     #[inline]
from(q: Quat) -> Self1039     fn from(q: Quat) -> Self {
1040         Self(q.0)
1041     }
1042 }
1043 
1044 impl From<Quat> for (f32, f32, f32, f32) {
1045     #[inline]
from(q: Quat) -> Self1046     fn from(q: Quat) -> Self {
1047         Vec4::from(q).into()
1048     }
1049 }
1050 
1051 impl From<Quat> for [f32; 4] {
1052     #[inline]
from(q: Quat) -> Self1053     fn from(q: Quat) -> Self {
1054         Vec4::from(q).into()
1055     }
1056 }
1057 
1058 impl From<Quat> for v128 {
1059     #[inline]
from(q: Quat) -> Self1060     fn from(q: Quat) -> Self {
1061         q.0
1062     }
1063 }
1064 
1065 impl Deref for Quat {
1066     type Target = crate::deref::Vec4<f32>;
1067     #[inline]
deref(&self) -> &Self::Target1068     fn deref(&self) -> &Self::Target {
1069         unsafe { &*(self as *const Self).cast() }
1070     }
1071 }
1072 
1073 impl DerefMut for Quat {
1074     #[inline]
deref_mut(&mut self) -> &mut Self::Target1075     fn deref_mut(&mut self) -> &mut Self::Target {
1076         unsafe { &mut *(self as *mut Self).cast() }
1077     }
1078 }
1079