feat(simulation): add Worker AI polygon runtime and terrain navigation

simulation/ai-polygon/spawn_clearance.py

@@ -0,0 +1,54 @@
+"""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())