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

2026-09-25 16:40:45 +03:00
parent a7c64e009d
commit f01bd39037
88 changed files with 9918 additions and 108 deletions
@@ -0,0 +1,105 @@
+"""Offline authoring check for a stable, metre-wide start on a scan proxy.
+
+Uses world geometry only to prepare a scene. No candidate map enters navigation.
+An operator/engineer still verifies the chosen start against the visual trail.
+"""
+
+import argparse
+import json
+import math
+import sys
+from pathlib import Path
+
+import numpy as np
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+from spawn_clearance import obstructing_triangles
+from terrain import load_glb
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--terrain", type=Path, required=True)
+    args = parser.parse_args()
+    manifest = json.loads((args.terrain / "terrain.json").read_text(encoding="utf-8-sig"))
+    settings = manifest["settings"]
+    points, indices = load_glb(args.terrain / "terrain.collision.glb")
+    triangles = points[indices]
+    low, high = triangles.min(axis=1), triangles.max(axis=1)
+    center = np.array(settings["spawn_xy"])
+    selected = (
+        (low[:, :2] <= center + 2.5).all(axis=1)
+        & (high[:, :2] >= center - 2.5).all(axis=1)
+        & (low[:, 2] < settings["ground_z"] + 1.5)
+        & (high[:, 2] > settings["ground_z"] - 0.8)
+    )
+    triangles, low, high = triangles[selected], low[selected], high[selected]
+
+    def heights(x, y):
+        hits = triangles[
+            (low[:, 0] <= x) & (high[:, 0] >= x) & (low[:, 1] <= y) & (high[:, 1] >= y)
+        ]
+        if not len(hits):
+            return np.empty(0)
+        a, b, c = hits[:, 0], hits[:, 1], hits[:, 2]
+        den = (b[:, 1] - c[:, 1]) * (a[:, 0] - c[:, 0]) + (c[:, 0] - b[:, 0]) * (a[:, 1] - c[:, 1])
+        valid = np.abs(den) > 1e-8
+        a, b, c, den = a[valid], b[valid], c[valid], den[valid]
+        u = ((b[:, 1] - c[:, 1]) * (x - c[:, 0]) + (c[:, 0] - b[:, 0]) * (y - c[:, 1])) / den
+        v = ((c[:, 1] - a[:, 1]) * (x - c[:, 0]) + (a[:, 0] - c[:, 0]) * (y - c[:, 1])) / den
+        inside = (u >= -1e-6) & (v >= -1e-6) & (u + v <= 1 + 1e-6)
+        return (u * a[:, 2] + v * b[:, 2] + (1 - u - v) * c[:, 2])[inside]
+
+    angle = math.radians(settings["heading_degrees"])
+    rotation = np.array([[math.cos(angle), -math.sin(angle)], [math.sin(angle), math.cos(angle)]])
+    footprint = np.array([[x, y] for x in (-0.5, 0, 0.5) for y in (-0.5, 0, 0.5)]) @ rotation.T
+    candidates = []
+    for dx in np.arange(-2, 2.01, 0.2):
+        for dy in np.arange(-2, 2.01, 0.2):
+            position = center + [dx, dy]
+            support = []
+            for x, y in footprint + position:
+                z = heights(x, y)
+                near = z[np.abs(z - settings["ground_z"]) < 0.8]
+                if not len(near):
+                    break
+                ground = near.max()
+                if np.any((z > ground + 0.12) & (z < ground + 1)):
+                    break
+                support.append(float(ground))
+            if len(support) != 9:
+                continue
+            design = np.column_stack((footprint, np.ones(9)))
+            plane = np.linalg.lstsq(design, np.asarray(support), rcond=None)[0]
+            residual = float(np.max(np.abs(design @ plane - support)))
+            slope = math.degrees(math.atan(np.linalg.norm(plane[:2])))
+            if residual > 0.08 or slope > 20:
+                continue
+            if obstructing_triangles(
+                points, indices, position, settings["heading_degrees"], plane
+            ):
+                continue
+            candidates.append(
+                {
+                    "xy": position.tolist(),
+                    "ground_z": float(np.median(support)),
+                    "height_span": max(support) - min(support),
+                    "offset_m": math.hypot(dx, dy),
+                    "residual_m": residual,
+                    "slope_degrees": slope,
+                }
+            )
+    candidates.sort(key=lambda row: row["offset_m"] + 2 * row["height_span"])
+    print(
+        json.dumps(
+            {
+                "world_sha256": manifest["source_sha256"],
+                "candidate_count": len(candidates),
+                "candidates": candidates[:12],
+            }
+        )
+    )
+
+
+if __name__ == "__main__":
+    main()