106 lines
4.1 KiB
Python
106 lines
4.1 KiB
Python
"""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()
|