Basic isometry types (#14269)

# 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

@@ -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<Vec3A>, direction: impl Into<Dir3A>, 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<Vec3A>,
@@ -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<Vec3A>,

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<Isometry2d> 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<Vec2> for Isometry2d {
+    type Output = Vec2;
+
+    #[inline]
+    fn mul(self, rhs: Vec2) -> Self::Output {
+        self.transform_point(rhs)
+    }
+}
+
+impl Mul<Dir2> 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 = <f32 as AbsDiffEq>::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<Vec3A>, 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<Vec3A>) -> 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>) -> Vec3A {
+        self.rotation * point.into() + self.translation
+    }
+}
+
+impl From<Isometry3d> for Affine3 {
+    #[inline]
+    fn from(iso: Isometry3d) -> Self {
+        Affine3 {
+            matrix3: Mat3::from_quat(iso.rotation),
+            translation: iso.translation.into(),
+        }
+    }
+}
+
+impl From<Isometry3d> 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<Vec3A> for Isometry3d {
+    type Output = Vec3A;
+
+    #[inline]
+    fn mul(self, rhs: Vec3A) -> Self::Output {
+        self.transform_point(rhs)
+    }
+}
+
+impl Mul<Vec3> for Isometry3d {
+    type Output = Vec3;
+
+    #[inline]
+    fn mul(self, rhs: Vec3) -> Self::Output {
+        self.transform_point(rhs).into()
+    }
+}
+
+impl Mul<Dir3> 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 = <f32 as AbsDiffEq>::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);
+    }
+}

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;