43 lines
1.6 KiB
Python
43 lines
1.6 KiB
Python
"""Prevent a reconstructed trunk/face from being admitted between ground rays."""
|
|
|
|
import importlib.util
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
SPEC = importlib.util.spec_from_file_location(
|
|
"spawn_clearance", Path(__file__).parents[1] / "simulation/ai-polygon/spawn_clearance.py"
|
|
)
|
|
MODULE = importlib.util.module_from_spec(SPEC)
|
|
SPEC.loader.exec_module(MODULE)
|
|
|
|
|
|
def count(points, xy=(0, 0), heading=0, plane=(0, 0, 0)):
|
|
return MODULE.obstructing_triangles(np.array(points), np.array([[0, 1, 2]]), xy, heading, plane)
|
|
|
|
|
|
def test_narrow_trunk_between_support_rays_and_crossing_triangle():
|
|
assert count([[0.21, 0.21, 0], [0.23, 0.21, 0], [0.21, 0.21, 2]]) == 1
|
|
# All vertices outside the prism, but the face crosses through its centre.
|
|
assert count([[-2, 0, 0.3], [2, 0, 0.3], [0, 2, 0.3]]) == 1
|
|
|
|
|
|
def test_ground_step_and_overhead_geometry_do_not_block_start():
|
|
for z in (0, 0.05, 0.10, 1.5):
|
|
assert count([[-2, -2, z], [2, -2, z], [0, 2, z]]) == 0
|
|
assert count([[-2, -2, 0.12], [2, -2, 0.12], [0, 2, 0.12]]) == 1
|
|
|
|
|
|
def test_triangle_bounding_box_alone_does_not_reject_empty_corner():
|
|
assert count([[0.4, 2, 0.5], [2, 0.4, 0.5], [2, 2, 0.5]]) == 0
|
|
|
|
|
|
def test_clearance_tracks_translation_heading_and_support_slope():
|
|
points = np.array([[0.6, 0, 0.3], [0.7, 0, 0.3], [0.6, 0.03, 0.8]])
|
|
assert count(points) == 0
|
|
assert count(points, heading=45) == 1
|
|
points[:, 2] += points[:, 0] * 0.2 + 4
|
|
points[:, :2] += [10, -3]
|
|
assert count(points, (10, -3), 45, (0.2, 0, 4)) == 1
|
|
assert count([[8, -5, 3.6], [12, -5, 4.4], [10, -1, 4]], (10, -3), 45, (0.2, 0, 4)) == 0
|