"""BuiltinPhysics2D world: pure-Python 2D backend implementing the 2D seam.
Role
----
Concrete :class:`~simvx.core.physics.world2d.Physics2DWorld` implementation, pure
Python (numpy only). The 2D sibling of
:class:`~simvx.core.physics.builtin.world.BuiltinPhysics`: the engine's default,
always-available 2D backend and the behavioural parity target for the optional
native pymunk (Chipmunk2D) backend.
This is the **basic tier**: semi-implicit (symplectic) Euler integration, scalar
rotation, real per-shape scalar moment of inertia. Where the basic approach
simplifies versus a production solver, the simplification is commented and the
eventual pymunk backend is named; nothing silently fakes a result.
Stage T2b scope
---------------
This sub-stage adds the narrowphase + sequential-impulse contact solver on top of
the T2a integrator: ``step()`` is now integrate -> collide -> solve -> position-
correct (mirroring the 3D :meth:`~simvx.core.physics.builtin.world.BuiltinPhysics.step`).
The 2D narrowphase is genuinely new code: exact circle-circle, ROTATION-honouring
circle-box (a 2D improvement over the 3D AABB approximation), circle/capsule
segment reductions, and the keystone **poly-poly SAT + Sutherland-Hodgman edge
clipping** (box and segment route through the poly path as degenerate polys).
Concave (static edge soup) is broadphase-AABB-culled then tested per candidate
segment. The solver mirrors the 3D structure with 2D scalar-cross-product angular
terms (the 2D system carries a real scalar moment of inertia, so contacts apply
linear AND angular impulse, unlike the linear-only 3D basic tier).
Stage T2c scope
---------------
This sub-stage adds forces, joints, sleeping, and CCD on top of the T2b solver:
- forces: apply_force / apply_torque / apply_impulse (force/torque accumulate and
clear each step; impulse is instantaneous; the lever-arm variants add a scalar
angular term via the 2D cross product).
- joints: create_fixed / pin / hinge / spring / groove joint + remove_joint, solved
in the SAME velocity loop as contacts plus a Baumgarte position pass. (A 2D hinge
== pin this tier since 2D rotation is 1-DOF; motor / limit is a follow-on. The
groove is a 1-DOF slider-on-a-line, the last sub-stage T2g.)
- sleeping: a settled DYNAMIC body sleeps after _SLEEP_TIME and wakes at every
disturbance (set / force / impulse / a live joint / an awake contact).
- CCD: ``continuous`` bodies centre-sweep vs STATIC and clamp to the TOI.
Subsequent sub-stages filled the remaining seam: queries / events / kinematic
sweep (T2d), one-way platforms + the character controller (T2e), the tree / node
integration (T2f), and the groove joint (create_groove_joint, the last sub-stage
T2g, with the GUI demo). No ``NotImplementedError`` seam stubs remain: every
``Physics2DWorld`` method is implemented.
The class is fully instantiable; the integrate -> CCD -> collide -> solve (contacts
+ joints) -> correct -> sleep path (create_body / forces / joints -> step -> read_*)
is complete.
Basic-tier honesty (T2c)
------------------------
- Joints converge in a few iterations (small _VELOCITY_ITERATIONS /
_POSITION_ITERATIONS): a joint chain sags slightly and stiff springs are soft.
Motors, angular limits and breakable joints are deferred to the pymunk
(Chipmunk2D) backend. (Contact warm-starting landed in Stage P2; see the header.)
- CCD is a CENTRE sweep vs STATIC only (no rotational / dynamic-vs-dynamic sweep);
a fast body grazing a corner can still tunnel it. pymunk honours it properly.
Basic-tier honesty (T2b)
------------------------
- SAT manifold on a DEEP first-frame interpenetration can pick a sub-optimal
minimum-overlap axis (the shallowest-overlap axis is not always the true MTV
when two boxes are nearly co-centred); Baumgarte position correction recovers
it over the following steps. A full clipping-with-feature-caching manifold and
robust deep-overlap handling are deferred to the pymunk (Chipmunk2D) backend.
- Warm-starting / accumulated-impulse caching (Stage P2): each contact's per-
manifold-point normal + tangent impulse is cached by the persistent body-pair id
and applied before the iteration loop (standard Box2D technique), so a tall stack
converges in a couple of iterations instead of rebuilding its support from zero.
Full feature-id manifold persistence (vs the point-index keying used here) is the
pymunk concern.
- Capsule / poly / segment / concave scalar moments are the AABB-box estimate
from T2a (``_moment_from_aabb``); the exact per-shape composite is a pymunk
concern.
- Concave broadphase is a linear AABB scan over candidate segments (a BVH is
deferred to pymunk), and concave is STATIC-ONLY: it is never the moving shape.
"""
from __future__ import annotations
import math
from dataclasses import dataclass, field
import numpy as np
from ...math import Vec2
from ..capability import Capability
from ..material import CombineMode, _combine
from ..world import BodyMode, ContactPhase
from ..world2d import (
_DEFAULT_UP_2D,
BodyHandle,
CharacterHandle,
CharacterMoveResult2D,
Contact2D,
ContactEvent2D,
JointHandle,
OverlapEvent2D,
Physics2DWorld,
RaycastHit2D,
ShapeHandle,
)
# Solver tuning (Stage T2b). Copied VERBATIM from the 3D builtin tier
# (builtin/world.py) so the 2D solver converges identically: same Baumgarte bias,
# same penetration slop, same iteration counts, same restitution rest-threshold.
# The 3D module owns the canonical rationale for each value; do not diverge.
_BAUMGARTE = 0.2 # fraction of penetration corrected per step (positional bias)
_SLOP = 0.001 # penetration tolerated without correction (metres), reduces jitter
_RESTITUTION_THRESHOLD = 1.0 # m/s: below this a contact gets e_eff = 0 (no bounce)
_VELOCITY_ITERATIONS = 8 # sequential-impulse velocity passes per step
_POSITION_ITERATIONS = 3 # Baumgarte position passes per step
_JOINT_EPS = 1e-9 # degenerate-direction guard (zero-length spring / anchor delta)
# Sleeping thresholds (Stage T2c). Copied VERBATIM from the 3D builtin tier
# (builtin/world.py): a DYNAMIC body whose linear AND angular speed stay below
# these for _SLEEP_TIME continuous seconds goes to SLEEP (skipped by _integrate
# and the contact / joint velocity solve until woken), killing residual jitter on
# resting stacks. The 3D module owns the canonical rationale; do not diverge.
_SLEEP_LINEAR = 0.05 # m/s: below this (and angular) the sleep timer accumulates
_SLEEP_ANGULAR = 0.05 # rad/s
_SLEEP_TIME = 0.5 # seconds of continuous sub-threshold motion before sleeping
# One-way platform tuning (Stage T2e). A one-way body O only collides bodies
# approaching from its `+one_way_normal` (solid) side. A contact is REJECTED
# (pass-through) when the other body's relative velocity along `+one_way_normal`
# exceeds this small positive epsilon (it is moving UP THROUGH the platform), or
# when the contact normal disagrees with `+one_way_normal` beyond the tolerance
# (it is hitting the underside / a side edge, not landing on top). Basic-tier
# honesty: this is a VELOCITY-GATED filter, not continuous one-way CCD. A body
# faster than platform-thickness/dt can still pop through between substeps; full
# continuous one-way handling is deferred to the pymunk (Chipmunk2D) backend.
_ONE_WAY_VEL_EPS = 0.01 # m/s: rel-velocity along +normal above this = passing through
_ONE_WAY_NORMAL_TOL = 0.1 # contact-vs-platform normal disagreement tolerance (1 - cos)
def _as_array2(v: object) -> np.ndarray:
"""Coerce a Vec2 / sequence to a float32 ``(2,)`` array (always a copy)."""
return np.array(v, dtype=np.float32).reshape(2)
def _layers_match(layer_a: int, mask_a: int, layer_b: int, mask_b: int) -> bool:
"""Canonical body-body pair-acceptance test (AND rule), copied from 3D.
A pair (a, b) collides iff BOTH bodies opt in to the other:
``(mask_a & layer_b) and (mask_b & layer_a)``. Symmetric, matches Box2D /
Rapier and the 3D ``_layers_match`` verbatim. One-directional SENSOR / query
filtering is a separate convention living in the query paths (T2d), not here.
"""
return bool((mask_a & layer_b) and (mask_b & layer_a))
def _cross_2d(a: np.ndarray, b: np.ndarray) -> float:
"""Scalar 2D cross product ``a.x * b.y - a.y * b.x``.
The 2D analogue of the 3D vector cross: it returns the signed z-component the
two in-plane vectors would produce. Used for the contact effective-mass lever
arms (``cross(r, n)``) and the angular impulse application.
"""
return float(a[0] * b[1] - a[1] * b[0])
def _rotate_2d(v: np.ndarray, cos: float, sin: float) -> np.ndarray:
"""Rotate a ``(2,)`` vector by the rotation ``[[cos, -sin], [sin, cos]]``."""
return np.array([cos * v[0] - sin * v[1], sin * v[0] + cos * v[1]], dtype=np.float32)
def _perp_2d(r: np.ndarray) -> np.ndarray:
"""Left perpendicular ``[-ry, rx]`` of a ``(2,)`` vector.
The 2D analogue of ``omega x r`` for a SCALAR spin ``omega``: the velocity a
point at offset ``r`` gains from a unit CCW spin is ``perp(r)``, so
``omega x r == omega * perp(r)``. Used by the point-constraint velocity solve
(the 2D sibling of the 3D ``np.cross(omega, r)``).
"""
return np.array([-r[1], r[0]], dtype=np.float32)
def _closest_point_on_segment_2d(p: np.ndarray, a: np.ndarray, b: np.ndarray) -> np.ndarray:
"""Closest point to ``p`` on the segment ``[a, b]`` (float32 (2,)).
The 2D reduction of the 3D ``_closest_point_on_segment``: identical clamp-the-
projection-parameter maths over ``(2,)`` arrays. Degenerate-segment guard: a
~zero-length segment returns ``a`` (so a degenerate capsule == circle never
divides by zero).
"""
ab = b - a
denom = float(np.dot(ab, ab))
if denom < 1e-12:
return a.astype(np.float32)
t = float(np.dot(p - a, ab)) / denom
t = min(1.0, max(0.0, t))
out: np.ndarray = a + t * ab
return out.astype(np.float32)
def _closest_points_on_segments_2d(
p1: np.ndarray, q1: np.ndarray, p2: np.ndarray, q2: np.ndarray
) -> tuple[np.ndarray, np.ndarray]:
"""Closest points between segments ``[p1, q1]`` and ``[p2, q2]`` (float32 (2,)).
The 2D reduction of the 3D ``_closest_points_on_segments`` (Ericson RTCD
S5.1.9): identical both-degenerate / one-degenerate / parallel-``det~0``
handling with the clamp-then-recompute step, over ``(2,)`` arrays.
"""
eps = 1e-12
d1 = q1 - p1
d2 = q2 - p2
r = p1 - p2
a = float(np.dot(d1, d1))
e = float(np.dot(d2, d2))
f = float(np.dot(d2, r))
if a <= eps and e <= eps:
return p1.astype(np.float32), p2.astype(np.float32)
if a <= eps:
s = 0.0
t = min(1.0, max(0.0, f / e))
else:
c = float(np.dot(d1, r))
if e <= eps:
t = 0.0
s = min(1.0, max(0.0, -c / a))
else:
b = float(np.dot(d1, d2))
denom = a * e - b * b # always >= 0
s = min(1.0, max(0.0, (b * f - c * e) / denom)) if denom > eps else 0.0
t = (b * s + f) / e
if t < 0.0:
t = 0.0
s = min(1.0, max(0.0, -c / a))
elif t > 1.0:
t = 1.0
s = min(1.0, max(0.0, (b - c) / a))
c1 = p1 + d1 * s
c2 = p2 + d2 * t
return c1.astype(np.float32), c2.astype(np.float32)
@dataclass(slots=True)
class _Shape2D:
"""Internal 2D collision shape.
``kind`` discriminates the geometry; ``params`` holds the analytic scalars as
a single float32 array whose layout depends on the kind, and ``points`` holds
the vertex / segment data for the kinds that need it:
- ``"circle"``: ``params = [radius]``; ``points = None``.
- ``"box"``: ``params = [hx, hy]`` (half-extents); ``points = None``.
- ``"capsule"``: ``params = [radius, half_len]`` where
``half_len = max(0.0, height / 2 - radius)`` is the Y-axis central-segment
half-length (endpoints ``centre +- [0, half_len]``); ``points = None``.
- ``"segment"``: ``params = [radius]`` (thickness); ``points = (2, 2)`` the
two endpoints ``[a, b]`` (body-local).
- ``"poly"``: ``params = [hx, hy]`` local AABB half-extents of the cloud (for
the moment estimate + broadphase culling); ``points = (N, 2)`` CCW vertices.
- ``"concave"``: ``params = [hx, hy]`` overall AABB half-extents; ``points =
(N, 2, 2)`` segment soup (each ``[start, end]``). STATIC-ONLY.
"""
kind: str # "circle" | "box" | "capsule" | "segment" | "poly" | "concave"
params: np.ndarray # float32 analytic parameters (layout per `kind`)
points: np.ndarray | None = None # vertex / segment data for segment/poly/concave
@dataclass(slots=True)
class _Body2D:
"""Internal 2D rigid body.
Pose and velocity are plain float32 arrays / Python floats so the integrator
can do vector maths without per-op ``Vec2`` wrapping. Rotation is a SCALAR
angle in radians; angular velocity / torque / moment are scalars.
``inverse_mass`` / ``inverse_moment`` are 0 for ``STATIC`` / ``KINEMATIC``
(infinite mass / inertia), so one impulse formula handles every body-type
pairing (consumed by the T2b solver).
"""
shape: _Shape2D
body_type: BodyMode
position: np.ndarray # float32 (2,)
rotation: float # radians (CCW positive)
mass: float
inverse_mass: float
moment: float # scalar moment of inertia (kg*m^2); real per-shape value
inverse_moment: float # 1 / moment for DYNAMIC, else 0 (infinite)
linear_velocity: np.ndarray = field(default_factory=lambda: np.zeros(2, dtype=np.float32))
angular_velocity: float = 0.0 # radians/s (CCW positive)
# Continuous force/torque accumulators (filled by apply_force/apply_torque in
# T2c, consumed as acceleration in _integrate, cleared each step).
force: np.ndarray = field(default_factory=lambda: np.zeros(2, dtype=np.float32))
torque: float = 0.0 # scalar N*m
# Layer/mask filtering (defaulted so transient probe bodies need not pass them).
collision_layer: int = 0x00000001
collision_mask: int = 0xFFFFFFFF
# Sensor (trigger) flag (consumed by the T2d overlap stream).
is_sensor: bool = False
# Surface material coefficients (consumed by the T2b solver), stored verbatim.
friction: float = 0.5
restitution: float = 0.0
friction_combine: CombineMode = CombineMode.AVERAGE
restitution_combine: CombineMode = CombineMode.AVERAGE
# CCD flag (consumed by the T2c sweep). Defaulted False (discrete).
continuous: bool = False
# Sleeping (T2c adds the sleep update). For T2a a body's asleep flag stays
# False: nothing sets it until the post-solve sleep pass exists.
asleep: bool = False
_sleep_timer: float = 0.0
# One-way platform (T2e). Per-body flag + world-space "solid side" normal.
# Defaulted off; set_one_way (T2e) flips it.
one_way: bool = False
one_way_normal: np.ndarray = field(default_factory=lambda: np.array([0.0, 1.0], dtype=np.float32))
@dataclass(slots=True)
class _Contact2D:
"""A resolved 2D contact between two bodies in narrow phase (T2b).
2D sibling of :class:`~simvx.core.physics.builtin.world._Contact`. ``normal``
points from body ``a`` toward body ``b`` (unit length, world space); pushing
``a`` along ``-normal`` and ``b`` along ``+normal`` separates them. ``points``
is the world-space contact point list (1 for a vertex/circle feature, up to 2
for a clipped poly edge), used as the lever arm for the angular impulse.
"""
a: BodyHandle
b: BodyHandle
normal: np.ndarray # float32 (2,), unit, a -> b
depth: float # penetration depth (> 0)
points: list[np.ndarray] # world-space contact point(s), float32 (2,)
# Warm-starting (Stage P2). Per-manifold-point accumulated clamped NORMAL and
# TANGENT (friction) impulse magnitudes, carried across steps (keyed by the
# canonical body-pair id + point index) and applied before the iteration loop,
# then written back, so a resting 2D stack converges in a couple of iterations
# instead of rebuilding its support from zero each frame. ``vbias`` is the
# per-point restitution velocity bias, computed ONCE pre-solve. Unlike the 3D
# linear-only tier, the 2D solver carries a stable per-point lever arm and a
# well-defined in-plane tangent (perpendicular to the contact normal), so BOTH
# normal and friction warm-start cleanly. All default empty / zero so a fresh
# contact warm-starts with nothing (the common non-stacking case is unchanged);
# ``_solve_contact`` sizes them to ``len(points)`` on first use.
jn: list[float] = field(default_factory=list) # accumulated normal impulse per point
jt: list[float] = field(default_factory=list) # accumulated tangent (friction) impulse per point
vbias: list[float] = field(default_factory=list) # restitution velocity bias per point
@dataclass(slots=True)
class _Character2D:
"""Internal 2D character controller (CharacterVirtual-style) (Stage T2e).
2D sibling of :class:`~simvx.core.physics.builtin.world._Character`. Collides
against world bodies via the narrowphase but is NEVER in :attr:`_bodies`, so the
solver / bulk readers / event diffs never see it (the node holds the handle,
like the 3D character). Holds its own pose plus the fixed collider config
captured at create time. Rotation is a SCALAR (2D); ``slope_limit`` is radians.
"""
shape: _Shape2D
position: np.ndarray # float32 (2,)
rotation: float # radians (CCW positive)
up: np.ndarray # float32 (2,), unit
slope_limit: float # radians
step_height: float
skin_width: float
collision_layer: int = 0x00000001
collision_mask: int = 0xFFFFFFFF
def _moment_circle(mass: float, radius: float) -> float:
"""Solid-disc moment of inertia about its centre: ``0.5 * m * r^2``."""
return 0.5 * mass * radius * radius
def _moment_box(mass: float, hx: float, hy: float) -> float:
"""Solid-rectangle moment about its centre: ``m * (w^2 + h^2) / 12``.
``hx`` / ``hy`` are HALF-extents, so full width ``w = 2*hx`` and height
``h = 2*hy``: ``m * ((2hx)^2 + (2hy)^2) / 12 = m * (hx^2 + hy^2) / 3``.
"""
return mass * (hx * hx + hy * hy) / 3.0
def _moment_capsule(mass: float, radius: float, half_len: float) -> float:
"""Reasonable scalar moment for a Y-axis capsule about its centre.
Basic-tier honesty: rather than the exact rod+two-caps composite, the capsule
is treated as the box that bounds it (full size ``2*radius`` by
``2*(half_len + radius)``), giving a sensible scalar that scales correctly
with both radius and length. The exact composite (and an inertia tensor for
the rounded caps) is deferred to the pymunk backend.
"""
hx = radius
hy = half_len + radius
return _moment_box(mass, hx, hy)
def _moment_from_aabb(mass: float, hx: float, hy: float) -> float:
"""Scalar moment estimate for segment / poly / concave via their AABB box.
Basic-tier honesty: a polygon's true moment is the per-triangle composite and
a segment's is the thin-rod formula; both are approximated here by the moment
of their bounding box (``_moment_box``), a cheap scalar that scales with the
shape's extent. Exact per-shape moments are deferred to the pymunk backend.
The AABB half-extents are floored to a small epsilon so an axis-thin shape
(a horizontal segment) still yields a positive, finite moment.
"""
hx = max(float(hx), 1e-4)
hy = max(float(hy), 1e-4)
return _moment_box(mass, hx, hy)
# -- joint / constraint records (Stage T2c) ----------------------------------
#
# 2D siblings of the 3D builtin joint records (builtin/world.py). One @dataclass
# per joint kind, stored in BuiltinPhysics2D._joints keyed by an opaque JointHandle
# from a SEPARATE counter so joint handles never alias body handles (parity with
# the 3D namespace split). Anchors are stored as body-LOCAL offsets in WORLD axes
# captured at create time; the basic tier re-derives the world anchor each step as
# ``pos + r`` and so IGNORES body rotation of the anchor (documented honesty
# caveat, parity with the 3D joints). Angular terms use the real per-shape scalar
# inverse moment of inertia (2D carries a genuine scalar moment, unlike the
# linear-only 3D basic tier), folded into the scalar-cross effective mass.
@dataclass(slots=True)
class _FixedConstraint2D:
"""Weld: lock the full relative pose of ``b`` in ``a``'s frame (2D).
Captures, at create time, the world-axes offset of b's centre from a's centre
(``rel_pos``) and the relative angle ``rel_angle = b.rotation - a.rotation``.
The point part is solved at a's centre (``r_a = 0``); the angular part drives
the relative angular velocity to zero plus a Baumgarte bias toward
``rel_angle``. (3D uses a relative quaternion; 2D rotation is a scalar, so the
relative orientation is a single angle and the angular lock is 1-DOF.)
"""
a: BodyHandle
b: BodyHandle
rel_pos: np.ndarray # float32 (2,), b_centre - a_centre captured at create (world axes)
rel_angle: float # b.rotation - a.rotation captured at create (radians)
@dataclass(slots=True)
class _PinConstraint2D:
"""Point-to-point: keep the two world anchors coincident, rotation free (2D).
``r_a`` / ``r_b`` are the world-axes offsets from each body centre to the
shared anchor captured at create (``anchor - pos``). The world anchor on each
body is re-derived each step as ``pos + r`` (rotation ignored, basic tier).
"""
a: BodyHandle
b: BodyHandle
r_a: np.ndarray # float32 (2,), anchor - pos_a at create
r_b: np.ndarray # float32 (2,), anchor - pos_b at create
@dataclass(slots=True)
class _HingeConstraint2D:
"""Hinge two bodies at an anchor (2D).
In 2D, rotation is a single DOF, so a hinge that leaves rotation free is
BEHAVIOURALLY IDENTICAL to a pin (there is no off-axis rotation to lock): this
record carries only the point part (``r_a`` / ``r_b``) and solves exactly like
:class:`_PinConstraint2D`. A separate record (rather than reusing the pin) keeps
the type distinct so a motor / angular-limit follow-on can extend it without
changing the pin. Motors and angular limits are a DOCUMENTED follow-on, NOT
this tier (mirrors the 3D hinge's deferred motor/limit; pymunk's
``PivotJoint`` + ``SimpleMotor`` / ``RotaryLimitJoint`` is the eventual home).
"""
a: BodyHandle
b: BodyHandle
r_a: np.ndarray # float32 (2,), anchor - pos_a at create
r_b: np.ndarray # float32 (2,), anchor - pos_b at create
@dataclass(slots=True)
class _SpringConstraint2D:
"""Soft distance-spring between the two body CENTRES (compliant, not rigid) (2D).
Pulls the two centres toward ``rest_length`` apart with spring constant
``stiffness`` (N/m) and damping ``damping`` (N*s/m). Solved as a soft velocity
impulse with a ``k*x`` bias; never position-corrected. Identical maths to the
3D ``_SpringConstraint`` over ``(2,)`` vectors.
"""
a: BodyHandle
b: BodyHandle
rest_length: float
stiffness: float
damping: float
@dataclass(slots=True)
class _GrooveConstraint2D:
"""Slider-on-a-line: keep ``b``'s anchor on a groove segment of ``a`` (2D) (T2g).
The pymunk-native ``GrooveJoint`` semantics: ``b``'s local anchor ``anchor_b``
is constrained to lie on the segment ``[groove_a, groove_b]`` defined in ``a``'s
frame. Motion ALONG the groove direction is free; the component PERPENDICULAR to
the groove is cancelled (a pin in one direction only). The anchor is CLAMPED to
the closed segment, so it cannot slide off the ends.
Like the other 2D joints (basic tier, parity with the 3D records), anchors are
captured as body-LOCAL offsets in WORLD axes at create time and the world anchor
is re-derived each step as ``pos + r`` (body rotation of the anchor is IGNORED).
So the groove direction is fixed in world axes at create time rather than
rotating with ``a``: documented honesty caveat (matches the pin/hinge anchor
treatment). Exact rotating-groove handling, endpoint-clamp softness and a slide
motor are deferred to the pymunk backend.
Fields:
ga / gb: groove endpoints as world-axes offsets from ``a``'s centre,
captured at create (``groove_world - pos_a``).
r_b: ``b``'s anchor as a world-axes offset from ``b``'s centre, captured at
create (``anchor_world - pos_b``).
"""
a: BodyHandle
b: BodyHandle
ga: np.ndarray # float32 (2,), groove_a_world - pos_a at create
gb: np.ndarray # float32 (2,), groove_b_world - pos_a at create
r_b: np.ndarray # float32 (2,), anchor_b_world - pos_b at create
# Union of the five 2D constraint records. All carry ``.a`` / ``.b`` body handles
# (the common fields the destroy purge keys on). Groove (the pymunk-native
# slider-on-a-line) is the T2g addition.
_Constraint2D = (
_FixedConstraint2D | _PinConstraint2D | _HingeConstraint2D | _SpringConstraint2D | _GrooveConstraint2D
)
[docs]
class BuiltinPhysics2D(Physics2DWorld):
"""Pure-Python default 2D backend (basic tier).
See :class:`~simvx.core.physics.world2d.Physics2DWorld` for the full contract.
Implements the COMPLETE seam: shapes + bodies + integrator + bulk readers
(T2a), narrowphase + sequential-impulse solver (T2b), forces + joints + sleeping
+ CCD (T2c), queries + events + kinematic sweep (T2d), one-way platforms + the
character controller (T2e), and the groove joint (T2g). No method is a stub.
"""
def __init__(self, *, gravity: Vec2 | None = None) -> None:
# Default Y-up gravity Vec2(0, -9.81); built inside (Vec2 is a mutable
# ndarray subclass, never construct it in an argument default).
if gravity is None:
gravity = Vec2(0.0, -9.81)
super().__init__(gravity=gravity)
self._shapes: dict[ShapeHandle, _Shape2D] = {}
self._bodies: dict[BodyHandle, _Body2D] = {}
self._order: list[BodyHandle] = []
self._next_handle: int = 0
# Joints / constraints (Stage T2c). A SEPARATE handle counter so a joint
# handle never aliases a body handle (parity with the 3D backend). Empty
# for jointless scenes (zero solver cost).
self._joints: dict[JointHandle, _Constraint2D] = {}
self._next_joint: int = 0
# Collision-event diffing (Stage T2d). ``_touching`` is the set of
# currently-overlapping body-body pairs as canonical (a <= b) handle keys,
# carried across steps; ``_contact_events`` is the per-step buffer drained
# by ``drain_contact_events``. Empty for non-overlapping scenes. 2D sibling
# of the 3D backend's ``_touching`` / ``_contact_events``.
self._touching: set[tuple[BodyHandle, BodyHandle]] = set()
self._contact_events: list[ContactEvent2D] = []
# Sensor-overlap diffing (Stage T2d): a SECOND, independent stream of
# DIRECTED ``(sensor, other)`` keys (one-directional, the sensor decides
# via its mask). ``_overlapping`` carries across steps; ``_overlap_events``
# is the per-step buffer drained by ``drain_overlap_events``.
self._overlapping: set[tuple[BodyHandle, BodyHandle]] = set()
self._overlap_events: list[OverlapEvent2D] = []
# Character controllers (Stage T2e). A SEPARATE handle counter so a
# character handle never aliases a body handle (parity with the 3D
# backend); characters are NOT in ``_bodies`` and so never appear in the
# solver, the bulk readers, or the event diffs. Empty for character-free
# scenes.
self._characters: dict[CharacterHandle, _Character2D] = {}
self._next_char: int = 0
# Warm-start cache (Stage P2): persistent accumulated contact impulses keyed
# by the canonical body-pair id -> (jn list, jt list) over the manifold
# points. Each step seeds the step's contacts from last step's value,
# applies it as a warm-start, then writes the converged value back; pairs
# that stopped touching are dropped. Empty for a scene with no resting
# contacts (zero cost). 2D sibling of the 3D backend's ``_warm_contacts``.
self._warm_contacts: dict[tuple[BodyHandle, BodyHandle], tuple[list[float], list[float]]] = {}
[docs]
def capabilities(self) -> frozenset[Capability]:
"""No Tier-3 capabilities: the builtin 2D tier is an honest rigid-body solver.
2D sibling of :meth:`BuiltinPhysics.capabilities`: advertises NONE of
:class:`Capability`. Explicit (not inherited) so the claim is a deliberate,
tested promise.
"""
return frozenset()
[docs]
@property
def body_count(self) -> int:
"""Number of bodies currently in the world (``len`` of the body table)."""
return len(self._bodies)
[docs]
def clear(self) -> None:
"""Remove every body, character, and joint, emptying the world.
See :meth:`~simvx.core.physics.world2d.Physics2DWorld.clear`. 2D sibling of
:meth:`BuiltinPhysics.clear`: resets the body / character / joint tables and
the per-step edge-diff + warm-start caches. Gravity, shapes, and handle
counters are intentionally NOT reset.
"""
self._bodies.clear()
self._characters.clear()
self._joints.clear()
self._order.clear()
self._touching.clear()
self._contact_events.clear()
self._overlapping.clear()
self._overlap_events.clear()
self._warm_contacts.clear()
def _alloc_handle(self) -> int:
h = self._next_handle
self._next_handle += 1
return h
# -- shapes -------------------------------------------------------------
[docs]
def create_circle(self, radius: float) -> ShapeHandle:
assert radius > 0.0, f"circle radius must be > 0, got {radius}"
handle = self._alloc_handle()
self._shapes[handle] = _Shape2D("circle", np.array([radius], dtype=np.float32))
return handle
[docs]
def create_box(self, half_extents: Vec2) -> ShapeHandle:
he = _as_array2(half_extents)
assert np.all(he > 0.0), f"box half_extents must all be > 0, got {tuple(he)}"
handle = self._alloc_handle()
self._shapes[handle] = _Shape2D("box", he.copy())
return handle
[docs]
def create_capsule(self, radius: float, height: float) -> ShapeHandle:
assert radius > 0.0 and height > 0.0, f"capsule radius/height must be > 0, got {radius}, {height}"
half_len = max(0.0, height * 0.5 - radius)
handle = self._alloc_handle()
self._shapes[handle] = _Shape2D("capsule", np.array([radius, half_len], dtype=np.float32))
return handle
[docs]
def create_segment(self, a: Vec2, b: Vec2, radius: float = 0.0) -> ShapeHandle:
assert radius >= 0.0, f"segment radius must be >= 0, got {radius}"
pa = _as_array2(a)
pb = _as_array2(b)
assert float(np.dot(pb - pa, pb - pa)) > 1e-12, "segment endpoints must differ"
handle = self._alloc_handle()
self._shapes[handle] = _Shape2D(
"segment", np.array([radius], dtype=np.float32), points=np.stack([pa, pb]).astype(np.float32)
)
return handle
[docs]
def create_convex_polygon(self, points: np.ndarray) -> ShapeHandle:
pts = np.asarray(points, dtype=np.float32).reshape(-1, 2)
assert pts.shape[1] == 2, f"polygon points must be (N, 2), got {pts.shape}"
assert pts.shape[0] >= 3, f"convex polygon needs >= 3 points, got {pts.shape[0]}"
lo = pts.min(axis=0)
hi = pts.max(axis=0)
aabb_half = ((hi - lo) * 0.5).astype(np.float32)
handle = self._alloc_handle()
self._shapes[handle] = _Shape2D("poly", aabb_half, points=pts.copy())
return handle
[docs]
def create_concave_polygon(self, segments: np.ndarray) -> ShapeHandle:
segs = np.asarray(segments, dtype=np.float32).reshape(-1, 2, 2)
assert segs.shape[0] >= 1, f"concave polygon needs >= 1 segment, got {segs.shape[0]}"
flat = segs.reshape(-1, 2)
lo = flat.min(axis=0)
hi = flat.max(axis=0)
aabb_half = ((hi - lo) * 0.5).astype(np.float32)
handle = self._alloc_handle()
self._shapes[handle] = _Shape2D("concave", aabb_half, points=segs.copy())
return handle
# -- bodies -------------------------------------------------------------
def _shape_moment(self, shape: _Shape2D, mass: float) -> float:
"""Real per-shape scalar moment of inertia about the body centre.
- circle -> 0.5 * m * r^2
- box -> m * (w^2 + h^2) / 12 (w/h full extents)
- capsule -> bounding-box moment (radius x (half_len + radius)) (basic tier)
- segment / poly / concave -> AABB-box moment estimate (basic tier)
"""
kind = shape.kind
if kind == "circle":
return _moment_circle(mass, float(shape.params[0]))
if kind == "box":
return _moment_box(mass, float(shape.params[0]), float(shape.params[1]))
if kind == "capsule":
return _moment_capsule(mass, float(shape.params[0]), float(shape.params[1]))
# segment / poly / concave: AABB-box estimate (deferred to pymunk).
return _moment_from_aabb(mass, float(shape.params[0]), float(shape.params[1]))
[docs]
def create_body(
self,
shape: ShapeHandle,
body_type: BodyMode,
transform: object,
*,
mass: float = 1.0,
collision_layer: int = 1,
collision_mask: int = 0xFFFFFFFF,
is_sensor: bool = False,
friction: float = 0.5,
restitution: float = 0.0,
friction_combine: CombineMode = CombineMode.AVERAGE,
restitution_combine: CombineMode = CombineMode.AVERAGE,
continuous: bool = False,
) -> BodyHandle:
assert shape in self._shapes, f"unknown shape handle {shape!r}"
shp = self._shapes[shape]
# Concave polygons are STATIC-ONLY colliders (the 2D analogue of the 3D
# static triangle mesh): no inertia, level geometry. Single choke point.
if shp.kind == "concave" and body_type is not BodyMode.STATIC:
raise ValueError(
f"ConcavePolygonShape2D (segment soup) is a STATIC-only collider; got body_type={body_type}. "
"Use a primitive or convex polygon for DYNAMIC/KINEMATIC bodies."
)
position, rotation = self._unpack_transform(transform)
if body_type is BodyMode.DYNAMIC:
assert mass > 0.0, f"dynamic body mass must be > 0, got {mass}"
inv_mass = 1.0 / mass
moment = self._shape_moment(shp, mass)
inv_moment = 1.0 / moment if moment > 0.0 else 0.0
else:
# STATIC and KINEMATIC are treated as infinite mass / inertia.
inv_mass = 0.0
moment = 0.0
inv_moment = 0.0
handle = self._alloc_handle()
self._bodies[handle] = _Body2D(
shape=shp,
body_type=body_type,
position=position,
rotation=rotation,
mass=mass,
inverse_mass=inv_mass,
moment=moment,
inverse_moment=inv_moment,
collision_layer=collision_layer,
collision_mask=collision_mask,
is_sensor=is_sensor,
friction=friction,
restitution=restitution,
friction_combine=friction_combine,
restitution_combine=restitution_combine,
continuous=continuous,
)
return handle
[docs]
def destroy_body(self, handle: BodyHandle) -> None:
# Idempotent: freeing an already-removed body is a safe no-op (e.g. clear()
# emptied the world, then a node's on_exit_tree destroys its stale handle).
if handle not in self._bodies:
return
del self._bodies[handle]
if handle in self._order:
self._order.remove(handle)
# Drop the destroyed body from the event-diff sets (Stage T2d) so the next
# step's diff never references a freed handle. A still-touching pair simply
# vanishes from ``_touching`` here (no EXIT event is synthesised for a body
# that no longer exists: the node was destroyed, so there is nothing to
# notify); the remaining body re-diffs cleanly. 2D sibling of the 3D purge.
if self._touching:
self._touching = {pair for pair in self._touching if handle not in pair}
if self._overlapping:
self._overlapping = {pair for pair in self._overlapping if handle not in pair}
# Silently drop any joint referencing the destroyed body (parity with the
# 3D backend): a joint must never solve against a freed body if the node
# teardown order leaves it briefly alive. This is the safety net behind
# remove_joint's no-op-on-unknown contract, so the joint node's own
# on_exit_tree idempotently no-ops in either teardown order.
if self._joints:
self._joints = {h: j for h, j in self._joints.items() if j.a != handle and j.b != handle}
[docs]
def set_body_transform(self, handle: BodyHandle, transform: object) -> None:
body = self._bodies[handle]
body.position, body.rotation = self._unpack_transform(transform)
self._wake(body) # a teleport is a disturbance: wake a sleeping body
[docs]
def set_body_velocity(self, handle: BodyHandle, linear: Vec2, angular: float = 0.0) -> None:
body = self._bodies[handle]
body.linear_velocity = _as_array2(linear)
body.angular_velocity = float(angular)
self._wake(body) # a direct velocity write is a disturbance
[docs]
def set_body_mode(self, handle: BodyHandle, mode: BodyMode) -> None:
body = self._bodies[handle]
# Re-assert the concave static-only contract (set_body_mode bypasses
# create_body): a live concave body can never be flipped off STATIC.
if body.shape.kind == "concave" and mode is not BodyMode.STATIC:
raise ValueError(
f"ConcavePolygonShape2D (segment soup) is a STATIC-only collider; cannot set body_mode={mode}."
)
body.body_type = mode
if mode is BodyMode.DYNAMIC:
assert body.mass > 0.0, f"dynamic body mass must be > 0, got {body.mass}"
body.inverse_mass = 1.0 / body.mass
body.moment = self._shape_moment(body.shape, body.mass)
body.inverse_moment = 1.0 / body.moment if body.moment > 0.0 else 0.0
else:
body.inverse_mass = 0.0 # STATIC / KINEMATIC -> infinite mass / inertia
body.moment = 0.0
body.inverse_moment = 0.0
self._wake(body)
[docs]
def body_velocity(self, handle: BodyHandle) -> tuple[Vec2, float]:
body = self._bodies[handle]
return Vec2(body.linear_velocity), float(body.angular_velocity)
[docs]
def body_transform(self, handle: BodyHandle) -> tuple[Vec2, float]:
body = self._bodies[handle]
return Vec2(body.position), float(body.rotation)
[docs]
def sleeping(self, handle: BodyHandle) -> bool:
# STATIC / KINEMATIC bodies keep asleep=False (never integrated, never
# woken): they are 'always immovable', not 'asleep'. A sleeping DYNAMIC body
# remains a full collider and reports its frozen pose via read_transforms.
return self._bodies[handle].asleep
def _wake(self, body: _Body2D) -> None:
"""Wake a sleeping body: clear the asleep flag and reset its sleep timer.
A no-op (cheap) for an already-awake body, so every disturbance site
(set_body_transform / set_body_velocity / set_body_mode / apply_* / a live
joint / an awake-vs-sleeper contact) can call it unconditionally.
"""
body.asleep = False
body._sleep_timer = 0.0
@staticmethod
def _unpack_transform(transform: object) -> tuple[np.ndarray, float]:
"""Extract (position float32 (2,), rotation float radians) from input.
Accepts a ``Transform2D`` (``.position`` + scalar ``.rotation``), a bare
``Vec2`` / sequence (position only, zero rotation), or a
``(position, rotation)`` pair where rotation is a scalar in radians.
"""
pos = getattr(transform, "position", None)
if pos is not None:
rot = getattr(transform, "rotation", 0.0)
return _as_array2(pos), float(rot)
if isinstance(transform, tuple) and len(transform) == 2:
p, r = transform
# A 2-tuple is ambiguous: (Vec2-pair == position) vs (position, angle).
# Disambiguate by the second element: a scalar is the rotation angle, a
# 2-vector means the tuple itself is a bare position.
if isinstance(r, int | float):
return _as_array2(p), float(r)
return _as_array2(transform), 0.0
# Bare position (Vec2 / sequence): zero rotation.
return _as_array2(transform), 0.0
# -- stepping (T2b: integrate -> collide -> solve -> position-correct) ---
[docs]
def step(self, dt: float) -> None:
# integrate -> collide -> sequential-impulse solve (contacts + joints) ->
# Baumgarte position correction -> contact/overlap event diff -> post-solve
# sleep pass, mirroring the 3D step() loop exactly.
# Clear last step's buffered events at the top: a caller that never drains
# still gets a per-step (not accumulating) buffer (3D parity).
self._contact_events = []
self._overlap_events = []
self._integrate(dt)
contacts = self._collide()
# Live overlap set BEFORE resolve, so the manifold geometry is the pre-solve
# contact. Canonical key -> _Contact2D (one per pair from _collide).
current: dict[tuple[BodyHandle, BodyHandle], _Contact2D] = {self._canon(c.a, c.b): c for c in contacts}
# Pre-solve relative velocity per pair (b w.r.t. a, in the contact's own
# a->b orientation) captured before the solver mutates velocities.
rel_vel: dict[tuple[BodyHandle, BodyHandle], np.ndarray] = {}
for key, c in current.items():
ba, bb = self._bodies[c.a], self._bodies[c.b]
rel_vel[key] = (bb.linear_velocity - ba.linear_velocity).astype(np.float32)
impulses: dict[tuple[BodyHandle, BodyHandle], float] = {}
if contacts or self._joints:
self._solve(contacts, dt, impulses)
# Rebuild the warm-start cache (Stage P2) from THIS step's contacts, always:
# a step with no contacts (every pair separated) must clear it so a stale
# pair never warm-starts a contact that no longer exists. The converged
# per-point accumulators live on the _Contact2D objects _solve mutated.
self._warm_contacts = {self._canon(c.a, c.b): (list(c.jn), list(c.jt)) for c in contacts}
self._diff_contacts(current, rel_vel, impulses)
# Sensor overlap pass (Stage T2d): a SEPARATE one-directional sweep, run
# after the collision pass (resolution is already done; overlaps carry no
# manifold and apply no impulse). Feeds its own edge-diffed event stream.
self._diff_overlaps(self._collide_sensors())
# Sleep pass (Stage T2c): evaluate the sub-threshold sleep timer on the
# SETTLED (post-solve) velocity. A resting body's pre-solve velocity still
# carries this step's gravity impulse (cancelled by the contact solver), so
# the test must run here, not inside _integrate (mirrors the 3D ordering).
self._update_sleeping(dt)
# -- collision events (broadphase-diffed edges) (T2d) ------------------
def _diff_contacts(
self,
current: dict[tuple[BodyHandle, BodyHandle], _Contact2D],
rel_vel: dict[tuple[BodyHandle, BodyHandle], np.ndarray],
impulses: dict[tuple[BodyHandle, BodyHandle], float],
) -> None:
"""Diff this step's overlaps against ``_touching`` and buffer edge events.
2D sibling of the 3D ``_diff_contacts``: ENTER = newly overlapping pairs,
EXIT = pairs that stopped overlapping. Stored ``_touching`` keys keep
canonical handle order so EXIT reports the same ids. Pairs are already
layer/mask-filtered and sensor-gated by ``_collide``, so the filtering is
inherited for free (sensors feed the SEPARATE overlap stream, never here).
"""
live = current.keys()
for key in live - self._touching:
c = current[key]
ba = self._bodies[c.a]
# Contact point: the overlap midpoint along the a->b normal (a's surface
# toward b offset inward by half the depth), mirroring the 3D payload.
point = (ba.position + c.normal * (-0.5 * c.depth)).astype(np.float32)
self._contact_events.append(
ContactEvent2D(
a=c.a,
b=c.b,
phase=ContactPhase.ENTER,
point=Vec2(point),
normal=Vec2(c.normal),
impulse=float(impulses.get(key, 0.0)),
rel_velocity=Vec2(rel_vel[key]),
)
)
zero = Vec2(0.0, 0.0)
for key in self._touching - live:
a, b = key
self._contact_events.append(
ContactEvent2D(
a=a, b=b, phase=ContactPhase.EXIT, point=zero, normal=zero, impulse=0.0, rel_velocity=zero
)
)
self._touching = set(live)
# -- sensor overlap (broadphase-driven, one-directional) ---------------
def _collide_sensors(self) -> set[tuple[BodyHandle, BodyHandle]]:
"""One-directional sensor overlap sweep over all pairs (basic tier).
2D sibling of the 3D ``_collide_sensors``: a SECOND O(n^2) sweep, SEPARATE
from ``_collide``'s AND-filtered collision pass (kept separate so the
one-directional sensor filter never contaminates the body-body AND rule).
For any pair where at least one body is a sensor it tests detection per the
design: a sensor ``S`` detects a body ``O`` iff
``S.collision_mask & O.collision_layer`` (the observer decides; ``O``'s mask
is irrelevant). Pure body-body pairs are skipped (the collision pass's job).
Sensor-vs-sensor checks BOTH directions independently (each passing
direction emits its own directed key). The ``inverse_mass == 0`` early-out
from ``_collide`` is NOT applied here: a static sensor over a static body
must still detect (overlap needs no movable body). The narrowphase only
needs a boolean overlap, so a single ``_narrow(...) is not None`` per pair
reuses the exact tier-honest geometry collisions use. Returns the live set
of directed ``(sensor, other)`` keys this step. Asleep bodies are full
colliders, so a sensor still detects a sleeping body (no asleep gate).
"""
live: set[tuple[BodyHandle, BodyHandle]] = set()
items = list(self._bodies.items())
for i in range(len(items)):
ha, ba = items[i]
for j in range(i + 1, len(items)):
hb, bb = items[j]
# At least one must be a sensor; pure body-body is the collision
# pass's job. Concave (STATIC edge soup) routes through _narrow fine.
if not (ba.is_sensor or bb.is_sensor):
continue
# Overlap is symmetric, so test the geometry once per pair.
if self._narrow(ha, ba, hb, bb) is None:
continue
# Emit a directed key per passing detection direction.
if ba.is_sensor and (ba.collision_mask & bb.collision_layer):
live.add((ha, hb))
if bb.is_sensor and (bb.collision_mask & ba.collision_layer):
live.add((hb, ha))
return live
def _diff_overlaps(self, live: set[tuple[BodyHandle, BodyHandle]]) -> None:
"""Diff this step's directed sensor overlaps and buffer edge events.
2D sibling of the 3D ``_diff_overlaps``: ENTER = newly overlapping directed
keys, EXIT = directed keys that stopped overlapping. Over DIRECTED
``(sensor, other)`` keys (never canonicalised, or sensor-vs-sensor's two
directions would collapse and thrash). Filtering is inherited from
``_collide_sensors``.
"""
for sensor, other in live - self._overlapping:
self._overlap_events.append(OverlapEvent2D(sensor=sensor, other=other, phase=ContactPhase.ENTER))
for sensor, other in self._overlapping - live:
self._overlap_events.append(OverlapEvent2D(sensor=sensor, other=other, phase=ContactPhase.EXIT))
self._overlapping = live
[docs]
def drain_overlap_events(self) -> list[OverlapEvent2D]:
events = self._overlap_events
self._overlap_events = []
return events
def _integrate(self, dt: float) -> None:
"""Semi-implicit (symplectic) Euler: velocity first, then position.
DYNAMIC bodies take gravity + accumulated force as linear acceleration and
torque * inverse_moment as angular acceleration, then integrate position
and scalar rotation. KINEMATIC bodies integrate their SET velocity only
(immune to gravity / forces). STATIC bodies are never integrated.
Accumulated force/torque is cleared at the end so a continuous force lasts
exactly one step (re-add per fixed step; T2c fills apply_force/torque).
"""
gravity = np.asarray(self._gravity, dtype=np.float32)
for body in self._bodies.values():
if body.body_type is BodyMode.DYNAMIC:
if body.asleep:
# Asleep: frozen, still a collider. Clear stray accumulators.
body.force[:] = 0.0
body.torque = 0.0
continue
accel = gravity + body.force * body.inverse_mass
body.linear_velocity = body.linear_velocity + accel * dt
body.angular_velocity = body.angular_velocity + body.torque * body.inverse_moment * dt
target = body.position + body.linear_velocity * dt
# CCD (Stage T2c): a `continuous` body sweeps its centre old->target
# vs STATIC geometry and clamps to the TOI so it cannot tunnel a thin
# wall this step; the discrete _collide/_solve then resolves the
# resulting resting contact normally. A discrete body writes target
# directly (resting / stacking unchanged).
body.position = self._ccd_advance(body, target) if body.continuous else target
body.rotation = body.rotation + body.angular_velocity * dt
elif body.body_type is BodyMode.KINEMATIC:
# Code-moved: integrate its set velocity, immune to gravity/forces.
body.position = body.position + body.linear_velocity * dt
body.rotation = body.rotation + body.angular_velocity * dt
# STATIC: never integrated.
# Auto-clear accumulators for ALL bodies (harmless for infinite-mass
# ones, which never accept a force): continuous forces last one step.
body.force[:] = 0.0
body.torque = 0.0
# -- collision detection (broad + narrow) (T2b) ------------------------
@staticmethod
def _canon(a: BodyHandle, b: BodyHandle) -> tuple[BodyHandle, BodyHandle]:
"""Stable canonical ordering for a body pair (handles are ints)."""
return (a, b) if a <= b else (b, a)
@staticmethod
def _aabb(body: _Body2D) -> tuple[np.ndarray, np.ndarray]:
"""World-space AABB ``(lo, hi)`` of a body's shape (broadphase cull).
Uses the shape's stored local AABB half-extents (params for box/poly/
concave, derived for circle/capsule/segment) expanded by the rotation:
for a rotated box/poly the world AABB is the local half-extents projected
onto the world axes, ``|R| @ half`` with ``|R|`` the absolute rotation
matrix. Circles are rotation-invariant. A loose-but-correct conservative
bound is fine here: it only culls pairs before the exact narrowphase.
"""
shp = body.shape
kind = shp.kind
if kind == "circle":
r = float(shp.params[0])
half = np.array([r, r], dtype=np.float32)
elif kind == "capsule":
r = float(shp.params[0])
hl = float(shp.params[1])
half = np.array([r, hl + r], dtype=np.float32) # local Y-axis capsule
elif kind == "segment":
r = float(shp.params[0])
pts = shp.points # (2, 2)
lo = pts.min(axis=0) - r
hi = pts.max(axis=0) + r
half = ((hi - lo) * 0.5).astype(np.float32)
else: # box / poly / concave: params are the local AABB half-extents
half = np.asarray(shp.params, dtype=np.float32)
c = math.cos(body.rotation)
s = math.sin(body.rotation)
abs_rot = np.array([[abs(c), abs(s)], [abs(s), abs(c)]], dtype=np.float32)
world_half = abs_rot @ half
# Segment / poly AABB centre is the cloud centre, which (for a non-centred
# cloud) is offset from the body origin; recompute via the local centre.
local_centre = np.zeros(2, dtype=np.float32)
if kind == "segment":
local_centre = shp.points.mean(axis=0).astype(np.float32)
elif kind in ("poly", "concave"):
local_centre = np.zeros(2, dtype=np.float32) # poly stored CCW about origin
centre = body.position + _rotate_2d(local_centre, c, s)
return (centre - world_half).astype(np.float32), (centre + world_half).astype(np.float32)
@staticmethod
def _aabb_overlap(lo_a: np.ndarray, hi_a: np.ndarray, lo_b: np.ndarray, hi_b: np.ndarray) -> bool:
"""Axis-aligned box overlap test (broadphase reject)."""
return bool(np.all(hi_a >= lo_b) and np.all(hi_b >= lo_a))
@staticmethod
def _one_way_rejects(
contact: _Contact2D,
ba: _Body2D,
bb: _Body2D,
vel_a: np.ndarray,
vel_b: np.ndarray,
) -> bool:
"""One-way platform pass-through filter (Stage T2e).
For a contact where one body is a one-way platform ``O``, keep the contact
only when the OTHER body is landing on ``O``'s solid (``+one_way_normal``)
side and reject it (the other body passes straight through) otherwise. The
standard Box2D / Godot rule, applied here AFTER manifold generation and
BEFORE the contact reaches the solver / event diff: a rejected pass-through
contact never collides, so it fires no impulse and no ContactEvent.
Returns ``True`` if the contact should be DISCARDED. A contact is discarded
when, taking the platform as ``O`` and the other body as ``X``:
1. the OTHER body's relative velocity along ``+one_way_normal`` exceeds
``+_ONE_WAY_VEL_EPS`` (``X`` is moving UP THROUGH the platform), OR
2. the contact normal (oriented platform -> other) disagrees with
``+one_way_normal`` beyond ``_ONE_WAY_NORMAL_TOL`` (``X`` is hitting the
underside or a side edge, not landing on top).
If BOTH bodies are one-way the test is run for each as the platform and the
contact is discarded if EITHER rejects it (each platform independently lets
the other pass from its non-solid side). Velocity-gated, not continuous: a
body faster than thickness/dt can pop through; full one-way CCD is a pymunk
concern (see ``_ONE_WAY_VEL_EPS``).
"""
if not (ba.one_way or bb.one_way):
return False
# ``contact.normal`` points a -> b. For platform == a the "platform -> other"
# normal is +normal and the other body is b; for platform == b it is -normal
# and the other body is a. Relative velocity is always (other - platform).
if ba.one_way and BuiltinPhysics2D._one_way_pair_rejects(
ba.one_way_normal, contact.normal, vel_b - vel_a
):
return True
if bb.one_way and BuiltinPhysics2D._one_way_pair_rejects(
bb.one_way_normal, -contact.normal, vel_a - vel_b
):
return True
return False
@staticmethod
def _one_way_pair_rejects(one_way_normal: np.ndarray, platform_to_other: np.ndarray, rel_vel: np.ndarray) -> bool:
"""Core one-way decision for a single platform (see :meth:`_one_way_rejects`).
``platform_to_other`` is the contact normal oriented from the platform body
toward the other body; ``rel_vel`` is the other body's velocity relative to
the platform. Discard when the other body moves along ``+one_way_normal``
faster than the epsilon (passing up through) OR the contact normal does not
agree with ``+one_way_normal`` (it is not a top landing).
"""
n = one_way_normal
# Passing up through the platform from below: relative motion along +normal.
if float(np.dot(rel_vel, n)) > _ONE_WAY_VEL_EPS:
return True
# Not a top landing: the contact normal must align with the solid side.
return float(np.dot(platform_to_other, n)) < 1.0 - _ONE_WAY_NORMAL_TOL
def _collide(self) -> list[_Contact2D]:
"""O(n^2) broadphase (AABB + layer/mask AND-rule) + analytic narrowphase.
Mirrors the 3D ``_collide``: sensors and double-infinite-mass pairs are
gated out before narrowphase, then the layer/mask AND-rule filters, then
the per-kind dispatch produces a ``_Contact2D`` (or ``None``). Adds a cheap
world-AABB overlap reject the 3D version omits (2D narrowphase, esp. SAT,
is more expensive per pair than the 3D analytic primitives, so the AABB
prefilter pays for itself; still O(n^2) pairs, BVH deferred to pymunk).
"""
contacts: list[_Contact2D] = []
items = list(self._bodies.items())
aabbs = {h: self._aabb(b) for h, b in items}
for i in range(len(items)):
ha, ba = items[i]
for j in range(i + 1, len(items)):
hb, bb = items[j]
# Sensors never produce a collision contact (gated before
# narrowphase, exactly like 3D): the overlap stream is T2d.
if ba.is_sensor or bb.is_sensor:
continue
# Two infinite-mass bodies can never be pushed apart: skip.
if ba.inverse_mass == 0.0 and bb.inverse_mass == 0.0:
continue
if not _layers_match(ba.collision_layer, ba.collision_mask, bb.collision_layer, bb.collision_mask):
continue
lo_a, hi_a = aabbs[ha]
lo_b, hi_b = aabbs[hb]
if not self._aabb_overlap(lo_a, hi_a, lo_b, hi_b):
continue
contact = self._narrow(ha, ba, hb, bb)
if contact is None:
continue
# One-way filter (Stage T2e): drop a contact where a one-way
# platform should let the other body pass through (coming up from
# below). Discarded BEFORE the contact reaches the solver or the
# event diff, so a pass-through fires no impulse and no ContactEvent.
if ba.one_way or bb.one_way:
if self._one_way_rejects(contact, ba, bb, ba.linear_velocity, bb.linear_velocity):
continue
contacts.append(contact)
return contacts
# -- world-space shape geometry helpers --------------------------------
@staticmethod
def _world_circle(body: _Body2D) -> tuple[np.ndarray, float]:
"""Circle as ``(world_centre, radius)``."""
return body.position.astype(np.float32), float(body.shape.params[0])
@staticmethod
def _world_capsule_segment(body: _Body2D) -> tuple[np.ndarray, np.ndarray, float]:
"""Capsule as ``(p0, p1, radius)`` in world space (Y-axis local segment).
The local segment endpoints ``+-[0, half_len]`` rotated by the body
rotation and offset to the body position. ``half_len == 0`` collapses to a
point (the capsule behaves as a circle), which the closest-point helpers
guard against.
"""
radius = float(body.shape.params[0])
half_len = float(body.shape.params[1])
c = math.cos(body.rotation)
s = math.sin(body.rotation)
offset = _rotate_2d(np.array([0.0, half_len], dtype=np.float32), c, s)
return (body.position - offset).astype(np.float32), (body.position + offset).astype(np.float32), radius
@staticmethod
def _world_segment(body: _Body2D) -> tuple[np.ndarray, np.ndarray, float]:
"""Segment shape as ``(a, b, radius)`` in world space."""
radius = float(body.shape.params[0])
c = math.cos(body.rotation)
s = math.sin(body.rotation)
pa = body.position + _rotate_2d(body.shape.points[0], c, s)
pb = body.position + _rotate_2d(body.shape.points[1], c, s)
return pa.astype(np.float32), pb.astype(np.float32), radius
@staticmethod
def _world_poly(body: _Body2D) -> np.ndarray:
"""World-space CCW vertices ``(N, 2)`` of a box / poly body.
A box becomes its 4 corners ``+-[hx, hy]`` (CCW); a poly its stored CCW
cloud. Both rotated by the body rotation and offset to the body position.
Thick segments do NOT route here (they inflate to a thin box in
:meth:`_poly_segment`); a segment has its own ``_world_segment`` core.
"""
kind = body.shape.kind
c = math.cos(body.rotation)
s = math.sin(body.rotation)
if kind == "box":
hx, hy = float(body.shape.params[0]), float(body.shape.params[1])
local = np.array([[-hx, -hy], [hx, -hy], [hx, hy], [-hx, hy]], dtype=np.float32)
elif kind == "poly":
local = body.shape.points
else:
raise AssertionError(f"_world_poly: unsupported kind {kind!r}")
rot = np.array([[c, -s], [s, c]], dtype=np.float32)
world: np.ndarray = (local @ rot.T) + body.position
return world.astype(np.float32)
# -- narrowphase dispatch ----------------------------------------------
def _narrow(self, ha: BodyHandle, ba: _Body2D, hb: BodyHandle, bb: _Body2D) -> _Contact2D | None:
"""Explicit kind-pair dispatch (no silent catch-all), mirroring 3D.
Every unordered pair routes to one canonical handler with a ``flip`` flag
so the returned ``_Contact2D.normal`` always points from the first body of
the ORIGINAL ``(a, b)`` order to the second. ``concave`` is the static edge
soup and is always the OTHER body (the moving shape is a primitive); an
unknown pair asserts rather than silently mis-handling.
"""
ka, kb = ba.shape.kind, bb.shape.kind
# -- concave static edge soup: the moving primitive vs the soup --
if kb == "concave":
return self._shape_vs_concave(ha, ba, hb, bb, flip=False)
if ka == "concave":
return self._shape_vs_concave(hb, bb, ha, ba, flip=True)
# -- circle pairings (exact analytic / segment reductions) --
if ka == "circle" and kb == "circle":
return self._circle_circle(ha, ba, hb, bb)
if ka == "circle" and kb == "box":
return self._circle_box(ha, ba, hb, bb, flip=False)
if ka == "box" and kb == "circle":
return self._circle_box(hb, bb, ha, ba, flip=True)
if ka == "circle" and kb == "capsule":
return self._circle_capsule(ha, ba, hb, bb, flip=False)
if ka == "capsule" and kb == "circle":
return self._circle_capsule(hb, bb, ha, ba, flip=True)
if ka == "circle" and kb == "segment":
return self._circle_segment(ha, ba, hb, bb, flip=False)
if ka == "segment" and kb == "circle":
return self._circle_segment(hb, bb, ha, ba, flip=True)
if ka == "circle" and kb == "poly":
return self._circle_poly(ha, ba, hb, bb, flip=False)
if ka == "poly" and kb == "circle":
return self._circle_poly(hb, bb, ha, ba, flip=True)
# -- capsule pairings (segment / segment-segment reductions) --
if ka == "capsule" and kb == "capsule":
return self._capsule_capsule(ha, ba, hb, bb)
if ka == "capsule" and kb == "segment":
return self._capsule_segment(ha, ba, hb, bb, flip=False)
if ka == "segment" and kb == "capsule":
return self._capsule_segment(hb, bb, ha, ba, flip=True)
# Capsule vs box / poly: reduce the capsule to its core segment vs the
# poly's edges (closest feature) then a circle test. Routed through the
# generic thick-segment-vs-poly path. Basic-tier: single closest feature,
# one contact point (full two-point capsule-on-edge manifold deferred to
# pymunk).
if ka == "capsule" and kb in ("box", "poly"):
return self._capsule_poly(ha, ba, hb, bb, flip=False)
if kb == "capsule" and ka in ("box", "poly"):
return self._capsule_poly(hb, bb, ha, ba, flip=True)
# -- polygon vs thick segment: inflate the segment to a thin box --
# A thick segment (radius > 0) is NOT a zero-thickness poly: SAT on its core
# line would miss the radius gap. Inflate it into a thin oriented box
# (core +- radius perpendicular) so the SAT path accounts for thickness and
# still yields a 2-point manifold (a box on a floor segment cannot rock).
if ka in ("box", "poly") and kb == "segment":
return self._poly_segment(ha, ba, hb, bb, flip=False)
if kb in ("box", "poly") and ka == "segment":
return self._poly_segment(hb, bb, ha, ba, flip=True)
# -- polygon family via 2D SAT + Sutherland-Hodgman clipping --
# box and poly are full polygons; box-box / box-poly / poly-poly route here.
poly_kinds = ("box", "poly")
if ka in poly_kinds and kb in poly_kinds:
verts_a = self._world_poly(ba)
verts_b = self._world_poly(bb)
return self._poly_poly(ha, verts_a, hb, verts_b)
if ka == "segment" and kb == "segment":
# Two thick segments: reduce to a capsule-style closest-segment test
# (both are STATIC level geometry in practice; one of the pair must be
# non-static to even reach here, so this is a rare moving-segment case).
return self._segment_segment(ha, ba, hb, bb)
raise AssertionError(f"_narrow: unhandled 2D shape pair ({ka!r}, {kb!r})")
def _poly_segment(
self, h_p: BodyHandle, b_p: _Body2D, h_s: BodyHandle, b_s: _Body2D, *, flip: bool
) -> _Contact2D | None:
"""Box / poly vs thick segment: inflate the segment to a thin box, then SAT.
The segment core ``[a, b]`` with thickness ``radius`` becomes a 4-vertex
oriented box (core endpoints offset by ``+-radius`` along the core's
perpendicular). Running the standard SAT + clip path against that thin box
accounts for the radius AND produces the 2-point manifold a flat box needs
to rest without rocking. ``flip`` records original order (segment, poly).
"""
pa, pb, radius = self._world_segment(b_s)
core = pb - pa
cl = float(np.linalg.norm(core))
if cl < 1e-9:
return None
perp = np.array([-core[1], core[0]], dtype=np.float32) / cl
r = max(radius, 1e-4) # floor a zero-thickness segment to a sliver box
seg_verts = np.array([pa - perp * r, pb - perp * r, pb + perp * r, pa + perp * r], dtype=np.float32)
poly_verts = self._world_poly(b_p)
# Canonical SAT order: (poly=first, segment-box=second), normal poly -> seg.
contact = self._poly_poly(h_p, poly_verts, h_s, seg_verts)
if contact is None:
return None
if flip:
return _Contact2D(h_s, h_p, (-contact.normal).astype(np.float32), contact.depth, contact.points)
return contact
def _segment_segment(self, ha: BodyHandle, ba: _Body2D, hb: BodyHandle, bb: _Body2D) -> _Contact2D | None:
"""Thick segment vs thick segment: closest-points-on-segments + radii.
Rare path (segments are usually STATIC level geometry; reaching here needs
a non-static segment). Reduces to the two cores' closest points plus the
thickness radii, like ``_capsule_capsule``.
"""
a0, a1, ra = self._world_segment(ba)
b0, b1, rb = self._world_segment(bb)
c1, c2 = _closest_points_on_segments_2d(a0, a1, b0, b1)
delta = c2 - c1 # a -> b
dist = float(np.linalg.norm(delta))
rsum = ra + rb
if dist >= rsum:
return None
normal = (delta / dist).astype(np.float32) if dist > 1e-9 else np.array([0.0, 1.0], dtype=np.float32)
point = (c1 + normal * ra).astype(np.float32)
return _Contact2D(ha, hb, normal, rsum - dist, [point])
@staticmethod
def _oriented(
ha: BodyHandle,
hb: BodyHandle,
normal_a_to_b: np.ndarray,
depth: float,
points: list[np.ndarray],
*,
flip: bool,
) -> _Contact2D:
"""Build a ``_Contact2D`` from a normal computed in canonical (a->b) order.
``normal_a_to_b`` points from the canonical first body ``ha`` to ``hb``.
When ``flip`` is set the ORIGINAL pair order was reversed, so emit as
``(hb, ha)`` with the negated normal (mirrors the 3D ``_oriented``).
"""
if flip:
return _Contact2D(hb, ha, (-normal_a_to_b).astype(np.float32), depth, points)
return _Contact2D(ha, hb, normal_a_to_b.astype(np.float32), depth, points)
# -- circle narrowphase ------------------------------------------------
def _circle_circle(self, ha: BodyHandle, ba: _Body2D, hb: BodyHandle, bb: _Body2D) -> _Contact2D | None:
"""Exact circle-circle: centre distance vs the radius sum."""
ca, ra = self._world_circle(ba)
cb, rb = self._world_circle(bb)
delta = cb - ca # a -> b
dist = float(np.linalg.norm(delta))
rsum = ra + rb
if dist >= rsum:
return None
normal = (delta / dist).astype(np.float32) if dist > 1e-9 else np.array([0.0, 1.0], dtype=np.float32)
point = (ca + normal * ra).astype(np.float32) # on a's surface toward b
return _Contact2D(ha, hb, normal, rsum - dist, [point])
def _circle_box(
self, h_c: BodyHandle, b_c: _Body2D, h_b: BodyHandle, b_b: _Body2D, *, flip: bool
) -> _Contact2D | None:
"""Circle vs box, HONOURING box rotation (a 2D improvement over 3D AABB).
Transform the circle centre into the box-LOCAL frame (apply the inverse 2x2
rotation), do the exact closest-point-on-AABB test there, then rotate the
resulting normal back to world space. This is exact for any box orientation,
unlike the 3D ``_sphere_box`` which ignores box orientation (its documented
basic-tier caveat). ``flip`` records original order (box, circle).
"""
centre, radius = self._world_circle(b_c)
half = np.asarray(b_b.shape.params, dtype=np.float32)
c = math.cos(b_b.rotation)
s = math.sin(b_b.rotation)
# World -> box-local: rotate the centre-relative vector by -rotation.
rel = centre - b_b.position
local = _rotate_2d(rel, c, -s) # inverse rotation == rotate by -theta
closest = np.clip(local, -half, half).astype(np.float32)
delta = local - closest
dist_sq = float(np.dot(delta, delta))
if dist_sq > radius * radius:
return None
if dist_sq > 1e-18:
dist = dist_sq**0.5
n_local = (delta / dist).astype(np.float32) # box -> circle, box-local
depth = radius - dist
contact_local = closest
else:
# Circle centre inside the box: push out along the least-penetrated axis.
penetration = half - np.abs(local)
axis = int(np.argmin(penetration))
sign = 1.0 if local[axis] >= 0.0 else -1.0
n_local = np.zeros(2, dtype=np.float32)
n_local[axis] = sign
depth = radius + float(penetration[axis])
contact_local = closest
# Rotate normal + contact point back to world (box -> circle in world).
n_world = _rotate_2d(n_local, c, s)
point = (b_b.position + _rotate_2d(contact_local, c, s)).astype(np.float32)
# Canonical pair is (box=first, circle=second): normal box -> circle.
if flip:
return _Contact2D(h_b, h_c, n_world, depth, [point])
return _Contact2D(h_c, h_b, (-n_world).astype(np.float32), depth, [point])
def _circle_capsule(
self, h_c: BodyHandle, b_c: _Body2D, h_cap: BodyHandle, b_cap: _Body2D, *, flip: bool
) -> _Contact2D | None:
"""Circle vs capsule: circle test at the closest point on the capsule core.
Reduces to ``circle(closest_segment_point, cap.radius)`` vs the circle. The
canonical first body is the circle. ``flip`` records original order
(capsule, circle).
"""
centre, cr = self._world_circle(b_c)
p0, p1, capr = self._world_capsule_segment(b_cap)
closest = _closest_point_on_segment_2d(centre, p0, p1)
delta = closest - centre # circle -> capsule core
dist = float(np.linalg.norm(delta))
rsum = cr + capr
if dist >= rsum:
return None
normal = (delta / dist).astype(np.float32) if dist > 1e-9 else np.array([0.0, 1.0], dtype=np.float32)
point = (centre + normal * cr).astype(np.float32)
# Canonical (circle=first, capsule=second): normal circle -> capsule.
if flip:
return _Contact2D(h_cap, h_c, (-normal).astype(np.float32), rsum - dist, [point])
return _Contact2D(h_c, h_cap, normal, rsum - dist, [point])
def _circle_segment(
self, h_c: BodyHandle, b_c: _Body2D, h_s: BodyHandle, b_s: _Body2D, *, flip: bool
) -> _Contact2D | None:
"""Circle vs (thick) segment: closest point on the segment + radius."""
centre, cr = self._world_circle(b_c)
pa, pb, sr = self._world_segment(b_s)
closest = _closest_point_on_segment_2d(centre, pa, pb)
delta = closest - centre # circle -> segment
dist = float(np.linalg.norm(delta))
rsum = cr + sr
if dist >= rsum:
return None
normal = (delta / dist).astype(np.float32) if dist > 1e-9 else np.array([0.0, 1.0], dtype=np.float32)
point = (centre + normal * cr).astype(np.float32)
if flip:
return _Contact2D(h_s, h_c, (-normal).astype(np.float32), rsum - dist, [point])
return _Contact2D(h_c, h_s, normal, rsum - dist, [point])
def _circle_poly(
self, h_c: BodyHandle, b_c: _Body2D, h_p: BodyHandle, b_p: _Body2D, *, flip: bool
) -> _Contact2D | None:
"""Circle vs convex polygon: closest point on the poly boundary / interior.
Find the closest point on the polygon (its edges if the centre is outside,
else the centre is inside and we push out along the least-penetrated edge).
``flip`` records original order (poly, circle).
"""
centre, cr = self._world_circle(b_c)
verts = self._world_poly(b_p)
n = len(verts)
# Test whether the centre is inside (all edge half-plane tests positive for
# CCW winding) and track the closest boundary point + the deepest edge.
inside = True
best_pt = None
best_dist_sq = float("inf")
max_sep = -float("inf")
max_sep_normal = np.array([0.0, 1.0], dtype=np.float32)
for i in range(n):
a = verts[i]
b = verts[(i + 1) % n]
edge = b - a
edge_n = np.array([edge[1], -edge[0]], dtype=np.float32) # outward for CCW
ln = float(np.linalg.norm(edge_n))
if ln > 1e-9:
edge_n = (edge_n / ln).astype(np.float32)
sep = float(np.dot(centre - a, edge_n)) # signed dist to this edge
if sep > 0.0:
inside = False
if sep > max_sep:
max_sep = sep
max_sep_normal = edge_n
cp = _closest_point_on_segment_2d(centre, a, b)
d_sq = float(np.dot(centre - cp, centre - cp))
if d_sq < best_dist_sq:
best_dist_sq = d_sq
best_pt = cp
if inside:
# Centre inside the poly: normal is the least-penetrated edge normal
# (max_sep is the closest, i.e. smallest-magnitude negative, edge).
normal = max_sep_normal # poly -> circle (outward)
depth = cr - max_sep # max_sep <= 0 inside, so depth > cr
point = (centre - normal * cr).astype(np.float32)
else:
dist = best_dist_sq**0.5
if dist >= cr:
return None
assert best_pt is not None
delta = centre - best_pt # poly -> circle
normal = (delta / dist).astype(np.float32) if dist > 1e-9 else max_sep_normal
depth = cr - dist
point = best_pt.astype(np.float32)
# Canonical (poly=first, circle=second): normal poly -> circle.
if flip:
return _Contact2D(h_p, h_c, normal.astype(np.float32), depth, [point])
return _Contact2D(h_c, h_p, (-normal).astype(np.float32), depth, [point])
# -- capsule narrowphase -----------------------------------------------
def _capsule_capsule(self, ha: BodyHandle, ba: _Body2D, hb: BodyHandle, bb: _Body2D) -> _Contact2D | None:
"""Capsule vs capsule: circle test at the two cores' closest points."""
a0, a1, ra = self._world_capsule_segment(ba)
b0, b1, rb = self._world_capsule_segment(bb)
c1, c2 = _closest_points_on_segments_2d(a0, a1, b0, b1)
delta = c2 - c1 # a -> b
dist = float(np.linalg.norm(delta))
rsum = ra + rb
if dist >= rsum:
return None
normal = (delta / dist).astype(np.float32) if dist > 1e-9 else np.array([0.0, 1.0], dtype=np.float32)
point = (c1 + normal * ra).astype(np.float32)
return _Contact2D(ha, hb, normal, rsum - dist, [point])
def _capsule_segment(
self, h_cap: BodyHandle, b_cap: _Body2D, h_seg: BodyHandle, b_seg: _Body2D, *, flip: bool
) -> _Contact2D | None:
"""Capsule vs (thick) segment: closest points of the two cores + radii."""
a0, a1, ra = self._world_capsule_segment(b_cap)
s0, s1, sr = self._world_segment(b_seg)
c1, c2 = _closest_points_on_segments_2d(a0, a1, s0, s1)
delta = c2 - c1 # capsule core -> segment
dist = float(np.linalg.norm(delta))
rsum = ra + sr
if dist >= rsum:
return None
normal = (delta / dist).astype(np.float32) if dist > 1e-9 else np.array([0.0, 1.0], dtype=np.float32)
point = (c1 + normal * ra).astype(np.float32)
# Canonical (capsule=first, segment=second): normal capsule -> segment.
if flip:
return _Contact2D(h_seg, h_cap, (-normal).astype(np.float32), rsum - dist, [point])
return _Contact2D(h_cap, h_seg, normal, rsum - dist, [point])
def _capsule_poly(
self, h_cap: BodyHandle, b_cap: _Body2D, h_p: BodyHandle, b_p: _Body2D, *, flip: bool
) -> _Contact2D | None:
"""Capsule vs box / poly: capsule core (segment) vs the poly's closest edge.
Basic-tier: reduce the capsule to its core segment, find the closest point
between that segment and the polygon boundary, then a circle-radius test at
the closest feature. One contact point (a two-point capsule-on-flat-edge
manifold, which stops the capsule rocking, is deferred to pymunk; Baumgarte
+ the iterated solver still settle it, just with more residual sway).
``flip`` records original order (poly, capsule).
"""
a0, a1, capr = self._world_capsule_segment(b_cap)
verts = self._world_poly(b_p)
n = len(verts)
# Closest points between the capsule core segment and every poly edge.
best_dist_sq = float("inf")
best_c_cap = a0
best_c_poly = verts[0]
for i in range(n):
e0 = verts[i]
e1 = verts[(i + 1) % n]
cc, cp = _closest_points_on_segments_2d(a0, a1, e0, e1)
d_sq = float(np.dot(cc - cp, cc - cp))
if d_sq < best_dist_sq:
best_dist_sq = d_sq
best_c_cap = cc
best_c_poly = cp
dist = best_dist_sq**0.5
# If the capsule core point is inside the poly the closest-edge distance is
# still the boundary distance, but the core is penetrating: detect via the
# outward edge half-planes and flip the normal sign accordingly.
core_inside = self._point_in_poly(best_c_cap, verts)
if not core_inside and dist >= capr:
return None
delta = best_c_poly - best_c_cap # capsule core -> poly boundary
if core_inside:
# Core inside: push the capsule out of the poly. Normal points from the
# poly toward the capsule core (out through the nearest boundary).
normal_p_to_cap = (best_c_cap - best_c_poly).astype(np.float32)
ln = float(np.linalg.norm(normal_p_to_cap))
normal_p_to_cap = normal_p_to_cap / ln if ln > 1e-9 else np.array([0.0, 1.0], dtype=np.float32)
depth = capr + dist
normal_cap_to_p = -normal_p_to_cap
else:
up = np.array([0.0, 1.0], dtype=np.float32)
normal_cap_to_p = (delta / dist).astype(np.float32) if dist > 1e-9 else up
depth = capr - dist
point = (best_c_cap + normal_cap_to_p * capr).astype(np.float32)
# Canonical (capsule=first, poly=second): normal capsule -> poly.
if flip:
return _Contact2D(h_p, h_cap, (-normal_cap_to_p).astype(np.float32), depth, [point])
return _Contact2D(h_cap, h_p, normal_cap_to_p, depth, [point])
@staticmethod
def _point_in_poly(p: np.ndarray, verts: np.ndarray) -> bool:
"""True if ``p`` is inside the CCW convex polygon ``verts`` (all edges)."""
n = len(verts)
if n < 3:
return False # a degenerate 2-vertex "poly" (segment) has no interior
for i in range(n):
a = verts[i]
b = verts[(i + 1) % n]
edge = b - a
# CCW: interior is to the LEFT of each edge (cross(edge, p - a) >= 0).
if _cross_2d(edge, p - a) < 0.0:
return False
return True
# -- polygon SAT + Sutherland-Hodgman clipping (the keystone) ----------
def _poly_poly(self, ha: BodyHandle, va: np.ndarray, hb: BodyHandle, vb: np.ndarray) -> _Contact2D | None:
"""Convex polygon vs convex polygon via 2D SAT + edge clipping (keystone).
Takes the two polygons' world-space CCW vertex arrays ``va`` / ``vb`` (so
callers can pass an inflated segment box). Algorithm (Box2D-style, the
standard 2D convex manifold):
1. SAT: for every edge normal of A and of B, project both polygons and find
the axis of MINIMUM overlap. If any axis fully separates them, no
contact. The minimum-overlap axis is the contact normal; its overlap is
the penetration depth.
2. Manifold: identify the REFERENCE edge (on the polygon owning the minimum
axis) and the INCIDENT edge (the most anti-parallel edge of the other
polygon), then clip the incident edge against the reference edge's two
side planes (Sutherland-Hodgman) and keep the clipped points that lie
below the reference face. Those are the contact points (1 or 2).
Box and poly both flow through here (box is a 4-vertex poly); thick segments
are inflated to a thin box by the caller (:meth:`_poly_segment`). Basic-tier
honesty: on a DEEP first-frame overlap the minimum-overlap axis can be
sub-optimal (not the true MTV when nearly co-centred); Baumgarte recovers
it. No warm-starting (deferred to pymunk).
"""
# SAT over A's edge normals, then B's. Track the global minimum overlap.
sep_a, axis_a, edge_a = self._sat_axis(va, vb)
if sep_a is None:
return None # a separating axis from A: no overlap
sep_b, axis_b, edge_b = self._sat_axis(vb, va)
if sep_b is None:
return None
# Minimum-overlap axis wins. Both seps are positive overlaps here (None
# would have returned). Bias toward A's axis on a near-tie for stability.
eps = 1e-4
if sep_a <= sep_b + eps:
ref_verts, inc_verts = va, vb
ref_edge = edge_a
normal = axis_a # points out of A (the reference) toward B
depth = sep_a
flip_owner = False
else:
ref_verts, inc_verts = vb, va
ref_edge = edge_b
normal = axis_b # points out of B (the reference) toward A
depth = sep_b
flip_owner = True
points = self._clip_manifold(ref_verts, inc_verts, ref_edge, normal)
if not points:
# Clipping produced no point (numerical edge case): fall back to the
# midpoint between the two polygon centroids projected onto the normal.
mid = ((va.mean(axis=0) + vb.mean(axis=0)) * 0.5).astype(np.float32)
points = [mid]
# ``normal`` points out of the REFERENCE polygon toward the incident one.
# Normalise to the canonical a -> b orientation.
if flip_owner:
# Reference was B, so normal points B -> A; negate for a -> b.
normal = (-normal).astype(np.float32)
return _Contact2D(ha, hb, normal.astype(np.float32), float(depth), points)
@staticmethod
def _poly_edge_normals(verts: np.ndarray) -> list[tuple[np.ndarray, int]]:
"""Outward unit edge normals of a CCW polygon as ``(normal, edge_index)``.
A degenerate 2-vertex poly (segment) yields ONE edge but TWO opposite
candidate normals (both perpendiculars) so SAT can separate on either side.
"""
n = len(verts)
out: list[tuple[np.ndarray, int]] = []
if n == 2:
edge = verts[1] - verts[0]
perp = np.array([edge[1], -edge[0]], dtype=np.float32)
ln = float(np.linalg.norm(perp))
if ln > 1e-9:
perp = (perp / ln).astype(np.float32)
out.append((perp, 0))
out.append(((-perp).astype(np.float32), 0))
return out
for i in range(n):
edge = verts[(i + 1) % n] - verts[i]
normal = np.array([edge[1], -edge[0]], dtype=np.float32) # outward (CCW)
ln = float(np.linalg.norm(normal))
if ln > 1e-9:
normal = (normal / ln).astype(np.float32)
out.append((normal, i))
return out
def _sat_axis(self, ref: np.ndarray, other: np.ndarray) -> tuple[float | None, np.ndarray, int]:
"""Minimum-penetration axis of ``ref``'s edge normals against ``other``.
For each outward edge normal of ``ref``, the separation is
``min_over_other(dot(v - ref_face_point, normal))``: the furthest the
``other`` polygon protrudes back past ``ref``'s face along the OUTWARD
normal. A positive separation on ANY axis means a separating axis exists
(no overlap): return ``(None, ...)``. Otherwise return
``(overlap_depth, axis, edge_index)`` for the SHALLOWEST overlap (the least
negative separation -> smallest positive depth).
"""
best_depth = float("inf")
best_axis = np.array([0.0, 1.0], dtype=np.float32)
best_edge = 0
for normal, edge_idx in self._poly_edge_normals(ref):
face_pt = ref[edge_idx]
# Furthest protrusion of `other` past this face along the outward normal.
sep = min(float(np.dot(v - face_pt, normal)) for v in other)
if sep > 0.0:
return None, best_axis, best_edge # separating axis: no overlap
depth = -sep # penetration along this axis (>= 0)
if depth < best_depth:
best_depth = depth
best_axis = normal
best_edge = edge_idx
return best_depth, best_axis, best_edge
def _clip_manifold(
self, ref_verts: np.ndarray, inc_verts: np.ndarray, ref_edge: int, normal: np.ndarray
) -> list[np.ndarray]:
"""Clip the incident edge against the reference edge side planes (S-H).
``normal`` is the reference face's outward normal (out of ``ref`` toward
``inc``). Picks the incident edge of ``inc`` most anti-parallel to
``normal``, clips it against the two side planes of the reference edge, and
keeps the clipped endpoints whose depth past the reference face is <= 0
(i.e. penetrating). Returns 0..2 world contact points.
"""
n_ref = len(ref_verts)
if n_ref >= 2:
rv0 = ref_verts[ref_edge]
rv1 = ref_verts[(ref_edge + 1) % n_ref]
else: # pragma: no cover - a poly always has >= 2 vertices
return []
# Incident edge: the edge of `inc` whose normal is most anti-parallel to
# the reference normal (the face pointing back at the reference).
inc_edges = self._poly_edge_normals(inc_verts)
n_inc = len(inc_verts)
if n_inc == 2:
iv0, iv1 = inc_verts[0], inc_verts[1]
else:
best_dot = float("inf")
best_i = 0
for en, ei in inc_edges:
d = float(np.dot(en, normal))
if d < best_dot:
best_dot = d
best_i = ei
iv0 = inc_verts[best_i]
iv1 = inc_verts[(best_i + 1) % n_inc]
# Reference edge tangent (side-plane normals are +-tangent).
tangent = rv1 - rv0
tl = float(np.linalg.norm(tangent))
if tl < 1e-9:
return []
tangent = (tangent / tl).astype(np.float32)
# Clip the incident segment [iv0, iv1] to the two side planes:
# side 1: dot(p - rv0, tangent) >= 0 (past the start)
# side 2: dot(p - rv1, -tangent) >= 0 (before the end)
pts = self._clip_segment(iv0, iv1, rv0, tangent)
if len(pts) < 2:
return []
pts = self._clip_segment(pts[0], pts[1], rv1, (-tangent).astype(np.float32))
if len(pts) < 2:
return []
# Keep only the clipped points that lie ON OR BELOW the reference face
# (penetrating: depth past the face along the outward normal <= 0).
out: list[np.ndarray] = []
for p in pts:
if float(np.dot(p - rv0, normal)) <= 1e-4:
out.append(p.astype(np.float32))
return out
@staticmethod
def _clip_segment(a: np.ndarray, b: np.ndarray, plane_pt: np.ndarray, plane_n: np.ndarray) -> list[np.ndarray]:
"""Clip segment ``[a, b]`` to the half-plane ``dot(p - plane_pt, n) >= 0``.
Returns the (up to 2) points of the part of the segment inside the
half-plane: both endpoints if both inside, the inside endpoint plus the
crossing point if one is outside, empty if both outside.
"""
da = float(np.dot(a - plane_pt, plane_n))
db = float(np.dot(b - plane_pt, plane_n))
out: list[np.ndarray] = []
if da >= 0.0:
out.append(a.astype(np.float32))
if db >= 0.0:
out.append(b.astype(np.float32))
if da * db < 0.0: # endpoints straddle the plane: add the crossing point
t = da / (da - db)
cross = (a + t * (b - a)).astype(np.float32)
out.append(cross)
return out
# -- concave (static edge soup) narrowphase ----------------------------
def _shape_vs_concave(
self, h_m: BodyHandle, b_m: _Body2D, h_c: BodyHandle, b_c: _Body2D, *, flip: bool
) -> _Contact2D | None:
"""Moving primitive vs STATIC concave edge soup (basic tier).
The concave shape (``b_c``) is the 2D analogue of a 3D static triangle
mesh: STATIC-ONLY, so it is NEVER the moving shape (asserted at
create_body / set_body_mode; re-asserted here defensively). Broadphase: a
LINEAR AABB scan over the soup's candidate segments culls by the moving
shape's world AABB (a BVH is deferred to pymunk). Each candidate segment is
tested against the moving primitive by reusing the existing thick-segment
narrowphase (circle / capsule) or, for a box/poly mover, a poly-vs-segment
SAT; the DEEPEST contact over all candidates wins. ``flip`` records that
the original pair order put the concave first.
"""
assert b_c.body_type is BodyMode.STATIC, "concave (edge soup) must be STATIC; never the moving shape"
mover = b_m
c = math.cos(b_c.rotation)
s = math.sin(b_c.rotation)
segs = b_c.shape.points # (N, 2, 2) body-local
lo_m, hi_m = self._aabb(mover)
# Make a transient single-segment body to reuse the per-pair narrowphase.
best: _Contact2D | None = None
for k in range(len(segs)):
a_local = segs[k, 0]
b_local = segs[k, 1]
a_world = b_c.position + _rotate_2d(a_local, c, s)
b_world = b_c.position + _rotate_2d(b_local, c, s)
seg_lo = np.minimum(a_world, b_world).astype(np.float32)
seg_hi = np.maximum(a_world, b_world).astype(np.float32)
if not self._aabb_overlap(lo_m, hi_m, seg_lo, seg_hi):
continue # broadphase cull
seg_body = self._transient_segment_body(b_c, a_world, b_world)
contact = self._narrow(h_m, mover, h_c, seg_body)
if contact is None:
continue
if best is None or contact.depth > best.depth:
best = contact
if best is None:
return None
# _narrow returned a contact oriented (mover -> seg_body) i.e. (h_m -> h_c).
# Apply the outer flip so the original pair order is honoured.
if flip:
return _Contact2D(best.b, best.a, (-best.normal).astype(np.float32), best.depth, best.points)
return best
def _transient_segment_body(self, template: _Body2D, a_world: np.ndarray, b_world: np.ndarray) -> _Body2D:
"""Build a transient STATIC zero-thickness segment _Body2D in world space.
Used only inside :meth:`_shape_vs_concave` to feed one soup segment through
the regular per-pair narrowphase. World-space endpoints, identity rotation,
infinite mass, material copied from the concave body so friction /
restitution carry through to the solver.
"""
# Store the segment as a body at the origin with world-space points and zero
# rotation (the points are already world space, so identity transform).
seg_pts = np.stack([a_world, b_world]).astype(np.float32)
shp = _Shape2D("segment", np.array([0.0], dtype=np.float32), points=seg_pts)
return _Body2D(
shape=shp,
body_type=BodyMode.STATIC,
position=np.zeros(2, dtype=np.float32),
rotation=0.0,
mass=0.0,
inverse_mass=0.0,
moment=0.0,
inverse_moment=0.0,
collision_layer=template.collision_layer,
collision_mask=template.collision_mask,
friction=template.friction,
restitution=template.restitution,
friction_combine=template.friction_combine,
restitution_combine=template.restitution_combine,
)
# -- sequential-impulse contact solver (T2b) ---------------------------
def _solve(
self,
contacts: list[_Contact2D],
dt: float,
impulses: dict[tuple[BodyHandle, BodyHandle], float] | None = None,
) -> None:
"""Sequential-impulse velocity solve of contacts AND joints + position pass.
Mirrors the 3D ``_solve`` structure exactly:
0. Spring PRE-PASS, applied ONCE per step (before the iterated loop).
Springs are an EXPLICIT soft force, not a hard constraint: iterating a
soft impulse N times would multiply its effective stiffness by N and
overshoot / explode, so each spring contributes exactly one impulse per
step. It composes with the iterated solve because it only mutates
velocity once up front.
1. Velocity loop, ``_VELOCITY_ITERATIONS`` passes, over contacts AND the
RIGID joints (Pin / Hinge / Fixed) TOGETHER (each pass solves every
contact then every rigid joint, so they compose and converge).
2. Contact Baumgarte position correction ONCE per step (NOT per velocity
iteration: running it N times over-pushes a resting body so it separates
and re-enters), then the RIGID-joint position loop
(``_POSITION_ITERATIONS`` Baumgarte passes for anchor / angle error).
The 2D solver adds the angular terms the 3D basic tier omits: it carries a
real scalar moment of inertia, so each contact/joint impulse applies a
scalar angular component via the 2D cross product (``cross(r, n)``) and the
effective mass folds in ``inv_moment * cross^2``. Sensors never reach here
(gated in ``_collide``). No warm-starting (deferred to pymunk): each step
solves from zero.
The ``impulses`` side-table (Stage T2d) is a pure event-payload channel: the
settled total normal impulse per pair is written ONLY on the LAST velocity
pass (so it reports the converged value), keyed canonically; ``None`` skips
it entirely (the solver path that runs without event diffing).
"""
joints = list(self._joints.values())
springs = [j for j in joints if isinstance(j, _SpringConstraint2D)]
rigid = [j for j in joints if not isinstance(j, _SpringConstraint2D)]
# Step 0: spring pre-pass (once per step). Rigid joints iterate below.
for s in springs:
self._solve_spring_velocity(s, dt)
# Step 0a: capture each contact's per-point restitution velocity bias ONCE,
# from the PRE-solve (pre-warm-start) relative normal velocity (Stage P2).
# With accumulated-impulse warm-starting the per-iteration target must be a
# fixed velocity bias, not the live vn (driven to zero, which would clamp
# the restitution back out).
for c in contacts:
self._prepare_contact_bias(c)
# Step 0b: warm-start (Stage P2). Seed each contact's per-point accumulated
# impulses from last step's cached value (persistent body-pair id) and APPLY
# them to the bodies' velocities + spins before the iteration loop, so a
# resting stack starts near its converged solution. Fresh contacts seed zero.
self._warm_start_contacts(contacts)
# Step 1: iterated velocity loop (contacts + rigid joints together). Each
# contact solve applies a DELTA impulse against its per-point running
# accumulators (warm-started above), clamping the TOTAL.
last = _VELOCITY_ITERATIONS - 1
for it in range(_VELOCITY_ITERATIONS):
write = impulses if it == last else None
for c in contacts:
self._solve_contact(c, write)
for j in rigid:
self._solve_joint(j)
# (The warm-start cache is rebuilt in step() from this step's contacts, so
# a step that solved only joints / no contacts still clears stale pairs.)
# Step 2: single contact positional projection (3D contact parity), then
# the rigid-joint Baumgarte position passes.
for c in contacts:
self._correct_contact_position(c)
if rigid:
for _ in range(_POSITION_ITERATIONS):
for j in rigid:
self._solve_joint_position(j)
def _prepare_contact_bias(self, c: _Contact2D) -> None:
"""Compute per-point restitution velocity bias once, pre-solve (Stage P2).
For each manifold point ``vbias = -e_eff * vn`` where ``vn`` is the
approaching relative normal velocity AT THE POINT measured BEFORE
warm-starting and ``e_eff`` is the per-contact combined restitution gated by
the rest-threshold (a slow / resting contact gets ``e_eff = 0``: no perpetual
re-launch jitter). The iterated solve then drives each point's ``vn`` to its
``vbias`` target, keeping the bounce stable under accumulated-impulse
clamping. Also (re)sizes the per-point accumulators to match the manifold.
"""
ba = self._bodies[c.a]
bb = self._bodies[c.b]
n = c.normal
e = _combine(ba.restitution, bb.restitution, ba.restitution_combine, bb.restitution_combine)
c.vbias = [0.0] * len(c.points)
for i, point in enumerate(c.points):
ra = (point - ba.position).astype(np.float32)
rb = (point - bb.position).astype(np.float32)
vel_a = ba.linear_velocity + ba.angular_velocity * _perp_2d(ra)
vel_b = bb.linear_velocity + bb.angular_velocity * _perp_2d(rb)
vn = float(np.dot(vel_b - vel_a, n))
if vn < 0.0:
e_eff = e if -vn > _RESTITUTION_THRESHOLD else 0.0
c.vbias[i] = -e_eff * vn # >= 0: post-bounce separating speed target
# Fresh accumulators (warm-start overwrites them when a cache hit exists).
c.jn = [0.0] * len(c.points)
c.jt = [0.0] * len(c.points)
def _warm_start_contacts(self, contacts: list[_Contact2D]) -> None:
"""Seed + apply each contact's cached per-point impulses (Stage P2).
For every contact this step, look up the previous step's converged
``(jn, jt)`` lists under the canonical body-pair id and, if present and the
manifold point count matches, seed the per-point accumulators and APPLY the
cached impulse ``P = jn * n + jt * t`` at each point (linear + the scalar
angular component via the 2D cross). A resting stack thus begins each step
carrying last step's support, so the iteration loop only corrects the small
residual. A point-count mismatch (the manifold changed: a box tipped from a
2-point to a 1-point contact) drops the stale cache for that pair and starts
that contact from zero, which is correct (the old impulses no longer map to
the new geometry). Skips asleep / double-infinite-mass pairs exactly as
``_solve_contact`` does.
"""
if not self._warm_contacts:
return # no resting history (fresh scene): nothing to seed
for c in contacts:
cached = self._warm_contacts.get(self._canon(c.a, c.b))
if cached is None:
continue
jn_cache, jt_cache = cached
if len(jn_cache) != len(c.points):
continue # manifold changed: drop the stale cache, start from zero
ba = self._bodies[c.a]
bb = self._bodies[c.b]
if ba.asleep and bb.asleep:
continue
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
continue
n = c.normal
t = np.array([-n[1], n[0]], dtype=np.float32) # in-plane tangent (CCW perp of n)
c.jn = list(jn_cache)
c.jt = list(jt_cache)
for i, point in enumerate(c.points):
ra = (point - ba.position).astype(np.float32)
rb = (point - bb.position).astype(np.float32)
impulse = (c.jn[i] * n + c.jt[i] * t).astype(np.float32)
ba.linear_velocity = ba.linear_velocity - impulse * ba.inverse_mass
bb.linear_velocity = bb.linear_velocity + impulse * bb.inverse_mass
ba.angular_velocity = ba.angular_velocity - ba.inverse_moment * _cross_2d(ra, impulse)
bb.angular_velocity = bb.angular_velocity + bb.inverse_moment * _cross_2d(rb, impulse)
def _solve_contact(
self, c: _Contact2D, impulses: dict[tuple[BodyHandle, BodyHandle], float] | None = None
) -> None:
"""One sequential VELOCITY pass for a single 2D contact: normal + friction.
For each contact point: drive the relative velocity along the a->b normal to
the point's restitution target (``c.vbias[i]``, captured once pre-solve),
then a Coulomb friction impulse along the in-plane tangent clamped by ``mu``
times the accumulated normal impulse. Unlike the 3D linear-only basic tier,
the 2D solver uses the contact-point lever arm: relative velocity at the
point is ``v + omega x r`` (2D: ``omega`` scalar, ``omega x r = omega *
perp(r)``), the effective mass folds in ``inv_moment * cross(r, axis)^2``,
and the impulse applies a scalar angular component ``inv_moment *
cross(r, impulse)``. So a box landing off-centre tips realistically.
Warm-starting (Stage P2): the solve works on the contact's per-point running
accumulators ``c.jn[i]`` / ``c.jt[i]`` (warm-started in
:meth:`_warm_start_contacts`). Each pass computes the DELTA impulse this
iteration, clamps the new TOTAL (normal ``>= 0``; friction within the
``mu * jn`` Coulomb cone), and applies only the delta. This is the standard
Box2D accumulated-impulse form: identical to the previous from-zero solve for
a fresh contact (accumulators start at zero) but lets a warm-started resting
contact converge immediately. Friction uses the FIXED in-plane tangent
(perpendicular to the contact normal), so its accumulator's direction is
stable across iterations and steps (the 2D manifold has no slide-direction
ambiguity the 3D linear tier suffers from).
``impulses`` (Stage T2d) is non-None only on the final velocity pass: the
per-point ACCUMULATED normal impulses are summed and recorded under the
canonical pair key, so the contact-event payload reports the settled total
normal impulse (mirrors the 3D ``impulses`` side-table).
"""
ba = self._bodies[c.a]
bb = self._bodies[c.b]
# Wake-on-contact (Stage T2c, mirrors 3D): if exactly one of the pair is
# asleep and the other is an awake DYNAMIC / KINEMATIC body (a real
# disturbance), wake the sleeper so it responds. Two sleepers stay asleep
# (a settled stack touching a settled stack); a sleeper vs STATIC stays
# asleep (resting on the floor must not perpetually re-wake). This is the
# per-body contact-graph wake: a newly-dropped awake body wakes the top of a
# sleeping stack, which next step (now awake) wakes the one below, cascading.
if ba.asleep != bb.asleep:
sleeper, other = (ba, bb) if ba.asleep else (bb, ba)
if other.body_type is not BodyMode.STATIC and not other.asleep:
self._wake(sleeper)
if ba.asleep and bb.asleep:
return # both asleep: skip the velocity solve (no work)
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return # both infinite-mass (already filtered, but be safe)
n = c.normal
t = np.array([-n[1], n[0]], dtype=np.float32) # fixed in-plane tangent (CCW perp of n)
mu = _combine(ba.friction, bb.friction, ba.friction_combine, bb.friction_combine)
jn_total = 0.0 # accumulated normal impulse (event payload, T2d)
for i, point in enumerate(c.points):
ra = (point - ba.position).astype(np.float32)
rb = (point - bb.position).astype(np.float32)
# Relative velocity of b w.r.t. a AT the contact point (include spin).
vel_a = ba.linear_velocity + ba.angular_velocity * _perp_2d(ra)
vel_b = bb.linear_velocity + bb.angular_velocity * _perp_2d(rb)
rel_vel = vel_b - vel_a
vn = float(np.dot(rel_vel, n))
# Effective normal mass with the angular lever arms (2D scalar cross).
rn_a = _cross_2d(ra, n)
rn_b = _cross_2d(rb, n)
k_n = inv_sum + ba.inverse_moment * rn_a * rn_a + bb.inverse_moment * rn_b * rn_b
if k_n > 0.0:
# Delta normal impulse to drive vn to the restitution target, then
# clamp the running TOTAL non-negative and apply only the delta.
d_jn = -(vn - c.vbias[i]) / k_n
new_jn = max(c.jn[i] + d_jn, 0.0)
d_jn = new_jn - c.jn[i]
c.jn[i] = new_jn
impulse = d_jn * n
ba.linear_velocity = ba.linear_velocity - impulse * ba.inverse_mass
bb.linear_velocity = bb.linear_velocity + impulse * bb.inverse_mass
ba.angular_velocity = ba.angular_velocity - ba.inverse_moment * _cross_2d(ra, impulse)
bb.angular_velocity = bb.angular_velocity + bb.inverse_moment * _cross_2d(rb, impulse)
jn_total += c.jn[i]
# Tangential Coulomb friction against the POST-normal relative velocity,
# along the FIXED tangent, accumulated + clamped to the Coulomb cone.
if mu == 0.0 or c.jn[i] == 0.0:
continue
rt_a = _cross_2d(ra, t)
rt_b = _cross_2d(rb, t)
k_t = inv_sum + ba.inverse_moment * rt_a * rt_a + bb.inverse_moment * rt_b * rt_b
if k_t <= 0.0:
continue
vel_a = ba.linear_velocity + ba.angular_velocity * _perp_2d(ra)
vel_b = bb.linear_velocity + bb.angular_velocity * _perp_2d(rb)
vt = float(np.dot(vel_b - vel_a, t)) # signed tangential speed along t
d_jt = -vt / k_t
max_jt = mu * c.jn[i] # Coulomb cap against the accumulated normal impulse
new_jt = min(max(c.jt[i] + d_jt, -max_jt), max_jt)
d_jt = new_jt - c.jt[i]
c.jt[i] = new_jt
impulse_t = (d_jt * t).astype(np.float32)
ba.linear_velocity = ba.linear_velocity - impulse_t * ba.inverse_mass
bb.linear_velocity = bb.linear_velocity + impulse_t * bb.inverse_mass
ba.angular_velocity = ba.angular_velocity - ba.inverse_moment * _cross_2d(ra, impulse_t)
bb.angular_velocity = bb.angular_velocity + bb.inverse_moment * _cross_2d(rb, impulse_t)
# Record the settled total accumulated normal impulse for the contact-event
# payload (only on the final velocity pass, when ``impulses`` is non-None).
if impulses is not None:
impulses[self._canon(c.a, c.b)] = jn_total
def _correct_contact_position(self, c: _Contact2D) -> None:
"""Baumgarte positional correction to drain residual penetration.
Same maths as the 3D ``_correct_contact_position`` (linear positional split
by inverse mass), applied once per contact per step (matching the 3D
contact path). 2D position correction stays LINEAR (no rotational positional
bias) like the 3D tier; the velocity solve carries the angular response.
Slop tolerated; Baumgarte fraction copied verbatim from 3D.
"""
ba = self._bodies[c.a]
bb = self._bodies[c.b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return
corr_mag = max(c.depth - _SLOP, 0.0) * _BAUMGARTE / inv_sum
if corr_mag > 0.0:
correction = corr_mag * c.normal
ba.position = ba.position - correction * ba.inverse_mass
bb.position = bb.position + correction * bb.inverse_mass
# -- joint velocity solve (rigid joints) -------------------------------
def _solve_joint(self, j: _Constraint2D) -> None:
"""Dispatch one velocity-level pass for a single RIGID joint (2D).
Explicit per-kind dispatch (no catch-all). Springs are NOT handled here:
they are an explicit soft force applied ONCE per step in the :meth:`_solve`
pre-pass (iterating a soft impulse would over-apply it). Pin and Hinge are
a single point constraint (a 2D hinge == pin this tier); Fixed adds a
scalar angular lock. A live joint constantly disturbs its bodies, so wake
either sleeper (cheap no-op when both are awake), mirroring the 3D path.
"""
ja, jb = self._bodies[j.a], self._bodies[j.b]
if ja.asleep or jb.asleep:
self._wake(ja)
self._wake(jb)
if isinstance(j, _PinConstraint2D | _HingeConstraint2D):
# 2D hinge leaves the single rotational DOF free, so it is exactly a
# pin (no off-axis rotation to lock). Motor / limit is a follow-on.
self._solve_point_velocity(j.a, j.b, j.r_a, j.r_b)
elif isinstance(j, _GrooveConstraint2D):
self._solve_groove_velocity(j)
elif isinstance(j, _FixedConstraint2D):
# Weld: lock the two centres' relative velocity (r = 0 on both, so the
# cross terms vanish: a pure linear lock of (vb - va)) plus the scalar
# angular lock.
zero = np.zeros(2, dtype=np.float32)
self._solve_point_velocity(j.a, j.b, zero, zero)
self._solve_fixed_angular_velocity(j)
else: # pragma: no cover - only rigid kinds reach here (springs pre-passed)
raise AssertionError(f"unknown rigid 2D joint record {type(j).__name__}")
def _solve_point_velocity(self, a: BodyHandle, b: BodyHandle, r_a: np.ndarray, r_b: np.ndarray) -> None:
"""2-DOF point-to-point velocity solve at the two anchor offsets (2D).
Drives the relative velocity AT the anchors to zero:
``v_rel = (vb + wb x r_b) - (va + wa x r_a)`` with ``w x r == w * perp(r)``
(scalar spin). The impulse satisfies ``K J = -v_rel`` where ``K`` is the
2x2 effective-mass matrix
``K = (inv_ma + inv_mb) I - inv_Ia [r_a]x^2 - inv_Ib [r_b]x^2``. In 2D the
skew-square is ``[r]x^2 = perp(r) perp(r)^T`` (since a unit scalar spin
maps ``r -> perp(r)``), so ``K = inv_sum I + sum inv_I perp(r) perp(r)^T``.
Folding the angular cross-coupling into the DENOMINATOR ``K`` (not just the
applied torque) is what keeps an off-centre / spinning anchor STABLE, the
2D analogue of the 3D point solve. The 2D system carries a real scalar
inverse moment of inertia, so this is exact for the tier.
"""
ba, bb = self._bodies[a], self._bodies[b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return # both infinite-mass: nothing to solve
pa = _perp_2d(r_a)
pb = _perp_2d(r_b)
va = ba.linear_velocity + ba.angular_velocity * pa
vb = bb.linear_velocity + bb.angular_velocity * pb
v_rel = (vb - va).astype(np.float64)
k = inv_sum * np.eye(2, dtype=np.float64)
k += ba.inverse_moment * np.outer(pa.astype(np.float64), pa.astype(np.float64))
k += bb.inverse_moment * np.outer(pb.astype(np.float64), pb.astype(np.float64))
impulse = np.linalg.solve(k, -v_rel).astype(np.float32)
ba.linear_velocity = ba.linear_velocity - impulse * ba.inverse_mass
bb.linear_velocity = bb.linear_velocity + impulse * bb.inverse_mass
ba.angular_velocity = ba.angular_velocity - ba.inverse_moment * _cross_2d(r_a, impulse)
bb.angular_velocity = bb.angular_velocity + bb.inverse_moment * _cross_2d(r_b, impulse)
def _solve_groove_velocity(self, j: _GrooveConstraint2D) -> None:
"""1-DOF groove velocity solve: cancel the relative velocity PERPENDICULAR
to the groove line, leaving motion ALONG the groove free (2D) (T2g).
A groove is a point-to-point constraint restricted to a single axis: the
perpendicular to the groove direction. The anchor on ``a`` is the closest
point on the (clamped) groove segment to ``b``'s anchor; the anchor on ``b``
is its fixed local offset ``r_b``. With ``r_a`` / ``r_b`` the world-axes
offsets to those two anchors and ``perp`` the unit groove normal, this drives
the SCALAR relative velocity ``v_rel . perp`` to zero (the parallel component
is untouched, so the slider runs freely along the groove). Same scalar-cross
effective mass as the contact / point solve:
``k = inv_sum + inv_Ia*cross(r_a, perp)^2 + inv_Ib*cross(r_b, perp)^2``, with
the impulse applied along ``perp`` plus its scalar angular component.
Basic-tier honesty: anchors are re-derived as ``pos + r`` (groove direction
fixed in world axes at create, not rotating with ``a``); the perpendicular is
a HARD 1-DOF lock with no endpoint-clamp softness or slide motor (deferred to
the pymunk ``GrooveJoint``). The endpoint clamp lives in the position pass
(``_correct_groove_position``); the velocity pass only enforces the line.
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return # both infinite-mass: nothing to solve
# World groove endpoints + b's world anchor (anchors not rotated, basic tier).
ga = ba.position + j.ga
gb = ba.position + j.gb
anchor_b = bb.position + j.r_b
groove = (gb - ga).astype(np.float32)
glen = float(np.linalg.norm(groove))
if glen < _JOINT_EPS:
return # degenerate groove: no direction (guarded at create, be safe)
gdir = (groove / glen).astype(np.float32)
perp = _perp_2d(gdir) # unit normal to the groove line
# The constraint anchor on ``a`` is the closest groove point to b's anchor,
# clamped to the closed segment so the lever arm matches the slider position.
anchor_a = _closest_point_on_segment_2d(anchor_b, ga, gb)
r_a = (anchor_a - ba.position).astype(np.float32)
r_b = j.r_b
pa = _perp_2d(r_a)
pb = _perp_2d(r_b)
va = ba.linear_velocity + ba.angular_velocity * pa
vb = bb.linear_velocity + bb.angular_velocity * pb
v_rel = (vb - va).astype(np.float32)
vn = float(np.dot(v_rel, perp)) # relative velocity across the groove line
rn_a = _cross_2d(r_a, perp)
rn_b = _cross_2d(r_b, perp)
k = inv_sum + ba.inverse_moment * rn_a * rn_a + bb.inverse_moment * rn_b * rn_b
if k <= 0.0:
return
jn = -vn / k
impulse = (jn * perp).astype(np.float32)
ba.linear_velocity = ba.linear_velocity - impulse * ba.inverse_mass
bb.linear_velocity = bb.linear_velocity + impulse * bb.inverse_mass
ba.angular_velocity = ba.angular_velocity - ba.inverse_moment * _cross_2d(r_a, impulse)
bb.angular_velocity = bb.angular_velocity + bb.inverse_moment * _cross_2d(r_b, impulse)
def _solve_fixed_angular_velocity(self, j: _FixedConstraint2D) -> None:
"""Lock the single relative rotational DOF (full 2D weld angular part).
Drives the scalar relative angular velocity ``w_rel = wb - wa`` to zero:
impulse ``dL = -w_rel / (inv_Ia + inv_Ib)`` applied to each spin. Skips
when both moments are infinite (inverse moment sum zero).
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_sum = ba.inverse_moment + bb.inverse_moment
if inv_sum == 0.0:
return
w_rel = bb.angular_velocity - ba.angular_velocity
dl = -w_rel / inv_sum
ba.angular_velocity = ba.angular_velocity - dl * ba.inverse_moment
bb.angular_velocity = bb.angular_velocity + dl * bb.inverse_moment
def _solve_spring_velocity(self, j: _SpringConstraint2D, dt: float) -> None:
"""Soft distance-spring velocity impulse between the two body CENTRES (2D).
Standard soft-constraint form, identical to the 3D ``_solve_spring_velocity``
over ``(2,)`` vectors: ``d = pos_b - pos_a``, ``L = |d|``, ``n = d/L``,
extension ``x = L - rest_length``, normal velocity ``v_n = (vb - va) . n``.
The spring FORCE is ``F = -(k*x + c*v_n)`` (Hooke + viscous damping); the
per-step IMPULSE is ``F * dt``, applied as a velocity change split by
inverse mass: ``J = -(k*x + c*v_n) * dt * m_eff`` with
``m_eff = 1/(inv_ma + inv_mb)``. Degenerate ``L < eps`` skips (no
direction).
HONESTY: an EXPLICIT (forward-Euler) soft impulse, so a high ``stiffness``
or ``damping`` relative to the fixed ``dt`` can overshoot / diverge; nothing
is silently clamped (stability bound ~``dt < 2 / sqrt(k * inv_sum)``). Stiff
springs need a smaller fixed ``dt`` or the pymunk backend. No position
correction (the spring is intentionally compliant).
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return
d = bb.position - ba.position
length = float(np.linalg.norm(d))
if length < _JOINT_EPS:
return # coincident centres: no spring direction this step
n = (d / length).astype(np.float32)
x = length - j.rest_length
v_n = float(np.dot(bb.linear_velocity - ba.linear_velocity, n))
m_eff = 1.0 / inv_sum
jmag = -(j.stiffness * x + j.damping * v_n) * dt * m_eff
impulse = (jmag * n).astype(np.float32)
ba.linear_velocity = ba.linear_velocity - impulse * ba.inverse_mass
bb.linear_velocity = bb.linear_velocity + impulse * bb.inverse_mass
# -- joint position solve (Baumgarte, rigid joints only) ----------------
def _solve_joint_position(self, j: _Constraint2D) -> None:
"""Dispatch one Baumgarte position pass for a single RIGID joint (2D).
Springs are intentionally compliant (no position correction) and are
skipped. Pin / Hinge correct the anchor-coincidence error; Fixed
additionally corrects the relative-angle error.
"""
if isinstance(j, _PinConstraint2D | _HingeConstraint2D):
self._correct_point_position(j.a, j.b, j.r_a, j.r_b)
elif isinstance(j, _GrooveConstraint2D):
self._correct_groove_position(j)
elif isinstance(j, _FixedConstraint2D):
# Hold the captured relative offset: target pos_b == pos_a + rel_pos,
# i.e. anchor offsets r_a = 0 on a, r_b = -rel_pos on b make the
# coincidence error C = (pos_b + r_b) - (pos_a + r_a) =
# (pos_b - rel_pos) - pos_a, zero exactly when the offset is preserved.
zero = np.zeros(2, dtype=np.float32)
self._correct_point_position(j.a, j.b, zero, (-j.rel_pos).astype(np.float32))
self._correct_fixed_angle(j)
# _SpringConstraint2D: intentionally no position correction.
def _correct_point_position(self, a: BodyHandle, b: BodyHandle, r_a: np.ndarray, r_b: np.ndarray) -> None:
"""Baumgarte push so the two world anchors coincide (linear, mass-split) (2D).
Positional error ``C = world_anchor_b - world_anchor_a`` with
``world_anchor = pos + r`` (anchor offset NOT rotated, basic tier). The
bodies are pushed apart by ``_BAUMGARTE * C`` split by inverse mass, with a
``_SLOP`` deadband to avoid jitter (matching the 3D joint Baumgarte).
"""
ba, bb = self._bodies[a], self._bodies[b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return
c = (bb.position + r_b) - (ba.position + r_a)
err = float(np.linalg.norm(c))
if err <= _SLOP:
return
corr = (_BAUMGARTE * c).astype(np.float32)
ba.position = ba.position + corr * (ba.inverse_mass / inv_sum)
bb.position = bb.position - corr * (bb.inverse_mass / inv_sum)
def _correct_groove_position(self, j: _GrooveConstraint2D) -> None:
"""Baumgarte push so ``b``'s anchor sits on the (clamped) groove line (2D) (T2g).
The positional error is the offset of ``b``'s world anchor from the closest
point on the closed groove segment: ``C = anchor_b - closest``. This single
vector captures BOTH the perpendicular line error AND the endpoint clamp
(when the slider has run past an end, ``closest`` is the endpoint, so ``C``
also pulls it back along the groove). The bodies are pushed by
``_BAUMGARTE * C`` split by inverse mass with the ``_SLOP`` deadband, exactly
like :meth:`_correct_point_position` (linear; the velocity solve carries the
angular response). The clamp is HARD (no endpoint softness; deferred to the
pymunk ``GrooveJoint``).
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return
ga = ba.position + j.ga
gb = ba.position + j.gb
anchor_b = bb.position + j.r_b
closest = _closest_point_on_segment_2d(anchor_b, ga, gb)
c = (anchor_b - closest).astype(np.float32) # error: b's anchor off the groove
err = float(np.linalg.norm(c))
if err <= _SLOP:
return
corr = (_BAUMGARTE * c).astype(np.float32)
# C points FROM the groove (on a) TO b's anchor, so push b back toward a.
ba.position = ba.position + corr * (ba.inverse_mass / inv_sum)
bb.position = bb.position - corr * (bb.inverse_mass / inv_sum)
def _correct_fixed_angle(self, j: _FixedConstraint2D) -> None:
"""Baumgarte angular push toward the captured relative angle (2D weld).
Current relative angle ``rel = b.rotation - a.rotation``; the scalar error
is ``rel_angle - rel`` (no quaternion / shortest-arc needed in 2D, but the
error is wrapped to ``(-pi, pi]`` so a multi-turn body takes the short way).
Applied as a scalar angular nudge split by inverse moment of inertia.
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_sum = ba.inverse_moment + bb.inverse_moment
if inv_sum == 0.0:
return
rel = bb.rotation - ba.rotation
err = j.rel_angle - rel
err = (err + math.pi) % (2.0 * math.pi) - math.pi # shortest-arc wrap
if abs(err) <= _JOINT_EPS:
return
corr = _BAUMGARTE * err
ba.rotation = ba.rotation - corr * (ba.inverse_moment / inv_sum)
bb.rotation = bb.rotation + corr * (bb.inverse_moment / inv_sum)
# -- CCD (continuous, centre-sweep vs STATIC) --------------------------
def _ccd_advance(self, body: _Body2D, target: np.ndarray) -> np.ndarray:
"""Sweep body centre old->target vs STATIC bodies; clamp to TOI (basic tier).
2D sibling of the 3D ``_ccd_advance``. Anti-tunnelling for a fast DYNAMIC
body: casts the body CENTRE (origin = old pos, direction = displacement)
against STATIC bodies only, extended by the mover's smallest feature so the
centre ray stops a feature-radius before the surface (approximating a swept
shape). On a hit the body is placed at ``hit - feature`` along the ray and
the velocity component INTO the surface is zeroed; the discrete solver then
resolves the resting contact normally (restitution / friction / Baumgarte)
on the same step.
Basic-tier honesty: CENTRE sweep vs STATIC only. No rotational sweep, no
dynamic-vs-dynamic CCD, and the segment cast shares the tier's per-shape
AABB-ish geometry (a fast body grazing a corner can still tunnel it). The
pymunk backend honours ``continuous`` properly. ``continuous`` defaults
False, so existing bodies are unchanged.
"""
old = body.position
motion = target - old
dist = float(np.linalg.norm(motion))
if dist < 1e-9:
return target
feature = max(self._feature_size_2d(body.shape), 1e-4)
direction = (motion / dist).astype(np.float32)
best_toi = dist
best_n: np.ndarray | None = None
for other in self._bodies.values():
if other is body or other.body_type is not BodyMode.STATIC:
continue
if not _layers_match(
body.collision_layer, body.collision_mask, other.collision_layer, other.collision_mask
):
continue
hit = self._raycast_body_2d(other, old, direction, dist + feature)
if hit is None:
continue
toi = hit[0] - feature # stop a feature-radius short of the surface
if toi < best_toi:
best_toi = max(0.0, toi)
best_n = hit[1]
if best_n is None:
return target
stopped = (old + direction * best_toi).astype(np.float32)
# Zero the velocity component INTO the surface; the discrete solver then
# handles the resting contact this step.
vn = float(np.dot(body.linear_velocity, best_n))
if vn < 0.0:
body.linear_velocity = body.linear_velocity - vn * best_n
return stopped
@staticmethod
def _feature_size_2d(shape: _Shape2D) -> float:
"""Smallest feature size of a 2D shape (radius / min half-extent).
Sizes the CCD feature margin so the swept centre stops a feature-radius
before the surface. Circle / capsule / segment use their radius; box / poly
/ concave use the smallest local AABB half-extent.
"""
kind = shape.kind
if kind in ("circle", "capsule", "segment"):
return float(shape.params[0]) # radius (thinnest feature)
return float(np.min(shape.params[:2])) # box / poly / concave half-extent
def _raycast_body_2d(
self, body: _Body2D, o: np.ndarray, d: np.ndarray, max_dist: float
) -> tuple[float, np.ndarray] | None:
"""Cast a ray (unit ``d``) at one STATIC body, returning ``(t, normal)``.
A minimal segment cast used ONLY by CCD (the full query suite is T2d). It
reduces every STATIC shape to a circle, (thick) segment, or oriented box
the cast already needs: circle / capsule via point-to-segment-core inflate,
segment / concave via the line cores, box / poly via the convex-edge slab.
Returns the nearest positive ``t`` within ``max_dist`` and the surface
normal (opposing the ray), or ``None`` on a miss. Basic-tier: this is the
same centre-sweep-vs-STATIC honesty as the 3D ``_ccd_advance``.
"""
kind = body.shape.kind
if kind == "circle":
centre, r = self._world_circle(body)
return self._ray_circle_2d(o, d, max_dist, centre, r)
if kind == "capsule":
p0, p1, r = self._world_capsule_segment(body)
return self._ray_capsule_2d(o, d, max_dist, p0, p1, r)
if kind == "segment":
pa, pb, r = self._world_segment(body)
return self._ray_capsule_2d(o, d, max_dist, pa, pb, max(r, 1e-4))
if kind == "concave":
return self._ray_concave_2d(body, o, d, max_dist)
# box / poly: convex polygon edge cast.
return self._ray_poly_2d(self._world_poly(body), o, d, max_dist)
@staticmethod
def _ray_circle_2d(
o: np.ndarray, d: np.ndarray, max_dist: float, centre: np.ndarray, radius: float
) -> tuple[float, np.ndarray] | None:
"""Ray vs circle: nearest entry ``t`` and outward surface normal."""
m = (o - centre).astype(np.float32)
b = float(np.dot(m, d))
c = float(np.dot(m, m)) - radius * radius
if c > 0.0 and b > 0.0:
return None # origin outside, pointing away
disc = b * b - c
if disc < 0.0:
return None
t = -b - disc**0.5
if t < 0.0:
t = 0.0 # origin inside the circle: hit at the origin
if t > max_dist:
return None
hit = o + d * t
n = (hit - centre).astype(np.float32)
ln = float(np.linalg.norm(n))
normal = (n / ln).astype(np.float32) if ln > 1e-9 else (-d).astype(np.float32)
return t, normal
def _ray_capsule_2d(
self, o: np.ndarray, d: np.ndarray, max_dist: float, p0: np.ndarray, p1: np.ndarray, radius: float
) -> tuple[float, np.ndarray] | None:
"""Ray vs (thick) segment / capsule: exact core-line + radius cap test.
Two analytic candidates, nearest positive ``t`` within ``max_dist`` wins:
(1) the ray vs the core LINE inflated to a slab of half-width ``radius``
(the flat side of a thick segment / the straight part of a capsule), and
(2) the ray vs the two end CAP circles. This is exact for the CCD centre
sweep even when the wall is much thinner than a discrete step (the coarse
marching sampler missed thin walls). The normal opposes the ray.
"""
core = (p1 - p0).astype(np.float32)
cl = float(np.linalg.norm(core))
best: tuple[float, np.ndarray] | None = None
def consider(cand: tuple[float, np.ndarray] | None) -> None:
nonlocal best
if cand is not None and (best is None or cand[0] < best[0]):
best = cand
if cl > 1e-9:
tangent = core / cl
normal = np.array([-tangent[1], tangent[0]], dtype=np.float32) # core perpendicular
denom = float(np.dot(d, normal))
side = float(np.dot(o - p0, normal)) # signed distance of origin to the core line
if abs(denom) > 1e-12:
# Cross each slab face (core line offset by +-radius along the normal).
for off in (radius, -radius):
t = (off - side) / denom
if 0.0 <= t <= max_dist:
hit = o + d * t
# Within the core span (not past the caps)?
along = float(np.dot(hit - p0, tangent))
if -1e-6 <= along <= cl + 1e-6:
face_n = normal if side >= 0.0 else (-normal).astype(np.float32)
consider((t, face_n.astype(np.float32)))
# End caps (and the degenerate zero-length segment == a single circle).
consider(self._ray_circle_2d(o, d, max_dist, p0, radius))
if cl > 1e-9:
consider(self._ray_circle_2d(o, d, max_dist, p1, radius))
return best
def _ray_poly_2d(
self, verts: np.ndarray, o: np.ndarray, d: np.ndarray, max_dist: float
) -> tuple[float, np.ndarray] | None:
"""Ray vs convex polygon via the half-plane slab test (entry t + normal)."""
n = len(verts)
t_enter = 0.0
t_exit = max_dist
enter_normal: np.ndarray | None = None
for i in range(n):
a = verts[i]
b = verts[(i + 1) % n]
edge = b - a
outward = np.array([edge[1], -edge[0]], dtype=np.float32) # outward for CCW
ln = float(np.linalg.norm(outward))
if ln <= 1e-9:
continue
outward = (outward / ln).astype(np.float32)
denom = float(np.dot(d, outward))
dist = float(np.dot(o - a, outward)) # signed distance to the face plane
if abs(denom) < 1e-12:
if dist > 0.0:
return None # parallel and outside this face: miss
continue
t = -dist / denom
if denom < 0.0: # entering the half-plane
if t > t_enter:
t_enter = t
enter_normal = outward
else: # exiting
t_exit = min(t_exit, t)
if t_enter > t_exit:
return None
if enter_normal is None or t_enter > max_dist:
return None
return t_enter, enter_normal
def _ray_concave_2d(
self, body: _Body2D, o: np.ndarray, d: np.ndarray, max_dist: float
) -> tuple[float, np.ndarray] | None:
"""Ray vs STATIC concave edge soup: nearest thin-segment crossing."""
c = math.cos(body.rotation)
s = math.sin(body.rotation)
segs = body.shape.points # (N, 2, 2) body-local
best: tuple[float, np.ndarray] | None = None
for k in range(len(segs)):
a_world = body.position + _rotate_2d(segs[k, 0], c, s)
b_world = body.position + _rotate_2d(segs[k, 1], c, s)
hit = self._ray_capsule_2d(o, d, max_dist, a_world, b_world, 1e-4)
if hit is not None and (best is None or hit[0] < best[0]):
best = hit
return best
# -- sleeping (post-solve pass) ----------------------------------------
def _update_sleeping(self, dt: float) -> None:
"""Post-solve sleep pass over every awake DYNAMIC body (2D).
2D sibling of the 3D ``_update_sleeping``: a body whose SETTLED linear AND
|angular| speed stay below the thresholds for _SLEEP_TIME continuous seconds
goes to sleep (skipped by _integrate and the contact / joint velocity solve
until woken), killing residual jitter on resting stacks. Reads the
post-solve velocity (see step()): a resting body's pre-solve velocity
carries this step's gravity, which the solver cancels. STATIC / KINEMATIC
never sleep (never integrated, never timed).
"""
lin_sq = _SLEEP_LINEAR * _SLEEP_LINEAR
for body in self._bodies.values():
if body.body_type is not BodyMode.DYNAMIC or body.asleep:
continue
slow = (
float(np.dot(body.linear_velocity, body.linear_velocity)) < lin_sq
and abs(body.angular_velocity) < _SLEEP_ANGULAR
)
if slow:
body._sleep_timer += dt
if body._sleep_timer >= _SLEEP_TIME:
body.asleep = True
body.linear_velocity[:] = 0.0 # kill residual jitter velocity
body.angular_velocity = 0.0
else:
body._sleep_timer = 0.0
# -- bulk transfer (the keystone) --------------------------------------
[docs]
def register_bodies(self, handles: list[BodyHandle]) -> None:
for h in handles:
assert h in self._bodies, f"register_bodies: unknown body handle {h!r}"
self._order = list(handles)
[docs]
def read_velocities(self, out: np.ndarray) -> None:
# (N, 3) = [lx, ly, omega]. Fills in place, allocates nothing.
self._check_velocities_out(out, len(self._order))
for i, handle in enumerate(self._order):
body = self._bodies[handle]
out[i, 0] = body.linear_velocity[0]
out[i, 1] = body.linear_velocity[1]
out[i, 2] = body.angular_velocity
# -- forces (T2c) -------------------------------------------------------
[docs]
def apply_impulse(self, handle: BodyHandle, impulse: Vec2, *, at: Vec2 | None = None, angular: float = 0.0) -> None:
body = self._bodies[handle]
if body.inverse_mass == 0.0:
return # infinite-mass (STATIC / KINEMATIC): physically inert
self._wake(body) # an applied impulse is a disturbance: wake a sleeper
lin = _as_array2(impulse)
body.linear_velocity = body.linear_velocity + lin * body.inverse_mass
# Scalar angular response: explicit angular impulse plus the lever-arm
# contribution from an off-centre application point (2D cross is scalar).
if angular != 0.0:
body.angular_velocity = body.angular_velocity + float(angular) * body.inverse_moment
if at is not None:
r = _as_array2(at) - body.position
body.angular_velocity = body.angular_velocity + body.inverse_moment * _cross_2d(r, lin)
[docs]
def apply_force(self, handle: BodyHandle, force: Vec2, *, at: Vec2 | None = None) -> None:
body = self._bodies[handle]
if body.inverse_mass == 0.0:
return
self._wake(body) # an applied force is a disturbance: wake a sleeper
f = _as_array2(force)
body.force = body.force + f
if at is not None:
r = _as_array2(at) - body.position
body.torque += _cross_2d(r, f) # scalar torque from the lever arm
[docs]
def apply_torque(self, handle: BodyHandle, torque: float) -> None:
body = self._bodies[handle]
if body.inverse_mass == 0.0:
return
self._wake(body) # an applied torque is a disturbance: wake a sleeper
body.torque += float(torque)
# -- joints / constraints (T2c) ----------------------------------------
def _alloc_joint(self) -> int:
"""Allocate a joint handle from the SEPARATE joint counter.
Kept distinct from ``_alloc_handle`` so a joint handle never aliases a body
handle (parity with the 3D backend).
"""
h = self._next_joint
self._next_joint += 1
return h
[docs]
def create_fixed_joint(self, a: BodyHandle, b: BodyHandle) -> JointHandle:
assert a in self._bodies, f"create_fixed_joint: unknown body handle {a!r}"
assert b in self._bodies, f"create_fixed_joint: unknown body handle {b!r}"
ba, bb = self._bodies[a], self._bodies[b]
rel_pos = (bb.position - ba.position).astype(np.float32)
rel_angle = float(bb.rotation - ba.rotation) # captured relative angle
handle = self._alloc_joint()
self._joints[handle] = _FixedConstraint2D(a=a, b=b, rel_pos=rel_pos, rel_angle=rel_angle)
return handle
[docs]
def create_pin_joint(self, a: BodyHandle, b: BodyHandle, anchor: Vec2) -> JointHandle:
assert a in self._bodies, f"create_pin_joint: unknown body handle {a!r}"
assert b in self._bodies, f"create_pin_joint: unknown body handle {b!r}"
ba, bb = self._bodies[a], self._bodies[b]
anc = _as_array2(anchor)
handle = self._alloc_joint()
self._joints[handle] = _PinConstraint2D(
a=a, b=b, r_a=(anc - ba.position).astype(np.float32), r_b=(anc - bb.position).astype(np.float32)
)
return handle
[docs]
def create_hinge_joint(self, a: BodyHandle, b: BodyHandle, anchor: Vec2) -> JointHandle:
# 2D rotation is 1-DOF, so a 2D hinge that leaves rotation free is
# behaviourally IDENTICAL to a pin this tier (no off-axis rotation to lock).
# Motors and angular limits are a DOCUMENTED follow-on (see
# _HingeConstraint2D); pymunk's PivotJoint + SimpleMotor / RotaryLimitJoint
# is the eventual home.
assert a in self._bodies, f"create_hinge_joint: unknown body handle {a!r}"
assert b in self._bodies, f"create_hinge_joint: unknown body handle {b!r}"
ba, bb = self._bodies[a], self._bodies[b]
anc = _as_array2(anchor)
handle = self._alloc_joint()
self._joints[handle] = _HingeConstraint2D(
a=a, b=b, r_a=(anc - ba.position).astype(np.float32), r_b=(anc - bb.position).astype(np.float32)
)
return handle
[docs]
def create_spring_joint(
self, a: BodyHandle, b: BodyHandle, rest_length: float, stiffness: float, damping: float
) -> JointHandle:
assert a in self._bodies, f"create_spring_joint: unknown body handle {a!r}"
assert b in self._bodies, f"create_spring_joint: unknown body handle {b!r}"
ba, bb = self._bodies[a], self._bodies[b]
rl = float(rest_length)
if rl < 0.0:
# Auto-capture the current centre distance as the rest length (the
# documented rest_length == -1 convention).
rl = float(np.linalg.norm(bb.position - ba.position))
handle = self._alloc_joint()
self._joints[handle] = _SpringConstraint2D(
a=a, b=b, rest_length=rl, stiffness=float(stiffness), damping=float(damping)
)
return handle
[docs]
def create_groove_joint(
self, a: BodyHandle, b: BodyHandle, groove_a: Vec2, groove_b: Vec2, anchor_b: Vec2
) -> JointHandle:
"""Constrain ``b``'s ``anchor_b`` to slide along ``a``'s groove segment (T2g).
``groove_a`` / ``groove_b`` are body-LOCAL points in ``a``'s frame defining
the groove segment; ``anchor_b`` is a body-local point in ``b``'s frame. They
are converted to the world-axes offsets the constraint records use (anchors
captured at create, rotation ignored, basic tier; see
:class:`_GrooveConstraint2D`). The world groove direction must be non-zero.
"""
assert a in self._bodies, f"create_groove_joint: unknown body handle {a!r}"
assert b in self._bodies, f"create_groove_joint: unknown body handle {b!r}"
ba, bb = self._bodies[a], self._bodies[b]
c_a, s_a = math.cos(ba.rotation), math.sin(ba.rotation)
c_b, s_b = math.cos(bb.rotation), math.sin(bb.rotation)
# Local anchor -> world point -> world-axes offset from each body centre.
ga_world = ba.position + _rotate_2d(_as_array2(groove_a), c_a, s_a)
gb_world = ba.position + _rotate_2d(_as_array2(groove_b), c_a, s_a)
anc_world = bb.position + _rotate_2d(_as_array2(anchor_b), c_b, s_b)
assert float(np.linalg.norm(gb_world - ga_world)) > _JOINT_EPS, "groove endpoints must differ"
handle = self._alloc_joint()
self._joints[handle] = _GrooveConstraint2D(
a=a,
b=b,
ga=(ga_world - ba.position).astype(np.float32),
gb=(gb_world - ba.position).astype(np.float32),
r_b=(anc_world - bb.position).astype(np.float32),
)
return handle
[docs]
def remove_joint(self, handle: JointHandle) -> None:
# No-op on an unknown handle: the joint may already have been silently
# dropped by destroy_body's purge when one of its bodies was freed (the
# SAME silent-drop contract used by the 3D backend, not an error-swallowing
# shim), so a joint node's on_exit_tree is idempotent in either order.
self._joints.pop(handle, None)
# -- one-way platforms (T2e) -------------------------------------------
[docs]
def set_one_way(self, handle: BodyHandle, enabled: bool, normal: Vec2 = _DEFAULT_UP_2D) -> None:
"""Mark a body as a one-way platform (Stage T2e).
Stores the enabled flag and the world-space pass-through ("solid side")
normal on the body. When enabled, the contact filter (:meth:`_one_way_rejects`,
applied in :meth:`_collide`, :meth:`move_and_collide` and the character
sweep) keeps a contact only when the other body lands from the ``+normal``
side and discards it when the other body passes up through. The normal is
normalised (a degenerate zero normal falls back to ``+Y``, a floor).
"""
body = self._bodies[handle]
body.one_way = bool(enabled)
n = _as_array2(normal)
length = float(np.linalg.norm(n))
body.one_way_normal = (n / length).astype(np.float32) if length > 1e-9 else _DEFAULT_UP_2D.copy()
self._wake(body) # toggling one-way changes contact resolution: a disturbance
# -- queries (T2d) ------------------------------------------------------
[docs]
def raycast(self, origin: Vec2, direction: Vec2, max_dist: float, *, mask: int = 0xFFFFFFFF) -> RaycastHit2D | None:
"""Cast a ray, return the nearest body whose layer matches ``mask``.
2D sibling of the 3D ``raycast``: a single query-mask convention
(``mask & body.collision_layer``: the OBSERVER decides, unlike the body-body
AND rule). Reuses the T2c ``_raycast_body_2d`` per-shape helpers (the exact
analytic casts CCD already needs) over every body; the nearest positive
``t`` within ``max_dist`` wins. An infinite ``max_dist`` is fine (the casts
are analytic). Returns ``None`` on a clean miss.
"""
o = _as_array2(origin)
d = _as_array2(direction)
length = float(np.linalg.norm(d))
if length < 1e-12:
return None
d = (d / length).astype(np.float32) # normalised ray direction
best: RaycastHit2D | None = None
best_dist = max_dist
for handle, body in self._bodies.items():
if not (mask & body.collision_layer): # single query-mask convention
continue
hit = self._raycast_body_2d(body, o, d, best_dist)
if hit is None:
continue
t, normal = hit
if t < best_dist:
best_dist = t
point = Vec2((o + d * t).astype(np.float32))
best = RaycastHit2D(body=handle, point=point, normal=Vec2(normal), distance=t)
return best
[docs]
def raycast_all(
self, origin: Vec2, direction: Vec2, max_dist: float, *, mask: int = 0xFFFFFFFF
) -> list[RaycastHit2D]:
"""Cast a ray, return EVERY hit within ``max_dist`` sorted by distance.
2D sibling of the 3D ``raycast_all``: the same per-body ``_raycast_body_2d``
cast as :meth:`raycast`, but collects all hits and sorts by distance (so the
first element is the nearest). Single query-mask convention.
"""
o = _as_array2(origin)
d = _as_array2(direction)
length = float(np.linalg.norm(d))
if length < 1e-12:
return []
d = (d / length).astype(np.float32)
hits: list[RaycastHit2D] = []
for handle, body in self._bodies.items():
if not (mask & body.collision_layer):
continue
hit = self._raycast_body_2d(body, o, d, max_dist)
if hit is None:
continue
t, normal = hit
point = Vec2((o + d * t).astype(np.float32))
hits.append(RaycastHit2D(body=handle, point=point, normal=Vec2(normal), distance=t))
hits.sort(key=lambda h: h.distance)
return hits
def _make_probe_body(self, shape: _Shape2D, position: np.ndarray, rotation: float) -> _Body2D:
"""Build a transient infinite-mass KINEMATIC probe body wrapping ``shape``.
Used by :meth:`shapecast` / :meth:`overlap` to reuse the ``_narrow``
primitives without entering the body table (2D sibling of the 3D
``_make_probe``). Layer/mask stay at the permissive defaults; the single
query mask is applied per-body by the caller.
"""
return _Body2D(
shape=shape,
body_type=BodyMode.KINEMATIC,
position=_as_array2(position),
rotation=float(rotation),
mass=0.0,
inverse_mass=0.0,
moment=0.0,
inverse_moment=0.0,
)
[docs]
def shapecast(
self, shape: ShapeHandle, origin: Vec2, direction: Vec2, max_dist: float, *, mask: int = 0xFFFFFFFF
) -> Contact2D | None:
"""Sweep ``shape`` from ``origin`` along ``direction``, earliest-TOI contact.
Basic-tier honesty: a SUBSTEPPED sweep (mirrors the 3D ``shapecast``), not a
true continuous TOI: a transient probe body is advanced along the ray and
the first substep that the narrowphase reports penetrating an OTHER body is
the earliest hit; the probe backs off to the previous non-penetrating
substep and reports the contact. Substep count is sized from the probe's
smallest feature, capped at 64. A fast sweep past a very thin collider can
tunnel between substeps; an analytic shape-cast (conservative advancement) is
deferred to the pymunk backend. Single query-mask convention
(``mask & body.layer``). Concave shapes are STATIC-only and cannot be the
moving probe (asserted). Returns ``None`` when the sweep stays clear.
"""
assert shape in self._shapes, f"unknown shape handle {shape!r}"
# A substepped sweep needs a finite length to size its substeps: an unbounded
# shapecast is undefined for the basic tier (raycast is analytic and accepts
# inf, but a swept shape is not). Fail loudly rather than overflow ceil().
assert math.isfinite(max_dist), "shapecast max_dist must be finite (a swept shape needs a bounded sweep length)"
probe_shape = self._shapes[shape]
if probe_shape.kind == "concave":
raise ValueError("concave shapes are STATIC-only and cannot be used as a moving query shape")
o = _as_array2(origin)
d = _as_array2(direction)
length = float(np.linalg.norm(d))
if length < 1e-12 or max_dist <= 0.0:
return None
dir_unit = (d / length).astype(np.float32)
motion = dir_unit * float(max_dist)
probe = self._make_probe_body(probe_shape, o, 0.0)
feature = max(self._feature_size_2d(probe_shape), 1e-4)
steps = min(64, max(1, math.ceil(max_dist / (feature * 0.5))))
probe_handle = -1
prev_frac = 0.0
for s in range(1, steps + 1):
frac = s / steps
probe.position = (o + motion * frac).astype(np.float32)
for ho, bo in self._bodies.items():
if not (mask & bo.collision_layer):
continue
contact = self._narrow(probe_handle, probe, ho, bo)
if contact is None:
continue
# _Contact2D.normal points a -> b; here a = probe (mover), b = other,
# so it points INTO the other. The public Contact2D.normal must point
# back toward the mover (the separating direction): negate.
n = (-contact.normal).astype(np.float32)
reached = (o + motion * prev_frac).astype(np.float32)
travelled = float(max_dist) * prev_frac
point = (reached - n * feature).astype(np.float32)
return Contact2D(body=ho, point=Vec2(point), normal=Vec2(n), distance=travelled)
prev_frac = frac
return None
[docs]
def overlap(self, shape: ShapeHandle, transform: object, *, mask: int = 0xFFFFFFFF) -> list[BodyHandle]:
"""All bodies overlapping ``shape`` at ``transform``, sorted by handle.
2D sibling of the 3D ``overlap``: places a transient probe body at
``transform`` (a ``Transform2D`` / bare position / ``(position, rotation)``
pair, via the same ``_unpack_transform``) and returns every body it overlaps
whose ``layer & mask`` is set. Broadphase AABB cull then the exact
narrowphase, mirroring ``_collide``'s prefilter. Sorted by handle for
determinism. Concave shapes are STATIC-only and cannot be the probe.
"""
assert shape in self._shapes, f"unknown shape handle {shape!r}"
probe_shape = self._shapes[shape]
if probe_shape.kind == "concave":
raise ValueError("concave shapes are STATIC-only and cannot be used as a moving query shape")
position, rotation = self._unpack_transform(transform)
probe = self._make_probe_body(probe_shape, position, rotation)
lo_p, hi_p = self._aabb(probe)
result: list[BodyHandle] = []
for ho, bo in self._bodies.items():
if not (mask & bo.collision_layer): # single query-mask convention
continue
lo_b, hi_b = self._aabb(bo)
if not self._aabb_overlap(lo_p, hi_p, lo_b, hi_b):
continue # broadphase cull before the exact narrowphase
if self._narrow(-1, probe, ho, bo) is not None:
result.append(ho)
return sorted(result)
[docs]
def move_and_collide(self, handle: BodyHandle, motion: Vec2) -> Contact2D | None:
"""Move body ``handle`` by ``motion``, stop at the first contact.
2D sibling of the 3D ``move_and_collide`` and the kinematic-sweep primitive
the character controller (T2e) builds on. Basic-tier honesty: a SUBSTEPPED
narrowphase sweep (the narrowphase is analytic overlap, not a continuous
TOI), sized from the mover's smallest feature and capped at 64 substeps. It
advances the body along ``motion``; the first substep that penetrates an
OTHER body backs the mover off to the previous non-penetrating substep and
reports the contact. A fast mover vs a very thin collider can still tunnel
between substeps (replaced by an analytic sweep in the pymunk backend).
Returns ``None`` (and applies the full move) when the path stays clear.
"""
body = self._bodies[handle]
start = body.position.copy()
m = _as_array2(motion)
dist = float(np.linalg.norm(m))
if dist < 1e-9:
return None
feature = max(self._feature_size_2d(body.shape), 1e-4)
steps = min(64, max(1, math.ceil(dist / (feature * 0.5))))
prev_frac = 0.0
for s in range(1, steps + 1):
frac = s / steps
body.position = (start + m * frac).astype(np.float32)
for ho, bo in self._bodies.items():
if ho == handle:
continue
if not _layers_match(
body.collision_layer, body.collision_mask, bo.collision_layer, bo.collision_mask
):
continue
contact = self._narrow(handle, body, ho, bo)
if contact is None:
continue
# One-way filter (Stage T2e): a one-way platform only blocks the
# sweep when the mover lands on its solid side, so a kinematic body
# (or character) sweeping UP through the platform is not stopped. The
# mover's relative approach is its motion direction this step (the
# bodies' stored linear velocities do not drive a kinematic sweep).
if (body.one_way or bo.one_way) and self._one_way_rejects(contact, body, bo, m, bo.linear_velocity):
continue
# Back off to the last non-penetrating substep. _Contact2D.normal
# points a -> b (mover -> other); the public normal must point back
# toward the mover (the separating direction): negate.
body.position = (start + m * prev_frac).astype(np.float32)
travelled = dist * prev_frac
n = (-contact.normal).astype(np.float32)
point = (body.position - n * feature).astype(np.float32)
return Contact2D(body=ho, point=Vec2(point), normal=Vec2(n), distance=travelled)
prev_frac = frac
# No contact across all substeps: apply the full move.
body.position = (start + m).astype(np.float32)
return None
# -- character controller (T2e) ----------------------------------------
def _alloc_char_handle(self) -> int:
h = self._next_char
self._next_char += 1
return h
[docs]
def create_character(
self,
shape: ShapeHandle,
transform: object,
*,
up: Vec2 = _DEFAULT_UP_2D,
slope_limit: float = math.radians(45.0),
step_height: float = 0.0,
skin_width: float = 0.001,
collision_layer: int = 1,
collision_mask: int = 0xFFFFFFFF,
) -> CharacterHandle:
"""Create a kinematic character controller (Stage T2e).
2D sibling of the 3D ``create_character``: the character is NOT registered
as a body (the node holds the handle), so it never appears in the solver,
the bulk readers, or ``read_transforms``. ``up`` is normalised; ``slope_limit``
is radians (the maximum floor incline the character treats as ground).
"""
assert shape in self._shapes, f"unknown shape handle {shape!r}"
position, rotation = self._unpack_transform(transform)
u = _as_array2(up)
ulen = float(np.linalg.norm(u))
u = (u / ulen).astype(np.float32) if ulen > 1e-9 else _DEFAULT_UP_2D.copy()
handle = self._alloc_char_handle()
self._characters[handle] = _Character2D(
shape=self._shapes[shape],
position=position,
rotation=rotation,
up=u,
slope_limit=float(slope_limit),
step_height=float(step_height),
skin_width=float(skin_width),
collision_layer=collision_layer,
collision_mask=collision_mask,
)
return handle
[docs]
def destroy_character(self, handle: CharacterHandle) -> None:
assert handle in self._characters, f"unknown character handle {handle!r}"
del self._characters[handle]
def _sweep_character_2d(self, char: _Character2D, motion: np.ndarray) -> Contact2D | None:
"""Substepped sweep of a character's shape along ``motion`` vs bodies (T2e).
2D sibling of the 3D ``_sweep_character``: wraps the character's shape +
pose in a transient KINEMATIC ``_Body2D`` so the existing ``_narrow``
primitives apply unchanged, then runs the same substepped scan as
:meth:`move_and_collide`. Returns a :class:`Contact2D` (post-back-off
position, separating normal pointing toward the mover) or ``None``.
Only contacts that OPPOSE the sweep (``dot(motion_dir, separating_normal)
< 0``) are reported: a non-opposing touch (the floor the character already
rests on while it moves horizontally) is not a blocker, otherwise every
slide would halt at distance 0 and the character could never walk (matches
Jolt's ``CharacterVirtual`` and the 3D path). The one-way filter is honoured
so a one-way platform only blocks the character from above (landing).
"""
dist = float(np.linalg.norm(motion))
if dist < 1e-9:
return None
direction = (motion / dist).astype(np.float32)
probe = _Body2D(
shape=char.shape,
body_type=BodyMode.KINEMATIC,
position=char.position.copy(),
rotation=char.rotation,
mass=0.0,
inverse_mass=0.0,
moment=0.0,
inverse_moment=0.0,
collision_layer=char.collision_layer,
collision_mask=char.collision_mask,
)
start = char.position.copy()
feature = max(self._feature_size_2d(char.shape), 1e-4)
steps = min(64, max(1, math.ceil(dist / (feature * 0.5))))
# A handle distinct from every body handle so _narrow's self-skip never
# accidentally matches; the probe is not in self._bodies.
probe_handle = -1
prev_frac = 0.0
for s in range(1, steps + 1):
frac = s / steps
probe.position = (start + motion * frac).astype(np.float32)
for ho, bo in self._bodies.items():
if not _layers_match(
probe.collision_layer, probe.collision_mask, bo.collision_layer, bo.collision_mask
):
continue
contact = self._narrow(probe_handle, probe, ho, bo)
if contact is None:
continue
# One-way filter: the character's relative approach is this sweep's
# motion; a one-way platform lets it pass up through and only blocks
# a top landing (so the character can jump up through and land on it).
if bo.one_way and self._one_way_rejects(contact, probe, bo, motion, bo.linear_velocity):
continue
# a = probe (mover), b = other: negate to point toward the mover.
n = (-contact.normal).astype(np.float32)
# Skip non-opposing touches: the mover is not advancing into this
# surface, so it does not block this sweep direction.
if float(np.dot(direction, n)) > -1e-4:
continue
reached = (start + motion * prev_frac).astype(np.float32)
travelled = dist * prev_frac
point = (reached - n * feature).astype(np.float32)
return Contact2D(body=ho, point=Vec2(point), normal=Vec2(n), distance=travelled)
prev_frac = frac
return None
[docs]
def character_move_and_slide(
self, handle: CharacterHandle, velocity: Vec2, dt: float, *, up: Vec2, max_slides: int = 4
) -> CharacterMoveResult2D:
"""Arcade collide-and-slide against world bodies (basic tier) (T2e).
2D sibling of the 3D ``character_move_and_slide``: sweeps the character
along ``velocity * dt``; on each blocking contact it advances to the contact,
classifies floor / wall / ceiling from the contact normal vs ``up`` and the
stored ``slope_limit``, deflects both the remaining motion and the velocity
out of the surface (projects out the normal component), and repeats up to
``max_slides`` times. A walkable surface within a small snap distance below
the feet sets ``on_floor`` + ``floor_normal`` via a dedicated downward probe
(the horizontal sweep intentionally ignores the non-opposing floor a walking
character rests on). Basic-tier honesty: a single up / forward / down
``_try_step_up_2d`` probe handles a ledge up to ``step_height`` (one step
only); a multi-step stair and a robust depenetration are pymunk concerns.
"""
char = self._characters[handle]
u = _as_array2(up)
ulen = float(np.linalg.norm(u))
up_n = (u / ulen).astype(np.float32) if ulen > 1e-9 else char.up.copy()
vel = _as_array2(velocity)
motion = (vel * float(dt)).astype(np.float32)
cos_slope = math.cos(char.slope_limit)
on_floor = on_wall = on_ceiling = False
floor_n = up_n.copy()
for _ in range(max_slides):
if float(np.linalg.norm(motion)) < 1e-9:
break
hit = self._sweep_character_2d(char, motion)
if hit is None:
char.position = (char.position + motion).astype(np.float32)
break
travelled = float(hit.distance)
full = float(np.linalg.norm(motion))
frac = travelled / full if full > 1e-9 else 0.0
char.position = (char.position + motion * frac).astype(np.float32) # advance to contact
n = _as_array2(hit.normal) # separating normal, toward mover, unit
d = float(np.dot(n, up_n))
if d > cos_slope:
on_floor = True
floor_n = n.copy()
elif d < -cos_slope:
on_ceiling = True
else:
on_wall = True
if char.step_height > 0.0 and self._try_step_up_2d(char, motion * (1.0 - frac), up_n):
# Step-up consumed the remaining horizontal motion this slide.
break
# Deflect remaining motion and velocity out of the surface.
remaining = motion * (1.0 - frac)
remaining = (remaining - n * min(0.0, float(np.dot(remaining, n)))).astype(np.float32)
vel = (vel - n * min(0.0, float(np.dot(vel, n)))).astype(np.float32)
motion = remaining
# Dedicated ground probe: the horizontal sweep above intentionally ignores
# the non-opposing floor a walking character rests on, so floor state is
# established here with a short downward cast (mirrors Jolt's GetGroundState
# being valid right after the move). A one-way platform is honoured by the
# sweep so the probe still reports it as ground when resting on top.
snap = max(char.skin_width * 4.0, 1e-3)
ground = self._sweep_character_2d(char, -up_n * snap)
if ground is not None:
gn = _as_array2(ground.normal)
if float(np.dot(gn, up_n)) > cos_slope:
on_floor = True
floor_n = gn.copy()
return CharacterMoveResult2D(
velocity=Vec2(vel),
on_floor=on_floor,
on_wall=on_wall,
on_ceiling=on_ceiling,
floor_normal=Vec2(floor_n),
)
def _try_step_up_2d(self, char: _Character2D, horizontal: np.ndarray, up_n: np.ndarray) -> bool:
"""Single up / forward / down step probe (basic tier: one step only) (T2e).
2D sibling of the 3D ``_try_step_up``: lifts by ``step_height``, sweeps the
blocked horizontal motion, then drops back down. Commits the new pose if the
up + forward legs were unobstructed and the drop lands on ground; otherwise
leaves the character untouched and returns ``False`` so the caller falls back
to wall-slide. Basic-tier: a single step (a full multi-step stair walk and a
per-substep ground snap are deferred to the pymunk backend).
"""
if float(np.linalg.norm(horizontal)) < 1e-9:
return False
origin = char.position.copy()
up_motion = (up_n * char.step_height).astype(np.float32)
if self._sweep_character_2d(char, up_motion) is not None:
return False # not enough headroom to step up
char.position = (char.position + up_motion).astype(np.float32)
if self._sweep_character_2d(char, horizontal) is not None:
char.position = origin # forward leg still blocked at step height
return False
char.position = (char.position + horizontal).astype(np.float32)
down = (-up_n * char.step_height).astype(np.float32)
drop = self._sweep_character_2d(char, down)
if drop is None:
# No ground within step_height below: not a real step, revert.
char.position = origin
return False
char.position = (char.position + down * (float(drop.distance) / char.step_height)).astype(np.float32)
return True
__all__ = ["BuiltinPhysics2D"]