"""BuiltinPhysics world: pure-Python backend implementing the seam.
Role
----
Concrete :class:`~simvx.core.physics.world.PhysicsWorld` implementation, pure
Python (numpy only). This is the engine's default, always-available backend and
the behavioural parity target for the optional native backend.
This is the **basic tier**: semi-implicit (symplectic) Euler integration, an
O(n^2) broad+narrow phase over sphere/box pairs, and a sequential normal-impulse
solver with Baumgarte positional correction and a small penetration slop. It is
written for clarity over raw throughput: dynamic bodies settle on static ground
without sinking or tunnelling at moderate speeds, which is the acceptance bar for
the tier. Rotation is integrated for round-tripping but contacts apply a linear
impulse at the centre of mass only (no inertia tensor); that is intentional for
the basic backend.
Shape set (T1a): sphere, box, capsule, cylinder. Capsules are done properly via
segment closest-point reductions (every capsule pairing reduces to a sphere test
at the closest segment point(s), reusing the sphere depth/normal maths). Cylinders
are the honest weak shape of the tier: cylinder-sphere and the ray queries are
exact analytic finite-cylinder maths, but cylinder-box is an AABB-of-cylinder
approximation and cylinder-cylinder / capsule-cylinder approximate the cylinder as
a capsule (rounding the rims). Each approximate pairing carries an explicit
"deferred to Jolt" comment at its call site; none silently fakes a result.
Swept queries (shapecast / overlap / character sweep) for capsule and cylinder
inherit the same substepped (non-continuous) tunnelling honesty as the existing
box queries: capsules are oriented only by translation (the segment stays Y-axis),
and the cylinder-box approximation applies inside sweeps too.
Shape set (T1b): adds convex hull + static triangle mesh.
- STATIC TRIANGLE MESH (``"mesh"``): the high-value path, done properly. A moving
sphere / capsule / box collides against the mesh triangles via
closest-point-on-triangle (Ericson RTCD 5.1.5), reduced to a sphere-at-a-point
test per candidate triangle; box-vs-triangle uses the box's bounding sphere (a
documented approximation, like cylinder-box). Raycast uses Moller-Trumbore.
Meshes are STATIC-ONLY (enforced at ``create_body`` / ``set_body_mode``) so the
mesh is never the MOVING shape. Broadphase is a linear per-triangle-AABB cull
(a BVH is deferred to Jolt).
- CONVEX HULL (``"hull"``): basic tier. GJK gives EXACT boolean overlap;
penetration depth + normal use EPA-lite (bounded-iteration expanding polytope
with a search-direction fallback) and are APPROXIMATE. Hull rotation is IGNORED
(cloud treated in world axes offset by body position), like every other basic
shape. Hull raycast is the AABB-of-cloud slab test (an explicit approximation;
exact hull raycast is deferred to Jolt). A true robust hull / EPA manifold is a
Jolt concern. Capsule / cylinder vs hull route through GJK by exposing a capsule
support function (cylinder is approximated as a capsule, the existing rim caveat).
"""
from __future__ import annotations
import math
from dataclasses import dataclass, field
import numpy as np
from ...math import Quat, Vec3
from ..capability import Capability
from ..material import CombineMode, _combine
from ..world import (
_DEFAULT_UP,
BodyHandle,
BodyMode,
CharacterHandle,
CharacterMoveResult,
Contact,
ContactEvent,
ContactPhase,
JointHandle,
OverlapEvent,
PhysicsWorld,
RaycastHit,
ShapeHandle,
)
# Solver tuning. Conservative, readable defaults for the basic tier.
_BAUMGARTE = 0.2 # fraction of penetration corrected per step (positional bias)
_SLOP = 0.001 # penetration tolerated without correction (metres), reduces jitter
# Restitution velocity rest-threshold (Stage T1d): 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 (Stage T1c, 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 a deliberate follow-on, NOT T1c.
_VELOCITY_ITERATIONS = 8 # combined contact+joint velocity passes per step
_POSITION_ITERATIONS = 3 # Baumgarte position passes for rigid joints per step
_JOINT_EPS = 1e-9 # degenerate-direction guard (zero-length spring / anchor delta)
# Reusable zero anchor offset for the Fixed-joint point solve (r = 0 on both
# bodies). 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)
# Sleeping thresholds (Stage T1e, basic tier). A DYNAMIC body whose linear AND
# angular speed stay below these for _SLEEP_TIME continuous seconds goes to SLEEP:
# skipped by _integrate and _solve_contact until woken, killing residual jitter on
# resting stacks and saving solver work. Same order as Jolt's sleep settings.
_SLEEP_LINEAR = 0.05 # m/s: below this (and angular) the sleep timer accumulates
_SLEEP_ANGULAR = 0.05 # rad/s
_SLEEP_TIME = 0.5 # seconds of continuous sub-threshold motion before sleeping
@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
@dataclass(slots=True)
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.
"""
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
@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: _Shape
body_type: BodyMode
position: np.ndarray # float32 (3,)
orientation: Quat
mass: float
inverse_mass: float
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 (Stage R2b). 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 (design S7).
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 / _sweep_character stay non-sensors.
is_sensor: bool = False
# Surface material coefficients (Stage T1d), 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 / _sweep_character
# 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
# CCD flag (Stage T1e). 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 /
# _sweep_character probes are discrete and unchanged.
continuous: bool = False
# Sleeping (Stage T1e). asleep DYNAMIC bodies are skipped by _integrate and the
# contact velocity solve until woken. _sleep_timer accumulates dt while both
# speeds stay below the thresholds; reaching _SLEEP_TIME sets asleep=True. 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. 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
@dataclass(slots=True)
class _Character:
"""Internal character controller (CharacterVirtual-style).
Collides against world bodies but is never in :attr:`_bodies`, so the solver
/ bulk readers never see it. Holds its own pose plus the fixed collider
config captured at create time. ``slope_limit`` is stored in radians.
"""
shape: _Shape
position: np.ndarray # float32 (3,)
orientation: Quat
up: np.ndarray # float32 (3,), unit
slope_limit: float # radians
step_height: float
skin_width: float
collision_layer: int = 0x00000001
collision_mask: int = 0xFFFFFFFF
def _layers_match(layer_a: int, mask_a: int, layer_b: int, mask_b: int) -> bool:
"""Canonical Design S6 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 (Design S6) and lives in the
raycast / shapecast / overlap paths, not here.
"""
return bool((mask_a & layer_b) and (mask_b & layer_a))
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 _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.
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 _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 _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 (Stage P2): ``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, P2)
vbias: float = 0.0 # restitution velocity bias, computed ONCE pre-solve (P2)
# accumulated tangential (friction) impulse, within-step (Coulomb-clamped total)
jt_vec: np.ndarray = field(default_factory=lambda: np.zeros(3, dtype=np.float32))
# -- joint / constraint records (Stage T1c) ----------------------------------
#
# 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, matching the character-namespace pattern. Anchors are
# stored as body-LOCAL offsets in WORLD axes captured at create time; the basic
# tier re-derives the world anchor each step as ``pos + r`` and so IGNORES body
# rotation of the anchor (documented honesty caveat, parity with the
# hull-rotation-ignored narrowphase). All angular terms use ``inverse_mass`` as
# the inverse-inertia scalar (no inertia tensor in the basic tier, per T1a).
@dataclass(slots=True)
class _FixedConstraint:
"""Weld: lock the full relative transform of ``b`` in ``a``'s frame.
Captures, at create time, the world-axes offset of b's centre from a's centre
(``rel_pos``) and the relative orientation (``rel_quat``, the rotation taking
a's frame to b's frame, i.e. ``a.orientation.inverse() * b.orientation``).
The point part is solved at a's centre (``r_a = 0``); the angular part drives
the full relative angular velocity to zero plus a Baumgarte bias toward
``rel_quat``.
"""
a: BodyHandle
b: BodyHandle
rel_pos: np.ndarray # float32 (3,), b_centre - a_centre captured at create (world axes)
rel_quat: Quat # a.orientation.inverse() * b.orientation captured at create
@dataclass(slots=True)
class _PinConstraint:
"""Point-to-point: keep the two world anchors coincident, rotation free.
``r_a`` / ``r_b`` are the world-axes offsets from each body centre to the
shared anchor captured at create (``anchor - pos``). The world anchor on each
body is re-derived each step as ``pos + r`` (rotation ignored, basic tier).
"""
a: BodyHandle
b: BodyHandle
r_a: np.ndarray # float32 (3,), anchor - pos_a at create
r_b: np.ndarray # float32 (3,), anchor - pos_b at create
@dataclass(slots=True)
class _HingeConstraint:
"""Pin at an anchor + an angular lock leaving one free DOF about ``axis``.
Point part identical to :class:`_PinConstraint` (``r_a`` / ``r_b``). The
angular part keeps the per-body hinge axes (``axis_a`` / ``axis_b``, world
axes captured at create) parallel, locking the two off-axis rotational DOF.
``axis`` is the shared world-space hinge axis (normalised) used to project out
the free DOF.
"""
a: BodyHandle
b: BodyHandle
r_a: np.ndarray # float32 (3,), anchor - pos_a at create
r_b: np.ndarray # float32 (3,), anchor - pos_b at create
axis: np.ndarray # float32 (3,), unit world hinge axis captured at create
axis_a: np.ndarray # float32 (3,), per-body axis on a at create (== axis, world)
axis_b: np.ndarray # float32 (3,), per-body axis on b at create (== axis, world)
@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
[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
# Characters live in a DISTINCT handle namespace so they never collide
# with body handles and are never returned by register_bodies.
self._characters: dict[CharacterHandle, _Character] = {}
self._next_char: int = 0
# Collision-event diffing (Stage R2a). ``_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] = []
# Sensor-overlap diffing (Stage R3a), 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 (Stage T1c). A SEPARATE handle counter so a joint
# handle never aliases a body handle (parity with the character
# namespace). 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
# Warm-start cache (Stage P2): 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] = {}
[docs]
def capabilities(self) -> frozenset[Capability]:
"""No Tier-3 capabilities: the builtin tier is an honest rigid-body solver.
It advertises NONE of :class:`Capability` (no cross-platform determinism,
no vehicles, no soft bodies): the Tier-1 rigid-body surface it shares with
every backend is its whole offering. A node needing a Tier-3 feature must
select a native backend that lists it. Explicit (not inherited) so the
claim is a deliberate, tested promise, not an accident of the default.
"""
return frozenset()
[docs]
@property
def body_count(self) -> int:
"""Number of bodies currently in the world (``len`` of the body table)."""
return len(self._bodies)
[docs]
def clear(self) -> None:
"""Remove every body, character, and joint, emptying the world.
See :meth:`~simvx.core.physics.world.PhysicsWorld.clear`. Resets the body /
character / 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._characters.clear()
self._joints.clear()
self._order.clear()
self._touching.clear()
self._contact_events.clear()
self._overlapping.clear()
self._overlap_events.clear()
self._warm_contacts.clear()
def _alloc_handle(self) -> int:
h = self._next_handle
self._next_handle += 1
return h
# -- shapes -------------------------------------------------------------
[docs]
def create_sphere(self, radius: float) -> ShapeHandle:
assert radius > 0.0, f"sphere radius must be > 0, got {radius}"
handle = self._alloc_handle()
self._shapes[handle] = _Shape("sphere", np.array([radius], dtype=np.float32))
return handle
[docs]
def create_box(self, half_extents: Vec3) -> ShapeHandle:
he = _as_array(half_extents)
assert np.all(he > 0.0), f"box half_extents must all be > 0, got {tuple(he)}"
handle = self._alloc_handle()
self._shapes[handle] = _Shape("box", he.copy())
return handle
[docs]
def create_capsule(self, radius: float, height: float) -> ShapeHandle:
assert radius > 0.0 and height > 0.0, f"capsule radius/height must be > 0, got {radius}, {height}"
half_len = max(0.0, height * 0.5 - radius)
handle = self._alloc_handle()
self._shapes[handle] = _Shape("capsule", np.array([radius, half_len], dtype=np.float32))
return handle
[docs]
def create_cylinder(self, radius: float, height: float) -> ShapeHandle:
assert radius > 0.0 and height > 0.0, f"cylinder radius/height must be > 0, got {radius}, {height}"
handle = self._alloc_handle()
self._shapes[handle] = _Shape("cylinder", np.array([radius, height * 0.5], dtype=np.float32))
return handle
[docs]
def create_convex_hull(self, points: np.ndarray) -> ShapeHandle:
pts = np.asarray(points, dtype=np.float32).reshape(-1, 3)
assert pts.shape[1] == 3, f"hull points must be (N, 3), got {pts.shape}"
assert pts.shape[0] >= 4, 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)
handle = self._alloc_handle()
self._shapes[handle] = _Shape("hull", aabb_half, hull_points=pts.copy())
return handle
[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)
assert idx.size > 0 and idx.size % 3 == 0, f"mesh indices must be a non-zero multiple of 3, got {idx.size}"
assert int(idx.min()) >= 0 and int(idx.max()) < verts.shape[0], "mesh index out of range"
tris = idx.reshape(-1, 3)
tri_v = verts[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)
tri_lo = tri_v.min(axis=1).astype(np.float32)
tri_hi = tri_v.max(axis=1).astype(np.float32)
aabb_lo = verts.min(axis=0).astype(np.float32)
aabb_hi = verts.max(axis=0).astype(np.float32)
mesh = _MeshData(
vertices=verts.copy(),
tris=tris.copy(),
tri_normals=tri_normals,
tri_lo=tri_lo,
tri_hi=tri_hi,
aabb_lo=aabb_lo,
aabb_hi=aabb_hi,
)
aabb_half = ((aabb_hi - aabb_lo) * 0.5).astype(np.float32)
handle = self._alloc_handle()
self._shapes[handle] = _Shape("mesh", aabb_half, mesh=mesh)
return handle
# -- bodies -------------------------------------------------------------
[docs]
def create_body(
self,
shape: ShapeHandle,
body_type: BodyMode,
transform: object,
*,
mass: float = 1.0,
collision_layer: int = 1,
collision_mask: int = 0xFFFFFFFF,
is_sensor: bool = False,
friction: float = 0.5,
restitution: float = 0.0,
friction_combine: CombineMode = CombineMode.AVERAGE,
restitution_combine: CombineMode = CombineMode.AVERAGE,
continuous: bool = False,
) -> BodyHandle:
assert shape in self._shapes, f"unknown shape handle {shape!r}"
shp = self._shapes[shape]
# Triangle meshes are STATIC-ONLY colliders (every serious engine, incl.
# Jolt, enforces this): a mesh has no inertia and is level geometry. This
# seam 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."
)
position, orientation = self._unpack_transform(transform)
if body_type is BodyMode.DYNAMIC:
assert mass > 0.0, f"dynamic body mass must be > 0, got {mass}"
inv_mass = 1.0 / mass
else:
# STATIC and KINEMATIC are treated as infinite mass.
inv_mass = 0.0
handle = self._alloc_handle()
self._bodies[handle] = _Body(
shape=self._shapes[shape],
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=friction,
restitution=restitution,
friction_combine=friction_combine,
restitution_combine=restitution_combine,
continuous=continuous,
)
return handle
[docs]
def destroy_body(self, handle: BodyHandle) -> None:
# Idempotent: freeing an already-removed body is a safe no-op. 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
del self._bodies[handle]
if handle in self._order:
self._order.remove(handle)
# Drop any touching pair involving the destroyed body SILENTLY: the node
# is gone, so it must never fire a dangling ``separated`` next step.
if self._touching:
self._touching = {pair for pair in self._touching if handle not in pair}
# Same silent-drop for the directed sensor-overlap set, in EITHER
# position, so a destroyed sensor / watched body never fires a dangling
# EXIT to a freed node next step.
if self._overlapping:
self._overlapping = {pair for pair in self._overlapping if handle not in pair}
# Silently drop any joint referencing the destroyed body (parity with the
# _touching / _overlapping purge): 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:
self._joints = {h: j for h, j in self._joints.items() if j.a != handle and j.b != handle}
[docs]
def set_body_transform(self, handle: BodyHandle, transform: object) -> None:
body = self._bodies[handle]
body.position, body.orientation = self._unpack_transform(transform)
self._wake(body) # a teleport is a disturbance: wake a sleeping body
[docs]
def set_body_velocity(
self,
handle: BodyHandle,
linear: 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)
self._wake(body) # a direct velocity write is a disturbance: wake a sleeper
[docs]
def set_body_mode(self, handle: BodyHandle, mode: BodyMode) -> 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}."
)
body.body_type = mode
if mode is BodyMode.DYNAMIC:
assert body.mass > 0.0, f"dynamic body mass must be > 0, got {body.mass}"
body.inverse_mass = 1.0 / body.mass
else:
body.inverse_mass = 0.0 # STATIC / KINEMATIC -> infinite mass
# A mode flip is a disturbance; also a STATIC->DYNAMIC body must start awake.
self._wake(body)
[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 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(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(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(body) # an applied torque is a disturbance: wake a sleeper
body.torque = body.torque + _as_array(torque)
# -- joints / constraints (Stage T1c) ----------------------------------
def _alloc_joint(self) -> int:
"""Allocate a joint handle from the SEPARATE joint counter.
Kept distinct from ``_alloc_handle`` so a joint handle never aliases a
body handle (parity with the character namespace).
"""
h = self._next_joint
self._next_joint += 1
return h
[docs]
def create_fixed_joint(self, a: BodyHandle, b: BodyHandle) -> JointHandle:
assert a in self._bodies, f"create_fixed_joint: unknown body handle {a!r}"
assert b in self._bodies, f"create_fixed_joint: unknown body handle {b!r}"
ba, bb = self._bodies[a], self._bodies[b]
rel_pos = (bb.position - ba.position).astype(np.float32)
rel_quat = ba.orientation.inverse() * bb.orientation # a-frame -> b-frame
handle = self._alloc_joint()
self._joints[handle] = _FixedConstraint(a=a, b=b, rel_pos=rel_pos, rel_quat=rel_quat)
return handle
[docs]
def create_pin_joint(self, a: BodyHandle, b: BodyHandle, anchor: Vec3) -> JointHandle:
assert a in self._bodies, f"create_pin_joint: unknown body handle {a!r}"
assert b in self._bodies, f"create_pin_joint: unknown body handle {b!r}"
ba, bb = self._bodies[a], self._bodies[b]
anc = _as_array(anchor)
handle = self._alloc_joint()
self._joints[handle] = _PinConstraint(
a=a, b=b, r_a=(anc - ba.position).astype(np.float32), r_b=(anc - bb.position).astype(np.float32)
)
return handle
[docs]
def create_hinge_joint(self, a: BodyHandle, b: BodyHandle, anchor: Vec3, axis: Vec3) -> JointHandle:
assert a in self._bodies, f"create_hinge_joint: unknown body handle {a!r}"
assert b in self._bodies, f"create_hinge_joint: unknown body handle {b!r}"
ba, bb = self._bodies[a], self._bodies[b]
anc = _as_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.
assert n > _JOINT_EPS, f"create_hinge_joint: axis must be non-zero, got {tuple(ax)}"
ax = (ax / n).astype(np.float32)
handle = self._alloc_joint()
self._joints[handle] = _HingeConstraint(
a=a,
b=b,
r_a=(anc - ba.position).astype(np.float32),
r_b=(anc - bb.position).astype(np.float32),
axis=ax,
axis_a=ax.copy(),
axis_b=ax.copy(),
)
return handle
[docs]
def create_spring_joint(
self, a: BodyHandle, b: BodyHandle, rest_length: float, stiffness: float, damping: float
) -> JointHandle:
assert a in self._bodies, f"create_spring_joint: unknown body handle {a!r}"
assert b in self._bodies, f"create_spring_joint: unknown body handle {b!r}"
handle = self._alloc_joint()
self._joints[handle] = _SpringConstraint(
a=a, b=b, rest_length=float(rest_length), stiffness=float(stiffness), damping=float(damping)
)
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.
self._joints.pop(handle, None)
@staticmethod
def _unpack_transform(transform: object) -> tuple[np.ndarray, Quat]:
"""Extract (position float32 (3,), orientation Quat) from a flexible input.
Accepts a ``Transform3D`` (``.position`` + ``.rotation``), a ``Vec3`` /
sequence (position only, identity orientation), or a ``(position,
orientation)`` pair.
"""
pos = getattr(transform, "position", None)
if pos is not None:
rot = getattr(transform, "rotation", None)
orientation = Quat(rot) if isinstance(rot, Quat) else Quat()
return _as_array(pos), orientation
if isinstance(transform, tuple) and len(transform) == 2:
p, r = transform
orientation = Quat(r) if isinstance(r, Quat) else Quat()
return _as_array(p), orientation
# 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 seam.
"""
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 relative velocity per pair (b w.r.t. a), in the contact's
# own a->b orientation, captured before _resolve mutates velocities.
rel_vel: dict[tuple[BodyHandle, BodyHandle], np.ndarray] = {}
for key, c in current.items():
ba, bb = self._bodies[c.a], self._bodies[c.b]
rel_vel[key] = (bb.linear_velocity - ba.linear_velocity).astype(np.float32)
impulses: dict[tuple[BodyHandle, BodyHandle], float] = {}
if contacts or self._joints:
self._solve(contacts, dt, impulses)
# Rebuild the warm-start cache (Stage P2) from THIS step's contacts, always:
# a step with no contacts (every pair separated) must clear it so a stale
# pair never warm-starts a contact that no longer exists. The converged
# 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, rel_vel, 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 (Stage T1e): 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.
self._update_sleeping(dt)
def _diff_contacts(
self,
current: dict[tuple[BodyHandle, BodyHandle], _Contact],
rel_vel: dict[tuple[BodyHandle, BodyHandle], np.ndarray],
impulses: dict[tuple[BodyHandle, BodyHandle], float],
) -> None:
"""Diff this step's overlaps against ``_touching`` and buffer edge events.
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.
"""
live = current.keys()
for key in live - self._touching:
c = current[key]
ba = self._bodies[c.a]
# Contact point: midpoint of the overlap along the a->b normal,
# i.e. a's surface toward b offset inward by half the depth.
point = (ba.position + c.normal * (-0.5 * c.depth)).astype(np.float32)
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)),
rel_velocity=Vec3(rel_vel[key]),
)
)
zero = Vec3(0.0, 0.0, 0.0)
for key in self._touching - live:
a, b = key
self._contact_events.append(
ContactEvent(a=a, b=b, phase=ContactPhase.EXIT, point=zero, normal=zero, impulse=0.0, rel_velocity=zero)
)
self._touching = set(live)
# -- sensor overlap (broadphase-driven, one-directional) ---------------
def _collide_sensors(self) -> set[tuple[BodyHandle, BodyHandle]]:
"""One-directional sensor overlap sweep over all pairs (basic tier).
A SECOND O(n^2) sweep, SEPARATE from ``_collide``'s AND-filtered
collision pass (kept separate so the one-directional sensor filter never
contaminates the body-body AND rule). For any pair where at least one
body is a sensor it tests detection per Design S6: 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())
for i in range(len(items)):
ha, ba = items[i]
for j in range(i + 1, len(items)):
hb, bb = items[j]
# At least one must be a sensor; pure body-body is the
# collision pass's job.
if not (ba.is_sensor or bb.is_sensor):
continue
# 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 (design S7). 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 + accumulated force as acceleration; torque -> angular
# (inverse_mass stand-in for inverse inertia, basic tier).
accel = gravity + body.force * body.inverse_mass
body.linear_velocity = body.linear_velocity + accel * dt
body.angular_velocity = body.angular_velocity + body.torque * body.inverse_mass * dt
target = body.position + body.linear_velocity * dt
# CCD (Stage T1e): 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) -> None:
"""Post-solve sleep pass over every awake DYNAMIC body.
A body whose SETTLED linear AND angular speed stay below the thresholds for
_SLEEP_TIME continuous seconds goes to sleep (skipped by _integrate and the
contact 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.
"""
lin_sq = _SLEEP_LINEAR * _SLEEP_LINEAR
ang_sq = _SLEEP_ANGULAR * _SLEEP_ANGULAR
for body in self._bodies.values():
if body.body_type is not BodyMode.DYNAMIC or body.asleep:
continue
slow = (
float(np.dot(body.linear_velocity, body.linear_velocity)) < lin_sq
and float(np.dot(body.angular_velocity, body.angular_velocity)) < ang_sq
)
if slow:
body._sleep_timer += dt
if body._sleep_timer >= _SLEEP_TIME:
body.asleep = True
body.linear_velocity[:] = 0.0 # kill residual jitter velocity
body.angular_velocity[:] = 0.0
else:
body._sleep_timer = 0.0
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
# -- collision detection (broad + narrow) ------------------------------
def _collide(self) -> list[_Contact]:
"""O(n^2) broad phase + analytic narrow phase over all body pairs."""
contacts: list[_Contact] = []
items = list(self._bodies.items())
for i in range(len(items)):
ha, ba = items[i]
for j in range(i + 1, len(items)):
hb, bb = items[j]
# 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
contact = self._narrow(ha, ba, hb, bb)
if contact is not None:
contacts.append(contact)
return contacts
@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)
# Mesh vs hull is not supported: a mesh is static and a hull is a movable
# primitive, but hull-vs-mesh would need closest-point over the hull
# surface (not implemented in the basic tier). Treat as no contact rather
# than asserting, since a scene may legitimately contain 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).
Honesty (basic tier): the mesh's ROTATION is IGNORED (triangles are taken
in mesh-local space offset only by the body position), and box-vs-triangle
approximates the box by its bounding sphere (deferred to Jolt). 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"
# 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: bounding-sphere approximation (documented; deferred to Jolt)
half = b_mover.shape.params
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
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)
depth = radius - 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)
@staticmethod
def _box_box(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.
"""
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))
normal = np.zeros(3, dtype=np.float32)
normal[axis] = 1.0 if delta[axis] >= 0.0 else -1.0
return _Contact(ha, hb, normal, float(overlap[axis]))
# -- 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 (Stage T1c). Restructure vs the original single-pass
``_resolve``:
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, ``_VELOCITY_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; contact Baumgarte stays INLINE in ``_solve_contact``,
unchanged, to minimise behavioural drift for existing contact tests.
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, for the RIGID joints
only: a Baumgarte split that pushes the bodies to satisfy the
positional constraint (anchor coincidence / orientation lock). 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 a follow-on, NOT T1c.
``impulses`` is a pure side-table for the collision-event payload; it
never feeds back into the solver maths.
"""
joints = list(self._joints.values())
# 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 (Stage P2). 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 (Stage P2). 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 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).
last = _VELOCITY_ITERATIONS - 1
for it in range(_VELOCITY_ITERATIONS):
write = impulses if it == last else None
for c in contacts:
self._solve_contact(c, write)
for j in rigid:
self._solve_joint(j, 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 1b: contact Baumgarte position correction, ONCE per step (NOT per
# velocity iteration). The velocity solve above benefits from iterating,
# but the positional correction is a one-shot projection: running it N
# times would over-push a resting body out past the contact and make it
# separate / re-enter (contact-event thrash), so it stays a single pass,
# preserving the original single-pass settling behaviour for contact tests.
for c in contacts:
self._correct_contact_position(c)
# Step 2: rigid-joint position (Baumgarte) passes.
if rigid:
for _ in range(_POSITION_ITERATIONS):
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 (Stage P2).
``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.
"""
ba = self._bodies[c.a]
bb = self._bodies[c.b]
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 (Stage P2).
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.
"""
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
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 _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 (run once per step in :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 (Stage P2): 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]
# Wake-on-contact (Stage T1e): if exactly one of the pair is asleep and the
# other is an awake DYNAMIC / KINEMATIC body (a real disturbance), wake the
# sleeper so it responds. Two sleepers stay asleep (a settled stack touching
# a settled stack); a sleeper vs STATIC stays asleep (resting on the floor
# must not perpetually re-wake). This is the per-body contact-graph wake: a
# newly-dropped awake body wakes the top of a sleeping stack, which next step
# (now awake) wakes the one below it, propagating down. Island-atomic wake is
# the documented follow-on.
if ba.asleep != bb.asleep:
sleeper, other = (ba, bb) if ba.asleep else (bb, ba)
if other.body_type is not BodyMode.STATIC and not other.asleep:
self._wake(sleeper)
if ba.asleep and bb.asleep:
return # both asleep: skip the velocity solve (no work)
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return # both infinite-mass (already filtered, but be safe)
n = c.normal
# 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 _correct_contact_position(self, c: _Contact) -> None:
"""Baumgarte positional correction to drain residual penetration (once/step).
Identical maths to the original inline ``_resolve`` correction; split out
so it runs EXACTLY once per step regardless of the velocity-iteration
count, preserving the original single-pass contact settling (no over-push
/ separate-re-enter thrash).
"""
ba = self._bodies[c.a]
bb = self._bodies[c.b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return
corr_mag = max(c.depth - _SLOP, 0.0) * _BAUMGARTE / inv_sum
if corr_mag > 0.0:
correction = corr_mag * c.normal
ba.position = ba.position - correction * ba.inverse_mass
bb.position = bb.position + correction * bb.inverse_mass
# -- joint velocity solve (Stage T1c) ----------------------------------
def _solve_joint(self, j: _Constraint, 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`).
"""
del dt # rigid-joint velocity solve is dt-independent (springs use dt)
# A live joint constantly disturbs its bodies: wake either sleeper so the
# constraint keeps solving (Stage T1e). Cheap no-op when both are awake.
ja, jb = self._bodies[j.a], self._bodies[j.b]
if ja.asleep or jb.asleep:
self._wake(ja)
self._wake(jb)
if isinstance(j, _PinConstraint):
self._solve_point_velocity(j.a, j.b, j.r_a, j.r_b)
elif isinstance(j, _HingeConstraint):
self._solve_point_velocity(j.a, j.b, j.r_a, j.r_b)
self._solve_hinge_angular_velocity(j)
elif isinstance(j, _FixedConstraint):
# Weld: lock the two centres' RELATIVE velocity (so they translate as
# one; the captured offset is held by the position pass) + a full
# angular lock. r = 0 on both, so the cross terms vanish: a pure
# linear lock of (vb - va) plus the all-axes angular lock.
self._solve_point_velocity(j.a, j.b, _ZERO3, _ZERO3)
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 _solve_point_velocity(
self, a: BodyHandle, b: BodyHandle, r_a: np.ndarray, r_b: np.ndarray
) -> None:
"""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, per T1a), 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.
"""
ba, bb = self._bodies[a], self._bodies[b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return # both infinite-mass: nothing to solve
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
def _solve_hinge_angular_velocity(self, j: _HingeConstraint) -> None:
"""Lock the two off-axis rotational DOF, leaving free spin about ``axis``.
Drives the relative angular velocity perpendicular to the hinge axis to
zero (the component along ``axis`` 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).
"""
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
axis = j.axis
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).
"""
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.
"""
if isinstance(j, _PinConstraint):
self._correct_point_position(j.a, j.b, j.r_a, j.r_b)
elif isinstance(j, _HingeConstraint):
self._correct_point_position(j.a, j.b, j.r_a, j.r_b)
self._correct_hinge_orientation(j)
elif isinstance(j, _FixedConstraint):
# Hold the captured relative offset: target pos_b == pos_a + rel_pos,
# i.e. anchor offsets r_a = 0 on a and r_b = -rel_pos on b make the
# coincidence error C = (pos_b + r_b) - (pos_a + r_a) =
# (pos_b - rel_pos) - pos_a, which is zero exactly when the offset is
# preserved (using r=0 here would instead collapse the two centres).
self._correct_point_position(j.a, j.b, _ZERO3, -j.rel_pos)
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`` (anchor offset NOT rotated, basic tier). The
bodies are pushed apart by ``_BAUMGARTE * C`` split by inverse mass. A
``_SLOP`` deadband avoids jitter, matching the contact Baumgarte style.
"""
ba, bb = self._bodies[a], self._bodies[b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return
c = (bb.position + r_b) - (ba.position + r_a)
err = float(np.linalg.norm(c))
if err <= _SLOP:
return
corr = (_BAUMGARTE * c).astype(np.float32)
ba.position = ba.position + corr * (ba.inverse_mass / inv_sum)
bb.position = bb.position - corr * (bb.inverse_mass / inv_sum)
def _correct_hinge_orientation(self, j: _HingeConstraint) -> None:
"""Baumgarte angular push keeping the two hinge axes parallel.
Small-angle off-axis error ``C_ang = axis_a x axis_b`` (the rotation
needed to align the axes). Applied as an angular nudge split by inverse
mass (inverse-inertia stand-in). Basic tier ignores axis rotation, so
``axis_a`` / ``axis_b`` stay at their captured world values; this mainly
drains residual velocity-solve error.
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 0.0:
return
c_ang = np.cross(j.axis_a, j.axis_b).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, inv_sum)
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.
"""
ba, bb = self._bodies[j.a], self._bodies[j.b]
inv_sum = ba.inverse_mass + bb.inverse_mass
if inv_sum == 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, inv_sum)
@staticmethod
def _apply_angular_correction(ba: _Body, bb: _Body, corr: np.ndarray, inv_sum: float) -> 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.
"""
da = (-corr * (ba.inverse_mass / inv_sum)).astype(np.float32)
db = (corr * (bb.inverse_mass / inv_sum)).astype(np.float32)
if ba.inverse_mass > 0.0:
ba.orientation = _integrate_orientation(ba.orientation, da, 1.0)
if bb.inverse_mass > 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:
assert h in self._bodies, f"register_bodies: unknown body handle {h!r}"
self._order = list(handles)
[docs]
def read_velocities(self, out: np.ndarray) -> None:
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 move_and_collide(self, handle: BodyHandle, motion: Vec3) -> Contact | None:
"""Substepped narrow-phase sweep of a kinematic body (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
uses a substepped sweep whose substep count is sized from the mover's
smallest feature: it advances along ``motion``, and the first substep that
penetrates an OTHER body backs the mover off to the previous
non-penetrating substep and reports a contact. Fast movers vs very thin
colliders can still tunnel between substeps; this matches the solver's
basic-tier honesty and is replaced by a GJK/Jolt pass later.
"""
body = self._bodies[handle]
start = body.position.copy()
m = _as_array(motion)
dist = float(np.linalg.norm(m))
if dist < 1e-9:
return None
feature = max(_feature_size(body.shape), 1e-4)
steps = min(64, max(1, math.ceil(dist / (feature * 0.5))))
prev_frac = 0.0
for s in range(1, steps + 1):
frac = s / steps
body.position = start + m * frac
for ho, bo in self._bodies.items():
if ho == handle:
continue
if not _layers_match(body.collision_layer, body.collision_mask, bo.collision_layer, bo.collision_mask):
continue
contact = self._narrow(handle, body, ho, bo)
if contact is not None:
# Back off to the last non-penetrating substep (minus skin).
body.position = start + m * prev_frac
travelled = dist * prev_frac
# _Contact.normal points a -> b; here a = mover, b = other,
# so it points INTO the other. The public Contact.normal must
# point toward the mover (separating direction): negate.
n = (-contact.normal).astype(np.float32)
point = (body.position - n * feature).astype(np.float32)
return Contact(body=ho, point=Vec3(point), normal=Vec3(n), distance=travelled)
prev_frac = frac
# No contact across all substeps: full move.
body.position = start + m
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,
)
[docs]
def shapecast(
self,
shape: ShapeHandle,
origin: Vec3,
direction: Vec3,
max_dist: float,
*,
mask: int = 0xFFFFFFFF,
) -> Contact | None:
"""Substepped shape sweep, earliest-TOI contact (basic tier).
Mirrors :meth:`move_and_collide`'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.
"""
assert shape in self._shapes, f"unknown shape handle {shape!r}"
# A substepped sweep needs a finite length to size its substeps: an
# unbounded shapecast is undefined for the basic tier (raycast is
# analytic and accepts inf, but a swept shape is not). Fail loudly with
# guidance rather than overflow when ceil()-ing an infinite step count.
assert math.isfinite(max_dist), "shapecast max_dist must be finite (a swept shape needs a bounded sweep length)"
if self._shapes[shape].kind == "mesh":
raise ValueError("mesh shapes cannot be used as a moving query shape (static-only)")
probe_shape = self._shapes[shape]
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 Contact(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.
"""
assert shape in self._shapes, f"unknown shape handle {shape!r}"
if self._shapes[shape].kind == "mesh":
raise ValueError("mesh shapes cannot be used as a moving query shape (static-only)")
position, orientation = self._unpack_transform(transform)
probe = self._make_probe(self._shapes[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)
# -- character controller ----------------------------------------------
def _alloc_char_handle(self) -> int:
h = self._next_char
self._next_char += 1
return h
@staticmethod
def _normalised(v: np.ndarray) -> np.ndarray:
n = float(np.linalg.norm(v))
if n < 1e-12:
return np.array([0.0, 1.0, 0.0], dtype=np.float32)
return (v / n).astype(np.float32)
[docs]
def create_character(
self,
shape: ShapeHandle,
transform: object,
*,
up: Vec3 = _DEFAULT_UP,
slope_limit: float = math.radians(45.0),
step_height: float = 0.0,
skin_width: float = 0.001,
collision_layer: int = 1,
collision_mask: int = 0xFFFFFFFF,
) -> CharacterHandle:
assert shape in self._shapes, f"unknown shape handle {shape!r}"
position, orientation = self._unpack_transform(transform)
handle = self._alloc_char_handle()
self._characters[handle] = _Character(
shape=self._shapes[shape],
position=position,
orientation=orientation,
up=self._normalised(_as_array(up)),
slope_limit=float(slope_limit),
step_height=float(step_height),
skin_width=float(skin_width),
collision_layer=collision_layer,
collision_mask=collision_mask,
)
return handle
[docs]
def destroy_character(self, handle: CharacterHandle) -> None:
assert handle in self._characters, f"unknown character handle {handle!r}"
del self._characters[handle]
def _sweep_character(self, char: _Character, motion: np.ndarray) -> Contact | None:
"""Substepped sweep of a character's shape along ``motion`` vs bodies.
Wraps the character's shape + pose in a transient KINEMATIC ``_Body`` so
the existing ``_narrow`` primitives apply unchanged, then runs the same
substepped scan as :meth:`move_and_collide` against every world body.
Returns a :class:`Contact` (with the character's post-back-off position
and a separating normal pointing toward the mover) or ``None``.
Only contacts that OPPOSE the sweep (the mover advancing into the surface,
``dot(motion_dir, separating_normal) < 0``) are reported. A non-opposing
contact (e.g. the floor the character already rests on while it moves
horizontally) is a touch, not a blocker, and is skipped: otherwise a
resting contact would halt every slide at distance 0 and the character
could never walk. This matches true shape-cast semantics (a cast only hits
what it moves into) and is what Jolt's ``CharacterVirtual`` does too.
"""
dist = float(np.linalg.norm(motion))
if dist < 1e-9:
return None
direction = motion / dist
probe = _Body(
shape=char.shape,
body_type=BodyMode.KINEMATIC,
position=char.position.copy(),
orientation=char.orientation,
mass=0.0,
inverse_mass=0.0,
collision_layer=char.collision_layer,
collision_mask=char.collision_mask,
)
start = char.position.copy()
feature = max(_feature_size(char.shape), 1e-4)
steps = min(64, max(1, math.ceil(dist / (feature * 0.5))))
prev_frac = 0.0
# 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
for s in range(1, steps + 1):
frac = s / steps
probe.position = start + motion * frac
for ho, bo in self._bodies.items():
if not _layers_match(
probe.collision_layer, probe.collision_mask, bo.collision_layer, bo.collision_mask
):
continue
contact = self._narrow(probe_handle, probe, ho, bo)
if contact is None:
continue
# a = probe (mover), b = other: negate to point toward mover.
n = (-contact.normal).astype(np.float32)
# Skip non-opposing touches: the mover is not advancing into this
# surface, so it does not block this sweep direction.
if float(np.dot(direction, n)) > -1e-4:
continue
reached = start + motion * prev_frac
travelled = dist * prev_frac
point = (reached - n * feature).astype(np.float32)
return Contact(body=ho, point=Vec3(point), normal=Vec3(n), distance=travelled)
prev_frac = frac
return None
[docs]
def character_move_and_slide(
self,
handle: CharacterHandle,
velocity: Vec3,
dt: float,
*,
up: Vec3,
max_slides: int = 4,
) -> CharacterMoveResult:
"""Arcade collide-and-slide against world bodies (basic tier).
Sweeps the character along ``velocity * dt``; on each contact it
classifies floor/wall/ceiling from the contact normal vs ``up`` (and the
stored ``slope_limit``), deflects both the remaining motion and the
velocity along the contact normal, and repeats up to ``max_slides`` times.
An optional single up/forward/down step-up probe handles ledges up to
``step_height`` (basic tier: one step only).
"""
char = self._characters[handle]
up_n = self._normalised(_as_array(up))
vel = _as_array(velocity)
motion = vel * float(dt)
cos_slope = math.cos(char.slope_limit)
on_floor = on_wall = on_ceiling = False
floor_n = up_n.copy()
for _ in range(max_slides):
if float(np.linalg.norm(motion)) < 1e-9:
break
hit = self._sweep_character(char, motion)
if hit is None:
char.position = char.position + motion
break
travelled = float(hit.distance)
full = float(np.linalg.norm(motion))
frac = travelled / full if full > 1e-9 else 0.0
char.position = char.position + motion * frac # advance to contact
n = _as_array(hit.normal) # separating normal, toward mover, unit
d = float(np.dot(n, up_n))
if d > cos_slope:
on_floor = True
floor_n = n.copy()
elif d < -0.7:
on_ceiling = True
else:
on_wall = True
if char.step_height > 0.0 and self._try_step_up(char, motion * (1.0 - frac), up_n):
# Step-up consumed the remaining horizontal motion this slide.
break
# Deflect remaining motion and velocity out of the surface.
remaining = motion * (1.0 - frac)
remaining = remaining - n * min(0.0, float(np.dot(remaining, n)))
vel = vel - n * min(0.0, float(np.dot(vel, n)))
motion = remaining
# Dedicated ground probe: the horizontal sweep above intentionally ignores
# the non-opposing floor a walking character rests on, so floor state is
# established here with a short downward cast. A walkable surface within a
# small snap distance below the feet sets on_floor + floor_normal (matching
# Jolt's ``GetGroundState`` being valid right after the move).
snap = max(char.skin_width * 4.0, 1e-3)
ground = self._sweep_character(char, -up_n * snap)
if ground is not None:
gn = _as_array(ground.normal)
if float(np.dot(gn, up_n)) > cos_slope:
on_floor = True
floor_n = gn.copy()
return CharacterMoveResult(
velocity=Vec3(vel),
on_floor=on_floor,
on_wall=on_wall,
on_ceiling=on_ceiling,
floor_normal=Vec3(floor_n),
)
def _try_step_up(self, char: _Character, horizontal: np.ndarray, up_n: np.ndarray) -> bool:
"""Single up/forward/down step probe (basic tier: one step only).
Lifts by ``step_height``, sweeps the blocked horizontal motion, then drops
back down. Commits the new pose if the up + forward legs were unobstructed
and the drop lands on ground; otherwise leaves the character untouched and
returns ``False`` so the caller falls back to wall-slide.
"""
if float(np.linalg.norm(horizontal)) < 1e-9:
return False
origin = char.position.copy()
up_motion = up_n * char.step_height
if self._sweep_character(char, up_motion) is not None:
return False # not enough headroom to step up
char.position = char.position + up_motion
if self._sweep_character(char, horizontal) is not None:
char.position = origin # forward leg still blocked at step height
return False
char.position = char.position + horizontal
down = -up_n * char.step_height
drop = self._sweep_character(char, down)
if drop is None:
# No ground within step_height below: not a real step, revert.
char.position = origin
return False
char.position = char.position + down * (float(drop.distance) / char.step_height)
return True
__all__ = ["BuiltinPhysics"]