"""Physics/collision nodes: CharacterBody, CollisionShape, Area, ShapeCast (2D and 3D)."""
import logging
import math
import numpy as np
from .descriptors import Collision, Property
from .math.types import Vec2, Vec3
from .nodes_2d.node2d import Node2D
from .nodes_3d.node3d import Node3D
from .properties import Bitmask, Colour
from .shapecast import ShapeCast2D, ShapeCast3D # noqa: F401
from .signals import Signal
log = logging.getLogger(__name__)
# ---------------------------------------------------------------------------
# Gizmo helper: generate circle line segments
# ---------------------------------------------------------------------------
_CIRCLE_SEGMENTS = 24
def _circle_lines_2d(cx: float, cy: float, radius: float, segments: int = _CIRCLE_SEGMENTS) -> list[tuple[Vec2, Vec2]]:
"""Return line segments approximating a circle in 2D."""
lines: list[tuple[Vec2, Vec2]] = []
step = math.tau / segments
prev = Vec2(cx + radius, cy)
for i in range(1, segments + 1):
angle = i * step
cur = Vec2(cx + radius * math.cos(angle), cy + radius * math.sin(angle))
lines.append((prev, cur))
prev = cur
return lines
def _circle_lines_3d(
center: Vec3, u: Vec3, v: Vec3, radius: float, segments: int = _CIRCLE_SEGMENTS,
) -> list[tuple[Vec3, Vec3]]:
"""Return line segments approximating a circle in 3D on the plane defined by u, v axes."""
lines: list[tuple[Vec3, Vec3]] = []
step = math.tau / segments
prev = center + u * radius
for i in range(1, segments + 1):
angle = i * step
cur = center + u * (radius * math.cos(angle)) + v * (radius * math.sin(angle))
lines.append((prev, cur))
prev = cur
return lines
# ============================================================================
# Shared mixins to deduplicate 2D/3D logic
# ============================================================================
class _CharacterBodyBase:
"""Shared logic for CharacterBody2D and CharacterBody3D."""
def _character_body_init(self):
"""Initialize contact tracking signals. Called from __init__."""
self.body_entered = Signal()
self.body_exited = Signal()
self._prev_contacts: set = set()
def _emit_contact_signals(self, current_contacts: set):
"""Compare current contacts with previous frame and emit enter/exit signals."""
for body in current_contacts - self._prev_contacts:
self.body_entered(body)
for body in self._prev_contacts - current_contacts:
self.body_exited(body)
self._prev_contacts = current_contacts
def is_on_floor(self) -> bool:
"""True if last collision normal aligns with floor_normal (dot > 0.7)."""
if not self.last_collision:
return False
return self.last_collision.normal.dot(self.floor_normal) > 0.7
def is_on_wall(self) -> bool:
"""True if last collision normal is mostly perpendicular to floor_normal."""
if not self.last_collision:
return False
return abs(self.last_collision.normal.dot(self.floor_normal)) < 0.3
def is_on_ceiling(self) -> bool:
"""True if last collision normal points opposite to floor_normal (dot < -0.7)."""
if not self.last_collision:
return False
return self.last_collision.normal.dot(self.floor_normal) < -0.7
class _CollisionShapeBase:
"""Shared logic for CollisionShape2D and CollisionShape3D."""
radius = Property(1.0, range=(0, 10000), hint="Collision radius")
def overlaps_point(self, point) -> bool:
"""Test if a point is inside this shape."""
diff = self.world_position - point
if hasattr(diff, 'z'):
return (diff.x**2 + diff.y**2 + diff.z**2) <= self.radius ** 2
return (diff.x*diff.x + diff.y*diff.y) <= self.radius * self.radius
class _AreaBase:
"""Shared logic for Area2D and Area3D."""
collision_layer = Bitmask(1, group="Collision")
collision_mask = Bitmask(1, group="Collision")
monitoring = Property(True, hint="Whether to detect overlaps")
def _layers_match(self, other) -> bool:
# Areas are observers: only the area's mask vs the other's layer
# matters. The other object's mask is irrelevant: sensors decide
# what they watch, the watched don't need an opinion. Matches Godot's
# Area semantics. Not bidirectional (that's for body↔body collisions).
return bool(self.collision_mask & other.collision_layer)
def _any_shape_overlaps(self, my_shapes, other_shapes) -> bool:
return any(s.overlaps(o) for s in my_shapes for o in other_shapes)
@property
def overlapping_bodies(self) -> list:
"""Bodies currently overlapping this area."""
return list(self._overlapping_bodies)
@property
def overlapping_areas(self) -> list:
"""Areas currently overlapping this area."""
return list(self._overlapping_areas)
def has_overlapping_bodies(self) -> bool:
return bool(self._overlapping_bodies)
def has_overlapping_areas(self) -> bool:
return bool(self._overlapping_areas)
# ============================================================================
# CharacterBody2D
# ============================================================================
[docs]
class CharacterBody2D(_CharacterBodyBase, Node2D):
"""2D node with velocity, movement, and collision response.
Uses CollisionShape2D children for circle or rectangle collision.
Screen coordinates: Y-down, so floor normal defaults to (0, -1).
"""
collision_layer = Bitmask(1, group="Collision")
collision_mask = Bitmask(1, group="Collision")
max_slides = Property(4, range=(1, 16), hint="Max collision iterations per frame", group="Collision")
max_substeps = Property(
8, range=(1, 32),
hint="Cap on substeps per move_and_slide call (tunnelling tradeoff)",
group="Collision",
)
def __init__(self, collision=None, **kwargs):
super().__init__(**kwargs)
self._character_body_init()
self.velocity = Vec2()
self.floor_normal = Vec2(0, -1) # Y-down screen coords: floor points up
self.last_collision: Collision | None = None
if collision is not None:
if isinstance(collision, int | float):
collision = CollisionShape2D(name="Collision", radius=collision)
elif isinstance(collision, tuple | list | np.ndarray):
collision = CollisionShape2D(name="Collision", extents=Vec2(collision))
self.collision = self.add_child(collision)
else:
self.collision = None
[docs]
def move_and_slide(self, dt: float = 1.0):
"""Apply velocity with collision response. Resolves overlaps and slides along surfaces.
Sub-stepping prevents tunneling at high velocities by splitting movement into
ceil(disp / col_radius) iterations, capped at ``max_substeps``. The cap protects
frame rate at the risk of tunneling for extreme velocities; raise it for fast
bullets/projectiles.
"""
speed_sq = self.velocity.x ** 2 + self.velocity.y ** 2
col_radius = self.collision.radius if self.collision else 8.0
disp = (speed_sq ** 0.5) * dt
steps = min(self.max_substeps, max(1, math.ceil(disp / col_radius)))
sub_dt = dt / steps
self.last_collision = None
all_contacted: set[CharacterBody2D] = set()
for _ in range(steps):
self.position += self.velocity * sub_dt
if not self.collision or not self._tree:
continue
for _ in range(self.max_slides):
overlaps = self.get_overlapping()
if not overlaps:
break
other = overlaps[0]
all_contacted.add(other)
pen = self.collision.get_penetration(other.collision)
if pen is None:
break
normal, penetration = pen
self.position += normal * penetration
vdot = self.velocity.dot(normal)
if vdot < 0:
self.velocity -= normal * vdot
self.last_collision = Collision(
normal=normal, collider=other,
position=other.collision.world_position + normal * other.collision.radius,
depth=penetration,
)
self._emit_contact_signals(all_contacted)
[docs]
def get_overlapping(self, group: str = None, body_type: type = None) -> list[CharacterBody2D]:
"""Find all CharacterBody2D nodes whose collision overlaps ours."""
if not self.collision or not self._tree:
return []
candidates = self._tree.get_group(group) if group else self._tree.root.find_all(CharacterBody2D)
return [
b for b in candidates
if b is not self
and (body_type is None or isinstance(b, body_type))
and isinstance(b, CharacterBody2D)
and b.collision
and self.collision.overlaps(b.collision)
]
# ============================================================================
# 2D collision helpers (circle / AABB)
# ============================================================================
def _circle_aabb_overlap(cp: Vec2, cr: float, rp: Vec2, re: Vec2) -> bool:
"""Circle (center *cp*, radius *cr*) vs AABB (center *rp*, half-extents *re*)."""
cx, cy = float(cp[0]), float(cp[1])
dx = cx - max(float(rp[0] - re[0]), min(cx, float(rp[0] + re[0])))
dy = cy - max(float(rp[1] - re[1]), min(cy, float(rp[1] + re[1])))
return dx * dx + dy * dy <= cr * cr
def _pen_cc(ap: Vec2, ar: float, bp: Vec2, br: float) -> tuple[Vec2, float] | None:
"""Circle-circle penetration: push *a* out of *b*."""
diff = ap - bp
dist_sq = diff.x * diff.x + diff.y * diff.y
radii = ar + br
if dist_sq > radii * radii:
return None
if dist_sq < 1e-10:
return (Vec2(0, -1), radii)
dist = math.sqrt(dist_sq)
return (diff * (1.0 / dist), radii - dist)
def _pen_rr(ap: Vec2, ae: Vec2, bp: Vec2, be: Vec2) -> tuple[Vec2, float] | None:
"""Rect-rect (AABB) penetration: push *a* out of *b*."""
dx, dy = ap.x - bp.x, ap.y - bp.y
ox = ae.x + be.x - abs(dx)
oy = ae.y + be.y - abs(dy)
if ox <= 0 or oy <= 0:
return None
if ox < oy:
return (Vec2(1.0 if dx > 0 else -1.0, 0.0), ox)
return (Vec2(0.0, 1.0 if dy > 0 else -1.0), oy)
def _pen_circle_rect(cp: Vec2, cr: float, rp: Vec2, re: Vec2) -> tuple[Vec2, float] | None:
"""Penetration of circle into rect. Normal pushes circle out of rect."""
cx, cy = float(cp[0]), float(cp[1])
rx, ry, ex, ey = float(rp[0]), float(rp[1]), float(re[0]), float(re[1])
# Closest point on rect to circle centre
clx = max(rx - ex, min(cx, rx + ex))
cly = max(ry - ey, min(cy, ry + ey))
dx, dy = cx - clx, cy - cly
dist_sq = dx * dx + dy * dy
if dist_sq > cr * cr:
return None
if dist_sq < 1e-10:
# Circle centre inside rect: push to nearest edge
to_l, to_r = cx - (rx - ex), (rx + ex) - cx
to_t, to_b = cy - (ry - ey), (ry + ey) - cy
m = min(to_l, to_r, to_t, to_b)
if m == to_l:
return (Vec2(-1, 0), to_l + cr)
if m == to_r:
return (Vec2(1, 0), to_r + cr)
if m == to_t:
return (Vec2(0, -1), to_t + cr)
return (Vec2(0, 1), to_b + cr)
dist = math.sqrt(dist_sq)
return (Vec2(dx / dist, dy / dist), cr - dist)
# ============================================================================
# CollisionShape2D
# ============================================================================
[docs]
class CollisionShape2D(_CollisionShapeBase, Node2D):
"""2D collision shape: circle (default) or axis-aligned rectangle.
Circle mode (default): set ``radius``. Rectangle mode: set ``extents``
to a ``Vec2(half_width, half_height)``. When ``extents`` is non-zero the
shape is treated as an AABB and ``radius`` is ignored for overlap tests.
"""
radius = Property(1.0, range=(0, 10000), hint="Collision radius (circle mode)", group="Collision")
extents = Property(Vec2(), hint="Half-size (rect mode; zero = circle)", group="Collision")
gizmo_colour = Colour((0.2, 0.8, 0.2, 0.6))
[docs]
@property
def is_rect(self) -> bool:
"""True when extents are non-zero (rectangle mode)."""
e = self.extents
return float(e[0]) > 0 and float(e[1]) > 0
# -- overlap tests -------------------------------------------------------
[docs]
def overlaps(self, other: CollisionShape2D) -> bool:
"""Test overlap with another CollisionShape2D (circle or rect)."""
sr, orr = self.is_rect, other.is_rect
sp, op = self.world_position, other.world_position
if not sr and not orr:
diff = sp - op
radii = self.radius + other.radius
return diff.x * diff.x + diff.y * diff.y <= radii * radii
if sr and orr:
se, oe = self.extents, other.extents
return abs(sp.x - op.x) <= se.x + oe.x and abs(sp.y - op.y) <= se.y + oe.y
# One circle, one rect
if sr:
return _circle_aabb_overlap(op, other.radius, sp, self.extents)
return _circle_aabb_overlap(sp, self.radius, op, other.extents)
[docs]
def overlaps_point(self, point: tuple[float, float] | np.ndarray) -> bool:
"""Test if a point is inside this shape."""
p = self.world_position
if self.is_rect:
e = self.extents
return abs(float(point[0]) - p.x) <= e.x and abs(float(point[1]) - p.y) <= e.y
diff = p - point
return diff.x * diff.x + diff.y * diff.y <= self.radius * self.radius
# -- penetration for collision response ----------------------------------
[docs]
def get_penetration(self, other: CollisionShape2D) -> tuple[Vec2, float] | None:
"""Return ``(normal, depth)`` to push **self** out of **other**, or ``None``."""
sr, orr = self.is_rect, other.is_rect
sp, op = self.world_position, other.world_position
if not sr and not orr:
return _pen_cc(sp, self.radius, op, other.radius)
if sr and orr:
return _pen_rr(sp, self.extents, op, other.extents)
if not sr and orr:
return _pen_circle_rect(sp, self.radius, op, other.extents)
# self=rect, other=circle → flip circle-rect result
result = _pen_circle_rect(op, other.radius, sp, self.extents)
if result is None:
return None
n, d = result
return (Vec2(-n.x, -n.y), d)
# -- gizmo ---------------------------------------------------------------
[docs]
def get_gizmo_lines(self) -> list[tuple[Vec2, Vec2]]:
"""Return wireframe line segments for the collision shape in world space."""
p = self.world_position
if self.is_rect:
e = self.extents
tl, tr = Vec2(p.x - e.x, p.y - e.y), Vec2(p.x + e.x, p.y - e.y)
br, bl = Vec2(p.x + e.x, p.y + e.y), Vec2(p.x - e.x, p.y + e.y)
return [(tl, tr), (tr, br), (br, bl), (bl, tl)]
return _circle_lines_2d(p.x, p.y, float(self.radius))
# ============================================================================
# Area2D: Trigger zone with enter/exit signals
# ============================================================================
[docs]
class Area2D(_AreaBase, Node2D):
"""2D trigger zone that detects overlapping bodies and areas.
Add CollisionShape2D children to define the detection region.
Emits body_entered/body_exited when CharacterBody2D nodes overlap,
and area_entered/area_exited for other Area2D nodes.
"""
collision_layer = Bitmask(1, group="Collision")
collision_mask = Bitmask(1, group="Collision")
monitoring = Property(True, hint="Whether to detect overlaps", group="Collision")
gizmo_colour = Colour((0.2, 0.6, 1.0, 0.5))
def __init__(self, **kwargs):
super().__init__(**kwargs)
self.body_entered = Signal()
self.body_exited = Signal()
self.area_entered = Signal()
self.area_exited = Signal()
self._overlapping_bodies: set[CharacterBody2D] = set()
self._overlapping_areas: set[Area2D] = set()
[docs]
def get_gizmo_lines(self) -> list[tuple[Vec2, Vec2]]:
"""Return gizmo lines from child collision shapes, or a default diamond if none."""
lines: list[tuple[Vec2, Vec2]] = []
for child in self.children:
if isinstance(child, CollisionShape2D) and hasattr(child, 'get_gizmo_lines'):
lines.extend(child.get_gizmo_lines())
if not lines:
p = self.world_position
s = 15.0
corners = [Vec2(p.x, p.y - s), Vec2(p.x + s, p.y), Vec2(p.x, p.y + s), Vec2(p.x - s, p.y)]
for i in range(4):
lines.append((corners[i], corners[(i + 1) % 4]))
return lines
[docs]
def on_physics_process(self, dt: float):
if not self.monitoring or not self._tree:
return
my_shapes = self.find_all(CollisionShape2D)
if not my_shapes:
return
# Check bodies
current_bodies: set[CharacterBody2D] = set()
for body in self._tree.root.find_all(CharacterBody2D):
if not self._layers_match(body) or not body.collision:
continue
if isinstance(body.collision, CollisionShape2D):
body_shapes = [body.collision]
else:
body_shapes = body.find_all(CollisionShape2D)
if self._any_shape_overlaps(my_shapes, body_shapes):
current_bodies.add(body)
for body in current_bodies - self._overlapping_bodies:
self.body_entered(body)
for body in self._overlapping_bodies - current_bodies:
self.body_exited(body)
self._overlapping_bodies = current_bodies
# Check areas
current_areas: set[Area2D] = set()
for area in self._tree.root.find_all(Area2D):
if area is self or not self._layers_match(area):
continue
other_shapes = area.find_all(CollisionShape2D)
if self._any_shape_overlaps(my_shapes, other_shapes):
current_areas.add(area)
for area in current_areas - self._overlapping_areas:
self.area_entered(area)
for area in self._overlapping_areas - current_areas:
self.area_exited(area)
self._overlapping_areas = current_areas
# ============================================================================
# CharacterBody3D
# ============================================================================
[docs]
class CharacterBody3D(_CharacterBodyBase, Node3D):
"""3D node with velocity, movement, and collision response.
Uses CollisionShape3D children: sphere, box, or capsule. Y-up
convention: floor normal defaults to ``(0, 1, 0)``. Collision response
routes through :meth:`CollisionShape3D.get_penetration`, so all shape
pairings the shape node supports work transparently here.
"""
collision_layer = Bitmask(1, group="Collision")
collision_mask = Bitmask(1, group="Collision")
max_slides = Property(4, range=(1, 16), hint="Max collision iterations per frame", group="Collision")
def __init__(self, collision=None, **kwargs):
super().__init__(**kwargs)
self._character_body_init()
self.velocity: tuple[float, float, float] | np.ndarray = Vec3()
self.floor_normal = Vec3(0, 1, 0)
self.last_collision: Collision | None = None
if collision is not None:
if isinstance(collision, int | float):
collision = CollisionShape3D(name="Collision", radius=collision)
elif isinstance(collision, tuple | list | np.ndarray):
collision = CollisionShape3D(name="Collision", kind="box", extents=Vec3(collision))
self.collision = self.add_child(collision)
else:
self.collision = None
[docs]
def move_and_slide(self, dt: float = 1.0):
"""Apply velocity with collision response. Resolves overlaps and slides along surfaces."""
self.position += self.velocity * dt
self.last_collision = None
if not self.collision or not self._tree:
self._emit_contact_signals(set())
return
contacted: set[CharacterBody3D] = set()
for _ in range(self.max_slides):
overlaps = self.get_overlapping()
if not overlaps:
break
other = overlaps[0]
contacted.add(other)
pen = self.collision.get_penetration(other.collision)
if pen is None:
break
normal, penetration = pen
if penetration <= 0:
break
self.position += normal * penetration
vel = Vec3(self.velocity) if not isinstance(self.velocity, Vec3) else self.velocity
vdot = vel.dot(normal)
if vdot < 0:
self.velocity = vel - normal * vdot
self.last_collision = Collision(
normal=normal, collider=other,
position=other.collision.world_position + normal * _approx_contact_radius(other.collision),
depth=penetration,
)
self._emit_contact_signals(contacted)
[docs]
def get_overlapping(self, group: str = None, body_type: type = None) -> list[CharacterBody3D]:
if not self.collision or not self._tree:
return []
candidates = self._tree.get_group(group) if group else self._tree.root.find_all(CharacterBody3D)
return [
b for b in candidates
if b is not self
and (body_type is None or isinstance(b, body_type))
and isinstance(b, CharacterBody3D)
and b.collision
and self.collision.overlaps(b.collision)
]
# ============================================================================
# CollisionShape3D
# ============================================================================
[docs]
class CollisionShape3D(_CollisionShapeBase, Node3D):
"""Sphere, axis-aligned box, or Y-axis capsule collision shape.
Set :attr:`kind` to pick the shape. ``"sphere"`` uses :attr:`radius`,
``"box"`` uses :attr:`extents` (Vec3 of half-widths in X/Y/Z), and
``"capsule"`` is a Y-axis capsule with :attr:`radius` + :attr:`height`
(total height including hemispherical caps; ``height >= 2 * radius`` for
a meaningful cylinder section). Mirrors the 2D pattern but uses an
explicit ``kind`` enum because three shape variants don't auto-detect
cleanly the way 2D's circle/rect does.
"""
kind = Property(
"sphere", enum=("sphere", "box", "capsule"),
hint="Shape type: sphere uses radius; box uses extents; capsule uses radius+height",
group="Collision",
)
radius = Property(1.0, range=(0, 10000), hint="Collision radius (sphere / capsule)", group="Collision")
extents = Property(Vec3(), hint="Half-size XYZ (box mode)", group="Collision")
height = Property(2.0, range=(0, 10000), hint="Capsule total height (capsule mode)", group="Collision")
pickable = Property(False, hint="Receive picked() on mouse click")
gizmo_colour = Colour((0.2, 0.8, 0.2, 0.6))
[docs]
def overlaps(self, other: CollisionShape3D) -> bool:
sp, op = self.world_position, other.world_position
sk, ok = self.kind, other.kind
if sk == "sphere" and ok == "sphere":
diff = sp - op
radii = self.radius + other.radius
return diff.x * diff.x + diff.y * diff.y + diff.z * diff.z <= radii * radii
if sk == "box" and ok == "box":
se, oe = self.extents, other.extents
return (
abs(sp.x - op.x) <= se.x + oe.x
and abs(sp.y - op.y) <= se.y + oe.y
and abs(sp.z - op.z) <= se.z + oe.z
)
if sk == "sphere" and ok == "box":
return _sphere_aabb_overlap_3d(sp, self.radius, op, other.extents)
if sk == "box" and ok == "sphere":
return _sphere_aabb_overlap_3d(op, other.radius, sp, self.extents)
# Capsule cases: reduce to sphere-sphere at the closest points of the
# capsule's central segment(s).
if sk == "capsule":
sa, sb = _capsule_endpoints(sp, self.height, self.radius)
if ok == "capsule":
oa, ob = _capsule_endpoints(op, other.height, other.radius)
cp_self, cp_other = _closest_points_on_segments(sa, sb, oa, ob)
diff = cp_self - cp_other
radii = self.radius + other.radius
return diff.x * diff.x + diff.y * diff.y + diff.z * diff.z <= radii * radii
if ok == "sphere":
cp = _closest_point_on_segment(sa, sb, op)
diff = cp - op
radii = self.radius + other.radius
return diff.x * diff.x + diff.y * diff.y + diff.z * diff.z <= radii * radii
# capsule vs box
cp = _closest_point_on_aabb_to_segment(op, other.extents, sa, sb)
return _sphere_aabb_overlap_3d(cp, self.radius, op, other.extents)
# other.kind == "capsule": flip and recurse
return other.overlaps(self)
[docs]
def overlaps_point(self, point: tuple[float, float, float] | np.ndarray) -> bool:
p = self.world_position
px = float(point[0])
py = float(point[1])
pz = float(point[2])
if self.kind == "box":
e = self.extents
return abs(px - p.x) <= e.x and abs(py - p.y) <= e.y and abs(pz - p.z) <= e.z
if self.kind == "capsule":
a, b = _capsule_endpoints(p, self.height, self.radius)
cp = _closest_point_on_segment(a, b, Vec3(px, py, pz))
dx, dy, dz = cp.x - px, cp.y - py, cp.z - pz
return dx * dx + dy * dy + dz * dz <= self.radius * self.radius
diff = p - Vec3(px, py, pz)
return diff.x * diff.x + diff.y * diff.y + diff.z * diff.z <= self.radius * self.radius
[docs]
def get_penetration(self, other: CollisionShape3D) -> tuple[Vec3, float] | None:
"""Return ``(normal, depth)`` to push **self** out of **other**, or ``None``.
Sphere-sphere, sphere-box, and box-box are analytic. Capsule pairings
approximate via the closest-points-on-segment reduction described in
:meth:`overlaps`: accurate for vertical capsules (the common stand-up
character collider) and good enough for sliding response on near-axis
cases. A future GJK+EPA pass can replace this for full general capsule
contact resolution.
"""
sp, op = self.world_position, other.world_position
sk, ok = self.kind, other.kind
if sk == "sphere" and ok == "sphere":
return _pen3_ss(sp, self.radius, op, other.radius)
if sk == "box" and ok == "box":
return _pen3_bb(sp, self.extents, op, other.extents)
if sk == "sphere" and ok == "box":
return _pen3_sphere_box(sp, self.radius, op, other.extents)
if sk == "box" and ok == "sphere":
res = _pen3_sphere_box(op, other.radius, sp, self.extents)
if res is None:
return None
n, d = res
return (Vec3(-n.x, -n.y, -n.z), d)
if sk == "capsule":
sa, sb = _capsule_endpoints(sp, self.height, self.radius)
if ok == "capsule":
oa, ob = _capsule_endpoints(op, other.height, other.radius)
cp_self, cp_other = _closest_points_on_segments(sa, sb, oa, ob)
return _pen3_ss(cp_self, self.radius, cp_other, other.radius)
if ok == "sphere":
cp = _closest_point_on_segment(sa, sb, op)
return _pen3_ss(cp, self.radius, op, other.radius)
cp = _closest_point_on_aabb_to_segment(op, other.extents, sa, sb)
return _pen3_sphere_box(cp, self.radius, op, other.extents)
# other.kind == "capsule"
flipped = other.get_penetration(self)
if flipped is None:
return None
n, d = flipped
return (Vec3(-n.x, -n.y, -n.z), d)
[docs]
def get_gizmo_lines(self) -> list[tuple[Vec3, Vec3]]:
"""Return wireframe line segments for the collision shape."""
p = self.world_position
if self.kind == "box":
e = self.extents
return _box_wireframe_3d(p, e)
if self.kind == "capsule":
return _capsule_wireframe_3d(p, float(self.radius), float(self.height))
r = float(self.radius)
lines: list[tuple[Vec3, Vec3]] = []
lines.extend(_circle_lines_3d(p, Vec3(1, 0, 0), Vec3(0, 1, 0), r)) # XY
lines.extend(_circle_lines_3d(p, Vec3(1, 0, 0), Vec3(0, 0, 1), r)) # XZ
lines.extend(_circle_lines_3d(p, Vec3(0, 1, 0), Vec3(0, 0, 1), r)) # YZ
return lines
# ============================================================================
# 3D collision helpers: penetration math for sphere / box / capsule
# ============================================================================
def _approx_contact_radius(shape: CollisionShape3D) -> float:
"""Approximate "radius" used to place the contact point on the other body.
Sphere → radius; box → smallest half-extent; capsule → radius. The contact
point this seeds is informational (the Collision result the user inspects
after move_and_slide), not used in the physics step itself, so the
approximation is acceptable.
"""
k = shape.kind
if k == "box":
e = shape.extents
return float(min(e.x, e.y, e.z))
return float(shape.radius)
def _sphere_aabb_overlap_3d(cp: Vec3, cr: float, rp: Vec3, re: Vec3) -> bool:
"""Sphere (centre *cp*, radius *cr*) vs AABB (centre *rp*, half-extents *re*)."""
dx = float(cp.x) - max(float(rp.x - re.x), min(float(cp.x), float(rp.x + re.x)))
dy = float(cp.y) - max(float(rp.y - re.y), min(float(cp.y), float(rp.y + re.y)))
dz = float(cp.z) - max(float(rp.z - re.z), min(float(cp.z), float(rp.z + re.z)))
return dx * dx + dy * dy + dz * dz <= cr * cr
def _pen3_ss(ap: Vec3, ar: float, bp: Vec3, br: float) -> tuple[Vec3, float] | None:
"""Sphere-sphere penetration: push *a* out of *b*."""
diff = ap - bp
dist_sq = diff.x * diff.x + diff.y * diff.y + diff.z * diff.z
radii = ar + br
if dist_sq > radii * radii:
return None
if dist_sq < 1e-10:
return (Vec3(0, 1, 0), radii)
dist = math.sqrt(dist_sq)
return (Vec3(diff.x / dist, diff.y / dist, diff.z / dist), radii - dist)
def _pen3_bb(ap: Vec3, ae: Vec3, bp: Vec3, be: Vec3) -> tuple[Vec3, float] | None:
"""AABB-AABB penetration: pick the smallest of the three axis overlaps."""
dx, dy, dz = ap.x - bp.x, ap.y - bp.y, ap.z - bp.z
ox = ae.x + be.x - abs(dx)
oy = ae.y + be.y - abs(dy)
oz = ae.z + be.z - abs(dz)
if ox <= 0 or oy <= 0 or oz <= 0:
return None
if ox <= oy and ox <= oz:
return (Vec3(1.0 if dx >= 0 else -1.0, 0.0, 0.0), ox)
if oy <= oz:
return (Vec3(0.0, 1.0 if dy >= 0 else -1.0, 0.0), oy)
return (Vec3(0.0, 0.0, 1.0 if dz >= 0 else -1.0), oz)
def _pen3_sphere_box(cp: Vec3, cr: float, rp: Vec3, re: Vec3) -> tuple[Vec3, float] | None:
"""Penetration of sphere into box. Normal pushes sphere out of box."""
cx, cy, cz = float(cp.x), float(cp.y), float(cp.z)
rx, ry, rz = float(rp.x), float(rp.y), float(rp.z)
ex, ey, ez = float(re.x), float(re.y), float(re.z)
clx = max(rx - ex, min(cx, rx + ex))
cly = max(ry - ey, min(cy, ry + ey))
clz = max(rz - ez, min(cz, rz + ez))
dx, dy, dz = cx - clx, cy - cly, cz - clz
dist_sq = dx * dx + dy * dy + dz * dz
if dist_sq > cr * cr:
return None
if dist_sq < 1e-10:
# Sphere centre inside box: push along the axis with the shallowest exit.
to_minx, to_maxx = cx - (rx - ex), (rx + ex) - cx
to_miny, to_maxy = cy - (ry - ey), (ry + ey) - cy
to_minz, to_maxz = cz - (rz - ez), (rz + ez) - cz
m = min(to_minx, to_maxx, to_miny, to_maxy, to_minz, to_maxz)
if m == to_minx:
return (Vec3(-1, 0, 0), to_minx + cr)
if m == to_maxx:
return (Vec3(1, 0, 0), to_maxx + cr)
if m == to_miny:
return (Vec3(0, -1, 0), to_miny + cr)
if m == to_maxy:
return (Vec3(0, 1, 0), to_maxy + cr)
if m == to_minz:
return (Vec3(0, 0, -1), to_minz + cr)
return (Vec3(0, 0, 1), to_maxz + cr)
dist = math.sqrt(dist_sq)
return (Vec3(dx / dist, dy / dist, dz / dist), cr - dist)
def _capsule_endpoints(centre: Vec3, height: float, radius: float) -> tuple[Vec3, Vec3]:
"""Return the two hemisphere-centre endpoints of a Y-axis capsule.
The capsule is the Minkowski sum of a Y-axis segment of length
``max(0, height - 2*radius)`` and a sphere of *radius*. Endpoints sit at
``centre ± (half_seg) * Y`` so the swept sphere reaches ``height/2``
above/below the centre.
"""
half_seg = max(0.0, height * 0.5 - radius)
return (
Vec3(centre.x, centre.y - half_seg, centre.z),
Vec3(centre.x, centre.y + half_seg, centre.z),
)
def _closest_point_on_segment(a: Vec3, b: Vec3, p: Vec3) -> Vec3:
ab = b - a
denom = ab.x * ab.x + ab.y * ab.y + ab.z * ab.z
if denom < 1e-12:
return a
t = ((p.x - a.x) * ab.x + (p.y - a.y) * ab.y + (p.z - a.z) * ab.z) / denom
t = max(0.0, min(1.0, t))
return Vec3(a.x + ab.x * t, a.y + ab.y * t, a.z + ab.z * t)
def _closest_points_on_segments(
p1: Vec3, q1: Vec3, p2: Vec3, q2: Vec3,
) -> tuple[Vec3, Vec3]:
"""Closest points on two segments (Real-Time Collision Detection §5.1.9)."""
d1 = q1 - p1
d2 = q2 - p2
r = p1 - p2
a = d1.x * d1.x + d1.y * d1.y + d1.z * d1.z
e = d2.x * d2.x + d2.y * d2.y + d2.z * d2.z
f = d2.x * r.x + d2.y * r.y + d2.z * r.z
if a <= 1e-12 and e <= 1e-12:
return p1, p2
if a <= 1e-12:
s = 0.0
t = max(0.0, min(1.0, f / e))
else:
c = d1.x * r.x + d1.y * r.y + d1.z * r.z
if e <= 1e-12:
t = 0.0
s = max(0.0, min(1.0, -c / a))
else:
b = d1.x * d2.x + d1.y * d2.y + d1.z * d2.z
denom = a * e - b * b
s = max(0.0, min(1.0, (b * f - c * e) / denom)) if denom else 0.0
t = (b * s + f) / e
if t < 0.0:
t = 0.0
s = max(0.0, min(1.0, -c / a))
elif t > 1.0:
t = 1.0
s = max(0.0, min(1.0, (b - c) / a))
return (
Vec3(p1.x + d1.x * s, p1.y + d1.y * s, p1.z + d1.z * s),
Vec3(p2.x + d2.x * t, p2.y + d2.y * t, p2.z + d2.z * t),
)
def _closest_point_on_aabb_to_segment(
box_centre: Vec3, box_extents: Vec3, seg_a: Vec3, seg_b: Vec3,
samples: int = 6,
) -> Vec3:
"""Approximate closest point on the segment to the AABB.
Sample the segment at *samples* equally spaced t values, pick the one
whose clamped-to-AABB distance is smallest. Cheap and good enough for
capsule-box collision response; an analytic solution is possible but adds
significant complexity for marginal gain on near-axis capsules.
"""
best_pt = seg_a
best_dist_sq = float("inf")
bmin = Vec3(box_centre.x - box_extents.x, box_centre.y - box_extents.y, box_centre.z - box_extents.z)
bmax = Vec3(box_centre.x + box_extents.x, box_centre.y + box_extents.y, box_centre.z + box_extents.z)
for i in range(samples + 1):
t = i / samples
px = seg_a.x + (seg_b.x - seg_a.x) * t
py = seg_a.y + (seg_b.y - seg_a.y) * t
pz = seg_a.z + (seg_b.z - seg_a.z) * t
clx = max(bmin.x, min(px, bmax.x))
cly = max(bmin.y, min(py, bmax.y))
clz = max(bmin.z, min(pz, bmax.z))
dx, dy, dz = px - clx, py - cly, pz - clz
d_sq = dx * dx + dy * dy + dz * dz
if d_sq < best_dist_sq:
best_dist_sq = d_sq
best_pt = Vec3(px, py, pz)
return best_pt
def _box_wireframe_3d(centre: Vec3, extents: Vec3) -> list[tuple[Vec3, Vec3]]:
"""12 edges of an axis-aligned box centred at *centre*."""
cx, cy, cz = float(centre.x), float(centre.y), float(centre.z)
ex, ey, ez = float(extents.x), float(extents.y), float(extents.z)
c000 = Vec3(cx - ex, cy - ey, cz - ez)
c100 = Vec3(cx + ex, cy - ey, cz - ez)
c010 = Vec3(cx - ex, cy + ey, cz - ez)
c110 = Vec3(cx + ex, cy + ey, cz - ez)
c001 = Vec3(cx - ex, cy - ey, cz + ez)
c101 = Vec3(cx + ex, cy - ey, cz + ez)
c011 = Vec3(cx - ex, cy + ey, cz + ez)
c111 = Vec3(cx + ex, cy + ey, cz + ez)
return [
(c000, c100), (c100, c110), (c110, c010), (c010, c000), # bottom
(c001, c101), (c101, c111), (c111, c011), (c011, c001), # top
(c000, c001), (c100, c101), (c110, c111), (c010, c011), # uprights
]
def _capsule_wireframe_3d(centre: Vec3, radius: float, height: float) -> list[tuple[Vec3, Vec3]]:
"""Y-axis capsule: ring at each cap centre, plus 4 connecting verticals."""
a, b = _capsule_endpoints(centre, height, radius)
lines: list[tuple[Vec3, Vec3]] = []
# Cap rings (full circles for visual reference: wireframe approximation)
lines.extend(_circle_lines_3d(a, Vec3(1, 0, 0), Vec3(0, 0, 1), radius))
lines.extend(_circle_lines_3d(b, Vec3(1, 0, 0), Vec3(0, 0, 1), radius))
lines.extend(_circle_lines_3d(a, Vec3(1, 0, 0), Vec3(0, 1, 0), radius))
lines.extend(_circle_lines_3d(a, Vec3(0, 0, 1), Vec3(0, 1, 0), radius))
# Side verticals connecting the two cap centres at ±X / ±Z
for ux, uz in ((radius, 0.0), (-radius, 0.0), (0.0, radius), (0.0, -radius)):
lines.append((
Vec3(a.x + ux, a.y, a.z + uz),
Vec3(b.x + ux, b.y, b.z + uz),
))
return lines
# ============================================================================
# Area3D: Trigger zone with enter/exit signals
# ============================================================================
[docs]
class Area3D(_AreaBase, Node3D):
"""3D trigger zone that detects overlapping bodies and areas.
Add CollisionShape3D children to define the detection region.
Emits body_entered/body_exited when CharacterBody3D nodes overlap,
and area_entered/area_exited for other Area3D nodes.
"""
collision_layer = Bitmask(1, group="Collision")
collision_mask = Bitmask(1, group="Collision")
monitoring = Property(True, hint="Whether to detect overlaps", group="Collision")
gizmo_colour = Colour((0.2, 0.6, 1.0, 0.5))
def __init__(self, **kwargs):
super().__init__(**kwargs)
self.body_entered = Signal()
self.body_exited = Signal()
self.area_entered = Signal()
self.area_exited = Signal()
self._overlapping_bodies: set[CharacterBody3D] = set()
self._overlapping_areas: set[Area3D] = set()
[docs]
def get_gizmo_lines(self) -> list[tuple[Vec3, Vec3]]:
"""Return gizmo lines from child collision shapes, or a small axis cross if none."""
lines: list[tuple[Vec3, Vec3]] = []
for child in self.children:
if isinstance(child, CollisionShape3D) and hasattr(child, 'get_gizmo_lines'):
lines.extend(child.get_gizmo_lines())
if not lines:
p = self.world_position
s = 0.5
lines = [
(Vec3(p.x - s, p.y, p.z), Vec3(p.x + s, p.y, p.z)),
(Vec3(p.x, p.y - s, p.z), Vec3(p.x, p.y + s, p.z)),
(Vec3(p.x, p.y, p.z - s), Vec3(p.x, p.y, p.z + s)),
]
return lines
[docs]
def on_physics_process(self, dt: float):
if not self.monitoring or not self._tree:
return
my_shapes = self.find_all(CollisionShape3D)
if not my_shapes:
return
# Check bodies
current_bodies: set[CharacterBody3D] = set()
for body in self._tree.root.find_all(CharacterBody3D):
if not self._layers_match(body) or not body.collision:
continue
body_shapes = (
[body.collision] if isinstance(body.collision, CollisionShape3D) else body.find_all(CollisionShape3D)
)
if self._any_shape_overlaps(my_shapes, body_shapes):
current_bodies.add(body)
for body in current_bodies - self._overlapping_bodies:
self.body_entered(body)
for body in self._overlapping_bodies - current_bodies:
self.body_exited(body)
self._overlapping_bodies = current_bodies
# Check areas
current_areas: set[Area3D] = set()
for area in self._tree.root.find_all(Area3D):
if area is self or not self._layers_match(area):
continue
other_shapes = area.find_all(CollisionShape3D)
if self._any_shape_overlaps(my_shapes, other_shapes):
current_areas.add(area)
for area in current_areas - self._overlapping_areas:
self.area_entered(area)
for area in self._overlapping_areas - current_areas:
self.area_exited(area)
self._overlapping_areas = current_areas