@woosh/meep-engine 2.138.20 → 2.140.0

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

@@ -0,0 +1,639 @@
+import { line3_closest_points_segment_segment } from "../../../core/geom/3d/line/line3_closest_points_segment_segment.js";
+import { quat3_to_matrix3 } from "../../../core/geom/3d/quaternion/quat3_to_matrix3.js";
+
+/**
+ * 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);
+
+// Output of line3_closest_points_segment_segment for the edge-cross
+// SAT-winner path: (s, t) parameters along the two edges.
+const scratch_edge_st = new Float64Array(2);
+
+// --- helpers ---------------------------------------------------------------
+
+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
+) {
+    quat3_to_matrix3(scratch_axes_a, 0, aqx, aqy, aqz, aqw);
+    quat3_to_matrix3(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: closest-pair on the two involved edges ----
+    //
+    // When SAT identifies an edge-edge separating axis (source 6..14 =
+    // 6 + i*3 + j, where `i` is box A's local edge-axis index and
+    // `j` is box B's), the contact happens where one specific edge of
+    // A crosses (or is closest to) one specific edge of B. The earlier
+    // body-centre-midpoint fallback was sufficient for "we know they
+    // collide" but produced lever arms that confused the solver — for
+    // skewed-box stacks this led to slow drift.
+    //
+    // Procedure:
+    //   1. Decode (i, j) from `best_source`.
+    //   2. Among box A's 4 parallel edges along axis i, pick the one
+    //      whose corner is most in the −n direction (closest to B).
+    //   3. Same for box B with +n direction.
+    //   4. `line3_closest_points_segment_segment` returns (s, t)
+    //      parameters; reconstruct the world contact on each edge.
+    if (best_source >= 6) {
+        const ax_src = best_source - 6;
+        const i_a = (ax_src / 3) | 0;
+        const j_b = ax_src - i_a * 3;
+
+        // Edge directions in world.
+        const edge_a_dx = ta[i_a * 3];
+        const edge_a_dy = ta[i_a * 3 + 1];
+        const edge_a_dz = ta[i_a * 3 + 2];
+        const edge_b_dx = tb[j_b * 3];
+        const edge_b_dy = tb[j_b * 3 + 1];
+        const edge_b_dz = tb[j_b * 3 + 2];
+
+        // Edge half-extents (along the chosen axis).
+        const edge_a_half = i_a === 0 ? ahx : (i_a === 1 ? ahy : ahz);
+        const edge_b_half = j_b === 0 ? bhx : (j_b === 1 ? bhy : bhz);
+
+        // Perpendicular axes (the other two) and their half-extents.
+        let u_a_idx, v_a_idx;
+        if (i_a === 0)      { u_a_idx = 1; v_a_idx = 2; }
+        else if (i_a === 1) { u_a_idx = 2; v_a_idx = 0; }
+        else                { u_a_idx = 0; v_a_idx = 1; }
+        const u_a_x = ta[u_a_idx * 3], u_a_y = ta[u_a_idx * 3 + 1], u_a_z = ta[u_a_idx * 3 + 2];
+        const v_a_x = ta[v_a_idx * 3], v_a_y = ta[v_a_idx * 3 + 1], v_a_z = ta[v_a_idx * 3 + 2];
+        const half_u_a = u_a_idx === 0 ? ahx : (u_a_idx === 1 ? ahy : ahz);
+        const half_v_a = v_a_idx === 0 ? ahx : (v_a_idx === 1 ? ahy : ahz);
+
+        let u_b_idx, v_b_idx;
+        if (j_b === 0)      { u_b_idx = 1; v_b_idx = 2; }
+        else if (j_b === 1) { u_b_idx = 2; v_b_idx = 0; }
+        else                { u_b_idx = 0; v_b_idx = 1; }
+        const u_b_x = tb[u_b_idx * 3], u_b_y = tb[u_b_idx * 3 + 1], u_b_z = tb[u_b_idx * 3 + 2];
+        const v_b_x = tb[v_b_idx * 3], v_b_y = tb[v_b_idx * 3 + 1], v_b_z = tb[v_b_idx * 3 + 2];
+        const half_u_b = u_b_idx === 0 ? bhx : (u_b_idx === 1 ? bhy : bhz);
+        const half_v_b = v_b_idx === 0 ? bhx : (v_b_idx === 1 ? bhy : bhz);
+
+        // The contact normal `n` (= best_n) points B → A. From A's
+        // perspective B is in the −n direction; pick the A-corner most
+        // in that direction. From B's perspective A is in +n; pick the
+        // B-corner most in +n.
+        const minus_n_dot_u_a = -(nx * u_a_x + ny * u_a_y + nz * u_a_z);
+        const minus_n_dot_v_a = -(nx * v_a_x + ny * v_a_y + nz * v_a_z);
+        const u_sign_a = minus_n_dot_u_a >= 0 ? 1 : -1;
+        const v_sign_a = minus_n_dot_v_a >= 0 ? 1 : -1;
+
+        const plus_n_dot_u_b = nx * u_b_x + ny * u_b_y + nz * u_b_z;
+        const plus_n_dot_v_b = nx * v_b_x + ny * v_b_y + nz * v_b_z;
+        const u_sign_b = plus_n_dot_u_b >= 0 ? 1 : -1;
+        const v_sign_b = plus_n_dot_v_b >= 0 ? 1 : -1;
+
+        // Box-A edge centre = A_centre + u_sign_a * half_u_a * u_a
+        //                              + v_sign_a * half_v_a * v_a.
+        const corner_a_cx = ax + u_a_x * u_sign_a * half_u_a + v_a_x * v_sign_a * half_v_a;
+        const corner_a_cy = ay + u_a_y * u_sign_a * half_u_a + v_a_y * v_sign_a * half_v_a;
+        const corner_a_cz = az + u_a_z * u_sign_a * half_u_a + v_a_z * v_sign_a * half_v_a;
+        const edge_a_p1_x = corner_a_cx - edge_a_dx * edge_a_half;
+        const edge_a_p1_y = corner_a_cy - edge_a_dy * edge_a_half;
+        const edge_a_p1_z = corner_a_cz - edge_a_dz * edge_a_half;
+        const edge_a_p2_x = corner_a_cx + edge_a_dx * edge_a_half;
+        const edge_a_p2_y = corner_a_cy + edge_a_dy * edge_a_half;
+        const edge_a_p2_z = corner_a_cz + edge_a_dz * edge_a_half;
+
+        const corner_b_cx = bx + u_b_x * u_sign_b * half_u_b + v_b_x * v_sign_b * half_v_b;
+        const corner_b_cy = by + u_b_y * u_sign_b * half_u_b + v_b_y * v_sign_b * half_v_b;
+        const corner_b_cz = bz + u_b_z * u_sign_b * half_u_b + v_b_z * v_sign_b * half_v_b;
+        const edge_b_p1_x = corner_b_cx - edge_b_dx * edge_b_half;
+        const edge_b_p1_y = corner_b_cy - edge_b_dy * edge_b_half;
+        const edge_b_p1_z = corner_b_cz - edge_b_dz * edge_b_half;
+        const edge_b_p2_x = corner_b_cx + edge_b_dx * edge_b_half;
+        const edge_b_p2_y = corner_b_cy + edge_b_dy * edge_b_half;
+        const edge_b_p2_z = corner_b_cz + edge_b_dz * edge_b_half;
+
+        line3_closest_points_segment_segment(
+            scratch_edge_st,
+            edge_a_p1_x, edge_a_p1_y, edge_a_p1_z, edge_a_p2_x, edge_a_p2_y, edge_a_p2_z,
+            edge_b_p1_x, edge_b_p1_y, edge_b_p1_z, edge_b_p2_x, edge_b_p2_y, edge_b_p2_z
+        );
+        const s = scratch_edge_st[0];
+        const t = scratch_edge_st[1];
+        const contact_a_x = edge_a_p1_x + s * (edge_a_p2_x - edge_a_p1_x);
+        const contact_a_y = edge_a_p1_y + s * (edge_a_p2_y - edge_a_p1_y);
+        const contact_a_z = edge_a_p1_z + s * (edge_a_p2_z - edge_a_p1_z);
+        const contact_b_x = edge_b_p1_x + t * (edge_b_p2_x - edge_b_p1_x);
+        const contact_b_y = edge_b_p1_y + t * (edge_b_p2_y - edge_b_p1_y);
+        const contact_b_z = edge_b_p1_z + t * (edge_b_p2_z - edge_b_p1_z);
+
+        out[3] = 1;
+        out[4] = contact_a_x; out[5] = contact_a_y; out[6] = contact_a_z;
+        out[7] = contact_b_x; out[8] = contact_b_y; out[9] = contact_b_z;
+        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/box_triangle_contact.d.ts

@@ -0,0 +1,30 @@
+/**
+ * @param {number[]|Float64Array} out length >= {@link BOX_TRIANGLE_OUT_LENGTH}
+ * @param {number} bcx box centre x (world)
+ * @param {number} bcy
+ * @param {number} bcz
+ * @param {number} bqx box quaternion x
+ * @param {number} bqy
+ * @param {number} bqz
+ * @param {number} bqw
+ * @param {number} bhx box half-extent x (body frame)
+ * @param {number} bhy
+ * @param {number} bhz
+ * @param {number} ax triangle vertex A x (world)
+ * @param {number} ay
+ * @param {number} az
+ * @param {number} bx triangle vertex B x (world)
+ * @param {number} by
+ * @param {number} bz
+ * @param {number} cx triangle vertex C x (world)
+ * @param {number} cy
+ * @param {number} cz
+ * @returns {boolean} true if box and triangle overlap
+ */
+export function box_triangle_contact(out: number[] | Float64Array, bcx: number, bcy: number, bcz: number, bqx: number, bqy: number, bqz: number, bqw: number, bhx: number, bhy: number, bhz: number, ax: number, ay: number, az: number, bx: number, by: number, bz: number, cx: number, cy: number, cz: number): boolean;
+/**
+ * Length of `out` required by {@link box_triangle_contact}.
+ * @type {number}
+ */
+export const BOX_TRIANGLE_OUT_LENGTH: number;
+//# sourceMappingURL=box_triangle_contact.d.ts.map

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

@@ -0,0 +1 @@
+{"version":3,"file":"box_triangle_contact.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/narrowphase/box_triangle_contact.js"],"names":[],"mappings":"AAuNA;;;;;;;;;;;;;;;;;;;;;;GAsBG;AACH,0CAtBW,MAAM,EAAE,GAAC,YAAY,OACrB,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,OACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,MACN,MAAM,GACJ,OAAO,CAuJnB;AAxUD;;;GAGG;AACH,sCAFU,MAAM,CAEyD"}