@jgengine/core 0.6.0 → 0.8.0

package/dist/physics/physicsWorld.js

@@ -1,5 +1,10 @@
+export const SHAPE_BOX = 0;
+export const SHAPE_SPHERE = 1;
 const FLAG_SLEEPING = 1;
 const FLAG_STATIC = 2;
+const FLAG_DEAD = 4;
+/** Cap on `nx*ny*nz` broadphase cells — guards a huge-bounds/tiny-cellSize config from hanging `step()`. */
+export const MAX_BROADPHASE_CELLS = 1_000_000;
 /** Grid cell containing a point, clamped into the grid. Pure — exported for tests. */
 export function cellCoord(value, min, cellSize, cells) {
     const raw = Math.floor((value - min) / cellSize);
@@ -26,6 +31,8 @@ export class PhysicsWorld {
     sleepMove2;
     sleepSteps;
     wakeThreshold;
+    jointCapacity;
+    jointCorrection;
     posX;
     posY;
     posZ;
@@ -37,6 +44,8 @@ export class PhysicsWorld {
     halfZ;
     invMass;
     flags;
+    /** `SHAPE_BOX` (0) or `SHAPE_SPHERE` (1) per body; spheres keep their radius in halfX/halfY/halfZ. */
+    shape;
     /** 1 for a body that took part in a contact this frame; reset each `step()`. Render tint reads this. */
     contact;
     sleepTimer;
@@ -56,6 +65,41 @@ export class PhysicsWorld {
     awakeList;
     awakeSlot;
     awakeCount = 0;
+    jSpring;
+    jActive;
+    jBodyA;
+    jBodyB;
+    jAnchorAX;
+    jAnchorAY;
+    jAnchorAZ;
+    jAnchorBX;
+    jAnchorBY;
+    jAnchorBZ;
+    jRest;
+    jStiffness;
+    jDamping;
+    jMaxImpulse;
+    jFree;
+    jFreeCount = 0;
+    jointHigh = 0;
+    jointActiveCount = 0;
+    // Removed bodies tombstone (FLAG_DEAD) rather than compact: `id` is the SoA slot itself, handed
+    // out to callers and stored raw in joints/game state, so a slot can never move once allocated.
+    // `bodyHigh` is the high-water mark of ever-used slots; `bodyFree` recycles dead ones for addBody.
+    bodyFree;
+    bodyFreeCount = 0;
+    bodyHigh = 0;
+    collisionListener = null;
+    collisionMinSpeed = 0;
+    collisionEvent = {
+        a: 0,
+        b: 0,
+        nx: 0,
+        ny: 0,
+        nz: 0,
+        approachSpeed: 0,
+        impulse: 0,
+    };
     bodyCount = 0;
     accumulator = 0;
     stats = {
@@ -86,6 +130,8 @@ export class PhysicsWorld {
         this.sleepMove2 = sleepMoveEps * sleepMoveEps;
         this.sleepSteps = config.sleepThresholdSteps ?? 30;
         this.wakeThreshold = config.wakeThreshold ?? 0.5;
+        this.jointCapacity = Math.max(0, config.jointCapacity ?? 256);
+        this.jointCorrection = config.jointCorrection ?? 0.5;
         const cap = config.capacity;
         this.posX = new Float32Array(cap);
         this.posY = new Float32Array(cap);
@@ -98,6 +144,7 @@ export class PhysicsWorld {
         this.halfZ = new Float32Array(cap);
         this.invMass = new Float32Array(cap);
         this.flags = new Uint8Array(cap);
+        this.shape = new Uint8Array(cap);
         this.contact = new Uint8Array(cap);
         this.sleepTimer = new Uint16Array(cap);
         this.prevX = new Float32Array(cap);
@@ -108,12 +155,34 @@ export class PhysicsWorld {
         this.ny = Math.max(1, Math.ceil(span(1) / this.cellSize));
         this.nz = Math.max(1, Math.ceil(span(2) / this.cellSize));
         this.numCells = this.nx * this.ny * this.nz;
+        if (this.numCells > MAX_BROADPHASE_CELLS) {
+            throw new Error(`PhysicsWorld: broadphase grid too large (${this.nx}x${this.ny}x${this.nz} = ${this.numCells} cells, ` +
+                `cap ${MAX_BROADPHASE_CELLS}). bounds=[${config.bounds.min}]..[${config.bounds.max}] cellSize=${this.cellSize}; ` +
+                `raise cellSize or shrink bounds.`);
+        }
         this.cellStart = new Int32Array(this.numCells + 1);
         this.cursor = new Int32Array(this.numCells);
         this.sorted = new Int32Array(cap);
         this.bodyCell = new Int32Array(cap);
         this.awakeList = new Int32Array(cap);
         this.awakeSlot = new Int32Array(cap).fill(-1);
+        this.bodyFree = new Int32Array(cap);
+        const jc = this.jointCapacity;
+        this.jSpring = new Uint8Array(jc);
+        this.jActive = new Uint8Array(jc);
+        this.jBodyA = new Int32Array(jc);
+        this.jBodyB = new Int32Array(jc);
+        this.jAnchorAX = new Float32Array(jc);
+        this.jAnchorAY = new Float32Array(jc);
+        this.jAnchorAZ = new Float32Array(jc);
+        this.jAnchorBX = new Float32Array(jc);
+        this.jAnchorBY = new Float32Array(jc);
+        this.jAnchorBZ = new Float32Array(jc);
+        this.jRest = new Float32Array(jc);
+        this.jStiffness = new Float32Array(jc);
+        this.jDamping = new Float32Array(jc);
+        this.jMaxImpulse = new Float32Array(jc);
+        this.jFree = new Int32Array(jc);
     }
     addAwake(i) {
         if (this.awakeSlot[i] >= 0)
@@ -135,13 +204,133 @@ export class PhysicsWorld {
     get count() {
         return this.bodyCount;
     }
+    /** One past the highest slot `addBody` has ever handed out — the range `removeBody` leaves holes in. */
+    get highWater() {
+        return this.bodyHigh;
+    }
     get cells() {
         return { nx: this.nx, ny: this.ny, nz: this.nz, total: this.numCells };
     }
+    get jointCount() {
+        return this.jointActiveCount;
+    }
+    /**
+     * Deliver every impacting contact (approach speed ≥ `minApproachSpeed`) to `listener` during
+     * `step`. The event object is reused — read/copy it, never retain. Pass `null` to detach. This is
+     * the seam crash-damage and destruction read; contacts otherwise stay inside the sim.
+     */
+    onCollision(listener, minApproachSpeed = 0) {
+        this.collisionListener = listener;
+        this.collisionMinSpeed = minApproachSpeed;
+    }
+    hingeJoint(options) {
+        return this.addJoint(options, false, 0);
+    }
+    fixedJoint(options) {
+        return this.addJoint(options, false, 0);
+    }
+    distanceJoint(options) {
+        return this.addJoint(options, false, null);
+    }
+    springJoint(options) {
+        return this.addJoint(options, true, null);
+    }
+    addJoint(options, spring, restOverride) {
+        let id;
+        if (this.jFreeCount > 0) {
+            id = this.jFree[--this.jFreeCount];
+        }
+        else {
+            if (this.jointHigh >= this.jointCapacity)
+                throw new Error("PhysicsWorld: joint capacity exceeded");
+            id = this.jointHigh++;
+        }
+        const bodyB = options.bodyB ?? -1;
+        const aA = options.anchorA;
+        const aB = options.anchorB;
+        this.jBodyA[id] = options.bodyA;
+        this.jBodyB[id] = bodyB;
+        this.jAnchorAX[id] = aA?.[0] ?? 0;
+        this.jAnchorAY[id] = aA?.[1] ?? 0;
+        this.jAnchorAZ[id] = aA?.[2] ?? 0;
+        this.jAnchorBX[id] = aB?.[0] ?? 0;
+        this.jAnchorBY[id] = aB?.[1] ?? 0;
+        this.jAnchorBZ[id] = aB?.[2] ?? 0;
+        this.jSpring[id] = spring ? 1 : 0;
+        this.jStiffness[id] = options.stiffness ?? 40;
+        this.jDamping[id] = options.damping ?? 6;
+        this.jMaxImpulse[id] = options.maxImpulse ?? Number.POSITIVE_INFINITY;
+        this.jRest[id] = restOverride ?? options.restLength ?? this.jointDistance(id);
+        this.jActive[id] = 1;
+        this.jointActiveCount += 1;
+        return id;
+    }
+    removeJoint(id) {
+        if (this.jActive[id] !== 1)
+            return;
+        this.jActive[id] = 0;
+        this.jFree[this.jFreeCount++] = id;
+        this.jointActiveCount -= 1;
+    }
+    /** Move a joint's world anchor point (only meaningful when `bodyB` is a world anchor). */
+    setJointAnchor(id, x, y, z) {
+        this.jAnchorBX[id] = x;
+        this.jAnchorBY[id] = y;
+        this.jAnchorBZ[id] = z;
+    }
+    /** Move a joint's local anchor offset on A (world axes, no rotation applied by the solver). */
+    setJointAnchorA(id, x, y, z) {
+        this.jAnchorAX[id] = x;
+        this.jAnchorAY[id] = y;
+        this.jAnchorAZ[id] = z;
+    }
+    setJointRest(id, restLength) {
+        this.jRest[id] = restLength;
+    }
+    jointDistance(id) {
+        const bA = this.jBodyA[id];
+        const bB = this.jBodyB[id];
+        const ax = (bA >= 0 ? this.posX[bA] : 0) + this.jAnchorAX[id];
+        const ay = (bA >= 0 ? this.posY[bA] : 0) + this.jAnchorAY[id];
+        const az = (bA >= 0 ? this.posZ[bA] : 0) + this.jAnchorAZ[id];
+        const bx = bB >= 0 ? this.posX[bB] + this.jAnchorBX[id] : this.jAnchorBX[id];
+        const by = bB >= 0 ? this.posY[bB] + this.jAnchorBY[id] : this.jAnchorBY[id];
+        const bz = bB >= 0 ? this.posZ[bB] + this.jAnchorBZ[id] : this.jAnchorBZ[id];
+        const dx = bx - ax;
+        const dy = by - ay;
+        const dz = bz - az;
+        return Math.sqrt(dx * dx + dy * dy + dz * dz);
+    }
+    /** Write [ax,ay,az,bx,by,bz] world endpoints per active joint into `out`; returns the joint count. */
+    readJointSegments(out) {
+        let n = 0;
+        for (let id = 0; id < this.jointHigh; id += 1) {
+            if (this.jActive[id] !== 1)
+                continue;
+            const bA = this.jBodyA[id];
+            const bB = this.jBodyB[id];
+            const o = n * 6;
+            out[o] = (bA >= 0 ? this.posX[bA] : 0) + this.jAnchorAX[id];
+            out[o + 1] = (bA >= 0 ? this.posY[bA] : 0) + this.jAnchorAY[id];
+            out[o + 2] = (bA >= 0 ? this.posZ[bA] : 0) + this.jAnchorAZ[id];
+            out[o + 3] = bB >= 0 ? this.posX[bB] + this.jAnchorBX[id] : this.jAnchorBX[id];
+            out[o + 4] = bB >= 0 ? this.posY[bB] + this.jAnchorBY[id] : this.jAnchorBY[id];
+            out[o + 5] = bB >= 0 ? this.posZ[bB] + this.jAnchorBZ[id] : this.jAnchorBZ[id];
+            n += 1;
+        }
+        return n;
+    }
     addBody(options) {
-        if (this.bodyCount >= this.capacity)
-            throw new Error("PhysicsWorld: capacity exceeded");
-        const i = this.bodyCount++;
+        let i;
+        if (this.bodyFreeCount > 0) {
+            i = this.bodyFree[--this.bodyFreeCount];
+        }
+        else {
+            if (this.bodyHigh >= this.capacity)
+                throw new Error("PhysicsWorld: capacity exceeded");
+            i = this.bodyHigh++;
+        }
+        this.bodyCount += 1;
         this.posX[i] = options.position[0];
         this.posY[i] = options.position[1];
         this.posZ[i] = options.position[2];
@@ -149,9 +338,18 @@ export class PhysicsWorld {
         this.velX[i] = v?.[0] ?? 0;
         this.velY[i] = v?.[1] ?? 0;
         this.velZ[i] = v?.[2] ?? 0;
-        this.halfX[i] = options.halfExtents[0];
-        this.halfY[i] = options.halfExtents[1];
-        this.halfZ[i] = options.halfExtents[2];
+        if (options.shape === "sphere") {
+            this.halfX[i] = options.radius;
+            this.halfY[i] = options.radius;
+            this.halfZ[i] = options.radius;
+            this.shape[i] = SHAPE_SPHERE;
+        }
+        else {
+            this.halfX[i] = options.halfExtents[0];
+            this.halfY[i] = options.halfExtents[1];
+            this.halfZ[i] = options.halfExtents[2];
+            this.shape[i] = SHAPE_BOX;
+        }
         const isStatic = options.static === true;
         const mass = options.mass ?? 1;
         this.invMass[i] = isStatic || mass <= 0 ? 0 : 1 / mass;
@@ -162,17 +360,69 @@ export class PhysicsWorld {
             f |= FLAG_SLEEPING;
         this.flags[i] = f;
         this.sleepTimer[i] = 0;
+        this.contact[i] = 0;
         this.awakeSlot[i] = -1;
         this.bodyCell[i] = this.cellOf(i);
         if ((f & FLAG_SLEEPING) === 0)
             this.addAwake(i);
         return i;
     }
+    /**
+     * Remove a body: it drops out of integration, the solver, and the broadphase, and its slot is
+     * queued for the next `addBody` to reuse. `id` and every other body's `id` stay valid — a dead
+     * slot is tombstoned (`FLAG_DEAD`), never swapped, since ids are handed out raw and stored as-is
+     * in joints and game state. There is no persistent contact set to consult, so — conservatively —
+     * any sleeping body whose AABB touches the removed body's (padded by `slop`) is woken; a body
+     * that was resting on it always qualifies.
+     */
+    removeBody(id) {
+        if (id < 0 || id >= this.bodyHigh)
+            return;
+        if ((this.flags[id] & FLAG_DEAD) !== 0)
+            return;
+        this.wakeTouching(id);
+        this.removeAwake(id);
+        this.flags[id] = FLAG_DEAD;
+        this.invMass[id] = 0;
+        this.velX[id] = 0;
+        this.velY[id] = 0;
+        this.velZ[id] = 0;
+        this.sleepTimer[id] = 0;
+        this.contact[id] = 0;
+        this.bodyCount -= 1;
+        this.bodyFree[this.bodyFreeCount++] = id;
+    }
+    wakeTouching(id) {
+        const pad = this.slop + 0.01;
+        const minX = this.posX[id] - this.halfX[id] - pad;
+        const maxX = this.posX[id] + this.halfX[id] + pad;
+        const minY = this.posY[id] - this.halfY[id] - pad;
+        const maxY = this.posY[id] + this.halfY[id] + pad;
+        const minZ = this.posZ[id] - this.halfZ[id] - pad;
+        const maxZ = this.posZ[id] + this.halfZ[id] + pad;
+        for (let j = 0; j < this.bodyHigh; j += 1) {
+            if (j === id)
+                continue;
+            const fj = this.flags[j];
+            if ((fj & (FLAG_DEAD | FLAG_STATIC)) !== 0 || (fj & FLAG_SLEEPING) === 0)
+                continue;
+            if (this.posX[j] - this.halfX[j] > maxX || this.posX[j] + this.halfX[j] < minX)
+                continue;
+            if (this.posY[j] - this.halfY[j] > maxY || this.posY[j] + this.halfY[j] < minY)
+                continue;
+            if (this.posZ[j] - this.halfZ[j] > maxZ || this.posZ[j] + this.halfZ[j] < minZ)
+                continue;
+            this.wake(j);
+        }
+    }
     isSleeping(i) {
         return (this.flags[i] & FLAG_SLEEPING) !== 0;
     }
+    isAlive(i) {
+        return (this.flags[i] & FLAG_DEAD) === 0;
+    }
     wake(i) {
-        if ((this.flags[i] & FLAG_STATIC) !== 0)
+        if ((this.flags[i] & (FLAG_STATIC | FLAG_DEAD)) !== 0)
             return;
         this.flags[i] &= ~FLAG_SLEEPING;
         this.sleepTimer[i] = 0;
@@ -181,9 +431,40 @@ export class PhysicsWorld {
         this.prevZ[i] = this.posZ[i];
         this.addAwake(i);
     }
+    /** Overwrite a body's velocity directly, waking it if asleep. */
+    setVelocity(id, x, y, z) {
+        if ((this.flags[id] & FLAG_DEAD) !== 0)
+            return;
+        this.velX[id] = x;
+        this.velY[id] = y;
+        this.velZ[id] = z;
+        this.wake(id);
+    }
+    /** Move a body, keeping its current velocity, and wake it if asleep. */
+    setPosition(id, x, y, z) {
+        if ((this.flags[id] & FLAG_DEAD) !== 0)
+            return;
+        this.posX[id] = x;
+        this.posY[id] = y;
+        this.posZ[id] = z;
+        this.prevX[id] = x;
+        this.prevY[id] = y;
+        this.prevZ[id] = z;
+        this.bodyCell[id] = this.cellOf(id);
+        this.wake(id);
+    }
+    /** Move a body and zero its velocity — a hard reset (respawn, teleporter) instead of a slide. */
+    teleport(id, x, y, z) {
+        this.setPosition(id, x, y, z);
+        if ((this.flags[id] & FLAG_DEAD) !== 0)
+            return;
+        this.velX[id] = 0;
+        this.velY[id] = 0;
+        this.velZ[id] = 0;
+    }
     wakeAll() {
-        for (let i = 0; i < this.bodyCount; i += 1) {
-            if ((this.flags[i] & FLAG_STATIC) === 0) {
+        for (let i = 0; i < this.bodyHigh; i += 1) {
+            if ((this.flags[i] & (FLAG_STATIC | FLAG_DEAD)) === 0) {
                 this.flags[i] &= ~FLAG_SLEEPING;
                 this.sleepTimer[i] = 0;
                 this.addAwake(i);
@@ -192,8 +473,13 @@ export class PhysicsWorld {
     }
     clear() {
         this.bodyCount = 0;
+        this.bodyHigh = 0;
+        this.bodyFreeCount = 0;
         this.accumulator = 0;
         this.awakeCount = 0;
+        this.jointHigh = 0;
+        this.jFreeCount = 0;
+        this.jointActiveCount = 0;
     }
     /** Advance by a real frame delta; runs whole fixed substeps and carries the remainder. */
     step(frameDt) {
@@ -205,7 +491,7 @@ export class PhysicsWorld {
         let substeps = 0;
         this.stats.contacts = 0;
         this.stats.pairs = 0;
-        this.contact.fill(0, 0, this.bodyCount);
+        this.contact.fill(0, 0, this.bodyHigh);
         while (this.accumulator >= this.fixedDt) {
             this.substep(this.fixedDt);
             this.accumulator -= this.fixedDt;
@@ -243,8 +529,11 @@ export class PhysicsWorld {
         }
         this.buildGrid();
         const iterations = this.solverIterations;
-        for (let it = 0; it < iterations; it += 1)
+        for (let it = 0; it < iterations; it += 1) {
             this.solve(it === 0, it === iterations - 1);
+            if (this.jointActiveCount > 0)
+                this.solveJoints(dt);
+        }
         for (let a = 0; a < this.awakeCount; a += 1)
             this.constrainBounds(this.awakeList[a]);
         this.updateSleep();
@@ -295,7 +584,7 @@ export class PhysicsWorld {
         }
     }
     buildGrid() {
-        const n = this.bodyCount;
+        const n = this.bodyHigh;
         const start = this.cellStart;
         start.fill(0);
         // Sleeping bodies never move, so their cell index is cached from addBody / their last awake
@@ -305,13 +594,19 @@ export class PhysicsWorld {
             const i = this.awakeList[a];
             this.bodyCell[i] = this.cellOf(i);
         }
-        for (let i = 0; i < n; i += 1)
+        // Dead slots (tombstoned by removeBody) sit inside [0, bodyHigh) but never enter the grid.
+        for (let i = 0; i < n; i += 1) {
+            if ((this.flags[i] & FLAG_DEAD) !== 0)
+                continue;
             start[this.bodyCell[i] + 1] += 1;
+        }
         for (let c = 0; c < this.numCells; c += 1) {
             start[c + 1] += start[c];
             this.cursor[c] = start[c];
         }
         for (let i = 0; i < n; i += 1) {
+            if ((this.flags[i] & FLAG_DEAD) !== 0)
+                continue;
             const c = this.bodyCell[i];
             this.sorted[this.cursor[c]++] = i;
         }
@@ -362,39 +657,94 @@ export class PhysicsWorld {
         }
     }
     resolve(i, j, restitutionPass, countPass) {
-        const dx = this.posX[j] - this.posX[i];
-        const px = this.halfX[i] + this.halfX[j] - Math.abs(dx);
-        if (px <= 0)
-            return;
-        const dy = this.posY[j] - this.posY[i];
-        const py = this.halfY[i] + this.halfY[j] - Math.abs(dy);
-        if (py <= 0)
-            return;
-        const dz = this.posZ[j] - this.posZ[i];
-        const pz = this.halfZ[i] + this.halfZ[j] - Math.abs(dz);
-        if (pz <= 0)
-            return;
-        if (countPass) {
-            this.stats.pairs += 1;
-            this.stats.contacts += 1;
-            this.contact[i] = 1;
-            this.contact[j] = 1;
-        }
         let nX = 0;
         let nY = 0;
         let nZ = 0;
-        let pen;
-        if (px <= py && px <= pz) {
-            nX = dx < 0 ? -1 : 1;
-            pen = px;
+        let pen = 0;
+        const sphereI = this.shape[i] === SHAPE_SPHERE;
+        const sphereJ = this.shape[j] === SHAPE_SPHERE;
+        let needsAxisNormal = true;
+        if (sphereI && sphereJ) {
+            const dx = this.posX[j] - this.posX[i];
+            const dy = this.posY[j] - this.posY[i];
+            const dz = this.posZ[j] - this.posZ[i];
+            const radiusSum = this.halfX[i] + this.halfX[j];
+            const dist2 = dx * dx + dy * dy + dz * dz;
+            if (dist2 >= radiusSum * radiusSum)
+                return;
+            const dist = Math.sqrt(dist2);
+            if (dist > 1e-9) {
+                nX = dx / dist;
+                nY = dy / dist;
+                nZ = dz / dist;
+                pen = radiusSum - dist;
+            }
+            else {
+                nY = 1;
+                pen = radiusSum;
+            }
+            needsAxisNormal = false;
         }
-        else if (py <= pz) {
-            nY = dy < 0 ? -1 : 1;
-            pen = py;
+        else if (sphereI !== sphereJ) {
+            const box = sphereI ? j : i;
+            const sph = sphereI ? i : j;
+            const loX = this.posX[box] - this.halfX[box];
+            const hiX = this.posX[box] + this.halfX[box];
+            const loY = this.posY[box] - this.halfY[box];
+            const hiY = this.posY[box] + this.halfY[box];
+            const loZ = this.posZ[box] - this.halfZ[box];
+            const hiZ = this.posZ[box] + this.halfZ[box];
+            const sx = this.posX[sph];
+            const sy = this.posY[sph];
+            const sz = this.posZ[sph];
+            const dx = sx - (sx < loX ? loX : sx > hiX ? hiX : sx);
+            const dy = sy - (sy < loY ? loY : sy > hiY ? hiY : sy);
+            const dz = sz - (sz < loZ ? loZ : sz > hiZ ? hiZ : sz);
+            const dist2 = dx * dx + dy * dy + dz * dz;
+            if (dist2 > 1e-12) {
+                const radius = this.halfX[sph];
+                if (dist2 >= radius * radius)
+                    return;
+                const dist = Math.sqrt(dist2);
+                const towardSphere = sph === j ? 1 / dist : -1 / dist;
+                nX = dx * towardSphere;
+                nY = dy * towardSphere;
+                nZ = dz * towardSphere;
+                pen = radius - dist;
+                needsAxisNormal = false;
+            }
         }
-        else {
-            nZ = dz < 0 ? -1 : 1;
-            pen = pz;
+        if (needsAxisNormal) {
+            const dx = this.posX[j] - this.posX[i];
+            const px = this.halfX[i] + this.halfX[j] - Math.abs(dx);
+            if (px <= 0)
+                return;
+            const dy = this.posY[j] - this.posY[i];
+            const py = this.halfY[i] + this.halfY[j] - Math.abs(dy);
+            if (py <= 0)
+                return;
+            const dz = this.posZ[j] - this.posZ[i];
+            const pz = this.halfZ[i] + this.halfZ[j] - Math.abs(dz);
+            if (pz <= 0)
+                return;
+            if (px <= py && px <= pz) {
+                nX = dx < 0 ? -1 : 1;
+                pen = px;
+            }
+            else if (py <= pz) {
+                nY = dy < 0 ? -1 : 1;
+                pen = py;
+            }
+            else {
+                nZ = dz < 0 ? -1 : 1;
+                pen = pz;
+            }
+        }
+        if (countPass) {
+            this.stats.pairs += 1;
+            this.stats.contacts += 1;
+            this.contact[i] = 1;
+            this.contact[j] = 1;
         }
         const rvx = this.velX[j] - this.velX[i];
         const rvy = this.velY[j] - this.velY[i];
@@ -416,6 +766,17 @@ export class PhysicsWorld {
         if (rvn < 0) {
             const e = restitutionPass && -rvn > this.restitutionThreshold ? this.restitution : 0;
             const jn = (-(1 + e) * rvn) / imSum;
+            if (restitutionPass && this.collisionListener !== null && -rvn >= this.collisionMinSpeed) {
+                const ev = this.collisionEvent;
+                ev.a = i;
+                ev.b = j;
+                ev.nx = nX;
+                ev.ny = nY;
+                ev.nz = nZ;
+                ev.approachSpeed = -rvn;
+                ev.impulse = jn;
+                this.collisionListener(ev);
+            }
             this.velX[i] -= jn * imI * nX;
             this.velY[i] -= jn * imI * nY;
             this.velZ[i] -= jn * imI * nZ;
@@ -452,6 +813,97 @@ export class PhysicsWorld {
         this.posY[j] += corr * imJ * nY;
         this.posZ[j] += corr * imJ * nZ;
     }
+    solveJoints(dt) {
+        const beta = this.jointCorrection;
+        const slop = this.slop;
+        for (let id = 0; id < this.jointHigh; id += 1) {
+            if (this.jActive[id] !== 1)
+                continue;
+            const bA = this.jBodyA[id];
+            const bB = this.jBodyB[id];
+            const ax = (bA >= 0 ? this.posX[bA] : 0) + this.jAnchorAX[id];
+            const ay = (bA >= 0 ? this.posY[bA] : 0) + this.jAnchorAY[id];
+            const az = (bA >= 0 ? this.posZ[bA] : 0) + this.jAnchorAZ[id];
+            const bx = bB >= 0 ? this.posX[bB] + this.jAnchorBX[id] : this.jAnchorBX[id];
+            const by = bB >= 0 ? this.posY[bB] + this.jAnchorBY[id] : this.jAnchorBY[id];
+            const bz = bB >= 0 ? this.posZ[bB] + this.jAnchorBZ[id] : this.jAnchorBZ[id];
+            const dx = bx - ax;
+            const dy = by - ay;
+            const dz = bz - az;
+            const dist = Math.sqrt(dx * dx + dy * dy + dz * dz);
+            if (dist < 1e-9)
+                continue;
+            const nX = dx / dist;
+            const nY = dy / dist;
+            const nZ = dz / dist;
+            const c = dist - this.jRest[id];
+            const imA = bA >= 0 ? this.invMass[bA] : 0;
+            const imB = bB >= 0 ? this.invMass[bB] : 0;
+            const imSum = imA + imB;
+            if (imSum === 0)
+                continue;
+            const vax = bA >= 0 ? this.velX[bA] : 0;
+            const vay = bA >= 0 ? this.velY[bA] : 0;
+            const vaz = bA >= 0 ? this.velZ[bA] : 0;
+            const vbx = bB >= 0 ? this.velX[bB] : 0;
+            const vby = bB >= 0 ? this.velY[bB] : 0;
+            const vbz = bB >= 0 ? this.velZ[bB] : 0;
+            const rvn = (vbx - vax) * nX + (vby - vay) * nY + (vbz - vaz) * nZ;
+            if (this.jSpring[id] === 1) {
+                if (Math.abs(c) > slop) {
+                    if (bA >= 0)
+                        this.wake(bA);
+                    if (bB >= 0)
+                        this.wake(bB);
+                }
+                let jn = (-(this.jStiffness[id] * c + this.jDamping[id] * rvn) * dt) / imSum;
+                const maxJ = this.jMaxImpulse[id];
+                if (jn > maxJ)
+                    jn = maxJ;
+                else if (jn < -maxJ)
+                    jn = -maxJ;
+                if (bA >= 0) {
+                    this.velX[bA] -= jn * imA * nX;
+                    this.velY[bA] -= jn * imA * nY;
+                    this.velZ[bA] -= jn * imA * nZ;
+                }
+                if (bB >= 0) {
+                    this.velX[bB] += jn * imB * nX;
+                    this.velY[bB] += jn * imB * nY;
+                    this.velZ[bB] += jn * imB * nZ;
+                }
+                continue;
+            }
+            if (Math.abs(c) > slop) {
+                if (bA >= 0)
+                    this.wake(bA);
+                if (bB >= 0)
+                    this.wake(bB);
+            }
+            const jn = -rvn / imSum;
+            if (bA >= 0) {
+                this.velX[bA] -= jn * imA * nX;
+                this.velY[bA] -= jn * imA * nY;
+                this.velZ[bA] -= jn * imA * nZ;
+            }
+            if (bB >= 0) {
+                this.velX[bB] += jn * imB * nX;
+                this.velY[bB] += jn * imB * nY;
+                this.velZ[bB] += jn * imB * nZ;
+            }
+            const corr = (c * beta) / imSum;
+            if (bA >= 0) {
+                this.posX[bA] += corr * imA * nX;
+                this.posY[bA] += corr * imA * nY;
+                this.posZ[bA] += corr * imA * nZ;
+            }
+            if (bB >= 0) {
+                this.posX[bB] -= corr * imB * nX;
+                this.posY[bB] -= corr * imB * nY;
+                this.posZ[bB] -= corr * imB * nZ;
+            }
+        }
+    }
     updateSleep() {
         // A body sleeps when it has actually stopped moving — measured by displacement over
         // the substep, not instantaneous velocity, which can stay "stuck" nonzero on a body