Aabb plane intersection

pyrr/geometric_tests.py

@@ -5,7 +5,7 @@
 from __future__ import absolute_import, division, print_function
 import math
 import numpy as np
-from . import rectangle, vector, vector3, plane
+from . import rectangle, vector, vector3, plane, aabb
 from .utils import all_parameters_as_numpy_arrays, parameters_as_numpy_arrays, solve_quadratic_equation
 
 """
@@ -306,6 +306,42 @@ def ray_intersect_aabb(ray, aabb):
     point = ray[0] + (ray[1] * t)
     return point
 
+@all_parameters_as_numpy_arrays
+def plane_intersect_aabb(plane_in, aabb_in):
+    """Calculates one intersection point of the plane and the aabb.
+       This point is granted to be inside the aabb and along the ray passing by the aabb center and
+       parallel to the plane normal. Otherwise explained it is the intersecting point nearest to the
+       aabb center.
+    :param numpy.array plane: The plane to check.
+    :param numpy.array aabb: The Axis-Aligned Bounding Box to check against.
+    :rtype: numpy.array
+    :return: Returns a point if an intersection occurs.
+        Returns None if no intersection occurs.
+    """
+    """
+    https://gdbooks.gitbooks.io/3dcollisions/content/Chapter2/static_aabb_plane.html
+    """
+
+    # Convert AABB to center-extents representation
+    obj_aabb = aabb.create_from_points(aabb_in)
+    aabb_center = aabb.centre_point(obj_aabb)
+    bary_extent = aabb.maximum(obj_aabb) - aabb_center
+
+    plane_origin = plane.position(plane_in)
+    plane_normal = plane.normal(plane_in)
+
+    # Compute the projection interval radius of aabb onto L(t) = aabb_center + t * plane_normal
+    r = np.dot(bary_extent, np.abs(plane_normal))
+
+    # Compute distance of aabb center from plane origin along plane normal
+    s = np.dot(plane_normal, plane_origin - aabb_center)
+
+    # Intersection occurs when distance s falls within [-r,+r] interval
+    if abs(s) <= r:
+        return aabb_center + plane_normal * s
+    else:
+        return None
+
 @all_parameters_as_numpy_arrays
 def point_height_above_plane(point, pl):
     """Calculates how high a point is above a plane.

tests/test_geometric_tests.py

@@ -6,7 +6,7 @@
     import unittest
 import numpy as np
 from pyrr import geometric_tests as gt
-from pyrr import line, plane, ray, sphere
+from pyrr import line, plane, ray, sphere, aabb
 
 
 class test_geometric_tests(unittest.TestCase):
@@ -208,6 +208,86 @@ def test_ray_intersect_aabb_ray_on_a_boundary_plane(self):
         r = np.array([[1.0,0.0,0.0], [0.0,1.0,1.0]])
         result = gt.ray_intersect_aabb(r, a)
         self.assertTrue(np.array_equal(result, [1.0, 1.0, 1.0]))
+
+    def test_plane_intersect_aabb_valid_1(self):
+        aabb_min  = [-100.0, 50.0,-80.0]
+        aabb_max = [200.0, 90.0, -20.0]
+        aabb = np.array([aabb_min, aabb_max])
+        plane_orig = [0.0, 0.0, 0.0]
+        p_pos = np.array(plane_orig)
+        plane_normal = [1.0, 0.0, 0.0]
+        p_norm = np.array(plane_normal)
+        p = plane.create_from_position(p_pos, p_norm)
+        result = gt.plane_intersect_aabb(p, aabb)
+        expected_result = np.array([plane_orig[0], (aabb_min[1]+aabb_max[1]) * 0.5, (aabb_min[2]+aabb_max[2]) * 0.5])
+        self.assertTrue(np.array_equal(result, expected_result))
+
+    def test_plane_intersect_aabb_valid_2(self):
+        aabb_min  = [-100.0, 50.0,-80.0]
+        aabb_max = [200.0, 90.0, -20.0]
+        aabb = np.array([aabb_min, aabb_max])
+        plane_orig = [0.0, 60.0, 0.0]
+        p_pos = np.array(plane_orig)
+        plane_normal = [0.0, 1.0, 0.0]
+        p_norm = np.array(plane_normal)
+        p = plane.create_from_position(p_pos, p_norm)
+        result = gt.plane_intersect_aabb(p, aabb)
+        expected_result = np.array([(aabb_min[0]+aabb_max[0]) * 0.5, plane_orig[1], (aabb_min[2]+aabb_max[2]) * 0.5])
+        self.assertTrue(np.array_equal(result, expected_result))        
+
+    def test_plane_intersect_aabb_valid_3(self):
+        aabb_min  = [-100.0, 50.0,-80.0]
+        aabb_max = [200.0, 90.0, -20.0]
+        aabb = np.array([aabb_min, aabb_max])
+        plane_orig = [0.0, 0.0, -21.30]
+        p_pos = np.array(plane_orig)
+        plane_normal = [ 0.0, 0.0, -1.0]
+        p_norm = np.array(plane_normal)
+        p = plane.create_from_position(p_pos, p_norm)
+        result = gt.plane_intersect_aabb(p, aabb)
+        expected_result = np.array([(aabb_min[0]+aabb_max[0]) * 0.5, (aabb_min[1]+aabb_max[1]) * 0.5, plane_orig[2]])
+        self.assertTrue(np.array_equal(result, expected_result))
+
+    def test_plane_intersect_aabb_on_boundary(self):
+        aabb_min  = [-100.0, 50.0,-80.0]
+        aabb_max = [200.0, 90.0, -20.0]
+        aabb = np.array([aabb_min, aabb_max])
+        plane_orig = [0.0, 0.0, -80.0]
+        p_pos = np.array(plane_orig)
+        plane_normal = [ 0.0, 0.0, -1.0]
+        p_norm = np.array(plane_normal)
+        p = plane.create_from_position(p_pos, p_norm)
+        result = gt.plane_intersect_aabb(p, aabb)
+        expected_result = np.array([(aabb_min[0]+aabb_max[0]) * 0.5, (aabb_min[1]+aabb_max[1]) * 0.5, plane_orig[2]])
+        self.assertTrue(np.array_equal(result, expected_result))        
+
+    def test_plane_intersect_aabb_invalid_1(self):
+        aabb_min  = [-100.0, 50.0,-80.0]
+        aabb_max = [200.0, 90.0, -20.0]
+        aabb = np.array([aabb_min, aabb_max])
+        plane_orig = [-200.0, 40.0, -90.0]
+        p_pos = np.array(plane_orig)
+        plane_normal = np.array(aabb_max) - np.array(aabb_min)
+        p_norm = plane_normal
+        p = plane.create_from_position(p_pos, p_norm)
+        result = gt.plane_intersect_aabb(p, aabb)
+        self.assertEqual(result, None)
+
+    def test_plane_intersect_aabb_invalid_2(self):
+        aabb_min  = [-100.0, 50.0,-80.0]
+        aabb_max = [200.0, 90.0, -20.0]
+        aabb = np.array([aabb_min, aabb_max])
+        plane_orig = [aabb_max[0], aabb_min[1], aabb_max[2]]
+        p_pos = np.array(plane_orig)
+        aabb_xy = np.array(aabb_max) - np.array(aabb_min)
+        aabb_xy[2] = aabb_min[2]
+        plane_normal = np.cross(np.array(aabb_max) - np.array(aabb_min), aabb_xy)
+        plane_normal = plane_normal / np.linalg.norm(plane_normal)
+        p_norm = plane_normal
+        p_pos = p_pos + plane_normal * -10.0
+        p = plane.create_from_position(p_pos, p_norm)
+        result = gt.plane_intersect_aabb(p, aabb)
+        self.assertEqual(result, None)        
 
     def test_point_height_above_plane(self):
         pl = plane.create([0., 1., 0.], 1.)