@woosh/meep-engine 2.146.0 → 2.148.0

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

@@ -1,19 +1,26 @@
 import { assert } from "../../../core/assert.js";
+import { BinaryBuffer } from "../../../core/binary/BinaryBuffer.js";
 import { BVH } from "../../../core/bvh2/bvh3/BVH.js";
 import Signal from "../../../core/events/signal/Signal.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";
 import { ResourceAccessKind } from "../../../core/model/ResourceAccessKind.js";
 import { ResourceAccessSpecification } from "../../../core/model/ResourceAccessSpecification.js";
 import { System } from "../../ecs/System.js";
 import { Transform } from "../../ecs/transform/Transform.js";
+import { Interpolated } from "../../interpolation/Interpolated.js";
 import { body_id_index, BodyStorage } from "../body/BodyStorage.js";
-import { aabb3_transform_oriented } from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";
+import { SBS_STRIDE, SolverBodyState } from "../body/SolverBodyState.js";
 import { compute_fat_world_aabb } from "../broadphase/compute_fat_world_aabb.js";
 import { generate_pairs } from "../broadphase/generate_pairs.js";
 import { PairList } from "../broadphase/PairList.js";
-import { CONTACT_STRIDE, ManifoldStore } from "../contact/ManifoldStore.js";
+import { ccd_resolve } from "../ccd/linear_sweep.js";
+import { 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 { world_inverse_inertia_apply } 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";
@@ -21,32 +28,28 @@ import { narrowphase_step } from "../narrowphase/narrowphase_step.js";
 import { overlap_shape as overlap_shape_query } from "../queries/overlap_shape.js";
 import { raycast as raycast_query } from "../queries/raycast.js";
 import { shape_cast as shape_cast_query } from "../queries/shape_cast.js";
-import { ccd_resolve } from "../ccd/linear_sweep.js";
-import { returnTrue } from "../../../core/function/returnTrue.js";
 import {
+    apply_restitution,
     prepare_contacts,
-    refresh_contacts,
     redetect_concave_contacts,
-    warm_start_contacts,
-    solve_velocity,
-    apply_restitution,
+    refresh_contacts,
     solve_position,
+    solve_velocity,
+    warm_start_contacts,
 } from "../solver/solve_contacts.js";
-import { solve_joints } from "../constraint/solve_constraints.js";
-import { JOINT_WORLD, JOINT_UNALLOCATED } from "./Joint.js";
-import { world_inverse_inertia_apply } from "../inertia/world_inverse_inertia.js";
+import { BodyKind } from "./BodyKind.js";
+import { Collider, COLLIDER_UNBOUND } from "./Collider.js";
+import { JOINT_UNALLOCATED, JOINT_WORLD } from "./Joint.js";
 import { PhysicsEvents } from "./PhysicsEvents.js";
+import { RIGID_BODY_UNALLOCATED, RigidBody } from "./RigidBody.js";
+import { RigidBodyFlags } from "./RigidBodyFlags.js";
+import { SleepState } from "./SleepState.js";
 
 /**
  * Scratch for {@link applyImpulseAt}'s angular delta calculation.
  * @type {Float64Array}
  */
 const scratch_angular_delta = new Float64Array(3);
-import { BodyKind } from "./BodyKind.js";
-import { Collider, COLLIDER_UNBOUND } from "./Collider.js";
-import { RIGID_BODY_UNALLOCATED, RigidBody } from "./RigidBody.js";
-import { RigidBodyFlags } from "./RigidBodyFlags.js";
-import { SleepState } from "./SleepState.js";
 
 /**
  * Reusable scratch buffer for world-AABB construction so the link path is
@@ -94,6 +97,7 @@ export class PhysicsSystem extends System {
 
         /**
          * @type {BodyStorage}
+         * @readonly
          */
         this.storage = new BodyStorage();
 
@@ -110,6 +114,7 @@ export class PhysicsSystem extends System {
         /**
          * Persistent contact-manifold cache. One slot per active pair.
          * @type {ManifoldStore}
+         * @readonly
          */
         this.manifolds = new ManifoldStore();
 
@@ -137,6 +142,18 @@ export class PhysicsSystem extends System {
          */
         this.islands = new IslandBuilder();
 
+        /**
+         * Data-oriented mirror of the per-body solver hot state (velocity,
+         * inverse mass / inertia, orientation), packed into one ArrayBuffer
+         * and indexed by body slot. Gathered from the `RigidBody` /
+         * `Transform` components once per step (after islands are built),
+         * mutated in place by the TGS substep loop, and the persistent
+         * velocity scattered back at the end. Keeps the solver's hottest inner
+         * loop free of component-object dereferences.
+         * @type {SolverBodyState}
+         */
+        this.__solver_state = new SolverBodyState();
+
         /**
          * Velocity-squared threshold below which a body is eligible to start
          * accumulating sleep time. Combined linear + angular kinetic-ish
@@ -238,7 +255,11 @@ export class PhysicsSystem extends System {
          * @type {RigidBody[]}
          */
         this.__bodies = [];
-        /** @type {Transform[]} */
+
+        /**
+         * @type {Transform[]}
+         * @readonly
+         */
         this.__transforms = [];
 
         /**
@@ -312,13 +333,26 @@ export class PhysicsSystem extends System {
          */
         this.__ccd_start_pos = new Float64Array(0);
 
+
         /**
-         * Bound reference to {@link __pair_filter} so we hand the same
-         * callable to {@link generate_pairs} each step without per-step
-         * allocation.
+         * Optional shared interpolation log to produce per-step pose snapshots
+         * into (the {@link InterpolationSystem}'s log). When set — and only then —
+         * each fixedUpdate restores every awake {@link Interpolated} body's live
+         * components from the previous tick (undoing render-time interpolation so
+         * the sim reads authoritative state), then records the post-step state
+         * under the current `entityManager.fixedStepTick`. Null on a headless /
+         * non-rendering world, where the producer work is skipped entirely.
+         * @type {InterpolationLog|null}
+         */
+        this.interpolationLog = null;
+
+        /**
+         * Reusable decode buffer for restoring interpolated snapshots.
          * @private
+         * @type {BinaryBuffer}
          */
-        this.__pair_filter_bound = (idA, idB) => this.__pair_filter(idA, idB);
+        this.__interp_scratch = new BinaryBuffer();
+        this.__interp_scratch.fromArrayBuffer(new ArrayBuffer(256));
     }
 
     /**
@@ -338,13 +372,21 @@ export class PhysicsSystem extends System {
     __pair_filter(idA, idB) {
         const idxA = body_id_index(idA);
         const idxB = body_id_index(idB);
+
         const rbA = this.__bodies[idxA];
         const rbB = this.__bodies[idxB];
-        if (rbA === undefined || rbB === undefined) return false;
+
+        if (rbA === undefined || rbB === undefined) {
+            return false;
+        }
 
         // Layer/mask gate (symmetric).
-        if (((rbA.layer & rbB.mask) | 0) === 0) return false;
-        if (((rbB.layer & rbA.mask) | 0) === 0) return false;
+        if (((rbA.layer & rbB.mask) | 0) === 0) {
+            return false;
+        }
+        if (((rbB.layer & rbA.mask) | 0) === 0) {
+            return false;
+        }
 
         // User callback gate, if installed.
         const fn = this.__contact_filter;
@@ -812,7 +854,7 @@ export class PhysicsSystem extends System {
         if (rigidBody.kind !== BodyKind.Dynamic) {
             return;
         }
-        rigidBody.accumulatedForce.add( force);
+        rigidBody.accumulatedForce.add(force);
         this.__wake_body(rigidBody);
     }
 
@@ -829,6 +871,50 @@ export class PhysicsSystem extends System {
         }
     }
 
+    /**
+     * Teleport a body to a new pose, bypassing integration: writes the body's
+     * Transform directly and wakes it. For an interpolated body this also flags a
+     * render `snap` on its {@link Interpolated} component, so the producer keeps
+     * this pose (rather than restoring the previous tick over it) and the
+     * renderer shows the new pose without sliding across the jump.
+     *
+     * This is the authoritative way to reposition an interpolated body — a raw
+     * `Transform` write would be undone by the per-step restore. Velocity is left
+     * as-is; zero it via {@link setLinearVelocity} if the teleport should also
+     * stop the body.
+     *
+     * @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
+     */
+    setPose(rigidBody, position, rotation) {
+        const idx = body_id_index(rigidBody._bodyId);
+        const transform = this.__transforms[idx];
+        if (transform === undefined) {
+            return;
+        }
+
+        transform.position.set(position.x, position.y, position.z);
+        transform.rotation.set(rotation.x, rotation.y, rotation.z, rotation.w);
+
+        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
+        // as authoritative this frame instead of interpolating across the jump.
+        const em = this.entityManager;
+        if (em !== null && em !== undefined) {
+            const dataset = em.dataset;
+            if (dataset !== null && dataset !== undefined) {
+                const entity = this.storage.entity_at(idx);
+                const interpolated = dataset.getComponent(entity, Interpolated);
+                if (interpolated !== undefined && interpolated !== null) {
+                    interpolated.snap = true;
+                }
+            }
+        }
+    }
+
     /**
      * Force the body awake. Static bodies are ignored.
      * @param {RigidBody} rigidBody
@@ -931,8 +1017,12 @@ export class PhysicsSystem extends System {
             rb.sleep_group_next = -1;
             rb.sleep_group_prev = -1;
             rb.sleepState = SleepState.Sleeping;
-            rb.linearVelocity[0] = 0; rb.linearVelocity[1] = 0; rb.linearVelocity[2] = 0;
-            rb.angularVelocity[0] = 0; rb.angularVelocity[1] = 0; rb.angularVelocity[2] = 0;
+            rb.linearVelocity[0] = 0;
+            rb.linearVelocity[1] = 0;
+            rb.linearVelocity[2] = 0;
+            rb.angularVelocity[0] = 0;
+            rb.angularVelocity[1] = 0;
+            rb.angularVelocity[2] = 0;
             storage.mark_sleeping(idx);
             return;
         }
@@ -946,22 +1036,16 @@ export class PhysicsSystem extends System {
             rb.sleep_group_next = next_idx;
             rb.sleep_group_prev = prev_idx;
             rb.sleepState = SleepState.Sleeping;
-            rb.linearVelocity[0] = 0; rb.linearVelocity[1] = 0; rb.linearVelocity[2] = 0;
-            rb.angularVelocity[0] = 0; rb.angularVelocity[1] = 0; rb.angularVelocity[2] = 0;
+            rb.linearVelocity[0] = 0;
+            rb.linearVelocity[1] = 0;
+            rb.linearVelocity[2] = 0;
+            rb.angularVelocity[0] = 0;
+            rb.angularVelocity[1] = 0;
+            rb.angularVelocity[2] = 0;
             storage.mark_sleeping(idx);
         }
     }
 
-    /**
-     * Get the body index for a packed body id without revalidation. Used by
-     * query traversals that already trust the id came from a live BVH leaf.
-     * @param {number} packed_body_id
-     * @returns {number}
-     */
-    __index_of(packed_body_id) {
-        return body_id_index(packed_body_id);
-    }
-
     /**
      * Broadphase raycast against both BVHs. Fills `result` with the nearest
      * hit and returns `true` on hit, `false` on miss.
@@ -1168,7 +1252,7 @@ export class PhysicsSystem extends System {
                 const lv = rb.linearVelocity;
                 const av = rb.angularVelocity;
                 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];
+                    + av[0] * av[0] + av[1] * av[1] + av[2] * av[2];
                 if (v_sqr > max_v_sqr) max_v_sqr = v_sqr;
             }
 
@@ -1245,14 +1329,22 @@ export class PhysicsSystem extends System {
             const pt = payload.point;
             const nm = payload.normal;
             if (has_contact) {
-                const wax = data[slot_off],     way = data[slot_off + 1], waz = data[slot_off + 2];
+                const wax = data[slot_off], way = data[slot_off + 1], waz = data[slot_off + 2];
                 const wbx = data[slot_off + 3], wby = data[slot_off + 4], wbz = data[slot_off + 5];
-                pt[0] = (wax + wbx) * 0.5; pt[1] = (way + wby) * 0.5; pt[2] = (waz + wbz) * 0.5;
-                nm[0] = data[slot_off + 6]; nm[1] = data[slot_off + 7]; nm[2] = data[slot_off + 8];
+                pt[0] = (wax + wbx) * 0.5;
+                pt[1] = (way + wby) * 0.5;
+                pt[2] = (waz + wbz) * 0.5;
+                nm[0] = data[slot_off + 6];
+                nm[1] = data[slot_off + 7];
+                nm[2] = data[slot_off + 8];
                 payload.depth = data[slot_off + 9];
             } else {
-                pt[0] = 0; pt[1] = 0; pt[2] = 0;
-                nm[0] = 0; nm[1] = 0; nm[2] = 0;
+                pt[0] = 0;
+                pt[1] = 0;
+                pt[2] = 0;
+                nm[0] = 0;
+                nm[1] = 0;
+                nm[2] = 0;
                 payload.depth = 0;
             }
             payload.entityA = entA;
@@ -1260,9 +1352,16 @@ export class PhysicsSystem extends System {
 
             let event_name;
             let signal;
-            if (kind === ContactEventKind.Begin)      { event_name = PhysicsEvents.ContactBegin; signal = this.onContactBegin; }
-            else if (kind === ContactEventKind.Stay)  { event_name = PhysicsEvents.ContactStay;  signal = this.onContactStay;  }
-            else                                       { event_name = PhysicsEvents.ContactEnd;   signal = this.onContactEnd;   }
+            if (kind === ContactEventKind.Begin) {
+                event_name = PhysicsEvents.ContactBegin;
+                signal = this.onContactBegin;
+            } else if (kind === ContactEventKind.Stay) {
+                event_name = PhysicsEvents.ContactStay;
+                signal = this.onContactStay;
+            } else {
+                event_name = PhysicsEvents.ContactEnd;
+                signal = this.onContactEnd;
+            }
 
             signal.send1(payload);
 
@@ -1273,12 +1372,115 @@ export class PhysicsSystem extends System {
         }
     }
 
+    /**
+     * Producer — restore pass. At the top of a fixed step, reset every awake
+     * {@link Interpolated} body's live components to their authoritative state
+     * from the previous tick's snapshot, undoing any render-time interpolation
+     * the {@link InterpolationSystem} wrote between frames so the sim integrates
+     * from truth, not an interpolated pose. A body with no previous snapshot
+     * (first step ever, or just woken) is left as-is — its live state is already
+     * authoritative. No-op unless {@link interpolationLog} is wired.
+     * @private
+     */
+    __interp_restore() {
+        const log = this.interpolationLog;
+        const em = this.entityManager;
+        if (log === null || em === null || em === undefined) return;
+        const dataset = em.dataset;
+        if (dataset === null || dataset === undefined) return;
+
+        const prev_tick = em.fixedStepTick - 1;
+        if (prev_tick < 0) return;
+
+        const storage = this.storage;
+        const count = storage.awake_count;
+        const scratch = this.__interp_scratch;
+
+        for (let i = 0; i < count; i++) {
+            const idx = storage.awake_at(i);
+            const entity = storage.entity_at(idx);
+            const interpolated = dataset.getComponent(entity, Interpolated);
+            if (interpolated === undefined || interpolated === null) continue;
+            // A teleported body (snap set, e.g. via setPose) keeps its live pose
+            // this step — restoring the previous tick would undo the teleport.
+            if (interpolated.snap) continue;
+            const key = interpolated.key;
+            if (key < 0) continue;
+
+            const interpolands = interpolated.interpolands;
+            for (let k = 0; k < interpolands.length; k++) {
+                const ip = interpolands[k];
+                scratch.position = 0;
+                // Snap to the previous tick's snapshot (both offsets equal → t irrelevant).
+                const ok = log.interpolate(scratch, key, ip.type_id, prev_tick, prev_tick, 0, ip.interpolation_adapter);
+                if (!ok) continue;
+                const target = dataset.getComponent(entity, ip.component_class);
+                if (target === undefined || target === null) continue;
+                scratch.position = 0;
+                ip.serialization_adapter.deserialize(scratch, target);
+            }
+        }
+    }
+
+    /**
+     * Producer — record pass. At the end of a fixed step, snapshot every awake
+     * {@link Interpolated} body's live components into the shared log under the
+     * current `entityManager.fixedStepTick`. The render-time
+     * {@link InterpolationSystem} blends consecutive ticks from these snapshots.
+     * Only awake (moving) bodies are recorded, so the log stays sparse. No-op
+     * unless {@link interpolationLog} is wired.
+     * @private
+     */
+    __interp_record() {
+        const log = this.interpolationLog;
+        const em = this.entityManager;
+        if (log === null || em === null || em === undefined) return;
+        const dataset = em.dataset;
+        if (dataset === null || dataset === undefined) return;
+
+        const tick = em.fixedStepTick;
+        const storage = this.storage;
+        const count = storage.awake_count;
+
+        log.begin_tick(tick);
+        for (let i = 0; i < count; i++) {
+            const idx = storage.awake_at(i);
+            const entity = storage.entity_at(idx);
+            const interpolated = dataset.getComponent(entity, Interpolated);
+            if (interpolated === undefined || interpolated === null) continue;
+            const key = interpolated.key;
+            if (key < 0) continue;
+
+            const interpolands = interpolated.interpolands;
+            for (let k = 0; k < interpolands.length; k++) {
+                const ip = interpolands[k];
+                const target = dataset.getComponent(entity, ip.component_class);
+                if (target === undefined || target === null) continue;
+                const buf = log.begin_record(key, ip.type_id);
+                ip.serialization_adapter.serialize(buf, target);
+                log.end_record();
+            }
+        }
+        log.end_tick();
+    }
+
     fixedUpdate(dt) {
+        // Producer: restore authoritative pose (undo render interpolation)
+        // before the sim reads any Transform this step.
+        this.__interp_restore();
+
         const gx = this.gravity.x;
         const gy = this.gravity.y;
         const gz = this.gravity.z;
 
-        const count = this.storage.awake_count;
+        const storage = this.storage;
+        const bodies = this.__bodies;
+        const transforms = this.__transforms;
+        const joints = this.__joints;
+        const manifolds = this.manifolds;
+
+
+        const count = storage.awake_count;
 
         // Stage 1: consume the per-frame force / torque accumulators into
         // velocity at the full `dt`, exactly once (a user force is a per-frame
@@ -1288,9 +1490,11 @@ export class PhysicsSystem extends System {
         // that substep's contact warm-start, keeping resting stacks at zero
         // velocity.
         for (let i = 0; i < count; i++) {
-            const idx = this.storage.awake_at(i);
-            const rb = this.__bodies[idx];
-            const tr = this.__transforms[idx];
+            const idx = storage.awake_at(i);
+
+            const rb = bodies[idx];
+            const tr = transforms[idx];
+
             integrate_velocity_forces(rb, tr, dt);
         }
 
@@ -1300,19 +1504,30 @@ export class PhysicsSystem extends System {
         // is a sub-millimetre slack difference, safely inside the margin.
         const lists = this.__body_collider_lists;
         for (let i = 0; i < count; i++) {
-            const idx = this.storage.awake_at(i);
-            const rb = this.__bodies[idx];
+
+            const idx = storage.awake_at(i);
+            const rb = bodies[idx];
             const list = lists[idx];
-            if (list === undefined) continue;
+
+            if (list === undefined) {
+                continue;
+            }
+
             const lv = rb.linearVelocity;
-            for (let k = 0; k < list.length; k++) {
+
+            const list_length = list.length;
+
+            for (let k = 0; k < list_length; k++) {
+
                 const entry = list[k];
+
                 compute_fat_world_aabb(
                     scratch_world_aabb, 0,
                     entry.collider.shape, entry.transform,
                     lv[0], lv[1], lv[2],
                     dt
                 );
+
                 this.dynamicBvh.node_move_aabb(entry.bvhNode, scratch_world_aabb);
             }
         }
@@ -1321,13 +1536,14 @@ export class PhysicsSystem extends System {
         // outer-step motion, so the pair set stays valid across all substeps
         // — broadphase runs once.
         generate_pairs(
-            this.storage,
+            storage,
             this.dynamicBvh,
             this.staticBvh,
-            this.manifolds,
+            manifolds,
             lists,
             this.pairs,
-            this.__pair_filter_bound,
+            this.__pair_filter,
+            this,
         );
 
         // Stage 4: wake propagation — through broadphase pairs, then through
@@ -1338,11 +1554,11 @@ export class PhysicsSystem extends System {
         // 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.
-        narrowphase_step(this.pairs, this.manifolds, this.__body_collider_lists);
+        narrowphase_step(this.pairs, manifolds, this.__body_collider_lists);
 
         // Stage 6: partition awake bodies + touched contacts into islands.
         // Consumed by the solver (flattened contact list) and the sleep test.
-        this.islands.build(this.storage, this.manifolds, this.__bodies, this.__body_collider_lists, this.__joints);
+        this.islands.build(storage, manifolds, bodies, this.__body_collider_lists, joints);
 
         // Stage 7: TGS substep loop.
         //
@@ -1359,7 +1575,7 @@ export class PhysicsSystem extends System {
         // the loop.
         const N = this.substeps;
         const h = dt / N;
-        const count_after_wake = this.storage.awake_count;
+        const count_after_wake = storage.awake_count;
 
         // CCD: capture start-of-step positions for flagged bodies over the
         // post-wake awake set (poses are unchanged until the substep loop below
@@ -1368,21 +1584,37 @@ export class PhysicsSystem extends System {
         // start matches the end the resolve pass reads. Zero-cost when no body
         // is flagged.
         const ccd_on = this.ccdEnabled;
+
         if (ccd_on) {
-            const ccd_need = this.storage.high_water_mark * 3;
+
+            const ccd_need = storage.high_water_mark * 3;
+
             if (this.__ccd_start_pos.length < ccd_need) {
                 this.__ccd_start_pos = new Float64Array(ccd_need);
             }
+
             const ccd_start = this.__ccd_start_pos;
+
             for (let i = 0; i < count_after_wake; i++) {
-                const idx = this.storage.awake_at(i);
-                const rb = this.__bodies[idx];
-                if (rb.kind !== BodyKind.Dynamic) continue;
-                if ((rb.flags & RigidBodyFlags.CCD) === 0) continue;
+                const idx = storage.awake_at(i);
+                const rb = bodies[idx];
+
+                if (rb.kind !== BodyKind.Dynamic) {
+                    continue;
+                }
+                if ((rb.flags & RigidBodyFlags.CCD) === 0) {
+                    continue;
+                }
+
                 const list = this.__body_collider_lists[idx];
-                if (list === undefined || list.length === 0) continue;
+
+                if (list === undefined || list.length === 0) {
+                    continue;
+                }
+
                 const cp = list[0].transform.position;
                 const cb = idx * 3;
+
                 ccd_start[cb] = cp[0];
                 ccd_start[cb + 1] = cp[1];
                 ccd_start[cb + 2] = cp[2];
@@ -1395,55 +1627,117 @@ export class PhysicsSystem extends System {
         // reallocate again this step.
         this.__reset_pseudo_velocity();
         const pseudoVel = this.__pseudo_velocity;
-        const pseudo_len = this.storage.high_water_mark * 6;
+        const pseudo_len = storage.high_water_mark * 6;
+
+        // Gather the data-oriented solver state for every body the substep loop
+        // will touch: the post-wake awake set (all dynamics, at their post-force
+        // velocity) plus the static / kinematic anchors and jointed partners
+        // referenced by this step's contacts and joints. From here the substep
+        // loop reads / writes velocity + orientation through `ss_data` with no
+        // component-object dereference; persistent velocity is scattered back
+        // onto the RigidBodies after the solve.
+        const solver_state = this.__solver_state;
+        solver_state.begin(storage.high_water_mark);
+
+        for (let i = 0; i < count_after_wake; i++) {
+            const idx = storage.awake_at(i);
+            solver_state.gather(idx, bodies[idx], transforms[idx]);
+        }
+
+        const island_contacts = this.islands.contact_data;
+        const island_contact_total = this.islands.contact_offsets[this.islands.island_count];
+
+        for (let i = 0; i < island_contact_total; i++) {
+            const slot = island_contacts[i];
+
+            const ia = body_id_index(manifolds.bodyA(slot));
+            const ib = body_id_index(manifolds.bodyB(slot));
+
+            solver_state.gather(ia, bodies[ia], transforms[ia]);
+            solver_state.gather(ib, bodies[ib], transforms[ib]);
+        }
+
+        const joint_count = joints.length;
+        for (let i = 0; i < joint_count; i++) {
+            const joint = joints[i];
 
-        prepare_contacts(this.manifolds, this, h);
+            if (joint === undefined || joint === null) {
+                continue;
+            }
+
+            if (!storage.is_valid(joint._bodyIdA)) {
+                continue;
+            }
+
+            const ia = body_id_index(joint._bodyIdA);
+
+            solver_state.gather(ia, bodies[ia], transforms[ia]);
+
+            if (joint._bodyIdB !== JOINT_WORLD && storage.is_valid(joint._bodyIdB)) {
+
+                const ib = body_id_index(joint._bodyIdB);
+
+                solver_state.gather(ib, bodies[ib], transforms[ib]);
+
+            }
+        }
+
+        const ss_data = solver_state.data;
+
+        prepare_contacts(manifolds, this, h);
 
         for (let s = 0; s < N; s++) {
+
             // Gravity (+ damping) for this substep.
             for (let i = 0; i < count_after_wake; i++) {
-                const idx = this.storage.awake_at(i);
-                integrate_velocity_gravity(this.__bodies[idx], this.__transforms[idx], gx, gy, gz, h);
+                const idx = storage.awake_at(i);
+                integrate_velocity_gravity(ss_data, idx * SBS_STRIDE, bodies[idx], gx, gy, gz, h);
             }
 
             // Re-derive contact geometry at the current poses: concave pairs
             // re-run narrowphase (fresh feature/normal as the body rocks),
             // convex pairs rotate frozen anchors analytically. Then replay
             // the per-substep warm-start and solve velocity.
-            redetect_concave_contacts(this.manifolds, this);
-            refresh_contacts(this.manifolds, this);
-            warm_start_contacts(this.manifolds, this);
-            solve_velocity(this.manifolds, this, this.velocityIterations);
+            redetect_concave_contacts(manifolds, this);
+            refresh_contacts(manifolds, this.__transforms);
+            warm_start_contacts(manifolds, this);
+            solve_velocity(manifolds, this, this.velocityIterations);
 
             // 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.
-            if (this.__joints.length > 0) {
-                solve_joints(this.__joints, this, h, this.jointIterations);
+            if (joints.length > 0) {
+                solve_joints(joints, this, h, this.jointIterations);
             }
 
             // 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.
             pseudoVel.fill(0, 0, pseudo_len);
-            solve_position(this.manifolds, this, this.positionIterations);
+            solve_position(manifolds, this, this.positionIterations);
 
             for (let i = 0; i < count_after_wake; i++) {
-                const idx = this.storage.awake_at(i);
-                const rb = this.__bodies[idx];
-                const tr = this.__transforms[idx];
+                const idx = storage.awake_at(i);
+                const rb = bodies[idx];
+                const tr = transforms[idx];
                 const base = idx * 6;
-                integrate_position(rb, tr, h,
-                    pseudoVel[base],     pseudoVel[base + 1], pseudoVel[base + 2],
+                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]);
             }
         }
 
         // Stage 8: one-shot restitution, after the substep loop, keyed off
         // the approach velocity captured at prepare time.
-        apply_restitution(this.manifolds, this);
+        apply_restitution(manifolds, this);
+
+        // Scatter the solved persistent linear / angular velocity back onto the
+        // RigidBody components (pose was written through to the Transforms each
+        // substep). After this the bodies are authoritative again for CCD,
+        // interpolation recording, and the sleep test below.
+        solver_state.scatter(bodies);
 
         // Stage 8.5: continuous collision — sweep CCD-flagged fast movers along
         // their net step translation and stop them at the first blocker, so they
@@ -1454,18 +1748,24 @@ export class PhysicsSystem extends System {
             ccd_resolve(this);
         }
 
+        // Producer: record the post-step authoritative pose of every awake
+        // interpolated body under this step's tick. Before the sleep test, so a
+        // body that settles this step still records its final pose for the last
+        // interpolation interval.
+        this.__interp_record();
+
         // Stage 9: sleep test.
         this.__sleep_test(dt);
 
         // Stage 10: diff manifolds against the previous frame and dispatch
         // Begin / Stay / End events. MUST run before advance_frame, which
         // rolls the touched flags.
-        diff_manifolds(this.manifolds, this.storage, this.contactEvents);
+        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.
-        this.manifolds.advance_frame();
+        manifolds.advance_frame();
     }
 }