From 6f2eec8f78ed3f8b49f99b64c70cd430ad60eb84 Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Thu, 1 Feb 2024 22:08:24 +0200 Subject: [PATCH] Support rotating `Direction3d` by `Quat` (#11649) # Objective It's often necessary to rotate directions, but it currently has to be done like this: ```rust Direction3d::new_unchecked(quat * *direction) ``` It'd be nice if you could rotate `Direction3d` directly: ```rust quat * direction ``` ## Solution Implement `Mul` for `Quat` ~~and the other way around.~~ (Glam doesn't impl `Mul` or `MulAssign` for `Vec3`) The quaternion must be a unit quaternion to keep the direction normalized, so there is a `debug_assert!` to be sure. Almost all `Quat` constructors produce unit quaternions, so there should only be issues if doing something like `quat + quat` instead of `quat * quat`, using `Quat::from_xyzw` directly, or when you have significant enough drift caused by e.g. physics simulation that doesn't normalize rotation. In general, these would probably cause unexpected results anyway. I also moved tests around slightly to make `dim2` and `dim3` more consistent (`dim3` had *two* separate `test` modules for some reason). In the future, we'll probably want a `Rotation2d` type that would support the same for `Direction2d`. I considered implementing `Mul` for `Direction2d`, but that would probably be more questionable since `Mat2` isn't as clearly associated with rotations as `Quat` is. --- crates/bevy_math/src/primitives/dim2.rs | 100 +++++++++--------- crates/bevy_math/src/primitives/dim3.rs | 128 +++++++++++++----------- 2 files changed, 122 insertions(+), 106 deletions(-) diff --git a/crates/bevy_math/src/primitives/dim2.rs b/crates/bevy_math/src/primitives/dim2.rs index c0ad0b2d1c695..55061ba0db4df 100644 --- a/crates/bevy_math/src/primitives/dim2.rs +++ b/crates/bevy_math/src/primitives/dim2.rs @@ -785,31 +785,6 @@ mod tests { use super::*; use approx::assert_relative_eq; - #[test] - fn circle_math() { - let circle = Circle { radius: 3.0 }; - assert_eq!(circle.diameter(), 6.0, "incorrect diameter"); - assert_eq!(circle.area(), 28.274334, "incorrect area"); - assert_eq!(circle.perimeter(), 18.849556, "incorrect perimeter"); - } - - #[test] - fn ellipse_math() { - let ellipse = Ellipse::new(3.0, 1.0); - assert_eq!(ellipse.area(), 9.424778, "incorrect area"); - } - - #[test] - fn triangle_math() { - let triangle = Triangle2d::new( - Vec2::new(-2.0, -1.0), - Vec2::new(1.0, 4.0), - Vec2::new(7.0, 0.0), - ); - assert_eq!(triangle.area(), 21.0, "incorrect area"); - assert_eq!(triangle.perimeter(), 22.097439, "incorrect perimeter"); - } - #[test] fn direction_creation() { assert_eq!(Direction2d::new(Vec2::X * 12.5), Ok(Direction2d::X)); @@ -835,6 +810,56 @@ mod tests { ); } + #[test] + fn rectangle_closest_point() { + let rectangle = Rectangle::new(2.0, 2.0); + assert_eq!(rectangle.closest_point(Vec2::X * 10.0), Vec2::X); + assert_eq!(rectangle.closest_point(Vec2::NEG_ONE * 10.0), Vec2::NEG_ONE); + assert_eq!( + rectangle.closest_point(Vec2::new(0.25, 0.1)), + Vec2::new(0.25, 0.1) + ); + } + + #[test] + fn circle_closest_point() { + let circle = Circle { radius: 1.0 }; + assert_eq!(circle.closest_point(Vec2::X * 10.0), Vec2::X); + assert_eq!( + circle.closest_point(Vec2::NEG_ONE * 10.0), + Vec2::NEG_ONE.normalize() + ); + assert_eq!( + circle.closest_point(Vec2::new(0.25, 0.1)), + Vec2::new(0.25, 0.1) + ); + } + + #[test] + fn circle_math() { + let circle = Circle { radius: 3.0 }; + assert_eq!(circle.diameter(), 6.0, "incorrect diameter"); + assert_eq!(circle.area(), 28.274334, "incorrect area"); + assert_eq!(circle.perimeter(), 18.849556, "incorrect perimeter"); + } + + #[test] + fn ellipse_math() { + let ellipse = Ellipse::new(3.0, 1.0); + assert_eq!(ellipse.area(), 9.424778, "incorrect area"); + } + + #[test] + fn triangle_math() { + let triangle = Triangle2d::new( + Vec2::new(-2.0, -1.0), + Vec2::new(1.0, 4.0), + Vec2::new(7.0, 0.0), + ); + assert_eq!(triangle.area(), 21.0, "incorrect area"); + assert_eq!(triangle.perimeter(), 22.097439, "incorrect perimeter"); + } + #[test] fn triangle_winding_order() { let mut cw_triangle = Triangle2d::new( @@ -936,29 +961,4 @@ mod tests { < 1e-7, ); } - - #[test] - fn rectangle_closest_point() { - let rectangle = Rectangle::new(2.0, 2.0); - assert_eq!(rectangle.closest_point(Vec2::X * 10.0), Vec2::X); - assert_eq!(rectangle.closest_point(Vec2::NEG_ONE * 10.0), Vec2::NEG_ONE); - assert_eq!( - rectangle.closest_point(Vec2::new(0.25, 0.1)), - Vec2::new(0.25, 0.1) - ); - } - - #[test] - fn circle_closest_point() { - let circle = Circle { radius: 1.0 }; - assert_eq!(circle.closest_point(Vec2::X * 10.0), Vec2::X); - assert_eq!( - circle.closest_point(Vec2::NEG_ONE * 10.0), - Vec2::NEG_ONE.normalize() - ); - assert_eq!( - circle.closest_point(Vec2::new(0.25, 0.1)), - Vec2::new(0.25, 0.1) - ); - } } diff --git a/crates/bevy_math/src/primitives/dim3.rs b/crates/bevy_math/src/primitives/dim3.rs index 150db82ecc57e..459c8e26ee9ea 100644 --- a/crates/bevy_math/src/primitives/dim3.rs +++ b/crates/bevy_math/src/primitives/dim3.rs @@ -1,7 +1,7 @@ use std::f32::consts::{FRAC_PI_3, PI}; use super::{Circle, InvalidDirectionError, Primitive3d}; -use crate::Vec3; +use crate::{Quat, Vec3}; /// A normalized vector pointing in a direction in 3D space #[derive(Clone, Copy, Debug, PartialEq)] @@ -85,6 +85,21 @@ impl std::ops::Neg for Direction3d { } } +impl std::ops::Mul for Quat { + type Output = Direction3d; + + /// Rotates the [`Direction3d`] using a [`Quat`]. + fn mul(self, direction: Direction3d) -> Self::Output { + let rotated = self * *direction; + + // Make sure the result is normalized. + // This can fail for non-unit quaternions. + debug_assert!(rotated.is_normalized()); + + Direction3d::new_unchecked(rotated) + } +} + #[cfg(feature = "approx")] impl approx::AbsDiffEq for Direction3d { type Epsilon = f32; @@ -687,6 +702,62 @@ mod tests { use super::*; use approx::assert_relative_eq; + #[test] + fn direction_creation() { + assert_eq!(Direction3d::new(Vec3::X * 12.5), Ok(Direction3d::X)); + assert_eq!( + Direction3d::new(Vec3::new(0.0, 0.0, 0.0)), + Err(InvalidDirectionError::Zero) + ); + assert_eq!( + Direction3d::new(Vec3::new(f32::INFINITY, 0.0, 0.0)), + Err(InvalidDirectionError::Infinite) + ); + assert_eq!( + Direction3d::new(Vec3::new(f32::NEG_INFINITY, 0.0, 0.0)), + Err(InvalidDirectionError::Infinite) + ); + assert_eq!( + Direction3d::new(Vec3::new(f32::NAN, 0.0, 0.0)), + Err(InvalidDirectionError::NaN) + ); + assert_eq!( + Direction3d::new_and_length(Vec3::X * 6.5), + Ok((Direction3d::X, 6.5)) + ); + + // Test rotation + assert!( + (Quat::from_rotation_z(std::f32::consts::FRAC_PI_2) * Direction3d::X) + .abs_diff_eq(Vec3::Y, 10e-6) + ); + } + + #[test] + fn cuboid_closest_point() { + let cuboid = Cuboid::new(2.0, 2.0, 2.0); + assert_eq!(cuboid.closest_point(Vec3::X * 10.0), Vec3::X); + assert_eq!(cuboid.closest_point(Vec3::NEG_ONE * 10.0), Vec3::NEG_ONE); + assert_eq!( + cuboid.closest_point(Vec3::new(0.25, 0.1, 0.3)), + Vec3::new(0.25, 0.1, 0.3) + ); + } + + #[test] + fn sphere_closest_point() { + let sphere = Sphere { radius: 1.0 }; + assert_eq!(sphere.closest_point(Vec3::X * 10.0), Vec3::X); + assert_eq!( + sphere.closest_point(Vec3::NEG_ONE * 10.0), + Vec3::NEG_ONE.normalize() + ); + assert_eq!( + sphere.closest_point(Vec3::new(0.25, 0.1, 0.3)), + Vec3::new(0.25, 0.1, 0.3) + ); + } + #[test] fn sphere_math() { let sphere = Sphere { radius: 4.0 }; @@ -790,58 +861,3 @@ mod tests { assert_relative_eq!(torus.volume(), 4.97428, epsilon = 0.00001); } } - -#[cfg(test)] -mod test { - use super::*; - - #[test] - fn direction_creation() { - assert_eq!(Direction3d::new(Vec3::X * 12.5), Ok(Direction3d::X)); - assert_eq!( - Direction3d::new(Vec3::new(0.0, 0.0, 0.0)), - Err(InvalidDirectionError::Zero) - ); - assert_eq!( - Direction3d::new(Vec3::new(f32::INFINITY, 0.0, 0.0)), - Err(InvalidDirectionError::Infinite) - ); - assert_eq!( - Direction3d::new(Vec3::new(f32::NEG_INFINITY, 0.0, 0.0)), - Err(InvalidDirectionError::Infinite) - ); - assert_eq!( - Direction3d::new(Vec3::new(f32::NAN, 0.0, 0.0)), - Err(InvalidDirectionError::NaN) - ); - assert_eq!( - Direction3d::new_and_length(Vec3::X * 6.5), - Ok((Direction3d::X, 6.5)) - ); - } - - #[test] - fn cuboid_closest_point() { - let cuboid = Cuboid::new(2.0, 2.0, 2.0); - assert_eq!(cuboid.closest_point(Vec3::X * 10.0), Vec3::X); - assert_eq!(cuboid.closest_point(Vec3::NEG_ONE * 10.0), Vec3::NEG_ONE); - assert_eq!( - cuboid.closest_point(Vec3::new(0.25, 0.1, 0.3)), - Vec3::new(0.25, 0.1, 0.3) - ); - } - - #[test] - fn sphere_closest_point() { - let sphere = Sphere { radius: 1.0 }; - assert_eq!(sphere.closest_point(Vec3::X * 10.0), Vec3::X); - assert_eq!( - sphere.closest_point(Vec3::NEG_ONE * 10.0), - Vec3::NEG_ONE.normalize() - ); - assert_eq!( - sphere.closest_point(Vec3::new(0.25, 0.1, 0.3)), - Vec3::new(0.25, 0.1, 0.3) - ); - } -}