"""Physics engine — rigid body dynamics, collision response, joints, raycasting.
Pure-Python physics built on numpy. Integrates with the node lifecycle via
``physics_process(dt)`` and the existing collision shapes from ``simvx.core.collision``.
Architecture:
PhysicsServer (singleton) manages bodies and steps the simulation.
RigidBody2D/3D nodes register themselves with the server on ``enter_tree``
and unregister on ``exit_tree``. The SceneTree's physics tick calls
``physics_process`` on all nodes, which triggers force integration.
"""
from __future__ import annotations
from __future__ import annotations
import logging
import math
from dataclasses import dataclass, field
from enum import IntEnum
from typing import Any
import numpy as np
from ..collision import (
BoxShape,
CollisionShape,
RayHit,
SpatialHashGrid,
SphereShape,
_aabb_overlap,
_gjk_intersect,
)
from ..nodes_2d.node2d import Node2D
from ..nodes_3d.node3d import Node3D
from ..descriptors import Property, Signal
from ..math.types import Quat, Vec2, Vec3
log = logging.getLogger(__name__)
__all__ = [
"PhysicsMaterial",
"BodyMode",
"PhysicsServer",
"RigidBody2D",
"RigidBody3D",
"StaticBody2D",
"StaticBody3D",
"KinematicBody2D",
"KinematicBody3D",
"Joint2D",
"Joint3D",
"PinJoint2D",
"PinJoint3D",
"HingeJoint3D",
]
# ============================================================================
# PhysicsMaterial
# ============================================================================
[docs]
@dataclass(slots=True)
class PhysicsMaterial:
"""Surface properties for physics bodies.
Attributes:
friction: Coefficient of friction [0..1]. Higher = more grip.
restitution: Coefficient of restitution [0..1]. 1 = perfectly elastic bounce.
density: Mass per unit volume. Used for auto-computing mass from shape volume.
"""
friction: float = 0.5
restitution: float = 0.3
density: float = 1.0
# ============================================================================
# BodyMode
# ============================================================================
[docs]
class BodyMode(IntEnum):
"""Physics body simulation mode."""
DYNAMIC = 0 # Fully simulated — affected by forces and collisions
KINEMATIC = 1 # Moved by code only — not affected by forces, but participates in collisions
STATIC = 2 # Never moves — infinite mass, zero velocity
# ============================================================================
# Contact — Collision contact point
# ============================================================================
# ============================================================================
# Internal body record for PhysicsServer
# ============================================================================
def _quat_to_matrix(q) -> np.ndarray:
"""Convert a quaternion to a 3x3 rotation matrix."""
x, y, z, w = float(q.x), float(q.y), float(q.z), float(q.w)
return np.array([
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
], dtype=np.float32)
@dataclass
class _PhysicsBody:
"""Internal bookkeeping for a registered physics body."""
node: Any # RigidBody2D or RigidBody3D
shape: CollisionShape | None = None
position: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float32))
rotation_matrix: np.ndarray = field(default_factory=lambda: np.eye(3, dtype=np.float32))
aabb_min: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float32))
aabb_max: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float32))
def update_aabb(self):
if not self.shape:
return
he = getattr(self.shape, 'half_extents', None)
if he is not None and not np.allclose(self.rotation_matrix, np.eye(3)):
# Rotated box: compute world-space AABB from oriented extents
abs_rot = np.abs(self.rotation_matrix)
world_he = abs_rot @ np.asarray(he, dtype=np.float32)
self.aabb_min = self.position - world_he
self.aabb_max = self.position + world_he
else:
self.aabb_min, self.aabb_max = self.shape.get_aabb(self.position)
# ============================================================================
# PhysicsServer — Singleton world manager
# ============================================================================
[docs]
class PhysicsServer:
"""Central physics simulation manager.
Manages all physics bodies, runs broadphase/narrowphase collision detection,
resolves contacts with impulse-based response, and provides raycasting.
Usage:
server = PhysicsServer.get()
server.set_gravity(Vec3(0, -9.8, 0))
"""
_instance: PhysicsServer | None = None
def __init__(self, cell_size: float = 2.0):
self._bodies: dict[int, _PhysicsBody] = {}
self._joints: list = [] # Registered joint nodes
self._gravity = np.array([0.0, -9.8, 0.0], dtype=np.float32)
self._contacts: list[Contact] = []
self._prev_overlaps: set[tuple[int, int]] = set()
self._spatial_hash = SpatialHashGrid(cell_size=cell_size)
# Solver parameters
self._solver_iterations = 8
self._joint_iterations = 20 # Dedicated joint constraint iterations
self._position_correction_factor = 0.4 # Baumgarte stabilization
self._slop = 0.005 # Allowed penetration before correction
[docs]
@classmethod
def get(cls) -> PhysicsServer:
"""Return the singleton PhysicsServer, creating it if needed."""
if cls._instance is None:
cls._instance = cls()
return cls._instance
[docs]
@classmethod
def reset(cls) -> None:
"""Reset the singleton (useful for tests)."""
cls._instance = None
# ---- Configuration ----
[docs]
def set_gravity(self, gravity) -> None:
"""Set global gravity vector.
Args:
gravity: Gravity as Vec3, tuple, or ndarray. Default (0, -9.8, 0).
"""
if isinstance(gravity, Vec3):
self._gravity = np.array([gravity.x, gravity.y, gravity.z], dtype=np.float32)
else:
self._gravity = np.asarray(gravity, dtype=np.float32)
@property
def gravity(self) -> np.ndarray:
return self._gravity.copy()
# ---- Body management ----
[docs]
def add_body(self, body) -> None:
"""Register a physics body node with the server."""
bid = id(body)
if bid in self._bodies:
return
pb = _PhysicsBody(node=body)
self._sync_body(pb)
self._bodies[bid] = pb
[docs]
def remove_body(self, body) -> None:
"""Unregister a physics body from the server."""
bid = id(body)
if bid in self._bodies:
del self._bodies[bid]
# Clean up overlap tracking
self._prev_overlaps = {p for p in self._prev_overlaps if bid not in p}
[docs]
def add_joint(self, joint) -> None:
"""Register a joint constraint with the server for solving during step()."""
if joint not in self._joints:
self._joints.append(joint)
[docs]
def remove_joint(self, joint) -> None:
"""Unregister a joint constraint from the server."""
try:
self._joints.remove(joint)
except ValueError:
pass
def _sync_body(self, pb: _PhysicsBody) -> None:
"""Synchronize internal body state from the node's current transform and shape."""
node = pb.node
pos = self._get_position(node)
pb.position = pos
pb.shape = self._get_shape(node)
# Sync rotation for oriented box collision
if isinstance(node, Node3D) and hasattr(node, 'rotation') and hasattr(node.rotation, 'x'):
pb.rotation_matrix = _quat_to_matrix(node.rotation)
pb.update_aabb()
def _get_position(self, node) -> np.ndarray:
"""Extract 3D position from a 2D or 3D node."""
if isinstance(node, Node3D):
p = node.position
return np.array([p.x, p.y, p.z], dtype=np.float32)
elif isinstance(node, Node2D):
p = node.position
return np.array([p.x, p.y, 0.0], dtype=np.float32)
return np.zeros(3, dtype=np.float32)
def _set_position(self, node, pos: np.ndarray) -> None:
"""Write back a 3D position to a 2D or 3D node."""
if isinstance(node, Node3D):
node.position = Vec3(float(pos[0]), float(pos[1]), float(pos[2]))
elif isinstance(node, Node2D):
node.position = Vec2(float(pos[0]), float(pos[1]))
def _get_shape(self, node) -> CollisionShape | None:
"""Find the first collision shape attached to a body node."""
shape = getattr(node, "_physics_shape", None)
if shape is not None:
return shape
return None
def _get_velocity(self, node) -> np.ndarray:
"""Extract linear velocity as a 3D numpy array."""
v = node.linear_velocity
if isinstance(v, Vec3):
return np.array([v.x, v.y, v.z], dtype=np.float32)
elif isinstance(v, Vec2):
return np.array([v.x, v.y, 0.0], dtype=np.float32)
elif isinstance(v, tuple | list):
if len(v) == 2:
return np.array([v[0], v[1], 0.0], dtype=np.float32)
return np.array(v[:3], dtype=np.float32)
return np.asarray(v, dtype=np.float32).flatten()[:3]
def _set_velocity(self, node, vel: np.ndarray) -> None:
"""Write back linear velocity to the node."""
if isinstance(node, Node3D):
node.linear_velocity = Vec3(float(vel[0]), float(vel[1]), float(vel[2]))
elif isinstance(node, Node2D):
node.linear_velocity = Vec2(float(vel[0]), float(vel[1]))
def _get_angular_velocity(self, node) -> np.ndarray:
"""Extract angular velocity as a 3D numpy array."""
v = node.angular_velocity
if isinstance(v, Vec3):
return np.array([v.x, v.y, v.z], dtype=np.float32)
elif isinstance(v, int | float):
return np.array([0.0, 0.0, float(v)], dtype=np.float32)
elif isinstance(v, Vec2):
return np.array([0.0, 0.0, v.x], dtype=np.float32)
return np.asarray(v, dtype=np.float32).flatten()[:3]
def _set_angular_velocity(self, node, av: np.ndarray) -> None:
"""Write back angular velocity to the node."""
if isinstance(node, Node3D):
node.angular_velocity = Vec3(float(av[0]), float(av[1]), float(av[2]))
elif isinstance(node, Node2D):
node.angular_velocity = float(av[2])
# ---- Simulation step ----
[docs]
def step(self, dt: float) -> None:
"""Advance the physics simulation by dt seconds.
This performs:
1. Sync positions from nodes
2. Apply gravity and forces → integrate velocities
3. Broadphase + narrowphase collision detection
4. Iterative impulse-based collision response
5. Integrate positions
6. Write results back to nodes
7. Emit body_entered/body_exited signals
"""
if dt <= 0 or not self._bodies:
return
bodies = list(self._bodies.values())
# 1. Sync positions from nodes
for pb in bodies:
self._sync_body(pb)
# 2. Apply gravity and integrate velocities for dynamic bodies
for pb in bodies:
node = pb.node
if node.mode != BodyMode.DYNAMIC:
continue
vel = self._get_velocity(node)
av = self._get_angular_velocity(node)
inv_mass = 1.0 / node.mass if node.mass > 0 else 0.0
# Gravity — flip Y for 2D nodes (screen space Y-down vs world Y-up)
gravity = self._gravity.copy()
if isinstance(node, Node2D):
gravity[1] = -gravity[1]
vel += gravity * node.gravity_scale * dt
# Accumulated force
force = np.asarray(node._accumulated_force, dtype=np.float32)
vel += force * inv_mass * dt
node._accumulated_force = np.zeros(3, dtype=np.float32)
# Accumulated torque
torque = np.asarray(node._accumulated_torque, dtype=np.float32)
# Simplified moment of inertia (treat as unit sphere)
inertia = node.mass * 0.4 # 2/5 * m * r^2 with r=1
if inertia > 0:
av += torque / inertia * dt
node._accumulated_torque = np.zeros(3, dtype=np.float32)
# Damping
vel *= max(0.0, 1.0 - node.linear_damp * dt)
av *= max(0.0, 1.0 - node.angular_damp * dt)
self._set_velocity(node, vel)
self._set_angular_velocity(node, av)
# 3. Collision detection
self._contacts.clear()
self._detect_collisions(bodies)
# 4. Solve collision constraints (impulse-based)
for _ in range(self._solver_iterations):
for contact in self._contacts:
self._solve_contact(contact, dt)
# 4b. Solve joint constraints (velocity-level + position correction)
for _ in range(self._joint_iterations):
for joint in self._joints:
if joint.body_a and joint.body_b:
joint._solve_constraint(dt)
# 5. Integrate positions
for pb in bodies:
node = pb.node
if node.mode == BodyMode.STATIC:
continue
if node.mode == BodyMode.KINEMATIC:
# Kinematic bodies move by their velocity but aren't affected by forces
vel = self._get_velocity(node)
pb.position += vel * dt
self._set_position(node, pb.position)
pb.update_aabb()
continue
# Dynamic bodies
vel = self._get_velocity(node)
pb.position += vel * dt
self._set_position(node, pb.position)
# Integrate rotation (simplified — axis-angle from angular velocity)
av = self._get_angular_velocity(node)
if isinstance(node, Node3D):
av_mag = float(np.linalg.norm(av))
if av_mag > 1e-8:
axis = av / av_mag
angle = av_mag * dt
node.rotation = node.rotation.rotate((float(axis[0]), float(axis[1]), float(axis[2])), angle)
elif isinstance(node, Node2D):
node.rotation += float(av[2]) * dt
pb.update_aabb()
# 5b. Post-integration joint position correction (snap bodies back to constraint manifold)
for joint in self._joints:
if joint.body_a and joint.body_b:
joint._position_correct(dt)
# 6. Position correction (push apart overlapping bodies)
for contact in self._contacts:
self._correct_position(contact)
# 7. Emit enter/exit signals
self._emit_overlap_signals()
# ---- Collision detection ----
def _detect_collisions(self, bodies: list[_PhysicsBody]) -> None:
"""Broadphase (spatial-hash) + narrowphase collision detection.
Each step: clear the grid, insert every body's AABB keyed by its list
index, then for each body query neighbouring cells and run narrowphase
on candidate pairs where ``j > i`` (dedupes pairs — each processed once).
"""
grid = self._spatial_hash
grid.clear()
for i, pb in enumerate(bodies):
if pb.shape is not None:
grid.insert(i, pb.aabb_min, pb.aabb_max)
for i, a in enumerate(bodies):
if a.shape is None:
continue
for j in grid.query(a.aabb_min, a.aabb_max):
if j <= i:
continue # skip self and already-processed pairs
b = bodies[j]
# Skip static-static pairs
if a.node.mode == BodyMode.STATIC and b.node.mode == BodyMode.STATIC:
continue
# Grid cells are loose; still need the precise AABB test
if not _aabb_overlap(a.aabb_min, a.aabb_max, b.aabb_min, b.aabb_max):
continue
contact = self._narrow_phase(a, b)
if contact is not None:
self._contacts.append(contact)
def _narrow_phase(self, a: _PhysicsBody, b: _PhysicsBody) -> Contact | None:
"""Compute contact data for two overlapping bodies.
Uses analytical solutions for sphere-sphere, sphere-box, and box-box.
Falls back to GJK for generic convex shapes.
"""
sa, sb = a.shape, b.shape
# Sphere vs Sphere
if isinstance(sa, SphereShape) and isinstance(sb, SphereShape):
return self._sphere_sphere(a, b)
# Sphere vs Box (order matters)
if isinstance(sa, SphereShape) and isinstance(sb, BoxShape):
return self._sphere_box(a, b, flip=False)
if isinstance(sa, BoxShape) and isinstance(sb, SphereShape):
return self._sphere_box(b, a, flip=True)
# Box vs Box
if isinstance(sa, BoxShape) and isinstance(sb, BoxShape):
return self._box_box(a, b)
# Generic GJK fallback
if _gjk_intersect(sa, a.position, sb, b.position):
# Approximate contact — direction between centers
diff = b.position - a.position
dist = float(np.linalg.norm(diff))
if dist < 1e-10:
normal = np.array([0.0, 1.0, 0.0], dtype=np.float32)
else:
normal = diff / dist
point = (a.position + b.position) * 0.5
return Contact(body_a=a.node, body_b=b.node, normal=normal, point=point, depth=0.01)
return None
def _sphere_sphere(self, a: _PhysicsBody, b: _PhysicsBody) -> Contact | None:
"""Analytical sphere-sphere collision."""
diff = b.position - a.position
dist = float(np.linalg.norm(diff))
ra, rb = a.shape.radius, b.shape.radius
overlap = ra + rb - dist
if overlap <= 0:
return None
if dist < 1e-10:
normal = np.array([0.0, 1.0, 0.0], dtype=np.float32)
else:
normal = diff / dist
point = a.position + normal * ra
return Contact(body_a=a.node, body_b=b.node, normal=normal, point=point, depth=overlap)
def _sphere_box(self, sphere_pb: _PhysicsBody, box_pb: _PhysicsBody, flip: bool) -> Contact | None:
"""Analytical sphere-box collision with oriented box support."""
radius = sphere_pb.shape.radius
he = np.asarray(box_pb.shape.half_extents, dtype=np.float32)
rot = box_pb.rotation_matrix # 3x3 rotation matrix
rot_t = rot.T # inverse rotation (transpose of orthonormal matrix)
# Transform sphere center into box's local coordinate system
diff_world = sphere_pb.position - box_pb.position
diff_local = rot_t @ diff_world
# Closest point on axis-aligned box in local space
clamped = np.clip(diff_local, -he, he)
to_sphere_local = diff_local - clamped
dist = float(np.linalg.norm(to_sphere_local))
if dist > radius:
return None
overlap = radius - dist
if dist < 1e-10:
# Sphere center is inside box — push along smallest penetration axis
abs_diff = np.abs(diff_local)
axis = int(np.argmin(he - abs_diff))
normal_local = np.zeros(3, dtype=np.float32)
normal_local[axis] = 1.0 if diff_local[axis] >= 0 else -1.0
overlap = float(he[axis] - abs_diff[axis]) + radius
else:
normal_local = to_sphere_local / dist
# Transform normal and closest point back to world space
normal = rot @ normal_local
closest = box_pb.position + rot @ clamped
# Normal currently points from box surface toward sphere. The solver
# convention (matching sphere-sphere/box-box) expects normal from body_a
# toward body_b, so negate when sphere is body_a and keep when box is body_a.
if flip:
a_node, b_node = box_pb.node, sphere_pb.node
else:
a_node, b_node = sphere_pb.node, box_pb.node
normal = -normal
return Contact(body_a=a_node, body_b=b_node, normal=normal, point=closest, depth=overlap)
def _box_box(self, a: _PhysicsBody, b: _PhysicsBody) -> Contact | None:
"""AABB box-box collision using separating axis test."""
he_a = a.shape.half_extents
he_b = b.shape.half_extents
diff = b.position - a.position
overlap = (he_a + he_b) - np.abs(diff)
if np.any(overlap <= 0):
return None
# Minimum penetration axis
axis = int(np.argmin(overlap))
depth = float(overlap[axis])
normal = np.zeros(3, dtype=np.float32)
normal[axis] = 1.0 if diff[axis] >= 0 else -1.0
point = (a.position + b.position) * 0.5
return Contact(body_a=a.node, body_b=b.node, normal=normal, point=point, depth=depth)
# ---- Impulse solver ----
def _solve_contact(self, contact: Contact, dt: float) -> None:
"""Apply impulse-based collision response for a single contact."""
a, b = contact.body_a, contact.body_b
normal = contact.normal
inv_mass_a = (1.0 / a.mass) if a.mode == BodyMode.DYNAMIC and a.mass > 0 else 0.0
inv_mass_b = (1.0 / b.mass) if b.mode == BodyMode.DYNAMIC and b.mass > 0 else 0.0
if inv_mass_a == 0 and inv_mass_b == 0:
return # Both immovable
vel_a = self._get_velocity(a)
vel_b = self._get_velocity(b)
relative_vel = vel_b - vel_a
vel_along_normal = float(np.dot(relative_vel, normal))
# Don't resolve if velocities are separating
if vel_along_normal > 0:
return
# Restitution — use minimum of the two materials
mat_a = a.physics_material if hasattr(a, "physics_material") and a.physics_material else PhysicsMaterial()
mat_b = b.physics_material if hasattr(b, "physics_material") and b.physics_material else PhysicsMaterial()
restitution = min(mat_a.restitution, mat_b.restitution)
# Normal impulse magnitude
j = -(1.0 + restitution) * vel_along_normal
j /= inv_mass_a + inv_mass_b
impulse = normal * j
if a.mode == BodyMode.DYNAMIC:
vel_a -= impulse * inv_mass_a
self._set_velocity(a, vel_a)
if b.mode == BodyMode.DYNAMIC:
vel_b += impulse * inv_mass_b
self._set_velocity(b, vel_b)
# Friction impulse (tangential)
relative_vel = vel_b - vel_a # Recompute after normal impulse
tangent = relative_vel - normal * float(np.dot(relative_vel, normal))
t_len = float(np.linalg.norm(tangent))
if t_len < 1e-10:
return
tangent /= t_len
friction = math.sqrt(mat_a.friction * mat_b.friction) # Geometric mean
jt = -float(np.dot(relative_vel, tangent))
jt /= inv_mass_a + inv_mass_b
# Coulomb friction clamp
if abs(jt) > abs(j) * friction:
jt = abs(j) * friction * (1.0 if jt > 0 else -1.0)
friction_impulse = tangent * jt
if a.mode == BodyMode.DYNAMIC:
self._set_velocity(a, self._get_velocity(a) - friction_impulse * inv_mass_a)
if b.mode == BodyMode.DYNAMIC:
self._set_velocity(b, self._get_velocity(b) + friction_impulse * inv_mass_b)
def _correct_position(self, contact: Contact) -> None:
"""Baumgarte position correction to prevent sinking."""
a, b = contact.body_a, contact.body_b
inv_mass_a = (1.0 / a.mass) if a.mode == BodyMode.DYNAMIC and a.mass > 0 else 0.0
inv_mass_b = (1.0 / b.mass) if b.mode == BodyMode.DYNAMIC and b.mass > 0 else 0.0
total_inv = inv_mass_a + inv_mass_b
if total_inv == 0:
return
correction_mag = max(contact.depth - self._slop, 0.0) * self._position_correction_factor / total_inv
correction = contact.normal * correction_mag
if a.mode == BodyMode.DYNAMIC:
pb_a = self._bodies.get(id(a))
if pb_a:
pb_a.position -= correction * inv_mass_a
self._set_position(a, pb_a.position)
pb_a.update_aabb()
if b.mode == BodyMode.DYNAMIC:
pb_b = self._bodies.get(id(b))
if pb_b:
pb_b.position += correction * inv_mass_b
self._set_position(b, pb_b.position)
pb_b.update_aabb()
# ---- Overlap signals ----
def _emit_overlap_signals(self) -> None:
"""Emit body_entered/body_exited signals based on current contacts."""
current_overlaps: set[tuple[int, int]] = set()
for contact in self._contacts:
pair = (id(contact.body_a), id(contact.body_b))
pair = (min(pair), max(pair)) # Canonical order
current_overlaps.add(pair)
# New overlaps → body_entered
for pair in current_overlaps - self._prev_overlaps:
a_id, b_id = pair
a_pb = self._bodies.get(a_id)
b_pb = self._bodies.get(b_id)
if a_pb and b_pb:
if hasattr(a_pb.node, "body_entered"):
a_pb.node.body_entered(b_pb.node)
if hasattr(b_pb.node, "body_entered"):
b_pb.node.body_entered(a_pb.node)
# Lost overlaps → body_exited
for pair in self._prev_overlaps - current_overlaps:
a_id, b_id = pair
a_pb = self._bodies.get(a_id)
b_pb = self._bodies.get(b_id)
if a_pb and b_pb:
if hasattr(a_pb.node, "body_exited"):
a_pb.node.body_exited(b_pb.node)
if hasattr(b_pb.node, "body_exited"):
b_pb.node.body_exited(a_pb.node)
self._prev_overlaps = current_overlaps
# ---- Queries ----
[docs]
def raycast(
self,
origin,
direction,
max_dist: float = 1000.0,
layer_mask: int = 0xFFFFFFFF,
) -> list[RayHit]:
"""Cast a ray through the physics world.
Args:
origin: Ray start position (Vec3, tuple, or ndarray).
direction: Ray direction (will be normalized).
max_dist: Maximum ray length.
layer_mask: Bitmask to filter bodies by collision layer.
Returns:
List of RayHit sorted by distance.
"""
from ..collision import _ray_aabb, _ray_sphere
if isinstance(origin, Vec2 | Vec3):
origin = np.array(list(origin), dtype=np.float32)
if len(origin) == 2:
origin = np.array([origin[0], origin[1], 0.0], dtype=np.float32)
else:
origin = np.asarray(origin, dtype=np.float32)
if isinstance(direction, Vec2 | Vec3):
direction = np.array(list(direction), dtype=np.float32)
if len(direction) == 2:
direction = np.array([direction[0], direction[1], 0.0], dtype=np.float32)
else:
direction = np.asarray(direction, dtype=np.float32)
d_len = float(np.linalg.norm(direction))
if d_len < 1e-10:
return []
direction = direction / d_len
hits: list[RayHit] = []
for pb in self._bodies.values():
node = pb.node
layer = getattr(node, "collision_layer", 1)
if not (layer_mask & layer):
continue
if pb.shape is None:
continue
# AABB pre-test
t_aabb = _ray_aabb(origin, direction, pb.aabb_min, pb.aabb_max, max_dist)
if t_aabb is None:
continue
# Shape-specific test
if isinstance(pb.shape, SphereShape):
t = _ray_sphere(origin, direction, pb.position, pb.shape.radius, max_dist)
elif isinstance(pb.shape, BoxShape):
bmin = pb.position - pb.shape.half_extents
bmax = pb.position + pb.shape.half_extents
t = _ray_aabb(origin, direction, bmin, bmax, max_dist)
else:
t = t_aabb
if t is not None:
hits.append(RayHit(body=node, point=origin + direction * t, distance=t))
hits.sort(key=lambda h: h.distance)
return hits
[docs]
def get_overlapping_bodies(self, body) -> list:
"""Return all bodies currently overlapping the given body."""
bid = id(body)
result = []
for contact in self._contacts:
if id(contact.body_a) == bid:
result.append(contact.body_b)
elif id(contact.body_b) == bid:
result.append(contact.body_a)
return result
@property
def body_count(self) -> int:
return len(self._bodies)
# ============================================================================
# _PhysicsBodyMixin — Shared logic for RigidBody2D/3D
# ============================================================================
class _PhysicsBodyMixin:
"""Mixin providing physics body behavior for Node2D/Node3D subclasses.
Not intended for direct use. Provides force accumulation, collision shape
management, and PhysicsServer registration.
"""
def _physics_body_init(self):
"""Initialize physics body state. Called from __init__."""
self._accumulated_force = np.zeros(3, dtype=np.float32)
self._accumulated_torque = np.zeros(3, dtype=np.float32)
self._physics_shape: CollisionShape | None = None
self.body_entered = Signal()
self.body_exited = Signal()
def apply_force(self, force, position=None) -> None:
"""Apply a continuous force (integrated over the next physics step).
Args:
force: Force vector (Vec2/Vec3/tuple).
position: Optional application point for computing torque.
If None, force is applied at the center of mass.
"""
if isinstance(force, Vec3):
f = np.array([force.x, force.y, force.z], dtype=np.float32)
elif isinstance(force, Vec2):
f = np.array([force.x, force.y, 0.0], dtype=np.float32)
else:
f = np.asarray(force, dtype=np.float32)
if f.shape[0] == 2:
f = np.array([f[0], f[1], 0.0], dtype=np.float32)
self._accumulated_force += f
if position is not None:
# Compute torque from off-center force application
if isinstance(position, Vec3):
r = np.array([position.x, position.y, position.z], dtype=np.float32)
elif isinstance(position, Vec2):
r = np.array([position.x, position.y, 0.0], dtype=np.float32)
else:
r = np.asarray(position, dtype=np.float32)
if r.shape[0] == 2:
r = np.array([r[0], r[1], 0.0], dtype=np.float32)
self._accumulated_torque += np.cross(r, f)
def apply_impulse(self, impulse, position=None) -> None:
"""Apply an instantaneous impulse (immediately changes velocity).
Args:
impulse: Impulse vector (Vec2/Vec3/tuple).
position: Optional application point for computing angular impulse.
"""
if isinstance(impulse, Vec3):
imp = np.array([impulse.x, impulse.y, impulse.z], dtype=np.float32)
elif isinstance(impulse, Vec2):
imp = np.array([impulse.x, impulse.y, 0.0], dtype=np.float32)
else:
imp = np.asarray(impulse, dtype=np.float32)
if imp.shape[0] == 2:
imp = np.array([imp[0], imp[1], 0.0], dtype=np.float32)
if self.mass > 0:
vel = imp / self.mass
if isinstance(self, Node3D):
lv = self.linear_velocity
self.linear_velocity = Vec3(lv.x + vel[0], lv.y + vel[1], lv.z + vel[2])
elif isinstance(self, Node2D):
lv = self.linear_velocity
self.linear_velocity = Vec2(lv.x + vel[0], lv.y + vel[1])
if position is not None and self.mass > 0:
if isinstance(position, Vec3):
r = np.array([position.x, position.y, position.z], dtype=np.float32)
elif isinstance(position, Vec2):
r = np.array([position.x, position.y, 0.0], dtype=np.float32)
else:
r = np.asarray(position, dtype=np.float32)
if r.shape[0] == 2:
r = np.array([r[0], r[1], 0.0], dtype=np.float32)
inertia = self.mass * 0.4
angular_impulse = np.cross(r, imp)
if inertia > 0:
dav = angular_impulse / inertia
if isinstance(self, Node3D):
av = self.angular_velocity
self.angular_velocity = Vec3(av.x + dav[0], av.y + dav[1], av.z + dav[2])
elif isinstance(self, Node2D):
self.angular_velocity += float(dav[2])
def apply_torque(self, torque) -> None:
"""Apply a continuous torque (integrated over the next physics step).
Args:
torque: Torque vector (Vec3/float for 2D).
"""
if isinstance(torque, int | float):
self._accumulated_torque[2] += torque
elif isinstance(torque, Vec3):
self._accumulated_torque += np.array([torque.x, torque.y, torque.z], dtype=np.float32)
elif isinstance(torque, Vec2):
self._accumulated_torque[2] += torque.x
else:
self._accumulated_torque += np.asarray(torque, dtype=np.float32)
def set_collision_shape(self, shape: CollisionShape) -> None:
"""Set the collision shape used by the physics engine.
Args:
shape: A CollisionShape (SphereShape, BoxShape, ConvexShape).
"""
self._physics_shape = shape
def _register_with_server(self) -> None:
"""Register this body with the PhysicsServer."""
PhysicsServer.get().add_body(self)
def _unregister_from_server(self) -> None:
"""Unregister this body from the PhysicsServer."""
PhysicsServer.get().remove_body(self)
# ============================================================================
# RigidBody3D
# ============================================================================
[docs]
class RigidBody3D(_PhysicsBodyMixin, Node3D):
"""3D rigid body with full physics simulation.
Supports dynamic, kinematic, and static modes. Collision shapes are set
via ``set_collision_shape()`` or auto-detected from child nodes.
Signals:
body_entered: Emitted when another body starts overlapping.
body_exited: Emitted when a previously overlapping body separates.
Example:
class Ball(RigidBody3D):
mass = Property(2.0)
physics_material = PhysicsMaterial(restitution=0.8)
def ready(self):
self.set_collision_shape(SphereShape(radius=0.5))
"""
mass = Property(1.0, range=(0.001, 100000), hint="Body mass in kg", group="Physics")
gravity_scale = Property(1.0, range=(-10, 10), hint="Gravity multiplier", group="Physics")
linear_damp = Property(0.0, range=(0, 100), hint="Linear velocity damping", group="Physics")
angular_damp = Property(0.0, range=(0, 100), hint="Angular velocity damping", group="Physics")
collision_layer = Property(1, hint="Collision layer bitmask", group="Collision")
collision_mask = Property(1, hint="Collision mask bitmask", group="Collision")
def __init__(self, **kwargs):
# Extract physics-specific kwargs before super().__init__
mode = kwargs.pop("mode", BodyMode.DYNAMIC)
physics_material = kwargs.pop("physics_material", None)
linear_velocity = kwargs.pop("linear_velocity", None)
angular_velocity = kwargs.pop("angular_velocity", None)
super().__init__(**kwargs)
self._physics_body_init()
self.mode: BodyMode = mode
self.physics_material: PhysicsMaterial = physics_material or PhysicsMaterial()
self.linear_velocity: Vec3 = Vec3(linear_velocity) if linear_velocity else Vec3()
self.angular_velocity: Vec3 = Vec3(angular_velocity) if angular_velocity else Vec3()
[docs]
def enter_tree(self) -> None:
self._register_with_server()
[docs]
def exit_tree(self) -> None:
self._unregister_from_server()
[docs]
def physics_process(self, dt: float) -> None:
"""Called each physics tick. Override for custom behavior.
The PhysicsServer handles integration; this hook runs after forces
are accumulated but before the server step.
"""
pass
# ============================================================================
# RigidBody2D
# ============================================================================
[docs]
class RigidBody2D(_PhysicsBodyMixin, Node2D):
"""2D rigid body with full physics simulation.
Same API as RigidBody3D but works in 2D space (x, y).
Angular velocity is a scalar (rotation around the z-axis).
Example:
class Ball2D(RigidBody2D):
mass = Property(1.0)
def ready(self):
self.set_collision_shape(SphereShape(radius=10))
"""
mass = Property(1.0, range=(0.001, 100000), hint="Body mass in kg", group="Physics")
gravity_scale = Property(1.0, range=(-10, 10), hint="Gravity multiplier", group="Physics")
linear_damp = Property(0.0, range=(0, 100), hint="Linear velocity damping", group="Physics")
angular_damp = Property(0.0, range=(0, 100), hint="Angular velocity damping", group="Physics")
collision_layer = Property(1, hint="Collision layer bitmask", group="Collision")
collision_mask = Property(1, hint="Collision mask bitmask", group="Collision")
def __init__(self, **kwargs):
mode = kwargs.pop("mode", BodyMode.DYNAMIC)
physics_material = kwargs.pop("physics_material", None)
linear_velocity = kwargs.pop("linear_velocity", None)
angular_velocity = kwargs.pop("angular_velocity", None)
super().__init__(**kwargs)
self._physics_body_init()
self.mode: BodyMode = mode
self.physics_material: PhysicsMaterial = physics_material or PhysicsMaterial()
self.linear_velocity: Vec2 = Vec2(linear_velocity) if linear_velocity else Vec2()
self.angular_velocity: float = float(angular_velocity) if angular_velocity else 0.0
[docs]
def enter_tree(self) -> None:
self._register_with_server()
[docs]
def exit_tree(self) -> None:
self._unregister_from_server()
[docs]
def physics_process(self, dt: float) -> None:
pass
# ============================================================================
# StaticBody3D / StaticBody2D — Immovable bodies
# ============================================================================
[docs]
class StaticBody3D(_PhysicsBodyMixin, Node3D):
"""Immovable 3D physics body. Used for floors, walls, and obstacles.
Has infinite effective mass — never moved by forces or collisions,
but other bodies collide against it normally.
Example:
ground = StaticBody3D(position=Vec3(0, -1, 0))
ground.set_collision_shape(BoxShape(half_extents=(50, 1, 50)))
"""
collision_layer = Property(1, hint="Collision layer bitmask", group="Collision")
collision_mask = Property(1, hint="Collision mask bitmask", group="Collision")
def __init__(self, **kwargs):
physics_material = kwargs.pop("physics_material", None)
super().__init__(**kwargs)
self._physics_body_init()
self.mode: BodyMode = BodyMode.STATIC
self.mass: float = 0.0 # Infinite mass (inverse = 0)
self.physics_material: PhysicsMaterial = physics_material or PhysicsMaterial()
self.linear_velocity: Vec3 = Vec3()
self.angular_velocity: Vec3 = Vec3()
self.gravity_scale: float = 0.0
self.linear_damp: float = 0.0
self.angular_damp: float = 0.0
[docs]
def enter_tree(self) -> None:
self._register_with_server()
[docs]
def exit_tree(self) -> None:
self._unregister_from_server()
[docs]
class StaticBody2D(_PhysicsBodyMixin, Node2D):
"""Immovable 2D physics body. Used for platforms and walls.
Same behavior as StaticBody3D but in 2D space.
"""
collision_layer = Property(1, hint="Collision layer bitmask", group="Collision")
collision_mask = Property(1, hint="Collision mask bitmask", group="Collision")
def __init__(self, **kwargs):
physics_material = kwargs.pop("physics_material", None)
super().__init__(**kwargs)
self._physics_body_init()
self.mode: BodyMode = BodyMode.STATIC
self.mass: float = 0.0
self.physics_material: PhysicsMaterial = physics_material or PhysicsMaterial()
self.linear_velocity: Vec2 = Vec2()
self.angular_velocity: float = 0.0
self.gravity_scale: float = 0.0
self.linear_damp: float = 0.0
self.angular_damp: float = 0.0
[docs]
def enter_tree(self) -> None:
self._register_with_server()
[docs]
def exit_tree(self) -> None:
self._unregister_from_server()
# ============================================================================
# KinematicBody3D / KinematicBody2D — Code-driven bodies
# ============================================================================
[docs]
class KinematicBody3D(_PhysicsBodyMixin, Node3D):
"""3D body moved by code that participates in collisions.
Not affected by gravity or forces, but pushes dynamic bodies.
Use ``move_and_collide()`` for movement with collision detection.
Example:
class Platform(KinematicBody3D):
def physics_process(self, dt):
self.linear_velocity = Vec3(0, math.sin(time) * 2, 0)
"""
collision_layer = Property(1, hint="Collision layer bitmask", group="Collision")
collision_mask = Property(1, hint="Collision mask bitmask", group="Collision")
def __init__(self, **kwargs):
physics_material = kwargs.pop("physics_material", None)
linear_velocity = kwargs.pop("linear_velocity", None)
super().__init__(**kwargs)
self._physics_body_init()
self.mode: BodyMode = BodyMode.KINEMATIC
self.mass: float = 1.0 # Mass for push calculations
self.physics_material: PhysicsMaterial = physics_material or PhysicsMaterial()
self.linear_velocity: Vec3 = Vec3(linear_velocity) if linear_velocity else Vec3()
self.angular_velocity: Vec3 = Vec3()
self.gravity_scale: float = 0.0
self.linear_damp: float = 0.0
self.angular_damp: float = 0.0
[docs]
def enter_tree(self) -> None:
self._register_with_server()
[docs]
def exit_tree(self) -> None:
self._unregister_from_server()
[docs]
def move_and_collide(self, velocity, dt: float = 1.0) -> Contact | None:
"""Move by velocity*dt and return the first contact, or None."""
if isinstance(velocity, Vec3):
v = np.array([velocity.x, velocity.y, velocity.z], dtype=np.float32)
else:
v = np.asarray(velocity, dtype=np.float32)
self.position = self.position + Vec3(*(v * dt))
return None # Collision handled by PhysicsServer.step()
[docs]
def physics_process(self, dt: float) -> None:
pass
[docs]
class KinematicBody2D(_PhysicsBodyMixin, Node2D):
"""2D body moved by code that participates in collisions.
Same behavior as KinematicBody3D but in 2D space.
"""
collision_layer = Property(1, hint="Collision layer bitmask", group="Collision")
collision_mask = Property(1, hint="Collision mask bitmask", group="Collision")
def __init__(self, **kwargs):
physics_material = kwargs.pop("physics_material", None)
linear_velocity = kwargs.pop("linear_velocity", None)
super().__init__(**kwargs)
self._physics_body_init()
self.mode: BodyMode = BodyMode.KINEMATIC
self.mass: float = 1.0
self.physics_material: PhysicsMaterial = physics_material or PhysicsMaterial()
self.linear_velocity: Vec2 = Vec2(linear_velocity) if linear_velocity else Vec2()
self.angular_velocity: float = 0.0
self.gravity_scale: float = 0.0
self.linear_damp: float = 0.0
self.angular_damp: float = 0.0
[docs]
def enter_tree(self) -> None:
self._register_with_server()
[docs]
def exit_tree(self) -> None:
self._unregister_from_server()
[docs]
def move_and_collide(self, velocity, dt: float = 1.0) -> Contact | None:
"""Move by velocity*dt and return the first contact, or None."""
if isinstance(velocity, Vec2):
v = np.array([velocity.x, velocity.y], dtype=np.float32)
else:
v = np.asarray(velocity, dtype=np.float32)
self.position = self.position + Vec2(*(v * dt))
return None
[docs]
def physics_process(self, dt: float) -> None:
pass
# ============================================================================
# Joints — Constraint solving
# ============================================================================
[docs]
class Joint2D(Node2D):
"""Base class for 2D physics joints.
Connects two physics bodies with a constraint. Registers with the
PhysicsServer so constraints are solved during the physics step,
between velocity integration and position integration.
Attributes:
body_a: First connected body (set after adding to tree).
body_b: Second connected body (set after adding to tree).
"""
def __init__(self, body_a=None, body_b=None, **kwargs):
super().__init__(**kwargs)
self.body_a = body_a
self.body_b = body_b
def _solve_constraint(self, dt: float) -> None:
"""Override to implement velocity-level constraint solving."""
pass
def _position_correct(self, dt: float) -> None:
"""Override to implement post-integration position projection."""
pass
[docs]
def enter_tree(self) -> None:
PhysicsServer.get().add_joint(self)
[docs]
def exit_tree(self) -> None:
PhysicsServer.get().remove_joint(self)
[docs]
class Joint3D(Node3D):
"""Base class for 3D physics joints.
Connects two physics bodies with a constraint. Registers with the
PhysicsServer so constraints are solved during the physics step.
"""
def __init__(self, body_a=None, body_b=None, **kwargs):
super().__init__(**kwargs)
self.body_a = body_a
self.body_b = body_b
def _solve_constraint(self, dt: float) -> None:
"""Override to implement velocity-level constraint solving."""
pass
def _position_correct(self, dt: float) -> None:
"""Override to implement post-integration position projection."""
pass
[docs]
def enter_tree(self) -> None:
PhysicsServer.get().add_joint(self)
[docs]
def exit_tree(self) -> None:
PhysicsServer.get().remove_joint(self)
[docs]
class PinJoint2D(Joint2D):
"""Pin joint -- constrains two 2D bodies to maintain a fixed distance.
Uses velocity-level constraint solving with Baumgarte stabilization.
Solved by the PhysicsServer during its step, not in physics_process.
Attributes:
distance: Rest distance between anchors. 0 = compute from initial positions.
stiffness: Constraint stiffness [0..1]. 1 = rigid, lower = springy.
damping: Velocity damping factor for the constraint.
bias_factor: Baumgarte stabilization factor (higher = faster correction).
"""
def __init__(
self, body_a=None, body_b=None, distance: float = 0.0,
stiffness: float = 1.0, damping: float = 0.5, bias_factor: float = 0.3, **kwargs,
):
super().__init__(body_a=body_a, body_b=body_b, **kwargs)
self._rest_distance = distance
self.stiffness = stiffness
self.damping = damping
self.bias_factor = bias_factor
self._initialized = False
def _ensure_init(self):
if self._initialized or not self.body_a or not self.body_b:
return
if self._rest_distance <= 0:
diff = self.body_a.position - self.body_b.position
self._rest_distance = max(diff.length(), 1e-6)
self._initialized = True
def _solve_constraint(self, dt: float) -> None:
self._ensure_init()
if self._rest_distance <= 0:
return
pos_a, pos_b = self.body_a.position, self.body_b.position
diff = pos_b - pos_a
dist = diff.length()
if dist < 1e-10:
return
normal = diff * (1.0 / dist)
error = dist - self._rest_distance
mode_a = getattr(self.body_a, "mode", BodyMode.DYNAMIC)
mode_b = getattr(self.body_b, "mode", BodyMode.DYNAMIC)
mass_a = getattr(self.body_a, "mass", 1.0) if mode_a == BodyMode.DYNAMIC else 0.0
mass_b = getattr(self.body_b, "mass", 1.0) if mode_b == BodyMode.DYNAMIC else 0.0
inv_a = (1.0 / mass_a) if mass_a > 0 else 0.0
inv_b = (1.0 / mass_b) if mass_b > 0 else 0.0
total_inv = inv_a + inv_b
if total_inv == 0:
return
# Velocity-level constraint: correct relative velocity along constraint axis
vel_a = self.body_a.linear_velocity if hasattr(self.body_a, "linear_velocity") else Vec2()
vel_b = self.body_b.linear_velocity if hasattr(self.body_b, "linear_velocity") else Vec2()
rel_vel = Vec2(vel_b.x - vel_a.x, vel_b.y - vel_a.y)
vn = rel_vel.dot(normal)
# Baumgarte bias: feeds position error back into velocity constraint
bias = self.bias_factor * error / max(dt, 1e-6)
# Impulse magnitude: corrects velocity along normal + bias for position error
impulse_mag = -(vn + bias) * self.stiffness / total_inv
impulse = normal * impulse_mag
if mode_a == BodyMode.DYNAMIC:
self.body_a.linear_velocity = Vec2(vel_a.x - impulse.x * inv_a, vel_a.y - impulse.y * inv_a)
if mode_b == BodyMode.DYNAMIC:
self.body_b.linear_velocity = Vec2(vel_b.x + impulse.x * inv_b, vel_b.y + impulse.y * inv_b)
[docs]
class PinJoint3D(Joint3D):
"""Pin joint -- constrains two 3D bodies to maintain a fixed distance.
Uses velocity-level constraint solving with Baumgarte stabilization.
Solved by the PhysicsServer during its step.
Attributes:
distance: Rest distance. 0 = auto-compute from initial positions.
stiffness: Constraint stiffness [0..1].
damping: Velocity damping along the constraint axis.
bias_factor: Baumgarte stabilization factor.
"""
def __init__(
self, body_a=None, body_b=None, distance: float = 0.0,
stiffness: float = 1.0, damping: float = 0.5, bias_factor: float = 0.3, **kwargs,
):
super().__init__(body_a=body_a, body_b=body_b, **kwargs)
self._rest_distance = distance
self.stiffness = stiffness
self.damping = damping
self.bias_factor = bias_factor
self._initialized = False
def _ensure_init(self):
if self._initialized or not self.body_a or not self.body_b:
return
if self._rest_distance <= 0:
diff = self.body_a.position - self.body_b.position
self._rest_distance = max(diff.length(), 1e-6)
self._initialized = True
def _solve_constraint(self, dt: float) -> None:
self._ensure_init()
if self._rest_distance <= 0:
return
pos_a, pos_b = self.body_a.position, self.body_b.position
diff = pos_b - pos_a
dist = diff.length()
if dist < 1e-10:
return
normal = diff * (1.0 / dist)
error = dist - self._rest_distance
mode_a = getattr(self.body_a, "mode", BodyMode.DYNAMIC)
mode_b = getattr(self.body_b, "mode", BodyMode.DYNAMIC)
mass_a = getattr(self.body_a, "mass", 1.0) if mode_a == BodyMode.DYNAMIC else 0.0
mass_b = getattr(self.body_b, "mass", 1.0) if mode_b == BodyMode.DYNAMIC else 0.0
inv_a = (1.0 / mass_a) if mass_a > 0 else 0.0
inv_b = (1.0 / mass_b) if mass_b > 0 else 0.0
total_inv = inv_a + inv_b
if total_inv == 0:
return
# Velocity-level constraint with Baumgarte bias
vel_a = self.body_a.linear_velocity if hasattr(self.body_a, "linear_velocity") else Vec3()
vel_b = self.body_b.linear_velocity if hasattr(self.body_b, "linear_velocity") else Vec3()
rel_vel = vel_b - vel_a
vn = rel_vel.dot(normal)
bias = self.bias_factor * error / max(dt, 1e-6)
impulse_mag = -(vn + bias) * self.stiffness / total_inv
impulse = normal * impulse_mag
if mode_a == BodyMode.DYNAMIC:
self.body_a.linear_velocity = vel_a - impulse * inv_a
if mode_b == BodyMode.DYNAMIC:
self.body_b.linear_velocity = vel_b + impulse * inv_b
[docs]
class HingeJoint3D(Joint3D):
"""Hinge joint -- constrains two 3D bodies to rotate around a shared axis.
Maintains a fixed distance in the hinge plane and restricts motion to
rotation around a single axis. Two-phase solver:
1. _solve_constraint (pre-integration): velocity-level Baumgarte correction
constrains velocity to be tangential and in the hinge plane.
2. _position_correct (post-integration): hard position projection snaps
the body back to the correct distance on the constraint manifold.
Attributes:
axis: Hinge rotation axis in world space (normalized).
distance: Rest distance between body centres.
stiffness: Position constraint stiffness.
damping: Velocity damping.
bias_factor: Baumgarte stabilization factor.
angular_limit_min: Minimum angle in degrees (0 = no limit).
angular_limit_max: Maximum angle in degrees (0 = no limit).
"""
def __init__(
self, body_a=None, body_b=None, axis=None, distance: float = 0.0,
stiffness: float = 1.0, damping: float = 0.5, bias_factor: float = 0.3,
angular_limit_min: float = 0.0, angular_limit_max: float = 0.0, **kwargs,
):
super().__init__(body_a=body_a, body_b=body_b, **kwargs)
self.axis = Vec3(axis) if axis else Vec3(0, 1, 0)
self._rest_distance = distance
self.stiffness = stiffness
self.damping = damping
self.bias_factor = bias_factor
self.angular_limit_min = angular_limit_min
self.angular_limit_max = angular_limit_max
self._reference_angle: float = 0.0
self._initialized = False
def _ensure_init(self):
if self._initialized or not self.body_a or not self.body_b:
return
if self._rest_distance <= 0:
diff = self.body_a.position - self.body_b.position
self._rest_distance = max(diff.length(), 1e-6)
# Capture reference angle for angular limits
rel_q = self.body_a.rotation.inverse() * self.body_b.rotation
dot = rel_q.x * self.axis.x + rel_q.y * self.axis.y + rel_q.z * self.axis.z
self._reference_angle = 2.0 * math.atan2(dot, rel_q.w)
self._initialized = True
def _axis_n(self) -> np.ndarray:
"""Return normalized hinge axis as numpy array."""
a = np.array([self.axis.x, self.axis.y, self.axis.z], dtype=np.float32)
length = float(np.linalg.norm(a))
return a / length if length > 1e-10 else a
def _inv_masses(self):
"""Return (mode_a, mode_b, inv_a, inv_b, total_inv)."""
mode_a = getattr(self.body_a, "mode", BodyMode.DYNAMIC)
mode_b = getattr(self.body_b, "mode", BodyMode.DYNAMIC)
mass_a = getattr(self.body_a, "mass", 1.0) if mode_a == BodyMode.DYNAMIC else 0.0
mass_b = getattr(self.body_b, "mass", 1.0) if mode_b == BodyMode.DYNAMIC else 0.0
inv_a = (1.0 / mass_a) if mass_a > 0 else 0.0
inv_b = (1.0 / mass_b) if mass_b > 0 else 0.0
return mode_a, mode_b, inv_a, inv_b, inv_a + inv_b
def _solve_constraint(self, dt: float) -> None:
"""Velocity-level constraint: project velocity to be tangential and in the hinge plane."""
self._ensure_init()
mode_a, mode_b, inv_a, inv_b, total_inv = self._inv_masses()
if total_inv == 0:
return
axis_n = self._axis_n()
pos_a = np.array([self.body_a.position.x, self.body_a.position.y, self.body_a.position.z], dtype=np.float32)
pos_b = np.array([self.body_b.position.x, self.body_b.position.y, self.body_b.position.z], dtype=np.float32)
diff = pos_b - pos_a
# Lever arm in hinge plane (remove axis component)
along_axis = float(np.dot(diff, axis_n))
diff_planar = diff - axis_n * along_axis
planar_dist = float(np.linalg.norm(diff_planar))
# --- Constrain velocity of dynamic bodies ---
if mode_b == BodyMode.DYNAMIC and hasattr(self.body_b, "linear_velocity"):
vel_b = self.body_b.linear_velocity
vel_arr = np.array([vel_b.x, vel_b.y, vel_b.z], dtype=np.float32)
if planar_dist > 1e-10:
radial = diff_planar / planar_dist
# Remove radial component (keep only tangential)
vel_arr -= radial * float(np.dot(vel_arr, radial))
# Remove axis component (body stays in hinge plane)
vel_arr -= axis_n * float(np.dot(vel_arr, axis_n))
# Baumgarte bias: feed distance error back as radial velocity correction
if self._rest_distance > 0 and planar_dist > 1e-10:
dist_error = planar_dist - self._rest_distance
bias = self.bias_factor * dist_error / max(dt, 1e-6)
radial = diff_planar / planar_dist
vel_arr -= radial * bias * (inv_b / total_inv)
# Baumgarte bias for axis drift (push back towards hinge plane)
if abs(along_axis) > 1e-10:
axis_bias = self.bias_factor * along_axis / max(dt, 1e-6)
vel_arr -= axis_n * axis_bias * (inv_b / total_inv)
self.body_b.linear_velocity = Vec3(float(vel_arr[0]), float(vel_arr[1]), float(vel_arr[2]))
if mode_a == BodyMode.DYNAMIC and hasattr(self.body_a, "linear_velocity"):
vel_a = self.body_a.linear_velocity
vel_arr = np.array([vel_a.x, vel_a.y, vel_a.z], dtype=np.float32)
if planar_dist > 1e-10:
radial = diff_planar / planar_dist
vel_arr -= radial * float(np.dot(vel_arr, radial))
vel_arr -= axis_n * float(np.dot(vel_arr, axis_n))
if self._rest_distance > 0 and planar_dist > 1e-10:
dist_error = planar_dist - self._rest_distance
bias = self.bias_factor * dist_error / max(dt, 1e-6)
radial = diff_planar / planar_dist
vel_arr += radial * bias * (inv_a / total_inv)
if abs(along_axis) > 1e-10:
axis_bias = self.bias_factor * along_axis / max(dt, 1e-6)
vel_arr += axis_n * axis_bias * (inv_a / total_inv)
self.body_a.linear_velocity = Vec3(float(vel_arr[0]), float(vel_arr[1]), float(vel_arr[2]))
# --- Angular constraint: project angular velocity onto hinge axis only ---
for body, mode in [(self.body_b, mode_b), (self.body_a, mode_a)]:
if mode != BodyMode.DYNAMIC:
continue
if not hasattr(body, "angular_velocity") or not isinstance(body.angular_velocity, Vec3):
continue
av = body.angular_velocity
av_arr = np.array([av.x, av.y, av.z], dtype=np.float32)
along_av = float(np.dot(av_arr, axis_n))
body.angular_velocity = Vec3(*(axis_n * along_av))
# --- Angular limits (degrees, 0 = disabled) ---
if self.angular_limit_min != 0 or self.angular_limit_max != 0:
self._enforce_angular_limits(mode_a, mode_b, inv_a, inv_b, total_inv, axis_n)
def _position_correct(self, dt: float) -> None:
"""Post-integration position projection: snap body back to hinge constraint manifold.
This runs AFTER position integration, so it gets the final word on where
the body actually ends up. Standard practice in all real physics engines.
"""
self._ensure_init()
mode_a, mode_b, inv_a, inv_b, total_inv = self._inv_masses()
if total_inv == 0:
return
axis_n = self._axis_n()
pos_a = np.array([self.body_a.position.x, self.body_a.position.y, self.body_a.position.z], dtype=np.float32)
pos_b = np.array([self.body_b.position.x, self.body_b.position.y, self.body_b.position.z], dtype=np.float32)
diff = pos_b - pos_a
# Decompose into axis and planar components
along_axis = float(np.dot(diff, axis_n))
diff_planar = diff - axis_n * along_axis
planar_dist = float(np.linalg.norm(diff_planar))
if self._rest_distance <= 0:
return
# --- 1. Project to correct distance in hinge plane ---
if planar_dist > 1e-10:
radial = diff_planar / planar_dist
target_planar = radial * self._rest_distance
else:
# Degenerate: body exactly on axis. Pick an arbitrary direction in the plane.
perp = np.array([1.0, 0.0, 0.0], dtype=np.float32)
if abs(float(np.dot(perp, axis_n))) > 0.9:
perp = np.array([0.0, 0.0, 1.0], dtype=np.float32)
perp = perp - axis_n * float(np.dot(perp, axis_n))
perp = perp / float(np.linalg.norm(perp))
target_planar = perp * self._rest_distance
# Target position: anchor + planar offset (no axis component — stays in hinge plane)
target_b = pos_a + target_planar
correction = target_b - pos_b
# Distribute correction by inverse mass ratio
if mode_b == BodyMode.DYNAMIC:
new_b = pos_b + correction * (inv_b / total_inv)
self.body_b.position = Vec3(float(new_b[0]), float(new_b[1]), float(new_b[2]))
if mode_a == BodyMode.DYNAMIC:
new_a = pos_a - correction * (inv_a / total_inv)
self.body_a.position = Vec3(float(new_a[0]), float(new_a[1]), float(new_a[2]))
# --- 2. Set body rotation from orbital angle ---
# The hinge fully determines rotation: the body's Y-axis rotation must
# match the angle of the lever arm in the hinge plane.
if mode_b == BodyMode.DYNAMIC and hasattr(self.body_b, "rotation"):
corrected_b = np.array([self.body_b.position.x, self.body_b.position.y, self.body_b.position.z], dtype=np.float32)
corrected_a = np.array([self.body_a.position.x, self.body_a.position.y, self.body_a.position.z], dtype=np.float32)
arm = corrected_b - corrected_a
arm_planar = arm - axis_n * float(np.dot(arm, axis_n))
arm_len = float(np.linalg.norm(arm_planar))
if arm_len > 1e-10:
# Compute angle between current arm and initial arm direction
# Initial arm is along the direction at rest_distance when joint was created
if not hasattr(self, "_init_dir"):
self._init_dir = arm_planar / arm_len
init_dir = self._init_dir
cur_dir = arm_planar / arm_len
# Signed angle between init_dir and cur_dir around the hinge axis
cos_a = float(np.dot(init_dir, cur_dir))
cross = np.cross(init_dir, cur_dir)
sin_a = float(np.dot(cross, axis_n))
angle = math.atan2(sin_a, cos_a)
self.body_b.rotation = Quat.from_axis_angle(self.axis, angle)
# --- 3. Correct velocity to match the position correction ---
# After snapping position, the radial velocity component is now invalid.
# Re-project velocity to be tangential to the corrected lever arm.
for body, mode, inv_m in [(self.body_b, mode_b, inv_b), (self.body_a, mode_a, inv_a)]:
if mode != BodyMode.DYNAMIC or not hasattr(body, "linear_velocity"):
continue
vel = body.linear_velocity
vel_arr = np.array([vel.x, vel.y, vel.z], dtype=np.float32)
# Recompute lever arm from corrected positions
bp = np.array([body.position.x, body.position.y, body.position.z], dtype=np.float32)
ap = np.array([self.body_a.position.x, self.body_a.position.y, self.body_a.position.z], dtype=np.float32)
arm = bp - ap if body is self.body_b else ap - bp
arm_planar = arm - axis_n * float(np.dot(arm, axis_n))
arm_len = float(np.linalg.norm(arm_planar))
if arm_len > 1e-10:
radial = arm_planar / arm_len
vel_arr -= radial * float(np.dot(vel_arr, radial))
vel_arr -= axis_n * float(np.dot(vel_arr, axis_n))
body.linear_velocity = Vec3(float(vel_arr[0]), float(vel_arr[1]), float(vel_arr[2]))
def _enforce_angular_limits(self, mode_a, mode_b, inv_a, inv_b, total_inv, axis_n):
"""Clamp relative rotation to angular limit range."""
rel_q = self.body_a.rotation.inverse() * self.body_b.rotation
dot = rel_q.x * self.axis.x + rel_q.y * self.axis.y + rel_q.z * self.axis.z
twist_angle = 2.0 * math.atan2(dot, rel_q.w)
twist_angle = math.atan2(math.sin(twist_angle), math.cos(twist_angle))
current_angle = twist_angle - self._reference_angle
current_angle = math.atan2(math.sin(current_angle), math.cos(current_angle))
limit_min_rad = math.radians(self.angular_limit_min)
limit_max_rad = math.radians(self.angular_limit_max)
if current_angle < limit_min_rad:
correction = limit_min_rad - current_angle
elif current_angle > limit_max_rad:
correction = limit_max_rad - current_angle
else:
return
if abs(correction) < 1e-6:
return
axis_vec = self.axis
if mode_a == BodyMode.DYNAMIC:
self.body_a.rotation = self.body_a.rotation.rotate(axis_vec, -correction * inv_a / total_inv)
if mode_b == BodyMode.DYNAMIC:
self.body_b.rotation = self.body_b.rotation.rotate(axis_vec, correction * inv_b / total_inv)
# Zero out angular velocity along axis that pushes past limit
for body, mode in [(self.body_a, mode_a), (self.body_b, mode_b)]:
if mode != BodyMode.DYNAMIC:
continue
av = body.angular_velocity
av_arr = np.array([av.x, av.y, av.z], dtype=np.float32)
along = float(np.dot(av_arr, axis_n))
body.angular_velocity = Vec3(*(av_arr - axis_n * along))
# ============================================================================
# RayCast3D — Persistent ray query node
# ============================================================================
[docs]
class RayCast3D(Node3D):
"""Persistent ray query node for 3D physics.
Casts a ray from the node's global position towards ``target_position``
(in local space) each physics frame and caches the result. Useful for
ground detection, line-of-sight checks, lasers, etc.
Example:
ray = RayCast3D(target_position=Vec3(0, -2, 0))
player.add_child(ray)
# Later, in physics_process:
if ray.is_colliding():
print("Hit:", ray.get_collider(), "at", ray.get_collision_point())
"""
target_position = Property(Vec3(0, -1, 0), hint="Local-space ray endpoint")
collision_mask = Property(0xFFFFFFFF, hint="Collision layer mask for filtering")
enabled = Property(True, hint="Enable/disable raycasting")
gizmo_colour = Property((1.0, 0.6, 0.0, 0.6), hint="Editor gizmo colour")
[docs]
def get_gizmo_lines(self) -> list[tuple[Vec3, Vec3]]:
"""Return ray line with arrowhead from position to target in world space."""
p = self.world_position
target = Vec3(self.target_position)
direction = self.world_rotation * target
end = p + direction
lines: list[tuple[Vec3, Vec3]] = [(p, end)]
ln = direction.length()
if ln > 1e-6:
nd = direction * (1.0 / ln)
up = Vec3(0, 1, 0) if abs(nd.y) < 0.9 else Vec3(1, 0, 0)
right = Vec3(*np.cross(nd, up))
rn = right.length()
if rn > 1e-6:
right = right * (1.0 / rn)
head = min(0.15, ln * 0.2)
lines.append((end, end - nd * head + right * head * 0.4))
lines.append((end, end - nd * head - right * head * 0.4))
return lines
def __init__(self, target_position=None, collision_mask: int = 0xFFFFFFFF, enabled: bool = True, **kwargs):
super().__init__(**kwargs)
if target_position is not None:
self.target_position = Vec3(target_position)
self.collision_mask = collision_mask
self.enabled = enabled
self._hits: list[RayHit] = []
[docs]
def physics_process(self, dt: float) -> None:
self._hits = []
if not self.enabled:
return
server = PhysicsServer.get()
origin = self.world_position
# target_position is in local space — convert to world direction
target = Vec3(self.target_position)
direction = self.world_rotation * target
max_dist = float(np.linalg.norm(np.array([direction.x, direction.y, direction.z], dtype=np.float32)))
if max_dist < 1e-10:
return
self._hits = server.raycast(origin, direction, max_dist=max_dist, layer_mask=self.collision_mask)
[docs]
def is_colliding(self) -> bool:
"""True if the ray hit something on the last physics frame."""
return len(self._hits) > 0
[docs]
def get_collider(self) -> Node3D | None:
"""Return the first body hit, or None."""
return self._hits[0].body if self._hits else None
[docs]
def get_collision_point(self) -> Vec3:
"""Return the world-space collision point of the first hit."""
return Vec3(self._hits[0].point) if self._hits else Vec3()
[docs]
def get_collision_normal(self) -> Vec3:
"""Return the collision normal (stub — RayHit lacks normal data)."""
return Vec3()
[docs]
def get_all_hits(self) -> list[RayHit]:
"""Return all hits from the last cast, sorted by distance."""
return list(self._hits)
# ============================================================================
# RayCast2D — Persistent ray query node
# ============================================================================
[docs]
class RayCast2D(Node2D):
"""Persistent ray query node for 2D physics.
Casts a ray from the node's global position towards ``target_position``
(in local space) each physics frame and caches the result.
Example:
ray = RayCast2D(target_position=Vec2(0, 100))
player.add_child(ray)
if ray.is_colliding():
print("Hit:", ray.get_collider())
"""
target_position = Property(Vec2(0, 1), hint="Local-space ray endpoint")
collision_mask = Property(0xFFFFFFFF, hint="Collision layer mask for filtering")
enabled = Property(True, hint="Enable/disable raycasting")
gizmo_colour = Property((1.0, 0.6, 0.0, 0.6), hint="Editor gizmo colour")
[docs]
def get_gizmo_lines(self) -> list[tuple[Vec2, Vec2]]:
"""Return ray line with arrowhead from position to target in world space."""
p = self.world_position
target = Vec2(self.target_position)
angle = self.world_rotation
cos_a, sin_a = math.cos(angle), math.sin(angle)
direction = Vec2(target.x * cos_a - target.y * sin_a, target.x * sin_a + target.y * cos_a)
end = Vec2(p.x + direction.x, p.y + direction.y)
lines: list[tuple[Vec2, Vec2]] = [(p, end)]
ln = direction.length()
if ln > 1e-6:
ndx, ndy = direction.x / ln, direction.y / ln
px, py = -ndy, ndx
head = min(8.0, ln * 0.2)
lines.append((end, Vec2(end.x - ndx * head + px * head * 0.4, end.y - ndy * head + py * head * 0.4)))
lines.append((end, Vec2(end.x - ndx * head - px * head * 0.4, end.y - ndy * head - py * head * 0.4)))
return lines
def __init__(self, target_position=None, collision_mask: int = 0xFFFFFFFF, enabled: bool = True, **kwargs):
super().__init__(**kwargs)
if target_position is not None:
self.target_position = Vec2(target_position)
self.collision_mask = collision_mask
self.enabled = enabled
self._hits: list[RayHit] = []
[docs]
def physics_process(self, dt: float) -> None:
self._hits = []
if not self.enabled:
return
server = PhysicsServer.get()
origin = self.world_position
# Rotate target_position by node's global rotation
target = Vec2(self.target_position)
angle = self.world_rotation
cos_a, sin_a = math.cos(angle), math.sin(angle)
direction = Vec2(target.x * cos_a - target.y * sin_a, target.x * sin_a + target.y * cos_a)
max_dist = float(direction.length())
if max_dist < 1e-10:
return
self._hits = server.raycast(origin, direction, max_dist=max_dist, layer_mask=self.collision_mask)
[docs]
def is_colliding(self) -> bool:
"""True if the ray hit something on the last physics frame."""
return len(self._hits) > 0
[docs]
def get_collider(self) -> Node2D | None:
"""Return the first body hit, or None."""
return self._hits[0].body if self._hits else None
[docs]
def get_collision_point(self) -> Vec2:
"""Return the world-space collision point of the first hit."""
if self._hits:
p = self._hits[0].point
return Vec2(float(p[0]), float(p[1]))
return Vec2()
[docs]
def get_collision_normal(self) -> Vec2:
"""Return the collision normal (stub — RayHit lacks normal data)."""
return Vec2()
[docs]
def get_all_hits(self) -> list[RayHit]:
"""Return all hits from the last cast, sorted by distance."""
return list(self._hits)