@woosh/meep-engine 2.138.20 → 2.139.0

package/src/engine/physics/solver/solve_contacts.js

@@ -0,0 +1,598 @@
+import { BodyKind } from "../ecs/BodyKind.js";
+import { ColliderFlags } from "../ecs/ColliderFlags.js";
+import { CONTACT_STRIDE } from "../contact/ManifoldStore.js";
+import { RigidBodyFlags } from "../ecs/RigidBodyFlags.js";
+import { world_inverse_inertia_apply } from "../inertia/world_inverse_inertia.js";
+import { friction_cone_clamp } from "./friction_cone.js";
+
+/**
+ * A pair is "sensor-only" when either body OR either body's primary
+ * collider carries the IsSensor flag. The manifold still exists (so
+ * Begin/Stay/End events fire from the manifold-diff pass) but no impulse
+ * is applied.
+ *
+ * @param {RigidBody} rbA
+ * @param {Collider} colA
+ * @param {RigidBody} rbB
+ * @param {Collider} colB
+ * @returns {boolean}
+ */
+function pair_is_sensor(rbA, colA, rbB, colB) {
+    return (rbA.flags & RigidBodyFlags.IsSensor) !== 0
+        || (rbB.flags & RigidBodyFlags.IsSensor) !== 0
+        || (colA.flags & ColliderFlags.IsSensor) !== 0
+        || (colB.flags & ColliderFlags.IsSensor) !== 0;
+}
+
+/**
+ * Default velocity-iteration count per `solve_contacts` call. With TGS
+ * substepping driven by the PhysicsSystem, each tick now produces multiple
+ * solver calls (one per substep), so the per-call iter count is set lower
+ * than the classic PGS default of 10. Total iterations per tick =
+ * substeps × iters_per_substep; the recommended pairing is 4 × 2 = 8,
+ * which matches Box2D's TGS defaults and outperforms 1 × 10 on tall
+ * stacks (impulses get position-integrated between substeps, so info
+ * propagation through a contact chain compounds across substeps rather
+ * than fighting against accumulating position drift inside one solve).
+ *
+ * Direct callers that want classic PGS can still pass `iters = 10` and
+ * skip substepping by calling once with the full tick `dt`.
+ * @type {number}
+ */
+const DEFAULT_VELOCITY_ITERATIONS = 2;
+
+/**
+ * Baumgarte position-correction gain. Smaller values are "softer" (slower to
+ * push bodies apart but more stable); larger values can introduce jitter.
+ * 0.2 is the Catto / Box2D default.
+ * @type {number}
+ */
+const BAUMGARTE_BETA = 0.2;
+
+/**
+ * Penetration allowed without applying position correction. Eliminates
+ * micro-jitter at near-zero overlap.
+ * @type {number}
+ */
+const PENETRATION_SLOP = 0.005;
+
+/**
+ * Velocity below which restitution is suppressed (no "buzz" of micro-bounces
+ * when bodies are nearly resting on each other).
+ * @type {number}
+ */
+const RESTITUTION_VELOCITY_THRESHOLD = 1.0;
+
+/**
+ * Pre-step scratch stride per contact: 16 doubles
+ *   0..2 : rA  (contact point − bodyA position)
+ *   3..5 : rB  (contact point − bodyB position)
+ *   6..8 : t1  (tangent 1, unit, world)
+ *   9..11: t2  (tangent 2, unit, world)
+ *   12   : m_eff_n
+ *   13   : m_eff_t1
+ *   14   : m_eff_t2
+ *   15   : bias_n  (combined Baumgarte + restitution target velocity)
+ * @type {number}
+ */
+const PRE_STRIDE = 16;
+
+/** Per-contact index list — 4 uint32 per contact: slot, idx, idxA, idxB. */
+const INDEX_STRIDE = 4;
+
+let scratch_pre = new Float64Array(64 * PRE_STRIDE);
+let scratch_idx = new Uint32Array(64 * INDEX_STRIDE);
+let scratch_mu = new Float64Array(64);
+
+function ensure_capacity(n) {
+    if (scratch_pre.length < n * PRE_STRIDE) {
+        scratch_pre = new Float64Array(n * PRE_STRIDE * 2);
+    }
+    if (scratch_idx.length < n * INDEX_STRIDE) {
+        scratch_idx = new Uint32Array(n * INDEX_STRIDE * 2);
+    }
+    if (scratch_mu.length < n) {
+        scratch_mu = new Float64Array(n * 2);
+    }
+}
+
+/**
+ * Build an orthonormal tangent basis perpendicular to a unit normal. Standard
+ * "least-aligned axis" trick: pick the world axis least aligned with `n`,
+ * cross with `n`, then cross again for the second tangent.
+ *
+ * @param {Float64Array} out  6 floats: t1.xyz then t2.xyz
+ * @param {number} off
+ * @param {number} nx
+ * @param {number} ny
+ * @param {number} nz
+ */
+function build_tangents(out, off, nx, ny, nz) {
+    // Pick the world axis least aligned with n.
+    const ax = nx < 0 ? -nx : nx;
+    const ay = ny < 0 ? -ny : ny;
+    const az = nz < 0 ? -nz : nz;
+
+    let rx, ry, rz;
+    if (ax <= ay && ax <= az) {
+        rx = 1; ry = 0; rz = 0;
+    } else if (ay <= az) {
+        rx = 0; ry = 1; rz = 0;
+    } else {
+        rx = 0; ry = 0; rz = 1;
+    }
+    // t1 = normalize(n × r)
+    let t1x = ny * rz - nz * ry;
+    let t1y = nz * rx - nx * rz;
+    let t1z = nx * ry - ny * rx;
+    const inv = 1 / Math.sqrt(t1x * t1x + t1y * t1y + t1z * t1z);
+    t1x *= inv; t1y *= inv; t1z *= inv;
+
+    // t2 = n × t1  (already unit since n and t1 are orthogonal unit vectors)
+    const t2x = ny * t1z - nz * t1y;
+    const t2y = nz * t1x - nx * t1z;
+    const t2z = nx * t1y - ny * t1x;
+
+    out[off]     = t1x; out[off + 1] = t1y; out[off + 2] = t1z;
+    out[off + 3] = t2x; out[off + 4] = t2y; out[off + 5] = t2z;
+}
+
+/**
+ * @param {RigidBody} rb
+ * @returns {number}
+ */
+function inv_mass_of(rb) {
+    if (rb.kind !== BodyKind.Dynamic) return 0;
+    return rb.mass > 0 ? 1 / rb.mass : 0;
+}
+
+/**
+ * Combine two friction coefficients. Geometric mean — matches the Bullet /
+ * PhysX default and never goes beyond the smaller of the two.
+ * @param {number} a
+ * @param {number} b
+ * @returns {number}
+ */
+function combine_friction(a, b) {
+    return Math.sqrt(a * b);
+}
+
+/**
+ * Combine two restitution coefficients. Maximum — visually bounciest pair
+ * wins; matches Unity / common game-engine default.
+ * @param {number} a
+ * @param {number} b
+ * @returns {number}
+ */
+function combine_restitution(a, b) {
+    return a > b ? a : b;
+}
+
+const scratch_clamp = new Float64Array(2);
+const scratch_inertia_a = new Float64Array(3);
+const scratch_inertia_b = new Float64Array(3);
+
+/**
+ * Apply an impulse `P` at body-relative offset `r`, advancing both the linear
+ * velocity (Δv = P/m) and angular velocity (Δω = I⁻¹·(r × P)).
+ *
+ * Writes go directly to the typed-array backing of `linearVelocity` and
+ * `angularVelocity` — bypassing {@link Vector3#set} on purpose. Each `set()`
+ * runs a hasHandlers / sendN dispatch path which is dead weight in the
+ * solver inner loop (no observer is subscribed to a body's velocity in
+ * normal use, and even when one is, fanning out per-iteration writes is
+ * useless — callers want the post-step value).
+ *
+ * @param {RigidBody} rb
+ * @param {Transform} transform
+ * @param {number} invM
+ * @param {number} rx
+ * @param {number} ry
+ * @param {number} rz
+ * @param {number} Px
+ * @param {number} Py
+ * @param {number} Pz
+ * @param {number} sign +1 for body A, -1 for body B (the contact normal points
+ *                       from B → A, so A receives +P and B receives −P).
+ * @param {Float64Array} scratch_inertia 3-vector scratch for the angular term
+ */
+function apply_impulse_to_body(
+    rb, transform, invM,
+    rx, ry, rz,
+    Px, Py, Pz,
+    sign,
+    scratch_inertia
+) {
+    if (invM === 0) return;
+
+    const sPx = sign * Px;
+    const sPy = sign * Py;
+    const sPz = sign * Pz;
+
+    const lv = rb.linearVelocity;
+    lv[0] += sPx * invM;
+    lv[1] += sPy * invM;
+    lv[2] += sPz * invM;
+
+    // r × P (already-signed)
+    const tx = ry * sPz - rz * sPy;
+    const ty = rz * sPx - rx * sPz;
+    const tz = rx * sPy - ry * sPx;
+
+    world_inverse_inertia_apply(scratch_inertia, 0, rb, transform, tx, ty, tz);
+
+    const av = rb.angularVelocity;
+    av[0] += scratch_inertia[0];
+    av[1] += scratch_inertia[1];
+    av[2] += scratch_inertia[2];
+}
+
+/**
+ * Quadratic-form contribution of one body to the constraint effective mass
+ * along a unit axis. Computes `(r × axis)^T · I⁻¹_world · (r × axis)`.
+ *
+ * @param {RigidBody} rb
+ * @param {Transform} transform
+ * @param {number} rx
+ * @param {number} ry
+ * @param {number} rz
+ * @param {number} ax axis x
+ * @param {number} ay
+ * @param {number} az
+ * @param {Float64Array} scratch_inertia
+ * @returns {number}
+ */
+function angular_jacobian_contribution(
+    rb, transform,
+    rx, ry, rz,
+    ax, ay, az,
+    scratch_inertia
+) {
+    if (rb.kind !== BodyKind.Dynamic) return 0;
+    const ii = rb.inverseInertiaLocal;
+    if (ii.x === 0 && ii.y === 0 && ii.z === 0) return 0;
+
+    const rxax = ry * az - rz * ay;
+    const rxay = rz * ax - rx * az;
+    const rxaz = rx * ay - ry * ax;
+
+    world_inverse_inertia_apply(scratch_inertia, 0, rb, transform, rxax, rxay, rxaz);
+
+    return rxax * scratch_inertia[0]
+         + rxay * scratch_inertia[1]
+         + rxaz * scratch_inertia[2];
+}
+
+/**
+ * Sequential-impulse contact solver. Partitions the touched contact set into
+ * islands (via {@link IslandBuilder}, owned by the PhysicsSystem and rebuilt
+ * upstream in the pipeline), then iterates each island independently.
+ *
+ * Per-island iteration matters for two reasons:
+ *  1. Impulse propagation converges inside an island without waiting for
+ *     unrelated bodies' Gauss-Seidel updates from previous outer loops.
+ *  2. Disconnected awake bodies don't pay each other's solver cost — adding
+ *     an unrelated active body to the scene scales O(island_size) rather than
+ *     O(global_active).
+ *
+ * Coulomb friction is applied as a disk clamp in the contact tangent plane.
+ * Position correction is folded into the velocity solve via Baumgarte bias.
+ *
+ * The `apply_warm_start` flag gates the warm-start impulse application.
+ * In a classic PGS call (one solve per tick), pass `true` (default) — the
+ * cached impulses from the previous tick get replayed onto the velocity,
+ * seeding the iterations near the converged answer. In TGS substepping,
+ * the PhysicsSystem calls this once per substep; only substep 0 should
+ * apply warm-start, because subsequent substeps continue from the solver
+ * state left by the previous substep (the impulses in `data` already
+ * reflect the current velocity, so re-applying them would double-count).
+ *
+ * Inertial: angular response uses the full world-frame inverse inertia
+ * `R · diag(I⁻¹_local) · R^T` via {@link world_inverse_inertia_apply},
+ * correct for arbitrary rotations + diagonal local inertia.
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {PhysicsSystem} system  PhysicsSystem; reads `system.islands`.
+ * @param {number} dt step duration (full tick `dt` for PGS, sub-tick for TGS)
+ * @param {number} [iters]
+ * @param {boolean} [apply_warm_start]
+ */
+export function solve_contacts(manifolds, system, dt, iters = DEFAULT_VELOCITY_ITERATIONS, apply_warm_start = true) {
+    if (dt <= 0) return;
+
+    const islands = system.islands;
+    if (islands === undefined || islands === null) return;
+
+    const island_count = islands.island_count;
+    if (island_count === 0) return;
+
+    const slot_list = islands.contact_data;
+    const slot_offsets = islands.contact_offsets;
+
+    for (let i = 0; i < island_count; i++) {
+        const start = slot_offsets[i];
+        const end = slot_offsets[i + 1];
+        if (end === start) continue;
+        solve_island(manifolds, system, dt, iters, slot_list, start, end, apply_warm_start);
+    }
+}
+
+/**
+ * Solve a single island's contact constraints. Operates on the slice
+ * `slot_list[slot_start..slot_end)` of manifold slot ids. Module-scoped
+ * scratch buffers are reused across islands (resized by doubling when an
+ * island has more contacts than seen before in the same step).
+ *
+ * @param {ManifoldStore} manifolds
+ * @param {PhysicsSystem} system
+ * @param {number} dt
+ * @param {number} iters
+ * @param {Uint32Array} slot_list
+ * @param {number} slot_start
+ * @param {number} slot_end
+ * @param {boolean} apply_warm_start whether to replay cached impulses (see
+ *   {@link solve_contacts})
+ */
+function solve_island(manifolds, system, dt, iters, slot_list, slot_start, slot_end, apply_warm_start) {
+    // 1. Count contacts in this island so we can size scratch.
+    // Stale-but-cached slots (the island builder skips them upstream) and
+    // sensor-only slots contribute zero impulse but we still want exact
+    // sizing — sensor slots are filtered at the per-pair check below and
+    // contribute zero to `contact_total`.
+    let contact_total = 0;
+    for (let i = slot_start; i < slot_end; i++) {
+        const slot = slot_list[i];
+        contact_total += manifolds.contact_count(slot);
+    }
+    if (contact_total === 0) return;
+    ensure_capacity(contact_total);
+
+    const data = manifolds.data_buffer;
+    const pre = scratch_pre;
+    const idx = scratch_idx;
+    const mus = scratch_mu;
+
+    // 2. Pre-step + warm-start.
+    let c = 0;
+    for (let i = slot_start; i < slot_end; i++) {
+        const slot = slot_list[i];
+        const idxA = system.__index_of(manifolds.bodyA(slot));
+        const idxB = system.__index_of(manifolds.bodyB(slot));
+        const rbA = system.__bodies[idxA];
+        const rbB = system.__bodies[idxB];
+        const trA = system.__transforms[idxA];
+        const trB = system.__transforms[idxB];
+        // Material parameters come from the "primary" collider of each body
+        // — for single-collider bodies this is the only one; for compound
+        // bodies it's a v1 approximation (per-contact source-collider
+        // tracking is a follow-up). Fall back to defaults if a body has no
+        // attached colliders.
+        const colA = system.__primary_collider(idxA);
+        const colB = system.__primary_collider(idxB);
+        if (colA === null || colB === null) continue;
+        // Sensor pairs: contacts persist in the manifold (for events) but
+        // produce no impulse. Skip the solver work for this pair entirely.
+        if (pair_is_sensor(rbA, colA, rbB, colB)) continue;
+
+        const invMA = inv_mass_of(rbA);
+        const invMB = inv_mass_of(rbB);
+
+        const cc = manifolds.contact_count(slot);
+        const slot_off = manifolds.slot_data_offset(slot);
+        const restitution_combined = combine_restitution(colA.restitution, colB.restitution);
+        const friction_combined = combine_friction(colA.friction, colB.friction);
+
+        for (let k = 0; k < cc; k++) {
+            const off = slot_off + k * CONTACT_STRIDE;
+
+            const wax = data[off],     way = data[off + 1], waz = data[off + 2];
+            const wbx = data[off + 3], wby = data[off + 4], wbz = data[off + 5];
+            const nx  = data[off + 6], ny  = data[off + 7], nz  = data[off + 8];
+            const depth = data[off + 9];
+
+            // Application point: midpoint of the two surface witnesses.
+            const px = (wax + wbx) * 0.5;
+            const py = (way + wby) * 0.5;
+            const pz = (waz + wbz) * 0.5;
+
+            const rax = px - trA.position.x;
+            const ray = py - trA.position.y;
+            const raz = pz - trA.position.z;
+            const rbx = px - trB.position.x;
+            const rby = py - trB.position.y;
+            const rbz = pz - trB.position.z;
+
+            const pre_off = c * PRE_STRIDE;
+            pre[pre_off]     = rax; pre[pre_off + 1] = ray; pre[pre_off + 2] = raz;
+            pre[pre_off + 3] = rbx; pre[pre_off + 4] = rby; pre[pre_off + 5] = rbz;
+            build_tangents(pre, pre_off + 6, nx, ny, nz);
+
+            const t1x = pre[pre_off + 6], t1y = pre[pre_off + 7], t1z = pre[pre_off + 8];
+            const t2x = pre[pre_off + 9], t2y = pre[pre_off + 10], t2z = pre[pre_off + 11];
+
+            // K = invMA + invMB + (rA×axis)^T·I_w⁻¹_A·(rA×axis) + (rB×axis)^T·I_w⁻¹_B·(rB×axis)
+            const k_n = invMA + invMB
+                + angular_jacobian_contribution(rbA, trA, rax, ray, raz, nx, ny, nz, scratch_inertia_a)
+                + angular_jacobian_contribution(rbB, trB, rbx, rby, rbz, nx, ny, nz, scratch_inertia_b);
+            const k_t1 = invMA + invMB
+                + angular_jacobian_contribution(rbA, trA, rax, ray, raz, t1x, t1y, t1z, scratch_inertia_a)
+                + angular_jacobian_contribution(rbB, trB, rbx, rby, rbz, t1x, t1y, t1z, scratch_inertia_b);
+            const k_t2 = invMA + invMB
+                + angular_jacobian_contribution(rbA, trA, rax, ray, raz, t2x, t2y, t2z, scratch_inertia_a)
+                + angular_jacobian_contribution(rbB, trB, rbx, rby, rbz, t2x, t2y, t2z, scratch_inertia_b);
+
+            pre[pre_off + 12] = k_n  > 0 ? 1 / k_n  : 0;
+            pre[pre_off + 13] = k_t1 > 0 ? 1 / k_t1 : 0;
+            pre[pre_off + 14] = k_t2 > 0 ? 1 / k_t2 : 0;
+
+            // Pre-impulse relative velocity at the contact point, including
+            // angular contribution. Convention: n is from B toward A;
+            // dv = vA_at_contact − vB_at_contact; vn = dv · n.
+            // Cache the typed-array views once — Vector3 extends Float64Array,
+            // so `lv[0]` is a direct memory read, no signal observer chain.
+            const lvA = rbA.linearVelocity, avA = rbA.angularVelocity;
+            const lvB = rbB.linearVelocity, avB = rbB.angularVelocity;
+            const vAx_at = lvA[0] + avA[1] * raz - avA[2] * ray;
+            const vAy_at = lvA[1] + avA[2] * rax - avA[0] * raz;
+            const vAz_at = lvA[2] + avA[0] * ray - avA[1] * rax;
+            const vBx_at = lvB[0] + avB[1] * rbz - avB[2] * rby;
+            const vBy_at = lvB[1] + avB[2] * rbx - avB[0] * rbz;
+            const vBz_at = lvB[2] + avB[0] * rby - avB[1] * rbx;
+            const dvx = vAx_at - vBx_at;
+            const dvy = vAy_at - vBy_at;
+            const dvz = vAz_at - vBz_at;
+            const vn_pre = dvx * nx + dvy * ny + dvz * nz;
+
+            // Baumgarte position correction: gentle separation push when depth
+            // exceeds the slop tolerance.
+            let bias = 0;
+            if (depth > PENETRATION_SLOP) {
+                bias = -BAUMGARTE_BETA / dt * (depth - PENETRATION_SLOP);
+                // negative because the solver formula is `lambda = m_eff * (-vn + bias_pos)`
+                // → bias_pos = beta/dt * pen → we'll flip below by using `lambda = -m_eff * (vn + bias)`
+                // For clarity: target vn_new = -bias_pos (i.e. positive separation velocity).
+                // Stored sign convention: lambda = -m_eff * (vn + bias) with bias < 0 to push apart.
+            }
+            // Restitution: if closing fast, push the post-impulse vn to -e * vn_pre.
+            if (vn_pre < -RESTITUTION_VELOCITY_THRESHOLD) {
+                bias += restitution_combined * vn_pre;
+                // restitution_combined > 0, vn_pre < 0 → contribution is negative,
+                // reinforcing separation.
+            }
+            pre[pre_off + 15] = bias;
+
+            mus[c] = friction_combined;
+
+            idx[c * INDEX_STRIDE]     = slot;
+            idx[c * INDEX_STRIDE + 1] = k;
+            idx[c * INDEX_STRIDE + 2] = idxA;
+            idx[c * INDEX_STRIDE + 3] = idxB;
+
+            if (apply_warm_start) {
+                // Warm-start: replay cached impulses immediately so the
+                // iteration loop starts close to the answer. With TGS
+                // substepping, this only fires on substep 0 — later
+                // substeps continue from the impulses left by the
+                // previous substep's iterations (those are already
+                // reflected in the current velocity, re-applying would
+                // double-count).
+                const j_n  = data[off + 10];
+                const j_t1 = data[off + 11];
+                const j_t2 = data[off + 12];
+                const Px = nx * j_n + t1x * j_t1 + t2x * j_t2;
+                const Py = ny * j_n + t1y * j_t1 + t2y * j_t2;
+                const Pz = nz * j_n + t1z * j_t1 + t2z * j_t2;
+
+                apply_impulse_to_body(rbA, trA, invMA, rax, ray, raz, Px, Py, Pz, +1, scratch_inertia_a);
+                apply_impulse_to_body(rbB, trB, invMB, rbx, rby, rbz, Px, Py, Pz, -1, scratch_inertia_b);
+            }
+
+            c++;
+        }
+    }
+
+    const contact_count = c;
+
+    // 3. Velocity iterations.
+    for (let iter = 0; iter < iters; iter++) {
+        for (let ci = 0; ci < contact_count; ci++) {
+            const slot = idx[ci * INDEX_STRIDE];
+            const cidx = idx[ci * INDEX_STRIDE + 1];
+            const idxA = idx[ci * INDEX_STRIDE + 2];
+            const idxB = idx[ci * INDEX_STRIDE + 3];
+            const rbA = system.__bodies[idxA];
+            const rbB = system.__bodies[idxB];
+            const trA = system.__transforms[idxA];
+            const trB = system.__transforms[idxB];
+            const invMA = inv_mass_of(rbA);
+            const invMB = inv_mass_of(rbB);
+
+            const pre_off = ci * PRE_STRIDE;
+            const rax = pre[pre_off],     ray = pre[pre_off + 1], raz = pre[pre_off + 2];
+            const rbx = pre[pre_off + 3], rby = pre[pre_off + 4], rbz = pre[pre_off + 5];
+            const t1x = pre[pre_off + 6], t1y = pre[pre_off + 7], t1z = pre[pre_off + 8];
+            const t2x = pre[pre_off + 9], t2y = pre[pre_off + 10], t2z = pre[pre_off + 11];
+            const m_eff_n  = pre[pre_off + 12];
+            const m_eff_t1 = pre[pre_off + 13];
+            const m_eff_t2 = pre[pre_off + 14];
+            const bias_n   = pre[pre_off + 15];
+
+            const slot_off = manifolds.slot_data_offset(slot);
+            const off = slot_off + cidx * CONTACT_STRIDE;
+            const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];
+
+            // --- Current relative velocity at contact (linear + angular) ---
+            // Cache typed-array views once; direct indexing avoids the
+            // accessor chain on Vector3.x/y/z.
+            const lvA = rbA.linearVelocity, avA = rbA.angularVelocity;
+            const lvB = rbB.linearVelocity, avB = rbB.angularVelocity;
+
+            const vAx_at = lvA[0] + avA[1] * raz - avA[2] * ray;
+            const vAy_at = lvA[1] + avA[2] * rax - avA[0] * raz;
+            const vAz_at = lvA[2] + avA[0] * ray - avA[1] * rax;
+            const vBx_at = lvB[0] + avB[1] * rbz - avB[2] * rby;
+            const vBy_at = lvB[1] + avB[2] * rbx - avB[0] * rbz;
+            const vBz_at = lvB[2] + avB[0] * rby - avB[1] * rbx;
+            const dvx = vAx_at - vBx_at;
+            const dvy = vAy_at - vBy_at;
+            const dvz = vAz_at - vBz_at;
+
+            // --- Normal impulse ---
+            const vn = dvx * nx + dvy * ny + dvz * nz;
+            const j_n_accum = data[off + 10];
+            const lambda_n = -m_eff_n * (vn + bias_n);
+            const sum_n = j_n_accum + lambda_n;
+            const new_j_n = sum_n > 0 ? sum_n : 0;
+            const delta_j_n = new_j_n - j_n_accum;
+            data[off + 10] = new_j_n;
+
+            if (delta_j_n !== 0) {
+                const Pnx = nx * delta_j_n;
+                const Pny = ny * delta_j_n;
+                const Pnz = nz * delta_j_n;
+                apply_impulse_to_body(rbA, trA, invMA, rax, ray, raz, Pnx, Pny, Pnz, +1, scratch_inertia_a);
+                apply_impulse_to_body(rbB, trB, invMB, rbx, rby, rbz, Pnx, Pny, Pnz, -1, scratch_inertia_b);
+            }
+
+            // --- Friction impulse (Coulomb disk in tangent plane) ---
+            // Recompute relative velocity at contact after the normal impulse.
+            // lvA/avA/lvB/avB still alias the same arrays, so we just re-read.
+            const vAx2 = lvA[0] + avA[1] * raz - avA[2] * ray;
+            const vAy2 = lvA[1] + avA[2] * rax - avA[0] * raz;
+            const vAz2 = lvA[2] + avA[0] * ray - avA[1] * rax;
+            const vBx2 = lvB[0] + avB[1] * rbz - avB[2] * rby;
+            const vBy2 = lvB[1] + avB[2] * rbx - avB[0] * rbz;
+            const vBz2 = lvB[2] + avB[0] * rby - avB[1] * rbx;
+            const dvx2 = vAx2 - vBx2;
+            const dvy2 = vAy2 - vBy2;
+            const dvz2 = vAz2 - vBz2;
+
+            const vt1 = dvx2 * t1x + dvy2 * t1y + dvz2 * t1z;
+            const vt2 = dvx2 * t2x + dvy2 * t2y + dvz2 * t2z;
+
+            const j_t1_accum = data[off + 11];
+            const j_t2_accum = data[off + 12];
+            const lambda_t1 = -m_eff_t1 * vt1;
+            const lambda_t2 = -m_eff_t2 * vt2;
+
+            const want_t1 = j_t1_accum + lambda_t1;
+            const want_t2 = j_t2_accum + lambda_t2;
+            const max_friction = mus[ci] * new_j_n;
+            friction_cone_clamp(scratch_clamp, 0, want_t1, want_t2, max_friction);
+            const new_j_t1 = scratch_clamp[0];
+            const new_j_t2 = scratch_clamp[1];
+            const delta_t1 = new_j_t1 - j_t1_accum;
+            const delta_t2 = new_j_t2 - j_t2_accum;
+            data[off + 11] = new_j_t1;
+            data[off + 12] = new_j_t2;
+
+            if (delta_t1 !== 0 || delta_t2 !== 0) {
+                const Ptx = t1x * delta_t1 + t2x * delta_t2;
+                const Pty = t1y * delta_t1 + t2y * delta_t2;
+                const Ptz = t1z * delta_t1 + t2z * delta_t2;
+                apply_impulse_to_body(rbA, trA, invMA, rax, ray, raz, Ptx, Pty, Ptz, +1, scratch_inertia_a);
+                apply_impulse_to_body(rbB, trB, invMB, rbx, rby, rbz, Ptx, Pty, Ptz, -1, scratch_inertia_b);
+            }
+        }
+    }
+}

package/src/core/geom/3d/topology/struct/binary/io/edge/OrderedEdge.d.ts

@@ -1,34 +0,0 @@
-export class OrderedEdge {
-    /**
-     *
-     * @param {number} v1
-     * @param {number} v2
-     */
-    constructor(v1: number, v2: number);
-    v_low: number;
-    v_high: number;
-    /**
-     *
-     * @param {number} v1
-     * @param {number} v2
-     */
-    set(v1: number, v2: number): void;
-    /**
-     *
-     * @param {OrderedEdge} other
-     */
-    copy(other: OrderedEdge): void;
-    clone(): OrderedEdge;
-    /**
-     *
-     * @return {number}
-     */
-    hash(): number;
-    /**
-     *
-     * @param {OrderedEdge} other
-     * @returns {boolean}
-     */
-    equals(other: OrderedEdge): boolean;
-}
-//# sourceMappingURL=OrderedEdge.d.ts.map

package/src/core/geom/3d/topology/struct/binary/io/edge/OrderedEdge.d.ts.map

@@ -1 +0,0 @@
-{"version":3,"file":"OrderedEdge.d.ts","sourceRoot":"","sources":["../../../../../../../../../../src/core/geom/3d/topology/struct/binary/io/edge/OrderedEdge.js"],"names":[],"mappings":"AAEA;IACI;;;;OAIG;IACH,gBAHW,MAAM,MACN,MAAM,EAUhB;IANO,cAAe;IACf,eAAgB;IAOxB;;;;OAIG;IACH,QAHW,MAAM,MACN,MAAM,QAUhB;IAED;;;OAGG;IACH,YAFW,WAAW,QAKrB;IAED,qBAEC;IAED;;;OAGG;IACH,QAFY,MAAM,CAKjB;IAED;;;;OAIG;IACH,cAHW,WAAW,GACT,OAAO,CAMnB;CACJ"}

package/src/core/geom/3d/topology/struct/binary/io/edge/OrderedEdge.js

@@ -1,66 +0,0 @@
-import { split_by_2 } from "../../../../../../../binary/split_by_2.js";
-
-export class OrderedEdge {
-    /**
-     *
-     * @param {number} v1
-     * @param {number} v2
-     */
-    constructor(v1, v2) {
-        if (v1 < v2) {
-            this.v_low = v1;
-            this.v_high = v2;
-        } else {
-            this.v_low = v2;
-            this.v_high = v1;
-        }
-    }
-
-    /**
-     *
-     * @param {number} v1
-     * @param {number} v2
-     */
-    set(v1, v2) {
-        if (v1 < v2) {
-            this.v_low = v1;
-            this.v_high = v2;
-        } else {
-            this.v_low = v2;
-            this.v_high = v1;
-        }
-    }
-
-    /**
-     *
-     * @param {OrderedEdge} other
-     */
-    copy(other) {
-        this.v_low = other.v_low;
-        this.v_high = other.v_high;
-    }
-
-    clone() {
-        return new OrderedEdge(this.v_low, this.v_high);
-    }
-
-    /**
-     *
-     * @return {number}
-     */
-    hash() {
-        // resulting hashes are close by
-        return split_by_2(this.v_low) | (split_by_2(this.v_high) << 1);
-    }
-
-    /**
-     *
-     * @param {OrderedEdge} other
-     * @returns {boolean}
-     */
-    equals(other) {
-        return this.v_low === other.v_low
-            && this.v_high === other.v_high
-            ;
-    }
-}