testbed: room world engine + headless raycaster renderer (bridge skeleton)
This commit is contained in:
opencode
@@ -0,0 +1,5 @@
|
||||
.venv/
|
||||
__pycache__/
|
||||
*.pyc
|
||||
*.egg-info/
|
||||
.pytest_cache/
|
||||
@@ -0,0 +1,421 @@
|
||||
"""Headless first-person renderer for the testbed room.
|
||||
|
||||
A small raycaster (Pillow-only, no GPU, no display) that produces an honest
|
||||
frame: it derives every pixel from the actual world state in ``Room``. Used by
|
||||
the ``vision`` and ``depth`` sensor tools.
|
||||
|
||||
Camera model: yaw = heading around Y, pitch = camera tilt. 90° horizontal FOV.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
from PIL import Image
|
||||
|
||||
from testbed.room.world import ROOM_HEIGHT, ROOM_SIZE, Box, Room
|
||||
|
||||
# Palette
|
||||
FOG = (14, 14, 20)
|
||||
FLOOR_A = (56, 56, 62)
|
||||
FLOOR_B = (64, 64, 70)
|
||||
FLOOR_LINE = (42, 42, 48)
|
||||
CEIL = (40, 40, 46)
|
||||
SKY = (12, 16, 30)
|
||||
WALL = (96, 96, 106)
|
||||
WALL_TOP_BAND = (70, 70, 78)
|
||||
|
||||
BEACON_OFF = ((0.85, 1.0, 1.0), (0.42, 0.82, 0.92), (0.10, 0.45, 0.62))
|
||||
BEACON_ON = ((1.0, 0.98, 0.78), (1.0, 0.75, 0.35), (0.78, 0.43, 0.0))
|
||||
|
||||
MAX_VIEW = 20.0
|
||||
_EPS = 1e-9
|
||||
|
||||
|
||||
class Raycaster:
|
||||
"""Renders the room from the capsule's camera. Stateless per frame."""
|
||||
|
||||
def __init__(self, width: int = 160, height: int = 120):
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.tan_fx = 1.0 # 90° horizontal FOV
|
||||
self.tan_fy = self.tan_fx * height / width
|
||||
|
||||
def render(
|
||||
self, room: Room, max_depth: float = 10.0
|
||||
) -> tuple[Image.Image, list[list[float]]]:
|
||||
"""Render the current view. Returns (RGB image, aligned depth grid)."""
|
||||
w, h = self.width, self.height
|
||||
cap = room.capsule
|
||||
px = cap.x
|
||||
pz = cap.z
|
||||
eye = cap.eye_height
|
||||
yaw = math.radians(cap.yaw_deg)
|
||||
pitch = math.radians(cap.pitch_deg)
|
||||
tan_pitch = math.tan(pitch)
|
||||
|
||||
fx = math.sin(yaw)
|
||||
fz = math.cos(yaw)
|
||||
rx = math.cos(yaw)
|
||||
rz = -math.sin(yaw)
|
||||
|
||||
pixels = bytearray(w * h * 3)
|
||||
depth = [[MAX_VIEW] * w for _ in range(h)]
|
||||
|
||||
slabs = self._slabs(room)
|
||||
|
||||
for c in range(w):
|
||||
u = (2.0 * (c + 0.5) / w - 1.0) * self.tan_fx
|
||||
dx = fx + rx * u
|
||||
dz = fz + rz * u
|
||||
inv = 1.0 / math.hypot(dx, dz)
|
||||
dx *= inv
|
||||
dz *= inv
|
||||
|
||||
dist, surf = self._cast(px, pz, dx, dz, slabs)
|
||||
|
||||
if surf is None:
|
||||
t_wall = math.inf
|
||||
s_coord = 0.0
|
||||
else:
|
||||
t_wall = dist
|
||||
s_coord = surf["s"]
|
||||
|
||||
row_top = self._row_of(ROOM_HEIGHT, t_wall, eye, tan_pitch)
|
||||
row_bot = self._row_of(0.0, t_wall, eye, tan_pitch)
|
||||
r_top = max(0, int(math.ceil(min(row_top, row_bot))))
|
||||
r_bot = min(h, int(math.floor(max(row_top, row_bot))))
|
||||
|
||||
base = c * 3
|
||||
for r in range(h):
|
||||
v = self.tan_fy * (1.0 - 2.0 * (r + 0.5) / h)
|
||||
down = v + tan_pitch
|
||||
o = (r * w + c) * 3
|
||||
if r_top <= r < r_bot and t_wall < MAX_VIEW:
|
||||
col = self._shade_wall(surf, dist, s_coord)
|
||||
if r == r_top:
|
||||
col = _lerp(col, WALL_TOP_BAND, 0.55)
|
||||
t = dist
|
||||
elif down < -_EPS:
|
||||
t = eye / -down
|
||||
col = self._shade_floor(px + dx * t, pz + dz * t, t)
|
||||
elif down > _EPS:
|
||||
t = (ROOM_HEIGHT - eye) / down
|
||||
col = _fog(CEIL, t)
|
||||
else:
|
||||
col = SKY
|
||||
t = math.inf
|
||||
if t < MAX_VIEW:
|
||||
depth[r][c] = t
|
||||
pixels[o] = col[0]
|
||||
pixels[o + 1] = col[1]
|
||||
pixels[o + 2] = col[2]
|
||||
|
||||
self._draw_beacon(room, pixels, depth, fx, fz, rx, rz, eye, tan_pitch)
|
||||
|
||||
img = Image.frombuffer("RGB", (w, h), bytes(pixels), "raw", "RGB", 0, 1)
|
||||
grid = self._depth_grid(depth, max_depth)
|
||||
return img, grid
|
||||
|
||||
# ---------- helpers ----------
|
||||
|
||||
def _slabs(self, room: Room) -> list[dict]:
|
||||
"""Room walls as thin slabs + the obstacle boxes, all axis-aligned."""
|
||||
e = 0.01
|
||||
slabs: list[dict] = []
|
||||
slabs.append(
|
||||
{
|
||||
"x0": -e,
|
||||
"x1": ROOM_SIZE + e,
|
||||
"z0": -e,
|
||||
"z1": e,
|
||||
"kind": "wall",
|
||||
"id": "wall_north",
|
||||
"top": ROOM_HEIGHT,
|
||||
"color": WALL,
|
||||
"face": "z0",
|
||||
}
|
||||
)
|
||||
slabs.append(
|
||||
{
|
||||
"x0": -e,
|
||||
"x1": ROOM_SIZE + e,
|
||||
"z0": ROOM_SIZE - e,
|
||||
"z1": ROOM_SIZE + e,
|
||||
"kind": "wall",
|
||||
"id": "wall_south",
|
||||
"top": ROOM_HEIGHT,
|
||||
"color": WALL,
|
||||
"face": "z1",
|
||||
}
|
||||
)
|
||||
slabs.append(
|
||||
{
|
||||
"x0": -e,
|
||||
"x1": e,
|
||||
"z0": -e,
|
||||
"z1": ROOM_SIZE + e,
|
||||
"kind": "wall",
|
||||
"id": "wall_west",
|
||||
"top": ROOM_HEIGHT,
|
||||
"color": WALL,
|
||||
"face": "x0",
|
||||
}
|
||||
)
|
||||
slabs.append(
|
||||
{
|
||||
"x0": ROOM_SIZE - e,
|
||||
"x1": ROOM_SIZE + e,
|
||||
"z0": -e,
|
||||
"z1": ROOM_SIZE + e,
|
||||
"kind": "wall",
|
||||
"id": "wall_east",
|
||||
"top": ROOM_HEIGHT,
|
||||
"color": WALL,
|
||||
"face": "x1",
|
||||
}
|
||||
)
|
||||
for box in room.boxes:
|
||||
slabs.append(
|
||||
{
|
||||
"x0": box.x_min(),
|
||||
"x1": box.x_max(),
|
||||
"z0": box.z_min(),
|
||||
"z1": box.z_max(),
|
||||
"kind": "box",
|
||||
"id": box.id,
|
||||
"top": box.height,
|
||||
"color": box.color,
|
||||
"face": "box",
|
||||
"box": box,
|
||||
}
|
||||
)
|
||||
return slabs
|
||||
|
||||
def _cast(self, ox: float, oz: float, dx: float, dz: float, slabs: list[dict]):
|
||||
"""Nearest 2D hit of the ray against all slabs. Returns (dist, surface)."""
|
||||
best = math.inf
|
||||
best_surf = None
|
||||
for s in slabs:
|
||||
t = _ray_aabb(ox, oz, dx, dz, s["x0"], s["x1"], s["z0"], s["z1"])
|
||||
if t is not None and t < best:
|
||||
best = t
|
||||
best_surf = s
|
||||
if best_surf is None:
|
||||
return best, None
|
||||
hit_x = ox + dx * best
|
||||
hit_z = oz + dz * best
|
||||
face = best_surf["face"]
|
||||
if face == "z0" or face == "z1":
|
||||
s_coord = hit_x
|
||||
elif face == "x0" or face == "x1":
|
||||
s_coord = hit_z
|
||||
else:
|
||||
# Box: pick the face by comparing t to each plane.
|
||||
s_coord = (
|
||||
hit_x if best_surf["box"].half_w <= best_surf["box"].half_d else hit_z
|
||||
)
|
||||
return best, {"surf": best_surf, "s": s_coord, "hit_x": hit_x, "hit_z": hit_z}
|
||||
|
||||
def _row_of(self, world_h: float, t: float, eye: float, tan_pitch: float) -> float:
|
||||
"""Screen row (float) where a height `world_h` at distance `t` lands."""
|
||||
if math.isinf(t):
|
||||
return 0.0 if world_h > eye else float(self.height)
|
||||
vv = (world_h - eye) / t - tan_pitch
|
||||
return (1.0 - vv / self.tan_fy) / 2.0 * self.height
|
||||
|
||||
def _shade_wall(
|
||||
self, surf: dict | None, dist: float, s: float
|
||||
) -> tuple[int, int, int]:
|
||||
if surf is None:
|
||||
return SKY
|
||||
s_def = surf["surf"]
|
||||
color = s_def["color"]
|
||||
face = s_def["face"]
|
||||
shade = 1.0
|
||||
if face == "x0":
|
||||
shade = 0.9
|
||||
elif face == "x1":
|
||||
shade = 1.08
|
||||
elif face == "z1":
|
||||
shade = 1.0
|
||||
elif face == "z0":
|
||||
shade = 0.95
|
||||
# Concrete panel grid: subtle stripes every 1 m.
|
||||
if int(abs(s) * 1.0) % 2 == 0:
|
||||
shade *= 1.06
|
||||
col = (
|
||||
min(255, int(color[0] * shade)),
|
||||
min(255, int(color[1] * shade)),
|
||||
min(255, int(color[2] * shade)),
|
||||
)
|
||||
return _fog(col, dist)
|
||||
|
||||
def _shade_floor(self, wx: float, wz: float, t: float) -> tuple[int, int, int]:
|
||||
cell = int(math.floor(wx)) + int(math.floor(wz))
|
||||
col = FLOOR_A if cell % 2 == 0 else FLOOR_B
|
||||
fx = wx - math.floor(wx)
|
||||
fz = wz - math.floor(wz)
|
||||
if min(fx, 1.0 - fx, fz, 1.0 - fz) < 0.07:
|
||||
col = FLOOR_LINE
|
||||
return _fog(col, t)
|
||||
|
||||
def _draw_beacon(
|
||||
self,
|
||||
room: Room,
|
||||
pixels: bytearray,
|
||||
depth: list[list[float]],
|
||||
fx: float,
|
||||
fz: float,
|
||||
rx: float,
|
||||
rz: float,
|
||||
eye: float,
|
||||
tan_pitch: float,
|
||||
) -> None:
|
||||
"""Billboard the beacon with a soft glow, occluded by the depth buffer."""
|
||||
w, h = self.width, self.height
|
||||
b = room.beacon
|
||||
rel_x = b.x - room.capsule.x
|
||||
rel_z = b.z - room.capsule.z
|
||||
along = rel_x * fx + rel_z * fz
|
||||
if along < 0.25:
|
||||
return
|
||||
right = rel_x * rx + rel_z * rz
|
||||
col_c = (right / along / self.tan_fx + 1.0) / 2.0 * w
|
||||
vv = (b.height - eye) / along - tan_pitch
|
||||
row_c = (1.0 - vv / self.tan_fy) / 2.0 * h
|
||||
r_px = b.radius / along / self.tan_fx * w / 2.0
|
||||
if r_px < 1.2:
|
||||
r_px = 1.2
|
||||
glow = r_px * 3.4
|
||||
|
||||
pulse = 1.0
|
||||
if b.active:
|
||||
pulse = 0.9 + 0.1 * math.sin(room.tick * 0.6)
|
||||
core, mid, outer = BEACON_ON if b.active else BEACON_OFF
|
||||
core = tuple(pulse * v for v in core)
|
||||
mid = tuple(pulse * v for v in mid)
|
||||
outer = tuple(pulse * v for v in outer)
|
||||
|
||||
c0 = max(0, int(col_c - glow))
|
||||
c1 = min(w, int(col_c + glow) + 1)
|
||||
r0 = max(0, int(row_c - glow))
|
||||
r1 = min(h, int(row_c + glow) + 1)
|
||||
inv_r2 = 1.0 / (r_px * r_px)
|
||||
for r in range(r0, r1):
|
||||
for c in range(c0, c1):
|
||||
if depth[r][c] <= along - 0.06:
|
||||
continue # occluded by a nearer surface
|
||||
dc = c - col_c
|
||||
dr = r - row_c
|
||||
d2 = (dc * dc + dr * dr) * inv_r2
|
||||
if d2 > glow * glow * inv_r2:
|
||||
continue
|
||||
if d2 <= 1.0:
|
||||
a = 1.0 - d2 * 0.55
|
||||
col = core
|
||||
elif d2 <= 4.0:
|
||||
f = (d2 - 1.0) / 3.0
|
||||
a = 0.85 * (1.0 - f)
|
||||
col = tuple(core[i] + (mid[i] - core[i]) * f for i in range(3))
|
||||
else:
|
||||
f = (d2 - 4.0) / (glow * glow * inv_r2 - 4.0)
|
||||
a = 0.5 * (1.0 - f)
|
||||
col = tuple(mid[i] + (outer[i] - mid[i]) * f for i in range(3))
|
||||
o = (r * w + c) * 3
|
||||
base = (pixels[o], pixels[o + 1], pixels[o + 2])
|
||||
out = (
|
||||
int(base[0] * (1 - a) + col[0] * 255 * a),
|
||||
int(base[1] * (1 - a) + col[1] * 255 * a),
|
||||
int(base[2] * (1 - a) + col[2] * 255 * a),
|
||||
)
|
||||
pixels[o] = min(255, out[0])
|
||||
pixels[o + 1] = min(255, out[1])
|
||||
pixels[o + 2] = min(255, out[2])
|
||||
|
||||
def _depth_grid(
|
||||
self, depth: list[list[float]], max_depth: float
|
||||
) -> list[list[float]]:
|
||||
"""Downsample the per-pixel depth to 1/4 resolution (40x30 at 160x120)."""
|
||||
w, h = self.width, self.height
|
||||
gw, gh = w // 4, h // 4
|
||||
grid = [[0.0] * gw for _ in range(gh)]
|
||||
for r in range(gh):
|
||||
for c in range(gw):
|
||||
total = 0.0
|
||||
n = 0
|
||||
for rr in range(r * 4, r * 4 + 4):
|
||||
row = depth[rr]
|
||||
for cc in range(c * 4, c * 4 + 4):
|
||||
t = row[cc]
|
||||
if t >= MAX_VIEW or t > max_depth:
|
||||
continue
|
||||
total += t
|
||||
n += 1
|
||||
grid[r][c] = round(total / n, 2) if n else 0.0
|
||||
return grid
|
||||
|
||||
|
||||
def _ray_aabb(
|
||||
ox: float,
|
||||
oz: float,
|
||||
dx: float,
|
||||
dz: float,
|
||||
x0: float,
|
||||
x1: float,
|
||||
z0: float,
|
||||
z1: float,
|
||||
) -> float | None:
|
||||
"""Slab test in 2D. Returns entry distance or None."""
|
||||
if abs(dx) < _EPS:
|
||||
if ox < x0 - _EPS or ox > x1 + _EPS:
|
||||
return None
|
||||
tx0, tx1 = -math.inf, math.inf
|
||||
else:
|
||||
tx0, tx1 = (x0 - ox) / dx, (x1 - ox) / dx
|
||||
if tx0 > tx1:
|
||||
tx0, tx1 = tx1, tx0
|
||||
if abs(dz) < _EPS:
|
||||
if oz < z0 - _EPS or oz > z1 + _EPS:
|
||||
return None
|
||||
tz0, tz1 = -math.inf, math.inf
|
||||
else:
|
||||
tz0, tz1 = (z0 - oz) / dz, (z1 - oz) / dz
|
||||
if tz0 > tz1:
|
||||
tz0, tz1 = tz1, tz0
|
||||
tin = max(tx0, tz0)
|
||||
tout = min(tx1, tz1)
|
||||
if tout < 0.0 or tin > tout:
|
||||
return None
|
||||
if tin > _EPS:
|
||||
return tin
|
||||
return None if tout <= _EPS else tout
|
||||
|
||||
|
||||
def _fog(color: tuple[int, int, int], t: float) -> tuple[int, int, int]:
|
||||
k = min(1.0, t / MAX_VIEW)
|
||||
k = k**1.4
|
||||
return (
|
||||
int(color[0] + (FOG[0] - color[0]) * k),
|
||||
int(color[1] + (FOG[1] - color[1]) * k),
|
||||
int(color[2] + (FOG[2] - color[2]) * k),
|
||||
)
|
||||
|
||||
|
||||
def _lerp(
|
||||
a: tuple[int, int, int], b: tuple[int, int, int], k: float
|
||||
) -> tuple[int, int, int]:
|
||||
return (
|
||||
int(a[0] + (b[0] - a[0]) * k),
|
||||
int(a[1] + (b[1] - a[1]) * k),
|
||||
int(a[2] + (b[2] - a[2]) * k),
|
||||
)
|
||||
|
||||
|
||||
def render_preview(
|
||||
room: Room, path: str, width: int = 320, height: int = 240
|
||||
) -> Image.Image:
|
||||
"""Debug helper: render the current view and save it to a PNG file."""
|
||||
img, _ = Raycaster(width, height).render(room)
|
||||
img.save(path)
|
||||
return img
|
||||
@@ -0,0 +1,343 @@
|
||||
"""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)
|
||||
ok, h = self._try_displace(fx * step, fz * step)
|
||||
if not ok and h is not None:
|
||||
h.impulse = remaining
|
||||
hit = h
|
||||
break
|
||||
moved += step
|
||||
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)."""
|
||||
cap = self.capsule
|
||||
nx = cap.x + dx
|
||||
nz = cap.z + dz
|
||||
|
||||
# Room walls: clamp to bounds (the capsule cannot leave the room).
|
||||
r = cap.radius
|
||||
if nx < r or nx > ROOM_SIZE - r or nz < r or nz > ROOM_SIZE - r:
|
||||
hit = self._wall_hit(nx, nz)
|
||||
return False, hit
|
||||
|
||||
blocked: Hit | None = None
|
||||
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
|
||||
)
|
||||
if blocked is not None:
|
||||
return False, blocked
|
||||
|
||||
cap.x = nx
|
||||
cap.z = nz
|
||||
return True, None
|
||||
|
||||
def _wall_hit(self, nx: float, nz: float) -> Hit:
|
||||
r = self.capsule.radius
|
||||
if nx < r:
|
||||
return Hit(other="wall_west", normal_x=1.0, normal_z=0.0, impulse=0.0)
|
||||
if nx > ROOM_SIZE - r:
|
||||
return Hit(other="wall_east", normal_x=-1.0, normal_z=0.0, impulse=0.0)
|
||||
if nz < r:
|
||||
return Hit(other="wall_north", normal_x=0.0, normal_z=1.0, impulse=0.0)
|
||||
return Hit(other="wall_south", normal_x=0.0, normal_z=-1.0, impulse=0.0)
|
||||
|
||||
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_delta: float = 0.0, pitch_delta: float = 0.0) -> None:
|
||||
cap = self.capsule
|
||||
cap.yaw_deg = (cap.yaw_deg + yaw_delta) % 360.0
|
||||
cap.pitch_deg = min(85.0, max(-85.0, cap.pitch_deg + pitch_delta))
|
||||
|
||||
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),
|
||||
},
|
||||
}
|
||||
Reference in New Issue
Block a user