diff --git a/pyrr/geometric_tests.py b/pyrr/geometric_tests.py index b2201be..68ba2f4 100755 --- a/pyrr/geometric_tests.py +++ b/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. diff --git a/tests/test_geometric_tests.py b/tests/test_geometric_tests.py index 3974d08..8f06d31 100644 --- a/tests/test_geometric_tests.py +++ b/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.)