godot-core: builtin: reimplement Plane functions/methods

Author: T4rmyn

godot-core/src/builtin/plane.rs

@@ -9,24 +9,26 @@ use std::ops::Neg;
 use godot_ffi as sys;
 use sys::{ffi_methods, GodotFfi};
 
-use super::{is_equal_approx, real, Vector3};
+use super::{is_zero_approx, is_equal_approx, real, Vector3};
+
+pub const CMP_EPSILON: real = 0.00001;
 
 /// 3D plane in [Hessian normal form](https://mathworld.wolfram.com/HessianNormalForm.html).
 ///
 /// The Hessian form defines all points `point` which satisfy the equation
 /// `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.
 #[derive(Copy, Clone, PartialEq, Debug)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
 #[repr(C)]
 pub struct Plane {
+    /// The plane's normal component.
     pub normal: Vector3,
+    /// The plane's d component.
     pub d: real,
 }
 
@@ -105,6 +107,90 @@ impl Plane {
         }
     }
 
+    /// 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`.
+    ///
+    /// _Godot equivalent: `Plane.distance_to(Vector3 point)`_
+    #[inline]
+    pub fn distance_to(&self, point: Vector3) -> real {
+        self.normal.dot(point) - self.d
+    }
+
+    /// Returns the center of the `self` `Plane` in `Vector3` form. 
+    ///
+    /// _Godot equivalent: `Plane.get_center()`_
+    #[inline]
+    pub fn get_center(&self) -> Vector3 {
+        self.normal * self.d
+    }
+
+    /// 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]
+    pub fn has_point(&self, point: Vector3, tolerance: Option<real>) -> bool {
+        let dist: real = (self.normal.dot(point) - self.d).abs();
+	    dist <= tolerance.unwrap_or(CMP_EPSILON)
+    }
+
+    /// 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` is returned.
+    ///
+    /// _Godot equivalent: `Plane.intersect_3(Place b, Plane c)`_
+    #[inline]
+    pub fn intersect_3(&self, b: &Self, c: &Self) -> Option<Vector3> {
+        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)) /
+                      denom;
+        Some(result)
+    }
+
+    /// 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`
+    /// is returned.
+    ///
+    /// _Godot equivalent: `Plane.intersects_ray(Vector3 from, Vector3 dir)`_
+    #[inline]
+    pub fn intersects_ray(&self, from: Vector3, dir: Vector3) -> Option<Vector3> {
+	    let den: real = self.normal.dot(dir);
+        if is_zero_approx(den) {
+            return None;
+        }
+        let dist: real = (self.normal.dot(from) - self.d) / den;
+        if dist > CMP_EPSILON {
+            return None;
+        }
+        Some(from + dir * -dist)
+    }
+
+    /// 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.
+    ///
+    /// _Godot equivalent: `Plane.intersects_segment(Vector3 from, Vector3 to)`_
+    #[inline]
+    pub fn intersects_segment(&self, from: Vector3, to: Vector3) -> Option<Vector3> {
+        let segment: Vector3 = from - to;
+        let den: real = self.normal.dot(segment);
+        if is_zero_approx(den) {
+            return None;
+        }
+        let dist: real = (self.normal.dot(from) - self.d) / den;
+        if dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON) {
+            return None;
+        }
+        Some(from + segment * -dist)
+    }
+
     /// Returns `true` if the two `Plane`s are approximately equal, by calling `is_equal_approx` on
     /// `normal` and `d` or on `-normal` and `-d`.
     ///
@@ -115,6 +201,39 @@ impl Plane {
             || (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`. 
+    ///
+    /// _Godot equivalent: `Plane.is_finite()`_
+    #[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`.
+    ///
+    /// _Godot equivalent: `Plane.is_point_over(Vector3 point)`_
+    #[inline]
+    pub fn is_point_over(&self, point: Vector3) -> bool {
+        self.normal.dot(point) > self.d
+    }
+
+    /// Returns a normalized copy of the `self` `Plane`.
+    ///
+    /// _Godot equivalent: `Plane.normalized()`_
+    #[inline]
+    pub fn normalized(&self) -> Self {
+	    self.normal.normalized();
+        self.clone()
+    }
+
+    /// Returns a orthogonal of the variable `point` into a point in the `self` `Plane`.
+    ///
+    /// _Godot equivalent: `Plane.project(Vector3 point)`_
+    #[inline]
+    pub fn project(&self, point: Vector3) -> Vector3 {
+        point - self.normal * self.distance_to(point)
+    }
+
     #[inline]
     fn assert_normalized(self) {
         assert!(
@@ -202,4 +321,36 @@ mod test {
 
         crate::builtin::test_utils::roundtrip(&plane, expected_json);
     }
+
+    /// Tests plane methods.
+    #[test]
+    fn tests_methods() {
+        // Random planes including (0.0, 0.0, 0.0).
+        let a: Plane = Plane::new(Vector3::new(1.0, 2.0, 3.0).normalized(), 0.0);
+        let b: Plane = Plane::new(Vector3::new(0.5, 2.1, 7.1).normalized(), 0.0);
+        let c: Plane = Plane::new(Vector3::new(4.3, 9.2, 3.14).normalized(), 0.0);
+        let d: Vector3 = Vector3::new(0.0, 0.0, 0.0);
+        let e: Vector3 = Vector3::new(0.0, 0.0, 1.0);
+        assert_eq!(a.distance_to(d), 0.0);
+        assert_eq!(a.get_center(), d);
+        assert_eq!(a.has_point(d, None), true);
+        assert_eq!(match a.intersect_3(&b, &c) {
+            Some(x) => x,
+            None => Vector3::new(1.0, 1.0, 1.0),
+        }, d);
+        assert_eq!(a.is_finite(), true);
+        assert_eq!(a.is_point_over(d), false);
+        assert_eq!(a.is_point_over(e), true);
+        assert_eq!(a.is_point_over(-e), false);
+        assert_eq!(match a.intersects_ray(d, e) {
+            Some(x) => x,
+            None => Vector3::new(1.0, 1.0, 1.0),
+        }, d);
+        assert_eq!(match a.intersects_segment(d, e) {
+            Some(x) => x,
+            None => Vector3::new(1.0, 1.0, 1.0),
+        }, d);
+        assert_eq!(a.is_equal_approx(&a.normalized()), true);
+        assert_eq!(a.project(Plane::new(a.normalized().normal, 3.0).get_center()), a.get_center());
+    }
 }