281 lines
12 KiB
Python
281 lines
12 KiB
Python
"""Die Physikwelt: verwaltet Körper und Wände und führt Simulationsschritte aus."""
|
||
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
|
||
from sandbox.config import PhysicsConfig
|
||
from sandbox.physics.body import Body, Material
|
||
from sandbox.physics.broadphase import find_candidate_pairs
|
||
from sandbox.physics.collision import Contact, collide
|
||
from sandbox.physics.sleep import update_sleep, wake_touched_bodies
|
||
from sandbox.physics.solver import ContactSolver
|
||
from sandbox.physics.vector import ZERO, Vector2
|
||
|
||
|
||
class World:
|
||
"""Rechteckige Welt mit Wänden an allen vier Seiten.
|
||
|
||
Koordinaten: Ursprung oben links, x nach rechts, y nach unten, Einheit px. Die Welt
|
||
kennt keinen Bildschirm – wo sie gezeichnet wird, entscheidet die Anwendung.
|
||
|
||
Ein Aufruf von :meth:`step` rückt die Simulation um einen festen Zeitschritt vor
|
||
(semi-impliziter Euler mit Kontaktauflösung, Details in :meth:`step`).
|
||
|
||
Args:
|
||
width: Breite der Welt in px.
|
||
height: Höhe der Welt in px.
|
||
config: Physikparameter.
|
||
|
||
Attributes:
|
||
width: Breite der Welt in px.
|
||
height: Höhe der Welt in px.
|
||
config: Physikparameter.
|
||
bodies: Alle dynamischen Körper in Einfügereihenfolge (später eingefügte liegen beim
|
||
Zeichnen oben).
|
||
walls: Die vier statischen Wände (links, rechts, oben, unten).
|
||
contacts: Die Kontakte des letzten Schritts (für die Debug-Anzeige).
|
||
step_count: Anzahl bisher ausgeführter Schritte.
|
||
"""
|
||
|
||
def __init__(self, width: float, height: float, config: PhysicsConfig) -> None:
|
||
if not (width > 0.0 and height > 0.0):
|
||
raise ValueError(f"Die Welt braucht eine positive Größe: {width} x {height}")
|
||
self.width = width
|
||
self.height = height
|
||
self.config = config
|
||
self.bodies: list[Body] = []
|
||
self.contacts: list[Contact] = []
|
||
self.step_count = 0
|
||
self._gravity_enabled = True
|
||
self._solver = ContactSolver(config)
|
||
|
||
wall_material = Material(
|
||
density=0.0, restitution=config.wall_restitution, friction=config.wall_friction
|
||
)
|
||
# Jede Wandnormale zeigt in die Welt hinein; frei ist die Seite normal·p ≥ offset.
|
||
self.walls: list[Body] = [
|
||
Body.wall(Vector2(1.0, 0.0), 0.0, wall_material), # links: x ≥ 0
|
||
Body.wall(Vector2(-1.0, 0.0), -width, wall_material), # rechts: x ≤ width
|
||
Body.wall(Vector2(0.0, 1.0), 0.0, wall_material), # oben: y ≥ 0
|
||
Body.wall(Vector2(0.0, -1.0), -height, wall_material), # unten: y ≤ height
|
||
]
|
||
|
||
# ------------------------------------------------------------------ Schwerkraft
|
||
|
||
@property
|
||
def gravity_enabled(self) -> bool:
|
||
"""Ob die Schwerkraft wirkt."""
|
||
return self._gravity_enabled
|
||
|
||
@gravity_enabled.setter
|
||
def gravity_enabled(self, enabled: bool) -> None:
|
||
if enabled != self._gravity_enabled:
|
||
self._gravity_enabled = enabled
|
||
# Schlafende Körper würden die Änderung sonst nicht bemerken und z. B. ohne
|
||
# Schwerkraft am Boden kleben bleiben bzw. mit Schwerkraft in der Luft hängen.
|
||
self.wake_all()
|
||
|
||
@property
|
||
def gravity(self) -> Vector2:
|
||
"""Aktuell wirkende Schwerkraftbeschleunigung (Nullvektor, wenn abgeschaltet)."""
|
||
if not self._gravity_enabled:
|
||
return ZERO
|
||
return Vector2(*self.config.gravity)
|
||
|
||
# ------------------------------------------------------------------ Körperverwaltung
|
||
|
||
def add_body(self, body: Body) -> Body:
|
||
"""Fügt einen dynamischen Körper hinzu und gibt ihn zurück."""
|
||
if body.is_static:
|
||
raise ValueError("Statische Körper werden von der Welt selbst verwaltet (Wände)")
|
||
self.bodies.append(body)
|
||
return body
|
||
|
||
def remove_body(self, body: Body) -> None:
|
||
"""Entfernt einen Körper und weckt alles, was auf ihm gelegen haben könnte.
|
||
|
||
Dafür genügt es, seine Schlaf-Insel zu wecken: Schlafende Körper, die einander
|
||
berühren, sind immer gemeinsam eingeschlafen und gehören damit zur selben Insel.
|
||
"""
|
||
self.bodies.remove(body)
|
||
body.wake()
|
||
|
||
def clear(self) -> None:
|
||
"""Entfernt alle Körper."""
|
||
self.bodies.clear()
|
||
self.contacts = []
|
||
self._solver.reset()
|
||
|
||
def wake_all(self) -> None:
|
||
"""Weckt alle Körper auf."""
|
||
for body in self.bodies:
|
||
body.wake()
|
||
|
||
def body_at(self, point: Vector2, tolerance: float = 0.0) -> Body | None:
|
||
"""Oberster Körper an einem Punkt (später eingefügte Körper liegen oben).
|
||
|
||
Args:
|
||
point: Punkt in Weltkoordinaten.
|
||
tolerance: Vergrößert die Körper beim Test, damit kleine Objekte leichter zu
|
||
treffen sind.
|
||
"""
|
||
for body in reversed(self.bodies):
|
||
if body.contains_point(point, tolerance):
|
||
return body
|
||
return None
|
||
|
||
def clamp_inside(self, position: Vector2, half_extents: tuple[float, float]) -> Vector2:
|
||
"""Verschiebt eine Mittelpunktsposition so, dass ein Körper der Größe ganz hineinpasst."""
|
||
half_x, half_y = half_extents
|
||
x = min(max(position.x, half_x), self.width - half_x)
|
||
y = min(max(position.y, half_y), self.height - half_y)
|
||
if x == position.x and y == position.y:
|
||
return position
|
||
return Vector2(x, y)
|
||
|
||
# ------------------------------------------------------------------ Simulation
|
||
|
||
def step(self, dt: float) -> None:
|
||
"""Rückt die Simulation um ``dt`` Sekunden vor.
|
||
|
||
Ablauf (semi-impliziter Euler mit Kontaktauflösung):
|
||
|
||
1. Zustand merken: Position und Winkel für die Zeichen-Interpolation, die
|
||
Geschwindigkeit für die Aufprallgeschwindigkeit beim Abprallen.
|
||
2. Geschwindigkeiten integrieren: ``v += g·dt`` (plus Dämpfung).
|
||
3. Kontakte suchen: Sweep-and-Prune, dann genaue Tests; schlafende Körper, die
|
||
berührt werden, samt ihrer Schlaf-Insel wecken; danach Wandkontakte der wachen
|
||
Körper.
|
||
4. Impulse lösen (Restitution, Reibung, Rollwiderstand) und Geschwindigkeiten
|
||
begrenzen.
|
||
5. Positionen integrieren: ``x += v·dt`` – mit der *neuen* Geschwindigkeit. Genau
|
||
das macht den Euler "semi-implizit" (symplektisch): Anders als beim expliziten
|
||
Euler wächst die Energie dabei nicht systematisch an, Umlaufbahnen und
|
||
Schwingungen bleiben stabil.
|
||
6. Verbliebene Durchdringungen über die Positionen abbauen.
|
||
7. Notbremse: kein Körper ragt weiter als ``max_wall_penetration`` in eine Wand.
|
||
8. Schlafmodus aktualisieren.
|
||
"""
|
||
awake = [body for body in self.bodies if not body.sleeping]
|
||
self._remember_state(awake)
|
||
|
||
self._integrate_velocities(awake, dt)
|
||
|
||
contacts = self._find_body_contacts()
|
||
woken = wake_touched_bodies(contacts)
|
||
self._remember_state(woken)
|
||
awake.extend(woken)
|
||
contacts.extend(self._find_wall_contacts(awake))
|
||
|
||
self._solver.solve_velocities(contacts)
|
||
self._clamp_velocities(awake)
|
||
self._integrate_positions(awake, dt)
|
||
self._solver.correct_positions()
|
||
self._enforce_bounds(awake)
|
||
|
||
if self.config.sleep_enabled:
|
||
update_sleep(awake, contacts, dt, self.config)
|
||
self.contacts = contacts
|
||
self.step_count += 1
|
||
|
||
@staticmethod
|
||
def _remember_state(bodies: list[Body]) -> None:
|
||
"""Merkt sich den Zustand zu Beginn des Schritts (siehe ``Body.previous_*``)."""
|
||
for body in bodies:
|
||
body.previous_position = body.position
|
||
body.previous_angle = body.angle
|
||
body.previous_velocity = body.velocity
|
||
|
||
def _integrate_velocities(self, bodies: list[Body], dt: float) -> None:
|
||
"""Wendet Schwerkraft und Dämpfung auf die Geschwindigkeiten an."""
|
||
gravity = self.gravity
|
||
gravity_dx = gravity.x * dt
|
||
gravity_dy = gravity.y * dt
|
||
# Dämpfung als v ← v / (1 + c·dt): Näherung von v·e^(−c·dt), die anders als
|
||
# v·(1 − c·dt) auch bei großem c·dt nie das Vorzeichen umkehrt.
|
||
linear_factor = 1.0 / (1.0 + dt * self.config.linear_damping)
|
||
angular_factor = 1.0 / (1.0 + dt * self.config.angular_damping)
|
||
for body in bodies:
|
||
if body.held:
|
||
continue # Die Geschwindigkeit gehaltener Körper gibt die Maus vor.
|
||
velocity = body.velocity
|
||
body.velocity = Vector2(
|
||
(velocity.x + gravity_dx) * linear_factor,
|
||
(velocity.y + gravity_dy) * linear_factor,
|
||
)
|
||
body.angular_velocity *= angular_factor
|
||
|
||
def _find_body_contacts(self) -> list[Contact]:
|
||
"""Kontakte zwischen dynamischen Körpern (grobe Vorauswahl, dann genauer Test)."""
|
||
contacts: list[Contact] = []
|
||
for body_a, body_b in find_candidate_pairs(self.bodies):
|
||
contact = collide(body_a, body_b)
|
||
if contact is not None:
|
||
contacts.append(contact)
|
||
return contacts
|
||
|
||
def _find_wall_contacts(self, bodies: list[Body]) -> list[Contact]:
|
||
"""Kontakte der wachen Körper mit den Wänden."""
|
||
left, right, top, bottom = self.walls
|
||
width = self.width
|
||
height = self.height
|
||
contacts: list[Contact] = []
|
||
for body in bodies:
|
||
min_x, min_y, max_x, max_y = body.bounds()
|
||
# Billiger Vortest über die umschließende Box, genauer Test nur bei Bedarf.
|
||
for touches, wall in (
|
||
(min_x <= 0.0, left),
|
||
(max_x >= width, right),
|
||
(min_y <= 0.0, top),
|
||
(max_y >= height, bottom),
|
||
):
|
||
if touches:
|
||
contact = collide(body, wall)
|
||
if contact is not None:
|
||
contacts.append(contact)
|
||
return contacts
|
||
|
||
def _clamp_velocities(self, bodies: list[Body]) -> None:
|
||
"""Begrenzt Geschwindigkeit und Drehung (Schutz vor Tunneln und Explosionen)."""
|
||
max_speed = self.config.max_speed
|
||
max_speed_sq = max_speed * max_speed
|
||
max_angular = self.config.max_angular_speed
|
||
for body in bodies:
|
||
velocity = body.velocity
|
||
speed_sq = velocity.length_squared()
|
||
if speed_sq > max_speed_sq:
|
||
body.velocity = velocity * (max_speed / math.sqrt(speed_sq))
|
||
if abs(body.angular_velocity) > max_angular:
|
||
body.angular_velocity = math.copysign(max_angular, body.angular_velocity)
|
||
|
||
def _integrate_positions(self, bodies: list[Body], dt: float) -> None:
|
||
"""``x += v·dt`` und ``θ += ω·dt`` mit den bereits aktualisierten Geschwindigkeiten."""
|
||
full_turn = math.tau
|
||
for body in bodies:
|
||
velocity = body.velocity
|
||
position = body.position
|
||
body.position = Vector2(position.x + velocity.x * dt, position.y + velocity.y * dt)
|
||
if body.angular_velocity != 0.0:
|
||
# Winkel im Bereich [−π, π] halten, damit er über lange Zeit nicht an
|
||
# Genauigkeit verliert.
|
||
body.angle = math.remainder(body.angle + body.angular_velocity * dt, full_turn)
|
||
|
||
def _enforce_bounds(self, bodies: list[Body]) -> None:
|
||
"""Notbremse: Kein Körper ragt weiter als ``max_wall_penetration`` in eine Wand.
|
||
|
||
Im Normalfall halten die Wandkontakte die Körper in der Welt. Die Grenze liegt über
|
||
der größten Eindringtiefe eines frei fliegenden Körpers pro Schritt und greift daher
|
||
nur in Extremfällen, etwa wenn ein Körper von einem gezogenen Objekt gegen die Wand
|
||
gequetscht wird. Geschwindigkeiten bleiben unangetastet: Den eigentlichen Stoß mit
|
||
der Wand löst im nächsten Schritt der Solver.
|
||
"""
|
||
allowed = self.config.max_wall_penetration
|
||
for body in bodies:
|
||
half_x, half_y = body.half_extents()
|
||
position = body.position
|
||
x = min(max(position.x, half_x - allowed), self.width - half_x + allowed)
|
||
y = min(max(position.y, half_y - allowed), self.height - half_y + allowed)
|
||
if x != position.x or y != position.y:
|
||
body.position = Vector2(x, y)
|