@woosh/meep-engine 2.155.0 → 2.156.0

package/src/engine/physics/CONSTRAINT_SOLVER_IMPROVEMENTS_PLAN.md

@@ -0,0 +1,364 @@
+# Constraint-Solver Improvements Plan — A2 / A3 / A4
+
+**Status:** complete — A2 baked (no flags); A3/A4 dropped on evidence; jitter goal
+redirected to sleep · **Date:** 2026-06-09 · **Area:** `engine/physics` solver
+
+> **Outcome (numbers + reasoning in `CONSTRAINT_SOLVER_BENCH_LOG.md`).** Burned in with
+> **no flags/toggles** — one solver path. The Monte-Carlo **median over 150 seeds** was
+> the decider (single-seed results were basin-luck):
+> - **A2** (block 3×3 point-solve + symmetric sweep) — **BAKED**: −9–16% joint-solve
+>   time, exact ball-socket, mass-ratio-robust; residual-neutral in the median. The one
+>   robust win.
+> - **A3** (ω under-relaxation) — **REMOVED**: raises the *median* residual, chaotic /
+>   bimodal in ω on the metastable scene, +46% transient penetration. A disproven
+>   bandaid, not shipped.
+> - **A4** (contact-bias smoothing) — **REVERTED**: neutral, benefit unproven; not worth
+>   churning contact determinism.
+>
+> All config fields removed. Full physics suite **865 pass / 0 fail**; one golden band
+> re-baselined for A2's (honest) slightly-livelier settle.
+>
+> **The settled-jitter goal is not closed by the solver — and the data shows why:** the
+> residual is **basin/sleep-dominated, not convergence-dominated** (per-seed variance ≫
+> any config difference), so no convergence/relaxation lever moves the median. The real
+> lever is **island sleep** (item **B** below: aggregate-energy + hysteresis criterion),
+> then **A1** (reduced coordinates) to remove the maximal-coordinate residual at source.
+> A2 ships as a faster/more-correct foundation; B is the recommended next step.
+
+## 0. Goal & prime directive
+
+Close the residual-jitter / "never sleeps" gap on coupled, over-constrained islands
+(ragdolls, jointed chains) **without** resorting to "bump the iteration count". Three
+systemic changes, all staying inside the existing maximal-coordinate TGS solver:
+
+- **A4 — smooth the split-impulse position bias** near the penetration slop (kill the
+  bang-bang relay that drives contact-side chatter).
+- **A2 — block joint point-solve + root-ordered symmetric Gauss-Seidel** (remove
+  intra-joint coupling error and the directional bias of one-way PGS).
+- **A3 — constraint-level dissipation**: under-relaxation (SOR ω) on the joint solve +
+  optional compliance/damping on the locked joint DOFs (so the limit cycle *decays*
+  instead of persisting; replaces leaning on global velocity damping).
+
+**Prime directive for this work:** *every significant step is gated by the benchmark
+suite.* The engine is real-time and single-thread JS — a stray allocation, a polymorphic
+deopt, or an extra hot-loop branch can erase the win. We do **not** discover performance
+regressions after the fact: each change ships with an **a-priori expected perf delta**
+and a **hard gate**. A measured regression beyond its anticipated budget **halts the
+phase** until it is either optimized away or signed off with a recorded rationale.
+
+This is the engineering counterpart to the reduced-coordinate option (A1), which is
+explicitly **out of scope** here (see §8). A2/A3/A4 narrow the gap inside the current
+architecture; A1 is a separate strategic bet.
+
+## 1. Background
+
+The solver is already modern (see `JOLT_REVIEW.md`, `RAPIER_REVIEW.md`, `BULLET_REVIEW.md`,
+`PLAN.md`): TGS, 4 substeps with per-substep position re-integration, per-substep
+warm-starting for **both** contacts and joints, Catto split-impulse, persistent
+feature-ID manifolds, atomic island sleeping. The jitter is therefore **not** a missing
+technique — it is:
+
+1. **An unconverged coupled-island limit cycle.** 10 maximal-coordinate cone-twist joints
+   + self/ground contacts + ~4:1 mass ratio, solved by sequential impulse, leave a
+   residual that → 0 only in the iteration limit. The split-impulse position bias
+   re-derives from penetration around the 5 mm slop every substep — a relay forcing term —
+   while the only dissipation is blunt global velocity damping. Forcing + weak damping =
+   a low-amplitude limit cycle.
+2. **Above the sleep gate.** The residual spikes to ~0.7 rad/s, 7× the
+   `√sleepVelocitySqrThreshold ≈ 0.1` gate, so the island's sleep timer keeps resetting
+   and it never deactivates. (Same phenomenon the KEVA bench documents: a 5,320-body
+   island that "never fully quiesces, so atomic-island sleep rarely triggers".)
+
+The quality signal is already pinned by `PhysicsSystem.ragdoll.spec.js`:
+`GOLDEN.settledLinSpeed` (0.4–1.2 m/s, obs 0.73) and `GOLDEN.settledAngSpeed`
+(0.4–1.2 rad/s, obs 0.75), plus the 1000-seed Monte-Carlo sweep. Those are our quality
+acceptance instruments; this plan adds the *performance* acceptance instruments alongside.
+
+## 2. The three changes (scope summary)
+
+| ID | Change | Primary files | Risk | A-priori perf delta (gate) |
+|----|--------|---------------|------|----------------------------|
+| **A4** | Smooth contact position bias near slop | `solver/solve_contacts.js` | Low | ≤ **+2%** mean/p99 on contact-heavy benches; ~0% ragdoll |
+| **A2** | Block 3×3 joint point-solve; root-order joints; symmetric (alternating) sweep | `constraint/solve_constraints.js`, `island/*`, `ecs/PhysicsSystem.js` | Medium | ≤ **+10%** joint-heavy at *equal* `jointIterations`; **target net-neutral / negative** after iteration tuning; ~0% on contact-only benches |
+| **A3** | SOR under-relaxation + locked-DOF compliance/damping | `constraint/solve_constraints.js`, `ecs/Joint.js`, `ecs/PhysicsSystem.js` | Low–Med | ≤ **+2%** joint-heavy |
+
+Cumulative worst-case before tuning: **≈ +8–14%** on the joint-heavy ragdoll-field bench,
+**≈ +2–3%** contact-heavy, **≈ 0%** contact-only-no-joint. After A2 enables a
+`jointIterations` reduction at equal-or-better convergence (Phase 4), the joint-heavy
+target is **≤ +3% or net-negative**. These are the numbers the gates enforce.
+
+## 3. Measurement methodology & acceptance gates
+
+> This section is the backbone of the plan. No code change is merged until its
+> measurement step is green.
+
+### 3.1 Benchmark suite (existing — the perf canaries)
+
+Run from the moh root (`npx jest --config jest.conf.json -t "<name>"`, flip `test.skip`→`test`):
+
+- `ecs/PhysicsSystem.bench.spec.js`
+  - *falling tower* (1000 random shapes) — broad mixed contact load
+  - *settling grid* (1024) — **sleep canary** (must still sleep fast)
+  - *KEVA tower* (5,320) — large-island contact stress (heaviest; run alone)
+  - *high-churn lifecycle* (10k ticks) — alloc/leak/drift detector
+  - *reproducibility* (8 trials bit-identical) — **determinism gate**
+- `constraint/swing_twist.bench.spec.js` — joint math microbench (A2/A3 canary)
+- `ecs/PhysicsMeshStack.bench.spec.js`, `ecs/PhysicsNarrowphaseRain.bench.spec.js`,
+  `queries/raycast.bench.spec.js` — **control benches**: should be ~0% on all three
+  changes; non-zero here means we touched code we didn't mean to.
+
+### 3.2 New benches to add (Phase 0)
+
+`ecs/PhysicsSystem.ragdoll.bench.spec.js` (reuse `buildScene` from the ragdoll spec):
+
+- **micro** — 1 ragdoll × 600 steps: per-tick joint-solve cost in isolation (A2/A3 signal,
+  low contact noise).
+- **macro / field** — *N* ragdolls (start N=64, also report N=256) dropped into a shared
+  pile: realistic joint+contact+self-collision load. This is the **headline A2/A3 perf
+  bench**.
+- **settle metric** — for the field scene, report **(a)** ticks-until-island-sleep and
+  **(b)** fraction of bodies asleep at t=10 s, plus the settled-speed peak. This is the
+  *quality-of-settling* number the whole effort is about; it must **improve** (more sleep,
+  less residual), never regress.
+
+Follow the existing bench conventions exactly: `test.skip` by default, `performance.now()`
+per `fixedUpdate`, `compute_stats`, fixed seeds, report `sys.storage.awake_count`.
+
+### 3.3 Quality / stability gates (must stay green on every change)
+
+1. `PhysicsSystem.ragdoll.spec.js` — all 3 active tests pass; `GOLDEN.settledLinSpeed`
+   / `settledAngSpeed` bands hold (or **improve**, with a deliberate re-baseline recorded).
+2. **Determinism** — bit-identical: the ragdoll determinism test (256 steps) **and** the
+   bench reproducibility trial. No `toBeCloseTo`; `===` only.
+3. **No NaN/Inf**, no fall-through (`count_fall_through == 0` on the thick-slab benches),
+   KEVA stack holds.
+4. **Sleep not broken** — settling grid still reaches near-full sleep in the last second.
+5. Monte-Carlo sweep (`RAGDOLL_SWEEP_RUNS=200`) — no NaN seeds; worst settled speed within
+   band across the seed space.
+
+### 3.4 Performance protocol (JS noise control)
+
+JS timing is noisy; measurements are only trustworthy under a fixed protocol:
+
+- **Prefer same-process A/B via config flags** (§3.6): run each bench twice in one process,
+  flag-off then flag-on, and report the **delta**. This cancels machine/JIT/thermal
+  variance — the cleanest signal and the primary acceptance measurement.
+- **Warm-up**: discard the first full bench run (JIT tier-up) before timing.
+- **Repetition**: 5 runs; report **median of the per-run medians** for p50 and the
+  **max of per-run p99** for tail.
+- **Noise band**: deltas **< 3%** are reported but **not** gated (within run-to-run noise).
+  Gates trigger only on deltas that exceed both 3% **and** the budget.
+- **Environment**: record `node -v`, OS, machine; same machine for baseline and
+  comparison; AC power, no competing load. Pin the Node version for the whole effort.
+- KEVA (~15 min/run) is run **once per phase** (pre/post), not per-commit; the
+  lightweight benches (ragdoll micro/field, settling grid, swing_twist) run per-commit.
+
+### 3.5 Regression policy & anticipated-budget gate
+
+The user requirement — *"if performance regresses it must be anticipated and within
+expected limits"* — is operationalized as:
+
+- Each phase has a **budget** (the per-bench numbers in §2 / the table below).
+- After implementing a phase, run the protocol (§3.4). For every bench:
+  - delta **≤ budget** → **accept**, record the number.
+  - budget **< delta** within a documented stretch (e.g. before the Phase-4 iteration
+    cut) → **accept only with a one-line signed rationale** in the results log naming why
+    and the recovery plan.
+  - delta **> budget** with no rationale → **STOP**. Optimize (profile for alloc/deopt/
+    branch cost) or revert. Do not proceed to the next phase on an unexplained regression.
+- Quality gate failing → **STOP** regardless of perf (correctness-first; no loosening
+  tolerances to pass — see project policy).
+
+**Consolidated budget table** (filled with measured baselines in Phase 0):
+
+| Bench | Metric | Baseline (Phase 0) | After A4 | After A2 (equal iters) | After A3 | After Phase-4 tune |
+|-------|--------|--------------------|----------|------------------------|----------|--------------------|
+| ragdoll field (N=64) | p50 / p99 tick | TBD | budget +0% | +≤10% | +≤2% | **≤ +3% or ↓** |
+| ragdoll field — sleep% @10s | higher=better | TBD | ≥ baseline | **↑** | **↑** | **↑** |
+| settling grid | p50; last-1s | TBD | +≤2% | +≤1% | +≤1% | ≤ +3% |
+| KEVA tower | mean tick | TBD | +≤2% | +≤1% | +≤1% | ≤ +3% |
+| falling tower | p50 / p99 | TBD | +≤2% | +≤1% | +≤1% | ≤ +3% |
+| swing_twist micro | mean | TBD | ~0% | +≤15%* | +≤3% | ≤ +5% |
+| MeshStack / Rain / raycast | p50 | TBD | ~0% | ~0% | ~0% | ~0% |
+
+\* swing_twist is a pure joint-math microbench with no iteration-count amortization, so it
+shows A2's per-iteration cost most starkly; the field bench (with iteration tuning) is the
+real-world number.
+
+### 3.6 Config flags for clean A/B (and uniform-flow burn-in)
+
+Add solver config fields (defaults preserve current behavior so Phase 0 baseline ==
+current `master`), letting each change be measured in isolation and rolled back instantly:
+
+- `contactBiasSmoothing: boolean` (A4)
+- `jointBlockSolve: boolean`, `jointSweepSymmetric: boolean` (A2)
+- `jointRelaxation: number` (A3, ω; default **1.0** = no relaxation)
+- `jointComplianceLin: number` / `jointDampingLin: number` (A3; default **0** = rigid)
+
+These follow the existing config-knob style (`velocityIterations`, `jointIterations`, …),
+not sentinels. **Burn-in (Phase 4):** once a *boolean* is validated as strictly better,
+**collapse to the single path and delete the flag** (uniform control flow — we don't keep
+two joint solvers forever). The *numeric* knobs (`jointRelaxation`, compliance/damping)
+stay as real tuning config with sensible defaults.
+
+### 3.7 Results log
+
+Commit `CONSTRAINT_SOLVER_BENCH_LOG.md` next to this plan: one table per phase with
+machine/Node, baseline vs after, delta, pass/fail, and any signed rationale. The plan is
+the contract; the log is the evidence.
+
+## 4. Phases
+
+### Phase 0 — Baseline & harness (no solver changes)
+
+**Objective:** lock a trustworthy baseline and the new benches before touching the solver.
+
+- [ ] Add `ecs/PhysicsSystem.ragdoll.bench.spec.js` (micro + field + settle metric, §3.2).
+- [ ] Add the config flags (§3.6) wired as **no-ops at default** (pure plumbing; verify
+      determinism + golden bands unchanged → proves the plumbing is inert).
+- [ ] Run the **full** suite under the protocol (§3.4); fill the §3.5 baseline column and
+      seed `CONSTRAINT_SOLVER_BENCH_LOG.md`. Record `node -v`, machine.
+- [ ] Confirm all §3.3 quality gates green on baseline.
+- **Exit:** baseline numbers committed; flags inert; determinism intact.
+
+### Phase 1 — A4: smooth contact position bias
+
+**Why first:** smallest, most contained change (one function, contact-side only), so it
+validates the gate discipline before we touch the joint hot path. Independent of A2/A3.
+
+- [ ] In `solve_contacts.js` position pass, replace the hard slop gate
+      (`if depth>slop: bias=-spook_a*(depth-slop)` then `MAX_POSITION_BIAS` clamp) with a
+      **smooth ramp** across a small band above the slop (e.g. quadratic ease-in to the
+      linear region, or softplus), removing the C0 kink and the saturation corner that the
+      contact rings on. Keep the asymptotic linear gain identical so steady-state push-out
+      is unchanged.
+- [ ] Behind `contactBiasSmoothing` flag.
+- **Expected perf:** +2–4 flops/contact/position-iter (×4 substeps). Budget **≤ +2%** on
+  KEVA / grid / falling tower; **~0%** ragdoll & control benches.
+- [ ] **Bench gate:** flag-off vs flag-on A/B on KEVA, settling grid, falling tower,
+      ragdoll field, + control benches. Confirm within budget; record in log.
+- [ ] **Quality gate:** §3.3, with attention to fall-through (bias change must not let
+      penetration leak) and settled penetration band in the ragdoll spec.
+- **Exit:** within budget, quality green, logged. (Decision to default-on deferred to
+  Phase 4 burn-in.)
+
+### Phase 2 — A2: block point-solve + root-ordered symmetric Gauss-Seidel
+
+**Objective:** remove intra-joint coupling error and one-way-sweep directional bias — the
+structural joint-convergence win.
+
+- [ ] **Block the 3 locked-linear DOFs** of each joint: form the 3×3 effective-mass
+      `K = (invMₐ+invM_b)I + skew(rₐ)ᵀinvIₐskew(rₐ) + skew(r_b)ᵀinvI_b skew(r_b)`,
+      solve the coupled impulse via a **closed-form, zero-alloc, regularized** 3×3 inverse
+      (guard near-singular → ε or per-DOF fallback). Keep `LIMITED`/`MOTOR`/`SPRING`
+      angular DOFs per-DOF (inequalities don't block cleanly). Behind `jointBlockSolve`.
+      *(Standard Catto block-solver technique applied to the ball-socket point.)*
+- [ ] **Root-order joints** per island: BFS from a chosen root over island connectivity
+      (reuse `island/` data); store one pooled ordered-index array per island.
+- [ ] **Symmetric sweep**: alternate forward (root→tip) / backward each joint iteration.
+      Behind `jointSweepSymmetric`.
+- [ ] Preserve **warm-start** semantics exactly (per-substep replay); the block solve must
+      accumulate the same `dofImpulse` representation so warm-start and determinism hold.
+- **Expected perf:** per-joint-iteration linear-block arithmetic +40–80% vs 3 scalars,
+  ×`jointIterations`(8)×substeps(4). Budget **≤ +10%** joint-heavy / swing_twist **≤ +15%**
+  at equal iterations; **~0%** on contact-only & control benches (joint path not hit — a key
+  canary that the change is properly scoped).
+- [ ] **Bench gate:** A/B each flag *independently* then together, on ragdoll micro/field +
+      swing_twist + KEVA (KEVA has few joints → confirms ~0% there) + controls.
+- [ ] **Quality gate:** §3.3 + **convergence improvement evidence**: settled speed should
+      drop and/or sleep% rise on the ragdoll field bench at equal `jointIterations`.
+      Determinism re-baselined deliberately (the solve order/maths changed → exact values
+      change; update determinism reference + golden bands in one commit, recorded).
+- **Exit:** within budget, measurable convergence/sleep improvement, quality green, logged.
+
+### Phase 3 — A3: constraint-level dissipation
+
+**Objective:** make the residual *decay* (targeted dissipation), built on A2's solver.
+
+- [ ] **Under-relaxation (SOR ω<1)** on the joint velocity solve: scale the per-iteration
+      impulse delta by `jointRelaxation` (default 1.0). One multiply per DOF/iter.
+- [ ] **Locked-DOF compliance + damping**: give the locked joint DOFs a small SPOOK
+      compliance with damping by **reusing the existing `SPRING`/γ regularization path**
+      (`solve_constraints.js`), driven by `jointComplianceLin`/`jointDampingLin`
+      (default 0 = rigid). Soft *with damping* (not soft alone — soft alone rings).
+- [ ] Add per-joint override fields on `Joint` only if a global default proves insufficient
+      (prefer one global default; avoid per-joint sentinel sprawl).
+- **Expected perf:** ω ~free (<0.5%); compliance adds a few flops/locked-DOF/iter. Budget
+  **≤ +2%** joint-heavy; ~0% elsewhere.
+- [ ] **Tuning sweep:** grid `jointRelaxation ∈ {1.0, 0.9, 0.8, 0.7}` ×
+      `compliance/damping ∈ {off, light, medium}` measured against the **settled-speed /
+      sleep%** metric on the ragdoll field bench (and the Monte-Carlo sweep for robustness
+      across seeds). Pick the point that minimizes residual **without** making the ragdoll
+      visibly spongy (watch joint-anchor error / limit overshoot).
+- [ ] **Bench gate** + **quality gate** (§3.3); confirm the chosen ω/compliance lands the
+      ragdoll into **sleep within 10 s** (the headline outcome) and tighten the golden
+      bands to the new, lower settled speed (recorded re-baseline).
+- **Exit:** ragdoll sleeps / residual materially reduced; within budget; logged.
+
+### Phase 4 — Iteration tuning, burn-in, final regression sweep
+
+**Objective:** convert A2's better convergence into a perf *win*, and clean up.
+
+- [ ] With A2+A3 on, **reduce `jointIterations`** (8 → trial 6, 5, 4) and re-measure: find
+      the lowest count that holds the quality gates. This is where the joint-heavy budget
+      returns to **≤ +3% or net-negative**. (Note: this is *not* "bump iterations" — it's
+      spending A2's convergence headroom to **cut** work.)
+- [ ] **Burn-in**: for each validated boolean flag, collapse to the single code path and
+      delete the flag (uniform control flow). Keep numeric knobs as documented config with
+      the chosen defaults.
+- [ ] **Full final regression sweep** including KEVA and the high-churn + reproducibility
+      lifecycle benches; fill the final budget column; sign off any residual overage.
+- [ ] Update `PLAN.md` / relevant `*_REVIEW.md` cross-references; note remaining gap vs A1.
+- **Exit:** all gates green, log complete, flags burned in, defaults set.
+
+## 5. Risk register & rollback
+
+| Risk | Mitigation |
+|------|------------|
+| Hot-loop **allocation** in the 3×3 block path (GC → tail-latency spike) | All scratch in pooled typed arrays / locals; verify via p99 + high-churn drift bench; profile allocations |
+| **Deopt / megamorphism** from new branches | Keep flags monomorphic; burn-in removes branches; control benches must stay ~0% |
+| Block **near-singular** `K` (degenerate config) | Regularized inverse + per-DOF fallback; covered by a degenerate-config unit test |
+| **Determinism** drift (cross-platform FP from new math/order) | Re-baseline deterministically in one commit; reproducibility bench is a gate |
+| A3 **over-damping** → spongy ragdoll / joint drift | Tuning sweep gates on joint-anchor error + limit overshoot, not just residual |
+| Accidental scope creep into contacts (A2) | Contact-only & control benches must read ~0%; any movement = STOP & investigate |
+| Each change is independently **revertible** | Config flags until Phase-4 burn-in; one phase per commit series |
+
+## 6. Definition of done
+
+- Ragdoll **sleeps within 10 s** (or residual settled speed materially reduced and golden
+  bands re-baselined lower); Monte-Carlo sweep robust across seeds.
+- Every bench within its final budget (§3.5); all quality gates (§3.3) green; determinism
+  bit-identical; `CONSTRAINT_SOLVER_BENCH_LOG.md` complete with machine/Node and any
+  signed rationales.
+- Flags burned in; tuning knobs documented with defaults.
+
+## 7. Out of scope (referenced, not done here)
+
+- **A1 — reduced-coordinate (Featherstone) articulation.** The exact-joint formulation;
+  a separate strategic pillar (parallel solver + contact-coupling layer + two-formulation
+  maintenance tax). A2/A3/A4 narrow the gap inside the current architecture first; A1 is
+  evaluated on its own once these land and are measured.
+- **B — joint-aware island sleep** (aggregate-energy + hysteresis sleep criterion). A
+  complementary, mostly-orthogonal change to `__sleep_test` in `PhysicsSystem.js`. If
+  A3 already gets the ragdoll under the existing gate, B may be unnecessary; if not, B is
+  the cheap finisher. Track separately.
+
+## 8. Appendix — touch points (current `master`)
+
+- **Config knobs** — `ecs/PhysicsSystem.js`: `substeps`(4), `velocityIterations`(4),
+  `positionIterations`(1), `jointIterations`(8), `sleepVelocitySqrThreshold`(0.01),
+  `sleepTimeThreshold`(0.5). Add A2/A3/A4 flags here.
+- **A4** — `solver/solve_contacts.js`: `PENETRATION_SLOP`(0.005), `CONTACT_RELAXATION`(4.75),
+  `MAX_POSITION_BIAS`(3), SPOOK gains (`g_spook_a`), per-contact position-bias block.
+- **A2/A3** — `constraint/solve_constraints.js`: joint velocity solve, per-DOF loop,
+  per-substep warm-start, `LIMITED` inequality handling, `SPRING`/γ compliance path
+  (reused by A3), `swing_twist_error`. Joint ordering: `island/` (union-find + builder).
+- **A3 joint fields** — `ecs/Joint.js` (`DofMode`, `dofImpulse`), `constraint/DofMode.js`.
+- **Benches** — `ecs/PhysicsSystem.bench.spec.js`, `constraint/swing_twist.bench.spec.js`,
+  `ecs/PhysicsMeshStack.bench.spec.js`, `ecs/PhysicsNarrowphaseRain.bench.spec.js`,
+  `queries/raycast.bench.spec.js`; **new** `ecs/PhysicsSystem.ragdoll.bench.spec.js`.
+- **Quality** — `ecs/PhysicsSystem.ragdoll.spec.js` (`GOLDEN` bands, determinism,
+  Monte-Carlo sweep), `ecs/PhysicsSystem.bench.spec.js` reproducibility trial.
+
+> Line numbers intentionally omitted in favor of symbol names — they drift; grep the
+> symbols. Verify each touch point before editing.

package/src/engine/physics/PLAN.md

@@ -834,11 +834,12 @@ correctness gain.
 
 Phasing (each phase: implement → spec → run from `H:/git/moh` → commit):
 
-1. [x] **Ray-primitive helpers** — landed as `narrowphase/ray_shapes.js`
-   (local-frame `ray_sphere_local` / `ray_box_local` / `ray_capsule_local`,
-   not `core/geom`: the (`t`, normal, miss = `Infinity`, first-hit-from-outside)
-   convention is raycast-specific, and the dispatch shares one ray→local
-   transform across them). Built local-frame (unit direction ⇒ `t` preserved;
+1. [x] **Ray-primitive helpers** — originally landed as `narrowphase/ray_shapes.js`,
+   later EXTRACTED to `core/geom/3d/{sphere,box,capsule}/{sphere3,box3,capsule3}_raycast.js`
+   (the (`t`, normal, miss = `Infinity`, first-hit-from-outside) convention is a
+   fine general raycast contract, and a ray-vs-capsule primitive was otherwise
+   missing engine-wide; the dispatch still shares one ray→local transform across
+   them). Built local-frame (unit direction ⇒ `t` preserved;
    rotate the local normal back). Triangle MT is inlined in the concave path
    (the existing `computeTriangleRayIntersection` writes a `SurfacePoint3` and
    returns no `t` — unsuited to the buffer-flyweight loop). Colocated specs.

package/src/engine/physics/constraint/solve_constraints.d.ts

@@ -33,6 +33,9 @@ export function swing_twist_error(dx: number, dy: number, dz: number, dw: number
  * @param {number} dt_sub  substep size in seconds (the SPOOK gain is derived
  *   from it, matching the contact solver's per-substep position stiffness)
  * @param {number} iters  velocity iterations
+ * @param {boolean} [reverse]  A2: traverse the joint array tip→root this call
+ *   (the caller alternates it per substep for a symmetric Gauss-Seidel sweep, so
+ *   chain impulses propagate both ways). Default false ⇒ original root→tip order.
  */
-export function solve_joints(joints: Joint[], system: PhysicsSystem, dt_sub: number, iters: number): void;
+export function solve_joints(joints: Joint[], system: PhysicsSystem, dt_sub: number, iters: number, reverse?: boolean): void;
 //# sourceMappingURL=solve_constraints.d.ts.map

package/src/engine/physics/constraint/solve_constraints.d.ts.map

@@ -1 +1 @@
-{"version":3,"file":"solve_constraints.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/constraint/solve_constraints.js"],"names":[],"mappings":"AAiOA;;;;;;;;;;;;;;;;;;;;GAoBG;AACH,sCAHW,MAAM,MAAa,MAAM,MAAa,MAAM,MAAa,MAAM,OAC/D,YAAY,QAkCtB;AA0JD;;;;;;;;;;;;;GAaG;AACH,qCANW,OAAO,iCAEP,MAAM,SAEN,MAAM,QAsPhB"}
+{"version":3,"file":"solve_constraints.d.ts","sourceRoot":"","sources":["../../../../../src/engine/physics/constraint/solve_constraints.js"],"names":[],"mappings":"AA6RA;;;;;;;;;;;;;;;;;;;;GAoBG;AACH,sCAHW,MAAM,MAAa,MAAM,MAAa,MAAM,MAAa,MAAM,OAC/D,YAAY,QAkCtB;AA0JD;;;;;;;;;;;;;;;;GAgBG;AACH,qCATW,OAAO,iCAEP,MAAM,SAEN,MAAM,YACN,OAAO,QA2SjB"}

package/src/engine/physics/constraint/solve_constraints.js

@@ -123,6 +123,15 @@ const ang_clo = new Float64Array(3);
 const ang_chi = new Float64Array(3);
 const ang_gamma = new Float64Array(3);
 
+// A2 (block solve) scratch: the coupled 3×3 effective-mass of a fully-LOCKED
+// linear point constraint and its inverse, stored as the 6 symmetric entries
+// [00, 01, 02, 11, 12, 22]; plus the per-body cross products `r×aₖ` and their
+// inverse-inertia images `Iw⁻¹·(r×aₖ)`.
+const lin_K = new Float64Array(6);
+const lin_Kinv = new Float64Array(6);
+const block_c = new Float64Array(9);
+const block_w = new Float64Array(9);
+
 /**
  * Angular effective-mass contribution of one body about a unit world axis:
  * `a · Iw⁻¹ · a`. Zero for non-dynamic / rotation-locked bodies (whose
@@ -223,6 +232,57 @@ function clamp_bias(b) {
     return b;
 }
 
+/**
+ * A2 — accumulate one body's angular contribution to the 3×3 point-constraint
+ * effective-mass block `K` (frame axes): for each frame axis `aₖ`,
+ * `cₖ = r×aₖ` and `wₖ = Iw⁻¹·cₖ`, then `K_kl += cₖ·w_l`. Symmetric, so only the
+ * upper triangle is written. Rotation-locked / non-dynamic bodies (zero inverse
+ * inertia) add nothing. The mass term `(invMA+invMB)·I` is seeded by the caller.
+ *
+ * @param {Float64Array} ss @param {number} base
+ * @param {number} rx @param {number} ry @param {number} rz  lever arm
+ * @param {Float64Array} fa  frame-A world basis (rows = axes)
+ * @param {Float64Array} K   length-6 symmetric accumulator [00,01,02,11,12,22]
+ */
+function accumulate_point_block(ss, base, rx, ry, rz, fa, K) {
+    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
+    if (iix === 0 && iiy === 0 && iiz === 0) return;
+    const qx = ss[base + SBS_QX], qy = ss[base + SBS_QY], qz = ss[base + SBS_QZ], qw = ss[base + SBS_QW];
+    for (let k = 0; k < 3; k++) {
+        const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
+        const cx = ry * az - rz * ay, cy = rz * ax - rx * az, cz = rx * ay - ry * ax;
+        block_c[3 * k] = cx; block_c[3 * k + 1] = cy; block_c[3 * k + 2] = cz;
+        world_inverse_inertia_apply_raw(block_w, 3 * k, iix, iiy, iiz, qx, qy, qz, qw, cx, cy, cz);
+    }
+    K[0] += block_c[0] * block_w[0] + block_c[1] * block_w[1] + block_c[2] * block_w[2];
+    K[1] += block_c[0] * block_w[3] + block_c[1] * block_w[4] + block_c[2] * block_w[5];
+    K[2] += block_c[0] * block_w[6] + block_c[1] * block_w[7] + block_c[2] * block_w[8];
+    K[3] += block_c[3] * block_w[3] + block_c[4] * block_w[4] + block_c[5] * block_w[5];
+    K[4] += block_c[3] * block_w[6] + block_c[4] * block_w[7] + block_c[5] * block_w[8];
+    K[5] += block_c[6] * block_w[6] + block_c[7] * block_w[7] + block_c[8] * block_w[8];
+}
+
+/**
+ * Invert a symmetric positive-definite 3×3 (6-entry form) in closed form,
+ * writing the inverse (same 6-entry form) to `out`. Returns the determinant; a
+ * near-singular block (|det| < 1e-12) returns 0 and leaves `out` untouched so
+ * the caller falls back to the per-axis solve.
+ *
+ * @param {Float64Array} K  [00,01,02,11,12,22] @param {Float64Array} out
+ * @returns {number} determinant
+ */
+function invert_sym3(K, out) {
+    const a = K[0], b = K[1], c = K[2], d = K[3], e = K[4], f = K[5];
+    const A00 = d * f - e * e, A01 = c * e - b * f, A02 = b * e - c * d;
+    const A11 = a * f - c * c, A12 = b * c - a * e, A22 = a * d - b * b;
+    const det = a * A00 + b * A01 + c * A02;
+    if (det < 1e-12 && det > -1e-12) return 0;
+    const inv = 1 / det;
+    out[0] = A00 * inv; out[1] = A01 * inv; out[2] = A02 * inv;
+    out[3] = A11 * inv; out[4] = A12 * inv; out[5] = A22 * inv;
+    return det;
+}
+
 /**
  * Swing-twist decomposition of the relative rotation `qD = conj(qA)⊗qB`, giving
  * the per-frame-axis angular positions used by wide-cone angular DOFs.
@@ -443,8 +503,11 @@ function fill_row(joint, dofIndex, k, keff, pos, vel, spook_locked, spook_spec,
  * @param {number} dt_sub  substep size in seconds (the SPOOK gain is derived
  *   from it, matching the contact solver's per-substep position stiffness)
  * @param {number} iters  velocity iterations
+ * @param {boolean} [reverse]  A2: traverse the joint array tip→root this call
+ *   (the caller alternates it per substep for a symmetric Gauss-Seidel sweep, so
+ *   chain impulses propagate both ways). Default false ⇒ original root→tip order.
  */
-export function solve_joints(joints, system, dt_sub, iters) {
+export function solve_joints(joints, system, dt_sub, iters, reverse) {
     const n = joints.length;
     if (n === 0 || dt_sub <= 0) return;
 
@@ -454,7 +517,8 @@ export function solve_joints(joints, system, dt_sub, iters) {
     const storage = system.storage;
     const ss = system.__solver_state.data;
 
-    for (let ji = 0; ji < n; ji++) {
+    for (let jj = 0; jj < n; jj++) {
+        const ji = reverse ? (n - 1 - jj) : jj;
         const joint = joints[ji];
         if (joint === undefined || joint === null) continue;
 
@@ -541,26 +605,48 @@ export function solve_joints(joints, system, dt_sub, iters) {
 
         // --- Linear DOF row setup (frame axes). Position along axis k is
         //     `C · axis_k`; convention is A−B (impulse +to A / −to B). ---
+        let useLinBlock = false;
         if (linConstrained) {
-            for (let k = 0; k < 3; k++) {
-                lin_active[k] = 0;
-                const m = mode[k];
-                if (m === DofMode.FREE) { imp[k] = 0; continue; }
-                const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
-                const keff = axis_effective_mass(ss, baseA, invMA, rAx, rAy, rAz, ax, ay, az)
-                    + (to_world ? 0 : axis_effective_mass(ss, baseB, invMB, rBx, rBy, rBz, ax, ay, az));
-                const pos = Cx * ax + Cy * ay + Cz * az;
-                // Relative anchor velocity along the axis (A−B), for LIMITED
-                // bound selection. Cheap; only the projection is used.
-                let vel = 0;
-                if (m === DofMode.LIMITED) {
-                    const rvx = (ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * rAz - ss[baseA + SBS_AV_Z] * rAy) - (to_world ? 0 : (ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rBz - ss[baseB + SBS_AV_Z] * rBy));
-                    const rvy = (ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rAx - ss[baseA + SBS_AV_X] * rAz) - (to_world ? 0 : (ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rBx - ss[baseB + SBS_AV_X] * rBz));
-                    const rvz = (ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * rAy - ss[baseA + SBS_AV_Y] * rAx) - (to_world ? 0 : (ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rBy - ss[baseB + SBS_AV_Y] * rBx));
-                    vel = rvx * ax + rvy * ay + rvz * az;
+            // A2: a fully-LOCKED linear triple is a ball-socket point — solve it
+            // as one coupled 3×3 block (frame axes) instead of axis-by-axis,
+            // removing the intra-joint coupling error the sequential per-axis
+            // Gauss-Seidel leaves behind. Falls back to per-axis if the block is
+            // near-singular.
+            if (mode[0] === DofMode.LOCKED && mode[1] === DofMode.LOCKED && mode[2] === DofMode.LOCKED) {
+                lin_K[0] = lin_K[3] = lin_K[5] = invMA + invMB;
+                lin_K[1] = lin_K[2] = lin_K[4] = 0;
+                accumulate_point_block(ss, baseA, rAx, rAy, rAz, fa, lin_K);
+                if (!to_world) accumulate_point_block(ss, baseB, rBx, rBy, rBz, fa, lin_K);
+                if (invert_sym3(lin_K, lin_Kinv) !== 0) {
+                    for (let k = 0; k < 3; k++) {
+                        const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
+                        lin_bias[k] = clamp_bias(spook_locked * (Cx * ax + Cy * ay + Cz * az));
+                        lin_active[k] = 1;
+                    }
+                    useLinBlock = true;
+                }
+            }
+            if (!useLinBlock) {
+                for (let k = 0; k < 3; k++) {
+                    lin_active[k] = 0;
+                    const m = mode[k];
+                    if (m === DofMode.FREE) { imp[k] = 0; continue; }
+                    const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
+                    const keff = axis_effective_mass(ss, baseA, invMA, rAx, rAy, rAz, ax, ay, az)
+                        + (to_world ? 0 : axis_effective_mass(ss, baseB, invMB, rBx, rBy, rBz, ax, ay, az));
+                    const pos = Cx * ax + Cy * ay + Cz * az;
+                    // Relative anchor velocity along the axis (A−B), for LIMITED
+                    // bound selection. Cheap; only the projection is used.
+                    let vel = 0;
+                    if (m === DofMode.LIMITED) {
+                        const rvx = (ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * rAz - ss[baseA + SBS_AV_Z] * rAy) - (to_world ? 0 : (ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rBz - ss[baseB + SBS_AV_Z] * rBy));
+                        const rvy = (ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rAx - ss[baseA + SBS_AV_X] * rAz) - (to_world ? 0 : (ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rBx - ss[baseB + SBS_AV_X] * rBz));
+                        const rvz = (ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * rAy - ss[baseA + SBS_AV_Y] * rAx) - (to_world ? 0 : (ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rBy - ss[baseB + SBS_AV_Y] * rBx));
+                        vel = rvx * ax + rvy * ay + rvz * az;
+                    }
+                    fill_row(joint, k, k, keff, pos, vel, spook_locked, spook_spec, dt_sub,
+                        lin_active, lin_eff, lin_bias, lin_clo, lin_chi, lin_gamma);
                 }
-                fill_row(joint, k, k, keff, pos, vel, spook_locked, spook_spec, dt_sub,
-                    lin_active, lin_eff, lin_bias, lin_clo, lin_chi, lin_gamma);
             }
         } else {
             lin_active[0] = lin_active[1] = lin_active[2] = 0;
@@ -650,22 +736,50 @@ export function solve_joints(joints, system, dt_sub, iters) {
             // Linear rows (frame axes). The relative anchor velocity is
             // recomputed before each row so successive rows see the prior
             // impulses (Gauss-Seidel coupling).
-            for (let k = 0; k < 3; k++) {
-                if (!lin_active[k]) continue;
-                const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
+            if (useLinBlock) {
+                // A2: coupled 3×3 point solve — one relative anchor velocity,
+                // projected onto the frame axes, solved against Kⁱⁿᵛ in a single
+                // Newton step (bilateral, no clamp). The point constraint is
+                // satisfied exactly each iteration; only its coupling with the
+                // angular rows remains to iterate.
                 const rvx = (ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * rAz - ss[baseA + SBS_AV_Z] * rAy) - (to_world ? 0 : (ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rBz - ss[baseB + SBS_AV_Z] * rBy));
                 const rvy = (ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rAx - ss[baseA + SBS_AV_X] * rAz) - (to_world ? 0 : (ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rBx - ss[baseB + SBS_AV_X] * rBz));
                 const rvz = (ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * rAy - ss[baseA + SBS_AV_Y] * rAx) - (to_world ? 0 : (ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rBy - ss[baseB + SBS_AV_Y] * rBx));
-                const vrel = rvx * ax + rvy * ay + rvz * az;
-                const old = imp[k];
-                // `+ γ·old` regularises a spring row (γ = 0 otherwise → exact).
-                let nv = old - lin_eff[k] * (vrel + lin_bias[k] + lin_gamma[k] * old);
-                if (nv < lin_clo[k]) nv = lin_clo[k]; else if (nv > lin_chi[k]) nv = lin_chi[k];
-                imp[k] = nv;
-                const d = nv - old;
-                if (d !== 0) {
-                    apply_impulse(ss, baseA, invMA, rAx, rAy, rAz, d * ax, d * ay, d * az, +1);
-                    if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, d * ax, d * ay, d * az, -1);
+                const a0x = fa[0], a0y = fa[1], a0z = fa[2];
+                const a1x = fa[3], a1y = fa[4], a1z = fa[5];
+                const a2x = fa[6], a2y = fa[7], a2z = fa[8];
+                const b0 = rvx * a0x + rvy * a0y + rvz * a0z + lin_bias[0];
+                const b1 = rvx * a1x + rvy * a1y + rvz * a1z + lin_bias[1];
+                const b2 = rvx * a2x + rvy * a2y + rvz * a2z + lin_bias[2];
+                const d0 = -(lin_Kinv[0] * b0 + lin_Kinv[1] * b1 + lin_Kinv[2] * b2);
+                const d1 = -(lin_Kinv[1] * b0 + lin_Kinv[3] * b1 + lin_Kinv[4] * b2);
+                const d2 = -(lin_Kinv[2] * b0 + lin_Kinv[4] * b1 + lin_Kinv[5] * b2);
+                imp[0] += d0; imp[1] += d1; imp[2] += d2;
+                const Px = d0 * a0x + d1 * a1x + d2 * a2x;
+                const Py = d0 * a0y + d1 * a1y + d2 * a2y;
+                const Pz = d0 * a0z + d1 * a1z + d2 * a2z;
+                if (Px !== 0 || Py !== 0 || Pz !== 0) {
+                    apply_impulse(ss, baseA, invMA, rAx, rAy, rAz, Px, Py, Pz, +1);
+                    if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, Px, Py, Pz, -1);
+                }
+            } else {
+                for (let k = 0; k < 3; k++) {
+                    if (!lin_active[k]) continue;
+                    const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
+                    const rvx = (ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * rAz - ss[baseA + SBS_AV_Z] * rAy) - (to_world ? 0 : (ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rBz - ss[baseB + SBS_AV_Z] * rBy));
+                    const rvy = (ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rAx - ss[baseA + SBS_AV_X] * rAz) - (to_world ? 0 : (ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rBx - ss[baseB + SBS_AV_X] * rBz));
+                    const rvz = (ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * rAy - ss[baseA + SBS_AV_Y] * rAx) - (to_world ? 0 : (ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rBy - ss[baseB + SBS_AV_Y] * rBx));
+                    const vrel = rvx * ax + rvy * ay + rvz * az;
+                    const old = imp[k];
+                    // `+ γ·old` regularises a spring row (γ = 0 otherwise → exact).
+                    let nv = old - lin_eff[k] * (vrel + lin_bias[k] + lin_gamma[k] * old);
+                    if (nv < lin_clo[k]) nv = lin_clo[k]; else if (nv > lin_chi[k]) nv = lin_chi[k];
+                    imp[k] = nv;
+                    const d = nv - old;
+                    if (d !== 0) {
+                        apply_impulse(ss, baseA, invMA, rAx, rAy, rAz, d * ax, d * ay, d * az, +1);
+                        if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, d * ax, d * ay, d * az, -1);
+                    }
                 }
             }