@woosh/meep-engine 2.156.0 → 2.157.0

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

@@ -1,6 +1,7 @@
 import { assert } from "../../../core/assert.js";
 import { BinaryBuffer } from "../../../core/binary/BinaryBuffer.js";
 import { BVH } from "../../../core/bvh2/bvh3/BVH.js";
+import { bvh_query_user_data_overlaps_aabb } from "../../../core/bvh2/bvh3/query/bvh_query_user_data_overlaps_aabb.js";
 import { returnTrue } from "../../../core/function/returnTrue.js";
 import { aabb3_transform_oriented } from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";
 import Vector3 from "../../../core/geom/Vector3.js";
@@ -11,7 +12,7 @@ import { Transform } from "../../ecs/transform/Transform.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_fat_world_aabb } from "../broadphase/compute_fat_world_aabb.js";
+import { compute_world_aabb, fatten_world_aabb } from "../broadphase/compute_fat_world_aabb.js";
 import { generate_pairs } from "../broadphase/generate_pairs.js";
 import { PairList } from "../broadphase/PairList.js";
 import { ccd_resolve } from "../ccd/linear_sweep.js";
@@ -38,6 +39,7 @@ import {
 } from "../solver/solve_contacts.js";
 import { BodyKind } from "./BodyKind.js";
 import { Collider, COLLIDER_UNBOUND } from "./Collider.js";
+import { find_non_finite_physics_state } from "./find_non_finite_physics_state.js";
 import { JOINT_UNALLOCATED, JOINT_WORLD } from "./Joint.js";
 import { PhysicsEvents } from "./PhysicsEvents.js";
 import { RIGID_BODY_UNALLOCATED, RigidBody } from "./RigidBody.js";
@@ -50,6 +52,13 @@ import { SleepState } from "./SleepState.js";
  */
 const scratch_angular_delta = new Float64Array(3);
 
+/**
+ * Scratch for {@link applyImpulseAt}'s LockRot*-masked inverse-inertia
+ * diagonal (the same mask {@link SolverBodyState#gather} applies).
+ * @type {{x: number, y: number, z: number}}
+ */
+const scratch_locked_inertia = { x: 0, y: 0, z: 0 };
+
 /**
  * Reusable scratch buffer for world-AABB construction so the link path is
  * allocation-free in steady state.
@@ -64,6 +73,29 @@ const scratch_world_aabb = new Float64Array(6);
  */
 const scratch_local_aabb = new Float64Array(6);
 
+/**
+ * Candidate buffer for {@link PhysicsSystem#__wake_sleepers_overlapping_leaf}.
+ * Grows by doubling: a large removed support (a level floor) can overlap many
+ * sleeping bodies, all of which must wake.
+ * @type {Uint32Array}
+ */
+let scratch_wake_candidates = new Uint32Array(64);
+
+/**
+ * Scratch for a leaf's currently-stored fat AABB during the stage-2
+ * containment test (refit skip).
+ * @type {Float64Array}
+ */
+const scratch_stored_aabb = new Float64Array(6);
+
+/**
+ * Scratch boxes for {@link PhysicsSystem#__swept_proximity} — the mover's
+ * velocity-swept tight AABB and the sleeper's tight AABB.
+ * @type {Float64Array}
+ */
+const scratch_prox_mover = new Float64Array(6);
+const scratch_prox_sleeper = new Float64Array(6);
+
 /**
  * Rigid-body physics system.
  *
@@ -118,8 +150,10 @@ export class PhysicsSystem extends System {
         this.manifolds = new ManifoldStore();
 
         /**
-         * Per-frame list of broadphase-overlapping pairs (canonical
-         * `(min, max)`). Cleared at the top of each step.
+         * Per-frame list of broadphase-overlapping pairs, each in
+         * entity-canonical role order (A = the smaller entity id — see
+         * generate_pairs). Cleared at the top of each step; purely
+         * intra-step state.
          * @type {PairList}
          */
         this.pairs = new PairList();
@@ -197,6 +231,26 @@ export class PhysicsSystem extends System {
          */
         this.positionIterations = 1;
 
+        /**
+         * Contact softness frequency, Hz (Box2D-v3 soft step, hybrid form:
+         * the velocity rows get the hertz/ζ-derived mass + impulse-decay
+         * scales; penetration recovery stays in the split-impulse position
+         * pass). The decay term is what lets resting stacks converge
+         * asymptotically instead of ringing in the hard form's limit cycle
+         * just above the sleep threshold. Clamped per step to an eighth of
+         * the substep rate. `0` = hard contacts (bit-exact legacy rows).
+         * Pairs with a non-dynamic side use 2× this frequency (stiffer, so
+         * piles don't sag into the ground). @type {number}
+         */
+        this.contactHertz = 30;
+
+        /**
+         * Contact damping ratio ζ for the soft scales — heavily over-damped
+         * by design (Box2D v3 default), so contacts never oscillate.
+         * @type {number}
+         */
+        this.contactDampingRatio = 10;
+
         /**
          * Reusable contact-event payload. Listeners must copy any fields they
          * intend to retain past their own scope. Reset before each dispatch.
@@ -266,6 +320,22 @@ export class PhysicsSystem extends System {
          */
         this.__joint_free = [];
 
+        /**
+         * Live joints in CANONICAL SOLVE ORDER: ascending `(entityA,
+         * entityB)`, equal keys in link order. The id-indexed
+         * {@link __joints} registry reflects link/unlink HISTORY (slot
+         * reuse), which two engines holding identical joints need not
+         * share — the joint solver's Gauss-Seidel sweep must consume
+         * joints in an order derivable from state alone or the engines
+         * diverge. Maintained by {@link link_joint} / {@link unlink_joint};
+         * consumed by the substep loop. Multiple joints between the SAME
+         * entity pair keep their relative link order — reconstruction
+         * replays links in the same order, so that too is state-derived.
+         * @private
+         * @type {Joint[]}
+         */
+        this.__joints_sorted = [];
+
         /**
          * Velocity iterations per substep for the joint solver. Joints (and
          * especially joint chains) want a few more iterations than contacts to
@@ -491,6 +561,43 @@ export class PhysicsSystem extends System {
         bvh.release_node(node);
     }
 
+    /**
+     * Wake every sleeping dynamic body whose broadphase leaf overlaps the
+     * given leaf's AABB. Called BEFORE removing a NON-Dynamic body's leaf
+     * (unlink / collider detach): statics and kinematics join no sleep group,
+     * their sleepers' manifolds are dormant-frozen, and the broadphase never
+     * revisits sleeping bodies — so without this, a pile resting on the
+     * removed support would sleep in mid-air forever.
+     *
+     * Dynamic removals don't need it: a sleeping pile and its dynamic support
+     * slept as one island and share a sleep group, which unlink already
+     * dissolves by waking the group.
+     *
+     * @private
+     * @param {BVH} bvh tree currently holding `node`
+     * @param {number} node leaf about to be removed
+     */
+    __wake_sleepers_overlapping_leaf(bvh, node) {
+        bvh.node_get_aabb(node, scratch_world_aabb);
+
+        let n;
+        for (;;) {
+            n = bvh_query_user_data_overlaps_aabb(scratch_wake_candidates, 0, this.dynamicBvh, scratch_world_aabb);
+            if (n <= scratch_wake_candidates.length) break;
+            // TRUE count returned, overflow writes dropped — grow and re-query.
+            scratch_wake_candidates = new Uint32Array(Math.max(n, scratch_wake_candidates.length * 2));
+        }
+
+        for (let i = 0; i < n; i++) {
+            const body_id = scratch_wake_candidates[i];
+            if (!this.storage.is_valid(body_id)) continue;
+            const rb = this.__bodies[body_id_index(body_id)];
+            if (rb === undefined) continue;
+            if (rb.sleepState !== SleepState.Sleeping) continue;
+            this.__wake_body(rb);
+        }
+    }
+
     /**
      * Lifecycle entry: invoked when an entity gains the (RigidBody, Transform)
      * tuple. Allocates the body's slot in storage and seeds an empty collider
@@ -505,7 +612,6 @@ export class PhysicsSystem extends System {
     link(rigidBody, transform, entity) {
         const packed = this.storage.allocate(entity);
         rigidBody._bodyId = packed;
-        rigidBody.sleepState = SleepState.Awake;
 
         const index = body_id_index(packed);
         this.storage.set_kind(index, rigidBody.kind);
@@ -515,8 +621,24 @@ export class PhysicsSystem extends System {
         this.__transforms[index] = transform;
         this.__body_collider_lists[index] = [];
 
+        // The component's sleep fields are AUTHORITATIVE persistent state
+        // (see RigidBody.sleepState): a body restored from a snapshot — or
+        // re-linked after an unlink — resumes exactly as drowsy or asleep
+        // as it was, instead of being forced awake to re-settle. That keeps
+        // a reconstructed world on the original's wake/sleep schedule (the
+        // reconstruction-determinism contract). A freshly-constructed
+        // component defaults to Awake, so plain spawns are unchanged.
+        //
+        // A restored sleeper arrives as a sleep-group SINGLETON (the group
+        // links are world-instance-tied body indices, reset here); group
+        // membership is re-established by the contact-state import or, for
+        // a body slept in isolation, is already correct.
+        rigidBody.sleep_group_next = -1;
+        rigidBody.sleep_group_prev = -1;
+
         // Static bodies do not need to live in the active list — they never move.
-        if (rigidBody.kind === BodyKind.Static) {
+        if (rigidBody.kind === BodyKind.Static
+            || (rigidBody.kind === BodyKind.Dynamic && rigidBody.sleepState === SleepState.Sleeping)) {
             this.storage.mark_sleeping(index);
         }
     }
@@ -545,9 +667,17 @@ export class PhysicsSystem extends System {
         const index = body_id_index(packed);
         const list = this.__body_collider_lists[index];
         if (list !== undefined) {
+            // A removed static / kinematic body may have been the support
+            // under sleeping bodies — wake them before the leaves disappear
+            // (their AABBs are the query keys).
+            const wake_neighbours = rigidBody.kind !== BodyKind.Dynamic;
+            const bvh = rigidBody.kind === BodyKind.Static ? this.staticBvh : this.dynamicBvh;
             for (let i = 0; i < list.length; i++) {
                 const entry = list[i];
                 if (entry.bvhNode !== COLLIDER_UNBOUND) {
+                    if (wake_neighbours) {
+                        this.__wake_sleepers_overlapping_leaf(bvh, entry.bvhNode);
+                    }
                     this.__remove_from_broadphase(rigidBody, entry.bvhNode);
                 }
                 entry.collider._bvhNode = COLLIDER_UNBOUND;
@@ -559,6 +689,21 @@ export class PhysicsSystem extends System {
         this.__transforms[index] = undefined;
         this.__body_collider_lists[index] = undefined;
 
+        // Purge joints referencing the freed body. The solver already skips
+        // them via the generation-checked is_valid, but the 8-bit generation
+        // wraps after 256 reuse cycles of one slot (~seconds under a pooled
+        // spawner) — at which point a stale joint handle ALIASES an unrelated
+        // new body and silently reactivates against it. Removing the joint at
+        // the source closes the ABA window outright.
+        const joints = this.__joints;
+        for (let i = 0; i < joints.length; i++) {
+            const joint = joints[i];
+            if (joint === undefined || joint === null) continue;
+            if (joint._bodyIdA === packed || joint._bodyIdB === packed) {
+                this.unlink_joint(joint);
+            }
+        }
+
         this.storage.free(packed);
         rigidBody._bodyId = RIGID_BODY_UNALLOCATED;
     }
@@ -609,6 +754,12 @@ export class PhysicsSystem extends System {
         if (body_index === -1) return;
 
         const rb = this.__bodies[body_index];
+        // Same rule as unlink: removing a static / kinematic leaf may pull
+        // the support from under sleeping bodies — wake them first.
+        if (rb.kind !== BodyKind.Dynamic) {
+            const bvh = rb.kind === BodyKind.Static ? this.staticBvh : this.dynamicBvh;
+            this.__wake_sleepers_overlapping_leaf(bvh, collider._bvhNode);
+        }
         this.__remove_from_broadphase(rb, collider._bvhNode);
 
         const list = this.__body_collider_lists[body_index];
@@ -659,6 +810,11 @@ export class PhysicsSystem extends System {
             joint._bodyIdB = this.__bodies[idxB]._bodyId;
         }
 
+        // A (re-)linked joint starts cold: the accumulated warm-start impulse
+        // belongs to whatever configuration the joint solved BEFORE it was
+        // unlinked — replaying it against the new bodies is a phantom kick.
+        joint.dofImpulse.fill(0);
+
         // Reuse a freed slot if available so joint ids stay dense-ish.
         let id;
         if (this.__joint_free.length > 0) {
@@ -668,6 +824,20 @@ export class PhysicsSystem extends System {
         }
         joint._jointId = id;
         this.__joints[id] = joint;
+
+        // Insert at the upper bound of the joint's (entityA, entityB) key in
+        // the canonical solve-order list — equal keys keep link order.
+        const sorted = this.__joints_sorted;
+        let pos = sorted.length;
+        while (pos > 0) {
+            const prev = sorted[pos - 1];
+            if (prev.entityA < joint.entityA
+                || (prev.entityA === joint.entityA && prev.entityB <= joint.entityB)) {
+                break;
+            }
+            pos--;
+        }
+        sorted.splice(pos, 0, joint);
     }
 
     /**
@@ -681,6 +851,10 @@ export class PhysicsSystem extends System {
         this.__joints[id] = undefined;
         this.__joint_free.push(id);
         joint._jointId = JOINT_UNALLOCATED;
+
+        const sorted = this.__joints_sorted;
+        const pos = sorted.indexOf(joint);
+        if (pos !== -1) sorted.splice(pos, 1);
     }
 
     /**
@@ -754,7 +928,15 @@ export class PhysicsSystem extends System {
         const ty = rz * impulse.x - rx * impulse.z;
         const tz = rx * impulse.y - ry * impulse.x;
 
-        world_inverse_inertia_apply(scratch_angular_delta, 0, rigidBody.inverseInertiaLocal, transform.rotation, tx, ty, tz);
+        // LockRot* masks the BODY-LOCAL inverse-inertia diagonal here exactly
+        // as the solver gather does, so a user impulse cannot spin a locked
+        // DOF either.
+        const flags = rigidBody.flags;
+        const ii = rigidBody.inverseInertiaLocal;
+        scratch_locked_inertia.x = (flags & RigidBodyFlags.LockRotX) !== 0 ? 0 : ii.x;
+        scratch_locked_inertia.y = (flags & RigidBodyFlags.LockRotY) !== 0 ? 0 : ii.y;
+        scratch_locked_inertia.z = (flags & RigidBodyFlags.LockRotZ) !== 0 ? 0 : ii.z;
+        world_inverse_inertia_apply(scratch_angular_delta, 0, scratch_locked_inertia, transform.rotation, tx, ty, tz);
 
         rigidBody.angularVelocity.set(
             rigidBody.angularVelocity.x + scratch_angular_delta[0],
@@ -861,6 +1043,11 @@ export class PhysicsSystem extends System {
      * as-is; zero it via {@link setLinearVelocity} if the teleport should also
      * stop the body.
      *
+     * Works for STATIC bodies too: their broadphase leaves (which the step
+     * never refits) are re-homed to the new pose, and sleeping bodies at both
+     * the old and the new location are woken — a moved support drops what
+     * rested on it, and whatever the static moved into must react.
+     *
      * @param {RigidBody} rigidBody
      * @param {Vector3|{x:number,y:number,z:number}} position world position
      * @param {Quaternion|{x:number,y:number,z:number,w:number}} rotation world unit-quaternion rotation
@@ -875,7 +1062,29 @@ export class PhysicsSystem extends System {
         transform.position.set(position.x, position.y, position.z);
         transform.rotation.set(rotation.x, rotation.y, rotation.z, rotation.w);
 
-        this.__wake_body(rigidBody);
+        if (rigidBody.kind === BodyKind.Static) {
+            // The step never refits a static's leaves (stage 2 covers awake
+            // bodies only), so a moved static would keep its stale broadphase
+            // AABB forever: nothing collides at the new location, ghost pairs
+            // at the old one. Re-home each leaf now, and wake the sleepers at
+            // BOTH locations — anything resting on the support that just
+            // moved away, and anything the support just moved into.
+            const list = this.__body_collider_lists[idx];
+            if (list !== undefined) {
+                for (let i = 0; i < list.length; i++) {
+                    const entry = list[i];
+                    if (entry.bvhNode === COLLIDER_UNBOUND) continue;
+                    this.__wake_sleepers_overlapping_leaf(this.staticBvh, entry.bvhNode);
+                    this.__remove_from_broadphase(rigidBody, entry.bvhNode);
+                    const node = this.__insert_into_broadphase(rigidBody, entry.collider, entry.transform);
+                    entry.bvhNode = node;
+                    entry.collider._bvhNode = node;
+                    this.__wake_sleepers_overlapping_leaf(this.staticBvh, node);
+                }
+            }
+        } else {
+            this.__wake_body(rigidBody);
+        }
 
         // Flag a snap on the body's Interpolated component, if any, so the
         // interpolation producer (restore) and consumer (blend) treat this pose
@@ -913,6 +1122,12 @@ export class PhysicsSystem extends System {
         if (rigidBody.sleepState === SleepState.Sleeping) {
             return;
         }
+        // An unlinked (or stale) body has no storage slot — indexing with its
+        // dead id would corrupt the awake-list bookkeeping of whatever body
+        // now owns that index.
+        if (!this.storage.is_valid(rigidBody._bodyId)) {
+            return;
+        }
         rigidBody.sleepState = SleepState.Sleeping;
         const index = body_id_index(rigidBody._bodyId);
         this.storage.mark_sleeping(index);
@@ -936,6 +1151,9 @@ export class PhysicsSystem extends System {
     __wake_body(rb) {
         if (rb.kind !== BodyKind.Dynamic) return;
         if (rb.sleepState === SleepState.Awake) return;
+        // Same stale-id guard as sleep(): an unlinked body's dead id must not
+        // index into (and corrupt) another body's awake-list slot.
+        if (!this.storage.is_valid(rb._bodyId)) return;
         const index = body_id_index(rb._bodyId);
 
         // Remember the next-in-chain before clearing the body's own pointers;
@@ -1112,30 +1330,145 @@ export class PhysicsSystem extends System {
     }
 
     /**
-     * Wake any sleeping body that appears in this step's broadphase pair list.
-     * A pair means the BVH AABBs overlap — even if the sleeper hasn't moved,
-     * an awake neighbour has come into contact range and the sleeper must
-     * participate in narrowphase / solve.
+     * Wake propagation through contact pairs. Runs BEFORE broadphase so a
+     * body woken here participates in THIS frame's broadphase / narrowphase
+     * / islands end to end — it never simulates a step without its support
+     * contacts (free-fall penetration pop).
+     *
+     * Reads ONLY persistent manifold state (the slots whose pair the
+     * broadphase touched last frame — `prev_touched`), never the transient
+     * pair list: manifold state is part of the engine's exportable contact
+     * state, so a reconstructed engine wakes on exactly the original's
+     * schedule.
+     *
+     * A sleeper wakes when:
+     *  - its pair holds REAL contacts (`contact_count > 0`), unless the
+     *    awake side is a PARKED non-Dynamic (a stationary kinematic platform
+     *    or a static — sleepState of either is never updated — transmits no
+     *    motion; the moment it gets velocity, the riders wake); or
+     *  - the pair has no contacts yet but the awake side's velocity-swept
+     *    tight AABB just REACHED the sleeper's tight AABB (false→true edge
+     *    of {@link __swept_proximity}) — one precautionary wake so an
+     *    incoming body never spends its first touching frame against a
+     *    non-integrating sleeper, over-embedding by v·dt. Edge-triggered on
+     *    a pure function of body state: pair CREATION time (the old
+     *    trigger) depends on broadphase fat-box history, which a
+     *    reconstructed engine does not share; and level-triggered proximity
+     *    would re-wake a pile next to a hovering body every frame. Slot
+     *    existence alone wakes nothing.
      * @private
+     * @param {number} dt step size — extent of the predicate's velocity sweep
      */
-    __wake_pairs() {
-        const list = this.pairs;
-        const n = list.count;
+    __wake_pairs(dt) {
         const bodies = this.__bodies;
-        for (let i = 0; i < n; i++) {
-            const idA = list.get_a(i);
-            const idB = list.get_b(i);
-            const idxA = body_id_index(idA);
-            const idxB = body_id_index(idB);
-            const a = bodies[idxA];
-            const b = bodies[idxB];
-            if (a !== undefined && a.sleepState === SleepState.Sleeping) {
-                this.__wake_body(a);
+        const storage = this.storage;
+        const manifolds = this.manifolds;
+        const live_count = manifolds.count;
+        for (let i = 0; i < live_count; i++) {
+            const slot = manifolds.live_at(i);
+            // "Was in last frame's broadphase": acquire set touched, the
+            // end-of-step advance rolled it to prev_touched. (A dormant
+            // slot can carry a stale prev_touched from its sleep frame —
+            // the both-asleep guard below skips it.)
+            if (!manifolds.was_touched_prev(slot)) continue;
+
+            const idA = manifolds.bodyA(slot);
+            const idB = manifolds.bodyB(slot);
+            // Stale-slot guards: a body may have been unlinked since (and
+            // its index reused by an unrelated body).
+            if (!storage.is_valid(idA) || !storage.is_valid(idB)) continue;
+            const a = bodies[body_id_index(idA)];
+            const b = bodies[body_id_index(idB)];
+            if (a === undefined || b === undefined) continue;
+
+            const aSleep = a.sleepState === SleepState.Sleeping;
+            const bSleep = b.sleepState === SleepState.Sleeping;
+            if (aSleep === bSleep) continue; // both awake, or a dormant pair
+
+            if (manifolds.contact_count(slot) > 0) {
+                const mover = aSleep ? b : a;
+                if (mover.kind !== BodyKind.Dynamic) {
+                    const lv = mover.linearVelocity;
+                    const av = mover.angularVelocity;
+                    if (lv[0] === 0 && lv[1] === 0 && lv[2] === 0
+                        && av[0] === 0 && av[1] === 0 && av[2] === 0) {
+                        continue; // parked platform / static — transmits no motion
+                    }
+                }
+                this.__wake_body(aSleep ? a : b);
+            } else {
+                // No contacts: pre-contact precautionary wake on the
+                // proximity EDGE. The predicate needs no parked exemption —
+                // a zero-velocity mover's swept box is its tight box, so it
+                // only fires on actual tight-box overlap, once.
+                const mover_idx = body_id_index(aSleep ? idB : idA);
+                const sleeper_idx = body_id_index(aSleep ? idA : idB);
+                const prox = this.__swept_proximity(mover_idx, sleeper_idx, dt);
+                const had_prox = manifolds.is_prox(slot);
+                manifolds.set_prox(slot, prox);
+                if (prox && !had_prox) {
+                    this.__wake_body(aSleep ? a : b);
+                }
             }
-            if (b !== undefined && b.sleepState === SleepState.Sleeping) {
-                this.__wake_body(b);
+        }
+    }
+
+    /**
+     * Stage-0 pre-contact wake predicate: does any of `mover`'s collider
+     * tight AABBs, swept directionally by the mover's linear velocity over
+     * `dt`, overlap any of `sleeper`'s collider tight AABBs?
+     *
+     * A pure function of CURRENT body state (poses, shapes, velocity) — no
+     * broadphase or pad history — so every engine holding the same bodies
+     * evaluates it identically. Conservative containment: the swept box is
+     * inside the mover's stage-2 fat box (pad = 2·|v|·dt + slack ⊇ v·dt)
+     * and the sleeper's tight box is inside its stored leaf, so whenever
+     * the predicate is true the broadphase has the pair THIS frame in any
+     * engine — the manifold slot the wake stage needs is guaranteed to
+     * exist on the frame the edge fires.
+     *
+     * Linear sweep only (matching the broadphase pad): a purely angular
+     * approach is woken one frame later by its first actual contact.
+     *
+     * @private
+     * @param {number} mover_idx body index of the awake side
+     * @param {number} sleeper_idx body index of the sleeping side
+     * @param {number} dt
+     * @returns {boolean}
+     */
+    __swept_proximity(mover_idx, sleeper_idx, dt) {
+        const mover_list = this.__body_collider_lists[mover_idx];
+        const sleeper_list = this.__body_collider_lists[sleeper_idx];
+        if (mover_list === undefined || sleeper_list === undefined) return false;
+
+        const lv = this.__bodies[mover_idx].linearVelocity;
+        const px = lv[0] * dt;
+        const py = lv[1] * dt;
+        const pz = lv[2] * dt;
+
+        for (let i = 0; i < mover_list.length; i++) {
+            const m = mover_list[i];
+            if (m.bvhNode === COLLIDER_UNBOUND) continue;
+            compute_world_aabb(scratch_prox_mover, 0, m.collider.shape, m.transform);
+            if (px < 0) scratch_prox_mover[0] += px; else scratch_prox_mover[3] += px;
+            if (py < 0) scratch_prox_mover[1] += py; else scratch_prox_mover[4] += py;
+            if (pz < 0) scratch_prox_mover[2] += pz; else scratch_prox_mover[5] += pz;
+
+            for (let k = 0; k < sleeper_list.length; k++) {
+                const s = sleeper_list[k];
+                if (s.bvhNode === COLLIDER_UNBOUND) continue;
+                compute_world_aabb(scratch_prox_sleeper, 0, s.collider.shape, s.transform);
+                if (scratch_prox_mover[0] <= scratch_prox_sleeper[3]
+                    && scratch_prox_mover[3] >= scratch_prox_sleeper[0]
+                    && scratch_prox_mover[1] <= scratch_prox_sleeper[4]
+                    && scratch_prox_mover[4] >= scratch_prox_sleeper[1]
+                    && scratch_prox_mover[2] <= scratch_prox_sleeper[5]
+                    && scratch_prox_mover[5] >= scratch_prox_sleeper[2]) {
+                    return true;
+                }
             }
         }
+        return false;
     }
 
     /**
@@ -1168,6 +1501,23 @@ export class PhysicsSystem extends System {
             const aSleep = a.sleepState === SleepState.Sleeping;
             const bSleep = b.sleepState === SleepState.Sleeping;
             if (aSleep === bSleep) continue; // both awake or both asleep — leave as is
+
+            // The "awake" side must be capable of pulling the sleeper. A
+            // static anchor's sleepState is never updated (it reads Awake
+            // forever), so without this rule a body jointed to a static could
+            // never sleep — perpetual mismatch wake. A kinematic driver
+            // counts only while it is actually moving: a parked motor leaves
+            // its chain asleep, and starting it wakes the chain (same motion
+            // rule as __wake_pairs).
+            const mover = aSleep ? b : a;
+            if (mover.kind !== BodyKind.Dynamic) {
+                const lv = mover.linearVelocity;
+                const av = mover.angularVelocity;
+                if (lv[0] === 0 && lv[1] === 0 && lv[2] === 0
+                    && av[0] === 0 && av[1] === 0 && av[2] === 0) {
+                    continue;
+                }
+            }
             if (aSleep) this.__wake_body(a); else this.__wake_body(b);
         }
     }
@@ -1314,6 +1664,23 @@ export class PhysicsSystem extends System {
                 nm[1] = data[slot_off + 7];
                 nm[2] = data[slot_off + 8];
                 payload.depth = data[slot_off + 9];
+
+                // The event pair is entity-canonical (min, max); the slot's
+                // stored normal points from the slot's OWN body B toward its
+                // body A — an assignment that follows packed-id order, i.e.
+                // slot allocation history. Mirror the normal when the
+                // canonicalization swapped the pair, so the payload contract
+                // ("normal points from entityB toward entityA") holds
+                // identically in every engine regardless of slot history.
+                const slot_idA = manifolds.bodyA(slot);
+                const slot_entA = this.storage.is_valid(slot_idA)
+                    ? this.storage.entity_at(body_id_index(slot_idA))
+                    : -1;
+                if (slot_entA !== entA) {
+                    nm[0] = -nm[0];
+                    nm[1] = -nm[1];
+                    nm[2] = -nm[2];
+                }
             } else {
                 pt[0] = 0;
                 pt[1] = 0;
@@ -1447,6 +1814,17 @@ export class PhysicsSystem extends System {
         const joints = this.__joints;
         const manifolds = this.manifolds;
 
+        // 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
+        // frame's awake set end to end (forces, refit, broadphase,
+        // narrowphase, islands, solve): waking later would give it one
+        // unsupported step (gravity with no contacts → penetration pop).
+        // Reads only manifold + body state — both part of the exportable
+        // world state — never the transient pair list, so a reconstructed
+        // engine wakes on the original's schedule.
+        this.__wake_pairs(dt);
+        this.__wake_joints();
 
         const count = storage.awake_count;
 
@@ -1489,14 +1867,36 @@ export class PhysicsSystem extends System {
 
                 const entry = list[k];
 
-                compute_fat_world_aabb(
-                    scratch_world_aabb, 0,
-                    entry.collider.shape, entry.transform,
-                    lv[0], lv[1], lv[2],
-                    dt
-                );
+                // Refit skip — the fat margin's whole purpose: while the
+                // body's FATTENED box (tight + velocity pad) stays inside the
+                // leaf's stored AABB, the stored box still covers everything
+                // the body can reach this step, so the set + refit-to-root is
+                // pure waste. A resting field pays only this containment test
+                // per leaf per frame (at v ≈ 0 the fat box is the tight box
+                // plus the constant slack, which the stored box includes).
+                //
+                // The containment probe MUST be the fat box, not the tight
+                // one: the leaf is how the broadphase and every swept query
+                // discover this body. A body whose velocity grew while its
+                // tight box still sat inside the stored leaf (freshly linked
+                // with a launch velocity, or accelerated by an impulse) would
+                // 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);
+                const node = entry.bvhNode;
+                this.dynamicBvh.node_get_aabb(node, scratch_stored_aabb);
+                if (scratch_world_aabb[0] >= scratch_stored_aabb[0]
+                    && scratch_world_aabb[1] >= scratch_stored_aabb[1]
+                    && scratch_world_aabb[2] >= scratch_stored_aabb[2]
+                    && scratch_world_aabb[3] <= scratch_stored_aabb[3]
+                    && scratch_world_aabb[4] <= scratch_stored_aabb[4]
+                    && scratch_world_aabb[5] <= scratch_stored_aabb[5]) {
+                    continue;
+                }
 
-                this.dynamicBvh.node_move_aabb(entry.bvhNode, scratch_world_aabb);
+                this.dynamicBvh.node_move_aabb(node, scratch_world_aabb);
             }
         }
 
@@ -1514,11 +1914,6 @@ export class PhysicsSystem extends System {
             this,
         );
 
-        // Stage 4: wake propagation — through broadphase pairs, then through
-        // joints (a joint must not have one body awake and one asleep).
-        this.__wake_pairs();
-        this.__wake_joints();
-
         // Stage 5: narrowphase — once per outer step. The substep loop below
         // re-derives each contact's penetration analytically from the moved
         // poses rather than re-running geometry.
@@ -1541,7 +1936,10 @@ export class PhysicsSystem extends System {
         // separate between substeps — the TGS stack-stability mechanism —
         // without re-running narrowphase. Restitution is applied once after
         // the loop.
-        const N = this.substeps;
+        // 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.
+        const N = this.substeps > 1 ? this.substeps | 0 : 1;
         const h = dt / N;
         const count_after_wake = storage.awake_count;
 
@@ -1676,18 +2074,26 @@ export class PhysicsSystem extends System {
             // (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.
-            if (joints.length > 0) {
+            // Solved in the entity-canonical order (NOT joint-id order, which
+            // encodes link/unlink history) — see __joints_sorted.
+            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(joints, this, h, this.jointIterations, (s & 1) === 1);
+                solve_joints(this.__joints_sorted, this, h, this.jointIterations, (s & 1) === 1);
             }
 
             // Position correction writes pseudo-velocity (zeroed first so it
             // is a fresh per-substep correction), folded into the pose by the
-            // position integrate and then discarded.
+            // 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).
+            const contacts_soft = this.contactHertz > 0;
             pseudoVel.fill(0, 0, pseudo_len);
-            solve_position(manifolds, this, this.positionIterations);
+            if (!contacts_soft) {
+                solve_position(manifolds, this, this.positionIterations);
+            }
 
             for (let i = 0; i < count_after_wake; i++) {
                 const idx = storage.awake_at(i);
@@ -1698,6 +2104,14 @@ export class PhysicsSystem extends System {
                     pseudoVel[base], pseudoVel[base + 1], pseudoVel[base + 2],
                     pseudoVel[base + 3], pseudoVel[base + 4], pseudoVel[base + 5]);
             }
+
+            // Relax pass (soft contacts only): one biasless sweep over the
+            // contact rows AFTER position integration, removing the real
+            // momentum the velocity-bias push injected this substep
+            // (Box2D-v3 substep order: solve -> integrate -> relax).
+            if (contacts_soft) {
+                solve_velocity(manifolds, this, 1, false);
+            }
         }
 
         // Stage 8: one-shot restitution, after the substep loop, keyed off
@@ -1734,9 +2148,17 @@ export class PhysicsSystem extends System {
         diff_manifolds(manifolds, storage, this.contactEvents);
         this.__dispatch_contact_events();
 
-        // Stage 11 (end-of-step): roll touched → prev_touched and evict slots
-        // whose pair has not been touched within the grace window.
-        manifolds.advance_frame();
+        // Stage 11 (end-of-step): roll touched → prev_touched and the
+        // had-contacts flag, evict slots whose pair has not been touched
+        // within the grace window. Dormant pairs (both bodies asleep) are
+        // frozen — contacts and warm-start impulses survive the nap.
+        manifolds.advance_frame(storage);
+
+        // Dev tripwire (assert.* is compiled out of prod): no NaN / Infinity
+        // may survive a step in body state or contact manifolds. NaN is
+        // self-concealing downstream — it fails every depth comparison and
+        // sleep threshold — so it must fail loudly here, at the step boundary.
+        assert.equal(find_non_finite_physics_state(this), "", "non-finite physics state after fixedUpdate");
     }
 }