New utility methods on InfinitePlane3d (#14651)

# Objective

Some algorithms don't really work well or are not efficient in 3D space.
When we know we have points on an `InfinitePlane3d` it would be helpful
to have some utility methods to reversibly transform points on the plane
to 2D space to apply some algorithms there.

## Solution

This PR adds a few of methods to project 3D points on a plane to 2D
points and inject them back. Additionally there are some other small
common helper methods.

## Testing

- added some tests that cover the new methods

---------

Co-authored-by: Matty <weatherleymatthew@gmail.com>

crates/bevy_math/src/primitives/dim3.rs

@@ -1,12 +1,13 @@
 use std::f32::consts::{FRAC_PI_3, PI};
 
 use super::{Circle, Measured2d, Measured3d, Primitive2d, Primitive3d};
-use crate::{ops, ops::FloatPow, Dir3, InvalidDirectionError, Mat3, Vec2, Vec3};
+use crate::{ops, ops::FloatPow, Dir3, InvalidDirectionError, Isometry3d, Mat3, Vec2, Vec3};
 
 #[cfg(feature = "bevy_reflect")]
 use bevy_reflect::{std_traits::ReflectDefault, Reflect};
 #[cfg(all(feature = "serialize", feature = "bevy_reflect"))]
 use bevy_reflect::{ReflectDeserialize, ReflectSerialize};
+use glam::Quat;
 
 /// A sphere primitive, representing the set of all points some distance from the origin
 #[derive(Clone, Copy, Debug, PartialEq)]
@@ -214,6 +215,115 @@ impl InfinitePlane3d {
 
         (Self { normal }, translation)
     }
+
+    /// Computes the shortest distance between a plane transformed with the given `isometry` and a
+    /// `point`. The result is a signed value; it's positive if the point lies in the half-space
+    /// that the plane's normal vector points towards.
+    #[inline]
+    pub fn signed_distance(&self, isometry: Isometry3d, point: Vec3) -> f32 {
+        self.normal.dot(isometry.inverse() * point)
+    }
+
+    /// Injects the `point` into this plane transformed with the given `isometry`.
+    ///
+    /// This projects the point orthogonally along the shortest path onto the plane.
+    #[inline]
+    pub fn project_point(&self, isometry: Isometry3d, point: Vec3) -> Vec3 {
+        point - self.normal * self.signed_distance(isometry, point)
+    }
+
+    /// Computes an [`Isometry3d`] which transforms points from the plane in 3D space with the given
+    /// `origin` to the XY-plane.
+    ///
+    /// ## Guarantees
+    ///
+    /// * the transformation is a [congruence] meaning it will preserve all distances and angles of
+    ///   the transformed geometry
+    /// * uses the least rotation possible to transform the geometry
+    /// * if two geometries are transformed with the same isometry, then the relations between
+    ///   them, like distances, are also preserved
+    /// * compared to projections, the transformation is lossless (up to floating point errors)
+    ///   reversible
+    ///
+    /// ## Non-Guarantees
+    ///
+    /// * the rotation used is generally not unique
+    /// * the orientation of the transformed geometry in the XY plane might be arbitrary, to
+    ///   enforce some kind of alignment the user has to use an extra transformation ontop of this
+    ///   one
+    ///
+    /// See [`isometries_xy`] for example usescases.
+    ///
+    /// [congruence]: https://en.wikipedia.org/wiki/Congruence_(geometry)
+    /// [`isometries_xy`]: `InfinitePlane3d::isometries_xy`
+    #[inline]
+    pub fn isometry_into_xy(&self, origin: Vec3) -> Isometry3d {
+        let rotation = Quat::from_rotation_arc(self.normal.as_vec3(), Vec3::Z);
+        let transformed_origin = rotation * origin;
+        Isometry3d::new(-Vec3::Z * transformed_origin.z, rotation)
+    }
+
+    /// Computes an [`Isometry3d`] which transforms points from the XY-plane to this plane with the
+    /// given `origin`.
+    ///
+    /// ## Guarantees
+    ///
+    /// * the transformation is a [congruence] meaning it will preserve all distances and angles of
+    ///   the transformed geometry
+    /// * uses the least rotation possible to transform the geometry
+    /// * if two geometries are transformed with the same isometry, then the relations between
+    ///   them, like distances, are also preserved
+    /// * compared to projections, the transformation is lossless (up to floating point errors)
+    ///   reversible
+    ///
+    /// ## Non-Guarantees
+    ///
+    /// * the rotation used is generally not unique
+    /// * the orientation of the transformed geometry in the XY plane might be arbitrary, to
+    ///   enforce some kind of alignment the user has to use an extra transformation ontop of this
+    ///   one
+    ///
+    /// See [`isometries_xy`] for example usescases.
+    ///
+    /// [congruence]: https://en.wikipedia.org/wiki/Congruence_(geometry)
+    /// [`isometries_xy`]: `InfinitePlane3d::isometries_xy`
+    #[inline]
+    pub fn isometry_from_xy(&self, origin: Vec3) -> Isometry3d {
+        self.isometry_into_xy(origin).inverse()
+    }
+
+    /// Computes both [isometries] which transforms points from the plane in 3D space with the
+    /// given `origin` to the XY-plane and back.
+    ///
+    /// [isometries]: `Isometry3d`
+    ///
+    /// # Example
+    ///
+    /// The projection and its inverse can be used to run 2D algorithms on flat shapes in 3D. The
+    /// workflow would usually look like this:
+    ///
+    /// ```
+    /// # use bevy_math::{Vec3, Dir3};
+    /// # use bevy_math::primitives::InfinitePlane3d;
+    ///
+    /// let triangle_3d @ [a, b, c] = [Vec3::X, Vec3::Y, Vec3::Z];
+    /// let center = (a + b + c) / 3.0;
+    ///
+    /// let plane = InfinitePlane3d::new(Vec3::ONE);
+    ///
+    /// let (to_xy, from_xy) = plane.isometries_xy(center);
+    ///
+    /// let triangle_2d = triangle_3d.map(|vec3| to_xy * vec3).map(|vec3| vec3.truncate());
+    ///
+    /// // apply some algorithm to `triangle_2d`
+    ///
+    /// let triangle_3d = triangle_2d.map(|vec2| vec2.extend(0.0)).map(|vec3| from_xy * vec3);
+    /// ```
+    #[inline]
+    pub fn isometries_xy(&self, origin: Vec3) -> (Isometry3d, Isometry3d) {
+        let projection = self.isometry_into_xy(origin);
+        (projection, projection.inverse())
+    }
 }
 
 /// An infinite line going through the origin along a direction in 3D space.
@@ -1257,10 +1367,58 @@ mod tests {
     }
 
     #[test]
-    fn infinite_plane_from_points() {
-        let (plane, translation) = InfinitePlane3d::from_points(Vec3::X, Vec3::Z, Vec3::NEG_X);
+    fn infinite_plane_math() {
+        let (plane, origin) = InfinitePlane3d::from_points(Vec3::X, Vec3::Z, Vec3::NEG_X);
         assert_eq!(*plane.normal, Vec3::NEG_Y, "incorrect normal");
-        assert_eq!(translation, Vec3::Z * 0.33333334, "incorrect translation");
+        assert_eq!(origin, Vec3::Z * 0.33333334, "incorrect translation");
+
+        let point_in_plane = Vec3::X + Vec3::Z;
+        assert_eq!(
+            plane.signed_distance(Isometry3d::from_translation(origin), point_in_plane),
+            0.0,
+            "incorrect distance"
+        );
+        assert_eq!(
+            plane.project_point(Isometry3d::from_translation(origin), point_in_plane),
+            point_in_plane,
+            "incorrect point"
+        );
+
+        let point_outside = Vec3::Y;
+        assert_eq!(
+            plane.signed_distance(Isometry3d::from_translation(origin), point_outside),
+            -1.0,
+            "incorrect distance"
+        );
+        assert_eq!(
+            plane.project_point(Isometry3d::from_translation(origin), point_outside),
+            Vec3::ZERO,
+            "incorrect point"
+        );
+
+        let point_outside = Vec3::NEG_Y;
+        assert_eq!(
+            plane.signed_distance(Isometry3d::from_translation(origin), point_outside),
+            1.0,
+            "incorrect distance"
+        );
+        assert_eq!(
+            plane.project_point(Isometry3d::from_translation(origin), point_outside),
+            Vec3::ZERO,
+            "incorrect point"
+        );
+
+        let area_f = |[a, b, c]: [Vec3; 3]| (a - b).cross(a - c).length() * 0.5;
+        let (proj, inj) = plane.isometries_xy(origin);
+
+        let triangle = [Vec3::X, Vec3::Y, Vec3::ZERO];
+        assert_eq!(area_f(triangle), 0.5, "incorrect area");
+
+        let triangle_proj = triangle.map(|vec3| proj * vec3);
+        assert_relative_eq!(area_f(triangle_proj), 0.5);
+
+        let triangle_proj_inj = triangle_proj.map(|vec3| inj * vec3);
+        assert_relative_eq!(area_f(triangle_proj_inj), 0.5);
     }
 
     #[test]