@woosh/meep-engine 2.138.19 → 2.139.0

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

@@ -0,0 +1,1159 @@
+import { assert } from "../../../core/assert.js";
+import { BVH } from "../../../core/bvh2/bvh3/BVH.js";
+import Signal from "../../../core/events/signal/Signal.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 { body_id_index, BodyStorage } from "../body/BodyStorage.js";
+import { aabb_transform_oriented } from "../broadphase/aabb_transform_oriented.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 { ContactEventBuffer, ContactEventKind } from "../events/ContactEventBuffer.js";
+import { diff_manifolds } from "../events/diff_manifolds.js";
+import { integrate_position } from "../integration/integrate_position.js";
+import { integrate_velocity } from "../integration/integrate_velocity.js";
+import { IslandBuilder } from "../island/IslandBuilder.js";
+import { narrowphase_step } from "../narrowphase/narrowphase_step.js";
+import { raycast as raycast_query } from "../queries/raycast.js";
+import { returnTrue } from "../../../core/function/returnTrue.js";
+import { solve_contacts } from "../solver/solve_contacts.js";
+import { world_inverse_inertia_apply } from "../inertia/world_inverse_inertia.js";
+import { PhysicsEvents } from "./PhysicsEvents.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
+ * allocation-free in steady state.
+ * @type {Float64Array}
+ */
+const scratch_world_aabb = new Float64Array(6);
+
+/**
+ * Reusable scratch buffer for the local AABB returned by
+ * {@link AbstractShape3D#compute_bounding_box}.
+ * @type {Float64Array}
+ */
+const scratch_local_aabb = new Float64Array(6);
+
+/**
+ * Rigid-body physics system.
+ *
+ * v1 scope: pool + active list + two BVHs (static / dynamic), Transform sync
+ * contract, and the user-facing API surface (gravity, force / impulse, layer
+ * filtering, wake / sleep, contact filter). The per-step simulation pipeline
+ * (broadphase pair generation, narrowphase, solver, islands, sleep test,
+ * contact-event emission) is built on top of this skeleton in subsequent
+ * iterations.
+ *
+ * Dependency tuple is `(RigidBody, Collider, Transform)` — every body has
+ * exactly one collider on the same entity. Compound bodies via child collider
+ * entities are an extension point handled by a follow-up sub-observer.
+ *
+ * @author Alex Goldring
+ * @copyright Company Named Limited (c) 2026
+ */
+export class PhysicsSystem extends System {
+
+    constructor() {
+        super();
+
+        this.dependencies = [RigidBody, Transform];
+
+        this.components_used = [
+            ResourceAccessSpecification.from(RigidBody, ResourceAccessKind.Read | ResourceAccessKind.Write),
+            ResourceAccessSpecification.from(Collider, ResourceAccessKind.Read),
+            ResourceAccessSpecification.from(Transform, ResourceAccessKind.Read | ResourceAccessKind.Write),
+        ];
+
+        /**
+         * @type {BodyStorage}
+         */
+        this.storage = new BodyStorage();
+
+        /**
+         * @type {BVH}
+         */
+        this.staticBvh = new BVH();
+
+        /**
+         * @type {BVH}
+         */
+        this.dynamicBvh = new BVH();
+
+        /**
+         * Persistent contact-manifold cache. One slot per active pair.
+         * @type {ManifoldStore}
+         */
+        this.manifolds = new ManifoldStore();
+
+        /**
+         * Per-frame list of broadphase-overlapping pairs (canonical
+         * `(min, max)`). Cleared at the top of each step.
+         * @type {PairList}
+         */
+        this.pairs = new PairList();
+
+        /**
+         * Per-frame contact-event buffer. Populated by the manifold diff
+         * pass at end-of-step and consumed by the dispatch pass.
+         * @type {ContactEventBuffer}
+         */
+        this.contactEvents = new ContactEventBuffer();
+
+        /**
+         * Per-frame island partitioning of the awake-body + contact graph.
+         * Rebuilt after narrowphase, consumed by the solver (and, in the
+         * follow-up slice, by the per-island atomic sleep test). Bodies
+         * static and kinematic act as constraint anchors and do not enlarge
+         * islands.
+         * @type {IslandBuilder}
+         */
+        this.islands = new IslandBuilder();
+
+        /**
+         * 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.
+         * @type {number}
+         */
+        this.sleepVelocitySqrThreshold = 0.01;
+
+        /**
+         * Number of TGS substeps per fixed tick. Each substep does
+         * `integrate_velocity → solve_contacts → integrate_position` with
+         * `dt / substeps` as its time step; narrowphase + island building
+         * run once per tick before the substep loop. Higher values give
+         * better convergence on tall stacks (impulses propagate through
+         * deeper contact chains thanks to per-substep position
+         * integration) at proportionally higher solver cost.
+         *
+         * Box2D's default is 4; we match. Set to 1 to fall back to
+         * classic PGS behaviour (one solve per tick at full `dt`).
+         * @type {number}
+         */
+        this.solverSubsteps = 4;
+
+        /**
+         * Velocity iterations within each TGS substep. With 4 substeps,
+         * the total iterations per tick is `solverSubsteps × this` = 8 —
+         * comparable to the classic PGS default of 10, but distributed
+         * across substeps with position integration between them, which
+         * is dramatically more stable on stacks.
+         * @type {number}
+         */
+        this.solverItersPerSubstep = 2;
+
+        /**
+         * Seconds of below-threshold motion before a body is moved to the
+         * sleeping set. Box2D default is 0.5 s.
+         * @type {number}
+         */
+        this.sleepTimeThreshold = 0.5;
+
+        /**
+         * Reusable contact-event payload. Listeners must copy any fields they
+         * intend to retain past their own scope. Reset before each dispatch.
+         * @private
+         */
+        this.__contact_payload = {
+            entityA: -1,
+            entityB: -1,
+            kind: 0,
+            depth: 0,
+            normal: new Vector3(),
+            point: new Vector3(),
+        };
+
+        /**
+         * World gravity, m/s². Applied each step scaled by per-body gravityScale.
+         * @readonly
+         * @type {Vector3}
+         */
+        this.gravity = new Vector3(0, -9.81, 0);
+
+        /**
+         * Emitted at end-of-step for newly established contact pairs.
+         * @readonly
+         * @type {Signal}
+         */
+        this.onContactBegin = new Signal();
+
+        /**
+         * Emitted at end-of-step for contact pairs that persisted from the previous step.
+         * @readonly
+         * @type {Signal}
+         */
+        this.onContactStay = new Signal();
+
+        /**
+         * Emitted at end-of-step for contact pairs that disappeared.
+         * @readonly
+         * @type {Signal}
+         */
+        this.onContactEnd = new Signal();
+
+        /**
+         * Optional global contact filter. Called for each surviving broadphase
+         * pair; returning `false` discards the pair.
+         * @type {((entityA: number, entityB: number, colliderA: Collider, colliderB: Collider) => boolean) | null}
+         * @private
+         */
+        this.__contact_filter = null;
+
+        /**
+         * Per-body component side-tables indexed by body index (NOT packed id).
+         * Sparse arrays — populated at link, cleared at unlink. The hot loop
+         * iterates `storage.awake_at(i)` and dereferences these.
+         * @type {RigidBody[]}
+         */
+        this.__bodies = [];
+        /** @type {Transform[]} */
+        this.__transforms = [];
+
+        /**
+         * Per-body list of attached colliders. Each entry stores the
+         * Collider component, its world Transform, the entity that owns
+         * it (the body entity for same-entity attachments, a child entity
+         * for compound bodies), and the BVH leaf id assigned at attach
+         * time. A body may have zero or more attached colliders.
+         *
+         * @type {Array<Array<{collider: Collider, transform: Transform, entity: number, bvhNode: number}>>}
+         */
+        this.__body_collider_lists = [];
+
+        /**
+         * Bound reference to {@link __pair_filter} so we hand the same
+         * callable to {@link generate_pairs} each step without per-step
+         * allocation.
+         * @private
+         */
+        this.__pair_filter_bound = (idA, idB) => this.__pair_filter(idA, idB);
+    }
+
+    /**
+     * Symmetric layer/mask check + optional user callback. Called per
+     * candidate pair during broadphase. Returns `true` to accept the pair.
+     *
+     * Layer/mask rule: pair (A, B) collides iff
+     *   `(A.layer & B.mask) !== 0 && (B.layer & A.mask) !== 0`.
+     * The user's contact-filter callback is consulted only for pairs that
+     * pass the bitmask test (cheap gate first).
+     *
+     * @private
+     * @param {number} idA packed body id
+     * @param {number} idB packed body id
+     * @returns {boolean}
+     */
+    __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;
+
+        // Layer/mask gate (symmetric).
+        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;
+        if (fn !== null) {
+            const entA = this.storage.entity_at(idxA);
+            const entB = this.storage.entity_at(idxB);
+            const colA = this.__primary_collider(idxA);
+            const colB = this.__primary_collider(idxB);
+            if (!fn(entA, entB, colA, colB)) return false;
+        }
+        return true;
+    }
+
+    /**
+     * First attached collider of a body, or `null` if none. Used by the
+     * solver to read material parameters (friction / restitution) when the
+     * per-contact source-collider identity isn't tracked yet (v1 limitation
+     * — multi-collider bodies with mixed materials lose precision here).
+     *
+     * @private
+     * @param {number} body_idx
+     * @returns {Collider|null}
+     */
+    __primary_collider(body_idx) {
+        const list = this.__body_collider_lists[body_idx];
+        return (list !== undefined && list.length > 0) ? list[0].collider : null;
+    }
+
+    /**
+     * Replace the world gravity vector. Effective on the next step.
+     * @param {Vector3|{x:number,y:number,z:number}} v
+     */
+    setGravity(v) {
+        this.gravity.set(v.x, v.y, v.z);
+    }
+
+    /**
+     * Install (or remove with `null`) the contact filter callback.
+     * @param {((entityA:number, entityB:number, colliderA:Collider, colliderB:Collider) => boolean) | null} fn
+     */
+    setContactFilter(fn) {
+        this.__contact_filter = fn;
+    }
+
+    /**
+     * @returns {((entityA:number, entityB:number, colliderA:Collider, colliderB:Collider) => boolean) | null}
+     */
+    getContactFilter() {
+        return this.__contact_filter;
+    }
+
+    /**
+     * @private
+     * @param {RigidBody} rb
+     * @param {Collider} collider
+     * @param {Transform} transform
+     * @returns {number} BVH node id
+     */
+    __insert_into_broadphase(rb, collider, transform) {
+        const shape = collider.shape;
+
+        assert.notNull(shape, 'Collider.shape must be set before attaching');
+
+        shape.compute_bounding_box(scratch_local_aabb);
+
+        const p = transform.position;
+        const q = transform.rotation;
+
+        aabb_transform_oriented(
+            scratch_world_aabb, 0,
+            scratch_local_aabb[0], scratch_local_aabb[1], scratch_local_aabb[2],
+            scratch_local_aabb[3], scratch_local_aabb[4], scratch_local_aabb[5],
+            p.x, p.y, p.z,
+            q.x, q.y, q.z, q.w
+        );
+
+        const bvh = rb.kind === BodyKind.Static ? this.staticBvh : this.dynamicBvh;
+        const node = bvh.allocate_node();
+
+        bvh.node_set_aabb_primitive(
+            node,
+            scratch_world_aabb[0], scratch_world_aabb[1], scratch_world_aabb[2],
+            scratch_world_aabb[3], scratch_world_aabb[4], scratch_world_aabb[5]
+        );
+        bvh.node_set_user_data(node, rb._bodyId);
+        bvh.insert_leaf(node);
+
+        return node;
+    }
+
+    /**
+     * @private
+     * @param {RigidBody} rb
+     * @param {number} node
+     */
+    __remove_from_broadphase(rb, node) {
+        const bvh = rb.kind === BodyKind.Static ? this.staticBvh : this.dynamicBvh;
+        bvh.remove_leaf(node);
+        bvh.release_node(node);
+    }
+
+    /**
+     * Lifecycle entry: invoked when an entity gains the (RigidBody, Transform)
+     * tuple. Allocates the body's slot in storage and seeds an empty collider
+     * list. Colliders are attached separately via {@link attach_collider} (the
+     * paired {@link ColliderObserverSystem} drives this from the dataset; in
+     * tests, call manually).
+     *
+     * @param {RigidBody} rigidBody
+     * @param {Transform} transform
+     * @param {number} entity
+     */
+    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);
+        this.storage.set_flags(index, rigidBody.flags);
+
+        this.__bodies[index] = rigidBody;
+        this.__transforms[index] = transform;
+        this.__body_collider_lists[index] = [];
+
+        // Static bodies do not need to live in the active list — they never move.
+        if (rigidBody.kind === BodyKind.Static) {
+            this.storage.mark_sleeping(index);
+        }
+    }
+
+    /**
+     * Detach every collider attached to this body, free its slot, and clear
+     * the side-tables.
+     *
+     * @param {RigidBody} rigidBody
+     * @param {Transform} transform
+     * @param {number} entity
+     */
+    unlink(rigidBody, transform, entity) {
+        const packed = rigidBody._bodyId;
+
+        assert.equal(this.storage.is_valid(packed), true, 'unlink: stale or absent body id');
+
+        // If the body is sleeping inside a multi-member sleep group, dissolve
+        // the group by waking every member first. Otherwise the surviving
+        // members would hold dangling indices in their sleep-group chain
+        // pointing at a slot that has just been freed.
+        if (rigidBody.sleep_group_next !== -1) {
+            this.__wake_body(rigidBody);
+        }
+
+        const index = body_id_index(packed);
+        const list = this.__body_collider_lists[index];
+        if (list !== undefined) {
+            for (let i = 0; i < list.length; i++) {
+                const entry = list[i];
+                if (entry.bvhNode !== COLLIDER_UNBOUND) {
+                    this.__remove_from_broadphase(rigidBody, entry.bvhNode);
+                }
+                entry.collider._bvhNode = COLLIDER_UNBOUND;
+                entry.collider._bodyId = -1;
+            }
+        }
+
+        this.__bodies[index] = undefined;
+        this.__transforms[index] = undefined;
+        this.__body_collider_lists[index] = undefined;
+
+        this.storage.free(packed);
+        rigidBody._bodyId = RIGID_BODY_UNALLOCATED;
+    }
+
+    /**
+     * Attach a collider to an existing body. The collider can live on the
+     * same entity as the body (single-collider body) or on a child entity
+     * (compound body). The world transform passed here is the collider's
+     * own — for a same-entity collider it is the body's Transform; for a
+     * child collider it is the child entity's Transform.
+     *
+     * Idempotent — re-attaching the same collider is a no-op.
+     *
+     * @param {number} body_entity entity that owns the body
+     * @param {Collider} collider
+     * @param {Transform} transform world transform of the collider
+     * @param {number} [collider_entity] entity owning the collider (defaults to body_entity)
+     */
+    attach_collider(body_entity, collider, transform, collider_entity = body_entity) {
+        // Find the body by walking the storage entity table. The body must
+        // have been allocated via `link` before any colliders are attached.
+        const body_index = this.__find_body_index_by_entity(body_entity);
+        assert.notEqual(body_index, -1, `attach_collider: no body found for entity ${body_entity}`);
+
+        const rb = this.__bodies[body_index];
+        // Idempotent: skip if collider already attached.
+        if (collider._bvhNode !== COLLIDER_UNBOUND) return;
+
+        const node = this.__insert_into_broadphase(rb, collider, transform);
+
+        collider._bvhNode = node;
+        collider._bodyId = rb._bodyId;
+        this.__body_collider_lists[body_index].push({
+            collider, transform, entity: collider_entity, bvhNode: node,
+        });
+    }
+
+    /**
+     * Reverse of {@link attach_collider}. Idempotent.
+     *
+     * @param {number} body_entity
+     * @param {Collider} collider
+     */
+    detach_collider(body_entity, collider) {
+        if (collider._bvhNode === COLLIDER_UNBOUND) return;
+
+        const body_index = this.__find_body_index_by_entity(body_entity);
+        if (body_index === -1) return;
+
+        const rb = this.__bodies[body_index];
+        this.__remove_from_broadphase(rb, collider._bvhNode);
+
+        const list = this.__body_collider_lists[body_index];
+        for (let i = 0; i < list.length; i++) {
+            if (list[i].collider === collider) {
+                list.splice(i, 1);
+                break;
+            }
+        }
+
+        collider._bvhNode = COLLIDER_UNBOUND;
+        collider._bodyId = -1;
+    }
+
+    /**
+     * Linear scan over body slots looking for the one whose entity matches.
+     * O(N) where N is the live body count — only called on the link/unlink
+     * paths, not during simulation, so the scan cost is bounded.
+     *
+     * @private
+     * @param {number} entity
+     * @returns {number} body index or -1
+     */
+    __find_body_index_by_entity(entity) {
+        const hwm = this.storage.high_water_mark;
+        for (let i = 0; i < hwm; i++) {
+            if (this.storage.entity_at(i) === entity) return i;
+        }
+        return -1;
+    }
+
+    /**
+     * Resolve a packed body id to its entity, or `-1` if the id is stale.
+     * @param {number} packed_body_id
+     * @returns {number}
+     */
+    entityOf(packed_body_id) {
+        if (!this.storage.is_valid(packed_body_id)) return -1;
+        return this.storage.entity_at(body_id_index(packed_body_id));
+    }
+
+    /**
+     * Number of live bodies (regardless of awake/sleeping state).
+     * @returns {number}
+     */
+    get bodyCount() {
+        return this.storage.size;
+    }
+
+    /**
+     * Apply an instantaneous change of momentum at the body's centre of mass.
+     * Linear-only — see {@link applyImpulseAt} for an off-centre impulse that
+     * also produces angular response.
+     *
+     * Wakes the body if it is asleep.
+     *
+     * @param {RigidBody} rigidBody
+     * @param {Vector3|{x:number,y:number,z:number}} impulse
+     */
+    applyImpulse(rigidBody, impulse) {
+        if (rigidBody.kind !== BodyKind.Dynamic) {
+            return;
+        }
+        const inv_m = rigidBody.mass > 0 ? 1 / rigidBody.mass : 0;
+        rigidBody.linearVelocity.addScaled(impulse, inv_m);
+
+        this.__wake_body(rigidBody);
+    }
+
+    /**
+     * Apply an instantaneous change of momentum at a specific world-space point.
+     * Off-centre impulses produce both linear (Δv = P/m) and angular
+     * (Δω = I_w⁻¹·(r × P)) response.
+     *
+     * Wakes the body if it is asleep.
+     *
+     * @param {RigidBody} rigidBody
+     * @param {Transform} transform body's current world Transform (used for r and I_w)
+     * @param {Vector3|{x:number,y:number,z:number}} impulse
+     * @param {Vector3|{x:number,y:number,z:number}} worldPoint
+     */
+    applyImpulseAt(rigidBody, transform, impulse, worldPoint) {
+        if (rigidBody.kind !== BodyKind.Dynamic) {
+            return;
+        }
+        const inv_m = rigidBody.mass > 0 ? 1 / rigidBody.mass : 0;
+
+        rigidBody.linearVelocity.set(
+            rigidBody.linearVelocity.x + impulse.x * inv_m,
+            rigidBody.linearVelocity.y + impulse.y * inv_m,
+            rigidBody.linearVelocity.z + impulse.z * inv_m
+        );
+
+        const rx = worldPoint.x - transform.position.x;
+        const ry = worldPoint.y - transform.position.y;
+        const rz = worldPoint.z - transform.position.z;
+
+        // Δω = I_w⁻¹ · (r × P)
+        const tx = ry * impulse.z - rz * impulse.y;
+        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, transform, tx, ty, tz);
+
+        rigidBody.angularVelocity.set(
+            rigidBody.angularVelocity.x + scratch_angular_delta[0],
+            rigidBody.angularVelocity.y + scratch_angular_delta[1],
+            rigidBody.angularVelocity.z + scratch_angular_delta[2]
+        );
+
+        this.__wake_body(rigidBody);
+    }
+
+    /**
+     * Accumulate a continuous torque (world-space) for integration on the
+     * next fixedUpdate. Pairs with {@link applyForce} for the rotational case.
+     *
+     * Wakes the body if asleep.
+     *
+     * @param {RigidBody} rigidBody
+     * @param {Vector3|{x:number,y:number,z:number}} torque
+     */
+    applyTorque(rigidBody, torque) {
+        if (rigidBody.kind !== BodyKind.Dynamic) {
+            return;
+        }
+        rigidBody.accumulatedTorque.set(
+            rigidBody.accumulatedTorque.x + torque.x,
+            rigidBody.accumulatedTorque.y + torque.y,
+            rigidBody.accumulatedTorque.z + torque.z
+        );
+        this.__wake_body(rigidBody);
+    }
+
+    /**
+     * 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.
+     *
+     * Wakes the body if asleep.
+     *
+     * @param {RigidBody} rigidBody
+     * @param {Transform} transform body's current world Transform
+     * @param {Vector3|{x:number,y:number,z:number}} force
+     * @param {Vector3|{x:number,y:number,z:number}} worldPoint
+     */
+    applyForceAt(rigidBody, transform, force, worldPoint) {
+        if (rigidBody.kind !== BodyKind.Dynamic) {
+            return;
+        }
+        rigidBody.accumulatedForce.set(
+            rigidBody.accumulatedForce.x + force.x,
+            rigidBody.accumulatedForce.y + force.y,
+            rigidBody.accumulatedForce.z + force.z
+        );
+
+        const rx = worldPoint.x - transform.position.x;
+        const ry = worldPoint.y - transform.position.y;
+        const rz = worldPoint.z - transform.position.z;
+
+        rigidBody.accumulatedTorque.set(
+            rigidBody.accumulatedTorque.x + (ry * force.z - rz * force.y),
+            rigidBody.accumulatedTorque.y + (rz * force.x - rx * force.z),
+            rigidBody.accumulatedTorque.z + (rx * force.y - ry * force.x)
+        );
+
+        this.__wake_body(rigidBody);
+    }
+
+    /**
+     * Accumulate a continuous force to be integrated next fixedUpdate step.
+     * Wakes the body if asleep.
+     *
+     * @param {RigidBody} rigidBody
+     * @param {Vector3|{x:number,y:number,z:number}} force
+     */
+    applyForce(rigidBody, force) {
+        if (rigidBody.kind !== BodyKind.Dynamic) {
+            return;
+        }
+        rigidBody.accumulatedForce.add( force);
+        this.__wake_body(rigidBody);
+    }
+
+    /**
+     * Replace the linear velocity. Wakes the body if asleep.
+     *
+     * @param {RigidBody} rigidBody
+     * @param {Vector3|{x:number,y:number,z:number}} v
+     */
+    setLinearVelocity(rigidBody, v) {
+        rigidBody.linearVelocity.copy(v);
+        if (rigidBody.kind === BodyKind.Dynamic) {
+            this.__wake_body(rigidBody);
+        }
+    }
+
+    /**
+     * Force the body awake. Static bodies are ignored.
+     * @param {RigidBody} rigidBody
+     */
+    wake(rigidBody) {
+        this.__wake_body(rigidBody);
+    }
+
+    /**
+     * Force the body asleep. Dynamic bodies will not re-enter the active list
+     * until a wake event occurs.
+     * @param {RigidBody} rigidBody
+     */
+    sleep(rigidBody) {
+        if (rigidBody.kind !== BodyKind.Dynamic) {
+            return;
+        }
+        if (rigidBody.sleepState === SleepState.Sleeping) {
+            return;
+        }
+        rigidBody.sleepState = SleepState.Sleeping;
+        const index = body_id_index(rigidBody._bodyId);
+        this.storage.mark_sleeping(index);
+    }
+
+    /**
+     * Wake a body and atomically wake every other body it was last sleeping
+     * with (its "sleep group"). Sleep groups are circular doubly-linked lists
+     * threaded through every member of an island when it sleeps atomically;
+     * waking any one member walks the chain and wakes the rest in the same
+     * call.
+     *
+     * Without this, a 100-block stack hit at the base would wake one block
+     * per frame as the broadphase propagated awareness up the stack — a
+     * visible ~1.6 s wave at 60 fps. Atomic wake eliminates the wave.
+     *
+     * No-op for non-dynamic bodies. Idempotent for already-awake bodies.
+     * @private
+     * @param {RigidBody} rb
+     */
+    __wake_body(rb) {
+        if (rb.kind !== BodyKind.Dynamic) return;
+        if (rb.sleepState === SleepState.Awake) return;
+        const index = body_id_index(rb._bodyId);
+
+        // Remember the next-in-chain before clearing the body's own pointers;
+        // the rest of the group is reached by walking forward from there.
+        const start_next = rb.sleep_group_next;
+
+        rb.sleepState = SleepState.Awake;
+        rb.sleep_timer = 0;
+        rb.sleep_group_next = -1;
+        rb.sleep_group_prev = -1;
+        this.storage.mark_awake(index);
+
+        if (start_next === -1 || start_next === index) return;
+
+        // Walk the (now-broken) chain forward until we loop back. The chain
+        // is circular so we know when to stop; defensive `-1` guards against
+        // corruption from a body being unlinked mid-sleep-group.
+        let cur = start_next;
+        while (cur !== -1 && cur !== index) {
+            const cur_rb = this.__bodies[cur];
+            if (cur_rb === undefined) break;
+            const nxt = cur_rb.sleep_group_next;
+            cur_rb.sleepState = SleepState.Awake;
+            cur_rb.sleep_timer = 0;
+            cur_rb.sleep_group_next = -1;
+            cur_rb.sleep_group_prev = -1;
+            this.storage.mark_awake(cur);
+            cur = nxt;
+        }
+    }
+
+    /**
+     * Atomically put every body in a contiguous range of island members to
+     * sleep. Members are threaded into a circular doubly-linked list so any
+     * future `wake` on any member walks the chain and revives them all.
+     *
+     * Velocities are zeroed because the body is by definition at rest at
+     * this point — the alternative (storing residual velocities for "softer"
+     * wake) is what Bullet does, but for a deterministic game-physics target
+     * fully resetting is simpler and avoids drift while sleeping.
+     *
+     * @private
+     * @param {Uint32Array} member_array view (or full array) of body indices
+     * @param {number} start
+     * @param {number} end
+     */
+    __atomic_sleep_island_range(member_array, start, end) {
+        const count = end - start;
+        if (count === 0) return;
+        const bodies = this.__bodies;
+        const storage = this.storage;
+
+        if (count === 1) {
+            const idx = member_array[start];
+            const rb = bodies[idx];
+            if (rb === undefined) return;
+            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;
+            storage.mark_sleeping(idx);
+            return;
+        }
+
+        for (let i = 0; i < count; i++) {
+            const idx = member_array[start + i];
+            const rb = bodies[idx];
+            if (rb === undefined) continue;
+            const next_idx = member_array[start + ((i + 1) % count)];
+            const prev_idx = member_array[start + ((i - 1 + count) % count)];
+            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;
+            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.
+     *
+     * Narrowphase refinement against the actual shape geometry is a
+     * follow-up — for now `result.t` is the distance to the leaf's
+     * inflated AABB and `result.normal` is the AABB face normal. Both are
+     * exact for AABB-shaped colliders.
+     *
+     * @param {Ray3} ray  origin + unit direction + `tMax`
+     * @param {PhysicsSurfacePoint} result populated on hit; untouched on miss
+     * @param {(entity:number, collider:Collider)=>boolean} [filter] defaults
+     *   to {@link returnTrue} (accept every candidate)
+     * @returns {boolean}
+     */
+    raycast(ray, result, filter = returnTrue) {
+        return raycast_query(this, ray, result, filter);
+    }
+
+    /**
+     * Run one simulation step. v1 pipeline: integrate velocity → integrate
+     * position → refit broadphase AABBs. Narrowphase, solver, islands and
+     * contact-event emission land in subsequent slices.
+     *
+     * @param {number} dt
+     */
+    /**
+     * 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.
+     * @private
+     */
+    __wake_pairs() {
+        const list = this.pairs;
+        const n = list.count;
+        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);
+            }
+            if (b !== undefined && b.sleepState === SleepState.Sleeping) {
+                this.__wake_body(b);
+            }
+        }
+    }
+
+    /**
+     * Per-island atomic sleep test. Walks each island once and applies the
+     * decision uniformly across all members:
+     *
+     *  - 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
+     *    {@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
+     *    the same step (members get threaded into a sleep-group chain so
+     *    {@link __wake_body} can wake them all in one call).
+     *  - Otherwise the island has at least one active member, so every
+     *    member's timer is reset.
+     *
+     * This is the design-plan atomic-island sleep — replaces the per-body
+     * approximation that lived in this slot during the previous slice.
+     * Weakly-connected piles no longer chatter awake when a single member
+     * blips above threshold; piles fall asleep and wake up as one.
+     *
+     * @private
+     * @param {number} dt
+     */
+    __sleep_test(dt) {
+        const threshold_sqr = this.sleepVelocitySqrThreshold;
+        const time_threshold = this.sleepTimeThreshold;
+        const bodies = this.__bodies;
+        const islands = this.islands;
+        const island_count = islands.island_count;
+        const body_offsets = islands.body_offsets;
+        const body_data = islands.body_data;
+
+        for (let isl = 0; isl < island_count; isl++) {
+            const start = body_offsets[isl];
+            const end = body_offsets[isl + 1];
+            if (end === start) continue;
+
+            // Pass 1: find max v² + check DisableSleep across the island.
+            let max_v_sqr = 0;
+            let any_disable_sleep = false;
+            for (let i = start; i < end; i++) {
+                const idx = body_data[i];
+                const rb = bodies[idx];
+                if (rb === undefined) continue;
+                if ((rb.flags & RigidBodyFlags.DisableSleep) !== 0) {
+                    any_disable_sleep = true;
+                    break;
+                }
+                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];
+                if (v_sqr > max_v_sqr) max_v_sqr = v_sqr;
+            }
+
+            if (any_disable_sleep) {
+                // Whole island is exempt — reset every member's timer.
+                for (let i = start; i < end; i++) {
+                    const rb = bodies[body_data[i]];
+                    if (rb !== undefined) rb.sleep_timer = 0;
+                }
+                continue;
+            }
+
+            if (max_v_sqr < threshold_sqr) {
+                // Island is at rest — increment every member's timer; if the
+                // slowest-stabilising member has crossed the time threshold,
+                // every member has (they were incremented together this step),
+                // so atomic-sleep the island.
+                let min_timer = Infinity;
+                for (let i = start; i < end; i++) {
+                    const rb = bodies[body_data[i]];
+                    if (rb === undefined) continue;
+                    rb.sleep_timer += dt;
+                    if (rb.sleep_timer < min_timer) min_timer = rb.sleep_timer;
+                }
+                if (min_timer >= time_threshold) {
+                    this.__atomic_sleep_island_range(body_data, start, end);
+                }
+            } else {
+                // At least one member is active — reset every timer.
+                for (let i = start; i < end; i++) {
+                    const rb = bodies[body_data[i]];
+                    if (rb !== undefined) rb.sleep_timer = 0;
+                }
+            }
+        }
+    }
+
+    /**
+     * Dispatch every buffered contact event through:
+     *  - the system-level Signals ({@link onContactBegin}/Stay/End), always; and
+     *  - the entity-level event channel via `dataset.sendEvent(entity, name, payload)`,
+     *    when a dataset is attached.
+     *
+     * Payload is a reused scratch object; listeners must copy anything they
+     * intend to retain past the listener body.
+     * @private
+     */
+    __dispatch_contact_events() {
+        const events = this.contactEvents;
+        const n = events.count;
+        if (n === 0) return;
+
+        const ecd = (this.entityManager !== null && this.entityManager !== undefined)
+            ? this.entityManager.dataset
+            : null;
+        const manifolds = this.manifolds;
+        const data = manifolds.data_buffer;
+
+        const payload = this.__contact_payload;
+
+        for (let i = 0; i < n; i++) {
+            const kind = events.kind_at(i);
+            const entA = events.entityA_at(i);
+            const entB = events.entityB_at(i);
+            const slot = events.slot_at(i);
+
+            // Use the deepest contact of the manifold as the representative
+            // point/normal/depth for the event. v1: contact 0 only.
+            const slot_off = manifolds.slot_data_offset(slot);
+            const has_contact = manifolds.contact_count(slot) > 0;
+
+            // Scratch payload — no observers subscribe to its component
+            // vectors; write indices directly to skip the Signal dispatch.
+            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 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];
+                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;
+                payload.depth = 0;
+            }
+            payload.entityA = entA;
+            payload.entityB = entB;
+
+            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;   }
+
+            signal.send1(payload);
+
+            if (ecd !== null && ecd !== undefined) {
+                if (entA >= 0) ecd.sendEvent(entA, event_name, payload);
+                if (entB >= 0) ecd.sendEvent(entB, event_name, payload);
+            }
+        }
+    }
+
+    fixedUpdate(dt) {
+        const gx = this.gravity.x;
+        const gy = this.gravity.y;
+        const gz = this.gravity.z;
+
+        const lists = this.__body_collider_lists;
+
+        // ─── Once-per-tick stages (broadphase, narrowphase, islands) ────
+        //
+        // Position-dependent work — broadphase refit, pair generation,
+        // narrowphase contact extraction, island partitioning — runs ONCE
+        // at the top of the tick using the pose at tick-start. Substep
+        // position integration happens inside the inner loop further down;
+        // intra-tick motion is bounded by the fat AABB margin (it uses the
+        // full-tick dt for sweep volume) so manifolds stay valid across
+        // substeps without re-running narrowphase.
+
+        // Stage 1: refit each awake body's collider leaves with a fat AABB
+        // padded for the full tick's swept extent.
+        const count = this.storage.awake_count;
+        for (let i = 0; i < count; i++) {
+            const idx = this.storage.awake_at(i);
+            const rb = this.__bodies[idx];
+            const list = lists[idx];
+            if (list === undefined) continue;
+            const lv = rb.linearVelocity;
+            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);
+            }
+        }
+
+        // Stage 2: broadphase pair generation (canonical min,max body pairs).
+        generate_pairs(
+            this.storage,
+            this.dynamicBvh,
+            this.staticBvh,
+            this.manifolds,
+            lists,
+            this.pairs,
+            this.__pair_filter_bound,
+        );
+
+        // Stage 3: wake propagation. Any sleeping body whose leaf overlaps
+        // an awake leaf this tick must participate in narrowphase + solve.
+        this.__wake_pairs();
+
+        // Stage 4: narrowphase — closed-form fast paths or GJK+EPA per pair.
+        narrowphase_step(this.pairs, this.manifolds, this);
+
+        // Stage 5: partition awake bodies + touched contacts into islands.
+        this.islands.build(this.storage, this.manifolds, this.__bodies, this.__body_collider_lists);
+
+        // ─── TGS substep loop ───────────────────────────────────────────
+        //
+        // Each substep advances velocity by `gravity * sub_dt`, runs a
+        // short velocity-iteration sweep with Baumgarte bias scaled to
+        // `sub_dt`, and integrates position by `sub_dt`. Warm-start only
+        // fires on substep 0 — subsequent substeps continue from the
+        // impulses already accumulated in the manifold data buffer (those
+        // are reflected in the current velocity state; re-applying would
+        // double-count).
+        //
+        // Position correction is K× stronger over a tick than classic PGS
+        // for the same Baumgarte beta, because the velocity-correcting
+        // impulse fires K times and each one's position contribution
+        // integrates immediately. This is the mechanism that lets TGS
+        // resolve tall stacks where single-shot PGS just spreads info
+        // through the chain without resolving cumulative drift.
+        const substeps = this.solverSubsteps | 0;
+        const iters_per_substep = this.solverItersPerSubstep | 0;
+        const sub_dt = dt / substeps;
+
+        // Re-read awake_count because __wake_pairs may have grown it.
+        const count_after_wake = this.storage.awake_count;
+
+        for (let s = 0; s < substeps; s++) {
+            // Integrate velocity (gravity + accumulated forces) for this substep.
+            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];
+                integrate_velocity(rb, tr, gx, gy, gz, sub_dt);
+            }
+
+            // Solve velocity constraints over the current pose. Bias term
+            // inside the solver uses `sub_dt`, so per-substep Baumgarte
+            // correction is K× stronger than full-dt PGS would apply.
+            solve_contacts(this.manifolds, this, sub_dt, iters_per_substep, s === 0);
+
+            // Advance position with the solved velocity.
+            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];
+                integrate_position(rb, tr, sub_dt);
+            }
+        }
+
+        // ─── End-of-tick stages (sleep, events, manifold cleanup) ───────
+
+        // Stage 6: sleep test using the full-tick `dt` for the sleep timer.
+        this.__sleep_test(dt);
+
+        // Stage 7: 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);
+        this.__dispatch_contact_events();
+
+        // Stage 8 (end-of-step): roll touched → prev_touched and evict slots
+        // whose pair has not been touched within the grace window.
+        this.manifolds.advance_frame();
+    }
+}
+
+/**
+ * @readonly
+ * @type {boolean}
+ */
+PhysicsSystem.prototype.isPhysicsSystem = true;
+
+// Re-export for convenience.
+export { BodyKind, RigidBodyFlags };