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

package/dist/cjs/physics3d-joints.js

@@ -0,0 +1,637 @@
+"use strict";
+/**
+ * Impulse-level joint constraints for the deterministic rigid-body solver:
+ * ball (point-to-point), hinge (axis + limits + motor), weld, distance (rope),
+ * and grab (world-target point motor — the gravity-gun/carry primitive). Each
+ * joint is solved as accumulated impulses inside the solver's iteration loop
+ * via the `SolverJointSolve` interface; hinge angle and warm-start impulses live
+ * in the canonical joint cache.
+ *
+ * Angles (hinge `minAngle`/`maxAngle`/`motorSpeed`) are in radians / radians per
+ * tick — natural units for the radian-valued angular velocity the solver
+ * integrates.
+ */
+Object.defineProperty(exports, "__esModule", { value: true });
+exports.createSolverJoints = createSolverJoints;
+const physics3d_shared_js_1 = require("./physics3d-shared.js");
+const JOINT_BAUMGARTE = 0.2;
+function requireIndex(id, jointId, indexById) {
+    const index = indexById.get(id);
+    if (index === undefined) {
+        throw new Error(`physics3d joint ${jointId}: references unknown body ${id}`);
+    }
+    return index;
+}
+function assertFiniteVec(v, label, jointId) {
+    if (!Number.isFinite(v.x) || !Number.isFinite(v.y) || !Number.isFinite(v.z)) {
+        throw new Error(`physics3d joint ${jointId}: ${label} must be finite`);
+    }
+}
+/** Effective-mass matrix for a point constraint between bodies a and b. */
+function pointConstraintK(ctx, aIndex, bIndex, rA, rB) {
+    const imA = ctx.invMass[aIndex];
+    const imB = ctx.invMass[bIndex];
+    const linear = (0, physics3d_shared_js_1.mat3FromDiag)({ x: imA.x + imB.x, y: imA.y + imB.y, z: imA.z + imB.z });
+    const skewA = (0, physics3d_shared_js_1.skewMat3)(rA);
+    const skewB = (0, physics3d_shared_js_1.skewMat3)(rB);
+    const angA = (0, physics3d_shared_js_1.mat3Multiply)((0, physics3d_shared_js_1.mat3Multiply)(skewA, ctx.invInertia[aIndex]), skewA);
+    const angB = (0, physics3d_shared_js_1.mat3Multiply)((0, physics3d_shared_js_1.mat3Multiply)(skewB, ctx.invInertia[bIndex]), skewB);
+    return (0, physics3d_shared_js_1.mat3Sub)((0, physics3d_shared_js_1.mat3Sub)(linear, angA), angB);
+}
+function ballJoint(joint, aIndex, bIndex, warm, weld) {
+    const state = {
+        impulse: warm ? { x: warm.impulseX, y: warm.impulseY, z: warm.impulseZ } : { x: 0, y: 0, z: 0 },
+        axialImpulse: 0,
+        hingeAngle: 0,
+    };
+    let rA = { x: 0, y: 0, z: 0 };
+    let rB = { x: 0, y: 0, z: 0 };
+    let kInv = [0, 0, 0, 0, 0, 0, 0, 0, 0];
+    let biasTarget = { x: 0, y: 0, z: 0 };
+    let angKInv = [0, 0, 0, 0, 0, 0, 0, 0, 0];
+    return {
+        bodyIndices: [aIndex, bIndex],
+        prepare(ctx) {
+            const A = ctx.bodies[aIndex];
+            const B = ctx.bodies[bIndex];
+            rA = (0, physics3d_shared_js_1.rotatePointByQuat)(A.q, joint.anchorA);
+            rB = (0, physics3d_shared_js_1.rotatePointByQuat)(B.q, joint.anchorB);
+            const c = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.p, rB), (0, physics3d_shared_js_1.addVec3)(A.p, rA));
+            // split-impulse: pseudo-velocity target that closes position error without
+            // pumping real momentum (Baumgarte in the real solve pumps energy).
+            biasTarget = (0, physics3d_shared_js_1.scaleVec3)(c, -JOINT_BAUMGARTE / ctx.dt);
+            kInv = (0, physics3d_shared_js_1.invertMat3)(pointConstraintK(ctx, aIndex, bIndex, rA, rB));
+            if (weld) {
+                angKInv = (0, physics3d_shared_js_1.invertMat3)((0, physics3d_shared_js_1.mat3Sub)((0, physics3d_shared_js_1.mat3Sub)((0, physics3d_shared_js_1.mat3FromDiag)({ x: 0, y: 0, z: 0 }), scaleMat3(ctx.invInertia[aIndex], -1)), scaleMat3(ctx.invInertia[bIndex], -1)));
+            }
+            ctx.apply(aIndex, bIndex, rA, rB, state.impulse);
+        },
+        iterate(ctx) {
+            const A = ctx.bodies[aIndex];
+            const B = ctx.bodies[bIndex];
+            const relVel = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.v, (0, physics3d_shared_js_1.crossVec3)(B.w, rB)), (0, physics3d_shared_js_1.addVec3)(A.v, (0, physics3d_shared_js_1.crossVec3)(A.w, rA)));
+            const dImpulse = (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.mulMat3Vec)(kInv, relVel), -1);
+            state.impulse = (0, physics3d_shared_js_1.addVec3)(state.impulse, dImpulse);
+            ctx.apply(aIndex, bIndex, rA, rB, dImpulse);
+            if (weld) {
+                const relOmega = (0, physics3d_shared_js_1.subVec3)(B.w, A.w);
+                const angImpulse = (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.mulMat3Vec)(angKInv, relOmega), -1);
+                ctx.applyAngular(aIndex, bIndex, angImpulse);
+            }
+            // position correction on pseudo-velocities
+            const relBias = ctx.biasRelativeVelocity(aIndex, bIndex, rA, rB);
+            const dBias = (0, physics3d_shared_js_1.mulMat3Vec)(kInv, (0, physics3d_shared_js_1.subVec3)(biasTarget, relBias));
+            ctx.applyBias(aIndex, bIndex, rA, rB, dBias);
+        },
+        emitCache() {
+            const reaction2 = (0, physics3d_shared_js_1.dotVec3)(state.impulse, state.impulse);
+            const broken = joint.breakImpulse !== undefined && reaction2 > joint.breakImpulse * joint.breakImpulse;
+            return {
+                jointId: joint.id, impulseX: state.impulse.x, impulseY: state.impulse.y, impulseZ: state.impulse.z, axialImpulse: 0, hingeAngle: 0,
+                ...(broken ? { broken: true } : {}),
+            };
+        },
+    };
+}
+function scaleMat3(m, s) {
+    return m.map((value) => value * s);
+}
+function hingeJoint(joint, aIndex, bIndex, warm) {
+    const state = {
+        impulse: warm ? { x: warm.impulseX, y: warm.impulseY, z: warm.impulseZ } : { x: 0, y: 0, z: 0 },
+        axialImpulse: warm?.axialImpulse ?? 0,
+        hingeAngle: warm?.hingeAngle ?? 0,
+    };
+    let rA = { x: 0, y: 0, z: 0 };
+    let rB = { x: 0, y: 0, z: 0 };
+    let kInv = [0, 0, 0, 0, 0, 0, 0, 0, 0];
+    let biasTarget = { x: 0, y: 0, z: 0 };
+    let axis = { x: 0, y: 1, z: 0 };
+    let t1 = { x: 1, y: 0, z: 0 };
+    let t2 = { x: 0, y: 0, z: 1 };
+    let angMassAxis = 0;
+    let angMassT1 = 0;
+    let angMassT2 = 0;
+    let limitImpulse = 0;
+    let ctxRef;
+    const angularMass = (ctx, t) => {
+        const denom = (0, physics3d_shared_js_1.dotVec3)(t, (0, physics3d_shared_js_1.mulMat3Vec)(ctx.invInertia[aIndex], t)) + (0, physics3d_shared_js_1.dotVec3)(t, (0, physics3d_shared_js_1.mulMat3Vec)(ctx.invInertia[bIndex], t));
+        /* istanbul ignore next -- a hinge with a real dynamic body has non-zero inverse inertia along the axis; the zero guard is defensive. */
+        return denom === 0 ? 0 : 1 / denom;
+    };
+    return {
+        bodyIndices: [aIndex, bIndex],
+        prepare(ctx) {
+            const A = ctx.bodies[aIndex];
+            const B = ctx.bodies[bIndex];
+            rA = (0, physics3d_shared_js_1.rotatePointByQuat)(A.q, joint.anchorA);
+            rB = (0, physics3d_shared_js_1.rotatePointByQuat)(B.q, joint.anchorB);
+            biasTarget = (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.p, rB), (0, physics3d_shared_js_1.addVec3)(A.p, rA)), -JOINT_BAUMGARTE / ctx.dt);
+            kInv = (0, physics3d_shared_js_1.invertMat3)(pointConstraintK(ctx, aIndex, bIndex, rA, rB));
+            const rawAxis = (0, physics3d_shared_js_1.rotatePointByQuat)(A.q, joint.axisA);
+            const len = (0, physics3d_shared_js_1.solverSqrt)((0, physics3d_shared_js_1.dotVec3)(rawAxis, rawAxis));
+            /* istanbul ignore next -- axisA is validated non-zero and rotation preserves length, so len is never zero. */
+            axis = len === 0 ? { x: 0, y: 1, z: 0 } : (0, physics3d_shared_js_1.scaleVec3)(rawAxis, 1 / len);
+            // deterministic perpendicular basis
+            t1 = Math.abs(axis.x) >= 0.57735 ? unit({ x: axis.y, y: -axis.x, z: 0 }) : unit({ x: 0, y: axis.z, z: -axis.y });
+            t2 = (0, physics3d_shared_js_1.crossVec3)(axis, t1);
+            angMassAxis = angularMass(ctx, axis);
+            angMassT1 = angularMass(ctx, t1);
+            angMassT2 = angularMass(ctx, t2);
+            limitImpulse = 0;
+            ctxRef = ctx;
+            ctx.apply(aIndex, bIndex, rA, rB, state.impulse);
+            if (state.axialImpulse !== 0) {
+                ctx.applyAngular(aIndex, bIndex, (0, physics3d_shared_js_1.scaleVec3)(axis, state.axialImpulse));
+            }
+        },
+        iterate(ctx) {
+            const A = ctx.bodies[aIndex];
+            const B = ctx.bodies[bIndex];
+            // point constraint first so its r×P perturbation of the axial DOF is
+            // corrected by the motor/limit rows that follow (split-impulse: no
+            // position Baumgarte in the real solve, so it cannot pump the motor).
+            const relVel = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.v, (0, physics3d_shared_js_1.crossVec3)(B.w, rB)), (0, physics3d_shared_js_1.addVec3)(A.v, (0, physics3d_shared_js_1.crossVec3)(A.w, rA)));
+            const dPoint = (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.mulMat3Vec)(kInv, relVel), -1);
+            state.impulse = (0, physics3d_shared_js_1.addVec3)(state.impulse, dPoint);
+            ctx.apply(aIndex, bIndex, rA, rB, dPoint);
+            const relBias = ctx.biasRelativeVelocity(aIndex, bIndex, rA, rB);
+            ctx.applyBias(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.mulMat3Vec)(kInv, (0, physics3d_shared_js_1.subVec3)(biasTarget, relBias)));
+            // perpendicular angular locks
+            for (const [t, mass] of [[t1, angMassT1], [t2, angMassT2]]) {
+                const relW = (0, physics3d_shared_js_1.dotVec3)((0, physics3d_shared_js_1.subVec3)(B.w, A.w), t);
+                ctx.applyAngular(aIndex, bIndex, (0, physics3d_shared_js_1.scaleVec3)(t, -mass * relW));
+            }
+            // motor
+            if (joint.motorSpeed !== undefined) {
+                const maxTorque = joint.maxMotorTorque ?? 0;
+                const relAxial = (0, physics3d_shared_js_1.dotVec3)((0, physics3d_shared_js_1.subVec3)(B.w, A.w), axis);
+                let dImpulse = angMassAxis * (joint.motorSpeed - relAxial);
+                const maxImpulse = maxTorque * ctx.dt;
+                const newAcc = (0, physics3d_shared_js_1.clampNumber)(state.axialImpulse + dImpulse, -maxImpulse, maxImpulse);
+                dImpulse = newAcc - state.axialImpulse;
+                state.axialImpulse = newAcc;
+                ctx.applyAngular(aIndex, bIndex, (0, physics3d_shared_js_1.scaleVec3)(axis, dImpulse));
+            }
+            // limits (inequality, biased by how far past)
+            if (joint.minAngle !== undefined && state.hingeAngle <= joint.minAngle) {
+                const overshoot = joint.minAngle - state.hingeAngle;
+                const relAxial = (0, physics3d_shared_js_1.dotVec3)((0, physics3d_shared_js_1.subVec3)(B.w, A.w), axis);
+                let dImpulse = angMassAxis * (-relAxial + (JOINT_BAUMGARTE / ctx.dt) * overshoot);
+                const newAcc = Math.max(0, limitImpulse + dImpulse);
+                dImpulse = newAcc - limitImpulse;
+                limitImpulse = newAcc;
+                ctx.applyAngular(aIndex, bIndex, (0, physics3d_shared_js_1.scaleVec3)(axis, dImpulse));
+            }
+            else if (joint.maxAngle !== undefined && state.hingeAngle >= joint.maxAngle) {
+                const overshoot = state.hingeAngle - joint.maxAngle;
+                const relAxial = (0, physics3d_shared_js_1.dotVec3)((0, physics3d_shared_js_1.subVec3)(B.w, A.w), axis);
+                let dImpulse = angMassAxis * (-relAxial - (JOINT_BAUMGARTE / ctx.dt) * overshoot);
+                const newAcc = Math.min(0, limitImpulse + dImpulse);
+                dImpulse = newAcc - limitImpulse;
+                limitImpulse = newAcc;
+                ctx.applyAngular(aIndex, bIndex, (0, physics3d_shared_js_1.scaleVec3)(axis, dImpulse));
+            }
+        },
+        emitCache() {
+            // integrate the hinge angle from the post-solve relative axial velocity
+            /* istanbul ignore else -- prepare() always runs before emitCache(), so ctxRef is defined. */
+            if (ctxRef !== undefined) {
+                const relAxial = (0, physics3d_shared_js_1.dotVec3)((0, physics3d_shared_js_1.subVec3)(ctxRef.bodies[bIndex].w, ctxRef.bodies[aIndex].w), axis);
+                state.hingeAngle += relAxial * ctxRef.dt;
+            }
+            const reaction2 = (0, physics3d_shared_js_1.dotVec3)(state.impulse, state.impulse);
+            const broken = joint.breakImpulse !== undefined && reaction2 > joint.breakImpulse * joint.breakImpulse;
+            return {
+                jointId: joint.id,
+                impulseX: state.impulse.x,
+                impulseY: state.impulse.y,
+                impulseZ: state.impulse.z,
+                axialImpulse: state.axialImpulse,
+                hingeAngle: state.hingeAngle,
+                ...(broken ? { broken: true } : {}),
+            };
+        },
+    };
+}
+function unit(v) {
+    const l = (0, physics3d_shared_js_1.solverSqrt)((0, physics3d_shared_js_1.dotVec3)(v, v));
+    /* istanbul ignore next -- unit() is only applied to non-zero perpendicular basis vectors derived from the hinge axis. */
+    return l === 0 ? { x: 0, y: 0, z: 0 } : (0, physics3d_shared_js_1.scaleVec3)(v, 1 / l);
+}
+function distanceJoint(joint, aIndex, bIndex, warm) {
+    const state = {
+        impulse: { x: warm?.axialImpulse ?? 0, y: 0, z: 0 },
+        axialImpulse: warm?.axialImpulse ?? 0,
+        hingeAngle: 0,
+    };
+    let rA = { x: 0, y: 0, z: 0 };
+    let rB = { x: 0, y: 0, z: 0 };
+    let n = { x: 0, y: 1, z: 0 };
+    let mass = 0;
+    let length = 0;
+    return {
+        bodyIndices: [aIndex, bIndex],
+        prepare(ctx) {
+            const A = ctx.bodies[aIndex];
+            const B = ctx.bodies[bIndex];
+            rA = (0, physics3d_shared_js_1.rotatePointByQuat)(A.q, joint.anchorA);
+            rB = (0, physics3d_shared_js_1.rotatePointByQuat)(B.q, joint.anchorB);
+            const d = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.p, rB), (0, physics3d_shared_js_1.addVec3)(A.p, rA));
+            length = (0, physics3d_shared_js_1.solverSqrt)((0, physics3d_shared_js_1.dotVec3)(d, d));
+            n = length === 0 ? { x: 0, y: 1, z: 0 } : (0, physics3d_shared_js_1.scaleVec3)(d, 1 / length);
+            const raxn = (0, physics3d_shared_js_1.crossVec3)(rA, n);
+            const rbxn = (0, physics3d_shared_js_1.crossVec3)(rB, n);
+            const imA = ctx.invMass[aIndex];
+            const imB = ctx.invMass[bIndex];
+            const linear = n.x * n.x * (imA.x + imB.x) + n.y * n.y * (imA.y + imB.y) + n.z * n.z * (imA.z + imB.z);
+            const denom = linear + (0, physics3d_shared_js_1.dotVec3)(raxn, (0, physics3d_shared_js_1.mulMat3Vec)(ctx.invInertia[aIndex], raxn)) + (0, physics3d_shared_js_1.dotVec3)(rbxn, (0, physics3d_shared_js_1.mulMat3Vec)(ctx.invInertia[bIndex], rbxn));
+            /* istanbul ignore next -- a distance joint with a real dynamic body has non-zero effective mass; the zero guard is defensive. */
+            mass = denom === 0 ? 0 : 1 / denom;
+            state.axialImpulse = warm?.axialImpulse ?? 0;
+            ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(n, state.axialImpulse));
+        },
+        iterate(ctx) {
+            const A = ctx.bodies[aIndex];
+            const B = ctx.bodies[bIndex];
+            const relVel = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.v, (0, physics3d_shared_js_1.crossVec3)(B.w, rB)), (0, physics3d_shared_js_1.addVec3)(A.v, (0, physics3d_shared_js_1.crossVec3)(A.w, rA)));
+            const vn = (0, physics3d_shared_js_1.dotVec3)(relVel, n);
+            const overMax = length - joint.maxLength;
+            const underMin = joint.minLength !== undefined ? joint.minLength - length : -Infinity;
+            if (overMax > 0) {
+                const targetBias = (JOINT_BAUMGARTE / ctx.dt) * overMax;
+                let dLambda = -mass * (vn + targetBias);
+                const newAcc = Math.min(0, state.axialImpulse + dLambda);
+                dLambda = newAcc - state.axialImpulse;
+                state.axialImpulse = newAcc;
+                ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(n, dLambda));
+            }
+            else if (underMin > 0) {
+                const targetBias = (JOINT_BAUMGARTE / ctx.dt) * underMin;
+                let dLambda = mass * (-vn + targetBias);
+                const newAcc = Math.max(0, state.axialImpulse + dLambda);
+                dLambda = newAcc - state.axialImpulse;
+                state.axialImpulse = newAcc;
+                ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(n, dLambda));
+            }
+            else if (state.axialImpulse !== 0) {
+                // rope is slack — remove the warm-started pull applied in prepare(),
+                // otherwise it keeps pulling for one frame after going limp.
+                ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(n, -state.axialImpulse));
+                state.axialImpulse = 0;
+            }
+        },
+        emitCache() {
+            const reaction2 = state.axialImpulse * state.axialImpulse;
+            const broken = joint.breakImpulse !== undefined && reaction2 > joint.breakImpulse * joint.breakImpulse;
+            return {
+                jointId: joint.id, impulseX: 0, impulseY: 0, impulseZ: 0, axialImpulse: state.axialImpulse, hingeAngle: 0,
+                ...(broken ? { broken: true } : {}),
+            };
+        },
+    };
+}
+function prismaticJoint(joint, aIndex, bIndex, warm) {
+    const state = {
+        impulse: warm ? { x: warm.impulseX, y: warm.impulseY, z: warm.impulseZ } : { x: 0, y: 0, z: 0 },
+        axialImpulse: warm?.axialImpulse ?? 0,
+        hingeAngle: 0,
+    };
+    let rA = { x: 0, y: 0, z: 0 };
+    let rB = { x: 0, y: 0, z: 0 };
+    let axis = { x: 1, y: 0, z: 0 };
+    let t1 = { x: 0, y: 1, z: 0 };
+    let t2 = { x: 0, y: 0, z: 1 };
+    let angKInv = [0, 0, 0, 0, 0, 0, 0, 0, 0];
+    let massT1 = 0;
+    let massT2 = 0;
+    let massAxial = 0;
+    let translation = 0;
+    let biasT1 = 0;
+    let biasT2 = 0;
+    let limitImpulse = 0;
+    const rowMass = (ctx, t) => {
+        const raxn = (0, physics3d_shared_js_1.crossVec3)(rA, t);
+        const rbxn = (0, physics3d_shared_js_1.crossVec3)(rB, t);
+        const imA = ctx.invMass[aIndex];
+        const imB = ctx.invMass[bIndex];
+        const linear = t.x * t.x * (imA.x + imB.x) + t.y * t.y * (imA.y + imB.y) + t.z * t.z * (imA.z + imB.z);
+        const denom = linear + (0, physics3d_shared_js_1.dotVec3)(raxn, (0, physics3d_shared_js_1.mulMat3Vec)(ctx.invInertia[aIndex], raxn)) + (0, physics3d_shared_js_1.dotVec3)(rbxn, (0, physics3d_shared_js_1.mulMat3Vec)(ctx.invInertia[bIndex], rbxn));
+        /* istanbul ignore next -- a prismatic joint with a real dynamic body has non-zero row mass; the zero guard is defensive. */
+        return denom === 0 ? 0 : 1 / denom;
+    };
+    return {
+        bodyIndices: [aIndex, bIndex],
+        prepare(ctx) {
+            const A = ctx.bodies[aIndex];
+            const B = ctx.bodies[bIndex];
+            rA = (0, physics3d_shared_js_1.rotatePointByQuat)(A.q, joint.anchorA);
+            rB = (0, physics3d_shared_js_1.rotatePointByQuat)(B.q, joint.anchorB);
+            const c = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.p, rB), (0, physics3d_shared_js_1.addVec3)(A.p, rA));
+            const rawAxis = (0, physics3d_shared_js_1.rotatePointByQuat)(A.q, joint.axisA);
+            const len = (0, physics3d_shared_js_1.solverSqrt)((0, physics3d_shared_js_1.dotVec3)(rawAxis, rawAxis));
+            /* istanbul ignore next -- axisA is validated non-zero and rotation preserves length, so len is never zero. */
+            axis = len === 0 ? { x: 1, y: 0, z: 0 } : (0, physics3d_shared_js_1.scaleVec3)(rawAxis, 1 / len);
+            t1 = Math.abs(axis.x) >= 0.57735 ? unit({ x: axis.y, y: -axis.x, z: 0 }) : unit({ x: 0, y: axis.z, z: -axis.y });
+            t2 = (0, physics3d_shared_js_1.crossVec3)(axis, t1);
+            translation = (0, physics3d_shared_js_1.dotVec3)(c, axis);
+            massT1 = rowMass(ctx, t1);
+            massT2 = rowMass(ctx, t2);
+            massAxial = rowMass(ctx, axis);
+            angKInv = (0, physics3d_shared_js_1.invertMat3)((0, physics3d_shared_js_1.mat3Sub)((0, physics3d_shared_js_1.mat3Sub)((0, physics3d_shared_js_1.mat3FromDiag)({ x: 0, y: 0, z: 0 }), scaleMat3(ctx.invInertia[aIndex], -1)), scaleMat3(ctx.invInertia[bIndex], -1)));
+            biasT1 = (-JOINT_BAUMGARTE / ctx.dt) * (0, physics3d_shared_js_1.dotVec3)(c, t1);
+            biasT2 = (-JOINT_BAUMGARTE / ctx.dt) * (0, physics3d_shared_js_1.dotVec3)(c, t2);
+            limitImpulse = 0;
+            ctx.apply(aIndex, bIndex, rA, rB, state.impulse);
+            if (state.axialImpulse !== 0) {
+                ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(axis, state.axialImpulse));
+            }
+        },
+        iterate(ctx) {
+            const A = ctx.bodies[aIndex];
+            const B = ctx.bodies[bIndex];
+            // perpendicular rows: kill anchor drift off the slide axis.
+            // Sequential-impulse discipline: re-read the relative velocity after each
+            // applied row (one stale snapshot for both rows under-corrects — same GS
+            // lesson as the vehicle tire solve).
+            for (const [t, mass, biasT] of [[t1, massT1, biasT1], [t2, massT2, biasT2]]) {
+                const relVel = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.v, (0, physics3d_shared_js_1.crossVec3)(B.w, rB)), (0, physics3d_shared_js_1.addVec3)(A.v, (0, physics3d_shared_js_1.crossVec3)(A.w, rA)));
+                const vn = (0, physics3d_shared_js_1.dotVec3)(relVel, t);
+                const dLambda = -mass * vn;
+                state.impulse = (0, physics3d_shared_js_1.addVec3)(state.impulse, (0, physics3d_shared_js_1.scaleVec3)(t, dLambda));
+                ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(t, dLambda));
+                const relBias = ctx.biasRelativeVelocity(aIndex, bIndex, rA, rB);
+                ctx.applyBias(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(t, mass * (biasT - (0, physics3d_shared_js_1.dotVec3)(relBias, t))));
+            }
+            // full angular lock (weld-style)
+            const relOmega = (0, physics3d_shared_js_1.subVec3)(B.w, A.w);
+            ctx.applyAngular(aIndex, bIndex, (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.mulMat3Vec)(angKInv, relOmega), -1));
+            // linear motor along the axis
+            if (joint.motorSpeed !== undefined) {
+                const maxForce = joint.maxMotorForce ?? 0;
+                const relAfterLock = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.v, (0, physics3d_shared_js_1.crossVec3)(B.w, rB)), (0, physics3d_shared_js_1.addVec3)(A.v, (0, physics3d_shared_js_1.crossVec3)(A.w, rA)));
+                const relAxial = (0, physics3d_shared_js_1.dotVec3)(relAfterLock, axis);
+                let dImpulse = massAxial * (joint.motorSpeed - relAxial);
+                const maxImpulse = maxForce * ctx.dt;
+                const newAcc = (0, physics3d_shared_js_1.clampNumber)(state.axialImpulse + dImpulse, -maxImpulse, maxImpulse);
+                dImpulse = newAcc - state.axialImpulse;
+                state.axialImpulse = newAcc;
+                ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(axis, dImpulse));
+            }
+            // translation limits (inequality, biased by overshoot — mirrors distance)
+            const relNow = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.v, (0, physics3d_shared_js_1.crossVec3)(B.w, rB)), (0, physics3d_shared_js_1.addVec3)(A.v, (0, physics3d_shared_js_1.crossVec3)(A.w, rA)));
+            const vnAxial = (0, physics3d_shared_js_1.dotVec3)(relNow, axis);
+            if (joint.maxTranslation !== undefined && translation > joint.maxTranslation) {
+                const overshoot = translation - joint.maxTranslation;
+                let dLambda = -massAxial * (vnAxial + (JOINT_BAUMGARTE / ctx.dt) * overshoot);
+                const newAcc = Math.min(0, limitImpulse + dLambda);
+                dLambda = newAcc - limitImpulse;
+                limitImpulse = newAcc;
+                ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(axis, dLambda));
+            }
+            else if (joint.minTranslation !== undefined && translation < joint.minTranslation) {
+                const overshoot = joint.minTranslation - translation;
+                let dLambda = massAxial * (-vnAxial + (JOINT_BAUMGARTE / ctx.dt) * overshoot);
+                const newAcc = Math.max(0, limitImpulse + dLambda);
+                dLambda = newAcc - limitImpulse;
+                limitImpulse = newAcc;
+                ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(axis, dLambda));
+            }
+        },
+        emitCache() {
+            // perpendicular reaction only — the axialImpulse is the motor accumulator,
+            // and motors/limits are excluded from breaking.
+            const reaction2 = (0, physics3d_shared_js_1.dotVec3)(state.impulse, state.impulse);
+            const broken = joint.breakImpulse !== undefined && reaction2 > joint.breakImpulse * joint.breakImpulse;
+            return {
+                jointId: joint.id,
+                impulseX: state.impulse.x,
+                impulseY: state.impulse.y,
+                impulseZ: state.impulse.z,
+                axialImpulse: state.axialImpulse,
+                hingeAngle: 0,
+                ...(broken ? { broken: true } : {}),
+            };
+        },
+    };
+}
+function springJoint(joint, aIndex, bIndex, warm) {
+    const state = {
+        impulse: { x: 0, y: 0, z: 0 },
+        axialImpulse: warm?.axialImpulse ?? 0,
+        hingeAngle: 0,
+    };
+    const omega = 6.283185307179586 * joint.frequencyHz;
+    let rA = { x: 0, y: 0, z: 0 };
+    let rB = { x: 0, y: 0, z: 0 };
+    let n = { x: 0, y: 1, z: 0 };
+    let mass = 0;
+    let length = 0;
+    let gamma = 0;
+    let bias = 0;
+    let massSoft = 0;
+    return {
+        bodyIndices: [aIndex, bIndex],
+        prepare(ctx) {
+            const A = ctx.bodies[aIndex];
+            const B = ctx.bodies[bIndex];
+            rA = (0, physics3d_shared_js_1.rotatePointByQuat)(A.q, joint.anchorA);
+            rB = (0, physics3d_shared_js_1.rotatePointByQuat)(B.q, joint.anchorB);
+            const d = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.p, rB), (0, physics3d_shared_js_1.addVec3)(A.p, rA));
+            length = (0, physics3d_shared_js_1.solverSqrt)((0, physics3d_shared_js_1.dotVec3)(d, d));
+            n = length === 0 ? { x: 0, y: 1, z: 0 } : (0, physics3d_shared_js_1.scaleVec3)(d, 1 / length);
+            const raxn = (0, physics3d_shared_js_1.crossVec3)(rA, n);
+            const rbxn = (0, physics3d_shared_js_1.crossVec3)(rB, n);
+            const imA = ctx.invMass[aIndex];
+            const imB = ctx.invMass[bIndex];
+            const linear = n.x * n.x * (imA.x + imB.x) + n.y * n.y * (imA.y + imB.y) + n.z * n.z * (imA.z + imB.z);
+            const denom = linear + (0, physics3d_shared_js_1.dotVec3)(raxn, (0, physics3d_shared_js_1.mulMat3Vec)(ctx.invInertia[aIndex], raxn)) + (0, physics3d_shared_js_1.dotVec3)(rbxn, (0, physics3d_shared_js_1.mulMat3Vec)(ctx.invInertia[bIndex], rbxn));
+            mass = denom === 0 ? 0 : 1 / denom;
+            // Box2D soft-constraint coefficients from frequency + damping ratio.
+            const c = 2 * mass * joint.dampingRatio * omega;
+            const k = mass * omega * omega;
+            const softDenom = c + ctx.dt * k;
+            gamma = softDenom === 0 ? 0 : 1 / (ctx.dt * softDenom);
+            const beta = ctx.dt * k * gamma;
+            bias = (beta / ctx.dt) * (length - joint.restLength);
+            massSoft = mass === 0 ? 0 : 1 / (1 / mass + gamma);
+            state.axialImpulse = warm?.axialImpulse ?? 0;
+            ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(n, state.axialImpulse));
+        },
+        iterate(ctx) {
+            const A = ctx.bodies[aIndex];
+            const B = ctx.bodies[bIndex];
+            const relVel = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(B.v, (0, physics3d_shared_js_1.crossVec3)(B.w, rB)), (0, physics3d_shared_js_1.addVec3)(A.v, (0, physics3d_shared_js_1.crossVec3)(A.w, rA)));
+            const cdot = (0, physics3d_shared_js_1.dotVec3)(relVel, n);
+            const dLambda = -massSoft * (cdot + bias + gamma * state.axialImpulse);
+            state.axialImpulse += dLambda;
+            ctx.apply(aIndex, bIndex, rA, rB, (0, physics3d_shared_js_1.scaleVec3)(n, dLambda));
+        },
+        emitCache() {
+            return { jointId: joint.id, impulseX: 0, impulseY: 0, impulseZ: 0, axialImpulse: state.axialImpulse, hingeAngle: 0 };
+        },
+    };
+}
+function grabJoint(joint, bodyIndex, warm) {
+    const state = {
+        impulse: warm ? { x: warm.impulseX, y: warm.impulseY, z: warm.impulseZ } : { x: 0, y: 0, z: 0 },
+        axialImpulse: 0,
+        hingeAngle: 0,
+    };
+    let r = { x: 0, y: 0, z: 0 };
+    let kInv = [0, 0, 0, 0, 0, 0, 0, 0, 0];
+    let desiredVel = { x: 0, y: 0, z: 0 };
+    const target = { x: joint.targetX, y: joint.targetY, z: joint.targetZ };
+    const maxImpulse = joint.maxForce * 1;
+    return {
+        bodyIndices: [bodyIndex],
+        prepare(ctx) {
+            const body = ctx.bodies[bodyIndex];
+            r = (0, physics3d_shared_js_1.rotatePointByQuat)(body.q, joint.anchor);
+            const worldAnchor = (0, physics3d_shared_js_1.addVec3)(body.p, r);
+            const imB = ctx.invMass[bodyIndex];
+            const linear = (0, physics3d_shared_js_1.mat3FromDiag)(imB);
+            const skewR = (0, physics3d_shared_js_1.skewMat3)(r);
+            const ang = (0, physics3d_shared_js_1.mat3Multiply)((0, physics3d_shared_js_1.mat3Multiply)(skewR, ctx.invInertia[bodyIndex]), skewR);
+            kInv = (0, physics3d_shared_js_1.invertMat3)((0, physics3d_shared_js_1.mat3Sub)(linear, ang));
+            const damping = joint.damping ?? 1;
+            desiredVel = (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.subVec3)(target, worldAnchor), damping / ctx.dt);
+            ctx.applyToBody(bodyIndex, r, state.impulse);
+        },
+        iterate(ctx) {
+            const body = ctx.bodies[bodyIndex];
+            const currentVel = (0, physics3d_shared_js_1.addVec3)(body.v, (0, physics3d_shared_js_1.crossVec3)(body.w, r));
+            const dImpulse = (0, physics3d_shared_js_1.mulMat3Vec)(kInv, (0, physics3d_shared_js_1.subVec3)(desiredVel, currentVel));
+            const target2 = (0, physics3d_shared_js_1.addVec3)(state.impulse, dImpulse);
+            const clamped = clampMagnitude(target2, maxImpulse * ctx.dt);
+            const applied = (0, physics3d_shared_js_1.subVec3)(clamped, state.impulse);
+            state.impulse = clamped;
+            ctx.applyToBody(bodyIndex, r, applied);
+        },
+        emitCache() {
+            return { jointId: joint.id, impulseX: state.impulse.x, impulseY: state.impulse.y, impulseZ: state.impulse.z, axialImpulse: 0, hingeAngle: 0 };
+        },
+    };
+}
+function clampMagnitude(v, maxLength) {
+    const lenSq = (0, physics3d_shared_js_1.dotVec3)(v, v);
+    const maxSq = maxLength * maxLength;
+    if (lenSq <= maxSq || lenSq === 0) {
+        return v;
+    }
+    const scale = maxLength / (0, physics3d_shared_js_1.solverSqrt)(lenSq);
+    return (0, physics3d_shared_js_1.scaleVec3)(v, scale);
+}
+function validateJoint(joint) {
+    if (joint.type === 'grab') {
+        assertFiniteVec(joint.anchor, 'anchor', joint.id);
+        if (!Number.isFinite(joint.maxForce) || joint.maxForce <= 0) {
+            throw new Error(`physics3d joint ${joint.id}: maxForce must be a positive finite number`);
+        }
+        if (!Number.isFinite(joint.targetX) || !Number.isFinite(joint.targetY) || !Number.isFinite(joint.targetZ)) {
+            throw new Error(`physics3d joint ${joint.id}: target must be finite`);
+        }
+        return;
+    }
+    assertFiniteVec(joint.anchorA, 'anchorA', joint.id);
+    assertFiniteVec(joint.anchorB, 'anchorB', joint.id);
+    if ('breakImpulse' in joint && joint.breakImpulse !== undefined && (!Number.isFinite(joint.breakImpulse) || joint.breakImpulse <= 0)) {
+        throw new Error(`physics3d joint ${joint.id}: breakImpulse must be positive`);
+    }
+    if (joint.type === 'hinge') {
+        if ((0, physics3d_shared_js_1.dotVec3)(joint.axisA, joint.axisA) === 0) {
+            throw new Error(`physics3d joint ${joint.id}: hinge axisA must be non-zero`);
+        }
+        if (joint.minAngle !== undefined && joint.maxAngle !== undefined && joint.minAngle > joint.maxAngle) {
+            throw new Error(`physics3d joint ${joint.id}: minAngle must not exceed maxAngle`);
+        }
+        if (joint.maxMotorTorque !== undefined && joint.maxMotorTorque <= 0) {
+            throw new Error(`physics3d joint ${joint.id}: maxMotorTorque must be positive`);
+        }
+    }
+    if (joint.type === 'distance') {
+        if (!Number.isFinite(joint.maxLength) || joint.maxLength <= 0) {
+            throw new Error(`physics3d joint ${joint.id}: maxLength must be a positive finite number`);
+        }
+        if (joint.minLength !== undefined && joint.minLength > joint.maxLength) {
+            throw new Error(`physics3d joint ${joint.id}: minLength must not exceed maxLength`);
+        }
+    }
+    if (joint.type === 'prismatic') {
+        assertFiniteVec(joint.axisA, 'axisA', joint.id);
+        if ((0, physics3d_shared_js_1.dotVec3)(joint.axisA, joint.axisA) === 0) {
+            throw new Error(`physics3d joint ${joint.id}: prismatic axisA must be non-zero`);
+        }
+        for (const [name, value] of [['minTranslation', joint.minTranslation], ['maxTranslation', joint.maxTranslation], ['motorSpeed', joint.motorSpeed]]) {
+            if (value !== undefined && !Number.isFinite(value)) {
+                throw new Error(`physics3d joint ${joint.id}: ${name} must be a finite number`);
+            }
+        }
+        if (joint.minTranslation !== undefined && joint.maxTranslation !== undefined && joint.minTranslation > joint.maxTranslation) {
+            throw new Error(`physics3d joint ${joint.id}: minTranslation must not exceed maxTranslation`);
+        }
+        if (joint.maxMotorForce !== undefined && (!Number.isFinite(joint.maxMotorForce) || joint.maxMotorForce <= 0)) {
+            throw new Error(`physics3d joint ${joint.id}: maxMotorForce must be positive`);
+        }
+    }
+    if (joint.type === 'spring') {
+        if (!Number.isFinite(joint.frequencyHz) || joint.frequencyHz <= 0) {
+            throw new Error(`physics3d joint ${joint.id}: frequencyHz must be a positive finite number`);
+        }
+        if (!Number.isFinite(joint.dampingRatio) || joint.dampingRatio < 0) {
+            throw new Error(`physics3d joint ${joint.id}: dampingRatio must be a non-negative finite number`);
+        }
+        if (!Number.isFinite(joint.restLength) || joint.restLength < 0) {
+            throw new Error(`physics3d joint ${joint.id}: restLength must be a non-negative finite number`);
+        }
+    }
+}
+function brokenJoint(jointId) {
+    return {
+        bodyIndices: [],
+        prepare() { },
+        iterate() { },
+        emitCache() {
+            return { jointId, impulseX: 0, impulseY: 0, impulseZ: 0, axialImpulse: 0, hingeAngle: 0, broken: true };
+        },
+    };
+}
+/** Build solver joint objects from joint definitions, seeded from the joint cache. */
+function createSolverJoints(joints, indexById, jointCache) {
+    const cacheById = new Map(jointCache.map((entry) => [entry.jointId, entry]));
+    const seenIds = new Set();
+    return [...joints]
+        .sort((left, right) => (left.id < right.id ? -1 : left.id > right.id ? 1 : 0))
+        .map((joint) => {
+        if (seenIds.has(joint.id)) {
+            throw new Error(`physics3d joint ${joint.id}: duplicate joint id`);
+        }
+        seenIds.add(joint.id);
+        validateJoint(joint);
+        const warm = cacheById.get(joint.id);
+        if (warm?.broken === true) {
+            return brokenJoint(joint.id);
+        }
+        if (joint.type === 'grab') {
+            return grabJoint(joint, requireIndex(joint.body, joint.id, indexById), warm);
+        }
+        const aIndex = requireIndex(joint.a, joint.id, indexById);
+        const bIndex = requireIndex(joint.b, joint.id, indexById);
+        if (joint.type === 'ball') {
+            return ballJoint(joint, aIndex, bIndex, warm, false);
+        }
+        if (joint.type === 'weld') {
+            return ballJoint(joint, aIndex, bIndex, warm, true);
+        }
+        if (joint.type === 'hinge') {
+            return hingeJoint(joint, aIndex, bIndex, warm);
+        }
+        if (joint.type === 'spring') {
+            return springJoint(joint, aIndex, bIndex, warm);
+        }
+        if (joint.type === 'prismatic') {
+            return prismaticJoint(joint, aIndex, bIndex, warm);
+        }
+        return distanceJoint(joint, aIndex, bIndex, warm);
+    });
+}