From 6c9ec88e541b320924ede16665fcc763a1627599 Mon Sep 17 00:00:00 2001 From: Matty Date: Sun, 14 Jul 2024 11:27:42 -0400 Subject: [PATCH] Basic isometry types (#14269) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # Objective Introduce isometry types for describing relative and absolute position in mathematical contexts. ## Solution For the time being, this is a very minimal implementation. This implements the following faculties for two- and three-dimensional isometry types: - Identity transformations - Creation from translations and/or rotations - Inverses - Multiplication (composition) of isometries with each other - Application of isometries to points (as vectors) - Conversion of isometries to affine transformations There is obviously a lot more that could be added, so I erred on the side of adding things that I knew would be useful, with the idea of expanding this in the near future as needed. (I also fixed some random doc problems in `bevy_math`.) --- ## Design One point of interest here is the matter of if/when to use aligned types. In the implementation of 3d isometries, I used `Vec3A` rather than `Vec3` because it has no impact on size/alignment, but I'm still not sure about that decision (although it is easily changed). For 2d isometries — which are encoded by four floats — the idea of shoving them into a single 128-bit buffer (`__m128` or whatever) sounds kind of enticing, but it's more involved and would involve writing unsafe code, so I didn't do that for now. ## Future work - Expand the API to include shortcuts like `inverse_mul` and `inverse_transform` for efficiency reasons. - Include more convenience constructors and methods (e.g. `from_xy`, `from_xyz`). - Refactor `bevy_math::bounding` to use the isometry types. - Add conversions to/from isometries for `Transform`/`GlobalTransform` in `bevy_transform`. --- crates/bevy_math/src/bounding/raycast3d.rs | 12 +- crates/bevy_math/src/isometry.rs | 422 +++++++++++++++++++++ crates/bevy_math/src/lib.rs | 1 + 3 files changed, 432 insertions(+), 3 deletions(-) create mode 100644 crates/bevy_math/src/isometry.rs diff --git a/crates/bevy_math/src/bounding/raycast3d.rs b/crates/bevy_math/src/bounding/raycast3d.rs index c69b790a79..fa36a438d6 100644 --- a/crates/bevy_math/src/bounding/raycast3d.rs +++ b/crates/bevy_math/src/bounding/raycast3d.rs @@ -19,7 +19,9 @@ pub struct RayCast3d { } impl RayCast3d { - /// Construct a [`RayCast3d`] from an origin, [`Dir3A`], and max distance. + /// Construct a [`RayCast3d`] from an origin, [direction], and max distance. + /// + /// [direction]: crate::direction::Dir3 pub fn new(origin: impl Into, direction: impl Into, max: f32) -> Self { let direction = direction.into(); Self { @@ -108,7 +110,9 @@ pub struct AabbCast3d { } impl AabbCast3d { - /// Construct an [`AabbCast3d`] from an [`Aabb3d`], origin, [`Dir3A`], and max distance. + /// Construct an [`AabbCast3d`] from an [`Aabb3d`], origin, [direction], and max distance. + /// + /// [direction]: crate::direction::Dir3 pub fn new( aabb: Aabb3d, origin: impl Into, @@ -151,7 +155,9 @@ pub struct BoundingSphereCast { } impl BoundingSphereCast { - /// Construct a [`BoundingSphereCast`] from a [`BoundingSphere`], origin, [`Dir3A`], and max distance. + /// Construct a [`BoundingSphereCast`] from a [`BoundingSphere`], origin, [direction], and max distance. + /// + /// [direction]: crate::direction::Dir3 pub fn new( sphere: BoundingSphere, origin: impl Into, diff --git a/crates/bevy_math/src/isometry.rs b/crates/bevy_math/src/isometry.rs new file mode 100644 index 0000000000..5cc84edafa --- /dev/null +++ b/crates/bevy_math/src/isometry.rs @@ -0,0 +1,422 @@ +//! Isometry types for expressing rigid motions in two and three dimensions. +//! +//! These are often used to express the relative positions of two entities (e.g. primitive shapes). +//! For example, in determining whether a sphere intersects a cube, one needs to know how the two are +//! positioned relative to one another in addition to their sizes. +//! If the two had absolute positions and orientations described by isometries `cube_iso` and `sphere_iso`, +//! then `cube_iso.inverse() * sphere_iso` would describe the relative orientation, which is sufficient for +//! answering this query. + +use crate::{Affine2, Affine3, Affine3A, Dir2, Dir3, Mat3, Mat3A, Quat, Rot2, Vec2, Vec3, Vec3A}; +use std::ops::Mul; + +#[cfg(feature = "approx")] +use approx::{AbsDiffEq, RelativeEq, UlpsEq}; + +#[cfg(feature = "bevy_reflect")] +use bevy_reflect::{std_traits::ReflectDefault, Reflect}; +#[cfg(all(feature = "bevy_reflect", feature = "serialize"))] +use bevy_reflect::{ReflectDeserialize, ReflectSerialize}; + +/// An isometry in two dimensions. +#[derive(Copy, Clone, Default, Debug, PartialEq)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr( + feature = "bevy_reflect", + derive(Reflect), + reflect(Debug, PartialEq, Default) +)] +#[cfg_attr( + all(feature = "serialize", feature = "bevy_reflect"), + reflect(Serialize, Deserialize) +)] +pub struct Isometry2d { + /// The rotational part of a two-dimensional isometry. + pub rotation: Rot2, + /// The translational part of a two-dimensional isometry. + pub translation: Vec2, +} + +impl Isometry2d { + /// The identity isometry which represents the rigid motion of not doing anything. + pub const IDENTITY: Self = Isometry2d { + rotation: Rot2::IDENTITY, + translation: Vec2::ZERO, + }; + + /// Create a two-dimensional isometry from a rotation and a translation. + #[inline] + pub fn new(translation: Vec2, rotation: Rot2) -> Self { + Isometry2d { + rotation, + translation, + } + } + + /// Create a two-dimensional isometry from a rotation. + #[inline] + pub fn from_rotation(rotation: Rot2) -> Self { + Isometry2d { + rotation, + translation: Vec2::ZERO, + } + } + + /// Create a two-dimensional isometry from a translation. + #[inline] + pub fn from_translation(translation: Vec2) -> Self { + Isometry2d { + rotation: Rot2::IDENTITY, + translation, + } + } + + /// The inverse isometry that undoes this one. + #[inline] + pub fn inverse(&self) -> Self { + let inv_rot = self.rotation.inverse(); + Isometry2d { + rotation: inv_rot, + translation: inv_rot * -self.translation, + } + } + + /// Transform a point by rotating and translating it using this isometry. + #[inline] + pub fn transform_point(&self, point: Vec2) -> Vec2 { + self.rotation * point + self.translation + } +} + +impl From for Affine2 { + #[inline] + fn from(iso: Isometry2d) -> Self { + Affine2 { + matrix2: iso.rotation.into(), + translation: iso.translation, + } + } +} + +impl Mul for Isometry2d { + type Output = Self; + + #[inline] + fn mul(self, rhs: Self) -> Self::Output { + Isometry2d { + rotation: self.rotation * rhs.rotation, + translation: self.rotation * rhs.translation + self.translation, + } + } +} + +impl Mul for Isometry2d { + type Output = Vec2; + + #[inline] + fn mul(self, rhs: Vec2) -> Self::Output { + self.transform_point(rhs) + } +} + +impl Mul for Isometry2d { + type Output = Dir2; + + #[inline] + fn mul(self, rhs: Dir2) -> Self::Output { + self.rotation * rhs + } +} + +#[cfg(feature = "approx")] +impl AbsDiffEq for Isometry2d { + type Epsilon = ::Epsilon; + + fn default_epsilon() -> Self::Epsilon { + f32::default_epsilon() + } + + fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { + self.rotation.abs_diff_eq(&other.rotation, epsilon) + && self.translation.abs_diff_eq(other.translation, epsilon) + } +} + +#[cfg(feature = "approx")] +impl RelativeEq for Isometry2d { + fn default_max_relative() -> Self::Epsilon { + Self::default_epsilon() + } + + fn relative_eq( + &self, + other: &Self, + epsilon: Self::Epsilon, + max_relative: Self::Epsilon, + ) -> bool { + self.rotation + .relative_eq(&other.rotation, epsilon, max_relative) + && self + .translation + .relative_eq(&other.translation, epsilon, max_relative) + } +} + +#[cfg(feature = "approx")] +impl UlpsEq for Isometry2d { + fn default_max_ulps() -> u32 { + 4 + } + + fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool { + self.rotation.ulps_eq(&other.rotation, epsilon, max_ulps) + && self + .translation + .ulps_eq(&other.translation, epsilon, max_ulps) + } +} + +/// An isometry in three dimensions. +#[derive(Copy, Clone, Default, Debug, PartialEq)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr( + feature = "bevy_reflect", + derive(Reflect), + reflect(Debug, PartialEq, Default) +)] +#[cfg_attr( + all(feature = "serialize", feature = "bevy_reflect"), + reflect(Serialize, Deserialize) +)] +pub struct Isometry3d { + /// The rotational part of a three-dimensional isometry. + pub rotation: Quat, + /// The translational part of a three-dimensional isometry. + pub translation: Vec3A, +} + +impl Isometry3d { + /// The identity isometry which represents the rigid motion of not doing anything. + pub const IDENTITY: Self = Isometry3d { + rotation: Quat::IDENTITY, + translation: Vec3A::ZERO, + }; + + /// Create a three-dimensional isometry from a rotation and a translation. + #[inline] + pub fn new(translation: impl Into, rotation: Quat) -> Self { + Isometry3d { + rotation, + translation: translation.into(), + } + } + + /// Create a three-dimensional isometry from a rotation. + #[inline] + pub fn from_rotation(rotation: Quat) -> Self { + Isometry3d { + rotation, + translation: Vec3A::ZERO, + } + } + + /// Create a three-dimensional isometry from a translation. + #[inline] + pub fn from_translation(translation: impl Into) -> Self { + Isometry3d { + rotation: Quat::IDENTITY, + translation: translation.into(), + } + } + + /// The inverse isometry that undoes this one. + #[inline] + pub fn inverse(&self) -> Self { + let inv_rot = self.rotation.inverse(); + Isometry3d { + rotation: inv_rot, + translation: inv_rot * -self.translation, + } + } + + /// Transform a point by rotating and translating it using this isometry. + #[inline] + pub fn transform_point(&self, point: impl Into) -> Vec3A { + self.rotation * point.into() + self.translation + } +} + +impl From for Affine3 { + #[inline] + fn from(iso: Isometry3d) -> Self { + Affine3 { + matrix3: Mat3::from_quat(iso.rotation), + translation: iso.translation.into(), + } + } +} + +impl From for Affine3A { + #[inline] + fn from(iso: Isometry3d) -> Self { + Affine3A { + matrix3: Mat3A::from_quat(iso.rotation), + translation: iso.translation, + } + } +} + +impl Mul for Isometry3d { + type Output = Self; + + #[inline] + fn mul(self, rhs: Self) -> Self::Output { + Isometry3d { + rotation: self.rotation * rhs.rotation, + translation: self.rotation * rhs.translation + self.translation, + } + } +} + +impl Mul for Isometry3d { + type Output = Vec3A; + + #[inline] + fn mul(self, rhs: Vec3A) -> Self::Output { + self.transform_point(rhs) + } +} + +impl Mul for Isometry3d { + type Output = Vec3; + + #[inline] + fn mul(self, rhs: Vec3) -> Self::Output { + self.transform_point(rhs).into() + } +} + +impl Mul for Isometry3d { + type Output = Dir3; + + #[inline] + fn mul(self, rhs: Dir3) -> Self::Output { + self.rotation * rhs + } +} + +#[cfg(feature = "approx")] +impl AbsDiffEq for Isometry3d { + type Epsilon = ::Epsilon; + + fn default_epsilon() -> Self::Epsilon { + f32::default_epsilon() + } + + fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { + self.rotation.abs_diff_eq(other.rotation, epsilon) + && self.translation.abs_diff_eq(other.translation, epsilon) + } +} + +#[cfg(feature = "approx")] +impl RelativeEq for Isometry3d { + fn default_max_relative() -> Self::Epsilon { + Self::default_epsilon() + } + + fn relative_eq( + &self, + other: &Self, + epsilon: Self::Epsilon, + max_relative: Self::Epsilon, + ) -> bool { + self.rotation + .relative_eq(&other.rotation, epsilon, max_relative) + && self + .translation + .relative_eq(&other.translation, epsilon, max_relative) + } +} + +#[cfg(feature = "approx")] +impl UlpsEq for Isometry3d { + fn default_max_ulps() -> u32 { + 4 + } + + fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool { + self.rotation.ulps_eq(&other.rotation, epsilon, max_ulps) + && self + .translation + .ulps_eq(&other.translation, epsilon, max_ulps) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::{vec2, vec3}; + use approx::assert_abs_diff_eq; + use std::f32::consts::{FRAC_PI_2, FRAC_PI_3}; + + #[test] + fn mul_2d() { + let iso1 = Isometry2d::new(vec2(1.0, 0.0), Rot2::FRAC_PI_2); + let iso2 = Isometry2d::new(vec2(0.0, 1.0), Rot2::FRAC_PI_2); + let expected = Isometry2d::new(vec2(0.0, 0.0), Rot2::PI); + assert_abs_diff_eq!(iso1 * iso2, expected); + } + + #[test] + fn mul_3d() { + let iso1 = Isometry3d::new(vec3(1.0, 0.0, 0.0), Quat::from_rotation_x(FRAC_PI_2)); + let iso2 = Isometry3d::new(vec3(0.0, 1.0, 0.0), Quat::IDENTITY); + let expected = Isometry3d::new(vec3(1.0, 0.0, 1.0), Quat::from_rotation_x(FRAC_PI_2)); + assert_abs_diff_eq!(iso1 * iso2, expected); + } + + #[test] + fn identity_2d() { + let iso = Isometry2d::new(vec2(-1.0, -0.5), Rot2::degrees(75.0)); + assert_abs_diff_eq!(Isometry2d::IDENTITY * iso, iso); + assert_abs_diff_eq!(iso * Isometry2d::IDENTITY, iso); + } + + #[test] + fn identity_3d() { + let iso = Isometry3d::new(vec3(-1.0, 2.5, 3.3), Quat::from_rotation_z(FRAC_PI_3)); + assert_abs_diff_eq!(Isometry3d::IDENTITY * iso, iso); + assert_abs_diff_eq!(iso * Isometry3d::IDENTITY, iso); + } + + #[test] + fn inverse_2d() { + let iso = Isometry2d::new(vec2(-1.0, -0.5), Rot2::degrees(75.0)); + let inv = iso.inverse(); + assert_abs_diff_eq!(iso * inv, Isometry2d::IDENTITY); + assert_abs_diff_eq!(inv * iso, Isometry2d::IDENTITY); + } + + #[test] + fn inverse_3d() { + let iso = Isometry3d::new(vec3(-1.0, 2.5, 3.3), Quat::from_rotation_z(FRAC_PI_3)); + let inv = iso.inverse(); + assert_abs_diff_eq!(iso * inv, Isometry3d::IDENTITY); + assert_abs_diff_eq!(inv * iso, Isometry3d::IDENTITY); + } + + #[test] + fn transform_2d() { + let iso = Isometry2d::new(vec2(0.5, -0.5), Rot2::FRAC_PI_2); + let point = vec2(1.0, 1.0); + assert_abs_diff_eq!(vec2(-0.5, 0.5), iso * point); + } + + #[test] + fn transform_3d() { + let iso = Isometry3d::new(vec3(1.0, 0.0, 0.0), Quat::from_rotation_y(FRAC_PI_2)); + let point = vec3(1.0, 1.0, 1.0); + assert_abs_diff_eq!(vec3(2.0, 1.0, -1.0), iso * point); + } +} diff --git a/crates/bevy_math/src/lib.rs b/crates/bevy_math/src/lib.rs index e41d0acc49..0f06969a3d 100644 --- a/crates/bevy_math/src/lib.rs +++ b/crates/bevy_math/src/lib.rs @@ -19,6 +19,7 @@ mod compass; pub mod cubic_splines; mod direction; mod float_ord; +pub mod isometry; pub mod primitives; mod ray; mod rects;