@series-inc/rundot-syncplay 5.23.0 → 5.24.0-beta.1

package/dist/cjs/physics3d-solver.js

@@ -14,8 +14,13 @@
 Object.defineProperty(exports, "__esModule", { value: true });
 exports.__solverInternals = exports.WARM_KEY_BASE = exports.SOLVER_CONSTANTS = void 0;
 exports.generateManifold = generateManifold;
+exports.combinedContactFriction = combinedContactFriction;
 exports.solveRigidStep = solveRigidStep;
+exports.minExtentOf = minExtentOf;
+exports.boundingRadiusOf = boundingRadiusOf;
 const physics3d_shared_js_1 = require("./physics3d-shared.js");
+const physics3d_hull_js_1 = require("./physics3d-hull.js");
+const physics3d_heightfield_js_1 = require("./physics3d-heightfield.js");
 const ZERO = { x: 0, y: 0, z: 0 };
 /** Expand a compound body into primitive sub-bodies in world space. */
 function primitiveViews(body) {
@@ -29,7 +34,7 @@ function primitiveViews(body) {
         }));
     }
     /* istanbul ignore next -- mesh bodies are routed to meshManifold before primitiveViews is ever reached; this branch only satisfies the SolverShape union. */
-    if (shape.kind === 'mesh') {
+    if (shape.kind === 'mesh' || shape.kind === 'heightfield') {
         return [];
     }
     return [{ index: body.index, p: body.p, q: body.q, shape }];
@@ -43,7 +48,7 @@ function primitiveViews(body) {
  */
 function asPrimitive(shape) {
     /* istanbul ignore next -- callers guard compound, and mesh bodies are routed to meshManifold first; type guard only. */
-    if (shape.kind === 'compound' || shape.kind === 'mesh') {
+    if (shape.kind === 'compound' || shape.kind === 'mesh' || shape.kind === 'heightfield') {
         throw new Error('asPrimitive requires a primitive shape');
     }
     return shape;
@@ -51,7 +56,7 @@ function asPrimitive(shape) {
 function generateManifold(a, b, speculativeMargin = 0) {
     // mesh is one-sided static geometry; the other shape's primitives collide
     // against each triangle.
-    if (a.shape.kind === 'mesh' || b.shape.kind === 'mesh') {
+    if (a.shape.kind === 'mesh' || b.shape.kind === 'mesh' || a.shape.kind === 'heightfield' || b.shape.kind === 'heightfield') {
         return meshManifold(a, b, speculativeMargin);
     }
     if (a.shape.kind !== 'compound' && b.shape.kind !== 'compound') {
@@ -128,17 +133,38 @@ function collidePrimitives(a, b, centerA, centerB, speculativeMargin, featureBas
     if (ka === 'capsule' && kb === 'box') {
         return capsuleBox(a, b, centerA, centerB, speculativeMargin, featureBase, false);
     }
-    /* istanbul ignore next -- when a box operand reaches this point kb is necessarily 'capsule' (box-box/box-sphere returned earlier), so the second condition is always true. */
     if (ka === 'box' && kb === 'capsule') {
         return capsuleBox(b, a, centerB, centerA, speculativeMargin, featureBase, true);
     }
-    /* istanbul ignore next -- every primitive shape pairing is enumerated above; this is an exhaustiveness guard. */
+    if (ka === 'hull' && kb === 'hull') {
+        return collideHulls(a, b, centerA, centerB, speculativeMargin, featureBase);
+    }
+    if (ka === 'hull' && kb === 'box') {
+        return collideHulls(a, boxAsHull(b), centerA, centerB, speculativeMargin, featureBase);
+    }
+    if (ka === 'box' && kb === 'hull') {
+        return collideHulls(boxAsHull(a), b, centerA, centerB, speculativeMargin, featureBase);
+    }
+    if (ka === 'hull' && kb === 'sphere') {
+        return sphereHull(b, a, centerB, centerA, speculativeMargin, featureBase, true);
+    }
+    if (ka === 'sphere' && kb === 'hull') {
+        return sphereHull(a, b, centerA, centerB, speculativeMargin, featureBase, false);
+    }
+    if (ka === 'hull' && kb === 'capsule') {
+        return capsuleHull(b, a, centerB, centerA, speculativeMargin, featureBase, true);
+    }
+    /* istanbul ignore else -- every primitive shape pairing is enumerated above; the fallthrough is an exhaustiveness guard. */
+    if (ka === 'capsule' && kb === 'hull') {
+        return capsuleHull(a, b, centerA, centerB, speculativeMargin, featureBase, false);
+    }
+    /* istanbul ignore next -- unreachable exhaustiveness guard. */
     return undefined;
 }
 // ---------- sphere / capsule closed forms ----------
 function radiusOf(shape) {
-    /* istanbul ignore next -- radiusOf is only called for sphere/capsule operands; the box arm is defensive. */
-    return shape.kind === 'box' ? 0 : shape.radius;
+    /* istanbul ignore next -- radiusOf is only called for sphere/capsule operands; the non-radius arm is defensive. */
+    return shape.kind === 'sphere' || shape.kind === 'capsule' ? shape.radius : 0;
 }
 /**
  * Build a single contact point from two world points on the surfaces of two
@@ -617,6 +643,7 @@ function collideBoxes(A, B, centerA, centerB, speculativeMargin, featureBase) {
         swap = poly;
         poly = spare;
         spare = swap;
+        /* istanbul ignore next -- SAT already proved the faces overlap, so the Sutherland-Hodgman clip against the reference face's own side planes always leaves at least one vertex (20k-pair box/hull fuzz: never hit). */
         if (polyCount === 0) {
             return undefined;
         }
@@ -648,12 +675,411 @@ function collideBoxes(A, B, centerA, centerB, speculativeMargin, featureBase) {
     }
     return { a: 0, b: 0, normal, points };
 }
+// ---------- convex hull SAT + face clipping ----------
+/** Outward bias on reference side planes so boundary-incident corners survive fp clipping. */
+const CLIP_BOUNDARY_TOL = 1e-4;
+// Box-as-hull view: the 8 corners of a box are a valid convex hull, so hull×box
+// reuses the generalized hull path. Keyed by the box shape object identity (pure
+// derived data, never canonical state — same rationale as quatMat3Cache).
+// rdm-ignore-next-line determinism/no-module-mutable-state: pure derived box-hull view keyed by immutable box shape identity
+const boxHullViewCache = new WeakMap();
+function boxAsHull(box) {
+    const shape = box.shape;
+    /* istanbul ignore next -- boxAsHull is only called for box operands; type guard only. */
+    if (shape.kind !== 'box') {
+        throw new Error('boxAsHull requires a box');
+    }
+    let data = boxHullViewCache.get(shape);
+    if (data === undefined) {
+        const h = shape.half;
+        const verts = [];
+        for (const sx of [-1, 1]) {
+            for (const sy of [-1, 1]) {
+                for (const sz of [-1, 1]) {
+                    verts.push({ x: sx * h.x, y: sy * h.y, z: sz * h.z });
+                }
+            }
+        }
+        data = (0, physics3d_hull_js_1.buildSolverHull)(verts);
+        boxHullViewCache.set(shape, data);
+    }
+    return { index: box.index, p: box.p, q: box.q, shape: { kind: 'hull', data } };
+}
+function hullData(shape) {
+    /* istanbul ignore next -- callers guard hull; type guard only. */
+    if (shape.kind !== 'hull') {
+        throw new Error('hullData requires a hull');
+    }
+    return shape.data;
+}
+/** World-space hull vertices (support samples for mesh + queries). */
+function hullWorldVertices(hull) {
+    const data = hullData(hull.shape);
+    return data.vertices.map((v) => (0, physics3d_shared_js_1.addVec3)(hull.p, (0, physics3d_shared_js_1.rotatePointByQuat)(hull.q, v)));
+}
+function hullWorldFaces(hull, worldVerts) {
+    const data = hullData(hull.shape);
+    const rot = quatToMat3Cached(hull.q);
+    return data.faces.map((face) => {
+        const nx = rot[0] * face.normal.x + rot[1] * face.normal.y + rot[2] * face.normal.z;
+        const ny = rot[3] * face.normal.x + rot[4] * face.normal.y + rot[5] * face.normal.z;
+        const nz = rot[6] * face.normal.x + rot[7] * face.normal.y + rot[8] * face.normal.z;
+        const v0 = worldVerts[face.verts[0]];
+        const offset = nx * v0.x + ny * v0.y + nz * v0.z;
+        return { nx, ny, nz, offset, verts: face.verts };
+    });
+}
+function hullWorldEdgeDirs(hull) {
+    const data = hullData(hull.shape);
+    return data.edgeDirs.map((d) => (0, physics3d_shared_js_1.rotatePointByQuat)(hull.q, d));
+}
+/**
+ * Project a hull (given its world vertices) onto an axis, returning the max
+ * support value dot(n, v). Used for SAT separation along arbitrary axes.
+ */
+function hullSupportMax(worldVerts, nx, ny, nz) {
+    let max = -Infinity;
+    for (const v of worldVerts) {
+        const d = nx * v.x + ny * v.y + nz * v.z;
+        if (d > max) {
+            max = d;
+        }
+    }
+    return max;
+}
+function hullSupportMin(worldVerts, nx, ny, nz) {
+    let min = Infinity;
+    for (const v of worldVerts) {
+        const d = nx * v.x + ny * v.y + nz * v.z;
+        if (d < min) {
+            min = d;
+        }
+    }
+    return min;
+}
+/** Separation of B beyond A along axis n (positive = separated). */
+function axisSeparation(vertsA, vertsB, nx, ny, nz) {
+    const maxA = hullSupportMax(vertsA, nx, ny, nz);
+    const minB = hullSupportMin(vertsB, nx, ny, nz);
+    return minB - maxA;
+}
+function collideHulls(A, B, centerA, centerB, speculativeMargin, featureBase) {
+    const vertsA = hullWorldVertices(A);
+    const vertsB = hullWorldVertices(B);
+    const facesA = hullWorldFaces(A, vertsA);
+    const facesB = hullWorldFaces(B, vertsB);
+    const dx = B.p.x - A.p.x;
+    const dy = B.p.y - A.p.y;
+    const dz = B.p.z - A.p.z;
+    // Face SAT: use each face's outward normal directly (already oriented).
+    let faceSep = -Infinity;
+    let faceRefIsA = true;
+    let faceIndex = -1;
+    let faceNx = 0;
+    let faceNy = 0;
+    let faceNz = 0;
+    for (let i = 0; i < facesA.length; i += 1) {
+        const f = facesA[i];
+        // Reference face outward normal points away from A. All A verts satisfy
+        // dot(n,v) <= offset; separation = min_B(dot(n,v)) - offset (>0 = gap).
+        // The best reference face maximizes separation (shallowest penetration).
+        // Strict-greater keeps the lowest sorted-index face on near-ties (stable).
+        const s = hullSupportMin(vertsB, f.nx, f.ny, f.nz) - f.offset;
+        if (s > faceSep + (i === 0 ? 0 : 1e-9)) {
+            faceSep = s;
+            faceRefIsA = true;
+            faceIndex = i;
+            faceNx = f.nx;
+            faceNy = f.ny;
+            faceNz = f.nz;
+        }
+    }
+    for (let i = 0; i < facesB.length; i += 1) {
+        const f = facesB[i];
+        const s = hullSupportMin(vertsA, f.nx, f.ny, f.nz) - f.offset;
+        // Hysteresis: B's face wins only when strictly deeper, so a resting stack
+        // keeps a stable reference frame-to-frame (warm-start anchors don't jump).
+        if (s > faceSep + 1e-9) {
+            faceSep = s;
+            faceRefIsA = false;
+            faceIndex = i;
+            faceNx = f.nx;
+            faceNy = f.ny;
+            faceNz = f.nz;
+        }
+    }
+    if (faceSep > speculativeMargin) {
+        return undefined;
+    }
+    // Edge SAT: cross products of edge-direction pairs.
+    const edgesA = hullWorldEdgeDirs(A);
+    const edgesB = hullWorldEdgeDirs(B);
+    let edgeSep = -Infinity;
+    let edgeNx = 0;
+    let edgeNy = 0;
+    let edgeNz = 0;
+    let edgeFound = false;
+    let edgePairIndex = -1;
+    for (let i = 0; i < edgesA.length; i += 1) {
+        const ea = edgesA[i];
+        for (let j = 0; j < edgesB.length; j += 1) {
+            const eb = edgesB[j];
+            let cx = ea.y * eb.z - ea.z * eb.y;
+            let cy = ea.z * eb.x - ea.x * eb.z;
+            let cz = ea.x * eb.y - ea.y * eb.x;
+            const l2 = cx * cx + cy * cy + cz * cz;
+            if (l2 < 1e-8) {
+                continue;
+            }
+            const inv = 1 / (0, physics3d_shared_js_1.solverSqrt)(l2);
+            cx *= inv;
+            cy *= inv;
+            cz *= inv;
+            // orient axis from A toward B
+            if (cx * dx + cy * dy + cz * dz < 0) {
+                cx = -cx;
+                cy = -cy;
+                cz = -cz;
+            }
+            const sep = axisSeparation(vertsA, vertsB, cx, cy, cz);
+            if (sep > speculativeMargin) {
+                return undefined;
+            }
+            if (sep > edgeSep + 1e-9) {
+                edgeSep = sep;
+                edgeNx = cx;
+                edgeNy = cy;
+                edgeNz = cz;
+                edgeFound = true;
+                edgePairIndex = i * edgesB.length + j;
+            }
+        }
+    }
+    // qu3e-style hysteresis: prefer the edge axis only when decisively shallower
+    // separation (deeper penetration) than the face axis, and only for real overlaps.
+    const facePen = -faceSep;
+    const edgePen = -edgeSep;
+    const edgePreferred = edgeFound && facePen > 0 && edgePen < facePen * 0.9 - 5e-3;
+    if (edgePreferred) {
+        return hullEdgeContact(A, B, edgesA, edgesB, edgePairIndex, edgeNx, edgeNy, edgeNz, edgePen, centerA, centerB, featureBase);
+    }
+    return hullFaceContact(faceRefIsA ? A : B, faceRefIsA ? B : A, faceRefIsA ? vertsA : vertsB, faceRefIsA ? vertsB : vertsA, faceRefIsA ? facesA : facesB, faceRefIsA ? facesB : facesA, faceIndex, faceNx, faceNy, faceNz, faceRefIsA, speculativeMargin, centerA, centerB, featureBase);
+}
+function hullEdgeContact(A, B, edgesA, edgesB, pairIndex, nx, ny, nz, penetration, centerA, centerB, featureBase) {
+    const i = Math.trunc(pairIndex / edgesB.length);
+    const j = pairIndex % edgesB.length;
+    const vertsA = hullWorldVertices(A);
+    const vertsB = hullWorldVertices(B);
+    // support vertices of A along +n and B along -n approximate the contacting edges
+    const pA = supportVertex(vertsA, nx, ny, nz);
+    const pB = supportVertex(vertsB, -nx, -ny, -nz);
+    const eA = edgesA[i];
+    const eB = edgesB[j];
+    const seg = closestSegmentSegment((0, physics3d_shared_js_1.subVec3)(pA, (0, physics3d_shared_js_1.scaleVec3)(eA, 1)), (0, physics3d_shared_js_1.addVec3)(pA, (0, physics3d_shared_js_1.scaleVec3)(eA, 1)), (0, physics3d_shared_js_1.subVec3)(pB, (0, physics3d_shared_js_1.scaleVec3)(eB, 1)), (0, physics3d_shared_js_1.addVec3)(pB, (0, physics3d_shared_js_1.scaleVec3)(eB, 1)));
+    const mid = (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.addVec3)(seg.c1, seg.c2), 0.5);
+    const normal = { x: nx, y: ny, z: nz };
+    return {
+        a: 0,
+        b: 0,
+        normal,
+        points: [{ rA: (0, physics3d_shared_js_1.subVec3)(mid, centerA), rB: (0, physics3d_shared_js_1.subVec3)(mid, centerB), penetration, feature: featureBase + 4096 + pairIndex }],
+    };
+}
+function supportVertex(worldVerts, nx, ny, nz) {
+    let best = worldVerts[0];
+    let bestD = -Infinity;
+    for (const v of worldVerts) {
+        const d = nx * v.x + ny * v.y + nz * v.z;
+        if (d > bestD) {
+            bestD = d;
+            best = v;
+        }
+    }
+    return best;
+}
+function hullFaceContact(ref, inc, refVerts, incVerts, refFaces, incFaces, refFaceIndex, nx, ny, nz, refIsA, speculativeMargin, centerA, centerB, featureBase) {
+    void ref;
+    void inc;
+    const refFace = refFaces[refFaceIndex];
+    // Incident face = the incident-hull face most anti-parallel to the reference normal.
+    let incFaceIndex = 0;
+    let minDot = Infinity;
+    for (let i = 0; i < incFaces.length; i += 1) {
+        const f = incFaces[i];
+        const d = f.nx * nx + f.ny * ny + f.nz * nz;
+        // Strict-less keeps the lowest sorted-index incident face on near-ties, so
+        // the clipped polygon (and its feature ids) stay stable frame-to-frame.
+        if (d < minDot - (i === 0 ? Infinity : 1e-9)) {
+            minDot = d;
+            incFaceIndex = i;
+        }
+    }
+    const incFace = incFaces[incFaceIndex];
+    // Load incident face polygon into scratch.
+    ensureClipSlots(clipScratch.a, incFace.verts.length);
+    let poly = clipScratch.a;
+    let spare = clipScratch.b;
+    let polyCount = 0;
+    for (const vi of incFace.verts) {
+        const v = incVerts[vi];
+        const slot = poly[polyCount];
+        slot.x = v.x;
+        slot.y = v.y;
+        slot.z = v.z;
+        slot.id = featureBase + refFaceIndex * 64 + polyCount + 1;
+        polyCount += 1;
+    }
+    // Clip the incident polygon against each reference-face side plane. Each side
+    // plane's normal must point OUTWARD from the reference polygon so clipPlaneScratch
+    // (keeps dc <= 0) retains interior points. Orientation is fixed against the
+    // face centroid rather than assuming a winding convention.
+    const rv = refFace.verts;
+    let rcx = 0;
+    let rcy = 0;
+    let rcz = 0;
+    for (const vi of rv) {
+        rcx += refVerts[vi].x;
+        rcy += refVerts[vi].y;
+        rcz += refVerts[vi].z;
+    }
+    const rInv = 1 / rv.length;
+    rcx *= rInv;
+    rcy *= rInv;
+    rcz *= rInv;
+    for (let e = 0; e < rv.length; e += 1) {
+        const a = refVerts[rv[e]];
+        const b = refVerts[rv[(e + 1) % rv.length]];
+        const edge = (0, physics3d_shared_js_1.subVec3)(b, a);
+        let pnx = ny * edge.z - nz * edge.y;
+        let pny = nz * edge.x - nx * edge.z;
+        let pnz = nx * edge.y - ny * edge.x;
+        const l2 = pnx * pnx + pny * pny + pnz * pnz;
+        /* istanbul ignore next -- a reference-face edge is never degenerate: buildSolverHull dedupes vertices and box faces are axis-aligned quads (20k-pair fuzz: never hit). */
+        if (l2 < 1e-12) {
+            continue;
+        }
+        const invl = 1 / (0, physics3d_shared_js_1.solverSqrt)(l2);
+        pnx *= invl;
+        pny *= invl;
+        pnz *= invl;
+        let offset = pnx * a.x + pny * a.y + pnz * a.z;
+        // Flip to outward: the centroid (interior) must lie on the negative side.
+        /* istanbul ignore else -- reference faces are wound CCW about their outward normal, so `n x edge` always points inward and the flip always runs (20k-pair box/hull fuzz: 48532 flips, 0 skips). The guard stays as a winding assertion. */
+        if (pnx * rcx + pny * rcy + pnz * rcz > offset) {
+            pnx = -pnx;
+            pny = -pny;
+            pnz = -pnz;
+            offset = -offset;
+        }
+        // Bias the side plane slightly outward so incident-face corners that lie
+        // exactly on a reference-face boundary survive floating-point clipping
+        // (dropping them mid-settle collapses a resting face manifold to 1-3 points
+        // on one side and injects drift). Box clipping avoids this via exact axis
+        // alignment; the generic hull path needs the tolerance.
+        offset += CLIP_BOUNDARY_TOL;
+        polyCount = clipPlaneScratch(poly, polyCount, pnx, pny, pnz, offset, e + 1, spare);
+        const swap = poly;
+        poly = spare;
+        spare = swap;
+        if (polyCount === 0) {
+            return undefined;
+        }
+    }
+    const refFaceDist = refFace.offset;
+    const points = [];
+    for (let i = 0; i < polyCount; i += 1) {
+        const vert = poly[i];
+        const depth = refFaceDist - (nx * vert.x + ny * vert.y + nz * vert.z);
+        if (depth >= -speculativeMargin) {
+            const onRefX = vert.x + nx * depth;
+            const onRefY = vert.y + ny * depth;
+            const onRefZ = vert.z + nz * depth;
+            const midX = (vert.x + onRefX) * 0.5;
+            const midY = (vert.y + onRefY) * 0.5;
+            const midZ = (vert.z + onRefZ) * 0.5;
+            points.push({
+                rA: { x: midX - centerA.x, y: midY - centerA.y, z: midZ - centerA.z },
+                rB: { x: midX - centerB.x, y: midY - centerB.y, z: midZ - centerB.z },
+                penetration: depth,
+                feature: vert.id,
+            });
+        }
+    }
+    /* istanbul ignore next -- SAT overlap guarantees the incident face's deepest surviving vertex is within the speculative margin (20k-pair fuzz: never hit). */
+    if (points.length === 0) {
+        return undefined;
+    }
+    // Normal in caller A->B frame: reference face outward normal points ref->inc.
+    // If ref is A, normal ref->inc = A->B = +n. If ref is B, A->B = -n.
+    const normal = refIsA ? { x: nx, y: ny, z: nz } : { x: -nx, y: -ny, z: -nz };
+    return { a: 0, b: 0, normal, points: reducePoints(points) };
+}
+function sphereHull(sphere, hull, cSphere, cHull, margin, feature, flip) {
+    /* istanbul ignore next -- dispatch guarantees a (sphere, hull) pair; type guard only. */
+    if (sphere.shape.kind !== 'sphere' || hull.shape.kind !== 'hull') {
+        return undefined;
+    }
+    const radius = sphere.shape.radius;
+    const data = hull.shape.data;
+    const localCenter = (0, physics3d_shared_js_1.rotatePointByQuatInverse)(hull.q, (0, physics3d_shared_js_1.subVec3)(sphere.p, hull.p));
+    const closest = (0, physics3d_hull_js_1.closestPointOnHullLocal)(localCenter, data);
+    if (closest.inside) {
+        // sphere center inside the hull: push out along the least-violated face normal.
+        const nLocal = closest.normal;
+        const nWorld = (0, physics3d_shared_js_1.rotatePointByQuat)(hull.q, nLocal); // outward from hull
+        const penetration = -closest.distance + radius;
+        const nSphereToHull = (0, physics3d_shared_js_1.scaleVec3)(nWorld, -1);
+        return emitSingle(nSphereToHull, sphere.p, penetration, cSphere, cHull, feature, flip);
+    }
+    const closestWorld = (0, physics3d_shared_js_1.addVec3)(hull.p, (0, physics3d_shared_js_1.rotatePointByQuat)(hull.q, closest.point));
+    const delta = (0, physics3d_shared_js_1.subVec3)(sphere.p, closestWorld);
+    const dist = (0, physics3d_shared_js_1.solverSqrt)((0, physics3d_shared_js_1.dotVec3)(delta, delta));
+    const penetration = radius - dist;
+    if (penetration < -margin) {
+        return undefined;
+    }
+    /* istanbul ignore next -- a sphere centre coincident with its closest hull point is `inside` and returned above, so dist is positive here. */
+    const nHullToSphere = dist > 1e-9 ? (0, physics3d_shared_js_1.scaleVec3)(delta, 1 / dist) : (0, physics3d_shared_js_1.rotatePointByQuat)(hull.q, closest.normal);
+    const nSphereToHull = (0, physics3d_shared_js_1.scaleVec3)(nHullToSphere, -1);
+    const mid = (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.addVec3)(closestWorld, (0, physics3d_shared_js_1.subVec3)(sphere.p, (0, physics3d_shared_js_1.scaleVec3)(nHullToSphere, radius))), 0.5);
+    return emitSingle(nSphereToHull, mid, penetration, cSphere, cHull, feature, flip);
+}
+function capsuleHull(capsule, hull, cCapsule, cHull, margin, feature, flip) {
+    const seg = capsuleSegment(capsule);
+    const samples = [seg.start, seg.end, (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.addVec3)(seg.start, seg.end), 0.5)];
+    const points = [];
+    let normal;
+    let deepest = -Infinity;
+    samples.forEach((sample, i) => {
+        const sphere = { index: capsule.index, p: sample, q: capsule.q, shape: { kind: 'sphere', radius: seg.radius } };
+        const sub = sphereHull(sphere, hull, cCapsule, cHull, margin, feature + i, flip);
+        if (sub === undefined) {
+            return;
+        }
+        for (const point of sub.points) {
+            points.push(point);
+        }
+        const d = sub.points[0].penetration;
+        if (d > deepest) {
+            deepest = d;
+            normal = sub.normal;
+        }
+    });
+    if (points.length === 0 || normal === undefined) {
+        return undefined;
+    }
+    return { a: 0, b: 0, normal, points };
+}
+function hullSupportSamples(hull) {
+    return hullWorldVertices(hull);
+}
 // ---------- mesh (one-sided static triangle) ----------
 function meshManifold(a, b, speculativeMargin) {
-    const meshIsA = a.shape.kind === 'mesh';
+    const meshIsA = a.shape.kind === 'mesh' || a.shape.kind === 'heightfield';
     const meshBody = meshIsA ? a : b;
     const otherBody = meshIsA ? b : a;
-    if (meshBody.shape.kind !== 'mesh' || otherBody.shape.kind === 'mesh') {
+    if ((meshBody.shape.kind !== 'mesh' && meshBody.shape.kind !== 'heightfield')
+        || otherBody.shape.kind === 'mesh' || otherBody.shape.kind === 'heightfield') {
         return undefined;
     }
     const prims = primitiveViews(otherBody);
@@ -661,35 +1087,71 @@ function meshManifold(a, b, speculativeMargin) {
     let normal;
     let deepest = -Infinity;
     let feature = 0;
-    const triangles = meshBody.shape.triangles.map((tri) => [
+    const otherHalf = shapeHalfExtent(otherBody.shape, otherBody.q);
+    const localTriangles = meshBody.shape.kind === 'mesh'
+        ? meshBody.shape.triangles
+        : (0, physics3d_heightfield_js_1.heightfieldLocalTriangles)(meshBody.shape, {
+            minX: otherBody.p.x - otherHalf.x - speculativeMargin - meshBody.p.x,
+            maxX: otherBody.p.x + otherHalf.x + speculativeMargin - meshBody.p.x,
+            minY: otherBody.p.y - otherHalf.y - speculativeMargin - meshBody.p.y,
+            maxY: otherBody.p.y + otherHalf.y + speculativeMargin - meshBody.p.y,
+            minZ: otherBody.p.z - otherHalf.z - speculativeMargin - meshBody.p.z,
+            maxZ: otherBody.p.z + otherHalf.z + speculativeMargin - meshBody.p.z,
+        });
+    const triangles = localTriangles.map((tri) => [
         (0, physics3d_shared_js_1.addVec3)(meshBody.p, (0, physics3d_shared_js_1.rotatePointByQuat)(meshBody.q, tri[0])),
         (0, physics3d_shared_js_1.addVec3)(meshBody.p, (0, physics3d_shared_js_1.rotatePointByQuat)(meshBody.q, tri[1])),
         (0, physics3d_shared_js_1.addVec3)(meshBody.p, (0, physics3d_shared_js_1.rotatePointByQuat)(meshBody.q, tri[2])),
     ]);
     for (const prim of prims) {
-        /* istanbul ignore next -- primitiveViews only yields box/sphere/capsule sub-bodies (compound children are primitives); this skip is unreachable. */
-        if (prim.shape.kind !== 'sphere' && prim.shape.kind !== 'capsule' && prim.shape.kind !== 'box') {
+        /* istanbul ignore next -- primitiveViews only yields box/sphere/capsule/hull sub-bodies; this skip is unreachable. */
+        if (prim.shape.kind !== 'sphere' && prim.shape.kind !== 'capsule' && prim.shape.kind !== 'box' && prim.shape.kind !== 'hull') {
             continue;
         }
-        const radius = prim.shape.kind === 'box'
-            ? 0
-            : prim.shape.radius;
-        // Represent boxes by their center as a fallback point-sphere against triangles
-        // (dynamic box-vs-mesh uses the box's support samples).
+        const radius = prim.shape.kind === 'sphere' || prim.shape.kind === 'capsule'
+            ? prim.shape.radius
+            : 0;
+        // Boxes/hulls sample their support vertices against triangles (like dynamic
+        // box-vs-mesh); sphere/capsule use center/segment samples.
         const samplePoints = prim.shape.kind === 'box'
             ? boxSupportSamples(prim)
-            : prim.shape.kind === 'capsule'
-                ? capsuleSamples(prim)
-                : [prim.p];
+            : prim.shape.kind === 'hull'
+                ? hullSupportSamples(prim)
+                : prim.shape.kind === 'capsule'
+                    ? capsuleSamples(prim)
+                    : [prim.p];
         for (const triangle of triangles) {
             for (const sample of samplePoints) {
                 const closest = closestPointOnTriangle(sample, triangle[0], triangle[1], triangle[2]);
                 const delta = (0, physics3d_shared_js_1.subVec3)(sample, closest);
-                const dist = (0, physics3d_shared_js_1.solverSqrt)((0, physics3d_shared_js_1.dotVec3)(delta, delta));
                 const triNormal = triangleNormal(triangle[0], triangle[1], triangle[2]);
-                const side = (0, physics3d_shared_js_1.dotVec3)(delta, triNormal);
-                const penetration = radius - dist;
-                if (penetration < -speculativeMargin || side < 0) {
+                const planeDist = (0, physics3d_shared_js_1.dotVec3)((0, physics3d_shared_js_1.subVec3)(sample, triangle[0]), triNormal);
+                const centerPlaneDist = (0, physics3d_shared_js_1.dotVec3)((0, physics3d_shared_js_1.subVec3)(prim.p, triangle[0]), triNormal);
+                const relativeNormalVelocity = (0, physics3d_shared_js_1.dotVec3)((0, physics3d_shared_js_1.subVec3)(otherBody.v ?? ZERO, meshBody.v ?? ZERO), triNormal);
+                // delta = perp + planeDist·n, so perp² = |delta|² - planeDist². A zero
+                // perp means the sample projects onto the face interior (or an edge),
+                // where the signed plane distance — not the unsigned closest-point
+                // distance — carries the penetration. Zero-radius shapes (box, hull)
+                // have no other source of depth, so this is the branch that lets them
+                // rest on and be pushed by a mesh at all.
+                const delta2 = (0, physics3d_shared_js_1.dotVec3)(delta, delta);
+                const perp2 = delta2 - planeDist * planeDist;
+                let penetration;
+                if (perp2 <= exports.SOLVER_CONSTANTS.MESH_FACE_PROJECTION_TOL2) {
+                    if (planeDist < -exports.SOLVER_CONSTANTS.MESH_BEHIND_FACE_RECOVERY
+                        && (centerPlaneDist < 0 || relativeNormalVelocity > 0)) {
+                        continue;
+                    }
+                    penetration = radius - planeDist;
+                }
+                else {
+                    // Edge/vertex region: one-sided, front-face only.
+                    if (planeDist < 0) {
+                        continue;
+                    }
+                    penetration = radius - (0, physics3d_shared_js_1.solverSqrt)(delta2);
+                }
+                if (penetration < -speculativeMargin) {
                     continue;
                 }
                 const contactNormal = triNormal; // one-sided: mesh -> other
@@ -787,7 +1249,7 @@ function closestPointOnTriangle(p, a, b, c) {
 // Sequential-impulse velocity solver + split impulse + islands + sleep
 // ============================================================================
 exports.SOLVER_CONSTANTS = {
-    BAUMGARTE: 0.2,
+    BAUMGARTE: 0.2, // joints only (split impulse) — contacts now soft
     SLOP: 0.005,
     WAKE_VEL2: 0.005,
     SLEEP_VEL2: 0.005,
@@ -796,7 +1258,50 @@ exports.SOLVER_CONSTANTS = {
     WARM_MATCH_TOL2: 0.01,
     LINEAR_DAMPING: 0.995,
     ANGULAR_DAMPING: 0.98,
+    SUBSTEPS: 6,
+    /**
+     * Relax passes per substep = RELAX_ITERATION_SCALE × velocityIterations. The
+     * bias-free relax must drain the warm-start over-push that multiple bias-solve
+     * iterations accumulate into `accN`; under-draining lets weak-gravity stacks
+     * ratchet apart. 2× fully settles the tall-stack / mass-ratio batteries.
+     */
+    RELAX_ITERATION_SCALE: 2,
+    CONTACT_ZETA: 10,
+    /** hertz = CONTACT_HERTZ_FRACTION · substeps/dt (dimensionless ω·h = π/2). */
+    CONTACT_HERTZ_FRACTION: 0.25,
+    /** Static/kinematic-involved pairs solve 2× stiffer (box2d staticSoftness). */
+    STATIC_HERTZ_SCALE: 2,
+    /** Clamp on soft-bias velocity, length units per tick. */
+    MAX_CONTACT_PUSHOUT: 0.05,
+    /**
+     * Squared in-plane distance under which a mesh support sample counts as
+     * projecting onto the triangle face rather than its edge/vertex region.
+     * This absolute tolerance equals the squared distance quantum and is reliable
+     * for the supported game-coordinate range below 1e4.
+     */
+    MESH_FACE_PROJECTION_TOL2: 1e-12,
+    /** Maximum solver-creep depth recoverable behind a one-sided mesh face. */
+    MESH_BEHIND_FACE_RECOVERY: 0.02,
+    /**
+     * A ccd body is "fast" (TOI-bound: pre-advanced to first contact with its approach
+     * velocity arrested) when its intended per-step center motion exceeds this fraction
+     * of its smallest world half-extent (box3d b3_isFast). Below it, the in-loop
+     * speculative margin already suffices.
+     */
+    CCD_FAST_MOTION_FRACTION: 0.5,
+    /**
+     * box3d B3_SPECULATIVE_DISTANCE (4·SLOP): the small fixed speculative margin for ccd
+     * contacts. It only bridges the exact-TOI clamp's residual separation so a clamped
+     * fast body forms its contact next step; a wide velocity-scaled margin instead
+     * generates diverging far-gap contacts (#2821). Fast bodies are kept from tunneling
+     * by the exact-TOI position clamp (runRigidSolver), not by a wide margin.
+     */
+    CCD_SPECULATIVE_MARGIN: 0.02,
 };
+const TWO_PI = 6.283185307179586;
+function combinedContactFriction(frictionA10, frictionB10) {
+    return (0, physics3d_shared_js_1.solverSqrt)((frictionA10 / 10) * (frictionB10 / 10));
+}
 /**
  * Warm-cache maps pack the canonical index pair (a<b) into one exact integer
  * `a * WARM_KEY_BASE + b` — string pair keys were measurable churn on large piles.
@@ -805,6 +1310,14 @@ exports.WARM_KEY_BASE = 2 ** 20;
 function solid(b) {
     return b.isDynamic && !b.sleeping;
 }
+/**
+ * Bodies whose pose advances this step. Kinematic bodies integrate but are
+ * never solved: `solid` still gates every impulse, gravity, damping and sleep
+ * path, so a kinematic body's authored velocity survives the step untouched.
+ */
+function integrates(b) {
+    return b.isKinematic || solid(b);
+}
 function worldInvInertiaLocked(b) {
     if (!solid(b)) {
         return [0, 0, 0, 0, 0, 0, 0, 0, 0];
@@ -871,11 +1384,12 @@ function takeConstraint() {
     if (constraintPool.high === constraintPool.slots.length) {
         constraintPool.slots.push({
             manifoldIndex: 0, a: 0, b: 0,
-            normal: ZERO, t1: ZERO, t2: ZERO, rA: ZERO, rB: ZERO,
-            penetration: 0, speculative: false,
-            massN: 0, massT1: 0, massT2: 0, bias: 0,
-            accN: 0, accT1: 0, accT2: 0, accNb: 0,
-            mu: 0, restitution: 0,
+            normal: ZERO, t1: ZERO, t2: ZERO, rA: ZERO, rB: ZERO, rA0: ZERO,
+            penetration: 0,
+            massN: 0, massT1: 0, massT2: 0,
+            biasRate: 0, massScale: 1, impulseScale: 0, vn0: 0,
+            accN: 0, accT1: 0, accT2: 0, accR1: 0, accR2: 0, accNStep: 0,
+            mu: 0, rolling: 0, restitution: 0,
         });
     }
     const slot = constraintPool.slots[constraintPool.high];
@@ -890,19 +1404,30 @@ function solveRigidStep(bodies, options) {
     }
     constraintPool.high = 0;
     const dt = options.dt;
-    // 1. reset pseudo-velocities; apply gravity + damping to awake dynamics.
-    // v/w are copied so the solver owns them: callers may pass aliased vectors
-    // (canonical angularVel, shared zero constants) and the hot scalar impulse
-    // path mutates these objects in place.
+    const h = dt / options.substeps;
+    // 1. reset pseudo-velocities; copy v/w so the solver owns them (callers may
+    // pass aliased vectors — canonical angularVel, shared zero constants — and the
+    // hot scalar impulse path mutates these objects in place). Gravity + damping +
+    // position integration now happen inside the sub-step loop. Locked axes are
+    // zeroed here so residual input velocity on a locked axis never integrates.
     for (const b of bodies) {
         b.vb = { x: 0, y: 0, z: 0 };
         b.wb = { x: 0, y: 0, z: 0 };
-        b.v = { x: b.v.x, y: b.v.y, z: b.v.z };
-        b.w = { x: b.w.x, y: b.w.y, z: b.w.z };
         if (!solid(b)) {
+            b.v = { x: b.v.x, y: b.v.y, z: b.v.z };
+            b.w = { x: b.w.x, y: b.w.y, z: b.w.z };
             continue;
         }
-        b.v.y = b.v.y + options.gravityY * b.gravityScale * dt;
+        b.v = {
+            x: b.invMassVec.x === 0 ? 0 : b.v.x,
+            y: b.invMassVec.y === 0 ? 0 : b.v.y,
+            z: b.invMassVec.z === 0 ? 0 : b.v.z,
+        };
+        b.w = {
+            x: b.angularLock.x === 0 ? 0 : b.w.x,
+            y: b.angularLock.y === 0 ? 0 : b.w.y,
+            z: b.angularLock.z === 0 ? 0 : b.w.z,
+        };
     }
     // 2. narrow-phase manifolds (canonical order i<j).
     const manifolds = [];
@@ -921,7 +1446,7 @@ function solveRigidStep(bodies, options) {
         if ((A.mask & B.layer) === 0 || (B.mask & A.layer) === 0) {
             return;
         }
-        const margin = A.ccd || B.ccd ? speculativeMarginFor(A, B, dt) : 0;
+        const margin = A.ccd || B.ccd ? exports.SOLVER_CONSTANTS.CCD_SPECULATIVE_MARGIN : 0;
         const manifold = generateManifold(toGeom(A), toGeom(B), margin);
         if (manifold !== undefined) {
             manifolds.push(manifold);
@@ -984,11 +1509,40 @@ function solveRigidStep(bodies, options) {
         const A = bodies[m.a];
         const B = bodies[m.b];
         const [t1, t2] = tangentBasis(m.normal);
-        const combinedMu = Math.floor((toInt10(A.matFriction) + toInt10(B.matFriction)) / 2) / 10;
+        const combinedMu = combinedContactFriction(toInt10(A.matFriction), toInt10(B.matFriction));
         const combinedRest = Math.max(toInt10(A.matRestitution), toInt10(B.matRestitution)) / 10;
+        const combinedRolling = Math.max(A.matRolling, B.matRolling);
+        // Soft-constraint coefficients (box2d SoftStep): static/kinematic-involved
+        // pairs solve stiffer. hertz = fraction · substeps/dt keeps ω·h = π/2.
+        const softStatic = !A.isDynamic || !B.isDynamic;
+        const hertz = (exports.SOLVER_CONSTANTS.CONTACT_HERTZ_FRACTION * options.substeps / dt)
+            * (softStatic ? exports.SOLVER_CONSTANTS.STATIC_HERTZ_SCALE : 1);
+        const omega = TWO_PI * hertz;
+        const a1 = 2 * exports.SOLVER_CONSTANTS.CONTACT_ZETA + omega * h;
+        const a2 = h * omega * a1;
+        const a3 = 1 / (1 + a2);
+        const biasRate = omega / a1;
+        const massScale = a2 * a3;
+        const impulseScale = a3;
         const warmPts = options.warm.get(m.a * exports.WARM_KEY_BASE + m.b);
         for (const pt of m.points) {
-            const matched = matchWarm(warmPts, pt.rA);
+            // A separated (speculative) contact anchors at the gap midpoint, so a wide margin
+            // over a large gap yields anchors far from BOTH body centers (moment arms ≈ half
+            // the gap), whose ω×r terms blow up the coupled relax at extreme speed (the #2821
+            // cluster divergence). Pull each anchor back to its own body's surface witness
+            // point along the normal by half the separation; a touching/penetrating contact
+            // (penetration ≥ 0) is untouched, so resting stacks and the golden batteries are
+            // byte-identical.
+            let rA = pt.rA;
+            let rB = pt.rB;
+            if (pt.penetration < 0) {
+                const half = -pt.penetration * 0.5;
+                rA = { x: pt.rA.x - m.normal.x * half, y: pt.rA.y - m.normal.y * half, z: pt.rA.z - m.normal.z * half };
+                rB = { x: pt.rB.x + m.normal.x * half, y: pt.rB.y + m.normal.y * half, z: pt.rB.z + m.normal.z * half };
+            }
+            rA = (0, physics3d_shared_js_1.subVec3)(rA, (0, physics3d_shared_js_1.rotatePointByQuat)(A.q, A.localCenter));
+            rB = (0, physics3d_shared_js_1.subVec3)(rB, (0, physics3d_shared_js_1.rotatePointByQuat)(B.q, B.localCenter));
+            const matched = matchWarm(warmPts, rA);
             const constraint = takeConstraint();
             constraint.manifoldIndex = manifoldIndex;
             constraint.a = m.a;
@@ -996,22 +1550,29 @@ function solveRigidStep(bodies, options) {
             constraint.normal = m.normal;
             constraint.t1 = t1;
             constraint.t2 = t2;
-            constraint.rA = pt.rA;
-            constraint.rB = pt.rB;
+            // Working anchors are fresh copies (rotated per substep); rA0 keeps the
+            // frame-start anchor for the cache.
+            constraint.rA = { x: rA.x, y: rA.y, z: rA.z };
+            constraint.rB = { x: rB.x, y: rB.y, z: rB.z };
+            constraint.rA0 = rA;
             constraint.penetration = pt.penetration;
-            constraint.speculative = pt.penetration < 0;
-            constraint.massN = effectiveMass(m.normal, pt.rA, pt.rB, invMass[m.a], invMass[m.b], invInertia[m.a], invInertia[m.b]);
-            constraint.massT1 = effectiveMass(t1, pt.rA, pt.rB, invMass[m.a], invMass[m.b], invInertia[m.a], invInertia[m.b]);
-            constraint.massT2 = effectiveMass(t2, pt.rA, pt.rB, invMass[m.a], invMass[m.b], invInertia[m.a], invInertia[m.b]);
-            constraint.bias = (exports.SOLVER_CONSTANTS.BAUMGARTE / dt) * Math.max(0, pt.penetration - exports.SOLVER_CONSTANTS.SLOP);
+            constraint.massN = effectiveMass(m.normal, rA, rB, invMass[m.a], invMass[m.b], invInertia[m.a], invInertia[m.b]);
+            constraint.massT1 = effectiveMass(t1, rA, rB, invMass[m.a], invMass[m.b], invInertia[m.a], invInertia[m.b]);
+            constraint.massT2 = effectiveMass(t2, rA, rB, invMass[m.a], invMass[m.b], invInertia[m.a], invInertia[m.b]);
+            constraint.biasRate = biasRate;
+            constraint.massScale = massScale;
+            constraint.impulseScale = impulseScale;
+            // Approach velocity captured pre-substep (before any gravity) for restitution.
+            constraint.vn0 = relativeVelocityAlong(A, B, rA, rB, m.normal.x, m.normal.y, m.normal.z);
             constraint.accN = matched?.normalImpulse ?? 0;
             constraint.accT1 = matched?.tangentImpulse1 ?? 0;
             constraint.accT2 = matched?.tangentImpulse2 ?? 0;
-            constraint.accNb = 0;
+            constraint.accR1 = 0;
+            constraint.accR2 = 0;
+            constraint.accNStep = 0;
             constraint.mu = combinedMu;
+            constraint.rolling = combinedRolling;
             constraint.restitution = combinedRest;
-            // warm-start impulse, scalar form of normal*accN + (t1*accT1 + t2*accT2)
-            applyImpulseScalar(bodies, invMass, invInertia, constraint.a, constraint.b, constraint.rA, constraint.rB, m.normal.x * constraint.accN + (t1.x * constraint.accT1 + t2.x * constraint.accT2), m.normal.y * constraint.accN + (t1.y * constraint.accT1 + t2.y * constraint.accT2), m.normal.z * constraint.accN + (t1.z * constraint.accT1 + t2.z * constraint.accT2), false);
             constraints.push(constraint);
         }
     });
@@ -1020,7 +1581,7 @@ function solveRigidStep(bodies, options) {
         bodies,
         invMass,
         invInertia,
-        dt,
+        dt: h,
         apply: (a, b, rA, rB, impulse) => applyImpulse(bodies, invMass, invInertia, a, b, rA, rB, impulse),
         applyAngular: (a, b, angularImpulse) => {
             const A = bodies[a];
@@ -1043,17 +1604,90 @@ function solveRigidStep(bodies, options) {
         applyBias: (a, b, rA, rB, impulse) => applyBiasImpulseAt(bodies, invMass, invInertia, a, b, rA, rB, impulse),
         biasRelativeVelocity: (a, b, rA, rB) => relativeBiasVelocity(bodies[a], bodies[b], rA, rB),
     };
-    for (const joint of joints) {
-        joint.prepare(jointCtx);
-    }
-    for (let iter = 0; iter < options.velocityIterations; iter += 1) {
+    // 5. TGS-Soft sub-step loop: gravity(h) → joint prepare + contact warm-start →
+    // soft-bias iterations → position integration(h) + anchor/penetration update →
+    // bias-free relax → per-substep joint-angle integration. One restitution pass
+    // follows all substeps, driven by the pre-step approach velocity.
+    for (let substep = 0; substep < options.substeps; substep += 1) {
+        for (const b of bodies) {
+            if (solid(b) && b.invMassVec.y !== 0) {
+                b.v.y = b.v.y + options.gravityY * b.gravityScale * h;
+            }
+        }
         for (const joint of joints) {
-            joint.iterate(jointCtx);
+            joint.prepare(jointCtx);
+        }
+        for (const c of constraints) {
+            applyImpulseScalar(bodies, invMass, invInertia, c.a, c.b, c.rA, c.rB, c.normal.x * c.accN + (c.t1.x * c.accT1 + c.t2.x * c.accT2), c.normal.y * c.accN + (c.t1.y * c.accT1 + c.t2.y * c.accT2), c.normal.z * c.accN + (c.t1.z * c.accT1 + c.t2.z * c.accT2));
+        }
+        for (let iter = 0; iter < options.velocityIterations; iter += 1) {
+            for (const joint of joints) {
+                joint.iterate(jointCtx);
+            }
+            for (const c of constraints) {
+                solveContactSoft(bodies, invMass, invInertia, c, h, true);
+            }
+        }
+        for (const b of bodies) {
+            if (!integrates(b)) {
+                continue;
+            }
+            b.c = (0, physics3d_shared_js_1.addVec3)(b.c, (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.addVec3)(b.v, b.vb), h));
+            if (b.hasOrientation) {
+                b.q = (0, physics3d_shared_js_1.integrateQuat)(b.q, (0, physics3d_shared_js_1.addVec3)(b.w, b.wb), h);
+            }
+            b.p = (0, physics3d_shared_js_1.subVec3)(b.c, (0, physics3d_shared_js_1.rotatePointByQuat)(b.q, b.localCenter));
         }
         for (const c of constraints) {
-            solveContactConstraint(bodies, invMass, invInertia, c, dt);
+            const A = bodies[c.a];
+            const B = bodies[c.b];
+            const dpA = integrates(A) ? (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.addVec3)(A.v, A.vb), h) : ZERO;
+            const dpB = integrates(B) ? (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.addVec3)(B.v, B.vb), h) : ZERO;
+            const rotA = integrates(A) ? (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.crossVec3)((0, physics3d_shared_js_1.addVec3)(A.w, A.wb), c.rA), h) : ZERO;
+            const rotB = integrates(B) ? (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.crossVec3)((0, physics3d_shared_js_1.addVec3)(B.w, B.wb), c.rB), h) : ZERO;
+            const rel = (0, physics3d_shared_js_1.subVec3)((0, physics3d_shared_js_1.addVec3)(dpB, rotB), (0, physics3d_shared_js_1.addVec3)(dpA, rotA));
+            c.penetration -= (0, physics3d_shared_js_1.dotVec3)(rel, c.normal);
+            c.rA = (0, physics3d_shared_js_1.addVec3)(c.rA, rotA);
+            c.rB = (0, physics3d_shared_js_1.addVec3)(c.rB, rotB);
+        }
+        for (const b of bodies) {
+            b.vb = { x: 0, y: 0, z: 0 };
+            b.wb = { x: 0, y: 0, z: 0 };
+        }
+        const relaxIterations = options.velocityIterations * exports.SOLVER_CONSTANTS.RELAX_ITERATION_SCALE;
+        for (const c of constraints) {
+            c.accR1 = 0;
+            c.accR2 = 0;
+        }
+        for (let iter = 0; iter < relaxIterations; iter += 1) {
+            for (const c of constraints) {
+                solveContactSoft(bodies, invMass, invInertia, c, h, false);
+            }
+        }
+        // Accumulate the per-substep normal impulse so contact events report the
+        // full-step reaction (the raw accN converges to the sustaining per-substep value).
+        for (const c of constraints) {
+            c.accNStep += c.accN;
+        }
+        for (const joint of joints) {
+            joint.postSubstep?.(jointCtx);
         }
     }
+    // 5b. restitution: replay the pre-step approach velocity for bouncy contacts.
+    for (const c of constraints) {
+        if (c.restitution <= 0 || c.vn0 >= -exports.SOLVER_CONSTANTS.RESTITUTION_THRESHOLD || c.accN <= 0) {
+            continue;
+        }
+        const A = bodies[c.a];
+        const B = bodies[c.b];
+        const vn = relativeVelocityAlong(A, B, c.rA, c.rB, c.normal.x, c.normal.y, c.normal.z);
+        let dLambda = c.massN * (-c.restitution * c.vn0 - vn);
+        const newAcc = Math.max(0, c.accN + dLambda);
+        dLambda = newAcc - c.accN;
+        c.accN = newAcc;
+        c.accNStep += dLambda;
+        applyImpulseScalar(bodies, invMass, invInertia, c.a, c.b, c.rA, c.rB, c.normal.x * dLambda, c.normal.y * dLambda, c.normal.z * dLambda);
+    }
     // 6. store warm cache (index-pair keyed, deterministic order).
     const warm = new Map();
     for (const c of constraints) {
@@ -1063,12 +1697,13 @@ function solveRigidStep(bodies, options) {
             list = [];
             warm.set(key, list);
         }
-        list.push({ anchor: c.rA, normalImpulse: c.accN, tangentImpulse1: c.accT1, tangentImpulse2: c.accT2 });
+        list.push({ anchor: c.rA0, normalImpulse: c.accN, tangentImpulse1: c.accT1, tangentImpulse2: c.accT2 });
     }
     // `jointBodies` (built at the wake pass) keeps joint members awake — a motor or
     // grab can drive them below the sleep velocity threshold, and joints are step
     // inputs the sleeper can't see.
-    // 7. damping + integrate (real + pseudo velocity).
+    // 7. damping (once per step) + axis-lock zeroing + sleep counters. Positions
+    // were already integrated per substep in step 5.
     for (const b of bodies) {
         if (!solid(b)) {
             continue;
@@ -1087,10 +1722,6 @@ function solveRigidStep(bodies, options) {
             y: b.angularLock.y === 0 ? 0 : b.w.y,
             z: b.angularLock.z === 0 ? 0 : b.w.z,
         };
-        b.p = (0, physics3d_shared_js_1.addVec3)(b.p, (0, physics3d_shared_js_1.scaleVec3)((0, physics3d_shared_js_1.addVec3)(b.v, b.vb), dt));
-        if (b.hasOrientation) {
-            b.q = (0, physics3d_shared_js_1.integrateQuat)(b.q, (0, physics3d_shared_js_1.addVec3)(b.w, b.wb), dt);
-        }
         const speed2 = (0, physics3d_shared_js_1.dotVec3)(b.v, b.v) + (0, physics3d_shared_js_1.dotVec3)(b.w, b.w);
         b.lowVelFrames = jointBodies.has(b.index) || speed2 >= exports.SOLVER_CONSTANTS.SLEEP_VEL2 ? 0 : b.lowVelFrames + 1;
     }
@@ -1099,7 +1730,7 @@ function solveRigidStep(bodies, options) {
     // 9. per-manifold accumulated normal impulse for contact events.
     const impulseByManifold = new Array(manifolds.length).fill(0);
     for (const c of constraints) {
-        impulseByManifold[c.manifoldIndex] += c.accN;
+        impulseByManifold[c.manifoldIndex] += c.accNStep;
     }
     const contactImpulses = manifolds.map((m, index) => ({
         a: m.a,
@@ -1115,6 +1746,17 @@ function toInt10(unit) {
 }
 /** World AABB of a solver body's shape (orientation-aware), swept by v·dt when ccd. */
 function solverBodyAabb(body, dt) {
+    if (body.shape.kind === 'heightfield') {
+        const bounds = (0, physics3d_heightfield_js_1.heightfieldLocalBounds)(body.shape);
+        return {
+            minX: body.p.x + bounds.minX,
+            maxX: body.p.x + bounds.maxX,
+            minY: body.p.y + bounds.minY,
+            maxY: body.p.y + bounds.maxY,
+            minZ: body.p.z + bounds.minZ,
+            maxZ: body.p.z + bounds.maxZ,
+        };
+    }
     const half = shapeHalfExtent(body.shape, body.q);
     let minX = body.p.x - half.x;
     let maxX = body.p.x + half.x;
@@ -1151,6 +1793,14 @@ function shapeHalfExtent(shape, q) {
         const reach = shape.radius + shape.halfHeight;
         return { x: reach, y: reach, z: reach };
     }
+    if (shape.kind === 'hull') {
+        let ext = { x: 0, y: 0, z: 0 };
+        for (const vertex of shape.data.vertices) {
+            const rotated = (0, physics3d_shared_js_1.rotatePointByQuat)(q, vertex);
+            ext = { x: Math.max(ext.x, Math.abs(rotated.x)), y: Math.max(ext.y, Math.abs(rotated.y)), z: Math.max(ext.z, Math.abs(rotated.z)) };
+        }
+        return ext;
+    }
     if (shape.kind === 'mesh') {
         let ext = { x: 0, y: 0, z: 0 };
         for (const tri of shape.triangles) {
@@ -1161,6 +1811,14 @@ function shapeHalfExtent(shape, q) {
         }
         return ext;
     }
+    if (shape.kind === 'heightfield') {
+        const bounds = (0, physics3d_heightfield_js_1.heightfieldLocalBounds)(shape);
+        return {
+            x: Math.max(Math.abs(bounds.minX), Math.abs(bounds.maxX)),
+            y: Math.max(Math.abs(bounds.minY), Math.abs(bounds.maxY)),
+            z: Math.max(Math.abs(bounds.minZ), Math.abs(bounds.maxZ)),
+        };
+    }
     // compound: union of child extents about the body center.
     let ext = { x: 0, y: 0, z: 0 };
     for (const child of shape.children) {
@@ -1174,15 +1832,68 @@ function shapeHalfExtent(shape, q) {
     }
     return ext;
 }
-function speculativeMarginFor(a, b, dt) {
-    const relSpeed = (0, physics3d_shared_js_1.solverSqrt)(Math.max((0, physics3d_shared_js_1.dotVec3)(a.v, a.v), (0, physics3d_shared_js_1.dotVec3)(b.v, b.v)));
-    return relSpeed * dt + exports.SOLVER_CONSTANTS.SLOP;
+/**
+ * Smallest world half-extent of a shape (the box3d fast-flag denominator). Uses
+ * the orientation-aware `shapeHalfExtent`, so a rotated thin box reports its true
+ * thin axis. Returns 0 for a degenerate (flat/zero-extent) shape; callers treat a
+ * non-positive minExtent as "never fast" (Req 10, no divide).
+ */
+function minExtentOf(shape, q) {
+    const h = shapeHalfExtent(shape, q);
+    return Math.min(h.x, h.y, h.z);
+}
+/**
+ * Rotation-invariant bounding radius (max distance from the shape center to any
+ * surface point). Used as a conservative sphere for the fast-body TOI cast: a
+ * bounding sphere contacts a target no later than the true shape would, so the
+ * cast never lets a fast body tunnel (it stops it short, never past first contact).
+ */
+function boundingRadiusOf(shape) {
+    if (shape.kind === 'box') {
+        return (0, physics3d_shared_js_1.solverSqrt)(shape.half.x * shape.half.x + shape.half.y * shape.half.y + shape.half.z * shape.half.z);
+    }
+    if (shape.kind === 'sphere') {
+        return shape.radius;
+    }
+    if (shape.kind === 'capsule') {
+        return shape.radius + shape.halfHeight;
+    }
+    if (shape.kind === 'hull') {
+        let max2 = 0;
+        for (const v of shape.data.vertices) {
+            max2 = Math.max(max2, v.x * v.x + v.y * v.y + v.z * v.z);
+        }
+        return (0, physics3d_shared_js_1.solverSqrt)(max2);
+    }
+    if (shape.kind === 'mesh') {
+        let max2 = 0;
+        for (const tri of shape.triangles) {
+            for (const v of tri) {
+                max2 = Math.max(max2, v.x * v.x + v.y * v.y + v.z * v.z);
+            }
+        }
+        return (0, physics3d_shared_js_1.solverSqrt)(max2);
+    }
+    if (shape.kind === 'heightfield') {
+        const bounds = (0, physics3d_heightfield_js_1.heightfieldLocalBounds)(shape);
+        return (0, physics3d_shared_js_1.solverSqrt)(Math.max(bounds.minX * bounds.minX, bounds.maxX * bounds.maxX)
+            + Math.max(bounds.minY * bounds.minY, bounds.maxY * bounds.maxY)
+            + Math.max(bounds.minZ * bounds.minZ, bounds.maxZ * bounds.maxZ));
+    }
+    let max = 0;
+    for (const child of shape.children) {
+        const offset = (0, physics3d_shared_js_1.solverSqrt)(child.offset.x * child.offset.x + child.offset.y * child.offset.y + child.offset.z * child.offset.z);
+        max = Math.max(max, offset + boundingRadiusOf(child.shape));
+    }
+    return max;
 }
 function toGeom(b) {
-    return { index: b.index, p: b.p, q: b.q, shape: b.shape };
+    return { index: b.index, p: b.p, q: b.q, shape: b.shape, v: b.v };
 }
 function wakeIfContacted(sleeper, other) {
-    if (sleeper.isDynamic && sleeper.sleeping && other.isDynamic && !other.sleeping
+    // A kinematic mover carries no impulse but does displace what it touches, so
+    // it has to wake a sleeping pile the same way an awake dynamic neighbour does.
+    if (sleeper.isDynamic && sleeper.sleeping && (other.isDynamic || other.isKinematic) && !other.sleeping
         && (0, physics3d_shared_js_1.dotVec3)(other.v, other.v) + (0, physics3d_shared_js_1.dotVec3)(other.w, other.w) > exports.SOLVER_CONSTANTS.WAKE_VEL2) {
         sleeper.sleeping = false;
         sleeper.lowVelFrames = 0;
@@ -1223,7 +1934,7 @@ function relativeBiasVelocity(A, B, rA, rB) {
  * order as the vector-helper form (bit-identical results), with zero
  * intermediate allocations. This runs per contact per velocity iteration.
  */
-function applyImpulseScalar(bodies, invMass, invInertia, a, b, rA, rB, px, py, pz, bias) {
+function applyImpulseScalar(bodies, invMass, invInertia, a, b, rA, rB, px, py, pz) {
     const A = bodies[a];
     const B = bodies[b];
     if (solid(A)) {
@@ -1232,8 +1943,8 @@ function applyImpulseScalar(bodies, invMass, invInertia, a, b, rA, rB, px, py, p
         const cz = rA.x * py - rA.y * px;
         const m = invInertia[a];
         const inv = invMass[a];
-        const lin = bias ? A.vb : A.v;
-        const ang = bias ? A.wb : A.w;
+        const lin = A.v;
+        const ang = A.w;
         lin.x = lin.x - px * inv.x;
         lin.y = lin.y - py * inv.y;
         lin.z = lin.z - pz * inv.z;
@@ -1247,8 +1958,8 @@ function applyImpulseScalar(bodies, invMass, invInertia, a, b, rA, rB, px, py, p
         const cz = rB.x * py - rB.y * px;
         const m = invInertia[b];
         const inv = invMass[b];
-        const lin = bias ? B.vb : B.v;
-        const ang = bias ? B.wb : B.w;
+        const lin = B.v;
+        const ang = B.w;
         lin.x = lin.x + px * inv.x;
         lin.y = lin.y + py * inv.y;
         lin.z = lin.z + pz * inv.z;
@@ -1269,42 +1980,78 @@ function solveTangentImpulse(bodies, invMass, invInertia, c, A, B, t, mass, acc,
     let dF = mass * -vt;
     const clamped = (0, physics3d_shared_js_1.clampNumber)(acc + dF, -maxF, maxF);
     dF = clamped - acc;
-    applyImpulseScalar(bodies, invMass, invInertia, c.a, c.b, c.rA, c.rB, t.x * dF, t.y * dF, t.z * dF, false);
+    applyImpulseScalar(bodies, invMass, invInertia, c.a, c.b, c.rA, c.rB, t.x * dF, t.y * dF, t.z * dF);
     return clamped;
 }
-function solveContactConstraint(bodies, invMass, invInertia, c, dt) {
+function angularEffectiveMass(direction, invA, invB) {
+    const response = (0, physics3d_shared_js_1.addVec3)((0, physics3d_shared_js_1.mulMat3Vec)(invA, direction), (0, physics3d_shared_js_1.mulMat3Vec)(invB, direction));
+    const inverseMass = (0, physics3d_shared_js_1.dotVec3)(direction, response);
+    return inverseMass > 0 ? 1 / inverseMass : 0;
+}
+function solveRollingResistance(bodies, invInertia, c, A, B) {
+    const maxImpulse = c.rolling * c.accN;
+    if (maxImpulse <= 0) {
+        return;
+    }
+    const relative = (0, physics3d_shared_js_1.subVec3)(B.w, A.w);
+    const massR1 = angularEffectiveMass(c.t1, invInertia[c.a], invInertia[c.b]);
+    const massR2 = angularEffectiveMass(c.t2, invInertia[c.a], invInertia[c.b]);
+    let nextR1 = c.accR1 - massR1 * (0, physics3d_shared_js_1.dotVec3)(relative, c.t1);
+    let nextR2 = c.accR2 - massR2 * (0, physics3d_shared_js_1.dotVec3)(relative, c.t2);
+    const magnitude = (0, physics3d_shared_js_1.solverSqrt)(nextR1 * nextR1 + nextR2 * nextR2);
+    if (magnitude > maxImpulse) {
+        const scale = maxImpulse / magnitude;
+        nextR1 *= scale;
+        nextR2 *= scale;
+    }
+    const deltaR1 = nextR1 - c.accR1;
+    const deltaR2 = nextR2 - c.accR2;
+    c.accR1 = nextR1;
+    c.accR2 = nextR2;
+    const impulse = (0, physics3d_shared_js_1.addVec3)((0, physics3d_shared_js_1.scaleVec3)(c.t1, deltaR1), (0, physics3d_shared_js_1.scaleVec3)(c.t2, deltaR2));
+    if (solid(A)) {
+        A.w = (0, physics3d_shared_js_1.subVec3)(A.w, (0, physics3d_shared_js_1.mulMat3Vec)(invInertia[c.a], impulse));
+    }
+    if (solid(B)) {
+        B.w = (0, physics3d_shared_js_1.addVec3)(B.w, (0, physics3d_shared_js_1.mulMat3Vec)(invInertia[c.b], impulse));
+    }
+}
+/**
+ * One soft-constraint contact solve (box2d SoftStep). `useBias` on the bias pass
+ * folds the soft position bias into real velocity; the bias-free relax pass
+ * (`useBias=false`) removes that transient energy after position integration.
+ */
+function solveContactSoft(bodies, invMass, invInertia, c, h, useBias) {
     const A = bodies[c.a];
     const B = bodies[c.b];
-    // normal (no Baumgarte — position error handled by split impulse)
     const vn = relativeVelocityAlong(A, B, c.rA, c.rB, c.normal.x, c.normal.y, c.normal.z);
-    const restThreshold = exports.SOLVER_CONSTANTS.RESTITUTION_THRESHOLD;
-    const restBias = vn < -restThreshold ? -c.restitution * vn : 0;
-    // speculative points: allow closing up to the gap, never push apart.
-    const speculativeBias = c.speculative ? c.penetration / dt : 0;
-    let dLambda = c.massN * (-(vn) + restBias + speculativeBias);
+    let bias = 0;
+    let massScale = 1;
+    let impulseScale = 0;
+    if (c.penetration < 0) {
+        // speculative / separated: allow closing exactly up to the gap, never push apart.
+        bias = c.penetration / h;
+    }
+    else if (useBias) {
+        const raw = c.biasRate * (c.penetration - exports.SOLVER_CONSTANTS.SLOP);
+        bias = (0, physics3d_shared_js_1.clampNumber)(raw, 0, exports.SOLVER_CONSTANTS.MAX_CONTACT_PUSHOUT / h);
+        massScale = c.massScale;
+        impulseScale = c.impulseScale;
+    }
+    let dLambda = massScale * c.massN * (-(vn) + bias) - impulseScale * c.accN;
     const newAcc = Math.max(0, c.accN + dLambda);
     dLambda = newAcc - c.accN;
     c.accN = newAcc;
-    applyImpulseScalar(bodies, invMass, invInertia, c.a, c.b, c.rA, c.rB, c.normal.x * dLambda, c.normal.y * dLambda, c.normal.z * dLambda, false);
-    // split-impulse position bias (skip speculative points).
-    if (!c.speculative) {
-        const relbX = (B.vb.x + (B.wb.y * c.rB.z - B.wb.z * c.rB.y)) - (A.vb.x + (A.wb.y * c.rA.z - A.wb.z * c.rA.y));
-        const relbY = (B.vb.y + (B.wb.z * c.rB.x - B.wb.x * c.rB.z)) - (A.vb.y + (A.wb.z * c.rA.x - A.wb.x * c.rA.z));
-        const relbZ = (B.vb.z + (B.wb.x * c.rB.y - B.wb.y * c.rB.x)) - (A.vb.z + (A.wb.x * c.rA.y - A.wb.y * c.rA.x));
-        const vnb = relbX * c.normal.x + relbY * c.normal.y + relbZ * c.normal.z;
-        let dLb = c.massN * (c.bias - vnb);
-        const newAccNb = Math.max(0, c.accNb + dLb);
-        dLb = newAccNb - c.accNb;
-        c.accNb = newAccNb;
-        applyImpulseScalar(bodies, invMass, invInertia, c.a, c.b, c.rA, c.rB, c.normal.x * dLb, c.normal.y * dLb, c.normal.z * dLb, true);
-    }
-    // friction (skip for pure speculative points).
-    if (c.speculative) {
-        return;
+    applyImpulseScalar(bodies, invMass, invInertia, c.a, c.b, c.rA, c.rB, c.normal.x * dLambda, c.normal.y * dLambda, c.normal.z * dLambda);
+    if (c.penetration < 0) {
+        return; // friction skipped for separated/speculative points (unchanged policy)
     }
     const maxF = c.mu * c.accN;
     c.accT1 = solveTangentImpulse(bodies, invMass, invInertia, c, A, B, c.t1, c.massT1, c.accT1, maxF);
     c.accT2 = solveTangentImpulse(bodies, invMass, invInertia, c, A, B, c.t2, c.massT2, c.accT2, maxF);
+    if (!useBias && c.rolling > 0) {
+        solveRollingResistance(bodies, invInertia, c, A, B);
+    }
 }
 function islandSleep(bodies, manifolds, joints) {
     const parent = new Map();