6e83bbd73f
Serveur FastAPI reçoit le flux JSONL (sim ou ROV réel) sur /ws/ingest, SLAM incrémental, rediffuse carte+poses sur /ws/live, GUI live et export PLY. Déployé Docker sur caddy-net, exposé /moulin-live/. Client PC stream_client.py. Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
87 lines
2.1 KiB
Python
87 lines
2.1 KiB
Python
"""
|
|
room.py — Géométrie chambre de moulin + raycast 2D. Copie standalone.
|
|
"""
|
|
|
|
import numpy as np
|
|
from typing import Optional, List, Tuple
|
|
|
|
ROOM_POLYGON = np.array([
|
|
[0.0, 0.0],
|
|
[8.0, 0.0],
|
|
[8.0, 5.0],
|
|
[3.0, 5.0],
|
|
[3.0, 8.0],
|
|
[0.0, 8.0],
|
|
], dtype=float)
|
|
|
|
NICHE_WALLS = np.array([
|
|
[[0.0, 4.0], [3.0, 4.0]],
|
|
[[0.0, 2.0], [3.0, 2.0]],
|
|
], dtype=float)
|
|
|
|
|
|
def _polygon_to_segments(poly: np.ndarray) -> List[Tuple[np.ndarray, np.ndarray]]:
|
|
segs = []
|
|
n = len(poly)
|
|
for i in range(n):
|
|
segs.append((poly[i], poly[(i + 1) % n]))
|
|
return segs
|
|
|
|
|
|
def get_all_segments() -> List[Tuple[np.ndarray, np.ndarray]]:
|
|
segs = _polygon_to_segments(ROOM_POLYGON)
|
|
for wall in NICHE_WALLS:
|
|
segs.append((wall[0], wall[1]))
|
|
return segs
|
|
|
|
|
|
_ALL_SEGMENTS = get_all_segments()
|
|
|
|
|
|
def _ray_segment_intersect(
|
|
origin: np.ndarray,
|
|
direction: np.ndarray,
|
|
p0: np.ndarray,
|
|
p1: np.ndarray,
|
|
) -> Optional[float]:
|
|
dx = direction[0]
|
|
dy = direction[1]
|
|
ex = p1[0] - p0[0]
|
|
ey = p1[1] - p0[1]
|
|
denom = -dx * ey + ex * dy
|
|
if abs(denom) < 1e-12:
|
|
return None
|
|
bx = p0[0] - origin[0]
|
|
by = p0[1] - origin[1]
|
|
t = (-bx * ey + ex * by) / denom
|
|
u = (dx * by - bx * dy) / denom
|
|
if t >= 0.0 and 0.0 <= u <= 1.0:
|
|
return t
|
|
return None
|
|
|
|
|
|
def range_to_walls(
|
|
x: float,
|
|
y: float,
|
|
bearing_world_deg: float,
|
|
max_range: float = 30.0,
|
|
) -> Optional[float]:
|
|
bearing_rad = np.deg2rad(bearing_world_deg)
|
|
direction = np.array([np.sin(bearing_rad), np.cos(bearing_rad)], dtype=float)
|
|
origin = np.array([x, y], dtype=float)
|
|
min_t = None
|
|
for p0, p1 in _ALL_SEGMENTS:
|
|
t = _ray_segment_intersect(origin, direction, p0, p1)
|
|
if t is not None and t > 1e-6:
|
|
if min_t is None or t < min_t:
|
|
min_t = t
|
|
if min_t is not None and min_t <= max_range:
|
|
return min_t
|
|
return None
|
|
|
|
|
|
def get_room_bounds() -> Tuple[float, float, float, float]:
|
|
pts = ROOM_POLYGON
|
|
return float(pts[:, 0].min()), float(pts[:, 0].max()), \
|
|
float(pts[:, 1].min()), float(pts[:, 1].max())
|