55 lines
2.3 KiB
Python
55 lines
2.3 KiB
Python
"""Scene-authoring admission for a full rover footprint, never an AI map.
|
|
|
|
Ground-height samples can miss a narrow trunk between sample rays. Test mesh
|
|
triangles against the occupied prism using the separating-axis theorem, which
|
|
also catches a face crossing the body when all of its vertices lie outside.
|
|
"""
|
|
|
|
import math
|
|
|
|
import numpy as np
|
|
|
|
|
|
def obstructing_triangles(vertices, faces, xy, heading, plane, *, step=0.1, height=1.1):
|
|
"""Count triangles above qualified step height inside the 1 x 1 m start.
|
|
|
|
``plane`` is z = a*(x-xy[0]) + b*(y-xy[1]) + c, fitted to the start's support.
|
|
This is a conservative preparation gate, not a claim of route traversability.
|
|
It neither changes the collider nor supplies privileged geometry to inference.
|
|
"""
|
|
triangles = np.asarray(vertices)[faces]
|
|
center_xy = np.asarray(xy)
|
|
selected = (triangles[:, :, :2].min(axis=1) <= center_xy + 0.71).all(axis=1) & (
|
|
triangles[:, :, :2].max(axis=1) >= center_xy - 0.71
|
|
).all(axis=1)
|
|
triangles = triangles[selected].astype(np.float64)
|
|
if not len(triangles):
|
|
return 0
|
|
delta = triangles[:, :, :2] - center_xy
|
|
angle = math.radians(heading)
|
|
rotation = np.array([[math.cos(angle), -math.sin(angle)], [math.sin(angle), math.cos(angle)]])
|
|
triangles[:, :, 2] -= delta @ np.asarray(plane[:2]) + plane[2]
|
|
triangles[:, :, :2] = delta @ rotation
|
|
low = step + 1e-4 # Same centimetre-scale capability boundary as navigation.
|
|
half = np.array([0.5, 0.5, (height - low) / 2])
|
|
triangles[:, :, 2] -= (height + low) / 2
|
|
selected = (triangles.min(axis=1) <= half).all(axis=1) & (triangles.max(axis=1) >= -half).all(
|
|
axis=1
|
|
)
|
|
triangles = triangles[selected]
|
|
if not len(triangles):
|
|
return 0
|
|
edges = np.roll(triangles, -1, axis=1) - triangles
|
|
axes = [np.cross(edges[:, 0], edges[:, 1])]
|
|
for edge in range(3):
|
|
for box_axis in np.eye(3):
|
|
axes.append(np.cross(edges[:, edge], box_axis))
|
|
overlaps = np.ones(len(triangles), dtype=bool)
|
|
for axis in axes:
|
|
projections = np.einsum("nvi,ni->nv", triangles, axis)
|
|
radius = np.abs(axis) @ half
|
|
overlaps &= (projections.min(axis=1) <= radius + 1e-10) & (
|
|
projections.max(axis=1) >= -radius - 1e-10
|
|
)
|
|
return int(overlaps.sum())
|