diff --git a/godot-core/src/builtin/plane.rs b/godot-core/src/builtin/plane.rs index df7f23472..ea71584bc 100644 --- a/godot-core/src/builtin/plane.rs +++ b/godot-core/src/builtin/plane.rs @@ -9,7 +9,9 @@ use std::ops::Neg; use godot_ffi as sys; use sys::{ffi_methods, GodotFfi}; -use super::{is_equal_approx, real, Vector3}; +use crate::builtin::math::*; + +use super::{real, Vector3}; /// 3D plane in [Hessian normal form](https://mathworld.wolfram.com/HessianNormalForm.html). /// @@ -17,8 +19,6 @@ use super::{is_equal_approx, real, Vector3}; /// `dot(normal, point) + d == 0`, where `normal` is the normal vector and `d` /// the distance from the origin. /// -/// Currently most methods are only available through [`InnerPlane`](super::inner::InnerPlane). -/// /// Note: almost all methods on `Plane` require that the `normal` vector have /// unit length and will panic if this invariant is violated. This is not separately /// annotated for each method. @@ -26,7 +26,9 @@ use super::{is_equal_approx, real, Vector3}; #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] #[repr(C)] pub struct Plane { + /// Normal vector pointing away from the plane. pub normal: Vector3, + /// Distance between the plane and the origin point. pub d: real, } @@ -105,16 +107,130 @@ impl Plane { } } + /// Finds the shortest distance between the plane and a point. + /// + /// Returns the shortest distance from the `self` `Plane` to the provided `Vector3`. The distance + /// will be positive if the `Vector3` is above the `Plane`, and will be negative if the `Vector3` + /// is below the `Plane`. + #[inline] + pub fn distance_to(&self, point: Vector3) -> real { + self.normal.dot(point) - self.d + } + + /// Finds the center point of the plane. + /// + /// Returns the center of the `self` `Plane` in `Vector3` form. + /// + /// _Godot equivalent: `Plane.get_center()`_ + #[inline] + pub fn center(&self) -> Vector3 { + self.normal * self.d + } + + /// Finds whether a point is inside the plane or not. + /// + /// Returns `true` if the specified `Vector3` is inside the `self` `Plane`, will return otherwise + /// if not. Tolerance of the function unless specified will be using the default value of 1e-05. + /// + /// _Godot equivalent: `Plane.has_point(Vector3 point, float tolerance=1e-05)`_ + #[inline] + #[doc(alias = "has_point")] + pub fn contains_point(&self, point: Vector3, tolerance: Option) -> bool { + let dist: real = (self.normal.dot(point) - self.d).abs(); + dist <= tolerance.unwrap_or(CMP_EPSILON) + } + + /// Finds the intersection point of three planes. + /// + /// Returns the intersection point of the `Plane`s of the current `Plane`, `b`, and `c` enclosed + /// in `Some`. If no intersection point is found, `None` will be returned. + #[inline] + pub fn intersect_3(&self, b: &Self, c: &Self) -> Option { + let normal0: Vector3 = self.normal; + let normal1: Vector3 = b.normal; + let normal2: Vector3 = c.normal; + let denom: real = normal0.cross(normal1).dot(normal2); + if is_zero_approx(denom) { + return None; + } + let result = normal1.cross(normal2) * self.d + + normal2.cross(normal0) * b.d + + normal0.cross(normal1) * c.d; + Some(result / denom) + } + + /// Finds the intersection point of the plane with a ray. + /// + /// Returns the intersection point of a ray consisting of the position `from` and the direction + /// normal `dir` with the `self` `Plane` enclosed in `Some`. If no intersection is found, `None` + /// will be returned. + #[inline] + pub fn intersects_ray(&self, from: Vector3, dir: Vector3) -> Option { + let denom: real = self.normal.dot(dir); + if is_zero_approx(denom) { + return None; + } + let dist: real = (self.normal.dot(from) - self.d) / denom; + if dist > CMP_EPSILON { + return None; + } + Some(from - dir * dist) + } + + /// Finds the intersection point of the plane with a line segment. + /// + /// Returns the intersection point of a segment from position `from` to position `to` with the + /// `self` `Plane` enclosed in `Some`. If no intersection is found, `None` will be returned. + #[inline] + pub fn intersects_segment(&self, from: Vector3, to: Vector3) -> Option { + let segment: Vector3 = from - to; + let denom: real = self.normal.dot(segment); + if is_zero_approx(denom) { + return None; + } + let dist: real = (self.normal.dot(from) - self.d) / denom; + if !(-CMP_EPSILON..=(1.0 + CMP_EPSILON)).contains(&dist) { + return None; + } + Some(from - segment * dist) + } + + /// Finds whether the two planes are approximately equal. + /// /// Returns `true` if the two `Plane`s are approximately equal, by calling `is_equal_approx` on /// `normal` and `d` or on `-normal` and `-d`. - /// - /// _Godot equivalent: `Plane.is_equal_approx()`_ #[inline] pub fn is_equal_approx(&self, other: &Self) -> bool { (self.normal.is_equal_approx(other.normal) && is_equal_approx(self.d, other.d)) || (self.normal.is_equal_approx(-other.normal) && is_equal_approx(self.d, -other.d)) } + /// Returns `true` if the `self` `Plane` is finite by calling `is_finite` on `normal` and `d`. + #[inline] + pub fn is_finite(&self) -> bool { + self.normal.is_finite() && self.d.is_finite() + } + + /// Returns `true` if `point` is located above the `self` `Plane`. + #[inline] + pub fn is_point_over(&self, point: Vector3) -> bool { + self.normal.dot(point) > self.d + } + + /// Returns a normalized copy of the `self` `Plane`. + #[inline] + pub fn normalized(self) -> Self { + Plane::new(self.normal.normalized(), self.d) + } + + /// Projects a point orthogonally to the plane. + /// + /// Returns the orthogonal projection of the variable `point` to the `self` `Plane`. + #[inline] + pub fn project(&self, point: Vector3) -> Vector3 { + point - self.normal * self.distance_to(point) + } + #[inline] fn assert_normalized(self) { assert!( @@ -157,6 +273,9 @@ impl std::fmt::Display for Plane { #[cfg(test)] mod test { + use crate::assert_eq_approx; + use crate::assert_ne_approx; + use super::*; /// Tests that none of the constructors panic for some simple planes. @@ -191,6 +310,301 @@ mod test { ); } + /// Tests `distance_to()`, `center()`, `contains_point()`, and `is_point_over()`. + #[test] + fn test_spatial_relations() { + // Random plane that passes the origin point. + let plane_a: Plane = Plane::new(Vector3::new(1.0, 2.0, 3.0).normalized(), 0.0); + // Parallels `plane_a`. + let plane_b: Plane = Plane::new(Vector3::new(1.0, 2.0, 3.0).normalized(), 1.0); + let plane_c: Plane = Plane::new(Vector3::new(1.0, 2.0, 3.0).normalized(), -6.5); + // Unrelated plane. + let plane_d: Plane = Plane::new(Vector3::new(-1.0, 6.0, -5.0).normalized(), 3.2); + + // Origin point and center of `plane_a`. + let vec_a: Vector3 = Vector3::new(0.0, 0.0, 0.0); + assert_eq!(plane_a.center(), vec_a); + // Center of `plane_b`. + let vec_b: Vector3 = plane_b.center(); + // Center of `plane_c`. + let vec_c: Vector3 = plane_c.center(); + + // The origin point should be in `plane_a`, so results in 0.0 distance. + assert!(plane_a.contains_point(vec_a, None)); + assert_eq!(plane_a.distance_to(vec_a), 0.0); + // No matter the normals, the absolute distance to the origin point should always be the absolute + // value of the plane's `d`. + assert_eq!(plane_a.distance_to(vec_a).abs(), plane_a.d.abs()); + assert_eq!(plane_b.distance_to(vec_a).abs(), plane_b.d.abs()); + assert_eq!(plane_c.distance_to(vec_a).abs(), plane_c.d.abs()); + assert_eq!(plane_d.distance_to(vec_a).abs(), plane_d.d.abs()); + // The absolute distance between a plane and its parallel's center should always be the difference + // between both the `d`s. + assert!(plane_b.contains_point(vec_b, None)); + assert_eq_approx!( + plane_a.distance_to(vec_b).abs(), + (plane_a.d - plane_b.d).abs(), + is_equal_approx + ); + assert!(plane_c.contains_point(vec_c, None)); + assert_eq_approx!( + plane_a.distance_to(vec_c).abs(), + (plane_a.d - plane_c.d).abs(), + is_equal_approx + ); + // As `plane_b` is higher than `plane_a` by having larger `d` value, then its center should be + // higher than `plane_a`. + assert!(plane_a.is_point_over(vec_b)); + // As `plane_c` is lower than `plane_a` by having smaller `d` value, then its center should be + // lower than `plane_a`. + assert!(!plane_a.is_point_over(vec_c)); + // By the reasonings stated above, then the following should be correct. + assert!(!plane_b.is_point_over(vec_a)); + assert!(!plane_b.is_point_over(vec_c)); + assert!(plane_c.is_point_over(vec_a)); + assert!(plane_c.is_point_over(vec_b)); + } + + /// Tests `intersect_3()` and `is_equal_approx()`. + #[test] + fn test_three_planes_intersections() { + // Planes that intersects in (0.0, 0.0, 0.0). + let plane_a: Plane = Plane::new(Vector3::new(1.0, 2.0, 3.0).normalized(), 0.0); + let plane_b: Plane = Plane::new(Vector3::new(3.5, 6.0, -3.0).normalized(), 0.0); + let plane_c: Plane = Plane::new(Vector3::new(-1.0, 6.0, 0.5).normalized(), 0.0); + // Planes that parallels `plane_a`. + let plane_d: Plane = Plane::new(Vector3::new(1.0, 2.0, 3.0).normalized(), 1.0); + let plane_e: Plane = Plane::new(Vector3::new(1.0, 2.0, 3.0).normalized(), 2.0); + // Planes that intersects `plane_a` and each other in a common line. + let plane_f: Plane = Plane::new(Vector3::new(1.0, 2.0, -7.0).normalized(), 0.0); + let plane_g: Plane = Plane::new(Vector3::new(1.0, 2.0, 4.0).normalized(), 0.0); + // Planes that intersects each other in 3 parallel lines. + let plane_h: Plane = Plane::new(Vector3::new(1.0, -3.0, 2.0).normalized(), 1.0); + let plane_i: Plane = Plane::new(Vector3::new(4.0, 1.0, -1.0).normalized(), 1.0); + let plane_j: Plane = Plane::new(Vector3::new(6.0, -5.0, 3.0).normalized(), 1.0); + + // Origin point. + let vec_a: Vector3 = Vector3::new(0.0, 0.0, 0.0); + + // Planes that have 0 as its `d` would intersect in the origin point. + assert_eq!(plane_a.intersect_3(&plane_b, &plane_c), Some(vec_a)); + // Three planes that parallel each other would not intersect in a point. + assert_eq!(plane_a.intersect_3(&plane_d, &plane_e), None); + // Two planes that parallel each other with an unrelated third plane would not intersect in + // a point. + assert_eq!(plane_b.intersect_3(&plane_d, &plane_e), None); + // Three coincident planes would intersect in every point, thus no unique solution. + assert!(plane_a.is_equal_approx(&plane_a)); + assert_eq!(plane_a.intersect_3(&plane_a, &plane_a), None); + // Two coincident planes with an unrelated third plane would intersect in every point along the + // intersection line, thus no unique solution. + assert!(plane_b.is_equal_approx(&plane_b)); + assert_eq!(plane_b.intersect_3(&plane_b, &plane_g), None); + // Two coincident planes with a parallel third plane would have no common intersection. + assert_eq!(plane_a.intersect_3(&plane_a, &plane_d), None); + // Three planes that intersects each other in a common line would intersect in every point along + // the line, thus no unique solution. + assert_eq!(plane_a.intersect_3(&plane_f, &plane_g), None); + // Three planes that intersects each other in 3 parallel lines would not intersect in a common + // point. + assert_eq!(plane_h.intersect_3(&plane_i, &plane_j), None); + } + + /// Tests `intersects_ray()`. + #[test] + fn test_ray_intersections() { + // Plane that is flat along the z-axis. + let plane: Plane = Plane::new(Vector3::new(0.0, 0.0, 1.0).normalized(), 0.0); + + // Origin point. + let vec_a: Vector3 = Vector3::new(0.0, 0.0, 0.0); + // Forms a straight line along the z-axis with `vec_a` that is perpendicular to `plane`. + let vec_b: Vector3 = Vector3::new(0.0, 0.0, 0.5); + let vec_c: Vector3 = Vector3::new(0.0, 0.0, 1.0); + let vec_d: Vector3 = Vector3::new(0.0, 0.0, -1.0); + // Forms a slanted line with `vec_a` relative to `plane`. + let vec_e: Vector3 = Vector3::new(0.5, 0.5, 0.0); + // Forms a line with `vec_c` that is parallel with `plane`. + let vec_f: Vector3 = Vector3::new(1.0, 0.0, 1.0); + + // From a point straight up from the origin point, a ray pointing straight down would cross + // the `plane` in the origin point. + assert_eq!(plane.intersects_ray(vec_b, vec_d), Some(vec_a)); + assert_eq!(plane.intersects_ray(vec_c, vec_d), Some(vec_a)); + // From a point straight down the origin point, a ray pointing straight up would cross the `plane` + // in the origin point. + assert_eq!(plane.intersects_ray(vec_d, vec_b), Some(vec_a)); + assert_eq!(plane.intersects_ray(vec_d, vec_c), Some(vec_a)); + // From a point straight up the origin point, the distance from the origin to the intersection point + // formed by a slanted ray should follow the Pythagorean Theorem. + assert_eq!( + plane.intersects_ray(vec_c, vec_e - vec_c).unwrap().length(), + (vec_c.distance_squared_to(vec_e) - vec_c.length_squared()).sqrt() + ); + assert_eq!( + plane.intersects_ray(vec_b, vec_e - vec_b).unwrap().length(), + (vec_b.distance_squared_to(vec_e) - vec_b.length_squared()).sqrt() + ); + // A ray parallel to the `plane` would not intersect the plane. + assert_eq!(plane.intersects_ray(vec_c, vec_f), None); + // A ray pointing to the opposite direction as the `plane` would not intersect it. + assert_eq!(plane.intersects_ray(vec_b, vec_c), None); + assert_eq!(plane.intersects_ray(vec_b, vec_e), None); + } + + /// Tests `intersects_segment()`. + #[test] + fn test_segment_intersections() { + // Plane that is flat along the z-axis. + let plane: Plane = Plane::new(Vector3::new(0.0, 0.0, 1.0).normalized(), 0.0); + + // Origin point. + let vec_a: Vector3 = Vector3::new(0.0, 0.0, 0.0); + // Forms a straight line along the z-axis with `vec_a` that is perpendicular to `plane`. + let vec_b: Vector3 = Vector3::new(0.0, 0.0, 0.5); + let vec_c: Vector3 = Vector3::new(0.0, 0.0, 1.0); + let vec_d: Vector3 = Vector3::new(0.0, 0.0, -1.0); + let vec_e: Vector3 = Vector3::new(0.0, 0.0, -0.5); + // Forms a slanted line with `vec_a` relative to `plane`. + let vec_f: Vector3 = Vector3::new(0.5, 0.5, 0.0); + // Forms a line with `vec_c` that is parallel with `plane`. + let vec_g: Vector3 = Vector3::new(1.0, 0.0, 1.0); + + // From a point straight up from the origin point, a segment pointing straight down would cross + // the `plane` in the origin point only if the segment ended on or beyond the `plane`. + assert_eq!(plane.intersects_segment(vec_b, vec_d), Some(vec_a)); + assert_eq!(plane.intersects_segment(vec_c, vec_d), Some(vec_a)); + assert_eq!(plane.intersects_segment(vec_b, vec_a), Some(vec_a)); + assert_eq!(plane.intersects_segment(vec_c, vec_a), Some(vec_a)); + assert_eq!(plane.intersects_segment(vec_c, vec_b), None); + // From a point straight down the origin point, a segment pointing straight up would cross the `plane` + // in the origin point only if the segment ended on or beyond the `plane`. + assert_eq!(plane.intersects_segment(vec_d, vec_a), Some(vec_a)); + assert_eq!(plane.intersects_segment(vec_d, vec_b), Some(vec_a)); + assert_eq!(plane.intersects_segment(vec_d, vec_c), Some(vec_a)); + assert_eq!(plane.intersects_segment(vec_d, vec_e), None); + // From a point straight up the origin point, the distance from the origin to the intersection point + // formed by a slanted segment should follow the Pythagorean Theorem only if the segment ended on or + // beyond the `plane`. + assert_eq!( + plane.intersects_segment(vec_c, vec_f).unwrap().length(), + (vec_c.distance_squared_to(vec_f) - vec_c.length_squared()).sqrt() + ); + assert_eq!( + plane.intersects_segment(vec_b, vec_f).unwrap().length(), + (vec_b.distance_squared_to(vec_f) - vec_b.length_squared()).sqrt() + ); + // A segment parallel to the `plane` would not intersect the plane. + assert_eq!(plane.intersects_segment(vec_c, vec_g), None); + // A segment pointing to the opposite direction as the `plane` would not intersect it. + assert_eq!(plane.intersects_segment(vec_b, vec_c), None); + assert_eq!(plane.intersects_segment(vec_b, vec_g), None); + } + + /// Tests `is_equal_approx()`. + #[test] + fn test_equal() { + // Initial normalized planes. + let plane_a: Plane = Plane::new(Vector3::new(0.0, 0.0, 1.0).normalized(), 0.0); + let plane_b: Plane = Plane::new(Vector3::new(0.01, 0.0, 1.0).normalized(), 0.0); + let plane_c: Plane = Plane::new(Vector3::new(0.0001, 0.0, 1.0).normalized(), 0.0); + let plane_d: Plane = Plane::new(Vector3::new(0.000001, 0.0, 1.0).normalized(), 0.0); + let plane_e: Plane = Plane::new(Vector3::new(0.000001, 0.0, 1.0).normalized(), 0.01); + let plane_f: Plane = Plane::new(Vector3::new(0.000001, 0.0, 1.0).normalized(), 0.000001); + + // Same planes should be equals. + assert_eq_approx!(&plane_a, &plane_a, Plane::is_equal_approx); + assert_eq_approx!(&plane_b, &plane_b, Plane::is_equal_approx); + // Although similar, planes below shouldn't be approximately equals. + assert_ne_approx!(&plane_a, &plane_b, Plane::is_equal_approx); + assert_ne_approx!(&plane_a, &plane_c, Plane::is_equal_approx); + // Planes below should be approximately equal because it's lower than the set tolerance constant. + assert_eq_approx!(&plane_a, &plane_d, Plane::is_equal_approx); + // Although approximately equal in the `normal` part, it is not approximately equal in the `d` + // part. + assert_ne_approx!(&plane_a, &plane_e, Plane::is_equal_approx); + // Both attributes are approximately equal. + assert_eq_approx!(&plane_a, &plane_f, Plane::is_equal_approx); + // Although considered approximately equal with `plane_a`, `plane_b` is not considered approximately + // equal with `plane_e` because the baseline comparison is tighter. + assert_ne_approx!(&plane_b, &plane_e, Plane::is_equal_approx); + } + + /// Tests `normalize()`. + #[test] + fn test_normalization() { + // Initial normalized planes (Denormalization not done here because the constructor panics + // when creating a plane with unnormalized planes). + let mut plane_a: Plane = Plane::new(Vector3::new(0.7, 2.0, 6.0).normalized(), 0.0); + let mut plane_b: Plane = Plane::new(Vector3::new(1.5, 7.2, 9.1).normalized(), 2.0); + let mut plane_c: Plane = Plane::new(Vector3::new(1.4, 9.1, 1.2).normalized(), 5.3); + let mut plane_d: Plane = Plane::new(Vector3::new(4.2, 2.9, 1.5).normalized(), 2.4); + let plane_e: Plane = Plane::new(Vector3::new(5.1, 3.0, 2.1).normalized(), 0.2); + + // Denormalize planes. + plane_a.normal = Vector3::new(0.7, 2.0, 6.0); + plane_b.normal = Vector3::new(1.5, 7.2, 9.1); + plane_c.normal = Vector3::new(1.4, 9.1, 1.2); + plane_d.normal = Vector3::new(4.2, 2.9, 1.5); + + // Verify normalization method. + assert_eq!(plane_a.normalized().normal, plane_a.normal.normalized()); + assert_eq!(plane_b.normalized().normal, plane_b.normal.normalized()); + assert_eq!(plane_c.normalized().normal, plane_c.normal.normalized()); + assert_eq!(plane_d.normalized().normal, plane_d.normal.normalized()); + assert_eq!(plane_e.normalized().normal, plane_e.normal.normalized()); + } + + /// Tests `is_finite()`. + #[test] + fn test_finite() { + // Initial ordinary planes. + let mut plane_a: Plane = Plane::new(Vector3::new(0.7, 2.0, -6.0).normalized(), 10.2); + let mut plane_b: Plane = Plane::new(Vector3::new(1.5, 7.2, 9.1).normalized(), 20.0); + let mut plane_c: Plane = Plane::new(Vector3::new(1.4, 9.1, 1.2).normalized(), 50.3); + let mut plane_d: Plane = Plane::new(Vector3::new(-4.2, 2.9, 1.5).normalized(), 20.4); + let plane_e: Plane = Plane::new(Vector3::new(7.2, -2.9, 2.2).normalized(), 3.3); + + // Test with `normal`s. + plane_a.normal = Vector3::new(0.7, real::INFINITY, -6.0); + plane_b.normal = Vector3::new(0.7, 2.0, real::NEG_INFINITY); + plane_d.normal = Vector3::new(real::NAN, real::INFINITY, real::NEG_INFINITY); + + // Test with `d`s. + plane_c.d = real::INFINITY; + plane_d.d = real::NAN; + + // Verify finiteness. + assert!(!plane_a.is_finite()); + assert!(!plane_b.is_finite()); + assert!(!plane_c.is_finite()); + assert!(!plane_d.is_finite()); + assert!(plane_e.is_finite()); + } + + /// Tests `project()` and `center()`. + #[test] + fn test_projection() { + // Plane that is flat along the z-axis. + let plane_a: Plane = Plane::new(Vector3::new(0.0, 0.0, 1.0).normalized(), 0.0); + // Parallels `plane_a` + let plane_b: Plane = Plane::new(Vector3::new(0.0, 0.0, 1.0).normalized(), 3.5); + + // Random vectors. + let vec_a: Vector3 = Vector3::new(0.0, 3.2, 1.5); + let vec_b: Vector3 = Vector3::new(1.1, 7.3, -6.4); + let vec_c: Vector3 = Vector3::new(0.5, -7.2, 0.2); + + // Projection of points to `plane_a` would result in the same points, with the z aspect of the + // vector be 0.0. + assert_eq!(plane_a.project(vec_a), Vector3::new(vec_a.x, vec_a.y, 0.0)); + assert_eq!(plane_a.project(vec_b), Vector3::new(vec_b.x, vec_b.y, 0.0)); + assert_eq!(plane_a.project(vec_c), Vector3::new(vec_c.x, vec_c.y, 0.0)); + // Projection of the center of a plane that parallels the plane that is being projected into + // is going to be the center of the plane that is being projected. + assert_eq!(plane_a.project(plane_b.center()), plane_a.center()); + } + #[cfg(feature = "serde")] #[test] fn serde_roundtrip() {