@driftengine/physics 3.61.0

package/dist/solver.js

@@ -0,0 +1,601 @@
+import { BODY_DYNAMIC } from './bodies.js';
+/**
+ * Contact constraints, and the substepped soft solve over them.
+ *
+ * **Soft rather than Baumgarte, and the difference is a property rather than a preference.** Every
+ * constraint carries a *hertz* and a damping ratio, from which the bias rate, mass scale and
+ * impulse scale are derived using the substep length. Stating stiffness as a frequency is what
+ * makes the substep count a quality dial: turn it up and nothing moves differently. A Baumgarte
+ * bias is a fraction of penetration per step, so it means something different at every step length,
+ * and the mitigations for that are all tuning constants — which `AGENTS.md` says never to assert.
+ *
+ * **Nothing here recomputes a manifold.** Anchors are stored in body-local frame at prepare time
+ * along with a separation offset, so the current separation is exact at any point inside the tick
+ * without re-running narrowphase per substep. `s = sBase + dot((pB + rB') − (pA + rA'), n)` is that
+ * identity, and it holds because `sBase` is defined to make it hold at prepare.
+ *
+ * **Every entry point takes an order array rather than a contiguous range.** Islands are disjoint
+ * by construction, so they can be solved in any order or simultaneously — but a constraint list
+ * built in pair order is not grouped by island. Indirecting through an order array groups them
+ * without permuting eleven parallel arrays, and it is what lets one `solveIsland` serve both
+ * executors.
+ *
+ * **Friction is solved before the normal**, against the normal impulse accumulated so far. Solving
+ * it after means the first iteration has no normal impulse to bound against and applies none, so a
+ * resting body slides for one iteration every substep.
+ */
+/** Contacts start with this much slack allowed before the bias fights them, in metres. */
+export const LINEAR_SLOP = 0.005;
+/** How fast the bias may push overlap out, in metres per second. */
+export const MAX_BIAS_VELOCITY = 4;
+/** Below this approach speed a bounce is not worth having, and jitters if it is applied. */
+export const RESTITUTION_THRESHOLD = 1;
+export class ContactConstraints {
+    /** One entry per manifold. */
+    count = 0;
+    bodyA;
+    bodyB;
+    nx;
+    ny;
+    nz;
+    t1x;
+    t1y;
+    t1z;
+    t2x;
+    t2y;
+    t2z;
+    friction;
+    restitution;
+    pointStart;
+    pointCount;
+    /** One entry per contact point. */
+    points = 0;
+    localA;
+    localB;
+    sBase;
+    normalMass;
+    tangent1Mass;
+    tangent2Mass;
+    normalImpulse;
+    tangent1Impulse;
+    tangent2Impulse;
+    /** Approach speed when the constraint was prepared, which is what restitution answers to. */
+    approach;
+    featureId;
+    /**
+     * Whether each side's anchor sits on a curved surface, one byte per point.
+     *
+     * A curved anchor is held in world frame rather than in the body's, for the reason `Manifold`
+     * states at length: the point where a ball touches the floor does not turn with the ball, and an
+     * anchor that turns anyway climbs its side and reports a gap that is not there. Per point rather
+     * than per manifold so `worldAnchors` needs only the point index, which is all its callers have.
+     */
+    curvedA;
+    curvedB;
+    cap;
+    pcap;
+    constructor(capacity = 128) {
+        this.cap = capacity;
+        this.pcap = capacity * 4;
+        this.bodyA = new Int32Array(this.cap);
+        this.bodyB = new Int32Array(this.cap);
+        this.nx = new Float32Array(this.cap);
+        this.ny = new Float32Array(this.cap);
+        this.nz = new Float32Array(this.cap);
+        this.t1x = new Float32Array(this.cap);
+        this.t1y = new Float32Array(this.cap);
+        this.t1z = new Float32Array(this.cap);
+        this.t2x = new Float32Array(this.cap);
+        this.t2y = new Float32Array(this.cap);
+        this.t2z = new Float32Array(this.cap);
+        this.friction = new Float32Array(this.cap);
+        this.restitution = new Float32Array(this.cap);
+        this.pointStart = new Int32Array(this.cap);
+        this.pointCount = new Int32Array(this.cap);
+        this.localA = new Float32Array(this.pcap * 3);
+        this.localB = new Float32Array(this.pcap * 3);
+        this.sBase = new Float32Array(this.pcap);
+        this.normalMass = new Float32Array(this.pcap);
+        this.tangent1Mass = new Float32Array(this.pcap);
+        this.tangent2Mass = new Float32Array(this.pcap);
+        this.normalImpulse = new Float32Array(this.pcap);
+        this.tangent1Impulse = new Float32Array(this.pcap);
+        this.tangent2Impulse = new Float32Array(this.pcap);
+        this.approach = new Float32Array(this.pcap);
+        this.featureId = new Int32Array(this.pcap);
+        this.curvedA = new Uint8Array(this.pcap);
+        this.curvedB = new Uint8Array(this.pcap);
+    }
+    clear() {
+        this.count = 0;
+        this.points = 0;
+    }
+    reserve(pointsNeeded) {
+        if (this.count === this.cap)
+            this.growConstraints();
+        while (this.points + pointsNeeded > this.pcap)
+            this.growPoints();
+    }
+    growConstraints() {
+        this.cap *= 2;
+        const g = (a) => {
+            const out = new a.constructor(this.cap);
+            out.set(a);
+            return out;
+        };
+        this.bodyA = g(this.bodyA);
+        this.bodyB = g(this.bodyB);
+        this.nx = g(this.nx);
+        this.ny = g(this.ny);
+        this.nz = g(this.nz);
+        this.t1x = g(this.t1x);
+        this.t1y = g(this.t1y);
+        this.t1z = g(this.t1z);
+        this.t2x = g(this.t2x);
+        this.t2y = g(this.t2y);
+        this.t2z = g(this.t2z);
+        this.friction = g(this.friction);
+        this.restitution = g(this.restitution);
+        this.pointStart = g(this.pointStart);
+        this.pointCount = g(this.pointCount);
+    }
+    growPoints() {
+        this.pcap *= 2;
+        const g = (a, stride = 1) => {
+            const out = new a.constructor(this.pcap * stride);
+            out.set(a);
+            return out;
+        };
+        this.localA = g(this.localA, 3);
+        this.localB = g(this.localB, 3);
+        this.sBase = g(this.sBase);
+        this.normalMass = g(this.normalMass);
+        this.tangent1Mass = g(this.tangent1Mass);
+        this.tangent2Mass = g(this.tangent2Mass);
+        this.normalImpulse = g(this.normalImpulse);
+        this.tangent1Impulse = g(this.tangent1Impulse);
+        this.tangent2Impulse = g(this.tangent2Impulse);
+        this.approach = g(this.approach);
+        this.featureId = g(this.featureId);
+        this.curvedA = g(this.curvedA);
+        this.curvedB = g(this.curvedB);
+    }
+}
+const invIA = new Float32Array(6);
+const invIB = new Float32Array(6);
+/**
+ * Turn one manifold into a constraint, with its effective masses and its local anchors.
+ *
+ * `warm` supplies the previous tick's impulses by feature id, or −1 where there is none. Warm
+ * starting is what lets a stack converge in a handful of iterations rather than a hundred.
+ */
+export function prepareContact(bodies, a, b, m, c, warmNormal, warmT1, warmT2) {
+    c.reserve(m.count);
+    const k = c.count++;
+    c.bodyA[k] = a;
+    c.bodyB[k] = b;
+    c.nx[k] = m.nx;
+    c.ny[k] = m.ny;
+    c.nz[k] = m.nz;
+    // An orthonormal basis around the normal, built from comparisons and one square root.
+    let t1x;
+    let t1y;
+    let t1z;
+    if (Math.abs(m.nx) >= 0.57735) {
+        t1x = m.ny;
+        t1y = -m.nx;
+        t1z = 0;
+    }
+    else {
+        t1x = 0;
+        t1y = m.nz;
+        t1z = -m.ny;
+    }
+    const tl = Math.sqrt(t1x * t1x + t1y * t1y + t1z * t1z) || 1;
+    t1x /= tl;
+    t1y /= tl;
+    t1z /= tl;
+    c.t1x[k] = t1x;
+    c.t1y[k] = t1y;
+    c.t1z[k] = t1z;
+    c.t2x[k] = m.ny * t1z - m.nz * t1y;
+    c.t2y[k] = m.nz * t1x - m.nx * t1z;
+    c.t2z[k] = m.nx * t1y - m.ny * t1x;
+    // A pair's friction and bounce are the geometric and the larger mean of the two bodies'.
+    c.friction[k] = Math.sqrt((bodies.friction[a] ?? 0) * (bodies.friction[b] ?? 0));
+    const ra = bodies.restitution[a] ?? 0;
+    const rb = bodies.restitution[b] ?? 0;
+    c.restitution[k] = ra > rb ? ra : rb;
+    bodies.worldInverseInertia(a, invIA);
+    bodies.worldInverseInertia(b, invIB);
+    const imA = bodies.invMass[a] ?? 0;
+    const imB = bodies.invMass[b] ?? 0;
+    c.pointStart[k] = c.points;
+    c.pointCount[k] = m.count;
+    for (let i = 0; i < m.count; i++) {
+        const p = c.points++;
+        const px = m.points[i * 3] ?? 0;
+        const py = m.points[i * 3 + 1] ?? 0;
+        const pz = m.points[i * 3 + 2] ?? 0;
+        const rax = px - (bodies.posX[a] ?? 0);
+        const ray = py - (bodies.posY[a] ?? 0);
+        const raz = pz - (bodies.posZ[a] ?? 0);
+        const rbx = px - (bodies.posX[b] ?? 0);
+        const rby = py - (bodies.posY[b] ?? 0);
+        const rbz = pz - (bodies.posZ[b] ?? 0);
+        /*
+         * A flat anchor is stored in its body's frame and turned forward with it; a curved one is
+         * stored in world frame and left alone. Both are read back by `worldAnchors`, which reads the
+         * same flag — so the pair of them is one decision written in two places and nowhere else.
+         */
+        c.curvedA[p] = m.curvedA ? 1 : 0;
+        c.curvedB[p] = m.curvedB ? 1 : 0;
+        if (m.curvedA) {
+            c.localA[p * 3] = rax;
+            c.localA[p * 3 + 1] = ray;
+            c.localA[p * 3 + 2] = raz;
+        }
+        else {
+            unrotate(bodies, a, rax, ray, raz, c.localA, p * 3);
+        }
+        if (m.curvedB) {
+            c.localB[p * 3] = rbx;
+            c.localB[p * 3 + 1] = rby;
+            c.localB[p * 3 + 2] = rbz;
+        }
+        else {
+            unrotate(bodies, b, rbx, rby, rbz, c.localB, p * 3);
+        }
+        /*
+         * The offset that makes `s = sBase + dot((pB + rB') − (pA + rA'), n)` reproduce this tick's
+         * separation exactly, and every later substep's without a second narrowphase.
+         */
+        const nx = m.nx;
+        const ny = m.ny;
+        const nz = m.nz;
+        const now = ((bodies.posX[b] ?? 0) + rbx - (bodies.posX[a] ?? 0) - rax) * nx +
+            ((bodies.posY[b] ?? 0) + rby - (bodies.posY[a] ?? 0) - ray) * ny +
+            ((bodies.posZ[b] ?? 0) + rbz - (bodies.posZ[a] ?? 0) - raz) * nz;
+        c.sBase[p] = (m.separations[i] ?? 0) - now;
+        c.normalMass[p] = effectiveMass(imA, imB, rax, ray, raz, rbx, rby, rbz, nx, ny, nz);
+        c.tangent1Mass[p] = effectiveMass(imA, imB, rax, ray, raz, rbx, rby, rbz, t1x, t1y, t1z);
+        c.tangent2Mass[p] = effectiveMass(imA, imB, rax, ray, raz, rbx, rby, rbz, c.t2x[k] ?? 0, c.t2y[k] ?? 0, c.t2z[k] ?? 0);
+        relativeVelocity(bodies, a, b, rax, ray, raz, rbx, rby, rbz);
+        c.approach[p] = REL[0] * nx + REL[1] * ny + REL[2] * nz;
+        c.featureId[p] = m.featureIds[i] ?? 0;
+        c.normalImpulse[p] = warmNormal ? (warmNormal[i] ?? 0) : 0;
+        c.tangent1Impulse[p] = warmT1 ? (warmT1[i] ?? 0) : 0;
+        c.tangent2Impulse[p] = warmT2 ? (warmT2[i] ?? 0) : 0;
+    }
+}
+/** Apply the impulses carried over from the previous tick, before the first iteration. */
+export function warmStart(bodies, c, order, from, to) {
+    for (let oi = from; oi < to; oi++) {
+        const k = order[oi] ?? 0;
+        const a = c.bodyA[k] ?? 0;
+        const b = c.bodyB[k] ?? 0;
+        bodies.worldInverseInertia(a, invIA);
+        bodies.worldInverseInertia(b, invIB);
+        const start = c.pointStart[k] ?? 0;
+        const end = start + (c.pointCount[k] ?? 0);
+        for (let p = start; p < end; p++) {
+            worldAnchors(bodies, a, b, c, p);
+            const px = (c.nx[k] ?? 0) * (c.normalImpulse[p] ?? 0) +
+                (c.t1x[k] ?? 0) * (c.tangent1Impulse[p] ?? 0) +
+                (c.t2x[k] ?? 0) * (c.tangent2Impulse[p] ?? 0);
+            const py = (c.ny[k] ?? 0) * (c.normalImpulse[p] ?? 0) +
+                (c.t1y[k] ?? 0) * (c.tangent1Impulse[p] ?? 0) +
+                (c.t2y[k] ?? 0) * (c.tangent2Impulse[p] ?? 0);
+            const pz = (c.nz[k] ?? 0) * (c.normalImpulse[p] ?? 0) +
+                (c.t1z[k] ?? 0) * (c.tangent1Impulse[p] ?? 0) +
+                (c.t2z[k] ?? 0) * (c.tangent2Impulse[p] ?? 0);
+            applyImpulse(bodies, a, b, ANCHOR, px, py, pz);
+        }
+    }
+}
+export function solveContacts(bodies, c, order, from, to, h, useBias, hertz, damping, 
+/* Defaulted so a consumer calling this directly — it is exported — keeps its old numbers. */
+friction = 'box') {
+    // The soft-constraint triple, from a frequency rather than from a per-step fraction.
+    const omega = 2 * Math.PI * hertz;
+    const a1 = 2 * damping + h * omega;
+    const a2 = h * omega * a1;
+    const a3 = 1 / (1 + a2);
+    const biasRate = omega / a1;
+    const massScale = a2 * a3;
+    const impulseScale = a3;
+    const invH = h > 0 ? 1 / h : 0;
+    for (let oi = from; oi < to; oi++) {
+        const k = order[oi] ?? 0;
+        const a = c.bodyA[k] ?? 0;
+        const b = c.bodyB[k] ?? 0;
+        bodies.worldInverseInertia(a, invIA);
+        bodies.worldInverseInertia(b, invIB);
+        const nx = c.nx[k] ?? 0;
+        const ny = c.ny[k] ?? 0;
+        const nz = c.nz[k] ?? 0;
+        const mu = c.friction[k] ?? 0;
+        const start = c.pointStart[k] ?? 0;
+        const end = start + (c.pointCount[k] ?? 0);
+        /*
+         * Friction first, bounded by the normal impulse standing from the previous iteration. Solving
+         * it after the normal costs the first iteration of every substep its friction entirely, because
+         * there is no accumulated normal impulse to bound against yet.
+         */
+        for (let p = start; p < end; p++) {
+            worldAnchors(bodies, a, b, c, p);
+            relativeVelocity(bodies, a, b, ANCHOR[0], ANCHOR[1], ANCHOR[2], ANCHOR[3], ANCHOR[4], ANCHOR[5]);
+            const maxF = mu * (c.normalImpulse[p] ?? 0);
+            if (friction === 'elliptical') {
+                solveFrictionCone(bodies, a, b, k, p, c, maxF);
+            }
+            else {
+                solveTangent(bodies, a, b, p, c.t1x[k] ?? 0, c.t1y[k] ?? 0, c.t1z[k] ?? 0, c.tangent1Mass, c.tangent1Impulse, maxF);
+                relativeVelocity(bodies, a, b, ANCHOR[0], ANCHOR[1], ANCHOR[2], ANCHOR[3], ANCHOR[4], ANCHOR[5]);
+                solveTangent(bodies, a, b, p, c.t2x[k] ?? 0, c.t2y[k] ?? 0, c.t2z[k] ?? 0, c.tangent2Mass, c.tangent2Impulse, maxF);
+            }
+        }
+        for (let p = start; p < end; p++) {
+            worldAnchors(bodies, a, b, c, p);
+            const s = (c.sBase[p] ?? 0) +
+                ((bodies.posX[b] ?? 0) + ANCHOR[3] - (bodies.posX[a] ?? 0) - ANCHOR[0]) * nx +
+                ((bodies.posY[b] ?? 0) + ANCHOR[4] - (bodies.posY[a] ?? 0) - ANCHOR[1]) * ny +
+                ((bodies.posZ[b] ?? 0) + ANCHOR[5] - (bodies.posZ[a] ?? 0) - ANCHOR[2]) * nz;
+            relativeVelocity(bodies, a, b, ANCHOR[0], ANCHOR[1], ANCHOR[2], ANCHOR[3], ANCHOR[4], ANCHOR[5]);
+            const vn = REL[0] * nx + REL[1] * ny + REL[2] * nz;
+            let bias = 0;
+            let ms = 1;
+            let is = 0;
+            if (s > 0) {
+                // Speculative: close exactly the gap this substep and no more.
+                bias = s * invH;
+            }
+            else if (useBias) {
+                const want = biasRate * (s + LINEAR_SLOP);
+                bias = want < -MAX_BIAS_VELOCITY ? -MAX_BIAS_VELOCITY : want > 0 ? 0 : want;
+                ms = massScale;
+                is = impulseScale;
+            }
+            const old = c.normalImpulse[p] ?? 0;
+            let impulse = -(c.normalMass[p] ?? 0) * ms * (vn + bias) - is * old;
+            const next = old + impulse > 0 ? old + impulse : 0;
+            impulse = next - old;
+            c.normalImpulse[p] = next;
+            applyImpulse(bodies, a, b, ANCHOR, nx * impulse, ny * impulse, nz * impulse);
+        }
+    }
+}
+/**
+ * One pass after every substep, giving back the bounce the solve suppressed.
+ *
+ * Applied once against the approach speed captured at prepare, so a drop returns to the same height
+ * whatever the substep count. A bounce driven from the *current* velocity instead compounds with
+ * every substep and turns the coefficient into a function of the dial.
+ */
+export function applyRestitution(bodies, c, order, from, to) {
+    for (let oi = from; oi < to; oi++) {
+        const k = order[oi] ?? 0;
+        const e = c.restitution[k] ?? 0;
+        if (e === 0)
+            continue;
+        const a = c.bodyA[k] ?? 0;
+        const b = c.bodyB[k] ?? 0;
+        bodies.worldInverseInertia(a, invIA);
+        bodies.worldInverseInertia(b, invIB);
+        const nx = c.nx[k] ?? 0;
+        const ny = c.ny[k] ?? 0;
+        const nz = c.nz[k] ?? 0;
+        const start = c.pointStart[k] ?? 0;
+        const end = start + (c.pointCount[k] ?? 0);
+        for (let p = start; p < end; p++) {
+            const approach = c.approach[p] ?? 0;
+            if (approach > -RESTITUTION_THRESHOLD || (c.normalImpulse[p] ?? 0) === 0)
+                continue;
+            worldAnchors(bodies, a, b, c, p);
+            relativeVelocity(bodies, a, b, ANCHOR[0], ANCHOR[1], ANCHOR[2], ANCHOR[3], ANCHOR[4], ANCHOR[5]);
+            const vn = REL[0] * nx + REL[1] * ny + REL[2] * nz;
+            const old = c.normalImpulse[p] ?? 0;
+            let impulse = -(c.normalMass[p] ?? 0) * (vn + e * approach);
+            const next = old + impulse > 0 ? old + impulse : 0;
+            impulse = next - old;
+            c.normalImpulse[p] = next;
+            applyImpulse(bodies, a, b, ANCHOR, nx * impulse, ny * impulse, nz * impulse);
+        }
+    }
+}
+/* ---------- shared scratch, so nothing allocates per contact ---------- */
+const ANCHOR = new Float64Array(6);
+const REL = new Float64Array(3);
+/**
+ * One friction axis, solved against the anchors and relative velocity already in scratch.
+ *
+ * **It takes no `ContactConstraints`, and it used to.** The parameter was dead: everything this
+ * needs is either in the module-scope `REL` and `ANCHOR` that the caller filled, or in the two
+ * arrays it is handed. A parameter nothing reads is a claim about what a function depends on, and
+ * this one said it depended on the whole constraint block.
+ *
+ * Found by a consumer's typecheck rather than by this repository's: one of them runs
+ * `noUnusedParameters` over engine source where the engine's own config does not, on the grounds
+ * that the flag costs the engine nothing and catches a real class of mistake. This is it being
+ * right, and it arrived as a red CI in a repository that had changed nothing.
+ */
+function solveTangent(bodies, a, b, p, tx, ty, tz, massArray, impulseArray, maxF) {
+    const vt = REL[0] * tx + REL[1] * ty + REL[2] * tz;
+    const old = impulseArray[p] ?? 0;
+    let impulse = -(massArray[p] ?? 0) * vt;
+    let next = old + impulse;
+    if (next > maxF)
+        next = maxF;
+    else if (next < -maxF)
+        next = -maxF;
+    impulse = next - old;
+    impulseArray[p] = next;
+    applyImpulse(bodies, a, b, ANCHOR, tx * impulse, ty * impulse, tz * impulse);
+}
+/**
+ * Both friction axes at once, with the pair projected onto a disc of radius `maxF`.
+ *
+ * **Two differences from the pair of `solveTangent` calls it replaces, and only one of them is the
+ * clamp.** The clamp is the point: the accumulated `(P1, P2)` is scaled onto the circle rather than
+ * each component being clipped to a square, so the total friction impulse is `mu * N` whatever the
+ * heading. The other is that **both rows read one relative velocity** — the box path re-reads it
+ * between the two axes, which makes the second row see the first row's push, and there is no
+ * meaningful way to keep that while solving them jointly. That is why this is not a change a stored
+ * replay survives, and why it is behind an option.
+ *
+ * The impulse is applied once, as the sum of the two deltas, rather than twice: two applications of
+ * a jointly-solved pair would each disturb the other's velocity between them, which is the sequential
+ * behaviour this exists to leave behind.
+ */
+function solveFrictionCone(bodies, a, b, k, p, c, maxF) {
+    const t1x = c.t1x[k] ?? 0;
+    const t1y = c.t1y[k] ?? 0;
+    const t1z = c.t1z[k] ?? 0;
+    const t2x = c.t2x[k] ?? 0;
+    const t2y = c.t2y[k] ?? 0;
+    const t2z = c.t2z[k] ?? 0;
+    const vt1 = REL[0] * t1x + REL[1] * t1y + REL[2] * t1z;
+    const vt2 = REL[0] * t2x + REL[1] * t2y + REL[2] * t2z;
+    const old1 = c.tangent1Impulse[p] ?? 0;
+    const old2 = c.tangent2Impulse[p] ?? 0;
+    let next1 = old1 - (c.tangent1Mass[p] ?? 0) * vt1;
+    let next2 = old2 - (c.tangent2Mass[p] ?? 0) * vt2;
+    const length = Math.sqrt(next1 * next1 + next2 * next2);
+    if (length > maxF) {
+        /* `length > maxF` implies `length > 0` for a non-negative bound, so the divide is safe: a zero
+           normal impulse gives `maxF = 0` and a zero length fails the compare. */
+        const scale = maxF / length;
+        next1 *= scale;
+        next2 *= scale;
+    }
+    const delta1 = next1 - old1;
+    const delta2 = next2 - old2;
+    c.tangent1Impulse[p] = next1;
+    c.tangent2Impulse[p] = next2;
+    applyImpulse(bodies, a, b, ANCHOR, t1x * delta1 + t2x * delta2, t1y * delta1 + t2y * delta2, t1z * delta1 + t2z * delta2);
+}
+/** `1 / (mA + mB + (rA×n)ᵀ IA (rA×n) + (rB×n)ᵀ IB (rB×n))`, or zero where nothing can move. */
+function effectiveMass(imA, imB, rax, ray, raz, rbx, rby, rbz, nx, ny, nz) {
+    const ax = ray * nz - raz * ny;
+    const ay = raz * nx - rax * nz;
+    const az = rax * ny - ray * nx;
+    const bx = rby * nz - rbz * ny;
+    const by = rbz * nx - rbx * nz;
+    const bz = rbx * ny - rby * nx;
+    const k = imA + imB + quadratic(invIA, ax, ay, az) + quadratic(invIB, bx, by, bz);
+    return k > 0 ? 1 / k : 0;
+}
+/** `vᵀ M v` for a symmetric M stored as six terms. */
+function quadratic(m, x, y, z) {
+    return ((m[0] ?? 0) * x * x +
+        (m[1] ?? 0) * y * y +
+        (m[2] ?? 0) * z * z +
+        2 * ((m[3] ?? 0) * x * y + (m[4] ?? 0) * x * z + (m[5] ?? 0) * y * z));
+}
+/** Rotate the stored local anchors into world, into `ANCHOR[0..5]`. */
+function worldAnchors(bodies, a, b, c, p) {
+    if (c.curvedA[p]) {
+        ANCHOR[0] = c.localA[p * 3] ?? 0;
+        ANCHOR[1] = c.localA[p * 3 + 1] ?? 0;
+        ANCHOR[2] = c.localA[p * 3 + 2] ?? 0;
+    }
+    else {
+        rotate(bodies, a, c.localA[p * 3] ?? 0, c.localA[p * 3 + 1] ?? 0, c.localA[p * 3 + 2] ?? 0, ANCHOR, 0);
+    }
+    if (c.curvedB[p]) {
+        ANCHOR[3] = c.localB[p * 3] ?? 0;
+        ANCHOR[4] = c.localB[p * 3 + 1] ?? 0;
+        ANCHOR[5] = c.localB[p * 3 + 2] ?? 0;
+    }
+    else {
+        rotate(bodies, b, c.localB[p * 3] ?? 0, c.localB[p * 3 + 1] ?? 0, c.localB[p * 3 + 2] ?? 0, ANCHOR, 3);
+    }
+}
+/** `(vB + wB × rB) − (vA + wA × rA)`, into `REL`. */
+function relativeVelocity(bodies, a, b, rax, ray, raz, rbx, rby, rbz) {
+    const wax = bodies.angX[a] ?? 0;
+    const way = bodies.angY[a] ?? 0;
+    const waz = bodies.angZ[a] ?? 0;
+    const wbx = bodies.angX[b] ?? 0;
+    const wby = bodies.angY[b] ?? 0;
+    const wbz = bodies.angZ[b] ?? 0;
+    REL[0] =
+        (bodies.velX[b] ?? 0) +
+            (wby * rbz - wbz * rby) -
+            (bodies.velX[a] ?? 0) -
+            (way * raz - waz * ray);
+    REL[1] =
+        (bodies.velY[b] ?? 0) +
+            (wbz * rbx - wbx * rbz) -
+            (bodies.velY[a] ?? 0) -
+            (waz * rax - wax * raz);
+    REL[2] =
+        (bodies.velZ[b] ?? 0) +
+            (wbx * rby - wby * rbx) -
+            (bodies.velZ[a] ?? 0) -
+            (wax * ray - way * rax);
+}
+/** Apply `+P` to B at `rB` and `−P` to A at `rA`. */
+function applyImpulse(bodies, a, b, anchors, px, py, pz) {
+    const imA = bodies.invMass[a] ?? 0;
+    if (imA > 0) {
+        bodies.velX[a] = (bodies.velX[a] ?? 0) - px * imA;
+        bodies.velY[a] = (bodies.velY[a] ?? 0) - py * imA;
+        bodies.velZ[a] = (bodies.velZ[a] ?? 0) - pz * imA;
+        const tx = anchors[1] * pz - anchors[2] * py;
+        const ty = anchors[2] * px - anchors[0] * pz;
+        const tz = anchors[0] * py - anchors[1] * px;
+        bodies.angX[a] = (bodies.angX[a] ?? 0) - multiply(invIA, tx, ty, tz, 0);
+        bodies.angY[a] = (bodies.angY[a] ?? 0) - multiply(invIA, tx, ty, tz, 1);
+        bodies.angZ[a] = (bodies.angZ[a] ?? 0) - multiply(invIA, tx, ty, tz, 2);
+    }
+    const imB = bodies.invMass[b] ?? 0;
+    if (imB > 0) {
+        bodies.velX[b] = (bodies.velX[b] ?? 0) + px * imB;
+        bodies.velY[b] = (bodies.velY[b] ?? 0) + py * imB;
+        bodies.velZ[b] = (bodies.velZ[b] ?? 0) + pz * imB;
+        const tx = anchors[4] * pz - anchors[5] * py;
+        const ty = anchors[5] * px - anchors[3] * pz;
+        const tz = anchors[3] * py - anchors[4] * px;
+        bodies.angX[b] = (bodies.angX[b] ?? 0) + multiply(invIB, tx, ty, tz, 0);
+        bodies.angY[b] = (bodies.angY[b] ?? 0) + multiply(invIB, tx, ty, tz, 1);
+        bodies.angZ[b] = (bodies.angZ[b] ?? 0) + multiply(invIB, tx, ty, tz, 2);
+    }
+}
+/** Row `row` of `M v`, for a symmetric M stored as six terms. */
+function multiply(m, x, y, z, row) {
+    if (row === 0)
+        return (m[0] ?? 0) * x + (m[3] ?? 0) * y + (m[4] ?? 0) * z;
+    if (row === 1)
+        return (m[3] ?? 0) * x + (m[1] ?? 0) * y + (m[5] ?? 0) * z;
+    return (m[4] ?? 0) * x + (m[5] ?? 0) * y + (m[2] ?? 0) * z;
+}
+function rotate(bodies, i, x, y, z, out, at) {
+    const qx = bodies.rotX[i] ?? 0;
+    const qy = bodies.rotY[i] ?? 0;
+    const qz = bodies.rotZ[i] ?? 0;
+    const qw = bodies.rotW[i] ?? 1;
+    // v + 2w(q × v) + 2(q × (q × v)), the standard quaternion rotation without a matrix.
+    const tx = 2 * (qy * z - qz * y);
+    const ty = 2 * (qz * x - qx * z);
+    const tz = 2 * (qx * y - qy * x);
+    out[at] = x + qw * tx + (qy * tz - qz * ty);
+    out[at + 1] = y + qw * ty + (qz * tx - qx * tz);
+    out[at + 2] = z + qw * tz + (qx * ty - qy * tx);
+}
+function unrotate(bodies, i, x, y, z, out, at) {
+    const qx = -(bodies.rotX[i] ?? 0);
+    const qy = -(bodies.rotY[i] ?? 0);
+    const qz = -(bodies.rotZ[i] ?? 0);
+    const qw = bodies.rotW[i] ?? 1;
+    const tx = 2 * (qy * z - qz * y);
+    const ty = 2 * (qz * x - qx * z);
+    const tz = 2 * (qx * y - qy * x);
+    out[at] = x + qw * tx + (qy * tz - qz * ty);
+    out[at + 1] = y + qw * ty + (qz * tx - qx * tz);
+    out[at + 2] = z + qw * tz + (qx * ty - qy * tx);
+}
+/** Whether a body is one the solver may move. */
+export function isDynamicBody(bodies, i) {
+    return bodies.type[i] === BODY_DYNAMIC;
+}