@woosh/meep-engine 3.38.0 → 3.40.0

package/src/engine/physics/ecs/PhysicsSystem.js

@@ -5,28 +5,38 @@ import {
     bvh_query_user_data_intersects_aabb
 } from "../../../core/bvh2/bvh3/query/bvh_query_user_data_intersects_aabb.js";
 import { returnTrue } from "../../../core/function/returnTrue.js";
-import { aabb3_transform_oriented } from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";
+import {
+    aabb3_transform_oriented,
+    aabb3_transform_oriented_array
+} from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";
+import { quat3_multiply_array } from "../../../core/geom/3d/quaternion/quat3_multiply.js";
 import Vector3 from "../../../core/geom/Vector3.js";
+import { f32_next_down } from "../../../core/math/f32_next_down.js";
+import { f32_next_up } from "../../../core/math/f32_next_up.js";
 import { ResourceAccessKind } from "../../../core/model/ResourceAccessKind.js";
 import { ResourceAccessSpecification } from "../../../core/model/ResourceAccessSpecification.js";
 import { System } from "../../ecs/System.js";
 import { t64_announce_change } from "../../ecs/transform/t64_announce_change.js";
 import { TRANSFORM64_EVENT_PLACED } from "../../ecs/transform/TRANSFORM64_EVENT_PLACED.js";
-import { Transform64 } from "../../ecs/transform/Transform64.js";
+import {
+    Transform64,
+    TRANSFORM64_ROTATION_OFFSET,
+    TRANSFORM64_TRANSLATION_OFFSET
+} from "../../ecs/transform/Transform64.js";
 import { Interpolated } from "../../interpolation/Interpolated.js";
 import { body_id_index, BodyStorage } from "../body/BodyStorage.js";
 import { SBS_STRIDE, SolverBodyState } from "../body/SolverBodyState.js";
-import { compute_world_aabb, fatten_world_aabb } from "../broadphase/compute_fat_world_aabb.js";
+import { compute_world_aabb, fatten_world_aabb_array } from "../broadphase/compute_fat_world_aabb.js";
 import { generate_pairs } from "../broadphase/generate_pairs.js";
 import { PairList } from "../broadphase/PairList.js";
-import { ccd_release_contacts, ccd_resolve } from "../ccd/linear_sweep.js";
-import { solve_joints } from "../constraint/solve_constraints.js";
+import { ccd_clamped_bodies, ccd_release_contacts, ccd_resolve } from "../ccd/linear_sweep.js";
+import { relax_joints, solve_joints } from "../constraint/solve_constraints.js";
 import { ManifoldStore } from "../contact/ManifoldStore.js";
 import { ContactEventBuffer, ContactEventKind } from "../events/ContactEventBuffer.js";
 import { diff_manifolds } from "../events/diff_manifolds.js";
 import { v3_add } from "../../../core/geom/vec3/v3_add.js";
 import { v3_set } from "../../../core/geom/vec3/v3_set.js";
-import { world_inverse_inertia_apply } from "../inertia/world_inverse_inertia.js";
+import { world_inverse_inertia_apply_raw } from "../inertia/world_inverse_inertia.js";
 import { integrate_position } from "../integration/integrate_position.js";
 import { integrate_velocity_forces, integrate_velocity_gravity } from "../integration/integrate_velocity.js";
 import { IslandBuilder } from "../island/IslandBuilder.js";
@@ -39,6 +49,7 @@ import {
     prepare_contacts,
     redetect_concave_contacts,
     refresh_contacts,
+    RESTITUTION_VELOCITY_THRESHOLD,
     solve_position,
     solve_velocity,
     warm_start_contacts,
@@ -95,6 +106,12 @@ let scratch_wake_candidates = new Uint32Array(64);
  */
 const scratch_stored_aabb = new Float64Array(6);
 
+/**
+ * A collider's pose, `[px, py, pz, qx, qy, qz, qw]`, for the broadphase refit's box transform.
+ * @type {Float64Array}
+ */
+const scratch_refit_pose = new Float64Array(7);
+
 /**
  * Scratch boxes for {@link PhysicsSystem#__swept_proximity} — the mover's
  * velocity-swept tight AABB and the sleeper's tight AABB.
@@ -103,12 +120,65 @@ const scratch_stored_aabb = new Float64Array(6);
 const scratch_prox_mover = new Float64Array(6);
 const scratch_prox_sleeper = new Float64Array(6);
 
+/**
+ * Doubles per body slot in {@link PhysicsSystem#__target_poses}: the packed id of the body that set
+ * the target, then position `[x, y, z]` and rotation `[x, y, z, w]`.
+ * @type {number}
+ */
+const TARGET_POSE_STRIDE = 8;
+
+/**
+ * Scratch for {@link PhysicsSystem#__derive_target_velocities}' rotation from the current
+ * orientation to the target, as `[x, y, z, w]`.
+ * @type {Float64Array}
+ */
+const scratch_target_dq = new Float64Array(4);
+
+/**
+ * Scratch for {@link PhysicsSystem#__derive_target_velocities}: the conjugate of the body's current
+ * orientation, `[x, y, z, w]`, the second operand of that rotation.
+ * @type {Float64Array}
+ */
+const scratch_target_conjugate = new Float64Array(4);
+
+/**
+ * What {@link PhysicsSystem#collidersOf} answers for a body that has none: one shared, frozen
+ * empty list.
+ * @type {ReadonlyArray<{collider: Collider, transform: Transform64, entity: number}>}
+ */
+const NO_COLLIDERS = Object.freeze([]);
+
 /**
  * {@link PhysicsSystem#interpolationLog} warns once per process, not once per system.
  * @type {boolean}
  */
 let interpolation_log_deprecation_warned = false;
 
+/**
+ * Round a world box outward to float32, in place: each lower bound down and each upper bound up
+ * to a float32 value, so the box keeps containing what it bounded once the broadphase stores it.
+ *
+ * The BVHs store float32, and a plain write rounds to the NEAREST float32 — inward on any side
+ * that rounds that way, by up to half a float32 step: 3 cm at 1e6 m. A static leaf is its body's
+ * tight box, so a query reaching the body through its leaf missed whatever of the body lay
+ * outside it. A ray dropped onto a 2 mm plate 1e6 m out missed it; so did rays a millimetre inside
+ * a 1 m crate's faces 1e5 m out. The refit skip in stage 2 also leans on it: a stationary body's
+ * box is contained by its own stored leaf only if that leaf did not shrink.
+ *
+ * @param {Float64Array} aabb `[x0, y0, z0, x1, y1, z1]`
+ */
+function aabb3_round_outward_f32(aabb) {
+    for (let i = 0; i < 3; i++) {
+        const lower = aabb[i];
+        const lower_f32 = Math.fround(lower);
+        aabb[i] = lower_f32 > lower ? f32_next_down(lower_f32) : lower_f32;
+
+        const upper = aabb[i + 3];
+        const upper_f32 = Math.fround(upper);
+        aabb[i + 3] = upper_f32 < upper ? f32_next_up(upper_f32) : upper_f32;
+    }
+}
+
 /**
  * Rigid-body physics system.
  *
@@ -202,10 +272,20 @@ export class PhysicsSystem extends System {
     __solver_state = new SolverBodyState();
 
     /**
-     * Velocity-squared threshold below which a body is eligible to start
-     * accumulating sleep time. Combined linear + angular kinetic-ish
-     * metric: `vx²+vy²+vz² + ωx²+ωy²+ωz²`. Default 0.01 corresponds to
-     * ~0.1 m/s linear or ~0.1 rad/s angular.
+     * Speed-squared threshold, (m/s)², below which a body is eligible to start
+     * accumulating sleep time. The speed measured is that of the body's far
+     * end, `|v|² + |ω|²·R²`, where `R` is the body's extent
+     * ({@link __body_extent}: a bound on how far its colliders reach from its
+     * centre, from their local bounding boxes) floored at 1 m. Default 0.01
+     * corresponds to ~0.1 m/s linear; for a body whose extent is no more than
+     * a metre, ~0.1 rad/s angular, exactly as before the extent was counted;
+     * for a larger one, ~0.1 m/s at its far end (a 100 m rod turning at
+     * 0.05 rad/s is moving 5 m/s at the tip, and stays awake).
+     *
+     * The far end of a long body magnifies any wobble of its contacts, and the
+     * test sees it at full size: a long plank resting on a bumpy mesh or
+     * heightmap floor whose contacts jitter can stay awake where a short body
+     * would sleep.
      * @type {number}
      */
     sleepVelocitySqrThreshold = 0.01;
@@ -240,6 +320,48 @@ export class PhysicsSystem extends System {
      */
     positionIterations = 1;
 
+    /**
+     * Restitution passes per step, after the substep loop: Gauss-Seidel sweeps
+     * over the contacts with non-zero restitution, each driving a bouncing
+     * contact's normal velocity toward `-e` times its approach. A body that
+     * strikes at several points at once needs more than one: after a single
+     * pass, a cube dropped flat at e = 0.8 left the ground spinning at 5 rad/s
+     * and rebounded at 0.77 of its impact speed instead of 0.8. Two (Box3D's
+     * default) bring it within half a percent of 0.8, all but without spin.
+     * Each contact's bounce is capped at `e` times the impulse it took, which
+     * stops an unconverged pass overshooting: in zero gravity, uncapped, one
+     * pass sent a cube tumbling onto frictionless ground at e = 1 off with up
+     * to 7% more kinetic energy than it struck with; capped, 0.2% at worst.
+     * More passes still change the result, not just the cost: they converge
+     * contacts that load each other further — an e = 1 stack of four boxes
+     * struck by a ball keeps 201 J of its 309 J after 10 s at 2 passes, 88 J
+     * at 8. `0` turns restitution off.
+     *
+     * Under gravity a bounce can still gain energy, mostly not from these
+     * passes: a landing body is corrected out of the depth it sank into the
+     * ground — by the penetration bias with soft contacts, by the position pass
+     * with hard ones — and that lift is potential energy, while restitution
+     * still returns the full approach speed. Tumbling cubes landing on
+     * frictionless ground at e = 1, 60 drops of 5 s: no drop gains at all at
+     * the defaults (soft contacts, 2 passes, 4 substeps); 22 gain over 0.1%
+     * at 8 substeps
+     * (worst +0.64%: the contact is stiffer against the shorter substep and
+     * recovers more depth); with hard contacts 53 (+2.2%), and +6.4% at 8
+     * substeps. At 2 passes, turning off the bias, the position pass or
+     * restitution leaves none gaining. At 1 pass 8 gain (+2.2%), and the pass
+     * itself is part of it: with the bias off, 4 still gain (+0.8%).
+     * @type {number}
+     */
+    restitutionIterations = 2;
+
+    /**
+     * Approach speed, m/s, below which a contact does not bounce whatever its
+     * restitution, so resting and slowly settling bodies do not buzz. The
+     * discrete solver and the CCD pass apply the same rule.
+     * @type {number}
+     */
+    restitutionThreshold = RESTITUTION_VELOCITY_THRESHOLD;
+
     /**
      * Contact softness frequency, Hz. Softening is what lets resting stacks
      * settle instead of ringing just above the sleep threshold; `0` gives
@@ -349,10 +471,14 @@ export class PhysicsSystem extends System {
     __joints_sorted = [];
 
     /**
-     * Velocity iterations per substep for the joint solver. Joints (and
-     * especially joint chains) want a few more iterations than contacts to
-     * propagate impulses along the chain; cheap because joints are far
-     * fewer than contacts.
+     * Joint solver passes per substep. Each pass is one Gauss-Seidel sweep over
+     * every joint, so joints that load each other — a chain, a ragdoll limb, a
+     * heavy body on a light rope — converge together: more passes, tighter
+     * joints under load. The first pass also warm-starts each joint; a relax
+     * pass over the limit / motor / spring rows after the position integrate
+     * comes on top. Cost is passes × joints; cheap because joints are far fewer
+     * than contacts. 0 turns the joint solver off, relax included: the joints
+     * still tie their bodies' islands together, but push on nothing.
      * @type {number}
      */
     jointIterations = 8;
@@ -361,13 +487,13 @@ export class PhysicsSystem extends System {
      * Per-body pseudo-velocity for the Catto split-impulse position
      * pass (TGS Phase 1). Flat layout, 6 doubles per body slot index:
      * `[lin.x, lin.y, lin.z, ang.x, ang.y, ang.z]`. Sized to
-     * `storage.high_water_mark * 6` at the top of each fixedUpdate
-     * and zeroed in place so unwritten slots contribute nothing to
-     * `integrate_position`.
+     * `storage.high_water_mark * 6` at the top of each fixedUpdate, and
+     * zero wherever `integrate_position` reads it unless a position pass
+     * wrote it this substep, so unwritten slots contribute nothing.
      *
      * The solver writes during its position pass; `integrate_position`
-     * reads and folds into the pose update on the same tick, then
-     * the next tick's zero-pass wipes the state. It NEVER lands in
+     * reads and folds into the pose update on the same substep, and the
+     * next substep wipes it first. It NEVER lands in
      * `linearVelocity` / `angularVelocity` — that's the point of
      * split impulse: depth correction does not contaminate persistent
      * velocity (so a `restitution = 0` impact stops cleanly).
@@ -376,6 +502,21 @@ export class PhysicsSystem extends System {
      */
     __pseudo_velocity = new Float64Array(0);
 
+    /**
+     * Length of the prefix of {@link __pseudo_velocity} that a position pass
+     * may have written since it was last zeroed; everything past it is zero.
+     * Set by the hard-contact position pass of {@link fixedUpdate} and of the
+     * `solve_contacts` driver. `0` when the whole buffer is zero — always,
+     * with soft contacts (the default), which run no position pass. The wipes
+     * zero this prefix only, and only when it is non-empty: zeroing the whole
+     * buffer every substep cost work proportional to the body-slot
+     * high-water mark, not to the bodies awake — most of a 0.6 ms step at
+     * 100k slots with ten awake.
+     * @private
+     * @type {number}
+     */
+    __pseudo_velocity_dirty = 0;
+
     /**
      * Master switch for the continuous-collision pass. When false the
      * {@link RigidBodyFlags.CCD} flag is ignored and no swept queries run.
@@ -393,6 +534,85 @@ export class PhysicsSystem extends System {
      */
     __ccd_start_pos = new Float64Array(0);
 
+    /**
+     * Start-of-step linear velocities of the awake DYNAMIC bodies — every one,
+     * not only the CCD-flagged — captured with {@link __ccd_start_pos} (after
+     * forces, before the substep loop) in any step with a flagged body awake;
+     * 3 doubles per body slot index. The CCD pass replaces each with the
+     * body's velocity change through the solve, which tells it how a body it
+     * struck fell through the step.
+     * @type {Float64Array}
+     */
+    __ccd_start_vel = new Float64Array(0);
+
+    /**
+     * The CCD movers of the current step: storage indices of the bodies whose
+     * start pose {@link __ccd_start_pos} took this step, the first
+     * {@link __ccd_mover_count} of them. The CCD pass sweeps these and no
+     * others — a body made a mover after the capture, by a listener on an
+     * announcement, has no start pose this step. Grows by doubling.
+     * @type {Uint32Array}
+     */
+    __ccd_movers = new Uint32Array(16);
+
+    /**
+     * Beside each of {@link __ccd_movers}, the collider its start pose was
+     * taken from, its body's primary at the time; a mover whose primary
+     * collider has changed since is not swept this step.
+     * @type {Array<Collider|null>}
+     */
+    __ccd_mover_colliders = [];
+
+    /**
+     * Number of live entries in {@link __ccd_movers}.
+     * @type {number}
+     */
+    __ccd_mover_count = 0;
+
+    /**
+     * Per-body extent, metres, indexed by body slot: a bound on the distance
+     * from the body's centre to any point of its colliders — the largest, over
+     * its colliders, of the local bounding box's corner radius plus the
+     * collider's offset from the body. Exact for a box centred on its body;
+     * larger than the body for other shapes (a sphere of radius r reads
+     * r·√3). Written for every awake body in stage 2 of each
+     * step, from the step's starting poses, and read by the sleep test to
+     * turn angular velocity into the speed of the body's far end. Rotation
+     * invariant (local boxes, not world ones), so a body's orientation does
+     * not change when it may sleep. Derived state: recomputed each step,
+     * never serialized. Grows to `storage.high_water_mark`.
+     * @type {Float64Array}
+     */
+    __body_extent = new Float64Array(0);
+
+    /**
+     * Pending {@link setTargetPose} targets, {@link TARGET_POSE_STRIDE} doubles per body slot
+     * index: the packed id of the body that set the target ({@link RIGID_BODY_UNALLOCATED} for
+     * none), then position `[x, y, z]` and rotation `[x, y, z, w]`. Keyed by the packed id rather
+     * than a flag, so a slot freed and reused by another body before the step never inherits a
+     * target. Grows to cover the slots targets are set on; never shrinks.
+     * @private
+     * @type {Float64Array}
+     */
+    __target_poses = new Float64Array(0);
+
+    /**
+     * Packed ids of the bodies holding a pending target in {@link __target_poses}, in call order:
+     * the first {@link __target_pose_count}. Consumed and emptied by the next {@link fixedUpdate},
+     * which resets the count and keeps the array's storage — truncating it gave the storage up, and
+     * the next step's first target allocated it again.
+     * @private
+     * @type {number[]}
+     */
+    __target_pose_bodies = [];
+
+    /**
+     * How many of {@link __target_pose_bodies} are pending.
+     * @private
+     * @type {number}
+     */
+    __target_pose_count = 0;
+
     /**
      * @type {InterpolationLog|null}
      */
@@ -501,10 +721,12 @@ export class PhysicsSystem extends System {
 
     /**
      * Resize {@link __pseudo_velocity} to cover every live body slot and
-     * zero its contents. Called at the top of each fixedUpdate so the
-     * split-impulse position pass starts from a clean state — bodies the
-     * solver doesn't touch contribute zero pseudo-velocity to
-     * {@link integrate_position}.
+     * bring it to all-zero, zeroing only what a position pass may have
+     * written since it last was ({@link __pseudo_velocity_dirty}), so bodies
+     * the solver doesn't touch contribute zero pseudo-velocity to
+     * {@link integrate_position}. Called at the top of each fixedUpdate, and
+     * by a caller of the `solve_contacts` driver whose buffer may hold a
+     * hard-contact step's last correction.
      *
      * Doubles on growth like the other physics scratches; never shrinks.
      *
@@ -514,8 +736,22 @@ export class PhysicsSystem extends System {
         const required = this.storage.high_water_mark * 6;
         if (this.__pseudo_velocity.length < required) {
             this.__pseudo_velocity = new Float64Array(required * 2);
-        } else if (required > 0) {
-            this.__pseudo_velocity.fill(0, 0, required);
+            this.__pseudo_velocity_dirty = 0;
+        } else {
+            this.__clear_pseudo_velocity();
+        }
+    }
+
+    /**
+     * Zero what a position pass may have written to {@link __pseudo_velocity}
+     * since it was last zeroed; nothing if none has run.
+     * @private
+     */
+    __clear_pseudo_velocity() {
+        const dirty = this.__pseudo_velocity_dirty;
+        if (dirty > 0) {
+            this.__pseudo_velocity.fill(0, 0, dirty);
+            this.__pseudo_velocity_dirty = 0;
         }
     }
 
@@ -534,6 +770,14 @@ export class PhysicsSystem extends System {
 
     /**
      * Install (or remove with `null`) the contact filter callback.
+     *
+     * It is asked during the step, and also from inside the CCD pass's sweep — a
+     * {@link shapeCast} — so it may run a {@link raycast} or an {@link overlap},
+     * but not a {@link shapeCast} (refused by an assert: a cast cannot run
+     * inside another). A production build strips asserts, and there nothing
+     * refuses it: the sweep's own cast is overwritten and CCD tunnels without a
+     * word — 200 of 200 fast spheres fired at a 2 cm wall went through it.
+     *
      * @param {((entityA:number, entityB:number, colliderA:Collider, colliderB:Collider) => boolean) | null} fn
      */
     setContactFilter(fn) {
@@ -571,6 +815,8 @@ export class PhysicsSystem extends System {
             transform.rotation_x, transform.rotation_y, transform.rotation_z, transform.rotation_w
         );
 
+        aabb3_round_outward_f32(scratch_world_aabb);
+
         const bvh = rb.kind === BodyKind.Static ? this.staticBvh : this.dynamicBvh;
         const node = bvh.allocate_node();
 
@@ -645,6 +891,13 @@ export class PhysicsSystem extends System {
      * @param {number} entity
      */
     link(rigidBody, transform, entity) {
+        // Every kind test in the engine is an identity comparison, so a value that is none of the
+        // kinds is taken for "not Static, not Dynamic" everywhere, silently.
+        const kind = rigidBody.kind;
+        assert.ok(kind === BodyKind.Static || kind === BodyKind.Dynamic
+            || kind === BodyKind.KinematicVelocity || kind === BodyKind.KinematicPosition,
+            `link: RigidBody.kind must be a BodyKind, was ${kind}`);
+
         const packed = this.storage.allocate(entity);
         rigidBody._bodyId = packed;
 
@@ -908,6 +1161,47 @@ export class PhysicsSystem extends System {
         return this.storage.entity_at(body_id_index(packed_body_id));
     }
 
+    /**
+     * Resolve a packed body id to the {@link RigidBody} linked under it, or `undefined` if the id
+     * is stale. The id a query reports names a body; this is how its flags, kind and velocity are
+     * read without the dataset.
+     *
+     * @param {number} packed_body_id as a query reports it
+     * @returns {RigidBody|undefined}
+     */
+    bodyOf(packed_body_id) {
+        if (!this.storage.is_valid(packed_body_id)) return undefined;
+        return this.__bodies[body_id_index(packed_body_id)];
+    }
+
+    /**
+     * The colliders attached to a body, in attach order. Each entry gives the `collider`, the world
+     * `transform` it is posed at — the body's own Transform64 for a collider on the body entity, the
+     * child entity's for a compound body's child — and the `entity` that owns the collider. These are
+     * the colliders a query tests when it reaches the body, each at its own transform, in this
+     * order; an overlap query reports the body on the first of them that overlaps.
+     *
+     * **This is the system's live list, not a copy: never write to it.** The array and its entries
+     * are what the broadphase, the narrowphase and every query read, so a `push`, `pop` or `splice`,
+     * or an edit to an entry, changes what the body collides with everywhere — a caller's `pop()`
+     * drops a collider from every query. Attach and detach colliders through
+     * {@link attach_collider} / {@link detach_collider}. The list changes in place on the body's
+     * next attach, detach or unlink; copy what must outlive that. A stale id has no colliders: the
+     * result is one shared, frozen empty array.
+     *
+     * A read-only view is not offered because it is not free: a Proxy that refuses writes made a
+     * walk of an 8-entry list 8.5x as slow to read, and a frozen copy per body would allocate on
+     * every attach and detach and still hand out the live entries.
+     *
+     * @param {number} packed_body_id as a query reports it
+     * @returns {ReadonlyArray<{collider: Collider, transform: Transform64, entity: number}>}
+     */
+    collidersOf(packed_body_id) {
+        if (!this.storage.is_valid(packed_body_id)) return NO_COLLIDERS;
+        const list = this.__body_collider_lists[body_id_index(packed_body_id)];
+        return list === undefined ? NO_COLLIDERS : list;
+    }
+
     /**
      * Number of live bodies (regardless of awake/sleeping state).
      * @returns {number}
@@ -1000,7 +1294,12 @@ export class PhysicsSystem extends System {
         scratch_locked_inertia[0] = (flags & RigidBodyFlags.LockRotX) !== 0 ? 0 : ii[0];
         scratch_locked_inertia[1] = (flags & RigidBodyFlags.LockRotY) !== 0 ? 0 : ii[1];
         scratch_locked_inertia[2] = (flags & RigidBodyFlags.LockRotZ) !== 0 ? 0 : ii[2];
-        world_inverse_inertia_apply(scratch_angular_delta, 0, scratch_locked_inertia, transform.rotation, tx, ty, tz);
+        world_inverse_inertia_apply_raw(
+            scratch_angular_delta, 0,
+            scratch_locked_inertia[0], scratch_locked_inertia[1], scratch_locked_inertia[2],
+            transform.rotation_x, transform.rotation_y, transform.rotation_z, transform.rotation_w,
+            tx, ty, tz
+        );
 
         v3_add(
             rigidBody.angularVelocity, 0,
@@ -1011,8 +1310,11 @@ export class PhysicsSystem extends System {
     }
 
     /**
-     * Accumulate a continuous torque (world-space) for integration on the
-     * next fixedUpdate. Pairs with {@link applyForce} for the rotational case.
+     * Accumulate a torque (world-space, N·m) for the next fixedUpdate. Pairs
+     * with {@link applyForce} for the rotational case, and is spent the same
+     * way: once, as the angular impulse `τ·dt` over the whole step, before the
+     * substeps — a per-step budget, to be applied again every step it should
+     * last.
      *
      * Wakes the body if asleep.
      *
@@ -1033,9 +1335,12 @@ export class PhysicsSystem extends System {
     }
 
     /**
-     * Apply a continuous force at a specific world-space point. The force
-     * generates both a linear acceleration (F/m) and a torque (r × F) about
-     * the body's centre of mass.
+     * Accumulate a force at a specific world-space point for the next
+     * fixedUpdate. The force generates both a linear acceleration (F/m) and a
+     * torque (r × F) about the body's centre of mass, `r` taken from
+     * `transform` now. Both are spent like {@link applyForce}'s: once, as
+     * `F·dt` and `(r × F)·dt` over the whole step, before the substeps — a
+     * per-step budget, to be applied again every step it should last.
      *
      * Wakes the body if asleep.
      *
@@ -1076,7 +1381,17 @@ export class PhysicsSystem extends System {
     }
 
     /**
-     * Accumulate a continuous force to be integrated next fixedUpdate step.
+     * Accumulate a force (world-space, newtons) for the next fixedUpdate.
+     *
+     * The step spends the accumulated force once: as the impulse `F·dt`, `dt`
+     * the whole step, added to the body's velocity before the substeps run,
+     * after which the accumulator is empty. It is not re-applied at each
+     * substep the way gravity is. So a force is a per-step budget — the same
+     * change of momentum whatever {@link substeps} is — and one that should
+     * last must be applied again every step. A force worked out from the
+     * body's state (a spring's pull, a drag) is evaluated at the pose and
+     * velocity it was computed from and acts unchanged through the step.
+     *
      * Wakes the body if asleep.
      *
      * @param {RigidBody} rigidBody
@@ -1148,6 +1463,9 @@ export class PhysicsSystem extends System {
      * as-is; zero it via {@link setLinearVelocity} if the teleport should also stop
      * the body.
      *
+     * Cancels a {@link setTargetPose} target set for the body earlier in the step, so the
+     * teleport is where the step finds it.
+     *
      * Works for STATIC bodies too: their broadphase leaves are re-homed to
      * the new pose, and sleepers at both the old and the new location are
      * woken — a moved support drops what rested on it, and whatever it moved
@@ -1178,6 +1496,14 @@ export class PhysicsSystem extends System {
             return;
         }
 
+        // A teleport supersedes a target set earlier in the step - the last call wins, as between
+        // two targets. Left pending, the target would be reached from the teleported pose, at the
+        // velocity that takes the body straight back.
+        const target_base = idx * TARGET_POSE_STRIDE;
+        if (target_base < this.__target_poses.length && this.__target_poses[target_base] === body_id) {
+            this.__target_poses[target_base] = RIGID_BODY_UNALLOCATED;
+        }
+
         transform.setTranslation(position[0], position[1], position[2]);
         transform.setRotation(rotation[0], rotation[1], rotation[2], rotation[3]);
 
@@ -1236,6 +1562,214 @@ export class PhysicsSystem extends System {
         }
     }
 
+    /**
+     * Move a {@link BodyKind.KinematicPosition} body to a pose over the next {@link fixedUpdate},
+     * as a motion rather than a teleport: a moving platform, a door, an animated collider.
+     *
+     * The target is consumed by the next `fixedUpdate(dt)`. Before anything in that step reads
+     * velocity, the step sets the body's {@link RigidBody#linearVelocity} to the displacement from
+     * its current position over `dt`, and its {@link RigidBody#angularVelocity} to the shortest-arc
+     * rotation from its current orientation to the target's, as axis × angle over `dt`. Those are
+     * the velocities the contact solver sees, so a body resting on it is carried by friction
+     * (along, and round when it turns), and a body in its way is pushed. The step integrates them
+     * like any velocity and then writes the target pose exactly, so the body ends the step on the
+     * target to the last bit, whatever the substep count does to the rotation's integration.
+     *
+     * - Calling it again before the step replaces the target; the last call wins.
+     * - The velocities stay on the body after the step, as Box3D's `b3Body_SetTargetTransform`
+     *   leaves them: a step with no new target integrates them like any kinematic velocity, so the
+     *   body keeps moving at the same rate. To stop it, target the pose it is at (zero velocity),
+     *   or set its velocities to zero.
+     * - It is a motion from the pose the body has when the step begins. {@link setPose} is the
+     *   teleport — it moves the body with no velocity, so nothing riding it is carried. The later
+     *   of the two calls wins: a setPose after this one cancels the target, and this one after a
+     *   setPose is a motion from the teleported pose.
+     * - Only a KinematicPosition body takes a target. A Dynamic body's motion is the solver's; a
+     *   KinematicVelocity body is driven by the velocity written to it; a Static one never moves.
+     * - Unlike Box3D, which takes the time step with the target and derives the velocity at the
+     *   call, the velocity is derived from the step's own `dt` when the step runs, so the two
+     *   cannot disagree. Kinematic bodies never sleep here, so there is no sleep-threshold skip.
+     *
+     * @param {RigidBody} rigidBody a linked {@link BodyKind.KinematicPosition} body
+     * @param {Vector3|ArrayLike<number>} position world position to reach, indexed as [x, y, z]
+     * @param {Quaternion|ArrayLike<number>} rotation world unit-quaternion rotation to reach,
+     *   indexed as [x, y, z, w]; asserted unit (squared length within 1e-6 of 1), since it is
+     *   written into the transform as given
+     */
+    setTargetPose(rigidBody, position, rotation) {
+        assert.defined(position, 'position');
+        assert.isNumber(position[0], 'position[0]');
+        assert.isNumber(position[1], 'position[1]');
+        assert.isNumber(position[2], 'position[2]');
+
+        assert.defined(rotation, 'rotation');
+        assert.isNumber(rotation[0], 'rotation[0]');
+        assert.isNumber(rotation[1], 'rotation[1]');
+        assert.isNumber(rotation[2], 'rotation[2]');
+        assert.isNumber(rotation[3], 'rotation[3]');
+
+        // The pose is written verbatim at the end of the step, so a non-unit rotation would land
+        // in the transform as a scale.
+        const rotation_length_sqr = rotation[0] * rotation[0] + rotation[1] * rotation[1]
+            + rotation[2] * rotation[2] + rotation[3] * rotation[3];
+
+        // the message is built only when the check fails: as an argument it was a string per call
+        if (!(Math.abs(rotation_length_sqr - 1) <= 1e-6)) {
+            assert.ok(false, `setTargetPose: rotation must be a unit quaternion, its squared length is ${rotation_length_sqr}`);
+        }
+
+        assert.equal(rigidBody.kind, BodyKind.KinematicPosition, 'setTargetPose: the body must be BodyKind.KinematicPosition');
+
+        const body_id = rigidBody._bodyId;
+        if (!this.storage.is_valid(body_id)) return;
+
+        const idx = body_id_index(body_id);
+        if (this.__bodies[idx] !== rigidBody) return;
+
+        const base = idx * TARGET_POSE_STRIDE;
+
+        let poses = this.__target_poses;
+        if (poses.length < base + TARGET_POSE_STRIDE) {
+            const grown = new Float64Array(Math.max(base + TARGET_POSE_STRIDE, poses.length * 2));
+            grown.set(poses);
+            for (let i = poses.length; i < grown.length; i += TARGET_POSE_STRIDE) {
+                grown[i] = RIGID_BODY_UNALLOCATED;
+            }
+            this.__target_poses = poses = grown;
+        }
+
+        if (poses[base] !== body_id) {
+            poses[base] = body_id;
+            this.__target_pose_bodies[this.__target_pose_count++] = body_id;
+        }
+
+        poses[base + 1] = position[0];
+        poses[base + 2] = position[1];
+        poses[base + 3] = position[2];
+        poses[base + 4] = rotation[0];
+        poses[base + 5] = rotation[1];
+        poses[base + 6] = rotation[2];
+        poses[base + 7] = rotation[3];
+    }
+
+    /**
+     * Start of {@link fixedUpdate}: give every body with a pending {@link setTargetPose} target the
+     * velocity that reaches it over `dt`. A step of no duration reaches nothing; its targets stay
+     * pending for the next one.
+     *
+     * @private
+     * @param {number} dt
+     */
+    __derive_target_velocities(dt) {
+        const pending = this.__target_pose_bodies;
+        const pending_count = this.__target_pose_count;
+        if (pending_count === 0 || !(dt > 0)) return;
+
+        const poses = this.__target_poses;
+        const inv_dt = 1 / dt;
+
+        for (let i = 0; i < pending_count; i++) {
+            const body_id = pending[i];
+            if (!this.storage.is_valid(body_id)) continue;
+
+            const idx = body_id_index(body_id);
+            const base = idx * TARGET_POSE_STRIDE;
+            if (poses[base] !== body_id) continue;
+
+            const rb = this.__bodies[idx];
+            if (rb.kind !== BodyKind.KinematicPosition) continue;
+
+            const tr = this.__transforms[idx];
+
+            // the transform's lanes, read in place: a component getter V8 leaves out of line hands
+            // its number back boxed
+            v3_set(rb.linearVelocity, 0,
+                (poses[base + 1] - tr[TRANSFORM64_TRANSLATION_OFFSET]) * inv_dt,
+                (poses[base + 2] - tr[TRANSFORM64_TRANSLATION_OFFSET + 1]) * inv_dt,
+                (poses[base + 3] - tr[TRANSFORM64_TRANSLATION_OFFSET + 2]) * inv_dt
+            );
+
+            // World-frame rotation from the current orientation to the target: dq = q_target ⊗ q⁻¹,
+            // the one a world angular velocity integrates (dq/dt = ½ ω ⊗ q). Its axis × angle over
+            // dt is the angular velocity; the shorter of the two arcs a quaternion pair describes.
+            // Both operands go in as arrays, the target straight from its slot.
+            scratch_target_conjugate[0] = -tr[TRANSFORM64_ROTATION_OFFSET];
+            scratch_target_conjugate[1] = -tr[TRANSFORM64_ROTATION_OFFSET + 1];
+            scratch_target_conjugate[2] = -tr[TRANSFORM64_ROTATION_OFFSET + 2];
+            scratch_target_conjugate[3] = tr[TRANSFORM64_ROTATION_OFFSET + 3];
+
+            quat3_multiply_array(scratch_target_dq, 0, poses, base + 4, scratch_target_conjugate, 0);
+
+            let dx = scratch_target_dq[0];
+            let dy = scratch_target_dq[1];
+            let dz = scratch_target_dq[2];
+            let dw = scratch_target_dq[3];
+
+            if (dw < 0) {
+                dx = -dx;
+                dy = -dy;
+                dz = -dz;
+                dw = -dw;
+            }
+
+            const sin_half = Math.sqrt(dx * dx + dy * dy + dz * dz);
+
+            if (sin_half > 0) {
+                const k = 2 * Math.atan2(sin_half, dw) / sin_half * inv_dt;
+                v3_set(rb.angularVelocity, 0, dx * k, dy * k, dz * k);
+            } else {
+                v3_set(rb.angularVelocity, 0, 0, 0, 0);
+            }
+        }
+    }
+
+    /**
+     * After the substep loop of {@link fixedUpdate}: write every body driven by
+     * {@link setTargetPose} onto its target exactly, and empty the pending list. Skipped, like
+     * {@link __derive_target_velocities}, by a step of no duration.
+     *
+     * @private
+     * @param {number} dt
+     */
+    __land_target_poses(dt) {
+        const pending = this.__target_pose_bodies;
+        const pending_count = this.__target_pose_count;
+        if (pending_count === 0 || !(dt > 0)) return;
+
+        const poses = this.__target_poses;
+
+        for (let i = 0; i < pending_count; i++) {
+            const body_id = pending[i];
+            const idx = body_id_index(body_id);
+            const base = idx * TARGET_POSE_STRIDE;
+
+            if (poses[base] !== body_id) continue;
+            poses[base] = RIGID_BODY_UNALLOCATED;
+
+            if (!this.storage.is_valid(body_id)) continue;
+
+            const rb = this.__bodies[idx];
+            if (rb.kind !== BodyKind.KinematicPosition) continue;
+
+            const tr = this.__transforms[idx];
+
+            // slot to lanes, no number handed to a call: V8 left setRotation out of line here, and a
+            // call that is not inlined boxes the numbers it is passed
+            tr[TRANSFORM64_TRANSLATION_OFFSET] = poses[base + 1];
+            tr[TRANSFORM64_TRANSLATION_OFFSET + 1] = poses[base + 2];
+            tr[TRANSFORM64_TRANSLATION_OFFSET + 2] = poses[base + 3];
+            tr[TRANSFORM64_ROTATION_OFFSET] = poses[base + 4];
+            tr[TRANSFORM64_ROTATION_OFFSET + 1] = poses[base + 5];
+            tr[TRANSFORM64_ROTATION_OFFSET + 2] = poses[base + 6];
+            tr[TRANSFORM64_ROTATION_OFFSET + 3] = poses[base + 7];
+
+            // the orientation was written, so the matrix is behind the components
+            tr.updateMatrix();
+        }
+
+        this.__target_pose_count = 0;
+    }
+
     /**
      * Force the body awake. Static bodies are ignored.
      * @param {RigidBody} rigidBody
@@ -1395,10 +1929,18 @@ export class PhysicsSystem extends System {
      * Raycast against both broadphase trees. Fills `result` with the nearest
      * hit and returns `true` on hit, `false` on miss.
      *
-     * Resolves to the BVH leaf, not the shape: `result.t` is the distance to
-     * the leaf's inflated AABB and `result.normal` is that AABB's face
-     * normal. Exact for AABB-shaped colliders, approximate for the rest —
-     * use {@link shapeCast} when you need the true surface.
+     * Resolves against the real shapes: `result.t` is the distance to the
+     * struck surface and `result.normal` its normal there, for every shape the
+     * engine defines (a shape from outside it falls back to its tight bounding
+     * box). Every collider of a compound body is tested; `result.collider_entity`
+     * names the one struck, and `result.triangle` the triangle of a mesh or
+     * heightmap collider. Sensors are passed through.
+     *
+     * The filter may run a {@link shapeCast} or an {@link overlap} of its own,
+     * but not another raycast (refused by an assert: queries of one kind share
+     * their working state). A production build strips asserts, and there
+     * nothing refuses it: the answer is silently wrong — a ray whose filter cast
+     * another ray named entity 8 at t 1.5, where the truth is entity 1 at 2.5.
      *
      * @param {Ray3} ray  origin + unit direction + `tMax`
      * @param {PhysicsSurfacePoint} result populated on hit; untouched on miss
@@ -1415,15 +1957,23 @@ export class PhysicsSystem extends System {
      * The shape starts at `ray.origin` oriented by `rotation` and translates
      * along `ray.direction` for up to `ray.tMax` units.
      *
-     * Unlike {@link raycast} this resolves against the real shapes, so `t` is
-     * a true time of impact rather than a distance to a bounding box.
+     * `t` is the time of impact — the swept shape at `t` is just apart from
+     * what it struck — and `result.position` the swept shape's centre there,
+     * not a point on the surface. `result.collider_entity` names the struck
+     * collider and `result.triangle` the triangle of a mesh or heightmap one.
+     *
+     * The filter may run a {@link raycast} or an {@link overlap} of its own,
+     * but not another shapeCast (refused by an assert: queries of one kind
+     * share their working state). A production build strips asserts, and there
+     * nothing refuses it: the outer cast silently misses what it should hit.
      *
      * @param {Ray3} ray  origin + unit direction + `tMax`
      * @param {AbstractShape3D} shape
      * @param {ArrayLike<number>} rotation fixed orientation of the swept shape
-     *   as `[x, y, z, w]`, a unit quaternion — which a Transform64's `rotation`
-     *   already is. Read by index, so an `{x, y, z, w}` object silently misses
-     *   everything rather than throwing
+     *   as `[x, y, z, w]`, a unit quaternion, in an array the caller retains —
+     *   not a Transform64's `rotation`, which allocates a view on every read.
+     *   Read by index, so an `{x, y, z, w}` object silently misses everything
+     *   rather than throwing
      * @param {PhysicsSurfacePoint} result populated on hit; untouched on miss
      * @param {(entity:number, collider:Collider)=>boolean} [filter]
      * @returns {boolean}
@@ -1444,13 +1994,17 @@ export class PhysicsSystem extends System {
      * available.
      *
      * `filter` runs before the shape test, which is the expensive part — use
-     * it to skip the caller's own body, allies, sensors.
+     * it to skip the caller's own body, allies, sensors. It may run a
+     * {@link raycast} or a {@link shapeCast} of its own, but not another overlap
+     * (refused by an assert: queries of one kind share their working state). A
+     * production build strips asserts, and there nothing refuses it: the outer
+     * overlap silently comes back short — 0 of the 8 bodies it overlapped.
      *
      * The query shape must be convex; a concave one throws.
      *
      * @param {AbstractShape3D} shape convex query shape, in its local frame
-     * @param {ArrayLike<number>} position world position of the query shape, as `[x, y, z]` -
-     *   which a Transform64's translation already is
+     * @param {ArrayLike<number>} position world position of the query shape, as `[x, y, z]`, in an
+     *   array the caller retains - not a Transform64's `translation`, which allocates a view per read
      * @param {ArrayLike<number>} rotation world rotation as `[x, y, z, w]`, a unit quaternion
      * @param {Uint32Array|number[]} output buffer to receive body_ids
      * @param {number} output_offset starting index in output
@@ -1690,7 +2244,8 @@ export class PhysicsSystem extends System {
      *
      *  - If any member carries {@link RigidBodyFlags.DisableSleep}, the
      *    entire island is exempt; every member's sleep_timer is reset.
-     *  - If `max(|v|² + |ω|²)` across all members is below
+     *  - If `max(|v|² + |ω|²·max(R, 1)²)` across all members (`R` the
+     *    member's extent, {@link __body_extent}) is below
      *    {@link sleepVelocitySqrThreshold}, every member's sleep_timer is
      *    incremented by `dt`. When the smallest member's timer crosses
      *    {@link sleepTimeThreshold}, the whole island sleeps atomically in
@@ -1715,6 +2270,8 @@ export class PhysicsSystem extends System {
         const island_count = islands.island_count;
         const body_offsets = islands.body_offsets;
         const body_data = islands.body_data;
+        // Every island member was awake at stage 2, which wrote its extent.
+        const body_extent = this.__body_extent;
 
         for (let isl = 0; isl < island_count; isl++) {
             const start = body_offsets[isl];
@@ -1732,10 +2289,20 @@ export class PhysicsSystem extends System {
                     any_disable_sleep = true;
                     break;
                 }
+                // Speed of the body's far end: angular velocity times the
+                // body's extent, floored at a metre — so a body whose extent is
+                // at most a metre is judged on |v|² + |ω|² exactly as it always
+                // was, and a 100 m rod turning at 0.05 rad/s, its tip moving at
+                // 5 m/s, is not at rest (Box3D: |v| + |ω × maxExtent|).
+                const extent = body_extent[idx];
+                const reach = extent > 1 ? extent : 1;
                 const lv = rb.linearVelocity;
                 const av = rb.angularVelocity;
+                const wx = av[0] * reach;
+                const wy = av[1] * reach;
+                const wz = av[2] * reach;
                 const v_sqr = lv[0] * lv[0] + lv[1] * lv[1] + lv[2] * lv[2]
-                    + av[0] * av[0] + av[1] * av[1] + av[2] * av[2];
+                    + wx * wx + wy * wy + wz * wz;
                 if (v_sqr > max_v_sqr) max_v_sqr = v_sqr;
             }
 
@@ -1896,6 +2463,11 @@ export class PhysicsSystem extends System {
         const joints = this.__joints;
         const manifolds = this.manifolds;
 
+        // Stage -1: turn each pending setTargetPose target into the velocity that reaches it over
+        // this step, before anything reads velocity — the parked-platform test in the wake
+        // propagation below, the broadphase pad, the solver.
+        this.__derive_target_velocities(dt);
+
         // Stage 0: wake propagation — through the persistent manifolds
         // (last frame's broadphase-touched pairs), then through joints.
         // Runs before everything else so a just-woken body is part of this
@@ -1930,6 +2502,13 @@ export class PhysicsSystem extends System {
         // pose, padded by the swept extent for the body's velocity. The fat
         // margin uses the post-force velocity; this frame's gravity increment
         // is a sub-millimetre slack difference, safely inside the margin.
+        // Records each awake body's extent for the sleep test on the way, from
+        // the same local boxes.
+        if (this.__body_extent.length < storage.high_water_mark) {
+            this.__body_extent = new Float64Array(storage.high_water_mark * 2);
+        }
+
+        const body_extent = this.__body_extent;
         const lists = this.__body_collider_lists;
         for (let i = 0; i < count; i++) {
 
@@ -1937,17 +2516,58 @@ export class PhysicsSystem extends System {
             const rb = bodies[idx];
             const list = lists[idx];
 
+            body_extent[idx] = 0;
+
             if (list === undefined) {
                 continue;
             }
 
             const lv = rb.linearVelocity;
+            const body_transform = transforms[idx];
 
             const list_length = list.length;
 
+            let extent = 0;
+
             for (let k = 0; k < list_length; k++) {
 
                 const entry = list[k];
+                const collider_transform = entry.transform;
+
+                // The tight world box, as compute_world_aabb builds it, from a
+                // local box this loop keeps for the extent below.
+                entry.collider.shape.compute_bounding_box(scratch_local_aabb);
+
+                // The pose goes in an array: handed to aabb3_transform_oriented as seven numbers
+                // (it is not inlined into this step), each would be boxed, per collider per step.
+                // It, and the offset below, are read off the transforms' lanes rather than through
+                // their accessors, which this step does not inline either and which return their
+                // numbers boxed.
+                const pose = scratch_refit_pose;
+                pose[0] = collider_transform[TRANSFORM64_TRANSLATION_OFFSET];
+                pose[1] = collider_transform[TRANSFORM64_TRANSLATION_OFFSET + 1];
+                pose[2] = collider_transform[TRANSFORM64_TRANSLATION_OFFSET + 2];
+                pose[3] = collider_transform[TRANSFORM64_ROTATION_OFFSET];
+                pose[4] = collider_transform[TRANSFORM64_ROTATION_OFFSET + 1];
+                pose[5] = collider_transform[TRANSFORM64_ROTATION_OFFSET + 2];
+                pose[6] = collider_transform[TRANSFORM64_ROTATION_OFFSET + 3];
+
+                aabb3_transform_oriented_array(scratch_world_aabb, 0, scratch_local_aabb, 0, pose, 0);
+
+                // Farthest corner of the local box from the collider's origin,
+                // plus the collider's offset from the body's centre: no point
+                // of the collider lies farther from the body's centre.
+                const ex = Math.max(-scratch_local_aabb[0], scratch_local_aabb[3]);
+                const ey = Math.max(-scratch_local_aabb[1], scratch_local_aabb[4]);
+                const ez = Math.max(-scratch_local_aabb[2], scratch_local_aabb[5]);
+                const ox = pose[0] - body_transform[TRANSFORM64_TRANSLATION_OFFSET];
+                const oy = pose[1] - body_transform[TRANSFORM64_TRANSLATION_OFFSET + 1];
+                const oz = pose[2] - body_transform[TRANSFORM64_TRANSLATION_OFFSET + 2];
+                const reach = Math.sqrt(ex * ex + ey * ey + ez * ez) + Math.sqrt(ox * ox + oy * oy + oz * oz);
+
+                if (reach > extent) {
+                    extent = reach;
+                }
 
                 // Refit skip — the fat margin's whole purpose: while the
                 // body's FATTENED box (tight + velocity pad) stays inside the
@@ -1965,8 +2585,7 @@ export class PhysicsSystem extends System {
                 // otherwise keep a leaf covering none of the path it travels
                 // this step — invisible to pair generation and to other
                 // bodies' CCD sweeps for the whole step.
-                compute_world_aabb(scratch_world_aabb, 0, entry.collider.shape, entry.transform);
-                fatten_world_aabb(scratch_world_aabb, 0, lv[0], lv[1], lv[2], dt);
+                fatten_world_aabb_array(scratch_world_aabb, 0, lv, 0, dt);
                 const node = entry.bvhNode;
                 this.dynamicBvh.node_get_aabb(node, scratch_stored_aabb);
                 if (scratch_world_aabb[0] >= scratch_stored_aabb[0]
@@ -1978,8 +2597,14 @@ export class PhysicsSystem extends System {
                     continue;
                 }
 
+                // Outward, so the stored leaf still contains this fat box: the
+                // next step's skip test is then exact for a body that has not
+                // moved, as well as conservative for every query in between.
+                aabb3_round_outward_f32(scratch_world_aabb);
                 this.dynamicBvh.node_move_aabb(node, scratch_world_aabb);
             }
+
+            body_extent[idx] = extent;
         }
 
         // Stage 3: broadphase pair generation. The fat AABBs cover the full
@@ -2016,8 +2641,8 @@ export class PhysicsSystem extends System {
         // at a resting contact (each cancels `h` of the other), so stacks hold
         // at zero velocity and sleep; the position correction adapts as bodies
         // separate between substeps — the TGS stack-stability mechanism —
-        // without re-running narrowphase. Restitution is applied once after
-        // the loop.
+        // without re-running narrowphase. Restitution is applied after the
+        // loop.
         // Clamp the substep count: 0 (or a negative / fractional value) would
         // feed h = dt / N = Infinity into prepare_contacts and a broken loop
         // bound into the solver.
@@ -2029,8 +2654,10 @@ export class PhysicsSystem extends System {
         // post-wake awake set (poses are unchanged until the substep loop below
         // integrates them). The CCD pass after the solver sweeps from here to
         // each body's final pose. Reads the primary collider's transform so the
-        // start matches the end the resolve pass reads. Zero-cost when no body
-        // is flagged.
+        // start matches the end the resolve pass reads. When any body is
+        // flagged, also the start-of-step velocity of every awake dynamic
+        // body, any of which a sweep can strike. Costs nothing more than the
+        // walk when no body is flagged.
         const ccd_on = this.ccdEnabled;
 
         if (ccd_on) {
@@ -2039,9 +2666,12 @@ export class PhysicsSystem extends System {
 
             if (this.__ccd_start_pos.length < ccd_need) {
                 this.__ccd_start_pos = new Float64Array(ccd_need);
+                this.__ccd_start_vel = new Float64Array(ccd_need);
             }
 
             const ccd_start = this.__ccd_start_pos;
+            const ccd_movers_before = this.__ccd_mover_count;
+            let ccd_count = 0;
 
             for (let i = 0; i < count_after_wake; i++) {
                 const idx = storage.awake_at(i);
@@ -2066,12 +2696,47 @@ export class PhysicsSystem extends System {
                 ccd_start[cb] = cp.translation_x;
                 ccd_start[cb + 1] = cp.translation_y;
                 ccd_start[cb + 2] = cp.translation_z;
+
+                if (ccd_count === this.__ccd_movers.length) {
+                    const grown = new Uint32Array(this.__ccd_movers.length * 2);
+                    grown.set(this.__ccd_movers);
+                    this.__ccd_movers = grown;
+                }
+                this.__ccd_movers[ccd_count] = idx;
+                this.__ccd_mover_colliders[ccd_count] = list[0].collider;
+                ccd_count++;
+            }
+
+            // Let go of last step's colliders past this step's count.
+            for (let k = ccd_count; k < ccd_movers_before; k++) {
+                this.__ccd_mover_colliders[k] = null;
+            }
+            this.__ccd_mover_count = ccd_count;
+
+            if (ccd_count > 0) {
+                const ccd_start_vel = this.__ccd_start_vel;
+
+                for (let i = 0; i < count_after_wake; i++) {
+                    const idx = storage.awake_at(i);
+                    const rb = bodies[idx];
+
+                    if (rb.kind !== BodyKind.Dynamic) {
+                        continue;
+                    }
+
+                    const lv = rb.linearVelocity;
+                    const cb = idx * 3;
+
+                    ccd_start_vel[cb] = lv[0];
+                    ccd_start_vel[cb + 1] = lv[1];
+                    ccd_start_vel[cb + 2] = lv[2];
+                }
             }
         }
 
         // Size the pseudo-velocity buffer ONCE (it may reallocate on growth),
-        // then capture the reference. Inside the loop we only zero its live
-        // region per substep — re-capturing is unnecessary since it won't
+        // then capture the reference. Inside the loop we only zero what a
+        // position pass wrote — re-capturing is unnecessary since it won't
         // reallocate again this step.
         this.__reset_pseudo_velocity();
         const pseudoVel = this.__pseudo_velocity;
@@ -2154,15 +2819,17 @@ export class PhysicsSystem extends System {
             // Joints share the substep: warm-start + velocity-solve the 6-DOF
             // constraints on real velocity, coupled with the contacts above
             // (a body touched by both sees one substep of Gauss-Seidel across
-            // contacts then joints). Position correction for locked DOFs is a
-            // SPOOK bias inside this solve, so no separate joint position pass.
-            // Solved in the entity-canonical order (NOT joint-id order, which
-            // encodes link/unlink history) — see __joints_sorted.
+            // contacts then joints), in jointIterations passes over all the
+            // joints. Position correction is a SPOOK bias inside this solve, so
+            // no separate joint position pass (the relax below takes the limits'
+            // push back out). Solved in the entity-canonical order (NOT joint-id
+            // order, which encodes link/unlink history) — see __joints_sorted.
+            const joints_reverse = (s & 1) === 1;
             if (this.__joints_sorted.length > 0) {
                 // Alternate the joint sweep direction on odd substeps so the chain
                 // is solved root→tip and tip→root across the step (symmetric
                 // Gauss-Seidel), cancelling the one-direction propagation bias.
-                solve_joints(this.__joints_sorted, this, h, this.jointIterations, (s & 1) === 1);
+                solve_joints(this.__joints_sorted, this, h, this.jointIterations, joints_reverse);
             }
 
             // Position correction writes pseudo-velocity (zeroed first so it
@@ -2170,21 +2837,20 @@ export class PhysicsSystem extends System {
             // position integrate and then discarded. SOFT contacts skip it:
             // their penetration recovery is the velocity-bias term, and the
             // relax pass below strips the momentum that bias injected
-            // (Box2D-v3 substep order).
+            // (Box2D-v3 substep order). Zeroing is a no-op unless a position
+            // pass has written since, so soft steps never pay for it.
             const contacts_soft = this.contactHertz > 0;
-            pseudoVel.fill(0, 0, pseudo_len);
+            this.__clear_pseudo_velocity();
             if (!contacts_soft) {
                 solve_position(manifolds, this, this.positionIterations);
+                this.__pseudo_velocity_dirty = pseudo_len;
             }
 
             for (let i = 0; i < count_after_wake; i++) {
                 const idx = storage.awake_at(i);
                 const rb = bodies[idx];
                 const tr = transforms[idx];
-                const base = idx * 6;
-                integrate_position(ss_data, idx * SBS_STRIDE, rb, tr, h,
-                    pseudoVel[base], pseudoVel[base + 1], pseudoVel[base + 2],
-                    pseudoVel[base + 3], pseudoVel[base + 4], pseudoVel[base + 5]);
+                integrate_position(ss_data, idx * SBS_STRIDE, rb, tr, h, pseudoVel, idx * 6);
             }
 
             // Relax pass (soft contacts only): one biasless sweep over the
@@ -2194,11 +2860,21 @@ export class PhysicsSystem extends System {
             if (contacts_soft) {
                 solve_velocity(manifolds, this, 1, false);
             }
+
+            // Joint relax, whatever the contact mode: one pass over the limit,
+            // motor and spring rows without their position-correction bias,
+            // removing the velocity a limit pushed its DOF out of violation with
+            // — the joints have no pseudo-velocity channel, and a one-sided row
+            // cannot take its push back, so it would carry the DOF onto its far
+            // stop.
+            if (this.__joints_sorted.length > 0) {
+                relax_joints(this.__joints_sorted, this, joints_reverse);
+            }
         }
 
-        // Stage 8: one-shot restitution, after the substep loop, keyed off
-        // the approach velocity captured at prepare time.
-        apply_restitution(manifolds, this);
+        // Stage 8: restitution, after the substep loop, keyed off the approach
+        // velocity captured at prepare time.
+        apply_restitution(manifolds, this, this.restitutionIterations);
 
         // Scatter the solved persistent linear / angular velocity back onto the
         // RigidBody components (pose was written through to the transforms each
@@ -2206,25 +2882,28 @@ export class PhysicsSystem extends System {
         // interpolation recording, and the sleep test below.
         solver_state.scatter(bodies);
 
+        // Bodies driven by setTargetPose end the step exactly on their targets, before the move is
+        // announced. The substeps put them there to rounding in translation; the rotation's
+        // first-order integration falls short by O(angle³).
+        this.__land_target_poses(dt);
+
         // The transforms moved and carry no signals of their own, so the move is announced here:
         // once per awake body per step, rather than once per substep write. Everything that used to
         // wake on position.onChanged - the render placement, the parent/child sync, fog-of-war -
         // wakes on this instead.
-        {
-            const ecd = (this.entityManager !== null && this.entityManager !== undefined)
-                ? this.entityManager.dataset
-                : null;
+        const ecd = (this.entityManager !== null && this.entityManager !== undefined)
+            ? this.entityManager.dataset
+            : null;
 
-            if (ecd !== null) {
-                const awake_count = storage.awake_count;
+        if (ecd !== null) {
+            const awake_count = storage.awake_count;
 
-                for (let i = 0; i < awake_count; i++) {
-                    const idx = storage.awake_at(i);
-                    const entity = storage.entity_at(idx);
+            for (let i = 0; i < awake_count; i++) {
+                const idx = storage.awake_at(i);
+                const entity = storage.entity_at(idx);
 
-                    if (entity >= 0) {
-                        t64_announce_change(ecd, entity);
-                    }
+                if (entity >= 0) {
+                    t64_announce_change(ecd, entity);
                 }
             }
         }
@@ -2234,8 +2913,27 @@ export class PhysicsSystem extends System {
         // can't tunnel through thin geometry between discrete steps. Runs on the
         // final post-solve poses, before the sleep test sees the clamped
         // velocities. No-op when no awake body is flagged.
+        //
+        // It has to run AFTER the announcement above, which is what composes a
+        // compound body's child colliders: the sweep reads its end pose from the
+        // primary collider's transform. So a body it clamps was announced at the
+        // pose it would have TUNNELLED to, and everything composed from that
+        // pose synchronously — attached children, child colliders, listeners —
+        // is stale. Announce those bodies again, now at their final pose.
         if (ccd_on) {
-            ccd_resolve(this);
+            const clamped = ccd_resolve(this, dt, h, count_after_wake);
+
+            if (clamped > 0 && ecd !== null) {
+                const clamped_bodies = ccd_clamped_bodies();
+
+                for (let i = 0; i < clamped; i++) {
+                    const entity = storage.entity_at(clamped_bodies[i]);
+
+                    if (entity >= 0) {
+                        t64_announce_change(ecd, entity);
+                    }
+                }
+            }
         }
 
         // Stage 9: sleep test.