VoxelForge/Standalone/Research/WorldAssemblerM3/Linux36/ProbeWalkFloorProfileL36.py

#!/usr/bin/env python3
"""Measure the exported walk surface along the passage under the tunnel camera."""
import argparse
import importlib.util
import json
import math
import re
from pathlib import Path

ROOT = Path(__file__).resolve().parents[4]
RING_PATH = ROOT / "Standalone/Research/WorldAssemblerM3/Linux32/AuditRingTerraceL32.py"
SPEC = importlib.util.spec_from_file_location("ring_profile", RING_PATH)
RING = importlib.util.module_from_spec(SPEC)
SPEC.loader.exec_module(RING)
CAMERA_RE = re.compile(r"^\[L34CaptureCamera\] view=(\S+).*?camera_m=\(([^)]*)\) "
                       r"target_m=\(([^)]*)\)", re.M)


def length(a, b):
    return math.sqrt(sum((a[i] - b[i]) ** 2 for i in range(3)))


def fit(values, spacing):
    best = (0.0, 0.0)
    for step in range(8, 129):
        period = step * 0.0625
        omega = 2.0 * math.pi / period
        cc = ss = cs = yc = ys = 0.0
        for i, value in enumerate(values):
            phase = omega * i * spacing
            c, s = math.cos(phase), math.sin(phase)
            cc += c * c
            ss += s * s
            cs += c * s
            yc += value * c
            ys += value * s
        determinant = cc * ss - cs * cs
        if determinant <= 1.0e-9:
            continue
        cosine = (yc * ss - ys * cs) / determinant
        sine = (ys * cc - yc * cs) / determinant
        amplitude = math.hypot(cosine, sine)
        if amplitude > best[1]:
            best = period, amplitude
    return best


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("mesh")
    parser.add_argument("layout")
    parser.add_argument("--view", default="owner-sw-mid-tunnel")
    parser.add_argument("--passage", default="passage/0-1/0")
    parser.add_argument("--window", type=float, default=32.0)
    parser.add_argument("--spacing", type=float, default=0.125)
    parser.add_argument("--amplitude-limit-cm", type=float, default=1.5)
    args = parser.parse_args()
    log = ROOT / "Saved/Linux/l34/run-capture-standard-set-final-radius3.log"
    cameras = {name: (tuple(map(float, camera.split(","))),
                      tuple(map(float, target.split(","))))
               for name, camera, target in CAMERA_RE.findall(
                   log.read_text(encoding="utf-8", errors="replace"))}
    if args.view not in cameras:
        raise SystemExit(f"missing camera target for {args.view}")
    camera, target = cameras[args.view]
    layout = json.loads(Path(args.layout).read_text(encoding="utf-8"))
    scale = layout["core_voxel_size_um"] / (256.0 * 1_000_000.0)
    passage = next(p for p in layout["passages"] if p["stable_id"] == args.passage)
    controls = [(tuple(v * scale for v in point["position_q"]),
                 point["radius_q"] * scale) for point in passage["control_points"]]
    lower_mouth = tuple(v * scale for v in passage["lower_mouth_q"])
    end = min(range(len(controls)), key=lambda i: length(controls[i][0], lower_mouth))
    controls = controls[:end + 1]
    path = []
    distance = 0.0
    for point in controls:
        if path:
            distance += length(path[-1][0], point[0])
        path.append((point[0], point[1], distance))
    closest = None
    for index, (a, b) in enumerate(zip(path, path[1:])):
        delta = tuple(b[0][axis] - a[0][axis] for axis in range(3))
        sq = sum(v * v for v in delta)
        t = max(0.0, min(1.0, sum((target[k] - a[0][k]) * delta[k]
                                  for k in range(3)) / sq)) if sq else 0.0
        point = tuple(a[0][k] + t * delta[k] for k in range(3))
        d2 = sum((point[k] - target[k]) ** 2 for k in range(3))
        if closest is None or d2 < closest[0]:
            closest = (d2, a[2] + t * (b[2] - a[2]), index, t)
    _, center_distance, segment, segment_t = closest

    def interpolate(distance_value):
        distance_value = max(0.0, min(path[-1][2], distance_value))
        for a, b in zip(path, path[1:]):
            if distance_value <= b[2]:
                t = (distance_value - a[2]) / max(1.0e-9, b[2] - a[2])
                point = tuple(a[0][k] + t * (b[0][k] - a[0][k]) for k in range(3))
                radius = a[1] + t * (b[1] - a[1])
                return point, radius
        return path[-1][0], path[-1][1]

    half = args.window * 0.5
    start = max(0.0, center_distance - half)
    finish = min(path[-1][2], center_distance + half)
    count = int((finish - start) / args.spacing) + 1
    positions = []
    expected = []
    for i in range(count):
        route_distance = start + i * args.spacing
        point, radius = interpolate(route_distance)
        positions.append((route_distance - center_distance, point))
        expected.append(point[2] - radius + 0.20)
    bounds = (min(p[0] for _, p in positions) - 8.0,
              max(p[0] for _, p in positions) + 8.0,
              min(p[1] for _, p in positions) - 8.0,
              max(p[1] for _, p in positions) + 8.0)
    zmin, zmax = min(expected) - 1.5, max(expected) + 1.5
    voxel_um, triangles, tiles = RING.read_scene(args.mesh, 0, zmin, zmax, bounds)
    bucket_size = max(2.0, 16.0 * voxel_um / 1_000_000.0)
    buckets = RING.build_buckets(triangles, True, bounds, bucket_size)
    values = []
    for (_, point), preferred in zip(positions, expected):
        hit = RING.sample_surface(buckets, point[0], point[1], True,
                                  bucket_size, preferred)
        values.append(hit[0] if hit else None)
    valid = [(index, z) for index, z in enumerate(values) if z is not None]
    if len(valid) < 0.8 * len(values):
        raise SystemExit(f"insufficient mesh floor coverage {len(valid)}/{len(values)}")
    residual = [values[i] - expected[i] for i in range(len(values))
                if values[i] is not None]
    mean = sum(residual) / len(residual)
    period, amplitude = fit([v - mean for v in residual], args.spacing)
    adjacent = [abs(values[i] - values[i - 1]) for i in range(1, len(values))
                if values[i] is not None and values[i - 1] is not None]
    route_segment_length = path[segment + 1][2] - path[segment][2]
    print(f"[WalkFloorProfileProbe] mesh={args.mesh} view={args.view} voxel_um={voxel_um} "
          f"tiles={tiles} samples={len(valid)}/{len(values)} window_m={finish-start:.3f} "
          f"camera_target_m=({target[0]:.3f},{target[1]:.3f},{target[2]:.3f}) "
          f"route_distance_m={center_distance:.3f} nearest_segment={segment} "
          f"segment_t={segment_t:.3f} segment_length_m={route_segment_length:.3f} "
          f"floor_expected_range_m={min(expected):.3f}..{max(expected):.3f} "
          f"residual_range_m={min(residual):.4f}..{max(residual):.4f} "
          f"period_m={period:.3f} amplitude_m={amplitude:.5f} "
          f"amplitude_cm={amplitude*100:.3f} adjacent_step_max_m="
        f"{max(adjacent, default=0.0):.4f} limit_cm={args.amplitude_limit_cm:.3f} "
        f"status={'PASS' if amplitude*100 <= args.amplitude_limit_cm else 'FAIL'}")
    out = Path(args.mesh).with_suffix(".walk-floor-profile.csv")
    with out.open("w", encoding="utf-8") as stream:
        stream.write("distance_m,route_x_m,route_y_m,expected_floor_m,mesh_floor_m,residual_m\n")
        for i, (distance_m, point) in enumerate(positions):
            z = values[i]
            stream.write(f"{distance_m:.3f},{point[0]:.6f},{point[1]:.6f},"
                         f"{expected[i]:.6f},{'' if z is None else f'{z:.6f}'},"
                         f"{'' if z is None else f'{z-expected[i]:.6f}'}\n")


if __name__ == "__main__":
    main()