Initial commit: BoxedMan core architecture with vision, calibration, physics, and autonomous pegman
This commit is contained in:
commit
5190192552
9 changed files with 887 additions and 0 deletions
7
.gitignore
vendored
Normal file
7
.gitignore
vendored
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
.venv/
|
||||
__pycache__/
|
||||
*.pyc
|
||||
.calibration.json
|
||||
config_local.json
|
||||
.vscode/
|
||||
.idea/
|
||||
82
README.md
Normal file
82
README.md
Normal file
|
|
@ -0,0 +1,82 @@
|
|||
# BoxedMan 📦🚶♂️
|
||||
|
||||
En interaktiv visualisering og fysisk projektionsinstallation bygget i Python.
|
||||
|
||||
* **Projektoren** viser en ren hvid flade, hvor en rød autonom "Peg Man" bevæger sig rundt i sin kasse.
|
||||
* **Webkameraet** overvåger projektionsfladen og detekterer fysiske skygger eller silhuetter (fx hænder, kroppe, papstykker).
|
||||
* Skyggerne omsættes i realtid til faste 2D-kollisionsobjekter (*solids*), som Peg Man kan gå på, klatre op ad, hoppe ned fra og interagere med.
|
||||
|
||||
---
|
||||
|
||||
## 🛠️ Teknologistak
|
||||
|
||||
* **Computer Vision**: OpenCV (`cv2`) – 4-punkts homografisk perspektivtransformation, adaptiv skyggeudtræk og polygonapproksimering.
|
||||
* **Fysikmotor**: Pymunk (Chipmunk 2D) – realtids segment/polygon kollisionshåndtering og raycasts.
|
||||
* **Grafik & Display**: Pygame-ce – fuldskærms rendering på projektor.
|
||||
|
||||
---
|
||||
|
||||
## 🚀 Installation & Kørsel
|
||||
|
||||
### 1. Klargør miljø
|
||||
```powershell
|
||||
# Opret og aktivér virtuelt miljø
|
||||
py -3.12 -m venv .venv
|
||||
.\.venv\Scripts\Activate.ps1
|
||||
|
||||
# Installer afhængigheder
|
||||
pip install -r requirements.txt
|
||||
```
|
||||
|
||||
### 2. Kalibrering (Første gang)
|
||||
Når du starter BoxedMan første gang med projektor og kamera sat op, åbnes kalibreringsvinduet automatisk.
|
||||
|
||||
Du kan også køre kalibreringen direkte når som helst:
|
||||
```powershell
|
||||
python main.py --calibrate
|
||||
```
|
||||
* Klik på de 4 hjørner af den hvide projektionsflade i kamera-vinduet i rækkefølgen:
|
||||
1. **Øverst-Venstre**
|
||||
2. **Øverst-Højre**
|
||||
3. **Nederst-Højre**
|
||||
4. **Nederst-Venstre**
|
||||
* Tryk på **'c'** for at bekræfte og gemme kalibreringen.
|
||||
* Tryk på **'r'** for at nulstille punkterne hvis du klikkede forkert.
|
||||
|
||||
### 3. Normal kørsel
|
||||
```powershell
|
||||
# Standard kørsel i vindue
|
||||
python main.py
|
||||
|
||||
# Kør i fuldskærm på projektoren
|
||||
python main.py --fullscreen
|
||||
|
||||
# Vælg et specifikt webkamera-indeks (hvis du har flere tilsluttet, fx 0, 1, 2)
|
||||
python main.py --cam 1
|
||||
```
|
||||
|
||||
### 4. Simulerings-tilstand (Uden webcam/projektor)
|
||||
Hvis du vil teste fysik og Peg Mans adfærd direkte på din skærm:
|
||||
```powershell
|
||||
python main.py --sim
|
||||
```
|
||||
* **Venstreklik og træk musen**: Tegn forhindringer/skygger.
|
||||
* **Højreklik**: Ryd alle tegnede forhindringer.
|
||||
|
||||
---
|
||||
|
||||
## ⌨️ Genvejstaster under kørsel
|
||||
|
||||
| Tast | Funktion |
|
||||
|---|---|
|
||||
| **b** | Tag et nyt baseline baggrundsbillede (når skærmen er helt tom for skygger) |
|
||||
| **d** | Slå Vision Debug Mask overlay til/fra |
|
||||
| **c** | Start 4-punkts kalibrering igen |
|
||||
| **r** | Nulstil Peg Mans position |
|
||||
| **ESC** | Afslut programmet |
|
||||
|
||||
---
|
||||
|
||||
## ⚙️ Konfiguration
|
||||
|
||||
Indstillinger som f.eks. kameraopløsning, projektorstørrelse, tyngdekraft, ganghastighed og skyggefølsomhed kan justeres i `src/config.py` eller gemmes i `config/settings.json`.
|
||||
175
main.py
Normal file
175
main.py
Normal file
|
|
@ -0,0 +1,175 @@
|
|||
"""Main application entry point for BoxedMan: integrates Vision, Physics, Agent, and Display."""
|
||||
|
||||
import sys
|
||||
import os
|
||||
import argparse
|
||||
import pygame
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from src.config import Settings, CalibrationData
|
||||
from src.vision import VisionPipeline
|
||||
from src.physics import PhysicsWorld
|
||||
from src.pegman import PegMan
|
||||
from src.calibration import Calibrator
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description="BoxedMan - Interactive Projector Installation")
|
||||
parser.add_argument("--calibrate", action="store_true", help="Run 4-point perspective camera calibration")
|
||||
parser.add_argument("--cam", type=int, default=None, help="Camera index override (e.g. 0, 1, 2)")
|
||||
parser.add_argument("--fullscreen", action="store_true", help="Launch in fullscreen mode on projector")
|
||||
parser.add_argument("--sim", action="store_true", help="Simulation mode without webcam (use mouse to draw obstacles)")
|
||||
args = parser.parse_args()
|
||||
|
||||
settings = Settings.load()
|
||||
if args.cam is not None:
|
||||
settings.camera_index = args.cam
|
||||
if args.fullscreen:
|
||||
settings.fullscreen = True
|
||||
|
||||
# 1. Calibration mode if requested or no calibration file exists
|
||||
calibration = CalibrationData.load()
|
||||
if args.calibrate or (calibration is None and not args.sim):
|
||||
print("[BoxedMan] Starting calibration utility...")
|
||||
calibrator = Calibrator(settings)
|
||||
calibration = calibrator.run()
|
||||
|
||||
# 2. Initialize Vision Pipeline (unless running pure simulation)
|
||||
vision: VisionPipeline = None
|
||||
if not args.sim:
|
||||
vision = VisionPipeline(settings, calibration)
|
||||
if not vision.start_camera():
|
||||
print("[BoxedMan] Could not start camera. Falling back to mouse simulation mode (--sim).")
|
||||
args.sim = True
|
||||
|
||||
# 3. Initialize Pygame Display
|
||||
pygame.init()
|
||||
flags = pygame.FULLSCREEN if settings.fullscreen else 0
|
||||
screen = pygame.display.set_mode((settings.projector_width, settings.projector_height), flags)
|
||||
pygame.display.set_caption("BoxedMan")
|
||||
clock = pygame.time.Clock()
|
||||
|
||||
# 4. Initialize Physics & Peg Man
|
||||
physics = PhysicsWorld(settings)
|
||||
pegman = PegMan(physics, settings, start_pos=(settings.projector_width // 2, settings.projector_height - 100))
|
||||
|
||||
# Mouse simulation obstacles
|
||||
mouse_polygons = []
|
||||
drawing_poly = []
|
||||
|
||||
# State variables
|
||||
show_debug_vision = False
|
||||
running = True
|
||||
|
||||
print("\n--- BOXEDMAN KØRER ---")
|
||||
print("Taster:")
|
||||
print(" 'b' : Tag nyt baggrundsreferencebillede (uden skygger/hænder på lærredet)")
|
||||
print(" 'd' : Vis / skjul vision debug overlay")
|
||||
print(" 'c' : Genkalibrér 4 hjørner")
|
||||
print(" 'r' : Nulstil Peg Mans position")
|
||||
print(" 'ESC' eller luk vindue for at afslutte.")
|
||||
if args.sim:
|
||||
print("SIMULATIONSTILSTAND AKTIV: Venstreklik og træk musen for at tegne forhindringer, Højreklik for at rydde.")
|
||||
print("----------------------\n")
|
||||
|
||||
while running:
|
||||
dt = clock.tick(settings.fps) / 1000.0
|
||||
dt = min(dt, 0.05) # Prevent physics explosion on lag spike
|
||||
|
||||
# --- Event Handling ---
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
running = False
|
||||
elif event.type == pygame.KEYDOWN:
|
||||
if event.key == pygame.K_ESCAPE:
|
||||
running = False
|
||||
elif event.key == pygame.K_b and vision:
|
||||
vision.capture_reference_background()
|
||||
elif event.key == pygame.K_d:
|
||||
show_debug_vision = not show_debug_vision
|
||||
elif event.key == pygame.K_r:
|
||||
pegman.body.position = (settings.projector_width // 2, settings.projector_height - 100)
|
||||
pegman.body.velocity = (0, 0)
|
||||
elif event.key == pygame.K_c and vision:
|
||||
vision.stop_camera()
|
||||
calibrator = Calibrator(settings)
|
||||
new_calib = calibrator.run()
|
||||
if new_calib:
|
||||
calibration = new_calib
|
||||
vision.calibration = calibration
|
||||
vision._update_homography()
|
||||
vision.start_camera()
|
||||
|
||||
# Mouse obstacle drawing for simulation mode
|
||||
elif args.sim:
|
||||
if event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
|
||||
drawing_poly = [event.pos]
|
||||
elif event.type == pygame.MOUSEMOTION and event.buttons[0]:
|
||||
if drawing_poly and np.hypot(event.pos[0] - drawing_poly[-1][0], event.pos[1] - drawing_poly[-1][1]) > 10:
|
||||
drawing_poly.append(event.pos)
|
||||
elif event.type == pygame.MOUSEBUTTONUP and event.button == 1:
|
||||
if len(drawing_poly) >= 3:
|
||||
mouse_polygons.append(np.array(drawing_poly, dtype=np.int32))
|
||||
drawing_poly = []
|
||||
elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 3:
|
||||
mouse_polygons.clear()
|
||||
|
||||
# --- Vision Processing ---
|
||||
detected_polygons = []
|
||||
debug_mask = None
|
||||
if vision:
|
||||
raw_frame = vision.read_raw_frame()
|
||||
if raw_frame is not None:
|
||||
warped = vision.warp_frame(raw_frame)
|
||||
detected_polygons, debug_mask = vision.extract_shadow_polygons(warped)
|
||||
|
||||
if args.sim:
|
||||
detected_polygons = mouse_polygons
|
||||
|
||||
# Update dynamic physics colliders based on detected shapes
|
||||
physics.update_shadow_geometry(detected_polygons)
|
||||
|
||||
# Step physics and agent behavior
|
||||
pegman.update(dt)
|
||||
physics.step(dt)
|
||||
|
||||
# --- Rendering ---
|
||||
# 1. Clear with clean white projector canvas
|
||||
screen.fill((255, 255, 255))
|
||||
|
||||
# 2. Render borders / bounding box
|
||||
pygame.draw.rect(screen, (30, 30, 30), (0, 0, settings.projector_width, settings.projector_height), 4)
|
||||
|
||||
# 3. Draw shadow outlines or solids (subtle styling on the white projector)
|
||||
for poly in detected_polygons:
|
||||
if len(poly) >= 3:
|
||||
pts = [(int(p[0]), int(p[1])) for p in poly]
|
||||
# Light neutral fill / outline
|
||||
pygame.draw.polygon(screen, (230, 230, 230), pts)
|
||||
pygame.draw.lines(screen, (100, 100, 100), True, pts, 2)
|
||||
|
||||
# Draw currently drawn sim line
|
||||
if drawing_poly and len(drawing_poly) > 1:
|
||||
pygame.draw.lines(screen, (150, 150, 150), False, drawing_poly, 2)
|
||||
|
||||
# 4. Render Peg Man
|
||||
pegman.render(screen)
|
||||
|
||||
# 5. Optional Debug Overlay for Vision
|
||||
if show_debug_vision and debug_mask is not None:
|
||||
mask_rgb = cv2.cvtColor(debug_mask, cv2.COLOR_GRAY2RGB)
|
||||
mask_surf = pygame.surfarray.make_surface(np.transpose(mask_rgb, (1, 0, 2)))
|
||||
mask_surf.set_alpha(128)
|
||||
screen.blit(mask_surf, (0, 0))
|
||||
|
||||
pygame.display.flip()
|
||||
|
||||
if vision:
|
||||
vision.stop_camera()
|
||||
pygame.quit()
|
||||
sys.exit(0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
4
requirements.txt
Normal file
4
requirements.txt
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
opencv-python>=4.9.0
|
||||
pygame-ce>=2.5.0
|
||||
pymunk>=6.8.0
|
||||
numpy>=1.26.0
|
||||
84
src/calibration.py
Normal file
84
src/calibration.py
Normal file
|
|
@ -0,0 +1,84 @@
|
|||
"""Interactive 4-point calibration utility to map camera perspective directly to projection area."""
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from typing import List, Optional
|
||||
from .config import Settings, CalibrationData
|
||||
|
||||
|
||||
class Calibrator:
|
||||
def __init__(self, settings: Settings):
|
||||
self.settings = settings
|
||||
self.points: List[List[float]] = []
|
||||
self.window_name = "BoxedMan - 4-Point Calibration (Click 4 Corners)"
|
||||
|
||||
def _mouse_callback(self, event, x, y, flags, param):
|
||||
if event == cv2.EVENT_LBUTTONDOWN:
|
||||
if len(self.points) < 4:
|
||||
self.points.append([float(x), float(y)])
|
||||
print(f"[Calibrator] Selected corner {len(self.points)}: ({x}, {y})")
|
||||
|
||||
def run(self) -> Optional[CalibrationData]:
|
||||
cap = cv2.VideoCapture(self.settings.camera_index, cv2.CAP_DSHOW)
|
||||
if not cap.isOpened():
|
||||
cap = cv2.VideoCapture(self.settings.camera_index)
|
||||
if not cap.isOpened():
|
||||
print(f"[Calibrator] Cannot open camera {self.settings.camera_index}")
|
||||
return None
|
||||
|
||||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.settings.camera_width)
|
||||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.settings.camera_height)
|
||||
|
||||
cv2.namedWindow(self.window_name)
|
||||
cv2.setMouseCallback(self.window_name, self._mouse_callback)
|
||||
|
||||
print("\n--- KALIBRERING ---")
|
||||
print("1. Klik på de 4 hjørner af den projicerede hvide boks i kamera-billedet i rækkefølge:")
|
||||
print(" [1] Øverst-Venstre -> [2] Øverst-Højre -> [3] Nederst-Højre -> [4] Nederst-Venstre")
|
||||
print("Tast 'r' for at nulstille punkter.")
|
||||
print("Tast 'c' for at gemme og fortsætte (når 4 punkter er valgt).")
|
||||
print("Tast 'q' for at annullere.\n")
|
||||
|
||||
saved = False
|
||||
while True:
|
||||
ret, frame = cap.read()
|
||||
if not ret:
|
||||
break
|
||||
|
||||
display = frame.copy()
|
||||
|
||||
# Draw already clicked points and lines
|
||||
for i, pt in enumerate(self.points):
|
||||
pos = (int(pt[0]), int(pt[1]))
|
||||
cv2.circle(display, pos, 6, (0, 0, 255), -1)
|
||||
cv2.putText(display, f"{i+1}", (pos[0] + 10, pos[1] - 10),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
|
||||
|
||||
if len(self.points) > 1:
|
||||
for i in range(len(self.points) - 1):
|
||||
p1 = (int(self.points[i][0]), int(self.points[i][1]))
|
||||
p2 = (int(self.points[i + 1][0]), int(self.points[i + 1][1]))
|
||||
cv2.line(display, p1, p2, (0, 255, 255), 2)
|
||||
if len(self.points) == 4:
|
||||
p1 = (int(self.points[3][0]), int(self.points[3][1]))
|
||||
p2 = (int(self.points[0][0]), int(self.points[0][1]))
|
||||
cv2.line(display, p1, p2, (0, 255, 255), 2)
|
||||
|
||||
cv2.imshow(self.window_name, display)
|
||||
key = cv2.waitKey(15) & 0xFF
|
||||
|
||||
if key == ord('r'):
|
||||
self.points.clear()
|
||||
print("[Calibrator] Punkter nulstillet.")
|
||||
elif key == ord('c') and len(self.points) == 4:
|
||||
calib = CalibrationData(points=self.points)
|
||||
calib.save()
|
||||
print("[Calibrator] Kalibrering gemt i config/calibration.json!")
|
||||
saved = True
|
||||
break
|
||||
elif key == ord('q') or key == 27:
|
||||
break
|
||||
|
||||
cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
return CalibrationData(points=self.points) if saved else None
|
||||
82
src/config.py
Normal file
82
src/config.py
Normal file
|
|
@ -0,0 +1,82 @@
|
|||
"""Configuration settings and calibration storage for BoxedMan."""
|
||||
|
||||
import json
|
||||
import os
|
||||
from dataclasses import dataclass, asdict
|
||||
from typing import List, Tuple, Optional
|
||||
|
||||
CONFIG_FILE = "config/settings.json"
|
||||
CALIBRATION_FILE = "config/calibration.json"
|
||||
|
||||
|
||||
@dataclass
|
||||
class Settings:
|
||||
# Camera settings
|
||||
camera_index: int = 0
|
||||
camera_width: int = 1280
|
||||
camera_height: int = 720
|
||||
camera_fps: int = 30
|
||||
|
||||
# Display / Projector settings
|
||||
projector_width: int = 1280
|
||||
projector_height: int = 720
|
||||
fullscreen: bool = False
|
||||
display_index: int = 0 # 0 for primary monitor, 1 for secondary/projector
|
||||
|
||||
# Vision / Shadow Detection
|
||||
# Threshold mode: 'diff' (against reference white frame) or 'adaptive' / 'brightness'
|
||||
shadow_threshold: int = 70 # Pixel value difference considered a shadow
|
||||
min_contour_area: float = 800.0 # Ignore tiny noise artifacts
|
||||
polygon_epsilon: float = 3.0 # Ramer-Douglas-Peucker simplification factor
|
||||
blur_kernel: int = 5
|
||||
temporal_smoothing: float = 0.5 # Alpha blending between frames to reduce jitter
|
||||
|
||||
# Simulation / Physics
|
||||
gravity: float = 900.0 # Pixels/sec^2
|
||||
fps: int = 60
|
||||
|
||||
# Peg man properties
|
||||
man_color: Tuple[int, int, int] = (220, 20, 20)
|
||||
man_width: int = 24
|
||||
man_height: int = 56
|
||||
walk_speed: float = 140.0
|
||||
jump_strength: float = 380.0
|
||||
climb_speed: float = 120.0
|
||||
|
||||
@classmethod
|
||||
def load(cls, path: str = CONFIG_FILE) -> "Settings":
|
||||
if os.path.exists(path):
|
||||
try:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
return cls(**data)
|
||||
except Exception as e:
|
||||
print(f"[Config] Error loading settings from {path}: {e}. Using defaults.")
|
||||
return cls()
|
||||
|
||||
def save(self, path: str = CONFIG_FILE) -> None:
|
||||
os.makedirs(os.path.dirname(path), exist_ok=True)
|
||||
with open(path, "w", encoding="utf-8") as f:
|
||||
json.dump(asdict(self), f, indent=4)
|
||||
|
||||
|
||||
@dataclass
|
||||
class CalibrationData:
|
||||
"""Stores 4 corners (top-left, top-right, bottom-right, bottom-left) in camera coordinates."""
|
||||
points: List[List[float]] # List of [x, y] in camera pixel coordinates
|
||||
|
||||
@classmethod
|
||||
def load(cls, path: str = CALIBRATION_FILE) -> Optional["CalibrationData"]:
|
||||
if os.path.exists(path):
|
||||
try:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
return cls(points=data["points"])
|
||||
except Exception as e:
|
||||
print(f"[Calibration] Error loading calibration from {path}: {e}")
|
||||
return None
|
||||
|
||||
def save(self, path: str = CALIBRATION_FILE) -> None:
|
||||
os.makedirs(os.path.dirname(path), exist_ok=True)
|
||||
with open(path, "w", encoding="utf-8") as f:
|
||||
json.dump({"points": self.points}, f, indent=4)
|
||||
229
src/pegman.py
Normal file
229
src/pegman.py
Normal file
|
|
@ -0,0 +1,229 @@
|
|||
"""Autonomous Peg Man agent: physics body, sensory raycasts, behavior state machine, and animation."""
|
||||
|
||||
import math
|
||||
import random
|
||||
import pygame
|
||||
import pymunk
|
||||
from enum import Enum
|
||||
from typing import Tuple, Optional
|
||||
from .config import Settings
|
||||
from .physics import PhysicsWorld
|
||||
|
||||
|
||||
class ManState(Enum):
|
||||
WANDERING = "wandering"
|
||||
WAITING = "waiting"
|
||||
JUMPING = "jumping"
|
||||
CLIMBING = "climbing"
|
||||
FALLING = "falling"
|
||||
|
||||
|
||||
class PegMan:
|
||||
def __init__(self, physics: PhysicsWorld, settings: Settings, start_pos: Tuple[float, float]):
|
||||
self.physics = physics
|
||||
self.settings = settings
|
||||
self.state = ManState.WANDERING
|
||||
self.state_timer = 0.0
|
||||
|
||||
# Movement attributes
|
||||
self.direction = 1 # 1 = right, -1 = left
|
||||
self.on_ground = False
|
||||
self.near_wall = False
|
||||
self.near_edge = False
|
||||
self.walk_cycle = 0.0
|
||||
|
||||
# Physical dimensions
|
||||
self.width = settings.man_width
|
||||
self.height = settings.man_height
|
||||
self.radius = self.width / 2.0
|
||||
|
||||
# Create rigid body (upright capsule)
|
||||
mass = 10.0
|
||||
moment = pymunk.moment_for_box(mass, (self.width, self.height))
|
||||
# Infinite moment keeps him upright without tumbling
|
||||
self.body = pymunk.Body(mass, float('inf'))
|
||||
self.body.position = start_pos
|
||||
|
||||
# Main collision shape (capsule / rounded segment)
|
||||
p1 = (0, -self.height / 2.0 + self.radius)
|
||||
p2 = (0, self.height / 2.0 - self.radius)
|
||||
self.shape = pymunk.Segment(self.body, p1, p2, self.radius)
|
||||
self.shape.friction = 0.4
|
||||
self.shape.elasticity = 0.0
|
||||
self.shape.filter = pymunk.ShapeFilter(
|
||||
categories=PhysicsWorld.CATEGORY_MAN,
|
||||
mask=PhysicsWorld.CATEGORY_WALL | PhysicsWorld.CATEGORY_SHADOW
|
||||
)
|
||||
|
||||
self.physics.space.add(self.body, self.shape)
|
||||
|
||||
@property
|
||||
def position(self) -> Tuple[float, float]:
|
||||
return float(self.body.position.x), float(self.body.position.y)
|
||||
|
||||
def _cast_ray(self, start: Tuple[float, float], end: Tuple[float, float]) -> Optional[pymunk.SegmentQueryInfo]:
|
||||
"""Performs a line segment query against walls and shadows."""
|
||||
mask = PhysicsWorld.CATEGORY_WALL | PhysicsWorld.CATEGORY_SHADOW
|
||||
shape_filter = pymunk.ShapeFilter(mask=mask)
|
||||
query = self.physics.space.segment_query_first(start, end, 1.0, shape_filter)
|
||||
if query and query.shape != self.shape:
|
||||
return query
|
||||
return None
|
||||
|
||||
def update_sensors(self) -> None:
|
||||
"""Fires raycasts to perceive the ground, obstacles, edges, and climbable surfaces."""
|
||||
px, py = self.position
|
||||
half_h = self.height / 2.0
|
||||
half_w = self.width / 2.0
|
||||
|
||||
# 1. Ground sensor (downwards from feet)
|
||||
feet_y = py + half_h
|
||||
ground_query = self._cast_ray((px, feet_y - 2), (px, feet_y + 8))
|
||||
self.on_ground = ground_query is not None
|
||||
|
||||
# 2. Forward wall / obstacle sensor (at waist and chest height)
|
||||
dir_x = self.direction
|
||||
wall_dist = half_w + 14.0
|
||||
waist_query = self._cast_ray((px, py), (px + dir_x * wall_dist, py))
|
||||
chest_query = self._cast_ray((px, py - 15), (px + dir_x * wall_dist, py - 15))
|
||||
knee_query = self._cast_ray((px, py + 15), (px + dir_x * wall_dist, py + 15))
|
||||
|
||||
self.near_wall = (waist_query is not None) or (knee_query is not None)
|
||||
self.high_wall = chest_query is not None and waist_query is not None
|
||||
|
||||
# 3. Ledge / step up sensor (overhead obstacle clear check)
|
||||
head_clear_query = self._cast_ray((px + dir_x * wall_dist, py - half_h - 10), (px + dir_x * wall_dist, py - half_h))
|
||||
self.can_climb_step = self.near_wall and not self.high_wall and (head_clear_query is None)
|
||||
|
||||
# 4. Pit / Edge sensor (downwards in front of movement direction)
|
||||
edge_x = px + dir_x * (half_w + 16.0)
|
||||
edge_query = self._cast_ray((edge_x, feet_y - 2), (edge_x, feet_y + 40))
|
||||
self.near_edge = (edge_query is None)
|
||||
|
||||
def update(self, dt: float) -> None:
|
||||
self.update_sensors()
|
||||
self.state_timer -= dt
|
||||
|
||||
# State transition logic
|
||||
if not self.on_ground and self.state not in (ManState.JUMPING, ManState.CLIMBING):
|
||||
self.state = ManState.FALLING
|
||||
|
||||
if self.state == ManState.FALLING:
|
||||
if self.on_ground:
|
||||
self.state = ManState.WANDERING
|
||||
self.state_timer = random.uniform(2.0, 5.0)
|
||||
|
||||
elif self.state == ManState.WANDERING:
|
||||
self.walk_cycle += dt * 10.0
|
||||
|
||||
# Check if we should climb a step
|
||||
if self.can_climb_step and self.on_ground:
|
||||
self.state = ManState.CLIMBING
|
||||
self.state_timer = 0.35
|
||||
|
||||
# Encountered a wall or edge
|
||||
elif self.near_wall or self.near_edge:
|
||||
# 30% chance to jump over if near edge or low obstacle, otherwise turn around
|
||||
if self.on_ground and random.random() < 0.35 and not self.high_wall:
|
||||
self.state = ManState.JUMPING
|
||||
self.body.velocity = (self.direction * self.settings.walk_speed * 1.1, -self.settings.jump_strength)
|
||||
self.state_timer = 0.5
|
||||
else:
|
||||
self.direction *= -1
|
||||
self.state = ManState.WAITING
|
||||
self.state_timer = random.uniform(0.6, 1.8)
|
||||
|
||||
elif self.state_timer <= 0:
|
||||
# Randomly stop to look around or turn
|
||||
if random.random() < 0.4:
|
||||
self.state = ManState.WAITING
|
||||
self.state_timer = random.uniform(1.0, 3.0)
|
||||
else:
|
||||
if random.random() < 0.3:
|
||||
self.direction *= -1
|
||||
self.state_timer = random.uniform(3.0, 7.0)
|
||||
|
||||
# Apply horizontal walking velocity
|
||||
vx = self.direction * self.settings.walk_speed
|
||||
self.body.velocity = (vx, self.body.velocity.y)
|
||||
|
||||
elif self.state == ManState.WAITING:
|
||||
# Stand still
|
||||
self.body.velocity = (self.body.velocity.x * 0.7, self.body.velocity.y)
|
||||
if self.state_timer <= 0:
|
||||
self.state = ManState.WANDERING
|
||||
self.state_timer = random.uniform(2.0, 5.0)
|
||||
|
||||
elif self.state == ManState.CLIMBING:
|
||||
# Ascend upwards and slightly forward to mount obstacle
|
||||
vx = self.direction * (self.settings.walk_speed * 0.7)
|
||||
vy = -self.settings.climb_speed
|
||||
self.body.velocity = (vx, vy)
|
||||
if self.state_timer <= 0:
|
||||
self.state = ManState.WANDERING
|
||||
self.state_timer = 2.0
|
||||
|
||||
elif self.state == ManState.JUMPING:
|
||||
if self.state_timer <= 0 and self.on_ground:
|
||||
self.state = ManState.WANDERING
|
||||
self.state_timer = random.uniform(2.0, 4.0)
|
||||
|
||||
# Keep within canvas bounds
|
||||
w = self.settings.projector_width
|
||||
px = max(self.radius, min(w - self.radius, self.body.position.x))
|
||||
if px != self.body.position.x:
|
||||
self.body.position = (px, self.body.position.y)
|
||||
self.direction *= -1
|
||||
|
||||
def render(self, surface: pygame.Surface) -> None:
|
||||
"""Renders the stylized red stick/peg man with procedural limbs."""
|
||||
px, py = self.position
|
||||
dir_x = self.direction
|
||||
color = self.settings.man_color
|
||||
line_w = 4
|
||||
|
||||
# Head
|
||||
head_radius = 8
|
||||
head_pos = (int(px), int(py - self.height / 2.0 + head_radius))
|
||||
pygame.draw.circle(surface, color, head_pos, head_radius)
|
||||
|
||||
# Eyes (directional dot)
|
||||
eye_x = head_pos[0] + dir_x * 4
|
||||
eye_y = head_pos[1] - 1
|
||||
pygame.draw.circle(surface, (255, 255, 255), (eye_x, eye_y), 2)
|
||||
|
||||
# Torso
|
||||
torso_top = (int(px), head_pos[1] + head_radius)
|
||||
pelvis = (int(px), int(py + 6))
|
||||
pygame.draw.line(surface, color, torso_top, pelvis, line_w)
|
||||
|
||||
# Procedural limbs based on movement state
|
||||
if self.state in (ManState.WANDERING, ManState.CLIMBING):
|
||||
leg_phase = math.sin(self.walk_cycle)
|
||||
arm_phase = math.cos(self.walk_cycle)
|
||||
elif self.state == ManState.JUMPING:
|
||||
leg_phase = 0.8
|
||||
arm_phase = -0.8
|
||||
else:
|
||||
leg_phase = 0.0
|
||||
arm_phase = 0.0
|
||||
|
||||
# Legs
|
||||
feet_y = py + self.height / 2.0
|
||||
leg1_end = (int(px + dir_x * leg_phase * 12), int(feet_y))
|
||||
leg2_end = (int(px - dir_x * leg_phase * 12), int(feet_y))
|
||||
|
||||
# Knees
|
||||
knee1 = (int(px + dir_x * leg_phase * 6), int(py + 18))
|
||||
knee2 = (int(px - dir_x * leg_phase * 6), int(py + 18))
|
||||
|
||||
pygame.draw.lines(surface, color, False, [pelvis, knee1, leg1_end], line_w)
|
||||
pygame.draw.lines(surface, color, False, [pelvis, knee2, leg2_end], line_w)
|
||||
|
||||
# Arms
|
||||
shoulder = (int(px), int(torso_top[1] + 5))
|
||||
arm1_end = (int(px + dir_x * 10 - arm_phase * 10), int(py + 8))
|
||||
arm2_end = (int(px - dir_x * 10 + arm_phase * 10), int(py + 8))
|
||||
|
||||
pygame.draw.line(surface, color, shoulder, arm1_end, line_w)
|
||||
pygame.draw.line(surface, color, shoulder, arm2_end, line_w)
|
||||
82
src/physics.py
Normal file
82
src/physics.py
Normal file
|
|
@ -0,0 +1,82 @@
|
|||
"""Physics world and dynamic shadow collider manager using Pymunk."""
|
||||
|
||||
import pymunk
|
||||
import numpy as np
|
||||
from typing import List, Tuple
|
||||
from .config import Settings
|
||||
|
||||
|
||||
class PhysicsWorld:
|
||||
# Collision categories / bitmasks
|
||||
CATEGORY_WALL = 0b0001
|
||||
CATEGORY_SHADOW = 0b0010
|
||||
CATEGORY_MAN = 0b0100
|
||||
CATEGORY_SENSOR = 0b1000
|
||||
|
||||
def __init__(self, settings: Settings):
|
||||
self.settings = settings
|
||||
self.space = pymunk.Space()
|
||||
self.space.gravity = (0, self.settings.gravity)
|
||||
self.space.iterations = 10
|
||||
|
||||
self.shadow_shapes: List[pymunk.Shape] = []
|
||||
self._setup_boundary_walls()
|
||||
|
||||
def _setup_boundary_walls(self) -> None:
|
||||
"""Creates the enclosing box (left, right, bottom, top) around the projector canvas."""
|
||||
w = float(self.settings.projector_width)
|
||||
h = float(self.settings.projector_height)
|
||||
thickness = 40.0
|
||||
|
||||
static_body = self.space.static_body
|
||||
|
||||
walls = [
|
||||
# Bottom floor
|
||||
pymunk.Segment(static_body, (-thickness, h - 2), (w + thickness, h - 2), thickness),
|
||||
# Left wall
|
||||
pymunk.Segment(static_body, (2, -thickness), (2, h + thickness), thickness),
|
||||
# Right wall
|
||||
pymunk.Segment(static_body, (w - 2, -thickness), (w - 2, h + thickness), thickness),
|
||||
# Ceiling
|
||||
pymunk.Segment(static_body, (-thickness, 2), (w + thickness, 2), thickness),
|
||||
]
|
||||
|
||||
for wall in walls:
|
||||
wall.friction = 1.0
|
||||
wall.elasticity = 0.0
|
||||
wall.filter = pymunk.ShapeFilter(categories=self.CATEGORY_WALL)
|
||||
self.space.add(wall)
|
||||
|
||||
def update_shadow_geometry(self, polygons: List[np.ndarray]) -> None:
|
||||
"""
|
||||
Replaces the current dynamic shadow colliders with new ones derived from vision contours.
|
||||
Uses chain segments or decomposed convex/concave polygons.
|
||||
Using closed segment chains ensures stable collisions with arbitrary shadow shapes.
|
||||
"""
|
||||
# Remove previous shadow colliders
|
||||
if self.shadow_shapes:
|
||||
self.space.remove(*self.shadow_shapes)
|
||||
self.shadow_shapes.clear()
|
||||
|
||||
static_body = self.space.static_body
|
||||
|
||||
for poly in polygons:
|
||||
n = len(poly)
|
||||
if n < 3:
|
||||
continue
|
||||
|
||||
# Create perimeter segment edges
|
||||
for i in range(n):
|
||||
p1 = tuple(map(float, poly[i]))
|
||||
p2 = tuple(map(float, poly[(i + 1) % n]))
|
||||
|
||||
seg = pymunk.Segment(static_body, p1, p2, radius=3.0)
|
||||
seg.friction = 0.9
|
||||
seg.elasticity = 0.0
|
||||
seg.filter = pymunk.ShapeFilter(categories=self.CATEGORY_SHADOW)
|
||||
|
||||
self.space.add(seg)
|
||||
self.shadow_shapes.append(seg)
|
||||
|
||||
def step(self, dt: float) -> None:
|
||||
self.space.step(dt)
|
||||
142
src/vision.py
Normal file
142
src/vision.py
Normal file
|
|
@ -0,0 +1,142 @@
|
|||
"""Vision pipeline for BoxedMan: camera input, perspective warping, and shadow contour detection."""
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from typing import List, Optional, Tuple
|
||||
from .config import Settings, CalibrationData
|
||||
|
||||
|
||||
class VisionPipeline:
|
||||
def __init__(self, settings: Settings, calibration: Optional[CalibrationData] = None):
|
||||
self.settings = settings
|
||||
self.calibration = calibration
|
||||
self.homography_matrix: Optional[np.ndarray] = None
|
||||
self.reference_frame: Optional[np.ndarray] = None
|
||||
self.cap: Optional[cv2.VideoCapture] = None
|
||||
|
||||
if self.calibration and len(self.calibration.points) == 4:
|
||||
self._update_homography()
|
||||
|
||||
def start_camera(self) -> bool:
|
||||
"""Initializes the webcam capture."""
|
||||
self.cap = cv2.VideoCapture(self.settings.camera_index, cv2.CAP_DSHOW)
|
||||
if not self.cap.isOpened():
|
||||
# Fallback to standard backend if DSHOW fails
|
||||
self.cap = cv2.VideoCapture(self.settings.camera_index)
|
||||
|
||||
if not self.cap.isOpened():
|
||||
print(f"[Vision] Could not open camera at index {self.settings.camera_index}")
|
||||
return False
|
||||
|
||||
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.settings.camera_width)
|
||||
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.settings.camera_height)
|
||||
self.cap.set(cv2.CAP_PROP_FPS, self.settings.camera_fps)
|
||||
print(f"[Vision] Camera {self.settings.camera_index} initialized.")
|
||||
return True
|
||||
|
||||
def stop_camera(self) -> None:
|
||||
if self.cap:
|
||||
self.cap.release()
|
||||
self.cap = None
|
||||
|
||||
def read_raw_frame(self) -> Optional[np.ndarray]:
|
||||
if not self.cap or not self.cap.isOpened():
|
||||
return None
|
||||
ret, frame = self.cap.read()
|
||||
if not ret:
|
||||
return None
|
||||
return frame
|
||||
|
||||
def set_calibration_points(self, points: List[List[float]]) -> None:
|
||||
self.calibration = CalibrationData(points=points)
|
||||
self.calibration.save()
|
||||
self._update_homography()
|
||||
|
||||
def _update_homography(self) -> None:
|
||||
"""Computes the 3x3 homography matrix from camera corner points to projector dimensions."""
|
||||
if not self.calibration or len(self.calibration.points) != 4:
|
||||
self.homography_matrix = None
|
||||
return
|
||||
|
||||
src_pts = np.float32(self.calibration.points)
|
||||
dst_pts = np.float32([
|
||||
[0, 0],
|
||||
[self.settings.projector_width, 0],
|
||||
[self.settings.projector_width, self.settings.projector_height],
|
||||
[0, self.settings.projector_height]
|
||||
])
|
||||
self.homography_matrix = cv2.getPerspectiveTransform(src_pts, dst_pts)
|
||||
print("[Vision] Perspective transform updated successfully.")
|
||||
|
||||
def warp_frame(self, frame: np.ndarray) -> np.ndarray:
|
||||
"""Warps the camera frame to match the projector view rectangle."""
|
||||
if self.homography_matrix is not None:
|
||||
return cv2.warpPerspective(
|
||||
frame,
|
||||
self.homography_matrix,
|
||||
(self.settings.projector_width, self.settings.projector_height)
|
||||
)
|
||||
return cv2.resize(frame, (self.settings.projector_width, self.settings.projector_height))
|
||||
|
||||
def capture_reference_background(self) -> None:
|
||||
"""Captures a baseline frame of the empty projected surface to compare against."""
|
||||
raw = self.read_raw_frame()
|
||||
if raw is not None:
|
||||
warped = self.warp_frame(raw)
|
||||
gray = cv2.cvtColor(warped, cv2.COLOR_BGR2GRAY)
|
||||
k = self.settings.blur_kernel | 1
|
||||
self.reference_frame = cv2.GaussianBlur(gray, (k, k), 0)
|
||||
print("[Vision] Background reference frame captured.")
|
||||
|
||||
def extract_shadow_polygons(self, warped_frame: np.ndarray) -> Tuple[List[np.ndarray], np.ndarray]:
|
||||
"""
|
||||
Detects shadows from the warped camera frame and returns simplified polygon vertices
|
||||
in projector coordinates, along with the binary shadow mask for debugging/display.
|
||||
"""
|
||||
# Convert to HSV to separate shadows/luminance from color (like the red peg man)
|
||||
hsv = cv2.cvtColor(warped_frame, cv2.COLOR_BGR2HSV)
|
||||
v_channel = hsv[:, :, 2] # Value / Brightness channel
|
||||
|
||||
k = self.settings.blur_kernel | 1
|
||||
blurred_v = cv2.GaussianBlur(v_channel, (k, k), 0)
|
||||
|
||||
if self.reference_frame is not None:
|
||||
# Difference against baseline reference
|
||||
# Shadows appear darker than baseline white screen
|
||||
diff = cv2.subtract(self.reference_frame, blurred_v)
|
||||
_, mask = cv2.threshold(diff, self.settings.shadow_threshold, 255, cv2.THRESH_BINARY)
|
||||
else:
|
||||
# Adaptive threshold / Otsu fallback when no baseline captured yet
|
||||
# Invert so dark areas (shadows) become 255 (white in mask)
|
||||
_, mask = cv2.threshold(
|
||||
blurred_v,
|
||||
0,
|
||||
255,
|
||||
cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU
|
||||
)
|
||||
|
||||
# Morphological clean up: remove small noise specs and fill internal holes
|
||||
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
|
||||
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel, iterations=1)
|
||||
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel, iterations=2)
|
||||
|
||||
# Extract contours
|
||||
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
|
||||
polygons: List[np.ndarray] = []
|
||||
for cnt in contours:
|
||||
area = cv2.contourArea(cnt)
|
||||
if area < self.settings.min_contour_area:
|
||||
continue
|
||||
|
||||
# Simplify contour with Ramer-Douglas-Peucker algorithm
|
||||
epsilon = self.settings.polygon_epsilon
|
||||
approx = cv2.approxPolyDP(cnt, epsilon, closed=True)
|
||||
|
||||
# We need at least 3 points for a valid 2D polygon
|
||||
if len(approx) >= 3:
|
||||
# Reshape to (N, 2)
|
||||
pts = approx.reshape(-1, 2)
|
||||
polygons.append(pts)
|
||||
|
||||
return polygons, mask
|
||||
Loading…
Add table
Reference in a new issue