"""BuiltinPhysics world: the dependency-free 3D reference solver.
Concrete :class:`~simvx.core.physics.world.PhysicsWorld` implementation, pure
Python (numpy only). It is always present, it needs nothing installed, it runs
everywhere the engine runs including the browser, and it is the behavioural parity
target the seam's cross-backend tests are written against.
What it is adequate for
-----------------------
Spheres and capsules on flat ground, low-density scenes, prototypes, and anything
whose gameplay does not turn on the exact resting pose of a stack. For oriented
boxes, hulls, meshes under load and dense stacking, install ``simvx-physics-jolt``:
installing it makes Jolt the default 3D backend, and ``PhysicsRoot(backend=
"builtin")`` pins this solver back.
Mechanism: semi-implicit (symplectic) Euler integration, a sweep-and-prune broad
phase feeding an analytic narrow phase, and a sequential normal-impulse solver with
Baumgarte positional correction and a small penetration slop. The GEOMETRY is
written for clarity over raw throughput; what is optimised is how OFTEN it runs.
The broad phase proposes only plausible pairs and a pair whose bodies have not
moved replays its last answer, so a settled scene costs almost nothing however many
bodies are in it.
Shape set: sphere, box, capsule, cylinder, convex hull and static triangle mesh.
EXACT
-----
- Every sphere and capsule pairing: capsule contacts reduce to a sphere test at the
closest point(s) of the segments, reusing the sphere depth/normal maths.
- Sphere / capsule / box against a static triangle mesh: closest-point-on-triangle
(Ericson RTCD 5.1.5) per candidate triangle, with the box reporting its OWN
support along the contact direction, so a box rests on a mesh floor at the height
it rests on a box floor. Mesh raycast is Moller-Trumbore.
- Cylinder-sphere and every ray query against a cylinder: analytic finite-cylinder
maths.
- Convex hull boolean overlap: GJK.
- All queries (raycast, shapecast, overlap, sweep_body) as to WHETHER they hit,
subject to the substepping note below.
APPROXIMATE, and named at the call site
---------------------------------------
None of these silently fakes a result; each carries a comment where it is computed.
- Cylinder-box: AABB of the cylinder.
- Cylinder-cylinder and capsule-cylinder: the cylinder is treated as a capsule, so
the rims round off. Cylinders are the honest weak shape here.
- Convex hull penetration depth and normal: EPA-lite, a bounded-iteration expanding
polytope with a search-direction fallback.
- Convex hull raycast: the slab test against the AABB of the point cloud.
- Contact response: one linear impulse at the centre of mass. Rotation is
integrated for round-tripping, but there is no inertia tensor, so an off-centre
impulse never spins a body. The 2D solver DOES carry a real moment of inertia;
see ``world2d.py``.
- Swept queries substep rather than solving a true time of impact, so a reported
``distance`` is a lower bound and a fast enough body can tunnel.
ROTATION IGNORED
----------------
``_box_box``, ``_hull_world_points`` and a mesh body's own orientation in
``_shape_vs_mesh``: each treats its geometry in world axes offset by the body
position. An oriented box against another box, a rotated hull, and a rotated mesh
level are therefore all wrong here.
REFUSED
-------
Convex hull against a triangle mesh. There is no routine for it, and what the pair
returned before it was refused was NO CONTACT, so a hull fell through a mesh floor
in silence. It raises at ``create_body``, ``set_body_shape``, ``set_body_filter``
and at the two query entry points, naming ``simvx-physics-jolt``. Box-vs-mesh is
NOT refused: it is exact (see above).
"""
from __future__ import annotations
import math
from collections.abc import Iterator
from dataclasses import dataclass, field
from itertools import chain
from typing import NamedTuple
import numpy as np
from ...math import Quat, Vec3
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,
BodyHandle,
BodyMode,
ContactEvent,
ContactPhase,
JointHandle,
OverlapEvent,
PhysicsWorld,
RaycastHit,
ShapeHandle,
SweepHit,
body_scale_unchanged,
is_unit_scale,
normalise_body_scale,
normalise_damping,
normalise_gravity_scale,
)
# Solver tuning. Conservative, readable defaults for the basic tier.
_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. Every reference solver carries this cap
# (Box2D ``b2_maxLinearCorrection`` and Jolt ``mMaxPenetrationDistance`` are both
# 0.2 m; Rapier and Box2D v3 cap a corrective speed instead), because without it a
# deeply overlapped spawn is teleported clear in one frame. It is expressed
# against the geometry rather than in absolute units because this engine has no
# world-scale knob: ``contact_slop`` is deliberately small at every scale, so it
# cannot serve as the reference. A unit crate's bounding radius is 0.866, which
# puts the cap at 0.217 and reproduces Box2D's 0.2 m almost exactly; a pixel-scale
# body gets a pixel-scale cap from the same line.
_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
# 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
# Restitution velocity rest-threshold: a contact whose approach speed
# is below this gets e_eff = 0 (no bounce), so bodies settling under gravity (per-
# step approach speeds well under 1 m/s) come to rest cleanly instead of jittering.
# A fast impact (> threshold) keeps full restitution and visibly bounces. Same
# order as box2d / Jolt's restitution velocity threshold. Restitution is now a
# PER-CONTACT combined value, so the old module-level _RESTITUTION constant is gone.
_RESTITUTION_THRESHOLD = 1.0 # m/s
# Joint / constraint solver iteration counts (basic tier). The
# velocity loop solves contacts AND joints together each iteration; the position
# loop is a separate Baumgarte pass for the rigid joints (Pin / Hinge / Fixed).
# These are deliberately small: this is BASIC-tier convergence, so a joint chain
# sags slightly and stiff springs are soft. SliderJoint, motors, angular limits
# and breakable joints are not implemented here.
# Neither count is a constant: the velocity count is the world's
# ``solver_iterations`` knob and the position count its ``position_iterations``,
# the two halves of a sequential-impulse solve a scene can dial independently.
_JOINT_EPS = 1e-9 # degenerate-direction guard (zero-length spring / anchor delta)
# 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). Module-level so the
# velocity loop never allocates it per iteration; the point/position solvers only
# READ it (never mutate), so sharing is safe.
_ZERO3 = np.zeros(3, dtype=np.float32)
# Byte image of that zero vector. Comparing an array's 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.
_ZERO3_BYTES = _ZERO3.tobytes()
# The three body-local unit axes, for turning a box's own frame into world axes.
_AXIS_X = np.array([1.0, 0.0, 0.0], dtype=np.float32)
_AXIS_Y = np.array([0.0, 1.0, 0.0], dtype=np.float32)
_AXIS_Z = np.array([0.0, 0.0, 1.0], dtype=np.float32)
def _zero3() -> np.ndarray:
"""A fresh writable zero vector, for the joint records' carried impulse."""
return np.zeros(3, dtype=np.float32)
# Sleeping thresholds are WORLD knobs (``sleep_velocity_threshold`` /
# ``sleep_time_threshold``), because that is the model the recommended backend has
# and per-body thresholds would have to be emulated everywhere. A DYNAMIC body
# whose linear AND angular speed stay below the velocity threshold for that many
# continuous seconds goes to SLEEP: skipped by _integrate and _solve_contact until
# woken, killing residual jitter on resting stacks and saving solver work.
# 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: it is about
# twice as fast on the loop. Measured crossover on a settling pile; the two paths
# cost the same at six bodies and the sweep pulls away from seven upward.
_BROADPHASE_MIN_BODIES = 7
# Slack added to every world AABB: a fixed part (metres) and a part proportional
# to the body's distance from the origin, because float32 spacing grows with
# magnitude. The bounds are rounded into a float32 table, so a hair of error either
# way is possible; a broadphase may only ever over-report, never miss a contact.
_BOUNDS_SLACK = 1e-4
_BOUNDS_SLACK_SCALE = 1e-6 # ~8 float32 ulps, so the rounding can never outrun it
def _all_pairs(n: int) -> Iterator[tuple[int, int]]:
"""Every ``i < j`` index pair, in the order the plain nested loop visits them."""
for i in range(n):
for j in range(i + 1, n):
yield i, j
@dataclass(slots=True)
class _MeshData:
"""Precomputed static triangle-mesh geometry (kind ``"mesh"``).
Built once at :meth:`BuiltinPhysics.create_mesh`. Holds the triangle soup plus
per-triangle face normals and AABBs for the basic-tier linear broadphase cull
(a BVH is deferred to Jolt). Degenerate (zero-area) triangles get a zeroed
normal and are skipped by the narrow / ray phases via a guarded check.
Attributes:
vertices: ``(M, 3)`` float32 vertex positions (mesh-local, == world here:
the basic tier ignores mesh rotation, offsetting only by body position).
tris: ``(T, 3)`` int64 triangle vertex indices.
tri_normals: ``(T, 3)`` float32 unit face normals (cross of two edges,
normalised); zero for a degenerate triangle.
tri_lo: ``(T, 3)`` float32 per-triangle AABB minimum corner.
tri_hi: ``(T, 3)`` float32 per-triangle AABB maximum corner.
aabb_lo: ``(3,)`` float32 overall mesh AABB minimum.
aabb_hi: ``(3,)`` float32 overall mesh AABB maximum.
"""
vertices: np.ndarray
tris: np.ndarray
tri_normals: np.ndarray
tri_lo: np.ndarray
tri_hi: np.ndarray
aabb_lo: np.ndarray
aabb_hi: np.ndarray
# ``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 _Shape:
"""Internal collision shape.
``kind`` discriminates the geometry; ``params`` holds it analytically as a
single float32 array whose layout depends on the kind:
- ``"sphere"``: ``params = [radius]``
- ``"box"``: ``params = [hx, hy, hz]`` (half-extents)
- ``"capsule"``: ``params = [radius, half_len]`` where
``half_len = max(0.0, height / 2 - radius)`` is the Y-axis central-segment
half-length. The segment endpoints are ``centre +- [0, half_len, 0]``;
``half_len == 0`` means the capsule behaves as a sphere of ``radius``.
- ``"cylinder"``: ``params = [radius, half_height]`` where
``half_height = height / 2`` (flat caps at ``+-half_height`` on Y).
- ``"hull"``: ``params = [hx, hy, hz]`` local AABB half-extents of the point
cloud (used by ``_feature_size`` + broadphase culling + hull raycast).
The cloud itself lives in :attr:`hull_points`.
- ``"mesh"``: ``params = [hx, hy, hz]`` overall mesh AABB half-extents (broad
culling). The triangle data lives in :attr:`mesh`. STATIC-ONLY.
``hull_points`` is set only for ``"hull"``; ``mesh`` only for ``"mesh"``; both
are ``None`` for the four analytic primitives.
``local_lo`` / ``local_hi`` are the shape's own AABB corners in body-local
space, and are set only for the two CLOUD kinds (``"hull"``, ``"mesh"``),
whose extent cannot be read off ``params`` alone (their cloud need not be
centred on the body origin). The broadphase reads them so it never has to
re-reduce a point cloud per step; the analytic kinds derive their bounds from
``params`` directly.
"""
kind: str # "sphere" | "box" | "capsule" | "cylinder" | "hull" | "mesh"
params: np.ndarray # float32 analytic parameters (layout per `kind`, see docstring)
hull_points: np.ndarray | None = None # kind == "hull": (N, 3) float32 point cloud
mesh: _MeshData | None = None # kind == "mesh": precomputed triangle data
local_lo: np.ndarray | None = None # cloud kinds: (3,) float32 local AABB minimum
local_hi: np.ndarray | None = None # cloud kinds: (3,) float32 local AABB maximum
@dataclass(slots=True)
class _Body:
"""Internal rigid body.
Pose and velocity are stored as plain float32 ``(3,)`` numpy arrays so the
integrator and solver can do vector maths without per-op ``Vec3`` wrapping.
``inverse_mass`` is 0 for ``STATIC`` / ``KINEMATIC`` (infinite mass), so a
single impulse formula handles every body-type pairing.
"""
# ``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: _Shape
body_type: BodyMode
position: np.ndarray # float32 (3,)
orientation: Quat
mass: float
inverse_mass: float
unscaled_shape: _Shape | None = None
scale: np.ndarray = field(default_factory=lambda: np.ones(3, dtype=np.float32))
linear_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float32))
angular_velocity: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float32))
# Continuous force/torque accumulators. Filled by apply_force /
# apply_torque, consumed as acceleration in _integrate, then cleared each
# step so a continuous force must be re-added per fixed step.
force: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float32))
torque: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float32))
# Layer/mask filtering (defaulted so transient probe bodies need not pass them).
collision_layer: int = 0x00000001
collision_mask: int = 0xFFFFFFFF
# Sensor (trigger) flag. A sensor participates in the broadphase but is
# EXCLUDED from collision resolution (gated out in _collide before _narrow,
# so no _Contact, no _resolve, no contact event, no bulk impulse) and instead
# feeds the SEPARATE one-directional overlap-event stream. Defaulted False so
# transient probe bodies in _make_probe stay non-sensors.
is_sensor: bool = False
# Surface material coefficients, stored verbatim from create_body.
# The contact solver combines a pair's values per-contact (see _solve_contact).
# Defaulted to match PhysicsMaterial() so transient _make_probe bodies need
# not pass them (parity with collision_layer / is_sensor).
friction: float = 0.5
restitution: float = 0.0
friction_combine: CombineMode = CombineMode.AVERAGE
restitution_combine: CombineMode = CombineMode.AVERAGE
# Per-body dynamics. Damping is the rate at which a body sheds motion with
# nothing touching it (``v *= max(0, 1 - damping * dt)`` in _integrate);
# gravity_scale multiplies the world gravity for this body alone. Defaulted to
# the seam values so a transient _make_probe body matches an ordinary one.
linear_damping: float = DEFAULT_LINEAR_DAMPING
angular_damping: float = DEFAULT_ANGULAR_DAMPING
gravity_scale: float = DEFAULT_GRAVITY_SCALE
# CCD flag. When True, _integrate sweeps this body's centre
# displacement vs STATIC bodies and clamps to the TOI (anti-tunnelling).
# Defaulted False so existing bodies and transient _make_probe probes are
# discrete and unchanged.
continuous: bool = False
# Sleeping. asleep DYNAMIC bodies are skipped by _integrate and the
# contact velocity solve until woken. _sleep_timer accumulates dt while both
# speeds stay below the world's thresholds; reaching the time one sets asleep. A
# sleeping DYNAMIC body is still a full collider and still scatter-registered
# (it reports its frozen pose / zero velocity). STATIC bodies are infinite-mass
# and simply never integrated: they are NEVER 'asleep' (sleeping() returns False
# for them), they were never awake, 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. Defaulted awake
# (asleep=False, timer 0.0) so transient probes and every existing body behave
# exactly as before.
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
def _layers_match(layer_a: int, mask_a: int, layer_b: int, mask_b: int) -> bool:
"""Canonical body-body pair-acceptance test (AND rule).
A pair (a, b) collides iff BOTH bodies opt in to the other:
``(mask_a & layer_b) and (mask_b & layer_a)``. Symmetric, and matches
Box2D / Rapier. Replaces the prior OR convention (where one body opting in
was enough). ``bool(int and int)`` yields the correct boolean. One-directional
SENSOR / query filtering is a separate convention and lives in the
raycast / shapecast / overlap paths, not here.
"""
return bool((mask_a & layer_b) and (mask_b & layer_a))
#: Shape kinds this backend refuses to pair with a triangle mesh, because it has
#: no narrow-phase routine for the pair and returns NO CONTACT rather than an
#: approximate one. Box-vs-mesh is NOT here: it is approximate at the call site
#: (the box's real support against the triangle plane) and is named as such.
_REFUSED_VS_MESH = ("hull",)
#: The authoring names of the refused kinds, for the error text.
_SHAPE_CLASS_NAMES = {"hull": "ConvexHullShape3D", "mesh": "ConcaveMeshShape3D"}
def _refused_pair_message(kind: str) -> str:
"""The refusal text for ``kind`` against a triangle mesh."""
return (
f"BuiltinPhysics cannot report a correct contact for a {_SHAPE_CLASS_NAMES.get(kind, kind)} "
"against a ConcaveMeshShape3D: the pair is refused rather than silently returning no contact. "
'Install simvx-physics-jolt and select it (PhysicsRoot(backend="jolt") or physics_backend="jolt"), '
"or give the two bodies non-overlapping collision layers."
)
def _at_rest(body: _Body) -> bool:
"""True when the body's linear velocity is exactly ``+0.0`` on all three axes.
A settled body reaches this state precisely: :meth:`BuiltinPhysics._update_sleeping`
writes the zeros when it puts a body to sleep, a STATIC body is created with
them and never integrated, and the contact solve drives a resting pair's
accumulated impulse to zero so nothing perturbs them again. Two bodies in that
state have no relative motion to cancel, which the contact solve exploits.
Deliberately a BYTE test, not ``any()``: it must exclude ``-0.0`` so the
arithmetic it stands in for is a no-op down to the sign of every zero.
"""
return body.linear_velocity.tobytes() == _ZERO3_BYTES
def _shiftable_mass(body: _Body) -> 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 ``orientation`` 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. This tier uses
inverse MASS as its inverse-inertia stand-in, so the same share governs the
orientation nudges.
"""
return 0.0 if body.asleep else body.inverse_mass
def _as_array(v: object) -> np.ndarray:
"""Coerce a Vec3 / sequence to a float32 ``(3,)`` array (always a copy)."""
return np.array(v, dtype=np.float32).reshape(3)
def _mesh_shape(vertices: np.ndarray, tris: np.ndarray) -> _Shape:
"""Build the ``"mesh"`` shape record (triangle data + bounds) for a vertex soup.
Args:
vertices: ``(N, 3)`` float32 vertex positions; taken as given (the caller
owns the copy).
tris: ``(T, 3)`` int64 triangle indices.
"""
tri_v = vertices[tris] # (T, 3, 3)
# Per-triangle face normals (cross of two edges), zeroed for degenerate
# (zero-area) triangles via a guarded length divide.
raw_n = np.cross(tri_v[:, 1] - tri_v[:, 0], tri_v[:, 2] - tri_v[:, 0])
lengths = np.linalg.norm(raw_n, axis=1, keepdims=True)
tri_normals = np.where(lengths > 1e-12, raw_n / np.where(lengths > 1e-12, lengths, 1.0), 0.0).astype(np.float32)
aabb_lo = vertices.min(axis=0).astype(np.float32)
aabb_hi = vertices.max(axis=0).astype(np.float32)
mesh = _MeshData(
vertices=vertices,
tris=tris,
tri_normals=tri_normals,
tri_lo=tri_v.min(axis=1).astype(np.float32),
tri_hi=tri_v.max(axis=1).astype(np.float32),
aabb_lo=aabb_lo,
aabb_hi=aabb_hi,
)
aabb_half = ((aabb_hi - aabb_lo) * 0.5).astype(np.float32)
return _Shape("mesh", aabb_half, mesh=mesh, local_lo=aabb_lo, local_hi=aabb_hi)
def _scaled_shape(base: _Shape, scale: np.ndarray) -> _Shape:
"""Return ``base`` resized by ``scale``, or ``base`` itself when unscaled.
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 is
what keeps an unscaled world at exactly its previous cost and its previous
numbers: the overwhelming majority of bodies share the one record the shape
table handed out, and the broadphase fingerprint compares records by identity.
The analytic kinds carry MAGNITUDES, so a mirroring (negative) component
scales them by its absolute value; the two cloud kinds are scaled signed,
which mirrors them properly. A mesh mirrored on an odd number of axes has its
winding reversed, and its face normals with it, exactly as the geometry says.
"""
if is_unit_scale(scale):
return base
mag = np.abs(scale)
kind = base.kind
if kind == "sphere":
return _Shape("sphere", (base.params * float(mag[0])).astype(np.float32))
if kind == "box":
return _Shape("box", (base.params * mag).astype(np.float32))
if kind == "capsule":
# [radius, half_len], both along the one uniform factor the seam enforces.
return _Shape("capsule", (base.params * float(mag[0])).astype(np.float32))
if kind == "cylinder":
# [radius, half_height]: radius from the uniform X/Z factor, height from Y.
return _Shape("cylinder", np.array([base.params[0] * mag[0], base.params[1] * mag[1]], dtype=np.float32))
if kind == "hull":
pts = (base.hull_points * scale).astype(np.float32)
lo, hi = pts.min(axis=0), pts.max(axis=0)
return _Shape("hull", ((hi - lo) * 0.5).astype(np.float32), hull_points=pts, local_lo=lo, local_hi=hi)
if kind == "mesh":
assert base.mesh is not None
return _mesh_shape((base.mesh.vertices * scale).astype(np.float32), base.mesh.tris)
raise ValueError(f"unknown shape kind {kind!r}")
def _skew_sq(r: np.ndarray) -> np.ndarray:
"""Square of the cross-product (skew-symmetric) matrix of ``r``: ``[r]_x^2``.
Used by the point-constraint effective-mass matrix. The identity
``[r]_x^2 = r r^T - |r|^2 I`` avoids building the skew matrix. Returns a
float64 ``(3, 3)`` (the constraint solve runs in float64 for conditioning).
"""
rr = r.astype(np.float64)
return np.outer(rr, rr) - float(np.dot(rr, rr)) * np.eye(3, dtype=np.float64)
def _feature_size(shape: _Shape) -> float:
"""Smallest feature size of a shape (sphere radius or min box half-extent).
Used to size sweep substeps so a swept body cannot tunnel a collider thinner
than its own smallest feature in a single substep.
"""
if shape.kind == "sphere":
return float(shape.params[0])
if shape.kind == "capsule":
return float(shape.params[0]) # radius (thinnest feature; segment adds length, not thinness)
if shape.kind == "cylinder":
return float(min(shape.params[0], shape.params[1])) # min(radius, half_height)
if shape.kind == "hull":
return float(np.min(shape.params[:3])) # min local AABB half-extent of the cloud
if shape.kind == "mesh":
# A mesh is STATIC-ONLY, so it is never the MOVING (swept) shape: this
# branch is unreachable on the sweep path. Return a small safe constant so
# a defensive caller never divides by zero.
return 1e-4
# box
return float(np.min(shape.params[:3]))
def _closest_point_on_segment(p: np.ndarray, a: np.ndarray, b: np.ndarray) -> np.ndarray:
"""Closest point to ``p`` on the segment ``[a, b]`` (float32 (3,)).
Degenerate-segment guard: if ``|b - a|^2`` is ~0 the segment is a point and
``a`` is returned (so a degenerate capsule == sphere 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(
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]``.
The standard Ericson (Real-Time Collision Detection, S5.1.9) routine,
handling both-degenerate, one-degenerate, and the parallel ``det ~ 0`` case
with the clamp-then-recompute step. Returns ``(c1, c2)`` (float32 (3,)).
"""
eps = 1e-12
d1 = q1 - p1 # direction of segment 1
d2 = q2 - p2 # direction of segment 2
r = p1 - p2
a = float(np.dot(d1, d1)) # squared length of segment 1
e = float(np.dot(d2, d2)) # squared length of segment 2
f = float(np.dot(d2, r))
if a <= eps and e <= eps:
# Both segments are points.
return p1.astype(np.float32), p2.astype(np.float32)
if a <= eps:
# Segment 1 is a point.
s = 0.0
t = min(1.0, max(0.0, f / e))
else:
c = float(np.dot(d1, r))
if e <= eps:
# Segment 2 is a point.
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
# Clamp t to [0, 1] and recompute s for the new t (clamped to [0, 1]).
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)
def _closest_point_on_aabb(p: np.ndarray, lo: np.ndarray, hi: np.ndarray) -> np.ndarray:
"""Closest point to ``p`` on the axis-aligned box ``[lo, hi]`` (float32 (3,))."""
clipped: np.ndarray = np.clip(p, lo, hi)
return clipped.astype(np.float32)
def _closest_point_on_triangle(p: np.ndarray, a: np.ndarray, b: np.ndarray, c: np.ndarray) -> np.ndarray:
"""Closest point to ``p`` on / inside triangle ``(a, b, c)`` (float32 (3,)).
The standard Ericson (Real-Time Collision Detection S5.1.5) Voronoi-region
barycentric routine: it tests the three vertex regions, the three edge
regions, and finally the face interior. All denominators are guarded so a
degenerate (zero-area / sliver) triangle never divides by zero. Pure numpy.
"""
ab = b - a
ac = c - a
ap = p - a
d1 = float(np.dot(ab, ap))
d2 = float(np.dot(ac, ap))
if d1 <= 0.0 and d2 <= 0.0:
return a.astype(np.float32) # vertex region A
bp = p - b
d3 = float(np.dot(ab, bp))
d4 = float(np.dot(ac, bp))
if d3 >= 0.0 and d4 <= d3:
return b.astype(np.float32) # vertex region B
vc = d1 * d4 - d3 * d2
if vc <= 0.0 and d1 >= 0.0 and d3 <= 0.0:
denom = d1 - d3
v = d1 / denom if abs(denom) > 1e-12 else 0.0
ab_pt: np.ndarray = a + v * ab
return ab_pt.astype(np.float32) # edge region AB
cp = p - c
d5 = float(np.dot(ab, cp))
d6 = float(np.dot(ac, cp))
if d6 >= 0.0 and d5 <= d6:
return c.astype(np.float32) # vertex region C
vb = d5 * d2 - d1 * d6
if vb <= 0.0 and d2 >= 0.0 and d6 <= 0.0:
denom = d2 - d6
w = d2 / denom if abs(denom) > 1e-12 else 0.0
ac_pt: np.ndarray = a + w * ac
return ac_pt.astype(np.float32) # edge region AC
va = d3 * d6 - d5 * d4
if va <= 0.0 and (d4 - d3) >= 0.0 and (d5 - d6) >= 0.0:
denom = (d4 - d3) + (d5 - d6)
w = (d4 - d3) / denom if abs(denom) > 1e-12 else 0.0
bc_pt: np.ndarray = b + w * (c - b)
return bc_pt.astype(np.float32) # edge region BC
# Face interior: project via barycentric coords.
denom_face = va + vb + vc
if abs(denom_face) < 1e-12:
return a.astype(np.float32) # fully degenerate triangle: any vertex
inv = 1.0 / denom_face
v = vb * inv
w = vc * inv
face_pt: np.ndarray = a + ab * v + ac * w
return face_pt.astype(np.float32)
# -- GJK / EPA support-mapped convex collision (hull pairings) ---------------
#
# Convex hull, sphere, box and capsule are all exposed as SUPPORT FUNCTIONS so a
# single GJK / EPA-lite path handles every hull pairing. GJK overlap is EXACT;
# the EPA-lite depth / normal is a bounded-iteration APPROXIMATION with a
# search-direction fallback (documented in _epa_lite). A true robust EPA manifold
# is deferred to the Jolt backend.
#: A direction component this small counts as perpendicular to that axis when
#: judging whether a support point is one point or a whole flat face.
_FLAT_TOL = 1e-3
def _support_cloud(points: np.ndarray, d: np.ndarray) -> np.ndarray:
"""Support point of a point cloud / hull: the vertex maximising ``dot(p, d)``."""
i = int(np.argmax(points @ d))
pt: np.ndarray = points[i]
return pt.astype(np.float32)
def _support_sphere(centre: np.ndarray, radius: float, d: np.ndarray) -> np.ndarray:
"""Support point of a sphere: ``centre + radius * normalise(d)``."""
n = float(np.linalg.norm(d))
if n < 1e-12:
return centre.astype(np.float32)
sphere_pt: np.ndarray = centre + (radius / n) * d
return sphere_pt.astype(np.float32)
def _support_box(centre: np.ndarray, half: np.ndarray, d: np.ndarray) -> np.ndarray:
"""Support point of an axis-aligned box (orientation ignored, basic tier)."""
box_pt: np.ndarray = centre + np.sign(d) * half
return box_pt.astype(np.float32)
def _support_capsule(p0: np.ndarray, p1: np.ndarray, radius: float, d: np.ndarray) -> np.ndarray:
"""Support point of a capsule: support of its segment plus ``radius * dir``.
The segment support is whichever endpoint maximises ``dot(p, d)``; the rounded
surface adds ``radius * normalise(d)``.
"""
base = p0 if float(np.dot(p0, d)) >= float(np.dot(p1, d)) else p1
n = float(np.linalg.norm(d))
if n < 1e-12:
return base.astype(np.float32)
cap_pt: np.ndarray = base + (radius / n) * d
return cap_pt.astype(np.float32)
def _support_cylinder(centre: np.ndarray, radius: float, half_height: float, d: np.ndarray) -> np.ndarray:
"""Support point of a Y-axis cylinder: the flat cap plus the radial rim.
Exact, unlike the capsule stand-in the hull narrow phase uses for cylinders:
the cap is picked by the sign of ``d.y`` and the rim by the direction of
``d`` in the XZ plane, so a cylinder resting on its cap supports at the cap
rather than at a rounded shoulder.
"""
radial = np.array([d[0], 0.0, d[2]], dtype=np.float32)
length = float(np.linalg.norm(radial))
offset = (radial * (radius / length)) if length > 1e-12 else np.zeros(3, dtype=np.float32)
offset[1] = half_height if float(d[1]) >= 0.0 else -half_height
cyl_pt: np.ndarray = centre + offset
return cyl_pt.astype(np.float32)
def _support_body(body: _Body, d: np.ndarray) -> np.ndarray:
"""World support point of a body's effective shape along ``d``.
The farthest point of the body's geometry in direction ``d``. Body rotation
is ignored throughout, matching the narrow phase that produced the contact
this is used to place: the box and the two cloud kinds are supported in their
unrotated local axes, and the capsule and the cylinder are built along world
Y whatever the body is turned to. Only the sphere is exact, because it is the
one kind rotation cannot change.
A mesh is reduced to its own AABB, which is all the broadphase keeps of it,
so on a mesh that is not flat the returned point can float above the surface
-- an AABB corner of a terrain sits at the height of its tallest peak. In
practice the branch is close to unreachable: :func:`_contact_point` takes the
body whose support localises the contact better, and an AABB the size of a
level always loses that comparison to whatever is standing on it. If a caller
ever needs the real surface here, the triangles are on ``shape.mesh``.
"""
shape = body.shape
if shape.kind == "sphere":
return _support_sphere(body.position, float(shape.params[0]), d)
if shape.kind == "box":
return _support_box(body.position, shape.params, d)
if shape.kind == "capsule":
offset = np.array([0.0, float(shape.params[1]), 0.0], dtype=np.float32)
return _support_capsule(body.position - offset, body.position + offset, float(shape.params[0]), d)
if shape.kind == "cylinder":
return _support_cylinder(body.position, float(shape.params[0]), float(shape.params[1]), d)
if shape.kind == "hull":
assert shape.hull_points is not None, "hull body has no point cloud"
return _support_cloud((shape.hull_points + body.position).astype(np.float32), d)
lo, hi = shape.local_lo, shape.local_hi
if lo is None or hi is None:
return _support_box(body.position, shape.params[:3], d)
return _support_box(body.position + (lo + hi) * 0.5, (hi - lo) * 0.5, d)
def _bounding_radius(shape: _Shape) -> float:
"""Radius of a sphere about the body origin that contains the whole shape.
The overall size of a shape, as opposed to :func:`_feature_size`'s thinnest
dimension. Read by :func:`_contact_point` to break a tie between two bodies
whose supports localise their contact equally well, and by
:meth:`BuiltinPhysics._prepare_contact_projections` as the scale the position
pass sizes its per-step drain budget against.
"""
if shape.kind == "sphere":
return float(shape.params[0])
if shape.kind == "capsule":
return float(shape.params[0] + shape.params[1])
if shape.kind == "cylinder":
return float(np.hypot(shape.params[0], shape.params[1]))
if shape.kind in ("hull", "mesh"):
lo, hi = shape.local_lo, shape.local_hi
if lo is not None and hi is not None:
return float(np.linalg.norm(np.maximum(np.abs(lo), np.abs(hi))))
return float(np.linalg.norm(shape.params[:3]))
def _support_spread(shape: _Shape, d: np.ndarray) -> float:
"""How far :func:`_support_body` may miss the contact patch along ``d``.
Zero where the support set is a single point -- a sphere in any direction, a
capsule anywhere but broadside, a cylinder's rim -- because then the support
IS where the two shapes touch. Otherwise the size of the face or edge the
support set spans, over which the returned point is a face centre or an
arbitrarily chosen endpoint: a box supported along its own face normal
returns that face's centre, which for a floor is anywhere up to a half extent
from the body standing on it.
Directions within ``_FLAT_TOL`` of perpendicular to an axis count as flat, so
a hair of numerical tilt in the normal cannot make a face read as a point and
win :func:`_contact_point`'s comparison. It steadies WHICH BODY that
comparison picks and nothing else: :func:`_support_box` is ``np.sign``, which
flips from a face's centre to its corner at a tilt of 1e-9 whatever this
returns, so how far the point can then be from the patch is what the spread
MEASURES rather than what it prevents.
"""
n = float(np.linalg.norm(d))
if n < 1e-12:
return float("inf")
unit = np.abs(d) / n
if shape.kind == "sphere":
return 0.0
if shape.kind == "capsule":
# Segment plus ball: one point unless d is broadside, where the whole
# side line is equally far along d and an endpoint is picked.
return 0.0 if float(unit[1]) > _FLAT_TOL else float(shape.params[1])
if shape.kind == "cylinder":
if float(np.hypot(unit[0], unit[2])) <= _FLAT_TOL:
return float(shape.params[0]) # down the axis: the whole cap
if float(unit[1]) <= _FLAT_TOL:
return float(shape.params[1]) # broadside: the whole rim line
return 0.0
lo, hi = shape.local_lo, shape.local_hi
if shape.kind in ("hull", "mesh") and lo is not None and hi is not None:
half = (hi - lo) * 0.5
else:
half = shape.params[:3]
flat: np.ndarray = np.where(unit <= _FLAT_TOL, np.abs(half), 0.0)
return float(np.linalg.norm(flat))
def _contact_point(ba: _Body, bb: _Body, normal: np.ndarray, depth: float) -> np.ndarray:
"""The one world point that stands for a contact between ``ba`` and ``bb``.
The linear-only narrow phase reports a normal and a depth but no witness
point, so the point is reconstructed from the pair's support mapping: the
surface of whichever body localises the contact better, taken along the
contact normal and pulled back half the penetration so it sits in the middle
of the overlap.
"Localises" is :func:`_support_spread`: a support that is one point lands on
the patch, while a support that is a whole face resolves to that face's centre
(a box) or to one of its vertices (a point cloud), either of which can be an
extent away from where the bodies really meet. A ball on a narrow platform is
placed by the ball, wherever on the platform it rests; a crate on a floor is
placed by the crate. Where neither support is a point -- box on box, hull on
floor -- the flatter body's face is the worse of the two to read, so the point
comes off the less flat one and resolves as above: on that body's surface and
within its own extent of the patch rather than necessarily in the middle of
it. Equal spread goes to the smaller body.
Rotation is ignored for boxes and clouds, as it is in the narrow phase that
produced ``normal``, so this is as approximate as the manifold it describes.
"""
spread_a = _support_spread(ba.shape, normal)
spread_b = _support_spread(bb.shape, -normal)
if abs(spread_a - spread_b) > 1e-6:
take_b = spread_b < spread_a
else:
take_b = _bounding_radius(bb.shape) <= _bounding_radius(ba.shape)
if take_b:
surface = _support_body(bb, -normal) # b's surface toward a, inside a
point: np.ndarray = surface + normal * (0.5 * depth)
else:
surface = _support_body(ba, normal) # a's surface toward b, inside b
point = surface - normal * (0.5 * depth)
return point.astype(np.float32)
def _point_velocity(body: _Body, point: np.ndarray) -> np.ndarray:
"""World velocity of the material point of ``body`` currently at ``point``.
``v + omega x (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.cross(body.angular_velocity, (point - body.position).astype(np.float32))
at_point: np.ndarray = body.linear_velocity + spin
return at_point.astype(np.float32)
def _gjk_overlap(support_a, support_b, *, max_iter: int = 32) -> tuple[bool, list[np.ndarray]]:
"""Boolean GJK overlap of two support-mapped convex sets.
``support_a(d)`` / ``support_b(d)`` return the farthest point of each set
along ``d``. Evolves a Minkowski-difference simplex (point -> line -> triangle
-> tetrahedron) toward the origin. Returns ``(overlap, simplex)`` where the
simplex (Minkowski-difference points) is handed to :func:`_epa_lite` on an
overlap. EXACT for overlap detection (modulo the orientation-ignored box
support). Bounded by ``max_iter`` with a documented bail (returns the current
state) so a pathological input can never loop.
"""
def mink(d: np.ndarray) -> np.ndarray:
m: np.ndarray = support_a(d) - support_b(-d)
return m.astype(np.float32)
d = np.array([1.0, 0.0, 0.0], dtype=np.float32)
simplex: list[np.ndarray] = [mink(d)]
d = (-simplex[0]).astype(np.float32)
for _ in range(max_iter):
if float(np.dot(d, d)) < 1e-18:
return True, simplex # origin on the simplex boundary
a = mink(d)
if float(np.dot(a, d)) < 0.0:
return False, simplex # farthest point did not pass the origin: separated
simplex.append(a)
contains, simplex, d = _gjk_do_simplex(simplex)
if contains:
return True, simplex
return False, simplex # bail: treat as separated (documented iteration cap)
def _gjk_do_simplex(simplex: list[np.ndarray]) -> tuple[bool, list[np.ndarray], np.ndarray]:
"""Process the current GJK simplex, returning ``(contains_origin, simplex, dir)``.
Standard Ericson / van den Bergen simplex routine for line / triangle /
tetrahedron cases; reduces the simplex to its origin-closest sub-feature and
yields the next search direction (toward the origin).
"""
if len(simplex) == 2:
b, a = simplex[0], simplex[1]
ab = b - a
ao = -a
if float(np.dot(ab, ao)) > 0.0:
d = np.cross(np.cross(ab, ao), ab)
return False, [b, a], _safe_dir(d, ao)
return False, [a], ao.astype(np.float32)
if len(simplex) == 3:
return _gjk_triangle(simplex)
return _gjk_tetra(simplex)
def _safe_dir(d: np.ndarray, fallback: np.ndarray) -> np.ndarray:
"""Return ``d`` if non-degenerate, else ``fallback`` (avoids a zero direction)."""
if float(np.dot(d, d)) < 1e-18:
return fallback.astype(np.float32)
return d.astype(np.float32)
def _gjk_triangle(simplex: list[np.ndarray]) -> tuple[bool, list[np.ndarray], np.ndarray]:
c, b, a = simplex[0], simplex[1], simplex[2]
ao = -a
ab = b - a
ac = c - a
abc = np.cross(ab, ac)
if float(np.dot(np.cross(abc, ac), ao)) > 0.0:
if float(np.dot(ac, ao)) > 0.0:
d = np.cross(np.cross(ac, ao), ac)
return False, [c, a], _safe_dir(d, ao)
return _gjk_do_simplex([b, a])
if float(np.dot(np.cross(ab, abc), ao)) > 0.0:
return _gjk_do_simplex([b, a])
if float(np.dot(abc, ao)) > 0.0:
return False, [c, b, a], _safe_dir(abc, ao)
return False, [b, c, a], _safe_dir(-abc, ao)
def _gjk_tetra(simplex: list[np.ndarray]) -> tuple[bool, list[np.ndarray], np.ndarray]:
d, c, b, a = simplex[0], simplex[1], simplex[2], simplex[3]
ao = -a
ab = b - a
ac = c - a
ad = d - a
abc = np.cross(ab, ac)
acd = np.cross(ac, ad)
adb = np.cross(ad, ab)
if float(np.dot(abc, ao)) > 0.0:
return _gjk_do_simplex([c, b, a])
if float(np.dot(acd, ao)) > 0.0:
return _gjk_do_simplex([d, c, a])
if float(np.dot(adb, ao)) > 0.0:
return _gjk_do_simplex([b, d, a])
return True, [d, c, b, a], np.zeros(3, dtype=np.float32) # origin enclosed
# A fixed direction set for the EPA-lite directional MTV sampler: the 6 axes, the
# 8 cube diagonals, and the 12 edge-midpoint directions (the 26 directions of a
# rhombicuboctahedron-ish sampling). Dense enough to recover the correct minimum
# translation axis for boxes / hulls in the common cases, while staying cheap.
def _mtv_directions() -> np.ndarray:
dirs: list[tuple[float, float, float]] = []
for x in (-1.0, 0.0, 1.0):
for y in (-1.0, 0.0, 1.0):
for z in (-1.0, 0.0, 1.0):
if x == 0.0 and y == 0.0 and z == 0.0:
continue
dirs.append((x, y, z))
arr = np.array(dirs, dtype=np.float32)
arr /= np.linalg.norm(arr, axis=1, keepdims=True)
return arr
_MTV_DIRS = _mtv_directions()
def _epa_lite(support_a, support_b, simplex: list[np.ndarray], *, max_iter: int = 24) -> tuple[np.ndarray, float]:
"""EPA-lite penetration depth + normal for two overlapping support-mapped sets.
Returns ``(normal, depth)`` where ``normal`` points in the Minkowski-difference
A->B separating direction (the direction to push A out of B) and ``depth`` is
the penetration along it.
HONEST SCOPE (basic tier): GJK overlap (the caller) is EXACT. This depth /
normal is an APPROXIMATION via a DIRECTIONAL MINIMUM-TRANSLATION-VECTOR (MTV)
sampler, NOT a full expanding-polytope EPA: for each of a fixed set of sampled
unit directions ``d`` (the 26 axis / diagonal / edge directions, plus the GJK
simplex directions) the penetration is ``support_minkowski(d) . d`` (how far
the Minkowski difference extends past the origin along ``d``); the SMALLEST
such positive depth is the MTV. This is robust by construction (no degenerate
polytope faces, never NaN, never loops) and recovers the exact MTV axis for
box / axis-aligned hull overlaps; for oblique hull faces it slightly
over-estimates depth between sampled directions. A true robust EPA contact
manifold is deferred to the Jolt backend. ``depth`` is clamped to ``>= 0`` so
it can never destabilise the solver. ``max_iter`` is unused here (kept for
signature stability / a future polytope refinement).
"""
del max_iter # directional sampler is non-iterative; kept for signature stability
def mink(dr: np.ndarray) -> np.ndarray:
m: np.ndarray = support_a(dr) - support_b(-dr)
return m.astype(np.float32)
# Seed the direction set with the fixed sampling plus any non-degenerate GJK
# simplex point directions (they bias toward the true separating axis).
dirs = [d.astype(np.float32) for d in _MTV_DIRS]
for v in simplex:
n = float(np.linalg.norm(v))
if n > 1e-6:
dirs.append((v / n).astype(np.float32))
best_n = np.array([0.0, 1.0, 0.0], dtype=np.float32)
best_d = float("inf")
for d in dirs:
depth = float(np.dot(mink(d), d)) # extent of the Minkowski diff past origin along d
if depth < 0.0:
# Separating axis found: the sets do not actually overlap along d. GJK
# already proved overlap, so this only happens from sampling noise on a
# shallow touch: treat as zero penetration along this direction.
continue
if depth < best_d:
best_d = depth
best_n = d
if not math.isfinite(best_d):
best_d = 0.0
return best_n.astype(np.float32), max(0.0, best_d)
def _quat_to_xyzw(q: Quat) -> tuple[float, float, float, float]:
"""Convert an engine ``Quat`` (w,x,y,z) to scalar-last xyzw for the contract."""
return (q.x, q.y, q.z, q.w)
def _rotate_by(q: Quat, v: np.ndarray) -> np.ndarray:
"""Turn a body-LOCAL ``(3,)`` vector into world axes by the body's orientation.
The standard ``v + 2w(u x v) + 2u x (u x v)`` form for a unit quaternion
``q = (w, u)``, which is what a joint's stored local arm goes through every
time the solver needs its world direction. Kept as a free function over plain
float32 arrays because the solver never wraps its vectors in ``Vec3``.
Written out in scalar Python floats (bit-identical to the equivalent
``numpy`` arithmetic, which also runs in double) rather than as array calls
on three-element vectors: at this size the per-call overhead of ``np.cross``
dwarfs the scalar multiplies, and this is on the joint solver's hot path.
The 2D solver's ``_rotate_2d`` is written the same way.
"""
x, y, z, w = q.x, q.y, q.z, q.w
vx, vy, vz = float(v[0]), float(v[1]), float(v[2])
tx = 2.0 * (y * vz - z * vy)
ty = 2.0 * (z * vx - x * vz)
tz = 2.0 * (x * vy - y * vx)
return np.array(
[
vx + w * tx + (y * tz - z * ty),
vy + w * ty + (z * tx - x * tz),
vz + w * tz + (x * ty - y * tx),
],
dtype=np.float32,
)
def _to_local(q: Quat, v: np.ndarray) -> np.ndarray:
"""Turn a WORLD-axes ``(3,)`` vector into a body-local one (inverse of :func:`_rotate_by`)."""
return _rotate_by(q.inverse(), v)
def _integrate_orientation(q: Quat, omega: np.ndarray, dt: float) -> Quat:
"""Integrate orientation by angular velocity ``omega`` (rad/s) over ``dt``.
Uses the standard quaternion derivative ``q' = 0.5 * omega_quat * q`` followed
by a renormalise. Adequate for the basic tier's small per-step rotations.
"""
wx, wy, wz = float(omega[0]), float(omega[1]), float(omega[2])
if wx == 0.0 and wy == 0.0 and wz == 0.0:
return q
omega_q = Quat(0.0, wx, wy, wz)
dq = omega_q * q
nw = q.w + 0.5 * dt * dq.w
nx = q.x + 0.5 * dt * dq.x
ny = q.y + 0.5 * dt * dq.y
nz = q.z + 0.5 * dt * dq.z
n = (nw * nw + nx * nx + ny * ny + nz * nz) ** 0.5
if n < 1e-12:
return q
return Quat(nw / n, nx / n, ny / n, nz / n)
@dataclass(slots=True)
class _Contact:
"""A resolved contact between two bodies in narrow phase.
``normal`` points from body ``a`` toward body ``b`` (unit length); pushing
``a`` along ``-normal`` and ``b`` along ``+normal`` separates them.
Warm-starting: ``jn`` accumulates the clamped NORMAL impulse
applied across this step's velocity iterations; it is SEEDED from the previous
step's cached value (keyed by the persistent body-pair id) and applied as a
warm-start before the iteration loop, then written back after, so a resting
stack converges in a couple of iterations instead of rebuilding its support
from zero each frame (the convergence win). Only the normal impulse is
warm-started across STEPS: the 3D linear-only basic tier has no feature-stable
manifold points to anchor a friction accumulator's slide DIRECTION across
frames. But the tangential impulse IS accumulated WITHIN a step across the
velocity iterations (``jt_vec``) and the TOTAL is clamped to the Coulomb cone
``mu * jn`` -- exactly like the normal impulse. Solving friction fresh and
clamping it independently each of the 8 velocity iterations (the old form)
applied up to 8x the Coulomb limit, fully arresting tangential motion even when
an applied force exceeded ``mu * jn`` (a grounded body could not be pushed).
``jt_vec`` is NOT warm-started across steps (fresh-zero each step, no stable
slide direction); ``jn`` defaults to zero so a fresh contact warm-starts with
nothing.
"""
a: BodyHandle
b: BodyHandle
normal: np.ndarray # float32 (3,), unit, a -> b
depth: float # penetration depth (> 0)
jn: float = 0.0 # accumulated clamped normal impulse (warm-started across steps)
vbias: float = 0.0 # restitution velocity bias, computed ONCE pre-solve
# accumulated tangential (friction) impulse, within-step (Coulomb-clamped total)
jt_vec: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float32))
# Position-pass state, fixed once per step by _prepare_contact_projections.
# ``depth_datum`` lets each pass re-measure the depth from the bodies' LIVE
# centres (``depth_datum - dot(pb - pa, normal)``) instead of re-running the
# narrowphase, which is exact because the position pass only translates along
# the normal. ``drain_budget`` is the separation this contact may still apply
# this step, decremented as it is spent. ``share_a`` / ``share_b`` are the
# inverse-mass split of that separation, resolved once because sleep and body
# mode cannot change inside the position loop.
depth_datum: float = 0.0
drain_budget: float = 0.0
share_a: float = 0.0
share_b: float = 0.0
# -- joint / constraint records ----------------------------------
#
# One @dataclass per joint kind, stored in BuiltinPhysics._joints keyed by an
# opaque JointHandle from a SEPARATE counter (_next_joint) so joint handles never
# alias body handles. Anchors and hinge axes are stored in each body's OWN frame,
# captured at create time, and turned back into world axes by that body's current
# orientation every time the solver reads them (:func:`_rotate_by`) -- the
# formulation every production solver uses, and what makes a pin on a spinning
# body orbit with it instead of hanging in the world axes it was built in. All
# angular terms use ``inverse_mass`` as the inverse-inertia scalar (the basic
# tier has no inertia tensor), and the narrowphase still treats most shapes as
# unrotated: joint anchoring is exact here, colliding geometry is not.
@dataclass(slots=True)
class _FixedConstraint:
"""Weld: lock the full relative transform of ``b`` in ``a``'s frame.
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 orientation (``rel_quat``,
the rotation taking a's frame to b's frame, i.e.
``a.orientation.inverse() * b.orientation``). Both are frame-relative, so the
whole assembly follows ``a`` when ``a`` turns. The point part is solved with
the arm ``a.orientation * rel_local`` on ``a`` and no arm on ``b`` (the target
point IS b's centre); the angular part drives the full relative angular
velocity to zero plus a Baumgarte bias toward ``rel_quat``.
"""
a: BodyHandle
b: BodyHandle
rel_local: np.ndarray # float32 (3,), b_centre - a_centre in a's frame at create
rel_quat: Quat # a.orientation.inverse() * b.orientation captured at create
impulse: np.ndarray = field(default_factory=_zero3) # point impulse carried between steps
@dataclass(slots=True)
class _PinConstraint:
"""Point-to-point: keep the two world anchors coincident, rotation free.
``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 + orientation * local``, so it turns with the body.
"""
a: BodyHandle
b: BodyHandle
local_a: np.ndarray # float32 (3,), anchor in a's frame at create
local_b: np.ndarray # float32 (3,), anchor in b's frame at create
impulse: np.ndarray = field(default_factory=_zero3) # point impulse carried between steps
@dataclass(slots=True)
class _HingeConstraint:
"""Pin at an anchor + an angular lock leaving one free DOF about the hinge axis.
Point part identical to :class:`_PinConstraint` (``local_a`` / ``local_b``).
The angular part keeps the two per-body hinge axes (``axis_a`` / ``axis_b``,
unit, each stored in its own body's frame) parallel, locking the two off-axis
rotational DOF. The free DOF is measured about ``a``'s current world axis, so
a hinge whose post turns carries its axis round with it.
"""
a: BodyHandle
b: BodyHandle
local_a: np.ndarray # float32 (3,), anchor in a's frame at create
local_b: np.ndarray # float32 (3,), anchor in b's frame at create
axis_a: np.ndarray # float32 (3,), unit hinge axis in a's frame at create
axis_b: np.ndarray # float32 (3,), unit hinge axis in b's frame at create
impulse: np.ndarray = field(default_factory=_zero3) # point impulse carried between steps
@dataclass(slots=True)
class _SpringConstraint:
"""Soft distance-spring between the two body CENTRES (compliant, not rigid).
Pulls the two COMs 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.
"""
a: BodyHandle
b: BodyHandle
rest_length: float
stiffness: float
damping: float
# Union of the four constraint records. All four carry ``.a`` / ``.b`` body
# handles (the common fields the destroy purge keys on).
_Constraint = _FixedConstraint | _PinConstraint | _HingeConstraint | _SpringConstraint
# 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 and is filtered out before
# the rigid loop. Naming the subset keeps that precondition in the signature
# rather than only in the comprehension that enforces it.
_RigidConstraint = _FixedConstraint | _PinConstraint | _HingeConstraint
class _JointFrame(NamedTuple):
"""A rigid joint's stored local vectors turned into world axes, for one solve phase.
``r_a`` / ``r_b`` are the two anchor arms (a weld's are ``a``'s captured offset
and no arm on ``b``, its constrained point being ``b``'s centre); ``axis_a`` /
``axis_b`` are a hinge's two axes, and the zero vector for the kinds that have
none. Built by :meth:`BuiltinPhysics._joint_frame` from the bodies' CURRENT
orientation, 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 frame each pass.
"""
r_a: np.ndarray
r_b: np.ndarray
axis_a: np.ndarray = _ZERO3
axis_b: np.ndarray = _ZERO3
[docs]
class BuiltinPhysics(PhysicsWorld):
"""Pure-Python default backend (basic tier).
See :class:`~simvx.core.physics.world.PhysicsWorld` for the full contract.
"""
def __init__(self, *, gravity: Vec3) -> None:
super().__init__(gravity=gravity)
self._shapes: dict[ShapeHandle, _Shape] = {}
self._bodies: dict[BodyHandle, _Body] = {}
self._order: list[BodyHandle] = []
self._next_handle: int = 0
# Collision-event diffing. ``_touching`` is the set of
# currently-overlapping body pairs as canonical ``(min, max)`` handle
# keys, carried across steps; ``_contact_events`` is the per-step buffer
# drained by ``drain_contact_events``. Empty for non-overlapping scenes.
self._touching: set[tuple[BodyHandle, BodyHandle]] = set()
self._contact_events: list[ContactEvent] = []
# ``_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 SEPARATE stream from contacts.
# ``_overlapping`` is the set of currently-overlapping sensor pairs as
# DIRECTED ``(sensor, other)`` keys (NOT canonicalised: sensor-vs-sensor
# tracks each direction independently), carried 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[OverlapEvent] = []
# Joints / constraints. A SEPARATE handle counter so a joint
# handle never aliases a body handle. Empty for jointless scenes (zero
# solver cost). A constraint record is one of the four _*Constraint
# dataclasses.
self._joints: dict[JointHandle, _Constraint] = {}
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, ...]] = {}
# Warm-start cache: persistent accumulated contact impulse keyed
# by the canonical body-pair id. 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: a single dict get/set per contact).
self._warm_contacts: dict[tuple[BodyHandle, BodyHandle], 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`` are ever 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 sweep
# sorts, 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], _Contact] = {}
self._pose_keys: dict[BodyHandle, tuple] = {}
self._bounds_lo: np.ndarray = np.zeros((0, 3), dtype=np.float32)
self._bounds_hi: np.ndarray = np.zeros((0, 3), dtype=np.float32)
self._bounds_epoch: int = -1
self._body_epoch: int = 0
# True while any body carries a triangle mesh: box-vs-mesh CULLS triangles
# by the box's bounding sphere, so a box needs the wider bound only then.
self._mesh_present: bool = False
# Live mesh bodies and their filters, so the refused-pair check is an
# empty-dict test in the ordinary case (no mesh in the world) and a scan
# over one or two entries otherwise, rather than over the body table.
self._mesh_bodies: dict[BodyHandle, tuple[int, int]] = {}
[docs]
def capabilities(self) -> frozenset[Capability]:
"""Advertise the measured contact impulse and continuous collision.
This backend owns its sequential-impulse solver, so it reads the
converged normal impulse straight off the last velocity iteration and
puts it in the contact-event payload: ``CONTACT_IMPULSE`` is a real
measurement here, not an estimate. ``CONTINUOUS`` is honoured by
``_integrate``, which sweeps a flagged body's centre displacement against
STATIC geometry and clamps it to the time of impact. It advertises no
cross-platform determinism (its float maths is reproducible on one machine
only), no vehicles and no soft bodies. ``SLEEP`` is honoured by
``_update_sleeping``, which parks a settled DYNAMIC body and skips it in
integrate and solve until something disturbs it. ``SENSOR_DETECTS_STATIC``
is honoured because ``_collide_sensors`` sweeps a sensor against every
body its mask admits rather than against a moving set, so a trigger over
level geometry reports it. Explicit (not inherited) so the claim is a
deliberate, tested promise, not an accident of the default.
"""
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.world.PhysicsWorld.clear`. Resets the body /
joint tables and the per-step edge-diff + warm-start caches so a
re-populated world starts from a clean broadphase state. Gravity, shapes, and
the handle counters are intentionally NOT reset (shapes are reusable
resources; monotonic handles keep freed handles from aliasing a live one).
"""
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._mesh_bodies.clear()
self._mesh_present = False
self._body_epoch += 1
def _alloc_handle(self) -> int:
h = self._next_handle
self._next_handle += 1
return h
def _refuse_unsupported_mesh_pair(
self, kind: str, layer: int, mask: int, *, exclude: BodyHandle | None = None
) -> None:
"""Refuse a pair this backend cannot report a correct contact for.
Hull-vs-mesh has no narrow-phase routine here: it would need closest-point
over the hull's SURFACE, and what the pair returns instead is no contact at
all, so a hull falls through a mesh floor. That is a silently wrong answer
rather than an approximate one, and this solver refuses it;
``simvx-physics-jolt`` does it properly.
Only a pair that could actually MEET is refused: the canonical AND
layer/mask rule decides, so a hull filtered away from the level mesh is
legal and stays legal. That makes layer separation a sanctioned escape
hatch, which is why every entry point that can re-join the two layers
(:meth:`create_body`, :meth:`set_body_shape`, :meth:`set_body_filter`)
calls this: without the third, a user who separates the layers to satisfy
the raise and then toggles a bit back is silently on the wrong answer again.
Args:
kind: Shape kind the body will carry after the call.
layer: Collision layer it will carry.
mask: Collision mask it will carry.
exclude: The body being mutated, so it is never tested against itself.
Raises:
ValueError: If the resulting world would hold a refused pair.
"""
if kind in _REFUSED_VS_MESH:
for mesh_handle, (m_layer, m_mask) in self._mesh_bodies.items():
if mesh_handle != exclude and _layers_match(layer, mask, m_layer, m_mask):
raise ValueError(_refused_pair_message(kind))
elif kind == "mesh":
# ``shape`` rather than ``unscaled_shape``: scaling never changes a
# record's kind, so the effective geometry answers this, and it is
# the field every body carries (``unscaled_shape`` is None whenever
# the two are the same record).
for handle, body in self._bodies.items():
if handle == exclude or body.shape.kind not in _REFUSED_VS_MESH:
continue
if _layers_match(layer, mask, body.collision_layer, body.collision_mask):
raise ValueError(_refused_pair_message(body.shape.kind))
# -- shapes -------------------------------------------------------------
def _store_shape(self, shape: _Shape) -> 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_sphere(self, radius: float) -> ShapeHandle:
if not radius > 0.0:
raise ValueError(f"sphere radius must be > 0, got {radius}")
return self._store_shape(_Shape("sphere", np.array([radius], dtype=np.float32)))
[docs]
def create_box(self, half_extents: Vec3) -> ShapeHandle:
he = _as_array(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(_Shape("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(_Shape("capsule", np.array([radius, half_len], dtype=np.float32)))
[docs]
def create_cylinder(self, radius: float, height: float) -> ShapeHandle:
if not (radius > 0.0 and height > 0.0):
raise ValueError(f"cylinder radius/height must be > 0, got {radius}, {height}")
return self._store_shape(_Shape("cylinder", np.array([radius, height * 0.5], dtype=np.float32)))
[docs]
def create_convex_hull(self, points: np.ndarray) -> ShapeHandle:
pts = np.asarray(points, dtype=np.float32).reshape(-1, 3)
if pts.shape[1] != 3:
raise ValueError(f"hull points must be (N, 3), got {pts.shape}")
if pts.shape[0] < 4:
raise ValueError(f"convex hull needs >= 4 points, got {pts.shape[0]}")
# Local AABB half-extents of the cloud: feeds _feature_size, broadphase
# culling, and the (approximate) hull raycast.
lo = pts.min(axis=0)
hi = pts.max(axis=0)
aabb_half = ((hi - lo) * 0.5).astype(np.float32)
return self._store_shape(
_Shape("hull", aabb_half, hull_points=pts.copy(), local_lo=lo.copy(), local_hi=hi.copy())
)
[docs]
def create_mesh(self, vertices: np.ndarray, indices: np.ndarray) -> ShapeHandle:
verts = np.asarray(vertices, dtype=np.float32).reshape(-1, 3)
idx = np.asarray(indices, dtype=np.int64).reshape(-1)
if idx.size == 0 or idx.size % 3 != 0:
raise ValueError(f"mesh indices must be a non-zero multiple of 3, got {idx.size}")
if int(idx.min()) < 0 or int(idx.max()) >= verts.shape[0]:
raise ValueError(f"mesh index out of range: {int(idx.min())}..{int(idx.max())} over {verts.shape[0]} verts")
return self._store_shape(_mesh_shape(verts.copy(), idx.reshape(-1, 3).copy()))
[docs]
def destroy_shape(self, shape: ShapeHandle) -> None:
"""Release this world's record of a shape handle.
See :meth:`~simvx.core.physics.world.PhysicsWorld.destroy_shape`. There is
nothing native to free here: the record is simply dropped from the shape
table. Bodies built from it hold their own ``_Shape`` record directly and
are unaffected. Unknown handles are a silent no-op.
"""
self._shapes.pop(shape, None)
def _shape_rec(self, shape: ShapeHandle) -> _Shape:
"""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 -------------------------------------------------------------
[docs]
def create_body(
self,
shape: ShapeHandle,
body_type: BodyMode,
transform: object,
*,
mass: float = 1.0,
scale: Vec3 | 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)
# Triangle meshes are STATIC-ONLY colliders (every serious engine, incl.
# Jolt, enforces this): a mesh has no inertia and is level geometry. This
# check is the single choke point and catches both the node path
# (PhysicsBody3D.on_enter_tree) and any direct backend test.
if shp.kind == "mesh" and body_type is not BodyMode.STATIC:
raise ValueError(
f"ConcaveMeshShape3D (triangle mesh) is a STATIC-only collider; got body_type={body_type}. "
"Use a primitive or convex hull 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}")
# Refused pairs are checked AFTER the shape-kind and mass invariants, so a
# caller with two problems is told about the more fundamental one first.
self._refuse_unsupported_mesh_pair(shp.kind, collision_layer, collision_mask)
# 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, orientation = self._unpack_transform(transform)
if body_type is BodyMode.DYNAMIC:
inv_mass = 1.0 / mass
else:
# STATIC and KINEMATIC are treated as infinite mass.
inv_mass = 0.0
body_scale = np.ones(3, dtype=np.float32) if scale is None else normalise_body_scale(scale, shp.kind)
surface = DEFAULT_PHYSICS_MATERIAL if material is None else material
handle = self._alloc_handle()
self._bodies[handle] = _Body(
shape=_scaled_shape(shp, body_scale),
unscaled_shape=shp,
scale=body_scale,
body_type=body_type,
position=position,
orientation=orientation,
mass=mass,
inverse_mass=inv_mass,
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,
)
if shp.kind == "mesh":
self._mesh_bodies[handle] = (collision_layer, collision_mask)
# A new body invalidates the broadphase row layout; the next collide pass
# rebuilds it (and re-derives whether the world holds a mesh).
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. A body can
# legitimately be gone already -- e.g. ``clear()`` emptied the world and the
# owning node's ``on_exit_tree`` then destroys its (now-stale) handle during
# teardown. Asserting here crashed that ordering; a no-op is correct.
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)
self._mesh_bodies.pop(handle, None)
# The broadphase row layout is now stale (and the world may have lost its
# last mesh); 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, and the next
# step's diff is what owes it: the body is already out of ``_bodies``, so
# it cannot appear in that step's live set, and both diffs rewrite their
# set from the live set. A destroyed pair therefore survives exactly one
# step, reports one EXIT, and is gone. The EXIT names a handle that no
# longer exists; like every EXIT its payload is degenerate, so the handle
# is an identity to match against, never something to query the world with.
# Silently drop any joint referencing the destroyed body: a joint must
# never solve against a freed body if the node teardown order leaves it
# briefly alive. This is the load-bearing safety net behind
# remove_joint's no-op-on-unknown contract: the joint node's own
# on_exit_tree then idempotently no-ops.
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: Vec3 | None = None, wake: bool = True
) -> None:
body = self._bodies[handle]
position, orientation = 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 = orientation != body.orientation or not np.array_equal(body.position, position)
# The node layer states its scale on every pose write, so the common case
# is a scale that has not moved. Settle that with a byte compare before
# paying to validate a value the body already holds.
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(scale, base.kind)
if not np.array_equal(new_scale, body.scale):
body.scale = new_scale
body.shape = _scaled_shape(base, new_scale)
# Resizing can change which bound a box needs and whether the
# world's mesh reach still holds, so the bounds table is stale.
self._body_epoch += 1
moved = True
body.position, body.orientation = position, orientation
self._disturb(handle, body, wake=wake, vacated=moved)
[docs]
def set_body_velocity(
self,
handle: BodyHandle,
linear: Vec3,
angular: Vec3 | None = None,
) -> None:
body = self._bodies[handle]
body.linear_velocity = _as_array(linear)
body.angular_velocity = np.zeros(3, dtype=np.float32) if angular is None else _as_array(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]
# set_body_mode bypasses create_body, so re-assert the mesh static-only
# contract here: a live mesh body can never be flipped to DYNAMIC/KINEMATIC.
if body.shape.kind == "mesh" and mode is not BodyMode.STATIC:
raise ValueError(
f"ConcaveMeshShape3D (triangle mesh) 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
if mode is BodyMode.DYNAMIC:
body.inverse_mass = 1.0 / body.mass
else:
body.inverse_mass = 0.0 # STATIC / KINEMATIC -> infinite mass
# 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)
# A mode flip is a disturbance: by default the body starts awake and takes
# whatever it was holding up with it.
self._disturb(handle, body, wake=wake, vacated=became_dynamic or became_kinematic)
# -- live edits to what create_body was given ---------------------------
[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.
# ``not mass > 0.0`` also rejects NaN.
if not mass > 0.0:
raise ValueError(f"body mass must be > 0, got {mass}")
body = self._bodies[handle]
body.mass = float(mass)
# Only a DYNAMIC body has a finite effective mass; STATIC / KINEMATIC keep
# inverse_mass 0 and pick the new value up at their next DYNAMIC flip.
if body.body_type is BodyMode.DYNAMIC:
body.inverse_mass = 1.0 / body.mass
# 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]
# Layer separation is the sanctioned escape hatch from a refused pair, so
# re-joining the layers has to be refused too or the hatch is a back door.
# Checked before any mutation, like the invariants above.
self._refuse_unsupported_mesh_pair(body.shape.kind, int(collision_layer), int(collision_mask), exclude=handle)
body.collision_layer = int(collision_layer)
body.collision_mask = int(collision_mask)
if handle in self._mesh_bodies:
self._mesh_bodies[handle] = (body.collision_layer, body.collision_mask)
# _collide re-evaluates every pair from scratch each step, so a pair that
# stops matching simply drops out of _touching and the diff reports its
# EXIT. The wake covers both directions of what that costs: a sleeper that
# newly matches a neighbour must be solvable against it on the 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 -- the EXIT alone
# leaves them asleep in mid-air, which no later step recovers.
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
# A sleeper is skipped by the contact velocity solve, so it would keep
# behaving like its old surface until something else woke it.
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 mesh STATIC-only invariant (create_body and
# set_body_mode are the other two): swapping a mesh onto a moving body is
# the same violation from the other side. Checked BEFORE any mutation so a
# rejected swap leaves the body exactly as it was.
if shp.kind == "mesh" and body.body_type is not BodyMode.STATIC:
raise ValueError(
f"ConcaveMeshShape3D (triangle mesh) is a STATIC-only collider; cannot place it on a "
f"body whose mode is {body.body_type}."
)
self._refuse_unsupported_mesh_pair(shp.kind, body.collision_layer, body.collision_mask, exclude=handle)
# 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 mesh check
# above, so a rejected swap leaves the body exactly as it was.
body.scale = normalise_body_scale(body.scale, shp.kind)
body.unscaled_shape = shp
body.shape = _scaled_shape(shp, body.scale)
if shp.kind == "mesh":
self._mesh_bodies[handle] = (body.collision_layer, body.collision_mask)
else:
self._mesh_bodies.pop(handle, None)
# Swapping a mesh in or out changes which bound a box needs, so re-derive
# the whole bounds table on the next collide pass.
self._body_epoch += 1
# The 3D solver has no inertia tensor (apply_impulse documents inverse_mass
# as the stand-in), so there is no per-shape moment to recompute here; the
# 2D backend, which does carry a real scalar moment, recomputes it.
# 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[Vec3, Vec3]:
body = self._bodies[handle]
return Vec3(body.linear_velocity), Vec3(body.angular_velocity)
[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'. See _Body docstring.
return self._bodies[handle].asleep
# -- forces -------------------------------------------------------------
[docs]
def apply_impulse(
self,
handle: BodyHandle,
impulse: Vec3,
*,
at: Vec3 | None = None,
angular: Vec3 | None = None,
) -> 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_array(impulse)
body.linear_velocity = body.linear_velocity + lin * body.inverse_mass
# Basic tier has no inertia tensor: scale angular by inverse_mass as a
# documented stand-in for the inverse inertia (same honesty as the
# linear-only contact solver). Real inertia is a Tier-2 / Jolt concern.
if angular is not None:
body.angular_velocity = body.angular_velocity + _as_array(angular) * body.inverse_mass
if at is not None:
r = _as_array(at) - body.position
body.angular_velocity = body.angular_velocity + np.cross(r, lin) * body.inverse_mass
[docs]
def apply_force(self, handle: BodyHandle, force: Vec3, *, at: Vec3 | 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_array(force)
body.force = body.force + f
if at is not None:
r = _as_array(at) - body.position
body.torque = body.torque + np.cross(r, f)
[docs]
def apply_torque(self, handle: BodyHandle, torque: Vec3) -> 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 = body.torque + _as_array(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.
"""
h = self._next_joint
self._next_joint += 1
return h
def _joint_ends(self, a: BodyHandle, b: BodyHandle, where: str) -> tuple[_Body, _Body]:
"""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(ba.orientation, bb.position - ba.position)
rel_quat = ba.orientation.inverse() * bb.orientation # a-frame -> b-frame
handle = self._alloc_joint()
j = _FixedConstraint(a=a, b=b, rel_local=rel_local, rel_quat=rel_quat)
self._joints[handle] = j
self._wake_joint_ends(j)
return handle
[docs]
def create_pin_joint(self, a: BodyHandle, b: BodyHandle, anchor: Vec3) -> JointHandle:
ba, bb = self._joint_ends(a, b, "create_pin_joint")
anc = _as_array(anchor)
handle = self._alloc_joint()
j = _PinConstraint(
a=a,
b=b,
local_a=_to_local(ba.orientation, anc - ba.position),
local_b=_to_local(bb.orientation, 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: Vec3, axis: Vec3) -> JointHandle:
ba, bb = self._joint_ends(a, b, "create_hinge_joint")
anc = _as_array(anchor)
ax = _as_array(axis)
n = float(np.linalg.norm(ax))
# A degenerate axis is a user error: a hinge with no axis is meaningless.
if n <= _JOINT_EPS:
raise ValueError(f"create_hinge_joint: axis must be non-zero, got {tuple(ax)}")
ax = (ax / n).astype(np.float32)
handle = self._alloc_joint()
j = _HingeConstraint(
a=a,
b=b,
local_a=_to_local(ba.orientation, anc - ba.position),
local_b=_to_local(bb.orientation, anc - bb.position),
axis_a=_to_local(ba.orientation, ax),
axis_b=_to_local(bb.orientation, ax),
)
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:
self._joint_ends(a, b, "create_spring_joint")
handle = self._alloc_joint()
j = _SpringConstraint(
a=a, b=b, rest_length=float(rest_length), stiffness=float(stiffness), damping=float(damping)
)
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. This
# is the SAME silent-drop contract used for touching / overlap pairs, not
# an error-swallowing shim (see the abstract docstring for the rationale),
# so a joint node's on_exit_tree is idempotent in either teardown 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)
@staticmethod
def _unpack_transform(transform: object) -> tuple[np.ndarray, Quat]:
"""Extract (position float32 (3,), orientation Quat) from a flexible input.
Accepts a ``Transform3D``-like (``.position`` plus ``.orientation`` or
``.rotation``, ``.orientation`` preferred), a ``Vec3`` / sequence
(position only, identity orientation), or a ``(position, orientation)``
pair (tuple or list) whose orientation must be a ``Quat``.
The pair form states a complete pose, so a non-``Quat`` orientation
raises ``TypeError`` rather than silently substituting identity. That
includes ``None``: pass ``Quat()`` to mean identity.
"""
pos = getattr(transform, "position", None)
if pos is not None:
rot = getattr(transform, "orientation", None)
if rot is None:
rot = getattr(transform, "rotation", None)
if rot is not None and not isinstance(rot, Quat):
raise TypeError(f"transform orientation must be a Quat, got {rot!r}")
return _as_array(pos), (Quat(rot) if rot is not None else Quat())
if isinstance(transform, (tuple, list)) and len(transform) == 2:
p, r = transform
if not isinstance(r, Quat):
raise TypeError(f"transform orientation must be a Quat (pass Quat() for identity), got {r!r}")
return _as_array(p), Quat(r)
# Bare position (Vec3 / sequence): identity orientation.
return _as_array(transform), Quat()
# -- stepping -----------------------------------------------------------
@staticmethod
def _canon(a: BodyHandle, b: BodyHandle) -> tuple[BodyHandle, BodyHandle]:
"""Stable canonical ordering for a body pair.
Backend handles are ``int`` so ``min``/``max`` give a step-stable key:
the same pair always maps to the same tuple regardless of narrow-phase
ordering, so ENTER/EXIT diffing never thrashes. Canonicalisation lives in
the backend (which knows its handles are ints), not in the abstract
``PhysicsWorld`` interface.
"""
return (a, b) if a <= b else (b, a)
[docs]
def step(self, dt: float) -> None:
# Clear last step's buffered events at the top: a caller that never
# drains still gets a per-step (not accumulating) buffer.
self._contact_events = []
self._overlap_events = []
self._integrate(dt)
contacts = self._collide()
# Live overlap set BEFORE resolve, so manifold geometry is the pre-solve
# contact. Canonical key -> _Contact (one per pair from _collide).
current: dict[tuple[BodyHandle, BodyHandle], _Contact] = {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 contact 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 = _contact_point(ba, bb, c.normal, c.depth)
rel = _point_velocity(bb, point) - _point_velocity(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
# normal impulse lives on the _Contact objects _solve mutated.
self._warm_contacts = {self._canon(c.a, c.b): c.jn for c in contacts}
self._diff_contacts(current, payload, impulses)
# Sensor overlap pass: 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.
# 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()
def _diff_contacts(
self,
current: dict[tuple[BodyHandle, BodyHandle], _Contact],
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.
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 by ``_collide``, so the
filtering is inherited for free.
``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(
ContactEvent(
a=c.a,
b=c.b,
phase=ContactPhase.ENTER,
point=Vec3(point),
normal=Vec3(c.normal),
impulse=float(impulses.get(key, 0.0)),
# Published even though this backend measures the real thing:
# it is the one shared formula, so a game keying an impact
# off it reads a comparable number on a backend that cannot
# measure one. Over the LINEAR difference, not the at-point
# value beside it, so the number does not carry where
# _contact_point happened to land.
impulse_estimate=contact_impulse_estimate(linear, c.normal, ba.inverse_mass, bb.inverse_mass),
rel_velocity=Vec3(rel),
)
)
zero = Vec3(0.0, 0.0, 0.0)
for key in exited:
a, b = key
self._contact_events.append(
ContactEvent(
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).
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 with
the one-directional rule: 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
(that is the collision pass's job).
Sensor-vs-sensor checks BOTH directions independently: each passing
direction emits its own directed key, so one may fire without the other.
The ``inverse_mass == 0`` early-continue from ``_collide`` is NOT applied
here: a static sensor over a static body must still detect (overlap needs
no movable body). Narrow-phase here 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.
"""
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 geometry once per pair.
if self._narrow(ha, ba, hb, bb) is None:
continue
# Emit a directed key per passing detection direction.
if ba.is_sensor and (ba.collision_mask & bb.collision_layer):
live.add((ha, hb))
if bb.is_sensor and (bb.collision_mask & ba.collision_layer):
live.add((hb, ha))
return live
def _diff_overlaps(self, live: set[tuple[BodyHandle, BodyHandle]]) -> None:
"""Diff this step's directed sensor overlaps and buffer edge events.
ENTER = newly overlapping directed keys; EXIT = directed keys that
stopped overlapping. Mirrors :meth:`_diff_contacts` but over DIRECTED
``(sensor, other)`` keys (never canonicalised, or sensor-vs-sensor's two
directions would collapse and thrash). Filtering is inherited from
:meth:`_collide_sensors`.
"""
for sensor, other in live - self._overlapping:
self._overlap_events.append(OverlapEvent(sensor=sensor, other=other, phase=ContactPhase.ENTER))
for sensor, other in self._overlapping - live:
self._overlap_events.append(OverlapEvent(sensor=sensor, other=other, phase=ContactPhase.EXIT))
self._overlapping = live
[docs]
def drain_overlap_events(self) -> list[OverlapEvent]:
events = self._overlap_events
self._overlap_events = []
return events
def _integrate(self, dt: float) -> None:
"""Semi-implicit (symplectic) Euler: velocity first, then position.
Accumulated continuous force/torque (from :meth:`apply_force` /
:meth:`apply_torque`) is folded into the DYNAMIC acceleration here,
BEFORE position integration, then cleared at the end of the step so a
continuous force must be re-added every fixed step. Impulses
(:meth:`apply_impulse`) bypass this path: they mutate velocity directly.
"""
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: not integrated; pose/velocity frozen, still a collider.
# Clear any stray accumulators so a force applied while asleep is
# never silently banked (apply_force / apply_torque always wake).
body.force[:] = 0.0
body.torque[:] = 0.0
continue
# gravity (scaled per body) + accumulated force as acceleration;
# torque -> angular (inverse_mass stand-in for inverse inertia,
# basic tier). Damping is applied to the velocity the body ALREADY
# HAS, before this step's acceleration is added: it is drag on the
# motion the body is carrying, and the acceleration a resting body
# is given is precisely what its contact is about to cancel.
# Damping that too leaves a residue the solver cannot remove, which
# a resting stack then jitters on -- measured at a scale where
# gravity is 980 units/s^2, a four-box stack never sleeps under the
# other order and settles exactly under this one. The two orders are
# identical whenever nothing is accelerating the body, which is the
# case damping exists for.
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
angular = body.angular_velocity * max(0.0, 1.0 - body.angular_damping * dt)
angular += body.torque * body.inverse_mass * dt
body.angular_velocity = angular
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/shallow contact normally. A discrete body writes
# target directly (resting / stacking unchanged).
body.position = self._ccd_advance(body, target) if body.continuous else target
body.orientation = _integrate_orientation(body.orientation, body.angular_velocity, dt)
# Sleep test is a SEPARATE post-solve pass (_update_sleeping in
# step()): a resting body carries a fresh per-step gravity velocity
# HERE that the contact solver cancels later, so the sub-threshold
# test must read the settled (post-solve) velocity, not this one.
elif body.body_type is BodyMode.KINEMATIC:
# Code-moved: integrate its set velocity, immune to gravity/contacts.
body.position = body.position + body.linear_velocity * dt
body.orientation = _integrate_orientation(body.orientation, body.angular_velocity, dt)
# STATIC: never integrated.
# Auto-clear the accumulators for ALL bodies (harmless for the
# infinite-mass ones, which never accept a force): continuous forces
# last exactly one step.
body.force[:] = 0.0
body.torque[:] = 0.0
def _ccd_advance(self, body: _Body, target: np.ndarray) -> np.ndarray:
"""Sweep body centre old->target vs STATIC bodies; clamp to TOI (basic tier).
Anti-tunnelling for a fast DYNAMIC body. Casts a RAY of the body's CENTRE
(origin = old pos, dir = displacement) against STATIC bodies only, expanding
the cast 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 (including restitution / friction / Baumgarte) on
the same step.
Basic-tier honesty: CENTRE ray vs STATIC only. No rotational sweep, no
dynamic-vs-dynamic CCD, AABB-ish narrowphase shared with the rest of the
tier (a fast body grazing a corner can still tunnel that corner). The Jolt
backend honours `continuous` properly (LinearCast vs all bodies).
"""
old = body.position
motion = target - old
dist = float(np.linalg.norm(motion))
if dist < 1e-9:
return target
feature = max(_feature_size(body.shape), 1e-4)
direction = motion / dist
best_toi = dist
best_n: np.ndarray | None = None
for h, other in self._bodies.items():
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._ray_body(h, other, old, direction, dist + feature)
if hit is None:
continue
toi = float(hit.distance) - feature # stop a feature-radius short of the surface
if toi < best_toi:
best_toi = max(0.0, toi)
best_n = _as_array(hit.normal)
if best_n is None:
return target
stopped = old + direction * best_toi
# Zero the velocity component INTO the surface; the discrete solver then
# handles the resting contact (restitution / friction / Baumgarte) 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.astype(np.float32)
def _update_sleeping(self, dt: float, contacts: list[_Contact]) -> None:
"""Post-solve sleep pass over every awake DYNAMIC body.
A body whose SETTLED linear AND angular speed stay below the thresholds for
the world's ``sleep_time_threshold`` goes to sleep (skipped by _integrate and the
contact 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.
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
ang_sq = lin_sq
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 or body.asleep or not body.can_sleep:
continue
slow = (
float(np.dot(body.linear_velocity, body.linear_velocity)) < lin_sq
and float(np.dot(body.angular_velocity, body.angular_velocity)) < ang_sq
)
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[_Contact]) -> None:
"""Latch ``asleep`` one contact island at a time.
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[_Contact]) -> 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.13 of a box for a six-high
stack, against 0.006 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(3, dtype=np.float32) for handle in movable}
still = np.zeros(3, 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
def _disturb(self, handle: BodyHandle, body: _Body, *, 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: _Body) -> 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 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: _Body) -> 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: _Constraint) -> 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: _Body) -> tuple[float, ...]:
"""The body's pose as plain floats: what "has it moved" is decided on.
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.
"""
q = body.orientation
p = body.position
return (float(p[0]), float(p[1]), float(p[2]), q.w, q.x, q.y, q.z)
def _wake_moved_constraint_ends(self, joints: list[_Constraint]) -> None:
"""Wake a sleeping constraint end when the OTHER end has MOVED.
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. 3D twin of the 2D rule, kept identical: they agree on
when a constraint wakes a sleeper, and only the pose they compare differs.
"""
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.
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, ...]] = {}
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]
# -- collision detection (broad + narrow) ------------------------------
def _collide(self) -> list[_Contact]:
"""Broad phase + analytic narrow phase, in the canonical all-pairs order.
Two mechanisms keep a big or a settled scene off the narrow phase, and
NEITHER may change a contact:
- the SWEEP-AND-PRUNE broadphase (:meth:`_candidate_pairs`) proposes only
the pairs whose world AABBs overlap. The bounds are conservative for
every pairing this backend implements (:meth:`_world_bounds`), so a
rejected pair provably had no contact to find.
- the UNCHANGED-PAIR skip reuses the previous step's narrow-phase result
for any pair whose two bodies are bit-identically posed, shaped and
filtered as they were at the previous collide pass. Nothing the narrow
phase reads has changed, so its answer cannot have changed either. This
is what makes a settled (sleeping) pile cost nothing: an asleep body is
not integrated, and once its resting penetration stops moving its pose
stops changing bit-for-bit, so the whole pile stays on this path.
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
impulse, 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.
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)
# Below the crossover the sweep costs more than it saves, and the bounds
# table is left cold (``_bounds_epoch`` is poisoned so it is rebuilt if the
# scene later grows past the crossover).
sweep = n >= _BROADPHASE_MIN_BODIES
unchanged = self._refresh_body_state(items, bounds=sweep)
pairs = self._candidate_pairs(n) if sweep else _all_pairs(n)
previous = self._pair_contacts
current: dict[tuple[BodyHandle, BodyHandle], _Contact] = {}
contacts: list[_Contact] = []
for i, j in pairs:
ha, ba = items[i]
hb, bb = items[j]
# Sensors never produce a collision contact: gate them out BEFORE
# narrowphase so no _Contact is ever made for a pair containing a
# sensor. That alone keeps sensors out of _resolve, the contact-
# event diff, AND the bulk solver (a DYNAMIC sensor is equally
# inert in response). Sensors are handled by the separate
# one-directional overlap pass (_collide_sensors).
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
# Layer/mask filtering: skip non-matching pairs before narrowphase.
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 _Contact(cached.a, cached.b, cached.normal, cached.depth)
else:
contact = self._narrow(ha, ba, hb, bb)
if contact is None:
continue
current[key] = contact
contacts.append(contact)
self._pair_contacts = current
return contacts
def _refresh_body_state(self, items: list[tuple[BodyHandle, _Body]], *, bounds: bool) -> 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
orientation and shape are the same objects, 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.
When ``bounds`` is set, 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.
"""
keys = self._pose_keys
rebuild = bounds and self._bounds_epoch != self._body_epoch
if rebuild:
self._bounds_lo = np.zeros((len(items), 3), dtype=np.float32)
self._bounds_hi = np.zeros((len(items), 3), dtype=np.float32)
self._bounds_epoch = self._body_epoch
self._mesh_present = any(body.shape.kind == "mesh" for _, body in items)
elif not bounds:
self._bounds_epoch = -1 # cold table: rebuild it if the scene grows
unchanged: list[bool] = []
rows: list[int] = []
moved_bounds: list[tuple[float, float, float, float, float, float]] = []
for row, (handle, body) in enumerate(items):
key = (
body.position.tobytes(),
body.orientation,
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 bounds and (rebuild or not same):
rows.append(row)
moved_bounds.append(self._world_bounds(body))
if rows:
block = np.array(moved_bounds, dtype=np.float32)
self._bounds_lo[rows] = block[:, :3]
self._bounds_hi[rows] = block[:, 3:]
return unchanged
def _world_bounds(self, body: _Body) -> tuple[float, float, float, float, float, float]:
"""Conservative world AABB of a body as ``(lo x, y, z, hi x, y, z)``.
Conservative in the terms THIS backend's narrow phase works in, which is
not always the shape a reader pictures:
- a capsule reaches ``radius`` past its segment ends, and a cylinder is
approximated by a capsule of its full half-height by the cylinder
pairings, so both take ``half_len + radius`` on Y;
- a box is orientation-ignored by box / capsule / hull pairings but
orientation-RESPECTING against a sphere, so it takes whichever of the
two is larger, and its bounding-SPHERE reach when the world holds a
triangle mesh (box-vs-mesh reduces the box to that sphere);
- hull and mesh clouds are placed by body position with rotation ignored,
exactly as the narrow phase reads them.
Every bound is widened by ``_BOUNDS_SLACK``, plus a term proportional to how
far from the origin the body is, so the rounding of these corners into a
float32 table can only ever over-report an overlap: a bound a hair too tight
would drop a real contact, which is the one thing a broadphase may not do.
"""
shape = body.shape
kind = shape.kind
params = shape.params
px, py, pz = float(body.position[0]), float(body.position[1]), float(body.position[2])
# Float32 spacing grows with magnitude, so the slack has to as well; the
# relative term is several ulps wide at any coordinate a scene can hold.
slack = _BOUNDS_SLACK + _BOUNDS_SLACK_SCALE * max(abs(px), abs(py), abs(pz))
if kind == "box":
hx, hy, hz = self._box_reach(body)
elif kind == "sphere":
hx = hy = hz = float(params[0])
elif kind in ("capsule", "cylinder"):
radius = float(params[0])
hx = hz = radius
hy = float(params[1]) + radius
else: # hull / mesh: a point cloud that need not be centred on the origin
lo, hi = shape.local_lo, shape.local_hi
assert lo is not None and hi is not None, f"{kind} shape has no local bounds"
return (
px + float(lo[0]) - slack,
py + float(lo[1]) - slack,
pz + float(lo[2]) - slack,
px + float(hi[0]) + slack,
py + float(hi[1]) + slack,
pz + float(hi[2]) + slack,
)
return (
px - hx - slack,
py - hy - slack,
pz - hz - slack,
px + hx + slack,
py + hy + slack,
pz + hz + slack,
)
def _box_reach(self, body: _Body) -> tuple[float, float, float]:
"""Half-extents of a box body's world AABB (see :meth:`_world_bounds`)."""
half = body.shape.params
q = body.orientation
if q.w == 1.0 and q.x == 0.0 and q.y == 0.0 and q.z == 0.0:
reach = (float(half[0]), float(half[1]), float(half[2]))
else:
# Rotated: the sphere-vs-box test works in the box's own frame, so the
# world extent is the oriented box's, |R| . half.
rotated = np.abs(q.to_mat4()[:3, :3]) @ half
reach = (
max(float(half[0]), float(rotated[0])),
max(float(half[1]), float(rotated[1])),
max(float(half[2]), float(rotated[2])),
)
if self._mesh_present:
# Box-vs-mesh culls triangles by the box's bounding sphere, the
# largest support any contact direction can ask it for.
radius = float(np.linalg.norm(half))
return (max(reach[0], radius), max(reach[1], radius), max(reach[2], radius))
return reach
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 axes 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 broadphase 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)
@staticmethod
def _seg_endpoints(body: _Body) -> tuple[np.ndarray, np.ndarray]:
"""Capsule segment endpoints ``(p0, p1) = position +- [0, half_len, 0]``.
Valid for a ``"capsule"`` shape; ``half_len == 0`` (degenerate capsule)
returns the two coincident endpoints (== a sphere centre), which the
closest-point helpers guard against dividing by.
"""
half_len = float(body.shape.params[1])
offset = np.array([0.0, half_len, 0.0], dtype=np.float32)
return (body.position - offset).astype(np.float32), (body.position + offset).astype(np.float32)
def _narrow(self, ha: BodyHandle, ba: _Body, hb: BodyHandle, bb: _Body) -> _Contact | None:
"""Explicit kind-pair dispatch (no silent catch-all).
Every unordered pair is canonicalised to one handler with a ``flip`` flag
(exactly like ``_sphere_box``) so the returned ``_Contact.normal`` always
points from the first body of the ORIGINAL ``(a, b)`` order to the second.
An unknown kind pair asserts rather than silently mis-handling.
"""
ka, kb = ba.shape.kind, bb.shape.kind
# -- sphere/box (existing behaviour, byte-for-byte) --
if ka == "sphere" and kb == "sphere":
return self._sphere_sphere(ha, ba, hb, bb)
if ka == "sphere" and kb == "box":
return self._sphere_box(ha, ba, hb, bb, flip=False)
if ka == "box" and kb == "sphere":
return self._sphere_box(hb, bb, ha, ba, flip=True)
if ka == "box" and kb == "box":
return self._box_box(ha, ba, hb, bb)
# -- capsule pairings (exact segment reductions) --
if ka == "capsule" and kb == "sphere":
return self._capsule_sphere(ha, ba, hb, bb, flip=False)
if ka == "sphere" and kb == "capsule":
return self._capsule_sphere(hb, bb, ha, ba, flip=True)
if ka == "capsule" and kb == "box":
return self._capsule_box(ha, ba, hb, bb, flip=False)
if ka == "box" and kb == "capsule":
return self._capsule_box(hb, bb, ha, ba, flip=True)
if ka == "capsule" and kb == "capsule":
return self._capsule_capsule(ha, ba, hb, bb)
# -- cylinder pairings (sphere exact; rest documented approximations) --
if ka == "cylinder" and kb == "sphere":
return self._cylinder_sphere(ha, ba, hb, bb, flip=False)
if ka == "sphere" and kb == "cylinder":
return self._cylinder_sphere(hb, bb, ha, ba, flip=True)
if ka == "cylinder" and kb == "box":
return self._cylinder_box(ha, ba, hb, bb, flip=False)
if ka == "box" and kb == "cylinder":
return self._cylinder_box(hb, bb, ha, ba, flip=True)
if ka == "cylinder" and kb == "cylinder":
return self._cylinder_cylinder(ha, ba, hb, bb)
if ka == "capsule" and kb == "cylinder":
return self._capsule_cylinder(ha, ba, hb, bb, flip=False)
if ka == "cylinder" and kb == "capsule":
return self._capsule_cylinder(hb, bb, ha, ba, flip=True)
# -- static triangle mesh (always the "other"; mover is a primitive) --
if kb == "mesh" and ka in ("sphere", "capsule", "box"):
return self._shape_vs_mesh(ha, ba, hb, bb, flip=False)
if ka == "mesh" and kb in ("sphere", "capsule", "box"):
return self._shape_vs_mesh(hb, bb, ha, ba, flip=True)
if ka == "mesh" and kb == "mesh":
# Two static meshes never collide (both inverse_mass == 0, already
# filtered in _collide); a sensor pass might still ask. None is correct.
return None
# -- convex hull (GJK overlap + EPA-lite depth, documented above) --
if ka == "hull" and kb == "sphere":
return self._hull_sphere(ha, ba, hb, bb, flip=False)
if ka == "sphere" and kb == "hull":
return self._hull_sphere(hb, bb, ha, ba, flip=True)
if ka == "hull" and kb == "box":
return self._hull_box(ha, ba, hb, bb, flip=False)
if ka == "box" and kb == "hull":
return self._hull_box(hb, bb, ha, ba, flip=True)
if ka == "hull" and kb == "capsule":
return self._hull_capsule(ha, ba, hb, bb, flip=False)
if ka == "capsule" and kb == "hull":
return self._hull_capsule(hb, bb, ha, ba, flip=True)
if ka == "hull" and kb == "cylinder":
return self._hull_cylinder(ha, ba, hb, bb, flip=False)
if ka == "cylinder" and kb == "hull":
return self._hull_cylinder(hb, bb, ha, ba, flip=True)
if ka == "hull" and kb == "hull":
return self._hull_hull(ha, ba, hb, bb)
# Hull-vs-mesh has no routine here: it would need closest-point over the
# hull's SURFACE. The pair is refused at create_body / set_body_shape /
# set_body_filter and at the query entry points, so the only way to reach
# this is a pair the layer/mask rule already excluded -- which _collide
# filters before _narrow -- or a SENSOR pass asking about one. No contact
# is the correct answer for both.
if {ka, kb} == {"mesh", "hull"}:
return None
raise AssertionError(f"_narrow: unhandled shape pair ({ka!r}, {kb!r})")
@staticmethod
def _oriented(ha: BodyHandle, hb: BodyHandle, normal_a_to_b: np.ndarray, depth: float, *, flip: bool) -> _Contact:
"""Build a ``_Contact`` 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 the pair
as ``(hb, ha)`` with the negated normal (mirrors ``_sphere_box``'s flip).
"""
if flip:
return _Contact(hb, ha, (-normal_a_to_b).astype(np.float32), depth)
return _Contact(ha, hb, normal_a_to_b.astype(np.float32), depth)
# -- capsule pairings (segment closest-point reductions) ---------------
def _capsule_sphere(
self, h_cap: BodyHandle, b_cap: _Body, h_sph: BodyHandle, b_sph: _Body, *, flip: bool
) -> _Contact | None:
"""Capsule vs sphere: sphere test at the closest segment point.
Reduces to ``sphere(centre=closest_seg_point, radius=cap.radius)`` vs the
sphere, reusing the sphere-sphere depth/normal maths. ``flip`` records that
the original pair order was (sphere, capsule).
"""
cap_r = float(b_cap.shape.params[0])
sph_r = float(b_sph.shape.params[0])
p0, p1 = self._seg_endpoints(b_cap)
closest = _closest_point_on_segment(b_sph.position, p0, p1)
delta = b_sph.position - closest # capsule(closest) -> sphere
dist = float(np.linalg.norm(delta))
rsum = cap_r + sph_r
if dist >= rsum:
return None
normal = (delta / dist).astype(np.float32) if dist > 1e-9 else np.array([0.0, 1.0, 0.0], dtype=np.float32)
# normal points capsule -> sphere, i.e. canonical first(cap) -> second(sph).
return self._oriented(h_cap, h_sph, normal, rsum - dist, flip=flip)
def _capsule_box(
self, h_cap: BodyHandle, b_cap: _Body, h_box: BodyHandle, b_box: _Body, *, flip: bool
) -> _Contact | None:
"""Capsule vs axis-aligned box (box orientation ignored: basic tier).
Works in box-local space. Finds the segment point nearest the box, the box
point nearest that, then refines the segment point once (one Gauss-Seidel
iteration: enough for the tier, approximate for deep oblique overlaps),
then mirrors ``_sphere_box``'s two cases (outside vs centre-inside-box).
``flip`` records that the original pair order was (box, capsule).
"""
radius = float(b_cap.shape.params[0])
half = b_box.shape.params
p0, p1 = self._seg_endpoints(b_cap)
p0l = p0 - b_box.position # segment in box-local space
p1l = p1 - b_box.position
# Seed: closest segment point to the box centre (origin in local space),
# then the box point nearest it, then one refinement of the segment point.
cseg = _closest_point_on_segment(np.zeros(3, dtype=np.float32), p0l, p1l)
cbox = _closest_point_on_aabb(cseg, -half, half)
cseg = _closest_point_on_segment(cbox, p0l, p1l)
cbox = _closest_point_on_aabb(cseg, -half, half)
delta = cseg - cbox # box(cbox) -> capsule(cseg)
dist = float(np.linalg.norm(delta))
if dist > 1e-9:
if dist > radius:
return None
n_cap_from_box = (delta / dist).astype(np.float32) # box -> capsule
depth = radius - dist
else:
# Segment point inside the box: push out along the least-penetrated axis.
penetration = half - np.abs(cseg)
axis = int(np.argmin(penetration))
sign = 1.0 if cseg[axis] >= 0.0 else -1.0
n_cap_from_box = np.zeros(3, dtype=np.float32)
n_cap_from_box[axis] = sign
depth = radius + float(penetration[axis])
# Canonical pair is (box=first, capsule=second): normal box -> capsule.
# _oriented expects normal first->second; here first==box, second==capsule.
if flip:
# Original order (box, capsule): emit as-is, normal box -> capsule.
return _Contact(h_box, h_cap, n_cap_from_box, depth)
# Original order (capsule, box): emit (capsule, box) with normal cap -> box.
return _Contact(h_cap, h_box, (-n_cap_from_box).astype(np.float32), depth)
def _capsule_capsule(self, ha: BodyHandle, ba: _Body, hb: BodyHandle, bb: _Body) -> _Contact | None:
"""Capsule vs capsule: sphere test at the closest points of the two segments."""
ra = float(ba.shape.params[0])
rb = float(bb.shape.params[0])
a0, a1 = self._seg_endpoints(ba)
b0, b1 = self._seg_endpoints(bb)
c1, c2 = _closest_points_on_segments(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, 0.0], dtype=np.float32)
# normal already points a -> b (a is the first body); no flip.
return _Contact(ha, hb, normal, rsum - dist)
# -- cylinder pairings (sphere exact; box/cylinder approximate) --------
def _cylinder_sphere(
self, h_cyl: BodyHandle, b_cyl: _Body, h_sph: BodyHandle, b_sph: _Body, *, flip: bool
) -> _Contact | None:
"""Cylinder vs sphere: exact closest-point on a finite Y-axis cylinder.
NOT an approximation: classic point-vs-finite-cylinder closest point with
the interior least-penetration fallback (radial wall vs cap face), mirroring
``_sphere_box``'s inside case. ``flip`` records original order (sphere, cyl).
"""
radius = float(b_cyl.shape.params[0])
half_h = float(b_cyl.shape.params[1])
sph_r = float(b_sph.shape.params[0])
rel = (b_sph.position - b_cyl.position).astype(np.float32)
dy = float(rel[1])
clamp_y = min(half_h, max(-half_h, dy))
radial = np.array([rel[0], 0.0, rel[2]], dtype=np.float32)
rdist = float(np.linalg.norm(radial))
cr = radial * (min(1.0, radius / rdist)) if rdist > 1e-9 else np.zeros(3, dtype=np.float32)
closest_local = np.array([cr[0], clamp_y, cr[2]], dtype=np.float32)
delta = rel - closest_local # cylinder surface -> sphere centre
dist = float(np.linalg.norm(delta))
inside = rdist <= radius and abs(dy) <= half_h
if not inside:
if dist >= sph_r:
return None
normal = (delta / dist).astype(np.float32) if dist > 1e-9 else np.array([0.0, 1.0, 0.0], dtype=np.float32)
depth = sph_r - dist
else:
# Sphere centre inside the cylinder: push out along whichever of the
# radial wall or a cap face is least penetrated (same idea as the box
# interior case).
radial_pen = radius - rdist # distance to the side wall
cap_pen = half_h - abs(dy) # distance to the nearer flat cap
if radial_pen <= cap_pen:
if rdist > 1e-9:
normal = (radial / rdist).astype(np.float32)
else:
normal = np.array([1.0, 0.0, 0.0], dtype=np.float32)
depth = sph_r + radial_pen
else:
normal = np.array([0.0, 1.0 if dy >= 0.0 else -1.0, 0.0], dtype=np.float32)
depth = sph_r + cap_pen
# normal points cylinder -> sphere (canonical first(cyl) -> second(sph)).
return self._oriented(h_cyl, h_sph, normal, depth, flip=flip)
def _cylinder_box(
self, h_cyl: BodyHandle, b_cyl: _Body, h_box: BodyHandle, b_box: _Body, *, flip: bool
) -> _Contact | None:
"""Cylinder vs box: APPROXIMATION (deferred to Jolt).
Basic-tier cylinder-vs-box approximates the cylinder by its tight AABB
(half-extents ``[radius, half_height, radius]``) and runs the existing
``_box_box`` overlap. The corners of that AABB over-report contact vs the
round side of the real cylinder; a true cylinder-box clip is deferred to
the Jolt backend. Stable for resting/stacking, which is the tier bar.
"""
radius = float(b_cyl.shape.params[0])
half_h = float(b_cyl.shape.params[1])
aabb_half = np.array([radius, half_h, radius], dtype=np.float32)
proxy_shape = _Shape("box", aabb_half)
proxy = _Body(
shape=proxy_shape,
body_type=b_cyl.body_type,
position=b_cyl.position,
orientation=b_cyl.orientation,
mass=b_cyl.mass,
inverse_mass=b_cyl.inverse_mass,
)
contact = self._box_box(h_cyl, proxy, h_box, b_box) # normal cyl(box-proxy) -> box
if contact is None:
return None
if flip:
return _Contact(h_box, h_cyl, (-contact.normal).astype(np.float32), contact.depth)
return contact
def _cylinder_cylinder(self, ha: BodyHandle, ba: _Body, hb: BodyHandle, bb: _Body) -> _Contact | None:
"""Cylinder vs cylinder: APPROXIMATION (deferred to Jolt).
Approximates BOTH cylinders as capsules (segment ``+-half_height`` on Y,
radius = radius) and runs ``_capsule_capsule``. This rounds the flat rims
(under-reports rim-on-rim corner contact); acceptable for the basic tier,
where cylinders rarely stack rim-on-rim. True cylinder-cylinder is deferred
to the Jolt backend.
"""
ca = self._cylinder_as_capsule_body(ba)
cb = self._cylinder_as_capsule_body(bb)
return self._capsule_capsule(ha, ca, hb, cb)
def _capsule_cylinder(
self, h_cap: BodyHandle, b_cap: _Body, h_cyl: BodyHandle, b_cyl: _Body, *, flip: bool
) -> _Contact | None:
"""Capsule vs cylinder: APPROXIMATION (deferred to Jolt).
Approximates the cylinder as a capsule and runs ``_capsule_capsule`` (same
rim-rounding caveat as cylinder-cylinder). ``flip`` records original order
(cylinder, capsule).
"""
cyl_as_cap = self._cylinder_as_capsule_body(b_cyl)
# Canonical order here is (capsule=first, cylinder=second).
contact = self._capsule_capsule(h_cap, b_cap, h_cyl, cyl_as_cap)
if contact is None:
return None
if flip:
return _Contact(h_cyl, h_cap, (-contact.normal).astype(np.float32), contact.depth)
return contact
@staticmethod
def _cylinder_as_capsule_body(body: _Body) -> _Body:
"""Wrap a cylinder body as a capsule body (segment +-half_height, same radius).
Used by the documented cylinder-cylinder / capsule-cylinder approximations.
The capsule segment half-length is the cylinder half-height (NOT reduced by
radius): this is the deliberate rim-rounding approximation, so the round
caps slightly overshoot the flat ones near the rim.
"""
radius = float(body.shape.params[0])
half_h = float(body.shape.params[1])
cap_shape = _Shape("capsule", np.array([radius, half_h], dtype=np.float32))
return _Body(
shape=cap_shape,
body_type=body.body_type,
position=body.position,
orientation=body.orientation,
mass=body.mass,
inverse_mass=body.inverse_mass,
)
# -- static triangle mesh (the high-value path) ------------------------
def _shape_vs_mesh(
self, h_mover: BodyHandle, b_mover: _Body, h_mesh: BodyHandle, b_mesh: _Body, *, flip: bool
) -> _Contact | None:
"""Moving primitive (sphere / capsule / box) vs a STATIC triangle mesh.
The mesh is always static and the "other" body; the mover is a primitive.
Each candidate triangle (broadphase-culled by per-triangle AABB) is
reduced to a sphere-at-a-point test via closest-point-on-triangle. The
deepest contact across all candidate triangles is returned (one-contact
manifold, like the rest of the basic tier; multi-contact is a Jolt
concern). Ties break by triangle index (stable contact events).
A box reports its own support along the contact direction rather than a
bounding sphere's: ``r = sum_i |half_i * (axis_i . n)|`` over the box's OWN
axes, taken from its orientation the way :meth:`_sphere_box` takes them, so
a box resting on a mesh floor rests ON it. Replacing the box with a sphere
of its diagonal radius reported the penetration of a body much larger than
the box, and the solver drove that fictitious depth to the contact slop: a
unit box floated with its centre at 0.865 above a mesh floor instead of 0.5,
and a spinning one reported a surface speed a third low because the
reconstructed contact point sat a fifth of a box above the surface. World
axes would not do: a bounding sphere is rotation-invariant and a world-axis
projection radius is not, so a box turned 45 degrees would get 0.5 where its
true support is 0.707 and would sink 0.2 into the floor instead.
Honesty (basic tier): the mesh's ROTATION is IGNORED (triangles are taken
in mesh-local space offset only by the body position). The broadphase is a
LINEAR per-triangle-AABB scan (a BVH is deferred to Jolt).
``flip`` records that the original pair order was (mesh, primitive); the
canonical contact is built mover->mesh then flip-corrected, matching
``_capsule_box``.
"""
mesh = b_mesh.shape.mesh
assert mesh is not None, "mesh body has no _MeshData"
box_axes: np.ndarray | None = None # set for a box mover, whose reach is directional
# Mover query reduced to a sphere/segment + radius in MESH-LOCAL space
# (mesh rotation ignored: subtract only the mesh body position).
offset = b_mesh.position
kind = b_mover.shape.kind
if kind == "sphere":
radius = float(b_mover.shape.params[0])
seg0 = (b_mover.position - offset).astype(np.float32)
seg1 = seg0
elif kind == "capsule":
radius = float(b_mover.shape.params[0])
p0, p1 = self._seg_endpoints(b_mover)
seg0 = (p0 - offset).astype(np.float32)
seg1 = (p1 - offset).astype(np.float32)
else: # box: its real support along each contact direction, built below
half = b_mover.shape.params
# The box's own axes as the ROWS of a 3x3, so ``axes @ n`` is the three
# ``axis_i . n`` at once. The bounding-sphere radius is still what the
# broadphase culls by: it is the largest support the box can have, so
# it admits every triangle any direction's support could reach.
box_axes = np.array(
[
_rotate_by(b_mover.orientation, _AXIS_X),
_rotate_by(b_mover.orientation, _AXIS_Y),
_rotate_by(b_mover.orientation, _AXIS_Z),
],
dtype=np.float32,
)
radius = float(np.linalg.norm(half))
seg0 = (b_mover.position - offset).astype(np.float32)
seg1 = seg0
# Broadphase: mover AABB in mesh-local space, cull triangles whose AABB
# does not overlap it. Linear scan; BVH deferred to Jolt.
lo = np.minimum(seg0, seg1) - radius
hi = np.maximum(seg0, seg1) + radius
tri_lo = mesh.tri_lo
tri_hi = mesh.tri_hi
overlap = np.all((tri_hi >= lo) & (tri_lo <= hi), axis=1)
candidates = np.nonzero(overlap)[0]
best_depth = -1.0
best_normal: np.ndarray | None = None
is_segment = bool(np.dot(seg1 - seg0, seg1 - seg0) > 1e-12)
for ti in candidates:
a, b, c = mesh.vertices[mesh.tris[ti]]
face_n = mesh.tri_normals[ti]
if is_segment:
centre, cp = self._closest_segment_triangle(seg0, seg1, a, b, c)
else:
centre = seg0
cp = _closest_point_on_triangle(seg0, a, b, c)
delta = centre - cp # triangle -> mover sample point
dist = float(np.linalg.norm(delta))
if dist >= radius:
continue # outside even the largest support the mover can have
if dist > 1e-6:
normal_local = (delta / dist).astype(np.float32)
else:
# Sample point exactly on the face: use the face normal, oriented
# toward the mover (so the mover is pushed off the correct side).
if float(np.dot(face_n, face_n)) < 1e-12:
continue # degenerate triangle, no usable normal
side = 1.0 if float(np.dot(centre - a, face_n)) >= 0.0 else -1.0
normal_local = (face_n * side).astype(np.float32)
# A sphere or capsule reaches its radius in every direction; a box
# reaches its own support in this one.
reach = radius if box_axes is None else float(np.abs(box_axes @ normal_local) @ half)
if dist >= reach:
continue
depth = reach - dist
# Stable tie-break: strict > keeps the first (lowest-index) triangle.
if depth > best_depth:
best_depth = depth
best_normal = normal_local
if best_normal is None:
return None
# normal_local points mesh -> mover. Canonical contact is mover->mesh, so
# the mover->mesh normal is -normal_local.
mover_to_mesh = (-best_normal).astype(np.float32)
if flip:
# Original order (mesh, mover): emit (mesh, mover), normal mesh->mover.
return _Contact(h_mesh, h_mover, best_normal.astype(np.float32), best_depth)
# Original order (mover, mesh): emit (mover, mesh), normal mover->mesh.
return _Contact(h_mover, h_mesh, mover_to_mesh, best_depth)
def _closest_segment_triangle(
self, p0: np.ndarray, p1: np.ndarray, a: np.ndarray, b: np.ndarray, c: np.ndarray
) -> tuple[np.ndarray, np.ndarray]:
"""Closest pair (segment point, triangle point) between ``[p0, p1]`` and tri.
Standard segment-triangle decomposition (basic tier, documented exact for
the tier): take the minimum over (a) each segment endpoint's closest point
on the triangle, (b) each triangle vertex's closest point on the segment,
and (c) the segment vs each of the three triangle edges. Returns the
closest ``(seg_point, tri_point)`` overall.
"""
best_seg = p0
best_tri = _closest_point_on_triangle(p0, a, b, c)
best_d2 = float(np.dot(best_seg - best_tri, best_seg - best_tri))
def consider(sp: np.ndarray, tp: np.ndarray) -> None:
nonlocal best_seg, best_tri, best_d2
d2 = float(np.dot(sp - tp, sp - tp))
if d2 < best_d2:
best_d2 = d2
best_seg = sp.astype(np.float32)
best_tri = tp.astype(np.float32)
# (a) segment endpoints vs triangle
consider(p1, _closest_point_on_triangle(p1, a, b, c))
# (b) triangle vertices vs segment
for tv in (a, b, c):
consider(_closest_point_on_segment(tv, p0, p1), tv)
# (c) segment vs each triangle edge
for e0, e1 in ((a, b), (b, c), (c, a)):
sp, tp = _closest_points_on_segments(p0, p1, e0, e1)
consider(sp, tp)
return best_seg, best_tri
# -- convex hull (GJK overlap + EPA-lite depth) ------------------------
@staticmethod
def _hull_world_points(body: _Body) -> np.ndarray:
"""World-space hull cloud (rotation IGNORED: cloud + body position)."""
pts = body.shape.hull_points
assert pts is not None, "hull body has no point cloud"
world_pts: np.ndarray = pts + body.position
return world_pts.astype(np.float32)
def _hull_vs_support(
self, h_hull: BodyHandle, b_hull: _Body, h_other: BodyHandle, support_other, *, flip: bool
) -> _Contact | None:
"""Run GJK + EPA-lite between the hull and any support-mapped convex set.
Returns a contact oriented hull->other in canonical order then flip
corrects. GJK overlap is exact; depth/normal is EPA-lite (approximate).
"""
pts = self._hull_world_points(b_hull)
def support_hull(d: np.ndarray) -> np.ndarray:
return _support_cloud(pts, d)
overlap, simplex = _gjk_overlap(support_hull, support_other)
if not overlap:
return None
# EPA normal points A->B i.e. hull->other (push hull out of other).
normal, depth = _epa_lite(support_hull, support_other, simplex)
if depth <= 0.0:
return None
n = float(np.linalg.norm(normal))
normal = (normal / n).astype(np.float32) if n > 1e-9 else np.array([0.0, 1.0, 0.0], dtype=np.float32)
return self._oriented(h_hull, h_other, normal, depth, flip=flip)
def _hull_sphere(
self, h_hull: BodyHandle, b_hull: _Body, h_sph: BodyHandle, b_sph: _Body, *, flip: bool
) -> _Contact | None:
centre = b_sph.position
radius = float(b_sph.shape.params[0])
def support(d: np.ndarray) -> np.ndarray:
return _support_sphere(centre, radius, d)
return self._hull_vs_support(h_hull, b_hull, h_sph, support, flip=flip)
def _hull_box(
self, h_hull: BodyHandle, b_hull: _Body, h_box: BodyHandle, b_box: _Body, *, flip: bool
) -> _Contact | None:
centre = b_box.position
half = b_box.shape.params
def support(d: np.ndarray) -> np.ndarray:
return _support_box(centre, half, d)
return self._hull_vs_support(h_hull, b_hull, h_box, support, flip=flip)
def _hull_capsule(
self, h_hull: BodyHandle, b_hull: _Body, h_cap: BodyHandle, b_cap: _Body, *, flip: bool
) -> _Contact | None:
radius = float(b_cap.shape.params[0])
p0, p1 = self._seg_endpoints(b_cap)
def support(d: np.ndarray) -> np.ndarray:
return _support_capsule(p0, p1, radius, d)
return self._hull_vs_support(h_hull, b_hull, h_cap, support, flip=flip)
def _hull_cylinder(
self, h_hull: BodyHandle, b_hull: _Body, h_cyl: BodyHandle, b_cyl: _Body, *, flip: bool
) -> _Contact | None:
"""Hull vs cylinder: APPROXIMATION (deferred to Jolt).
The cylinder is approximated by a capsule support (segment +-half_height,
radius), rounding the flat rims, then run through GJK / EPA-lite like the
other hull pairings. Same rim-rounding caveat as cylinder-cylinder.
"""
radius = float(b_cyl.shape.params[0])
half_h = float(b_cyl.shape.params[1])
p0 = (b_cyl.position - np.array([0.0, half_h, 0.0], dtype=np.float32)).astype(np.float32)
p1 = (b_cyl.position + np.array([0.0, half_h, 0.0], dtype=np.float32)).astype(np.float32)
def support(d: np.ndarray) -> np.ndarray:
return _support_capsule(p0, p1, radius, d)
return self._hull_vs_support(h_hull, b_hull, h_cyl, support, flip=flip)
def _hull_hull(self, ha: BodyHandle, ba: _Body, hb: BodyHandle, bb: _Body) -> _Contact | None:
pts_b = self._hull_world_points(bb)
def support_b(d: np.ndarray) -> np.ndarray:
return _support_cloud(pts_b, d)
# Canonical order is (a, b); normal points a->b. No flip needed.
return self._hull_vs_support(ha, ba, hb, support_b, flip=False)
@staticmethod
def _sphere_sphere(ha: BodyHandle, ba: _Body, hb: BodyHandle, bb: _Body) -> _Contact | None:
ra = float(ba.shape.params[0])
rb = float(bb.shape.params[0])
delta = bb.position - ba.position
dist = float(np.linalg.norm(delta))
radius_sum = ra + rb
if dist >= radius_sum:
return None
if dist > 1e-9:
normal = (delta / dist).astype(np.float32)
else:
# Coincident centres: pick an arbitrary stable axis.
normal = np.array([0.0, 1.0, 0.0], dtype=np.float32)
return _Contact(ha, hb, normal, radius_sum - dist)
@staticmethod
def _sphere_box(
h_sphere: BodyHandle,
b_sphere: _Body,
h_box: BodyHandle,
b_box: _Body,
*,
flip: bool,
) -> _Contact | None:
"""Sphere vs ORIENTED box (the box's orientation IS respected).
The sphere centre is taken into the box's local frame (via the box's
orientation), the closest point / penetration is solved there as an
axis-aligned box, and the contact normal is rotated back to world. This
makes ramps and tilted platforms (rotated box colliders) collide correctly
-- unlike the support-function path used for box-box / hull / mesh, which is
still orientation-ignored at the basic tier (documented follow-up).
Returns a contact with normal oriented a -> b according to the original
pair order (``flip`` records that the box was the first body).
"""
radius = float(b_sphere.shape.params[0])
half = b_box.shape.params
q = b_box.orientation
world_off = b_sphere.position - b_box.position # box centre -> sphere, world axes
# Into the box's LOCAL axes so the box is axis-aligned there.
lc = q.inverse() * Vec3(float(world_off[0]), float(world_off[1]), float(world_off[2]))
centre = np.array([lc.x, lc.y, lc.z], dtype=np.float32)
# Closest point on the box (box-local, axis-aligned space) to the sphere.
closest = np.clip(centre, -half, half)
delta = centre - closest
dist_sq = float(np.dot(delta, delta))
if dist_sq > radius * radius:
return None
dist = dist_sq**0.5
if dist > 1e-9:
n_local = (delta / dist).astype(np.float32) # box-local, box -> sphere
depth = radius - dist
else:
# Sphere centre inside the box: push out along the least-penetrated axis.
penetration = half - np.abs(centre)
axis = int(np.argmin(penetration))
sign = 1.0 if centre[axis] >= 0.0 else -1.0
n_local = np.zeros(3, dtype=np.float32)
n_local[axis] = sign
depth = radius + float(penetration[axis])
# Rotate the local box->sphere normal back into world axes.
nw = q * Vec3(float(n_local[0]), float(n_local[1]), float(n_local[2]))
n_sphere_from_box = np.array([nw.x, nw.y, nw.z], dtype=np.float32)
# _Contact.normal must point from the first body of the pair to the second.
if flip:
# Original pair was (box=a, sphere=b): normal box -> sphere.
return _Contact(h_box, h_sphere, n_sphere_from_box, depth)
# Original pair was (sphere=a, box=b): normal sphere -> box.
return _Contact(h_sphere, h_box, (-n_sphere_from_box).astype(np.float32), depth)
def _box_box(self, ha: BodyHandle, ba: _Body, hb: BodyHandle, bb: _Body) -> _Contact | None:
"""Axis-aligned box vs box via the separating-axis overlap (basic tier).
Box orientation is ignored (treated as AABBs centred at each body), which
matches the tier's scope and keeps resolution stable for stacked/resting
boxes. The contact normal is the axis of least overlap, unless a deep
overlap has flipped that to the far face and the pair's own history names
the near one (:meth:`_carried_axis`).
"""
half_a = ba.shape.params
half_b = bb.shape.params
delta = bb.position - ba.position # a -> b
overlap = (half_a + half_b) - np.abs(delta)
if np.any(overlap <= 0.0):
return None
axis = int(np.argmin(overlap))
sign = 1.0 if delta[axis] >= 0.0 else -1.0
depth = float(overlap[axis])
carried = self._carried_axis(ha, ba, hb, bb, axis, sign, depth)
if carried is not None:
axis, sign, depth = carried
normal = np.zeros(3, dtype=np.float32)
normal[axis] = sign
return _Contact(ha, hb, normal, depth)
def _carried_axis(
self,
ha: BodyHandle,
ba: _Body,
hb: BodyHandle,
bb: _Body,
axis: int,
sign: float,
depth: float,
) -> tuple[int, float, float] | None:
"""Re-pick the separating axis when a deep overlap has flipped it to the far face.
The 3D twin of the 2D solver's rule, and it exists for the same scene: a
body teleported into its support, or a collider grown through its floor,
is deep enough that the shortest way out is the FAR face, so recovery
pushes it onward along the path it came in by and it leaves under the
level for ever. 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, cached in ``_pair_contacts`` in either key
order and gated on ``_touching`` so a pass-through the filter discarded
carries nothing. 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 nothing is guessed.
Only the box pair carries it. The mesh path narrows every triangle under
one pair key, so its cached normal is not attributable to the triangle
being resolved; the hull family's EPA-lite returns one
minimum-penetration direction with no axis list to re-scan. Both keep the
blind choice, and the module docstring's basic-tier section records it.
The re-scan runs only when the blind choice points AGAINST the carried
normal AND the overlap is past half the smaller box's extent along the
chosen axis; an ordinary resting or sliding contact never reaches that.
Args:
axis: The blind choice's axis index, 0 / 1 / 2.
sign: Its direction along that axis in the canonical a -> b sense.
depth: Its penetration depth.
Returns:
``(axis, sign, depth)`` for the shallowest exit agreeing with the
carried signal, or None to keep the blind choice, including when no
direction agrees.
"""
previous = self._pair_contacts.get((ha, hb))
carried_sign = 1.0
if previous is None:
previous = self._pair_contacts.get((hb, ha))
carried_sign = -1.0 # cached a -> b in the other key order
if previous is None or self._canon(ha, hb) not in self._touching:
return None
carried = previous.normal * carried_sign # in the canonical a -> b sense
if float(carried[axis]) * sign >= 0.0:
return None # the blind choice already agrees with the carried signal
half_a = ba.shape.params
half_b = bb.shape.params
smaller = 2.0 * min(float(half_a[axis]), float(half_b[axis]))
if depth <= 0.5 * smaller:
return None
# Constrained re-scan. Along each world axis the pair can be parted in two
# directions, and each has its own depth: pushing b along +i needs
# ``ha + hb - delta_i``, along -i it needs ``ha + hb + delta_i``. Keep only
# the directions the carried normal agrees with, and take the shallowest.
delta = bb.position - ba.position
best: tuple[int, float, float] | None = None
for i in range(3):
reach = float(half_a[i]) + float(half_b[i])
for candidate in (1.0, -1.0):
if float(carried[i]) * candidate <= 0.0:
continue
d = reach - candidate * float(delta[i])
if best is None or d < best[2]:
best = (i, candidate, d)
return best
# -- collision resolution ----------------------------------------------
def _solve(
self,
contacts: list[_Contact],
dt: float,
impulses: dict[tuple[BodyHandle, BodyHandle], float],
) -> None:
"""Sequential-impulse solve of contacts AND joints, plus position passes.
Basic-tier honesty. Restructure vs the original single-pass
``_resolve``:
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`).
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 (the honest compliant form). It composes with the iterated
contact / hard-joint solve that follows because it only mutates
velocity once up front.
1. Velocity loop, ``solver_iterations`` passes, over contacts + the
RIGID joints (Pin / Hinge / Fixed). Each pass first solves every
contact (:meth:`_solve_contact`), then every rigid joint
(:meth:`_solve_joint`), so they compose and converge. Iterating the
contact solve N times (it was ONE pass before) also improves contact
convergence. The ``impulses`` side-table (for the contact-event
payload) is written only on the LAST velocity pass so it reports a
settled normal impulse.
2. Position loop, ``position_iterations`` passes, over the CONTACTS and
the RIGID joints: a Baumgarte split that pushes the bodies to satisfy
the positional constraint (penetration drained down to the contact
slop, anchor coincidence, orientation lock). Each pass re-measures its
own error from the live poses, which is what makes iterating the
contact half self-limiting rather than an N-fold over-push. Springs
are intentionally compliant and get NO position correction.
Convergence is deliberately a few iterations: joint chains sag slightly
and stiff springs are soft. SliderJoint, motors, limits and breakable
joints are not implemented here.
``impulses`` is a pure side-table for the collision-event payload; it
never feeds back into the solver maths.
"""
joints = list(self._joints.values())
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.
springs = [j for j in joints if isinstance(j, _SpringConstraint)]
rigid = [j for j in joints if not isinstance(j, _SpringConstraint)]
for s in springs:
self._solve_spring_velocity(s, dt)
# Step 0a: capture each contact's 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 (which the solver drives to zero, clamping
# the restitution back out). A slow contact (resting stack) gets zero bias
# via the rest-threshold, so settling is jitter-free; a fast impact keeps
# its bounce.
for c in contacts:
self._prepare_contact_bias(c)
# Step 0b: warm-start. Seed each contact's accumulated impulse
# from last step's cached value (persistent body-pair id) and APPLY it to
# the bodies' velocities before the iteration loop, so a resting stack
# starts near its converged solution and settles in a couple of passes.
# A fresh contact (no cache hit) seeds zero: the common single-impact case
# is unchanged. The cache is rebuilt below from this step's contacts only,
# so a pair that stopped touching is dropped.
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). Each contact
# solve now applies a DELTA impulse against its running accumulator (warm-
# started above), clamping the TOTAL (so warm-starting cannot over-apply).
iterations = self._solver_iterations
last = iterations - 1
# Each rigid joint's arms (and a hinge's axes) 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.
frames = [self._joint_frame(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, frames, 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, frame in zip(rigid, frames, strict=True):
self._solve_joint(j, frame, dt)
# (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. Both drain a POSITION error the velocity solve cannot
# reach, and both converge by iterating, so both run inside the
# ``position_iterations`` loop. What makes iterating the contact half safe
# is that each pass re-measures the depth from the bodies' live centres:
# a contact that has reached the slop asks for nothing on the next pass,
# so a resting body is never pushed out past 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: _Contact) -> None:
"""Compute the restitution velocity bias once, pre-solve.
``c.vbias = -e_eff * vn`` where ``vn`` is the approaching relative normal
velocity 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``, so no perpetual re-launch jitter). The iterated solve
then drives ``vn`` to ``-vbias`` (a target velocity), which keeps the
bounce stable under accumulated-impulse clamping.
A pair at exact rest has no approach speed to measure and keeps the zero
bias a fresh contact is born with, so it is not measured at all.
"""
ba = self._bodies[c.a]
bb = self._bodies[c.b]
if _at_rest(ba) and _at_rest(bb):
return # no approach speed to measure; the bias is already its default 0
vn = float(np.dot(bb.linear_velocity - ba.linear_velocity, c.normal))
if vn >= 0.0:
c.vbias = 0.0
return # separating: no restitution bias
e = _combine(ba.restitution, bb.restitution, ba.restitution_combine, bb.restitution_combine)
e_eff = e if -vn > _RESTITUTION_THRESHOLD else 0.0
c.vbias = -e_eff * vn # >= 0: the post-bounce separating speed target
def _warm_start_contacts(self, contacts: list[_Contact]) -> None:
"""Seed + apply each contact's cached NORMAL impulse.
For every contact this step, look up the previous step's converged normal
impulse ``jn`` under the canonical body-pair id and, if present, seed the
contact's accumulator and APPLY that impulse to the two bodies' velocities
up front (``P = jn * n``). A resting stack thus begins each step already
carrying last step's support impulse, so the iteration loop only has to
correct the small residual instead of rebuilding the whole support from
zero (the convergence win). A fresh contact (no cache hit) leaves ``jn``
at zero, so a first impact / the common non-stacking case warm-starts with
nothing and is unchanged.
Asleep-vs-asleep and double-infinite-mass pairs are skipped here exactly as
:meth:`_solve_contact` skips them, so warm-starting never nudges a body the
solver would not touch. So is a pair that has already rested its impulse
out to zero: seeding zero onto two zero velocities is arithmetic with no
result, and a settled pile is made of nothing else.
"""
if not self._warm_contacts:
return # no resting history (fresh scene): nothing to seed
for c in contacts:
jn = self._warm_contacts.get(self._canon(c.a, c.b))
if jn is None:
continue
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 jn == 0.0 and _at_rest(ba) and _at_rest(bb):
continue # a rested-out pair: seeding zero onto two zero velocities
c.jn = jn
impulse = jn * c.normal
ba.linear_velocity = ba.linear_velocity - impulse * ba.inverse_mass
bb.linear_velocity = bb.linear_velocity + impulse * bb.inverse_mass
def _warm_start_joints(self, rigid: list[_RigidConstraint], frames: list[_JointFrame], scale: float) -> None:
"""Seed + apply each rigid point constraint's impulse from last step.
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 angular locks (the hinge's off-axis pair, the weld's full lock) take no
seed: each is solved exactly in one pass from the current relative spin, with
no accumulation for a seed to carry.
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, frame in zip(rigid, frames, strict=True):
seed = (j.impulse * scale).astype(np.float32)
j.impulse = _zero3()
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 - np.cross(frame.r_a, seed) * ba.inverse_mass
bb.angular_velocity = bb.angular_velocity + np.cross(frame.r_b, seed) * bb.inverse_mass
j.impulse = seed
def _joint_warm_scale(self, dt: float) -> float:
"""How much of last step's joint impulse this step may seed.
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[_Contact]) -> 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.any():
self._wake_island(woken, sleeper)
elif other.body_type is not BodyMode.STATIC:
self._wake_island(woken, sleeper)
def _solve_contact(
self,
c: _Contact,
impulses: dict[tuple[BodyHandle, BodyHandle], float] | None,
) -> None:
"""One sequential VELOCITY pass for a single contact: normal + friction.
Cancels the approaching relative velocity along the a->b normal (plus a
per-contact combined restitution gated by a rest-threshold), then applies
a tangential Coulomb friction impulse opposing any sliding, clamped by
``mu`` times the normal impulse. The Baumgarte position correction is
SEPARATE (the position loop's :meth:`_correct_contact_position`), so this
may be iterated safely. ``impulses`` is non-None only on the final velocity
pass so the event payload reports the settled NORMAL impulse (its
documented meaning is unchanged: friction is never recorded there).
Warm-starting: the NORMAL solve works on the contact's running
accumulator ``c.jn`` (warm-started in :meth:`_warm_start_contacts`). Each
pass computes the DELTA normal impulse needed this iteration, clamps the new
TOTAL non-negative, and applies only the delta. This is the standard Box2D
accumulated-impulse form: identical to the previous from-zero solve for a
first contact (``c.jn`` starts at zero) but lets a warm-started resting
contact converge immediately. Friction stays the original per-pass solve
(recomputed from the current tangential velocity, clamped against ``c.jn``),
NOT warm-started: see the class docstring for why.
Basic-tier honesty: friction is a LINEAR tangential impulse at the centre
of mass only (no inertia tensor, no contact-point lever arm), so it cannot
induce or oppose SPIN: a sliding box decelerates but does not tumble. This
is consistent with the linear-only normal solver; real angular friction is
a Jolt concern.
"""
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 friction yet accumulated: the normal impulse this pass would add is
# -(0 - 0) / inv_sum, 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 c.vbias == 0.0 and c.jt_vec.tobytes() == _ZERO3_BYTES and _at_rest(ba) and _at_rest(bb):
if impulses is not None:
impulses[self._canon(c.a, c.b)] = c.jn
return
n = c.normal
# Delta normal impulse to drive vn to the restitution target ``-c.vbias``
# (the velocity bias captured once pre-solve in _prepare_contact_bias), then
# clamp the running TOTAL non-negative and apply only the delta (Box2D form).
rel_vel = bb.linear_velocity - ba.linear_velocity
vn = float(np.dot(rel_vel, n))
d_jn = -(vn - c.vbias) / inv_sum
new_jn = max(c.jn + d_jn, 0.0)
d_jn = new_jn - c.jn
c.jn = new_jn
if impulses is not None:
impulses[self._canon(c.a, c.b)] = c.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
# Tangential Coulomb friction, accumulated across this step's velocity
# iterations and clamped on the TOTAL (Box2D form, mirroring the normal
# impulse above). Each iteration adds the delta that would cancel the
# current slide, then the ACCUMULATED tangential impulse ``c.jt_vec`` is
# clamped to the cone ``mu * jn`` and only the delta is applied -- so total
# friction never exceeds ``mu * jn`` no matter how many iterations run. The
# old form clamped each iteration independently, applying up to 8x the cap
# and pinning a grounded body even when the applied force exceeded it.
mu = _combine(ba.friction, bb.friction, ba.friction_combine, bb.friction_combine)
max_friction = mu * max(c.jn, 0.0) # Coulomb cap on the TOTAL tangential impulse
if max_friction == 0.0:
return # frictionless surface or no normal support: no friction
rel_vel = bb.linear_velocity - ba.linear_velocity # post-normal
vt_vec = rel_vel - float(np.dot(rel_vel, n)) * n # tangential component
d_jt = -vt_vec / inv_sum # delta impulse to fully cancel the current slide
new_jt = c.jt_vec + d_jt
mag = float(np.linalg.norm(new_jt))
if mag > max_friction: # clamp the ACCUMULATED tangential impulse to the cone
new_jt = new_jt * (max_friction / mag)
applied = new_jt - c.jt_vec # apply only this iteration's delta
c.jt_vec = new_jt
ba.linear_velocity = ba.linear_velocity - applied * ba.inverse_mass
bb.linear_velocity = bb.linear_velocity + applied * bb.inverse_mass
def _prepare_contact_projections(self, contacts: list[_Contact]) -> None:
"""Fix each contact's depth datum and per-step correction budget.
Run once, before the position loop. The datum is what lets every pass
re-measure the penetration from the bodies' live centres
(:meth:`_correct_contact_position`) instead of re-running the narrowphase:
the position pass only translates along the contact normal, so
``depth_datum - dot(pb - pa, n)`` is the exact current depth for as long as
that holds, and it costs one dot product a pass.
The budget is the separation the contact may apply across the whole step,
``_MAX_DRAIN_FRACTION`` of the SMALLER body's bounding radius: the smaller
body is the one a large correction can shove clean through its partner, and
a huge floor must not license a huge push. Bounding radii are memoised on
shape identity for the step, so a pile of a hundred crates sharing one
shape record measures it once.
A pair with nothing to shift (both ends immovable or asleep) is left with
a zero budget, which is the pass's own skip condition.
"""
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(ba.shape)
radius_b = radii.get(id(bb.shape))
if radius_b is None:
radius_b = radii[id(bb.shape)] = _bounding_radius(bb.shape)
c.drain_budget = _MAX_DRAIN_FRACTION * min(radius_a, radius_b)
def _correct_contact_position(self, c: _Contact) -> None:
"""One Baumgarte pass draining this contact's residual penetration.
``_BAUMGARTE`` of the depth beyond the slop, split by inverse mass, capped
by what is left of the step's drain budget. This is the structure every
reference solver runs it in: one modest fraction, applied
``position_iterations`` times against a depth re-measured between passes,
which converges on the true pose instead of leaving two thirds of the
overlap behind. Re-measuring is what makes it safe to iterate: a contact
already at the slop asks for nothing, so a resting body is not pushed out
past its support however many passes run.
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 ----------------------------------
def _solve_joint(self, j: _Constraint, frame: _JointFrame, dt: float) -> None:
"""Dispatch one velocity-level pass for a single RIGID joint.
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).
The rigid joints apply hard point / angular impulses (their position error
is handled separately by :meth:`_solve_joint_position`).
``frame`` carries the joint's arms (and a hinge's axes) already in world
axes; the caller builds it once for the whole velocity loop, which cannot
turn a body under it.
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.
"""
del dt # rigid-joint velocity solve is dt-independent (springs use dt)
if isinstance(j, _PinConstraint | _HingeConstraint):
j.impulse = j.impulse + self._solve_point_velocity(j.a, j.b, frame.r_a, frame.r_b)
if isinstance(j, _HingeConstraint):
self._solve_hinge_angular_velocity(j, frame.axis_a)
elif isinstance(j, _FixedConstraint):
# 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 full angular lock follows.
j.impulse = j.impulse + self._solve_point_velocity(j.a, j.b, frame.r_a, frame.r_b)
self._solve_fixed_angular_velocity(j)
else: # pragma: no cover - only rigid kinds reach here (springs pre-passed)
raise AssertionError(f"unknown rigid joint record {type(j).__name__}")
def _joint_frame(self, j: _Constraint) -> _JointFrame:
"""Build a rigid joint's world-axes vectors from the bodies' current basis.
Every stored vector lives in the frame of the body carrying it, so the
solver needs each of them turned by that body's orientation before it can
use it. That is the whole of the anchoring formulation, and it is why a pin
on a spinning body orbits with it. A weld's constrained point is ``b``'s
centre, so it has an arm on ``a`` and none on ``b``.
"""
ba = self._bodies[j.a]
if isinstance(j, _PinConstraint | _HingeConstraint):
bb = self._bodies[j.b]
r_a = _rotate_by(ba.orientation, j.local_a)
r_b = _rotate_by(bb.orientation, j.local_b)
if isinstance(j, _HingeConstraint):
return _JointFrame(r_a, r_b, _rotate_by(ba.orientation, j.axis_a), _rotate_by(bb.orientation, j.axis_b))
return _JointFrame(r_a, r_b)
if isinstance(j, _FixedConstraint):
return _JointFrame(_rotate_by(ba.orientation, j.rel_local), _ZERO3)
# Only rigid kinds reach here: springs are pre-passed, never dispatched.
raise AssertionError(f"unknown rigid 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:
"""3-DOF point-to-point velocity solve at the two anchor offsets.
Drives the relative velocity at the anchors to zero:
``v_rel = (vb + wb x r_b) - (va + wa x r_a)``. The impulse satisfies
``K J = -v_rel`` where ``K`` is the 3x3 effective-mass matrix
``K = (inv_ma + inv_mb) I - inv_ma skew(r_a)^2 - inv_mb skew(r_b)^2``.
The angular terms use ``inverse_mass`` as the inverse-inertia SCALAR (no
inertia tensor in the basic tier), i.e. each body's inverse
inertia is ``inv_m * I``. Crucially the angular contribution is in the
DENOMINATOR ``K`` (not just the applied torque): folding the cross-coupling
into the effective mass is what keeps an off-centre / spinning anchor
STABLE (a scalar ``1/inv_sum`` denominator over-corrects and diverges).
``J`` is applied linearly to both centres and as a torque ``inv_m (r x J)``
to both spins.
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 _ZERO3 # both infinite-mass: nothing to solve
va = ba.linear_velocity + np.cross(ba.angular_velocity, r_a)
vb = bb.linear_velocity + np.cross(bb.angular_velocity, r_b)
v_rel = vb - va
# K = inv_sum I - inv_ma skew(r_a)^2 - inv_mb skew(r_b)^2. Since
# skew(r)^2 = r r^T - |r|^2 I, this is a small symmetric 3x3 solve.
k = inv_sum * np.eye(3, dtype=np.float64)
k -= ba.inverse_mass * _skew_sq(r_a)
k -= bb.inverse_mass * _skew_sq(r_b)
impulse = np.linalg.solve(k, -v_rel.astype(np.float64)).astype(np.float32)
ba.linear_velocity = ba.linear_velocity - impulse * ba.inverse_mass
bb.linear_velocity = bb.linear_velocity + impulse * bb.inverse_mass
# Cross-coupled angular response (inverse_mass = inverse-inertia scalar).
ba.angular_velocity = ba.angular_velocity - np.cross(r_a, impulse) * ba.inverse_mass
bb.angular_velocity = bb.angular_velocity + np.cross(r_b, impulse) * bb.inverse_mass
return impulse
def _solve_hinge_angular_velocity(self, j: _HingeConstraint, axis: np.ndarray) -> None:
"""Lock the two off-axis rotational DOF, leaving free spin about the hinge axis.
Drives the relative angular velocity perpendicular to the hinge axis to
zero (the component along it is left free): ``w_rel = wb - wa``,
``perp = w_rel - (w_rel . axis) axis``, impulse
``dL = -m_eff_ang * perp`` with ``m_eff_ang = 1/(inv_ma + inv_mb)``
(inverse-inertia stand-in, basic tier).
``axis`` is ``a``'s stored hinge axis turned into world by ``a``'s current
orientation, the usual convention (the constraint frame belongs to the
first body): a door on a post that is itself turning keeps swinging about
the post's axis rather than about the direction the post happened to point
when the joint was built.
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return
w_rel = bb.angular_velocity - ba.angular_velocity
perp = w_rel - float(np.dot(w_rel, axis)) * axis
dl = (-perp / inv_sum).astype(np.float32)
ba.angular_velocity = ba.angular_velocity - dl * ba.inverse_mass
bb.angular_velocity = bb.angular_velocity + dl * bb.inverse_mass
def _solve_fixed_angular_velocity(self, j: _FixedConstraint) -> None:
"""Lock ALL THREE relative rotational DOF (full weld angular part).
Drives the entire relative angular velocity ``w_rel = wb - wa`` to zero:
impulse ``dL = -m_eff_ang * w_rel`` with the basic-tier inverse-inertia
stand-in ``m_eff_ang = 1/(inv_ma + inv_mb)``.
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return
w_rel = bb.angular_velocity - ba.angular_velocity
dl = (-w_rel / inv_sum).astype(np.float32)
ba.angular_velocity = ba.angular_velocity - dl * ba.inverse_mass
bb.angular_velocity = bb.angular_velocity + dl * bb.inverse_mass
def _solve_spring_velocity(self, j: _SpringConstraint, dt: float) -> None:
"""Soft distance-spring velocity impulse between the two body CENTRES.
Standard soft-constraint form: ``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)`` (a
Hooke spring plus 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)``.
Both terms carry ``dt`` (force -> impulse): the ``k*x`` term is the spring
bias, the ``c*v_n`` term the viscous damping. Degenerate ``L < eps`` skips
(no direction).
HONESTY: this is an EXPLICIT (forward-Euler) soft impulse, so a high
``stiffness`` or ``damping`` relative to the fixed ``dt`` can overshoot /
oscillate / diverge; nothing is silently clamped. The stability bound is
roughly ``dt < 2 / sqrt(k * inv_sum)`` (and damping must satisfy
``c * inv_sum * dt < 2``); stiff springs need a smaller fixed ``dt`` or
the Jolt 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: _Constraint) -> None:
"""Dispatch one Baumgarte position pass for a single RIGID joint.
Springs are intentionally compliant and have no position correction, so
they are skipped here. Pin / Hinge correct the anchor-coincidence error;
Fixed additionally corrects the relative-orientation error.
The frame is rebuilt here on every pass, not hoisted as the velocity loop's
is: this loop turns bodies, and a stale arm across its passes is exactly the
drift the whole formulation exists to remove.
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, _PinConstraint | _HingeConstraint):
frame = self._joint_frame(j)
self._correct_point_position(j.a, j.b, frame.r_a, frame.r_b)
if isinstance(j, _HingeConstraint):
self._correct_hinge_orientation(j, frame.axis_a, frame.axis_b)
elif isinstance(j, _FixedConstraint):
# 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).
frame = self._joint_frame(j)
self._correct_point_position(j.a, j.b, frame.r_a, frame.r_b)
self._correct_fixed_orientation(j)
# _SpringConstraint: 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).
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 orientation. The bodies are pushed apart by
``_BAUMGARTE * C`` split by inverse mass. A ``_JOINT_SLOP`` deadband avoids
jitter, matching the contact Baumgarte style. 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_hinge_orientation(self, j: _HingeConstraint, axis_a: np.ndarray, axis_b: np.ndarray) -> None:
"""Baumgarte angular push keeping the two hinge axes parallel.
Small-angle error ``C_ang = axis_b x axis_a``: the world rotation vector
that carries ``b``'s hinge axis onto ``a``'s, each axis being that body's
stored local axis turned into world by its current orientation. Applied as
an angular nudge split by inverse mass (inverse-inertia stand-in), a
sleeping end taking none of it, and :meth:`_apply_angular_correction` turns
``a`` the opposite way so the two meet in the middle.
The order of the cross product is the whole correction: with the operands
the other way round each pass drives the axes further apart instead of
together, which is divergent rather than merely wrong. It could not show
while both axes were pinned to one shared world vector, because then the
cross product was identically zero and the pass did nothing at all.
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
if _shiftable_mass(ba) + _shiftable_mass(bb) == 0.0:
return
c_ang = np.cross(axis_b, axis_a).astype(np.float32)
if float(np.dot(c_ang, c_ang)) <= _JOINT_EPS:
return
corr = (_BAUMGARTE * c_ang).astype(np.float32)
self._apply_angular_correction(ba, bb, corr)
def _correct_fixed_orientation(self, j: _FixedConstraint) -> None:
"""Baumgarte angular push toward the captured relative orientation.
Current relative orientation ``rel = a.orientation.inverse() *
b.orientation``; the error rotation is ``rel_target * rel.inverse()``,
whose small-angle vector (2 * xyz of the error quaternion, sign-corrected
for the shortest arc) is the orientation error. Applied as an angular
nudge split by inverse mass, a sleeping end taking none of it.
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
if _shiftable_mass(ba) + _shiftable_mass(bb) == 0.0:
return
rel = ba.orientation.inverse() * bb.orientation
err_q = j.rel_quat * rel.inverse() # rotation taking current rel -> target
# Shortest-arc small-angle vector: 2 * (x, y, z), sign-flipped if w < 0.
sign = -1.0 if err_q.w < 0.0 else 1.0
theta = np.array([err_q.x, err_q.y, err_q.z], dtype=np.float32) * (2.0 * sign)
if float(np.dot(theta, theta)) <= _JOINT_EPS:
return
corr = (_BAUMGARTE * theta).astype(np.float32)
self._apply_angular_correction(ba, bb, corr)
@staticmethod
def _apply_angular_correction(ba: _Body, bb: _Body, corr: np.ndarray) -> None:
"""Apply a small-angle orientation nudge to both bodies, mass-split.
``corr`` is a small-angle rotation vector (axis * angle). It is split by
inverse mass (inverse-inertia stand-in) and integrated into each body's
orientation: ``+`` on b, ``-`` on a, so the two converge on the target. A
sleeping end is immovable and takes none of it (:func:`_shiftable_mass`),
so the whole nudge lands on the end that can still turn. Callers guard the
both-immovable case, so the split has a non-zero denominator.
"""
inv_a, inv_b = _shiftable_mass(ba), _shiftable_mass(bb)
inv_sum = inv_a + inv_b
da = (-corr * (inv_a / inv_sum)).astype(np.float32)
db = (corr * (inv_b / inv_sum)).astype(np.float32)
if inv_a > 0.0:
ba.orientation = _integrate_orientation(ba.orientation, da, 1.0)
if inv_b > 0.0:
bb.orientation = _integrate_orientation(bb.orientation, db, 1.0)
# -- bulk transfer ------------------------------------------------------
[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:
self._check_velocities_out(out, len(self._order))
for i, handle in enumerate(self._order):
body = self._bodies[handle]
out[i, 0:3] = body.linear_velocity
out[i, 3:6] = body.angular_velocity
# -- queries ------------------------------------------------------------
[docs]
def raycast(self, origin: Vec3, direction: Vec3, max_dist: float, *, mask: int = 0xFFFFFFFF) -> RaycastHit | None:
o = _as_array(origin)
d = _as_array(direction)
length = float(np.linalg.norm(d))
if length < 1e-12:
return None
d = d / length # normalised ray direction
best: RaycastHit | None = None
best_dist = max_dist
for handle, body in self._bodies.items():
# Single query-mask vs body-layer test (query convention).
if not (mask & body.collision_layer):
continue
hit = self._ray_body(handle, body, o, d, best_dist)
if hit is not None and hit.distance < best_dist:
best = hit
best_dist = hit.distance
return best
@staticmethod
def _ray_sphere(
handle: BodyHandle, body: _Body, o: np.ndarray, d: np.ndarray, max_dist: float
) -> RaycastHit | None:
radius = float(body.shape.params[0])
m = o - body.position
b = float(np.dot(m, d))
c = float(np.dot(m, m)) - radius * radius
# Ray origin outside and pointing away: no hit.
if c > 0.0 and b > 0.0:
return None
disc = b * b - c
if disc < 0.0:
return None
t = -b - disc**0.5
if t < 0.0:
t = -b + disc**0.5 # origin inside the sphere
if t < 0.0 or t > max_dist:
return None
point = o + d * t
normal = point - body.position
n = float(np.linalg.norm(normal))
normal = normal / n if n > 1e-9 else np.array([0.0, 1.0, 0.0], dtype=np.float32)
return RaycastHit(handle, Vec3(point), Vec3(normal), t)
@staticmethod
def _ray_box(handle: BodyHandle, body: _Body, o: np.ndarray, d: np.ndarray, max_dist: float) -> RaycastHit | None:
"""Slab test against the body's axis-aligned box (orientation ignored)."""
half = body.shape.params
lo = body.position - half
hi = body.position + half
tmin = 0.0
tmax = max_dist
normal_axis = 0
normal_sign = -1.0
for axis in range(3):
di = float(d[axis])
origin_a = float(o[axis])
if abs(di) < 1e-9:
# Ray parallel to the slab: miss if origin is outside it.
if origin_a < lo[axis] or origin_a > hi[axis]:
return None
continue
inv = 1.0 / di
t1 = (lo[axis] - origin_a) * inv
t2 = (hi[axis] - origin_a) * inv
sign = -1.0
if t1 > t2:
t1, t2 = t2, t1
sign = 1.0
if t1 > tmin:
tmin = t1
normal_axis = axis
normal_sign = sign
if t2 < tmax:
tmax = t2
if tmin > tmax:
return None
if tmin < 0.0 or tmin > max_dist:
return None
point = o + d * tmin
normal = np.zeros(3, dtype=np.float32)
normal[normal_axis] = normal_sign
return RaycastHit(handle, Vec3(point), Vec3(normal), tmin)
def _ray_body(
self, handle: BodyHandle, body: _Body, o: np.ndarray, d: np.ndarray, max_dist: float
) -> RaycastHit | None:
"""Per-kind ray dispatch (no silent box fallback for capsule/cylinder)."""
kind = body.shape.kind
if kind == "sphere":
return self._ray_sphere(handle, body, o, d, max_dist)
if kind == "box":
return self._ray_box(handle, body, o, d, max_dist)
if kind == "capsule":
return self._ray_capsule(handle, body, o, d, max_dist)
if kind == "cylinder":
return self._ray_cylinder(handle, body, o, d, max_dist)
if kind == "mesh":
return self._ray_mesh(handle, body, o, d, max_dist)
if kind == "hull":
return self._ray_hull(handle, body, o, d, max_dist)
raise AssertionError(f"_ray_body: unhandled shape kind {kind!r}")
@staticmethod
def _ray_mesh(handle: BodyHandle, body: _Body, o: np.ndarray, d: np.ndarray, max_dist: float) -> RaycastHit | None:
"""Moller-Trumbore ray vs static triangle mesh (exact). ``d`` is unit.
Iterates triangles broadphase-culled by a per-triangle-AABB slab test
(linear scan; BVH deferred to Jolt). Both faces are accepted (a level mesh
is hit from either side); the returned normal is the precomputed face
normal flipped to oppose the ray. Nearest positive ``t`` within
``max_dist`` wins. Mesh rotation is ignored (offset by body position only).
"""
mesh = body.shape.mesh
assert mesh is not None, "mesh body has no _MeshData"
offset = body.position
ol = (o - offset).astype(np.float32) # ray origin in mesh-local space
eps = 1e-8
best_t = max_dist
best_pt: np.ndarray | None = None
best_n: np.ndarray | None = None
verts = mesh.vertices
# Broadphase: ray-AABB slab cull per triangle (vectorised over all tris).
inv_d = np.where(np.abs(d) > 1e-12, 1.0 / np.where(np.abs(d) > 1e-12, d, 1.0), np.inf)
t1 = (mesh.tri_lo - ol) * inv_d
t2 = (mesh.tri_hi - ol) * inv_d
tmin = np.max(np.minimum(t1, t2), axis=1)
tmax = np.min(np.maximum(t1, t2), axis=1)
candidates = np.nonzero((tmax >= np.maximum(tmin, 0.0)) & (tmin <= max_dist))[0]
for ti in candidates:
a, b, c = verts[mesh.tris[ti]]
e1 = b - a
e2 = c - a
pvec = np.cross(d, e2)
det = float(np.dot(e1, pvec))
if abs(det) < eps:
continue # ray parallel to triangle (also catches degenerate tris)
inv_det = 1.0 / det
tvec = ol - a
u = float(np.dot(tvec, pvec)) * inv_det
if u < 0.0 or u > 1.0:
continue
qvec = np.cross(tvec, e1)
v = float(np.dot(d, qvec)) * inv_det
if v < 0.0 or u + v > 1.0:
continue
t = float(np.dot(e2, qvec)) * inv_det
if t < 0.0 or t >= best_t:
continue
best_t = t
best_pt = (o + d * t).astype(np.float32)
fn = mesh.tri_normals[ti]
best_n = (-fn if float(np.dot(fn, d)) > 0.0 else fn).astype(np.float32)
if best_pt is None or best_n is None:
return None
return RaycastHit(handle, Vec3(best_pt), Vec3(best_n), best_t)
@staticmethod
def _ray_hull(handle: BodyHandle, body: _Body, o: np.ndarray, d: np.ndarray, max_dist: float) -> RaycastHit | None:
"""Ray vs convex hull: AABB-of-cloud slab test (APPROXIMATION).
Basic tier, HONEST: there is no precomputed hull-face structure, so the
hull raycast uses the cloud's axis-aligned bounding box as a conservative
proxy (the same slab test as ``_ray_box``). This OVER-reports near hull
corners cut off by faces; an exact hull raycast (Cyrus-Beck against face
planes, or a GJK-raycast) is deferred to the Jolt backend. ``d`` is unit.
"""
half = body.shape.params # local AABB half-extents of the cloud
lo = body.position - half
hi = body.position + half
tmin = 0.0
tmax = max_dist
normal_axis = 0
normal_sign = -1.0
for axis in range(3):
di = float(d[axis])
origin_a = float(o[axis])
if abs(di) < 1e-9:
if origin_a < lo[axis] or origin_a > hi[axis]:
return None
continue
inv = 1.0 / di
t1 = (lo[axis] - origin_a) * inv
t2 = (hi[axis] - origin_a) * inv
sign = -1.0
if t1 > t2:
t1, t2 = t2, t1
sign = 1.0
if t1 > tmin:
tmin = t1
normal_axis = axis
normal_sign = sign
if t2 < tmax:
tmax = t2
if tmin > tmax:
return None
if tmin < 0.0 or tmin > max_dist:
return None
point = o + d * tmin
normal = np.zeros(3, dtype=np.float32)
normal[normal_axis] = normal_sign
return RaycastHit(handle, Vec3(point.astype(np.float32)), Vec3(normal), tmin)
@staticmethod
def _ray_capsule(
handle: BodyHandle, body: _Body, o: np.ndarray, d: np.ndarray, max_dist: float
) -> RaycastHit | None:
"""Ray vs Y-axis capsule = ray vs the infinite-cylinder wall (clipped to the
segment span) plus ray vs the two hemispherical cap spheres. Exact for the
tier. ``d`` is unit length.
"""
radius = float(body.shape.params[0])
half_len = float(body.shape.params[1])
c = body.position
p0 = c - np.array([0.0, half_len, 0.0], dtype=np.float32) # bottom cap centre
p1 = c + np.array([0.0, half_len, 0.0], dtype=np.float32) # top cap centre
best_t = max_dist
best_pt: np.ndarray | None = None
# -- side wall: ray vs infinite cylinder about the Y axis, clipped in Y --
if half_len > 1e-9:
ox = float(o[0] - c[0])
oz = float(o[2] - c[2])
dx = float(d[0])
dz = float(d[2])
a = dx * dx + dz * dz
if a > 1e-12:
b = 2.0 * (ox * dx + oz * dz)
cc = ox * ox + oz * oz - radius * radius
disc = b * b - 4.0 * a * cc
if disc >= 0.0:
sq = disc**0.5
for t in ((-b - sq) / (2.0 * a), (-b + sq) / (2.0 * a)):
if 0.0 <= t < best_t:
y = float(o[1] + d[1] * t)
if p0[1] <= y <= p1[1]:
best_t = t
best_pt = o + d * t
break
# -- cap spheres (at p0 and p1, radius) --
for cap_centre in (p0, p1):
m = o - cap_centre
b = float(np.dot(m, d))
cc = float(np.dot(m, m)) - radius * radius
if cc > 0.0 and b > 0.0:
continue
disc = b * b - cc
if disc < 0.0:
continue
t = -b - disc**0.5
if t < 0.0:
t = -b + disc**0.5
if 0.0 <= t < best_t:
best_t = t
best_pt = o + d * t
if best_pt is None:
return None
seg_pt = _closest_point_on_segment(best_pt, p0, p1)
normal = best_pt - seg_pt
n = float(np.linalg.norm(normal))
normal = (normal / n).astype(np.float32) if n > 1e-9 else np.array([0.0, 1.0, 0.0], dtype=np.float32)
return RaycastHit(handle, Vec3(best_pt.astype(np.float32)), Vec3(normal), best_t)
@staticmethod
def _ray_cylinder(
handle: BodyHandle, body: _Body, o: np.ndarray, d: np.ndarray, max_dist: float
) -> RaycastHit | None:
"""Ray vs finite Y-axis cylinder: infinite-cylinder wall quadratic clipped to
``[-half_height, +half_height]`` in Y, plus the two flat circular cap planes
(accept if radial dist <= radius). Exact analytic. ``d`` is unit length.
"""
radius = float(body.shape.params[0])
half_h = float(body.shape.params[1])
c = body.position
y_lo = float(c[1] - half_h)
y_hi = float(c[1] + half_h)
best_t = max_dist
best_pt: np.ndarray | None = None
best_normal: np.ndarray | None = None
# -- side wall --
ox = float(o[0] - c[0])
oz = float(o[2] - c[2])
dx = float(d[0])
dz = float(d[2])
a = dx * dx + dz * dz
if a > 1e-12:
b = 2.0 * (ox * dx + oz * dz)
cc = ox * ox + oz * oz - radius * radius
disc = b * b - 4.0 * a * cc
if disc >= 0.0:
sq = disc**0.5
for t in ((-b - sq) / (2.0 * a), (-b + sq) / (2.0 * a)):
if 0.0 <= t < best_t:
pt = o + d * t
if y_lo <= float(pt[1]) <= y_hi:
radial = np.array([float(pt[0] - c[0]), 0.0, float(pt[2] - c[2])], dtype=np.float32)
rn = float(np.linalg.norm(radial))
best_t = t
best_pt = pt
best_normal = (
(radial / rn).astype(np.float32)
if rn > 1e-9
else np.array([1.0, 0.0, 0.0], dtype=np.float32)
)
break
# -- flat caps (planes y = y_lo, y = y_hi) --
if abs(float(d[1])) > 1e-12:
for cap_y, sign in ((y_lo, -1.0), (y_hi, 1.0)):
t = (cap_y - float(o[1])) / float(d[1])
if 0.0 <= t < best_t:
pt = o + d * t
rx = float(pt[0] - c[0])
rz = float(pt[2] - c[2])
if rx * rx + rz * rz <= radius * radius:
best_t = t
best_pt = pt
best_normal = np.array([0.0, sign, 0.0], dtype=np.float32)
if best_pt is None or best_normal is None:
return None
return RaycastHit(handle, Vec3(best_pt.astype(np.float32)), Vec3(best_normal), best_t)
[docs]
def raycast_all(
self, origin: Vec3, direction: Vec3, max_dist: float, *, mask: int = 0xFFFFFFFF
) -> list[RaycastHit]:
o = _as_array(origin)
d = _as_array(direction)
length = float(np.linalg.norm(d))
if length < 1e-12:
return []
d = d / length # normalised ray direction
hits: list[RaycastHit] = []
for handle, body in self._bodies.items():
if not (mask & body.collision_layer):
continue
hit = self._ray_body(handle, body, o, d, max_dist)
if hit is not None:
hits.append(hit)
hits.sort(key=lambda h: h.distance)
return hits
# -- kinematic sweep ----------------------------------------------------
[docs]
def sweep_body(
self,
handle: BodyHandle,
motion: Vec3,
*,
from_transform: tuple[Vec3, Quat] | None = None,
skin: float = 0.0,
) -> SweepHit | None:
"""Substepped, non-mutating sweep of a body's shape (basic tier).
Conservative advancement is out of tier scope (the narrow phase is
analytic AABB-ish overlap, not a true continuous TOI shape-cast), so this
substeps along ``motion`` with a step count sized from the mover's
smallest feature, capped at 64. The first substep that penetrates a
blocking body brackets the contact between the last fraction proved clear
and that one, and the bracket is then BISECTED against that blocker alone
(``_SWEEP_REFINE_STEPS`` halvings), so the reported ``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. Fast movers vs very thin colliders can still tunnel BETWEEN
substeps, because the bisect refines a bracket the scan found rather than
finding brackets the scan missed.
Structurally non-mutating: the mover's shape and pose are wrapped in a
TRANSIENT ``_Body`` probe, so nothing in the body table is touched and
``from_transform`` costs nothing. ``skin`` is accepted and IGNORED: the
substep quantum is already larger than any sane skin (see
:attr:`~simvx.core.physics.world.SweepHit.distance`).
A body blocks only when it is not the mover, is not a sensor, passes the
canonical AND layer/mask rule, and presents a contact that OPPOSES the
sweep. 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`` and the mover could
never move. 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_array(motion)
dist = float(np.linalg.norm(m))
if dist < 1e-9:
return None
direction = m / dist
if from_transform is None:
start, orientation = body.position.copy(), body.orientation
else:
start, orientation = self._unpack_transform(from_transform)
probe = _Body(
shape=body.shape,
body_type=BodyMode.KINEMATIC,
position=start.copy(),
orientation=orientation,
mass=0.0,
inverse_mass=0.0,
collision_layer=body.collision_layer,
collision_mask=body.collision_mask,
)
feature = max(_feature_size(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
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
# 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: _Body = 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.
"""
probe.position = start + m * f
c = self._narrow(probe_handle, probe, ho, bo)
if c is None:
return False
return float(np.dot(direction, (-c.normal).astype(np.float32))) <= -1e-4
# The bracket [prev_frac, frac] is proved clear at one end and
# blocked at the other, so bisect it against THIS blocker only.
# Eight halvings take the reported bound from one substep of error
# to 1/256 of a substep, below any skin the policy uses.
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
# `point` is derived from the SAME fraction as `distance`: refining
# one without the other would make the two describe different poses,
# and `point` is public (MoveResult.collisions feeds apply_impulse).
reached = start + m * lo
travelled = dist * lo
point = (reached - n * feature).astype(np.float32)
return SweepHit(body=ho, point=Vec3(point), normal=Vec3(n), distance=travelled)
prev_frac = frac
return None
# -- shape queries ------------------------------------------------------
def _make_probe(self, shape: _Shape, position: np.ndarray, orientation: Quat) -> _Body:
"""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. Layer/mask are left at the
permissive defaults; the query mask is applied per-body by the caller.
"""
return _Body(
shape=shape,
body_type=BodyMode.KINEMATIC,
position=_as_array(position),
orientation=orientation,
mass=0.0,
inverse_mass=0.0,
)
def _refuse_unsupported_mesh_query(self, kind: str, mask: int) -> None:
"""Refuse a QUERY whose probe shape cannot be paired with a mesh in reach.
The query paths build a transient probe through :meth:`_make_probe` and
reach ``_narrow`` without passing :meth:`create_body`, so without this a
hull shapecast against a mesh floor would keep returning no contact, and
keep returning it unrefused. Uses the one-directional query-mask
convention (``mask & body.layer``) the query paths themselves apply.
Raises:
ValueError: If the probe kind is refused against a reachable mesh.
"""
if kind not in _REFUSED_VS_MESH:
return
for m_layer, _m_mask in self._mesh_bodies.values():
if mask & m_layer:
raise ValueError(_refused_pair_message(kind))
[docs]
def shapecast(
self,
shape: ShapeHandle,
origin: Vec3,
direction: Vec3,
max_dist: float,
*,
mask: int = 0xFFFFFFFF,
) -> SweepHit | None:
"""Substepped shape sweep, earliest-TOI contact (basic tier).
Mirrors :meth:`sweep_body`'s substepped scan but with a transient
probe shape (not a registered body) and the single query-mask convention
(``mask & body.layer``). Earliest TOI wins: substeps are the outer loop,
bodies the inner, so the first penetrating substep is the earliest hit.
"""
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 with
# guidance rather than overflow when ceil()-ing an infinite step count.
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 == "mesh":
raise ValueError("mesh shapes cannot be used as a moving query shape (static-only)")
self._refuse_unsupported_mesh_query(probe_shape.kind, mask)
o = _as_array(origin)
d = _as_array(direction)
length = float(np.linalg.norm(d))
if length < 1e-12 or max_dist <= 0.0:
return None
dir_unit = d / length
motion = dir_unit * float(max_dist)
probe = self._make_probe(probe_shape, o, Quat())
feature = max(_feature_size(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
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
# a = probe (mover), b = other: negate to point back toward origin.
n = (-contact.normal).astype(np.float32)
reached = o + motion * prev_frac
travelled = float(max_dist) * prev_frac
point = (reached - n * feature).astype(np.float32)
return SweepHit(body=ho, point=Vec3(point), normal=Vec3(n), distance=travelled)
prev_frac = frac
return None
[docs]
def overlap(self, shape: ShapeHandle, transform: object, *, mask: int = 0xFFFFFFFF) -> list[BodyHandle]:
"""Static shape-vs-body overlap test, all matches (basic tier).
Places a transient probe shape at ``transform`` and returns the handles
of every body it overlaps whose ``layer & mask`` is set, sorted by handle
for determinism. Single query-mask convention.
"""
probe_shape = self._shape_rec(shape)
if probe_shape.kind == "mesh":
raise ValueError("mesh shapes cannot be used as a moving query shape (static-only)")
self._refuse_unsupported_mesh_query(probe_shape.kind, mask)
position, orientation = self._unpack_transform(transform)
probe = self._make_probe(probe_shape, position, orientation)
result: list[BodyHandle] = []
for ho, bo in self._bodies.items():
if not (mask & bo.collision_layer):
continue
if self._narrow(-1, probe, ho, bo) is not None:
result.append(ho)
return sorted(result)
# -- body read-back -----------------------------------------------------
[docs]
def body_transform(self, handle: BodyHandle) -> tuple[Vec3, Quat]:
body = self._bodies[handle]
return Vec3(body.position), Quat(body.orientation)
__all__ = ["BuiltinPhysics"]