353 lines
11 KiB
Python
353 lines
11 KiB
Python
"""Room world: the environment owned by the testbed bridge.
|
|
|
|
Single source of truth for all world state. Nothing here is reachable by an
|
|
agent except through the sensor tools in ``testbed.bridge``.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
from dataclasses import dataclass, field
|
|
|
|
ROOM_SIZE = 16.0
|
|
ROOM_HEIGHT = 3.0
|
|
|
|
|
|
@dataclass
|
|
class Box:
|
|
"""Axis-aligned box obstacle."""
|
|
|
|
id: str
|
|
cx: float
|
|
cz: float
|
|
half_w: float
|
|
half_d: float
|
|
height: float
|
|
color: tuple[int, int, int]
|
|
|
|
def x_min(self) -> float:
|
|
return self.cx - self.half_w
|
|
|
|
def x_max(self) -> float:
|
|
return self.cx + self.half_w
|
|
|
|
def z_min(self) -> float:
|
|
return self.cz - self.half_d
|
|
|
|
def z_max(self) -> float:
|
|
return self.cz + self.half_d
|
|
|
|
|
|
@dataclass
|
|
class Beacon:
|
|
"""The single interactable object: a glowing pillar."""
|
|
|
|
id: str = "beacon"
|
|
x: float = 12.5
|
|
z: float = 12.5
|
|
height: float = 1.6
|
|
radius: float = 0.35
|
|
active: bool = False
|
|
activated_tick: int = -1
|
|
reach: float = 1.6
|
|
|
|
|
|
@dataclass
|
|
class Capsule:
|
|
"""The agent's body: position, heading (yaw), camera pitch, velocity."""
|
|
|
|
x: float = 1.5
|
|
z: float = 1.5
|
|
yaw_deg: float = 45.0
|
|
pitch_deg: float = 0.0
|
|
radius: float = 0.35
|
|
eye_height: float = 0.55
|
|
health: float = 100.0
|
|
speed: float = 0.0
|
|
|
|
def forward(self) -> tuple[float, float]:
|
|
"""Unit vector in XZ plane along the current heading."""
|
|
rad = math.radians(self.yaw_deg)
|
|
return math.sin(rad), math.cos(rad)
|
|
|
|
|
|
@dataclass
|
|
class Hit:
|
|
"""A collision: what was hit, where, and with what force."""
|
|
|
|
other: str
|
|
normal_x: float
|
|
normal_z: float
|
|
impulse: float
|
|
|
|
|
|
class Room:
|
|
"""A 16x16 room: floor, walls, a few boxes, one glowing beacon.
|
|
|
|
Tick semantics: ``tick_mode`` is ``event`` — the world advances only when
|
|
a tool is called. Every tool call bumps ``tick`` by one, so observations
|
|
and events share a monotonic clock (protocol §8).
|
|
"""
|
|
|
|
def __init__(self) -> None:
|
|
self.tick = 0
|
|
self.capsule = Capsule()
|
|
self.beacon = Beacon()
|
|
self.boxes: list[Box] = [
|
|
Box(
|
|
id="crate_red",
|
|
cx=6.0,
|
|
cz=6.0,
|
|
half_w=1.0,
|
|
half_d=1.0,
|
|
height=1.4,
|
|
color=(178, 64, 54),
|
|
),
|
|
Box(
|
|
id="crate_blue",
|
|
cx=11.0,
|
|
cz=3.5,
|
|
half_w=0.9,
|
|
half_d=0.9,
|
|
height=1.6,
|
|
color=(64, 96, 178),
|
|
),
|
|
Box(
|
|
id="crate_olive",
|
|
cx=4.0,
|
|
cz=11.0,
|
|
half_w=0.7,
|
|
half_d=0.7,
|
|
height=1.2,
|
|
color=(128, 128, 60),
|
|
),
|
|
]
|
|
# Pending audio events, drained by the hear sensor.
|
|
self._audio: list[dict] = []
|
|
|
|
# ---------- ticks ----------
|
|
|
|
def advance_tick(self) -> int:
|
|
"""Bump the world clock; returns the new tick."""
|
|
self.tick += 1
|
|
return self.tick
|
|
|
|
# ---------- audio ----------
|
|
|
|
def queue_audio(self, kind: str, direction_deg: float, intensity: float) -> None:
|
|
self._audio.append(
|
|
{
|
|
"kind": kind,
|
|
"direction_deg": round(direction_deg % 360.0, 1),
|
|
"intensity": round(intensity, 3),
|
|
}
|
|
)
|
|
|
|
def drain_audio(self) -> list[dict]:
|
|
sounds = self._audio
|
|
self._audio = []
|
|
return sounds
|
|
|
|
def hear_now(self) -> list[dict]:
|
|
"""The hear sensor: queued events plus the beacon hum if in range."""
|
|
sounds = self.drain_audio()
|
|
dx = self.beacon.x - self.capsule.x
|
|
dz = self.beacon.z - self.capsule.z
|
|
dist = math.hypot(dx, dz)
|
|
if dist <= 6.0:
|
|
bearing = math.degrees(math.atan2(dx, dz)) - self.capsule.yaw_deg
|
|
intensity = max(0.0, 1.0 - dist / 6.0)
|
|
if self.beacon.active:
|
|
intensity = min(1.0, intensity + 0.25)
|
|
sounds.append(
|
|
{
|
|
"kind": "beacon_hum",
|
|
"direction_deg": round(bearing % 360.0, 1),
|
|
"intensity": round(intensity, 3),
|
|
}
|
|
)
|
|
return sounds
|
|
|
|
# ---------- movement ----------
|
|
|
|
def move_forward(self, distance: float) -> tuple[float, Hit | None]:
|
|
"""Push the capsule forward along its heading, resolving collisions.
|
|
|
|
Moves in 0.1 m substeps. Returns (distance actually moved, hit or None).
|
|
"""
|
|
moved = 0.0
|
|
hit: Hit | None = None
|
|
remaining = max(0.0, distance)
|
|
if remaining <= 0.0:
|
|
return 0.0, None
|
|
fx, fz = self.capsule.forward()
|
|
while remaining > 0.0 and hit is None:
|
|
step = min(0.1, remaining)
|
|
bx, bz = self.capsule.x, self.capsule.z
|
|
ok, h = self._try_displace(fx * step, fz * step)
|
|
moved += math.hypot(self.capsule.x - bx, self.capsule.z - bz)
|
|
if not ok and h is not None:
|
|
h.impulse = max(0.0, distance - moved)
|
|
hit = h
|
|
break
|
|
remaining -= step
|
|
self.capsule.speed = moved / 0.1 if moved > 0 else 0.0
|
|
return moved, hit
|
|
|
|
def _try_displace(self, dx: float, dz: float) -> tuple[bool, Hit | None]:
|
|
"""Move by (dx, dz) with circle-vs-AABB resolution. Returns (moved, hit).
|
|
|
|
The capsule always ends at the resolved position: on a blocked step it
|
|
rests at the contact point (wall or box face).
|
|
"""
|
|
cap = self.capsule
|
|
r = cap.radius
|
|
nx = cap.x + dx
|
|
nz = cap.z + dz
|
|
|
|
blocked: Hit | None = None
|
|
if nx < r:
|
|
nx, blocked = (
|
|
r,
|
|
Hit(other="wall_west", normal_x=1.0, normal_z=0.0, impulse=0.0),
|
|
)
|
|
elif nx > ROOM_SIZE - r:
|
|
nx, blocked = (
|
|
ROOM_SIZE - r,
|
|
Hit(other="wall_east", normal_x=-1.0, normal_z=0.0, impulse=0.0),
|
|
)
|
|
elif nz < r:
|
|
nz, blocked = (
|
|
r,
|
|
Hit(other="wall_north", normal_x=0.0, normal_z=1.0, impulse=0.0),
|
|
)
|
|
elif nz > ROOM_SIZE - r:
|
|
nz, blocked = (
|
|
ROOM_SIZE - r,
|
|
Hit(other="wall_south", normal_x=0.0, normal_z=-1.0, impulse=0.0),
|
|
)
|
|
|
|
for _ in range(8):
|
|
contact = self._box_contact(nx, nz, r)
|
|
if contact is None:
|
|
break
|
|
nx, nz, normal_x, normal_z, other = contact
|
|
blocked = Hit(
|
|
other=other, normal_x=normal_x, normal_z=normal_z, impulse=0.0
|
|
)
|
|
|
|
cap.x = nx
|
|
cap.z = nz
|
|
return blocked is None, blocked
|
|
|
|
def _box_contact(
|
|
self, cx: float, cz: float, r: float
|
|
) -> tuple[float, float, float, float, str] | None:
|
|
"""If the circle at (cx, cz) overlaps a box, push it out and report contact."""
|
|
for box in self.boxes:
|
|
min_x, max_x = box.x_min(), box.x_max()
|
|
min_z, max_z = box.z_min(), box.z_max()
|
|
near_x = min(max(cx, min_x), max_x)
|
|
near_z = min(max(cz, min_z), max_z)
|
|
dx = cx - near_x
|
|
dz = cz - near_z
|
|
d2 = dx * dx + dz * dz
|
|
if d2 >= r * r:
|
|
continue
|
|
if d2 > 1e-12:
|
|
d = math.sqrt(d2)
|
|
push = (r - d) / d
|
|
return (
|
|
cx + dx * push,
|
|
cz + dz * push,
|
|
dx / d,
|
|
dz / d,
|
|
box.id,
|
|
)
|
|
# Center inside the box: push along the axis of least penetration.
|
|
ox = min(cx - min_x + r, max_x - cx + r)
|
|
oz = min(cz - min_z + r, max_z - cz + r)
|
|
if ox < oz:
|
|
nx = cx + ox if cx < box.cx else cx - ox
|
|
return (nx, cz, 1.0 if cx < box.cx else -1.0, 0.0, box.id)
|
|
nz = cz + oz if cz < box.cz else cz - oz
|
|
return (cx, nz, 0.0, 1.0 if cz < box.cz else -1.0, box.id)
|
|
return None
|
|
|
|
def turn(self, yaw_deg: float = 0.0, pitch_deg: float = 0.0) -> None:
|
|
cap = self.capsule
|
|
cap.yaw_deg = (cap.yaw_deg + yaw_deg) % 360.0
|
|
cap.pitch_deg = min(85.0, max(-85.0, cap.pitch_deg + pitch_deg))
|
|
|
|
def face(self, yaw_deg: float, pitch_deg: float) -> None:
|
|
self.capsule.yaw_deg = yaw_deg % 360.0
|
|
self.capsule.pitch_deg = min(85.0, max(-85.0, pitch_deg))
|
|
|
|
def look_at_beacon(self) -> tuple[float, float]:
|
|
"""Orient camera toward the beacon. Returns (yaw_deg, pitch_deg)."""
|
|
cap = self.capsule
|
|
dx = self.beacon.x - cap.x
|
|
dz = self.beacon.z - cap.z
|
|
dist = math.hypot(dx, dz) or 1.0
|
|
yaw = math.degrees(math.atan2(dx, dz))
|
|
pitch = math.degrees(math.atan2(self.beacon.height - cap.eye_height, dist))
|
|
self.face(yaw, pitch)
|
|
return yaw, pitch
|
|
|
|
def distance_to_beacon(self) -> float:
|
|
return math.hypot(
|
|
self.beacon.x - self.capsule.x, self.beacon.z - self.capsule.z
|
|
)
|
|
|
|
def interact_beacon(self) -> tuple[bool, str]:
|
|
"""Try to activate the beacon. Returns (success, message)."""
|
|
dist = self.distance_to_beacon()
|
|
if dist > self.beacon.reach:
|
|
return (
|
|
False,
|
|
f"too far: {dist:.1f} m from the beacon (need <= {self.beacon.reach} m)",
|
|
)
|
|
if self.beacon.active:
|
|
return True, "the beacon is already active and glowing warm yellow"
|
|
self.beacon.active = True
|
|
self.beacon.activated_tick = self.tick
|
|
return True, "the beacon lights up: a warm yellow glow floods the room"
|
|
|
|
# ---------- introspection helpers ----------
|
|
|
|
def surface_distances(self, x: float, z: float) -> dict[str, float]:
|
|
"""Distances to the nearest wall on each side (used by demo steering)."""
|
|
return {
|
|
"west": x,
|
|
"east": ROOM_SIZE - x,
|
|
"north": z,
|
|
"south": ROOM_SIZE - z,
|
|
}
|
|
|
|
def describe(self) -> dict:
|
|
return {
|
|
"room": {
|
|
"name": "testbed_room_01",
|
|
"width": ROOM_SIZE,
|
|
"depth": ROOM_SIZE,
|
|
"height": ROOM_HEIGHT,
|
|
},
|
|
"obstacles": [
|
|
{
|
|
"id": b.id,
|
|
"x": round(b.cx, 2),
|
|
"z": round(b.cz, 2),
|
|
"width": round(b.half_w * 2, 2),
|
|
"depth": round(b.half_d * 2, 2),
|
|
"height": round(b.height, 2),
|
|
}
|
|
for b in self.boxes
|
|
],
|
|
"beacon": {
|
|
"id": self.beacon.id,
|
|
"x": round(self.beacon.x, 2),
|
|
"z": round(self.beacon.z, 2),
|
|
"height": round(self.beacon.height, 2),
|
|
},
|
|
}
|