@woosh/meep-engine 2.154.0 → 2.156.0

package/src/engine/physics/gjk/gjk_epa_penetration.js

@@ -1,3 +1,11 @@
+import { v3_compute_triangle_normal } from "../../../core/geom/3d/triangle/v3_compute_triangle_normal.js";
+import { v3_array_copy } from "../../../core/geom/vec3/v3_array_copy.js";
+import { v3_cross } from "../../../core/geom/vec3/v3_cross.js";
+import { v3_dot } from "../../../core/geom/vec3/v3_dot.js";
+import { v3_length_sqr } from "../../../core/geom/vec3/v3_length_sqr.js";
+import { v3_subtract } from "../../../core/geom/vec3/v3_subtract.js";
+import { v3_triple_cross_product } from "../../../core/geom/vec3/v3_triple_cross_product.js";
+
 /**
  * Robust GJK + EPA penetration for convex shapes — the narrowphase's
  * convex-fallback and concave-per-triangle penetration query. Replaced the
@@ -13,11 +21,13 @@
  * difference's closest face to the origin. No cross-frame state → reset-and-
  * resimulate determinism preserved.
  *
- * Even with per-call allocation (plain arrays) it measured ~2.1× FASTER than the
- * replaced pair, which wasted all 64 iterations diverging on the degenerate
- * simplex. KNOWN FOLLOW-UP: port the hot path to typed-array pools to remove the
- * per-call allocation; the convex/concave EPA fallbacks are rarely hit today
- * (closed-form solvers + SAT cover the common pairs), so it has not been urgent.
+ * Allocation-free hot path: the GJK simplex lives in a flat `Float64Array(12)`
+ * and the EPA polytope in pooled flat typed arrays (vertices, face vertex-indices,
+ * face normals, face distances, horizon edges). All vector math goes through
+ * `v3_*` out-parameter helpers writing into module scratch — no per-call arrays
+ * or face objects. The pools are module-level scratch reused across calls; the
+ * engine is single-threaded and GJK/EPA is never re-entered, matching the rest
+ * of the physics core's scratch convention.
  *
  * @author Alex Goldring
  * @copyright Company Named Limited (c) 2026
@@ -26,25 +36,55 @@
 const GJK_MAX_ITER = 64;
 const EPA_MAX_ITER = 64;
 const EPA_EPS = 1e-8;
+const DIR_EPS = 1e-18;
+
+// EPA pool capacities. EPA adds at most one vertex per iteration, so vertices are
+// bounded by 4 + EPA_MAX_ITER (= 68), faces by Euler's 2V−4 (= 132) and horizon
+// edges by 3V−6 (= 198). These caps clear those bounds with margin, so for the
+// convex inputs the narrowphase feeds they are never reached; the guards below
+// degrade to "use the current best face" purely as a defence against degenerate
+// input rather than growing the pools.
+const VERT_CAP = 128;
+const FACE_CAP = 256;
+const EDGE_CAP = 256;
 
+// --- support scratch ---
 const _sA = new Float64Array(3);
 const _sB = new Float64Array(3);
+const _w = new Float64Array(3); // newly sampled support point
 
-function sub(a, b) { return [a[0] - b[0], a[1] - b[1], a[2] - b[2]]; }
-function dot(a, b) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; }
-function cross(a, b) {
-    return [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]];
-}
-function neg(a) { return [-a[0], -a[1], -a[2]]; }
-function lensq(a) { return a[0] * a[0] + a[1] * a[1] + a[2] * a[2]; }
-// triple product (a×b)×c = b(a·c) − a(b·c)
-function tripleProduct(a, b, c) {
-    const ac = dot(a, c), bc = dot(b, c);
-    return [b[0] * ac - a[0] * bc, b[1] * ac - a[1] * bc, b[2] * ac - a[2] * bc];
-}
+// --- GJK simplex + search direction ---
+const SIMPLEX = new Float64Array(12); // 4 points, 3 floats each; slot 0 = newest (a)
+const DIR = new Float64Array(3);
+
+// --- vector scratch (shared; GJK and EPA never run concurrently) ---
+const _ab = new Float64Array(3);
+const _ac = new Float64Array(3);
+const _ad = new Float64Array(3);
+const _abc = new Float64Array(3);
+const _acd = new Float64Array(3);
+const _adb = new Float64Array(3);
+const _t = new Float64Array(3);
+
+// --- EPA polytope pools ---
+const VERTS = new Float64Array(VERT_CAP * 3);
+const FACE_IDX = new Int32Array(FACE_CAP * 3); // vertex indices, 3 per face
+const FACE_N = new Float64Array(FACE_CAP * 3); // outward unit normal, 3 per face
+const FACE_DIST = new Float64Array(FACE_CAP); // perpendicular distance from origin
+const EDGE = new Int32Array(EDGE_CAP * 2); // horizon edges, 2 vertex indices each
+const CEN = new Float64Array(3); // polytope centroid (for outward orientation)
 
 /**
- * Minkowski-difference support: support_A(d) − support_B(−d), world space.
+ * A convex shape exposing a world-space support mapping: `support(out, off, dx,
+ * dy, dz)` writes the farthest point of the shape in direction `(dx, dy, dz)`
+ * into `out[off..off+2]` (e.g. {@link PosedShape}).
+ *
+ * @typedef {{ support: function(Float64Array, number, number, number, number): void }} SupportProvider
+ */
+
+/**
+ * Minkowski-difference support written to `out[off..off+2]`:
+ * support_A(d̂) − support_B(−d̂), world space.
  *
  * The direction is normalised first: the support contract assumes a UNIT vector
  * — `SphereShape3D`/`CapsuleShape3D` scale the raw direction by their radius
@@ -53,188 +93,436 @@ function tripleProduct(a, b, c) {
  * simplex evolution). Polytope supports are scale-invariant (vertex argmax), so
  * this only matters for curved providers, but it must hold for them. Matches the
  * sibling `minkowski_support.js`, which normalises for the same reason.
+ *
+ * @param {Float64Array|number[]} out destination for the CSO point
+ * @param {number} off offset into `out`
+ * @param {SupportProvider} A first support provider
+ * @param {SupportProvider} B second support provider
+ * @param {number} dx search-direction x (need not be unit length)
+ * @param {number} dy search-direction y
+ * @param {number} dz search-direction z
+ * @returns {void}
  */
-function csoSupport(A, B, d) {
-    let dx = d[0], dy = d[1], dz = d[2];
+function cso_support(out, off, A, B, dx, dy, dz) {
     const len2 = dx * dx + dy * dy + dz * dz;
     if (len2 > 1e-30) {
         const inv = 1 / Math.sqrt(len2);
-        dx *= inv; dy *= inv; dz *= inv;
+        dx *= inv;
+        dy *= inv;
+        dz *= inv;
     }
     A.support(_sA, 0, dx, dy, dz);
     B.support(_sB, 0, -dx, -dy, -dz);
-    return [_sA[0] - _sB[0], _sA[1] - _sB[1], _sA[2] - _sB[2]];
+    out[off] = _sA[0] - _sB[0];
+    out[off + 1] = _sA[1] - _sB[1];
+    out[off + 2] = _sA[2] - _sB[2];
 }
 
 /**
- * GJK intersection. Returns a rank-4 simplex (array of 4 CSO points) enclosing
- * the origin on overlap, or null if separated. `simplex[0]` is the most-recent
- * point throughout (Muratori convention).
+ * GJK intersection. Fills {@link SIMPLEX} with a rank-4 simplex enclosing the
+ * origin and returns 4 on overlap; returns 0 if the shapes are separated.
+ * `SIMPLEX` slot 0 is the most-recent point throughout (Muratori convention).
+ *
+ * @param {SupportProvider} A first support provider
+ * @param {SupportProvider} B second support provider
+ * @returns {number} 4 if the origin is enclosed (shapes overlap), 0 if separated
  */
 function gjk(A, B) {
-    let d = [1, 0, 0];
-    let s = csoSupport(A, B, d);
-    if (lensq(s) < 1e-18) { d = [0, 1, 0]; s = csoSupport(A, B, d); }
-    const simplex = [s];
-    d = neg(s);
+    cso_support(SIMPLEX, 0, A, B, 1, 0, 0);
+    if (v3_length_sqr(SIMPLEX[0], SIMPLEX[1], SIMPLEX[2]) < DIR_EPS) {
+        cso_support(SIMPLEX, 0, A, B, 0, 1, 0);
+    }
+    DIR[0] = -SIMPLEX[0];
+    DIR[1] = -SIMPLEX[1];
+    DIR[2] = -SIMPLEX[2];
+    let count = 1;
 
     for (let iter = 0; iter < GJK_MAX_ITER; iter++) {
-        if (lensq(d) < 1e-18) return null; // search direction collapsed → touching/sep
-        const a = csoSupport(A, B, d);
-        if (dot(a, d) < 0) return null;    // farthest point short of origin → separated
-        simplex.unshift(a);
-        if (doSimplex(simplex, d)) return simplex.slice(0, 4);
+        if (v3_length_sqr(DIR[0], DIR[1], DIR[2]) < DIR_EPS) {
+            return 0; // search direction collapsed → touching/separated
+        }
+        cso_support(_w, 0, A, B, DIR[0], DIR[1], DIR[2]);
+        if (v3_dot(_w[0], _w[1], _w[2], DIR[0], DIR[1], DIR[2]) < 0) {
+            return 0; // farthest point short of the origin → separated
+        }
+        // push the new point to slot 0, shifting the existing points down one slot
+        // (copyWithin is memmove-style: the overlapping forward copy is well-defined)
+        SIMPLEX.copyWithin(3, 0, count * 3);
+        SIMPLEX[0] = _w[0];
+        SIMPLEX[1] = _w[1];
+        SIMPLEX[2] = _w[2];
+        count++;
+
+        count = do_simplex(count);
+        if (count === 0) {
+            return 4; // origin enclosed by the tetrahedron
+        }
     }
-    return null;
+    return 0;
 }
 
 /**
- * Evolve the simplex toward the origin. Mutates `simplex` (in place) and writes
- * the next search direction into `dOut`. Returns true when a tetrahedron
- * encloses the origin.
+ * Evolve the simplex toward the origin. Reorders {@link SIMPLEX} in place and
+ * writes the next search direction into {@link DIR}. Returns the new simplex size,
+ * or 0 when a tetrahedron encloses the origin.
+ *
+ * @param {number} count current simplex size (2, 3, or 4)
+ * @returns {number} the new simplex size (1–3), or 0 when the origin is enclosed
  */
-function doSimplex(simplex, dOut) {
-    if (simplex.length === 2) return lineCase(simplex, dOut);
-    if (simplex.length === 3) return triangleCase(simplex, dOut);
-    return tetraCase(simplex, dOut);
+function do_simplex(count) {
+    if (count === 2) {
+        return line_case();
+    }
+    if (count === 3) {
+        return triangle_case();
+    }
+    return tetra_case();
 }
 
-function setDir(dOut, v) { dOut[0] = v[0]; dOut[1] = v[1]; dOut[2] = v[2]; }
-
-function lineCase(simplex, dOut) {
-    const a = simplex[0], b = simplex[1];
-    const ab = sub(b, a), ao = neg(a);
-    if (dot(ab, ao) > 0) {
-        const perp = tripleProduct(ab, ao, ab);
-        if (lensq(perp) < 1e-18) {
-            // origin on the line — pick any perpendicular
-            const ref = Math.abs(ab[0]) < 0.9 ? [1, 0, 0] : [0, 1, 0];
-            setDir(dOut, cross(ab, ref));
-        } else setDir(dOut, perp);
-    } else {
-        simplex.length = 1; // keep [a]
-        setDir(dOut, ao);
+/**
+ * Line (2-point) simplex sub-case: pick the Voronoi region of segment [a, b] the
+ * origin projects into and set the next search direction in {@link DIR}.
+ *
+ * @returns {number} the new simplex size (1 keeps [a], 2 keeps [a, b])
+ */
+function line_case() {
+    const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
+    const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
+    v3_subtract(_ab, 0, bx, by, bz, ax, ay, az); // ab = b − a
+    const aox = -ax, aoy = -ay, aoz = -az; // ao = −a
+
+    if (v3_dot(_ab[0], _ab[1], _ab[2], aox, aoy, aoz) > 0) {
+        v3_triple_cross_product(DIR, 0, _ab[0], _ab[1], _ab[2], aox, aoy, aoz); // (ab×ao)×ab
+        if (v3_length_sqr(DIR[0], DIR[1], DIR[2]) < DIR_EPS) {
+            // origin on the line — pick any perpendicular to ab
+            if (Math.abs(_ab[0]) < 0.9) {
+                v3_cross(DIR, 0, _ab[0], _ab[1], _ab[2], 1, 0, 0);
+            } else {
+                v3_cross(DIR, 0, _ab[0], _ab[1], _ab[2], 0, 1, 0);
+            }
+        }
+        return 2; // keep [a, b]
     }
-    return false;
+    DIR[0] = aox;
+    DIR[1] = aoy;
+    DIR[2] = aoz;
+    return 1; // keep [a]
 }
 
-function triangleCase(simplex, dOut) {
-    const a = simplex[0], b = simplex[1], c = simplex[2];
-    const ab = sub(b, a), ac = sub(c, a), ao = neg(a);
-    const abc = cross(ab, ac);
+/**
+ * Triangle (3-point) simplex sub-case: classify the origin against triangle
+ * [a, b, c]'s edges and faces, reorder {@link SIMPLEX}, and set the next search
+ * direction in {@link DIR}.
+ *
+ * @returns {number} the new simplex size (1, 2, or 3)
+ */
+function triangle_case() {
+    const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
+    const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
+    const cx = SIMPLEX[6], cy = SIMPLEX[7], cz = SIMPLEX[8];
+    v3_subtract(_ab, 0, bx, by, bz, ax, ay, az);
+    v3_subtract(_ac, 0, cx, cy, cz, ax, ay, az);
+    const aox = -ax, aoy = -ay, aoz = -az;
+    v3_cross(_abc, 0, _ab[0], _ab[1], _ab[2], _ac[0], _ac[1], _ac[2]); // abc = ab×ac
 
-    if (dot(cross(abc, ac), ao) > 0) {
-        if (dot(ac, ao) > 0) { // outside edge ac
-            simplex.length = 0; simplex.push(a, c);
-            setDir(dOut, tripleProduct(ac, ao, ac));
-            return false;
+    // outside edge ac?  (abc×ac) · ao > 0
+    v3_cross(_t, 0, _abc[0], _abc[1], _abc[2], _ac[0], _ac[1], _ac[2]);
+    if (v3_dot(_t[0], _t[1], _t[2], aox, aoy, aoz) > 0) {
+        if (v3_dot(_ac[0], _ac[1], _ac[2], aox, aoy, aoz) > 0) {
+            // keep [a, c]: move c into slot 1
+            v3_array_copy(SIMPLEX, 3, SIMPLEX, 6);
+            v3_triple_cross_product(DIR, 0, _ac[0], _ac[1], _ac[2], aox, aoy, aoz); // (ac×ao)×ac
+            return 2;
         }
-        return starCase(simplex, a, b, ao, dOut);
+        return star_case();
     }
-    if (dot(cross(ab, abc), ao) > 0) return starCase(simplex, a, b, ao, dOut);
 
-    // origin within the triangle's prism — above or below
-    if (dot(abc, ao) > 0) { simplex.length = 0; simplex.push(a, b, c); setDir(dOut, abc); }
-    else { simplex.length = 0; simplex.push(a, c, b); setDir(dOut, neg(abc)); }
-    return false;
-}
+    // outside edge ab?  (ab×abc) · ao > 0
+    v3_cross(_t, 0, _ab[0], _ab[1], _ab[2], _abc[0], _abc[1], _abc[2]);
+    if (v3_dot(_t[0], _t[1], _t[2], aox, aoy, aoz) > 0) {
+        return star_case();
+    }
 
-function starCase(simplex, a, b, ao, dOut) {
-    const ab = sub(b, a);
-    if (dot(ab, ao) > 0) {
-        simplex.length = 0; simplex.push(a, b);
-        setDir(dOut, tripleProduct(ab, ao, ab));
+    // origin within the triangle's prism — above or below the face
+    if (v3_dot(_abc[0], _abc[1], _abc[2], aox, aoy, aoz) > 0) {
+        DIR[0] = _abc[0];
+        DIR[1] = _abc[1];
+        DIR[2] = _abc[2];
     } else {
-        simplex.length = 0; simplex.push(a);
-        setDir(dOut, ao);
+        // below: reorder to [a, c, b] so the winding faces the origin; DIR = −abc
+        SIMPLEX[3] = cx;
+        SIMPLEX[4] = cy;
+        SIMPLEX[5] = cz;
+        SIMPLEX[6] = bx;
+        SIMPLEX[7] = by;
+        SIMPLEX[8] = bz;
+        DIR[0] = -_abc[0];
+        DIR[1] = -_abc[1];
+        DIR[2] = -_abc[2];
     }
-    return false;
+    return 3; // keep the triangle
 }
 
-function tetraCase(simplex, dOut) {
-    const a = simplex[0], b = simplex[1], c = simplex[2], d = simplex[3];
-    const ab = sub(b, a), ac = sub(c, a), ad = sub(d, a), ao = neg(a);
-    const abc = cross(ab, ac);
-    const acd = cross(ac, ad);
-    const adb = cross(ad, ab);
-
-    if (dot(abc, ao) > 0) { simplex.length = 0; simplex.push(a, b, c); return triangleCase(simplex, dOut); }
-    if (dot(acd, ao) > 0) { simplex.length = 0; simplex.push(a, c, d); return triangleCase(simplex, dOut); }
-    if (dot(adb, ao) > 0) { simplex.length = 0; simplex.push(a, d, b); return triangleCase(simplex, dOut); }
-    return true; // origin inside the tetrahedron
+/**
+ * Edge region shared by the two triangle edges that fold back toward [a]: keep
+ * segment [a, b] or vertex [a] and set the next search direction in {@link DIR}.
+ *
+ * @returns {number} the new simplex size (1 keeps [a], 2 keeps [a, b])
+ */
+function star_case() {
+    const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
+    const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
+    v3_subtract(_ab, 0, bx, by, bz, ax, ay, az);
+    const aox = -ax, aoy = -ay, aoz = -az;
+
+    if (v3_dot(_ab[0], _ab[1], _ab[2], aox, aoy, aoz) > 0) {
+        v3_triple_cross_product(DIR, 0, _ab[0], _ab[1], _ab[2], aox, aoy, aoz);
+        return 2; // keep [a, b]
+    }
+    DIR[0] = aox;
+    DIR[1] = aoy;
+    DIR[2] = aoz;
+    return 1; // keep [a]
 }
 
 /**
- * EPA over the Minkowski difference, seeded by a rank-4 enclosing simplex.
- * Writes the unit penetration normal (B→A) into `out` and returns the depth.
+ * Tetrahedron (4-point) simplex sub-case: if the origin lies outside one of the
+ * three faces incident to [a], reorder {@link SIMPLEX} to that triangle and
+ * recurse into {@link triangle_case}; otherwise the origin is enclosed.
+ *
+ * @returns {number} the new simplex size (1–3), or 0 when the origin is enclosed
  */
-function epa(A, B, simplex, out) {
-    const verts = simplex.map((p) => [p[0], p[1], p[2]]);
-    // centroid for outward orientation of the initial faces
-    const cen = [(verts[0][0] + verts[1][0] + verts[2][0] + verts[3][0]) / 4,
-        (verts[0][1] + verts[1][1] + verts[2][1] + verts[3][1]) / 4,
-        (verts[0][2] + verts[1][2] + verts[2][2] + verts[3][2]) / 4];
+function tetra_case() {
+    const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
+    const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
+    const cx = SIMPLEX[6], cy = SIMPLEX[7], cz = SIMPLEX[8];
+    const dx = SIMPLEX[9], dy = SIMPLEX[10], dz = SIMPLEX[11];
+    v3_subtract(_ab, 0, bx, by, bz, ax, ay, az);
+    v3_subtract(_ac, 0, cx, cy, cz, ax, ay, az);
+    v3_subtract(_ad, 0, dx, dy, dz, ax, ay, az);
+    const aox = -ax, aoy = -ay, aoz = -az;
+    v3_cross(_abc, 0, _ab[0], _ab[1], _ab[2], _ac[0], _ac[1], _ac[2]);
+    v3_cross(_acd, 0, _ac[0], _ac[1], _ac[2], _ad[0], _ad[1], _ad[2]);
+    v3_cross(_adb, 0, _ad[0], _ad[1], _ad[2], _ab[0], _ab[1], _ab[2]);
 
-    let faces = [[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]].map((f) => makeFace(verts, f, cen));
+    if (v3_dot(_abc[0], _abc[1], _abc[2], aox, aoy, aoz) > 0) {
+        // outside face abc → keep [a, b, c] (slots already correct)
+        return triangle_case();
+    }
+    if (v3_dot(_acd[0], _acd[1], _acd[2], aox, aoy, aoz) > 0) {
+        // outside face acd → reorder to [a, c, d]
+        SIMPLEX[3] = cx;
+        SIMPLEX[4] = cy;
+        SIMPLEX[5] = cz;
+        SIMPLEX[6] = dx;
+        SIMPLEX[7] = dy;
+        SIMPLEX[8] = dz;
+        return triangle_case();
+    }
+    if (v3_dot(_adb[0], _adb[1], _adb[2], aox, aoy, aoz) > 0) {
+        // outside face adb → reorder to [a, d, b]
+        SIMPLEX[3] = dx;
+        SIMPLEX[4] = dy;
+        SIMPLEX[5] = dz;
+        SIMPLEX[6] = bx;
+        SIMPLEX[7] = by;
+        SIMPLEX[8] = bz;
+        return triangle_case();
+    }
+    return 0; // origin inside the tetrahedron
+}
+
+/**
+ * Append a face spanning vertices (i, j, k) with an OUTWARD unit normal (away
+ * from the polytope centroid {@link CEN}); winding is flipped if needed to keep
+ * the normal outward.
+ *
+ * @param {number} i first vertex index into {@link VERTS}
+ * @param {number} j second vertex index into {@link VERTS}
+ * @param {number} k third vertex index into {@link VERTS}
+ * @param {number} face_count current number of faces (the new face's slot)
+ * @returns {number} the new face count
+ */
+function add_face(i, j, k, face_count) {
+    const ax = VERTS[i * 3], ay = VERTS[i * 3 + 1], az = VERTS[i * 3 + 2];
+    const bx = VERTS[j * 3], by = VERTS[j * 3 + 1], bz = VERTS[j * 3 + 2];
+    const cx = VERTS[k * 3], cy = VERTS[k * 3 + 1], cz = VERTS[k * 3 + 2];
+    // (b−a)×(c−a), normalised (degenerate → (0,1,0)); see v3_compute_triangle_normal
+    v3_compute_triangle_normal(_t, 0, ax, ay, az, bx, by, bz, cx, cy, cz);
+    let nx = _t[0], ny = _t[1], nz = _t[2];
+
+    // orient outward: the normal must point away from the interior (centroid)
+    if (v3_dot(nx, ny, nz, ax - CEN[0], ay - CEN[1], az - CEN[2]) < 0) {
+        nx = -nx;
+        ny = -ny;
+        nz = -nz;
+        const swap = j;
+        j = k;
+        k = swap;
+    }
+
+    const fb = face_count * 3;
+    FACE_IDX[fb] = i;
+    FACE_IDX[fb + 1] = j;
+    FACE_IDX[fb + 2] = k;
+    FACE_N[fb] = nx;
+    FACE_N[fb + 1] = ny;
+    FACE_N[fb + 2] = nz;
+    FACE_DIST[face_count] = v3_dot(nx, ny, nz, ax, ay, az); // ≥ 0 once outward
+    return face_count + 1;
+}
+
+/**
+ * Add a horizon edge (i, j) with cancellation: if the reverse edge (j, i) is
+ * already present, both are interior to the visible region and drop out.
+ * Order is irrelevant to the horizon, so removal is an O(1) swap-with-last.
+ *
+ * @param {number} edge_count current number of horizon edges in {@link EDGE}
+ * @param {number} i edge start (vertex index)
+ * @param {number} j edge end (vertex index)
+ * @returns {number} the new edge count
+ */
+function add_edge(edge_count, i, j) {
+    for (let k = 0; k < edge_count; k++) {
+        if (EDGE[k * 2] === j && EDGE[k * 2 + 1] === i) {
+            const last = edge_count - 1;
+            EDGE[k * 2] = EDGE[last * 2];
+            EDGE[k * 2 + 1] = EDGE[last * 2 + 1];
+            return last;
+        }
+    }
+    if (edge_count >= EDGE_CAP) {
+        return edge_count; // horizon pool full (unreachable for convex inputs)
+    }
+    EDGE[edge_count * 2] = i;
+    EDGE[edge_count * 2 + 1] = j;
+    return edge_count + 1;
+}
+
+/**
+ * Copy a face record (vertex indices, normal, distance) from slot `src` to slot
+ * `dst` — used to compact surviving faces over removed ones.
+ *
+ * @param {number} dst destination face slot
+ * @param {number} src source face slot
+ * @returns {void}
+ */
+function copy_face(dst, src) {
+    const db = dst * 3, sb = src * 3;
+    FACE_IDX[db] = FACE_IDX[sb];
+    FACE_IDX[db + 1] = FACE_IDX[sb + 1];
+    FACE_IDX[db + 2] = FACE_IDX[sb + 2];
+    FACE_N[db] = FACE_N[sb];
+    FACE_N[db + 1] = FACE_N[sb + 1];
+    FACE_N[db + 2] = FACE_N[sb + 2];
+    FACE_DIST[dst] = FACE_DIST[src];
+}
+
+/**
+ * EPA over the Minkowski difference, seeded by the rank-4 enclosing simplex left
+ * in {@link SIMPLEX} by {@link gjk}. Writes the unit penetration normal (B→A)
+ * into `out` and returns the depth.
+ *
+ * @param {SupportProvider} A first support provider
+ * @param {SupportProvider} B second support provider
+ * @param {Float64Array|number[]} out destination for the unit penetration normal (B→A)
+ * @returns {number} penetration depth, or 0 if the polytope degenerated
+ */
+function epa(A, B, out) {
+    // seed the vertex pool with the simplex points
+    v3_array_copy(VERTS, 0, SIMPLEX, 0);
+    v3_array_copy(VERTS, 3, SIMPLEX, 3);
+    v3_array_copy(VERTS, 6, SIMPLEX, 6);
+    v3_array_copy(VERTS, 9, SIMPLEX, 9);
+    let vert_count = 4;
+
+    CEN[0] = (VERTS[0] + VERTS[3] + VERTS[6] + VERTS[9]) / 4;
+    CEN[1] = (VERTS[1] + VERTS[4] + VERTS[7] + VERTS[10]) / 4;
+    CEN[2] = (VERTS[2] + VERTS[5] + VERTS[8] + VERTS[11]) / 4;
+
+    let face_count = 0;
+    face_count = add_face(0, 1, 2, face_count);
+    face_count = add_face(0, 2, 3, face_count);
+    face_count = add_face(0, 3, 1, face_count);
+    face_count = add_face(1, 3, 2, face_count);
 
     for (let iter = 0; iter < EPA_MAX_ITER; iter++) {
         // closest face to the origin
-        let best = faces[0];
-        for (let i = 1; i < faces.length; i++) if (faces[i].dist < best.dist) best = faces[i];
-
-        const w = csoSupport(A, B, best.n);
-        const wdist = dot(best.n, w) - best.dist;
-        if (wdist < EPA_EPS) break; // converged onto the closest face
-
-        const wi = verts.length;
-        verts.push(w);
-
-        // remove faces visible from w; collect horizon edges (those on exactly one removed face)
-        const edges = [];
-        const kept = [];
-        for (const f of faces) {
-            if (dot(f.n, w) - f.dist > EPA_EPS) {
-                addEdge(edges, f.v[0], f.v[1]);
-                addEdge(edges, f.v[1], f.v[2]);
-                addEdge(edges, f.v[2], f.v[0]);
-            } else kept.push(f);
+        let best = 0;
+        for (let f = 1; f < face_count; f++) {
+            if (FACE_DIST[f] < FACE_DIST[best]) {
+                best = f;
+            }
+        }
+
+        const bn = best * 3;
+        cso_support(_w, 0, A, B, FACE_N[bn], FACE_N[bn + 1], FACE_N[bn + 2]);
+        const wdist = v3_dot(FACE_N[bn], FACE_N[bn + 1], FACE_N[bn + 2], _w[0], _w[1], _w[2]) - FACE_DIST[best];
+        if (wdist < EPA_EPS) {
+            break; // converged onto the closest face
+        }
+        if (vert_count >= VERT_CAP) {
+            break; // vertex pool full (unreachable: EPA_MAX_ITER caps vertices at 68)
+        }
+
+        const wi = vert_count;
+        VERTS[wi * 3] = _w[0];
+        VERTS[wi * 3 + 1] = _w[1];
+        VERTS[wi * 3 + 2] = _w[2];
+        vert_count++;
+
+        // remove faces visible from w, compacting the survivors; collect the
+        // horizon (edges on exactly one removed face) with cancellation
+        let edge_count = 0;
+        let kept = 0;
+        for (let f = 0; f < face_count; f++) {
+            const fn = f * 3;
+            if (v3_dot(FACE_N[fn], FACE_N[fn + 1], FACE_N[fn + 2], _w[0], _w[1], _w[2]) - FACE_DIST[f] > EPA_EPS) {
+                const i0 = FACE_IDX[fn], i1 = FACE_IDX[fn + 1], i2 = FACE_IDX[fn + 2];
+                edge_count = add_edge(edge_count, i0, i1);
+                edge_count = add_edge(edge_count, i1, i2);
+                edge_count = add_edge(edge_count, i2, i0);
+            } else {
+                if (kept !== f) {
+                    copy_face(kept, f);
+                }
+                kept++;
+            }
+        }
+        face_count = kept;
+
+        for (let e = 0; e < edge_count; e++) {
+            if (face_count >= FACE_CAP) {
+                break; // face pool full (unreachable for convex inputs)
+            }
+            face_count = add_face(EDGE[e * 2], EDGE[e * 2 + 1], wi, face_count);
+        }
+        if (face_count === 0) {
+            break; // degenerate polytope (all faces visible + horizon cancelled)
         }
-        faces = kept;
-        for (const [i, j] of edges) faces.push(makeFace(verts, [i, j, wi], cen));
-        if (faces.length === 0) break; // degenerate polytope (all faces visible + horizon cancelled)
     }
 
-    // recompute the closest face (faces may have changed on the last iteration)
-    if (faces.length === 0) {
-        // Degenerate: no face survived. Report no usable axis (caller treats a
-        // non-positive depth as a miss) rather than dereferencing faces[0] and
-        // throwing a TypeError out of the narrowphase step.
-        out[0] = 0; out[1] = 0; out[2] = 0;
+    if (face_count === 0) {
+        // Degenerate: no face survived. Report no usable axis (the caller treats a
+        // non-positive depth as a miss) rather than reading a stale face and
+        // throwing out of the narrowphase step.
+        out[0] = 0;
+        out[1] = 0;
+        out[2] = 0;
         return 0;
     }
-    let best = faces[0];
-    for (let i = 1; i < faces.length; i++) if (faces[i].dist < best.dist) best = faces[i];
-    out[0] = best.n[0]; out[1] = best.n[1]; out[2] = best.n[2];
-    return best.dist;
-}
 
-/** Build a face with an OUTWARD unit normal (away from the polytope centroid). */
-function makeFace(verts, idx, cen) {
-    const a = verts[idx[0]], b = verts[idx[1]], c = verts[idx[2]];
-    let n = cross(sub(b, a), sub(c, a));
-    const l = Math.sqrt(lensq(n)) || 1;
-    n = [n[0] / l, n[1] / l, n[2] / l];
-    // orient outward: normal should point away from the polytope interior (centroid)
-    if (dot(n, sub(a, cen)) < 0) { n = neg(n); const t = idx[1]; idx = [idx[0], idx[2], t]; }
-    return { v: idx, n, dist: dot(n, a) }; // perpendicular distance from origin (≥0 once outward)
-}
-
-/** Add an edge with horizon cancellation: if the reverse edge is present, drop both. */
-function addEdge(edges, i, j) {
-    for (let k = 0; k < edges.length; k++) {
-        if (edges[k][0] === j && edges[k][1] === i) { edges.splice(k, 1); return; }
+    let best = 0;
+    for (let f = 1; f < face_count; f++) {
+        if (FACE_DIST[f] < FACE_DIST[best]) {
+            best = f;
+        }
     }
-    edges.push([i, j]);
+    const bn = best * 3;
+    out[0] = FACE_N[bn];
+    out[1] = FACE_N[bn + 1];
+    out[2] = FACE_N[bn + 2];
+    return FACE_DIST[best];
 }
 
 /**
@@ -242,14 +530,15 @@ function addEdge(edges, i, j) {
  * Writes the unit separation normal (B→A) into `out` and returns the depth
  * (positive on overlap, 0 if separated — `out` untouched on 0).
  *
- * @param {Float64Array|number[]} out length ≥ 3
- * @param {{support:Function}} A  world-space support provider (e.g. PosedShape)
- * @param {{support:Function}} B
+ * @param {Float64Array|number[]} out destination for the unit normal, length ≥ 3
+ * @param {SupportProvider} A world-space support provider (e.g. PosedShape)
+ * @param {SupportProvider} B world-space support provider (e.g. PosedShape)
  * @returns {number} penetration depth, or 0 if separated
  */
 export function gjk_epa_penetration(out, A, B) {
-    const simplex = gjk(A, B);
-    if (simplex === null || simplex.length < 4) return 0;
-    const depth = epa(A, B, simplex, out);
+    if (gjk(A, B) < 4) {
+        return 0;
+    }
+    const depth = epa(A, B, out);
     return depth > 0 && Number.isFinite(depth) ? depth : 0;
 }