@woosh/meep-engine 2.139.0 → 2.141.0

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

@@ -1,3 +1,6 @@
+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.
  *
@@ -36,6 +39,28 @@ const MAX_CONTACTS = 4;
 const CONTACT_STRIDE = 7;
 const PARALLEL_EPS_SQR = 1e-8;
 
+/**
+ * Reference-axis tie-break deadband. SAT picks the minimum-overlap axis, but
+ * for aligned boxes two axes (A's face normal and B's face normal along the
+ * contact direction) have *equal* overlap, so floating-point noise from a
+ * sub-degree wobble flips which one wins frame to frame. That flips the
+ * reference/incident assignment, which reorders the contact points, which
+ * flips the solver's Gauss-Seidel sweep order — injecting an alternating
+ * bias that makes symmetric stacks creep and never sleep.
+ *
+ * The deadband makes a later axis replace the current best only if it
+ * reduces the overlap by more than `best · TIE_REL + TIE_ABS`. Axes are
+ * tested A-faces, then B-faces, then edge-crosses, so ties resolve to the
+ * earlier axis (A's face, then a face over an edge) — a stable, consistent
+ * choice. Genuine minima (overlap smaller by more than the band) still win,
+ * so SAT correctness for non-aligned boxes is unchanged; only the
+ * noise-driven flip on near-perfect alignment is suppressed. Box2D's
+ * `b2CollidePolygons` uses the same relative+absolute hysteresis.
+ * @type {number}
+ */
+const TIE_REL = 0.02;
+const TIE_ABS = 1e-5;
+
 /**
  * Length of `out` required by {@link box_box_manifold}.
  * @type {number}
@@ -61,16 +86,11 @@ const clipped_uv_out = new Float64Array(8 * 2);
 // Stride 4 per candidate: x, y, z, depth.
 const candidates = new Float64Array(8 * 4);
 
-// --- helpers ---------------------------------------------------------------
+// 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);
 
-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);
-}
+// --- 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];
@@ -157,8 +177,8 @@ export function box_box_manifold(
     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);
+    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;
@@ -184,7 +204,11 @@ export function box_box_manifold(
         const dist = proj < 0 ? -proj : proj;
         const overlap = (rA + rB) - dist;
         if (overlap < 0) return true;
-        if (overlap < best_overlap) {
+        // Deadband: the first axis always wins; a later axis replaces it only
+        // if it reduces the overlap past the hysteresis band. This biases ties
+        // toward earlier-tested axes (A faces > B faces > edges) so aligned
+        // boxes pick a stable reference instead of flip-flopping on noise.
+        if (best_source === -1 || overlap < best_overlap - (best_overlap * TIE_REL + TIE_ABS)) {
             best_overlap = overlap;
             const sign = proj > 0 ? -1 : 1; // normal points B→A
             best_nx = ux * sign;
@@ -217,14 +241,112 @@ export function box_box_manifold(
     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 ----
+    // ---- 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 mx = (ax + bx) * 0.5;
-        const my = (ay + by) * 0.5;
-        const mz = (az + bz) * 0.5;
+        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] = mx; out[5] = my;  out[6] = mz;
-        out[7] = mx; out[8] = my;  out[9] = mz;
+        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;
     }

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"}