""" room.py — Géométrie de la chambre de moulin + raycast 2D. La chambre est un polygone (pièce en L avec une niche). Toutes les coordonnées sont en mètres, repère ENU local (x=Est, y=Nord). """ import numpy as np from typing import Optional, List, Tuple # --------------------------------------------------------------------------- # Géométrie : chambre en L avec une niche dans le mur Est # --------------------------------------------------------------------------- # # y # ^ # | # 8 +---+ # | | <- couloir Nord # 5 + +-------+ # | | <- grande salle # | niche | # 2 + +---+ | # | | | # 0 +-------+---+---> x # 0 3 5 8 # # La niche est dans la grande salle, côté Ouest (renfoncement à x=3..5, y=2..5) # La chambre principale va de x=0..8, y=0..5 # Le couloir va de x=0..3, y=5..8 ROOM_POLYGON = np.array([ [0.0, 0.0], # 0 — coin Sud-Ouest [8.0, 0.0], # 1 — coin Sud-Est [8.0, 5.0], # 2 — coin Est milieu [3.0, 5.0], # 3 — jonction couloir/salle [3.0, 8.0], # 4 — coin Nord-Est couloir [0.0, 8.0], # 5 — coin Nord-Ouest ], dtype=float) # Niche : renfoncement dans le mur Ouest de la grande salle # C'est un rectangle "creusé" dans x=0..3, y=2..4 # On l'ajoute comme polygone séparé (trou dans la géométrie) # Simplification : on le modélise comme murs supplémentaires NICHE_WALLS = np.array([ # mur du fond de la niche (à x=3, de y=2 à y=4) — déjà dans le polygone principal # mur Nord de la niche (y=4, de x=0 à x=3) [[0.0, 4.0], [3.0, 4.0]], # mur Sud de la niche (y=2, de x=0 à x=3) [[0.0, 2.0], [3.0, 2.0]], ], dtype=float) def _polygon_to_segments(poly: np.ndarray) -> List[Tuple[np.ndarray, np.ndarray]]: """Convertit un polygone fermé en liste de segments (p0, p1).""" 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]]: """Retourne tous les segments de murs de la chambre.""" segs = _polygon_to_segments(ROOM_POLYGON) for wall in NICHE_WALLS: segs.append((wall[0], wall[1])) return segs _ALL_SEGMENTS = get_all_segments() # --------------------------------------------------------------------------- # Raycast : intersection rayon / segments # --------------------------------------------------------------------------- def _ray_segment_intersect( origin: np.ndarray, direction: np.ndarray, p0: np.ndarray, p1: np.ndarray, ) -> Optional[float]: """ Intersection d'un rayon (origin + t*direction) avec un segment [p0, p1]. Retourne t > 0 si intersection, None sinon. Système : origin + t*D = p0 + u*(p1-p0) → M * [t, u]^T = p0 - origin, M = [D | -(p1-p0)] Résolu par règle de Cramer 2×2. """ dx = direction[0] dy = direction[1] # vecteur segment ex = p1[0] - p0[0] ey = p1[1] - p0[1] # det(M) = det([[dx, -ex], [dy, -ey]]) = dx*(-ey) - (-ex)*dy = -dx*ey + ex*dy denom = -dx * ey + ex * dy if abs(denom) < 1e-12: # Rayon parallèle au segment return None # Second membre : p0 - origin bx = p0[0] - origin[0] by = p0[1] - origin[1] # Cramer : t = det([[bx, -ex], [by, -ey]]) / denom t = (-bx * ey + ex * by) / denom # Cramer : u = det([[dx, bx], [dy, 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]: """ Calcule la distance depuis (x, y) jusqu'au premier mur dans la direction bearing_world_deg (convention : 0°=Nord/+Y, 90°=Est/+X, CW). Retourne la distance en mètres, ou None si aucune intersection dans max_range. """ # Conversion cap → vecteur direction ENU (x=Est, y=Nord) bearing_rad = np.deg2rad(bearing_world_deg) # cap 0°=Nord → direction (sin θ, cos θ) dans ENU 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 point_in_room(x: float, y: float) -> bool: """ Test d'appartenance à l'intérieur de la chambre. Utilise l'algorithme du ray casting (rayon vers +x infini). Tient compte des deux zones : grande salle + couloir, moins la niche. """ # Grande salle : x in [0,8], y in [0,5] in_main = (0 <= x <= 8) and (0 <= y <= 5) # Couloir : x in [0,3], y in [5,8] in_corridor = (0 <= x <= 3) and (5 <= y <= 8) # Niche (vide) : x in [0,3], y in [2,4] — cet espace est ouvert (pas de mur intérieur) # Simplifié : la niche est incluse dans main salle return in_main or in_corridor def get_room_bounds() -> Tuple[float, float, float, float]: """Retourne (xmin, xmax, ymin, ymax) de la chambre.""" pts = ROOM_POLYGON return float(pts[:, 0].min()), float(pts[:, 0].max()), \ float(pts[:, 1].min()), float(pts[:, 1].max()) # --------------------------------------------------------------------------- # Test rapide # --------------------------------------------------------------------------- if __name__ == "__main__": print("=== Test raycast chambre ===") # Depuis le centre de la grande salle cx, cy = 4.0, 2.5 for bearing in [0, 90, 180, 270]: d = range_to_walls(cx, cy, bearing) ds = f"{d:.3f}" if d is not None else "None" print(f" cap {bearing:3d}° → {ds} m") # Attendu approximatif : # 0° (Nord) → 2.5 m (mur y=5) # 90° (Est) → 4.0 m (mur x=8) # 180° (Sud) → 2.5 m (mur y=0) # 270° (Ouest) → 1.0 m (mur niche y=2→4? Non, ici y=2.5 donc mur x=0 → 4m) # Attention : niche = renfoncement dans mur Ouest basse, pas à y=2.5 # À y=2.5 (entre 2 et 4) le mur Ouest est à x=0 → 4.0 m print("\nPoint in room:") for pt, expected in [((4, 2.5), True), ((1, 6), True), ((5, 6), False), ((-1, 1), False)]: print(f" {pt} → {point_in_room(*pt)} (attendu {expected})")