bevy_math/
direction.rs

1use crate::{
2    primitives::{Primitive2d, Primitive3d},
3    Quat, Rot2, Vec2, Vec3, Vec3A, Vec4,
4};
5
6use core::f32::consts::FRAC_1_SQRT_2;
7use core::fmt;
8use derive_more::derive::Into;
9
10#[cfg(feature = "bevy_reflect")]
11use bevy_reflect::Reflect;
12
13#[cfg(all(feature = "serialize", feature = "bevy_reflect"))]
14use bevy_reflect::{ReflectDeserialize, ReflectSerialize};
15
16#[cfg(all(debug_assertions, feature = "std"))]
17use std::eprintln;
18
19use thiserror::Error;
20
21/// An error indicating that a direction is invalid.
22#[derive(Debug, PartialEq, Error)]
23pub enum InvalidDirectionError {
24    /// The length of the direction vector is zero or very close to zero.
25    #[error("The length of the direction vector is zero or very close to zero")]
26    Zero,
27    /// The length of the direction vector is `std::f32::INFINITY`.
28    #[error("The length of the direction vector is `std::f32::INFINITY`")]
29    Infinite,
30    /// The length of the direction vector is `NaN`.
31    #[error("The length of the direction vector is `NaN`")]
32    NaN,
33}
34
35impl InvalidDirectionError {
36    /// Creates an [`InvalidDirectionError`] from the length of an invalid direction vector.
37    pub fn from_length(length: f32) -> Self {
38        if length.is_nan() {
39            InvalidDirectionError::NaN
40        } else if !length.is_finite() {
41            // If the direction is non-finite but also not NaN, it must be infinite
42            InvalidDirectionError::Infinite
43        } else {
44            // If the direction is invalid but neither NaN nor infinite, it must be zero
45            InvalidDirectionError::Zero
46        }
47    }
48}
49
50/// Checks that a vector with the given squared length is normalized.
51///
52/// Warns for small error with a length threshold of approximately `1e-4`,
53/// and panics for large error with a length threshold of approximately `1e-2`.
54///
55/// The format used for the logged warning is `"Warning: {warning} The length is {length}`,
56/// and similarly for the error.
57#[cfg(debug_assertions)]
58fn assert_is_normalized(message: &str, length_squared: f32) {
59    use crate::ops;
60
61    let length_error_squared = ops::abs(length_squared - 1.0);
62
63    // Panic for large error and warn for slight error.
64    if length_error_squared > 2e-2 || length_error_squared.is_nan() {
65        // Length error is approximately 1e-2 or more.
66        panic!(
67            "Error: {message} The length is {}.",
68            ops::sqrt(length_squared)
69        );
70    } else if length_error_squared > 2e-4 {
71        // Length error is approximately 1e-4 or more.
72        #[cfg(feature = "std")]
73        #[expect(clippy::print_stderr, reason = "Allowed behind `std` feature gate.")]
74        {
75            eprintln!(
76                "Warning: {message} The length is {}.",
77                ops::sqrt(length_squared)
78            );
79        }
80    }
81}
82
83/// A normalized vector pointing in a direction in 2D space
84#[derive(Clone, Copy, Debug, PartialEq)]
85#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
86#[cfg_attr(
87    feature = "bevy_reflect",
88    derive(Reflect),
89    reflect(Debug, PartialEq, Clone)
90)]
91#[cfg_attr(
92    all(feature = "serialize", feature = "bevy_reflect"),
93    reflect(Serialize, Deserialize)
94)]
95#[doc(alias = "Direction2d")]
96pub struct Dir2(Vec2);
97impl Primitive2d for Dir2 {}
98
99impl Dir2 {
100    /// A unit vector pointing along the positive X axis.
101    pub const X: Self = Self(Vec2::X);
102    /// A unit vector pointing along the positive Y axis.
103    pub const Y: Self = Self(Vec2::Y);
104    /// A unit vector pointing along the negative X axis.
105    pub const NEG_X: Self = Self(Vec2::NEG_X);
106    /// A unit vector pointing along the negative Y axis.
107    pub const NEG_Y: Self = Self(Vec2::NEG_Y);
108    /// The directional axes.
109    pub const AXES: [Self; 2] = [Self::X, Self::Y];
110
111    /// The "north" direction, equivalent to [`Dir2::Y`].
112    pub const NORTH: Self = Self(Vec2::Y);
113    /// The "south" direction, equivalent to [`Dir2::NEG_Y`].
114    pub const SOUTH: Self = Self(Vec2::NEG_Y);
115    /// The "east" direction, equivalent to [`Dir2::X`].
116    pub const EAST: Self = Self(Vec2::X);
117    /// The "west" direction, equivalent to [`Dir2::NEG_X`].
118    pub const WEST: Self = Self(Vec2::NEG_X);
119    /// The "north-east" direction, between [`Dir2::NORTH`] and [`Dir2::EAST`].
120    pub const NORTH_EAST: Self = Self(Vec2::new(FRAC_1_SQRT_2, FRAC_1_SQRT_2));
121    /// The "north-west" direction, between [`Dir2::NORTH`] and [`Dir2::WEST`].
122    pub const NORTH_WEST: Self = Self(Vec2::new(-FRAC_1_SQRT_2, FRAC_1_SQRT_2));
123    /// The "south-east" direction, between [`Dir2::SOUTH`] and [`Dir2::EAST`].
124    pub const SOUTH_EAST: Self = Self(Vec2::new(FRAC_1_SQRT_2, -FRAC_1_SQRT_2));
125    /// The "south-west" direction, between [`Dir2::SOUTH`] and [`Dir2::WEST`].
126    pub const SOUTH_WEST: Self = Self(Vec2::new(-FRAC_1_SQRT_2, -FRAC_1_SQRT_2));
127
128    /// Create a direction from a finite, nonzero [`Vec2`], normalizing it.
129    ///
130    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
131    /// of the given vector is zero (or very close to zero), infinite, or `NaN`.
132    pub fn new(value: Vec2) -> Result<Self, InvalidDirectionError> {
133        Self::new_and_length(value).map(|(dir, _)| dir)
134    }
135
136    /// Create a [`Dir2`] from a [`Vec2`] that is already normalized.
137    ///
138    /// # Warning
139    ///
140    /// `value` must be normalized, i.e its length must be `1.0`.
141    pub fn new_unchecked(value: Vec2) -> Self {
142        #[cfg(debug_assertions)]
143        assert_is_normalized(
144            "The vector given to `Dir2::new_unchecked` is not normalized.",
145            value.length_squared(),
146        );
147
148        Self(value)
149    }
150
151    /// Create a direction from a finite, nonzero [`Vec2`], normalizing it and
152    /// also returning its original length.
153    ///
154    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
155    /// of the given vector is zero (or very close to zero), infinite, or `NaN`.
156    pub fn new_and_length(value: Vec2) -> Result<(Self, f32), InvalidDirectionError> {
157        let length = value.length();
158        let direction = (length.is_finite() && length > 0.0).then_some(value / length);
159
160        direction
161            .map(|dir| (Self(dir), length))
162            .ok_or(InvalidDirectionError::from_length(length))
163    }
164
165    /// Create a direction from its `x` and `y` components.
166    ///
167    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
168    /// of the vector formed by the components is zero (or very close to zero), infinite, or `NaN`.
169    pub fn from_xy(x: f32, y: f32) -> Result<Self, InvalidDirectionError> {
170        Self::new(Vec2::new(x, y))
171    }
172
173    /// Create a direction from its `x` and `y` components, assuming the resulting vector is normalized.
174    ///
175    /// # Warning
176    ///
177    /// The vector produced from `x` and `y` must be normalized, i.e its length must be `1.0`.
178    pub fn from_xy_unchecked(x: f32, y: f32) -> Self {
179        Self::new_unchecked(Vec2::new(x, y))
180    }
181
182    /// Returns the inner [`Vec2`]
183    pub const fn as_vec2(&self) -> Vec2 {
184        self.0
185    }
186
187    /// Performs a spherical linear interpolation between `self` and `rhs`
188    /// based on the value `s`.
189    ///
190    /// This corresponds to interpolating between the two directions at a constant angular velocity.
191    ///
192    /// When `s == 0.0`, the result will be equal to `self`.
193    /// When `s == 1.0`, the result will be equal to `rhs`.
194    ///
195    /// # Example
196    ///
197    /// ```
198    /// # use bevy_math::Dir2;
199    /// # use approx::{assert_relative_eq, RelativeEq};
200    /// #
201    /// let dir1 = Dir2::X;
202    /// let dir2 = Dir2::Y;
203    ///
204    /// let result1 = dir1.slerp(dir2, 1.0 / 3.0);
205    /// #[cfg(feature = "approx")]
206    /// assert_relative_eq!(result1, Dir2::from_xy(0.75_f32.sqrt(), 0.5).unwrap());
207    ///
208    /// let result2 = dir1.slerp(dir2, 0.5);
209    /// #[cfg(feature = "approx")]
210    /// assert_relative_eq!(result2, Dir2::from_xy(0.5_f32.sqrt(), 0.5_f32.sqrt()).unwrap());
211    /// ```
212    #[inline]
213    pub fn slerp(self, rhs: Self, s: f32) -> Self {
214        let angle = self.angle_to(rhs.0);
215        Rot2::radians(angle * s) * self
216    }
217
218    /// Get the rotation that rotates this direction to `other`.
219    #[inline]
220    pub fn rotation_to(self, other: Self) -> Rot2 {
221        // Rotate `self` to X-axis, then X-axis to `other`:
222        other.rotation_from_x() * self.rotation_to_x()
223    }
224
225    /// Get the rotation that rotates `other` to this direction.
226    #[inline]
227    pub fn rotation_from(self, other: Self) -> Rot2 {
228        other.rotation_to(self)
229    }
230
231    /// Get the rotation that rotates the X-axis to this direction.
232    #[inline]
233    pub fn rotation_from_x(self) -> Rot2 {
234        Rot2::from_sin_cos(self.0.y, self.0.x)
235    }
236
237    /// Get the rotation that rotates this direction to the X-axis.
238    #[inline]
239    pub fn rotation_to_x(self) -> Rot2 {
240        // (This is cheap, it just negates one component.)
241        self.rotation_from_x().inverse()
242    }
243
244    /// Get the rotation that rotates the Y-axis to this direction.
245    #[inline]
246    pub fn rotation_from_y(self) -> Rot2 {
247        // `x <- y`, `y <- -x` correspond to rotating clockwise by pi/2;
248        // this transforms the Y-axis into the X-axis, maintaining the relative position
249        // of our direction. Then we just use the same technique as `rotation_from_x`.
250        Rot2::from_sin_cos(-self.0.x, self.0.y)
251    }
252
253    /// Get the rotation that rotates this direction to the Y-axis.
254    #[inline]
255    pub fn rotation_to_y(self) -> Rot2 {
256        self.rotation_from_y().inverse()
257    }
258
259    /// Returns `self` after an approximate normalization, assuming the value is already nearly normalized.
260    /// Useful for preventing numerical error accumulation.
261    /// See [`Dir3::fast_renormalize`] for an example of when such error accumulation might occur.
262    #[inline]
263    pub fn fast_renormalize(self) -> Self {
264        let length_squared = self.0.length_squared();
265        // Based on a Taylor approximation of the inverse square root, see [`Dir3::fast_renormalize`] for more details.
266        Self(self * (0.5 * (3.0 - length_squared)))
267    }
268}
269
270impl TryFrom<Vec2> for Dir2 {
271    type Error = InvalidDirectionError;
272
273    fn try_from(value: Vec2) -> Result<Self, Self::Error> {
274        Self::new(value)
275    }
276}
277
278impl From<Dir2> for Vec2 {
279    fn from(value: Dir2) -> Self {
280        value.as_vec2()
281    }
282}
283
284impl core::ops::Deref for Dir2 {
285    type Target = Vec2;
286    fn deref(&self) -> &Self::Target {
287        &self.0
288    }
289}
290
291impl core::ops::Neg for Dir2 {
292    type Output = Self;
293    fn neg(self) -> Self::Output {
294        Self(-self.0)
295    }
296}
297
298impl core::ops::Mul<f32> for Dir2 {
299    type Output = Vec2;
300    fn mul(self, rhs: f32) -> Self::Output {
301        self.0 * rhs
302    }
303}
304
305impl core::ops::Mul<Dir2> for f32 {
306    type Output = Vec2;
307    fn mul(self, rhs: Dir2) -> Self::Output {
308        self * rhs.0
309    }
310}
311
312impl core::ops::Mul<Dir2> for Rot2 {
313    type Output = Dir2;
314
315    /// Rotates the [`Dir2`] using a [`Rot2`].
316    fn mul(self, direction: Dir2) -> Self::Output {
317        let rotated = self * *direction;
318
319        #[cfg(debug_assertions)]
320        assert_is_normalized(
321            "`Dir2` is denormalized after rotation.",
322            rotated.length_squared(),
323        );
324
325        Dir2(rotated)
326    }
327}
328
329impl fmt::Display for Dir2 {
330    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
331        write!(f, "{}", self.0)
332    }
333}
334
335#[cfg(any(feature = "approx", test))]
336impl approx::AbsDiffEq for Dir2 {
337    type Epsilon = f32;
338    fn default_epsilon() -> f32 {
339        f32::EPSILON
340    }
341    fn abs_diff_eq(&self, other: &Self, epsilon: f32) -> bool {
342        self.as_ref().abs_diff_eq(other.as_ref(), epsilon)
343    }
344}
345
346#[cfg(any(feature = "approx", test))]
347impl approx::RelativeEq for Dir2 {
348    fn default_max_relative() -> f32 {
349        f32::EPSILON
350    }
351    fn relative_eq(&self, other: &Self, epsilon: f32, max_relative: f32) -> bool {
352        self.as_ref()
353            .relative_eq(other.as_ref(), epsilon, max_relative)
354    }
355}
356
357#[cfg(any(feature = "approx", test))]
358impl approx::UlpsEq for Dir2 {
359    fn default_max_ulps() -> u32 {
360        4
361    }
362    fn ulps_eq(&self, other: &Self, epsilon: f32, max_ulps: u32) -> bool {
363        self.as_ref().ulps_eq(other.as_ref(), epsilon, max_ulps)
364    }
365}
366
367/// A normalized vector pointing in a direction in 3D space
368#[derive(Clone, Copy, Debug, PartialEq, Into)]
369#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
370#[cfg_attr(
371    feature = "bevy_reflect",
372    derive(Reflect),
373    reflect(Debug, PartialEq, Clone)
374)]
375#[cfg_attr(
376    all(feature = "serialize", feature = "bevy_reflect"),
377    reflect(Serialize, Deserialize)
378)]
379#[doc(alias = "Direction3d")]
380pub struct Dir3(Vec3);
381impl Primitive3d for Dir3 {}
382
383impl Dir3 {
384    /// A unit vector pointing along the positive X axis.
385    pub const X: Self = Self(Vec3::X);
386    /// A unit vector pointing along the positive Y axis.
387    pub const Y: Self = Self(Vec3::Y);
388    /// A unit vector pointing along the positive Z axis.
389    pub const Z: Self = Self(Vec3::Z);
390    /// A unit vector pointing along the negative X axis.
391    pub const NEG_X: Self = Self(Vec3::NEG_X);
392    /// A unit vector pointing along the negative Y axis.
393    pub const NEG_Y: Self = Self(Vec3::NEG_Y);
394    /// A unit vector pointing along the negative Z axis.
395    pub const NEG_Z: Self = Self(Vec3::NEG_Z);
396    /// The directional axes.
397    pub const AXES: [Self; 3] = [Self::X, Self::Y, Self::Z];
398
399    /// Create a direction from a finite, nonzero [`Vec3`], normalizing it.
400    ///
401    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
402    /// of the given vector is zero (or very close to zero), infinite, or `NaN`.
403    pub fn new(value: Vec3) -> Result<Self, InvalidDirectionError> {
404        Self::new_and_length(value).map(|(dir, _)| dir)
405    }
406
407    /// Create a [`Dir3`] from a [`Vec3`] that is already normalized.
408    ///
409    /// # Warning
410    ///
411    /// `value` must be normalized, i.e its length must be `1.0`.
412    pub fn new_unchecked(value: Vec3) -> Self {
413        #[cfg(debug_assertions)]
414        assert_is_normalized(
415            "The vector given to `Dir3::new_unchecked` is not normalized.",
416            value.length_squared(),
417        );
418
419        Self(value)
420    }
421
422    /// Create a direction from a finite, nonzero [`Vec3`], normalizing it and
423    /// also returning its original length.
424    ///
425    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
426    /// of the given vector is zero (or very close to zero), infinite, or `NaN`.
427    pub fn new_and_length(value: Vec3) -> Result<(Self, f32), InvalidDirectionError> {
428        let length = value.length();
429        let direction = (length.is_finite() && length > 0.0).then_some(value / length);
430
431        direction
432            .map(|dir| (Self(dir), length))
433            .ok_or(InvalidDirectionError::from_length(length))
434    }
435
436    /// Create a direction from its `x`, `y`, and `z` components.
437    ///
438    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
439    /// of the vector formed by the components is zero (or very close to zero), infinite, or `NaN`.
440    pub fn from_xyz(x: f32, y: f32, z: f32) -> Result<Self, InvalidDirectionError> {
441        Self::new(Vec3::new(x, y, z))
442    }
443
444    /// Create a direction from its `x`, `y`, and `z` components, assuming the resulting vector is normalized.
445    ///
446    /// # Warning
447    ///
448    /// The vector produced from `x`, `y`, and `z` must be normalized, i.e its length must be `1.0`.
449    pub fn from_xyz_unchecked(x: f32, y: f32, z: f32) -> Self {
450        Self::new_unchecked(Vec3::new(x, y, z))
451    }
452
453    /// Returns the inner [`Vec3`]
454    pub const fn as_vec3(&self) -> Vec3 {
455        self.0
456    }
457
458    /// Performs a spherical linear interpolation between `self` and `rhs`
459    /// based on the value `s`.
460    ///
461    /// This corresponds to interpolating between the two directions at a constant angular velocity.
462    ///
463    /// When `s == 0.0`, the result will be equal to `self`.
464    /// When `s == 1.0`, the result will be equal to `rhs`.
465    ///
466    /// # Example
467    ///
468    /// ```
469    /// # use bevy_math::Dir3;
470    /// # use approx::{assert_relative_eq, RelativeEq};
471    /// #
472    /// let dir1 = Dir3::X;
473    /// let dir2 = Dir3::Y;
474    ///
475    /// let result1 = dir1.slerp(dir2, 1.0 / 3.0);
476    /// #[cfg(feature = "approx")]
477    /// assert_relative_eq!(
478    ///     result1,
479    ///     Dir3::from_xyz(0.75_f32.sqrt(), 0.5, 0.0).unwrap(),
480    ///     epsilon = 0.000001
481    /// );
482    ///
483    /// let result2 = dir1.slerp(dir2, 0.5);
484    /// #[cfg(feature = "approx")]
485    /// assert_relative_eq!(result2, Dir3::from_xyz(0.5_f32.sqrt(), 0.5_f32.sqrt(), 0.0).unwrap());
486    /// ```
487    #[inline]
488    pub fn slerp(self, rhs: Self, s: f32) -> Self {
489        let quat = Quat::IDENTITY.slerp(Quat::from_rotation_arc(self.0, rhs.0), s);
490        Dir3(quat.mul_vec3(self.0))
491    }
492
493    /// Returns `self` after an approximate normalization, assuming the value is already nearly normalized.
494    /// Useful for preventing numerical error accumulation.
495    ///
496    /// # Example
497    /// The following seemingly benign code would start accumulating errors over time,
498    /// leading to `dir` eventually not being normalized anymore.
499    /// ```
500    /// # use bevy_math::prelude::*;
501    /// # let N: usize = 200;
502    /// let mut dir = Dir3::X;
503    /// let quaternion = Quat::from_euler(EulerRot::XYZ, 1.0, 2.0, 3.0);
504    /// for i in 0..N {
505    ///     dir = quaternion * dir;
506    /// }
507    /// ```
508    /// Instead, do the following.
509    /// ```
510    /// # use bevy_math::prelude::*;
511    /// # let N: usize = 200;
512    /// let mut dir = Dir3::X;
513    /// let quaternion = Quat::from_euler(EulerRot::XYZ, 1.0, 2.0, 3.0);
514    /// for i in 0..N {
515    ///     dir = quaternion * dir;
516    ///     dir = dir.fast_renormalize();
517    /// }
518    /// ```
519    #[inline]
520    pub fn fast_renormalize(self) -> Self {
521        // We numerically approximate the inverse square root by a Taylor series around 1
522        // As we expect the error (x := length_squared - 1) to be small
523        // inverse_sqrt(length_squared) = (1 + x)^(-1/2) = 1 - 1/2 x + O(x²)
524        // inverse_sqrt(length_squared) ≈ 1 - 1/2 (length_squared - 1) = 1/2 (3 - length_squared)
525
526        // Iterative calls to this method quickly converge to a normalized value,
527        // so long as the denormalization is not large ~ O(1/10).
528        // One iteration can be described as:
529        // l_sq <- l_sq * (1 - 1/2 (l_sq - 1))²;
530        // Rewriting in terms of the error x:
531        // 1 + x <- (1 + x) * (1 - 1/2 x)²
532        // 1 + x <- (1 + x) * (1 - x + 1/4 x²)
533        // 1 + x <- 1 - x + 1/4 x² + x - x² + 1/4 x³
534        // x <- -1/4 x² (3 - x)
535        // If the error is small, say in a range of (-1/2, 1/2), then:
536        // |-1/4 x² (3 - x)| <= (3/4 + 1/4 * |x|) * x² <= (3/4 + 1/4 * 1/2) * x² < x² < 1/2 x
537        // Therefore the sequence of iterates converges to 0 error as a second order method.
538
539        let length_squared = self.0.length_squared();
540        Self(self * (0.5 * (3.0 - length_squared)))
541    }
542}
543
544impl TryFrom<Vec3> for Dir3 {
545    type Error = InvalidDirectionError;
546
547    fn try_from(value: Vec3) -> Result<Self, Self::Error> {
548        Self::new(value)
549    }
550}
551
552impl core::ops::Deref for Dir3 {
553    type Target = Vec3;
554    fn deref(&self) -> &Self::Target {
555        &self.0
556    }
557}
558
559impl core::ops::Neg for Dir3 {
560    type Output = Self;
561    fn neg(self) -> Self::Output {
562        Self(-self.0)
563    }
564}
565
566impl core::ops::Mul<f32> for Dir3 {
567    type Output = Vec3;
568    fn mul(self, rhs: f32) -> Self::Output {
569        self.0 * rhs
570    }
571}
572
573impl core::ops::Mul<Dir3> for f32 {
574    type Output = Vec3;
575    fn mul(self, rhs: Dir3) -> Self::Output {
576        self * rhs.0
577    }
578}
579
580impl core::ops::Mul<Dir3> for Quat {
581    type Output = Dir3;
582
583    /// Rotates the [`Dir3`] using a [`Quat`].
584    fn mul(self, direction: Dir3) -> Self::Output {
585        let rotated = self * *direction;
586
587        #[cfg(debug_assertions)]
588        assert_is_normalized(
589            "`Dir3` is denormalized after rotation.",
590            rotated.length_squared(),
591        );
592
593        Dir3(rotated)
594    }
595}
596
597impl fmt::Display for Dir3 {
598    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
599        write!(f, "{}", self.0)
600    }
601}
602
603#[cfg(feature = "approx")]
604impl approx::AbsDiffEq for Dir3 {
605    type Epsilon = f32;
606    fn default_epsilon() -> f32 {
607        f32::EPSILON
608    }
609    fn abs_diff_eq(&self, other: &Self, epsilon: f32) -> bool {
610        self.as_ref().abs_diff_eq(other.as_ref(), epsilon)
611    }
612}
613
614#[cfg(feature = "approx")]
615impl approx::RelativeEq for Dir3 {
616    fn default_max_relative() -> f32 {
617        f32::EPSILON
618    }
619    fn relative_eq(&self, other: &Self, epsilon: f32, max_relative: f32) -> bool {
620        self.as_ref()
621            .relative_eq(other.as_ref(), epsilon, max_relative)
622    }
623}
624
625#[cfg(feature = "approx")]
626impl approx::UlpsEq for Dir3 {
627    fn default_max_ulps() -> u32 {
628        4
629    }
630    fn ulps_eq(&self, other: &Self, epsilon: f32, max_ulps: u32) -> bool {
631        self.as_ref().ulps_eq(other.as_ref(), epsilon, max_ulps)
632    }
633}
634
635/// A normalized SIMD vector pointing in a direction in 3D space.
636///
637/// This type stores a 16 byte aligned [`Vec3A`].
638/// This may or may not be faster than [`Dir3`]: make sure to benchmark!
639#[derive(Clone, Copy, Debug, PartialEq)]
640#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
641#[cfg_attr(
642    feature = "bevy_reflect",
643    derive(Reflect),
644    reflect(Debug, PartialEq, Clone)
645)]
646#[cfg_attr(
647    all(feature = "serialize", feature = "bevy_reflect"),
648    reflect(Serialize, Deserialize)
649)]
650#[doc(alias = "Direction3dA")]
651pub struct Dir3A(Vec3A);
652impl Primitive3d for Dir3A {}
653
654impl Dir3A {
655    /// A unit vector pointing along the positive X axis.
656    pub const X: Self = Self(Vec3A::X);
657    /// A unit vector pointing along the positive Y axis.
658    pub const Y: Self = Self(Vec3A::Y);
659    /// A unit vector pointing along the positive Z axis.
660    pub const Z: Self = Self(Vec3A::Z);
661    /// A unit vector pointing along the negative X axis.
662    pub const NEG_X: Self = Self(Vec3A::NEG_X);
663    /// A unit vector pointing along the negative Y axis.
664    pub const NEG_Y: Self = Self(Vec3A::NEG_Y);
665    /// A unit vector pointing along the negative Z axis.
666    pub const NEG_Z: Self = Self(Vec3A::NEG_Z);
667    /// The directional axes.
668    pub const AXES: [Self; 3] = [Self::X, Self::Y, Self::Z];
669
670    /// Create a direction from a finite, nonzero [`Vec3A`], normalizing it.
671    ///
672    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
673    /// of the given vector is zero (or very close to zero), infinite, or `NaN`.
674    pub fn new(value: Vec3A) -> Result<Self, InvalidDirectionError> {
675        Self::new_and_length(value).map(|(dir, _)| dir)
676    }
677
678    /// Create a [`Dir3A`] from a [`Vec3A`] that is already normalized.
679    ///
680    /// # Warning
681    ///
682    /// `value` must be normalized, i.e its length must be `1.0`.
683    pub fn new_unchecked(value: Vec3A) -> Self {
684        #[cfg(debug_assertions)]
685        assert_is_normalized(
686            "The vector given to `Dir3A::new_unchecked` is not normalized.",
687            value.length_squared(),
688        );
689
690        Self(value)
691    }
692
693    /// Create a direction from a finite, nonzero [`Vec3A`], normalizing it and
694    /// also returning its original length.
695    ///
696    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
697    /// of the given vector is zero (or very close to zero), infinite, or `NaN`.
698    pub fn new_and_length(value: Vec3A) -> Result<(Self, f32), InvalidDirectionError> {
699        let length = value.length();
700        let direction = (length.is_finite() && length > 0.0).then_some(value / length);
701
702        direction
703            .map(|dir| (Self(dir), length))
704            .ok_or(InvalidDirectionError::from_length(length))
705    }
706
707    /// Create a direction from its `x`, `y`, and `z` components.
708    ///
709    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
710    /// of the vector formed by the components is zero (or very close to zero), infinite, or `NaN`.
711    pub fn from_xyz(x: f32, y: f32, z: f32) -> Result<Self, InvalidDirectionError> {
712        Self::new(Vec3A::new(x, y, z))
713    }
714
715    /// Create a direction from its `x`, `y`, and `z` components, assuming the resulting vector is normalized.
716    ///
717    /// # Warning
718    ///
719    /// The vector produced from `x`, `y`, and `z` must be normalized, i.e its length must be `1.0`.
720    pub fn from_xyz_unchecked(x: f32, y: f32, z: f32) -> Self {
721        Self::new_unchecked(Vec3A::new(x, y, z))
722    }
723
724    /// Returns the inner [`Vec3A`]
725    pub const fn as_vec3a(&self) -> Vec3A {
726        self.0
727    }
728
729    /// Performs a spherical linear interpolation between `self` and `rhs`
730    /// based on the value `s`.
731    ///
732    /// This corresponds to interpolating between the two directions at a constant angular velocity.
733    ///
734    /// When `s == 0.0`, the result will be equal to `self`.
735    /// When `s == 1.0`, the result will be equal to `rhs`.
736    ///
737    /// # Example
738    ///
739    /// ```
740    /// # use bevy_math::Dir3A;
741    /// # use approx::{assert_relative_eq, RelativeEq};
742    /// #
743    /// let dir1 = Dir3A::X;
744    /// let dir2 = Dir3A::Y;
745    ///
746    /// let result1 = dir1.slerp(dir2, 1.0 / 3.0);
747    /// #[cfg(feature = "approx")]
748    /// assert_relative_eq!(
749    ///     result1,
750    ///     Dir3A::from_xyz(0.75_f32.sqrt(), 0.5, 0.0).unwrap(),
751    ///     epsilon = 0.000001
752    /// );
753    ///
754    /// let result2 = dir1.slerp(dir2, 0.5);
755    /// #[cfg(feature = "approx")]
756    /// assert_relative_eq!(result2, Dir3A::from_xyz(0.5_f32.sqrt(), 0.5_f32.sqrt(), 0.0).unwrap());
757    /// ```
758    #[inline]
759    pub fn slerp(self, rhs: Self, s: f32) -> Self {
760        let quat = Quat::IDENTITY.slerp(
761            Quat::from_rotation_arc(Vec3::from(self.0), Vec3::from(rhs.0)),
762            s,
763        );
764        Dir3A(quat.mul_vec3a(self.0))
765    }
766
767    /// Returns `self` after an approximate normalization, assuming the value is already nearly normalized.
768    /// Useful for preventing numerical error accumulation.
769    ///
770    /// See [`Dir3::fast_renormalize`] for an example of when such error accumulation might occur.
771    #[inline]
772    pub fn fast_renormalize(self) -> Self {
773        let length_squared = self.0.length_squared();
774        // Based on a Taylor approximation of the inverse square root, see [`Dir3::fast_renormalize`] for more details.
775        Self(self * (0.5 * (3.0 - length_squared)))
776    }
777}
778
779impl From<Dir3> for Dir3A {
780    fn from(value: Dir3) -> Self {
781        Self(value.0.into())
782    }
783}
784
785impl From<Dir3A> for Dir3 {
786    fn from(value: Dir3A) -> Self {
787        Self(value.0.into())
788    }
789}
790
791impl TryFrom<Vec3A> for Dir3A {
792    type Error = InvalidDirectionError;
793
794    fn try_from(value: Vec3A) -> Result<Self, Self::Error> {
795        Self::new(value)
796    }
797}
798
799impl From<Dir3A> for Vec3A {
800    fn from(value: Dir3A) -> Self {
801        value.0
802    }
803}
804
805impl core::ops::Deref for Dir3A {
806    type Target = Vec3A;
807    fn deref(&self) -> &Self::Target {
808        &self.0
809    }
810}
811
812impl core::ops::Neg for Dir3A {
813    type Output = Self;
814    fn neg(self) -> Self::Output {
815        Self(-self.0)
816    }
817}
818
819impl core::ops::Mul<f32> for Dir3A {
820    type Output = Vec3A;
821    fn mul(self, rhs: f32) -> Self::Output {
822        self.0 * rhs
823    }
824}
825
826impl core::ops::Mul<Dir3A> for f32 {
827    type Output = Vec3A;
828    fn mul(self, rhs: Dir3A) -> Self::Output {
829        self * rhs.0
830    }
831}
832
833impl core::ops::Mul<Dir3A> for Quat {
834    type Output = Dir3A;
835
836    /// Rotates the [`Dir3A`] using a [`Quat`].
837    fn mul(self, direction: Dir3A) -> Self::Output {
838        let rotated = self * *direction;
839
840        #[cfg(debug_assertions)]
841        assert_is_normalized(
842            "`Dir3A` is denormalized after rotation.",
843            rotated.length_squared(),
844        );
845
846        Dir3A(rotated)
847    }
848}
849
850impl fmt::Display for Dir3A {
851    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
852        write!(f, "{}", self.0)
853    }
854}
855
856#[cfg(feature = "approx")]
857impl approx::AbsDiffEq for Dir3A {
858    type Epsilon = f32;
859    fn default_epsilon() -> f32 {
860        f32::EPSILON
861    }
862    fn abs_diff_eq(&self, other: &Self, epsilon: f32) -> bool {
863        self.as_ref().abs_diff_eq(other.as_ref(), epsilon)
864    }
865}
866
867#[cfg(feature = "approx")]
868impl approx::RelativeEq for Dir3A {
869    fn default_max_relative() -> f32 {
870        f32::EPSILON
871    }
872    fn relative_eq(&self, other: &Self, epsilon: f32, max_relative: f32) -> bool {
873        self.as_ref()
874            .relative_eq(other.as_ref(), epsilon, max_relative)
875    }
876}
877
878#[cfg(feature = "approx")]
879impl approx::UlpsEq for Dir3A {
880    fn default_max_ulps() -> u32 {
881        4
882    }
883    fn ulps_eq(&self, other: &Self, epsilon: f32, max_ulps: u32) -> bool {
884        self.as_ref().ulps_eq(other.as_ref(), epsilon, max_ulps)
885    }
886}
887
888/// A normalized vector pointing in a direction in 4D space
889#[derive(Clone, Copy, Debug, PartialEq, Into)]
890#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
891#[cfg_attr(
892    feature = "bevy_reflect",
893    derive(Reflect),
894    reflect(Debug, PartialEq, Clone)
895)]
896#[cfg_attr(
897    all(feature = "serialize", feature = "bevy_reflect"),
898    reflect(Serialize, Deserialize)
899)]
900#[doc(alias = "Direction4d")]
901pub struct Dir4(Vec4);
902
903impl Dir4 {
904    /// A unit vector pointing along the positive X axis
905    pub const X: Self = Self(Vec4::X);
906    /// A unit vector pointing along the positive Y axis.
907    pub const Y: Self = Self(Vec4::Y);
908    /// A unit vector pointing along the positive Z axis.
909    pub const Z: Self = Self(Vec4::Z);
910    /// A unit vector pointing along the positive W axis.
911    pub const W: Self = Self(Vec4::W);
912    /// A unit vector pointing along the negative X axis.
913    pub const NEG_X: Self = Self(Vec4::NEG_X);
914    /// A unit vector pointing along the negative Y axis.
915    pub const NEG_Y: Self = Self(Vec4::NEG_Y);
916    /// A unit vector pointing along the negative Z axis.
917    pub const NEG_Z: Self = Self(Vec4::NEG_Z);
918    /// A unit vector pointing along the negative W axis.
919    pub const NEG_W: Self = Self(Vec4::NEG_W);
920    /// The directional axes.
921    pub const AXES: [Self; 4] = [Self::X, Self::Y, Self::Z, Self::W];
922
923    /// Create a direction from a finite, nonzero [`Vec4`], normalizing it.
924    ///
925    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
926    /// of the given vector is zero (or very close to zero), infinite, or `NaN`.
927    pub fn new(value: Vec4) -> Result<Self, InvalidDirectionError> {
928        Self::new_and_length(value).map(|(dir, _)| dir)
929    }
930
931    /// Create a [`Dir4`] from a [`Vec4`] that is already normalized.
932    ///
933    /// # Warning
934    ///
935    /// `value` must be normalized, i.e its length must be `1.0`.
936    pub fn new_unchecked(value: Vec4) -> Self {
937        #[cfg(debug_assertions)]
938        assert_is_normalized(
939            "The vector given to `Dir4::new_unchecked` is not normalized.",
940            value.length_squared(),
941        );
942        Self(value)
943    }
944
945    /// Create a direction from a finite, nonzero [`Vec4`], normalizing it and
946    /// also returning its original length.
947    ///
948    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
949    /// of the given vector is zero (or very close to zero), infinite, or `NaN`.
950    pub fn new_and_length(value: Vec4) -> Result<(Self, f32), InvalidDirectionError> {
951        let length = value.length();
952        let direction = (length.is_finite() && length > 0.0).then_some(value / length);
953
954        direction
955            .map(|dir| (Self(dir), length))
956            .ok_or(InvalidDirectionError::from_length(length))
957    }
958
959    /// Create a direction from its `x`, `y`, `z`, and `w` components.
960    ///
961    /// Returns [`Err(InvalidDirectionError)`](InvalidDirectionError) if the length
962    /// of the vector formed by the components is zero (or very close to zero), infinite, or `NaN`.
963    pub fn from_xyzw(x: f32, y: f32, z: f32, w: f32) -> Result<Self, InvalidDirectionError> {
964        Self::new(Vec4::new(x, y, z, w))
965    }
966
967    /// Create a direction from its `x`, `y`, `z`, and `w` components, assuming the resulting vector is normalized.
968    ///
969    /// # Warning
970    ///
971    /// The vector produced from `x`, `y`, `z`, and `w` must be normalized, i.e its length must be `1.0`.
972    pub fn from_xyzw_unchecked(x: f32, y: f32, z: f32, w: f32) -> Self {
973        Self::new_unchecked(Vec4::new(x, y, z, w))
974    }
975
976    /// Returns the inner [`Vec4`]
977    pub const fn as_vec4(&self) -> Vec4 {
978        self.0
979    }
980
981    /// Returns `self` after an approximate normalization, assuming the value is already nearly normalized.
982    /// Useful for preventing numerical error accumulation.
983    #[inline]
984    pub fn fast_renormalize(self) -> Self {
985        // We numerically approximate the inverse square root by a Taylor series around 1
986        // As we expect the error (x := length_squared - 1) to be small
987        // inverse_sqrt(length_squared) = (1 + x)^(-1/2) = 1 - 1/2 x + O(x²)
988        // inverse_sqrt(length_squared) ≈ 1 - 1/2 (length_squared - 1) = 1/2 (3 - length_squared)
989
990        // Iterative calls to this method quickly converge to a normalized value,
991        // so long as the denormalization is not large ~ O(1/10).
992        // One iteration can be described as:
993        // l_sq <- l_sq * (1 - 1/2 (l_sq - 1))²;
994        // Rewriting in terms of the error x:
995        // 1 + x <- (1 + x) * (1 - 1/2 x)²
996        // 1 + x <- (1 + x) * (1 - x + 1/4 x²)
997        // 1 + x <- 1 - x + 1/4 x² + x - x² + 1/4 x³
998        // x <- -1/4 x² (3 - x)
999        // If the error is small, say in a range of (-1/2, 1/2), then:
1000        // |-1/4 x² (3 - x)| <= (3/4 + 1/4 * |x|) * x² <= (3/4 + 1/4 * 1/2) * x² < x² < 1/2 x
1001        // Therefore the sequence of iterates converges to 0 error as a second order method.
1002
1003        let length_squared = self.0.length_squared();
1004        Self(self * (0.5 * (3.0 - length_squared)))
1005    }
1006}
1007
1008impl TryFrom<Vec4> for Dir4 {
1009    type Error = InvalidDirectionError;
1010
1011    fn try_from(value: Vec4) -> Result<Self, Self::Error> {
1012        Self::new(value)
1013    }
1014}
1015
1016impl core::ops::Deref for Dir4 {
1017    type Target = Vec4;
1018    fn deref(&self) -> &Self::Target {
1019        &self.0
1020    }
1021}
1022
1023impl core::ops::Neg for Dir4 {
1024    type Output = Self;
1025    fn neg(self) -> Self::Output {
1026        Self(-self.0)
1027    }
1028}
1029
1030impl core::ops::Mul<f32> for Dir4 {
1031    type Output = Vec4;
1032    fn mul(self, rhs: f32) -> Self::Output {
1033        self.0 * rhs
1034    }
1035}
1036
1037impl core::ops::Mul<Dir4> for f32 {
1038    type Output = Vec4;
1039    fn mul(self, rhs: Dir4) -> Self::Output {
1040        self * rhs.0
1041    }
1042}
1043
1044impl fmt::Display for Dir4 {
1045    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
1046        write!(f, "{}", self.0)
1047    }
1048}
1049
1050#[cfg(feature = "approx")]
1051impl approx::AbsDiffEq for Dir4 {
1052    type Epsilon = f32;
1053    fn default_epsilon() -> f32 {
1054        f32::EPSILON
1055    }
1056    fn abs_diff_eq(&self, other: &Self, epsilon: f32) -> bool {
1057        self.as_ref().abs_diff_eq(other.as_ref(), epsilon)
1058    }
1059}
1060
1061#[cfg(feature = "approx")]
1062impl approx::RelativeEq for Dir4 {
1063    fn default_max_relative() -> f32 {
1064        f32::EPSILON
1065    }
1066    fn relative_eq(&self, other: &Self, epsilon: f32, max_relative: f32) -> bool {
1067        self.as_ref()
1068            .relative_eq(other.as_ref(), epsilon, max_relative)
1069    }
1070}
1071
1072#[cfg(feature = "approx")]
1073impl approx::UlpsEq for Dir4 {
1074    fn default_max_ulps() -> u32 {
1075        4
1076    }
1077
1078    fn ulps_eq(&self, other: &Self, epsilon: f32, max_ulps: u32) -> bool {
1079        self.as_ref().ulps_eq(other.as_ref(), epsilon, max_ulps)
1080    }
1081}
1082
1083#[cfg(test)]
1084#[cfg(feature = "approx")]
1085mod tests {
1086    use crate::ops;
1087
1088    use super::*;
1089    use approx::assert_relative_eq;
1090
1091    #[test]
1092    fn dir2_creation() {
1093        assert_eq!(Dir2::new(Vec2::X * 12.5), Ok(Dir2::X));
1094        assert_eq!(
1095            Dir2::new(Vec2::new(0.0, 0.0)),
1096            Err(InvalidDirectionError::Zero)
1097        );
1098        assert_eq!(
1099            Dir2::new(Vec2::new(f32::INFINITY, 0.0)),
1100            Err(InvalidDirectionError::Infinite)
1101        );
1102        assert_eq!(
1103            Dir2::new(Vec2::new(f32::NEG_INFINITY, 0.0)),
1104            Err(InvalidDirectionError::Infinite)
1105        );
1106        assert_eq!(
1107            Dir2::new(Vec2::new(f32::NAN, 0.0)),
1108            Err(InvalidDirectionError::NaN)
1109        );
1110        assert_eq!(Dir2::new_and_length(Vec2::X * 6.5), Ok((Dir2::X, 6.5)));
1111    }
1112
1113    #[test]
1114    fn dir2_slerp() {
1115        assert_relative_eq!(
1116            Dir2::X.slerp(Dir2::Y, 0.5),
1117            Dir2::from_xy(ops::sqrt(0.5_f32), ops::sqrt(0.5_f32)).unwrap()
1118        );
1119        assert_eq!(Dir2::Y.slerp(Dir2::X, 0.0), Dir2::Y);
1120        assert_relative_eq!(Dir2::X.slerp(Dir2::Y, 1.0), Dir2::Y);
1121        assert_relative_eq!(
1122            Dir2::Y.slerp(Dir2::X, 1.0 / 3.0),
1123            Dir2::from_xy(0.5, ops::sqrt(0.75_f32)).unwrap()
1124        );
1125        assert_relative_eq!(
1126            Dir2::X.slerp(Dir2::Y, 2.0 / 3.0),
1127            Dir2::from_xy(0.5, ops::sqrt(0.75_f32)).unwrap()
1128        );
1129    }
1130
1131    #[test]
1132    fn dir2_to_rotation2d() {
1133        assert_relative_eq!(Dir2::EAST.rotation_to(Dir2::NORTH_EAST), Rot2::FRAC_PI_4);
1134        assert_relative_eq!(Dir2::NORTH.rotation_from(Dir2::NORTH_EAST), Rot2::FRAC_PI_4);
1135        assert_relative_eq!(Dir2::SOUTH.rotation_to_x(), Rot2::FRAC_PI_2);
1136        assert_relative_eq!(Dir2::SOUTH.rotation_to_y(), Rot2::PI);
1137        assert_relative_eq!(Dir2::NORTH_WEST.rotation_from_x(), Rot2::degrees(135.0));
1138        assert_relative_eq!(Dir2::NORTH_WEST.rotation_from_y(), Rot2::FRAC_PI_4);
1139    }
1140
1141    #[test]
1142    fn dir2_renorm() {
1143        // Evil denormalized Rot2
1144        let (sin, cos) = ops::sin_cos(1.0_f32);
1145        let rot2 = Rot2::from_sin_cos(sin * (1.0 + 1e-5), cos * (1.0 + 1e-5));
1146        let mut dir_a = Dir2::X;
1147        let mut dir_b = Dir2::X;
1148
1149        // We test that renormalizing an already normalized dir doesn't do anything
1150        assert_relative_eq!(dir_b, dir_b.fast_renormalize(), epsilon = 0.000001);
1151
1152        for _ in 0..50 {
1153            dir_a = rot2 * dir_a;
1154            dir_b = rot2 * dir_b;
1155            dir_b = dir_b.fast_renormalize();
1156        }
1157
1158        // `dir_a` should've gotten denormalized, meanwhile `dir_b` should stay normalized.
1159        assert!(
1160            !dir_a.is_normalized(),
1161            "Denormalization doesn't work, test is faulty"
1162        );
1163        assert!(dir_b.is_normalized(), "Renormalisation did not work.");
1164    }
1165
1166    #[test]
1167    fn dir3_creation() {
1168        assert_eq!(Dir3::new(Vec3::X * 12.5), Ok(Dir3::X));
1169        assert_eq!(
1170            Dir3::new(Vec3::new(0.0, 0.0, 0.0)),
1171            Err(InvalidDirectionError::Zero)
1172        );
1173        assert_eq!(
1174            Dir3::new(Vec3::new(f32::INFINITY, 0.0, 0.0)),
1175            Err(InvalidDirectionError::Infinite)
1176        );
1177        assert_eq!(
1178            Dir3::new(Vec3::new(f32::NEG_INFINITY, 0.0, 0.0)),
1179            Err(InvalidDirectionError::Infinite)
1180        );
1181        assert_eq!(
1182            Dir3::new(Vec3::new(f32::NAN, 0.0, 0.0)),
1183            Err(InvalidDirectionError::NaN)
1184        );
1185        assert_eq!(Dir3::new_and_length(Vec3::X * 6.5), Ok((Dir3::X, 6.5)));
1186
1187        // Test rotation
1188        assert!(
1189            (Quat::from_rotation_z(core::f32::consts::FRAC_PI_2) * Dir3::X)
1190                .abs_diff_eq(Vec3::Y, 10e-6)
1191        );
1192    }
1193
1194    #[test]
1195    fn dir3_slerp() {
1196        assert_relative_eq!(
1197            Dir3::X.slerp(Dir3::Y, 0.5),
1198            Dir3::from_xyz(ops::sqrt(0.5f32), ops::sqrt(0.5f32), 0.0).unwrap()
1199        );
1200        assert_relative_eq!(Dir3::Y.slerp(Dir3::Z, 0.0), Dir3::Y);
1201        assert_relative_eq!(Dir3::Z.slerp(Dir3::X, 1.0), Dir3::X, epsilon = 0.000001);
1202        assert_relative_eq!(
1203            Dir3::X.slerp(Dir3::Z, 1.0 / 3.0),
1204            Dir3::from_xyz(ops::sqrt(0.75f32), 0.0, 0.5).unwrap(),
1205            epsilon = 0.000001
1206        );
1207        assert_relative_eq!(
1208            Dir3::Z.slerp(Dir3::Y, 2.0 / 3.0),
1209            Dir3::from_xyz(0.0, ops::sqrt(0.75f32), 0.5).unwrap()
1210        );
1211    }
1212
1213    #[test]
1214    fn dir3_renorm() {
1215        // Evil denormalized quaternion
1216        let rot3 = Quat::from_euler(glam::EulerRot::XYZ, 1.0, 2.0, 3.0) * (1.0 + 1e-5);
1217        let mut dir_a = Dir3::X;
1218        let mut dir_b = Dir3::X;
1219
1220        // We test that renormalizing an already normalized dir doesn't do anything
1221        assert_relative_eq!(dir_b, dir_b.fast_renormalize(), epsilon = 0.000001);
1222
1223        for _ in 0..50 {
1224            dir_a = rot3 * dir_a;
1225            dir_b = rot3 * dir_b;
1226            dir_b = dir_b.fast_renormalize();
1227        }
1228
1229        // `dir_a` should've gotten denormalized, meanwhile `dir_b` should stay normalized.
1230        assert!(
1231            !dir_a.is_normalized(),
1232            "Denormalization doesn't work, test is faulty"
1233        );
1234        assert!(dir_b.is_normalized(), "Renormalisation did not work.");
1235    }
1236
1237    #[test]
1238    fn dir3a_creation() {
1239        assert_eq!(Dir3A::new(Vec3A::X * 12.5), Ok(Dir3A::X));
1240        assert_eq!(
1241            Dir3A::new(Vec3A::new(0.0, 0.0, 0.0)),
1242            Err(InvalidDirectionError::Zero)
1243        );
1244        assert_eq!(
1245            Dir3A::new(Vec3A::new(f32::INFINITY, 0.0, 0.0)),
1246            Err(InvalidDirectionError::Infinite)
1247        );
1248        assert_eq!(
1249            Dir3A::new(Vec3A::new(f32::NEG_INFINITY, 0.0, 0.0)),
1250            Err(InvalidDirectionError::Infinite)
1251        );
1252        assert_eq!(
1253            Dir3A::new(Vec3A::new(f32::NAN, 0.0, 0.0)),
1254            Err(InvalidDirectionError::NaN)
1255        );
1256        assert_eq!(Dir3A::new_and_length(Vec3A::X * 6.5), Ok((Dir3A::X, 6.5)));
1257
1258        // Test rotation
1259        assert!(
1260            (Quat::from_rotation_z(core::f32::consts::FRAC_PI_2) * Dir3A::X)
1261                .abs_diff_eq(Vec3A::Y, 10e-6)
1262        );
1263    }
1264
1265    #[test]
1266    fn dir3a_slerp() {
1267        assert_relative_eq!(
1268            Dir3A::X.slerp(Dir3A::Y, 0.5),
1269            Dir3A::from_xyz(ops::sqrt(0.5f32), ops::sqrt(0.5f32), 0.0).unwrap()
1270        );
1271        assert_relative_eq!(Dir3A::Y.slerp(Dir3A::Z, 0.0), Dir3A::Y);
1272        assert_relative_eq!(Dir3A::Z.slerp(Dir3A::X, 1.0), Dir3A::X, epsilon = 0.000001);
1273        assert_relative_eq!(
1274            Dir3A::X.slerp(Dir3A::Z, 1.0 / 3.0),
1275            Dir3A::from_xyz(ops::sqrt(0.75f32), 0.0, 0.5).unwrap(),
1276            epsilon = 0.000001
1277        );
1278        assert_relative_eq!(
1279            Dir3A::Z.slerp(Dir3A::Y, 2.0 / 3.0),
1280            Dir3A::from_xyz(0.0, ops::sqrt(0.75f32), 0.5).unwrap()
1281        );
1282    }
1283
1284    #[test]
1285    fn dir3a_renorm() {
1286        // Evil denormalized quaternion
1287        let rot3 = Quat::from_euler(glam::EulerRot::XYZ, 1.0, 2.0, 3.0) * (1.0 + 1e-5);
1288        let mut dir_a = Dir3A::X;
1289        let mut dir_b = Dir3A::X;
1290
1291        // We test that renormalizing an already normalized dir doesn't do anything
1292        assert_relative_eq!(dir_b, dir_b.fast_renormalize(), epsilon = 0.000001);
1293
1294        for _ in 0..50 {
1295            dir_a = rot3 * dir_a;
1296            dir_b = rot3 * dir_b;
1297            dir_b = dir_b.fast_renormalize();
1298        }
1299
1300        // `dir_a` should've gotten denormalized, meanwhile `dir_b` should stay normalized.
1301        assert!(
1302            !dir_a.is_normalized(),
1303            "Denormalization doesn't work, test is faulty"
1304        );
1305        assert!(dir_b.is_normalized(), "Renormalisation did not work.");
1306    }
1307
1308    #[test]
1309    fn dir4_creation() {
1310        assert_eq!(Dir4::new(Vec4::X * 12.5), Ok(Dir4::X));
1311        assert_eq!(
1312            Dir4::new(Vec4::new(0.0, 0.0, 0.0, 0.0)),
1313            Err(InvalidDirectionError::Zero)
1314        );
1315        assert_eq!(
1316            Dir4::new(Vec4::new(f32::INFINITY, 0.0, 0.0, 0.0)),
1317            Err(InvalidDirectionError::Infinite)
1318        );
1319        assert_eq!(
1320            Dir4::new(Vec4::new(f32::NEG_INFINITY, 0.0, 0.0, 0.0)),
1321            Err(InvalidDirectionError::Infinite)
1322        );
1323        assert_eq!(
1324            Dir4::new(Vec4::new(f32::NAN, 0.0, 0.0, 0.0)),
1325            Err(InvalidDirectionError::NaN)
1326        );
1327        assert_eq!(Dir4::new_and_length(Vec4::X * 6.5), Ok((Dir4::X, 6.5)));
1328    }
1329
1330    #[test]
1331    fn dir4_renorm() {
1332        // Evil denormalized matrix
1333        let mat4 = bevy_math::Mat4::from_quat(Quat::from_euler(glam::EulerRot::XYZ, 1.0, 2.0, 3.0))
1334            * (1.0 + 1e-5);
1335        let mut dir_a = Dir4::from_xyzw(1., 1., 0., 0.).unwrap();
1336        let mut dir_b = Dir4::from_xyzw(1., 1., 0., 0.).unwrap();
1337        // We test that renormalizing an already normalized dir doesn't do anything
1338        assert_relative_eq!(dir_b, dir_b.fast_renormalize(), epsilon = 0.000001);
1339        for _ in 0..50 {
1340            dir_a = Dir4(mat4 * *dir_a);
1341            dir_b = Dir4(mat4 * *dir_b);
1342            dir_b = dir_b.fast_renormalize();
1343        }
1344        // `dir_a` should've gotten denormalized, meanwhile `dir_b` should stay normalized.
1345        assert!(
1346            !dir_a.is_normalized(),
1347            "Denormalization doesn't work, test is faulty"
1348        );
1349        assert!(dir_b.is_normalized(), "Renormalisation did not work.");
1350    }
1351}
bevy_math/direction.rs

bevy_math/
direction.rs
