bevy_math/
direction.rs

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

bevy_math/
direction.rs
