@woosh/meep-engine 2.138.20 → 2.140.0

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

@@ -0,0 +1,375 @@
+import { computeTriangleClosestPointToPointBarycentric } from "../../../core/geom/3d/triangle/computeTriangleClosestPointToPointBarycentric.js";
+import { line3_closest_points_segment_segment } from "../../../core/geom/3d/line/line3_closest_points_segment_segment.js";
+import { capsule_world_segment } from "./capsule_contacts.js";
+import { sphere_triangle_contact } from "./sphere_triangle_contact.js";
+
+/**
+ * Multi-point capsule-vs-triangle contact generation.
+ *
+ * A capsule is the Minkowski sum of a line segment and a sphere of
+ * radius `r`, so capsule-vs-triangle reduces to:
+ *
+ *   1. Find the closest pair of points between the capsule's central
+ *      segment and the triangle (segment ↔ triangle distance).
+ *   2. If distance ≥ radius, no overlap.
+ *   3. Otherwise emit a primary contact at the segment-triangle
+ *      closest pair, then additionally treat each cap centre (segment
+ *      endpoint) as a sphere of cap-radius and run
+ *      {@link sphere_triangle_contact} to produce up to 2 endpoint
+ *      contacts. Spatial dedup against the primary so a capsule
+ *      touching with one cap doesn't emit two contacts at the same
+ *      point.
+ *
+ * The multi-point manifold (1 primary + 2 endpoints) is what allows a
+ * capsule resting flat on a triangle face to be stable — 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. The endpoint contacts pin the
+ * capsule's rotation about its long axis.
+ *
+ * Output layout per contact (matches
+ * {@link CAPSULE_BOX_CONTACT_STRIDE} so the same downstream wiring
+ * works):
+ *   0..2 : world contact on capsule side
+ *   3..5 : world contact on triangle side
+ *   6..8 : normal from triangle (B) toward capsule (A)
+ *   9    : depth (positive = penetration)
+ *
+ * @author Alex Goldring
+ * @copyright Company Named Limited (c) 2026
+ */
+
+/**
+ * Output stride per contact.
+ * @type {number}
+ */
+export const CAPSULE_TRIANGLE_CONTACT_STRIDE = 10;
+
+/**
+ * Maximum contacts emitted: 1 primary + 2 endpoint sphere contacts.
+ * @type {number}
+ */
+export const CAPSULE_TRIANGLE_MAX_CONTACTS = 3;
+
+/**
+ * Two world positions are treated as the same contact when their
+ * squared distance is below this threshold. Same value as the box
+ * variant — looser than the manifold's warm-start match tolerance
+ * (≈0.02) because we specifically want to dedupe coincident
+ * primary/endpoint contacts; small lateral shifts produce distinct
+ * contacts that should both be kept.
+ * @type {number}
+ */
+const DEDUPE_DIST_SQR = 1e-6;
+
+/**
+ * Numerical floor for "segment direction roughly parallel to triangle
+ * plane". Used to bail out of the case-0 (segment-plane intersection)
+ * branch before dividing by `d · n`.
+ * @type {number}
+ */
+const PARALLEL_EPS = 1e-12;
+
+/**
+ * Tolerance for "reconstructed point matches input" in the inside-
+ * triangle check during case-0.
+ * @type {number}
+ */
+const INSIDE_TRIANGLE_EPS_SQR = 1e-10;
+
+// --- scratch (allocation-free across calls) ---------------------------------
+
+const scratch_seg = new Float64Array(6);
+const scratch_bary = new Float64Array(2);
+const scratch_pair_st = new Float64Array(2);
+const scratch_seg_tri = new Float64Array(6); // closest segment / triangle world points
+const scratch_sphere = new Float64Array(10);
+
+/**
+ * Compute the closest pair of points between a 3D line segment
+ * `(P1, P2)` and a triangle `(A, B, C)`. Writes the closest segment
+ * point into `out[0..2]` and the closest triangle point into
+ * `out[3..5]`, and returns the (positive) distance between them.
+ *
+ * Implementation: take the minimum of 6 candidate distances —
+ *  - Case 0: segment crosses triangle's plane inside the face
+ *    → distance 0; both points coincide on the face.
+ *  - Cases 1, 2: each segment endpoint vs its closest point on the
+ *    triangle (via {@link computeTriangleClosestPointToPointBarycentric}).
+ *    Subsumes the "endpoint vs triangle vertex / edge / face" cases.
+ *  - Cases 3, 4, 5: segment vs each of the triangle's 3 edges, via
+ *    {@link line3_closest_points_segment_segment}. Covers the
+ *    "segment interior closest to triangle edge" case that the
+ *    endpoint queries miss.
+ *
+ * @param {number[]|Float64Array} out length >= 6
+ */
+function segment_triangle_closest(
+    out,
+    p1x, p1y, p1z, p2x, p2y, p2z,
+    ax, ay, az, bx, by, bz, cx, cy, cz
+) {
+    let best_dist_sqr = Infinity;
+    let best_sx = 0, best_sy = 0, best_sz = 0;
+    let best_tx = 0, best_ty = 0, best_tz = 0;
+
+    // --- Case 0: segment intersects triangle plane INSIDE the face -----------
+    const dx_seg = p2x - p1x, dy_seg = p2y - p1y, dz_seg = p2z - p1z;
+    const e1x = bx - ax, e1y = by - ay, e1z = bz - az;
+    const e2x = cx - ax, e2y = cy - ay, e2z = cz - az;
+    const tnx = e1y * e2z - e1z * e2y;
+    const tny = e1z * e2x - e1x * e2z;
+    const tnz = e1x * e2y - e1y * e2x;
+    const d_dot_n = dx_seg * tnx + dy_seg * tny + dz_seg * tnz;
+    if (Math.abs(d_dot_n) > PARALLEL_EPS) {
+        const t = ((ax - p1x) * tnx + (ay - p1y) * tny + (az - p1z) * tnz) / d_dot_n;
+        if (t >= 0 && t <= 1) {
+            const ix = p1x + dx_seg * t;
+            const iy = p1y + dy_seg * t;
+            const iz = p1z + dz_seg * t;
+            // Inside-triangle test: feed the intersection through the
+            // closest-point algorithm; it returns the BARYCENTRIC of the
+            // closest point on the triangle. If the closest point equals
+            // the input (within INSIDE_TRIANGLE_EPS_SQR), the intersection
+            // is inside the face.
+            computeTriangleClosestPointToPointBarycentric(
+                scratch_bary, 0,
+                ix, iy, iz,
+                ax, ay, az,
+                bx, by, bz,
+                cx, cy, cz
+            );
+            const alpha = scratch_bary[0];
+            const beta = scratch_bary[1];
+            const gamma = 1 - alpha - beta;
+            const rx = alpha * ax + beta * bx + gamma * cx;
+            const ry = alpha * ay + beta * by + gamma * cy;
+            const rz = alpha * az + beta * bz + gamma * cz;
+            const diff_x = rx - ix, diff_y = ry - iy, diff_z = rz - iz;
+            if (diff_x * diff_x + diff_y * diff_y + diff_z * diff_z < INSIDE_TRIANGLE_EPS_SQR) {
+                out[0] = ix; out[1] = iy; out[2] = iz;
+                out[3] = ix; out[4] = iy; out[5] = iz;
+                return 0;
+            }
+        }
+    }
+
+    // --- Case 1: P1 vs closest-on-triangle -----------------------------------
+    computeTriangleClosestPointToPointBarycentric(
+        scratch_bary, 0,
+        p1x, p1y, p1z,
+        ax, ay, az,
+        bx, by, bz,
+        cx, cy, cz
+    );
+    {
+        const alpha = scratch_bary[0];
+        const beta = scratch_bary[1];
+        const gamma = 1 - alpha - beta;
+        const qx = alpha * ax + beta * bx + gamma * cx;
+        const qy = alpha * ay + beta * by + gamma * cy;
+        const qz = alpha * az + beta * bz + gamma * cz;
+        const dx = p1x - qx, dy = p1y - qy, dz = p1z - qz;
+        const d_sqr = dx * dx + dy * dy + dz * dz;
+        if (d_sqr < best_dist_sqr) {
+            best_dist_sqr = d_sqr;
+            best_sx = p1x; best_sy = p1y; best_sz = p1z;
+            best_tx = qx;  best_ty = qy;  best_tz = qz;
+        }
+    }
+
+    // --- Case 2: P2 vs closest-on-triangle -----------------------------------
+    computeTriangleClosestPointToPointBarycentric(
+        scratch_bary, 0,
+        p2x, p2y, p2z,
+        ax, ay, az,
+        bx, by, bz,
+        cx, cy, cz
+    );
+    {
+        const alpha = scratch_bary[0];
+        const beta = scratch_bary[1];
+        const gamma = 1 - alpha - beta;
+        const qx = alpha * ax + beta * bx + gamma * cx;
+        const qy = alpha * ay + beta * by + gamma * cy;
+        const qz = alpha * az + beta * bz + gamma * cz;
+        const dx = p2x - qx, dy = p2y - qy, dz = p2z - qz;
+        const d_sqr = dx * dx + dy * dy + dz * dz;
+        if (d_sqr < best_dist_sqr) {
+            best_dist_sqr = d_sqr;
+            best_sx = p2x; best_sy = p2y; best_sz = p2z;
+            best_tx = qx;  best_ty = qy;  best_tz = qz;
+        }
+    }
+
+    // --- Cases 3..5: segment vs each triangle edge ---------------------------
+    // Helper inline to avoid array allocation.
+    function test_edge(e_p1x, e_p1y, e_p1z, e_p2x, e_p2y, e_p2z) {
+        line3_closest_points_segment_segment(
+            scratch_pair_st,
+            p1x, p1y, p1z, p2x, p2y, p2z,
+            e_p1x, e_p1y, e_p1z, e_p2x, e_p2y, e_p2z
+        );
+        const s = scratch_pair_st[0];
+        const t = scratch_pair_st[1];
+        const sx = p1x + s * (p2x - p1x);
+        const sy = p1y + s * (p2y - p1y);
+        const sz = p1z + s * (p2z - p1z);
+        const tx = e_p1x + t * (e_p2x - e_p1x);
+        const ty = e_p1y + t * (e_p2y - e_p1y);
+        const tz = e_p1z + t * (e_p2z - e_p1z);
+        const dx = sx - tx, dy = sy - ty, dz = sz - tz;
+        const d_sqr = dx * dx + dy * dy + dz * dz;
+        if (d_sqr < best_dist_sqr) {
+            best_dist_sqr = d_sqr;
+            best_sx = sx; best_sy = sy; best_sz = sz;
+            best_tx = tx; best_ty = ty; best_tz = tz;
+        }
+    }
+    test_edge(ax, ay, az, bx, by, bz);
+    test_edge(bx, by, bz, cx, cy, cz);
+    test_edge(cx, cy, cz, ax, ay, az);
+
+    out[0] = best_sx; out[1] = best_sy; out[2] = best_sz;
+    out[3] = best_tx; out[4] = best_ty; out[5] = best_tz;
+    return Math.sqrt(best_dist_sqr);
+}
+
+// --- main --------------------------------------------------------------------
+
+/**
+ * @param {number[]|Float64Array} out length >= CAPSULE_TRIANGLE_MAX_CONTACTS *
+ *   CAPSULE_TRIANGLE_CONTACT_STRIDE
+ * @param {number} c_cx capsule centre x
+ * @param {number} c_cy
+ * @param {number} c_cz
+ * @param {number} c_qx capsule rotation
+ * @param {number} c_qy
+ * @param {number} c_qz
+ * @param {number} c_qw
+ * @param {number} c_radius
+ * @param {number} c_half_h capsule half-height (not including caps)
+ * @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 {number} number of contacts emitted, in `[0, CAPSULE_TRIANGLE_MAX_CONTACTS]`
+ */
+export function capsule_triangle_contact(
+    out,
+    c_cx, c_cy, c_cz, c_qx, c_qy, c_qz, c_qw, c_radius, c_half_h,
+    ax, ay, az, bx, by, bz, cx, cy, cz
+) {
+    // Capsule's world-space segment endpoints.
+    capsule_world_segment(scratch_seg, c_cx, c_cy, c_cz, c_qx, c_qy, c_qz, c_qw, c_half_h);
+    const sax = scratch_seg[0], say = scratch_seg[1], saz = scratch_seg[2];
+    const sbx = scratch_seg[3], sby = scratch_seg[4], sbz = scratch_seg[5];
+
+    // Primary contact: closest segment point ↔ closest triangle point.
+    const dist = segment_triangle_closest(
+        scratch_seg_tri,
+        sax, say, saz, sbx, sby, sbz,
+        ax, ay, az, bx, by, bz, cx, cy, cz
+    );
+    if (dist >= c_radius) return 0;
+
+    const seg_pt_x = scratch_seg_tri[0];
+    const seg_pt_y = scratch_seg_tri[1];
+    const seg_pt_z = scratch_seg_tri[2];
+    const tri_pt_x = scratch_seg_tri[3];
+    const tri_pt_y = scratch_seg_tri[4];
+    const tri_pt_z = scratch_seg_tri[5];
+
+    // Normal points from the triangle closest point toward the segment.
+    let nx, ny, nz;
+    if (dist > 0) {
+        const inv = 1 / dist;
+        nx = (seg_pt_x - tri_pt_x) * inv;
+        ny = (seg_pt_y - tri_pt_y) * inv;
+        nz = (seg_pt_z - tri_pt_z) * inv;
+    } else {
+        // Segment crosses or touches the triangle plane on the face —
+        // pick the triangle's outward face normal as a deterministic
+        // tie-break, same convention as sphere_triangle_contact.
+        const e1x = bx - ax, e1y = by - ay, e1z = bz - az;
+        const e2x = cx - ax, e2y = cy - ay, e2z = cz - az;
+        const fnx = e1y * e2z - e1z * e2y;
+        const fny = e1z * e2x - e1x * e2z;
+        const fnz = e1x * e2y - e1y * e2x;
+        const fn_mag = Math.sqrt(fnx * fnx + fny * fny + fnz * fnz);
+        if (fn_mag === 0) return 0; // degenerate triangle
+        nx = fnx / fn_mag;
+        ny = fny / fn_mag;
+        nz = fnz / fn_mag;
+    }
+
+    const depth = c_radius - dist;
+
+    // Primary contact emission.
+    // Capsule surface point: segment point pushed -radius along the normal
+    // (back toward the triangle).
+    out[0] = seg_pt_x - nx * c_radius;
+    out[1] = seg_pt_y - ny * c_radius;
+    out[2] = seg_pt_z - nz * c_radius;
+    out[3] = tri_pt_x; out[4] = tri_pt_y; out[5] = tri_pt_z;
+    out[6] = nx;       out[7] = ny;       out[8] = nz;
+    out[9] = depth;
+    let count = 1;
+
+    // Endpoint sphere contacts. Inline the 2 endpoint queries to keep
+    // allocation-free.
+    count = try_endpoint_contact(out, count, sax, say, saz, c_radius, ax, ay, az, bx, by, bz, cx, cy, cz);
+    count = try_endpoint_contact(out, count, sbx, sby, sbz, c_radius, ax, ay, az, bx, by, bz, cx, cy, cz);
+
+    return count;
+}
+
+/**
+ * Run sphere_triangle_contact at one cap centre and append the
+ * resulting contact to `out` (with spatial dedup against the
+ * already-emitted entries). Returns the new contact count.
+ *
+ * @returns {number}
+ */
+function try_endpoint_contact(
+    out, count,
+    cap_x, cap_y, cap_z, radius,
+    ax, ay, az, bx, by, bz, cx, cy, cz
+) {
+    const ok = sphere_triangle_contact(
+        scratch_sphere,
+        cap_x, cap_y, cap_z, radius,
+        ax, ay, az, bx, by, bz, cx, cy, cz
+    );
+    if (!ok) return count;
+
+    const e_nx = scratch_sphere[0], e_ny = scratch_sphere[1], e_nz = scratch_sphere[2];
+    const e_depth = scratch_sphere[3];
+    const e_cap_x = scratch_sphere[4], e_cap_y = scratch_sphere[5], e_cap_z = scratch_sphere[6];
+    const e_tri_x = scratch_sphere[7], e_tri_y = scratch_sphere[8], e_tri_z = scratch_sphere[9];
+
+    // Dedup: compare capsule-side world position against every prior
+    // contact. Two distinct triangle-surface points always have
+    // distinct capsule-side projections along the cap-radius vector,
+    // so capsule-side position uniquely identifies a contact.
+    for (let k = 0; k < count; k++) {
+        const ko = k * CAPSULE_TRIANGLE_CONTACT_STRIDE;
+        const dx = e_cap_x - out[ko];
+        const dy = e_cap_y - out[ko + 1];
+        const dz = e_cap_z - out[ko + 2];
+        if (dx * dx + dy * dy + dz * dz < DEDUPE_DIST_SQR) return count;
+    }
+
+    const o = count * CAPSULE_TRIANGLE_CONTACT_STRIDE;
+    out[o]     = e_cap_x; out[o + 1] = e_cap_y; out[o + 2] = e_cap_z;
+    out[o + 3] = e_tri_x; out[o + 4] = e_tri_y; out[o + 5] = e_tri_z;
+    out[o + 6] = e_nx;    out[o + 7] = e_ny;    out[o + 8] = e_nz;
+    out[o + 9] = e_depth;
+    return count + 1;
+}

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

@@ -0,0 +1,91 @@
+/**
+ * Compute the penetration depth between two shapes at world poses,
+ * returning the depth and writing the separation direction.
+ *
+ * If the shapes overlap, the return value is the positive distance
+ * along `out_direction` you would have to translate `shape_a` to
+ * separate it from `shape_b`. `out_direction` is the unit vector
+ * pointing from `shape_b` toward `shape_a` — applying
+ * `out_direction * return_value` to `position_a` (or equivalently
+ * `-out_direction * return_value` to `position_b`) is the minimum
+ * translation that produces separation.
+ *
+ * If the shapes do not overlap (or EPA degenerates on a tangent
+ * contact), the return value is `0` and `out_direction` is left
+ * untouched. Callers should treat 0 as "no penetration".
+ *
+ * Sign convention matches the narrowphase's stored contact normal:
+ *   - `out_direction` ≡ "B → A" direction
+ *   - Negative of cast direction in shape_cast at the kiss point
+ *   - The outward normal of B's surface at the contact, pointing
+ *     toward A
+ *
+ * Built on the existing GJK + EPA + PosedShape primitives — same
+ * precision characteristics (essentially exact for sign-based supports
+ * like cubes; asymptotic on curved supports like spheres near tangent,
+ * where the polytope iteration cap leaves a small angular residual on
+ * the direction).
+ *
+ * ## Non-convex support
+ *
+ * Exactly **one** of the two shapes may be non-convex (heightmap,
+ * mesh). The non-convex shape is decomposed into triangles overlapping
+ * the convex shape's AABB (via the same machinery the narrowphase
+ * uses), and per-triangle GJK + EPA is run; the deepest contact's
+ * direction and depth are reported. Concave-vs-concave throws — the
+ * M×N triangle-pair cost is out of scope for this primitive (and is
+ * also refused by the narrowphase for dynamic pairs).
+ *
+ * ## Non-convex precision limits
+ *
+ * The concave path is a per-triangle half-space test (convex's deepest
+ * point along −face_normal, compared to the triangle's plane). This
+ * is exact for **heightmaps** — adjacent triangles cover the
+ * boundary cases, and the face normal IS the contact direction.
+ *
+ * For **closed meshes** the half-space test extrapolates each triangle
+ * as an infinite plane, which can over-report depth on side faces
+ * when the convex shape extends past a face's 2D extent. The
+ * deepest-wins aggregation then picks a "false-deepest" face whose
+ * direction may not be the geometrically optimal one. A closed-form
+ * triangle-vs-X solver per primitive shape would fix this; until
+ * then, the function reports *some* outward direction with positive
+ * depth, which still resolves penetration over multiple iterations.
+ *
+ * Bodies fully inside the concave solid (or below a heightmap
+ * surface) are correctly recovered: every face's deepest-inward
+ * support point lands on the inward side, so the half-space test
+ * fires, and the deepest face wins. The reported direction pushes
+ * the body outward through that face.
+ *
+ * @param {Float64Array|number[]} out_direction length ≥ 3; receives
+ *   the unit separation direction (B → A) on penetration
+ * @param {AbstractShape3D} shape_a  in shape_a's local frame; may be concave
+ * @param {{x:number,y:number,z:number}} position_a  world position of A
+ * @param {{x:number,y:number,z:number,w:number}} rotation_a  world rotation of A
+ * @param {AbstractShape3D} shape_b  in shape_b's local frame; may be concave
+ * @param {{x:number,y:number,z:number}} position_b  world position of B
+ * @param {{x:number,y:number,z:number,w:number}} rotation_b  world rotation of B
+ * @returns {number} penetration depth (positive) on overlap, 0 otherwise
+ * @throws {Error} if both shapes have `is_convex === false`
+ */
+export function compute_penetration(out_direction: Float64Array | number[], shape_a: AbstractShape3D, position_a: {
+    x: number;
+    y: number;
+    z: number;
+}, rotation_a: {
+    x: number;
+    y: number;
+    z: number;
+    w: number;
+}, shape_b: AbstractShape3D, position_b: {
+    x: number;
+    y: number;
+    z: number;
+}, rotation_b: {
+    x: number;
+    y: number;
+    z: number;
+    w: number;
+}): number;
+//# sourceMappingURL=compute_penetration.d.ts.map

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

@@ -0,0 +1 @@
+{"version":3,"file":"compute_penetration.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/narrowphase/compute_penetration.js"],"names":[],"mappings":"AAkEA;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;GAsEG;AACH,mDAXW,YAAY,GAAC,MAAM,EAAE,wCAGrB;IAAC,CAAC,EAAC,MAAM,CAAC;IAAA,CAAC,EAAC,MAAM,CAAC;IAAA,CAAC,EAAC,MAAM,CAAA;CAAC,cAC5B;IAAC,CAAC,EAAC,MAAM,CAAC;IAAA,CAAC,EAAC,MAAM,CAAC;IAAA,CAAC,EAAC,MAAM,CAAC;IAAA,CAAC,EAAC,MAAM,CAAA;CAAC,wCAErC;IAAC,CAAC,EAAC,MAAM,CAAC;IAAA,CAAC,EAAC,MAAM,CAAC;IAAA,CAAC,EAAC,MAAM,CAAA;CAAC,cAC5B;IAAC,CAAC,EAAC,MAAM,CAAC;IAAA,CAAC,EAAC,MAAM,CAAC;IAAA,CAAC,EAAC,MAAM,CAAC;IAAA,CAAC,EAAC,MAAM,CAAA;CAAC,GACnC,MAAM,CA4BlB"}