"""BuiltinPhysics2D world: the pure-Python 2D physics backend.
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.
Mechanism: semi-implicit (symplectic) Euler integration, scalar rotation, real
per-shape scalar moment of inertia. Where it simplifies versus a production solver
the simplification is commented and the pymunk backend is named as the exact
alternative; nothing silently fakes a result. This solver is a tier above its 3D
sibling, refuses no shape pair, and is not covered by that module's honesty list.
The GEOMETRY is written for clarity over raw throughput. What is optimised is how
OFTEN it runs: a sweep-and-prune broad phase proposes only the pairs whose world
AABBs meet, and a pair whose bodies have not moved replays its last answer instead
of re-deriving it. 2D needs both more than 3D does, because a 2D contact costs
more: this solver carries a real moment of inertia, so every contact point has a
lever arm and a Coulomb friction cone where the 3D tier applies one linear impulse
at the centre of mass.
``step()`` runs integrate -> CCD -> collide -> solve (contacts + joints) ->
position-correct -> sleep, mirroring
:meth:`~simvx.core.physics.builtin.world.BuiltinPhysics.step`. Every
``Physics2DWorld`` method is implemented: there are no stubs.
Narrowphase
-----------
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.
Dynamics
--------
- 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
equals a pin here since 2D rotation is 1-DOF, and there is no motor or limit. The
groove is a 1-DOF slider-on-a-line.
- sleeping: a settled DYNAMIC body sleeps after sleep_time_threshold and wakes at every
disturbance (set / force / impulse / an awake contact / a joint or spring whose
OTHER end moved since the previous step).
- CCD: ``continuous`` bodies centre-sweep vs STATIC and clamp to the TOI.
Basic-tier honesty
------------------
- Joints converge in a few iterations (small solver_iterations /
position_iterations) and stiff springs are soft. A loaded chain really sags:
six 1 kg beads at 1200 units/s^2 settle with the worst link 22.0% long here and
the tip 341 units down on 300 units of chain, where pymunk settles the same
chain on its rest length. Anchoring is exact on both -- what differs is how hard
the constraint pulls. Motors, angular limits and breakable joints are in the 2D
seam on NEITHER backend.
- 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.
- SAT resolves a DEEP interpenetration along the minimum-overlap axis, which is the
shortest way OUT rather than the way the body came in. Once the penetration passes
half the two shapes' summed extent along that axis, the shortest way out is the
FAR side, so a pair that was solidly touching at the previous step is recovered
out the face its remembered contact normal names (a resting body teleported
into its support, a collider grown through its floor). A pair with NO solid
history keeps the blind minimum deliberately: the entry face of a fresh deep
overlap is not knowable (a tunnelled arrival and an unstick shove present the
same state and need opposite answers), so past the midplane a history-less
pair still exits the far face.
The manifold points are the clipped incident edge, so on a deep overlap they can
also sit outside the reference collider, which makes the contact lever arms
approximate there. A full clipping-with-feature-caching manifold and true
deep-overlap handling are deferred to the pymunk (Chipmunk2D) backend.
- Warm-starting / accumulated-impulse caching: 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
(``_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
import numbers
from collections.abc import Iterator
from dataclasses import dataclass, field
from itertools import chain
import numpy as np
from ...math import Vec2
from ..capability import Capability, contact_impulse_estimate
from ..material import DEFAULT_PHYSICS_MATERIAL, CombineMode, PhysicsMaterial, _combine
from ..world import (
DEFAULT_ANGULAR_DAMPING,
DEFAULT_GRAVITY_SCALE,
DEFAULT_LINEAR_DAMPING,
BodyMode,
ContactPhase,
normalise_damping,
normalise_gravity_scale,
)
from ..world2d import (
_DEFAULT_UP_2D,
BodyHandle,
ContactEvent2D,
JointHandle,
OverlapEvent2D,
Physics2DWorld,
RaycastHit2D,
ShapeHandle,
SweepHit2D,
body_scale_unchanged,
is_unit_scale_2d,
normalise_body_scale_2d,
)
# Solver tuning. 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 the remaining penetration drained per position pass
# Ceiling on the separation one contact may apply in a single step, as a fraction
# of the smaller body's bounding radius. See the 3D module for the rationale.
_MAX_DRAIN_FRACTION = 0.25
#: Bisection halvings applied to a blocking sweep's reported distance. The substep
#: scan brackets the blocker between the last fraction it proved clear and the
#: first that penetrated; each halving halves that bracket, so eight
#: take the reported bound from one substep of error to 1/256 of a substep, below
#: any skin the character policy uses. They cost one narrowphase test each, against
#: ONE body, and only on a sweep that already found a blocker: a clear sweep pays
#: nothing.
_SWEEP_REFINE_STEPS = 8
# Deadband on JOINT anchor error, world units. Not the contact slop: that is a
# world knob (``contact_slop``) a scene sets for its own scale, while this is an
# internal of the joint Baumgarte push and no consumer has asked to tune it.
_JOINT_SLOP = 0.001
_RESTITUTION_THRESHOLD = 1.0 # m/s: below this a contact gets e_eff = 0 (no bounce)
# Neither count is a constant: the velocity count is the world's
# ``solver_iterations`` knob and the position count its ``position_iterations``.
_JOINT_EPS = 1e-9 # degenerate-direction guard (zero-length spring / anchor delta)
# Settle projection, run once over an island in the step it falls asleep (see
# ``_settle_island``). Two ceilings, and the lower wins. Passes per body, because
# a stack propagates one contact per pass and each contact between two movable
# bodies splits its correction, so the passes it takes grow with the height: a
# six-high stack of unit boxes drains in 104 and a fourteen-high one in 579,
# against the 576 and 1344 this allows. Total projections, so that a wide island
# cannot spend the first ceiling on its contact count: a forty-box pile carries
# 114 contacts, drains in 44 passes, and is allowed 175. The tolerance is how far
# above the slop a contact may sit and still count as drained.
_SETTLE_PASS_CEILING = 96
_SETTLE_WORK_CEILING = 20_000
_SETTLE_TOLERANCE = 1e-4
# How many consecutive steps an island may be refused the latch before it is
# latched at the best pose the projection reached. Without it a box wedged into a
# gap narrower than itself -- a penetration no displacement can drain -- would
# never sleep and would re-run the whole projection every step for the life of the
# world. Consecutive: the count is cleared on the latch and on any wake.
_SETTLE_REFUSAL_CEILING = 4
# Sleeping thresholds. Copied VERBATIM from the 3D builtin tier
# (builtin/world.py): a DYNAMIC body whose linear AND angular speed stay below
# these for sleep_time_threshold 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.
# ...and the sleep thresholds are the world's ``sleep_velocity_threshold`` /
# ``sleep_time_threshold``, for the reason the 3D twin gives.
# One-way platform tuning. 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)
# Broadphase. Below this body count the plain nested pair loop beats sorting the
# bounds and running the vectorised sweep (a handful of pairs is cheaper to test
# than to plan), so a two-body scene never pays for the structure. Measured on a
# settling pile: the sweep is a flat ~25 us of numpy dispatch whatever the scene,
# while the loop is ~0.03 * n^2 us of scalar comparisons, so they cost the same
# around forty bodies and the loop is thirty times cheaper at ten. The 3D backend
# crosses over much earlier (seven) because its loop pair source tests nothing at
# all; this one carries the AABB cull the 2D narrowphase has always needed.
_BROADPHASE_MIN_BODIES = 40
# Reusable zero anchor offset, for the weld's b-side point solve (the constrained
# point IS b's centre, so its arm is the zero vector). Never written through.
_ZERO2 = np.zeros(2, dtype=np.float32)
# Byte image of a zero velocity. Comparing a body's own bytes against it is an
# exact "is this vector all +0.0" test that costs a fraction of a numpy call, and
# it is what lets the contact solve prove a resting pair has nothing to do.
_ZERO2_BYTES = _ZERO2.tobytes()
def _zero2() -> np.ndarray:
"""A fresh writable zero vector, for the joint records' carried impulse."""
return np.zeros(2, dtype=np.float32)
def _is_positive_zero(x: float) -> bool:
"""True for ``+0.0`` alone: ``-0.0`` and every other value are rejected.
The rest tests below stand in for arithmetic that must be a no-op down to the
sign of every zero, and plain ``x == 0.0`` cannot see that sign.
"""
return x == 0.0 and math.copysign(1.0, x) > 0.0
def _scaled_shape_2d(base: _Shape2D, scale: np.ndarray) -> _Shape2D:
"""Return ``base`` resized by ``scale``, or ``base`` itself when unscaled.
The 2D twin of ``_scaled_shape``: scale lives on the body, so this is where a
body's own copy of the shared geometry is materialised. Returning ``base``
unchanged for a unit scale keeps an unscaled world at exactly its previous
cost and its previous numbers.
The analytic kinds carry MAGNITUDES and take the absolute factor; the point
kinds are scaled signed, so a mirroring component mirrors them. A polygon
mirrored on exactly one axis has its winding reversed with the points, which
is restored here so the CCW convention the narrow phase reads still holds.
"""
if is_unit_scale_2d(scale):
return base
mag = np.abs(scale)
kind = base.kind
if kind == "circle":
return _Shape2D("circle", (base.params * float(mag[0])).astype(np.float32))
if kind == "box":
return _Shape2D("box", (base.params * mag).astype(np.float32))
if kind == "capsule":
# [radius, half_len], both along the one uniform factor the seam enforces.
return _Shape2D("capsule", (base.params * float(mag[0])).astype(np.float32))
assert base.points is not None
if kind == "segment":
# [radius] thickness scales uniformly; the endpoints scale signed.
return _Shape2D(
"segment",
(base.params * float(mag[0])).astype(np.float32),
points=(base.points * scale).astype(np.float32),
)
pts = (base.points * scale).astype(np.float32)
if kind == "poly" and float(scale[0]) * float(scale[1]) < 0.0:
pts = pts[::-1].copy() # a single mirrored axis reverses the winding
flat = pts.reshape(-1, 2)
lo, hi = flat.min(axis=0), flat.max(axis=0)
return _Shape2D(kind, ((hi - lo) * 0.5).astype(np.float32), points=pts)
def _at_rest(body: _Body2D) -> bool:
"""True when the body's linear AND angular velocity are exactly ``+0.0``.
A settled body reaches this state precisely: :meth:`BuiltinPhysics2D._update_sleeping`
writes the zeros into every sleeper each step it stays asleep, and a STATIC body
is created with them and never integrated. Two bodies in that state have no
relative motion to cancel, at the centre of mass or at any contact point, which
the contact solve exploits when the pair also carries no accumulated impulse.
Angular velocity is part of the test because a 2D contact reads it: the relative
velocity at a contact point is ``v + omega x r``, so a spinning body is not at
rest here however still its centre is.
"""
return body.linear_velocity.tobytes() == _ZERO2_BYTES and _is_positive_zero(body.angular_velocity)
def _all_positive_zero(values: list[float]) -> bool:
"""True when every entry of a per-manifold-point accumulator is ``+0.0``."""
return all(_is_positive_zero(v) for v in values)
def _shiftable_mass(body: _Body2D) -> float:
"""Inverse mass a joint POSITION pass may move the body by: zero while asleep.
A sleeper is immovable for the Baumgarte pass, exactly as a STATIC body is.
The velocity solve can hand a sleeper an impulse harmlessly, because the sleep
pass holds every sleeper at zero velocity and the impulse is discarded rather
than banked; a position pass has no such backstop, since it writes ``position``
and ``rotation`` directly. Without this a joint drags a body that reports
``sleeping()`` as True, at any speed, for as long as it stays asleep.
A joint whose ends must move has already had them woken: the constraint wake
runs at the top of the step, so a body still asleep here is one no disturbance
reached this step, and holding it still is what "asleep" means.
"""
return 0.0 if body.asleep else body.inverse_mass
def _shiftable_moment(body: _Body2D) -> float:
"""Inverse moment a joint position pass may turn the body by (see :func:`_shiftable_mass`)."""
return 0.0 if body.asleep else body.inverse_moment
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, 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 _to_world_2d(body: _Body2D, local: np.ndarray) -> np.ndarray:
"""Turn a body-LOCAL ``(2,)`` offset into world axes by the body's current angle.
What a joint's stored local arm goes through every time the solver needs its
world direction, so an anchor turns with the body that carries it.
"""
return _rotate_2d(local, math.cos(body.rotation), math.sin(body.rotation))
def _to_local_2d(body: _Body2D, world: np.ndarray) -> np.ndarray:
"""Turn a WORLD-axes ``(2,)`` offset into the body's own frame (inverse of :func:`_to_world_2d`)."""
return _rotate_2d(world, math.cos(body.rotation), -math.sin(body.rotation))
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 _point_velocity_2d(body: _Body2D, point: np.ndarray) -> np.ndarray:
"""World velocity of the material point of ``body`` currently at ``point``.
``v + omega * perp(point - centre)``: the body's linear velocity plus the
surface speed its spin gives that offset. Reduces to the linear velocity for
a body that is not spinning, and for a point at the centre of mass.
"""
spin: np.ndarray = body.angular_velocity * _perp_2d((point - body.position).astype(np.float32))
at_point: np.ndarray = body.linear_velocity + spin
return at_point.astype(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)
# ``eq=False``: a shape record holds numpy arrays, so a field-wise ``==`` would
# raise on the ambiguous truth value of an array rather than answer the question.
# Identity is the only meaningful comparison here (a shape record IS the handle
# handed out), and the broadphase's per-body fingerprint compares them: a body
# that is rescaled gets a fresh record, which invalidates its cached pairs.
@dataclass(slots=True, eq=False)
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 contact solver).
"""
# ``shape`` is the EFFECTIVE geometry the narrow phase reads: already resized
# by ``scale``, so every solver / sweep / bounds site reads params directly and
# none of them knows scale exists. ``unscaled_shape`` is the record the world's
# shape table handed out, kept so a later rescale starts from the original
# rather than compounding; None means ``shape`` IS the unscaled record.
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)
unscaled_shape: _Shape2D | None = None
scale: np.ndarray = field(default_factory=lambda: np.ones(2, dtype=np.float32))
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
# 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 overlap stream).
is_sensor: bool = False
# Surface material coefficients (consumed by the contact solver), stored verbatim.
friction: float = 0.5
restitution: float = 0.0
friction_combine: CombineMode = CombineMode.AVERAGE
restitution_combine: CombineMode = CombineMode.AVERAGE
# Per-body dynamics: the rate at which the body sheds motion with nothing
# touching it (``v *= max(0, 1 - damping * dt)`` in _integrate), and its own
# multiplier on the world gravity. Defaulted to the seam values.
linear_damping: float = DEFAULT_LINEAR_DAMPING
angular_damping: float = DEFAULT_ANGULAR_DAMPING
gravity_scale: float = DEFAULT_GRAVITY_SCALE
# CCD flag (consumed by the continuous sweep). Defaulted False (discrete).
continuous: bool = False
# Sleeping. Set by the post-solve sleep pass; a body starts awake. Only a
# DYNAMIC body ever carries it: STATIC / KINEMATIC bodies are never integrated,
# so they are immovable rather than 'asleep', and set_body_mode clears the flag
# on the way out of DYNAMIC to keep that true -- the contact solve reads this
# field on BOTH bodies of a pair and skips the pair when both carry it, so a
# flagged STATIC body would quietly stop its resting contacts being solved.
asleep: bool = False
_sleep_timer: float = 0.0
# Consecutive steps this body's island has been refused the sleep latch
# because the settle projection could not drain it (_settle_island). Cleared
# on the latch and on every wake, so it counts one settling attempt only.
_settle_refusals: int = 0
# Whether this body is ALLOWED to sleep. False keeps it out of the sleep pass
# for the life of the world, so it is integrated and solved every step. It is
# deliberately NOT part of the broadphase fingerprint: the narrow phase never
# reads it, so adding it would cost a comparison per body per step and change
# no result. A body that may not sleep also never has its velocity zeroed by
# the sleep pass, so it never satisfies the at-rest replay predicate and stays
# off the replay path -- correct, and worth knowing before it is read as a
# defect in a profile.
can_sleep: bool = True
# One-way platform. Per-body flag + world-space "solid side" normal.
# Defaulted off; set_one_way 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.
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. 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
# Position-pass state, fixed once per step by _prepare_contact_projections.
# The 2D twin of the 3D contact's fields, with the same meanings: a depth
# datum each pass re-measures the live penetration against, the separation
# still allowed this step, and the inverse-mass split of it.
depth_datum: float = 0.0
drain_budget: float = 0.0
share_a: float = 0.0
share_b: float = 0.0
def _manifold_centre_2d(contact: _Contact2D) -> np.ndarray:
"""The one world point that stands for a contact manifold.
The mean of the clipped manifold points: the single point for a vertex or
circle feature, the middle of the clipped edge for a face-face pair. This is
the point the contact payload publishes and the point the relative velocity
is measured at, so the two always describe the same place.
"""
if len(contact.points) == 1:
return contact.points[0].astype(np.float32)
centre: np.ndarray = np.mean(contact.points, axis=0)
return centre.astype(np.float32)
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)
def _bounding_radius_2d(shape: _Shape2D) -> float:
"""Radius of a circle about the body origin that contains the whole shape.
The 2D twin of :func:`~simvx.core.physics.builtin.world._bounding_radius`, and
the scale reference the position pass sizes its per-step drain budget against.
A segment measures the farther endpoint plus its thickness; every other kind
reads its own analytic parameters.
"""
kind = shape.kind
if kind == "circle":
return float(shape.params[0])
if kind == "capsule":
return float(shape.params[0] + shape.params[1])
if kind == "segment":
points = shape.points
reach = 0.0 if points is None else float(np.max(np.linalg.norm(points, axis=1)))
return reach + float(shape.params[0])
return float(np.hypot(shape.params[0], shape.params[1])) # box / poly / concave
# -- joint / constraint records ----------------------------------
#
# 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 and groove endpoints are stored in the frame of
# the body that carries them, captured at create time, and turned back into world
# axes by that body's current angle every time the solver reads them
# (:func:`_to_world_2d`) -- the standard formulation, and what makes a pin on a
# spinning body orbit with it instead of hanging in the world axes it was built
# in. 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 offset of b's centre from a's centre expressed
in A'S OWN frame (``rel_local``) and the relative angle
``rel_angle = b.rotation - a.rotation``. Both are frame-relative, so the whole
assembly follows ``a`` when ``a`` turns. The point part is solved with the arm
``R_a * rel_local`` on ``a`` and no arm on ``b`` (the target point IS b's
centre); 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_local: np.ndarray # float32 (2,), b_centre - a_centre in a's frame at create
rel_angle: float # b.rotation - a.rotation captured at create (radians)
impulse: np.ndarray = field(default_factory=_zero2) # point impulse carried between steps
@dataclass(slots=True)
class _PinConstraint2D:
"""Point-to-point: keep the two world anchors coincident, rotation free (2D).
``local_a`` / ``local_b`` are the shared anchor expressed in each body's own
frame at create time. The world anchor on each body is rebuilt every solver
pass as ``pos + R * local``, so it turns with the body.
"""
a: BodyHandle
b: BodyHandle
local_a: np.ndarray # float32 (2,), anchor in a's frame at create
local_b: np.ndarray # float32 (2,), anchor in b's frame at create
impulse: np.ndarray = field(default_factory=_zero2) # point impulse carried between steps
@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 (``local_a`` / ``local_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
local_a: np.ndarray # float32 (2,), anchor in a's frame at create
local_b: np.ndarray # float32 (2,), anchor in b's frame at create
impulse: np.ndarray = field(default_factory=_zero2) # point impulse carried between steps
@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).
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 segment is closed, so
its ends act as a stop rather than the rail running on for ever.
Like the other 2D joints, the groove endpoints and ``b``'s anchor are held in
the frame of the body that carries them and turned into world axes by that
body's current angle, so the rail swings round with ``a`` as the shape says it
should. That end stop is SOFT: it is the same Baumgarte positional push the
other joints use, so a sustained load along the rail carries the slider PAST the
end and goes on carrying it. Ten seconds of push on a 1 kg slider, on a groove
reaching one unit either side of centre, leaves it 0.14 units beyond the end at
1 N and 6.9 units beyond at 50 N, where pymunk holds the endpoint to four
decimals; ``docs/core/physics_backends.md`` carries the measurement.
Fields:
ga / gb: groove endpoints in ``a``'s own frame, exactly as the caller
passed them.
local_b: ``b``'s anchor in ``b``'s own frame, exactly as the caller passed
it.
"""
a: BodyHandle
b: BodyHandle
ga: np.ndarray # float32 (2,), groove start in a's frame
gb: np.ndarray # float32 (2,), groove end in a's frame
local_b: np.ndarray # float32 (2,), b's anchor in b's frame
# 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) completes the set.
_Constraint2D = _FixedConstraint2D | _PinConstraint2D | _HingeConstraint2D | _SpringConstraint2D | _GrooveConstraint2D
# The three solved as rigid point rows, which is the subset that carries a
# ``.impulse`` to warm-start from. A spring is compliant and solved from its own
# ``k*x`` bias every step, so it has no accumulator; a groove's anchor on ``a``
# moves along the rail, so its impulse is not a carried quantity either. Naming
# the subset keeps that precondition in the signature rather than only in the
# comprehension and the ``isinstance`` skip that enforce it.
_RigidPointConstraint2D = _FixedConstraint2D | _PinConstraint2D | _HingeConstraint2D
[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 interface: shapes + bodies + integrator + bulk readers
, narrowphase + sequential-impulse solver, forces + joints + sleeping
+ CCD, queries + events + the swept body primitive, one-way
platforms, and the groove joint. 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. 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
# The pose every constraint END held at the close of the previous step.
# A constraint disturbs a sleeping end only when its OTHER end actually
# moved, and this is what that is measured against
# (see :meth:`_wake_moved_constraint_ends`). Rebuilt from the live joint
# set each step, so a jointless scene never allocates it, a removed joint
# needs no cleanup, and it costs the joint count rather than the body count.
self._constraint_poses: dict[BodyHandle, tuple[float, float, float]] = {}
# Collision-event diffing. ``_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] = []
# ``_touching`` keyed the other way: body -> the bodies it is in contact
# with, so a mutator can reach one body's neighbours without reading the
# whole contact set. Folded forward from each step's ENTER / EXIT deltas
# (see ``_retrack_touching``), so a settled scene maintains it for free.
self._touching_by_body: dict[BodyHandle, set[BodyHandle]] = {}
# Sensor-overlap diffing: 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] = []
# Warm-start cache: 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]]] = {}
# The ``dt`` the joint warm start last seeded against. An impulse carried
# into a step of a different length has to be rescaled by the ratio (see
# ``_joint_warm_scale``); zero means no previous step, so nothing to seed.
self._joint_warm_dt = 0.0
# Broadphase + narrow-phase reuse state, all rebuilt by ``_collide``:
# ``_pair_contacts`` is last step's narrow-phase result per tested pair
# (only ``a`` / ``b`` / ``normal`` / ``depth`` / ``points`` are read back);
# ``_pose_keys`` is each body's pose + filter fingerprint as of the last
# collide pass, which is what decides a pair can reuse that result;
# ``_bounds_lo`` / ``_bounds_hi`` are the per-body world AABBs the pair
# sources cull with, kept across steps and rewritten only for the bodies
# that moved. ``_body_epoch`` counts create / destroy so the bounds table
# knows when its row layout is stale.
self._pair_contacts: dict[tuple[BodyHandle, BodyHandle], _Contact2D] = {}
self._pose_keys: dict[BodyHandle, tuple] = {}
self._bounds_lo: np.ndarray = np.zeros((0, 2), dtype=np.float32)
self._bounds_hi: np.ndarray = np.zeros((0, 2), dtype=np.float32)
self._bounds_epoch: int = -1
self._body_epoch: int = 0
[docs]
def capabilities(self) -> frozenset[Capability]:
"""Advertise the measured contact impulse and continuous collision.
2D sibling of :meth:`BuiltinPhysics.capabilities`: this backend owns its
sequential-impulse solver, so the contact-event impulse is the converged
normal impulse it actually applied, and its integrator honours the
``CONTINUOUS`` flag by sweeping a flagged body against STATIC geometry,
and ``SLEEP`` by parking a settled DYNAMIC body. ``SENSOR_DETECTS_STATIC``
holds for the same reason it does in 3D: the sensor sweep considers every
body the mask admits, not only the ones that can move. No determinism,
vehicles or soft bodies. Explicit (not inherited) so the claim is a
deliberate, tested promise.
"""
return frozenset(
{
Capability.CONTACT_IMPULSE,
Capability.CONTINUOUS,
Capability.SLEEP,
Capability.SENSOR_DETECTS_STATIC,
}
)
[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 and joint, emptying the world.
See :meth:`~simvx.core.physics.world2d.Physics2DWorld.clear`. 2D sibling of
:meth:`BuiltinPhysics.clear`: resets the body / joint tables and the
per-step edge-diff + warm-start caches. Gravity, shapes, and handle
counters are intentionally NOT reset.
"""
self._bodies.clear()
self._joints.clear()
self._constraint_poses.clear()
self._order.clear()
self._touching.clear()
self._touching_by_body.clear()
self._contact_events.clear()
self._overlapping.clear()
self._overlap_events.clear()
self._warm_contacts.clear()
self._pair_contacts.clear()
self._pose_keys.clear()
self._body_epoch += 1
def _alloc_handle(self) -> int:
h = self._next_handle
self._next_handle += 1
return h
# -- shapes -------------------------------------------------------------
def _store_shape(self, shape: _Shape2D) -> ShapeHandle:
"""File a freshly built shape record under a new handle and return it."""
handle = self._alloc_handle()
self._shapes[handle] = shape
return handle
[docs]
def create_circle(self, radius: float) -> ShapeHandle:
if not radius > 0.0:
raise ValueError(f"circle radius must be > 0, got {radius}")
return self._store_shape(_Shape2D("circle", np.array([radius], dtype=np.float32)))
[docs]
def create_box(self, half_extents: Vec2) -> ShapeHandle:
he = _as_array2(half_extents)
if not np.all(he > 0.0):
raise ValueError(f"box half_extents must all be > 0, got {tuple(he)}")
return self._store_shape(_Shape2D("box", he.copy()))
[docs]
def create_capsule(self, radius: float, height: float) -> ShapeHandle:
if not (radius > 0.0 and height > 0.0):
raise ValueError(f"capsule radius/height must be > 0, got {radius}, {height}")
half_len = max(0.0, height * 0.5 - radius)
return self._store_shape(_Shape2D("capsule", np.array([radius, half_len], dtype=np.float32)))
[docs]
def create_segment(self, a: Vec2, b: Vec2, radius: float = 0.0) -> ShapeHandle:
if not radius >= 0.0:
raise ValueError(f"segment radius must be >= 0, got {radius}")
pa = _as_array2(a)
pb = _as_array2(b)
if not float(np.dot(pb - pa, pb - pa)) > 1e-12:
raise ValueError(f"segment endpoints must differ, got {tuple(pa)} and {tuple(pb)}")
return self._store_shape(
_Shape2D("segment", np.array([radius], dtype=np.float32), points=np.stack([pa, pb]).astype(np.float32))
)
[docs]
def create_convex_polygon(self, points: np.ndarray) -> ShapeHandle:
pts = np.asarray(points, dtype=np.float32).reshape(-1, 2)
if pts.shape[1] != 2:
raise ValueError(f"polygon points must be (N, 2), got {pts.shape}")
if pts.shape[0] < 3:
raise ValueError(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)
return self._store_shape(_Shape2D("poly", aabb_half, points=pts.copy()))
[docs]
def create_concave_polygon(self, segments: np.ndarray) -> ShapeHandle:
segs = np.asarray(segments, dtype=np.float32).reshape(-1, 2, 2)
if segs.shape[0] < 1:
raise ValueError(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)
return self._store_shape(_Shape2D("concave", aabb_half, points=segs.copy()))
[docs]
def destroy_shape(self, shape: ShapeHandle) -> None:
"""Release this world's record of a shape handle.
See :meth:`~simvx.core.physics.world2d.Physics2DWorld.destroy_shape`. 2D
sibling of :meth:`BuiltinPhysics.destroy_shape`: nothing native to free,
so the record is simply dropped from the shape table. Bodies built from it
hold their own ``_Shape2D`` record directly and are unaffected. Unknown
handles are a silent no-op.
"""
self._shapes.pop(shape, None)
def _shape_rec(self, shape: ShapeHandle) -> _Shape2D:
"""Return the record for ``shape``, or raise ``KeyError`` if it is not ours.
A shape handle is caller input, so an unknown one raises rather than
asserts: ``assert`` is compiled out by ``python -O``, and a released
handle would then build a body against a stale record. ``KeyError``
matches what every body-handle lookup on this backend already raises.
"""
rec = self._shapes.get(shape)
if rec is None:
raise KeyError(f"unknown shape handle {shape!r}")
return rec
# -- 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,
scale: Vec2 | None = None,
can_sleep: bool = True,
linear_damping: float = DEFAULT_LINEAR_DAMPING,
angular_damping: float = DEFAULT_ANGULAR_DAMPING,
gravity_scale: float = DEFAULT_GRAVITY_SCALE,
collision_layer: int = 1,
collision_mask: int = 0xFFFFFFFF,
is_sensor: bool = False,
material: PhysicsMaterial | None = None,
continuous: bool = False,
) -> BodyHandle:
shp = self._shape_rec(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."
)
# Mass is retained across set_body_mode, so it must be real in every mode:
# an immovable body created with a bad mass would carry it into a later
# DYNAMIC flip. ``not mass > 0.0`` also rejects NaN.
if not mass > 0.0:
raise ValueError(f"body mass must be > 0, got {mass}")
# Every knob is validated before a handle is minted, so a rejected argument
# costs the caller nothing: no handle is consumed and no body exists.
lin_damping = normalise_damping(linear_damping, "linear_damping")
ang_damping = normalise_damping(angular_damping, "angular_damping")
grav_scale = normalise_gravity_scale(gravity_scale)
position, rotation = self._unpack_transform(transform)
body_scale = np.ones(2, dtype=np.float32) if scale is None else normalise_body_scale_2d(scale, shp.kind)
effective = _scaled_shape_2d(shp, body_scale)
if body_type is BodyMode.DYNAMIC:
inv_mass = 1.0 / mass
moment = self._shape_moment(effective, 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
surface = DEFAULT_PHYSICS_MATERIAL if material is None else material
handle = self._alloc_handle()
self._bodies[handle] = _Body2D(
shape=effective,
unscaled_shape=shp,
scale=body_scale,
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=surface.friction,
restitution=surface.restitution,
friction_combine=surface.friction_combine,
restitution_combine=surface.restitution_combine,
linear_damping=lin_damping,
angular_damping=ang_damping,
gravity_scale=grav_scale,
continuous=continuous,
can_sleep=can_sleep,
)
# A new body invalidates the broadphase row layout; the next collide pass
# rebuilds it.
self._body_epoch += 1
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
# Destroying a support is the commonest way a game takes one away, so wake
# what it carried while its contacts are still on record: a sleeper left
# asleep here hangs in the air for the life of the world.
self._wake_supported(handle)
del self._bodies[handle]
self._pose_keys.pop(handle, None)
# The broadphase row layout is now stale; the next collide pass rebuilds it.
# Stale ``_pair_contacts`` entries need no purge: that table is rewritten
# from the live pairs every step, and handles are never reused, so they
# simply fall out.
self._body_epoch += 1
if handle in self._order:
self._order.remove(handle)
# The destroyed body's touching pairs and directed sensor edges are left in
# place on purpose: a pair that was in contact HAS stopped being in contact,
# so it is owed an EXIT like any other separation. The next step's diff owes
# it, because the body is already out of ``_bodies`` and so cannot appear in
# that step's live set, while both diffs rewrite their set from the live set.
# A destroyed pair therefore survives exactly one step, reports one EXIT, and
# is gone. 2D sibling of the 3D behaviour.
# 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:
kept = {}
for h, j in self._joints.items():
if j.a != handle and j.b != handle:
kept[h] = j
continue
# A constraint holds its ends up exactly as a support does, so
# losing one is a departure and the survivor is owed the same wake
# _wake_supported gives a contact neighbour. Nothing else can give
# it: the joint that would have disturbed it is the one going away.
self._wake_joint_ends(j)
self._joints = kept
[docs]
def set_body_transform(
self, handle: BodyHandle, transform: object, *, scale: Vec2 | None = None, wake: bool = True
) -> None:
body = self._bodies[handle]
position, rotation = self._unpack_transform(transform)
# Only a pose that actually CHANGES vacates anything, so a parked platform
# re-writing the pose it already holds leaves what it carries asleep. A
# rescale vacates too: it is a geometry change, exactly like a shape swap.
moved = rotation != body.rotation or not np.array_equal(body.position, position)
# A byte compare first: the node states its scale on every pose write, and
# validating an unchanged one again would dominate this hot path.
if scale is not None and not body_scale_unchanged(scale, body.scale):
base = body.unscaled_shape or body.shape
new_scale = normalise_body_scale_2d(scale, base.kind)
if not np.array_equal(new_scale, body.scale):
body.scale = new_scale
body.shape = _scaled_shape_2d(base, new_scale)
# The moment of inertia is a property of the geometry, so a resize
# re-derives it at the body's retained mass, exactly as a shape
# swap does.
self._apply_dynamic_mass(body)
self._body_epoch += 1
moved = True
body.position, body.rotation = position, rotation
self._disturb(handle, body, wake=wake, vacated=moved)
[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)
# A direct velocity write is a disturbance, and one that moves the body out
# from under whatever it was carrying: the island comes with it.
self._wake_island(handle, body)
[docs]
def set_body_mode(self, handle: BodyHandle, mode: BodyMode, *, wake: bool = True) -> 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}."
)
# Second entry point to the same invariant as create_body, and the
# last barrier before a body is marked DYNAMIC: it runs BEFORE any
# mutation so a rejected flip leaves the body exactly as it was.
if mode is BodyMode.DYNAMIC and not body.mass > 0.0:
raise ValueError(f"body mass must be > 0, got {body.mass}")
# A body that BECOMES movable stops holding up whatever rested on it, and
# falls out from under it on the very next step. Becoming STATIC disturbs
# nothing that was already at rest; becoming KINEMATIC does, because the
# body is about to be DRIVEN through whatever rests against it, and this
# seam is where that wake must come from: wake-on-contact leaves a sleeper
# alone against a kinematic body that holds still.
became_dynamic = mode is BodyMode.DYNAMIC and body.body_type is not BodyMode.DYNAMIC
became_kinematic = mode is BodyMode.KINEMATIC and body.body_type is not BodyMode.KINEMATIC
# Handing a body to the simulation without waking it PARKS it: DYNAMIC and
# asleep, out of the step until something disturbs it, which is what
# streaming a settled section in wants. A body forbidden to sleep has no
# parked state to be handed to, so ``sleep`` refuses it and it is freed awake.
park = became_dynamic and not wake
body.body_type = mode
self._apply_dynamic_mass(body)
if mode is not BodyMode.DYNAMIC:
# Only a DYNAMIC body can be asleep, so leaving DYNAMIC clears the flag
# whatever ``wake`` says: this is the record invariant being restored,
# not a wake the caller asked to suppress. It is load-bearing on both
# sides. Outward, ``sleeping()`` promises False for an immovable body.
# Inward, the solver reads ``asleep`` on BOTH bodies of a pair and skips
# the pair when both carry it, so a STATIC body left flagged would
# silently stop its resting contacts being solved -- and nothing would
# ever clear it, since the sleep pass skips non-DYNAMIC bodies and
# wake-on-contact refuses to wake a sleeper whose only awake peer is
# STATIC.
self._wake(body)
if park:
self.sleep(handle)
self._disturb(handle, body, wake=wake, vacated=became_dynamic or became_kinematic)
# -- live edits to what create_body was given ---------------------------
def _apply_dynamic_mass(self, body: _Body2D) -> None:
"""Refresh the effective mass / moment from ``body.mass`` and its shape.
DYNAMIC bodies get the finite inverses; STATIC and KINEMATIC stay at
infinite mass and inertia and pick the stored mass up at their next
DYNAMIC flip. Shared by :meth:`set_body_mass` and :meth:`set_body_shape`,
which change the two inputs the moment is computed from.
"""
if body.body_type is BodyMode.DYNAMIC:
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
body.moment = 0.0
body.inverse_moment = 0.0
[docs]
def set_body_mass(self, handle: BodyHandle, mass: float, *, wake: bool = True) -> None:
# Same guard as create_body / set_body_mode: the mass is retained across
# mode flips, so it must be real whatever the body's current mode.
if not mass > 0.0:
raise ValueError(f"body mass must be > 0, got {mass}")
body = self._bodies[handle]
body.mass = float(mass)
self._apply_dynamic_mass(body)
# The neighbours too: this body's contact response has changed under
# whatever is resting on it, and a sleeper reads the old one until woken.
self._disturb(handle, body, wake=wake, vacated=True)
[docs]
def set_body_filter(
self, handle: BodyHandle, collision_layer: int, collision_mask: int, *, wake: bool = True
) -> None:
body = self._bodies[handle]
body.collision_layer = int(collision_layer)
body.collision_mask = int(collision_mask)
# _collide re-evaluates every pair each step, so a pair that stops matching
# drops out of _touching and the diff reports its EXIT. The wake covers both
# directions of what that costs: a sleeper that newly matches must be
# solvable next step, and a filter that stops matching TAKES SUPPORT AWAY
# exactly as removing the body would, so the riders are owed the same wake.
self._disturb(handle, body, wake=wake, vacated=True)
[docs]
def set_body_material(self, handle: BodyHandle, material: PhysicsMaterial | None) -> None:
body = self._bodies[handle]
surface = DEFAULT_PHYSICS_MATERIAL if material is None else material
body.friction = surface.friction
body.restitution = surface.restitution
body.friction_combine = surface.friction_combine
body.restitution_combine = surface.restitution_combine
self._wake(body)
[docs]
def set_body_damping(self, handle: BodyHandle, linear: float, angular: float) -> None:
body = self._bodies[handle]
# Both rates are validated before either is written: the call describes the
# body's damping as a whole, so a rejected one must leave it as it was.
lin = normalise_damping(linear, "linear_damping")
ang = normalise_damping(angular, "angular_damping")
body.linear_damping = lin
body.angular_damping = ang
# Damping is applied by _integrate, which skips a sleeper, so the wake is
# what makes the new rate real. It takes no support away.
self._wake(body)
[docs]
def set_body_gravity_scale(self, handle: BodyHandle, scale: float) -> None:
body = self._bodies[handle]
body.gravity_scale = normalise_gravity_scale(scale)
# As above: gravity reaches the body through _integrate, so a body parked
# in mid-air at scale 0 would stay there when gravity was switched back on.
self._wake(body)
[docs]
def set_body_continuous(self, handle: BodyHandle, enabled: bool) -> None:
body = self._bodies[handle]
body.continuous = bool(enabled)
self._wake(body)
[docs]
def set_body_shape(self, handle: BodyHandle, shape: ShapeHandle, *, wake: bool = True) -> None:
shp = self._shape_rec(shape)
body = self._bodies[handle]
# Third entry point to the concave STATIC-only invariant (create_body
# and set_body_mode are the other two), checked before any mutation so a
# rejected swap leaves the body exactly as it was.
if shp.kind == "concave" and body.body_type is not BodyMode.STATIC:
raise ValueError(
f"ConcavePolygonShape2D (segment soup) is a STATIC-only collider; cannot place it on a "
f"body whose mode is {body.body_type}."
)
# The body keeps its scale across a swap, so the NEW geometry has to be
# able to carry it. Validated before any mutation, like the concave check
# above, so a rejected swap leaves the body exactly as it was.
body.scale = normalise_body_scale_2d(body.scale, shp.kind)
body.unscaled_shape = shp
body.shape = _scaled_shape_2d(shp, body.scale)
# The moment is a function of the shape AND the mass, so a swap recomputes
# it: a body must not keep the rotational response of geometry it no
# longer has.
self._apply_dynamic_mass(body)
# New geometry vacates whatever the old geometry held up, so the wake
# reaches the neighbours: a shrunken platform must drop its sleeper.
self._disturb(handle, body, wake=wake, vacated=True)
[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 body_mass(self, handle: BodyHandle) -> float:
# inverse_mass is the one field that already carries "immovable": it is 0
# for STATIC / KINEMATIC and for a DYNAMIC body it is 1 / the retained mass.
body = self._bodies[handle]
return float(body.mass) if body.inverse_mass > 0.0 else math.inf
[docs]
def wake(self, handle: BodyHandle) -> None:
self._wake(self._bodies[handle])
[docs]
def sleep(self, handle: BodyHandle) -> None:
body = self._bodies[handle]
# STATIC / KINEMATIC were never awake, and a body forbidden to sleep may
# not be put to sleep by hand either.
if body.body_type is not BodyMode.DYNAMIC or not body.can_sleep:
return
body.asleep = True
body._sleep_timer = 0.0
# The same zeroing the settle path does: a body parked mid-motion must not
# bank the velocity it had and carry it out of the next wake.
body.linear_velocity[:] = 0.0
body.angular_velocity = 0.0
[docs]
def set_body_can_sleep(self, handle: BodyHandle, enabled: bool) -> None:
body = self._bodies[handle]
body.can_sleep = bool(enabled)
if not body.can_sleep:
self._wake(body) # forbidding sleep takes effect now, not at the next settle
[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 _disturb(self, handle: BodyHandle, body: _Body2D, *, wake: bool, vacated: bool = False) -> None:
"""The one home for what a mutation does to sleep on this backend.
Every setter that can change what the solver reads ends in exactly one
call to this, so the wake is a property of the seam rather than a line each
setter remembers to add. ``vacated`` says the change disturbed the body's
NEIGHBOURS (the body moved, became movable or driven, changed geometry, or
changed its contact response), which is the case the ordinary
wake-on-contact cannot reach: a support that moved has already separated by
the time the collision pass runs, and a body that turned KINEMATIC holds
still until driven, so the neighbours must be woken by name from the
contact set the previous step left behind.
Args:
handle: The mutated body's handle.
body: Its record (the caller already has it; this avoids a re-lookup).
wake: The caller's ``wake=`` argument. False suppresses everything.
vacated: Whether the change also disturbs the body's neighbours.
"""
if not wake:
# A suppressed disturbance has to be invisible to the constraint wake as
# well, or a pose written with wake=False (streaming a settled section
# in, a pooled respawn) would still wake the far end of every joint on
# the body, one step later and by a different route.
if handle in self._constraint_poses:
self._constraint_poses[handle] = self._constraint_pose(body)
return
self._wake(body)
if vacated:
self._wake_supported(handle)
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
body._settle_refusals = 0 # the next settle is a fresh attempt
def _wake_island(self, handle: BodyHandle, body: _Body2D) -> None:
"""Wake one body, and the island it was sleeping in with it.
A sleeper does not sleep alone: the latch parks a whole contact island at
once (:meth:`_sleep_settled_islands`), so a disturbance that reaches one
member is owed all of it. Waking the one body and leaving the rest to the
per-step wake-on-contact does not work, because the woken body separates
from what it was carrying INSIDE the step it wakes in: a settled stack
rests on about a millimetre of overlap and one step of gravity is more than
that, so the contact that would have carried the wake upward is gone before
the next collision pass looks for it.
Costs nothing for a body that was already awake: the walk runs on the wake
of a sleeper only.
"""
was_asleep = body.asleep
self._wake(body)
if was_asleep:
self._wake_reachable([handle], {handle})
def _wake_reachable(self, frontier: list[BodyHandle], seen: set[BodyHandle]) -> None:
"""Wake every sleeper reachable from ``frontier`` through DYNAMIC contacts.
The sleep latch's island walk read the other way round: the same
dynamic-only adjacency out of ``_touching_by_body``
(:meth:`_sleep_settled_islands`), so what latched together wakes together.
STATIC and KINEMATIC bodies end the walk rather than carry it, or every pile
sharing a floor would be one island.
Only sleepers are woken. A body already awake is crossed and left as it is,
its sleep timer included: it is being solved anyway, and restarting its
timer would let one mover beside a settling pile hold that pile awake for
the life of the world.
``seen`` carries the bodies the caller has already dealt with. Peers are
looked up with ``.get``: a body destroyed this step leaves its edges in the
index until the next contact diff retires them.
"""
bodies = self._bodies
touching = self._touching_by_body
while frontier:
for peer in touching.get(frontier.pop(), ()):
if peer in seen:
continue
seen.add(peer)
body = bodies.get(peer)
if body is None or body.body_type is not BodyMode.DYNAMIC:
continue # destroyed this step, or the island ends here
if body.asleep:
self._wake(body)
frontier.append(peer)
def _wake_supported(self, handle: BodyHandle) -> None:
"""Wake what was touching ``handle`` at the end of the last step, and its island.
The ordinary wake-on-contact in :meth:`_wake_touching` only reaches a
sleeper through a contact that still exists, and it is evaluated AFTER
integration. A support that has just been teleported clear, or that has just
been flipped DYNAMIC and started to fall, has therefore already separated by
the time the collision pass runs: nothing wakes what it was holding up and
the sleeper hangs in the air for the life of the world. So whatever changes
a support out from under its riders must wake them by name, from the contact
set the previous step left behind.
By name reaches one level, and a stack is more than one level deep, so the
wake carries on through the island the riders belong to
(:meth:`_wake_reachable`). Waking only the box directly on the support left
everything above it asleep in mid-air: that box starts falling within the
step, the contact above it is gone by the next collision pass, and nothing
was left to wake the rest.
Costs the island's contact count, not the world's, and only on the step
something is actually taken away: a walk from an already-awake pile wakes
nobody and writes nothing. ``set_body_transform`` reaches this once per
moved body per frame, so the neighbours come from the ``_touching_by_body``
lookup; reading all of ``_touching`` instead would charge every mover for
every other contact in the scene.
"""
bodies = self._bodies
seen: set[BodyHandle] = {handle}
frontier: list[BodyHandle] = []
for other in self._touching_by_body.get(handle, ()):
seen.add(other)
body = bodies.get(other)
if body is None:
continue # a neighbour destroyed since the last step
self._wake(body)
if body.body_type is BodyMode.DYNAMIC:
frontier.append(other)
if frontier:
self._wake_reachable(frontier, seen)
def _wake_joint_ends(self, j: _Constraint2D) -> None:
"""Wake both ends of one constraint, skipping an already-freed body.
Attaching a constraint and taking one away are both disturbances at both
ends: a new one can pull on a body that had no reason to expect it, and
removing one drops whatever it was holding. Neither can be left to the
per-step wake rule, which measures the constraint's own geometry and so
says nothing about a constraint that did not exist a moment ago or no
longer does.
"""
for end in (j.a, j.b):
body = self._bodies.get(end)
if body is not None: # the end whose destruction dropped this joint
self._wake_island(end, body)
@staticmethod
def _constraint_pose(body: _Body2D) -> tuple[float, float, float]:
"""The body's pose as plain floats: what "has it moved" is decided on (2D).
Distinct from the broadphase's ``_pose_keys``, which fingerprints pose AND
collision filter for every body to decide a narrow-phase result can be
reused; this is pose alone, for the constraint ends only.
"""
return (float(body.position[0]), float(body.position[1]), body.rotation)
def _wake_moved_constraint_ends(self, joints: list[_Constraint2D]) -> None:
"""Wake a sleeping constraint end when the OTHER end has MOVED (2D).
Run once per step, before anything solves. A constraint can only impart
something new when its geometry changed, so a sleeping end is disturbed by
exactly one thing: its partner ending the previous step somewhere other
than where it started it. That covers the cases the two obvious rules miss.
A parked KINEMATIC anchor is never asleep here (only DYNAMIC bodies sleep),
so "the other end is awake" would keep a winched crate awake for the life
of the scene; and a constraint merely HOLDING a body against gravity passes
any impulse threshold every step, because equilibrium costs ``g * dt`` of
velocity change, which is an order of magnitude over the sleep threshold.
Movement is the property that separates a winch being reeled in from a
tether hanging slack on a pile that has settled.
Costs the joint count, not the body count, and only for a joint with a
sleeping end: an all-awake constraint short-circuits on ``asleep`` before
any pose is read.
"""
poses = self._constraint_poses
for j in joints:
ja, jb = self._bodies[j.a], self._bodies[j.b]
if ja.asleep:
was = poses.get(j.b)
# No record means the constraint is younger than a step; the wake it
# owes its ends was paid when it was created.
if was is not None and self._constraint_pose(jb) != was:
self._wake_island(j.a, ja)
if jb.asleep:
was = poses.get(j.a)
if was is not None and self._constraint_pose(ja) != was:
self._wake_island(j.b, jb)
def _record_constraint_poses(self) -> None:
"""Snapshot every constraint end's pose at the close of the step (2D).
Rebuilt rather than updated so a removed joint, or one dropped by the
destroy purge, takes its entries with it.
"""
poses: dict[BodyHandle, tuple[float, float, float]] = {}
for j in self._joints.values():
poses[j.a] = self._constraint_pose(self._bodies[j.a])
poses[j.b] = self._constraint_pose(self._bodies[j.b])
self._constraint_poses = poses
def _retrack_touching(
self,
entered: set[tuple[BodyHandle, BodyHandle]],
exited: set[tuple[BodyHandle, BodyHandle]],
) -> None:
"""Fold one step's contact deltas into the ``_touching_by_body`` lookup.
Carried forward rather than rebuilt, because the deltas are what a step
already knows: a settled pile enters and exits nothing and so costs nothing,
where rebuilding the lookup would re-read every resting contact every step.
"""
index = self._touching_by_body
for a, b in entered:
index.setdefault(a, set()).add(b)
index.setdefault(b, set()).add(a)
for a, b in exited:
for one, other in ((a, b), (b, a)):
peers = index.get(one)
if peers is not None:
peers.discard(other)
if not peers: # a body with no contacts holds no entry
del index[one]
@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 real scalar in radians.
Any real scalar is accepted, ``numpy`` floats included, since the engine's
own maths is float32.
"""
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, numbers.Real):
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 (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 contact point, the relative velocity THERE and the plain
# difference of the two linear velocities (both b w.r.t. a, in the
# contact's own a->b orientation), captured before the solver mutates
# poses and velocities. The event publishes the first as ``rel_velocity``
# and computes ``impulse_estimate`` from the second, which is what keeps
# that number free of this solver's choice of manifold point. Only a pair
# that is NEW this step can report an ENTER, and only an ENTER carries
# this payload, so a pair already in ``_touching`` is skipped: a settled
# stack pays nothing for it.
payload: dict[tuple[BodyHandle, BodyHandle], tuple[np.ndarray, np.ndarray, np.ndarray]] = {}
for key, c in current.items():
if key in self._touching:
continue
ba, bb = self._bodies[c.a], self._bodies[c.b]
point = _manifold_centre_2d(c)
rel = _point_velocity_2d(bb, point) - _point_velocity_2d(ba, point)
linear = bb.linear_velocity - ba.linear_velocity
payload[key] = (point, rel.astype(np.float32), linear.astype(np.float32))
impulses: dict[tuple[BodyHandle, BodyHandle], float] = {}
if contacts or self._joints:
self._solve(contacts, dt, impulses)
# Rebuild the warm-start cache 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, payload, impulses)
# Sensor overlap pass: 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: 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).
# This step's contacts come with it: an island that falls asleep is drained
# to the slop through them first (see _settle_island).
self._update_sleeping(dt, contacts)
# Close the step by recording where each constraint end came to rest, which
# is what next step's constraint wake compares against. Guarded on both
# sides so a jointless world touches nothing and a world that has just lost
# its last joint still drops the stale snapshot.
if self._joints or self._constraint_poses:
self._record_constraint_poses()
# -- collision events (broadphase-diffed edges) ------------------
def _diff_contacts(
self,
current: dict[tuple[BodyHandle, BodyHandle], _Contact2D],
payload: dict[tuple[BodyHandle, BodyHandle], tuple[np.ndarray, np.ndarray, 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).
``payload`` carries the pre-solve ``(point, rel_velocity, linear)`` of
every pair that is new this step, which is exactly the set that reports
an ENTER.
"""
live = current.keys()
entered = live - self._touching
exited = self._touching - live
for key in entered:
c = current[key]
ba, bb = self._bodies[c.a], self._bodies[c.b]
point, rel, linear = payload[key]
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)),
# Published even where the solver's own impulse is measured:
# it is the one shared formula over the one shared quantity,
# the LINEAR difference rather than the at-point value beside
# it (see the 3D sibling).
impulse_estimate=contact_impulse_estimate(linear, c.normal, ba.inverse_mass, bb.inverse_mass),
rel_velocity=Vec2(rel),
)
)
zero = Vec2(0.0, 0.0)
for key in exited:
a, b = key
self._contact_events.append(
ContactEvent2D(
a=a,
b=b,
phase=ContactPhase.EXIT,
point=zero,
normal=zero,
impulse=0.0,
impulse_estimate=0.0,
rel_velocity=zero,
)
)
self._touching = set(live)
self._retrack_touching(entered, exited)
# -- sensor overlap (broadphase-driven, one-directional) ---------------
def _collide_sensors(self) -> set[tuple[BodyHandle, BodyHandle]]:
"""One-directional sensor overlap sweep (basic tier).
2D sibling of the 3D ``_collide_sensors``: a SECOND sweep, SEPARATE
from ``_collide``'s AND-filtered collision pass (kept separate so the
one-directional sensor filter never contaminates the body-body AND rule).
Only pairs containing a sensor can emit anything, so only those are walked:
a scene with no sensor returns immediately, and one with a handful pays for
those bodies alone rather than for every pair in the world. Each unordered
pair is still visited exactly once and in ascending body-table order, which
is the order the live set is built in and therefore the order
:meth:`_diff_overlaps` reports its events in.
For any such pair 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())
sensors = [i for i, (_, body) in enumerate(items) if body.is_sensor]
if not sensors:
return live
# Every pair holding a sensor, deduplicated (sensor vs sensor is reachable
# from both ends) and restored to ascending body-table order: the set below
# is built in this order, and a set's iteration order follows the order it
# was filled in, which is the order the overlap events come out in.
n = len(items)
pairs = sorted({(i, j) if i < j else (j, i) for i in sensors for j in range(n) if j != i})
for i, j in pairs:
ha, ba = items[i]
hb, bb = items[j]
# Overlap is symmetric, so test the geometry once per pair. Concave
# (STATIC edge soup) routes through _narrow fine.
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 it per fixed step to sustain a force).
"""
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
# Gravity is scaled per body; damping is drag on the velocity the
# body ALREADY HAS, so it is applied before this step's
# acceleration rather than to the sum (see the 3D twin: damping
# the increment a resting contact is about to cancel leaves a
# residue the solver cannot remove, and a stack jitters on it).
if body.gravity_scale == 1.0:
accel = gravity + body.force * body.inverse_mass
else:
accel = gravity * body.gravity_scale + body.force * body.inverse_mass
linear = body.linear_velocity * max(0.0, 1.0 - body.linear_damping * dt)
linear += accel * dt
body.linear_velocity = linear
spin = body.angular_velocity * max(0.0, 1.0 - body.angular_damping * dt)
body.angular_velocity = spin + body.torque * body.inverse_moment * dt
target = body.position + body.linear_velocity * dt
# CCD: 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) ------------------------
@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.
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]:
"""Broad phase + analytic narrowphase, in the canonical all-pairs order.
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``), and finally
the one-way platform filter can drop a pass-through.
Two mechanisms keep a big or a settled scene off that narrowphase, and
NEITHER may change a contact:
- the pair SOURCE (:meth:`_candidate_pairs` over the sorted bounds, or
:meth:`_loop_pairs` below the crossover) proposes only the pairs whose
world AABBs meet. Both apply the same ``_aabb`` bounds and the same
overlap predicate the nested loop used to apply per pair, so a rejected
pair is one the loop rejected too.
- the UNCHANGED-PAIR skip reuses the previous step's narrowphase result for
any pair whose two bodies are bit-identically posed, shaped and filtered
as they were at the previous collide pass. Nothing the narrowphase reads
has changed, so its answer cannot have changed either. This is what takes
a settled scene off the geometry entirely: an asleep body is not
integrated, and once its resting penetration stops moving its pose stops
changing bit for bit, so SAT and the edge clipping never run again.
The skip reuses the CONTACT, not merely the fact of one, so the pair stays
in the returned list: it keeps its place in ``_touching``, its warm-start
impulses, its entry in the per-body contact index, and its silence in the
event diff. Dropping it instead would fire a spurious EXIT and break
wake-on-contact. The one-way filter is re-run over the replayed contact
rather than replayed with it, because it reads the live velocities.
Candidate pairs are visited in ascending ``(i, j)`` body-table order, the
order the plain nested loop used. Contact order is part of the result: the
solver iterates this list and float addition is not associative.
"""
items = list(self._bodies.items())
n = len(items)
unchanged = self._refresh_bounds(items)
pairs = self._candidate_pairs(n) if n >= _BROADPHASE_MIN_BODIES else self._loop_pairs(n)
previous = self._pair_contacts
current: dict[tuple[BodyHandle, BodyHandle], _Contact2D] = {}
contacts: list[_Contact2D] = []
for i, j in pairs:
ha, ba = items[i]
hb, bb = items[j]
# Sensors never produce a collision contact (gated before
# narrowphase, exactly like 3D): sensors go through the overlap stream.
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
# Cache key. Built from the visit order, which is stable for the life of
# a pair (the body table never reorders what it already holds), so it
# needs no canonicalising.
key = (ha, hb)
if unchanged[i] and unchanged[j]:
# Both bodies are exactly as the last pass saw them, and the three
# gates above read the same values, so this pair was evaluated then
# and its answer still holds: a hit is replayed, a miss stays a miss.
cached = previous.get(key)
contact = (
None
if cached is None
else _Contact2D(cached.a, cached.b, cached.normal, cached.depth, list(cached.points))
)
else:
contact = self._narrow(ha, ba, hb, bb)
if contact is None:
continue
current[key] = contact
# One-way filter: 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.
# Kept OUT of the cache decision: it reads this step's velocities, so it
# is re-run over a replayed contact exactly as over a fresh one.
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)
self._pair_contacts = current
return contacts
def _refresh_bounds(self, items: list[tuple[BodyHandle, _Body2D]]) -> list[bool]:
"""Flag the bodies unchanged since the last pass, refreshing what moved.
A body counts as unchanged only when its position is bit-identical, its
rotation is the same down to the sign of a zero, its shape is the same
object, and every value the pair gates read (inverse mass, layer, mask,
sensor flag) still compares equal. That is the complete input set of
:meth:`_narrow` and of the gates in :meth:`_collide`, so two unchanged
bodies must yield the previous answer. Anything that edits a body goes
through the setters above, so a live edit always lands in this fingerprint.
The world AABB table is refreshed in the same pass: rows for the bodies that
changed, everything when the body set itself changed. A settled scene
therefore rewrites nothing, where the old pass rebuilt every body's AABB
every step.
"""
keys = self._pose_keys
rebuild = self._bounds_epoch != self._body_epoch
if rebuild:
self._bounds_lo = np.zeros((len(items), 2), dtype=np.float32)
self._bounds_hi = np.zeros((len(items), 2), dtype=np.float32)
self._bounds_epoch = self._body_epoch
unchanged: list[bool] = []
rows: list[int] = []
moved_lo: list[np.ndarray] = []
moved_hi: list[np.ndarray] = []
for row, (handle, body) in enumerate(items):
key = (
body.position.tobytes(),
body.rotation,
math.copysign(1.0, body.rotation),
body.shape,
body.inverse_mass,
body.collision_layer,
body.collision_mask,
body.is_sensor,
)
same = keys.get(handle) == key
if not same:
keys[handle] = key
unchanged.append(same)
if rebuild or not same:
lo, hi = self._aabb(body)
rows.append(row)
moved_lo.append(lo)
moved_hi.append(hi)
if rows:
self._bounds_lo[rows] = moved_lo
self._bounds_hi[rows] = moved_hi
return unchanged
def _loop_pairs(self, n: int) -> Iterator[tuple[int, int]]:
"""The nested pair loop, culled by the cached bounds (small scenes).
The pair source below :data:`_BROADPHASE_MIN_BODIES`, where planning a sweep
costs more than testing the handful of pairs outright. Reads the same bounds
table :meth:`_candidate_pairs` sorts, as plain Python floats (a float32
converts exactly, so the comparisons are the ones numpy would make), which
is itself faster than the per-pair ``np.all`` the loop used to run.
"""
lo = self._bounds_lo.tolist()
hi = self._bounds_hi.tolist()
for i in range(n):
lo_i, hi_i = lo[i], hi[i]
for j in range(i + 1, n):
lo_j, hi_j = lo[j], hi[j]
if lo_j[0] <= hi_i[0] and lo_i[0] <= hi_j[0] and lo_j[1] <= hi_i[1] and lo_i[1] <= hi_j[1]:
yield i, j
def _candidate_pairs(self, n: int) -> Iterator[tuple[int, int]]:
"""Sweep-and-prune over the cached bounds: the pairs worth narrow-phasing.
Sorts the bodies once along the axis their bounds are most spread over,
takes each body's run of later bodies whose interval starts before its own
ends (one vectorised ``searchsorted``, no Python sweep loop), then rejects
those survivors on the other axis in a single numpy pass.
Sweep-and-prune rather than a uniform grid because the case that matters
here is a settled pile, which is exactly where a grid degenerates: pile
everything into one or two cells and the grid is all-pairs again with extra
bookkeeping. The sweep only ever pays for intervals that genuinely overlap.
A BVH would beat it on a sparse world, but it has to be rebuilt or refitted
as bodies move, and this tier's budget is better spent not sorting at all.
Yields ascending ``(i, j)`` body-table index pairs: the broad phase changes
WHICH pairs are tested, never in which order, because the contact list it
feeds is iterated by a sequential-impulse solver.
"""
lo = self._bounds_lo
hi = self._bounds_hi
# Sweep on the axis the bodies are most spread over: the more spread, the
# fewer intervals overlap, the fewer candidates survive the sweep.
axis = int(np.argmax(np.ptp(lo + hi, axis=0)))
order = np.argsort(lo[:, axis], kind="stable")
lo_axis = lo[order, axis]
# Per body (in sweep order), the end of the run of later bodies whose
# interval starts at or before this one's end.
ends = np.searchsorted(lo_axis, hi[order, axis], side="right")
starts = np.arange(1, n + 1)
counts = np.maximum(ends - starts, 0)
total = int(counts.sum())
if total == 0:
return iter(())
# Ragged concatenation of range(start, end) per body, without a loop.
left = np.repeat(order, counts)
right = order[np.arange(total) - np.repeat(np.cumsum(counts) - counts, counts) + np.repeat(starts, counts)]
keep = np.all((lo[left] <= hi[right]) & (lo[right] <= hi[left]), axis=1)
left = left[keep]
right = right[keep]
first = np.minimum(left, right)
second = np.maximum(left, right)
# Back into nested-loop order: ascending first index, then second.
sequence = np.argsort(first * n + second, kind="stable")
return zip(first[sequence].tolist(), second[sequence].tolist(), strict=True)
# -- 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`). The
minimum-overlap axis is the shortest way out, not the way the body came in,
so past the two shapes' midplane it flips to the FAR face; a pair with
solid contact history is then recovered out the face its remembered
normal names (:meth:`_carried_axis`), and a history-less pair keeps the
blind choice. Basic-tier honesty: once
the overlap is deep enough that the incident edge has passed through the
reference face, the clipped points land outside the reference polygon
entirely, so the contact lever arms are approximate there. Full deep-overlap
robustness is 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
carried = self._carried_axis(ha, va, hb, vb, normal, depth, flip_owner)
if carried is not None:
axis, edge_idx, depth, axis_is_a = carried
if axis_is_a:
ref_verts, inc_verts, ref_edge, normal, flip_owner = va, vb, edge_idx, axis, False
else:
ref_verts, inc_verts, ref_edge, normal, flip_owner = vb, va, edge_idx, axis, 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)
def _carried_axis(
self,
ha: BodyHandle,
va: np.ndarray,
hb: BodyHandle,
vb: np.ndarray,
normal: np.ndarray,
depth: float,
flip_owner: bool,
) -> tuple[np.ndarray, int, float, bool] | None:
"""Re-pick the SAT axis when a deep overlap has flipped it to the far face.
The minimum-penetration axis is the shortest way out, and past half the
two polygons' summed extent along it the shortest way out is the FAR face:
recovery then pushes the body onward along the path it came in by, and it
exits the wrong side. Which side it came in by is not in this step's
geometry at all, so it is carried: the pair's contact normal from the
most recent completed narrowphase pass, normally the previous step
(cached in ``_pair_contacts``, in either key order, and gated on
``_touching`` so a one-way pass-through the filter discarded carries
nothing). That covers the recorded defect shapes, a resting body
teleported into its support or a collider grown through its floor. A
pair with NO solid history keeps the blind choice deliberately: the
entry face of a fresh deep overlap is not knowable (a tunnelled arrival
and an unstick shove present the same state and need opposite answers),
so nothing is guessed. A concave-soup pair also keeps the blind choice:
the soup narrows every segment under one pair key, so its cached normal
is not attributable to the segment being resolved.
The re-scan runs only when the blind choice points AGAINST the carried
normal AND the overlap is past half the smaller polygon's extent along
the chosen axis; against box-like colliders an ordinary resting or
sliding contact never reaches that, and against a segment sliver, whose
gate is tiny, the agreement early-out carries the everyday path.
Returns ``(axis, edge_index, depth, axis_is_a)`` for the shallowest axis
agreeing with the carried signal (``axis`` in its owner's outward frame,
exactly as :meth:`_sat_axis` reports it), or None to keep the blind
choice, including when no axis agrees.
"""
prev = self._pair_contacts.get((ha, hb))
sign = 1.0
if prev is None:
prev = self._pair_contacts.get((hb, ha))
sign = -1.0 # cached a -> b in the other key order
if prev is None or self._canon(ha, hb) not in self._touching:
# Only a contact that was SOLID last step carries a normal. The pair
# cache also holds one-way pass-throughs the filter discarded, and a
# ghost the solver never acted on must not steer the recovery (a body
# admitted up through a platform would be pinned to its underside).
# A pair with no such history keeps the blind minimum: the entry face
# of a fresh deep overlap is simply not knowable (the same state can
# be a tunnelled arrival or an unstick shove on its way out, which
# need opposite answers), and guessing from the relative velocity
# measurably drags shoved-out bodies back through the geometry.
return None
px = sign * float(prev.normal[0])
py = sign * float(prev.normal[1])
# The blind choice in canonical a -> b orientation, as plain scalars: this
# runs for every polygon contact, and the common answer is "agrees".
cx = float(normal[0])
cy = float(normal[1])
if flip_owner:
cx = -cx
cy = -cy
if cx * px + cy * py >= 0.0:
return None # the blind choice already agrees with the carried signal
preferred = np.array([px, py], dtype=np.float32)
canonical = np.array([cx, cy], dtype=np.float32)
proj_a = va @ canonical
proj_b = vb @ canonical
smaller = min(float(proj_a.max() - proj_a.min()), float(proj_b.max() - proj_b.min()))
if depth <= 0.5 * smaller:
# Against box-like colliders an ordinary resting or sliding contact
# never reaches half the smaller extent. Against a segment sliver
# the gate is tiny and the agreement early-out above carries the
# everyday path instead.
return None
body_a = self._bodies.get(ha)
body_b = self._bodies.get(hb)
if body_a is None or body_b is None:
return None # a transient query probe: nothing was cached for it
if body_a.shape.kind == "concave" or body_b.shape.kind == "concave":
# A concave soup narrows every segment under ONE pair key, so the
# cached normal belongs to whichever segment won last step and
# cannot be attributed to the segment being resolved now. Steering
# this segment by that normal ejects a body wedged between two
# opposing faces (a corridor 1% too small); the blind choice stands.
return None
# Constrained re-scan over both polygons' axes, keeping only the axes
# whose canonical a -> b direction agrees with the carried signal. Every
# axis overlaps (SAT found no separating axis), so each depth is >= 0.
best: tuple[np.ndarray, int, float, bool] | None = None
for axis, edge_idx in self._poly_edge_normals(va):
if float(np.dot(axis, preferred)) <= 0.0:
continue # A's axes are already canonical: out of A toward B
face_pt = va[edge_idx]
d = -min(float(np.dot(v - face_pt, axis)) for v in vb)
if best is None or d < best[2]:
best = (axis, edge_idx, d, True)
for axis, edge_idx in self._poly_edge_normals(vb):
if float(np.dot(axis, preferred)) >= 0.0:
continue # B's axes are negated for a -> b, so agreement flips sign
face_pt = vb[edge_idx]
d = -min(float(np.dot(v - face_pt, axis)) for v in va)
if best is None or d < best[2]:
best = (axis, edge_idx, d, False)
return best
@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 ---------------------------
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:
Ahead of all of it, once per step, the constraint wake pass: a constraint
disturbs a sleeping end only when its OTHER end moved (see
:meth:`_wake_moved_constraint_ends`). Its contact twin
(:meth:`_wake_touching`) runs once per step too, just before the loop.
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, ``solver_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. Position loop, ``position_iterations`` Baumgarte passes, over the
contacts AND the rigid joints together: penetration drained down to the
contact slop, anchor coincidence, angle lock. Each pass re-measures its
own error, which is what keeps the contact half self-limiting.
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``). Contacts ARE warm-started (step 0b below); neither
the rigid joints nor the springs are, and both solve from zero every step
(the springs in the once-per-step pre-pass, step 0).
The ``impulses`` side-table 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)]
if joints:
# Before anything solves, so a woken end keeps this step's impulse
# instead of having it zeroed under it by the sleep pass.
self._wake_moved_constraint_ends(joints)
# 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.
# 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. 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 0c: wake-on-contact, once for the whole contact list. It sits AFTER
# the warm start on purpose: the warm start skips a pair with two sleeping
# ends, so waking first would seed a freshly woken island with last step's
# accumulated support.
self._wake_touching(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.
iterations = self._solver_iterations
last = iterations - 1
# Each rigid joint's anchor arms in world axes, built ONCE for the whole
# loop: nothing below turns a body, only velocities change, so every pass
# would otherwise rebuild the same vectors from the same basis.
arms = [self._joint_arms(j) for j in rigid]
# Step 0d: joint warm start, the constraint twin of step 0b. Contacts have
# been seeded from last step; the rigid point constraints are seeded here,
# from the same basis the loop below reads.
if rigid:
self._warm_start_joints(rigid, arms, self._joint_warm_scale(dt))
for it in range(iterations):
write = impulses if it == last else None
for c in contacts:
self._solve_contact(c, write)
for j, joint_arms in zip(rigid, arms, strict=True):
self._solve_joint(j, joint_arms)
# (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: position (Baumgarte) passes over the contacts and the rigid
# joints together (3D contact parity). Each pass re-measures its own error
# from the live poses, so iterating the contact half converges on the true
# pose instead of over-pushing a resting body out of its support.
if contacts:
self._prepare_contact_projections(contacts)
if contacts or rigid:
for _ in range(self._position_iterations):
for c in contacts:
self._correct_contact_position(c)
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.
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.
A pair at exact rest has no approach speed to measure, at its centres or at
any contact point, so it keeps the zero bias a fresh contact is born with
and is not measured at all.
"""
ba = self._bodies[c.a]
bb = self._bodies[c.b]
if _at_rest(ba) and _at_rest(bb):
# No approach speed to measure: every bias is its default 0, and the
# accumulators are the zeros the full pass would leave behind.
c.vbias = [0.0] * len(c.points)
c.jn = [0.0] * len(c.points)
c.jt = [0.0] * len(c.points)
return
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.
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. So is a pair that has already rested its impulses
out to zero: seeding zero onto two zero velocities is arithmetic with no
result, and a settled scene is made of nothing else.
"""
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
if _all_positive_zero(jn_cache) and _all_positive_zero(jt_cache) and _at_rest(ba) and _at_rest(bb):
continue # a rested-out pair: seeding zero onto two zero velocities
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 _warm_start_joints(
self,
rigid: list[_RigidPointConstraint2D | _GrooveConstraint2D],
arms: list[tuple[np.ndarray, np.ndarray]],
scale: float,
) -> None:
"""Seed + apply each rigid point constraint's impulse from last step (2D).
The contact twin of this (:meth:`_warm_start_contacts`) is why a resting
stack converges in a couple of passes instead of rebuilding its support from
zero. The joints had no such pass, and solved from zero every step, so the
iteration budget a chain needs grew with its length: a Gauss-Seidel sweep
carries tension one link per pass, and at the default eight passes a chain of
four links or more never came to rest at all.
Each point constraint carries the total impulse its passes applied last step
(``j.impulse``, accumulated in :meth:`_solve_joint`). Here that total is
re-applied, rescaled by the ratio of this step's ``dt`` to the previous one,
and the accumulator is reset to exactly what was applied so it keeps meaning
"the impulse this step used". The seed is undamped, as Box2D's is: a damped
one converges strictly worse at every chain length measured, and full
strength does not destabilise, because a point row is solved exactly rather
than clamped, so a pass that reads too large a seed simply takes it back.
The groove takes no seed: its anchor on ``a`` is wherever on the rail the
slider currently sits, so the impulse is not a carried quantity between two
fixed anchors. The angular locks (weld, and the 3D hinge) take none either:
each is a scalar row already solved exactly in one pass.
A pair asleep at both ends is skipped, exactly as the contact warm start
skips it. Nothing invalidates the carried impulse on a teleport, and nothing
needs to: unlike a contact accumulator, which is clamped and therefore
remembers, a point row is solved exactly, so the first pass that reads a
stale seed drives the anchor's relative velocity to zero regardless of it.
"""
for j, (r_a, r_b) in zip(rigid, arms, strict=True):
if isinstance(j, _GrooveConstraint2D):
continue
seed = (j.impulse * scale).astype(np.float32)
j.impulse = _zero2()
if not seed.any():
continue
ba, bb = self._bodies[j.a], self._bodies[j.b]
if ba.asleep and bb.asleep:
continue
if ba.inverse_mass + bb.inverse_mass == 0.0:
continue
ba.linear_velocity = ba.linear_velocity - seed * ba.inverse_mass
bb.linear_velocity = bb.linear_velocity + seed * bb.inverse_mass
ba.angular_velocity = ba.angular_velocity - ba.inverse_moment * _cross_2d(r_a, seed)
bb.angular_velocity = bb.angular_velocity + bb.inverse_moment * _cross_2d(r_b, seed)
j.impulse = seed
def _joint_warm_scale(self, dt: float) -> float:
"""How much of last step's joint impulse this step may seed (2D + 3D shape).
All of it, rescaled by ``dt / previous dt``: an impulse is a force integrated
over the step, so a step of a different length wants a proportionally
different one (Box2D's ``dtRatio``). The first step of a world has no
previous impulse to seed, and says so with a scale of zero.
"""
previous = self._joint_warm_dt
self._joint_warm_dt = dt
if previous <= 0.0:
return 0.0
return dt / previous
def _wake_touching(self, contacts: list[_Contact2D]) -> None:
"""Wake-on-contact for the whole contact list, ONCE per step.
If exactly one end of a contact is asleep and the other is an awake DYNAMIC
body or a MOVING 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); and a sleeper on a PARKED kinematic body stays asleep
too. A kinematic body is never asleep, so "the other is awake" says nothing
about it: what matters is whether it actually moved this step, the same
question the constraint wake rule asks (that rule compares poses; here
velocity answers it, because a kinematic body moves only by integrating its
set velocity, and a teleport wakes the riders by name through ``_disturb``,
so zero velocity is exactly "did not move"). The sleeper brings its island
(:meth:`_wake_island`), so a body arriving at the top of a sleeping stack
wakes the whole stack on the step it lands rather than one box per step, and
the wake cannot be lost to a contact that separates before the next
collision pass.
This is a per-step question, which is why it is asked here rather than in
:meth:`_solve_contact`, which the velocity loop calls ``solver_iterations``
times for every contact in the world. Neither input can change under the
loop: the loop writes velocities but never sleep flags, and the only
velocity it reads here belongs to a kinematic body, which no impulse moves.
Asked once, a sleeper on a parked kinematic support costs what a sleeper on
static ground costs, instead of a velocity test per solver iteration for as
long as it rests there.
"""
for c in contacts:
ba = self._bodies[c.a]
bb = self._bodies[c.b]
if ba.asleep == bb.asleep:
continue
sleeper, other, woken = (ba, bb, c.a) if ba.asleep else (bb, ba, c.b)
if other.body_type is BodyMode.KINEMATIC:
if other.linear_velocity.any() or other.angular_velocity != 0.0:
self._wake_island(woken, sleeper)
elif other.body_type is not BodyMode.STATIC:
self._wake_island(woken, sleeper)
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: 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`` 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]
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)
# Nothing to solve for a pair at exact rest with no restitution target and
# no impulse yet accumulated: at every manifold point the normal impulse
# this pass would add is -(0 - 0) / k_n, the friction delta cancels a slide
# of zero, and the already-clamped accumulators keep the values they hold.
# Every write below is then a no-op down to the sign of its zeros, so
# declining to make them leaves the state bit for bit where it was. This is
# the pass a settled scene spends all its time in: a sleeper resting on
# STATIC ground is not an asleep-vs-asleep pair, so the check above never
# catches it, and its support impulse has long since converged to zero (the
# body is not integrated, so there is no gravity for the contact to cancel).
if (
_at_rest(ba)
and _at_rest(bb)
and _all_positive_zero(c.vbias)
and _all_positive_zero(c.jn)
and _all_positive_zero(c.jt)
):
if impulses is not None:
impulses[self._canon(c.a, c.b)] = 0.0 # the sum of the zeros it holds
return
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 (contact-event payload)
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:
# A zero Coulomb cap (mu * jn) admits no tangent impulse, so a
# point still holding one gives it back before skipping. Without
# this release the point can never re-enter the friction branch,
# its stale tangent torque demands a balancing normal impulse at
# the other manifold point, and a resting pair settles into a
# self-consistent NON-ZERO fixed point: the at-rest skip above
# (which needs every accumulator at zero) then never arms, so a
# settled scene keeps solving every contact for ever.
if c.jt[i] != 0.0:
impulse_t = (-c.jt[i] * t).astype(np.float32)
c.jt[i] = 0.0
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)
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 _prepare_contact_projections(self, contacts: list[_Contact2D]) -> None:
"""Fix each contact's depth datum, drain budget and inverse-mass split.
The 2D twin of the 3D
:meth:`~simvx.core.physics.builtin.world.BuiltinPhysics._prepare_contact_projections`,
which owns the rationale: a datum so every pass can re-measure the depth
from the live centres for the price of one dot product, a per-step
separation ceiling taken from the smaller body's bounding radius, and the
mass split resolved once because sleep and body mode cannot change inside
the position loop. Bounding radii are memoised on shape identity, so a pile
sharing one shape record measures it once.
"""
radii: dict[int, float] = {}
for c in contacts:
ba = self._bodies[c.a]
bb = self._bodies[c.b]
inv_a, inv_b = _shiftable_mass(ba), _shiftable_mass(bb)
inv_sum = inv_a + inv_b
if inv_sum == 0.0:
c.drain_budget = 0.0
continue
c.share_a = inv_a / inv_sum
c.share_b = inv_b / inv_sum
c.depth_datum = c.depth + float(np.dot(bb.position - ba.position, c.normal))
radius_a = radii.get(id(ba.shape))
if radius_a is None:
radius_a = radii[id(ba.shape)] = _bounding_radius_2d(ba.shape)
radius_b = radii.get(id(bb.shape))
if radius_b is None:
radius_b = radii[id(bb.shape)] = _bounding_radius_2d(bb.shape)
c.drain_budget = _MAX_DRAIN_FRACTION * min(radius_a, radius_b)
def _correct_contact_position(self, c: _Contact2D) -> None:
"""One Baumgarte pass draining this contact's residual penetration.
Same maths as the 3D ``_correct_contact_position`` (linear positional split
by inverse mass, ``_BAUMGARTE`` of the depth beyond the slop, capped by
what is left of the step's drain budget), run the same
``position_iterations`` times against a depth re-measured between passes.
2D position correction stays LINEAR (no rotational positional bias) like
the 3D tier; the velocity solve carries the angular response.
A sleeper is immovable here, exactly as it is for the joint position pass
(:func:`_shiftable_mass`): a pair asleep at both ends is skipped, and where
one end is awake it takes the whole correction, as it does against STATIC
ground. Without that, "asleep" means only "not integrated": the pass goes on
draining a settled pile's residual penetration for the life of the world, so
a body that reports :meth:`sleeping` keeps changing the pose it reads back.
"""
budget = c.drain_budget
if budget <= 0.0:
return # nothing shiftable, or the step's whole allowance is spent
ba = self._bodies[c.a]
bb = self._bodies[c.b]
depth = c.depth_datum - float(np.dot(bb.position - ba.position, c.normal))
drain = (depth - self.contact_slop) * _BAUMGARTE
if drain <= 0.0:
return # inside the slop: this contact is done for the step
if drain > budget:
drain = budget
c.drain_budget = budget - drain
ba.position = ba.position - (drain * c.share_a) * c.normal
bb.position = bb.position + (drain * c.share_b) * c.normal
# -- joint velocity solve (rigid joints) -------------------------------
def _solve_joint(self, j: _Constraint2D, arms: tuple[np.ndarray, np.ndarray]) -> 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.
``arms`` carries the joint's anchor offsets already in world axes
(:meth:`_joint_arms`), built once for the whole velocity loop because
nothing in it turns a body.
Sleep is NOT decided here. Waking both ends on every pass, which is what
this did, is why a jointed body could never sleep at all: a joint holding a
body against gravity is a live constraint for the life of the scene, so the
wake never stopped firing. It is decided once per step by
:meth:`_wake_moved_constraint_ends`, on whether the other end moved.
Each point-constraint pass adds the impulse it applied to the joint's
running total, which is what next step's warm start seeds from
(:meth:`_warm_start_joints`); the total therefore always says what the step
actually applied.
"""
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.
j.impulse = j.impulse + self._solve_point_velocity(j.a, j.b, *arms)
elif isinstance(j, _GrooveConstraint2D):
self._solve_groove_velocity(j)
elif isinstance(j, _FixedConstraint2D):
# Weld: the constrained point IS b's centre, so the arm is a's alone --
# ``pos_a + R_a * rel_local``. Driving the velocity there to b's is what
# makes a welded body swing round with a turning ``a`` (with r = 0 on
# both, the weld would only lock (vb - va) and the assembly could not
# orbit). The scalar angular lock follows.
j.impulse = j.impulse + self._solve_point_velocity(j.a, j.b, *arms)
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 _joint_arms(self, j: _Constraint2D) -> tuple[np.ndarray, np.ndarray]:
"""A rigid joint's stored anchor offsets in world axes, from the current angles.
Pin and hinge carry one anchor per end. A weld's constrained point is
``b``'s centre, so it has an arm on ``a`` and none on ``b``. A groove
derives its own anchors from the bodies' positions inside its solve (the
anchor on ``a`` is wherever on the rail the slider currently sits) and
takes none from here.
Built once per solve PHASE rather than once per iteration: the velocity
loop only changes velocities, so every pass of it reads the same basis,
while the position loop turns bodies and so rebuilds the arms each pass --
a stale arm there is exactly the drift this formulation exists to remove.
"""
ba = self._bodies[j.a]
if isinstance(j, _PinConstraint2D | _HingeConstraint2D):
return _to_world_2d(ba, j.local_a), _to_world_2d(self._bodies[j.b], j.local_b)
if isinstance(j, _FixedConstraint2D):
return _to_world_2d(ba, j.rel_local), _ZERO2
if isinstance(j, _GrooveConstraint2D):
return _ZERO2, _ZERO2
# Only rigid kinds reach here: springs are pre-passed, never dispatched.
raise AssertionError(f"unknown rigid 2D joint record {type(j).__name__}") # pragma: no cover
def _solve_point_velocity(self, a: BodyHandle, b: BodyHandle, r_a: np.ndarray, r_b: np.ndarray) -> np.ndarray:
"""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.
Returns:
The impulse applied to ``b`` (negated on ``a``), so the caller can
accumulate the joint's total for the next step's warm start
(:meth:`_warm_start_joints`). The zero vector when nothing was solved.
"""
ba, bb = self._bodies[a], self._bodies[b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return _ZERO2 # 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)
return 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).
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 own local offset. 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.
The rail is rebuilt from ``a``'s current angle each pass, so a groove on a
turning carrier turns with it. The perpendicular is a rigid 1-DOF lock: no
compliance, no restitution. Nothing drives the slider ALONG the groove --
no backend offers a slide motor, so a slider is moved by forces on ``b``.
The end stop lives in the position pass (``_correct_groove_position``) and
is soft; 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, each turned by its own carrier.
ga = ba.position + _to_world_2d(ba, j.ga)
gb = ba.position + _to_world_2d(ba, j.gb)
r_b = _to_world_2d(bb, j.local_b)
anchor_b = bb.position + 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)
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).
A sleeping end is woken before this runs, by
:meth:`_wake_moved_constraint_ends`, and only when the spring's other end
moved. So a winch reels in a crate that had settled, while a spring hanging
slack on a pile that has stopped leaves it asleep. An impulse handed to an
end that stays asleep goes nowhere: the sleep pass holds a sleeper at zero
velocity, so it is discarded rather than banked. That backstop is a VELOCITY
one and does not extend to the rigid joints' Baumgarte position pass, which
writes poses directly and therefore has to exclude a sleeper itself (see
:func:`_shiftable_mass`).
"""
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.
Every correction below splits itself over the ends that are able to move:
a sleeping end is immovable here (:func:`_shiftable_mass`), which is what
keeps a rigid joint from dragging a body that reports ``sleeping()``.
"""
if isinstance(j, _PinConstraint2D | _HingeConstraint2D):
self._correct_point_position(j.a, j.b, *self._joint_arms(j))
elif isinstance(j, _GrooveConstraint2D):
self._correct_groove_position(j)
elif isinstance(j, _FixedConstraint2D):
# Hold the captured relative offset: target pos_b == pos_a + R_a *
# rel_local, so an arm of R_a * rel_local on a and none on b make the
# coincidence error C = (pos_b + r_b) - (pos_a + r_a) =
# pos_b - (pos_a + R_a * rel_local), zero exactly when the offset is
# preserved (r = 0 on both would instead collapse the two centres).
self._correct_point_position(j.a, j.b, *self._joint_arms(j))
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``, the caller having rebuilt each arm ``r`` from
its body's current angle. The bodies are pushed apart by
``_BAUMGARTE * C`` split by inverse mass, with a
``_JOINT_SLOP`` deadband to avoid jitter (matching the 3D joint Baumgarte).
A sleeping end takes no share of the push (:func:`_shiftable_mass`), so the
whole correction lands on the end that can still move, and a joint with two
sleeping ends is skipped outright.
"""
ba, bb = self._bodies[a], self._bodies[b]
inv_a, inv_b = _shiftable_mass(ba), _shiftable_mass(bb)
inv_sum = inv_a + inv_b
if inv_sum == 0.0:
return
c = (bb.position + r_b) - (ba.position + r_a)
err = float(np.linalg.norm(c))
if err <= _JOINT_SLOP:
return
corr = (_BAUMGARTE * c).astype(np.float32)
ba.position = ba.position + corr * (inv_a / inv_sum)
bb.position = bb.position - corr * (inv_b / inv_sum)
def _correct_groove_position(self, j: _GrooveConstraint2D) -> None:
"""Baumgarte push pulling ``b``'s anchor back onto the groove segment (2D).
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 stop
(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 ``_JOINT_SLOP`` deadband, exactly
like :meth:`_correct_point_position` (linear; the velocity solve carries the
angular response, and a sleeping end takes no share). Being a push rather
than a clamp, the end stop is only as firm as the load allows: a sustained
force along the rail carries the slider PAST the end and keeps carrying it.
:class:`_GrooveConstraint2D` has the measurement.
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_a, inv_b = _shiftable_mass(ba), _shiftable_mass(bb)
inv_sum = inv_a + inv_b
if inv_sum == 0.0:
return
ga = ba.position + _to_world_2d(ba, j.ga)
gb = ba.position + _to_world_2d(ba, j.gb)
anchor_b = bb.position + _to_world_2d(bb, j.local_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 <= _JOINT_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 * (inv_a / inv_sum)
bb.position = bb.position - corr * (inv_b / 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, with a
sleeping end taking none of it (:func:`_shiftable_moment`).
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_a, inv_b = _shiftable_moment(ba), _shiftable_moment(bb)
inv_sum = inv_a + inv_b
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 * (inv_a / inv_sum)
bb.rotation = bb.rotation + corr * (inv_b / 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, separate from the full query suite. 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, contacts: list[_Contact2D]) -> 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 world's thresholds for that many 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).
The pass also HOLDS every sleeper at zero velocity, which is what makes
'asleep' mean 'at rest' for as long as it lasts rather than only at the
moment it falls asleep.
The timer is per body; the LATCH is per contact island (see
:meth:`_sleep_settled_islands`), so a body that has timed out waits for its
neighbours instead of being woken by them on the next step.
Args:
dt: The step the timers advance by.
contacts: This step's contacts, which the latch drains the island
through. Empty means there are none to project.
"""
threshold = self._sleep_velocity_threshold
lin_sq = threshold * threshold
time_threshold = self._sleep_time_threshold
timed_out: set[BodyHandle] = set()
for handle, body in self._bodies.items():
if body.body_type is not BodyMode.DYNAMIC:
continue
if body.asleep:
# A sleeping body is skipped by _integrate, so anything the contact
# solver seeded into its velocity this step (the warm-started
# impulse of a contact it still has with the ground it rests on) is
# momentum it can never spend: left in place it grows step after
# step, invisibly, and is released in full the moment something
# wakes the body, which then leaves the scene at a speed it never
# had. Asleep means at rest, every step it stays asleep, not only
# on the step it fell asleep.
body.linear_velocity[:] = 0.0
body.angular_velocity = 0.0
continue
if not body.can_sleep:
continue
slow = (
float(np.dot(body.linear_velocity, body.linear_velocity)) < lin_sq
and abs(body.angular_velocity) < threshold
)
if slow:
body._sleep_timer += dt
if body._sleep_timer >= time_threshold:
timed_out.add(handle)
else:
body._sleep_timer = 0.0
if timed_out:
self._sleep_settled_islands(timed_out, contacts)
def _joint_peers(self) -> dict[BodyHandle, set[BodyHandle]]:
"""Each jointed body's constraint neighbours, for the sleep-island walk.
Built on demand rather than maintained: it is read only on a step where some
body's sleep timer ran out, and a jointless world pays a single empty-dict
return for it.
"""
peers: dict[BodyHandle, set[BodyHandle]] = {}
for j in self._joints.values():
peers.setdefault(j.a, set()).add(j.b)
peers.setdefault(j.b, set()).add(j.a)
return peers
def _sleep_settled_islands(self, timed_out: set[BodyHandle], contacts: list[_Contact2D]) -> None:
"""Latch ``asleep`` one contact island at a time (2D).
The timer says a body has stopped; it does not say the body will be left
alone. Wake-on-contact in :meth:`_wake_touching` wakes a sleeper whose
partner is an awake non-STATIC body, so a body that latches ahead of the
pile it is part of is woken again on the next step and its timer restarts.
Whether a stack ever settles then depends on its boxes crossing the timer
on the SAME step, which is coincidence, and a stack that never settles
keeps being solved and rests measurably lower than one that does. So the
latch waits for the island: every DYNAMIC body reachable through this
step's contacts commits together, or none of them does.
The walk crosses DYNAMIC bodies only. ``_touching_by_body`` also holds the
contacts with STATIC and KINEMATIC bodies, and reaching through those would
merge every pile that shares a floor into one island. It is also why the
rule has no effect on a lone box: its island is itself.
It crosses JOINT edges as well as contacts, and for the same reason: a joint
holds its two ends together as firmly as a contact does, and the constraint
wake (:meth:`_wake_moved_constraint_ends`) wakes a sleeping end whose partner
moved, so a link latching ahead of its chain is woken on the next step by the
very neighbours it left behind, having first had its velocity zeroed under
them. That is what kept a rope of four links or more churning between
latching and waking for the life of the scene once its residual velocity was
low enough to time out at all. Every joint kind counts, springs included,
because the wake rule this defends against counts them all too.
A body already asleep counts as settled (it is at rest, and wake-on-contact
has already had its say). A DYNAMIC body whose ``can_sleep`` is False never
qualifies, so it holds its island awake for as long as it is touching it,
which is the price of the guarantee and what Jolt's ``allowSleeping`` costs
there: a stack with one such box in it never sleeps on either backend.
The island is drained to the contact slop in the step it falls asleep and
frozen there (:meth:`_settle_island`), so a pile makes one visible settling
move instead of keeping the penetration it happened to hold at that moment
for the life of the world. An island the projection cannot drain is left
awake and tries again next step, for a bounded number of steps: penetration
the geometry makes unresolvable must not cost a pile its sleep.
Peers are looked up with ``.get``: a body destroyed this step leaves its
edges in the index until the next contact diff retires them.
"""
bodies = self._bodies
touching = self._touching_by_body
jointed = self._joint_peers()
seen: set[BodyHandle] = set()
for seed in timed_out:
if seed in seen:
continue # already walked as part of an earlier seed's island
seen.add(seed)
island = [seed]
frontier = [seed]
settled = True
while frontier:
handle = frontier.pop()
for peer in chain(touching.get(handle, ()), jointed.get(handle, ())):
if peer in seen:
continue
body = bodies.get(peer)
if body is None or body.body_type is not BodyMode.DYNAMIC:
continue # destroyed this step, or the island ends here
seen.add(peer)
island.append(peer)
frontier.append(peer)
if peer not in timed_out and not body.asleep:
settled = False
if not settled:
continue
latching = [h for h in island if not bodies[h].asleep]
if not self._settle_island(latching, contacts):
continue # deeper than the slop, with refusals left: it latches on a later step
for handle in latching:
body = bodies[handle]
body.asleep = True
body.linear_velocity[:] = 0.0 # kill residual jitter velocity
body.angular_velocity = 0.0
def _settle_island(self, latching: list[BodyHandle], contacts: list[_Contact2D]) -> bool:
"""Project a latching island down to the contact slop, or refuse the latch.
A sleeper is immovable for the position pass
(:meth:`_correct_contact_position`), so the pose an island falls asleep in
is the pose it keeps. The velocity solve alone leaves a settled pile a
little inside itself -- the position pass drains that a fraction per step --
so an island that froze the moment its timers ran out would keep whatever
penetration it happened to hold: measured at 0.11 of a box for a six-high
stack, against 0.005 once drained. It is drained here instead, in the step
it falls asleep, and frozen at the drained pose.
Gauss-Seidel over the island's contacts: the same linear projection
:meth:`_correct_contact_position` applies per pass, run to convergence,
with each pass reading the depth the previous passes left. The depths are
re-measured on entry, because the ones this step's contacts carry were taken
before the ordinary correction ran and projecting those would push the
island back out through the ground by what the step had already drained.
Only the bodies about to latch may move: a STATIC or KINEMATIC partner is
immovable by nature, and one that is already asleep is immovable by the
rule this method exists to serve, so a new pile settling against a sleeping
one takes the whole correction itself.
The projection knows nothing about joints: it drains contact penetration
only, so a displacement it writes can leave a joint on a latching body
holding a small violation that stays for the life of the sleep (the joint
position pass treats sleepers as immovable too).
The ceiling is the lower of a per-body pass count and a total projection
budget, so a tall island gets the passes its height needs while a wide one
cannot spend them on its contact count. Both are counts rather than a time
limit, so two runs of the same scene make the same decision.
Refusing is bounded. Some penetration is geometrically unresolvable -- a box
wedged into a gap narrower than itself, one pinned under a ceiling too low
for it -- and no displacement drains it, so an unbounded refusal would keep
that island awake for the life of the world and re-run the whole projection
every step. After ``_SETTLE_REFUSAL_CEILING`` consecutive refusals the
island latches anyway, at the best pose the projection reached: the
projected pose when it is measurably shallower than the one the island
holds, and otherwise the pose it holds, because the projection oscillates
rather than converges when the contacts disagree and its last pose can be
the deeper one.
Returns:
True when the island latched, the displacements written; False when the
projection ran out of passes and the island has refusals left, in which
case it keeps the pose it had, stays awake, and tries again on a later
step, by which time the ordinary per-step correction has drained it
further.
"""
if not contacts or not latching:
self._latch_settled(latching, None)
return True
bodies = self._bodies
movable = set(latching)
shifting = []
for c in contacts:
if c.a not in movable and c.b not in movable:
continue
fresh = self._narrow(c.a, bodies[c.a], c.b, bodies[c.b])
if fresh is not None: # None: the correction already parted this pair
shifting.append(fresh)
if not shifting:
self._latch_settled(latching, None)
return True
offsets = {handle: np.zeros(2, dtype=np.float32) for handle in movable}
still = np.zeros(2, dtype=np.float32)
slop = self.contact_slop
held = max(c.depth for c in shifting) - slop # the deepest the island holds now
ceiling = min(_SETTLE_PASS_CEILING * len(movable), max(1, _SETTLE_WORK_CEILING // len(shifting)))
for _ in range(ceiling):
worst = 0.0
for c in shifting:
inv_a = bodies[c.a].inverse_mass if c.a in movable else 0.0
inv_b = bodies[c.b].inverse_mass if c.b in movable else 0.0
inv_sum = inv_a + inv_b
if inv_sum == 0.0:
continue
shift = offsets.get(c.b, still) - offsets.get(c.a, still)
excess = c.depth - float(np.dot(shift, c.normal)) - slop
if excess <= 0.0:
continue
worst = max(worst, excess)
correction = (excess / inv_sum) * c.normal
if inv_a > 0.0:
offsets[c.a] -= correction * inv_a
if inv_b > 0.0:
offsets[c.b] += correction * inv_b
if worst <= _SETTLE_TOLERANCE:
self._latch_settled(latching, offsets)
return True
refusals = max(bodies[handle]._settle_refusals for handle in latching)
if refusals < _SETTLE_REFUSAL_CEILING:
for handle in latching:
bodies[handle]._settle_refusals = refusals + 1
return False
# Out of refusals: this island is not going to drain. Keep the projected
# pose only where it is shallower than the one the island already holds,
# measured over the same contacts with the offsets in place.
reached = 0.0
for c in shifting:
shift = offsets.get(c.b, still) - offsets.get(c.a, still)
reached = max(reached, c.depth - float(np.dot(shift, c.normal)) - slop)
self._latch_settled(latching, offsets if reached < held else None)
return True
def _latch_settled(self, latching: list[BodyHandle], offsets: dict[BodyHandle, np.ndarray] | None) -> None:
"""Write a settled island's displacements and clear its refusal count.
``offsets`` is None when the island is latching at the pose it already
holds, which is what an undrainable one does (:meth:`_settle_island`).
"""
bodies = self._bodies
if offsets is not None:
for handle, offset in offsets.items():
bodies[handle].position = bodies[handle].position + offset
for handle in latching:
bodies[handle]._settle_refusals = 0
# -- bulk transfer (the keystone) --------------------------------------
[docs]
def register_bodies(self, handles: list[BodyHandle]) -> None:
for h in handles:
if h not in self._bodies:
raise KeyError(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 -------------------------------------------------------
[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_island(handle, 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_island(handle, 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_island(handle, body) # an applied torque is a disturbance: wake a sleeper
body.torque += float(torque)
# -- joints / constraints ----------------------------------------
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
def _joint_ends(self, a: BodyHandle, b: BodyHandle, where: str) -> tuple[_Body2D, _Body2D]:
"""Return both bodies of a joint, or raise ``KeyError`` naming the bad end.
Joint endpoints are caller input, so an unknown handle raises rather than
asserts: ``assert`` is compiled out by ``python -O``, and the constraint
would then be filed against a handle no body answers to. ``KeyError``
matches every other body-handle lookup on this backend.
"""
try:
return self._bodies[a], self._bodies[b]
except KeyError as exc:
raise KeyError(f"{where}: unknown body handle {exc.args[0]!r}") from None
[docs]
def create_fixed_joint(self, a: BodyHandle, b: BodyHandle) -> JointHandle:
ba, bb = self._joint_ends(a, b, "create_fixed_joint")
# Both halves of the captured pose are relative to a's frame, so the weld
# rides round with a rather than holding a fixed world offset.
rel_local = _to_local_2d(ba, bb.position - ba.position)
rel_angle = float(bb.rotation - ba.rotation) # captured relative angle
handle = self._alloc_joint()
j = _FixedConstraint2D(a=a, b=b, rel_local=rel_local, rel_angle=rel_angle)
self._joints[handle] = j
self._wake_joint_ends(j)
return handle
[docs]
def create_pin_joint(self, a: BodyHandle, b: BodyHandle, anchor: Vec2) -> JointHandle:
ba, bb = self._joint_ends(a, b, "create_pin_joint")
anc = _as_array2(anchor)
handle = self._alloc_joint()
j = _PinConstraint2D(
a=a,
b=b,
local_a=_to_local_2d(ba, anc - ba.position),
local_b=_to_local_2d(bb, anc - bb.position),
)
self._joints[handle] = j
self._wake_joint_ends(j)
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.
ba, bb = self._joint_ends(a, b, "create_hinge_joint")
anc = _as_array2(anchor)
handle = self._alloc_joint()
j = _HingeConstraint2D(
a=a,
b=b,
local_a=_to_local_2d(ba, anc - ba.position),
local_b=_to_local_2d(bb, anc - bb.position),
)
self._joints[handle] = j
self._wake_joint_ends(j)
return handle
[docs]
def create_spring_joint(
self, a: BodyHandle, b: BodyHandle, rest_length: float, stiffness: float, damping: float
) -> JointHandle:
ba, bb = self._joint_ends(a, b, "create_spring_joint")
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()
j = _SpringConstraint2D(a=a, b=b, rest_length=rl, stiffness=float(stiffness), damping=float(damping))
self._joints[handle] = j
self._wake_joint_ends(j)
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.
``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 stored exactly as given -- the constraint records keep every arm in
its own body's frame (see :class:`_GrooveConstraint2D`) -- so the rail turns
with ``a``. The groove direction must be non-zero.
"""
self._joint_ends(a, b, "create_groove_joint")
ga = _as_array2(groove_a)
gb = _as_array2(groove_b)
if float(np.linalg.norm(gb - ga)) <= _JOINT_EPS:
raise ValueError(f"groove endpoints must differ, got {tuple(ga)} and {tuple(gb)}")
handle = self._alloc_joint()
j = _GrooveConstraint2D(a=a, b=b, ga=ga, gb=gb, local_b=_as_array2(anchor_b))
self._joints[handle] = j
self._wake_joint_ends(j)
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.
j = self._joints.pop(handle, None)
if j is not None:
# Cutting a tether drops whatever it was holding: both ends are owed a
# wake, or a body left hanging in the air sleeps there for good.
self._wake_joint_ends(j)
# -- one-way platforms -------------------------------------------
[docs]
def set_one_way(self, handle: BodyHandle, enabled: bool, normal: Vec2 = _DEFAULT_UP_2D) -> None:
"""Mark a body as a one-way platform.
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` and :meth:`sweep_body`) 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).
A platform that stops resolving contacts from one side has taken support
away from whatever was resting on that side, so a real change here is a
support-taking mutation like a filter edit or a shape swap: the wake
reaches the body's sleeping neighbours and not only the platform itself.
Without it a settled stack would hang in the air above a platform that no
longer holds it, with no later step able to notice. Any real change takes
the same path, including the off direction and a normal edit, which ADD
support rather than take it: the affected bodies still need a step awake
to settle against the new rule. A write that changes neither the flag nor
the normal takes nothing away and wakes nothing, so a game may drive this
every frame.
"""
body = self._bodies[handle]
n = _as_array2(normal)
length = float(np.linalg.norm(n))
unit = (n / length).astype(np.float32) if length > 1e-9 else _DEFAULT_UP_2D.copy()
changed = bool(enabled) != body.one_way or not np.array_equal(unit, body.one_way_normal)
body.one_way = bool(enabled)
body.one_way_normal = unit
if changed:
self._disturb(handle, body, wake=True, vacated=True)
# -- queries ------------------------------------------------------
[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 ``_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
) -> SweepHit2D | 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 (rejected). Returns ``None`` when the sweep stays clear.
"""
probe_shape = self._shape_rec(shape)
# 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().
if not math.isfinite(max_dist):
raise ValueError("shapecast max_dist must be finite (a swept shape needs a bounded sweep length)")
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 SweepHit2D(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.
"""
probe_shape = self._shape_rec(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 sweep_body(
self,
handle: BodyHandle,
motion: Vec2,
*,
from_transform: tuple[Vec2, float] | None = None,
skin: float = 0.0,
) -> SweepHit2D | None:
"""Substepped, non-mutating sweep of a body's shape (basic tier).
2D sibling of :meth:`~simvx.core.physics.builtin.world.BuiltinPhysics.sweep_body`
and the primitive the collide-and-slide policy is built 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; the substep that penetrates a blocking body brackets
the contact, and the bracket is BISECTED against that blocker alone, so
``distance`` is a bound good to ``|motion| / (substeps * 2**8)`` rather than
to one whole substep. It is still a lower bound and may be exactly ``0.0``
for a sweep that begins in contact. A fast mover vs a very thin collider can
still tunnel between substeps (an analytic sweep is a pymunk concern).
Structurally non-mutating: the mover's shape and pose are wrapped in a
TRANSIENT ``_Body2D`` probe, so nothing in the body table is touched and
``from_transform`` costs nothing. ``skin`` is accepted and IGNORED (the
substep quantum already exceeds any sane skin).
A body blocks only when it is not the mover, is not a sensor, passes the
canonical AND layer/mask rule, is not rejected by the one-way filter, and
presents a contact that OPPOSES the sweep. The one-way test runs BEFORE the
opposition test and honours a one-way MOVER as well as a one-way blocker, so
a body sweeping UP through a one-way platform passes and a top landing is
blocked. Sensors take no part in collision resolution, so one never blocks a
sweep and (because the ground and step-up probes route through here) never
counts as ground or as a step surface. A non-opposing touch, e.g. the floor a
character already rests on while it walks, is a touch and not a blocker:
reporting it would halt every slide at distance ``0``. Fraction ``0`` is
never sampled (the substep loop starts at ``s = 1``), so this backend cannot
report the cast axis as a normal.
"""
body = self._bodies[handle]
m = _as_array2(motion)
dist = float(np.linalg.norm(m))
if dist < 1e-9:
return None
direction = (m / dist).astype(np.float32)
if from_transform is None:
start, rotation = body.position.copy(), body.rotation
else:
start, rotation = self._unpack_transform(from_transform)
probe = _Body2D(
shape=body.shape,
body_type=BodyMode.KINEMATIC,
position=start.copy(),
rotation=rotation,
mass=0.0,
inverse_mass=0.0,
moment=0.0,
inverse_moment=0.0,
collision_layer=body.collision_layer,
collision_mask=body.collision_mask,
one_way=body.one_way,
one_way_normal=body.one_way_normal,
)
feature = max(self._feature_size_2d(body.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 + m * frac).astype(np.float32)
for ho, bo in self._bodies.items():
if ho == handle:
continue
# Skipped inline, not prefiltered into a list: the scan returns on
# the first blocking hit, so a sensor-free world pays nothing.
if bo.is_sensor:
continue
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 mover's relative approach is this
# sweep's motion (the bodies' stored linear velocities do not drive
# a kinematic sweep).
if (probe.one_way or bo.one_way) and self._one_way_rejects(contact, probe, bo, m, bo.linear_velocity):
continue
# a = probe (mover), b = other: negate to point toward the mover.
n = (-contact.normal).astype(np.float32)
if float(np.dot(direction, n)) > -1e-4:
continue
def _blocks_at(f: float, ho: BodyHandle = ho, bo: _Body2D = bo) -> bool:
"""The scan's own acceptance test, re-asked at fraction ``f``.
A closure rather than a copy of the predicate, so the bisect can
only ever converge on the blocker the scan actually found. The
ONE-WAY filter is inside it for the same reason: omitting it
would treat a contact the scan skipped as blocking and converge
on a shorter bound than the real blocker's. The float32 cast on
the probe pose matches the scan's, so the refined answer is
computed at the precision the scan used.
"""
probe.position = (start + m * f).astype(np.float32)
c = self._narrow(probe_handle, probe, ho, bo)
if c is None:
return False
if (probe.one_way or bo.one_way) and self._one_way_rejects(c, probe, bo, m, bo.linear_velocity):
return False
return float(np.dot(direction, (-c.normal).astype(np.float32))) <= -1e-4
# See the 3D twin for the bracket and the refinement count.
lo, hi = prev_frac, frac
for _ in range(_SWEEP_REFINE_STEPS):
mid = 0.5 * (lo + hi)
if _blocks_at(mid):
hi = mid
else:
lo = mid
reached = (start + m * lo).astype(np.float32)
travelled = dist * lo
point = (reached - n * feature).astype(np.float32)
return SweepHit2D(body=ho, point=Vec2(point), normal=Vec2(n), distance=travelled)
prev_frac = frac
return None
__all__ = ["BuiltinPhysics2D"]