@woosh/meep-engine 2.138.20 → 2.139.0

package/src/engine/physics/narrowphase/box_box_manifold.js

@@ -0,0 +1,543 @@
+/**
+ * Multi-point manifold construction for two oriented bounding boxes.
+ *
+ * Algorithm:
+ *  1. SAT over the canonical 15 axes (3 face normals of A, 3 of B, 9 edge-pair
+ *     crosses) to find the smallest-overlap axis and remember whether it came
+ *     from a face normal of A, a face normal of B, or an edge-pair cross.
+ *  2. Face axis ⇒ identify a reference face (on the box that owned the axis)
+ *     and an incident face (on the opposite box, the face whose outward
+ *     normal is most antiparallel to the contact normal). Project the four
+ *     incident corners into the reference face's 2-D coordinate frame and
+ *     clip the resulting quad against the reference face rectangle via
+ *     Sutherland-Hodgman. Each surviving vertex with positive penetration
+ *     becomes a contact point.
+ *  3. Edge-cross axis ⇒ a single contact at the body-centre midpoint
+ *     (v1 fallback; closest-edge-points refinement is a follow-up).
+ *  4. If more than {@link MAX_CONTACTS} survive, reduce by picking the
+ *     deepest first, then three more that maximise the perimeter of the
+ *     resulting quad.
+ *
+ * Contact convention (matches the manifold cache): the normal points from
+ * box B toward box A; positive depth means penetration.
+ *
+ * Output layout:
+ *   out[0..2] : world normal
+ *   out[3]    : contact count (0 .. {@link MAX_CONTACTS})
+ *   out[4 + k*7 + 0..2] : world contact on A's surface
+ *   out[4 + k*7 + 3..5] : world contact on B's surface
+ *   out[4 + k*7 + 6]    : penetration depth (positive)
+ *
+ * @author Alex Goldring
+ * @copyright Company Named Limited (c) 2026
+ */
+
+const MAX_CONTACTS = 4;
+const CONTACT_STRIDE = 7;
+const PARALLEL_EPS_SQR = 1e-8;
+
+/**
+ * Length of `out` required by {@link box_box_manifold}.
+ * @type {number}
+ */
+export const BOX_BOX_OUT_LENGTH = 4 + MAX_CONTACTS * CONTACT_STRIDE;
+
+// --- shared scratch storage -------------------------------------------------
+
+const scratch_axes_a = new Float64Array(9);
+const scratch_axes_b = new Float64Array(9);
+
+// 4 incident face corners in world space.
+const incident_world = new Float64Array(12);
+// Clipped polygon — worst case 8 vertices (quad clipped by 4 planes).
+const clip_in  = new Float64Array(8 * 3);
+const clip_out = new Float64Array(8 * 3);
+// 2-D projection of incident corners onto reference-face uv plane.
+const incident_uv  = new Float64Array(8 * 2);
+const clipped_uv_in  = new Float64Array(8 * 2);
+const clipped_uv_out = new Float64Array(8 * 2);
+
+// Surviving 3-D contact points along with their (signed) depth.
+// Stride 4 per candidate: x, y, z, depth.
+const candidates = new Float64Array(8 * 4);
+
+// --- helpers ---------------------------------------------------------------
+
+function quat_to_axes(out, off, qx, qy, qz, qw) {
+    const xx = qx * qx, yy = qy * qy, zz = qz * qz;
+    const xy = qx * qy, xz = qx * qz, yz = qy * qz;
+    const wx = qw * qx, wy = qw * qy, wz = qw * qz;
+    out[off]     = 1 - 2 * (yy + zz); out[off + 1] = 2 * (xy + wz);     out[off + 2] = 2 * (xz - wy);
+    out[off + 3] = 2 * (xy - wz);     out[off + 4] = 1 - 2 * (xx + zz); out[off + 5] = 2 * (yz + wx);
+    out[off + 6] = 2 * (xz + wy);     out[off + 7] = 2 * (yz - wx);     out[off + 8] = 1 - 2 * (xx + yy);
+}
+
+function projected_half_extent(axes, off, hx, hy, hz, lx, ly, lz) {
+    const px = lx * axes[off]     + ly * axes[off + 1] + lz * axes[off + 2];
+    const py = lx * axes[off + 3] + ly * axes[off + 4] + lz * axes[off + 5];
+    const pz = lx * axes[off + 6] + ly * axes[off + 7] + lz * axes[off + 8];
+    return (px < 0 ? -px : px) * hx
+         + (py < 0 ? -py : py) * hy
+         + (pz < 0 ? -pz : pz) * hz;
+}
+
+/**
+ * Sutherland-Hodgman clip of `points_in_uv` (point_count vertices, stride 2)
+ * against the half-plane `coord_idx ≤ bound` (or `coord_idx ≥ bound` when
+ * `keep_below` is false). Result is written to `points_out_uv`, returns the
+ * vertex count.
+ *
+ * @param {Float64Array} points_in
+ * @param {number} point_count
+ * @param {Float64Array} points_out
+ * @param {number} coord_idx 0 = u, 1 = v
+ * @param {number} bound
+ * @param {boolean} keep_below true to keep points with coord ≤ bound
+ * @returns {number} surviving vertex count
+ */
+function clip_against_axis(points_in, point_count, points_out, coord_idx, bound, keep_below) {
+    let out_count = 0;
+    for (let i = 0; i < point_count; i++) {
+        const j = (i + 1) % point_count;
+        const ax = points_in[i * 2],     ay = points_in[i * 2 + 1];
+        const bx = points_in[j * 2],     by = points_in[j * 2 + 1];
+        const av = coord_idx === 0 ? ax : ay;
+        const bv = coord_idx === 0 ? bx : by;
+        const a_inside = keep_below ? (av <= bound) : (av >= bound);
+        const b_inside = keep_below ? (bv <= bound) : (bv >= bound);
+
+        if (a_inside) {
+            // emit a
+            points_out[out_count * 2] = ax;
+            points_out[out_count * 2 + 1] = ay;
+            out_count++;
+        }
+        if (a_inside !== b_inside) {
+            // emit intersection on the bounding line
+            // Parameterise: p(t) = a + t * (b - a); solve coord(p(t)) = bound.
+            const denom = bv - av;
+            const t = denom !== 0 ? (bound - av) / denom : 0;
+            points_out[out_count * 2]     = ax + (bx - ax) * t;
+            points_out[out_count * 2 + 1] = ay + (by - ay) * t;
+            out_count++;
+        }
+    }
+    return out_count;
+}
+
+// --- main -------------------------------------------------------------------
+
+/**
+ *
+ * @param {number[]|Float64Array} out length >= BOX_BOX_OUT_LENGTH
+ * @param {number} ax centre x of A
+ * @param {number} ay
+ * @param {number} az
+ * @param {number} aqx A rotation quaternion x
+ * @param {number} aqy
+ * @param {number} aqz
+ * @param {number} aqw
+ * @param {number} ahx A half-extent x in body frame
+ * @param {number} ahy
+ * @param {number} ahz
+ * @param {number} bx centre x of B
+ * @param {number} by
+ * @param {number} bz
+ * @param {number} bqx
+ * @param {number} bqy
+ * @param {number} bqz
+ * @param {number} bqw
+ * @param {number} bhx
+ * @param {number} bhy
+ * @param {number} bhz
+ * @returns {boolean} true if overlap
+ */
+export function box_box_manifold(
+    out,
+    ax, ay, az, aqx, aqy, aqz, aqw, ahx, ahy, ahz,
+    bx, by, bz, bqx, bqy, bqz, bqw, bhx, bhy, bhz
+) {
+    quat_to_axes(scratch_axes_a, 0, aqx, aqy, aqz, aqw);
+    quat_to_axes(scratch_axes_b, 0, bqx, bqy, bqz, bqw);
+
+    const ta = scratch_axes_a;
+    const tb = scratch_axes_b;
+
+    const dx = bx - ax;
+    const dy = by - ay;
+    const dz = bz - az;
+
+    // SAT — track winning axis + source.
+    // source: 0..2 = A face normal, 3..5 = B face normal, 6..14 = edge cross.
+    let best_overlap = Infinity;
+    let best_nx = 0, best_ny = 0, best_nz = 0;
+    let best_source = -1;
+
+    function test_axis(lx, ly, lz, source) {
+        const len_sqr = lx * lx + ly * ly + lz * lz;
+        if (len_sqr < PARALLEL_EPS_SQR) return false;
+        const inv_len = 1 / Math.sqrt(len_sqr);
+        const ux = lx * inv_len, uy = ly * inv_len, uz = lz * inv_len;
+        const rA = projected_half_extent(ta, 0, ahx, ahy, ahz, ux, uy, uz);
+        const rB = projected_half_extent(tb, 0, bhx, bhy, bhz, ux, uy, uz);
+        const proj = dx * ux + dy * uy + dz * uz;
+        const dist = proj < 0 ? -proj : proj;
+        const overlap = (rA + rB) - dist;
+        if (overlap < 0) return true;
+        if (overlap < best_overlap) {
+            best_overlap = overlap;
+            const sign = proj > 0 ? -1 : 1; // normal points B→A
+            best_nx = ux * sign;
+            best_ny = uy * sign;
+            best_nz = uz * sign;
+            best_source = source;
+        }
+        return false;
+    }
+
+    if (test_axis(ta[0], ta[1], ta[2], 0)) return false;
+    if (test_axis(ta[3], ta[4], ta[5], 1)) return false;
+    if (test_axis(ta[6], ta[7], ta[8], 2)) return false;
+    if (test_axis(tb[0], tb[1], tb[2], 3)) return false;
+    if (test_axis(tb[3], tb[4], tb[5], 4)) return false;
+    if (test_axis(tb[6], tb[7], tb[8], 5)) return false;
+
+    for (let i = 0; i < 3; i++) {
+        const aix = ta[i * 3], aiy = ta[i * 3 + 1], aiz = ta[i * 3 + 2];
+        for (let j = 0; j < 3; j++) {
+            const bjx = tb[j * 3], bjy = tb[j * 3 + 1], bjz = tb[j * 3 + 2];
+            const cx = aiy * bjz - aiz * bjy;
+            const cy = aiz * bjx - aix * bjz;
+            const cz = aix * bjy - aiy * bjx;
+            if (test_axis(cx, cy, cz, 6 + i * 3 + j)) return false;
+        }
+    }
+
+    // We have a contact. Output the normal.
+    const nx = best_nx, ny = best_ny, nz = best_nz;
+    out[0] = nx; out[1] = ny; out[2] = nz;
+
+    // ---- Edge-cross axis: single-point fallback ----
+    if (best_source >= 6) {
+        const mx = (ax + bx) * 0.5;
+        const my = (ay + by) * 0.5;
+        const mz = (az + bz) * 0.5;
+        out[3] = 1;
+        out[4] = mx; out[5] = my;  out[6] = mz;
+        out[7] = mx; out[8] = my;  out[9] = mz;
+        out[10] = best_overlap;
+        return true;
+    }
+
+    // ---- Face axis: face clipping ----
+    // Determine reference and incident.
+    const ref_is_A = best_source < 3;
+    // Reference axis index in the owning box (0..2).
+    const ref_axis_idx = ref_is_A ? best_source : best_source - 3;
+
+    // Reference box pose.
+    const ref_axes = ref_is_A ? ta : tb;
+    const inc_axes = ref_is_A ? tb : ta;
+    const ref_h_x = ref_is_A ? ahx : bhx;
+    const ref_h_y = ref_is_A ? ahy : bhy;
+    const ref_h_z = ref_is_A ? ahz : bhz;
+    const inc_h_x = ref_is_A ? bhx : ahx;
+    const inc_h_y = ref_is_A ? bhy : ahy;
+    const inc_h_z = ref_is_A ? bhz : ahz;
+    const ref_cx = ref_is_A ? ax : bx;
+    const ref_cy = ref_is_A ? ay : by;
+    const ref_cz = ref_is_A ? az : bz;
+    const inc_cx = ref_is_A ? bx : ax;
+    const inc_cy = ref_is_A ? by : ay;
+    const inc_cz = ref_is_A ? bz : az;
+
+    // Reference face outward normal in world: the face of the ref box facing
+    // the incident box. The contact normal points B→A; the ref face's
+    // outward normal must point toward the incident box (= -n if ref is A,
+    // = +n if ref is B).
+    const ref_out_nx = ref_is_A ? -nx : nx;
+    const ref_out_ny = ref_is_A ? -ny : ny;
+    const ref_out_nz = ref_is_A ? -nz : nz;
+
+    // Ref face's three axis-in-world directions: the axis-direction matching
+    // ref_axis_idx is the OUTWARD normal; the other two are the face tangents.
+    // Sign of the reference-face axis aligned with ref_out:
+    const ref_axis_world_x = ref_axes[ref_axis_idx * 3];
+    const ref_axis_world_y = ref_axes[ref_axis_idx * 3 + 1];
+    const ref_axis_world_z = ref_axes[ref_axis_idx * 3 + 2];
+    const ref_axis_dot = ref_axis_world_x * ref_out_nx
+                       + ref_axis_world_y * ref_out_ny
+                       + ref_axis_world_z * ref_out_nz;
+    const ref_axis_sign = ref_axis_dot >= 0 ? 1 : -1;
+
+    // Face plane: passes through `ref_cx + ref_axis * ref_axis_sign * ref_h_<axis>`,
+    // with outward normal = `ref_axis_world * ref_axis_sign`.
+    const ref_h_along_axis = ref_axis_idx === 0 ? ref_h_x : (ref_axis_idx === 1 ? ref_h_y : ref_h_z);
+    const ref_face_origin_x = ref_cx + ref_axis_world_x * ref_axis_sign * ref_h_along_axis;
+    const ref_face_origin_y = ref_cy + ref_axis_world_y * ref_axis_sign * ref_h_along_axis;
+    const ref_face_origin_z = ref_cz + ref_axis_world_z * ref_axis_sign * ref_h_along_axis;
+
+    // Reference face's two tangent axes (u, v) in world, with the corresponding half-extents.
+    let u_axis_idx, v_axis_idx;
+    if (ref_axis_idx === 0)      { u_axis_idx = 1; v_axis_idx = 2; }
+    else if (ref_axis_idx === 1) { u_axis_idx = 2; v_axis_idx = 0; }
+    else                         { u_axis_idx = 0; v_axis_idx = 1; }
+
+    const ux = ref_axes[u_axis_idx * 3],     uy = ref_axes[u_axis_idx * 3 + 1], uz = ref_axes[u_axis_idx * 3 + 2];
+    const vx = ref_axes[v_axis_idx * 3],     vy = ref_axes[v_axis_idx * 3 + 1], vz = ref_axes[v_axis_idx * 3 + 2];
+    const half_u = u_axis_idx === 0 ? ref_h_x : (u_axis_idx === 1 ? ref_h_y : ref_h_z);
+    const half_v = v_axis_idx === 0 ? ref_h_x : (v_axis_idx === 1 ? ref_h_y : ref_h_z);
+
+    // ---- Pick incident face on the OTHER box ----
+    // Its outward normal should be MOST antiparallel to the ref face outward normal
+    // (i.e., the face of the incident box that is "looking back" at the ref face).
+    let inc_axis_idx = 0, inc_axis_sign = 1;
+    let max_anti = -Infinity;
+    for (let i = 0; i < 3; i++) {
+        const iax = inc_axes[i * 3], iay = inc_axes[i * 3 + 1], iaz = inc_axes[i * 3 + 2];
+        const d_pos = iax * ref_out_nx + iay * ref_out_ny + iaz * ref_out_nz;
+        // We want dot ≈ -1, so most negative dot is the most anti-parallel
+        if (-d_pos > max_anti) { max_anti = -d_pos; inc_axis_idx = i; inc_axis_sign = -1; }
+        if ( d_pos > max_anti) { max_anti =  d_pos; inc_axis_idx = i; inc_axis_sign =  1; }
+    }
+    // Now the incident face outward normal must be antiparallel to ref_out_n.
+    // We chose inc_axis_sign as the sign of dot(inc_axis_dir, ref_out_n). The
+    // incident face's outward direction is the OPPOSITE sign (facing back at ref).
+    const inc_outward_sign = -inc_axis_sign;
+
+    const inc_h_along_axis = inc_axis_idx === 0 ? inc_h_x : (inc_axis_idx === 1 ? inc_h_y : inc_h_z);
+    // Incident face centre in world.
+    const inc_face_cx = inc_cx + inc_axes[inc_axis_idx * 3]     * inc_outward_sign * inc_h_along_axis;
+    const inc_face_cy = inc_cy + inc_axes[inc_axis_idx * 3 + 1] * inc_outward_sign * inc_h_along_axis;
+    const inc_face_cz = inc_cz + inc_axes[inc_axis_idx * 3 + 2] * inc_outward_sign * inc_h_along_axis;
+
+    // Incident face's two tangent axes in world, with their half-extents.
+    let i_u_idx, i_v_idx;
+    if (inc_axis_idx === 0)      { i_u_idx = 1; i_v_idx = 2; }
+    else if (inc_axis_idx === 1) { i_u_idx = 2; i_v_idx = 0; }
+    else                         { i_u_idx = 0; i_v_idx = 1; }
+    const iux = inc_axes[i_u_idx * 3], iuy = inc_axes[i_u_idx * 3 + 1], iuz = inc_axes[i_u_idx * 3 + 2];
+    const ivx = inc_axes[i_v_idx * 3], ivy = inc_axes[i_v_idx * 3 + 1], ivz = inc_axes[i_v_idx * 3 + 2];
+    const i_half_u = i_u_idx === 0 ? inc_h_x : (i_u_idx === 1 ? inc_h_y : inc_h_z);
+    const i_half_v = i_v_idx === 0 ? inc_h_x : (i_v_idx === 1 ? inc_h_y : inc_h_z);
+
+    // Build incident face's 4 world corners (ordered CCW around the face).
+    const signs_u = [ 1,  1, -1, -1];
+    const signs_v = [-1,  1,  1, -1];
+    for (let i = 0; i < 4; i++) {
+        const su = signs_u[i] * i_half_u;
+        const sv = signs_v[i] * i_half_v;
+        incident_world[i * 3]     = inc_face_cx + iux * su + ivx * sv;
+        incident_world[i * 3 + 1] = inc_face_cy + iuy * su + ivy * sv;
+        incident_world[i * 3 + 2] = inc_face_cz + iuz * su + ivz * sv;
+    }
+
+    // Project incident corners into ref-face uv coordinates.
+    // For each corner C: uv = ((C - ref_face_origin) · u_axis,  ... · v_axis).
+    for (let i = 0; i < 4; i++) {
+        const ex = incident_world[i * 3]     - ref_face_origin_x;
+        const ey = incident_world[i * 3 + 1] - ref_face_origin_y;
+        const ez = incident_world[i * 3 + 2] - ref_face_origin_z;
+        incident_uv[i * 2]     = ex * ux + ey * uy + ez * uz;
+        incident_uv[i * 2 + 1] = ex * vx + ey * vy + ez * vz;
+    }
+
+    // Sutherland-Hodgman clip against the rectangle |u|<=half_u, |v|<=half_v.
+    // Apply 4 successive single-axis clips.
+    let n = 4;
+    // Copy into clipped_uv_in first.
+    for (let i = 0; i < n * 2; i++) clipped_uv_in[i] = incident_uv[i];
+
+    n = clip_against_axis(clipped_uv_in, n, clipped_uv_out, 0, half_u, true);
+    if (n === 0) return out_empty(out);
+    n = clip_against_axis(clipped_uv_out, n, clipped_uv_in, 0, -half_u, false);
+    if (n === 0) return out_empty(out);
+    n = clip_against_axis(clipped_uv_in, n, clipped_uv_out, 1, half_v, true);
+    if (n === 0) return out_empty(out);
+    n = clip_against_axis(clipped_uv_out, n, clipped_uv_in, 1, -half_v, false);
+    if (n === 0) return out_empty(out);
+
+    // Each surviving uv-point is on the ref face rectangle. We need to:
+    //  1. Map uv back to world → that's the contact point on the *incident* face
+    //     (still on the inc face plane at that uv).
+    //  2. Compute depth = penetration along contact normal n (B→A).
+    //  3. The contact on the *reference* face is the projection of the inc-face
+    //     point onto the reference plane along the contact normal.
+
+    // To re-derive the world position from a uv coord: we need to keep both the
+    // uv coord and the corresponding world point. But we just clipped uv-only,
+    // so the world position is the projection back. The depth resolves it:
+    //
+    //   ref plane: normal = ref_outward, origin at ref_face_origin
+    //   incident point world = ref_face_origin + u*u_axis + v*v_axis  + (signed projection along ref_out)
+    //
+    // The component along ref_out for the original incident corner is what we lost
+    // by going to uv. We recompute it from the original incident face plane (which
+    // is at a fixed offset from ref): the inc corner's signed distance from the ref
+    // plane is `(corner - ref_face_origin) · ref_out_n`.
+    //
+    // After clipping in uv, we can't directly recover that distance without
+    // re-projecting onto the inc face plane. We do that here.
+
+    const ref_out_x = ref_axis_world_x * ref_axis_sign;
+    const ref_out_y = ref_axis_world_y * ref_axis_sign;
+    const ref_out_z = ref_axis_world_z * ref_axis_sign;
+
+    // Inc face plane (in world): point inc_face_c, normal inc_outward
+    // = inc_axes[inc_axis_idx*3..+2] * inc_outward_sign.
+    const inc_out_x = inc_axes[inc_axis_idx * 3]     * inc_outward_sign;
+    const inc_out_y = inc_axes[inc_axis_idx * 3 + 1] * inc_outward_sign;
+    const inc_out_z = inc_axes[inc_axis_idx * 3 + 2] * inc_outward_sign;
+
+    // For a clipped uv point P_uv, the corresponding world point P_world on the
+    // incident face plane satisfies:
+    //   P_world = ref_face_origin + u*u_axis + v*v_axis + t * ref_out  for some t
+    //   (P_world - inc_face_c) · inc_out = 0
+    // Substitute and solve for t. Let:
+    //   X = ref_face_origin + u*u_axis + v*v_axis  (a point on the line)
+    //   t such that (X + t*ref_out - inc_face_c) · inc_out = 0
+    //   t * (ref_out · inc_out) = (inc_face_c - X) · inc_out
+    const ref_dot_inc = ref_out_x * inc_out_x + ref_out_y * inc_out_y + ref_out_z * inc_out_z;
+    // Faces are nearly parallel → ref_dot_inc ≈ -1 (anti-parallel). Robust.
+
+    // Build candidates: for each clipped uv point.
+    let cand_count = 0;
+    const clipped_uv = clipped_uv_in;
+    for (let i = 0; i < n; i++) {
+        const u = clipped_uv[i * 2];
+        const v = clipped_uv[i * 2 + 1];
+
+        const x_x = ref_face_origin_x + u * ux + v * vx;
+        const x_y = ref_face_origin_y + u * uy + v * vy;
+        const x_z = ref_face_origin_z + u * uz + v * vz;
+
+        let world_inc_x, world_inc_y, world_inc_z;
+        if (ref_dot_inc !== 0) {
+            const num_x = inc_face_cx - x_x;
+            const num_y = inc_face_cy - x_y;
+            const num_z = inc_face_cz - x_z;
+            const t = (num_x * inc_out_x + num_y * inc_out_y + num_z * inc_out_z) / ref_dot_inc;
+            world_inc_x = x_x + ref_out_x * t;
+            world_inc_y = x_y + ref_out_y * t;
+            world_inc_z = x_z + ref_out_z * t;
+        } else {
+            // Faces orthogonal — degenerate, fall back to uv point on ref plane.
+            world_inc_x = x_x; world_inc_y = x_y; world_inc_z = x_z;
+        }
+
+        // Penetration: signed distance of inc-face contact from the ref plane,
+        // measured into the ref box (= along -ref_out). Positive means penetrating.
+        const sep_x = world_inc_x - ref_face_origin_x;
+        const sep_y = world_inc_y - ref_face_origin_y;
+        const sep_z = world_inc_z - ref_face_origin_z;
+        const sep_along_ref_out = sep_x * ref_out_x + sep_y * ref_out_y + sep_z * ref_out_z;
+        const depth = -sep_along_ref_out;
+        if (depth < 0) continue;
+
+        candidates[cand_count * 4]     = world_inc_x;
+        candidates[cand_count * 4 + 1] = world_inc_y;
+        candidates[cand_count * 4 + 2] = world_inc_z;
+        candidates[cand_count * 4 + 3] = depth;
+        cand_count++;
+    }
+
+    if (cand_count === 0) return out_empty(out);
+
+    // ---- Reduce to ≤ MAX_CONTACTS by best-deepest + perimeter ----
+    const kept = reduce_contacts(cand_count);
+
+    // ---- Emit: contact-on-incident is the candidate's world point;
+    //          contact-on-reference is the same point projected onto the ref plane.
+    out[3] = kept;
+    for (let k = 0; k < kept; k++) {
+        const px = candidates[k * 4];
+        const py = candidates[k * 4 + 1];
+        const pz = candidates[k * 4 + 2];
+        const depth = candidates[k * 4 + 3];
+
+        // ref-side contact = project onto ref plane along ref_out.
+        const dist_to_plane = (px - ref_face_origin_x) * ref_out_x
+                            + (py - ref_face_origin_y) * ref_out_y
+                            + (pz - ref_face_origin_z) * ref_out_z;
+        const rpx = px - ref_out_x * dist_to_plane;
+        const rpy = py - ref_out_y * dist_to_plane;
+        const rpz = pz - ref_out_z * dist_to_plane;
+
+        let wax, way, waz, wbx_, wby_, wbz_;
+        if (ref_is_A) {
+            wax = rpx; way = rpy; waz = rpz;
+            wbx_ = px; wby_ = py; wbz_ = pz;
+        } else {
+            wax = px;  way = py;  waz = pz;
+            wbx_ = rpx; wby_ = rpy; wbz_ = rpz;
+        }
+
+        const base = 4 + k * CONTACT_STRIDE;
+        out[base]     = wax;  out[base + 1] = way;  out[base + 2] = waz;
+        out[base + 3] = wbx_; out[base + 4] = wby_; out[base + 5] = wbz_;
+        out[base + 6] = depth;
+    }
+    return true;
+}
+
+function out_empty(out) {
+    out[3] = 0;
+    return true; // overlap exists per SAT, just no clipped manifold
+}
+
+/**
+ * In-place reduction of `candidates` to at most {@link MAX_CONTACTS}:
+ *   1. Sort by depth descending; deepest stays at index 0.
+ *   2. From the remaining, greedily pick the points that maximise the spread
+ *      (sum of squared distances to already-kept points). This approximates
+ *      the maximum-area heuristic without needing the actual area.
+ *
+ * @param {number} n input count
+ * @returns {number} kept count (min(n, MAX_CONTACTS))
+ */
+function reduce_contacts(n) {
+    if (n <= MAX_CONTACTS) return n;
+
+    // Find deepest, swap into slot 0.
+    let deepest_idx = 0;
+    let deepest_val = candidates[3];
+    for (let i = 1; i < n; i++) {
+        if (candidates[i * 4 + 3] > deepest_val) {
+            deepest_val = candidates[i * 4 + 3];
+            deepest_idx = i;
+        }
+    }
+    if (deepest_idx !== 0) swap_candidate(0, deepest_idx);
+
+    // Greedy: for slot k in 1..MAX_CONTACTS-1, pick the remaining candidate
+    // whose min-distance-sq-to-already-kept-set is largest.
+    for (let k = 1; k < MAX_CONTACTS; k++) {
+        let best_score = -1;
+        let best_i = -1;
+        for (let i = k; i < n; i++) {
+            let min_d2 = Infinity;
+            for (let j = 0; j < k; j++) {
+                const dx = candidates[i * 4]     - candidates[j * 4];
+                const dy = candidates[i * 4 + 1] - candidates[j * 4 + 1];
+                const dz = candidates[i * 4 + 2] - candidates[j * 4 + 2];
+                const d2 = dx * dx + dy * dy + dz * dz;
+                if (d2 < min_d2) min_d2 = d2;
+            }
+            if (min_d2 > best_score) { best_score = min_d2; best_i = i; }
+        }
+        if (best_i !== k) swap_candidate(k, best_i);
+    }
+    return MAX_CONTACTS;
+}
+
+function swap_candidate(a, b) {
+    const ax = candidates[a * 4], ay = candidates[a * 4 + 1], az = candidates[a * 4 + 2], ad = candidates[a * 4 + 3];
+    candidates[a * 4]     = candidates[b * 4];
+    candidates[a * 4 + 1] = candidates[b * 4 + 1];
+    candidates[a * 4 + 2] = candidates[b * 4 + 2];
+    candidates[a * 4 + 3] = candidates[b * 4 + 3];
+    candidates[b * 4]     = ax;
+    candidates[b * 4 + 1] = ay;
+    candidates[b * 4 + 2] = az;
+    candidates[b * 4 + 3] = ad;
+}

package/src/engine/physics/narrowphase/capsule_contacts.d.ts

@@ -0,0 +1,122 @@
+/**
+ * Compute the two world endpoints of a capsule's central segment given its
+ * pose. The capsule is Y-aligned in body frame; the segment runs from
+ * `(0, -h/2, 0)` to `(0, +h/2, 0)`. Writes endpoints into `out` as
+ * `[ax, ay, az, bx, by, bz]`.
+ *
+ * @param {number[]|Float64Array} out length >= 6
+ * @param {number} cx capsule centre x
+ * @param {number} cy
+ * @param {number} cz
+ * @param {number} qx quaternion x
+ * @param {number} qy
+ * @param {number} qz
+ * @param {number} qw
+ * @param {number} half_height
+ */
+export function capsule_world_segment(out: number[] | Float64Array, cx: number, cy: number, cz: number, qx: number, qy: number, qz: number, qw: number, half_height: number): void;
+/**
+ * Capsule (A) vs sphere (B) contact, closed-form via point-segment distance.
+ *
+ * @param {number[]|Float64Array} out length >= 10
+ * @param {number} a_cx  capsule centre x
+ * @param {number} a_cy
+ * @param {number} a_cz
+ * @param {number} a_qx  capsule rotation
+ * @param {number} a_qy
+ * @param {number} a_qz
+ * @param {number} a_qw
+ * @param {number} a_radius
+ * @param {number} a_half_h capsule half-height (not including caps)
+ * @param {number} b_cx  sphere centre x
+ * @param {number} b_cy
+ * @param {number} b_cz
+ * @param {number} b_radius
+ * @returns {boolean} true on overlap
+ */
+export function capsule_sphere_contact(out: number[] | Float64Array, a_cx: number, a_cy: number, a_cz: number, a_qx: number, a_qy: number, a_qz: number, a_qw: number, a_radius: number, a_half_h: number, b_cx: number, b_cy: number, b_cz: number, b_radius: number): boolean;
+/**
+ * Two capsules (A and B). Reduces to segment-segment closest pair + radius.
+ *
+ * @param {number[]|Float64Array} out length >= 10
+ * @returns {boolean} true on overlap
+ */
+export function capsule_capsule_contact(out: number[] | Float64Array, a_cx: any, a_cy: any, a_cz: any, a_qx: any, a_qy: any, a_qz: any, a_qw: any, a_radius: any, a_half_h: any, b_cx: any, b_cy: any, b_cz: any, b_qx: any, b_qy: any, b_qz: any, b_qw: any, b_radius: any, b_half_h: any): boolean;
+/**
+ * Capsule (A) vs oriented box (B). Brings the capsule segment into box-local
+ * space, then refines via 2 fixed-point iterations between:
+ *   - closest point on the segment to the current box-side candidate
+ *   - clamped point on the box closest to the current segment-side candidate
+ *
+ * Two iterations suffice for the cases this engine targets (sphere-like cap
+ * geometry against axis-aligned-in-local box). When the segment passes
+ * through the box, the "inside" branch picks the smallest-overlap face
+ * deterministically, mirroring {@link sphere_box_contact}.
+ *
+ * @param {number[]|Float64Array} out length >= 10
+ * @returns {boolean}
+ */
+export function capsule_box_contact(out: number[] | Float64Array, a_cx: any, a_cy: any, a_cz: any, a_qx: any, a_qy: any, a_qz: any, a_qw: any, a_radius: any, a_half_h: any, b_cx: any, b_cy: any, b_cz: any, b_qx: any, b_qy: any, b_qz: any, b_qw: any, b_hx: any, b_hy: any, b_hz: any): boolean;
+/**
+ * Multi-point capsule-vs-OBB contact generation. Emits up to
+ * {@link CAPSULE_BOX_MAX_CONTACTS} contacts into `out` in the
+ * `(wax, way, waz, wbx, wby, wbz, nx, ny, nz, depth)` stride layout.
+ *
+ * The motivation: a capsule lying flat on a face needs more than one
+ * contact to rest stably — a single contact at the segment's closest point
+ * is positionally ambiguous (every interior segment point is equidistant
+ * from the face), and the body wobbles around whichever point the
+ * iterative closest-point solver happened to converge on. Two contacts at
+ * the cap centres pin the capsule's rotation about its long axis.
+ *
+ * Algorithm:
+ *  1. Closest-point-on-segment vs OBB → primary contact (same as
+ *     {@link capsule_box_contact}).
+ *  2. Treat each cap centre as a sphere of cap-radius and run
+ *     {@link sphere_box_contact} → up to two additional contacts.
+ *  3. Dedupe spatially against the primary so a capsule touching with one
+ *     cap doesn't emit two contacts at the same point.
+ *
+ * @param {Float64Array} out length >= CAPSULE_BOX_MAX_CONTACTS * CAPSULE_BOX_CONTACT_STRIDE
+ * @param {number} a_cx capsule centre x
+ * @param {number} a_cy
+ * @param {number} a_cz
+ * @param {number} a_qx capsule rotation
+ * @param {number} a_qy
+ * @param {number} a_qz
+ * @param {number} a_qw
+ * @param {number} a_radius
+ * @param {number} a_half_h
+ * @param {number} b_cx box centre x
+ * @param {number} b_cy
+ * @param {number} b_cz
+ * @param {number} b_qx
+ * @param {number} b_qy
+ * @param {number} b_qz
+ * @param {number} b_qw
+ * @param {number} b_hx
+ * @param {number} b_hy
+ * @param {number} b_hz
+ * @returns {number} number of contacts emitted, in `[0, CAPSULE_BOX_MAX_CONTACTS]`
+ */
+export function capsule_box_multi_contacts(out: Float64Array, a_cx: number, a_cy: number, a_cz: number, a_qx: number, a_qy: number, a_qz: number, a_qw: number, a_radius: number, a_half_h: number, b_cx: number, b_cy: number, b_cz: number, b_qx: number, b_qy: number, b_qz: number, b_qw: number, b_hx: number, b_hy: number, b_hz: number): number;
+/**
+ * Output stride per contact in {@link capsule_box_multi_contacts}.
+ * Matches the candidate-buffer layout used by `narrowphase_step`.
+ *
+ *   0..2  : world contact on capsule side
+ *   3..5  : world contact on box side
+ *   6..8  : normal from box (B) toward capsule (A)
+ *   9     : depth (positive = penetration)
+ *
+ * @type {number}
+ */
+export const CAPSULE_BOX_CONTACT_STRIDE: number;
+/**
+ * Maximum contact points {@link capsule_box_multi_contacts} can emit.
+ *  - 1 from the closest-point-on-segment vs OBB query (primary)
+ *  - up to 2 from the cap-centre sphere-vs-OBB queries (one per endpoint)
+ * @type {number}
+ */
+export const CAPSULE_BOX_MAX_CONTACTS: number;
+//# sourceMappingURL=capsule_contacts.d.ts.map

package/src/engine/physics/narrowphase/capsule_contacts.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"capsule_contacts.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/narrowphase/capsule_contacts.js"],"names":[],"mappings":"AA0EA;;;;;;;;;;;;;;;GAeG;AACH,2CAVW,MAAM,EAAE,GAAC,YAAY,MACrB,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,eACN,MAAM,QAOhB;AAID;;;;;;;;;;;;;;;;;;GAkBG;AACH,4CAhBW,MAAM,EAAE,GAAC,YAAY,QACrB,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,YACN,MAAM,YACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,YACN,MAAM,GACJ,OAAO,CA6CnB;AAMD;;;;;GAKG;AACH,6CAHW,MAAM,EAAE,GAAC,YAAY,yNACnB,OAAO,CAuEnB;AAOD;;;;;;;;;;;;;GAaG;AACH,yCAHW,MAAM,EAAE,GAAC,YAAY,4NACnB,OAAO,CA8FnB;AAyCD;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;GAyCG;AACH,gDAtBW,YAAY,QACZ,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,YACN,MAAM,YACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,QACN,MAAM,GACJ,MAAM,CAuElB;AApJD;;;;;;;;;;GAUG;AACH,yCAFU,MAAM,CAE6B;AAE7C;;;;;GAKG;AACH,uCAFU,MAAM,CAE0B"}