diff --git a/crates/bevy_math/src/primitives/dim3.rs b/crates/bevy_math/src/primitives/dim3.rs index ed692d7ead..7ee6a82d6f 100644 --- a/crates/bevy_math/src/primitives/dim3.rs +++ b/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]