def test_large_random_objects():
for i in range(1, 8):
for j in range(1, 8):
for k in range(1000):
arr1 = 10000.0*np.random.rand(i, 3)
arr2 = 10000.0*np.random.rand(j, 3)
opengjk.pygjk(arr1, arr2)
def test_hex_collision_3d(delta):
hex_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0],
[0, 0, 1], [1, 0, 1], [0, 1, 1], [1, 1, 1]],
dtype=np.float64)
P0 = np.array([1.5+delta, 1.5+delta, 0.5], dtype=np.float64)
P1 = np.array([2, 2, 1], dtype=np.float64)
P2 = np.array([2, 1.25, 0.25], dtype=np.float64)
P3 = P1 + P2 - P0
quad_1 = np.array([P0, P1, P2, P3], dtype=np.float64)
n = (np.cross(quad_1[1]-quad_1[0], quad_1[2]-quad_1[0]) /
np.linalg.norm(
np.cross(quad_1[1]-quad_1[0],
quad_1[2]-quad_1[0])))
quad_2 = quad_1 + n
hex_2 = np.zeros((8, 3), dtype=np.float64)
hex_2[:4, :] = quad_1
hex_2[4:, :] = quad_2
actual_distance = np.linalg.norm(
np.array([1, 1, P0[2]], dtype=np.float64)-hex_2[0])
distance = opengjk.pygjk(hex_1, hex_2)
if P0[0] < 1:
assert(np.isclose(distance, 0, atol=settol()))
else:
print("Computed distance ", distance,
"Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol()))
def test_line_point_distance(delta):
line = np.array([[0.1, 0.2, 0.3], [0.5, 0.8, 0.7]], dtype=np.float64)
point_on_line = line[0] + 0.27*(line[1]-line[0])
normal = np.cross(line[0], line[1])
point = point_on_line + delta * normal
distance = opengjk.pygjk(line, point)
actual_distance = distance_point_to_line_3D(
line[0], line[1], point)
print(distance, actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
def test_tetra_distance_3d(delta):
tetra_1 = np.array([[0, 0, 0.2], [1, 0, 0.1], [0, 1, 0.3],
[0, 0, 1]], dtype=np.float64)
tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
[0.5, 0.3, -delta]], dtype=np.float64)
actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
tetra_1[2], tetra_2[3])
distance = opengjk.pygjk(tetra_1, tetra_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
def test_line_line_distance(delta):
line = np.array([[-0.5, -0.7, -0.3], [1, 2, 3]], dtype=np.float64)
point_on_line = line[0] + 0.38*(line[1]-line[0])
normal = np.cross(line[0], line[1])
point = point_on_line + delta * normal
line_2 = np.array([point, [2, 5, 6]], dtype=np.float64)
distance = opengjk.pygjk(line, line_2)
actual_distance = distance_point_to_line_3D(
line[0], line[1], line_2[0])
print(distance, actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
def test_quad_distance2d(delta):
quad_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
[1, 1, 0]], dtype=np.float64)
quad_2 = np.array([[0, 1+delta, 0], [2, 2, 0],
[2, 4, 0], [4, 4, 0]], dtype=np.float64)
P1 = quad_1[2]
P2 = quad_1[3]
point = quad_2[0]
actual_distance = distance_point_to_line_3D(P1, P2, point)
distance = opengjk.pygjk(quad_1, quad_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
assert(np.isclose(distance, actual_distance, atol=settol() ))
def test_tri_distance(delta):
tri_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64)
tri_2 = np.array([[1, delta, 0], [3, 1.2, 0], [
1, 1, 0]], dtype=np.float64)
P1 = tri_1[2]
P2 = tri_1[1]
point = tri_2[0]
actual_distance = distance_point_to_line_3D(P1, P2, point)
distance = opengjk.pygjk(tri_1, tri_2)
print("Computed distance ", distance, "Actual distance ", actual_distance)
#embed()
assert(np.isclose(distance, actual_distance, atol=settol() ))
def test_cube_distance(c0, c1):
cubes = [np.array([[-1, -1, -1], [1, -1, -1], [-1, 1, -1], [1, 1, -1],
[-1, -1, 1], [1, -1, 1], [-1, 1, 1], [1, 1, 1]],
dtype=np.float64)]
r = R.from_euler('z', 45, degrees=True)
cubes.append(r.apply(cubes[0]))
r = R.from_euler('y', np.arctan2(1.0, np.sqrt(2)))
cubes.append(r.apply(cubes[1]))
r = R.from_euler('y', 45, degrees=True)
cubes.append(r.apply(cubes[0]))
dx = cubes[c0][:,0].max() - cubes[c1][:,0].min()
cube0 = cubes[c0]
for delta in [1e8, 1.0, 1e-4, 1e-8, 1e-12]:
cube1 = cubes[c1] + np.array([dx + delta, 0, 0])
distance = opengjk.pygjk(cube0, cube1)
print(distance, delta)
assert(np.isclose(distance, delta))
# #### # ###### # # ##### ##### # # # # # # This file is part of openGJK. # # # # OpenGJK is free software: you can redistribute it and/or modify # # it under the terms of the GNU General Public License as published by # # the Free Software Foundation, either version 3 of the License, or # # any later version. # # # # OpenGJK is distributed in the hope that it will be useful, # # but WITHOUT ANY WARRANTY; without even the implied warranty of # # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The # # GNU General Public License for more details. # # # # You should have received a copy of the GNU General Public License # # along with OpenGJK. If not, see <https://www.gnu.org/licenses/>. # # # # openGJK: open-source Gilbert-Johnson-Keerthi algorithm # # Copyright (C) Mattia Montanari 2018 - 2020 # # http://iel.eng.ox.ac.uk/?page_id=504 # # # # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # import numpy as np import openGJK_cython as opengjk a = np.array([[1., 1., 1.], [1., 1., 1.]]) b = np.array([[11., 1., 1.], [1., 1., 1.]]) d = opengjk.pygjk(a, b) print(d)
