422 lines
14 KiB
Python
422 lines
14 KiB
Python
"""Multibeam echosounder (МЛЭ) simulator helpers: seafloor mesh + OBJ export."""
|
||
|
||
from __future__ import annotations
|
||
|
||
import json
|
||
import math
|
||
import random
|
||
from datetime import datetime, timezone
|
||
from pathlib import Path
|
||
from typing import Any
|
||
|
||
|
||
def resolve_mle_dir(output_dir: str | Path = "mle_runs") -> Path:
|
||
out = Path(output_dir)
|
||
if not out.is_absolute():
|
||
project_root = Path(__file__).resolve().parent.parent
|
||
out = project_root / out
|
||
return out
|
||
|
||
|
||
LAST_SETTINGS_FILENAME = "_last_settings.json"
|
||
|
||
|
||
def save_last_settings(
|
||
settings: dict[str, Any],
|
||
*,
|
||
output_dir: str | Path = "mle_runs",
|
||
) -> dict[str, Any]:
|
||
"""Persist UI preset so it survives app restarts."""
|
||
base = resolve_mle_dir(output_dir)
|
||
base.mkdir(parents=True, exist_ok=True)
|
||
payload = {
|
||
"savedAt": datetime.now(timezone.utc).isoformat(),
|
||
"settings": settings or {},
|
||
}
|
||
path = base / LAST_SETTINGS_FILENAME
|
||
path.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
|
||
return {"ok": True, "path": str(path), "savedAt": payload["savedAt"]}
|
||
|
||
|
||
def load_last_settings(*, output_dir: str | Path = "mle_runs") -> dict[str, Any]:
|
||
"""Load last UI preset from mle_runs/_last_settings.json."""
|
||
path = resolve_mle_dir(output_dir) / LAST_SETTINGS_FILENAME
|
||
if not path.is_file():
|
||
return {"settings": None, "savedAt": None, "path": str(path)}
|
||
try:
|
||
data = json.loads(path.read_text(encoding="utf-8"))
|
||
except (OSError, json.JSONDecodeError):
|
||
return {"settings": None, "savedAt": None, "path": str(path)}
|
||
settings = data.get("settings") if isinstance(data, dict) else None
|
||
if not isinstance(settings, dict):
|
||
settings = data if isinstance(data, dict) else None
|
||
saved_at = data.get("savedAt") if isinstance(data, dict) else None
|
||
return {"settings": settings, "savedAt": saved_at, "path": str(path)}
|
||
|
||
|
||
def _height_at(
|
||
x: float,
|
||
y: float,
|
||
*,
|
||
base_z: float,
|
||
amplitude: float,
|
||
frequency: float,
|
||
hills: list[tuple[float, float, float, float]],
|
||
valleys: list[tuple[float, float, float, float]],
|
||
bumps: list[tuple[float, float, float, float]],
|
||
) -> float:
|
||
z = base_z + amplitude * math.sin(frequency * x) * math.cos(frequency * 0.7 * y)
|
||
for hx, hy, hamp, hrad in hills:
|
||
d2 = (x - hx) ** 2 + (y - hy) ** 2
|
||
z += hamp * math.exp(-d2 / max(hrad * hrad, 1e-6))
|
||
for vx, vy, vamp, vrad in valleys:
|
||
d2 = (x - vx) ** 2 + (y - vy) ** 2
|
||
z -= vamp * math.exp(-d2 / max(vrad * vrad, 1e-6))
|
||
for bx, by, bamp, brad in bumps:
|
||
d2 = (x - bx) ** 2 + (y - by) ** 2
|
||
z += bamp * math.exp(-d2 / max(brad * brad, 1e-6))
|
||
return z
|
||
|
||
|
||
def build_seafloor_params(
|
||
seed: int = 42,
|
||
*,
|
||
size_x: float | None = None,
|
||
size_y: float | None = None,
|
||
relief_scale_pct: float = 20.0,
|
||
corridor: dict[str, Any] | None = None,
|
||
) -> dict[str, Any]:
|
||
"""Build seafloor heightfield params.
|
||
|
||
When ``corridor`` is provided (AUV survey swath), random unevenness is placed
|
||
only inside that yellow echosounder strip. Feature size scales with
|
||
``relief_scale_pct`` percent of the strip width.
|
||
"""
|
||
rng = random.Random(int(seed))
|
||
relief_scale_pct = max(1.0, min(100.0, float(relief_scale_pct)))
|
||
|
||
corr = corridor if isinstance(corridor, dict) else {}
|
||
corr = {k: v for k, v in corr.items() if v is not None}
|
||
auv_x = float(corr.get("auvX", 0.0))
|
||
auv_y = float(corr.get("auvY", 0.0))
|
||
heading_deg = float(corr.get("headingDeg", 0.0))
|
||
survey_length = max(1.0, float(corr.get("surveyLength", 40.0)))
|
||
auv_depth = max(0.3, float(corr.get("auvDepth", 2.5)))
|
||
swath_deg = max(5.0, min(170.0, float(corr.get("swathAngleDeg", 90.0))))
|
||
|
||
half_swath = auv_depth * math.tan(math.radians(swath_deg) * 0.5)
|
||
strip_width = max(4.0, 2.0 * half_swath)
|
||
half_w = strip_width * 0.5
|
||
heading = math.radians(heading_deg)
|
||
hx, hy = math.cos(heading), math.sin(heading)
|
||
nx, ny = -math.sin(heading), math.cos(heading)
|
||
|
||
# AABB of the survey strip (+ padding) → terrain extent around origin.
|
||
pad = strip_width * 0.6 + 8.0
|
||
xs: list[float] = []
|
||
ys: list[float] = []
|
||
for t in (0.0, survey_length):
|
||
for lat in (-half_w, half_w):
|
||
xs.append(auv_x + hx * t + nx * lat)
|
||
ys.append(auv_y + hy * t + ny * lat)
|
||
reach = max(
|
||
max(abs(v) for v in xs) + pad,
|
||
max(abs(v) for v in ys) + pad,
|
||
strip_width + 12.0,
|
||
survey_length * 0.35 + 12.0,
|
||
)
|
||
auto_size = max(24.0, reach * 2.0)
|
||
size_x = max(4.0, float(size_x) if size_x is not None else auto_size)
|
||
size_y = max(4.0, float(size_y) if size_y is not None else auto_size)
|
||
|
||
# Mild background undulation (not the main corridor features).
|
||
base_z = rng.uniform(-8.0, -3.0)
|
||
amplitude = rng.uniform(0.05, 0.18)
|
||
frequency = rng.uniform(0.08, 0.25)
|
||
|
||
# Characteristic feature size = pct of yellow strip width.
|
||
feature_scale = strip_width * (relief_scale_pct / 100.0)
|
||
feature_scale = max(0.15, feature_scale)
|
||
|
||
def point_in_strip(t: float, lat: float) -> tuple[float, float]:
|
||
return (
|
||
auv_x + hx * t + nx * lat,
|
||
auv_y + hy * t + ny * lat,
|
||
)
|
||
|
||
n_hills = rng.randint(2, 4)
|
||
n_valleys = rng.randint(1, 3)
|
||
# More bumps when features are smaller so the strip stays filled.
|
||
density = max(0.35, min(1.6, 20.0 / max(relief_scale_pct, 1.0)))
|
||
n_bumps = int(round(rng.uniform(10, 18) * density))
|
||
n_bumps = max(6, min(36, n_bumps))
|
||
|
||
hills = []
|
||
for _ in range(n_hills):
|
||
t = rng.uniform(0.0, survey_length)
|
||
lat = rng.uniform(-half_w * 0.85, half_w * 0.85)
|
||
x, y = point_in_strip(t, lat)
|
||
rad = feature_scale * rng.uniform(0.9, 1.8)
|
||
amp = feature_scale * rng.uniform(0.25, 0.7)
|
||
hills.append((x, y, amp, rad))
|
||
|
||
valleys = []
|
||
for _ in range(n_valleys):
|
||
t = rng.uniform(0.0, survey_length)
|
||
lat = rng.uniform(-half_w * 0.85, half_w * 0.85)
|
||
x, y = point_in_strip(t, lat)
|
||
rad = feature_scale * rng.uniform(0.8, 1.6)
|
||
amp = feature_scale * rng.uniform(0.18, 0.5)
|
||
valleys.append((x, y, amp, rad))
|
||
|
||
bumps = []
|
||
for _ in range(n_bumps):
|
||
t = rng.uniform(0.0, survey_length)
|
||
lat = rng.uniform(-half_w * 0.98, half_w * 0.98)
|
||
x, y = point_in_strip(t, lat)
|
||
rad = feature_scale * rng.uniform(0.35, 1.15)
|
||
amp = feature_scale * rng.uniform(0.1, 0.4)
|
||
bumps.append((x, y, amp, rad))
|
||
|
||
return {
|
||
"seed": int(seed),
|
||
"sizeX": size_x,
|
||
"sizeY": size_y,
|
||
"baseZ": base_z,
|
||
"amplitude": amplitude,
|
||
"frequency": frequency,
|
||
"hills": hills,
|
||
"valleys": valleys,
|
||
"bumps": bumps,
|
||
"reliefScalePct": relief_scale_pct,
|
||
"stripWidth": strip_width,
|
||
"corridor": {
|
||
"auvX": auv_x,
|
||
"auvY": auv_y,
|
||
"headingDeg": heading_deg,
|
||
"surveyLength": survey_length,
|
||
"auvDepth": auv_depth,
|
||
"swathAngleDeg": swath_deg,
|
||
"stripWidth": strip_width,
|
||
},
|
||
}
|
||
|
||
|
||
def sample_seafloor_grid(
|
||
params: dict[str, Any],
|
||
*,
|
||
res_x: int = 80,
|
||
res_y: int = 120,
|
||
) -> dict[str, Any]:
|
||
"""Build a triangulated seafloor mesh over [-sizeX/2, sizeX/2] × [-sizeY/2, sizeY/2]."""
|
||
res_x = max(4, int(res_x))
|
||
res_y = max(4, int(res_y))
|
||
size_x = float(params["sizeX"])
|
||
size_y = float(params["sizeY"])
|
||
half_x = size_x * 0.5
|
||
half_y = size_y * 0.5
|
||
|
||
vertices: list[list[float]] = []
|
||
heights: list[list[float]] = []
|
||
for j in range(res_y):
|
||
row: list[float] = []
|
||
y = -half_y if res_y == 1 else (-half_y + size_y * j / (res_y - 1))
|
||
for i in range(res_x):
|
||
x = -half_x if res_x == 1 else (-half_x + size_x * i / (res_x - 1))
|
||
z = _height_at(
|
||
x,
|
||
y,
|
||
base_z=float(params["baseZ"]),
|
||
amplitude=float(params["amplitude"]),
|
||
frequency=float(params["frequency"]),
|
||
hills=params["hills"],
|
||
valleys=params["valleys"],
|
||
bumps=params["bumps"],
|
||
)
|
||
vertices.append([x, y, z])
|
||
row.append(z)
|
||
heights.append(row)
|
||
|
||
faces: list[list[int]] = []
|
||
for j in range(res_y - 1):
|
||
for i in range(res_x - 1):
|
||
a = j * res_x + i
|
||
b = a + 1
|
||
c = a + res_x
|
||
d = c + 1
|
||
faces.append([a + 1, c + 1, b + 1]) # 1-based OBJ indices
|
||
faces.append([b + 1, c + 1, d + 1])
|
||
|
||
return {
|
||
"resX": res_x,
|
||
"resY": res_y,
|
||
"vertices": vertices,
|
||
"faces": faces,
|
||
"heights": heights,
|
||
"vertexCount": len(vertices),
|
||
"faceCount": len(faces),
|
||
}
|
||
|
||
|
||
def export_mesh_obj(
|
||
vertices: list[list[float]],
|
||
faces: list[list[int]],
|
||
*,
|
||
object_name: str = "seafloor",
|
||
) -> str:
|
||
safe = "".join(ch if ch.isalnum() or ch in "_-" else "_" for ch in (object_name or "seafloor")) or "seafloor"
|
||
lines = [
|
||
f"# DotsToSurface MLE seafloor ({len(vertices)} vertices, {len(faces)} faces)",
|
||
f"o {safe}",
|
||
]
|
||
for v in vertices:
|
||
lines.append(f"v {v[0]:.8f} {v[1]:.8f} {v[2]:.8f}")
|
||
for f in faces:
|
||
lines.append(f"f {f[0]} {f[1]} {f[2]}")
|
||
return "\n".join(lines) + "\n"
|
||
|
||
|
||
def write_mesh_obj_file(
|
||
path: Path,
|
||
vertices: list[list[float]],
|
||
faces: list[list[int]],
|
||
*,
|
||
object_name: str = "seafloor",
|
||
) -> Path:
|
||
path.parent.mkdir(parents=True, exist_ok=True)
|
||
path.write_text(
|
||
export_mesh_obj(vertices, faces, object_name=object_name),
|
||
encoding="utf-8",
|
||
)
|
||
return path
|
||
|
||
|
||
def save_survey_surface(
|
||
*,
|
||
output_dir: str | Path,
|
||
vertices: list[list[float]],
|
||
faces: list[list[int]],
|
||
filename: str = "seafloor.obj",
|
||
) -> dict[str, Any]:
|
||
"""Overwrite the survey OBJ inside an existing MLE run folder."""
|
||
run_dir = Path(output_dir)
|
||
if not run_dir.is_absolute():
|
||
run_dir = resolve_mle_dir(run_dir)
|
||
if not run_dir.is_dir():
|
||
raise FileNotFoundError(f"MLE run folder not found: {run_dir}")
|
||
if not vertices:
|
||
raise ValueError("vertices must not be empty")
|
||
obj_path = write_mesh_obj_file(
|
||
run_dir / filename,
|
||
vertices,
|
||
faces,
|
||
object_name="seafloor",
|
||
)
|
||
return {
|
||
"outputDir": str(run_dir),
|
||
"seafloorObj": str(obj_path),
|
||
"vertexCount": len(vertices),
|
||
"faceCount": len(faces),
|
||
}
|
||
|
||
|
||
def prepare_mle_scene(
|
||
*,
|
||
seed: int = 42,
|
||
size_x: float | None = None,
|
||
size_y: float | None = None,
|
||
res_x: int = 80,
|
||
res_y: int = 120,
|
||
output_dir: str | Path = "mle_runs",
|
||
settings: dict[str, Any] | None = None,
|
||
relief_scale_pct: float = 20.0,
|
||
corridor: dict[str, Any] | None = None,
|
||
) -> dict[str, Any]:
|
||
"""Generate seafloor for simulation; create run folder with empty survey OBJ."""
|
||
base = resolve_mle_dir(output_dir)
|
||
base.mkdir(parents=True, exist_ok=True)
|
||
stamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
|
||
run_dir = base / stamp
|
||
n = 2
|
||
while run_dir.exists():
|
||
run_dir = base / f"{stamp}_{n}"
|
||
n += 1
|
||
run_dir.mkdir(parents=True, exist_ok=False)
|
||
|
||
# Prefer explicit corridor fields; fill gaps from settings snapshot.
|
||
corr: dict[str, Any] = {}
|
||
if isinstance(corridor, dict):
|
||
corr.update({k: v for k, v in corridor.items() if v is not None})
|
||
if isinstance(settings, dict):
|
||
auv = settings.get("auv") if isinstance(settings.get("auv"), dict) else {}
|
||
beams = settings.get("beams") if isinstance(settings.get("beams"), dict) else {}
|
||
motion = settings.get("motion") if isinstance(settings.get("motion"), dict) else {}
|
||
defaults_from_settings = {
|
||
"auvX": auv.get("x", settings.get("auvX")),
|
||
"auvY": auv.get("y", settings.get("auvY")),
|
||
"headingDeg": auv.get("headingDeg", settings.get("auvHeadingDeg")),
|
||
"surveyLength": motion.get("surveyLength", settings.get("surveyLength")),
|
||
"auvDepth": auv.get("depth", settings.get("auvDepth")),
|
||
"swathAngleDeg": beams.get("swathAngleDeg", settings.get("swathAngleDeg")),
|
||
}
|
||
for key, value in defaults_from_settings.items():
|
||
if corr.get(key) is None and value is not None:
|
||
corr[key] = value
|
||
|
||
params = build_seafloor_params(
|
||
seed,
|
||
size_x=size_x,
|
||
size_y=size_y,
|
||
relief_scale_pct=relief_scale_pct,
|
||
corridor=corr,
|
||
)
|
||
mesh = sample_seafloor_grid(params, res_x=res_x, res_y=res_y)
|
||
# Survey OBJ starts empty and is filled from multibeam hits during motion.
|
||
obj_path = run_dir / "seafloor.obj"
|
||
obj_path.write_text(
|
||
"# DotsToSurface MLE survey surface (populated during AUV motion)\no seafloor\n",
|
||
encoding="utf-8",
|
||
)
|
||
# Keep full terrain for reference / debugging (not the survey panel content).
|
||
write_mesh_obj_file(
|
||
run_dir / "terrain_full.obj",
|
||
mesh["vertices"],
|
||
mesh["faces"],
|
||
object_name="terrain_full",
|
||
)
|
||
|
||
manifest = {
|
||
"generatedAt": datetime.now(timezone.utc).isoformat(),
|
||
"runName": run_dir.name,
|
||
"outputDir": str(run_dir),
|
||
"seafloorObj": str(obj_path),
|
||
"params": params,
|
||
"mesh": {
|
||
"resX": mesh["resX"],
|
||
"resY": mesh["resY"],
|
||
"vertexCount": mesh["vertexCount"],
|
||
"faceCount": mesh["faceCount"],
|
||
},
|
||
"settings": settings or {},
|
||
}
|
||
(run_dir / "mle_run.json").write_text(
|
||
json.dumps(manifest, ensure_ascii=False, indent=2),
|
||
encoding="utf-8",
|
||
)
|
||
|
||
return {
|
||
"runName": run_dir.name,
|
||
"outputDir": str(run_dir),
|
||
"seafloorObj": str(obj_path),
|
||
"params": params,
|
||
"mesh": {
|
||
"resX": mesh["resX"],
|
||
"resY": mesh["resY"],
|
||
"vertices": mesh["vertices"],
|
||
"faces": mesh["faces"],
|
||
"heights": mesh["heights"],
|
||
"vertexCount": mesh["vertexCount"],
|
||
"faceCount": mesh["faceCount"],
|
||
},
|
||
}
|