diff --git a/CLAUDE.md b/CLAUDE.md index d8df4da..678d329 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -10,10 +10,10 @@ knowledge base — see § Quick links spec. | Field | Value | |---|---| | Phase | 1 (Etch ↔ ECS) | -| Current milestone | M1.1.1-HF4 — extension additive conflict: ratify reject (`E1797`) — code-complete, PR open (untagged hotfix on `main` = M1.1.2) | -| Last released tag | `v0.11.2-narrowphase-gjk` | -| Active branch | `phase-1/etch/hf4-extension-conflict-reject` (hotfix; PR open, not merged) | -| Next planned milestone | M1.1.3 — Forge 3D narrowphase: EPA + contact manifold (penetration depth, contact points, normal; seeded from the GJK terminal simplex) — fourth core sub-milestone of the M1.1 rigid arc (M1.1.0–15, `PhysicsModule` interface freeze at M1.1.15), per the Forme A execution order (module cores M1.1→M1.5, then M1.6 Asset Pipeline, then the M1.7 demo). M1.1.0 (foundations), M1.1.1 (broadphase dynamic multi-layer BVH), and M1.1.2 (narrowphase GJK convex detection) are CLOSED. The M1.0 core-language series (M1.0.0–M1.0.18, 19 sub-milestones) is CLOSED — C1.6 Etch-closure tag `v0.10.18-extension-additive-warning`. Reserved (Tier-1-dependent, NOT core gaps): `future` — the sole reserved await target; `override` — the last reserved top-level construct keyword (waits for a Tier-1 overridable module); `quantize` — reserved in `non_s3_keywords` (beat/bar musical clock, later Sequencer-adjacent milestone). | +| Current milestone | M1.1.3 — Forge 3D narrowphase: EPA + contact manifold — code-complete, PR #56 open | +| Last released tag | `v0.11.3-narrowphase-epa-manifold` | +| Active branch | `phase-1/forge/narrowphase-epa-manifold` (PR #56 open, not merged) | +| Next planned milestone | M1.1.4 — Forge 3D narrowphase fast paths (analytic sphere/sphere, sphere/box, capsule/capsule, box/box; each reproduces the generic GJK/EPA manifold, faster) — fifth core sub-milestone of the M1.1 rigid arc (M1.1.0–15, `PhysicsModule` freeze at M1.1.15). M1.1.0 (foundations), M1.1.1 (broadphase multi-layer BVH), M1.1.2 (narrowphase GJK), and M1.1.3 (narrowphase EPA + contact manifold; `narrowphase.zig` promoted to the `pipeline/narrowphase/` package) are CLOSED. The M1.0 core-language series (M1.0.0–M1.0.18) is CLOSED — C1.6 tag `v0.10.18-extension-additive-warning`. Reserved (Tier-1-dependent, NOT core gaps): `future`, `override`, `quantize`. | ## Tags @@ -59,6 +59,7 @@ knowledge base — see § Quick links spec. | `v0.11.0-forge-foundations` | 2026-07-13 | M1.1.0 — Forge 3D foundations: physics math + public API + BodyManager | First M1.1 (Forge 3D) sub-milestone — opens the largest Phase 1 milestone (~25 sub-milestones, C1.0/C1.1). `src/foundation/math/` born (first-consumer rule): generic `Vec(N,T)`/`Quat(T)`/`Mat3(T)`/`Aabb(T)`, `@Vector`-backed `Vec`, f32 aliases `Vec2`/`Vec3`/`Vec4`/`Quatf`/`Mat3f`/`Aabbf`, `@sizeOf(Vec3)==16` pin (= ECS `Transform` bulk-sync stride), zero `foundation/simd` import (sister-module rule). `src/modules/forge/api/`: extern ECS components (`RigidBody`/`CollisionShape`+`ShapeParams`/`PhysicsForces`; `Velocity` = re-export of `core.ecs.components.Velocity`, no duplicate) + descriptor/handle types mirroring `engine-tier-interfaces.md` §1 with four recorded deviations (tagged-union `ShapeDescriptor`; C1.1-complete 12-variant `ShapeType`; `linear_damping` default 0.05; `foundation.math`, not `core.math`) — spec patch pending (KB reconciliation at close). `forge_3d` (`forge_3d/`, imports only `foundation` + `weld_forge`): `Real` comptime scalar (`-Dphysics_f64` option, default f32; f64 test run green locally), shared `IdAllocator` (LIFO free-list, `index:24\|generation:8` generational handles, deterministic — no hash-map on the path), `ShapeStore` (sphere/box/capsule constructible, else `error.UnsupportedShape`; precomputed local AABB + unit-mass analytic inertia incl. capsule cylinder+hemispheres composite), `BodyManager` SoA (`std.MultiArrayList`), exact per-primitive world AABBs, `zig build test-forge-3d`. No stepping/broadphase/`ModuleContext`/interface instantiation (`PhysicsModule` freeze is M1.1.15). Notes-decision-7 amendment: rotation column is stride/order-compatible with `Transform.rot`, element align 4 — aligned SIMD columns, if ever needed, land via solver column allocation, never via the frozen `Quat` API. | | `v0.11.1-broadphase-bvh` | 2026-07-13 | M1.1.1 — Forge 3D broadphase: dynamic multi-layer BVH | Second M1.1 sub-milestone. **E0 directory flatten**: `solvers_3d/forge_3d/` → `forge_3d/` (7 pure `git mv` renames + 2 `build.zig` refs); the abandoned multi-solver wrapper is gone (`engine-directory-structure.md`/`engine-tier-interfaces.md` patched to the flat layout). `Aabb(T).surfaceArea()` added to `foundation/math` (SAH metric). `forge_3d/pipeline/broadphase.zig` (imports `foundation` math ONLY; scalar as comptime `T`, `user_data` an opaque `u32`): generic `Bvh(T)` — incremental dynamic AABB tree in the Box2D `b2DynamicTree` tradition (index-based `std.ArrayListUnmanaged(Node)` pool + LIFO free-list, no generation packing; fat AABBs from `BroadphaseConfig{margin=0.1}`; SAH best-cost-child descent; AVL-like rotation balancing; `validate()` invariant checker), `insert(gpa, aabb, ud) !u32` / `remove` / `update` (hysteresis, allocation-free) / `queryAabb(query, collector) u32` (visited-node counter). Multi-layer aggregate: `BroadphaseLayer` (static/dynamic/debris/trigger, `engine-physics-forge.md §1.2`), `default_layer_pairs` (4×4 symmetric `const` matrix; allowed = dyn×{dyn,static,debris,trigger} + debris×static), `Broadphase(T)` owning one `Bvh(T)` per layer + per-layer moved-log, `computePairs(gpa, out)` — moved-driven, self-excluded by `user_data`, sorted by packed `u64 (a<<32)|b` + adjacent-deduped, **zero hash containers** (determinism by construction, anticipates M1.1.14). M1.1.0 gap fixed: `Body.collision_layer: u8` stored by `BodyManager.addBody` + `collisionLayer(id) ?u8` getter (nothing consumes the layer yet — object-layer filtering is later). `forge_3d/root.zig` re-exports the pipeline at `Real`. Full suite green at f32 AND `-Dphysics_f64=true` (local). Deferred (not gaps): raycast/shapecast on the tree (M1.1.9–10), velocity-based AABB prediction (M1.1.5), object-layer `CollisionConfig` matrix wiring, trigger/debris `BodyDescriptor` assignment (M1.1.13). | | `v0.11.2-narrowphase-gjk` | 2026-07-18 | M1.1.2 — Forge 3D narrowphase: GJK convex intersection | Third M1.1 sub-milestone. `pipeline/narrowphase.zig` (imports `foundation` math ONLY; scalar comptime `T`, instantiated at `config.Real`): distance-based GJK on convex **cores** + inflation radius (Jolt convex-radius architecture). `SupportShape(T)` — `Core` union (point / segment:half_height / box:half_extents) + `radius` — with fixed-tie-break `support` (`dir.y == 0` → +Y endpoint; box component `== 0` → +half_extent). `RelativePose(T)` — frame-of-A precompute `rot_rel = conj(rot_a)·rot_b`, `pos_rel = conj(rot_a)·(pos_b − pos_a)` (conjugate, never `inverse()`) + `supportB`. `Simplex(T)` — triplet `Vertex (w, support_a, support_b)` + Ericson RTCD §5.1 Voronoi closest-origin solver over point/segment/triangle/tetrahedron (fixed region order; every division guarded → NaN-safe on duplicated/collinear/coplanar degeneracies). `gjk(T, shape_a, pose_a, shape_b, pose_b) GjkResult(T)` — bounded descent (`max_gjk_iterations = 32`, named relative progress tolerance, NEVER normalizes the search direction — squared distances, one `sqrt`), three regimes, classification hardened over six post-delivery correction cycles — NO classification decision depends on a geometric quantity (radius, shape size, `w`-vertex magnitude); every threshold is `k·floatEps(T)·coordScale` (coordScale = absolute support magnitude), absorbing only float noise: `separated` (`dist − r_sum` beyond `conv_k·floatEps·coordScale`, an ABSOLUTE additive margin, not `r_sum`-proportional — P1; `conv_k=16` bounds the pipeline's ACCUMULATED rounding so exact tangencies stay `.shallow` — P1b; `coord_scale = |Δpos| + coreExtent_a + coreExtent_b` is SYMMETRIC, so `.deep`/`.shallow`/`.separated` is invariant under an A/B swap — P1c) / `shallow` (else, cores disjoint; exact touch counts) both carry core distance + world closest points; `deep` (geometric ENCLOSURE — a non-degenerate tetra `count==4`, NO distance threshold on that path; plus a machine-epsilon numerical-NOISE floor `mach_eps=noise_k·floatEps` for degenerate coincident/collinear Minkowski configs scaled by the absolute support magnitude — P2, and an anti-cycling duplicate-support guard) carries the terminal simplex (EPA seed M1.1.3), no depth/normal. forge_3d integration: `shape.supportShape`, `BodyManager.rotation(id)` getter + `gjkPair(store,a,b)` (broadphase→narrowphase adapter, null on stale). Broadphase fat-AABB false positives classified `.separated`. `narrowphase.zig` 726 lines (brief-mandated single-file GJK stack, cf. `broadphase.zig` 686). Full suite green at f32 AND `-Dphysics_f64=true` (local). Out (later): EPA/manifold/depth/normal (M1.1.3), fast paths (M1.1.4). | +| `v0.11.3-narrowphase-epa-manifold` | 2026-07-19 | M1.1.3 — Forge 3D narrowphase: EPA + contact manifold | Fourth M1.1 sub-milestone. narrowphase.zig → `pipeline/narrowphase/` package (pure relocation). EPA (`epa.zig`) on the cores in A's frame from the `.deep` simplex → `EpaResult{normal world A→B, core depth, closest_a/b}`. Manifold (`manifold.zig`): `collide()`/`collideOrdered()` → one generator both regimes (shallow from GJK closest points, deep from EPA); face-face by A-frame supporting-face clipping, segment by clipSegment, edge/vertex/point-core by a single witness contact along the EPA axis; per-point `penetration = r_sum − s`. `feature_id`: five class-tagged producers in disjoint ranges, each encoding its real sub-feature — kept-vertex (face,vertex), edge×plane (side-plane,edge), reference corner (ref-vertex,incident-face), clipSegment (face,capsule-endpoint), point-core/single-contact (real vertex-or-face,real vertex-or-face) — unique per simultaneous contact (131,751-manifold sweep + face_face_min + capsule-pair sweeps) and frame-stable via `collidePair`'s BodyId order; bare `collide` is pose-canonical (no inter-frame guarantee). `ContactManifold`/`ContactPoint` FROZEN. Order-independence via `collidePair` BodyId order + normal negation (RD-4 exception: bit-identical coincident shapes). Integration: `BodyManager.collidePair`, `Real` re-exports. feature_id contract hardened across an extended Codex review cycle (FIX-1..12). Note: P1d extreme-aspect box `.deep` = GJK classification limit upstream of EPA → M1.1.4. Fast paths M1.1.4. | ### Hotfixes (untagged) @@ -115,6 +116,7 @@ Hotfix milestones are merged to `main` without a tag (Guy decision, - **M1.1.1 scope boundary (broadphase dynamic multi-layer BVH)**: `pipeline/broadphase.zig` imports `foundation` (math) ONLY — never `weld_forge`/`body*`/`config`; the scalar is the comptime parameter `T` (forge_3d instantiates at `config.Real` via `root.zig` re-exports), and `user_data` is an opaque `u32` (forge_3d passes the packed `BodyId`). Determinism by construction (anticipates M1.1.14): NO hash containers anywhere — index pool + LIFO free-list, tree shape a pure function of the op sequence, pairs sorted by packed `u64 (min<<32)|max` + adjacent-deduped; SAH/balance ties fixed. `insert` realizes the brief's schematic `insert(aabb, user_data) u32` as `insert(gpa, tight_aabb, user_data) !u32` (unmanaged-first convention `§3`, `BodyManager.addBody` precedent) — a realization, not a scope change (Guy-confirmed, no Recorded deviation). `computePairs` is **moved-driven** and consumes (clears) the per-layer moved-logs; a stale moved id (freed, not reused) is skipped via `isLiveLeaf`; `user_data` must be unique across all layers (documented on `Broadphase.insert`). The `default_layer_pairs` matrix is a `const`; an overridable `CollisionConfig` matrix is a LATER milestone (the field `Body.collision_layer` is stored now but nothing consumes it — object-layer pair filtering unwired). **OUT (not gaps, additive on the same structure):** raycast/shapecast tree traversal (M1.1.9–10), velocity-based AABB prediction / speculative expansion (M1.1.5), trigger/debris `BodyDescriptor` layer-assignment sources (M1.1.13 — only `body_type`-derived static/dynamic exercised now), `PhysicsWorld`/stepping/`ModuleContext`/`PhysicsModule` instantiation (M1.1.15), parallel/GPU broadphase (Phase 4+), narrowphase. **File-length note:** `broadphase.zig` is 686 lines (over the 500-line Review guideline) — the brief mandates `Bvh(T)` + `Broadphase(T)` + `BroadphaseLayer` + `BroadphaseConfig` + pair generation in this single file; conscious brief-driven overage, not lint-enforced. **E0 directory flatten** removed the `solvers_3d/` wrapper (`solvers_3d/forge_3d/` → `forge_3d/`); `engine-directory-structure.md` §9.1 and `engine-tier-interfaces.md` were pre-patched to the flat layout; the stale `solvers_{2d,3d}/` reference in `engine-zig-conventions.md §14` (KB, out of repo) remains for a KB reconciliation. - **M1.1.2 scope boundary (narrowphase GJK convex detection)**: `pipeline/narrowphase.zig` imports `foundation` (math) ONLY — never `weld_forge`/`body*`/`config`/`broadphase`; the scalar is the comptime `T` (forge_3d instantiates at `config.Real` via `root.zig` re-exports). GJK runs on the convex **cores** (point/segment/box) + an inflation radius (Jolt convex-radius architecture); touch = `dist(cores) ≤ r_a + r_b`, `separated` iff the core distance exceeds `r_a + r_b` beyond an ABSOLUTE noise-scaled contact margin `conv_k·floatEps(T)·coordScale` (exact touch → `shallow`, face-inclusive convention). Distance-based (NOT boolean) GJK: the simplex stores `(w, support_a, support_b)` triplets so closest points on A/B reconstruct from the barycentrics; the Voronoi simplex solver is Ericson (not Johnson, not signed-volumes). Determinism by construction (anticipates M1.1.14): no hash containers, no trig (dot/cross only), `max_gjk_iterations = 32`, named relative progress tolerance, fixed support tie-breaks; the search direction is NEVER normalized (squared distances, one `sqrt`); computation frozen in the frame of A (`rot_rel`/`pos_rel` precompute, conjugate-rotate never `inverse()`) — changing the frame after M1.1.14 would break bit-exactness. `GjkResult.deep` carries the terminal origin-enclosing simplex as the EPA seed — no depth/normal/manifold here. The forge_3d-side adapters (`shape.supportShape`, `BodyManager.gjkPair`) own the `Shape → SupportShape` conversion + the `BodyId`-level entry, mirroring the broadphase `user_data` discipline. **Classification robustness (six post-delivery correction cycles, Guy-directed — RESOLVES the E3 absolute-`deep_eps`, the Fix 3 vertex-relative residual, the P1/P2 remaining geometric dependencies, the P1b accumulated-rounding under-dimensioning, the P1c A/B-order asymmetry, AND the P1d anisotropic-tetra false-degeneracy; the extreme-aspect radius-0 box `.deep` residual is DOCUMENTED and deferred to M1.1.4/M1.1.3):** GUIDING PRINCIPLE — no `.deep`/`.shallow`/`.separated` decision depends on a GEOMETRIC quantity (radius, shape size, `w`-vertex magnitude); `.deep` = geometric enclosure; every remaining threshold is `k·floatEps(T)·coordScale` (coordScale = absolute support magnitude, `@sqrt(maxSupportMagSq)`), absorbing only float rounding noise. (a) `.deep` = a non-degenerate tetra enclosing the origin (`count==4`, degeneracy tested DIMENSIONLESSLY `|bary_det|² ≤ deg_rel²·(|b−a|²·|c−a|²·|d−a|²)` = squared normalized volume, aspect-ratio-invariant — P1d replaced the `maxEdgeSq³` normalization that rejected valid anisotropic tetra and read a sharp box's interior `.separated`), NO distance threshold on that path; (a2, Fix 3b→P2) the sole distance early-out (`degenerateOriginReached`) is a numerical-NOISE floor `closest·closest ≤ mach_eps²·maxSupportMagSq` (scaled by the ABSOLUTE support magnitude), with `mach_eps = noise_k·floatEps(T)` (P2 recalibrated from the 8×-eps `1e-6` literal — which was a visible geometric threshold, ~8.7e-5 at half-extent 50 — down to `noise_k`≈2 ULPs); the earlier vertex-relative `rel_deep²·maxVertexMag²` (Fix 3) reproduced the bug for large shapes and was removed; (b) an anti-cycling duplicate-support guard kills degenerate Minkowski tetrahedra at the source; (c, Fix 4→P1) `.separated iff dist − r_sum > conv_k·floatEps(T)·coordScale` — an ABSOLUTE additive margin (P1 removed the `r_sum·(1+contact_rel)` form, whose `contact_rel·r_sum` slack — 0.1 m at r_sum 1000 — was a collision margin beyond the core radius, out of scope; it mis-classified two r=500 spheres 5 cm apart as `.shallow`), honoring the frozen touch=shallow rule via the convergence-noise margin; (c2, P1b) `conv_k` = 16, not 2 — 2 ULP under-dimensioned the ACCUMULATED pipeline rounding (quaternion + Voronoi tetra + sqrt), so an exact tangency (Codex case: `dist − r_sum` = 7.15e-7 ≈ 2.1 ULP relative, zero convergence residue) tipped `.separated`; 16 ULP bounds it and stays at the noise level (the deep noise floor keeps `noise_k = 2`, a distinct point-noise floor — aligning them recreates a P1b/P2 false positive). Scale-robust, verified at O(1), half-extents 50–500, and at the f32 noise floor (~9 orders tighter in f64 — an f32 fundamental limit, not a geometric margin); a measure-zero EXACT tangency can still, in rare pathological configs (~1/20000 in sweep), tip `.separated` by one ULP — irreducible float limit, corrected next frame by penetration, no absolute guarantee claimed. (c3, P1c) the contact margin's `coord_scale` is SYMMETRIC by construction — `|pos_b − pos_a| + coreExtent(a) + coreExtent(b)` (`coreExtent` = the core's max local support magnitude, radius excluded: point→0, segment→half_height, box→`|half_extents|`) — replacing the A-frame terminal-support magnitude `@sqrt(maxSupportMagSq)`, which was frame-dependent and made one tangency classify `.separated` in one A/B order and `.shallow` in the other (a structural defect, not calibration). `.deep`/`.shallow`/`.separated` is therefore invariant under an A/B swap by construction, asserted by `gjk classification is order-independent`; `maxSupportMagSq` stays (anti-cycling guard + deep noise floor — `w`-magnitude, a distinct quantity). **File-length note:** `narrowphase.zig` is 726 lines (over the 500 Review guideline) — the brief mandates the whole GJK stack (support shapes + simplex solver + loop + result) in this single file (mirror of `broadphase.zig` at 686); conscious brief-driven overage. **OUT (not gaps; additive/later):** EPA + penetration depth + contact normal + manifold (M1.1.3 — in particular do NOT compute `depth = r_a+r_b−dist` or a normal for `.shallow`); pair-type fast paths ss/sb/cc/bb (M1.1.4); speculative-contact thresholds / margins beyond the core radius / box convex radius (box radius 0 here); `collision_layer`/`CollisionConfig` matrix wiring; stepping/`PhysicsWorld`/`PhysicsModule` instantiation (M1.1.15); raycast/shapecast/point queries (M1.1.9–10); shapes beyond sphere/box/capsule (`SupportShape.Core` stays additively extensible); batched/SIMD pair processing; `forge_2d` (M1.8); robust `.deep` for radius-0 box cores of EXTREME aspect ratio (>~50:1) — a GJK-f32 limit on sharp cores (tetra degeneracy at extreme anisotropy + premature anti-cycling termination before the enclosure tetra forms), reliable to ~30:1, deferred to M1.1.4 analytic box fast paths / M1.1.3 EPA (P1d). - **Extension-conflict semantics (last-wins vs reject) — RESOLVED (M1.1.1-HF4)**: the dedicated post-HF3 spec session ratified **reject** as the normative, permanent policy for additive extension conflicts, superseding the former "last-extension-wins" prose. The invariant is **{base components} ∪ {active extensions' components} conflict-free**; three rejected forms: (a) two extensions declare the same component; (b) an extension declares a component already carried by the base (or an earlier extension); (c) the same extension is listed twice. Static conflict → **fatal cook error** `E1797 ExtensionAdditiveConflict` (all three forms, distinct static message; no output; `weld check` fails in CI), guaranteeing `cooked ⇒ loadable`; dynamic activation → runtime `error.ExtensionComponentConflict` (a/b) / `error.ExtensionAlreadyActive` (c), atomic (entity unchanged). Specs corrected (`engine-scene-serialization.md` additive-conflict note §695–706, `etch-reference-part2.md §30.4–30.5`, `etch-diagnostics.md §18.3` E1797 row + the W1791→E1797 reclassification note); code comments cite `engine-scene-serialization.md` (the authority), not the HF3 brief. Deactivation needs no provenance — the reject invariant means no two active declarants ever share a component. Cook detection: form (c) is a resolver-independent lexical pass (so the public `cook` null-resolver path rejects duplicates); forms (a)/(b) need the prefab resolver and are `validate.structure`-guarded before any accessor getter. Alternative examined and rejected in the session: last-wins-with-provenance (masking + shadowed-byte side table + out-of-order deactivate + `.sav` shadow-stack serialization — ≈5–10× the code, touches the ECS reserve-then-mutate invariant and the serialization format, undoes the HF3 deactivate simplification — to support a design error). `@exclusive_with` remains a future, unimplemented escape hatch. +- **Scope boundary M1.1.3** — Deferred to M1.1.4: extreme-aspect (>~50:1) radius-0 box `.deep` classification (GJK-f32 limit upstream of EPA), and analytic fast paths superseding the generic clipper for common pairs. Deferred until the shapes ship (convex hull/compound/mesh, ~M1.1.19-20, co-designed with M1.1.6 warm-start): generic-clipper feature ids for non-fast-path convex shapes. In M1.1.3 the feature_id contract is fully met for box/sphere/capsule (all producers class-tagged, unique, frame-stable via collidePair). **Closed for M1.1.3 scope.** ## Non-negotiable rules diff --git a/briefs/M1.1.3-narrowphase-epa-manifold.md b/briefs/M1.1.3-narrowphase-epa-manifold.md new file mode 100644 index 0000000..b7f5c50 --- /dev/null +++ b/briefs/M1.1.3-narrowphase-epa-manifold.md @@ -0,0 +1,242 @@ +# M1.1.3 — Narrowphase: EPA + contact manifold + +> **Status:** PLANNED +> **Phase:** 1.1 +> **Branch:** `phase-1/forge/narrowphase-epa-manifold` +> **Planned tag:** `v0.11.3-narrowphase-epa-manifold` +> **Dependencies:** M1.1.2 (`v0.11.2-narrowphase-gjk`), M1.1.1-HF4 (untagged, on `main`) +> **Opened:** 2026-07-19 + +--- + +# FROZEN SECTION + +*Produced by Claude.ai. Not modifiable by Claude Code outside a Claude.ai round-trip (cf. § Recorded deviations).* + +## Context + +Fourth core sub-milestone of M1.1 (Forge 3D rigid branch, comptime core arc M1.1.0–15). Builds the penetration half of the narrowphase on top of the M1.1.2 distance-based GJK: EPA over the terminal `.deep` simplex (penetration axis + depth) and a **contact manifold** generator (points + normal + per-point depth) for both the `.shallow` and `.deep` regimes. The plan's acceptance is `pénétration → points + normale + depth corrects` (`engine-phase-1-plan.md`, M1.1.3). Algorithm scope is Jolt-inspired (EPA over the GJK terminal simplex; `ManifoldBetweenTwoFaces` supporting-face clipping — Ericson RTCD lineage), **not a clone**. Pair-type fast paths that reproduce this generic result faster are the NEXT sub-milestone (M1.1.4). + +This milestone also **promotes `pipeline/narrowphase.zig` to a package** `pipeline/narrowphase/` (Guy-approved): the GJK stack relocates verbatim, EPA and the manifold land as sibling files. Rationale: the single file is already 726 lines; EPA + clipping would push it to ~1350, and M1.1.4 grows it further. The relocation is done first (E1) and proven by the unchanged GJK suite before any new logic. `engine-directory-structure.md` is patched to the package layout at close. + +## Scope + +- **E1 — Package split + contact types + convention + `supportingFace`.** + 1. **Relocation (pure `git mv`, zero logic change).** `pipeline/narrowphase.zig` → the package `pipeline/narrowphase/`: + - `support.zig` — `SupportShape(T)` (relocated) + `RelativePose(T)` (relocated); `minkowskiSupport` promoted to a `pub` free function here (EPA and the manifold both need it); the shared geometry base. + - `gjk.zig` — `Simplex(T)`, `gjk`, `GjkResult(T)`, `max_gjk_iterations` and the GJK-internal helpers (relocated; imports `support.zig`). + - `root.zig` — package façade: re-exports everything the old file exposed (unchanged public names) so consumers change only their import path. `tests/narrowphase_test.zig` → `tests/gjk_test.zig` (`git mv`, imports updated). Consumers updated: `shape.zig`, `body_manager.zig`, `forge_3d/root.zig` (import `pipeline/narrowphase/root.zig`). The relocated GJK acceptance suite must pass **byte-identical in behavior** before E1 proceeds — the relocation-proof. + 2. **`supportingFace(dir) Face` on `SupportShape(T)`** (`support.zig`) — the feature (polygon) most parallel to `dir` (the feature maximizing `dir · p`, containing `support(dir)`), in the core's local frame, radius excluded. `Face = struct { verts: [4]Vec3, count: u8 }`: point core → 1 vertex; segment core → the segment (2) when `dir` is not aligned with ±Y, else the extremal endpoint (1); box core → the winning quad (4). Deterministic tie-breaks consistent with `support` (see Notes). A `supportingFaceB` on `RelativePose(T)` maps B's face into A's frame (mirror of `supportB`). + 3. **`ContactManifold(T)` / `ContactPoint(T)`** (`manifold.zig`) with the frozen normal / depth / feature-id convention (see Notes). No generator yet — types + convention only. + - Unit tests: `supportingFace` returns the correct feature and vertex count per core on cardinal and oblique directions, through a non-trivial relative rotation, byte-stable across runs; the manifold types round-trip. + +- **E2 — EPA (penetration axis + core depth).** `epa.zig` — `epa(T, shape_a, pos_a, rot_a, relpose, shape_b, seed: GjkResult(T)) EpaResult(T)`, run on the **cores** in A's frame (radius excluded, same Minkowski support as GJK). Seed = the `.deep` terminal simplex; **tetra-expand a `simplex_count < 4` seed** to a non-degenerate origin-enclosing tetrahedron first (the M1.1.2 flag-6 contract: low-dimensional Minkowski configs — coincident points, point-on-segment, crossing segments). Expanding-polytope loop: fixed-capacity face list, expand toward the closest face by adding `minkowskiSupport(closest-face-normal)`, silhouette re-triangulation, bounded iterations (named `max_epa_iterations`), every division guarded (never a NaN — degenerate expansion returns the best face found). `EpaResult(T) = { normal (world, A→B), depth (core penetration ≥ 0), closest_a, closest_b (world, reconstructed from the terminal face barycentrics like GJK) }`. Determinism by construction (no hash containers, fixed face-selection order, dot/cross only). Tests: penetration normal + depth within a named tolerance of the analytic value for sphere/sphere, sphere/box, box/box (axis-aligned and oriented), capsule/* deep overlaps; `epa(A,B)` and `epa(B,A)` give opposite normals, equal depth (order-independence); determinism (two runs bit-identical); a `simplex_count < 4` seed (e.g. crossing segments) expands and converges. + +- **E3 — Manifold generator (supporting-face clipping, both regimes).** `manifold.zig` — given a contact `normal` (world, A→B) and the two shapes/poses, generate the manifold by supporting-face clipping: `supportingFace(+n)` on A and `supportingFace(−n)` on B; reference face = the one whose face-normal is most parallel to `n`, incident = the other; clip the incident polygon against the reference face's side planes (Sutherland-Hodgman); keep the points on the penetrating side; **reduce to ≤ 4** (deepest + the three that maximize the contact area, deterministic tie-break); apply the inflation radii to land points on the surfaces and compute per-point `penetration`; assign a deterministic `feature_id` per point. The full feature matrix {point, segment, face}² must be correct (box/box face-face → up to 4; box/capsule, capsule/capsule edge cases → 1–2; anything against a sphere point → 1). Wire **both regimes** through this one generator: `.shallow` supplies `normal = normalize(closest_b − closest_a)` and no EPA; `.deep` supplies `normal` from E2's `EpaResult`. Tests: box/box face-face → 4-point manifold with correct per-point depth; box/box edge and box/capsule → 1–2; sphere/* → 1; capsule/capsule parallel overlap → 1–2; shallow sphere/sphere and sphere/capsule → 1; every case order-independent (swap A/B → normal negated, same points, same depths) and deterministic; per-point penetration continuous across the shallow↔deep boundary. + +- **E4 — forge_3d integration + close.** `pipeline/narrowphase/root.zig` — top-level `collide(T, shape_a, pos_a, rot_a, shape_b, pos_b, rot_b) ?ContactManifold(T)`: run `gjk`; `.separated` → null; `.shallow` → shallow manifold; `.deep` → EPA + deep manifold. `body_manager.zig` — `collidePair(store, a, b) ?ContactManifold(Real)` (mirror of `gjkPair`; null on any stale handle). `forge_3d/root.zig` re-exports the manifold surface at `Real` (`ContactManifold`, `ContactPoint`, `collide`, EPA symbols per naming conventions) + comptime test pins. Integration test: mixed scene through `BodyManager` + `Broadphase(Real)`, `computePairs` → unpack `user_data` as `BodyId` → `collidePair` per candidate; a broadphase fat-AABB false positive → null; a genuine overlap → a valid manifold. Spec patch: `engine-physics-forge.md` gains a narrowphase subsection (the three GJK regimes + `ContactManifold` + normal/depth convention + the mapping to `CollisionEnter.contact_point`); `engine-directory-structure.md` updated to the package layout. Full suite green at f32 and under `-Dphysics_f64=true` (local). `CLAUDE.md` updated per §3.4. + +## Out of scope + +- **P1d — robust `.deep` for radius-0 box cores of extreme aspect ratio (>~50:1) is NOT resolved here.** Correcting the M1.1.2 deferral note: P1d is a GJK **classification** limit *upstream* of EPA — a sharp box interior point can read `.separated`, in which case EPA is never entered. EPA only runs on a seed GJK already classified `.deep`; it cannot fix a false `.separated`. The genuine fix is the M1.1.4 analytic box/box + point/box fast path (which bypasses GJK). M1.1.3 owns only the *flag-6* input contract (tetra-expanding a low-dimensional `.deep` seed), not P1d. +- Pair-type fast paths (sphere/sphere, sphere/box, capsule/capsule, box/box) — M1.1.4. +- Warm-starting / contact caching / persistent manifolds across frames — the manifold is single-shot (see Notes); `feature_id` is produced now but consumed at M1.1.6 (Sequential Impulses warm-start). +- Collision margins / speculative contacts beyond the core radius; box convex radius (box radius stays 0). +- `collision_layer` consumption / `CollisionConfig` matrix wiring (standing debt, later milestone). +- Stepping, scheduler, `PhysicsWorld`, `PhysicsModule`/`ModuleContext` instantiation (freeze is M1.1.15). +- Raycast / shapecast / point queries (M1.1.9–10). +- Shapes beyond the three delivered primitives; `SupportShape.Core` and `supportingFace` must stay additively extensible (a new `Core` case), nothing more. +- Batched / SIMD pair processing (`engine-simd.md §7.3` targets are later). +- `forge_2d` (M1.8; 2D narrowphase is SAT + clipping, not GJK/EPA). + +## Specs to read first + +1. `engine-physics-forge.md` — §1 in full (pipeline, narrowphase GJK + EPA + fast paths; §1.5 V1 scope and phasing) and §3 (Collision System — `CollisionEnter`/`event.contact_point`, the gameplay surface the manifold ultimately feeds). +2. `engine-phase-1-plan.md` — M1.1 table (the exact M1.1.2 → M1.1.3 → M1.1.4 boundaries). +3. `engine-phase-1-criteria.md` — C1.1 (paths, isolation, determinism bar). +4. `engine-directory-structure.md` — forge section (`pipeline/narrowphase` placement; this milestone patches it to the package layout). +5. `engine-simd.md` — §7.3 (Forge 3D narrow-phase: SAT / GJK / contact-manifold clipping expectations — architecture awareness, no SIMD work here). +6. `engine-zig-conventions.md` — in full (naming, doc comments, §13 test pins / lazy-analysis guard for the new files). +7. `engine-coordinate-system.md` — axes, units, quaternion conventions. + +## Files to create or modify + +- `src/modules/forge/forge_3d/pipeline/narrowphase.zig` — **delete** (relocated; `git mv` its content into the package) +- `src/modules/forge/forge_3d/pipeline/narrowphase/support.zig` — create — `SupportShape(T)` (+ `supportingFace`), `RelativePose(T)` (+ `supportingFaceB`), `pub` `minkowskiSupport` (relocated + additions) +- `src/modules/forge/forge_3d/pipeline/narrowphase/gjk.zig` — create — `Simplex(T)`, `gjk`, `GjkResult(T)`, `max_gjk_iterations`, GJK helpers (relocated) +- `src/modules/forge/forge_3d/pipeline/narrowphase/epa.zig` — create — `EpaResult(T)`, `epa`, `max_epa_iterations` +- `src/modules/forge/forge_3d/pipeline/narrowphase/manifold.zig` — create — `ContactManifold(T)`, `ContactPoint(T)`, supporting-face clipping, shallow + deep generation +- `src/modules/forge/forge_3d/pipeline/narrowphase/root.zig` — create — package façade + `collide` entry + re-exports +- `src/modules/forge/forge_3d/shape.zig` — modify — import path only (behavior unchanged) +- `src/modules/forge/forge_3d/body_manager.zig` — modify — import path; `collidePair(store, a, b) ?ContactManifold(Real)` +- `src/modules/forge/forge_3d/root.zig` — modify — import path; manifold + EPA re-exports at `Real`; comptime test pins for the new files +- `src/modules/forge/forge_3d/tests/narrowphase_test.zig` → `tests/gjk_test.zig` — `git mv` + import path update +- `src/modules/forge/forge_3d/tests/epa_test.zig` — create — EPA acceptance suite +- `src/modules/forge/forge_3d/tests/manifold_test.zig` — create — manifold acceptance suite +- `briefs/M1.1.3-narrowphase-epa-manifold.md` — create — this brief +- `CLAUDE.md` — modify — §3.4 update (E4) +- Spec patches at close (produced by Claude.ai during the E4 review, applied by CC on-branch): `engine-physics-forge.md` (narrowphase subsection), `engine-directory-structure.md` (package layout). + +## Acceptance criteria + +### Tests + +- `tests/gjk_test.zig` — the entire relocated M1.1.2 GJK suite passes byte-identical in behavior after the split (relocation-proof; run before E1 adds anything). +- `tests/support` (in `gjk_test.zig` or a small `support` test) — `test "supporting face returns the correct feature per core"` — point → 1, segment → 1 or 2 by direction, box → the winning quad; cardinal + oblique + one non-trivial relative rotation; byte-stable. +- `tests/epa_test.zig` — `test "epa penetration axis and depth match analytic"` — sphere/sphere, sphere/box, box/box (axis-aligned + oriented), capsule overlaps; normal + depth within a named tolerance. +- `tests/epa_test.zig` — `test "epa is order-independent"` — `epa(A,B)` and `epa(B,A)` give opposite normals, equal depth. +- `tests/epa_test.zig` — `test "epa expands a low-dimensional deep seed"` — a `simplex_count < 4` `.deep` seed (crossing segments / point-on-segment) tetra-expands and converges within `max_epa_iterations`. +- `tests/epa_test.zig` — `test "epa is deterministic and iteration-bounded"` — two runs bit-identical; adversarial config terminates under the constant. +- `tests/manifold_test.zig` — `test "box-box face contact yields a multi-point manifold"` — face-face → up to 4 points, per-point depth correct, deterministic reduction. +- `tests/manifold_test.zig` — `test "manifold feature matrix"` — box/capsule + capsule/capsule → 1–2; sphere/* → 1; box/box edge → 1–2; each point on the surfaces, per-point depth correct. +- `tests/manifold_test.zig` — `test "manifold is order-independent"` — swap A/B → normal negated, same point set, same depths. +- `tests/manifold_test.zig` — `test "shallow and deep manifolds are continuous"` — a pair swept across the shallow↔deep boundary → per-point penetration continuous, no normal flip. +- `tests/manifold_test.zig` — `test "broadphase pairs to manifolds via collidePair"` — mixed scene through `BodyManager` + `Broadphase(Real)`; a fat-AABB false positive → null; a genuine overlap → a valid manifold; stale-handle pair → null. +- f64 instantiation: full suite green under `-Dphysics_f64=true` (local run; CI stays on the default matrix). + +### Benchmarks + +- None at this gate. Fast-path speedup comparisons are M1.1.4; C1.1 perf targets land with stepping (M1.1.5+). + +### Observable behavior + +- `zig build test` green (default) and green with `-Dphysics_f64=true` (local). +- The integration test demonstrably turns a genuine broadphase overlap into a valid manifold and a fat-AABB false positive into `null`. + +### CI + +- `zig build` clean, zero warnings, on the configured matrix +- `zig build test` green (debug + ReleaseSafe) +- `zig fmt --check` green +- `zig build lint` green +- `commit-msg` hook green on every commit of the branch + +## Conventions + +- **Branch:** `phase-1/forge/narrowphase-epa-manifold` +- **Final tag:** `v0.11.3-narrowphase-epa-manifold` (scheme `v0.(10+sub-phase).(sub-milestone)`, on the `v0.11.x` Forge 3D line) +- **PR title:** `Phase 1 / Forge / Narrowphase: EPA + contact manifold` +- **Commit convention:** Conventional Commits (cf. `engine-development-workflow.md §4.3`) +- **Merge strategy:** squash-and-merge (cf. `engine-development-workflow.md §4.6`) + +## Notes + +- **Dependency discipline (unchanged).** The whole `pipeline/narrowphase/` package imports `foundation` (math) ONLY — never `weld_forge`, never `body*.zig`, never `config.zig`, never `broadphase.zig`. The scalar arrives as the comptime `T`; forge_3d instantiates at `config.Real`. `Shape → SupportShape`, `collidePair`, and the `Real` re-exports live on the forge_3d side (`shape.zig`, `body_manager.zig`, `forge_3d/root.zig`). +- **Frame of A is frozen (M1.1.14).** EPA runs in A's frame, same as GJK; the contact normal and closest points are reconstructed in A's frame then mapped to world via `rot_a.rotateVec3(x).add(pos_a)` (the GJK closest-point pattern). Do not change the computation frame. +- **Cores + inflation (unchanged).** EPA and the clipper run on the cores (box radius 0), never a curved surface — no simplex/polytope degeneracy near contact. The inflation radii are applied only when placing contact points on the surfaces and computing per-point depth. +- **Normal / depth / point convention — FROZEN:** + ```zig + pub fn ContactManifold(comptime T: type) type { + return struct { + /// Unit, world-space, points from A to B — the axis along which to + /// translate B to reduce penetration. + normal: math.Vec(3, T), + /// Up to 4 contact points (box face-face max after reduction). + points: [4]ContactPoint(T), + /// Valid entries in `points`, 1..4. + count: u8, + }; + } + pub fn ContactPoint(comptime T: type) type { + return struct { + /// World-space point on the contact plane (midpoint of the two + /// surface points at this contact). + position: math.Vec(3, T), + /// Surface penetration along `normal`, >= 0 when overlapping. + penetration: T, + /// Deterministic, frame-stable per-contact identity for M1.1.6 + /// warm-starting (reference-feature id << 16 | incident-feature id). + /// Populated now; consumed at M1.1.6. Packing is an impl detail. + feature_id: u32, + }; + } + ``` + Order-independence is a hard requirement: `collide(A,B)` and `collide(B,A)` give negated normals, equal `count`, the same point set, and equal per-point `penetration`. **Exception (ratified, RD-4):** two shapes with BIT-IDENTICAL pose AND geometry are order-invariant only up to the measure-zero ambiguity of the A→B axis for coincident identical shapes — there is no geometric A→B direction, so `collide` returns the SAME (deterministic, arbitrary) normal in both call orders rather than a negated pair. This case is excluded from the `collide`-level order-independence guarantee; for real bodies `BodyManager.collidePair` imposes a canonical body-id order that restores full order-independence (distinct bodies always break the tie). Depth is continuous across the shallow↔deep boundary: shallow `= r_sum − dist`, deep `= core_depth + r_sum`, both → `r_sum` as `dist`/`core_depth` → 0. +- **Manifold strategy: single-shot supporting-face clipping (Jolt/Catto lineage), NOT persistent/incremental caching (Bullet).** Determinism (M1.1.14) forbids cross-frame accumulation state in the narrowphase; conforms to `engine-simd.md §7.3` (Contact manifold — geometric clipping — Jolt). Temporal coherence for warm-starting (M1.1.6) is carried by `feature_id` matching, not by a cached manifold here. +- **EPA algorithm.** Expanding-polytope over the Minkowski difference of the cores (Ericson RTCD / van den Bergen lineage): closest face → support in its normal direction → if the support does not advance beyond a named tolerance, converged; else insert, remove faces visible from the support, re-triangulate the silhouette. Fixed-capacity face buffer bounded by `max_epa_iterations`; guarded divisions; degenerate expansion returns the best face (never NaN). A `simplex_count < 4` seed is blown up to a tetrahedron first (deterministic blow-up directions from the seed's dimensionality). `minkowskiSupport` is reused from `support.zig`. +- **Manifold generation.** Reference/incident face selection by anti-parallelism to `normal`; Sutherland-Hodgman clip of incident against the reference side planes; keep penetrating points; reduce to ≤ 4 by keeping the deepest plus the three maximizing contact area (fixed tie-break for determinism). The generator must handle degenerate "polygons" (a 2-vertex segment feature, a 1-vertex point feature) — sphere → point, capsule → segment, box → quad. Shallow feeds a normal from the GJK closest points (guard the degenerate `dist ≈ 0` case, which sits at the shallow/deep boundary, by falling back to the search direction); deep feeds a normal from EPA. Same generator, two normal sources. +- **Split is a pure relocation.** E1's `git mv` must not change any GJK logic; the relocated suite proves it. Only after green does E1 add `supportingFace` and the manifold types. The M1.1.2 frozen invariants (frame-of-A, the six correction cycles, the classification thresholds) travel unchanged with the file. +- **Determinism by construction (anticipates M1.1.14).** No hash containers, no trigonometry (dot/cross only), bounded iterations (named `max_epa_iterations`), fixed face/point evaluation order, guarded divisions. `normalize` in `foundation/math` is unguarded (undefined on the zero vector) — guard every division site. +- **`CollisionEnter.contact_point` mapping.** The gameplay event surfaces a single representative point; the spec patch defines it as the manifold's deepest point (or the centroid — decide and document in the patch). The multi-point manifold stays internal to the solver. + +--- + +# LIVING SECTION + +*Maintained by Claude Code during the milestone. The log is not a marketing report: it serves review and post-mortem debugging.* + +## Specs read + +*Check before writing any production code. Confirms the spec was ingested in full, not merely skimmed.* + +- [x] `engine-physics-forge.md` (§1, §3) — 2026-07-19 +- [x] `engine-phase-1-plan.md` (M1.1) — 2026-07-19 +- [x] `engine-phase-1-criteria.md` (C1.1) — 2026-07-19 +- [x] `engine-directory-structure.md` (forge section) — 2026-07-19 +- [x] `engine-simd.md` (§7.3) — 2026-07-19 +- [x] `engine-zig-conventions.md` — 2026-07-19 +- [x] `engine-coordinate-system.md` — 2026-07-19 + +## Execution log + +*One entry per logical work sequence. Short, 1–3 lines. Commit regularly.* + +- **E1 recon + relocation-proof** (2026-07-19). `git describe main` = `v0.11.2-narrowphase-gjk-1-g921d455` ✓. Baseline forge_3d suite green (debug/ReleaseSafe/f64) before touching anything. Pure `git mv` `pipeline/narrowphase.zig` → package `pipeline/narrowphase/`: `support.zig` (SupportShape/RelativePose/Vertex + `minkowskiSupport` promoted `pub`), `gjk.zig` (Simplex/gjk/GjkResult + descent helpers), `root.zig` façade. `Simplex(T).Vertex` now aliases the shared `support.Vertex(T)` (identical type — no behavior change). Consumers repointed (`shape.zig`, `body_manager.zig`, `forge_3d/root.zig`); `tests/narrowphase_test.zig` → `tests/gjk_test.zig`. Relocated GJK suite passes byte-identical in behavior (45 tests, debug + f64). Commit `4d23ff9`. +- **E1 supportingFace + contact types** (2026-07-19). `Face(T)` + `SupportShape.supportingFace(dir)` + `RelativePose.supportingFaceB(dir)` in `support.zig`; `ContactManifold(T)`/`ContactPoint(T)` (FROZEN convention verbatim) in `manifold.zig`. Package `root.zig` + forge_3d re-exports extended. Two inline acceptance tests (supportingFace feature/count matrix; manifold types round-trip incl. f64). Suite 47/47 across debug/ReleaseSafe × f32/f64; lint/fmt/`zig build` clean. +- **E2 EPA (penetration axis + core depth)** (2026-07-19). `epa.zig` — an expanding-polytope loop over the Minkowski difference of the **cores** in A's frame (reuses `support.minkowskiSupport`), seeded off the `.deep` terminal simplex, mapped to world via `rot_a`/`pos_a` (mirror of `gjk.zig`). Fixed-capacity (allocation-free), determinism by construction (no hash, fixed face order, `max_epa_iterations = 32`, all divisions guarded). **Key correctness point:** faces are oriented outward from the polytope **centroid** (interior point), not from the origin — a GJK `.deep` seed can be a triangle coplanar with the origin (e.g. a point inside a box: terminal simplex is a triangle through the origin), and origin-side orientation stalls on a zero-distance base face whose only expansion support is the already-added apex (duplicate → premature break → depth 0). Centroid orientation makes the base normal point to the far side → EPA expands → converges. Genuinely < 3-D differences (crossing capsules, point-on-segment) → `degenerateResult` (unit separation normal + depth 0; the deep↔shallow boundary). Tests (`tests/epa_test.zig`, pinned in `forge_3d/root.zig`): analytic axis+depth (sphere/box, box/box axis-aligned + oriented, box/capsule), order-independence (opposite world normals, equal depth), low-dim seed expansion (crossing segments + point-on-segment converge to a sane touching result), eccentric thin-box deep overlap (many-iteration polytope, order-independent), determinism + iteration-bounded. +- **E2 adversarial review (read-only) + fix-as-you-go** (2026-07-19). An isolated read-only review of `epa.zig` (no wrong answer on any delivered/tested case) surfaced four robustness gaps, all fixed in-milestone: **(F1)** fan faces during expansion no longer reorient against the fixed `interior` centroid (whose sign test can flip for a face near-tangent to the interior in eccentric configs) — they inherit the horizon winding by construction (`makeWoundFace`), the standard EPA construction; `interior` now orients only the four initial tetra faces (a volumetric tetra where it is robust). **(F2)** a sliver fan face is bailed cleanly (keep the current best face) rather than silently dropped, which would leave a topological hole and corrupt `twinAmongVisible`. **(F3)** on the iteration-cap exit `best` is refreshed against the final grown polytope (was one expansion stale); guarded by `expanded_ok` so a bailed-expansion break keeps its valid pre-expansion face. **(F4)** the loop tolerances are recomputed from the expanded polytope's scale (the seed scale can be 0 for an origin-coincident low-dim seed). A new eccentric-thin-box test exercises F1's regime. Suite 52/52 across debug/ReleaseSafe × f32/f64; lint/fmt/`zig build` clean. **Deferred to E3 (within-milestone, not parked — F5):** the `degenerateResult` normal (depth-0 touching configs) is a deterministic unit normal but not sign-tied to A→B, so its order-independence is not guaranteed for the inherently-ambiguous measure-zero touching case; the manifold generator's hard order-independence requirement is enforced at E3 where the deep normal feeds the manifold. + - **Note (brief internal inconsistency, flagged — NOT unilaterally recorded as a deviation):** E1 scope point 2 describes `supportingFace(dir)` as "the feature most **anti-parallel** to `dir`", but (a) the same line's box example ("the winning quad"), (b) the E3 usage `supportingFace(+n)` on A / `supportingFace(−n)` on B, and (c) consistency with the existing `support(dir)` all require the feature that **maximizes `p · dir`** (normal most **parallel** to `dir`, containing `support(dir)`). Implemented the parallel reading — the only one under which the specified E3 clipping is correct. Raised at the E1 verdict; **confirmed by Guy (E1 GO), frozen text corrected — see Recorded deviations RD-1.** + +- **E3 Contact manifold generator** (2026-07-19). `manifold.zig` — `collide(T, shape_a, pos_a, rot_a, shape_b, pos_b, rot_b) ?ContactManifold(T)` + the supporting-face clipping generator, both regimes through one path. Runs in A's frame (Guy's E3 imperative): the world contact normal (shallow = `normalize(closest_b − closest_a)` guarded at the `dist ≈ 0` seam by the centre-to-centre fallback; deep = `EpaResult.normal`) is converted once to A-frame (`conj(rot_a)·n`), `supportingFace(+n_a)` on A / `supportingFace(−n_a)` on B clipped by Sutherland-Hodgman, points reduced to ≤ 4 (deepest + area-maximising), inflation applied, final normal + points mapped to world once. Reference = the face whose outward normal is most aligned with the contact axis (non-polygon feature → align 0; tie → A) — the RD-1-corrected formulation. Per-point penetration `r_sum − s` (continuous shallow↔deep). Point-core contact (sphere / end-on capsule) → 1 point from the closest points. **Order-independence (hard requirement + F5 resolution):** `collide` computes the pair in a caller-independent CANONICAL order (`poseAfter`: a strict total order on position → rotation → radius → core tag) and negates the normal for the swapped caller — so `collide(A,B)`/`collide(B,A)` give the same points/depths with negated normals even for the degenerate depth-0 configs (crossing capsules) whose axis is otherwise ambiguous; F5 (the E2-deferred degenerate-normal order-dependence) is thereby resolved at the manifold level as planned. Tests (`tests/manifold_test.zig`, pinned): box/box face → 4 points + per-point depth; feature matrix (sphere/* → 1, capsule/capsule + box/capsule → 1–2); order-independence (incl. the degenerate crossing-capsule case); shallow↔deep continuity (sphere sweep to coincidence). +- **E3 adversarial review (read-only) + fix-as-you-go** (2026-07-19). An isolated read-only review of the generator confirmed three real bugs the passing tests did not exercise; all fixed in-gate: **(A)** `poseAfter` was not a strict total order — a box's canonical key used `|half_extents|`, so two distinct boxes of equal magnitude (e.g. `(2,1,1)` vs `(1,2,1)`) tied and the canonical order (hence order-independence) became ill-defined; replaced with core-kind + lexicographic-extents comparison (found and fixed pre-emptively). **(F)** `reduceToFour`'s 4th point maximised distance from the `k0,k1,k2` triangle plane, but all contacts are coplanar → that distance is ≈ 0 for every point → the 4th latched onto index 0, duplicating a contact and dropping a real corner on rotated box/box (octagon→4); replaced with signed-area extremes about the `k0`–`k1` axis + coincident-point dedup. **(E)** running a 2-vertex capsule segment through the closed-polygon Sutherland-Hodgman double-emitted the crossing point; segment incidents now clip as an OPEN segment (Liang-Barsky). Two new tests exercise the F (rotated box/box → 4 distinct) and E (staggered capsules → 2 distinct) regimes, plus an equal-`|he|` box combo for A. Review-verified sound: canonical-order point-set equality, buffer ping-pong, side-plane orientation, penetration/position formulas, `faceNormalA`/align. Suite 58/58 across debug/ReleaseSafe × f32/f64; lint/fmt/`zig build` clean. + +- **E4 forge_3d integration** (2026-07-19). `body_manager.zig` — `collidePair(store, a, b) ?ContactManifold(Real)`, the `BodyId`-level manifold adapter (exact mirror of `gjkPair`: resolves both bodies' world poses + support shapes, calls `narrowphase.collide`, null on a stale handle or a separated pair). `forge_3d/root.zig` — `Real`-bound re-exports: `EpaResult`, `max_epa_iterations`, `ContactManifold`, `ContactPoint`, `epa` (wrapper), `collide` (wrapper) — mirroring the `gjk` entry. Integration test (`tests/manifold_test.zig`): a mixed scene through `BodyManager` + `Broadphase(Real)`, `computePairs` → unpack `user_data` as `BodyId` → `collidePair` per candidate; a broadphase fat-AABB false positive → null, a genuine overlap → a valid manifold (per-point depth checked), a stale-handle pair → null. Suite 59/59 across debug/ReleaseSafe × f32/f64; lint/fmt/`zig build` clean. §3.6.1(a) language pre-check (added `src` lines) clean. + +- **E4-close external review (Codex) fixes** (2026-07-19). A pre-closure external review, verified against source, found 4 real defects (2 P1, 2 P2); all fixed in M1.1.3 with trigger tests. **P1a (edge-edge depth):** the manifold measured penetration along the reference **face** normal `rn`, correct in face-face (`rn ≈ n_a`) but wrong in edge-edge (the contact axis `n_a` is an edge-cross no box quad aligns with) — depth diverged from the EPA value. Now dispatched on `|rn · n_a|`: a genuine face-face (`≥ 0.999`) keeps the quad clip; an edge/vertex contact routes to the witness-point single contact whose depth is along `n_a` (EPA/GJK-closest). **P1b (frame-stable feature_id):** the clip-buffer index was used as the id — a ±1e-4 pose change reorders it (warm-start would break). `Face` now carries stable local ids (box vertex = sign-pattern 0..7, box face = `axis·2+sign`); the clipper threads `(ref-feature << 16 | incident-feature)` per point — kept incident vertex → `(ref_face_id, inc_vertex_id)`, edge×plane intersection → `(ref_side_plane_id, inc_edge_id)` — surviving `reduceToFour`. **P2a (order-independence, identical geometry):** `poseAfter` cannot order bit-identical shape+pose; `collidePair` now drives the pipeline in a canonical **body-id** order (`min` first) and negates for the swapped caller (total, body-id-stable, warm-start-aligned), and `poseAfter`'s degenerate exception is documented. **P2b (reduction in A's frame):** the `reduceToFour` candidates were stored/dedup'd in WORLD space, losing precision far from the origin (tiny features round together → collapse to 1); they are now kept in A's frame, reduced there, and only the chosen ≤ 4 points map to world at the end. Four trigger tests: edge-edge depth ≈ EPA depth; feature_id stable under a ±1e-4 shift; `collidePair` body-id order on identical bodies; a 4-point face manifold survives at world coord 1e5 in f32. Suite 63/63 across debug/ReleaseSafe × f32/f64; lint/fmt/`zig build` clean. + +- **E4-close second external review (Codex) fixes** (2026-07-19). A second pre-closure review, verified against source, found 3 real defects (2 P1, 1 P2) in the `feature_id` / order-independence contract; all fixed in M1.1.3 with trigger tests. **P1a (unique incident-edge id):** `intersectionFid` keyed the incident edge by `@min(cur, nxt)` — not unique (a box vertex lies on 3 edges → distinct edges alias; repro: two unit boxes, B at (−0.9,1.9,−0.9) yaw 0.05 → two contacts both `0x800e8002`). Now the edge is the SORTED VERTEX PAIR `lo·8 + hi` (unique among the 12 box edges), and a point already an intersection carries its ORIGINAL incident-edge provenance through the multi-plane Sutherland-Hodgman passes rather than a recomputed key. Two distinct simultaneous contacts never share a `feature_id`. **P1b (body-id order must reach the generator):** `collidePairOrdered` called `collide`, which re-canonicalizes by POSE, so the `feature_id` reference/incident ownership followed the pose and flipped across a lexicographic boundary (repro yaw 45°, x −1e-4 → +1e-4 flips all four ids). `collideOrdered` (the fixed-order generator, no pose canonicalization) is now `pub` and re-exported (package `root.zig` + `forge_3d/root.zig` at `Real`); `collidePairOrdered` calls it in the received body-id order, so the `feature_id` is body-id-keyed and frame-stable; `collidePair`'s normal negation still gives order-independence. Public `collide` keeps its pose canonicalization (callers without a body id) unchanged. **P2 (ratify the collide exception):** the FROZEN order-independence note is amended (RD-4) to ratify the coincident-identical-shape exception; the `collide`/`poseAfter` doc-comments now reference a ratified exception, not a unilateral note. Trigger tests: the P1a repro → four distinct `feature_id`s; a `collidePair` across the pose-order boundary → unchanged `feature_id`s. Suite 65/65 across debug/ReleaseSafe × f32/f64; lint/fmt/`zig build` clean. + +- **E4-close third external review (Codex) fix + missing verification** (2026-07-19). **FIX-9 (reference-corner aliasing):** `incidentEdgeId` blindly inherited a neighbour's incident-edge id (`if (a_inc & 0x8000) return a_inc`) for a clip point sitting on TWO reference side planes — but such a point is a REFERENCE feature (a corner/vertex of the reference face inside the incident face), not an incident edge, so distinct contacts aliased (Codex: 4792 cases, `0x800b8025` shared). Re-encoded the `feature_id` as a class-tagged (reference_feature, incident_feature) pair — reference class ∈ {face, side-plane edge, corner/vertex}, incident class ∈ {vertex, edge, face} — with the three contact kinds in disjoint id ranges (`class_a`/`class_edge`/`class_c` in the top two bits). The clipper now detects a reference cut-edge (both endpoints on a COMMON reference plane, via `commonRefPlane` over the plane sets a point lies on) and emits a REFERENCE CORNER `(ref_vertex(P_prev, P_cur), inc_face)` instead of inheriting an incident edge; a genuine incident-edge crossing stays `(ref_edge, inc_edge)`; a kept vertex stays `(ref_face, inc_vertex)`. Two distinct simultaneous contacts can no longer share a `feature_id`. **TEST POSE-SWEEP:** the verification a single point-test could not give — a grid of deep box/box contacts (yaw × a light second-axis tilt × lateral offsets × two Y overlaps, plus the two Codex repros) asserting (a) pairwise-distinct `feature_id`s and (b) frame-stability of the id SET under a ±1e-4 offset via `collidePair`'s body-id order; (b) is gated on a stable contact normal, since a tied min-penetration axis legitimately flips the whole contact under a 1e-4 nudge (a real geometric transition, not an id instability). **DOC (Codex P2.2/P2.3):** the manifold header + public `collide` doc now carry the ratified RD-4 exception and state that a FRAME-STABLE `feature_id` is guaranteed via `collidePair` (body-id order), not via bare `collide` (whose pose-canonicalization flips reference/incident ownership at a pose boundary). Suite 67/67 across debug/ReleaseSafe × f32/f64; lint/fmt/`zig build` clean. +- **E4-close fourth external review (Codex) fix** (2026-07-19). **FIX-10 (reference corner incident-face encoding):** the FIX-9 reference-corner id encoded the incident half as a CONSTANT `class_c` with no `inc.face_id`, so two corners at the same reference vertex but on DIFFERENT incident faces (a supporting-face flip inter-frame) shared a `feature_id` (Codex: `0x82328000` for incident faces 5 and 0) — a within-frame non-issue (the sweep of 131 751 manifolds had zero same-frame duplicates), but a warm-start (M1.1.6) inter-frame mis-match. `inc.face_id` is now threaded from `clipIncident` → `clipPolygon`/`clipSegment` → `clipAgainstPlane` → `intersectionFid`, and a reference corner emits `(ref_vertex(P_prev, P_cur), class_c | inc_face_id)` — its incident half carries which incident face, so a face flip changes the id. Verification: a flip test (same staggered overlap, B upright vs rolled 90° about X ⇒ the same reference vertex but a different incident face ⇒ different corner `feature_id`), plus a pose-sweep assertion that every reference corner's low half is `class_c` + a real face id (0..5). Suite 68/68 across debug/ReleaseSafe × f32/f64; lint/fmt/`zig build` clean. +- **E4-close fifth external review (Codex) fix** (2026-07-19). **FIX-11 (single-contact fallback class-tagging):** the three `pointCoreContact` paths (point core, edge/vertex fallback, degenerate empty clip) emitted their `feature_id` through the untagged `packFeatureId`, landing in the SAME `class_a` range as a clip kept vertex — so a single-contact fallback id could equal a clip id for the same body pair across the `face_face_min` face-face↔fallback transition (Codex: `0x00050004` = `(ref face 5, inc vertex 4)` clipped AND `(ref face 5, inc face 4)` fallback, 0.001 rad apart). A single witness contact's incident feature is the incident FACE, so it now emits `(class_a | ref_face_id, class_c | inc_face_id)` via a new `singleContactFid`; the `(class_a ref, class_c inc)` class pair is used by NO clip id (kept vertex `(a,a)`, edge crossing `(edge,edge)`, reference corner `(c,c)`), so a fallback id can never alias a clip id for a body pair. `packFeatureId` (the last untagged packer) is removed, so the trap cannot re-arm. Verification: a threshold test that scans a tilt range to find, at the build's precision (`face_face_min` = 0.999 f32 / 0.9999 f64), both a clipped and a fallback box/box at one base pose and asserts the fallback id equals none of the clip ids; plus a pose-sweep assertion that no multi-point manifold carries the `(class_a, class_c)` fallback class pair. Suite 69/69 across debug/ReleaseSafe × f32/f64; lint/fmt/`zig build` clean. +- **E4-close sixth external review (Codex) fix** (2026-07-20). **FIX-12 (point-core sub-feature in the single-contact id):** FIX-11's `singleContactFid` encoded each half as a bare `face_id`, but a point/segment core has NO polygonal face — its `supportingFace` returns a 1-vertex `Face` whose `face_id` is a per-core CONSTANT (segment `6`, point `7`), so both endpoints of one capsule (the +Y and −Y end caps) and its side contact collapsed to the SAME `feature_id` — a warm-start (M1.1.6) would fuse the two ends of a capsule resting on a face. `singleContactFid` now takes the two `Face`s and, per side, encodes the real sub-feature: a 1-vertex face (`count == 1`) contributes its `vert_ids[0]` (segment endpoint `0`=+Y / `1`=−Y, point `0`), a polygon contributes its `face_id` — keeping the `(class_a, class_c)` class pair (still disjoint from every clip class pair). Threaded through all three single-contact sites (`pointCoreContact` for a point/segment core, the edge/vertex fallback, the degenerate empty-clip fallback). Verification: a capsule-endpoint test (a small sphere touching a capsule's +Y cap, −Y cap, and side → three distinct ids, all class `(a, c)`) + a pose-sweep over capsule/sphere and capsule/box pairs asserting pairwise-distinct, well-classed single-contact ids. Suite 71/71 across debug/ReleaseSafe × f32/f64; lint/fmt/`zig build` clean. + +## Recorded deviations + +*Any departure from the FROZEN SECTION, with justification and the Claude.ai round-trip that authorized it.* + +- **RD-3 — spec patches are KB-only, not applied on-branch (workflow clarification).** The E4 §Scope and § Files-to-modify say the `engine-physics-forge.md` (narrowphase subsection) and `engine-directory-structure.md` (package layout) patches are "applied by CC on-branch". That contradicts `engine-development-workflow.md §3.5`: the spec lives ONLY in the Claude.ai KB (there is no `spec/` directory in the repo). Corrected at Guy's E4 direction: CC does NOT edit any spec file; Guy produces the complete re-uploadable spec files at review and re-uploads them to the KB. Not a design change — a workflow clarification of where the spec artifacts live. + +- **RD-4 — `collide` order-independence exception for coincident identical shapes (ratified).** The FROZEN order-independence note (ContactManifold convention, § Notes) is amended to add an exception: two shapes with bit-identical pose AND geometry have no geometric A→B axis (measure-zero), so `collide` returns the same deterministic-but-arbitrary normal in both call orders rather than a negated pair — this case is excluded from the `collide`-level guarantee, and `BodyManager.collidePair` restores full order-independence for real bodies via a canonical body-id order. Authorized by Guy at the second E4-close review (Claude.ai round-trip); the `collide`/`poseAfter` code doc-comments now cite this ratified exception rather than a unilateral note. No behavior change (the code already implemented it since the first review's FIX-3) — this ratifies the contract. + +- **RD-1 — `supportingFace` / reference-face direction wording (drafting-error correction).** The FROZEN SECTION said the supporting feature is "most **anti-parallel** to `dir`" (E1 §Scope point 2) and the reference face is "most **anti-parallel** to `n`" (E3 §Scope). Both are drafting errors: the feature that a supporting function returns maximizes `dir · p` (normal most **parallel** to the query direction, containing `support(dir)`), and this is what the E3 clipping's own `supportingFace(+n)`/`supportingFace(−n)` usage, the box "winning quad" example, and consistency with `support(dir)` all require. Corrected both spots to "most **parallel**". Authorized by Guy at the E1 GO (Claude.ai round-trip), classified a drafting-error correction — same class as the M1.1.2 post-delivery frozen-note corrections. The E1 code already implemented the parallel reading; no code change. The Notes "Manifold generation" line's "by anti-parallelism to `normal`" is left as-is (it describes the mutual anti-parallelism of the two contact faces, not the reference-selection direction). + +- **RD-2 — `epa` signature carries `pos_a`/`rot_a` in addition to the frozen `relpose`.** The FROZEN E2 §Scope signature is `epa(T, shape_a, relpose, shape_b, seed) EpaResult(T)`, but the same section (and the E2 kickoff) requires `EpaResult` to carry **world-space** `normal` and `closest_a`/`closest_b` "reconstructed in A's frame then mapped to world via `rot_a.rotateVec3(x).add(pos_a)`" (Notes, "Frame of A is frozen"). That mapping is not expressible without `pos_a`/`rot_a` inside `epa`, and doing it at the caller (collide, E4) would make `EpaResult` A-frame — contradicting the frozen "world" annotation and breaking the epa-level order-independence test (world normals must be directly comparable). The implemented signature is `epa(T, shape_a, pos_a, rot_a, relpose, shape_b, seed) EpaResult(T)` — the minimal addition that honors the frozen world-space `EpaResult` (mirrors `gjk.zig`'s own `rot_a`/`pos_a`-based closest-point mapping). Authorized by Guy's E2 kickoff, which specifies `EpaResult.closest` as "monde … mappé via rot_a" and points at `gjk.zig` l.≈578–605 as the pattern. `relpose` is still passed in (reused from the GJK call, not recomputed); `pos_b`/`rot_b` are NOT needed (B enters through `relpose`). The manifold's `collide` entry (E4) will build `relpose` and pass `pos_a`/`rot_a` through. **Ratified at the E2 GO** (Guy): the frozen E2 §Scope signature is corrected to `epa(T, shape_a, pos_a, rot_a, relpose, shape_b, seed) EpaResult(T)`. + +## Blockers encountered + +*Design questions that stopped work (cf. `engine-development-workflow.md`, Questions à Guy — Cas 2). One entry per blocker.* + +## Closing notes + +*Filled at close: what worked, what deviated, what to flag in review, residual risks / intentional tech debt.* + +**Delivered.** The narrowphase back-half of M1.1: EPA penetration (axis + core depth, `epa.zig`, seeded off the GJK `.deep` terminal simplex), the supporting-face clipping contact-manifold generator (`manifold.zig` — both shallow/deep regimes through one path, entirely in A's frame), forge_3d integration (`BodyManager.collidePair`/`collidePairOrdered` + `Real`-bound re-exports), and the E1 package split (`pipeline/narrowphase.zig` → `pipeline/narrowphase/{support,gjk,epa,manifold,root}.zig`). Suite 71/71 at f32 AND `-Dphysics_f64=true`, debug + ReleaseSafe; `zig build lint` / `zig fmt --check` / `zig build` clean. + +**What worked.** The frame-of-A discipline (the M1.1.14 bit-exactness precursor) held end-to-end — GJK, EPA, and the clipper all compute in A's frame and map to world exactly once, so nothing reintroduces the large-coordinate cancellation the frame choice avoids. The adversarial-review cadence (two in-milestone read-only reviews at E2/E3 + six external Codex rounds pre-close) drove the `feature_id` contract from a fragile clip-buffer index to a robust class-tagged scheme without ever loosening a passing test. + +**`feature_id` — the hardened contract (FIX-1..12).** The per-contact identity was iterated across the extended external-review cycle to its final form: **five class-tagged producers** occupying **disjoint class-pair ranges** (16-bit halves, top-2-bit class), each half encoding its REAL sub-feature (box vertex sign-pattern 0..7, box face `axis·2+sign` 0..5, segment endpoint 0=+Y / 1=−Y): + 1. kept incident vertex → `(class_a ref-face, class_a inc-vertex)`; + 2. edge×plane crossing → `(class_edge ref-side-plane, class_edge inc-edge)`; + 3. reference corner → `(class_c ref-vertex, class_c inc-face)`; + 4. point/segment-core witness, and 5. the edge-vertex / degenerate-empty-clip single-contact fallback → `(class_a ref-feature, class_c inc-feature)` (a class pair used by NO multi-point clip producer, so a single-contact id can never alias a clip id for a body pair). +Uniqueness proven by the pose-sweep (131 751 deep box/box manifolds, zero same-frame duplicates) + a `face_face_min` threshold test exercising the clip↔fallback transition at the build's actual precision (f32 0.999 / f64 0.9999) + capsule-endpoint / capsule-pair tests distinguishing the two end caps from the side contact. FRAME-STABLE only through `BodyManager.collidePair` (fixed body-id order); the bare `collide` is pose-canonical and flips reference/incident ownership at a pose boundary (documented on the type, the entry, and RD-4). + +**Deviations (see Recorded deviations).** RD-1 (`supportingFace` "parallel" wording — drafting-error correction), RD-2 (`epa` signature carries `pos_a`/`rot_a` for the frozen world-space `EpaResult`), RD-3 (spec patches are KB-only, not on-branch), RD-4 (`collide` order-independence exception for bit-identical pose+geometry — ratified). + +**Spec synced.** `engine-physics-forge.md` (narrowphase subsection) + `engine-directory-structure.md` (package layout) re-uploaded to the Claude.ai KB by Guy at close (RD-3 — there is no `spec/` in the repo). + +**Residual risks / intentional scope-out (not debt).** The EPA `degenerateResult` normal for an exact depth-0 touch is deterministic but not A→B-sign-tied at the `collide` level (resolved at the manifold via the canonical order); the extreme-aspect (>~30:1) radius-0 box `.deep` GJK-f32 limit inherited from M1.1.2 stands (deferred to M1.1.4 analytic box fast paths / M1.1.3-adjacent EPA hardening); pair-type fast paths, speculative-contact margins, `CollisionConfig` object-layer wiring, and `PhysicsModule`/stepping instantiation are later sub-milestones (M1.1.4+ / M1.1.15). + +Status: CLOSED +Closed: 2026-07-19 diff --git a/src/modules/forge/forge_3d/body_manager.zig b/src/modules/forge/forge_3d/body_manager.zig index 38c47f8..cb4a50d 100644 --- a/src/modules/forge/forge_3d/body_manager.zig +++ b/src/modules/forge/forge_3d/body_manager.zig @@ -18,7 +18,7 @@ const api = @import("weld_forge"); const config = @import("config.zig"); const shape_mod = @import("shape.zig"); const body_mod = @import("body.zig"); -const narrowphase = @import("pipeline/narrowphase.zig"); +const narrowphase = @import("pipeline/narrowphase/root.zig"); const IdAllocator = @import("slot_alloc.zig").IdAllocator; const Real = config.Real; @@ -33,6 +33,7 @@ const Shape = shape_mod.Shape; const Body = body_mod.Body; const MotionProperties = body_mod.MotionProperties; const GjkResult = narrowphase.GjkResult(Real); +const ContactManifold = narrowphase.ContactManifold(Real); const ApiVec3 = @import("foundation").math.Vec3; const ApiQuat = @import("foundation").math.Quatf; @@ -149,6 +150,50 @@ pub const BodyManager = struct { self.bodies.items(.rotation)[ib], ); } + + /// Full narrowphase (GJK → shallow/deep contact manifold) for the pair + /// `a`/`b`, resolving each body's world pose and support shape (via `store`). + /// Returns null if the pair is separated, or if either handle — or its shape + /// — is stale/invalid. The `BodyId`-level manifold adapter for the + /// broadphase→narrowphase flow (mirror of `gjkPair`): unpack a `computePairs` + /// candidate's `user_data` as a `BodyId` and call this per pair. + /// + /// The pipeline is driven in a canonical BODY-ID order (`min(a, b)` first), + /// negating the normal for the `a > b` caller. This makes the whole + /// narrowphase order-independent even for the measure-zero case `collide`'s + /// pose key cannot break — two bodies with bit-identical shape AND pose — and + /// gives a stable, body-id-keyed order for M1.1.6 warm-starting. + pub fn collidePair(self: *const BodyManager, store: *const ShapeStore, a: BodyId, b: BodyId) ?ContactManifold { + if (a > b) { + var m = self.collidePairOrdered(store, b, a) orelse return null; + m.normal = m.normal.neg(); // caller wants a→b = −(b→a) + return m; + } + return self.collidePairOrdered(store, a, b); + } + + /// `collidePair` for a fixed (already-canonical body-id) order — validates both + /// handles/shapes then runs the manifold pipeline in THIS order. Calls + /// `collideOrdered` (not `collide`): `collide` would re-canonicalize by pose, + /// so the `feature_id` reference/incident ownership would follow the pose and + /// flip across a lexicographic pose boundary (Codex P1b). Driving by the fixed + /// body-id order instead keeps the feature_id frame-stable; `collidePair`'s + /// normal negation still gives order-independence. + fn collidePairOrdered(self: *const BodyManager, store: *const ShapeStore, a: BodyId, b: BodyId) ?ContactManifold { + const ia = self.alloc.validate(a) orelse return null; + const ib = self.alloc.validate(b) orelse return null; + const shape_a = store.get(self.bodies.items(.shape)[ia]) orelse return null; + const shape_b = store.get(self.bodies.items(.shape)[ib]) orelse return null; + return narrowphase.collideOrdered( + Real, + shape_mod.supportShape(shape_a), + self.bodies.items(.position)[ia], + self.bodies.items(.rotation)[ia], + shape_mod.supportShape(shape_b), + self.bodies.items(.position)[ib], + self.bodies.items(.rotation)[ib], + ); + } }; /// Exact world-space AABB of a shape at pose (`pos`, `rot`). diff --git a/src/modules/forge/forge_3d/pipeline/narrowphase/epa.zig b/src/modules/forge/forge_3d/pipeline/narrowphase/epa.zig new file mode 100644 index 0000000..814b7ff --- /dev/null +++ b/src/modules/forge/forge_3d/pipeline/narrowphase/epa.zig @@ -0,0 +1,547 @@ +//! `forge_3d/pipeline/narrowphase/epa.zig` — EPA (Expanding Polytope Algorithm): +//! the penetration axis + core depth of a `.deep` pair. +//! +//! Runs on the convex **cores** in A's frame (radius excluded, same Minkowski +//! support as GJK — reuses `support.minkowskiSupport`), seeded off the GJK +//! terminal `.deep` simplex. The polytope is a fixed-capacity face list expanded +//! toward the origin-closest face until a support in that face's normal no longer +//! advances the surface; the closest face then gives the penetration normal, the +//! core depth, and (via the terminal face barycentrics, exactly like GJK) the +//! world closest points. Depth/normal here are on the CORES; the inflation radii +//! and per-point depth are applied downstream by the manifold generator. +//! +//! **Frame of A + world mapping (brief Notes).** EPA computes in A's frame, then +//! maps the normal and closest points to world via `rot_a` / `pos_a` (the GJK +//! closest-point pattern, `gjk.zig`). The signature therefore carries `pos_a` / +//! `rot_a` in addition to the frozen `relpose` (see the brief RD-2): the frozen +//! `EpaResult` fields are documented world-space "mapped via rot_a", which is not +//! expressible without them. +//! +//! **Low-dimensional seeds (brief flag-6 contract).** A `simplex_count < 4` seed +//! (coincident cores, point-on-segment, crossing segments) is tetra-expanded to a +//! non-degenerate origin-enclosing tetrahedron before the loop. When the +//! Minkowski difference is genuinely < 3-D (the cores touch along a point / line / +//! plane — a zero core penetration), no tetra exists; EPA returns the best +//! lower-dimensional feature: a unit separation normal and depth 0 (the deep↔ +//! shallow boundary; the manifold's deep depth is then `0 + r_sum`). +//! +//! **Dependency discipline (brief Notes).** Imports `foundation` (math) + the +//! sibling `support.zig` / `gjk.zig` ONLY. Determinism by construction: no hash +//! containers, no trigonometry (dot/cross only), fixed face-evaluation order, +//! bounded iterations (`max_epa_iterations`), every division guarded (never a NaN; +//! `normalize` in `foundation/math` is unguarded — all divisions here are guarded). + +const std = @import("std"); +const math = @import("foundation").math; +const support = @import("support.zig"); +const gjk_mod = @import("gjk.zig"); + +/// Named iteration ceiling for the EPA expansion (brief Notes, anticipates the +/// M1.1.14 determinism freeze): the loop always terminates within this many +/// support queries. A well-formed penetration converges in a handful; the bound +/// backstops adversarial near-tangent configurations. +pub const max_epa_iterations: u32 = 32; + +/// EPA outcome — the penetration on the **cores**, world space: +/// - `normal`: unit, world, A→B (the axis to translate B along to separate). +/// - `depth`: core penetration ≥ 0 (origin-to-closest-face distance on the +/// Minkowski difference of the cores; the inflation `r_sum` is added by the +/// manifold generator, not here). +/// - `closest_a` / `closest_b`: the closest points on A's / B's cores, world +/// space, reconstructed from the terminal face barycentrics (as GJK does). +pub fn EpaResult(comptime T: type) type { + return struct { + normal: math.Vec(3, T), + depth: T, + closest_a: math.Vec(3, T), + closest_b: math.Vec(3, T), + }; +} + +/// One polytope face over the Minkowski difference of the cores: three vertex +/// indices (CCW as seen from outside), the outward unit normal (away from the +/// enclosed origin), and the origin-to-plane distance (`normal · vertex`, ≥ 0). +fn Face(comptime T: type) type { + return struct { + a: u32, + b: u32, + c: u32, + normal: math.Vec(3, T), + dist: T, + }; +} + +// Fixed capacities (allocation-free, determinism by construction). A closed +// triangulated convex polytope has F = 2V − 4 faces; each iteration adds one +// vertex, so V ≤ 4 + iterations and F ≤ 2V − 4. The buffers are sized generously +// above those bounds to absorb transient counts during re-triangulation. +const max_verts: usize = 4 + max_epa_iterations; // 36 +const max_faces: usize = 4 * max_epa_iterations; // 128 (> 2·36 − 4 = 68) +const max_silhouette: usize = 4 * max_epa_iterations; // 128 + +/// EPA on the cores of `shape_a`/`shape_b` at their world poses, seeded from the +/// GJK terminal `.deep` simplex. Computes in A's frame (B via `relpose`), maps +/// the result to world via `rot_a`/`pos_a`. See the file header for the frame, +/// the low-dimensional-seed contract, and determinism. +pub fn epa( + comptime T: type, + shape_a: support.SupportShape(T), + pos_a: math.Vec(3, T), + rot_a: math.Quat(T), + relpose: support.RelativePose(T), + shape_b: support.SupportShape(T), + seed: gjk_mod.GjkResult(T), +) EpaResult(T) { + const Vec3T = math.Vec(3, T); + const VertexT = support.Vertex(T); + const FaceT = Face(T); + + // --- Seed vertices from the terminal simplex --- + var verts: [max_verts]VertexT = undefined; + var vcount: usize = seed.simplex_count; + if (vcount == 0) { + // Not a deep seed (defensive: `epa` is only called on `.deep`). Return a + // zero-penetration result along a fixed axis rather than read undefined. + return degenerateFixedAxis(T, shape_a, rot_a, pos_a, relpose, shape_b); + } + for (0..vcount) |i| verts[i] = seed.simplex[i]; + + // Squared tolerance for the seed tetra-expansion (distinct / off-plane + // vertex tests) — relative to the seed's coordinate scale (the absolute + // support magnitude). The loop tolerances are recomputed post-expansion. + const rel: T = if (T == f32) 1.0e-4 else 1.0e-10; + const seed_tol = rel * polytopeScale(T, verts[0..vcount]); + const tol_sq = seed_tol * seed_tol; + + // --- Expand a low-dimensional seed to a non-degenerate tetrahedron --- + if (!expandToTetra(T, shape_a, relpose, shape_b, &verts, &vcount, tol_sq)) { + return degenerateResult(T, shape_a, rot_a, pos_a, relpose, shape_b, verts[0..vcount]); + } + + // Interior reference point (the tetra centroid), strictly inside the + // non-degenerate seed tetra — used to orient the four INITIAL faces outward + // (see `makeFace`). Fan faces inherit their winding from the horizon instead, + // so the interior is not consulted during expansion. + var interior = Vec3T.zero; + for (0..vcount) |i| interior = interior.add(verts[i].w); + interior = interior.scale(1.0 / @as(T, @floatFromInt(vcount))); + + // Recompute the tolerances from the expanded polytope's coordinate scale + // (the seed's scale can be 0 for an origin-coincident low-dim seed; the + // expanded tetra always has real extent). + const loop_scale = polytopeScale(T, verts[0..vcount]); + const loop_tol = rel * loop_scale; + const loop_tol_sq = loop_tol * loop_tol; + + // --- Build the initial tetrahedron faces --- + var faces: [max_faces]FaceT = undefined; + var fcount: usize = 0; + const tetra = [4][3]u32{ .{ 0, 1, 2 }, .{ 0, 1, 3 }, .{ 0, 2, 3 }, .{ 1, 2, 3 } }; + for (tetra) |t| { + if (makeFace(T, &verts, t[0], t[1], t[2], interior)) |f| { + faces[fcount] = f; + fcount += 1; + } + } + if (fcount < 4) { + // A degenerate seed tetra (collinear/coplanar 4th vertex slipped through). + return degenerateResult(T, shape_a, rot_a, pos_a, relpose, shape_b, verts[0..vcount]); + } + + // --- Expanding-polytope loop --- + // `best` is set to the current closest face at the top of every iteration, so + // on any `break` it holds a valid face of the last complete polytope (a face + // copy — safe even if a bailed expansion left `faces` partially modified, + // since vertices are only ever appended). `expanded_ok` tracks the + // iteration-cap exit, where the final successful expansion left `best` one + // step stale and it must be refreshed against the (valid) grown polytope. + var best = faces[closestFaceIndex(T, faces[0..fcount])]; + var expanded_ok = false; + var iter: u32 = 0; + while (iter < max_epa_iterations) : (iter += 1) { + expanded_ok = false; + best = faces[closestFaceIndex(T, faces[0..fcount])]; + + const w = support.minkowskiSupport(T, shape_a, relpose, shape_b, best.normal); + const d = w.w.dot(best.normal); + // Converged: the support in the closest face's normal does not advance + // past its plane beyond float noise ⇒ the face is on the surface. + if (d - best.dist <= loop_tol) break; + // Anti-cycling: the support duplicates an existing polytope vertex. + if (duplicate(T, verts[0..vcount], w.w, loop_tol_sq)) break; + if (vcount >= max_verts) break; + + // Re-triangulate: remove faces visible from `w`, add `w`, connect the + // silhouette. If the geometry degenerates, keep the current best face. + if (!expandPolytope(T, &verts, &vcount, &faces, &fcount, w)) break; + expanded_ok = true; + } + if (expanded_ok) best = faces[closestFaceIndex(T, faces[0..fcount])]; + + // --- Reconstruct closest points from the terminal face barycentrics --- + const va = verts[best.a]; + const vb = verts[best.b]; + const vc = verts[best.c]; + const tri = gjk_mod.Simplex(T).closestOriginTriangle(va.w, vb.w, vc.w); + const face_verts = [3]VertexT{ va, vb, vc }; + var ca = Vec3T.zero; + var cb = Vec3T.zero; + for (0..tri.count) |i| { + const fv = face_verts[tri.indices[i]]; + ca = ca.add(fv.support_a.scale(tri.bary[i])); + cb = cb.add(fv.support_b.scale(tri.bary[i])); + } + + return .{ + .normal = rot_a.rotateVec3(best.normal), + .depth = @max(best.dist, 0), + .closest_a = rot_a.rotateVec3(ca).add(pos_a), + .closest_b = rot_a.rotateVec3(cb).add(pos_a), + }; +} + +// --- Internal helpers --- + +/// Largest absolute support magnitude across the seed — the polytope's coordinate +/// scale for the relative tolerances (the scale of the `w = sa − sb` rounding). +fn polytopeScale(comptime T: type, verts: []const support.Vertex(T)) T { + var m: T = 0; + for (verts) |v| { + m = @max(m, v.support_a.dot(v.support_a)); + m = @max(m, v.support_b.dot(v.support_b)); + } + return @sqrt(m); +} + +/// Build a polytope face from three vertices, wound CCW as seen from outside +/// (outward normal away from the polytope `interior` reference point). Returns +/// null for a sliver (near-zero area) triangle. Orienting away from an interior +/// point — the polytope centroid — rather than away from the origin is what keeps +/// EPA correct when the origin is coplanar with a face (a low-dimensional GJK deep +/// seed, e.g. a point inside a box whose terminal simplex is a triangle through +/// the origin): the ambiguous origin-side test would pick the wrong outward +/// direction and stall on a zero-distance face. The winding is fixed so all faces +/// agree, which the silhouette extraction relies on. +fn makeFace(comptime T: type, verts: *const [max_verts]support.Vertex(T), ia: u32, ib0: u32, ic0: u32, interior: math.Vec(3, T)) ?Face(T) { + const va = verts[ia].w; + var ib = ib0; + var ic = ic0; + var vb = verts[ib].w; + var vc = verts[ic].w; + var n = vb.sub(va).cross(vc.sub(va)); + // Orient outward (away from the interior point); reverse winding to match. + if (n.dot(va.sub(interior)) < 0) { + const tmp = ib; + ib = ic; + ic = tmp; + vb = verts[ib].w; + vc = verts[ic].w; + n = vb.sub(va).cross(vc.sub(va)); + } + const len_sq = n.dot(n); + if (!(len_sq > 0)) return null; // sliver / degenerate triangle + const inv = 1.0 / @sqrt(len_sq); + const normal = n.scale(inv); + // Signed origin-to-plane distance (≥ 0 when the origin is on the interior + // side, 0 when coplanar); the final depth clamps to ≥ 0. + return .{ .a = ia, .b = ib, .c = ic, .normal = normal, .dist = normal.dot(va) }; +} + +/// Build a fan face (a silhouette edge `ia→ib` closed to the new vertex `ic`), +/// PRESERVING the inherited horizon winding rather than reorienting against the +/// interior point. The horizon edges are collected from the (consistently +/// outward-wound) visible faces, so `(ia, ib, ic)` is already outward-CCW and the +/// resulting fan is winding-consistent with the kept neighbours by construction — +/// no interior sign test, which can flip for a face near-tangent to the interior +/// (adversarial-review finding 1). Returns null for a sliver (near-collinear) +/// triangle, which the caller treats as a bail-out (keeping the current best +/// face) rather than leaving a topological hole (finding 2). +fn makeWoundFace(comptime T: type, verts: *const [max_verts]support.Vertex(T), ia: u32, ib: u32, ic: u32) ?Face(T) { + const va = verts[ia].w; + const n = verts[ib].w.sub(va).cross(verts[ic].w.sub(va)); + const len_sq = n.dot(n); + if (!(len_sq > 0)) return null; // sliver / degenerate triangle + const normal = n.scale(1.0 / @sqrt(len_sq)); + return .{ .a = ia, .b = ib, .c = ic, .normal = normal, .dist = normal.dot(va) }; +} + +/// Index of the face closest to the origin (minimum `dist`; first wins on ties — +/// deterministic). +fn closestFaceIndex(comptime T: type, faces: []const Face(T)) usize { + var best: usize = 0; + var best_d: T = faces[0].dist; + for (faces, 0..) |f, i| { + if (f.dist < best_d) { + best_d = f.dist; + best = i; + } + } + return best; +} + +/// Whether `w` duplicates an existing polytope vertex within `tol_sq` (anti- +/// cycling). Squared distance, no sqrt. +fn duplicate(comptime T: type, verts: []const support.Vertex(T), w: math.Vec(3, T), tol_sq: T) bool { + for (verts) |v| { + const dsq = w.sub(v.w).dot(w.sub(v.w)); + if (dsq <= tol_sq) return true; + } + return false; +} + +/// One EPA expansion step: append `w`, delete every face visible from it, and +/// re-triangulate the silhouette (each horizon edge → a new face to `w`). Returns +/// false if the step degenerates (no visible face, no silhouette, or every new +/// face is a sliver) — the caller then keeps the current best face. +fn expandPolytope( + comptime T: type, + verts: *[max_verts]support.Vertex(T), + vcount: *usize, + faces: *[max_faces]Face(T), + fcount: *usize, + w: support.Vertex(T), +) bool { + const FaceT = Face(T); + var visible: [max_faces]bool = undefined; + var any_visible = false; + for (0..fcount.*) |fi| { + const f = faces[fi]; + // Visible iff `w` is strictly beyond this face's plane. + visible[fi] = w.w.sub(verts[f.a].w).dot(f.normal) > 0; + if (visible[fi]) any_visible = true; + } + if (!any_visible) return false; + + // Collect the silhouette: a directed edge (i,j) of a visible face whose twin + // (j,i) belongs to no other visible face (the horizon between removed and + // kept faces). Consistent winding makes twins reversed. + var sil: [max_silhouette][2]u32 = undefined; + var scount: usize = 0; + for (0..fcount.*) |fi| { + if (!visible[fi]) continue; + const f = faces[fi]; + const edges = [3][2]u32{ .{ f.a, f.b }, .{ f.b, f.c }, .{ f.c, f.a } }; + for (edges) |e| { + if (!twinAmongVisible(FaceT, faces[0..fcount.*], visible[0..fcount.*], fi, e[1], e[0])) { + if (scount >= max_silhouette) return false; + sil[scount] = e; + scount += 1; + } + } + } + if (scount == 0) return false; + + // Compact out the visible faces. + var nf: usize = 0; + for (0..fcount.*) |fi| { + if (!visible[fi]) { + faces[nf] = faces[fi]; + nf += 1; + } + } + fcount.* = nf; + + // Append the new vertex. + const wi: u32 = @intCast(vcount.*); + verts[vcount.*] = w; + vcount.* += 1; + + // Fan the silhouette to the new vertex, preserving inherited winding. A + // sliver fan face would leave a topological hole (breaking the manifold + // invariant `twinAmongVisible` relies on), so bail out cleanly instead. + for (sil[0..scount]) |e| { + const f = makeWoundFace(T, verts, e[0], e[1], wi) orelse return false; + if (fcount.* >= max_faces) return false; + faces[fcount.*] = f; + fcount.* += 1; + } + return fcount.* >= 4; +} + +/// Whether directed edge (x,y) belongs to any visible face other than `skip`. +fn twinAmongVisible(comptime FaceT: type, faces: []const FaceT, visible: []const bool, skip: usize, x: u32, y: u32) bool { + for (faces, 0..) |f, fi| { + if (fi == skip or !visible[fi]) continue; + if ((f.a == x and f.b == y) or (f.b == x and f.c == y) or (f.c == x and f.a == y)) return true; + } + return false; +} + +/// Progressively add supports to reach a non-degenerate origin-enclosing +/// tetrahedron from a 1..4-vertex seed. Returns false when the Minkowski +/// difference is genuinely < 3-D (no tetra exists) — the caller then returns a +/// degenerate (zero-penetration) result. Blow-up directions are deterministic. +fn expandToTetra( + comptime T: type, + shape_a: support.SupportShape(T), + relpose: support.RelativePose(T), + shape_b: support.SupportShape(T), + verts: *[max_verts]support.Vertex(T), + vcount: *usize, + tol_sq: T, +) bool { + const Vec3T = math.Vec(3, T); + + // 1 → 2: a distinct second vertex (a segment). + if (vcount.* == 1) { + const dirs = [_]Vec3T{ Vec3T.unit_x, Vec3T.unit_x.neg(), Vec3T.unit_y, Vec3T.unit_y.neg(), Vec3T.unit_z, Vec3T.unit_z.neg() }; + for (dirs) |dir| { + const w = support.minkowskiSupport(T, shape_a, relpose, shape_b, dir); + if (w.w.sub(verts[0].w).dot(w.w.sub(verts[0].w)) > tol_sq) { + verts[1] = w; + vcount.* = 2; + break; + } + } + if (vcount.* == 1) return false; // point-like difference + } + + // 2 → 3: a third vertex off the segment line (a triangle). + if (vcount.* == 2) { + const ab = verts[1].w.sub(verts[0].w); + // Six candidate directions perpendicular to the segment (a base + // perpendicular and its rotations about the segment), deterministic. + const p0 = perpAxis(T, ab); + const p1 = ab.cross(p0); // second perpendicular (right-handed with p0, ab) + const cands = [_]Vec3T{ p0, p0.neg(), p1, p1.neg(), p0.add(p1), p0.sub(p1) }; + for (cands) |dir| { + const w = support.minkowskiSupport(T, shape_a, relpose, shape_b, dir); + if (distToLineSq(T, w.w, verts[0].w, verts[1].w) > tol_sq) { + verts[2] = w; + vcount.* = 3; + break; + } + } + if (vcount.* == 2) return false; // segment-like (1-D) difference + } + + // 3 → 4: a fourth vertex off the triangle plane (a tetra). + if (vcount.* == 3) { + var n = verts[1].w.sub(verts[0].w).cross(verts[2].w.sub(verts[0].w)); + const nl = n.dot(n); + if (!(nl > 0)) return false; // degenerate seed triangle + n = n.scale(1.0 / @sqrt(nl)); + inline for (.{ n, n.neg() }) |dir| { + if (vcount.* == 3) { + const w = support.minkowskiSupport(T, shape_a, relpose, shape_b, dir); + const off = w.w.sub(verts[0].w).dot(n); // signed plane distance + if (off * off > tol_sq) { + verts[3] = w; + vcount.* = 4; + } + } + } + if (vcount.* == 3) return false; // planar (2-D) difference + } + + return vcount.* == 4; +} + +/// A unit vector perpendicular to `v` (deterministic: cross with the least-aligned +/// basis axis). Falls back to `+X` for a near-zero `v`. +fn perpAxis(comptime T: type, v: math.Vec(3, T)) math.Vec(3, T) { + const Vec3T = math.Vec(3, T); + const a = v.toArray(); + const ax = @abs(a[0]); + const ay = @abs(a[1]); + const az = @abs(a[2]); + const axis = if (ax <= ay and ax <= az) Vec3T.unit_x else if (ay <= az) Vec3T.unit_y else Vec3T.unit_z; + const p = v.cross(axis); + const l2 = p.dot(p); + if (!(l2 > 0)) return Vec3T.unit_x; + return p.scale(1.0 / @sqrt(l2)); +} + +/// Squared distance from point `p` to the line through `a`,`b`. +fn distToLineSq(comptime T: type, p: math.Vec(3, T), a: math.Vec(3, T), b: math.Vec(3, T)) T { + const ab = b.sub(a); + const ap = p.sub(a); + const denom = ab.dot(ab); + if (!(denom > 0)) return ap.dot(ap); // a == b + const t = ap.dot(ab) / denom; + const closest = a.add(ab.scale(t)); + return p.sub(closest).dot(p.sub(closest)); +} + +/// Degenerate result for a < 3-D Minkowski difference (the cores touch along a +/// point / line / plane — zero core penetration). Returns a deterministic unit +/// separation normal and depth 0, with the closest points on the touching +/// feature (all mapped to world). This is the deep↔shallow boundary; the +/// manifold's deep depth is `0 + r_sum`. +fn degenerateResult( + comptime T: type, + shape_a: support.SupportShape(T), + rot_a: math.Quat(T), + pos_a: math.Vec(3, T), + relpose: support.RelativePose(T), + shape_b: support.SupportShape(T), + verts: []const support.Vertex(T), +) EpaResult(T) { + const Vec3T = math.Vec(3, T); + const S = gjk_mod.Simplex(T); + _ = shape_a; + _ = relpose; + _ = shape_b; + + switch (verts.len) { + 1 => { + // Point-like (coincident cores): fixed axis, depth 0. + return worldResult(T, rot_a, pos_a, Vec3T.unit_x, 0, verts[0].support_a, verts[0].support_b); + }, + 2 => { + // Segment-like: normal ⟂ the segment; closest on the segment. + const seg = S.closestOriginSegment(verts[0].w, verts[1].w); + const n = perpAxis(T, verts[1].w.sub(verts[0].w)); + var ca = Vec3T.zero; + var cb = Vec3T.zero; + for (0..seg.count) |i| { + ca = ca.add(verts[seg.indices[i]].support_a.scale(seg.bary[i])); + cb = cb.add(verts[seg.indices[i]].support_b.scale(seg.bary[i])); + } + return worldResult(T, rot_a, pos_a, n, @max(@sqrt(seg.closest.dot(seg.closest)), 0), ca, cb); + }, + else => { + // Planar (≥ 3 verts): normal = the plane normal; closest on the triangle. + const tri = S.closestOriginTriangle(verts[0].w, verts[1].w, verts[2].w); + var n = verts[1].w.sub(verts[0].w).cross(verts[2].w.sub(verts[0].w)); + const nl = n.dot(n); + n = if (nl > 0) n.scale(1.0 / @sqrt(nl)) else Vec3T.unit_x; + const face_verts = [3]support.Vertex(T){ verts[0], verts[1], verts[2] }; + var ca = Vec3T.zero; + var cb = Vec3T.zero; + for (0..tri.count) |i| { + ca = ca.add(face_verts[tri.indices[i]].support_a.scale(tri.bary[i])); + cb = cb.add(face_verts[tri.indices[i]].support_b.scale(tri.bary[i])); + } + return worldResult(T, rot_a, pos_a, n, @max(@sqrt(tri.closest.dot(tri.closest)), 0), ca, cb); + }, + } +} + +/// Fully-degenerate fallback (a defensive path for a non-deep seed): fixed axis, +/// depth 0, coincident closest points at A's origin. +fn degenerateFixedAxis( + comptime T: type, + shape_a: support.SupportShape(T), + rot_a: math.Quat(T), + pos_a: math.Vec(3, T), + relpose: support.RelativePose(T), + shape_b: support.SupportShape(T), +) EpaResult(T) { + _ = shape_a; + _ = relpose; + _ = shape_b; + const Vec3T = math.Vec(3, T); + return worldResult(T, rot_a, pos_a, Vec3T.unit_x, 0, Vec3T.zero, Vec3T.zero); +} + +/// Assemble an `EpaResult` from an A-frame normal + depth + A-frame closest +/// points, mapping normal and points to world via `rot_a`/`pos_a`. +fn worldResult(comptime T: type, rot_a: math.Quat(T), pos_a: math.Vec(3, T), normal_a: math.Vec(3, T), depth: T, ca: math.Vec(3, T), cb: math.Vec(3, T)) EpaResult(T) { + return .{ + .normal = rot_a.rotateVec3(normal_a), + .depth = depth, + .closest_a = rot_a.rotateVec3(ca).add(pos_a), + .closest_b = rot_a.rotateVec3(cb).add(pos_a), + }; +} diff --git a/src/modules/forge/forge_3d/pipeline/narrowphase.zig b/src/modules/forge/forge_3d/pipeline/narrowphase/gjk.zig similarity index 79% rename from src/modules/forge/forge_3d/pipeline/narrowphase.zig rename to src/modules/forge/forge_3d/pipeline/narrowphase/gjk.zig index 38295b3..cfec418 100644 --- a/src/modules/forge/forge_3d/pipeline/narrowphase.zig +++ b/src/modules/forge/forge_3d/pipeline/narrowphase/gjk.zig @@ -1,134 +1,39 @@ -//! `forge_3d/pipeline/narrowphase.zig` — distance-based GJK convex detection. +//! `forge_3d/pipeline/narrowphase/gjk.zig` — distance-based GJK convex detection. //! -//! Delivered gate by gate across M1.1.2: **E1–E3 (present)** — E1 the support -//! shapes + support functions + relative-pose precompute; E2 the Voronoi-region -//! simplex solver (`Simplex(T)`); E3 the bounded GJK descent loop (`gjk`) and -//! its three-regime `GjkResult(T)`. E4 adds the forge_3d integration -//! (`Shape → SupportShape`, `gjkPair`). EPA, penetration depth, contact normal, -//! and the fast paths are later sub-milestones (M1.1.3/4). +//! Delivered across M1.1.2: the Voronoi-region simplex solver (`Simplex(T)`), +//! the bounded GJK descent loop (`gjk`), and its three-regime `GjkResult(T)`. +//! The support shapes, relative pose, and `minkowskiSupport` this file consumes +//! live in the sibling `support.zig`; EPA (`epa.zig`) seeds off the `.deep` +//! terminal simplex `GjkResult` carries (M1.1.3). //! //! **Dependency discipline (brief Notes).** This file imports `foundation` -//! (math) ONLY — never `weld_forge`, never `body*.zig`, never `config.zig`, -//! never `broadphase.zig`. The scalar arrives as the comptime parameter `T`; -//! `forge_3d` instantiates it at `config.Real` (E4). The `Shape → SupportShape` -//! conversion and the `BodyId`-level `gjkPair` adapter live on the forge_3d -//! side, mirroring the broadphase `user_data` discipline. +//! (math) and the sibling `support.zig` ONLY — never `weld_forge`, never +//! `body*.zig`, never `config.zig`, never `broadphase.zig`. The scalar arrives +//! as the comptime parameter `T`; `forge_3d` instantiates it at `config.Real`. +//! The `Shape → SupportShape` conversion and the `BodyId`-level `gjkPair` adapter +//! live on the forge_3d side, mirroring the broadphase `user_data` discipline. //! //! **Cores + inflation radius (Jolt convex-radius architecture).** GJK never //! sees a curved surface: a sphere is a point, a capsule a segment, a box the -//! full box (radius 0). Support functions therefore run on the convex **core**; -//! the inflation `radius` is applied only in the touch test (`dist(cores) <= -//! r_a + r_b`, E3). Fast convergence, no simplex degeneracy near contact. +//! full box (radius 0). Support functions therefore run on the convex **core** +//! (`support.zig`); the inflation `radius` is applied only in the touch test +//! (`dist(cores) <= r_a + r_b`). Fast convergence, no simplex degeneracy near +//! contact. //! //! **Computation in the frame of A (brief Notes).** B is pre-transformed -//! relative to A once per pair (`RelativePose`); A's support runs untransformed. -//! Better precision far from the world origin (avoids large-coordinate -//! cancellation) and half the per-iteration transforms. Frozen now: changing -//! the computation frame after the M1.1.14 determinism freeze would break -//! validated bit-exactness. +//! relative to A once per pair (`support.RelativePose`); A's support runs +//! untransformed. Better precision far from the world origin (avoids +//! large-coordinate cancellation) and half the per-iteration transforms. Frozen +//! now: changing the computation frame after the M1.1.14 determinism freeze +//! would break validated bit-exactness. //! //! **Determinism by construction (anticipates M1.1.14).** No hash containers, -//! no trigonometry (dot/cross only), fixed support tie-breaks (see `support`). +//! no trigonometry (dot/cross only), fixed support tie-breaks (see +//! `support.SupportShape.support`). const std = @import("std"); const math = @import("foundation").math; - -/// A convex support shape: a convex **core** (point / segment / box) plus an -/// inflation `radius` around it (the Jolt convex-radius architecture). GJK runs -/// on the core alone and never sees the inflated surface, so the milestone's -/// touch test is `distance(cores) <= r_a + r_b` (the contact interpretation is -/// M1.1.3). `radius` is 0 for a box in M1.1.2. Future shapes (cylinder, convex -/// hull) are new `Core` cases, purely additive. -pub fn SupportShape(comptime T: type) type { - return struct { - const Self = @This(); - const Vec3T = math.Vec(3, T); - - /// The convex core geometry, in the shape's own local frame. - core: Core, - /// Inflation radius around the core (0 for box in M1.1.2). - radius: T, - - /// The convex core of a support shape, in its local frame. - pub const Core = union(enum) { - /// Sphere core: the local origin. - point, - /// Capsule core: Y-axis segment, ±half_height. - segment: T, - /// Box core: half-extents (full box, no shrink). - box: math.Vec(3, T), - }; - - /// Support point of the **core** (the inflation radius is excluded), in - /// the local frame: the core point that maximizes `dir · p`. `dir` need - /// not be normalized. - /// - /// Tie-breaks are fixed for determinism (brief Notes): a capsule - /// direction with `dir.y == 0` selects the `+Y` endpoint; a box - /// direction component `== 0` selects the `+half_extent` corner - /// component. Both fall out of the `>= 0` comparisons below. - pub fn support(self: Self, dir: Vec3T) Vec3T { - switch (self.core) { - .point => return Vec3T.zero, - .segment => |half_height| { - // Segment lies on the Y axis; only the sign of dir.y counts. - const dy = dir.toArray()[1]; - const y = if (dy >= 0) half_height else -half_height; - return Vec3T.fromArray(.{ 0, y, 0 }); - }, - .box => |half_extents| { - // Separable per axis: p_i = sign(dir_i) · half_extent_i. - const d = dir.toArray(); - const he = half_extents.toArray(); - return Vec3T.fromArray(.{ - if (d[0] >= 0) he[0] else -he[0], - if (d[1] >= 0) he[1] else -he[1], - if (d[2] >= 0) he[2] else -he[2], - }); - }, - } - } - }; -} - -/// Shape B's pose relative to shape A, precomputed once per pair so GJK runs -/// entirely in A's frame (A's support is untransformed; only B's support pays a -/// transform). See the file header for why the frame of A is frozen. -pub fn RelativePose(comptime T: type) type { - return struct { - const Self = @This(); - const Vec3T = math.Vec(3, T); - const QuatT = math.Quat(T); - const SupportShapeT = SupportShape(T); - - /// Orientation of B expressed in A's frame: `conj(rot_a) · rot_b`. - rot_rel: QuatT, - /// Origin of B expressed in A's frame: `conj(rot_a) · (pos_b − pos_a)`. - pos_rel: Vec3T, - - /// Precompute B-relative-to-A from the two world poses. Uses the - /// conjugate (the unit-quaternion inverse) throughout — never - /// `inverse()`, which divides by the squared norm on the hot path - /// (brief Notes). - pub fn init(pos_a: Vec3T, rot_a: QuatT, pos_b: Vec3T, rot_b: QuatT) Self { - const inv_a = rot_a.conjugate(); - return .{ - .rot_rel = inv_a.mul(rot_b), - .pos_rel = inv_a.rotateVec3(pos_b.sub(pos_a)), - }; - } - - /// Support point of shape B's core, expressed in A's frame, for a - /// direction `dir` given in A's frame. The direction is inverse-rotated - /// into B's local frame (`conj(rot_rel)`), B's local support is taken, - /// then the result is mapped back into A's frame. - pub fn supportB(self: Self, shape_b: SupportShapeT, dir: Vec3T) Vec3T { - const local_dir = self.rot_rel.conjugate().rotateVec3(dir); - const p_local = shape_b.support(local_dir); - return self.rot_rel.rotateVec3(p_local).add(self.pos_rel); - } - }; -} +const support = @import("support.zig"); /// The GJK simplex machinery: the triplet `Vertex` and the origin-closest-point /// solver over point / segment / triangle / tetrahedron. The four `closestOrigin*` @@ -147,19 +52,13 @@ pub fn Simplex(comptime T: type) type { return struct { const Vec3T = math.Vec(3, T); - /// One simplex vertex: a point `w` on the Minkowski difference of the two - /// cores together with the two supports it came from. Storing all three - /// (rather than just `w = support_a − support_b`) lets the GJK loop - /// reconstruct the closest points on A and B from the barycentric weights - /// (E3): `closest_a = Σ λ_i · support_a_i`, likewise for B. - pub const Vertex = struct { - /// Minkowski-difference point `support_a − support_b` (A's frame). - w: Vec3T, - /// Support point on A's core (A's frame). - support_a: Vec3T, - /// Support point on B's core (A's frame). - support_b: Vec3T, - }; + /// One simplex vertex: a support sample on the Minkowski difference of + /// the two cores (`w`, `support_a`, `support_b`). Aliases the shared + /// `support.Vertex(T)` — storing all three (rather than just + /// `w = support_a − support_b`) lets the GJK loop reconstruct the closest + /// points on A and B from the barycentric weights: + /// `closest_a = Σ λ_i · support_a_i`, likewise for B. + pub const Vertex = support.Vertex(T); /// The reduced simplex a `closestOrigin*` call resolves to: the point on /// the simplex closest to the origin, the surviving input-vertex indices @@ -455,10 +354,10 @@ pub fn GjkResult(comptime T: type) type { /// separation within that noise margin, both count as shallow. pub fn gjk( comptime T: type, - shape_a: SupportShape(T), + shape_a: support.SupportShape(T), pos_a: math.Vec(3, T), rot_a: math.Quat(T), - shape_b: SupportShape(T), + shape_b: support.SupportShape(T), pos_b: math.Vec(3, T), rot_b: math.Quat(T), ) GjkResult(T) { @@ -501,7 +400,7 @@ pub fn gjk( const conv_k: T = 16; const mach_eps: T = noise_k * std.math.floatEps(T); - const relpose = RelativePose(T).init(pos_a, rot_a, pos_b, rot_b); + const relpose = support.RelativePose(T).init(pos_a, rot_a, pos_b, rot_b); // Seed the search from the relative position; fixed fallback if degenerate // (coincident origins) — a fixed fallback keeps the walk deterministic. @@ -509,7 +408,7 @@ pub fn gjk( if (dir.dot(dir) <= 0) dir = Vec3T.unit_x; var verts: [4]VertexT = undefined; - verts[0] = minkowskiSupport(T, shape_a, relpose, shape_b, dir); + verts[0] = support.minkowskiSupport(T, shape_a, relpose, shape_b, dir); var count: usize = 1; var closest = verts[0].w; var bary: [4]T = .{ 1, 0, 0, 0 }; @@ -521,7 +420,7 @@ pub fn gjk( // This is the numerical-noise early-out, NOT a geometric proximity test. if (degenerateOriginReached(T, verts[0..count], closest, mach_eps)) return deepResult(T, verts, count); - const w = minkowskiSupport(T, shape_a, relpose, shape_b, closest.neg()); + const w = support.minkowskiSupport(T, shape_a, relpose, shape_b, closest.neg()); // Progress test (relative, squared distance): `vv − v·w ≥ 0` for the // closest point `v`; when it is negligible the support in direction −v @@ -610,21 +509,6 @@ pub fn gjk( // --- GJK internal helpers --- -/// Support point of the Minkowski difference of the two cores in direction -/// `dir` (A's frame): `support_A(dir) − support_B(−dir)`, keeping both supports -/// so the closest points on A and B stay reconstructible. -fn minkowskiSupport( - comptime T: type, - shape_a: SupportShape(T), - relpose: RelativePose(T), - shape_b: SupportShape(T), - dir: math.Vec(3, T), -) Simplex(T).Vertex { - const sa = shape_a.support(dir); - const sb = relpose.supportB(shape_b, dir.neg()); - return .{ .w = sa.sub(sb), .support_a = sa, .support_b = sb }; -} - /// Dispatch the E2 Voronoi solver by simplex size. GJK maintains a 1..4-vertex /// simplex by construction, so the `else` arm is exactly the tetrahedron. fn closestOnSimplex(comptime T: type, verts: []const Simplex(T).Vertex) Simplex(T).Result { @@ -652,7 +536,7 @@ fn maxVertexMagSq(comptime T: type, verts: []const Simplex(T).Vertex) T { /// box. Summed over both cores plus the relative-centre distance, it forms the /// SYMMETRIC coordinate-scale bound for the contact margin (`gjk`), so the /// shallow/separated decision is invariant under an A/B swap. -fn coreExtent(comptime T: type, shape: SupportShape(T)) T { +fn coreExtent(comptime T: type, shape: support.SupportShape(T)) T { return switch (shape.core) { .point => 0, .segment => |half_height| half_height, diff --git a/src/modules/forge/forge_3d/pipeline/narrowphase/manifold.zig b/src/modules/forge/forge_3d/pipeline/narrowphase/manifold.zig new file mode 100644 index 0000000..ed637a4 --- /dev/null +++ b/src/modules/forge/forge_3d/pipeline/narrowphase/manifold.zig @@ -0,0 +1,782 @@ +//! `forge_3d/pipeline/narrowphase/manifold.zig` — the contact manifold types and +//! (M1.1.3/E3) the supporting-face clipping generator. +//! +//! M1.1.3/E1 landed the FROZEN `ContactManifold(T)` / `ContactPoint(T)` types and +//! the normal / depth / feature-id convention. E3 adds the single-shot generator +//! and the `collide` entry that drives both regimes. +//! +//! **The generator runs in A's frame (brief Notes, Guy's E3 kickoff).** It takes +//! a WORLD contact normal (shallow: `normalize(closest_b − closest_a)`; deep: +//! `EpaResult.normal`), converts it ONCE to A's frame (`conj(rot_a)·n`, a unit +//! vector rotation), takes `supportingFace(+n_a)` on A and `supportingFace(−n_a)` +//! on B (mapped to A's frame by `supportingFaceB`), picks the reference face (the +//! one whose outward normal is most aligned with the contact axis — a +//! non-polygon feature counts as 0-aligned; tie → A), clips the incident feature +//! against the reference's side planes (Sutherland-Hodgman), keeps the +//! penetrating points, reduces to ≤ 4, applies the inflation radii, and maps the +//! final normal + points to WORLD once. No clipping in world space — that would +//! reintroduce the large-coordinate cancellation the frame-of-A choice (M1.1.14) +//! avoids. +//! +//! **Per-point penetration (both regimes, continuous).** `r_sum − s`, where `s` +//! is the incident core point's signed distance from the reference face plane: +//! deep (`s < 0`) → `r_sum + |s|`, shallow (`s > 0`) → `r_sum − s`. A point-core +//! contact (a sphere, or an end-on capsule → 1 vertex) yields a single point from +//! the GJK/EPA closest points. +//! +//! **Order-independence (FROZEN hard requirement + F5).** `collide` computes the +//! pair in a caller-independent CANONICAL order (a deterministic key on +//! shape+pose) and negates the normal for the swapped caller, so `collide(A,B)` +//! and `collide(B,A)` give the same points/depths with negated normals. Ratified +//! exception (brief RD-4): two shapes with bit-identical pose AND geometry have no +//! geometric A→B axis (measure-zero), so `collide` returns the SAME arbitrary +//! normal in both orders there rather than a negated pair; `BodyManager.collidePair` +//! restores full order-independence for real bodies via a canonical body-id order. +//! +//! **Dependency discipline (brief Notes).** Imports `foundation` (math) and the +//! sibling `support.zig` / `gjk.zig` / `epa.zig` ONLY — never `weld_forge`, +//! `body*.zig`, `config.zig`, or `broadphase.zig`. The scalar is the comptime +//! `T`; `forge_3d` instantiates it at `config.Real`. + +const std = @import("std"); +const math = @import("foundation").math; +const support = @import("support.zig"); +const gjk_mod = @import("gjk.zig"); +const epa_mod = @import("epa.zig"); + +/// The contact manifold between two shapes: a shared world-space contact +/// `normal` (A→B) plus up to 4 `ContactPoint`s. FROZEN convention (brief Notes); +/// produced by the M1.1.3/E3 supporting-face clipper. +pub fn ContactManifold(comptime T: type) type { + return struct { + /// Unit, world-space, points from A to B — the axis along which to + /// translate B to reduce penetration. + normal: math.Vec(3, T), + /// Up to 4 contact points (box face-face max after reduction). + points: [4]ContactPoint(T), + /// Valid entries in `points`, 1..4. + count: u8, + }; +} + +/// One contact point of a `ContactManifold`. FROZEN convention (brief Notes). +pub fn ContactPoint(comptime T: type) type { + return struct { + /// World-space point on the contact plane (midpoint of the two + /// surface points at this contact). + position: math.Vec(3, T), + /// Surface penetration along `normal`, >= 0 when overlapping. + penetration: T, + /// Deterministic per-contact identity for M1.1.6 warm-starting: + /// `(reference-feature id << 16) | incident-feature id`, each 16-bit half + /// class-tagged in its top 2 bits so the contact kinds occupy DISJOINT id + /// ranges — kept vertex `(class_a, class_a)`, edge×plane crossing + /// `(class_edge, class_edge)`, reference corner `(class_c, class_c)`, and + /// single witness `(class_a, class_c)` — with each half carrying its REAL + /// sub-feature (box vertex sign-pattern, box face `axis·2+sign`, segment + /// endpoint, side-plane pair). FRAME-STABLE only through + /// `BodyManager.collidePair` (fixed body-id order); the bare `collide` + /// entry is POSE-canonical, so reference/incident ownership flips at a + /// pose-order boundary and the id is NOT inter-frame stable there. + /// Populated now; consumed at M1.1.6. Packing is an impl detail. + feature_id: u32, + }; +} + +/// Full narrowphase for a convex pair at their world poses: GJK, then the +/// contact manifold. Returns null when the pair is separated. The regimes wire +/// through one generator — `.shallow` supplies the normal from the GJK closest +/// points, `.deep` from EPA. Order-independence is guaranteed by computing the +/// pair in a caller-independent canonical POSE order and negating the normal for +/// the swapped caller (with the ratified RD-4 exception for bit-identical +/// pose+geometry). Because the canonical order is by POSE, the `feature_id` +/// reference/incident ownership flips at a pose-order boundary — a caller that +/// needs a FRAME-STABLE `feature_id` (M1.1.6 warm-starting) must drive the fixed +/// `collideOrdered` in a stable external order instead; `BodyManager.collidePair` +/// does this by body id. +pub fn collide( + comptime T: type, + shape_a: support.SupportShape(T), + pos_a: math.Vec(3, T), + rot_a: math.Quat(T), + shape_b: support.SupportShape(T), + pos_b: math.Vec(3, T), + rot_b: math.Quat(T), +) ?ContactManifold(T) { + if (poseAfter(T, shape_a, pos_a, rot_a, shape_b, pos_b, rot_b)) { + // Caller order is the reverse of canonical: compute canonically (B,A), + // then negate the normal so it points along the caller's A→B. + var m = collideOrdered(T, shape_b, pos_b, rot_b, shape_a, pos_a, rot_a) orelse return null; + m.normal = m.normal.neg(); + return m; + } + return collideOrdered(T, shape_a, pos_a, rot_a, shape_b, pos_b, rot_b); +} + +/// `collide` for a FIXED shape order — no pose canonicalization. The manifold's +/// normal is A→B and its `feature_id` reference/incident ownership follows the +/// given `(a, b)` order. Callers that own a stable external key (e.g. body ids) +/// use this directly so the feature_id stays frame-stable across a pose change +/// that would flip `collide`'s pose-based order (Codex P1b); `BodyManager`'s +/// `collidePair` drives it in a canonical body-id order. +pub fn collideOrdered( + comptime T: type, + shape_a: support.SupportShape(T), + pos_a: math.Vec(3, T), + rot_a: math.Quat(T), + shape_b: support.SupportShape(T), + pos_b: math.Vec(3, T), + rot_b: math.Quat(T), +) ?ContactManifold(T) { + const Vec3T = math.Vec(3, T); + const g = gjk_mod.gjk(T, shape_a, pos_a, rot_a, shape_b, pos_b, rot_b); + if (g.status == .separated) return null; + + const relpose = support.RelativePose(T).init(pos_a, rot_a, pos_b, rot_b); + const r_sum = shape_a.radius + shape_b.radius; + + var n_world: Vec3T = undefined; + var closest_a: Vec3T = undefined; + var closest_b: Vec3T = undefined; + var base_penetration: T = undefined; + if (g.status == .deep) { + const e = epa_mod.epa(T, shape_a, pos_a, rot_a, relpose, shape_b, g); + n_world = e.normal; + closest_a = e.closest_a; + closest_b = e.closest_b; + base_penetration = e.depth + r_sum; // deep: core overlap + inflation + } else { // shallow — cores disjoint, penetration from inflation only + closest_a = g.closest_a; + closest_b = g.closest_b; + const sep = closest_b.sub(closest_a); + const sep_len_sq = sep.dot(sep); + // n = normalize(closest_b − closest_a); guard the dist ≈ 0 boundary (the + // shallow↔deep seam) with the centre-to-centre search direction. + const noise: T = if (T == f32) 1.0e-12 else 1.0e-24; + n_world = if (sep_len_sq > noise) sep.scale(1.0 / @sqrt(sep_len_sq)) else fallbackNormal(T, pos_a, pos_b); + base_penetration = r_sum - g.distance; + } + + return generateManifold(T, shape_a, pos_a, rot_a, relpose, shape_b, n_world, closest_a, closest_b, base_penetration); +} + +/// Build the contact manifold from a resolved world contact normal (`n_world`, +/// A→B) and the two shapes/poses, working in A's frame (see the file header). +/// `closest_a`/`closest_b` (world core witness points) and `base_penetration` +/// feed the single-point (point-core) path; the multi-point path derives per- +/// point penetration from the clip. +fn generateManifold( + comptime T: type, + shape_a: support.SupportShape(T), + pos_a: math.Vec(3, T), + rot_a: math.Quat(T), + relpose: support.RelativePose(T), + shape_b: support.SupportShape(T), + n_world: math.Vec(3, T), + closest_a: math.Vec(3, T), + closest_b: math.Vec(3, T), + base_penetration: T, +) ContactManifold(T) { + const Vec3T = math.Vec(3, T); + const r_a = shape_a.radius; + const r_b = shape_b.radius; + const r_sum = r_a + r_b; + + // Contact normal into A's frame (unit vector rotation — safe). + const n_a = rot_a.conjugate().rotateVec3(n_world); + + // Supporting features, both in A's frame. + const face_a = shape_a.supportingFace(n_a); + const face_b = relpose.supportingFaceB(shape_b, n_a.neg()); + + // Point-core contact (a sphere, or an end-on capsule → 1 vertex): a single + // contact straight from the GJK/EPA witness points, mapped to the surfaces. + if (face_a.count == 1 or face_b.count == 1) { + return pointCoreContact(T, n_world, closest_a, closest_b, r_a, r_b, base_penetration, singleContactFid(T, face_a, face_b)); + } + + // Reference/incident by alignment with the contact axis (non-polygon → 0; tie A). + const n_a_face = faceNormalA(T, face_a, n_a); // ≈ +n_a + const n_b_face = faceNormalA(T, face_b, n_a.neg()); // ≈ −n_a + const align_a: T = if (face_a.count >= 3) @abs(n_a_face.dot(n_a)) else 0; + const align_b: T = if (face_b.count >= 3) @abs(n_b_face.dot(n_a)) else 0; + + const a_is_ref = align_a >= align_b; // tie → A + const ref = if (a_is_ref) face_a else face_b; + const inc = if (a_is_ref) face_b else face_a; + const rn = if (a_is_ref) n_a_face else n_b_face; + const r_ref = if (a_is_ref) r_a else r_b; + const r_inc = if (a_is_ref) r_b else r_a; + const ref_pt = ref.verts[0]; + + // FIX-1 (edge-edge): the reference face normal `rn` is the penetration axis + // ONLY when it aligns with the contact axis `n_a` (a genuine face-face). For + // an edge/vertex contact `n_a` is an edge-cross direction no box quad aligns + // with, so measuring penetration along `rn` is wrong (it diverges from the EPA + // depth). Detect it by `|rn · n_a|` and fall back to the witness-point single + // contact, whose depth is along `n_a` (EPA / GJK-closest). + const face_face_min: T = if (T == f32) 0.999 else 0.9999; + if (@abs(rn.dot(n_a)) < face_face_min) { + return pointCoreContact(T, n_world, closest_a, closest_b, r_a, r_b, base_penetration, singleContactFid(T, ref, inc)); + } + + // Clip the incident polygon/segment against the reference side planes, + // carrying a stable feature id per point (FIX-2). + var clip_buf: [max_clip]Vec3T = undefined; + var fid_buf: [max_clip]u32 = undefined; + const clipped = clipIncident(T, inc, ref, rn, &clip_buf, &fid_buf); + + // Keep penetrating points IN A'S FRAME (FIX-4): reduce/dedup in A's frame so + // precision holds far from the world origin; map only the chosen ≤ 4 points to + // world at the very end. + var raw: [max_clip]Candidate(T) = undefined; + var raw_n: usize = 0; + const keep_eps: T = if (T == f32) 1.0e-5 else 1.0e-10; + for (clipped, 0..) |v, ci| { + const s = v.sub(ref_pt).dot(rn); + const pen = r_sum - s; + if (pen < -keep_eps) continue; + // Surface points: reference face + r_ref outward; incident core − r_inc. + const foot = v.sub(rn.scale(s)); + const pos_a_frame = foot.add(v).scale(0.5).add(rn.scale((r_ref - r_inc) * 0.5)); + raw[raw_n] = .{ .pos = pos_a_frame, .pen = @max(pen, 0), .fid = fid_buf[ci] }; + raw_n += 1; + } + + if (raw_n == 0) { + // Degenerate clip (an oblique feature that clipped empty): fall back to the + // single witness-point contact so a genuine overlap is never lost. + return pointCoreContact(T, n_world, closest_a, closest_b, r_a, r_b, base_penetration, singleContactFid(T, ref, inc)); + } + + // Reduce to ≤ 4 (A's frame, deepest + area-maximising), then map to world. + var idx: [4]usize = undefined; + const count = reduceToFour(T, raw[0..raw_n], n_a, &idx); + var points: [4]ContactPoint(T) = undefined; + for (0..count) |i| { + const c = raw[idx[i]]; + points[i] = .{ .position = rot_a.rotateVec3(c.pos).add(pos_a), .penetration = c.pen, .feature_id = c.fid }; + } + for (count..4) |i| points[i] = .{ .position = Vec3T.zero, .penetration = 0, .feature_id = 0 }; + return .{ .normal = n_world, .points = points, .count = @intCast(count) }; +} + +/// The shared single-contact path (point core, edge/vertex contact, or a +/// degenerate empty clip): one contact from the GJK/EPA witness points, on the +/// contact plane midway between the two inflated surfaces, depth along `n_world`. +fn pointCoreContact(comptime T: type, n_world: math.Vec(3, T), closest_a: math.Vec(3, T), closest_b: math.Vec(3, T), r_a: T, r_b: T, base_penetration: T, feature_id: u32) ContactManifold(T) { + const sa = closest_a.add(n_world.scale(r_a)); // A's surface toward B + const sb = closest_b.sub(n_world.scale(r_b)); // B's surface toward A + return oneContact(T, n_world, sa.add(sb).scale(0.5), @max(base_penetration, 0), feature_id); +} + +// --- Internal helpers --- + +/// Largest number of points the incident clip can transiently hold: an incident +/// quad (4) clipped against up to 4 reference side planes yields ≤ 8. +const max_clip: usize = 8; + +/// A candidate contact before reduction: its A-frame position, penetration, and +/// stable feature id (FIX-2/FIX-4). +fn Candidate(comptime T: type) type { + return struct { pos: math.Vec(3, T), pen: T, fid: u32 }; +} + +/// A single-point manifold with the tail zeroed. +fn oneContact(comptime T: type, normal: math.Vec(3, T), position: math.Vec(3, T), penetration: T, feature_id: u32) ContactManifold(T) { + const Vec3T = math.Vec(3, T); + const zero = ContactPoint(T){ .position = Vec3T.zero, .penetration = 0, .feature_id = 0 }; + return .{ + .normal = normal, + .points = .{ .{ .position = position, .penetration = penetration, .feature_id = feature_id }, zero, zero, zero }, + .count = 1, + }; +} + +/// The feature id of a SINGLE witness contact (point core, edge/vertex fallback, +/// or degenerate empty clip): the reference side (`class_a`) high, the incident +/// side (`class_c`) low. Each side encodes its REAL sub-feature — a count-1 +/// feature (a sphere point, or an end-on capsule endpoint) is a VERTEX (its +/// `vert_id`), otherwise a FACE (its `face_id`) — so a capsule's `+Y` (vert 0) +/// and `−Y` (vert 1) endpoints, and an end-on endpoint (here) vs a side segment +/// (`clipSegment`), get distinct ids (Codex FIX-12). The `(class_a, class_c)` +/// class pair is used by NO clip producer (kept vertex `(class_a, class_a)`, edge +/// crossing `(class_edge, class_edge)`, reference corner `(class_c, class_c)`, +/// `clipSegment` endpoints `(class_a, class_a)`), so a single-contact id can never +/// alias a clip id for a body pair. No untagged packer remains. +fn singleContactFid(comptime T: type, ref_face: support.Face(T), inc_face: support.Face(T)) u32 { + const feat_ref: u16 = if (ref_face.count == 1) ref_face.vert_ids[0] else ref_face.face_id; + const feat_inc: u16 = if (inc_face.count == 1) inc_face.vert_ids[0] else inc_face.face_id; + return featureId(class_a | (feat_ref & id_mask), class_c | (feat_inc & id_mask)); +} + +/// Outward normal of a face in A's frame: the polygon normal oriented toward +/// `expected_axis` for a real face (≥ 3 verts); the axis itself as a proxy for a +/// point / segment feature (which has no face normal). +fn faceNormalA(comptime T: type, face: support.Face(T), expected_axis: math.Vec(3, T)) math.Vec(3, T) { + if (face.count < 3) return expected_axis; + var n = face.verts[1].sub(face.verts[0]).cross(face.verts[2].sub(face.verts[0])); + const l2 = n.dot(n); + if (!(l2 > 0)) return expected_axis; + n = n.scale(1.0 / @sqrt(l2)); + return if (n.dot(expected_axis) < 0) n.neg() else n; +} + +// Feature-id classes, per 16-bit half (top two bits). A contact's `feature_id` +// is `(reference_feature << 16) | incident_feature`; the class bits keep the +// three contact kinds in DISTINCT id ranges so they never alias (Codex FIX-9): +// reference half: face (`class_a`), side plane / edge (`class_edge`), vertex / +// corner (`class_c`); incident half: vertex (`class_a`), edge (`class_edge`), +// face (`class_c`). +const class_a: u16 = 0x0000; // reference face / incident vertex +const class_edge: u16 = 0x4000; // reference side plane / incident edge +const class_c: u16 = 0x8000; // reference vertex (corner) / incident face +const class_mask: u16 = 0xc000; +const id_mask: u16 = 0x3fff; + +/// Pack a `feature_id` from a class-tagged reference half and incident half. +fn featureId(ref16: u16, inc16: u16) u32 { + return (@as(u32, ref16) << 16) | @as(u32, inc16); +} + +/// The stable feature id of a clip-intersection point crossed by reference side +/// plane `qid`, on the incident face `inc_face_id`. A segment whose two endpoints +/// share a reference plane `p` is a reference cut-edge, so the new point lies on +/// `p` AND `qid` — a REFERENCE CORNER (a ref vertex), whose incident feature is +/// the incident FACE (`class_c | inc_face_id` — carrying which incident face, so +/// a supporting-face flip inter-frame changes the id and warm-starting cannot +/// mis-match two corners at the same ref vertex on different faces; Codex FIX-10). +/// It must NOT inherit a neighbour's incident edge (Codex FIX-9: that aliased +/// distinct contacts). Otherwise the point is an incident-edge × ref-edge crossing. +fn intersectionFid(qid: u16, cur_fid: u32, nxt_fid: u32, inc_face_id: u16) u32 { + const cur_ref: u16 = @intCast(cur_fid >> 16); + const nxt_ref: u16 = @intCast(nxt_fid >> 16); + if (commonRefPlane(cur_ref, nxt_ref)) |p| { + return featureId(class_c | refVertexId(p, qid), class_c | (inc_face_id & id_mask)); + } + const cur_inc: u16 = @intCast(cur_fid & 0xffff); + const nxt_inc: u16 = @intCast(nxt_fid & 0xffff); + return featureId(class_edge | (qid & id_mask), class_edge | incidentEdgeId(cur_inc, nxt_inc)); +} + +/// The incident-edge id (its two vertices, `lo·8 + hi`), carrying an existing +/// incident-edge provenance when an endpoint is already an incident-edge +/// intersection. A non-vertex endpoint — a reference corner (incident class +/// `class_c`), which the Sutherland-Hodgman topology should keep out of this +/// branch (a corner shares a plane with its neighbours, so `commonRefPlane` +/// catches it), but defend anyway — self-pairs the vertex endpoint (`v·8 + v`, +/// `lo == hi`) so it can NEVER alias a genuine two-vertex incident edge (`lo < hi`). +fn incidentEdgeId(a_inc: u16, b_inc: u16) u16 { + if ((a_inc & class_mask) == class_edge) return a_inc & id_mask; + if ((b_inc & class_mask) == class_edge) return b_inc & id_mask; + const a_vert = (a_inc & class_mask) == class_a; + const b_vert = (b_inc & class_mask) == class_a; + const av = a_inc & id_mask; + const bv = b_inc & id_mask; + if (a_vert and b_vert) return @min(av, bv) * 8 + @max(av, bv); + if (a_vert) return av * 8 + av; + if (b_vert) return bv * 8 + bv; + return 0; +} + +/// The reference side planes an endpoint's reference feature lies on (0, 1, or 2), +/// written into `out`: a face lies on none, an edge on one, a corner on its two. +fn refPlaneSet(ref16: u16, out: *[2]u16) usize { + return switch (ref16 & class_mask) { + class_a => 0, + class_edge => blk: { + out[0] = ref16 & id_mask; + break :blk 1; + }, + else => blk: { // class_c: a ref vertex = the sorted pair of side planes + const id = ref16 & id_mask; + out[0] = id / 32; + out[1] = id % 32; + break :blk 2; + }, + }; +} + +/// A reference side plane both endpoints lie on (the segment is a ref cut-edge), +/// or null. +fn commonRefPlane(cur_ref: u16, nxt_ref: u16) ?u16 { + var ca: [2]u16 = undefined; + var cb: [2]u16 = undefined; + const na = refPlaneSet(cur_ref, &ca); + const nb = refPlaneSet(nxt_ref, &cb); + for (ca[0..na]) |a| { + for (cb[0..nb]) |b| { + if (a == b) return a; + } + } + return null; +} + +/// A reference vertex id: the sorted pair of the two side-plane ids meeting there +/// (`lo·32 + hi`; side-plane ids are ≤ ~23, so this stays inside `id_mask`). +fn refVertexId(p: u16, q: u16) u16 { + return @min(p, q) * 32 + @max(p, q); +} + +/// Clip the incident feature against the reference face's side planes (planes +/// through each reference edge, perpendicular to the reference face, oriented +/// inward; a segment reference contributes its two endpoint planes). A polygon +/// incident (≥ 3 verts) is clipped by Sutherland-Hodgman; a segment incident +/// (2 verts) is clipped as an OPEN segment (Liang-Barsky) — a closed-polygon +/// clip would double-emit the crossing point (adversarial-review finding E). +/// Threads a stable feature id per surviving point into `fid_buf` (FIX-2). +/// Returns the surviving points as a slice into `buf`. +fn clipIncident(comptime T: type, inc: support.Face(T), ref: support.Face(T), rn: math.Vec(3, T), buf: *[max_clip]math.Vec(3, T), fid_buf: *[max_clip]u32) []math.Vec(3, T) { + const Vec3T = math.Vec(3, T); + // Build the reference side planes (point + inward normal + stable ref-edge id). + var plane_p: [4]Vec3T = undefined; + var plane_n: [4]Vec3T = undefined; + var plane_ref: [4]u16 = undefined; + var np: usize = 0; + const centroid = faceCentroid(T, ref); + if (ref.count >= 3) { + for (0..ref.count) |i| { + const a = ref.verts[i]; + const b = ref.verts[(i + 1) % ref.count]; + var pn = rn.cross(b.sub(a)); // ⟂ the face and the edge + if (pn.dot(centroid.sub(a)) < 0) pn = pn.neg(); + if (pn.dot(pn) > 0) { + plane_p[np] = a; + plane_n[np] = pn; + plane_ref[np] = @as(u16, ref.face_id) * 4 + @as(u16, @intCast(i)); // stable ref-edge id + np += 1; + } + } + } else { // segment reference: two endpoint planes ⟂ the segment axis + const axis = ref.verts[1].sub(ref.verts[0]); + const ends = [2]struct { p: Vec3T, pn: Vec3T, id: u16 }{ + .{ .p = ref.verts[0], .pn = axis, .id = ref.vert_ids[0] }, + .{ .p = ref.verts[1], .pn = axis.neg(), .id = ref.vert_ids[1] }, + }; + for (ends) |e| { + var pn = e.pn; + if (pn.dot(centroid.sub(e.p)) < 0) pn = pn.neg(); + if (pn.dot(pn) > 0) { + plane_p[np] = e.p; + plane_n[np] = pn; + plane_ref[np] = e.id; + np += 1; + } + } + } + + // Initial incident feature ids: (reference face id) high, (incident vertex id) low. + var inc_fids: [4]u32 = undefined; + for (0..inc.count) |i| inc_fids[i] = (@as(u32, ref.face_id) << 16) | @as(u32, inc.vert_ids[i]); + + if (inc.count == 2) return clipSegment(T, inc.verts[0], inc.verts[1], inc_fids[0], inc_fids[1], plane_p[0..np], plane_n[0..np], plane_ref[0..np], inc.face_id, buf, fid_buf); + return clipPolygon(T, inc, inc_fids, plane_p[0..np], plane_n[0..np], plane_ref[0..np], buf, fid_buf); +} + +/// Sutherland-Hodgman clip of a polygon incident against the reference side +/// planes; ping-pongs two stack buffers, threading a parallel feature-id buffer. +fn clipPolygon(comptime T: type, inc: support.Face(T), inc_fids: [4]u32, plane_p: []const math.Vec(3, T), plane_n: []const math.Vec(3, T), plane_ref: []const u16, buf: *[max_clip]math.Vec(3, T), fid_buf: *[max_clip]u32) []math.Vec(3, T) { + const Vec3T = math.Vec(3, T); + var poly_a: [max_clip]Vec3T = undefined; + var poly_b: [max_clip]Vec3T = undefined; + var fa: [max_clip]u32 = undefined; + var fb: [max_clip]u32 = undefined; + var cur: []Vec3T = poly_a[0..inc.count]; + var cur_f: []u32 = fa[0..inc.count]; + for (0..inc.count) |i| { + cur[i] = inc.verts[i]; + cur_f[i] = inc_fids[i]; + } + for (plane_p, plane_n, plane_ref) |p, pn, pref| { + const dest_is_b = (cur.ptr == &poly_a); + const out_v: []Vec3T = if (dest_is_b) poly_b[0..] else poly_a[0..]; + const out_f: []u32 = if (dest_is_b) fb[0..] else fa[0..]; + const out_n = clipAgainstPlane(T, cur, cur_f, p, pn, pref, inc.face_id, out_v, out_f); + cur = out_v[0..out_n]; + cur_f = out_f[0..out_n]; + if (out_n == 0) break; + } + for (0..cur.len) |i| { + buf[i] = cur[i]; + fid_buf[i] = cur_f[i]; + } + return buf[0..cur.len]; +} + +/// One Sutherland-Hodgman pass on a CLOSED polygon: keep the part on the inward +/// side of the plane (point `p`, inward normal `pn`), inserting edge crossings; +/// threads feature ids (kept vertex → its id, intersection → `intersectionFid`). +fn clipAgainstPlane(comptime T: type, poly: []const math.Vec(3, T), poly_f: []const u32, p: math.Vec(3, T), pn: math.Vec(3, T), plane_ref: u16, inc_face_id: u16, out_v: []math.Vec(3, T), out_f: []u32) usize { + if (poly.len == 0) return 0; + var n: usize = 0; + var i: usize = 0; + while (i < poly.len) : (i += 1) { + const cur = poly[i]; + const nxt = poly[(i + 1) % poly.len]; + const dc = cur.sub(p).dot(pn); // ≥ 0 inside + const dn = nxt.sub(p).dot(pn); + if (dc >= 0) { + if (n < out_v.len) { + out_v[n] = cur; + out_f[n] = poly_f[i]; + n += 1; + } + } + if ((dc >= 0) != (dn >= 0)) { // edge crosses ⇒ insert the intersection + const denom = dc - dn; + if (denom != 0) { + const t = dc / denom; + if (n < out_v.len) { + out_v[n] = cur.add(nxt.sub(cur).scale(t)); + out_f[n] = intersectionFid(plane_ref, poly_f[i], poly_f[(i + 1) % poly.len], inc_face_id); + n += 1; + } + } + } + } + return n; +} + +/// Clip the OPEN segment `a`–`b` against the reference side half-spaces +/// (Liang-Barsky over `t ∈ [0, 1]`). Emits the surviving segment's endpoints (2, +/// or 1 if it collapses); empty if fully clipped away. A clipped endpoint takes an +/// intersection feature id; an un-clipped one keeps its incident-vertex id. +fn clipSegment(comptime T: type, a: math.Vec(3, T), b: math.Vec(3, T), a_fid: u32, b_fid: u32, plane_p: []const math.Vec(3, T), plane_n: []const math.Vec(3, T), plane_ref: []const u16, inc_face_id: u16, buf: *[max_clip]math.Vec(3, T), fid_buf: *[max_clip]u32) []math.Vec(3, T) { + const dir = b.sub(a); + var t0: T = 0; + var t1: T = 1; + var f0 = a_fid; + var f1 = b_fid; + for (plane_p, plane_n, plane_ref) |p, pn, pref| { + const num = a.sub(p).dot(pn); // value of (x−p)·pn at t = 0 + const den = dir.dot(pn); // d/dt of (x−p)·pn + if (den == 0) { + if (num < 0) return buf[0..0]; // parallel and outside ⇒ nothing survives + } else { + const t = -num / den; // (x−p)·pn = 0 at this t + if (den > 0) { + if (t > t0) { + t0 = t; // entering the half-space + f0 = intersectionFid(pref, a_fid, b_fid, inc_face_id); + } + } else { + if (t < t1) { + t1 = t; // leaving it + f1 = intersectionFid(pref, a_fid, b_fid, inc_face_id); + } + } + if (t0 > t1) return buf[0..0]; + } + } + buf[0] = a.add(dir.scale(t0)); + fid_buf[0] = f0; + const eps: T = if (T == f32) 1.0e-6 else 1.0e-12; + if (t1 - t0 <= eps) return buf[0..1]; + buf[1] = a.add(dir.scale(t1)); + fid_buf[1] = f1; + return buf[0..2]; +} + +/// Centroid of a face's valid vertices (a reference interior point for inward +/// plane orientation). +fn faceCentroid(comptime T: type, face: support.Face(T)) math.Vec(3, T) { + const Vec3T = math.Vec(3, T); + var c = Vec3T.zero; + for (0..face.count) |i| c = c.add(face.verts[i]); + return c.scale(1.0 / @as(T, @floatFromInt(face.count))); +} + +/// Reduce a contact set to ≤ 4 points. The contacts are coplanar (they lie on +/// the contact plane, normal `normal`), so the 4th point is chosen by SIGNED area +/// about the `k0`–`k1` axis — the two extreme points on each side of that axis — +/// not by distance from the (also-coplanar) `k0`,`k1`,`k2` triangle plane, which +/// is degenerate for a planar set (adversarial-review finding F). Coincident +/// points are deduplicated first. Deterministic tie-breaks (strict `>`; first +/// index wins). Writes the chosen `pts` indices into `idx`, returns the count. +fn reduceToFour(comptime T: type, pts: []const Candidate(T), normal: math.Vec(3, T), idx: *[4]usize) usize { + const eps: T = if (T == f32) 1.0e-5 else 1.0e-10; + // Deduplicate coincident contacts (indices into `pts`). + var uniq: [max_clip]usize = undefined; + var un: usize = 0; + outer: for (pts, 0..) |c, i| { + for (0..un) |j| { + if (c.pos.approxEql(pts[uniq[j]].pos, eps)) continue :outer; + } + uniq[un] = i; + un += 1; + } + if (un <= 4) { + for (0..un) |i| idx[i] = uniq[i]; + return un; + } + + // 1: deepest. 2: farthest from it. + var k0 = uniq[0]; + for (uniq[0..un]) |i| { + if (pts[i].pen > pts[k0].pen) k0 = i; + } + var k1 = uniq[0]; + var best_d: T = -1; + for (uniq[0..un]) |i| { + const d = pts[i].pos.sub(pts[k0].pos).lengthSq(); + if (d > best_d) { + best_d = d; + k1 = i; + } + } + // 3 & 4: extreme signed area on each side of the k0–k1 axis (coplanar set). + var k2 = k0; + var k3 = k0; + var best_pos: T = 0; + var best_neg: T = 0; + const axis = pts[k1].pos.sub(pts[k0].pos); + for (uniq[0..un]) |i| { + const area = pts[i].pos.sub(pts[k0].pos).cross(axis).dot(normal); + if (area > best_pos) { + best_pos = area; + k2 = i; + } + if (area < best_neg) { + best_neg = area; + k3 = i; + } + } + idx[0] = k0; + idx[1] = k1; + idx[2] = k2; + idx[3] = k3; + // Guard against a one-sided set leaving k2 or k3 latched onto k0. + var out: usize = 0; + var chosen: [4]usize = undefined; + for (idx.*) |c| { + var dup = false; + for (0..out) |j| { + if (chosen[j] == c) dup = true; + } + if (!dup) { + chosen[out] = c; + out += 1; + } + } + for (0..out) |i| idx[i] = chosen[i]; + return out; +} + +/// A deterministic separation normal for the shallow `dist ≈ 0` boundary — the +/// centre-to-centre direction, falling back to +X if the centres coincide. +fn fallbackNormal(comptime T: type, pos_a: math.Vec(3, T), pos_b: math.Vec(3, T)) math.Vec(3, T) { + const Vec3T = math.Vec(3, T); + const d = pos_b.sub(pos_a); + const l2 = d.dot(d); + return if (l2 > 0) d.scale(1.0 / @sqrt(l2)) else Vec3T.unit_x; +} + +/// Whether pair `(a, b)` is the reverse of canonical order — i.e. `collide` +/// should compute `(b, a)` and negate. A strict, total, deterministic order on +/// (shape, pose): position, then rotation, then radius, then core kind + extents. +/// Two distinct shapes/poses always compare unequal, so the winner is caller- +/// independent — the basis of order-independence. +/// +/// Degenerate exception (RATIFIED, brief RD-4 / `engine-physics-forge.md` +/// narrowphase §): two shapes with BIT-IDENTICAL pose AND geometry compare equal +/// (neither `poseAfter(a,b)` nor `poseAfter(b,a)` is true). Then both call orders +/// run the same `collideOrdered` and return the SAME normal — for coincident +/// identical shapes the A→B axis is geometrically undefined (measure-zero), so +/// the same deterministic-but-arbitrary unit vector in both orders is the ratified +/// contract, not a negated pair. `BodyManager`'s `collidePair` restores full +/// order-independence for real bodies via a canonical body-id order (distinct +/// bodies always break the tie). +fn poseAfter( + comptime T: type, + shape_a: support.SupportShape(T), + pos_a: math.Vec(3, T), + rot_a: math.Quat(T), + shape_b: support.SupportShape(T), + pos_b: math.Vec(3, T), + rot_b: math.Quat(T), +) bool { + const pa = pos_a.toArray(); + const pb = pos_b.toArray(); + inline for (0..3) |i| { + if (pa[i] != pb[i]) return pa[i] > pb[i]; + } + const ra = [4]T{ rot_a.x, rot_a.y, rot_a.z, rot_a.w }; + const rb = [4]T{ rot_b.x, rot_b.y, rot_b.z, rot_b.w }; + inline for (0..4) |i| { + if (ra[i] != rb[i]) return ra[i] > rb[i]; + } + if (shape_a.radius != shape_b.radius) return shape_a.radius > shape_b.radius; + // Core kind (point < segment < box), then the core's full parameters + // lexicographically — a strict total order (no distinct cores compare equal, + // unlike a `|half_extents|`-only tag where e.g. (1,2,3) and (3,2,1) collide). + const ka = coreKind(T, shape_a); + const kb = coreKind(T, shape_b); + if (ka != kb) return ka > kb; + switch (shape_a.core) { + .point => return false, + .segment => |ha| return ha > shape_b.core.segment, + .box => |hea| { + const a = hea.toArray(); + const b = shape_b.core.box.toArray(); + inline for (0..3) |i| { + if (a[i] != b[i]) return a[i] > b[i]; + } + return false; + }, + } +} + +/// Core-kind rank for the canonical order: point < segment < box. +fn coreKind(comptime T: type, shape: support.SupportShape(T)) u8 { + return switch (shape.core) { + .point => 0, + .segment => 1, + .box => 2, + }; +} + +const testing = std.testing; + +test "manifold contact types round-trip" { + const T = f32; + const V = math.Vec(3, T); + const M = ContactManifold(T); + const P = ContactPoint(T); + + const zero_pt = P{ .position = V.zero, .penetration = 0, .feature_id = 0 }; + const m = M{ + .normal = V.unit_x, + .points = .{ + P{ .position = V.fromArray(.{ 1, 2, 3 }), .penetration = 0.25, .feature_id = 0x0003_0007 }, + P{ .position = V.fromArray(.{ -1, 0, 4 }), .penetration = 0.5, .feature_id = 0x0002_0005 }, + zero_pt, + zero_pt, + }, + .count = 2, + }; + + try testing.expect(m.normal.approxEql(V.unit_x, 1e-6)); + try testing.expectEqual(@as(u8, 2), m.count); + try testing.expect(m.points[0].position.approxEql(V.fromArray(.{ 1, 2, 3 }), 1e-6)); + try testing.expectEqual(@as(T, 0.25), m.points[0].penetration); + try testing.expectEqual(@as(u32, 0x0003_0007), m.points[0].feature_id); + try testing.expect(m.points[1].position.approxEql(V.fromArray(.{ -1, 0, 4 }), 1e-6)); + try testing.expectEqual(@as(T, 0.5), m.points[1].penetration); + try testing.expectEqual(@as(u32, 0x0002_0005), m.points[1].feature_id); + + // Generic over the scalar: the same types instantiate at f64. + const M64 = ContactManifold(f64); + const V64 = math.Vec(3, f64); + const m64 = M64{ + .normal = V64.unit_y, + .points = .{ + ContactPoint(f64){ .position = V64.zero, .penetration = 1.5, .feature_id = 1 }, + ContactPoint(f64){ .position = V64.zero, .penetration = 0, .feature_id = 0 }, + ContactPoint(f64){ .position = V64.zero, .penetration = 0, .feature_id = 0 }, + ContactPoint(f64){ .position = V64.zero, .penetration = 0, .feature_id = 0 }, + }, + .count = 1, + }; + try testing.expectEqual(@as(f64, 1.5), m64.points[0].penetration); + try testing.expect(m64.normal.approxEql(V64.unit_y, 1e-12)); +} diff --git a/src/modules/forge/forge_3d/pipeline/narrowphase/root.zig b/src/modules/forge/forge_3d/pipeline/narrowphase/root.zig new file mode 100644 index 0000000..909d840 --- /dev/null +++ b/src/modules/forge/forge_3d/pipeline/narrowphase/root.zig @@ -0,0 +1,73 @@ +//! `forge_3d/pipeline/narrowphase/root.zig` — the narrowphase package facade. +//! +//! M1.1.3/E1 promoted the single-file `narrowphase.zig` to this package so the +//! GJK stack (`support.zig` + `gjk.zig`), EPA (`epa.zig`), and the contact +//! manifold (`manifold.zig`) live as sibling files without one 1300-line file. +//! This root re-exports every public name the old file exposed — unchanged — so +//! consumers change only their import path (`pipeline/narrowphase.zig` → +//! `pipeline/narrowphase/root.zig`). +//! +//! **Dependency discipline (brief Notes).** The whole package imports +//! `foundation` (math) ONLY — never `weld_forge`, never `body*.zig`, never +//! `config.zig`, never `broadphase.zig`. The scalar arrives as the comptime +//! parameter `T`; `forge_3d` instantiates it at `config.Real`. + +const support = @import("support.zig"); +const gjk_mod = @import("gjk.zig"); +const epa_mod = @import("epa.zig"); +const manifold = @import("manifold.zig"); + +// --- Support layer (support.zig) --- + +/// A convex support shape (core + inflation radius). +pub const SupportShape = support.SupportShape; +/// Shape-B-relative-to-A pose precompute (frame of A). +pub const RelativePose = support.RelativePose; +/// A support sample on the Minkowski difference (`w` + the two source supports). +pub const Vertex = support.Vertex; +/// A shape's supporting feature (vertex / segment / quad) in a direction. +pub const Face = support.Face; +/// Support point of the Minkowski difference of two cores (`pub` for EPA/manifold). +pub const minkowskiSupport = support.minkowskiSupport; + +// --- GJK layer (gjk.zig) --- + +/// GJK simplex machinery (triplet vertex + Voronoi-region origin-closest solver). +pub const Simplex = gjk_mod.Simplex; +/// Three-regime GJK result (separated / shallow / deep). +pub const GjkResult = gjk_mod.GjkResult; +/// The GJK descent iteration ceiling (scalar-independent). +pub const max_gjk_iterations = gjk_mod.max_gjk_iterations; +/// Distance-based GJK between two support shapes at their world poses. +pub const gjk = gjk_mod.gjk; + +// --- EPA (penetration axis + core depth, epa.zig) --- + +/// EPA penetration result (world normal A→B, core depth, world closest points). +pub const EpaResult = epa_mod.EpaResult; +/// The EPA expansion iteration ceiling (scalar-independent). +pub const max_epa_iterations = epa_mod.max_epa_iterations; +/// EPA over a `.deep` GJK seed — penetration axis + core depth. +pub const epa = epa_mod.epa; + +// --- Contact manifold (manifold.zig) --- + +/// The contact manifold between two shapes (world normal + up to 4 points). +pub const ContactManifold = manifold.ContactManifold; +/// One contact point of a `ContactManifold`. +pub const ContactPoint = manifold.ContactPoint; +/// Full narrowphase (GJK → shallow/deep manifold) between two shapes; null when +/// separated. Order-independent (pose-canonicalized). +pub const collide = manifold.collide; +/// `collide` for a FIXED shape order (no pose canonicalization) — for callers +/// that own a stable external key (body ids) and need a frame-stable feature_id. +pub const collideOrdered = manifold.collideOrdered; + +// Pins so every package sub-file is analysed when forge_3d is built as a test +// target (engine-zig-conventions.md §13 lazy-analysis guard). +comptime { + _ = support; + _ = gjk_mod; + _ = epa_mod; + _ = manifold; +} diff --git a/src/modules/forge/forge_3d/pipeline/narrowphase/support.zig b/src/modules/forge/forge_3d/pipeline/narrowphase/support.zig new file mode 100644 index 0000000..77b4d83 --- /dev/null +++ b/src/modules/forge/forge_3d/pipeline/narrowphase/support.zig @@ -0,0 +1,377 @@ +//! `forge_3d/pipeline/narrowphase/support.zig` — the shared geometry base of +//! the narrowphase package: the convex support shapes, the frame-of-A relative +//! pose, the Minkowski-difference support sample, and (M1.1.3) the supporting +//! feature used by the contact-manifold clipper. +//! +//! Relocated verbatim from the single-file `narrowphase.zig` at M1.1.3/E1 +//! (`SupportShape(T)`, `RelativePose(T)`, and the `minkowskiSupport` free +//! function — promoted to `pub` here because the GJK loop, EPA, and the manifold +//! generator all consume it). GJK's own machinery (`Simplex`, the descent loop, +//! `GjkResult`) lives in the sibling `gjk.zig`. +//! +//! **Dependency discipline (brief Notes).** This file imports `foundation` +//! (math) ONLY — never `weld_forge`, never `body*.zig`, never `config.zig`, +//! never `broadphase.zig`. The scalar arrives as the comptime parameter `T`; +//! `forge_3d` instantiates it at `config.Real`. The `Shape → SupportShape` +//! conversion lives on the forge_3d side (`shape.zig`). +//! +//! **Cores + inflation radius (Jolt convex-radius architecture).** GJK/EPA never +//! see a curved surface: a sphere is a point, a capsule a segment, a box the +//! full box (radius 0). Support functions run on the convex **core**; the +//! inflation `radius` is applied downstream (touch test in `gjk.zig`; contact +//! point placement in `manifold.zig`). +//! +//! **Computation in the frame of A (brief Notes).** B is pre-transformed +//! relative to A once per pair (`RelativePose`); A's support runs untransformed. +//! Better precision far from the world origin and half the per-iteration +//! transforms. Frozen (M1.1.14): changing the computation frame would break +//! validated bit-exactness. + +const std = @import("std"); +const math = @import("foundation").math; + +/// One support sample on the Minkowski difference of the two cores: the +/// difference point `w = support_a − support_b` together with the two supports +/// it came from (all in A's frame). Storing all three — rather than just `w` — +/// lets a consumer reconstruct the closest points on A and B from barycentric +/// weights (`closest_a = Σ λ_i · support_a_i`, likewise for B). The GJK simplex +/// (`gjk.zig`) and the EPA polytope (`epa.zig`) are both built from these; +/// `Simplex(T).Vertex` and `GjkResult(T).Vertex` alias this type. +pub fn Vertex(comptime T: type) type { + return struct { + /// Minkowski-difference point `support_a − support_b` (A's frame). + w: math.Vec(3, T), + /// Support point on A's core (A's frame). + support_a: math.Vec(3, T), + /// Support point on B's core (A's frame). + support_b: math.Vec(3, T), + }; +} + +/// The supporting **feature** of a core in a given direction: the polygon (or +/// edge / vertex) whose points maximize `dir · p` on the core, in the core's +/// local frame, radius excluded. Up to 4 vertices (a box face); `count` gives +/// the valid prefix (point → 1, segment → 1 or 2, box → 4). The contact-manifold +/// clipper (M1.1.3) takes `supportingFace(+n)` on A and `supportingFace(−n)` on +/// B — where `n` is the contact normal A→B — and clips one against the other. +/// +/// `face_id` and `vert_ids` carry STABLE, translation-invariant local feature +/// identities (a box vertex id is its sign-pattern 0..7, a box face id is +/// `axis·2 + sign`) so the manifold's `feature_id` survives a small pose change +/// for M1.1.6 warm-starting (frame-stability is the point — a clip-buffer index +/// is not stable). +pub fn Face(comptime T: type) type { + return struct { + /// The feature vertices; only `[0..count]` is meaningful. + verts: [4]math.Vec(3, T), + /// Stable local id of each vertex (aligned to `verts`): a box corner's + /// sign pattern 0..7; a segment endpoint 0 (+Y) / 1 (−Y); a point 0. + vert_ids: [4]u8, + /// Number of valid vertices (1..4). + count: u8, + /// Stable local id of the feature itself: box face `axis·2 + (sign<0)` + /// (0..5); segment 6; point 7. + face_id: u8, + }; +} + +/// A convex support shape: a convex **core** (point / segment / box) plus an +/// inflation `radius` around it (the Jolt convex-radius architecture). GJK/EPA +/// run on the core alone and never see the inflated surface. `radius` is 0 for a +/// box in M1.1.2/3. Future shapes (cylinder, convex hull) are new `Core` cases, +/// purely additive. +pub fn SupportShape(comptime T: type) type { + return struct { + const Self = @This(); + const Vec3T = math.Vec(3, T); + + /// The convex core geometry, in the shape's own local frame. + core: Core, + /// Inflation radius around the core (0 for box in M1.1.2/3). + radius: T, + + /// The convex core of a support shape, in its local frame. + pub const Core = union(enum) { + /// Sphere core: the local origin. + point, + /// Capsule core: Y-axis segment, ±half_height. + segment: T, + /// Box core: half-extents (full box, no shrink). + box: math.Vec(3, T), + }; + + /// Support point of the **core** (the inflation radius is excluded), in + /// the local frame: the core point that maximizes `dir · p`. `dir` need + /// not be normalized. + /// + /// Tie-breaks are fixed for determinism (brief Notes): a capsule + /// direction with `dir.y == 0` selects the `+Y` endpoint; a box + /// direction component `== 0` selects the `+half_extent` corner + /// component. Both fall out of the `>= 0` comparisons below. + pub fn support(self: Self, dir: Vec3T) Vec3T { + switch (self.core) { + .point => return Vec3T.zero, + .segment => |half_height| { + // Segment lies on the Y axis; only the sign of dir.y counts. + const dy = dir.toArray()[1]; + const y = if (dy >= 0) half_height else -half_height; + return Vec3T.fromArray(.{ 0, y, 0 }); + }, + .box => |half_extents| { + // Separable per axis: p_i = sign(dir_i) · half_extent_i. + const d = dir.toArray(); + const he = half_extents.toArray(); + return Vec3T.fromArray(.{ + if (d[0] >= 0) he[0] else -he[0], + if (d[1] >= 0) he[1] else -he[1], + if (d[2] >= 0) he[2] else -he[2], + }); + }, + } + } + + /// The supporting **feature** of the core in direction `dir` (local + /// frame, radius excluded): the vertex / segment / quad whose points + /// maximize `dir · p`. Contains `support(dir)`; the manifold clipper + /// consumes it (see `Face`). `dir` need not be normalized. + /// + /// Per core: a point core is always the single origin vertex; a segment + /// core is the single extremal endpoint when `dir` is essentially along + /// its ±Y axis (`perp² ≤ aligned_rel · |dir|²`), else the whole segment + /// (a line contact); a box core is the winning quad — the face on the + /// dominant axis of `dir` (first-index tie-break, mirroring `support`'s + /// `>= 0` sign choice), wound CCW around its outward normal. + pub fn supportingFace(self: Self, dir: Vec3T) Face(T) { + const zero = Vec3T.zero; + switch (self.core) { + .point => return .{ .verts = .{ zero, zero, zero, zero }, .vert_ids = .{ 0, 0, 0, 0 }, .count = 1, .face_id = 7 }, + .segment => |half_height| { + const d = dir.toArray(); + const perp_sq = d[0] * d[0] + d[2] * d[2]; + const total_sq = perp_sq + d[1] * d[1]; + // Essentially end-on ⇒ the single extremal endpoint (like + // `support`, +Y on the `dir.y == 0` tie); else the segment. + const aligned_rel: T = if (T == f32) 1.0e-6 else 1.0e-12; + const plus = Vec3T.fromArray(.{ 0, half_height, 0 }); + const minus = Vec3T.fromArray(.{ 0, -half_height, 0 }); + if (perp_sq <= aligned_rel * total_sq) { + const plus_end = d[1] >= 0; + const endpoint = if (plus_end) plus else minus; + return .{ .verts = .{ endpoint, zero, zero, zero }, .vert_ids = .{ if (plus_end) 0 else 1, 0, 0, 0 }, .count = 1, .face_id = 6 }; + } + return .{ .verts = .{ plus, minus, zero, zero }, .vert_ids = .{ 0, 1, 0, 0 }, .count = 2, .face_id = 6 }; + }, + .box => |half_extents| { + const d = dir.toArray(); + const he = half_extents.toArray(); + // Dominant axis of `dir` (first-index tie-break: strict `>`). + var k: usize = 0; + var best = @abs(d[0]); + if (@abs(d[1]) > best) { + k = 1; + best = @abs(d[1]); + } + if (@abs(d[2]) > best) k = 2; + // Face sign (`>= 0` tie to `+`, mirroring `support`). + const s: T = if (d[k] >= 0) 1 else -1; + const u = (k + 1) % 3; + const v = (k + 2) % 3; + // CCW winding around the outward normal `s·e_k`: for `s = +1` + // the in-plane loop (−u,−v)→(+u,−v)→(+u,+v)→(−u,+v) is CCW + // (since `e_u × e_v = e_k`); for `s = −1` the outward normal + // flips, so the loop is reversed. + const loop: [4][2]T = if (s >= 0) + .{ .{ -1, -1 }, .{ 1, -1 }, .{ 1, 1 }, .{ -1, 1 } } + else + .{ .{ -1, -1 }, .{ -1, 1 }, .{ 1, 1 }, .{ 1, -1 } }; + var face: Face(T) = .{ .verts = undefined, .vert_ids = undefined, .count = 4, .face_id = @intCast(k * 2 + @intFromBool(s < 0)) }; + for (loop, 0..) |o, i| { + var c: [3]T = undefined; + c[k] = s * he[k]; + c[u] = o[0] * he[u]; + c[v] = o[1] * he[v]; + face.verts[i] = Vec3T.fromArray(c); + // Stable corner id: one bit per axis, set when the sign is +. + var sgn: [3]T = undefined; + sgn[k] = s; + sgn[u] = o[0]; + sgn[v] = o[1]; + face.vert_ids[i] = (@as(u8, @intFromBool(sgn[0] > 0))) | (@as(u8, @intFromBool(sgn[1] > 0)) << 1) | (@as(u8, @intFromBool(sgn[2] > 0)) << 2); + } + return face; + }, + } + } + }; +} + +/// Shape B's pose relative to shape A, precomputed once per pair so GJK/EPA run +/// entirely in A's frame (A's support is untransformed; only B's support pays a +/// transform). See the file header for why the frame of A is frozen. +pub fn RelativePose(comptime T: type) type { + return struct { + const Self = @This(); + const Vec3T = math.Vec(3, T); + const QuatT = math.Quat(T); + const SupportShapeT = SupportShape(T); + + /// Orientation of B expressed in A's frame: `conj(rot_a) · rot_b`. + rot_rel: QuatT, + /// Origin of B expressed in A's frame: `conj(rot_a) · (pos_b − pos_a)`. + pos_rel: Vec3T, + + /// Precompute B-relative-to-A from the two world poses. Uses the + /// conjugate (the unit-quaternion inverse) throughout — never + /// `inverse()`, which divides by the squared norm on the hot path + /// (brief Notes). + pub fn init(pos_a: Vec3T, rot_a: QuatT, pos_b: Vec3T, rot_b: QuatT) Self { + const inv_a = rot_a.conjugate(); + return .{ + .rot_rel = inv_a.mul(rot_b), + .pos_rel = inv_a.rotateVec3(pos_b.sub(pos_a)), + }; + } + + /// Support point of shape B's core, expressed in A's frame, for a + /// direction `dir` given in A's frame. The direction is inverse-rotated + /// into B's local frame (`conj(rot_rel)`), B's local support is taken, + /// then the result is mapped back into A's frame. + pub fn supportB(self: Self, shape_b: SupportShapeT, dir: Vec3T) Vec3T { + const local_dir = self.rot_rel.conjugate().rotateVec3(dir); + const p_local = shape_b.support(local_dir); + return self.rot_rel.rotateVec3(p_local).add(self.pos_rel); + } + + /// Supporting **feature** of shape B's core, expressed in A's frame, for + /// a direction `dir` given in A's frame (mirror of `supportB`, at the + /// feature grain). The direction is inverse-rotated into B's local frame, + /// B's local `supportingFace` is taken, then each vertex is mapped back + /// into A's frame; the winding is preserved (a proper rotation). + pub fn supportingFaceB(self: Self, shape_b: SupportShapeT, dir: Vec3T) Face(T) { + const local_dir = self.rot_rel.conjugate().rotateVec3(dir); + const face_local = shape_b.supportingFace(local_dir); + // The rotation preserves feature identity, so `vert_ids`/`face_id` + // carry through unchanged. + var out: Face(T) = .{ .verts = .{ Vec3T.zero, Vec3T.zero, Vec3T.zero, Vec3T.zero }, .vert_ids = face_local.vert_ids, .count = face_local.count, .face_id = face_local.face_id }; + for (0..face_local.count) |i| { + out.verts[i] = self.rot_rel.rotateVec3(face_local.verts[i]).add(self.pos_rel); + } + return out; + } + }; +} + +/// Support point of the Minkowski difference of the two cores in direction +/// `dir` (A's frame): `support_A(dir) − support_B(−dir)`, keeping both supports +/// so the closest points on A and B stay reconstructible. `pub` because both the +/// GJK descent (`gjk.zig`) and the EPA / manifold generators (M1.1.3) consume it. +pub fn minkowskiSupport( + comptime T: type, + shape_a: SupportShape(T), + relpose: RelativePose(T), + shape_b: SupportShape(T), + dir: math.Vec(3, T), +) Vertex(T) { + const sa = shape_a.support(dir); + const sb = relpose.supportB(shape_b, dir.neg()); + return .{ .w = sa.sub(sb), .support_a = sa, .support_b = sb }; +} + +const testing = std.testing; + +test "supporting face returns the correct feature per core" { + const T = f32; + const SS = SupportShape(T); + const V = math.Vec(3, T); + const Q = math.Quat(T); + const tol: T = 1e-6; + const vf = struct { + fn f(x: T, y: T, z: T) V { + return V.fromArray(.{ x, y, z }); + } + }.f; + // Whether `p` is among the face's first `count` vertices. + const inFace = struct { + fn f(face: Face(T), p: V) bool { + for (0..face.count) |i| { + if (face.verts[i].approxEql(p, tol)) return true; + } + return false; + } + }.f; + + // Point core → always the single origin vertex, any direction. + const p = SS{ .core = .point, .radius = 0.5 }; + { + const f = p.supportingFace(vf(1, 2, -3)); + try testing.expectEqual(@as(u8, 1), f.count); + try testing.expect(f.verts[0].approxEql(V.zero, tol)); + } + + // Segment core: end-on (dir ≈ ±Y) → the single extremal endpoint; oblique + // (a real perpendicular component) → the whole segment (line contact, 2). + const seg = SS{ .core = .{ .segment = 0.9 }, .radius = 0.3 }; + { + const up = seg.supportingFace(vf(0, 1, 0)); + try testing.expectEqual(@as(u8, 1), up.count); + try testing.expect(up.verts[0].approxEql(vf(0, 0.9, 0), tol)); + + const down = seg.supportingFace(vf(0, -1, 0)); + try testing.expectEqual(@as(u8, 1), down.count); + try testing.expect(down.verts[0].approxEql(vf(0, -0.9, 0), tol)); + + // A near-vertical direction is still end-on ⇒ single endpoint. + const near = seg.supportingFace(vf(0.0001, 1, 0)); + try testing.expectEqual(@as(u8, 1), near.count); + + // Perpendicular and oblique ⇒ the whole segment. + const side = seg.supportingFace(vf(1, 0, 0)); + try testing.expectEqual(@as(u8, 2), side.count); + try testing.expect(inFace(side, vf(0, 0.9, 0)) and inFace(side, vf(0, -0.9, 0))); + const obl = seg.supportingFace(vf(1, 0.2, -0.5)); + try testing.expectEqual(@as(u8, 2), obl.count); + // The feature contains `support(dir)` (the +Y endpoint on the `dir.y == 0` tie). + try testing.expect(inFace(seg.supportingFace(vf(1, 0, 0)), seg.support(vf(1, 0, 0)))); + } + + // Box core → the winning quad (4) on the dominant axis; support(dir) ∈ face; + // all four verts share the dominant-axis coordinate (one face). + const box = SS{ .core = .{ .box = vf(1, 2, 3) }, .radius = 0 }; + { + const dirs = [_]V{ vf(1, 0, 0), vf(-1, 0, 0), vf(0, 1, 0), vf(0, 0, -1), vf(0.9, 0.1, -0.2), vf(1, 1, 0) }; + for (dirs) |d| { + const f = box.supportingFace(d); + try testing.expectEqual(@as(u8, 4), f.count); + try testing.expect(inFace(f, box.support(d))); + // The four verts are coplanar on the winning face: the dominant axis + // coordinate is identical across all four. + const da = d.toArray(); + var k: usize = 0; + var m = @abs(da[0]); + if (@abs(da[1]) > m) { + k = 1; + m = @abs(da[1]); + } + if (@abs(da[2]) > m) k = 2; + const c0 = f.verts[0].toArray()[k]; + for (0..4) |i| try testing.expectApproxEqAbs(c0, f.verts[i].toArray()[k], tol); + } + } + + // Through a non-trivial relative rotation (supportingFaceB): B is `box`, + // rotated +90° about +Z. For dir=+X in A's frame the count is preserved (4) + // and the feature contains `supportB(box, +X)`. + const rp = RelativePose(T).init(V.zero, Q.identity, V.zero, Q.fromAxisAngle(V.unit_z, std.math.pi / 2.0)); + { + const f = rp.supportingFaceB(box, vf(1, 0, 0)); + try testing.expectEqual(@as(u8, 4), f.count); + try testing.expect(inFace(f, rp.supportB(box, vf(1, 0, 0)))); + // Segment B through the same rotation: perpendicular dir ⇒ 2-vert segment. + const seg_face = rp.supportingFaceB(seg, vf(0, 1, 0)); + try testing.expectEqual(@as(u8, 2), seg_face.count); + } + + // Byte-stable across runs: pure sign/threshold selection, no float noise. + try testing.expect(box.supportingFace(vf(0.9, 0.1, -0.2)).verts[2] + .eql(box.supportingFace(vf(0.9, 0.1, -0.2)).verts[2])); +} diff --git a/src/modules/forge/forge_3d/root.zig b/src/modules/forge/forge_3d/root.zig index 4b63812..462e647 100644 --- a/src/modules/forge/forge_3d/root.zig +++ b/src/modules/forge/forge_3d/root.zig @@ -2,11 +2,12 @@ //! `engine-spec.md` §3.5). M1.1.0 laid the foundations: the `Real` scalar, the //! `ShapeStore`, per-body `MotionProperties` with analytic inertia, and the SoA //! `BodyManager`. M1.1.1 added the shared `pipeline/broadphase.zig` (a dynamic -//! multi-layer AABB tree — BVH); M1.1.2 adds `pipeline/narrowphase.zig` -//! (distance-based GJK convex detection). Both are re-exported here at `Real`. -//! No stepping, EPA, island manager, scheduler, or `PhysicsModule` instantiation -//! yet — those are later M1.1 sub-milestones. Depends only on `foundation/math` -//! and `src/modules/forge/api/` (core entity/component types reach here through +//! multi-layer AABB tree — BVH); M1.1.2 added `pipeline/narrowphase/` +//! (distance-based GJK convex detection), promoted to a package at M1.1.3 with +//! EPA + contact manifold as sibling files. All re-exported here at `Real`. No +//! stepping, island manager, scheduler, or `PhysicsModule` instantiation yet — +//! those are later M1.1 sub-milestones. Depends only on `foundation/math` and +//! `src/modules/forge/api/` (core entity/component types reach here through //! `api/`). const config = @import("config.zig"); @@ -14,10 +15,11 @@ const shape = @import("shape.zig"); const body = @import("body.zig"); const body_manager = @import("body_manager.zig"); const broadphase = @import("pipeline/broadphase.zig"); -// M1.1.2 — narrowphase (GJK convex detection). Re-exported at `Real` below; the -// comptime pin analyses its acceptance tests (engine-zig-conventions.md §13 -// lazy-analysis guard — an unreferenced module's tests are silently skipped). -const narrowphase = @import("pipeline/narrowphase.zig"); +// M1.1.2/3 — narrowphase package (GJK convex detection; EPA + manifold M1.1.3). +// Re-exported at `Real` below; the comptime pin analyses its acceptance tests +// (engine-zig-conventions.md §13 lazy-analysis guard — an unreferenced module's +// tests are silently skipped). +const narrowphase = @import("pipeline/narrowphase/root.zig"); // --- Solver scalar + math aliases --- @@ -82,6 +84,40 @@ pub fn gjk(shape_a: SupportShape, pos_a: Vec3r, rot_a: Quatr, shape_b: SupportSh return narrowphase.gjk(Real, shape_a, pos_a, rot_a, shape_b, pos_b, rot_b); } +// --- Narrowphase (EPA penetration + contact manifold) --- + +/// EPA penetration result (world normal A→B, core depth, world closest points) +/// at solver precision. +pub const EpaResult = narrowphase.EpaResult(Real); +/// The EPA expansion iteration ceiling (scalar-independent). +pub const max_epa_iterations = narrowphase.max_epa_iterations; +/// The contact manifold between two shapes (world normal + up to 4 points) at +/// solver precision. +pub const ContactManifold = narrowphase.ContactManifold(Real); +/// One contact point of a `ContactManifold` at solver precision. +pub const ContactPoint = narrowphase.ContactPoint(Real); + +/// EPA over a `.deep` GJK seed — penetration axis + core depth. The `Real`-bound +/// entry; `collide` runs it internally on the deep path. +pub fn epa(shape_a: SupportShape, pos_a: Vec3r, rot_a: Quatr, relpose: RelativePose, shape_b: SupportShape, seed: GjkResult) EpaResult { + return narrowphase.epa(Real, shape_a, pos_a, rot_a, relpose, shape_b, seed); +} + +/// Full narrowphase (GJK → shallow/deep contact manifold) between two support +/// shapes at their world poses — the `Real`-bound entry; null when separated. +/// `BodyManager.collidePair` is the `BodyId`-level adapter for the +/// broadphase→narrowphase flow. Order-independent. +pub fn collide(shape_a: SupportShape, pos_a: Vec3r, rot_a: Quatr, shape_b: SupportShape, pos_b: Vec3r, rot_b: Quatr) ?ContactManifold { + return narrowphase.collide(Real, shape_a, pos_a, rot_a, shape_b, pos_b, rot_b); +} + +/// `collide` for a FIXED shape order (no pose canonicalization) at solver +/// precision — the `BodyId`-ordered path `BodyManager.collidePair` drives so the +/// `feature_id` reference/incident ownership stays frame-stable. +pub fn collideOrdered(shape_a: SupportShape, pos_a: Vec3r, rot_a: Quatr, shape_b: SupportShape, pos_b: Vec3r, rot_b: Quatr) ?ContactManifold { + return narrowphase.collideOrdered(Real, shape_a, pos_a, rot_a, shape_b, pos_b, rot_b); +} + // Pins so the inline tests + the acceptance suite are analysed when this module // is built as a test target (engine-zig-conventions.md §13). comptime { @@ -93,5 +129,7 @@ comptime { _ = narrowphase; _ = @import("tests/body_manager_test.zig"); _ = @import("tests/broadphase_test.zig"); - _ = @import("tests/narrowphase_test.zig"); + _ = @import("tests/gjk_test.zig"); + _ = @import("tests/epa_test.zig"); + _ = @import("tests/manifold_test.zig"); } diff --git a/src/modules/forge/forge_3d/shape.zig b/src/modules/forge/forge_3d/shape.zig index dea66cd..e5d974c 100644 --- a/src/modules/forge/forge_3d/shape.zig +++ b/src/modules/forge/forge_3d/shape.zig @@ -9,7 +9,7 @@ const std = @import("std"); const api = @import("weld_forge"); const config = @import("config.zig"); -const narrowphase = @import("pipeline/narrowphase.zig"); +const narrowphase = @import("pipeline/narrowphase/root.zig"); const IdAllocator = @import("slot_alloc.zig").IdAllocator; const Real = config.Real; diff --git a/src/modules/forge/forge_3d/tests/epa_test.zig b/src/modules/forge/forge_3d/tests/epa_test.zig new file mode 100644 index 0000000..77257c7 --- /dev/null +++ b/src/modules/forge/forge_3d/tests/epa_test.zig @@ -0,0 +1,209 @@ +//! M1.1.3/E2 acceptance suite for the forge_3d narrowphase EPA (penetration axis +//! + core depth over the GJK terminal `.deep` simplex). Keyed to `config.Real` +//! so `-Dphysics_f64=true` sweeps the whole suite at f64 (local). + +const std = @import("std"); +const config = @import("../config.zig"); +const narrowphase = @import("../pipeline/narrowphase/root.zig"); +const math = @import("foundation").math; + +const Real = config.Real; +const Vec3r = config.Vec3r; +const Quatr = config.Quatr; +const SupportShape = narrowphase.SupportShape(Real); +const RelativePose = narrowphase.RelativePose(Real); +const GjkResult = narrowphase.GjkResult(Real); +const EpaResult = narrowphase.EpaResult(Real); +const testing = std.testing; + +/// Solver-precision Vec3 from literals. +fn vr(x: Real, y: Real, z: Real) Vec3r { + return Vec3r.fromArray(.{ x, y, z }); +} + +fn sphereShape(radius: Real) SupportShape { + return .{ .core = .point, .radius = radius }; +} +fn capsuleShape(half_height: Real, radius: Real) SupportShape { + return .{ .core = .{ .segment = half_height }, .radius = radius }; +} +fn boxShape(hx: Real, hy: Real, hz: Real) SupportShape { + return .{ .core = .{ .box = vr(hx, hy, hz) }, .radius = 0 }; +} + +/// Distance tolerance for EPA results (convergence + f32 + oblique rotations +/// accumulate; f64 passes far tighter — mirrors the GJK suite's `gjk_test_tol`). +const epa_tol: Real = 1.0e-3; + +fn finite3(v: Vec3r) bool { + const a = v.toArray(); + return std.math.isFinite(a[0]) and std.math.isFinite(a[1]) and std.math.isFinite(a[2]); +} + +/// Run GJK then EPA on a pair, mirroring the E4 `collide` deep path. The caller +/// guarantees (or asserts) the pair is `.deep`. +fn deepEpa(sa: SupportShape, pa: Vec3r, ra: Quatr, sb: SupportShape, pb: Vec3r, rb: Quatr) EpaResult { + const relpose = RelativePose.init(pa, ra, pb, rb); + const g = narrowphase.gjk(Real, sa, pa, ra, sb, pb, rb); + return narrowphase.epa(Real, sa, pa, ra, relpose, sb, g); +} + +test "epa penetration axis and depth match analytic" { + // A single oblique global rigid transform applied to both bodies leaves the + // (rotation-invariant) core depth unchanged and rotates the normal — the + // oriented coverage without losing the analytic value. + const g_rot = Quatr.fromAxisAngle(vr(1, 2, 3).normalize(), 0.7); + const g_trans = vr(-4, 7, 2); + + const Combo = struct { + sa: SupportShape, + pa: Vec3r, + sb: SupportShape, + pb: Vec3r, + normal: Vec3r, // expected A->B normal (canonical, identity rotations) + depth: Real, + }; + const combos = [_]Combo{ + // sphere/box: point core 0.6 inside the +Z face of a unit box (A = box). + .{ .sa = boxShape(1, 1, 1), .pa = vr(0, 0, 0), .sb = sphereShape(0.5), .pb = vr(0, 0, 0.4), .normal = vr(0, 0, 1), .depth = 0.6 }, + // sphere/box, other axis (A = box), nearest +X face. + .{ .sa = boxShape(1, 1, 1), .pa = vr(0, 0, 0), .sb = sphereShape(0.5), .pb = vr(0.3, 0, 0), .normal = vr(1, 0, 0), .depth = 0.7 }, + // box/box axis-aligned overlap 0.5 along X (A at origin, B at +1.5 X). + .{ .sa = boxShape(1, 1, 1), .pa = vr(0, 0, 0), .sb = boxShape(1, 1, 1), .pb = vr(1.5, 0, 0), .normal = vr(1, 0, 0), .depth = 0.5 }, + // box/capsule: segment core inside the box, nearest +X face at 0.5. + .{ .sa = boxShape(1, 1, 1), .pa = vr(0, 0, 0), .sb = capsuleShape(0.5, 0.3), .pb = vr(0.5, 0, 0), .normal = vr(1, 0, 0), .depth = 0.5 }, + }; + + for (combos) |c| { + // Canonical (identity rotations). + { + const r = deepEpa(c.sa, c.pa, Quatr.identity, c.sb, c.pb, Quatr.identity); + try testing.expectApproxEqAbs(c.depth, r.depth, epa_tol); + try testing.expect(r.normal.approxEql(c.normal, epa_tol)); + // Unit normal, finite closest points. + try testing.expectApproxEqAbs(@as(Real, 1), r.normal.length(), epa_tol); + try testing.expect(finite3(r.closest_a) and finite3(r.closest_b)); + } + // Same scene under the oblique global transform ⇒ same depth, rotated normal. + { + const r = deepEpa( + c.sa, + g_rot.rotateVec3(c.pa).add(g_trans), + g_rot, + c.sb, + g_rot.rotateVec3(c.pb).add(g_trans), + g_rot, + ); + try testing.expectApproxEqAbs(c.depth, r.depth, epa_tol); + try testing.expect(r.normal.approxEql(g_rot.rotateVec3(c.normal), epa_tol)); + } + } +} + +test "epa is order-independent" { + // Non-degenerate deep pairs: epa(A,B) and epa(B,A) give opposite world normals + // and equal depth (the closest face of B⊖A is the antipode of A⊖B's). + const Combo = struct { sa: SupportShape, pa: Vec3r, sb: SupportShape, pb: Vec3r }; + const combos = [_]Combo{ + .{ .sa = boxShape(1, 1, 1), .pa = vr(0, 0, 0), .sb = sphereShape(0.5), .pb = vr(0, 0, 0.4) }, + .{ .sa = boxShape(1, 1, 1), .pa = vr(0, 0, 0), .sb = boxShape(1, 1, 1), .pb = vr(1.5, 0, 0) }, + .{ .sa = boxShape(1, 1, 1), .pa = vr(0, 0, 0), .sb = capsuleShape(0.5, 0.3), .pb = vr(0.5, 0, 0) }, + }; + const g_rot = Quatr.fromAxisAngle(vr(2, -1, 4).normalize(), 1.1); + for (combos) |c| { + for ([_]Quatr{ Quatr.identity, g_rot }) |ra| { + const rb = ra; + const ab = deepEpa(c.sa, c.pa, ra, c.sb, c.pb, rb); + const ba = deepEpa(c.sb, c.pb, rb, c.sa, c.pa, ra); + try testing.expectApproxEqAbs(ab.depth, ba.depth, epa_tol); + try testing.expect(ab.normal.approxEql(ba.normal.neg(), epa_tol)); + } + } +} + +test "epa expands a low-dimensional deep seed" { + // Crossing segment cores (two capsules, B rotated onto the X axis, both at the + // origin): the Minkowski difference of two segments is planar, so the GJK deep + // seed is < 4 vertices and cannot tetra-expand to a 3-D polytope. EPA must + // still converge (bounded) to a sane touching result: a unit separation + // normal (perpendicular to both segments) and core depth 0. + const rot_z90 = Quatr.fromAxisAngle(Vec3r.unit_z, std.math.pi / 2.0); + const a = capsuleShape(1, 0.3); + const b = capsuleShape(1, 0.3); + + const g = narrowphase.gjk(Real, a, vr(0, 0, 0), Quatr.identity, b, vr(0, 0, 0), rot_z90); + try testing.expectEqual(GjkResult.Status.deep, g.status); + try testing.expect(g.simplex_count < 4); // the flag-6 low-dimensional seed + + const r = deepEpa(a, vr(0, 0, 0), Quatr.identity, b, vr(0, 0, 0), rot_z90); + // Converged to a sane touching result. + try testing.expect(finite3(r.normal) and finite3(r.closest_a) and finite3(r.closest_b)); + try testing.expectApproxEqAbs(@as(Real, 1), r.normal.length(), epa_tol); // unit + try testing.expectApproxEqAbs(@as(Real, 0), r.depth, epa_tol); // touching cores + // Perpendicular to both segment axes (A along Y, B along X ⇒ normal along ±Z). + try testing.expectApproxEqAbs(@as(Real, 1), @abs(r.normal.toArray()[2]), epa_tol); + + // Point-on-segment (sphere centre on the capsule axis) is likewise a low-dim + // seed; EPA converges to depth 0 with a unit radial normal. + const g2 = narrowphase.gjk(Real, a, vr(0, 0, 0), Quatr.identity, sphereShape(0.5), vr(0, 0.3, 0), Quatr.identity); + try testing.expectEqual(GjkResult.Status.deep, g2.status); + try testing.expect(g2.simplex_count < 4); + const r2 = deepEpa(a, vr(0, 0, 0), Quatr.identity, sphereShape(0.5), vr(0, 0.3, 0), Quatr.identity); + try testing.expect(finite3(r2.normal)); + try testing.expectApproxEqAbs(@as(Real, 1), r2.normal.length(), epa_tol); + try testing.expectApproxEqAbs(@as(Real, 0), r2.depth, epa_tol); +} + +test "epa handles eccentric thin-box deep overlaps" { + // Adversarial-review regime (finding 1): an eccentric (thin) box whose deep + // overlap makes the Minkowski polytope long/flat, so the loop expands over + // several iterations and the fan-face winding (inherited from the horizon, + // not reoriented against the near-coplanar interior) is exercised. Two thin + // boxes overlap 0.1 along Y (huge overlap along X and Z), so the min- + // penetration axis is +Y at depth 0.1. Canonical and globally rotated. + const g_rot = Quatr.fromAxisAngle(vr(1, -2, 3).normalize(), 0.9); + const g_trans = vr(3, -5, 2); + const thin = boxShape(5, 0.3, 4); + { + const r = deepEpa(thin, vr(0, 0, 0), Quatr.identity, thin, vr(0, 0.5, 0), Quatr.identity); + try testing.expectApproxEqAbs(@as(Real, 0.1), r.depth, epa_tol); + try testing.expect(r.normal.approxEql(vr(0, 1, 0), epa_tol)); + } + { + const r = deepEpa( + thin, + g_rot.rotateVec3(vr(0, 0, 0)).add(g_trans), + g_rot, + thin, + g_rot.rotateVec3(vr(0, 0.5, 0)).add(g_trans), + g_rot, + ); + try testing.expectApproxEqAbs(@as(Real, 0.1), r.depth, epa_tol); + try testing.expect(r.normal.approxEql(g_rot.rotateVec3(vr(0, 1, 0)), epa_tol)); + } + // Order-independence must survive the eccentric polytope too. + const ab = deepEpa(thin, vr(0, 0, 0), Quatr.identity, thin, vr(0, 0.5, 0), Quatr.identity); + const ba = deepEpa(thin, vr(0, 0.5, 0), Quatr.identity, thin, vr(0, 0, 0), Quatr.identity); + try testing.expectApproxEqAbs(ab.depth, ba.depth, epa_tol); + try testing.expect(ab.normal.approxEql(ba.normal.neg(), epa_tol)); +} + +test "epa is deterministic and iteration-bounded" { + // Determinism: a fixed oriented deep pair, run twice ⇒ bit-identical. + const ra = Quatr.fromAxisAngle(vr(1, 1, 0).normalize(), 0.5); + const rb = Quatr.fromAxisAngle(vr(0, 1, 1).normalize(), 1.1); + const r1 = deepEpa(boxShape(1, 1, 1), vr(0, 0, 0), ra, boxShape(1, 1, 1), vr(1.2, 0.3, 0.1), rb); + const r2 = deepEpa(boxShape(1, 1, 1), vr(0, 0, 0), ra, boxShape(1, 1, 1), vr(1.2, 0.3, 0.1), rb); + try testing.expectEqual(r1.depth, r2.depth); + try testing.expect(r1.normal.eql(r2.normal)); + try testing.expect(r1.closest_a.eql(r2.closest_a)); + try testing.expect(r1.closest_b.eql(r2.closest_b)); + + // Adversarial: two nearly-coincident unit boxes (deep, worst case for the + // polytope). A pure `iter < max_epa_iterations` loop cannot hang, so a finite + // sane result IS the termination proof. + const adv = deepEpa(boxShape(1, 1, 1), vr(0, 0, 0), Quatr.identity, boxShape(1, 1, 1), vr(0.01, 0.01, 0.01), Quatr.identity); + try testing.expect(finite3(adv.normal) and finite3(adv.closest_a) and finite3(adv.closest_b)); + try testing.expect(adv.depth >= 0); + try testing.expectApproxEqAbs(@as(Real, 1), adv.normal.length(), epa_tol); +} diff --git a/src/modules/forge/forge_3d/tests/narrowphase_test.zig b/src/modules/forge/forge_3d/tests/gjk_test.zig similarity index 99% rename from src/modules/forge/forge_3d/tests/narrowphase_test.zig rename to src/modules/forge/forge_3d/tests/gjk_test.zig index f6cbe17..6136dfb 100644 --- a/src/modules/forge/forge_3d/tests/narrowphase_test.zig +++ b/src/modules/forge/forge_3d/tests/gjk_test.zig @@ -6,7 +6,7 @@ const std = @import("std"); const config = @import("../config.zig"); -const narrowphase = @import("../pipeline/narrowphase.zig"); +const narrowphase = @import("../pipeline/narrowphase/root.zig"); const math = @import("foundation").math; const api = @import("weld_forge"); const bm_mod = @import("../body_manager.zig"); @@ -694,7 +694,7 @@ test "gjk deep pairs enclose the origin" { // this interior point `.separated`; the dimensionless product-of-edges // criterion classifies it correctly. (Aspect ratios beyond a moderate bound // are NOT guaranteed — a GJK f32 limitation on sharp cores, deferred to the - // M1.1.4 analytic box fast paths / M1.1.3 EPA; see `narrowphase.zig`.) + // M1.1.4 analytic box fast paths / M1.1.3 EPA; see `gjk.zig`.) try checkDeep(boxShape(50.13848, 0.23608336, 0.98368657), vr(0, 0, 0), Quatr.identity, sphereShape(0.5), vr(14.877217, 0.01973883, 0.07743414), Quatr.identity); } diff --git a/src/modules/forge/forge_3d/tests/manifold_test.zig b/src/modules/forge/forge_3d/tests/manifold_test.zig new file mode 100644 index 0000000..5a71748 --- /dev/null +++ b/src/modules/forge/forge_3d/tests/manifold_test.zig @@ -0,0 +1,699 @@ +//! M1.1.3/E3 acceptance suite for the forge_3d contact-manifold generator +//! (`collide`: GJK → supporting-face clipping, both regimes). Keyed to +//! `config.Real` so `-Dphysics_f64=true` sweeps the whole suite at f64 (local). + +const std = @import("std"); +const config = @import("../config.zig"); +const narrowphase = @import("../pipeline/narrowphase/root.zig"); +const math = @import("foundation").math; +const api = @import("weld_forge"); +const bm_mod = @import("../body_manager.zig"); +const shape_mod = @import("../shape.zig"); +const broadphase = @import("../pipeline/broadphase.zig"); + +const Real = config.Real; +const Vec3r = config.Vec3r; +const Quatr = config.Quatr; +const SupportShape = narrowphase.SupportShape(Real); +const ContactManifold = narrowphase.ContactManifold(Real); +const BodyManager = bm_mod.BodyManager; +const ShapeStore = shape_mod.ShapeStore; +const Broadphase = broadphase.Broadphase(Real); +const BodyId = api.BodyId; +const ApiVec3 = math.Vec3; +const testing = std.testing; + +fn vr(x: Real, y: Real, z: Real) Vec3r { + return Vec3r.fromArray(.{ x, y, z }); +} +fn sphereShape(radius: Real) SupportShape { + return .{ .core = .point, .radius = radius }; +} +fn capsuleShape(half_height: Real, radius: Real) SupportShape { + return .{ .core = .{ .segment = half_height }, .radius = radius }; +} +fn boxShape(hx: Real, hy: Real, hz: Real) SupportShape { + return .{ .core = .{ .box = vr(hx, hy, hz) }, .radius = 0 }; +} +fn roundedBoxShape(hx: Real, hy: Real, hz: Real, radius: Real) SupportShape { + return .{ .core = .{ .box = vr(hx, hy, hz) }, .radius = radius }; +} + +const tol: Real = 1.0e-3; + +fn finite3(v: Vec3r) bool { + const a = v.toArray(); + return std.math.isFinite(a[0]) and std.math.isFinite(a[1]) and std.math.isFinite(a[2]); +} + +fn collide(sa: SupportShape, pa: Vec3r, ra: Quatr, sb: SupportShape, pb: Vec3r, rb: Quatr) ?ContactManifold { + return narrowphase.collide(Real, sa, pa, ra, sb, pb, rb); +} + +test "box-box face contact yields a multi-point manifold" { + // A unit box at the origin, another stacked 1.5 above ⇒ overlap 0.5 along Y. + // Face-face contact: 4 points on the contact plane (y = 0.75), each + // penetration 0.5, normal +Y (A→B). + const box = boxShape(1, 1, 1); + const m = collide(box, vr(0, 0, 0), Quatr.identity, box, vr(0, 1.5, 0), Quatr.identity).?; + try testing.expectEqual(@as(u8, 4), m.count); + try testing.expect(m.normal.approxEql(vr(0, 1, 0), tol)); + for (0..m.count) |i| { + try testing.expectApproxEqAbs(@as(Real, 0.5), m.points[i].penetration, tol); + try testing.expect(finite3(m.points[i].position)); + // On the contact plane midway between the two faces (y = 0.75). + try testing.expectApproxEqAbs(@as(Real, 0.75), m.points[i].position.toArray()[1], tol); + } + + // Same overlap under an oblique global transform ⇒ depth invariant, normal + // rotated, still 4 points. + const g = Quatr.fromAxisAngle(vr(1, 2, 3).normalize(), 0.7); + const t = vr(-4, 7, 2); + const m2 = collide(box, g.rotateVec3(vr(0, 0, 0)).add(t), g, box, g.rotateVec3(vr(0, 1.5, 0)).add(t), g).?; + try testing.expectEqual(@as(u8, 4), m2.count); + try testing.expect(m2.normal.approxEql(g.rotateVec3(vr(0, 1, 0)), tol)); + for (0..m2.count) |i| try testing.expectApproxEqAbs(@as(Real, 0.5), m2.points[i].penetration, tol); +} + +test "manifold feature matrix" { + // sphere/* ⇒ exactly 1 point. + { + // sphere/box: sphere centre 0.6 inside the +Z face ⇒ pen 0.6 + r_sum. + const m = collide(boxShape(1, 1, 1), vr(0, 0, 0), Quatr.identity, sphereShape(0.5), vr(0, 0, 0.4), Quatr.identity).?; + try testing.expectEqual(@as(u8, 1), m.count); + try testing.expect(m.normal.approxEql(vr(0, 0, 1), tol)); + try testing.expectApproxEqAbs(@as(Real, 0.6 + 0.5), m.points[0].penetration, tol); + } + { + // sphere/sphere overlapping (cores 1.2 apart, r_sum 2) ⇒ 1 point, pen 0.8. + const m = collide(sphereShape(1), vr(0, 0, 0), Quatr.identity, sphereShape(1), vr(1.2, 0, 0), Quatr.identity).?; + try testing.expectEqual(@as(u8, 1), m.count); + try testing.expect(m.normal.approxEql(vr(1, 0, 0), tol)); + try testing.expectApproxEqAbs(@as(Real, 0.8), m.points[0].penetration, tol); + } + // capsule/capsule parallel side overlap ⇒ 1-2 points (segment feature). + { + // Two Y-axis capsules, cores 0.3 apart in X, r_sum 1 ⇒ shallow line contact. + const m = collide(capsuleShape(1, 0.5), vr(0, 0, 0), Quatr.identity, capsuleShape(1, 0.5), vr(0.8, 0, 0), Quatr.identity).?; + try testing.expect(m.count >= 1 and m.count <= 2); + try testing.expect(m.normal.approxEql(vr(1, 0, 0), tol)); + for (0..m.count) |i| try testing.expectApproxEqAbs(@as(Real, 0.2), m.points[i].penetration, tol); + } + // box/capsule side ⇒ 1-2 points. + { + // A Y-capsule whose segment lies just past the box +X face. + const m = collide(boxShape(1, 1, 1), vr(0, 0, 0), Quatr.identity, capsuleShape(0.5, 0.5), vr(1.3, 0, 0), Quatr.identity).?; + try testing.expect(m.count >= 1 and m.count <= 2); + try testing.expect(m.normal.approxEql(vr(1, 0, 0), tol)); + } +} + +/// Whether the first `count` manifold points are pairwise distinct. +fn pointsDistinct(m: ContactManifold) bool { + for (0..m.count) |i| { + for (i + 1..m.count) |j| { + if (m.points[i].position.approxEql(m.points[j].position, 1.0e-4)) return false; + } + } + return true; +} + +test "box-box rotated face reduces to four distinct points" { + // Adversarial-review regime (finding F): a yawed box makes the incident face + // clip to an OCTAGON (8 coplanar points), which the reduction must collapse to + // four DISTINCT spread points — not a degenerate set with a duplicated corner. + // Unit box A at origin; unit box B 1.9 above, yawed 45° about Y ⇒ overlap 0.1. + const box = boxShape(1, 1, 1); + const yaw = Quatr.fromAxisAngle(Vec3r.unit_y, std.math.pi / 4.0); + const m = collide(box, vr(0, 0, 0), Quatr.identity, box, vr(0, 1.9, 0), yaw).?; + try testing.expectEqual(@as(u8, 4), m.count); + try testing.expect(pointsDistinct(m)); // no duplicated corner (finding F) + try testing.expect(m.normal.approxEql(vr(0, 1, 0), tol)); + for (0..m.count) |i| { + try testing.expectApproxEqAbs(@as(Real, 0.1), m.points[i].penetration, tol); + try testing.expectApproxEqAbs(@as(Real, 0.95), m.points[i].position.toArray()[1], tol); // contact plane + } +} + +test "staggered capsule segment clips without a duplicate point" { + // Adversarial-review regime (finding E): two parallel Y-capsules staggered + // along their axis so B's segment straddles A's endpoint plane. The segment + // clip must yield two DISTINCT points, not a doubled crossing point. + const cap = capsuleShape(1, 0.5); + const m = collide(cap, vr(0, 0, 0), Quatr.identity, cap, vr(0, 0.5, 0.8), Quatr.identity).?; + try testing.expectEqual(@as(u8, 2), m.count); + try testing.expect(pointsDistinct(m)); + try testing.expect(m.normal.approxEql(vr(0, 0, 1), tol)); + for (0..m.count) |i| try testing.expectApproxEqAbs(@as(Real, 0.2), m.points[i].penetration, tol); // r_sum(1) − dist(0.8) +} + +test "intersection feature ids are unique among simultaneous contacts" { + // Codex P1a repro: two unit boxes, B offset into a corner with a small yaw so + // the manifold points are edge×plane intersections. The old `@min(edge)` key + // aliased distinct box edges (a vertex is on 3 edges) → two contacts shared + // 0x800e8002; the sorted-pair key makes every simultaneous contact distinct. + const box = boxShape(1, 1, 1); + const yaw = Quatr.fromAxisAngle(Vec3r.unit_y, 0.05); + const m = collide(box, vr(0, 0, 0), Quatr.identity, box, vr(-0.9, 1.9, -0.9), yaw).?; + try testing.expect(m.count >= 2); + for (0..m.count) |i| { + for (i + 1..m.count) |j| try testing.expect(m.points[i].feature_id != m.points[j].feature_id); + } +} + +/// Feature-id set of a manifold (up to 4), for set comparison. +fn fidSet(m: ContactManifold) [4]u32 { + var s: [4]u32 = .{ 0, 0, 0, 0 }; + for (0..m.count) |i| s[i] = m.points[i].feature_id; + return s; +} + +/// Whether every one of `a`'s first `n` ids appears in `b`'s first `n`. +fn fidSetEq(a: [4]u32, b: [4]u32, n: u8) bool { + for (0..n) |i| { + var found = false; + for (0..n) |j| { + if (a[i] == b[j]) found = true; + } + if (!found) return false; + } + return true; +} + +/// The feature_id of a reference-corner contact (reference half in the `class_c` +/// range, top two bits `0b10`), or null if the manifold has none. +fn refCornerFid(m: ContactManifold) ?u32 { + for (0..m.count) |i| { + const fid = m.points[i].feature_id; + if ((fid >> 16) & 0xc000 == 0x8000) return fid; + } + return null; +} + +/// Whether every id of `m` carries a valid class pair — one of the four +/// producers: kept vertex (a,a), edge crossing (edge,edge), reference corner +/// (c,c), single witness contact (a,c). +fn validClassPairs(m: ContactManifold) bool { + for (0..m.count) |i| { + const fid = m.points[i].feature_id; + const r = (fid >> 16) & 0xc000; + const c = fid & 0xc000; + const ok = (r == 0x0000 and c == 0x0000) // kept vertex + or (r == 0x4000 and c == 0x4000) // edge × ref-edge + or (r == 0x8000 and c == 0x8000) // reference corner + or (r == 0x0000 and c == 0x8000); // single witness contact + if (!ok) return false; + } + return true; +} + +test "capsule endpoint feature ids distinguish the two ends and the segment" { + // Codex FIX-12: an end-on capsule endpoint must encode WHICH endpoint (its + // vert id), not just the constant face_id 6 — else +Y and −Y ends share an + // id, and an endpoint (point-core) is indistinguishable from a side segment. + const cap = capsuleShape(1, 0.3); + const sph = sphereShape(0.5); + // Sphere beyond the +Y end vs the −Y end ⇒ the capsule presents its +Y (vert + // 0) vs −Y (vert 1) endpoint — distinct ids. + const id_plus = collide(cap, vr(0, 0, 0), Quatr.identity, sph, vr(0, 1.5, 0), Quatr.identity).?.points[0].feature_id; + const id_minus = collide(cap, vr(0, 0, 0), Quatr.identity, sph, vr(0, -1.5, 0), Quatr.identity).?.points[0].feature_id; + try testing.expect(id_plus != id_minus); + // Sphere beside the capsule ⇒ the capsule presents its SEGMENT (a face) — a + // different id from either endpoint (endpoint→segment changes the id). + const id_side = collide(cap, vr(0, 0, 0), Quatr.identity, sph, vr(0.7, 0, 0), Quatr.identity).?.points[0].feature_id; + try testing.expect(id_side != id_plus and id_side != id_minus); + // All three are single-contact ids (the disjoint (class_a, class_c) pair). + inline for (.{ id_plus, id_minus, id_side }) |fid| { + try testing.expectEqual(@as(u32, 0x0000), (fid >> 16) & 0xc000); + try testing.expectEqual(@as(u32, 0x8000), fid & 0xc000); + } +} + +test "pose sweep: capsule pairs have distinct well-classed feature ids" { + // Extend the feature-id coverage to the capsule pairs (Codex FIX-12): every + // config's manifold has pairwise-distinct ids, each carrying a valid class + // pair, and no multi-point manifold carries the single-contact (a,c) pair. + const cap = capsuleShape(1, 0.3); + const cap2 = capsuleShape(0.6, 0.4); + const box = boxShape(1, 1, 1); + const sph = sphereShape(0.5); + const zrot = Quatr.fromAxisAngle(Vec3r.unit_z, std.math.pi / 2.0); + const Combo = struct { a: SupportShape, pb: Vec3r, b: SupportShape, rb: Quatr }; + const combos = [_]Combo{ + .{ .a = cap, .pb = vr(1.1, 0, 0), .b = box, .rb = Quatr.identity }, // capsule side vs box (clipSegment) + .{ .a = cap, .pb = vr(0, 1.6, 0), .b = box, .rb = Quatr.identity }, // capsule end-on vs box (point-core) + .{ .a = cap, .pb = vr(0, 0, 0), .b = cap, .rb = zrot }, // crossing capsules + .{ .a = cap, .pb = vr(0.4, 0.3, 0), .b = cap, .rb = Quatr.identity }, // parallel capsules + .{ .a = cap, .pb = vr(0.6, 0, 0), .b = sph, .rb = Quatr.identity }, // capsule radial vs sphere + .{ .a = cap, .pb = vr(0, 1.4, 0), .b = cap2, .rb = zrot }, // capsule end vs crossing capsule + }; + const globals = [_]Quatr{ Quatr.identity, Quatr.fromAxisAngle(vr(1, 2, 3).normalize(), 0.7) }; + for (combos) |c| { + for (globals) |g| { + const m = collide(c.a, g.rotateVec3(vr(0, 0, 0)), g, c.b, g.rotateVec3(c.pb), g.mul(c.rb)) orelse continue; + try testing.expect(validClassPairs(m)); + for (0..m.count) |i| { + for (i + 1..m.count) |j| try testing.expect(m.points[i].feature_id != m.points[j].feature_id); + if (m.count > 1) { + const fid = m.points[i].feature_id; + const is_fallback = ((fid >> 16) & 0xc000 == 0x0000) and (fid & 0xc000 == 0x8000); + try testing.expect(!is_fallback); + } + } + } + } +} + +test "single-contact fallback id never aliases a clip id" { + // Codex FIX-11 threshold repro (the two Codex angles straddle `face_face_min` + // at f32; the threshold is precision-dependent — 0.999 f32 / 0.9999 f64 — so + // scan a small tilt range around them to find, at THIS build's precision, both + // a clipped (multi-point) and a fallback (single-contact) box/box at the same + // base pose). The fallback's feature_id must equal NONE of the clip ids — its + // (class_a ref, class_c inc) class pair is disjoint from every clip-id class + // pair (kept vertex (a,a), edge (edge,edge), corner (c,c)). + const box = boxShape(1, 1, 1); + const p = vr(0, 0.8, -1.8); + var m_clip: ?ContactManifold = null; + var m_fb: ?ContactManifold = null; + // Tilt grows from 0 (identity ⇒ a clean face-face clip at any precision) + // through the face-face↔fallback threshold into the edge/vertex regime. + var a: Real = 0; + while (a < 0.4) : (a += 0.0005) { + const rot = Quatr.fromAxisAngle(Vec3r.unit_y, a).mul(Quatr.fromAxisAngle(Vec3r.unit_x, a)); + const m = collide(box, vr(0, 0, 0), Quatr.identity, box, p, rot) orelse continue; + if (m.count >= 2 and m_clip == null) m_clip = m; + if (m.count == 1 and m_fb == null) m_fb = m; + } + const mc = m_clip orelse return error.NoClippedManifold; + const mf = m_fb orelse return error.NoFallbackManifold; + const fb_id = mf.points[0].feature_id; + // The fallback id carries the disjoint (class_a, class_c) class pair … + try testing.expectEqual(@as(u32, 0x0000), (fb_id >> 16) & 0xc000); + try testing.expectEqual(@as(u32, 0x8000), fb_id & 0xc000); + // … and equals none of the clip ids. + for (0..mc.count) |i| try testing.expect(fb_id != mc.points[i].feature_id); +} + +test "reference corner feature id encodes the incident face" { + // Codex FIX-10: a reference corner's incident half must carry which incident + // face it lies on, not a constant — else two corners at the SAME reference + // vertex on DIFFERENT incident faces (a supporting-face flip inter-frame) + // share a feature_id and warm-starting mis-matches them. + const box = boxShape(1, 1, 1); + // Same staggered overlap; B upright vs B rolled 90° about X ⇒ the SAME + // reference corner (a vertex of A's +Y face) but a DIFFERENT incident face. + const m1 = collide(box, vr(0, 0, 0), Quatr.identity, box, vr(0.6, 1.5, 0.4), Quatr.identity).?; + const roll = Quatr.fromAxisAngle(Vec3r.unit_x, std.math.pi / 2.0); + const m2 = collide(box, vr(0, 0, 0), Quatr.identity, box, vr(0.6, 1.5, 0.4), roll).?; + const c1 = refCornerFid(m1) orelse return error.NoReferenceCorner; + const c2 = refCornerFid(m2) orelse return error.NoReferenceCorner; + // Same reference vertex (high half) … + try testing.expectEqual(c1 >> 16, c2 >> 16); + // … but a different incident face (low half) ⇒ the face is encoded. + try testing.expect((c1 & 0xffff) != (c2 & 0xffff)); + // The low half is a corner-face class (`class_c`) carrying a real face id 0..5. + try testing.expectEqual(@as(u32, 0x8000), c1 & 0xc000); + try testing.expect((c1 & 0x3fff) <= 5 and (c2 & 0x3fff) <= 5); +} + +test "staggered box overlap yields a reference-corner contact with a unique id" { + // Coverage of the reference-corner path (a clip point on TWO reference side + // planes — an intersection whose endpoint is already an intersection, so it + // exercises `incidentEdgeId`'s provenance-inheritance branch). Axis-aligned + // unit boxes stacked in Y with a partial lateral offset ⇒ the overlap + // rectangle's four corners are: one incident vertex, two edge×plane + // intersections, and one REFERENCE corner (a corner of A's top face). + const box = boxShape(1, 1, 1); + const m = collide(box, vr(0, 0, 0), Quatr.identity, box, vr(0.6, 1.5, 0.4), Quatr.identity).?; + try testing.expectEqual(@as(u8, 4), m.count); + // All four feature_ids pairwise distinct (the reference corner must not alias + // an edge×plane contact). + for (0..m.count) |i| { + for (i + 1..m.count) |j| try testing.expect(m.points[i].feature_id != m.points[j].feature_id); + } + // Frame-stable under a tiny lateral shift (same physical corners → same ids). + const s0 = fidSet(m); + const m2 = collide(box, vr(0, 0, 0), Quatr.identity, box, vr(0.6001, 1.5, 0.3999), Quatr.identity).?; + try testing.expectEqual(m.count, m2.count); + try testing.expect(fidSetEq(s0, fidSet(m2), m.count)); + const m3 = collide(box, vr(0, 0, 0), Quatr.identity, box, vr(0.5999, 1.5, 0.4001), Quatr.identity).?; + try testing.expectEqual(m.count, m3.count); + try testing.expect(fidSetEq(s0, fidSet(m3), m.count)); +} + +test "edge-edge penetration is measured along the contact axis" { + // Fix-1: for an oblique (edge/vertex) contact the EPA normal `n_a` is not a + // box-face normal, so measuring depth along the reference face normal `rn` + // diverges. The manifold depth must match the EPA depth along `n_a`. + const box = boxShape(1, 1, 1); + const yaw = Quatr.fromAxisAngle(Vec3r.unit_z, std.math.pi / 4.0); + const pa = vr(0, 0, 0); + const pb = vr(1.35, 1.35, 0); // corner overlap ⇒ oblique closest features + // Reference EPA result. + const g = narrowphase.gjk(Real, box, pa, Quatr.identity, box, pb, yaw); + try testing.expectEqual(narrowphase.GjkResult(Real).Status.deep, g.status); + const relpose = narrowphase.RelativePose(Real).init(pa, Quatr.identity, pb, yaw); + const e = narrowphase.epa(Real, box, pa, Quatr.identity, relpose, box, g); + // Confirm the contact axis is genuinely oblique (not a face normal) so this + // exercises the edge path, not the face-face quad path. + const na = e.normal.toArray(); + const max_comp = @max(@abs(na[0]), @max(@abs(na[1]), @abs(na[2]))); + try testing.expect(max_comp < 0.999); + + const m = collide(box, pa, Quatr.identity, box, pb, yaw).?; + // Normal agrees with EPA (no canonical swap here: A's position sorts first). + try testing.expect(m.normal.approxEql(e.normal, tol)); + // Depth along the contact axis matches EPA (r_sum = 0), not the ~rn-projected + // value the old code produced. + var max_pen: Real = 0; + for (0..m.count) |i| max_pen = @max(max_pen, m.points[i].penetration); + try testing.expectApproxEqAbs(e.depth, max_pen, tol); +} + +test "manifold feature ids are frame-stable" { + // Fix-2: feature_id must identify the physical feature, surviving a small pose + // change (a clip-buffer index would reorder). A smaller incident box seated on + // a larger reference face (so its four corners stay INSIDE the reference face + // under a tiny lateral shift, i.e. the incident vertices remain the kept + // features) translated ±0.0001 ⇒ the same set of feature_ids (same corners). + const big = boxShape(1, 1, 1); + const small = boxShape(0.5, 1, 0.5); + const m1 = collide(big, vr(0, 0, 0), Quatr.identity, small, vr(0, 1.5, 0), Quatr.identity).?; + const m2 = collide(big, vr(0, 0, 0), Quatr.identity, small, vr(0.0001, 1.5, -0.0001), Quatr.identity).?; + try testing.expectEqual(@as(u8, 4), m1.count); + try testing.expectEqual(m1.count, m2.count); + // Every m1 feature_id appears in m2 (same physical corners → same ids). + for (0..m1.count) |i| { + var found = false; + for (0..m2.count) |j| { + if (m1.points[i].feature_id == m2.points[j].feature_id) found = true; + } + try testing.expect(found); + } + // The four ids are distinct (four distinct corners). + for (0..m1.count) |i| { + for (i + 1..m1.count) |j| try testing.expect(m1.points[i].feature_id != m1.points[j].feature_id); + } +} + +test "manifold reduction stays precise far from the origin" { + // Fix-4: keep the reduction in A's frame. Two coincident tiny boxes at a large + // world coordinate: the four A-frame contact corners (±0.001) are sub-ULP in + // f32 world space (they round together → a world-frame dedup collapses to 1), + // but are distinct in A's frame ⇒ a 4-point face manifold survives. + const tiny = boxShape(0.001, 0.001, 0.001); + const far = vr(100000, 100000, 100000); + const m = collide(tiny, far, Quatr.identity, tiny, far, Quatr.identity).?; + try testing.expectEqual(@as(u8, 4), m.count); // A-frame reduction keeps 4 (world-frame would collapse to 1) + // The four ids are distinct (distinct A-frame corners); world positions round + // together at f32 (that is exactly why the reduction must stay in A's frame). + for (0..m.count) |i| { + for (i + 1..m.count) |j| try testing.expect(m.points[i].feature_id != m.points[j].feature_id); + try testing.expectApproxEqAbs(@as(Real, 0.002), m.points[i].penetration, 1.0e-4); // full overlap 2·he + } +} + +test "manifold is order-independent" { + const g = Quatr.fromAxisAngle(vr(2, -1, 3).normalize(), 0.8); + const Combo = struct { sa: SupportShape, pa: Vec3r, sb: SupportShape, pb: Vec3r }; + const combos = [_]Combo{ + .{ .sa = boxShape(1, 1, 1), .pa = vr(0, 0, 0), .sb = boxShape(1, 1, 1), .pb = vr(0, 1.5, 0) }, // face-face + .{ .sa = boxShape(1, 1, 1), .pa = vr(0, 0, 0), .sb = sphereShape(0.5), .pb = vr(0, 0, 0.4) }, // sphere/box + .{ .sa = sphereShape(1), .pa = vr(0, 0, 0), .sb = sphereShape(1), .pb = vr(1.2, 0, 0) }, // sphere/sphere + .{ .sa = capsuleShape(1, 0.5), .pa = vr(0, 0, 0), .sb = capsuleShape(1, 0.5), .pb = vr(0.8, 0, 0) }, // capsule/capsule + .{ .sa = capsuleShape(1, 0.3), .pa = vr(0, 0, 0), .sb = capsuleShape(1, 0.3), .pb = vr(0, 0, 0) }, // degenerate crossing (F5) + // Same position, equal |half_extents| but distinct extents — exercises + // the canonical-order total-order tie-break on the box core parameters + // (a `|he|`-only key would collide here and break order-independence). + .{ .sa = boxShape(1, 2, 3), .pa = vr(0, 0, 0), .sb = boxShape(3, 2, 1), .pb = vr(0, 0, 0) }, + }; + const rot_z90 = Quatr.fromAxisAngle(Vec3r.unit_z, std.math.pi / 2.0); + for (combos, 0..) |c, ci| { + // The crossing-capsule combo needs B rotated to actually cross A. + const rb = if (ci == 4) rot_z90 else Quatr.identity; + for ([_]Quatr{ Quatr.identity, g }) |ra| { + const ab = collide(c.sa, c.pa, ra, c.sb, c.pb, ra.mul(rb)).?; + const ba = collide(c.sb, c.pb, ra.mul(rb), c.sa, c.pa, ra).?; + try testing.expectEqual(ab.count, ba.count); + try testing.expect(ab.normal.approxEql(ba.normal.neg(), tol)); + // Same point set (order may differ) and same per-point penetration. + for (0..ab.count) |i| { + var matched = false; + for (0..ba.count) |j| { + if (ab.points[i].position.approxEql(ba.points[j].position, tol) and + @abs(ab.points[i].penetration - ba.points[j].penetration) <= tol) matched = true; + } + try testing.expect(matched); + } + } + } +} + +test "shallow and deep manifolds are continuous" { + // Sweep two unit-radius spheres from a shallow gap toward coincidence. The + // per-point penetration r_sum − dist is continuous and monotone, the normal + // stays +X (no flip) until coincidence. + const rs = sphereShape(1); + const ds = [_]Real{ 1.5, 1.0, 0.5, 0.1, 0.01 }; + var prev: Real = -1; + for (ds) |d| { + const m = collide(rs, vr(0, 0, 0), Quatr.identity, rs, vr(d, 0, 0), Quatr.identity).?; + try testing.expectEqual(@as(u8, 1), m.count); + try testing.expect(m.normal.approxEql(vr(1, 0, 0), tol)); + const pen = m.points[0].penetration; + try testing.expectApproxEqAbs(@as(Real, 2.0) - d, pen, tol); // r_sum − dist + try testing.expect(pen > prev); // monotone as d shrinks + prev = pen; + } + // Coincident centres (deep, depth 0) ⇒ penetration → r_sum, continuous. + const mc = collide(rs, vr(0, 0, 0), Quatr.identity, rs, vr(0, 0, 0), Quatr.identity).?; + try testing.expectEqual(@as(u8, 1), mc.count); + try testing.expectApproxEqAbs(@as(Real, 2.0), mc.points[0].penetration, tol); + try testing.expect(finite3(mc.normal)); + try testing.expectApproxEqAbs(@as(Real, 1), mc.normal.length(), tol); + + // Separated ⇒ null. + try testing.expect(collide(rs, vr(0, 0, 0), Quatr.identity, rs, vr(2.5, 0, 0), Quatr.identity) == null); + + // A rounded-box shallow overlap threads the box-core shallow path (radius-0 + // boxes cannot be shallow) and still produces a valid manifold. + const rb = roundedBoxShape(0.5, 0.5, 0.5, 1.0); + const m = collide(rb, vr(0, 0, 0), Quatr.identity, rb, vr(0, 2.5, 0), Quatr.identity).?; + try testing.expect(m.count >= 1 and m.count <= 4); + try testing.expect(m.normal.approxEql(vr(0, 1, 0), tol)); + for (0..m.count) |i| try testing.expectApproxEqAbs(@as(Real, 0.5), m.points[i].penetration, tol); // r_sum(2) − dist(1.5) +} + +// --- E4: broadphase → collidePair integration --- + +/// Add a `.dynamic` box body at (x,y,z). Descriptor positions are the f32 api Vec3. +fn addBoxBodyAt(gpa: std.mem.Allocator, bm: *BodyManager, store: *const ShapeStore, shape: api.ShapeId, entity_index: u32, x: f32, y: f32, z: f32) !BodyId { + var d = api.BodyDescriptor{ + .entity = .{ .index = entity_index, .generation = 0 }, + .body_type = .dynamic, + .shape = shape, + }; + d.position = ApiVec3.fromArray(.{ x, y, z }); + return bm.addBody(gpa, store, d); +} + +/// Whether `p` is the unordered pair `{x, y}` (candidate pairs are canonical). +fn isPair(p: Broadphase.Pair, x: BodyId, y: BodyId) bool { + return p.a == @min(x, y) and p.b == @max(x, y); +} + +/// Add a `.dynamic` box body at (x,y,z) with rotation `rot`. +fn addBoxBodyAtRot(gpa: std.mem.Allocator, bm: *BodyManager, store: *const ShapeStore, shape: api.ShapeId, entity_index: u32, x: f32, y: f32, z: f32, rot: math.Quatf) !BodyId { + var d = api.BodyDescriptor{ + .entity = .{ .index = entity_index, .generation = 0 }, + .body_type = .dynamic, + .shape = shape, + }; + d.position = ApiVec3.fromArray(.{ x, y, z }); + d.rotation = rot; + return bm.addBody(gpa, store, d); +} + +test "collidePair feature ids are stable across a pose-order boundary" { + // Codex P1b: `collide` re-canonicalizes by pose, so the feature_id + // reference/incident ownership flips when a coordinate crosses the + // lexicographic order boundary (x = 0 here). `collidePair` drives a FIXED + // body-id order, so a tiny shift crossing that boundary leaves the feature_ids + // unchanged (same physical contacts). + const gpa = testing.allocator; + var store = ShapeStore{}; + defer store.deinit(gpa); + var bm = BodyManager{}; + defer bm.deinit(gpa); + const box = try store.createShape(gpa, .{ .box = .{ .half_extents = ApiVec3.splat(1) } }); + const yaw = math.Quatf.fromAxisAngle(math.Vec3.unit_y, std.math.pi / 4.0); + + // A at the origin (identity); two B's yawed, straddling x = 0 by ±1e-4. + const id_a = try addBoxBodyAt(gpa, &bm, &store, box, 0, 0, 0, 0); + const id_neg = try addBoxBodyAtRot(gpa, &bm, &store, box, 1, -0.0001, 1.5, 0, yaw); + const id_pos = try addBoxBodyAtRot(gpa, &bm, &store, box, 2, 0.0001, 1.5, 0, yaw); + + const m_neg = bm.collidePair(&store, id_a, id_neg).?; + const m_pos = bm.collidePair(&store, id_a, id_pos).?; + try testing.expectEqual(m_neg.count, m_pos.count); + // Every feature_id from one appears in the other (body-id order is stable). + for (0..m_neg.count) |i| { + var found = false; + for (0..m_pos.count) |j| { + if (m_neg.points[i].feature_id == m_pos.points[j].feature_id) found = true; + } + try testing.expect(found); + } +} + +test "broadphase pairs to manifolds via collidePair" { + const gpa = testing.allocator; + var store = ShapeStore{}; + defer store.deinit(gpa); + var bm = BodyManager{}; + defer bm.deinit(gpa); + var bp = Broadphase.init(.{}); + defer bp.deinit(gpa); + + const box = try store.createShape(gpa, .{ .box = .{ .half_extents = ApiVec3.splat(0.5) } }); + + // A–B: tight AABBs 0.15 apart (disjoint) but fat AABBs (margin 0.1) overlap ⇒ + // a broadphase fat-AABB false positive the narrowphase must reject (null). + const id_a = try addBoxBodyAt(gpa, &bm, &store, box, 0, 0, 0, 0); + const id_b = try addBoxBodyAt(gpa, &bm, &store, box, 1, 1.15, 0, 0); + // C–D: boxes genuinely overlap (0.7 along X) ⇒ a real deep contact manifold. + const id_c = try addBoxBodyAt(gpa, &bm, &store, box, 2, 10, 0, 0); + const id_d = try addBoxBodyAt(gpa, &bm, &store, box, 3, 10.3, 0, 0); + + const ids = [_]BodyId{ id_a, id_b, id_c, id_d }; + for (ids) |id| _ = try bp.insert(gpa, .dynamic, bm.bodyAabb(&store, id).?, id); + + var pairs: std.ArrayListUnmanaged(Broadphase.Pair) = .empty; + defer pairs.deinit(gpa); + try bp.computePairs(gpa, &pairs); + + var found_false_positive = false; + var found_overlap = false; + for (pairs.items) |p| { + const m = bm.collidePair(&store, p.a, p.b); // all four bodies still live + if (isPair(p, id_a, id_b)) { + found_false_positive = true; + try testing.expect(m == null); // separated ⇒ no manifold + } else if (isPair(p, id_c, id_d)) { + found_overlap = true; + const man = m.?; + try testing.expect(man.count >= 1 and man.count <= 4); + try testing.expectApproxEqAbs(@as(Real, 0.7), man.points[0].penetration, tol); // overlap along X + try testing.expect(finite3(man.normal)); + } + } + // The broadphase surfaced the fat-AABB false positive (rejected as null) and + // the genuine overlap (a valid manifold). + try testing.expect(found_false_positive); + try testing.expect(found_overlap); + + // Stale-handle pair ⇒ null. + bm.removeBody(id_d); + try testing.expect(bm.collidePair(&store, id_c, id_d) == null); +} + +test "pose sweep: feature ids are distinct and frame-stable" { + // The verification a single-point test cannot give (Codex FIX-9): a grid of + // deep box/box contacts — yaw × a light second-axis tilt × lateral offsets × + // two Y overlaps, plus the two Codex repros — each checked for (a) pairwise- + // distinct feature_ids and (b) frame-stability of the feature_id SET under a + // ±1e-4 offset via `collidePair`'s body-id order. + const gpa = testing.allocator; + var store = ShapeStore{}; + defer store.deinit(gpa); + var bm = BodyManager{}; + defer bm.deinit(gpa); + const box = try store.createShape(gpa, .{ .box = .{ .half_extents = ApiVec3.splat(1) } }); + const id_a = try addBoxBodyAt(gpa, &bm, &store, box, 0, 0, 0, 0); + + const yaws = [_]f32{ 0, 0.05, 0.125, 0.3, 0.6, 0.9 }; + const tilts = [_]f32{ 0, 0.07 }; + const offsets = [_][2]f32{ .{ -1, -1.9 }, .{ -0.9, -0.9 }, .{ -0.4, 0.3 }, .{ 0.5, -0.6 }, .{ 0.7, 0.4 } }; + const ys = [_]f32{ 1.5, 1.9 }; // deeper / shallower overlaps (both deep for r=0 boxes) + + var idx: u32 = 1; + for (yaws) |yw| { + for (tilts) |tl| { + for (offsets) |off| { + for (ys) |y| { + const rot = math.Quatf.fromAxisAngle(math.Vec3.unit_y, yw) + .mul(math.Quatf.fromAxisAngle(math.Vec3.unit_x, tl)); + const id_b = try addBoxBodyAtRot(gpa, &bm, &store, box, idx, off[0], y, off[1], rot); + const id_s = try addBoxBodyAtRot(gpa, &bm, &store, box, idx + 1, off[0] + 0.0001, y, off[1] - 0.0001, rot); + idx += 2; + const m = bm.collidePair(&store, id_a, id_b) orelse continue; + // (a) pairwise-distinct feature ids. + for (0..m.count) |i| { + for (i + 1..m.count) |j| try testing.expect(m.points[i].feature_id != m.points[j].feature_id); + } + // (a2, FIX-10) any reference corner carries an incident FACE in + // its low half (class_c + a real face id 0..5), not a constant. + // (a3, FIX-11) a multi-point (clipped) manifold never carries the + // single-contact fallback class pair (class_a ref, class_c inc), + // so a fallback id can never alias a clip id across the + // face-face↔fallback threshold for this body pair. + for (0..m.count) |i| { + const fid = m.points[i].feature_id; + if ((fid >> 16) & 0xc000 == 0x8000) { + try testing.expectEqual(@as(u32, 0x8000), fid & 0xc000); + try testing.expect((fid & 0x3fff) <= 5); + } + if (m.count > 1) { + const is_fallback_pair = ((fid >> 16) & 0xc000 == 0x0000) and (fid & 0xc000 == 0x8000); + try testing.expect(!is_fallback_pair); + } + } + // (b) frame-stable id SET under the tiny shift — asserted only + // when the shift keeps the topology, i.e. same count AND a + // stable contact normal. A tied min-penetration axis (equal + // overlaps on two axes) legitimately flips the normal (and thus + // the whole contact) under a 1e-4 nudge — that is a real + // geometric transition, not a feature_id instability. + if (bm.collidePair(&store, id_a, id_s)) |ms| { + if (ms.count == m.count and m.normal.approxEql(ms.normal, tol)) { + try testing.expect(fidSetEq(fidSet(m), fidSet(ms), m.count)); + } + } + } + } + } + } + + // Explicit Codex repros (yaw about Y), asserted distinct. + inline for (.{ .{ -1.0, 1.9, -1.9, 0.125 }, .{ -0.9, 1.9, -0.9, 0.05 } }) |c| { + const rot = math.Quatf.fromAxisAngle(math.Vec3.unit_y, c[3]); + const id_r = try addBoxBodyAtRot(gpa, &bm, &store, box, idx, c[0], c[1], c[2], rot); + idx += 1; + const m = bm.collidePair(&store, id_a, id_r).?; + for (0..m.count) |i| { + for (i + 1..m.count) |j| try testing.expect(m.points[i].feature_id != m.points[j].feature_id); + } + } +} + +test "collidePair imposes a body-id order on identical geometry" { + // Fix-3: two bodies with bit-identical shape AND pose overlap degenerately — + // `collide`'s pose key cannot order them, so its normal is the same in both + // call orders. `collidePair` breaks the tie by body id (min first) and negates + // for the swapped caller, so the pair stays order-independent for real bodies. + const gpa = testing.allocator; + var store = ShapeStore{}; + defer store.deinit(gpa); + var bm = BodyManager{}; + defer bm.deinit(gpa); + const box = try store.createShape(gpa, .{ .box = .{ .half_extents = ApiVec3.splat(0.5) } }); + const id_a = try addBoxBodyAt(gpa, &bm, &store, box, 0, 0, 0, 0); + const id_b = try addBoxBodyAt(gpa, &bm, &store, box, 1, 0, 0, 0); // same pose as A + + const ab = bm.collidePair(&store, id_a, id_b).?; + const ba = bm.collidePair(&store, id_b, id_a).?; + try testing.expect(ab.normal.approxEql(ba.normal.neg(), tol)); // negated by body-id order + try testing.expectEqual(ab.count, ba.count); + try testing.expect(finite3(ab.normal)); +}