@series-inc/rundot-syncplay 5.23.0-beta.7 → 5.23.0

package/dist/physics3d.js

@@ -1,5 +1,8 @@
 import { cloneCanonical, defaultChecksum } from './canonical.js';
 import { combineDeterministicPhysicsMaterial } from './deterministic-primitives.js';
+import { deterministicSqrt, mulMat3Vec, quantizeAngle, quantizeDistance, quaternionFromQuarterTurns, quatToMat3, rotatePointByQuatInverse, worldInvInertia, } from './physics3d-shared.js';
+import { generateManifold, solveRigidStep, WARM_KEY_BASE, } from './physics3d-solver.js';
+import { createSolverJoints, } from './physics3d-joints.js';
 export const deterministicPhysicsCallback3D = {
     collisionEnter: 1,
     collisionStay: 2,
@@ -18,32 +21,268 @@ export function createDeterministicPhysicsWorld3D(bodies) {
         bodies: normalizeBodies(bodies),
         activePairs: [],
         activePairMetadata: [],
+        contactCache: [],
+        jointCache: [],
     };
 }
+function worldInverseInertiaForBody(body) {
+    if (body.kind !== 'dynamic' || body.orientation === undefined) {
+        return [0, 0, 0, 0, 0, 0, 0, 0, 0];
+    }
+    const invInertiaLocal = deriveInverseInertia(body.shape, body.mass ?? 1);
+    const base = worldInvInertia(quatToMat3(body.orientation), invInertiaLocal);
+    const lock = body.lockAngular ?? {};
+    const lockArr = [lock.x === true ? 0 : 1, lock.y === true ? 0 : 1, lock.z === true ? 0 : 1];
+    const out = new Array(9).fill(0);
+    for (let row = 0; row < 3; row += 1) {
+        for (let col = 0; col < 3; col += 1) {
+            out[row * 3 + col] = base[row * 3 + col] * lockArr[row] * lockArr[col];
+        }
+    }
+    return out;
+}
+/**
+ * Bodies in the same contact island as `bodyId` (union-find over active pairs).
+ * Islands chain only through dynamic bodies — a static/kinematic body is a
+ * boundary, so wake does not leak across the floor to unrelated piles.
+ */
+function contactIslandFor(world, bodyId) {
+    const dynamicIds = new Set(world.bodies.filter((body) => body.kind === 'dynamic').map((body) => body.id));
+    const parent = new Map();
+    const find = (id) => {
+        if (!parent.has(id)) {
+            parent.set(id, id);
+        }
+        let root = id;
+        while (parent.get(root) !== root) {
+            root = parent.get(root);
+        }
+        return root;
+    };
+    const union = (a, b) => {
+        parent.set(find(a), find(b));
+    };
+    for (const pair of world.activePairMetadata ?? []) {
+        if (dynamicIds.has(pair.a) && dynamicIds.has(pair.b)) {
+            union(pair.a, pair.b);
+        }
+    }
+    const targetRoot = find(bodyId);
+    const island = new Set([bodyId]);
+    for (const id of parent.keys()) {
+        if (find(id) === targetRoot) {
+            island.add(id);
+        }
+    }
+    return island;
+}
+function assertFinitePoint(point, label) {
+    if (!isFiniteNumber(point.x) || !isFiniteNumber(point.y) || !isFiniteNumber(point.z)) {
+        throw new Error(`${label} must have finite components`);
+    }
+}
+/**
+ * Apply a batch of deterministic impulses in one canonical-world rebuild.
+ * Semantically the impulses accumulate in list order (same math as repeated
+ * `applyDeterministicImpulse3D`, but velocities are quantized once per body at
+ * the end rather than per impulse), and every touched body's contact island is
+ * woken. One `O(bodies)` rebuild instead of one per impulse — this is the
+ * fast path for per-frame controller output (vehicles, KCC pushes).
+ */
+export function applyDeterministicImpulses3D(world, impulses) {
+    if (impulses.length === 0) {
+        return world;
+    }
+    const bodyById = new Map(world.bodies.map((body) => [body.id, body]));
+    const linearDelta = new Map();
+    const angularDelta = new Map();
+    const invInertiaByBody = new Map();
+    for (const entry of impulses) {
+        assertFinitePoint(entry.impulse, 'applyDeterministicImpulses3D: impulse');
+        if (entry.worldPoint !== undefined) {
+            assertFinitePoint(entry.worldPoint, 'applyDeterministicImpulses3D: worldPoint');
+        }
+        const target = bodyById.get(entry.bodyId);
+        if (target === undefined) {
+            throw new Error(`applyDeterministicImpulses3D: unknown body ${entry.bodyId}`);
+        }
+        const mass = target.mass ?? 1;
+        const baseInvMass = target.kind === 'dynamic' ? 1 / mass : 0;
+        const lockLinear = target.lockLinear ?? {};
+        const previousLinear = linearDelta.get(entry.bodyId) ?? { x: 0, y: 0, z: 0 };
+        linearDelta.set(entry.bodyId, {
+            x: previousLinear.x + entry.impulse.x * (lockLinear.x === true ? 0 : baseInvMass),
+            y: previousLinear.y + entry.impulse.y * (lockLinear.y === true ? 0 : baseInvMass),
+            z: previousLinear.z + entry.impulse.z * (lockLinear.z === true ? 0 : baseInvMass),
+        });
+        if (entry.worldPoint !== undefined && target.orientation !== undefined && target.kind === 'dynamic') {
+            let invInertia = invInertiaByBody.get(entry.bodyId);
+            if (invInertia === undefined) {
+                invInertia = worldInverseInertiaForBody(target);
+                invInertiaByBody.set(entry.bodyId, invInertia);
+            }
+            const lever = { x: entry.worldPoint.x - target.x, y: entry.worldPoint.y - target.y, z: entry.worldPoint.z - target.z };
+            const angularKick = mulMat3Vec(invInertia, cross(lever, entry.impulse));
+            const previousAngular = angularDelta.get(entry.bodyId) ?? { x: 0, y: 0, z: 0 };
+            angularDelta.set(entry.bodyId, {
+                x: previousAngular.x + angularKick.x,
+                y: previousAngular.y + angularKick.y,
+                z: previousAngular.z + angularKick.z,
+            });
+        }
+    }
+    const woken = new Set();
+    for (const bodyId of linearDelta.keys()) {
+        for (const member of contactIslandFor(world, bodyId)) {
+            woken.add(member);
+        }
+    }
+    const bodies = world.bodies.map((body) => {
+        const dv = linearDelta.get(body.id);
+        if (dv === undefined) {
+            if (woken.has(body.id) && body.kind === 'dynamic') {
+                return { ...body, sleeping: false, lowVelFrames: 0 };
+            }
+            return body;
+        }
+        const updated = {
+            ...body,
+            vx: quantizeDistance(body.vx + dv.x),
+            vy: quantizeDistance(body.vy + dv.y),
+            vz: quantizeDistance(body.vz + dv.z),
+            sleeping: body.kind === 'dynamic' ? false : body.sleeping,
+            lowVelFrames: 0,
+        };
+        const dw = angularDelta.get(body.id);
+        if (dw === undefined) {
+            return updated;
+        }
+        return {
+            ...updated,
+            angularVel: {
+                x: quantizeDistance((body.angularVel?.x ?? 0) + dw.x),
+                y: quantizeDistance((body.angularVel?.y ?? 0) + dw.y),
+                z: quantizeDistance((body.angularVel?.z ?? 0) + dw.z),
+            },
+        };
+    });
+    return { ...world, bodies: normalizeBodies(bodies, 'trusted') };
+}
+/**
+ * Apply a deterministic impulse to a body (linear + angular via lever arm),
+ * waking the body and its contact island. Returns a new canonical world.
+ */
+export function applyDeterministicImpulse3D(world, bodyId, impulse, worldPoint) {
+    assertFinitePoint(impulse, 'applyDeterministicImpulse3D: impulse');
+    if (worldPoint !== undefined) {
+        assertFinitePoint(worldPoint, 'applyDeterministicImpulse3D: worldPoint');
+    }
+    const target = world.bodies.find((body) => body.id === bodyId);
+    if (target === undefined) {
+        throw new Error(`applyDeterministicImpulse3D: unknown body ${bodyId}`);
+    }
+    const island = contactIslandFor(world, bodyId);
+    const mass = target.mass ?? 1;
+    const baseInvMass = target.kind === 'dynamic' ? 1 / mass : 0;
+    const lockLinear = target.lockLinear ?? {};
+    const dv = {
+        x: impulse.x * (lockLinear.x === true ? 0 : baseInvMass),
+        y: impulse.y * (lockLinear.y === true ? 0 : baseInvMass),
+        z: impulse.z * (lockLinear.z === true ? 0 : baseInvMass),
+    };
+    let angular = target.angularVel;
+    if (worldPoint !== undefined && target.orientation !== undefined && target.kind === 'dynamic') {
+        const r = { x: worldPoint.x - target.x, y: worldPoint.y - target.y, z: worldPoint.z - target.z };
+        const angImpulse = cross(r, impulse);
+        const dw = mulMat3Vec(worldInverseInertiaForBody(target), angImpulse);
+        angular = {
+            x: (target.angularVel?.x ?? 0) + dw.x,
+            y: (target.angularVel?.y ?? 0) + dw.y,
+            z: (target.angularVel?.z ?? 0) + dw.z,
+        };
+    }
+    const bodies = world.bodies.map((body) => {
+        if (body.id === bodyId) {
+            const updated = {
+                ...body,
+                vx: quantizeDistance(body.vx + dv.x),
+                vy: quantizeDistance(body.vy + dv.y),
+                vz: quantizeDistance(body.vz + dv.z),
+                sleeping: body.kind === 'dynamic' ? false : body.sleeping,
+                lowVelFrames: 0,
+            };
+            return angular !== undefined
+                ? { ...updated, angularVel: { x: quantizeDistance(angular.x), y: quantizeDistance(angular.y), z: quantizeDistance(angular.z) } }
+                : updated;
+        }
+        if (island.has(body.id) && body.kind === 'dynamic') {
+            return { ...body, sleeping: false, lowVelFrames: 0 };
+        }
+        return body;
+    });
+    return { ...world, bodies: normalizeBodies(bodies, 'trusted') };
+}
+/** Set a body's linear (and optional angular) velocity deterministically, waking its island. */
+export function setDeterministicBodyVelocity3D(world, bodyId, velocity, angularVelocity) {
+    assertFinitePoint(velocity, 'setDeterministicBodyVelocity3D: velocity');
+    if (angularVelocity !== undefined) {
+        assertFinitePoint(angularVelocity, 'setDeterministicBodyVelocity3D: angularVelocity');
+    }
+    const target = world.bodies.find((body) => body.id === bodyId);
+    if (target === undefined) {
+        throw new Error(`setDeterministicBodyVelocity3D: unknown body ${bodyId}`);
+    }
+    const island = contactIslandFor(world, bodyId);
+    const bodies = world.bodies.map((body) => {
+        if (body.id === bodyId) {
+            const updated = {
+                ...body,
+                vx: quantizeDistance(velocity.x),
+                vy: quantizeDistance(velocity.y),
+                vz: quantizeDistance(velocity.z),
+                sleeping: body.kind === 'dynamic' ? false : body.sleeping,
+                lowVelFrames: 0,
+            };
+            return angularVelocity !== undefined
+                ? { ...updated, angularVel: { x: quantizeDistance(angularVelocity.x), y: quantizeDistance(angularVelocity.y), z: quantizeDistance(angularVelocity.z) } }
+                : updated;
+        }
+        if (island.has(body.id) && body.kind === 'dynamic') {
+            return { ...body, sleeping: false, lowVelFrames: 0 };
+        }
+        return body;
+    });
+    return { ...world, bodies: normalizeBodies(bodies, 'trusted') };
+}
 export function stepDeterministicPhysicsWorld3D(world, options = {}) {
     const scheduledQueries = sortScheduledQueries(options.scheduledQueries ?? []);
     const prePhysicsScheduledQueryResults = scheduledQueries
         .filter((query) => query.phase === 'pre-physics')
         .map((query) => ({ id: query.id, hits: runScheduledQuery(world, query.query) }));
+    if (options.dtTicks !== undefined && options.dtTicks <= 0) {
+        throw new Error(`physics3d step: dtTicks must be a positive finite number, received ${options.dtTicks}`);
+    }
+    const velocityIterations = options.velocityIterations ?? DEFAULT_VELOCITY_ITERATIONS;
+    if (!Number.isInteger(velocityIterations) || velocityIterations <= 0) {
+        throw new Error(`physics3d step: velocityIterations must be a positive integer, received ${velocityIterations}`);
+    }
     const dt = options.dtTicks ?? 1;
     const gravityY = options.gravityY ?? 0;
-    const integrated = normalizeBodies(world.bodies.map((body) => {
-        if (body.kind !== 'dynamic') {
-            return body;
-        }
-        const vy = body.vy + gravityY * dt;
-        return {
-            ...body,
-            x: body.x + body.vx * dt,
-            y: body.y + vy * dt,
-            z: body.z + body.vz * dt,
-            vy,
-            ...integrateAngularState(body, dt),
-        };
-    }));
-    const constrained = solveConstraints3D(integrated, options.constraints ?? []);
-    const solved = solvePenetrations(constrained.bodies);
-    const contacts = detectContacts(solved.bodies);
+    const rigid = runRigidSolver(world, dt, gravityY, velocityIterations, options.joints ?? []);
+    const constrained = solveConstraints3D(rigid.bodies, options.constraints ?? []);
+    const solvedBodies = constrained.bodies;
+    // Dynamic-involved solid-collision events come from the solver's manifolds (so
+    // an enter event coincides with the frame the arresting impulse is applied and
+    // carries its normalImpulse). Trigger/sensor pairs, and solid pairs the solver
+    // skips (no dynamic body — e.g. static-vs-static), come from overlap detection
+    // on the solved positions, preserving their existing event semantics.
+    const solidPairs = new Set(rigid.solidContacts.map((contact) => contact.pair));
+    // Only trigger/sensor pairs and fully non-dynamic pairs can survive the
+    // event filter, so reject everything else before the exact overlap test —
+    // dynamic solid pairs already got their events from the solver manifolds.
+    const overlapContacts = detectContacts(solvedBodies, (left, right) => left.trigger === true || left.sensor === true || right.trigger === true || right.sensor === true
+        || (left.kind !== 'dynamic' && right.kind !== 'dynamic')).filter((contact) => !solidPairs.has(contact.pair));
+    const contacts = [...rigid.solidContacts, ...overlapContacts].sort((left, right) => compareStrings(left.pair, right.pair));
     const activePairs = contacts.map((contact) => contact.pair).sort(compareStrings);
     const activePairMetadata = contacts.map((contact) => ({
         pair: contact.pair,
@@ -55,9 +294,13 @@ export function stepDeterministicPhysicsWorld3D(world, options = {}) {
     const previousPairMetadata = new Map((world.activePairMetadata ?? []).map((pair) => [pair.pair, pair]));
     const previousPairs = new Set(world.activePairs);
     const currentPairs = new Set(activePairs);
-    const bodyById = new Map(solved.bodies.map((body) => [body.id, body]));
+    const bodyById = new Map(solvedBodies.map((body) => [body.id, body]));
     const events = [
-        ...contacts.map((contact) => ({ ...contact, type: previousPairs.has(contact.pair) ? 'stay' : 'enter' })),
+        ...contacts.map((contact) => ({
+            ...contact,
+            type: previousPairs.has(contact.pair) ? 'stay' : 'enter',
+            normalImpulse: rigid.impulseByPair.get(contact.pair) ?? 0,
+        })),
         ...world.activePairs
             .filter((pair) => !currentPairs.has(pair))
             .sort(compareStrings)
@@ -71,26 +314,39 @@ export function stepDeterministicPhysicsWorld3D(world, options = {}) {
                 b: previous?.b ?? b,
                 trigger: previous?.trigger ?? false,
                 sensor: previous?.sensor ?? false,
+                normalImpulse: 0,
             };
         }),
     ]
         .filter((event) => contactEventEnabled(event, bodyById))
         .sort(compareContactEvents);
-    const nextWorld = { frame: world.frame + 1, bodies: solved.bodies, activePairs, activePairMetadata };
+    const nextWorld = {
+        frame: world.frame + 1,
+        bodies: solvedBodies,
+        activePairs,
+        activePairMetadata,
+        contactCache: rigid.contactCache,
+        jointCache: rigid.jointCache,
+    };
     const postPhysicsScheduledQueryResults = scheduledQueries
         .filter((query) => query.phase === 'post-physics')
         .map((query) => ({ id: query.id, hits: runScheduledQuery(nextWorld, query.query) }));
     const scheduledQueryResults = [...prePhysicsScheduledQueryResults, ...postPhysicsScheduledQueryResults];
+    const solverCorrections = rigid.narrowphaseContacts + constrained.corrections;
     return {
-        world: cloneCanonical(nextWorld),
+        // nextWorld is assembled from freshly built, already-canonical parts this
+        // step, so the cloneCanonical round trip it used to pass through was
+        // value-identical — and measured as the single largest per-step cost.
+        world: nextWorld,
         contacts: events,
+        brokenJointIds: rigid.brokenJointIds,
         scheduledQueryResults,
         prePhysicsScheduledQueryResults,
         postPhysicsScheduledQueryResults,
-        broadphasePairs: countCandidatePairs(solved.bodies),
-        narrowphaseContacts: contacts.length,
-        solverCorrections: solved.corrections + constrained.corrections,
-        checksum: options.computeChecksum === false ? '' : defaultChecksum({ nextWorld, scheduledQueryResults, events, solverCorrections: solved.corrections + constrained.corrections }),
+        broadphasePairs: countCandidatePairs(solvedBodies),
+        narrowphaseContacts: rigid.narrowphaseContacts,
+        solverCorrections,
+        checksum: options.computeChecksum === false ? '' : defaultChecksum({ nextWorld, scheduledQueryResults, events, solverCorrections }),
     };
 }
 function fastStressStepPhysicsWorld3D(world, scheduledQueries) {
@@ -114,6 +370,8 @@ function fastStressStepPhysicsWorld3D(world, scheduledQueries) {
         }).sort((left, right) => compareStrings(left.id, right.id)),
         activePairs: [],
         activePairMetadata: [],
+        contactCache: world.contactCache ?? [],
+        jointCache: world.jointCache ?? [],
     };
     const postPhysicsScheduledQueryResults = sortedScheduledQueries
         .filter((query) => query.phase === 'post-physics')
@@ -129,42 +387,279 @@ function fastStressStepPhysicsWorld3D(world, scheduledQueries) {
         solverCorrections: 0,
     };
 }
+const QUERY_INDEX_LEAF_SIZE = 4;
+const queryIndexCache = new WeakMap();
+/**
+ * Conservative world-space bounds as the narrow phase sees the body: bodies
+ * with a quaternion `orientation` use the |R|-transformed local AABB (the
+ * narrow phase ignores legacy quarter turns for them), everything else uses
+ * the exact quarter-turn-aware bounds.
+ */
+const queryIndexBoundsCache = new WeakMap();
+function queryIndexBoundsForBody(body) {
+    if (body.orientation === undefined) {
+        return toBounds(body);
+    }
+    const cached = queryIndexBoundsCache.get(body);
+    if (cached !== undefined) {
+        return cached;
+    }
+    const local = shapePoseBounds({ x: 0, y: 0, z: 0, shape: body.shape });
+    const centerX = (local.minX + local.maxX) / 2;
+    const centerY = (local.minY + local.maxY) / 2;
+    const centerZ = (local.minZ + local.maxZ) / 2;
+    const halfX = (local.maxX - local.minX) / 2;
+    const halfY = (local.maxY - local.minY) / 2;
+    const halfZ = (local.maxZ - local.minZ) / 2;
+    const m = quatToMat3(body.orientation);
+    const worldCenterX = body.x + m[0] * centerX + m[1] * centerY + m[2] * centerZ;
+    const worldCenterY = body.y + m[3] * centerX + m[4] * centerY + m[5] * centerZ;
+    const worldCenterZ = body.z + m[6] * centerX + m[7] * centerY + m[8] * centerZ;
+    const extentX = Math.abs(m[0]) * halfX + Math.abs(m[1]) * halfY + Math.abs(m[2]) * halfZ;
+    const extentY = Math.abs(m[3]) * halfX + Math.abs(m[4]) * halfY + Math.abs(m[5]) * halfZ;
+    const extentZ = Math.abs(m[6]) * halfX + Math.abs(m[7]) * halfY + Math.abs(m[8]) * halfZ;
+    const bounds = {
+        minX: worldCenterX - extentX,
+        maxX: worldCenterX + extentX,
+        minY: worldCenterY - extentY,
+        maxY: worldCenterY + extentY,
+        minZ: worldCenterZ - extentZ,
+        maxZ: worldCenterZ + extentZ,
+    };
+    queryIndexBoundsCache.set(body, bounds);
+    return bounds;
+}
+function buildQueryIndexNode(entries, order, start, end, nodes) {
+    let minX = Number.POSITIVE_INFINITY;
+    let maxX = Number.NEGATIVE_INFINITY;
+    let minY = Number.POSITIVE_INFINITY;
+    let maxY = Number.NEGATIVE_INFINITY;
+    let minZ = Number.POSITIVE_INFINITY;
+    let maxZ = Number.NEGATIVE_INFINITY;
+    for (let index = start; index < end; index += 1) {
+        const bounds = entries[order[index]].bounds;
+        minX = Math.min(minX, bounds.minX);
+        maxX = Math.max(maxX, bounds.maxX);
+        minY = Math.min(minY, bounds.minY);
+        maxY = Math.max(maxY, bounds.maxY);
+        minZ = Math.min(minZ, bounds.minZ);
+        maxZ = Math.max(maxZ, bounds.maxZ);
+    }
+    const nodeIndex = nodes.length;
+    if (end - start <= QUERY_INDEX_LEAF_SIZE) {
+        nodes.push({ minX, maxX, minY, maxY, minZ, maxZ, left: -1, right: -1, start, count: end - start });
+        return nodeIndex;
+    }
+    // split on the widest centroid axis; deterministic tiebreak order x → y → z
+    let axisMinX = Number.POSITIVE_INFINITY;
+    let axisMaxX = Number.NEGATIVE_INFINITY;
+    let axisMinY = Number.POSITIVE_INFINITY;
+    let axisMaxY = Number.NEGATIVE_INFINITY;
+    let axisMinZ = Number.POSITIVE_INFINITY;
+    let axisMaxZ = Number.NEGATIVE_INFINITY;
+    for (let index = start; index < end; index += 1) {
+        const bounds = entries[order[index]].bounds;
+        const cx = (bounds.minX + bounds.maxX) / 2;
+        const cy = (bounds.minY + bounds.maxY) / 2;
+        const cz = (bounds.minZ + bounds.maxZ) / 2;
+        axisMinX = Math.min(axisMinX, cx);
+        axisMaxX = Math.max(axisMaxX, cx);
+        axisMinY = Math.min(axisMinY, cy);
+        axisMaxY = Math.max(axisMaxY, cy);
+        axisMinZ = Math.min(axisMinZ, cz);
+        axisMaxZ = Math.max(axisMaxZ, cz);
+    }
+    const spanX = axisMaxX - axisMinX;
+    const spanY = axisMaxY - axisMinY;
+    const spanZ = axisMaxZ - axisMinZ;
+    const axis = spanY > spanX
+        ? (spanZ > spanY ? 2 : 1)
+        : (spanZ > spanX ? 2 : 0);
+    const centroidOnAxis = (entryIndex) => {
+        const bounds = entries[entryIndex].bounds;
+        if (axis === 0) {
+            return (bounds.minX + bounds.maxX) / 2;
+        }
+        return axis === 1 ? (bounds.minY + bounds.maxY) / 2 : (bounds.minZ + bounds.maxZ) / 2;
+    };
+    const slice = order.slice(start, end);
+    // full strict order (centroid, then body id) — never relies on sort stability
+    slice.sort((left, right) => centroidOnAxis(left) - centroidOnAxis(right)
+        || compareStrings(entries[left].body.id, entries[right].body.id));
+    for (let index = 0; index < slice.length; index += 1) {
+        order[start + index] = slice[index];
+    }
+    const mid = start + ((end - start) >> 1);
+    // reserve this node's slot before recursing so child indexes are stable
+    nodes.push({ minX, maxX, minY, maxY, minZ, maxZ, left: -1, right: -1, start: 0, count: 0 });
+    const left = buildQueryIndexNode(entries, order, start, mid, nodes);
+    const right = buildQueryIndexNode(entries, order, mid, end, nodes);
+    nodes[nodeIndex] = { minX, maxX, minY, maxY, minZ, maxZ, left, right, start: 0, count: 0 };
+    return nodeIndex;
+}
+/** O(1) body lookup backed by the per-world query-index cache. */
+export function deterministicPhysicsBodyById3D(world, bodyId) {
+    return queryIndexFor(world).bodyById.get(bodyId);
+}
+function buildQueryIndexTree(bodies) {
+    const entries = bodies.map((body) => ({ body, bounds: queryIndexBoundsForBody(body) }));
+    const order = entries.map((_, index) => index);
+    const nodes = [];
+    if (entries.length > 0) {
+        buildQueryIndexNode(entries, order, 0, entries.length, nodes);
+    }
+    return { entries, order, nodes };
+}
+/**
+ * Static bodies keep object identity across world snapshots (the solver and
+ * the trusted normalize paths pass them through by reference), so their
+ * subtree — usually the bulk of a level — is reused frame over frame; only
+ * the dynamic subtree is rebuilt. Identity comparison makes reuse exact.
+ */
+let staticQueryTreeCache;
+function sameBodyIdentities(left, right) {
+    if (left.length !== right.length) {
+        return false;
+    }
+    for (let index = 0; index < left.length; index += 1) {
+        if (left[index] !== right[index]) {
+            return false;
+        }
+    }
+    return true;
+}
+function queryIndexFor(world) {
+    const cached = queryIndexCache.get(world);
+    if (cached !== undefined) {
+        return cached;
+    }
+    const statics = [];
+    const movers = [];
+    for (const body of world.bodies) {
+        if (body.kind === 'static') {
+            statics.push(body);
+        }
+        else {
+            movers.push(body);
+        }
+    }
+    let staticTree;
+    if (staticQueryTreeCache !== undefined && sameBodyIdentities(staticQueryTreeCache.statics, statics)) {
+        staticTree = staticQueryTreeCache.tree;
+    }
+    else {
+        staticTree = buildQueryIndexTree(statics);
+        staticQueryTreeCache = { statics, tree: staticTree };
+    }
+    const index = {
+        trees: movers.length > 0 ? [staticTree, buildQueryIndexTree(movers)] : [staticTree],
+        bodyById: new Map(world.bodies.map((body) => [body.id, body])),
+    };
+    queryIndexCache.set(world, index);
+    return index;
+}
+/** Collect entries whose bounds pass `nodeTest`, walking each tree iteratively. */
+function collectQueryCandidates(index, nodeTest) {
+    const candidates = [];
+    for (const tree of index.trees) {
+        if (tree.nodes.length === 0) {
+            continue;
+        }
+        const stack = [0];
+        while (stack.length > 0) {
+            const node = tree.nodes[stack.pop()];
+            if (!nodeTest(node)) {
+                continue;
+            }
+            if (node.left === -1) {
+                for (let slot = node.start; slot < node.start + node.count; slot += 1) {
+                    const entry = tree.entries[tree.order[slot]];
+                    if (nodeTest(entry.bounds)) {
+                        candidates.push(entry);
+                    }
+                }
+                continue;
+            }
+            stack.push(node.right);
+            stack.push(node.left);
+        }
+    }
+    return candidates;
+}
+function rayQueryCandidates(index, query, expandBy) {
+    return collectQueryCandidates(index, (bounds) => {
+        const target = expandBy === undefined ? bounds : expandBoundsByBounds(bounds, expandBy);
+        return rayAabbDistanceExact(target, query.x, query.y, query.z, query.dx, query.dy, query.dz, query.maxDistance) !== undefined;
+    });
+}
 export function raycastDeterministicPhysics3D(world, query) {
     const normalizedQuery = normalizeRaycastQuery3D(query);
-    return world.bodies
-        .filter((body) => queryBodyAllowed(body, normalizedQuery))
-        .map((body) => {
-        const hit = rayBodyDistance(body, normalizedQuery);
-        return hit === undefined ? undefined : { bodyId: body.id, distance: hit };
+    return rayQueryCandidates(queryIndexFor(world), normalizedQuery)
+        .filter((entry) => queryBodyAllowed(entry.body, normalizedQuery))
+        .map((entry) => {
+        const hit = rayBodyDistance(entry.body, normalizedQuery);
+        return hit === undefined ? undefined : { bodyId: entry.body.id, distance: hit };
     })
         .filter((hit) => hit !== undefined)
         .sort(compareHits);
 }
 export function raycastDeterministicPhysics3DDetailed(world, query) {
     const normalizedQuery = normalizeRaycastQuery3D(query);
+    const bodyById = queryIndexFor(world).bodyById;
     return raycastDeterministicPhysics3D(world, query)
         .map((hit) => {
-        const body = world.bodies.find((candidate) => candidate.id === hit.bodyId);
+        const body = bodyById.get(hit.bodyId);
         return detailedHitForBody(body, normalizedQuery, hit.distance, 0);
     })
         .sort(compareHits);
 }
+/**
+ * Exact body-vs-query-shape overlap (the narrow phase of `overlapDeterministicPhysics3D`).
+ * Bodies with a full quaternion orientation use the exact solver geometry
+ * (SAT/closed-form) so overlap against tumbled props is correct; the legacy
+ * quarter-turn path is preserved for bodies without `orientation`.
+ */
+function bodyOverlapsQueryShape(body, shape, x, y, z, queryBounds) {
+    if (body.orientation !== undefined) {
+        return orientedBodyOverlapsShape(body, shape, x, y, z);
+    }
+    return boundsOverlap(toBounds(body), queryBounds)
+        && shapesOverlap3D({ x, y, z, shape }, bodyPose(body));
+}
 export function overlapDeterministicPhysics3D(world, query) {
     const queryBounds = shapeBounds(query.x, query.y, query.z, query.shape);
-    return world.bodies
-        .filter((body) => queryBodyAllowed(body, query))
-        .filter((body) => boundsOverlap(toBounds(body), queryBounds))
-        .filter((body) => shapesOverlap3D({ x: query.x, y: query.y, z: query.z, shape: query.shape }, bodyPose(body)))
-        .map((body) => body.id)
+    return collectQueryCandidates(queryIndexFor(world), (bounds) => boundsOverlap(bounds, queryBounds))
+        .filter((entry) => queryBodyAllowed(entry.body, query))
+        .filter((entry) => bodyOverlapsQueryShape(entry.body, query.shape, query.x, query.y, query.z, queryBounds))
+        .map((entry) => entry.body.id)
         .sort(compareStrings);
 }
+/** Exact overlap between an oriented body and a query shape via the solver manifold geometry. */
+function orientedBodyOverlapsShape(body, queryShape, qx, qy, qz) {
+    const bodyGeom = {
+        index: 0,
+        p: { x: body.x, y: body.y, z: body.z },
+        q: body.orientation ?? { x: 0, y: 0, z: 0, w: 1 },
+        shape: toSolverShape(body.shape),
+    };
+    const queryGeom = {
+        index: 1,
+        p: { x: qx, y: qy, z: qz },
+        q: { x: 0, y: 0, z: 0, w: 1 },
+        shape: toSolverShape(queryShape),
+    };
+    return generateManifold(bodyGeom, queryGeom) !== undefined;
+}
 export function shapeCastDeterministicPhysics3D(world, query) {
     const normalizedQuery = normalizeShapeCastQuery3D(query);
-    return world.bodies
-        .filter((body) => queryBodyAllowed(body, normalizedQuery))
-        .map((body) => {
-        const distance = shapeCastBodyDistance(body, normalizedQuery);
-        return distance === undefined ? undefined : { bodyId: body.id, distance };
+    // prune with body bounds Minkowski-expanded by the cast shape's local bounds,
+    // so the cast reduces to a segment-vs-AABB test against the tree
+    const castExtents = shapeBounds(0, 0, 0, query.shape);
+    return rayQueryCandidates(queryIndexFor(world), normalizedQuery, castExtents)
+        .filter((entry) => queryBodyAllowed(entry.body, normalizedQuery))
+        .map((entry) => {
+        const distance = shapeCastBodyDistance(entry.body, normalizedQuery);
+        return distance === undefined ? undefined : { bodyId: entry.body.id, distance };
     })
         .filter((hit) => hit !== undefined)
         .sort(compareHits);
@@ -172,9 +667,10 @@ export function shapeCastDeterministicPhysics3D(world, query) {
 export function shapeCastDeterministicPhysics3DDetailed(world, query) {
     const normalizedQuery = normalizeShapeCastQuery3D(query);
     const castRadius = query.shape.type === 'sphere' ? query.shape.radius : 0;
+    const bodyById = queryIndexFor(world).bodyById;
     return shapeCastDeterministicPhysics3D(world, query)
         .map((hit) => {
-        const body = world.bodies.find((candidate) => candidate.id === hit.bodyId);
+        const body = bodyById.get(hit.bodyId);
         return detailedHitForBody(body, normalizedQuery, hit.distance, castRadius);
     })
         .sort(compareHits);
@@ -784,11 +1280,269 @@ function callbackFixtureBody(id, x, trigger, callbackMask) {
         callbackMask,
     };
 }
-function detectContacts(bodies) {
+const DEFAULT_LINEAR_DAMPING = 0.995;
+const DEFAULT_ANGULAR_DAMPING = 0.98;
+const DEFAULT_VELOCITY_ITERATIONS = 10;
+function toSolverPrimitiveShape(shape) {
+    if (shape.type === 'box') {
+        return { kind: 'box', half: { x: shape.halfX, y: shape.halfY, z: shape.halfZ } };
+    }
+    if (shape.type === 'sphere') {
+        return { kind: 'sphere', radius: shape.radius };
+    }
+    return { kind: 'capsule', radius: shape.radius, halfHeight: shape.halfHeight };
+}
+function toSolverShape(shape) {
+    if (shape.type === 'box' || shape.type === 'sphere' || shape.type === 'capsule') {
+        return toSolverPrimitiveShape(shape);
+    }
+    if (shape.type === 'compound') {
+        return {
+            kind: 'compound',
+            children: shape.children.map((child) => ({
+                shape: toSolverPrimitiveShape(child.shape),
+                offset: child.offset,
+                q: quaternionFromQuarterTurns(child.quarterTurns ?? { x: 0, y: 0, z: 0 }),
+            })),
+        };
+    }
+    // mesh: local triangle-list, grouped into triples (static geometry).
+    const triangles = [];
+    for (let index = 0; index + 2 < shape.vertices.length; index += 3) {
+        triangles.push([shape.vertices[index], shape.vertices[index + 1], shape.vertices[index + 2]]);
+    }
+    return { kind: 'mesh', triangles };
+}
+export function deriveInverseInertia(shape, mass) {
+    if (shape.type === 'box') {
+        const sx = 2 * shape.halfX;
+        const sy = 2 * shape.halfY;
+        const sz = 2 * shape.halfZ;
+        const ix = (mass / 12) * (sy * sy + sz * sz);
+        const iy = (mass / 12) * (sx * sx + sz * sz);
+        const iz = (mass / 12) * (sx * sx + sy * sy);
+        return { x: 1 / ix, y: 1 / iy, z: 1 / iz };
+    }
+    if (shape.type === 'sphere') {
+        const i = (2 / 5) * mass * shape.radius * shape.radius;
+        return { x: 1 / i, y: 1 / i, z: 1 / i };
+    }
+    if (shape.type === 'capsule') {
+        const r = shape.radius;
+        const h = 2 * shape.halfHeight;
+        const iAxial = 0.5 * mass * r * r;
+        const iLateral = (mass / 12) * (3 * r * r + h * h);
+        return { x: 1 / iLateral, y: 1 / iAxial, z: 1 / iLateral };
+    }
+    if (shape.type === 'compound') {
+        // union-AABB box approximation of the compound inertia (v1).
+        const bounds = shapePoseBounds({ x: 0, y: 0, z: 0, shape });
+        const half = { x: (bounds.maxX - bounds.minX) / 2, y: (bounds.maxY - bounds.minY) / 2, z: (bounds.maxZ - bounds.minZ) / 2 };
+        return deriveInverseInertia({ type: 'box', halfX: Math.max(half.x, 1e-3), halfY: Math.max(half.y, 1e-3), halfZ: Math.max(half.z, 1e-3) }, mass);
+    }
+    // mesh: static, never rotates.
+    return { x: 0, y: 0, z: 0 };
+}
+function buildSolverBody(body, index) {
+    const isDynamic = body.kind === 'dynamic';
+    const mass = body.mass ?? 1;
+    const baseInvMass = isDynamic ? 1 / mass : 0;
+    const lockLinear = body.lockLinear ?? {};
+    const lockAngular = body.lockAngular ?? {};
+    const hasOrientation = body.orientation !== undefined;
+    return {
+        index,
+        id: body.id,
+        isDynamic,
+        sleeping: body.sleeping === true,
+        p: { x: body.x, y: body.y, z: body.z },
+        q: body.orientation ?? { x: 0, y: 0, z: 0, w: 1 },
+        v: { x: body.vx, y: body.vy, z: body.vz },
+        w: body.angularVel ?? { x: 0, y: 0, z: 0 },
+        vb: { x: 0, y: 0, z: 0 },
+        wb: { x: 0, y: 0, z: 0 },
+        invMassVec: {
+            x: lockLinear.x === true ? 0 : baseInvMass,
+            y: lockLinear.y === true ? 0 : baseInvMass,
+            z: lockLinear.z === true ? 0 : baseInvMass,
+        },
+        invInertiaLocal: isDynamic ? deriveInverseInertia(body.shape, mass) : { x: 0, y: 0, z: 0 },
+        angularLock: hasOrientation
+            ? { x: lockAngular.x === true ? 0 : 1, y: lockAngular.y === true ? 0 : 1, z: lockAngular.z === true ? 0 : 1 }
+            : { x: 0, y: 0, z: 0 },
+        matFriction: (body.material?.friction ?? 0) / 10,
+        matRestitution: (body.material?.restitution ?? 0) / 10,
+        linearDamping: body.linearDamping ?? DEFAULT_LINEAR_DAMPING,
+        angularDamping: body.angularDamping ?? DEFAULT_ANGULAR_DAMPING,
+        gravityScale: body.gravityScale ?? 1,
+        hasOrientation,
+        shape: toSolverShape(body.shape),
+        lowVelFrames: body.lowVelFrames ?? 0,
+        wakeForced: false,
+        ccd: body.ccd === true,
+        layer: body.layer,
+        mask: body.mask,
+        noCollide: body.trigger === true || body.sensor === true,
+    };
+}
+function warmCacheToMap(cache, indexById) {
+    const warm = new Map();
+    // Cache entries are sorted by pair, so resolve ids to indices once per run
+    // of equal pair strings instead of splitting every entry.
+    let runPair;
+    let a;
+    let b;
+    for (const entry of cache) {
+        if (entry.pair !== runPair) {
+            runPair = entry.pair;
+            const [aId, bId] = entry.pair.split('|');
+            a = indexById.get(aId);
+            b = indexById.get(bId);
+        }
+        if (a === undefined || b === undefined) {
+            continue;
+        }
+        const key = a * WARM_KEY_BASE + b;
+        let list = warm.get(key);
+        if (list === undefined) {
+            list = [];
+            warm.set(key, list);
+        }
+        list.push({
+            anchor: { x: entry.anchorX, y: entry.anchorY, z: entry.anchorZ },
+            normalImpulse: entry.normalImpulse,
+            tangentImpulse1: entry.tangentImpulse1,
+            tangentImpulse2: entry.tangentImpulse2,
+        });
+    }
+    return warm;
+}
+function runRigidSolver(world, dt, gravityY, velocityIterations, joints) {
+    const source = world.bodies;
+    const indexById = new Map(source.map((body, index) => [body.id, index]));
+    const solverBodies = source.map((body, index) => buildSolverBody(body, index));
+    const warm = warmCacheToMap(world.contactCache ?? [], indexById);
+    const previouslyBroken = new Set((world.jointCache ?? []).filter((entry) => entry.broken === true).map((entry) => entry.jointId));
+    const solverJoints = createSolverJoints(joints, indexById, world.jointCache ?? []);
+    // Suppress contacts for intact jointed pairs flagged disableCollision. A joint
+    // that breaks this step suppresses one final step (it is not yet in
+    // previouslyBroken), resuming collision next step.
+    const noCollideKeys = joints
+        .filter((joint) => joint.type !== 'grab' && 'disableCollision' in joint && joint.disableCollision === true && !previouslyBroken.has(joint.id))
+        .map((joint) => {
+        const jointBodies = joint;
+        const aIndex = indexById.get(jointBodies.a);
+        const bIndex = indexById.get(jointBodies.b);
+        if (aIndex === undefined || bIndex === undefined) {
+            return undefined; // createSolverJoints throws the canonical unknown-body error
+        }
+        return Math.min(aIndex, bIndex) * WARM_KEY_BASE + Math.max(aIndex, bIndex);
+    })
+        .filter((key) => key !== undefined);
+    const noCollidePairs = new Set(noCollideKeys);
+    const result = solveRigidStep(solverBodies, {
+        dt,
+        gravityY,
+        velocityIterations,
+        noCollidePairs: noCollidePairs.size > 0 ? noCollidePairs : undefined,
+        warm,
+        joints: solverJoints,
+    });
+    const bodies = source.map((body, index) => {
+        if (body.kind !== 'dynamic') {
+            return body;
+        }
+        const sb = solverBodies[index];
+        const mapped = {
+            ...body,
+            x: quantizeDistance(sb.p.x),
+            y: quantizeDistance(sb.p.y),
+            z: quantizeDistance(sb.p.z),
+            vx: quantizeDistance(sb.v.x),
+            vy: quantizeDistance(sb.v.y),
+            vz: quantizeDistance(sb.v.z),
+            sleeping: sb.sleeping,
+            lowVelFrames: sb.lowVelFrames,
+        };
+        if (body.orientation !== undefined) {
+            return {
+                ...mapped,
+                orientation: {
+                    x: quantizeDistance(sb.q.x),
+                    y: quantizeDistance(sb.q.y),
+                    z: quantizeDistance(sb.q.z),
+                    w: quantizeDistance(sb.q.w),
+                },
+                angularVel: {
+                    x: quantizeDistance(sb.w.x),
+                    y: quantizeDistance(sb.w.y),
+                    z: quantizeDistance(sb.w.z),
+                },
+            };
+        }
+        // legacy quarter-turn decoration for bodies without a quaternion orientation
+        return { ...mapped, ...integrateAngularState(body, dt) };
+    });
+    const contactCache = warmMapToCache(result.warm, solverBodies);
+    const jointCache = result.jointCache
+        .map((entry) => ({
+        jointId: entry.jointId,
+        impulseX: quantizeDistance(entry.impulseX),
+        impulseY: quantizeDistance(entry.impulseY),
+        impulseZ: quantizeDistance(entry.impulseZ),
+        axialImpulse: quantizeDistance(entry.axialImpulse),
+        hingeAngle: quantizeDistance(entry.hingeAngle),
+        ...(entry.broken === true ? { broken: true } : {}),
+    }))
+        .sort((left, right) => compareStrings(left.jointId, right.jointId));
+    const brokenJointIds = jointCache
+        .filter((entry) => entry.broken === true && !previouslyBroken.has(entry.jointId))
+        .map((entry) => entry.jointId);
+    const impulseByPair = new Map();
+    const solidContacts = [];
+    for (const impulse of result.contactImpulses) {
+        impulseByPair.set(impulse.pair, quantizeDistance(impulse.normalImpulse));
+        solidContacts.push({
+            pair: impulse.pair,
+            a: solverBodies[impulse.a].id,
+            b: solverBodies[impulse.b].id,
+            trigger: false,
+            sensor: false,
+        });
+    }
+    return { bodies, contactCache, jointCache, impulseByPair, solidContacts, narrowphaseContacts: result.manifolds.length, brokenJointIds };
+}
+function warmMapToCache(warm, bodies) {
+    const entries = [];
+    for (const [key, points] of warm.entries()) {
+        const aIndex = Math.floor(key / WARM_KEY_BASE);
+        const bIndex = key % WARM_KEY_BASE;
+        const pair = `${bodies[aIndex].id}|${bodies[bIndex].id}`;
+        for (const point of points) {
+            entries.push({
+                pair,
+                anchorX: quantizeDistance(point.anchor.x),
+                anchorY: quantizeDistance(point.anchor.y),
+                anchorZ: quantizeDistance(point.anchor.z),
+                normalImpulse: quantizeDistance(point.normalImpulse),
+                tangentImpulse1: quantizeDistance(point.tangentImpulse1),
+                tangentImpulse2: quantizeDistance(point.tangentImpulse2),
+            });
+        }
+    }
+    return entries.sort((left, right) => compareStrings(left.pair, right.pair)
+        || left.anchorX - right.anchorX
+        || left.anchorY - right.anchorY
+        || left.anchorZ - right.anchorZ);
+}
+function detectContacts(bodies, keepPair) {
     const contacts = [];
     for (const pair of broadphaseCandidatePairs(bodies)) {
         const left = pair.left;
         const right = pair.right;
+        if (keepPair !== undefined && !keepPair(left, right)) {
+            continue;
+        }
         if ((left.mask & right.layer) === 0 ||
             (right.mask & left.layer) === 0 ||
             !boundsOverlap(pair.leftBounds, pair.rightBounds) ||
@@ -800,99 +1554,6 @@ function detectContacts(bodies) {
     }
     return contacts.sort((left, right) => compareStrings(left.pair, right.pair));
 }
-function solvePenetrations(bodies) {
-    const next = bodies.map((body) => ({ ...body }));
-    let corrections = 0;
-    for (const pair of broadphaseCandidatePairs(next)) {
-        if (pair.left.trigger === true ||
-            pair.right.trigger === true ||
-            pair.left.sensor === true ||
-            pair.right.sensor === true ||
-            (pair.left.mask & pair.right.layer) === 0 ||
-            (pair.right.mask & pair.left.layer) === 0) {
-            continue;
-        }
-        const leftIndex = next.findIndex((body) => body.id === pair.left.id);
-        const rightIndex = next.findIndex((body) => body.id === pair.right.id);
-        const resolved = resolvePair(next[leftIndex], next[rightIndex]);
-        if (resolved.left !== next[leftIndex] || resolved.right !== next[rightIndex]) {
-            next[leftIndex] = resolved.left;
-            next[rightIndex] = resolved.right;
-            corrections += resolved.corrections;
-        }
-    }
-    return { bodies: normalizeBodies(next), corrections };
-}
-function resolvePair(leftBody, rightBody) {
-    if (leftBody.kind !== 'dynamic' && rightBody.kind !== 'dynamic') {
-        return { left: leftBody, right: rightBody, corrections: 0 };
-    }
-    const left = toBounds(leftBody);
-    const right = toBounds(rightBody);
-    if (!boundsOverlap(left, right) ||
-        !shapesOverlap3D(bodyPose(leftBody), bodyPose(rightBody))) {
-        return { left: leftBody, right: rightBody, corrections: 0 };
-    }
-    const pushLeft = right.minX - left.maxX;
-    const pushRight = right.maxX - left.minX;
-    const pushDown = right.minY - left.maxY;
-    const pushUp = right.maxY - left.minY;
-    const pushBack = right.minZ - left.maxZ;
-    const pushForward = right.maxZ - left.minZ;
-    const correction = [
-        { axis: 'x', delta: nearestAxisDelta(pushLeft, pushRight) },
-        { axis: 'y', delta: nearestAxisDelta(pushDown, pushUp) },
-        { axis: 'z', delta: nearestAxisDelta(pushBack, pushForward) },
-    ].sort((leftCandidate, rightCandidate) => Math.abs(leftCandidate.delta) - Math.abs(rightCandidate.delta) ||
-        compareStrings(leftCandidate.axis, rightCandidate.axis))[0];
-    const leftDynamic = leftBody.kind === 'dynamic';
-    const rightDynamic = rightBody.kind === 'dynamic';
-    const leftShare = leftDynamic && rightDynamic ? 0.5 : leftDynamic ? 1 : 0;
-    const rightShare = leftDynamic && rightDynamic ? -0.5 : rightDynamic ? -1 : 0;
-    const material = combineDeterministicPhysicsMaterial(leftBody.material ?? defaultPhysicsMaterial3D, rightBody.material ?? defaultPhysicsMaterial3D);
-    const leftResolved = applyAxisCorrection(leftBody, correction.axis, correction.delta * leftShare, material);
-    const rightResolved = applyAxisCorrection(rightBody, correction.axis, correction.delta * rightShare, material);
-    return { left: leftResolved, right: rightResolved, corrections: 1 };
-}
-function applyAxisCorrection(body, axis, delta, material) {
-    if (delta === 0) {
-        return body;
-    }
-    const quantized = quantizeDistance(delta);
-    const normalVelocity = (velocity) => Math.trunc((-velocity * material.restitution) / 10);
-    const tangentVelocity = (velocity) => Math.trunc((velocity * Math.max(0, 10 - material.friction)) / 10);
-    if (axis === 'x') {
-        return {
-            ...body,
-            x: quantizeDistance(body.x + quantized),
-            vx: normalVelocity(body.vx),
-            vy: tangentVelocity(body.vy),
-            vz: tangentVelocity(body.vz),
-        };
-    }
-    if (axis === 'y') {
-        return {
-            ...body,
-            y: quantizeDistance(body.y + quantized),
-            vx: tangentVelocity(body.vx),
-            vy: normalVelocity(body.vy),
-            vz: tangentVelocity(body.vz),
-        };
-    }
-    return {
-        ...body,
-        x: body.x,
-        y: body.y,
-        z: quantizeDistance(body.z + quantized),
-        vx: tangentVelocity(body.vx),
-        vy: tangentVelocity(body.vy),
-        vz: normalVelocity(body.vz),
-    };
-}
-function nearestAxisDelta(negativeDelta, positiveDelta) {
-    const useNegative = Number(Math.abs(negativeDelta) <= Math.abs(positiveDelta));
-    return negativeDelta * useNegative + positiveDelta * (1 - useNegative);
-}
 function solveConstraints3D(bodies, constraints) {
     const next = bodies.map((body) => ({ ...body }));
     let corrections = 0;
@@ -941,7 +1602,7 @@ function solveConstraints3D(bodies, constraints) {
             corrections += 1;
         }
     }
-    return { bodies: normalizeBodies(next), corrections };
+    return { bodies: normalizeBodies(next, 'trusted'), corrections };
 }
 function moveBodyBy(body, dx, dy, dz) {
     if (body.kind !== 'dynamic') {
@@ -992,14 +1653,109 @@ function broadphaseCandidatePairs(bodies) {
     }
     return pairs.sort((left, right) => compareStrings(left.left.id, right.left.id) || compareStrings(left.right.id, right.right.id));
 }
-function normalizeBodies(bodies) {
-    const sorted = bodies.map((body) => cloneCanonical(body)).sort((left, right) => compareStrings(left.id, right.id));
+/**
+ * Canonicalize a body list. `strict` (the user-input boundary) deep-clones and
+ * validates every body; `trusted` skips both for bodies the engine itself
+ * built — they are already validated, already canonical values (the clone
+ * round-trip preserves -0/NaN/undefined exactly, so skipping it is
+ * value-identical), and immutable by repo-wide discipline, so sharing their
+ * identity across world snapshots is safe (and lets derived-data caches keyed
+ * on body identity persist across frames).
+ */
+function normalizeBodies(bodies, mode = 'strict') {
+    const source = mode === 'strict' ? bodies.map((body) => cloneCanonical(body)) : [...bodies];
+    const sorted = source
+        .map((body) => (body.kind === 'dynamic' && body.sleeping === undefined ? { ...body, sleeping: false } : body))
+        .sort((left, right) => compareStrings(left.id, right.id));
     const duplicate = sorted.find((body, index) => index > 0 && body.id === sorted[index - 1].id);
     if (duplicate !== undefined) {
         throw new Error(`Duplicate deterministic physics body id ${duplicate.id}`);
     }
+    if (mode === 'strict') {
+        for (const body of sorted) {
+            validatePhysicsBody3D(body);
+        }
+    }
     return sorted;
 }
+function isFiniteNumber(value) {
+    return typeof value === 'number' && Number.isFinite(value);
+}
+function validatePhysicsBody3D(body) {
+    const id = body.id;
+    for (const [name, value] of [['x', body.x], ['y', body.y], ['z', body.z], ['vx', body.vx], ['vy', body.vy], ['vz', body.vz]]) {
+        if (!isFiniteNumber(value)) {
+            throw new Error(`physics3d body ${id}: ${name} must be a finite number`);
+        }
+    }
+    if (body.mass !== undefined && (!isFiniteNumber(body.mass) || body.mass <= 0)) {
+        throw new Error(`physics3d body ${id}: mass must be a positive finite number`);
+    }
+    if (body.angularVel !== undefined) {
+        for (const [name, value] of [['angularVel.x', body.angularVel.x], ['angularVel.y', body.angularVel.y], ['angularVel.z', body.angularVel.z]]) {
+            if (!isFiniteNumber(value)) {
+                throw new Error(`physics3d body ${id}: ${name} must be a finite number`);
+            }
+        }
+    }
+    for (const [name, value] of [['linearDamping', body.linearDamping], ['angularDamping', body.angularDamping], ['gravityScale', body.gravityScale]]) {
+        if (value !== undefined && !isFiniteNumber(value)) {
+            throw new Error(`physics3d body ${id}: ${name} must be a finite number`);
+        }
+    }
+    if (body.orientation !== undefined) {
+        const q = body.orientation;
+        for (const [name, value] of [['orientation.x', q.x], ['orientation.y', q.y], ['orientation.z', q.z], ['orientation.w', q.w]]) {
+            if (!isFiniteNumber(value)) {
+                throw new Error(`physics3d body ${id}: ${name} must be a finite number`);
+            }
+        }
+        if (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w === 0) {
+            throw new Error(`physics3d body ${id}: orientation quaternion must have non-zero length`);
+        }
+    }
+    validatePhysicsShape3D(id, body.shape);
+}
+function validatePhysicsShape3D(id, shape) {
+    if (shape.type === 'box') {
+        for (const [name, value] of [['halfX', shape.halfX], ['halfY', shape.halfY], ['halfZ', shape.halfZ]]) {
+            if (!isFiniteNumber(value) || value <= 0) {
+                throw new Error(`physics3d body ${id}: box ${name} must be a positive finite number`);
+            }
+        }
+        return;
+    }
+    if (shape.type === 'sphere') {
+        if (!isFiniteNumber(shape.radius) || shape.radius <= 0) {
+            throw new Error(`physics3d body ${id}: sphere radius must be a positive finite number`);
+        }
+        return;
+    }
+    if (shape.type === 'capsule') {
+        if (!isFiniteNumber(shape.radius) || shape.radius <= 0) {
+            throw new Error(`physics3d body ${id}: capsule radius must be a positive finite number`);
+        }
+        if (!isFiniteNumber(shape.halfHeight) || shape.halfHeight <= 0) {
+            throw new Error(`physics3d body ${id}: capsule halfHeight must be a positive finite number`);
+        }
+        return;
+    }
+    if (shape.type === 'compound') {
+        if (shape.children.length === 0) {
+            throw new Error(`physics3d body ${id}: compound collider must have at least one child`);
+        }
+        for (const child of shape.children) {
+            for (const [name, value] of [['offset.x', child.offset.x], ['offset.y', child.offset.y], ['offset.z', child.offset.z]]) {
+                if (!isFiniteNumber(value)) {
+                    throw new Error(`physics3d body ${id}: compound child ${name} must be a finite number`);
+                }
+            }
+            validatePhysicsShape3D(id, child.shape);
+        }
+        return;
+    }
+    // mesh: triangle-list validation happens in the geometry paths (meshTriangles).
+}
 function queryBodyAllowed(body, query) {
     if (query.layerMask !== undefined && (body.layer & query.layerMask) === 0) {
         return false;
@@ -1031,8 +1787,20 @@ function queryKindAllowsBody3D(body, query) {
     }
     return query.hitKinematics === true;
 }
+/**
+ * Per-body bounds memo. Bodies are immutable and, on trusted normalize paths,
+ * keep their identity across world snapshots while untouched (static props,
+ * sleeping bodies) — so their bounds are derived once, not once per frame.
+ */
+const bodyBoundsCache = new WeakMap();
 function toBounds(body) {
-    return shapePoseBounds(bodyPose(body));
+    const cached = bodyBoundsCache.get(body);
+    if (cached !== undefined) {
+        return cached;
+    }
+    const bounds = shapePoseBounds(bodyPose(body));
+    bodyBoundsCache.set(body, bounds);
+    return bounds;
 }
 function shapeBounds(x, y, z, shape) {
     return shapePoseBounds({ x, y, z, shape });
@@ -1043,6 +1811,18 @@ function bodyPose(body) {
 function shapePoseBounds(pose) {
     const { x, y, z, shape } = pose;
     if (shape.type === 'box') {
+        // unrotated boxes (the overwhelmingly common case) skip the 8-corner
+        // expansion; x + (-half) === x - half in IEEE, so values are identical
+        if ((pose.rx ?? 0) === 0 && (pose.ry ?? 0) === 0 && (pose.rz ?? 0) === 0) {
+            return {
+                minX: x - shape.halfX,
+                maxX: x + shape.halfX,
+                minY: y - shape.halfY,
+                maxY: y + shape.halfY,
+                minZ: z - shape.halfZ,
+                maxZ: z + shape.halfZ,
+            };
+        }
         return boundsFromPoints(boxLocalCorners(shape).map((point) => translateRotatedPoint(point, pose)));
     }
     if (shape.type === 'mesh') {
@@ -1051,6 +1831,21 @@ function shapePoseBounds(pose) {
         }
         return boundsFromPoints(shape.vertices.map((vertex) => translateRotatedPoint(vertex, pose)));
     }
+    if (shape.type === 'compound') {
+        const childBounds = shape.children.map((child) => {
+            const center = translateRotatedPoint(child.offset, pose);
+            return shapePoseBounds({
+                x: center.x,
+                y: center.y,
+                z: center.z,
+                rx: child.quarterTurns?.x,
+                ry: child.quarterTurns?.y,
+                rz: child.quarterTurns?.z,
+                shape: child.shape,
+            });
+        });
+        return unionBounds3D(childBounds);
+    }
     const radius = shape.type === 'sphere' ? shape.radius : shape.radius;
     const halfY = shape.type === 'capsule' ? shape.halfHeight + radius : radius;
     return { minX: x - radius, maxX: x + radius, minY: y - halfY, maxY: y + halfY, minZ: z - radius, maxZ: z + radius };
@@ -1156,6 +1951,12 @@ function angularGeometryProof() {
         && overlap.includes('rotated-box'));
 }
 function shapesOverlap3D(left, right) {
+    if (left.shape.type === 'compound') {
+        return compoundChildPoses(left).some((childPose) => shapesOverlap3D(childPose, right));
+    }
+    if (right.shape.type === 'compound') {
+        return compoundChildPoses(right).some((childPose) => shapesOverlap3D(left, childPose));
+    }
     if (left.shape.type === 'mesh') {
         return meshOverlapsShape(left, right);
     }
@@ -1222,7 +2023,29 @@ function meshOverlapsShape(mesh, other) {
         const bounds = shapeBounds(other.x, other.y, other.z, otherShape);
         return triangles.some((triangle) => triangleBoxOverlap(triangle, bounds));
     }
-    return triangles.some((left) => meshTriangles({ ...other, shape: otherShape }).some((right) => trianglesOverlap(left, right)));
+    if (otherShape.type === 'mesh') {
+        return triangles.some((left) => meshTriangles({ ...other, shape: otherShape }).some((right) => trianglesOverlap(left, right)));
+    }
+    // compound-vs-mesh: compound children are dynamic-only primitives; mesh colliders are static-only.
+    return compoundChildPoses(other).some((childPose) => meshOverlapsShape(mesh, childPose));
+}
+function compoundChildPoses(pose) {
+    const compound = pose.shape;
+    if (compound.type !== 'compound') {
+        return [];
+    }
+    return compound.children.map((child) => {
+        const center = translateRotatedPoint(child.offset, pose);
+        return {
+            x: center.x,
+            y: center.y,
+            z: center.z,
+            rx: (pose.rx ?? 0) + (child.quarterTurns?.x ?? 0),
+            ry: (pose.ry ?? 0) + (child.quarterTurns?.y ?? 0),
+            rz: (pose.rz ?? 0) + (child.quarterTurns?.z ?? 0),
+            shape: child.shape,
+        };
+    });
 }
 function meshTriangles(pose) {
     const shape = pose.shape;
@@ -1487,6 +2310,19 @@ function pointInBounds(point, bounds) {
         && point.y >= bounds.minY && point.y <= bounds.maxY
         && point.z >= bounds.minZ && point.z <= bounds.maxZ;
 }
+function unionBounds3D(boundsList) {
+    if (boundsList.length === 0) {
+        throw new Error('unionBounds3D requires at least one bounds');
+    }
+    return boundsList.reduce((accumulator, bounds) => ({
+        minX: Math.min(accumulator.minX, bounds.minX),
+        maxX: Math.max(accumulator.maxX, bounds.maxX),
+        minY: Math.min(accumulator.minY, bounds.minY),
+        maxY: Math.max(accumulator.maxY, bounds.maxY),
+        minZ: Math.min(accumulator.minZ, bounds.minZ),
+        maxZ: Math.max(accumulator.maxZ, bounds.maxZ),
+    }));
+}
 function subtractPoint(left, right) {
     return { x: left.x - right.x, y: left.y - right.y, z: left.z - right.z };
 }
@@ -1531,6 +2367,19 @@ function rayAabbDistance(bounds, query) {
     return distance === undefined ? undefined : quantizeDistance(distance);
 }
 function rayBodyDistance(body, query) {
+    if (body.orientation !== undefined) {
+        // transform the ray into the body's local frame (isometry preserves the hit
+        // distance for a unit-direction ray), then raycast the axis-aligned shape.
+        const localOrigin = rotatePointByQuatInverse(body.orientation, { x: query.x - body.x, y: query.y - body.y, z: query.z - body.z });
+        const localDir = rotatePointByQuatInverse(body.orientation, { x: query.dx, y: query.dy, z: query.dz });
+        const localQuery = {
+            ...query,
+            x: localOrigin.x, y: localOrigin.y, z: localOrigin.z,
+            dx: localDir.x, dy: localDir.y, dz: localDir.z,
+        };
+        const localBody = { ...body, x: 0, y: 0, z: 0, orientation: undefined, rx: undefined, ry: undefined, rz: undefined };
+        return rayBodyDistance(localBody, localQuery);
+    }
     if (body.shape.type === 'sphere') {
         return raySphereDistance(query.x, query.y, query.z, query.dx, query.dy, query.dz, body.x, body.y, body.z, body.shape.radius, query.maxDistance);
     }
@@ -1543,7 +2392,22 @@ function rayBodyDistance(body, query) {
     return rayAabbDistance(toBounds(body), query);
 }
 function shapeCastBodyDistance(body, query) {
-    if (overlapDeterministicPhysics3D(createDeterministicPhysicsWorld3D([body]), query).includes(body.id)) {
+    if (body.orientation !== undefined) {
+        // Transform the cast into the body's local frame (isometry preserves the hit
+        // distance for a unit-direction cast), then cast against the axis-aligned shape.
+        const localOrigin = rotatePointByQuatInverse(body.orientation, { x: query.x - body.x, y: query.y - body.y, z: query.z - body.z });
+        const localDir = rotatePointByQuatInverse(body.orientation, { x: query.dx, y: query.dy, z: query.dz });
+        const localQuery = {
+            ...query,
+            x: localOrigin.x, y: localOrigin.y, z: localOrigin.z,
+            dx: localDir.x, dy: localDir.y, dz: localDir.z,
+        };
+        const localBody = { ...body, x: 0, y: 0, z: 0, orientation: undefined, rx: undefined, ry: undefined, rz: undefined };
+        return shapeCastBodyDistance(localBody, localQuery);
+    }
+    // exact initial-overlap check without constructing a throwaway single-body
+    // world (normalizeBodies clones + validates per call — a measured hot spot)
+    if (bodyOverlapsQueryShape(body, query.shape, query.x, query.y, query.z, shapeBounds(query.x, query.y, query.z, query.shape))) {
         return 0;
     }
     if (query.shape.type === 'sphere') {
@@ -1790,41 +2654,6 @@ function expandBoundsByBounds(bounds, extents) {
         maxZ: bounds.maxZ - extents.minZ,
     };
 }
-function quantizeDistance(distance) {
-    // Pure integer rounding, not Number.prototype.toFixed — see physics2d
-    // quantizeDistance (cross-engine half-way divergence on checksummed state).
-    const quantized = Math.round(distance * 1e6) / 1e6;
-    return quantized === 0 ? 0 : quantized;
-}
-function quantizeAngle(value) {
-    const wrapped = value - Math.floor(value);
-    const quantizedAngle = Math.round(wrapped * 1e6) / 1e6;
-    return quantizedAngle === 0 ? 0 : quantizedAngle;
-}
-function deterministicSqrt(value) {
-    if (value <= 0) {
-        return 0;
-    }
-    // Seed the Newton iteration from the argument's magnitude (2^ceil(bits/2)):
-    // starting from `value` itself needs ~0.5*log2(value) halving steps before
-    // the quadratic regime, so 8 iterations only converged for tiny inputs —
-    // at realistic fixed-point magnitudes (dx^2+dy^2 ~ 1e10) the old seed
-    // returned results off by orders of magnitude. Every operation here is
-    // exact integer/IEEE-deterministic arithmetic, so all peers still agree.
-    let estimate = 1;
-    let magnitude = value;
-    while (magnitude >= 4) {
-        magnitude /= 4;
-        estimate *= 2;
-    }
-    if (magnitude > 1) {
-        estimate *= 2;
-    }
-    for (let iteration = 0; iteration < 8; iteration += 1) {
-        estimate = (estimate + value / estimate) / 2;
-    }
-    return quantizeDistance(estimate);
-}
 function countCandidatePairs(bodies) {
     return broadphaseCandidatePairs(bodies).length;
 }