@jgengine/core 0.7.0 → 0.9.0

package/dist/physics/physicsWorld.js

@@ -1,5 +1,14 @@
+export const SHAPE_BOX = 0;
+export const SHAPE_SPHERE = 1;
+export const JOINT_FIXED = 0;
+export const JOINT_HINGE = 1;
+export const JOINT_DISTANCE = 2;
+export const JOINT_SPRING = 3;
 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);
@@ -39,6 +48,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;
@@ -58,7 +69,7 @@ export class PhysicsWorld {
     awakeList;
     awakeSlot;
     awakeCount = 0;
-    jSpring;
+    jKind;
     jActive;
     jBodyA;
     jBodyB;
@@ -68,6 +79,9 @@ export class PhysicsWorld {
     jAnchorBX;
     jAnchorBY;
     jAnchorBZ;
+    jAxisX;
+    jAxisY;
+    jAxisZ;
     jRest;
     jStiffness;
     jDamping;
@@ -76,6 +90,12 @@ export class PhysicsWorld {
     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 = {
@@ -131,6 +151,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);
@@ -141,14 +162,20 @@ 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.jKind = new Uint8Array(jc);
         this.jActive = new Uint8Array(jc);
         this.jBodyA = new Int32Array(jc);
         this.jBodyB = new Int32Array(jc);
@@ -158,6 +185,9 @@ export class PhysicsWorld {
         this.jAnchorBX = new Float32Array(jc);
         this.jAnchorBY = new Float32Array(jc);
         this.jAnchorBZ = new Float32Array(jc);
+        this.jAxisX = new Float32Array(jc);
+        this.jAxisY = new Float32Array(jc);
+        this.jAxisZ = new Float32Array(jc);
         this.jRest = new Float32Array(jc);
         this.jStiffness = new Float32Array(jc);
         this.jDamping = new Float32Array(jc);
@@ -184,6 +214,10 @@ 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 };
     }
@@ -200,18 +234,25 @@ export class PhysicsWorld {
         this.collisionMinSpeed = minApproachSpeed;
     }
     hingeJoint(options) {
-        return this.addJoint(options, false, 0);
+        const raw = options.axis ?? [0, 1, 0];
+        const len = Math.hypot(raw[0], raw[1], raw[2]);
+        if (!(len > 1e-9))
+            throw new Error("PhysicsWorld: hingeJoint axis must be non-zero");
+        return this.addJoint(options, JOINT_HINGE, 0, [raw[0] / len, raw[1] / len, raw[2] / len]);
     }
     fixedJoint(options) {
-        return this.addJoint(options, false, 0);
+        return this.addJoint(options, JOINT_FIXED, 0, null);
     }
     distanceJoint(options) {
-        return this.addJoint(options, false, null);
+        return this.addJoint(options, JOINT_DISTANCE, null, null);
     }
     springJoint(options) {
-        return this.addJoint(options, true, null);
+        return this.addJoint(options, JOINT_SPRING, null, null);
     }
-    addJoint(options, spring, restOverride) {
+    addJoint(options, kind, restOverride, axis) {
+        if (kind !== JOINT_FIXED && kind !== JOINT_HINGE && kind !== JOINT_DISTANCE && kind !== JOINT_SPRING) {
+            throw new Error(`PhysicsWorld: joint kind ${kind} is not implemented`);
+        }
         let id;
         if (this.jFreeCount > 0) {
             id = this.jFree[--this.jFreeCount];
@@ -232,7 +273,10 @@ export class PhysicsWorld {
         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.jKind[id] = kind;
+        this.jAxisX[id] = axis?.[0] ?? 0;
+        this.jAxisY[id] = axis?.[1] ?? 0;
+        this.jAxisZ[id] = axis?.[2] ?? 0;
         this.jStiffness[id] = options.stiffness ?? 40;
         this.jDamping[id] = options.damping ?? 6;
         this.jMaxImpulse[id] = options.maxImpulse ?? Number.POSITIVE_INFINITY;
@@ -254,6 +298,12 @@ export class PhysicsWorld {
         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;
     }
@@ -291,9 +341,16 @@ export class PhysicsWorld {
         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];
@@ -301,9 +358,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;
@@ -314,17 +380,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;
@@ -333,9 +451,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);
@@ -344,6 +493,8 @@ export class PhysicsWorld {
     }
     clear() {
         this.bodyCount = 0;
+        this.bodyHigh = 0;
+        this.bodyFreeCount = 0;
         this.accumulator = 0;
         this.awakeCount = 0;
         this.jointHigh = 0;
@@ -360,7 +511,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;
@@ -453,25 +604,59 @@ 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
-        // substep — only awake bodies pay the cell recomputation each substep.
         const awakeCount = this.awakeCount;
         for (let a = 0; a < awakeCount; a += 1) {
             const i = this.awakeList[a];
             this.bodyCell[i] = this.cellOf(i);
         }
-        for (let i = 0; i < n; i += 1)
-            start[this.bodyCell[i] + 1] += 1;
+        const nx = this.nx;
+        const ny = this.ny;
+        for (let i = 0; i < n; i += 1) {
+            if ((this.flags[i] & FLAG_DEAD) !== 0)
+                continue;
+            const x0 = cellCoord(this.posX[i] - this.halfX[i], this.bounds.min[0], this.cellSize, this.nx);
+            const x1 = cellCoord(this.posX[i] + this.halfX[i], this.bounds.min[0], this.cellSize, this.nx);
+            const y0 = cellCoord(this.posY[i] - this.halfY[i], this.bounds.min[1], this.cellSize, this.ny);
+            const y1 = cellCoord(this.posY[i] + this.halfY[i], this.bounds.min[1], this.cellSize, this.ny);
+            const z0 = cellCoord(this.posZ[i] - this.halfZ[i], this.bounds.min[2], this.cellSize, this.nz);
+            const z1 = cellCoord(this.posZ[i] + this.halfZ[i], this.bounds.min[2], this.cellSize, this.nz);
+            for (let z = z0; z <= z1; z += 1) {
+                for (let y = y0; y <= y1; y += 1) {
+                    const rowBase = (z * ny + y) * nx;
+                    for (let x = x0; x <= x1; x += 1)
+                        start[rowBase + x + 1] += 1;
+                }
+            }
+        }
         for (let c = 0; c < this.numCells; c += 1) {
             start[c + 1] += start[c];
             this.cursor[c] = start[c];
         }
+        const total = start[this.numCells];
+        if (total > this.sorted.length) {
+            this.sorted = new Int32Array(Math.max(total, this.sorted.length * 2));
+        }
         for (let i = 0; i < n; i += 1) {
-            const c = this.bodyCell[i];
-            this.sorted[this.cursor[c]++] = i;
+            if ((this.flags[i] & FLAG_DEAD) !== 0)
+                continue;
+            const x0 = cellCoord(this.posX[i] - this.halfX[i], this.bounds.min[0], this.cellSize, this.nx);
+            const x1 = cellCoord(this.posX[i] + this.halfX[i], this.bounds.min[0], this.cellSize, this.nx);
+            const y0 = cellCoord(this.posY[i] - this.halfY[i], this.bounds.min[1], this.cellSize, this.ny);
+            const y1 = cellCoord(this.posY[i] + this.halfY[i], this.bounds.min[1], this.cellSize, this.ny);
+            const z0 = cellCoord(this.posZ[i] - this.halfZ[i], this.bounds.min[2], this.cellSize, this.nz);
+            const z1 = cellCoord(this.posZ[i] + this.halfZ[i], this.bounds.min[2], this.cellSize, this.nz);
+            for (let z = z0; z <= z1; z += 1) {
+                for (let y = y0; y <= y1; y += 1) {
+                    const rowBase = (z * ny + y) * nx;
+                    for (let x = x0; x <= x1; x += 1) {
+                        const c = rowBase + x;
+                        this.sorted[this.cursor[c]++] = i;
+                    }
+                }
+            }
         }
     }
     cellOf(i) {
@@ -483,26 +668,18 @@ export class PhysicsWorld {
     solve(restitutionPass, countPass) {
         const nx = this.nx;
         const ny = this.ny;
-        const nz = this.nz;
-        const nxny = nx * ny;
-        // A contact may wake a sleeping body mid-pass, appending it to the awake list; re-read the
-        // count each iteration so it is solved this substep too.
         for (let a = 0; a < this.awakeCount; a += 1) {
             const i = this.awakeList[a];
-            const c = this.bodyCell[i];
-            const cz = (c / nxny) | 0;
-            const cy = ((c - cz * nxny) / nx) | 0;
-            const cx = c - cz * nxny - cy * nx;
-            const z0 = cz > 0 ? cz - 1 : cz;
-            const z1 = cz < nz - 1 ? cz + 1 : cz;
-            const y0 = cy > 0 ? cy - 1 : cy;
-            const y1 = cy < ny - 1 ? cy + 1 : cy;
-            const x0 = cx > 0 ? cx - 1 : cx;
-            const x1 = cx < nx - 1 ? cx + 1 : cx;
-            for (let z = z0; z <= z1; z += 1) {
-                for (let y = y0; y <= y1; y += 1) {
+            const ix0 = cellCoord(this.posX[i] - this.halfX[i], this.bounds.min[0], this.cellSize, this.nx);
+            const ix1 = cellCoord(this.posX[i] + this.halfX[i], this.bounds.min[0], this.cellSize, this.nx);
+            const iy0 = cellCoord(this.posY[i] - this.halfY[i], this.bounds.min[1], this.cellSize, this.ny);
+            const iy1 = cellCoord(this.posY[i] + this.halfY[i], this.bounds.min[1], this.cellSize, this.ny);
+            const iz0 = cellCoord(this.posZ[i] - this.halfZ[i], this.bounds.min[2], this.cellSize, this.nz);
+            const iz1 = cellCoord(this.posZ[i] + this.halfZ[i], this.bounds.min[2], this.cellSize, this.nz);
+            for (let z = iz0; z <= iz1; z += 1) {
+                for (let y = iy0; y <= iy1; y += 1) {
                     const rowBase = (z * ny + y) * nx;
-                    for (let x = x0; x <= x1; x += 1) {
+                    for (let x = ix0; x <= ix1; x += 1) {
                         const cell = rowBase + x;
                         const end = this.cellStart[cell + 1];
                         for (let s = this.cellStart[cell]; s < end; s += 1) {
@@ -512,6 +689,14 @@ export class PhysicsWorld {
                             const jSleeping = (this.flags[j] & FLAG_SLEEPING) !== 0;
                             if (!jSleeping && j < i)
                                 continue;
+                            const jx0 = cellCoord(this.posX[j] - this.halfX[j], this.bounds.min[0], this.cellSize, this.nx);
+                            const jy0 = cellCoord(this.posY[j] - this.halfY[j], this.bounds.min[1], this.cellSize, this.ny);
+                            const jz0 = cellCoord(this.posZ[j] - this.halfZ[j], this.bounds.min[2], this.cellSize, this.nz);
+                            const mx0 = ix0 > jx0 ? ix0 : jx0;
+                            const my0 = iy0 > jy0 ? iy0 : jy0;
+                            const mz0 = iz0 > jz0 ? iz0 : jz0;
+                            if (x !== mx0 || y !== my0 || z !== mz0)
+                                continue;
                             this.resolve(i, j, restitutionPass, countPass);
                         }
                     }
@@ -520,39 +705,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];
@@ -627,6 +867,7 @@ export class PhysicsWorld {
         for (let id = 0; id < this.jointHigh; id += 1) {
             if (this.jActive[id] !== 1)
                 continue;
+            const kind = this.jKind[id];
             const bA = this.jBodyA[id];
             const bB = this.jBodyB[id];
             const ax = (bA >= 0 ? this.posX[bA] : 0) + this.jAnchorAX[id];
@@ -635,9 +876,91 @@ export class PhysicsWorld {
             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;
+            let dx = bx - ax;
+            let dy = by - ay;
+            let dz = bz - az;
+            const imA = bA >= 0 ? this.invMass[bA] : 0;
+            const imB = bB >= 0 ? this.invMass[bB] : 0;
+            const imSum = imA + imB;
+            if (imSum === 0)
+                continue;
+            if (kind === JOINT_HINGE) {
+                const ux = this.jAxisX[id];
+                const uy = this.jAxisY[id];
+                const uz = this.jAxisZ[id];
+                const along = dx * ux + dy * uy + dz * uz;
+                dx -= along * ux;
+                dy -= along * uy;
+                dz -= along * uz;
+                const dist = Math.sqrt(dx * dx + dy * dy + dz * dz);
+                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;
+                let rvx = vbx - vax;
+                let rvy = vby - vay;
+                let rvz = vbz - vaz;
+                const rvAlong = rvx * ux + rvy * uy + rvz * uz;
+                rvx -= rvAlong * ux;
+                rvy -= rvAlong * uy;
+                rvz -= rvAlong * uz;
+                if (dist > 1e-9) {
+                    const nX = dx / dist;
+                    const nY = dy / dist;
+                    const nZ = dz / dist;
+                    if (dist > slop) {
+                        if (bA >= 0)
+                            this.wake(bA);
+                        if (bB >= 0)
+                            this.wake(bB);
+                    }
+                    const rvn = rvx * nX + rvy * nY + rvz * nZ;
+                    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 = (dist * 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;
+                    }
+                }
+                else {
+                    const rvl = Math.sqrt(rvx * rvx + rvy * rvy + rvz * rvz);
+                    if (rvl > 1e-9) {
+                        const nX = rvx / rvl;
+                        const nY = rvy / rvl;
+                        const nZ = rvz / rvl;
+                        const jn = -rvl / 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;
+                        }
+                    }
+                }
+                continue;
+            }
             const dist = Math.sqrt(dx * dx + dy * dy + dz * dz);
             if (dist < 1e-9)
                 continue;
@@ -645,11 +968,6 @@ export class PhysicsWorld {
             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;
@@ -657,7 +975,7 @@ export class PhysicsWorld {
             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 (kind === JOINT_SPRING) {
                 if (Math.abs(c) > slop) {
                     if (bA >= 0)
                         this.wake(bA);
@@ -682,6 +1000,9 @@ export class PhysicsWorld {
                 }
                 continue;
             }
+            if (kind !== JOINT_FIXED && kind !== JOINT_DISTANCE) {
+                throw new Error(`PhysicsWorld: joint kind ${kind} is not implemented`);
+            }
             if (Math.abs(c) > slop) {
                 if (bA >= 0)
                     this.wake(bA);