From f7ff9f4eea38fa0fcb31e682ae918d03a4c9abd7 Mon Sep 17 00:00:00 2001 From: Erik Date: Sat, 25 Jul 2026 15:47:58 +0200 Subject: [PATCH] feat(physics): add differential flat BSP traversal Port every current containment, overlap, walkable, and six-path moving-collision query to immutable integer-indexed assets behind a graph-authoritative referee. Exact synthetic, installed-DAT, complete-resolver, and zero-allocation gates prove bit-identical behavior before connected dual publication. Co-authored-by: OpenAI Codex --- AGENTS.md | 12 +- CLAUDE.md | 12 +- docs/plans/2026-04-11-roadmap.md | 15 +- docs/plans/2026-05-12-milestones.md | 8 +- .../2026-07-25-modern-runtime-slice-i.md | 16 +- ...26-07-25-slice-i4-flat-bsp-differential.md | 87 + src/AcDream.Core/Physics/BSPQuery.cs | 59 +- src/AcDream.Core/Physics/CellTransit.cs | 64 +- .../Physics/CollisionTraversal.cs | 226 ++ src/AcDream.Core/Physics/FlatBspQuery.cs | 2132 +++++++++++++++++ src/AcDream.Core/Physics/PhysicsDataCache.cs | 25 + src/AcDream.Core/Physics/TransitionTypes.cs | 48 +- .../Physics/FlatBspQueryDifferentialTests.cs | 1249 ++++++++++ 13 files changed, 3907 insertions(+), 46 deletions(-) create mode 100644 docs/research/2026-07-25-slice-i4-flat-bsp-differential.md create mode 100644 src/AcDream.Core/Physics/CollisionTraversal.cs create mode 100644 src/AcDream.Core/Physics/FlatBspQuery.cs create mode 100644 tests/AcDream.Core.Tests/Physics/FlatBspQueryDifferentialTests.cs diff --git a/AGENTS.md b/AGENTS.md index 28a1b548..12a417b5 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -169,9 +169,16 @@ interface shares the render package's single mmap. Two complete 29,908,271,024-byte bakes produced 2,232,170 typed keys, zero failures, and the same SHA-256 across 16 and 7 workers. Corruption, cancellation, installed package, Release build, and 8,371-test / 5-skip gates pass. +Slice I4 is complete: every current containment, overlap, walkable, and +six-path moving-collision entry point has an integer-indexed flat shadow port. +The exact referee passes 13 focused tests, 7,500 installed-DAT comparisons, +1,200 complete resolver frames, and 10,000 warmed iterations at zero managed +bytes with no mismatch. Release build and 8,384 solution tests / 5 skips pass. +Production remains graph-authoritative until the connected shadow and cutover +gates. Slice H's retained UI/frame, bounded-light, and pooled/borrowed/direct network work is complete; the exact seven-checkpoint connected lifecycle/reconnect gate -passes. Slice I is active at I4 from +passes. Slice I is active at I5 from `docs/plans/2026-07-25-modern-runtime-slice-i.md`. The exact G4 visual rollback is `git revert ef1d263337997bb030eadb7b8e71d73dc659907a`; do not revert G3 or the portal-warmup corrections. Evidence: @@ -179,7 +186,8 @@ the portal-warmup corrections. Evidence: `docs/research/2026-07-25-slice-g5-production-profile.md` and `docs/research/2026-07-25-slice-i1-transition-scratch.md` and `docs/research/2026-07-25-slice-i2-flat-collision-schema.md` and -`docs/research/2026-07-25-slice-i3-prepared-collision-package.md`. +`docs/research/2026-07-25-slice-i3-prepared-collision-package.md` and +`docs/research/2026-07-25-slice-i4-flat-bsp-differential.md`. **Structural prerequisite before new M4 subsystem work:** all eight behavior-preserving `GameWindow` decomposition slices and the automated diff --git a/CLAUDE.md b/CLAUDE.md index 66ac413f..8cebfd66 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -167,9 +167,16 @@ interface shares the render package's single mmap. Two complete 29,908,271,024-byte bakes produced 2,232,170 typed keys, zero failures, and the same SHA-256 across 16 and 7 workers. Corruption, cancellation, installed package, Release build, and 8,371-test / 5-skip gates pass. +Slice I4 is complete: every current containment, overlap, walkable, and +six-path moving-collision entry point has an integer-indexed flat shadow port. +The exact referee passes 13 focused tests, 7,500 installed-DAT comparisons, +1,200 complete resolver frames, and 10,000 warmed iterations at zero managed +bytes with no mismatch. Release build and 8,384 solution tests / 5 skips pass. +Production remains graph-authoritative until the connected shadow and cutover +gates. Slice H's retained UI/frame, bounded-light, and pooled/borrowed/direct network work is complete; the exact seven-checkpoint connected lifecycle/reconnect gate -passes. Slice I is active at I4 from +passes. Slice I is active at I5 from `docs/plans/2026-07-25-modern-runtime-slice-i.md`. The exact G4 visual rollback is `git revert ef1d263337997bb030eadb7b8e71d73dc659907a`; do not revert G3 or the portal-warmup corrections. Evidence: @@ -177,7 +184,8 @@ the portal-warmup corrections. Evidence: `docs/research/2026-07-25-slice-g5-production-profile.md` and `docs/research/2026-07-25-slice-i1-transition-scratch.md` and `docs/research/2026-07-25-slice-i2-flat-collision-schema.md` and -`docs/research/2026-07-25-slice-i3-prepared-collision-package.md`. +`docs/research/2026-07-25-slice-i3-prepared-collision-package.md` and +`docs/research/2026-07-25-slice-i4-flat-bsp-differential.md`. **Structural prerequisite before new M4 subsystem work:** all eight behavior-preserving `GameWindow` decomposition slices and the automated diff --git a/docs/plans/2026-04-11-roadmap.md b/docs/plans/2026-04-11-roadmap.md index 5184d580..2f38852b 100644 --- a/docs/plans/2026-04-11-roadmap.md +++ b/docs/plans/2026-04-11-roadmap.md @@ -1654,13 +1654,20 @@ port in any phase — no separate listing here. > defines deterministic immutable collision records and validates exact > source identity over synthetic corruption cases and representative installed > DATs; production traversal remains on the graph oracle until the later -> differential gate. +> differential gate. Slice I3 appends the complete prepared collision package +> to the existing single mmap. Slice I4 now ports every current containment, +> overlap, walkable, and six-path moving-collision entry point to the immutable +> integer-indexed assets. Its exact referee passes 13 focused tests, 7,500 +> installed-DAT comparisons, 1,200 complete resolver frames, and 10,000 warmed +> iterations at zero managed bytes with no mismatch. Production remains +> graph-authoritative while I5 dual publication and the connected shadow gate +> are active. Release build and 8,384 solution tests / 5 skips pass. > The historical exact cutover > rollback remains > `git revert ef1d263337997bb030eadb7b8e71d73dc659907a`. Slice H is complete: > retained UI/frame work, exact bounded light selection, and > pooled/borrowed/direct network I/O pass 8,292 Release tests / 5 skips and the -> connected lifecycle/reconnect gate. Slice I is active at I4 from +> connected lifecycle/reconnect gate. Slice I is active at I5 from > [its detailed plan](2026-07-25-modern-runtime-slice-i.md). **Spec:** `docs/superpowers/specs/2026-07-05-modern-pipeline-design.md` (the @@ -1685,11 +1692,11 @@ hitch and follows. | MP1a | `AcDream.Content` extraction (GL-free MeshExtractor + boundary records out of App) | ✅ SHIPPED 2026-07-05 — user-gated (renders identical, zero tripwires, perf-neutral); 8 commits `651d041e`..`b0758d77` | | MP1b | Pak format + `acdream-bake` CLI + mmap `PakReader` + equivalence/full-scale gate | ✅ **SHIPPED 2026-07-24** — 729,888 EnvCell keys → 17,117 unique geometries + 712,771 aliases (42.6×), 751,141 total keys / 38,370 physical blobs, zero failures, 28,192.4 MiB, 81.4 s validated atomic publish; full-DAT gate also corrected a real collision in WB's legacy geometry hash. [Report](../research/2026-07-24-slice-b-full-bake-report.md) | | **MP-Alloc (safe batch)** | Reuse per-frame buffers: anim pose, particle draw-list, interior partition, animatedIds/drawableCells sets | ✅ SHIPPED + USER-GATED 2026-07-05 — dense-town frame-time spikes 20–87ms → 6–10ms, alloc spikes (30–75MB single-frame) eliminated, gen2 GC 5–11/window → ~0; user confirms FPS steady. 4 commits `b8c05e2b`..`91afea24` | -| MP-Alloc (hard sites) | EnvCell settled-camera rebuild gate + physics `Transition` reuse | 🟡 Physics half SHIPPED 2026-07-25 — retail-shaped reset-complete transition/query scratch is bit-identical and 0 B/resolve across five profiles; immutable flat collision storage and its complete deterministic prepared package are shipped; traversal differential work continues in Slice I4. | +| MP-Alloc (hard sites) | EnvCell settled-camera rebuild gate + physics `Transition` reuse | 🟡 Physics half SHIPPED 2026-07-25 — retail-shaped reset-complete transition/query scratch is bit-identical and 0 B/resolve across five profiles; immutable flat collision storage, its deterministic prepared package, and the zero-mismatch/zero-allocation flat traversal referee are shipped; connected dual publication continues in Slice I5. | | MP1c | Streaming cutover to pak + hitch gate | ✅ **SHIPPED 2026-07-24** — typed package-only production source, explicit Setup probes, non-allocating Portal-first lookup, exact translucency metadata, and ordered teardown; capped/uncapped/dense physical routes + user visual gate pass. Uncapped CPU p99 7.825→6.496 ms, GPU p99 3.406→2.706 ms, portal frame max 202.9→39.9 MiB, process allocation −41.8%, zero invalid Setup probes. [Report](../research/2026-07-24-slice-c-prepared-asset-cutover-report.md) | | MP2 | Retail particle distance/cell-view degradation: DAT-authored range, exact infinite/finite off-view update branches, emitter-first render scan, user-requested 2× default range | ✅ SHIPPED 2026-07-17 — Aerlinthe hotspot `0x32000223` resolves to retail 64m and defaults to 128m; landscape/entity streaming distance unchanged | | MP3 | Arch ECS render world + delta submission (the 300-FPS lever) | ⚪ — note: does NOT fix the steady-state GC churn (that's MP-Alloc); MP3 is the draw-submission throughput lever | -| MP4 | Zero-alloc frame loop + flat physics data (residual, post-MP-Alloc) | 🟡 ACTIVE — Slice I1 zero-allocation transition scratch, I2 immutable schema, and I3 complete deterministic collision package shipped; I4 flat traversal shadow implementation is active. | +| MP4 | Zero-alloc frame loop + flat physics data (residual, post-MP-Alloc) | 🟡 ACTIVE — Slice I1 zero-allocation transition scratch, I2 immutable schema, I3 deterministic collision package, and I4 exact flat traversal referee shipped; I5 dual publication and connected shadow verification are active. | | MP5 | Job-system parallelism | ⚪ stretch, evidence-gated | --- diff --git a/docs/plans/2026-05-12-milestones.md b/docs/plans/2026-05-12-milestones.md index 085caaad..c9501067 100644 --- a/docs/plans/2026-05-12-milestones.md +++ b/docs/plans/2026-05-12-milestones.md @@ -128,8 +128,14 @@ prepared-collision interface over the existing single mmap. Two complete 29,908,271,024-byte bakes contain 2,232,170 keys with zero failures and the same SHA-256 across different worker counts; cancellation/corruption gates and 8,371 solution tests / 5 skips pass. +Slice I4 ports every current collision entry point to integer-indexed flat +assets behind a graph-authoritative referee. Thirteen focused tests, 10,000 +randomized static/swept comparisons, 5,000 walkable comparisons, 7,500 +installed-DAT comparisons, and 1,200 complete resolver frames have exact +state/float-bit equality; 10,000 warmed flat iterations allocate zero managed +bytes. Release build and 8,384 solution tests / 5 skips pass. Slice H is complete: retained UI/frame work, exact bounded light selection, -and pooled/borrowed/direct network I/O pass. Slice I is active at I4 under +and pooled/borrowed/direct network I/O pass. Slice I is active at I5 under [`2026-07-25-modern-runtime-slice-i.md`](2026-07-25-modern-runtime-slice-i.md). The historical exact G4 visual rollback remains `git revert ef1d263337997bb030eadb7b8e71d73dc659907a`. diff --git a/docs/plans/2026-07-25-modern-runtime-slice-i.md b/docs/plans/2026-07-25-modern-runtime-slice-i.md index 6d61b228..b65bafb4 100644 --- a/docs/plans/2026-07-25-modern-runtime-slice-i.md +++ b/docs/plans/2026-07-25-modern-runtime-slice-i.md @@ -1,8 +1,8 @@ # Modern runtime Slice I — flat collision assets and zero-allocation physics -**Status:** I0–I3 COMPLETE — retail oracle, zero-allocation transition -scratch, deterministic immutable collision records, and complete prepared -package fixed; I4 active +**Status:** I0-I4 COMPLETE - retail oracle, zero-allocation transition +scratch, deterministic immutable collision records, complete prepared +package, and exact flat-traversal differential gate fixed; I5 active **Parent:** `2026-07-24-modern-runtime-architecture.md`, Slice I **Purpose:** remove parsed DAT object graphs and steady collision allocations without changing one retail collision decision, float, comparison, or traversal @@ -213,6 +213,16 @@ tests / 5 skips pass. Production traversal remains graph-only. Evidence: Gate: zero differential mismatch. Any mismatch blocks cutover; no tolerance band is permitted for deterministic scalar results. +**PASSED 2026-07-25:** the integer-indexed shadow ports every current +containment, overlap, walkable, and six-path moving-collision entry point. +The 13-test referee completed 10,000 randomized static/swept comparisons, +5,000 walkable comparisons, 7,500 installed-DAT comparisons across three +representative cells, all six dispatcher paths, and 1,200 complete resolver +frames with exact state and float-bit equality. Ten thousand warmed flat +iterations allocated zero managed bytes. Production remains graph-authoritative +until I5/I6. Release build and 8,384 solution tests / 5 skips pass. Evidence: +[`../research/2026-07-25-slice-i4-flat-bsp-differential.md`](../research/2026-07-25-slice-i4-flat-bsp-differential.md). + ### I5 — dual publication and connected shadow gate 1. Extend collision preparation so a `LandblockBuild` carries immutable flat diff --git a/docs/research/2026-07-25-slice-i4-flat-bsp-differential.md b/docs/research/2026-07-25-slice-i4-flat-bsp-differential.md new file mode 100644 index 00000000..8176ea8c --- /dev/null +++ b/docs/research/2026-07-25-slice-i4-flat-bsp-differential.md @@ -0,0 +1,87 @@ +# Slice I4 - differential flat BSP traversal + +**Date:** 2026-07-25 + +**Scope:** Modern Runtime Slice I4 only + +**Behavioral boundary:** the new integer-indexed traversal is a shadow +implementation. The parsed-DAT graph remains production-authoritative until +the Slice I5 connected shadow gate and Slice I6 cutover. + +## Result + +`FlatBspQuery` now ports every collision entry point used by the current +runtime over immutable, integer-indexed nodes and polygon ranges: + +- point-in-cell; +- sphere/cell overlap; +- static sphere/polygon overlap; +- swept sphere/polygon time-of-impact; +- walkable polygon selection; +- solid and solid-polygon traversal; +- the complete six-path moving-collision dispatcher. + +The port preserves retail's traversal order, strict comparisons, early +returns, and mutable query state. The positive child remains first where +retail visits it first; the negative child observes every valid-sphere, +contact, path, slide, and candidate mutation made by the positive child. +Only `BSPQuery.PolygonHitsSpherePrecise`, which is independent of graph +storage, is shared with the existing graph oracle. + +`CollisionTraversal` is the narrow temporary referee seam. Production starts +in `Graph` mode. Tests can run a cloned complete resolver in `Flat` mode, and +a missing flat asset fails explicitly instead of falling back to the graph. +Slice I6 removes this representation switch after connected acceptance. + +## Retail oracle + +The implementation follows the consolidated pseudocode and boundary table in +[`2026-07-25-slice-i-collision-layout-oracle.md`](2026-07-25-slice-i-collision-layout-oracle.md), +including: + +- `BSPTREE::find_collisions` at `0x0053A440`; +- `BSPNODE::point_inside_cell_bsp` at `0x0053C1F0`; +- `BSPNODE::sphere_intersects_cell_bsp` at `0x0053C260`; +- `BSPNODE::sphere_intersects_poly` at `0x0053CA30`; +- `BSPNODE::sphere_intersects_solid` at `0x0053CAF0`; +- `BSPNODE::find_walkable` at `0x0053CC80`; +- `BSPNODE::sphere_intersects_solid_poly` at `0x0053CD50`; +- `BSPLEAF::find_walkable` at `0x0053D6F0`. + +The same oracle records the cross-checks against ACE master +`65f092...` and Chorizite DatReaderWriter `c535987...`. Retail remains the +authority where those references differ. + +## Differential evidence + +The referee compares scalar results and all stateful outputs by exact float +bits. It covers selected polygons, vectors, planes, path state, collision and +object flags, contact-plane writes, final `ResolveResult`, persistent body +state, and full cell membership. + +| Gate | Executions | Result | +|---|---:|---:| +| Null, on-plane, radius-equality, equal-candidate, multi-polygon, and deep-tree fixtures | 4 focused fixtures | 0 mismatches | +| Static and swept overlap randomized sweep | 10,000 paired cases | 0 mismatches | +| Walkable randomized sweep | 5,000 paired cases | 0 mismatches | +| Six dispatcher paths | 6 complete transition fixtures | 0 mismatches | +| Installed-DAT sweep across `0x8A02016E`, `0x8A02017A`, and `0xA9B4013F` | 7,500 randomized cases | 0 mismatches | +| Complete graph/flat resolver sequences | 1,200 frames | 0 mismatches | +| Warmed flat traversal | 10,000 iterations | 0 managed bytes | + +The installed-DAT gate randomizes points, spheres, movement, insert paths, +orientations, scale, and boundary values. No tolerance band is used. + +## Verification + +- focused differential tests: 13 passed; +- complete Core suite: 3,276 passed / 2 skipped; +- `git diff --check`: no whitespace errors; +- full Release build: passed; +- complete Release suite: 8,384 passed / 5 skipped. + +No divergence-register row changes are required. This slice changes the +storage representation exercised by the referee, not production gameplay +behavior. Slice I5 next publishes the prepared flat assets alongside the +current graph under one landblock generation and runs the connected shadow +gate. diff --git a/src/AcDream.Core/Physics/BSPQuery.cs b/src/AcDream.Core/Physics/BSPQuery.cs index b8287038..a66c5d90 100644 --- a/src/AcDream.Core/Physics/BSPQuery.cs +++ b/src/AcDream.Core/Physics/BSPQuery.cs @@ -96,7 +96,7 @@ public static class BSPQuery /// /// ACE: Polygon.cs polygon_hits_sphere_precise. /// - private static bool PolygonHitsSpherePrecise( + internal static bool PolygonHitsSpherePrecise( in Plane polyPlane, ReadOnlySpan verts, Vector3 sphereCenter, @@ -2339,6 +2339,31 @@ public static class BSPQuery ref hitPolyId, ref hitNormal); } + /// + /// Allocation-free graph-oracle seam used by the Slice I flat traversal + /// differential harness. + /// + internal static bool SphereIntersectsPoly( + PhysicsBSPNode? node, + Dictionary resolved, + Vector3 sphereCenter, + float sphereRadius, + out ushort hitPolyId, + out Vector3 hitNormal) + { + hitPolyId = 0; + hitNormal = Vector3.Zero; + if (node is null) return false; + + return SphereIntersectsPolyStaticRecurse( + node, + resolved, + sphereCenter, + sphereRadius, + ref hitPolyId, + ref hitNormal); + } + private static bool SphereIntersectsPolyStaticRecurse( PhysicsBSPNode? node, Dictionary resolved, @@ -2431,6 +2456,38 @@ public static class BSPQuery return hitTime < float.MaxValue; } + /// + /// Allocation-free graph-oracle seam used by the Slice I flat traversal + /// differential harness. + /// + internal static bool SphereIntersectsPolyWithTime( + PhysicsBSPNode? node, + Dictionary resolved, + Vector3 sphereCenter, + float sphereRadius, + Vector3 movement, + out ushort hitPolyId, + out Vector3 hitNormal, + out float hitTime) + { + hitPolyId = 0; + hitNormal = Vector3.Zero; + hitTime = float.MaxValue; + if (node is null) return false; + + SphereIntersectsPolyWithTimeRecurse( + node, + resolved, + sphereCenter, + sphereRadius, + movement, + ref hitPolyId, + ref hitNormal, + ref hitTime); + + return hitTime < float.MaxValue; + } + private static void SphereIntersectsPolyWithTimeRecurse( PhysicsBSPNode? node, Dictionary resolved, diff --git a/src/AcDream.Core/Physics/CellTransit.cs b/src/AcDream.Core/Physics/CellTransit.cs index 694d15d7..f84737d2 100644 --- a/src/AcDream.Core/Physics/CellTransit.cs +++ b/src/AcDream.Core/Physics/CellTransit.cs @@ -159,15 +159,19 @@ public static class CellTransit // neighbour cell whether the sphere intersects its CellBSP. // The portal-plane side test is only the unloaded-cell load hint. var otherCell = cache.GetCellStruct(otherId); - if (otherCell?.CellBSP?.Root is not null) + if (otherCell is not null && + CollisionTraversal.HasCellContainment(cache, otherCell)) { for (int i = 0; i < sphereCount; i++) { var sphere = worldSpheres[i]; var otherLocalCenter = Vector3.Transform( sphere.Origin, otherCell.InverseWorldTransform); - bool hit = BSPQuery.SphereIntersectsCellBsp( - otherCell.CellBSP.Root, otherLocalCenter, sphere.Radius); + bool hit = CollisionTraversal.SphereIntersectsCell( + cache, + otherCell, + otherLocalCenter, + sphere.Radius); if (hit) { candidates.Add(otherId); @@ -372,7 +376,8 @@ public static class CellTransit continue; var otherCell = cache.GetCellStruct(portal.OtherCellId); - if (otherCell?.CellBSP?.Root is null) + if (otherCell is null || + !CollisionTraversal.HasCellContainment(cache, otherCell)) { if (PhysicsDiagnostics.ProbeIndoorBspEnabled) { @@ -398,8 +403,11 @@ public static class CellTransit { var sphere = worldSpheres[i]; var localCenter = Vector3.Transform(sphere.Origin, otherCell.InverseWorldTransform); - inside = BSPQuery.SphereIntersectsCellBsp( - otherCell.CellBSP.Root, localCenter, sphere.Radius); + inside = CollisionTraversal.SphereIntersectsCell( + cache, + otherCell, + localCenter, + sphere.Radius); if (PhysicsDiagnostics.ProbeIndoorBspEnabled) { @@ -599,19 +607,25 @@ public static class CellTransit if (start is null) return 0u; // this->point_in_cell(point) → return this (:311402-311405) - if (PointInCell(start, worldPoint)) return startCellId; + if (PointInCell(cache, start, worldPoint)) return startCellId; if (useStabList) { // arg3 != 0 → iterate stab_list, GetVisible + point_in_cell (:311444-311465) foreach (uint id in start.VisibleCellIds) - if (PointInCell(cache.GetCellStruct(id), worldPoint)) return id; + if (PointInCell(cache, cache.GetCellStruct(id), worldPoint)) return id; } else { // arg3 == 0 → iterate direct portals, GetOtherCell + point_in_cell (:311411-311434) foreach (var portal in start.Portals) - if (PointInCell(cache.GetCellStruct(portal.OtherCellId), worldPoint)) return portal.OtherCellId; + if (PointInCell( + cache, + cache.GetCellStruct(portal.OtherCellId), + worldPoint)) + { + return portal.OtherCellId; + } } return 0u; @@ -623,11 +637,19 @@ public static class CellTransit /// A cell with no hydrated returns false (see /// 's adaptation note). /// - private static bool PointInCell(CellPhysics? cell, Vector3 worldPoint) + private static bool PointInCell( + PhysicsDataCache cache, + CellPhysics? cell, + Vector3 worldPoint) { - if (cell?.CellBSP?.Root is null) return false; + if (cell is null || + !CollisionTraversal.HasCellContainment(cache, cell)) + { + return false; + } + var local = Vector3.Transform(worldPoint, cell.InverseWorldTransform); - return BSPQuery.PointInsideCellBsp(cell.CellBSP.Root, local); + return CollisionTraversal.PointInsideCell(cache, cell, local); } /// @@ -901,9 +923,14 @@ public static class CellTransit { // Interior candidate — point_in_cell via the cell BSP (vtable[0x84]). var cand = cache.GetCellStruct(candId); - if (cand?.CellBSP?.Root is null) continue; + if (cand is null || + !CollisionTraversal.HasCellContainment(cache, cand)) + { + continue; + } + var local = Vector3.Transform(worldSphereCenter, cand.InverseWorldTransform); - if (BSPQuery.PointInsideCellBsp(cand.CellBSP.Root, local)) + if (CollisionTraversal.PointInsideCell(cache, cand, local)) return candId; // interior-wins, stop (pseudo_c:308819) } else if (outdoorResult == 0u && containingOutdoorId != 0u && outdoorPickAllowed) @@ -950,10 +977,15 @@ public static class CellTransit if (currentLow >= 0x0100u) { var cur = cache.GetCellStruct(currentCellId); - if (cur?.CellBSP?.Root is not null) + if (cur is not null && + CollisionTraversal.HasCellContainment(cache, cur)) { var curLocal = Vector3.Transform(worldSphereCenter, cur.InverseWorldTransform); - if (!BSPQuery.SphereIntersectsCellBsp(cur.CellBSP.Root, curLocal, sphereRadius)) + if (!CollisionTraversal.SphereIntersectsCell( + cache, + cur, + curLocal, + sphereRadius)) { uint recovered = FindVisibleChildCell( cache, currentCellId, worldSphereCenter, useStabList: true); diff --git a/src/AcDream.Core/Physics/CollisionTraversal.cs b/src/AcDream.Core/Physics/CollisionTraversal.cs new file mode 100644 index 00000000..85f93815 --- /dev/null +++ b/src/AcDream.Core/Physics/CollisionTraversal.cs @@ -0,0 +1,226 @@ +using System.Numerics; + +namespace AcDream.Core.Physics; + +/// +/// Temporary Slice I referee mode. Graph remains production-authoritative; +/// Flat is enabled only by differential tests until dual publication and the +/// connected shadow gate complete. +/// +internal enum CollisionTraversalMode +{ + Graph, + Flat, +} + +/// +/// Narrow representation switch used while the graph and flat collision +/// worlds coexist. I6 removes the graph branch after acceptance. +/// +internal static class CollisionTraversal +{ + internal static bool HasCellContainment( + PhysicsDataCache cache, + CellPhysics cell) + { + if (cache.CollisionTraversalMode == CollisionTraversalMode.Flat) + { + FlatCellContainmentBsp flat = cell.FlatContainmentBsp ?? + throw MissingFlat("cell containment"); + return flat.RootIndex >= 0; + } + + return cell.CellBSP?.Root is not null; + } + + internal static bool HasPhysics( + PhysicsDataCache cache, + CellPhysics cell) + { + if (cache.CollisionTraversalMode == CollisionTraversalMode.Flat) + { + FlatPhysicsBsp flat = cell.FlatPhysicsBsp ?? + throw MissingFlat("cell physics"); + return flat.RootIndex >= 0; + } + + return cell.BSP?.Root is not null; + } + + internal static bool HasPhysics( + PhysicsDataCache cache, + GfxObjPhysics gfxObject) + { + if (cache.CollisionTraversalMode == CollisionTraversalMode.Flat) + { + FlatPhysicsBsp flat = gfxObject.FlatPhysicsBsp ?? + throw MissingFlat("GfxObj physics"); + return flat.RootIndex >= 0; + } + + return gfxObject.BSP.Root is not null; + } + + internal static FlatCollisionSphere RootBoundingSphere( + PhysicsDataCache cache, + CellPhysics cell) + { + if (cache.CollisionTraversalMode == CollisionTraversalMode.Flat) + { + FlatPhysicsBsp flat = cell.FlatPhysicsBsp ?? + throw MissingFlat("cell physics"); + return flat.Nodes[flat.RootIndex].BoundingSphere; + } + + DatReaderWriter.Types.Sphere sphere = cell.BSP!.Root!.BoundingSphere; + return new FlatCollisionSphere(sphere.Origin, sphere.Radius); + } + + internal static bool PointInsideCell( + PhysicsDataCache cache, + CellPhysics cell, + Vector3 localPoint) + { + if (cache.CollisionTraversalMode == CollisionTraversalMode.Flat) + { + FlatCellContainmentBsp flat = cell.FlatContainmentBsp ?? + throw MissingFlat("cell containment"); + return FlatBspQuery.PointInsideCellBsp(flat, localPoint); + } + + return BSPQuery.PointInsideCellBsp(cell.CellBSP?.Root, localPoint); + } + + internal static bool SphereIntersectsCell( + PhysicsDataCache cache, + CellPhysics cell, + Vector3 localCenter, + float radius) + { + if (cache.CollisionTraversalMode == CollisionTraversalMode.Flat) + { + FlatCellContainmentBsp flat = cell.FlatContainmentBsp ?? + throw MissingFlat("cell containment"); + return FlatBspQuery.SphereIntersectsCellBsp( + flat, + localCenter, + radius); + } + + return BSPQuery.SphereIntersectsCellBsp( + cell.CellBSP?.Root, + localCenter, + radius); + } + + internal static TransitionState FindCollisions( + PhysicsDataCache cache, + CellPhysics cell, + Transition transition, + Vector3 localSphereCenter, + float localSphereRadius, + bool hasLocalSphere1, + Vector3 localSphere1Center, + float localSphere1Radius, + Vector3 localCurrentCenter, + Vector3 localSpaceZ, + float scale, + Quaternion localToWorld, + PhysicsEngine? engine, + Vector3 worldOrigin) + { + if (cache.CollisionTraversalMode == CollisionTraversalMode.Flat) + { + FlatPhysicsBsp flat = cell.FlatPhysicsBsp ?? + throw MissingFlat("cell physics"); + return FlatBspQuery.FindCollisions( + flat, + transition, + localSphereCenter, + localSphereRadius, + hasLocalSphere1, + localSphere1Center, + localSphere1Radius, + localCurrentCenter, + localSpaceZ, + scale, + localToWorld, + engine, + worldOrigin); + } + + return BSPQuery.FindCollisions( + cell.BSP?.Root, + cell.Resolved, + transition, + localSphereCenter, + localSphereRadius, + hasLocalSphere1, + localSphere1Center, + localSphere1Radius, + localCurrentCenter, + localSpaceZ, + scale, + localToWorld, + engine, + worldOrigin); + } + + internal static TransitionState FindCollisions( + PhysicsDataCache cache, + GfxObjPhysics gfxObject, + Transition transition, + Vector3 localSphereCenter, + float localSphereRadius, + bool hasLocalSphere1, + Vector3 localSphere1Center, + float localSphere1Radius, + Vector3 localCurrentCenter, + Vector3 localSpaceZ, + float scale, + Quaternion localToWorld, + PhysicsEngine? engine, + Vector3 worldOrigin) + { + if (cache.CollisionTraversalMode == CollisionTraversalMode.Flat) + { + FlatPhysicsBsp flat = gfxObject.FlatPhysicsBsp ?? + throw MissingFlat("GfxObj physics"); + return FlatBspQuery.FindCollisions( + flat, + transition, + localSphereCenter, + localSphereRadius, + hasLocalSphere1, + localSphere1Center, + localSphere1Radius, + localCurrentCenter, + localSpaceZ, + scale, + localToWorld, + engine, + worldOrigin); + } + + return BSPQuery.FindCollisions( + gfxObject.BSP.Root, + gfxObject.Resolved, + transition, + localSphereCenter, + localSphereRadius, + hasLocalSphere1, + localSphere1Center, + localSphere1Radius, + localCurrentCenter, + localSpaceZ, + scale, + localToWorld, + engine, + worldOrigin); + } + + private static InvalidOperationException MissingFlat(string kind) => + new( + $"Flat collision traversal requires a prepared {kind} asset. " + + "Gameplay must not fall back silently."); +} diff --git a/src/AcDream.Core/Physics/FlatBspQuery.cs b/src/AcDream.Core/Physics/FlatBspQuery.cs new file mode 100644 index 00000000..dd39e821 --- /dev/null +++ b/src/AcDream.Core/Physics/FlatBspQuery.cs @@ -0,0 +1,2132 @@ +using System.Numerics; +using DatReaderWriter.Enums; + +namespace AcDream.Core.Physics; + +/// +/// Integer-indexed shadow port of the retail BSP traversals in +/// . The graph implementation remains authoritative +/// until the Slice I differential and connected gates pass with zero +/// mismatches. +/// +/// +/// Named-retail oracle: BSPTREE::find_collisions 0x0053A440, +/// BSPNODE::point_inside_cell_bsp 0x0053C1F0, +/// BSPNODE::sphere_intersects_cell_bsp 0x0053C260, +/// BSPNODE::sphere_intersects_poly 0x0053CA30, +/// BSPNODE::sphere_intersects_solid 0x0053CAF0, +/// BSPNODE::find_walkable 0x0053CC80, +/// BSPNODE::sphere_intersects_solid_poly 0x0053CD50, and +/// BSPLEAF::find_walkable 0x0053D6F0. Polygon math is shared +/// deliberately with the graph oracle; only node and polygon storage changes. +/// +/// +internal static class FlatBspQuery +{ + private struct CollisionSphere + { + public Vector3 Center; + public float Radius; + + public CollisionSphere(Vector3 center, float radius) + { + Center = center; + Radius = radius; + } + } + + private static ReadOnlySpan Vertices( + FlatPhysicsBsp tree, + in FlatCollisionPolygon polygon) + => tree.PolygonTable.Vertices.AsSpan( + polygon.VertexRange.Start, + polygon.VertexRange.Count); + + private static bool NodeIntersects( + in FlatPhysicsBspNode node, + CollisionSphere sphere) + { + Vector3 d = sphere.Center - node.BoundingSphere.Origin; + float radius = sphere.Radius + node.BoundingSphere.Radius; + return d.LengthSquared() < radius * radius; + } + + private static bool PosHitsSphere( + FlatPhysicsBsp tree, + int polygonIndex, + CollisionSphere sphere, + Vector3 movement, + ref Vector3 contactPoint, + ref int hitPolygonIndex) + { + FlatCollisionPolygon polygon = tree.PolygonTable.Polygons[polygonIndex]; + bool hit = BSPQuery.PolygonHitsSpherePrecise( + polygon.Plane, + Vertices(tree, polygon), + sphere.Center, + sphere.Radius, + ref contactPoint); + + // Retail writes the polygon before the movement-direction cull. + if (hit) + hitPolygonIndex = polygonIndex; + + float moveDot = Vector3.Dot(movement, polygon.Plane.Normal); + if (moveDot >= 0f) + return false; + + return hit; + } + + private static bool HitsSphere( + FlatPhysicsBsp tree, + int polygonIndex, + CollisionSphere sphere) + { + FlatCollisionPolygon polygon = tree.PolygonTable.Polygons[polygonIndex]; + Vector3 contactPoint = Vector3.Zero; + return BSPQuery.PolygonHitsSpherePrecise( + polygon.Plane, + Vertices(tree, polygon), + sphere.Center, + sphere.Radius, + ref contactPoint); + } + + private static bool WalkableHitsSphere( + FlatPhysicsBsp tree, + int polygonIndex, + SpherePath path, + CollisionSphere sphere, + Vector3 up) + { + FlatCollisionPolygon polygon = tree.PolygonTable.Polygons[polygonIndex]; + float dp = Vector3.Dot(up, polygon.Plane.Normal); + if (dp <= path.WalkableAllowance) + return false; + + Vector3 contactPoint = Vector3.Zero; + return BSPQuery.PolygonHitsSpherePrecise( + polygon.Plane, + Vertices(tree, polygon), + sphere.Center, + sphere.Radius, + ref contactPoint); + } + + private static bool CheckWalkable( + FlatPhysicsBsp tree, + int polygonIndex, + CollisionSphere sphere, + Vector3 up, + bool small) + { + FlatCollisionPolygon polygon = tree.PolygonTable.Polygons[polygonIndex]; + ReadOnlySpan vertices = Vertices(tree, polygon); + + float angleUp = Vector3.Dot(polygon.Plane.Normal, up); + if (angleUp < PhysicsGlobals.EPSILON) + return false; + + float angle = + (Vector3.Dot(polygon.Plane.Normal, sphere.Center) + polygon.Plane.D) / + angleUp; + Vector3 center = sphere.Center - up * angle; + + float radiusSquared = sphere.Radius * sphere.Radius; + if (small) + radiusSquared *= 0.25f; + + int previousIndex = vertices.Length - 1; + for (int i = 0; i < vertices.Length; i++) + { + Vector3 vertex = vertices[i]; + Vector3 previousVertex = vertices[previousIndex]; + previousIndex = i; + + Vector3 edge = vertex - previousVertex; + Vector3 displacement = center - previousVertex; + Vector3 cross = Vector3.Cross(polygon.Plane.Normal, edge); + float difference = Vector3.Dot(displacement, cross); + + if (difference < 0f) + { + if (cross.LengthSquared() * radiusSquared < difference * difference) + return false; + + float displacementAlongEdge = Vector3.Dot(displacement, edge); + if (displacementAlongEdge >= 0f && + displacementAlongEdge <= edge.LengthSquared()) + { + return true; + } + + return false; + } + + if (displacement.LengthSquared() <= radiusSquared) + return true; + } + + return true; + } + + private static bool AdjustSphereToPlane( + FlatPhysicsBsp tree, + int polygonIndex, + SpherePath path, + ref CollisionSphere validPosition, + Vector3 movement) + { + FlatCollisionPolygon polygon = tree.PolygonTable.Polygons[polygonIndex]; + + Vector3 inputCenter = validPosition.Center; + float walkInterpolationBefore = path.WalkInterp; + float positionDot = + Vector3.Dot(validPosition.Center, polygon.Plane.Normal) + polygon.Plane.D; + float movementDot = Vector3.Dot(movement, polygon.Plane.Normal); + float distance; + + if (movementDot <= PhysicsGlobals.EPSILON) + { + if (movementDot >= -PhysicsGlobals.EPSILON) + { + LogPushBackAdjust( + tree, + polygonIndex, + inputCenter, + validPosition.Center, + validPosition.Radius, + walkInterpolationBefore, + path.WalkInterp, + positionDot, + movementDot, + 0f, + applied: false); + return false; + } + + distance = positionDot - validPosition.Radius; + } + else + { + distance = -validPosition.Radius - positionDot; + } + + float inverseDistance = distance / movementDot; + float interpolation = (1f - inverseDistance) * path.WalkInterp; + + if (interpolation >= path.WalkInterp || interpolation < -0.5f) + { + LogPushBackAdjust( + tree, + polygonIndex, + inputCenter, + validPosition.Center, + validPosition.Radius, + walkInterpolationBefore, + path.WalkInterp, + positionDot, + movementDot, + inverseDistance, + applied: false); + return false; + } + + validPosition.Center -= movement * inverseDistance; + path.WalkInterp = interpolation; + + LogPushBackAdjust( + tree, + polygonIndex, + inputCenter, + validPosition.Center, + validPosition.Radius, + walkInterpolationBefore, + path.WalkInterp, + positionDot, + movementDot, + inverseDistance, + applied: true); + + if (PhysicsDiagnostics.ProbePolyDumpEnabled) + { + PhysicsDiagnostics.LogPolyDump( + path.CheckCellId, + MaterializeResolvedPolygon(tree, polygonIndex)); + } + + return true; + } + + private static void LogPushBackAdjust( + FlatPhysicsBsp tree, + int polygonIndex, + Vector3 inputCenter, + Vector3 outputCenter, + float radius, + float walkInterpolationBefore, + float walkInterpolationAfter, + float positionDot, + float movementDot, + float inverseDistance, + bool applied) + { + if (!PhysicsDiagnostics.ProbePushBackEnabled) + return; + + PhysicsDiagnostics.LogPushBackAdjust( + inputCenter, + outputCenter, + tree.PolygonTable.Polygons[polygonIndex].Plane, + radius, + walkInterpolationBefore, + walkInterpolationAfter, + positionDot, + movementDot, + inverseDistance, + applied); + } + + private static bool FindCrossedEdge( + FlatPhysicsBsp tree, + int polygonIndex, + CollisionSphere sphere, + Vector3 up, + ref Vector3 normal) + { + FlatCollisionPolygon polygon = tree.PolygonTable.Polygons[polygonIndex]; + if (!BSPQuery.FindCrossedEdge( + polygon.Plane, + Vertices(tree, polygon), + sphere.Center, + up, + out Vector3 crossedNormal)) + { + return false; + } + + normal = crossedNormal; + return true; + } + + private static Vector3 TransformNormal(Vector3 normal, Quaternion localToWorld) + { + Vector3 worldNormal = Vector3.Transform(normal, localToWorld); + return worldNormal.LengthSquared() > PhysicsGlobals.EpsilonSq + ? Vector3.Normalize(worldNormal) + : Vector3.UnitZ; + } + + private static Plane BuildWorldPlane( + Vector3 worldNormal, + ReadOnlySpan localVertices, + Quaternion localToWorld, + float scale, + Vector3 worldOrigin) + { + float d = localVertices.Length > 0 + ? -Vector3.Dot( + worldNormal, + Vector3.Transform(localVertices[0] * scale, localToWorld) + worldOrigin) + : 0f; + return new Plane(worldNormal, d); + } + + private static void AdjustToPlacementPolygon( + FlatPhysicsBsp tree, + int polygonIndex, + ref CollisionSphere validPosition, + ref CollisionSphere validPosition2, + bool hasValidPosition2, + float radius, + bool centerSolid, + bool clearCell) + { + FlatCollisionPolygon polygon = tree.PolygonTable.Polygons[polygonIndex]; + Vector3 moveDirection = Vector3.Zero; + + if (centerSolid) + { + moveDirection = polygon.Plane.Normal; + } + else + { + Vector3 up = Vector3.UnitZ; + if (!FindCrossedEdge( + tree, + polygonIndex, + validPosition, + up, + ref moveDirection)) + { + moveDirection = polygon.Plane.Normal; + } + } + + float distance = + Vector3.Dot(validPosition.Center, polygon.Plane.Normal) + polygon.Plane.D; + float pushAmount = radius - distance; + if (pushAmount <= 0f) + pushAmount = PhysicsGlobals.EPSILON; + + Vector3 offset = moveDirection * pushAmount; + validPosition.Center += offset; + if (hasValidPosition2) + validPosition2.Center += offset; + } + + private static float AdjustSphereToPolygon( + FlatPhysicsBsp tree, + int polygonIndex, + CollisionSphere checkPosition, + Vector3 currentPosition, + Vector3 movement) + { + FlatCollisionPolygon polygon = tree.PolygonTable.Polygons[polygonIndex]; + float positionDot = + Vector3.Dot(currentPosition, polygon.Plane.Normal) + polygon.Plane.D; + if (MathF.Abs(positionDot) < checkPosition.Radius) + return 1f; + + float movementDot = Vector3.Dot(movement, polygon.Plane.Normal); + if (MathF.Abs(movementDot) <= PhysicsGlobals.EPSILON) + return 0f; + + float radius = positionDot < 0f + ? -checkPosition.Radius + : checkPosition.Radius; + return (radius - positionDot) / movementDot; + } + + /// + /// Flat port of retail BSPNODE::point_inside_cell_bsp + /// (0x0053C1F0). + /// + public static bool PointInsideCellBsp( + FlatCellContainmentBsp tree, + Vector3 point) + { + ArgumentNullException.ThrowIfNull(tree); + return PointInsideCellBsp(tree, tree.RootIndex, point); + } + + private static bool PointInsideCellBsp( + FlatCellContainmentBsp tree, + int nodeIndex, + Vector3 point) + { + if (nodeIndex < 0) + return true; + + FlatCellBspNode node = tree.Nodes[nodeIndex]; + if (node.Type == BSPNodeType.Leaf) + return true; + + float distance = + Vector3.Dot(node.SplittingPlane.Normal, point) + node.SplittingPlane.D; + if (distance >= 0f) + { + return node.PositiveChildIndex >= 0 + ? PointInsideCellBsp(tree, node.PositiveChildIndex, point) + : true; + } + + return false; + } + + /// + /// Flat port of retail BSPNODE::sphere_intersects_cell_bsp + /// (0x0053C260). + /// + public static bool SphereIntersectsCellBsp( + FlatCellContainmentBsp tree, + Vector3 center, + float radius) + { + ArgumentNullException.ThrowIfNull(tree); + return SphereIntersectsCellBsp(tree, tree.RootIndex, center, radius); + } + + private static bool SphereIntersectsCellBsp( + FlatCellContainmentBsp tree, + int nodeIndex, + Vector3 center, + float radius) + { + if (nodeIndex < 0) + return true; + + FlatCellBspNode node = tree.Nodes[nodeIndex]; + if (node.Type == BSPNodeType.Leaf) + return true; + + float distance = + Vector3.Dot(node.SplittingPlane.Normal, center) + node.SplittingPlane.D; + float expandedRadius = radius + 0.01f; + if (distance < -expandedRadius) + return false; + + return node.PositiveChildIndex >= 0 + ? SphereIntersectsCellBsp( + tree, + node.PositiveChildIndex, + center, + radius) + : true; + } + + /// + /// Flat static sphere/polygon overlap shadow query. + /// + public static bool SphereIntersectsPoly( + FlatPhysicsBsp tree, + Vector3 sphereCenter, + float sphereRadius, + out ushort hitPolygonId, + out Vector3 hitNormal) + { + ArgumentNullException.ThrowIfNull(tree); + hitPolygonId = 0; + hitNormal = Vector3.Zero; + if (tree.RootIndex < 0) + return false; + + return SphereIntersectsPolyStaticRecurse( + tree, + tree.RootIndex, + sphereCenter, + sphereRadius, + ref hitPolygonId, + ref hitNormal); + } + + private static bool SphereIntersectsPolyStaticRecurse( + FlatPhysicsBsp tree, + int nodeIndex, + Vector3 center, + float radius, + ref ushort hitPolygonId, + ref Vector3 hitNormal) + { + if (nodeIndex < 0) + return false; + + FlatPhysicsBspNode node = tree.Nodes[nodeIndex]; + Vector3 difference = center - node.BoundingSphere.Origin; + float combinedRadius = radius + node.BoundingSphere.Radius; + if (difference.LengthSquared() >= combinedRadius * combinedRadius) + return false; + + if (node.Type == BSPNodeType.Leaf) + { + int end = node.PolygonIndexRange.EndExclusive; + for (int i = node.PolygonIndexRange.Start; i < end; i++) + { + int polygonIndex = tree.PolygonIndexStream[i]; + FlatCollisionPolygon polygon = + tree.PolygonTable.Polygons[polygonIndex]; + Vector3 contactPoint = Vector3.Zero; + if (BSPQuery.PolygonHitsSpherePrecise( + polygon.Plane, + Vertices(tree, polygon), + center, + radius, + ref contactPoint)) + { + hitPolygonId = polygon.Id; + hitNormal = polygon.Plane.Normal; + return true; + } + } + + return false; + } + + float splitDistance = + Vector3.Dot(node.SplittingPlane.Normal, center) + node.SplittingPlane.D; + float reach = radius - PhysicsGlobals.EPSILON; + + if (splitDistance >= reach) + { + return SphereIntersectsPolyStaticRecurse( + tree, + node.PositiveChildIndex, + center, + radius, + ref hitPolygonId, + ref hitNormal); + } + + if (splitDistance <= -reach) + { + return SphereIntersectsPolyStaticRecurse( + tree, + node.NegativeChildIndex, + center, + radius, + ref hitPolygonId, + ref hitNormal); + } + + if (SphereIntersectsPolyStaticRecurse( + tree, + node.PositiveChildIndex, + center, + radius, + ref hitPolygonId, + ref hitNormal)) + { + return true; + } + + return SphereIntersectsPolyStaticRecurse( + tree, + node.NegativeChildIndex, + center, + radius, + ref hitPolygonId, + ref hitNormal); + } + + /// + /// Flat swept-sphere BSP shadow query. + /// + public static bool SphereIntersectsPolyWithTime( + FlatPhysicsBsp tree, + Vector3 sphereCenter, + float sphereRadius, + Vector3 movement, + out ushort hitPolygonId, + out Vector3 hitNormal, + out float hitTime) + { + ArgumentNullException.ThrowIfNull(tree); + hitPolygonId = 0; + hitNormal = Vector3.Zero; + hitTime = float.MaxValue; + if (tree.RootIndex < 0) + return false; + + SphereIntersectsPolyWithTimeRecurse( + tree, + tree.RootIndex, + sphereCenter, + sphereRadius, + movement, + ref hitPolygonId, + ref hitNormal, + ref hitTime); + + return hitTime < float.MaxValue; + } + + private static void SphereIntersectsPolyWithTimeRecurse( + FlatPhysicsBsp tree, + int nodeIndex, + Vector3 center, + float radius, + Vector3 movement, + ref ushort hitPolygonId, + ref Vector3 hitNormal, + ref float bestTime) + { + if (nodeIndex < 0) + return; + + FlatPhysicsBspNode node = tree.Nodes[nodeIndex]; + Vector3 difference = center - node.BoundingSphere.Origin; + float combinedRadius = + radius + node.BoundingSphere.Radius + movement.Length() + 0.1f; + if (difference.LengthSquared() >= combinedRadius * combinedRadius) + return; + + if (node.Type == BSPNodeType.Leaf) + { + int end = node.PolygonIndexRange.EndExclusive; + for (int i = node.PolygonIndexRange.Start; i < end; i++) + { + int polygonIndex = tree.PolygonIndexStream[i]; + FlatCollisionPolygon polygon = + tree.PolygonTable.Polygons[polygonIndex]; + if (Vector3.Dot(movement, polygon.Plane.Normal) >= 0f) + continue; + + Vector3 contactPoint = Vector3.Zero; + if (BSPQuery.PolygonHitsSpherePrecise( + polygon.Plane, + Vertices(tree, polygon), + center, + radius, + ref contactPoint)) + { + if (0f < bestTime) + { + bestTime = 0f; + hitPolygonId = polygon.Id; + hitNormal = polygon.Plane.Normal; + } + + continue; + } + + Vector3 endCenter = center + movement; + if (BSPQuery.PolygonHitsSpherePrecise( + polygon.Plane, + Vertices(tree, polygon), + endCenter, + radius, + ref contactPoint) && + 1f < bestTime) + { + bestTime = 1f; + hitPolygonId = polygon.Id; + hitNormal = polygon.Plane.Normal; + } + } + + return; + } + + float splitDistance = + Vector3.Dot(node.SplittingPlane.Normal, center) + node.SplittingPlane.D; + float reach = radius + movement.Length(); + + if (splitDistance >= reach) + { + SphereIntersectsPolyWithTimeRecurse( + tree, + node.PositiveChildIndex, + center, + radius, + movement, + ref hitPolygonId, + ref hitNormal, + ref bestTime); + return; + } + + if (splitDistance <= -reach) + { + SphereIntersectsPolyWithTimeRecurse( + tree, + node.NegativeChildIndex, + center, + radius, + movement, + ref hitPolygonId, + ref hitNormal, + ref bestTime); + return; + } + + SphereIntersectsPolyWithTimeRecurse( + tree, + node.PositiveChildIndex, + center, + radius, + movement, + ref hitPolygonId, + ref hitNormal, + ref bestTime); + SphereIntersectsPolyWithTimeRecurse( + tree, + node.NegativeChildIndex, + center, + radius, + movement, + ref hitPolygonId, + ref hitNormal, + ref bestTime); + } + + private static bool SphereIntersectsPolyInternal( + FlatPhysicsBsp tree, + int nodeIndex, + CollisionSphere sphere, + Vector3 movement, + ref int hitPolygonIndex, + ref Vector3 contactPoint) + { + if (nodeIndex < 0) + return false; + + FlatPhysicsBspNode node = tree.Nodes[nodeIndex]; + if (!NodeIntersects(node, sphere)) + return false; + + if (node.Type == BSPNodeType.Leaf) + { + if (node.PolygonIndexRange.Count == 0) + return false; + + int end = node.PolygonIndexRange.EndExclusive; + for (int i = node.PolygonIndexRange.Start; i < end; i++) + { + int polygonIndex = tree.PolygonIndexStream[i]; + if (PosHitsSphere( + tree, + polygonIndex, + sphere, + movement, + ref contactPoint, + ref hitPolygonIndex)) + { + return true; + } + } + + return false; + } + + float distance = + Vector3.Dot(node.SplittingPlane.Normal, sphere.Center) + + node.SplittingPlane.D; + float reach = sphere.Radius - PhysicsGlobals.EPSILON; + + if (distance >= reach) + { + return SphereIntersectsPolyInternal( + tree, + node.PositiveChildIndex, + sphere, + movement, + ref hitPolygonIndex, + ref contactPoint); + } + + if (distance <= -reach) + { + return SphereIntersectsPolyInternal( + tree, + node.NegativeChildIndex, + sphere, + movement, + ref hitPolygonIndex, + ref contactPoint); + } + + if (node.PositiveChildIndex >= 0 && + SphereIntersectsPolyInternal( + tree, + node.PositiveChildIndex, + sphere, + movement, + ref hitPolygonIndex, + ref contactPoint)) + { + return true; + } + + if (node.NegativeChildIndex >= 0 && + SphereIntersectsPolyInternal( + tree, + node.NegativeChildIndex, + sphere, + movement, + ref hitPolygonIndex, + ref contactPoint)) + { + return true; + } + + return false; + } + + private static void FindWalkableInternal( + FlatPhysicsBsp tree, + int nodeIndex, + SpherePath path, + ref CollisionSphere validPosition, + Vector3 movement, + Vector3 up, + ref int hitPolygonIndex, + ref ushort hitPolygonId, + ref bool changed) + { + if (nodeIndex < 0) + return; + + FlatPhysicsBspNode node = tree.Nodes[nodeIndex]; + if (!NodeIntersects(node, validPosition)) + return; + + if (node.Type == BSPNodeType.Leaf) + { + if (node.PolygonIndexRange.Count == 0) + return; + + int end = node.PolygonIndexRange.EndExclusive; + for (int i = node.PolygonIndexRange.Start; i < end; i++) + { + int polygonIndex = tree.PolygonIndexStream[i]; + bool walkable = WalkableHitsSphere( + tree, + polygonIndex, + path, + validPosition, + up); + bool adjusted = walkable && + AdjustSphereToPlane( + tree, + polygonIndex, + path, + ref validPosition, + movement); + + if (walkable && adjusted) + { + changed = true; + hitPolygonIndex = polygonIndex; + hitPolygonId = tree.PolygonTable.Polygons[polygonIndex].Id; + } + } + + return; + } + + float distance = + Vector3.Dot(node.SplittingPlane.Normal, validPosition.Center) + + node.SplittingPlane.D; + float reach = validPosition.Radius - PhysicsGlobals.EPSILON; + + if (distance >= reach) + { + FindWalkableInternal( + tree, + node.PositiveChildIndex, + path, + ref validPosition, + movement, + up, + ref hitPolygonIndex, + ref hitPolygonId, + ref changed); + return; + } + + if (distance <= -reach) + { + FindWalkableInternal( + tree, + node.NegativeChildIndex, + path, + ref validPosition, + movement, + up, + ref hitPolygonIndex, + ref hitPolygonId, + ref changed); + return; + } + + FindWalkableInternal( + tree, + node.PositiveChildIndex, + path, + ref validPosition, + movement, + up, + ref hitPolygonIndex, + ref hitPolygonId, + ref changed); + FindWalkableInternal( + tree, + node.NegativeChildIndex, + path, + ref validPosition, + movement, + up, + ref hitPolygonIndex, + ref hitPolygonId, + ref changed); + } + + private static bool HitsWalkableInternal( + FlatPhysicsBsp tree, + int nodeIndex, + SpherePath path, + CollisionSphere sphere, + Vector3 up) + { + if (nodeIndex < 0) + return false; + + FlatPhysicsBspNode node = tree.Nodes[nodeIndex]; + if (!NodeIntersects(node, sphere)) + return false; + + if (node.Type == BSPNodeType.Leaf) + { + if (node.PolygonIndexRange.Count == 0) + return false; + + int end = node.PolygonIndexRange.EndExclusive; + for (int i = node.PolygonIndexRange.Start; i < end; i++) + { + int polygonIndex = tree.PolygonIndexStream[i]; + if (WalkableHitsSphere(tree, polygonIndex, path, sphere, up) && + CheckWalkable(tree, polygonIndex, sphere, up, small: true)) + { + return true; + } + } + + return false; + } + + float distance = + Vector3.Dot(node.SplittingPlane.Normal, sphere.Center) + + node.SplittingPlane.D; + float reach = sphere.Radius - PhysicsGlobals.EPSILON; + + if (distance >= reach) + { + return HitsWalkableInternal( + tree, + node.PositiveChildIndex, + path, + sphere, + up); + } + + if (distance <= -reach) + { + return HitsWalkableInternal( + tree, + node.NegativeChildIndex, + path, + sphere, + up); + } + + if (HitsWalkableInternal( + tree, + node.PositiveChildIndex, + path, + sphere, + up)) + { + return true; + } + + return HitsWalkableInternal( + tree, + node.NegativeChildIndex, + path, + sphere, + up); + } + + private static bool SphereIntersectsSolidInternal( + FlatPhysicsBsp tree, + int nodeIndex, + CollisionSphere sphere, + bool centerCheck) + { + if (nodeIndex < 0) + return false; + + FlatPhysicsBspNode node = tree.Nodes[nodeIndex]; + if (node.Type == BSPNodeType.Leaf) + { + if (node.PolygonIndexRange.Count == 0) + return false; + + if (centerCheck && node.Solid != 0) + { + if (PhysicsDiagnostics.ProbePlacementFailEnabled) + PhysicsDiagnostics.LastPlacementFailSolidLeaf = true; + return true; + } + + if (!NodeIntersects(node, sphere)) + return false; + + int end = node.PolygonIndexRange.EndExclusive; + for (int i = node.PolygonIndexRange.Start; i < end; i++) + { + int polygonIndex = tree.PolygonIndexStream[i]; + if (!HitsSphere(tree, polygonIndex, sphere)) + continue; + + RecordDiagnosticHit(tree, polygonIndex); + if (PhysicsDiagnostics.ProbePlacementFailEnabled) + { + FlatCollisionPolygon polygon = + tree.PolygonTable.Polygons[polygonIndex]; + PhysicsDiagnostics.LastPlacementFailPolyId = polygon.Id; + PhysicsDiagnostics.LastPlacementFailPolyNormal = + polygon.Plane.Normal; + PhysicsDiagnostics.LastPlacementFailPolyD = polygon.Plane.D; + } + + return true; + } + + return false; + } + + if (!NodeIntersects(node, sphere)) + return false; + + float distance = + Vector3.Dot(node.SplittingPlane.Normal, sphere.Center) + + node.SplittingPlane.D; + float reach = sphere.Radius - PhysicsGlobals.EPSILON; + + if (distance >= reach) + { + return SphereIntersectsSolidInternal( + tree, + node.PositiveChildIndex, + sphere, + centerCheck); + } + + if (distance <= -reach) + { + return SphereIntersectsSolidInternal( + tree, + node.NegativeChildIndex, + sphere, + centerCheck); + } + + if (distance < 0f) + { + if (SphereIntersectsSolidInternal( + tree, + node.PositiveChildIndex, + sphere, + centerCheck: false)) + { + return true; + } + + return SphereIntersectsSolidInternal( + tree, + node.NegativeChildIndex, + sphere, + centerCheck); + } + + if (SphereIntersectsSolidInternal( + tree, + node.PositiveChildIndex, + sphere, + centerCheck)) + { + return true; + } + + return SphereIntersectsSolidInternal( + tree, + node.NegativeChildIndex, + sphere, + centerCheck: false); + } + + private static bool SphereIntersectsSolidPolygonInternal( + FlatPhysicsBsp tree, + int nodeIndex, + CollisionSphere sphere, + float radius, + ref bool centerSolid, + ref int hitPolygonIndex, + bool centerCheck) + { + if (nodeIndex < 0) + return centerSolid; + + FlatPhysicsBspNode node = tree.Nodes[nodeIndex]; + if (node.Type == BSPNodeType.Leaf) + { + if (node.PolygonIndexRange.Count == 0) + return false; + + if (centerCheck && node.Solid != 0) + centerSolid = true; + + if (!NodeIntersects(node, sphere)) + return centerSolid; + + int end = node.PolygonIndexRange.EndExclusive; + for (int i = node.PolygonIndexRange.Start; i < end; i++) + { + int polygonIndex = tree.PolygonIndexStream[i]; + if (HitsSphere(tree, polygonIndex, sphere)) + { + hitPolygonIndex = polygonIndex; + return true; + } + } + + return centerSolid; + } + + if (!NodeIntersects(node, sphere)) + return centerSolid; + + float distance = + Vector3.Dot(node.SplittingPlane.Normal, sphere.Center) + + node.SplittingPlane.D; + float reach = radius - PhysicsGlobals.EPSILON; + + if (distance >= reach) + { + return SphereIntersectsSolidPolygonInternal( + tree, + node.PositiveChildIndex, + sphere, + radius, + ref centerSolid, + ref hitPolygonIndex, + centerCheck); + } + + if (distance <= -reach) + { + return SphereIntersectsSolidPolygonInternal( + tree, + node.NegativeChildIndex, + sphere, + radius, + ref centerSolid, + ref hitPolygonIndex, + centerCheck); + } + + if (distance <= 0f) + { + SphereIntersectsSolidPolygonInternal( + tree, + node.NegativeChildIndex, + sphere, + radius, + ref centerSolid, + ref hitPolygonIndex, + centerCheck); + if (hitPolygonIndex >= 0) + return centerSolid; + + return SphereIntersectsSolidPolygonInternal( + tree, + node.PositiveChildIndex, + sphere, + radius, + ref centerSolid, + ref hitPolygonIndex, + centerCheck: false); + } + + SphereIntersectsSolidPolygonInternal( + tree, + node.PositiveChildIndex, + sphere, + radius, + ref centerSolid, + ref hitPolygonIndex, + centerCheck); + if (hitPolygonIndex >= 0) + return centerSolid; + + return SphereIntersectsSolidPolygonInternal( + tree, + node.NegativeChildIndex, + sphere, + radius, + ref centerSolid, + ref hitPolygonIndex, + centerCheck: false); + } + + private static bool AdjustToPlane( + FlatPhysicsBsp tree, + ref CollisionSphere checkPosition, + Vector3 currentPosition, + int hitPolygonIndex, + Vector3 contactPoint) + { + Vector3 movement = checkPosition.Center - currentPosition; + double clearTime = 0.0; + double hitTime = 1.0; + int iteration = 0; + const int MaxIterations = 15; + + while (true) + { + float touchTime = AdjustSphereToPolygon( + tree, + hitPolygonIndex, + checkPosition, + currentPosition, + movement); + if (touchTime == 1f) + break; + + checkPosition.Center = currentPosition + movement * touchTime; + + int nextHitPolygonIndex = -1; + Vector3 nextContactPoint = Vector3.Zero; + if (!SphereIntersectsPolyInternal( + tree, + tree.RootIndex, + checkPosition, + movement, + ref nextHitPolygonIndex, + ref nextContactPoint)) + { + clearTime = touchTime; + break; + } + + if (nextHitPolygonIndex >= 0) + hitPolygonIndex = nextHitPolygonIndex; + + iteration++; + hitTime = touchTime; + if (iteration >= MaxIterations) + return false; + } + + while (iteration < MaxIterations) + { + double average = (clearTime + hitTime) * 0.5; + checkPosition.Center = + currentPosition + movement * (float)average; + + int nextHitPolygonIndex = -1; + Vector3 nextContactPoint = Vector3.Zero; + if (!SphereIntersectsPolyInternal( + tree, + tree.RootIndex, + checkPosition, + movement, + ref nextHitPolygonIndex, + ref nextContactPoint)) + { + clearTime = average; + } + else + { + hitTime = average; + } + + if (hitTime - clearTime < 0.02) + break; + iteration++; + } + + checkPosition.Center = currentPosition + movement * (float)clearTime; + return true; + } + + private static TransitionState CheckWalkableDispatch( + FlatPhysicsBsp tree, + SpherePath path, + CollisionSphere checkPosition, + Vector3 up) + { + CollisionSphere validPosition = checkPosition; + return HitsWalkableInternal( + tree, + tree.RootIndex, + path, + validPosition, + up) + ? TransitionState.Collided + : TransitionState.OK; + } + + private static TransitionState StepSphereDown( + FlatPhysicsBsp tree, + Transition transition, + CollisionSphere checkPosition, + Vector3 up, + float scale, + Quaternion localToWorld = default, + Vector3 worldOrigin = default) + { + if (localToWorld == default) + localToWorld = Quaternion.Identity; + + SpherePath path = transition.SpherePath; + CollisionInfo collisions = transition.CollisionInfo; + float stepDownAmount = -(path.StepDownAmt * path.WalkInterp); + Vector3 movement = up * stepDownAmount * (1f / scale); + + CollisionSphere validPosition = checkPosition; + bool changed = false; + int hitPolygonIndex = -1; + ushort hitPolygonId = 0; + + FindWalkableInternal( + tree, + tree.RootIndex, + path, + ref validPosition, + movement, + up, + ref hitPolygonIndex, + ref hitPolygonId, + ref changed); + + if (changed && hitPolygonIndex >= 0) + { + FlatCollisionPolygon hitPolygon = + tree.PolygonTable.Polygons[hitPolygonIndex]; + ReadOnlySpan vertices = Vertices(tree, hitPolygon); + Vector3 adjusted = validPosition.Center - checkPosition.Center; + Vector3 offset = + Vector3.Transform(adjusted, localToWorld) * scale; + path.AddOffsetToCheckPos(offset); + + Vector3 worldNormal = + TransformNormal(hitPolygon.Plane.Normal, localToWorld); + Plane worldPlane = BuildWorldPlane( + worldNormal, + vertices, + localToWorld, + scale, + worldOrigin); + collisions.SetContactPlane(worldPlane, path.CheckCellId, false); + path.SetWalkableTransformed( + worldPlane, + vertices, + localToWorld, + scale, + worldOrigin, + Vector3.UnitZ); + + RecordDiagnosticHit(tree, hitPolygonIndex); + return TransitionState.Adjusted; + } + + return TransitionState.OK; + } + + /// + /// Flat shadow port of the retail find-walkable traversal. + /// + public static bool FindWalkableSphere( + FlatPhysicsBsp tree, + Transition transition, + Vector3 sphereCenter, + float sphereRadius, + float probeDistance, + Vector3 up, + out int hitPolygonIndex, + out ushort hitPolygonId, + out Vector3 adjustedCenter) + { + ArgumentNullException.ThrowIfNull(tree); + ArgumentNullException.ThrowIfNull(transition); + + CollisionSphere validPosition = + new(sphereCenter, sphereRadius); + adjustedCenter = validPosition.Center; + hitPolygonIndex = -1; + hitPolygonId = 0; + + if (tree.RootIndex < 0) + return false; + + Vector3 movement = -up * probeDistance; + bool changed = false; + int polygonIndex = -1; + ushort polygonId = 0; + + FindWalkableInternal( + tree, + tree.RootIndex, + transition.SpherePath, + ref validPosition, + movement, + up, + ref polygonIndex, + ref polygonId, + ref changed); + + if (changed && polygonIndex >= 0) + { + hitPolygonIndex = polygonIndex; + hitPolygonId = polygonId; + adjustedCenter = validPosition.Center; + return true; + } + + return false; + } + + private static TransitionState StepSphereUp( + Transition transition, + Vector3 collisionNormal, + PhysicsEngine engine) + { + bool stepped = transition.DoStepUp(collisionNormal, engine); + if (PhysicsDiagnostics.ProbeIndoorBspEnabled) + { + SpherePath path = transition.SpherePath; + Console.WriteLine(FormattableString.Invariant( + $"[stepsphereup] cell=0x{path.CheckCellId:X8} stepUpFlag={path.StepUp} stepDownFlag={path.StepDown} n=({collisionNormal.X:F2},{collisionNormal.Y:F2},{collisionNormal.Z:F2}) stepped={stepped} pos=({path.CheckPos.X:F3},{path.CheckPos.Y:F3},{path.CheckPos.Z:F3})")); + } + + if (stepped) + return TransitionState.OK; + + TransitionState slideResult = + transition.SpherePath.StepUpSlide(transition); + if (PhysicsDiagnostics.ProbeIndoorBspEnabled) + { + Console.WriteLine(FormattableString.Invariant( + $"[stepsphereup] cell=0x{transition.SpherePath.CheckCellId:X8} → StepUpSlide={slideResult}")); + } + + return slideResult; + } + + private static TransitionState SlideSphere( + Transition transition, + Vector3 worldNormal) + => transition.SlideSphereInternal( + worldNormal, + transition.SpherePath.GlobalCurrCenter[0].Origin); + + private static TransitionState CollideWithPoint( + FlatPhysicsBsp tree, + Transition transition, + CollisionSphere checkPosition, + Vector3 currentPosition, + int hitPolygonIndex, + Vector3 contactPoint, + float scale, + Quaternion localToWorld = default) + { + if (localToWorld == default) + localToWorld = Quaternion.Identity; + + FlatCollisionPolygon hitPolygon = + tree.PolygonTable.Polygons[hitPolygonIndex]; + ObjectInfo objectInfo = transition.ObjectInfo; + SpherePath path = transition.SpherePath; + CollisionInfo collisions = transition.CollisionInfo; + Vector3 collisionNormal = + Vector3.Transform(hitPolygon.Plane.Normal, localToWorld); + + if ((objectInfo.State & ObjectInfoState.PerfectClip) == 0) + { + collisions.SetCollisionNormal(collisionNormal); + RecordDiagnosticHit(tree, hitPolygonIndex); + return TransitionState.Collided; + } + + CollisionSphere validPosition = checkPosition; + if (!AdjustToPlane( + tree, + ref validPosition, + currentPosition, + hitPolygonIndex, + contactPoint)) + { + RecordDiagnosticHit(tree, hitPolygonIndex); + return TransitionState.Collided; + } + + collisions.SetCollisionNormal(collisionNormal); + RecordDiagnosticHit(tree, hitPolygonIndex); + Vector3 adjusted = validPosition.Center - checkPosition.Center; + Vector3 offset = + Vector3.Transform(adjusted, localToWorld) * scale; + path.AddOffsetToCheckPos(offset); + return TransitionState.Adjusted; + } + + private static TransitionState NegativePolygonHitDispatch( + FlatPhysicsBsp tree, + SpherePath path, + int hitPolygonIndex, + bool stepUp, + Quaternion localToWorld = default) + { + if (localToWorld == default) + localToWorld = Quaternion.Identity; + + FlatCollisionPolygon hitPolygon = + tree.PolygonTable.Polygons[hitPolygonIndex]; + path.NegPolyHit = true; + path.NegStepUp = stepUp; + path.NegCollisionNormal = + Vector3.Transform(hitPolygon.Plane.Normal, localToWorld); + + if (PhysicsDiagnostics.ProbeIndoorBspEnabled) + { + Vector3 normal = hitPolygon.Plane.Normal; + Console.WriteLine(FormattableString.Invariant( + $"[neg-poly] cell=0x{path.CheckCellId:X8} stepUp={stepUp} stepDownFlag={path.StepDown} poly=0x{hitPolygon.Id:X4} nLocal=({normal.X:F3},{normal.Y:F3},{normal.Z:F3}) sides={hitPolygon.SidesType} checkPos=({path.CheckPos.X:F3},{path.CheckPos.Y:F3},{path.CheckPos.Z:F3})")); + } + + return TransitionState.OK; + } + + private static TransitionState PlacementInsert( + FlatPhysicsBsp tree, + Transition transition, + bool clearCell) + { + SpherePath path = transition.SpherePath; + CollisionSphere sphere0 = new( + path.LocalSphere[0].Origin, + path.LocalSphere[0].Radius); + float radius = sphere0.Radius; + + bool hasSphere1 = path.NumSphere > 1; + CollisionSphere sphere1 = default; + if (hasSphere1) + { + sphere1 = new CollisionSphere( + path.LocalSphere[1].Origin, + path.LocalSphere[1].Radius); + } + + const int MaxIterations = 20; + for (int iteration = 0; iteration < MaxIterations; iteration++) + { + bool centerSolid = false; + int hitPolygonIndex = -1; + if (SphereIntersectsSolidPolygonInternal( + tree, + tree.RootIndex, + sphere0, + radius, + ref centerSolid, + ref hitPolygonIndex, + clearCell)) + { + if (hitPolygonIndex >= 0) + { + AdjustToPlacementPolygon( + tree, + hitPolygonIndex, + ref sphere0, + ref sphere1, + hasSphere1, + radius, + centerSolid, + clearCell); + continue; + } + } + else + { + if (hasSphere1) + { + centerSolid = false; + hitPolygonIndex = -1; + if (SphereIntersectsSolidPolygonInternal( + tree, + tree.RootIndex, + sphere1, + radius, + ref centerSolid, + ref hitPolygonIndex, + clearCell)) + { + if (hitPolygonIndex >= 0) + { + AdjustToPlacementPolygon( + tree, + hitPolygonIndex, + ref sphere1, + ref sphere0, + hasValidPosition2: true, + radius, + centerSolid, + clearCell); + continue; + } + } + else + { + return PlacementInsertInner(sphere0, path, iteration); + } + } + else + { + return PlacementInsertInner(sphere0, path, iteration); + } + } + + radius *= 2f; + } + + return TransitionState.Collided; + } + + private static TransitionState PlacementInsertInner( + CollisionSphere sphere0, + SpherePath path, + int iteration) + { + if (iteration == 0) + return TransitionState.OK; + + Vector3 adjustment = + sphere0.Center - path.LocalSphere[0].Origin; + path.AddOffsetToCheckPos(adjustment); + return TransitionState.Adjusted; + } + + /// + /// Flat shadow port of retail BSPTREE::find_collisions + /// (0x0053A440), preserving the graph path's six dispatch branches. + /// + public static TransitionState FindCollisions( + FlatPhysicsBsp tree, + Transition transition, + DatReaderWriter.Types.Sphere localSphere, + DatReaderWriter.Types.Sphere? localSphere1, + Vector3 localCurrentCenter, + Vector3 localSpaceZ, + float scale, + Quaternion localToWorld = default, + PhysicsEngine? engine = null, + Vector3 worldOrigin = default) + { + ArgumentNullException.ThrowIfNull(tree); + ArgumentNullException.ThrowIfNull(transition); + + CollisionSphere sphere0 = + new(localSphere.Origin, localSphere.Radius); + bool hasSphere1 = localSphere1 is not null; + CollisionSphere sphere1 = hasSphere1 + ? new CollisionSphere(localSphere1!.Origin, localSphere1.Radius) + : default; + + return FindCollisionsCore( + tree, + transition, + sphere0, + hasSphere1, + sphere1, + localCurrentCenter, + localSpaceZ, + scale, + localToWorld, + engine, + worldOrigin); + } + + internal static TransitionState FindCollisions( + FlatPhysicsBsp tree, + Transition transition, + Vector3 localSphereCenter, + float localSphereRadius, + bool hasLocalSphere1, + Vector3 localSphere1Center, + float localSphere1Radius, + Vector3 localCurrentCenter, + Vector3 localSpaceZ, + float scale, + Quaternion localToWorld = default, + PhysicsEngine? engine = null, + Vector3 worldOrigin = default) + { + ArgumentNullException.ThrowIfNull(tree); + ArgumentNullException.ThrowIfNull(transition); + + return FindCollisionsCore( + tree, + transition, + new CollisionSphere(localSphereCenter, localSphereRadius), + hasLocalSphere1, + hasLocalSphere1 + ? new CollisionSphere( + localSphere1Center, + localSphere1Radius) + : default, + localCurrentCenter, + localSpaceZ, + scale, + localToWorld, + engine, + worldOrigin); + } + + private static TransitionState FindCollisionsCore( + FlatPhysicsBsp tree, + Transition transition, + CollisionSphere sphere0, + bool hasSphere1, + CollisionSphere sphere1, + Vector3 localCurrentCenter, + Vector3 localSpaceZ, + float scale, + Quaternion localToWorld, + PhysicsEngine? engine, + Vector3 worldOrigin) + { + if (tree.RootIndex < 0) + return TransitionState.OK; + if (localToWorld == default) + localToWorld = Quaternion.Identity; + + SpherePath path = transition.SpherePath; + CollisionInfo collisions = transition.CollisionInfo; + ObjectInfo objectInfo = transition.ObjectInfo; + Vector3 movement = sphere0.Center - localCurrentCenter; + + if (PhysicsDiagnostics.ProbePushBackEnabled) + { + PhysicsDiagnostics.LogPushBackDispatch( + sphereCenter: sphere0.Center, + movement, + collide: path.Collide, + insertType: (int)path.InsertType, + objState: unchecked((int)objectInfo.State), + walkInterpEntry: path.WalkInterp, + returnState: -1); + } + + Vector3 LocalToWorld(Vector3 value) + => Vector3.Transform(value, localToWorld); + + // Path 1: Placement or obstruction-ethereal. + if (path.InsertType == InsertType.Placement || + path.ObstructionEthereal) + { + bool clearCell = + !(path.BldgCheck && path.HitsInteriorCell); + + if (PhysicsDiagnostics.ProbePlacementFailEnabled) + { + PhysicsDiagnostics.LastPlacementFailPolyId = 0; + PhysicsDiagnostics.LastPlacementFailSolidLeaf = false; + } + + if (SphereIntersectsSolidInternal( + tree, + tree.RootIndex, + sphere0, + clearCell)) + { + if (PhysicsDiagnostics.ProbePlacementFailEnabled) + { + PhysicsDiagnostics.LogPlacementFail( + "Path1.sphere0", + sphere0.Center, + sphere0.Radius, + 0, + path.CheckCellId, + worldOrigin, + objectInfo.Ethereal); + } + + return TransitionState.Collided; + } + + if (PhysicsDiagnostics.ProbePlacementFailEnabled) + { + PhysicsDiagnostics.LastPlacementFailPolyId = 0; + PhysicsDiagnostics.LastPlacementFailSolidLeaf = false; + } + + if (hasSphere1 && + SphereIntersectsSolidInternal( + tree, + tree.RootIndex, + sphere1, + clearCell)) + { + if (PhysicsDiagnostics.ProbePlacementFailEnabled) + { + PhysicsDiagnostics.LogPlacementFail( + "Path1.sphere1", + sphere1.Center, + sphere1.Radius, + 1, + path.CheckCellId, + worldOrigin, + objectInfo.Ethereal); + } + + return TransitionState.Collided; + } + + return TransitionState.OK; + } + + // Path 2: CheckWalkable. + if (path.CheckWalkable) + { + return CheckWalkableDispatch( + tree, + path, + sphere0, + localSpaceZ); + } + + // Path 3: StepDown. + if (path.StepDown) + { + return StepSphereDown( + tree, + transition, + sphere0, + localSpaceZ, + scale, + localToWorld, + worldOrigin); + } + + // Path 4: Collide / walkable landing. + if (path.Collide) + { + CollisionSphere validPosition = sphere0; + int hitPolygonIndex = -1; + ushort hitPolygonId = 0; + bool changed = false; + + FindWalkableInternal( + tree, + tree.RootIndex, + path, + ref validPosition, + movement, + localSpaceZ, + ref hitPolygonIndex, + ref hitPolygonId, + ref changed); + + if (changed && hitPolygonIndex >= 0) + { + FlatCollisionPolygon hitPolygon = + tree.PolygonTable.Polygons[hitPolygonIndex]; + ReadOnlySpan vertices = + Vertices(tree, hitPolygon); + Vector3 localOffset = + validPosition.Center - sphere0.Center; + Vector3 worldOffset = + LocalToWorld(localOffset) * scale; + path.AddOffsetToCheckPos(worldOffset); + + Vector3 worldNormal = + TransformNormal(hitPolygon.Plane.Normal, localToWorld); + Plane worldPlane = BuildWorldPlane( + worldNormal, + vertices, + localToWorld, + scale, + worldOrigin); + collisions.SetContactPlane( + worldPlane, + path.CheckCellId, + false); + path.SetWalkableTransformed( + worldPlane, + vertices, + localToWorld, + scale, + worldOrigin, + Vector3.UnitZ); + RecordDiagnosticHit(tree, hitPolygonIndex); + return TransitionState.Adjusted; + } + + return TransitionState.OK; + } + + // Path 5: grounded contact. + if ((objectInfo.State & ObjectInfoState.Contact) != 0) + { + int hitPolygonIndex0 = -1; + Vector3 contact0 = Vector3.Zero; + bool hit0 = SphereIntersectsPolyInternal( + tree, + tree.RootIndex, + sphere0, + movement, + ref hitPolygonIndex0, + ref contact0); + + if (hit0) + { + RecordDiagnosticHit(tree, hitPolygonIndex0); + Vector3 worldNormal = LocalToWorld( + tree.PolygonTable.Polygons[hitPolygonIndex0].Plane.Normal); + if (engine is not null && !path.StepUp && !path.StepDown) + return StepSphereUp(transition, worldNormal, engine); + + return SlideSphere(transition, worldNormal); + } + + if (hasSphere1) + { + int hitPolygonIndex1 = -1; + Vector3 contact1 = Vector3.Zero; + bool hit1 = SphereIntersectsPolyInternal( + tree, + tree.RootIndex, + sphere1, + movement, + ref hitPolygonIndex1, + ref contact1); + + if (hit1) + { + RecordDiagnosticHit(tree, hitPolygonIndex1); + Vector3 worldNormal = LocalToWorld( + tree.PolygonTable.Polygons[hitPolygonIndex1].Plane.Normal); + return SlideSphere(transition, worldNormal); + } + + if (hitPolygonIndex1 >= 0) + { + RecordDiagnosticHit(tree, hitPolygonIndex1); + NegativePolygonHitDispatch( + tree, + path, + hitPolygonIndex1, + stepUp: false, + localToWorld); + return TransitionState.OK; + } + + if (hitPolygonIndex0 >= 0) + { + RecordDiagnosticHit(tree, hitPolygonIndex0); + NegativePolygonHitDispatch( + tree, + path, + hitPolygonIndex0, + stepUp: true, + localToWorld); + return TransitionState.OK; + } + } + + return TransitionState.OK; + } + + // Path 6: default airborne/path-clipped dispatch. + int defaultHitPolygonIndex0 = -1; + Vector3 defaultContact0 = Vector3.Zero; + bool defaultHit0 = SphereIntersectsPolyInternal( + tree, + tree.RootIndex, + sphere0, + movement, + ref defaultHitPolygonIndex0, + ref defaultContact0); + + if (defaultHit0 || defaultHitPolygonIndex0 >= 0) + { + if ((objectInfo.State & ObjectInfoState.PathClipped) != 0) + { + return CollideWithPoint( + tree, + transition, + sphere0, + localCurrentCenter, + defaultHitPolygonIndex0, + defaultContact0, + scale, + localToWorld); + } + + Vector3 worldNormal0 = LocalToWorld( + tree.PolygonTable.Polygons[defaultHitPolygonIndex0].Plane.Normal); + if (worldNormal0.Z < PhysicsGlobals.FloorZ) + { + Vector3 currentWorld = + path.GlobalCurrCenter[0].Origin; + Vector3 endWorld = + path.GlobalSphere[0].Origin; + Vector3 globalDelta = endWorld - currentWorld; + float difference = Vector3.Dot(worldNormal0, globalDelta); + if (difference < 0f) + path.AddOffsetToCheckPos(-worldNormal0 * difference); + + collisions.SetCollisionNormal(worldNormal0); + collisions.SetSlidingNormal(worldNormal0); + RecordDiagnosticHit(tree, defaultHitPolygonIndex0); + return TransitionState.Slid; + } + + path.SetCollide(worldNormal0); + path.WalkableAllowance = PhysicsGlobals.LandingZ; + RecordDiagnosticHit(tree, defaultHitPolygonIndex0); + return TransitionState.Adjusted; + } + + if (hasSphere1) + { + int defaultHitPolygonIndex1 = -1; + Vector3 defaultContact1 = Vector3.Zero; + bool defaultHit1 = SphereIntersectsPolyInternal( + tree, + tree.RootIndex, + sphere1, + movement, + ref defaultHitPolygonIndex1, + ref defaultContact1); + + if (defaultHit1 || defaultHitPolygonIndex1 >= 0) + { + Vector3 worldNormal1 = LocalToWorld( + tree.PolygonTable.Polygons[defaultHitPolygonIndex1].Plane.Normal); + if (worldNormal1.Z < PhysicsGlobals.FloorZ) + { + Vector3 currentWorld = + path.GlobalCurrCenter[0].Origin; + Vector3 endWorld = + path.GlobalSphere[0].Origin; + Vector3 globalDelta = endWorld - currentWorld; + float difference = + Vector3.Dot(worldNormal1, globalDelta); + if (difference < 0f) + path.AddOffsetToCheckPos(-worldNormal1 * difference); + + collisions.SetCollisionNormal(worldNormal1); + collisions.SetSlidingNormal(worldNormal1); + RecordDiagnosticHit(tree, defaultHitPolygonIndex1); + return TransitionState.Slid; + } + + path.SetCollide(worldNormal1); + path.WalkableAllowance = PhysicsGlobals.LandingZ; + RecordDiagnosticHit(tree, defaultHitPolygonIndex1); + return TransitionState.Adjusted; + } + } + + return TransitionState.OK; + } + + private static void RecordDiagnosticHit( + FlatPhysicsBsp tree, + int polygonIndex) + { + if (PhysicsDiagnostics.ProbeBuildingEnabled || + PhysicsDiagnostics.ProbeIndoorBspEnabled) + { + PhysicsDiagnostics.LastBspHitPoly = + MaterializeResolvedPolygon(tree, polygonIndex); + } + } + + private static ResolvedPolygon MaterializeResolvedPolygon( + FlatPhysicsBsp tree, + int polygonIndex) + { + FlatCollisionPolygon polygon = tree.PolygonTable.Polygons[polygonIndex]; + return new ResolvedPolygon + { + Id = polygon.Id, + Plane = polygon.Plane, + SidesType = polygon.SidesType, + NumPoints = polygon.NumPoints, + Vertices = Vertices(tree, polygon).ToArray(), + }; + } +} diff --git a/src/AcDream.Core/Physics/PhysicsDataCache.cs b/src/AcDream.Core/Physics/PhysicsDataCache.cs index 5772f361..9bc5c854 100644 --- a/src/AcDream.Core/Physics/PhysicsDataCache.cs +++ b/src/AcDream.Core/Physics/PhysicsDataCache.cs @@ -22,6 +22,14 @@ public sealed class PhysicsDataCache private readonly ConcurrentDictionary _setup = new(); private readonly ConcurrentDictionary _cellStruct = new(); + /// + /// Slice I graph/flat differential selector. Production remains on the + /// graph oracle until I6; tests use the flat value to run the same complete + /// resolver from cloned inputs. + /// + internal CollisionTraversalMode CollisionTraversalMode { get; set; } = + CollisionTraversalMode.Graph; + // ── Phase 2: building portal cache for outdoor→indoor entry ─────────── private readonly ConcurrentDictionary _buildings = new(); @@ -541,6 +549,12 @@ public sealed class GfxObjPhysics /// Populated once at cache time so BSP queries don't pay per-test lookup cost. /// public required Dictionary Resolved { get; init; } + + /// + /// Prepared integer-indexed shadow representation. Optional until Slice I5 + /// dual publication is complete. + /// + public FlatPhysicsBsp? FlatPhysicsBsp { get; init; } } /// Cached collision shape data for a Setup (character/creature capsule). @@ -578,6 +592,11 @@ public sealed class CellPhysics /// public required Dictionary Resolved { get; init; } + /// + /// Prepared integer-indexed physics BSP shadow. Optional until Slice I5. + /// + public FlatPhysicsBsp? FlatPhysicsBsp { get; init; } + // ── Indoor walking Phase 2 (2026-05-19): portal-graph fields ─────── /// @@ -590,6 +609,12 @@ public sealed class CellPhysics /// public DatReaderWriter.Types.CellBSPTree? CellBSP { get; init; } + /// + /// Prepared integer-indexed cell-containment BSP shadow. Optional until + /// Slice I5. + /// + public FlatCellContainmentBsp? FlatContainmentBsp { get; init; } + /// /// Portal connections to neighbouring cells, in cell-local space. /// Default: empty list. Source: envCell.CellPortals. diff --git a/src/AcDream.Core/Physics/TransitionTypes.cs b/src/AcDream.Core/Physics/TransitionTypes.cs index 64e56d78..46c64322 100644 --- a/src/AcDream.Core/Physics/TransitionTypes.cs +++ b/src/AcDream.Core/Physics/TransitionTypes.cs @@ -2298,7 +2298,11 @@ public sealed class Transition var cell = engine.DataCache.GetCellStruct(cellId); // R2 guard: stale CellPhysics loaded for render but not physics. - if (cell?.BSP?.Root is null) continue; + if (cell is null || + !CollisionTraversal.HasPhysics(engine.DataCache, cell)) + { + continue; + } // Transform sphere into THIS cell's local space. Mirrors the // primary-cell pattern at TransitionTypes.cs (FindEnvCollisions, @@ -2330,8 +2334,8 @@ public sealed class Transition if (PhysicsDiagnostics.ProbeIndoorBspEnabled) PhysicsDiagnostics.LastBspHitPoly = null; - var result = BSPQuery.FindCollisions( - cell.BSP.Root, cell.Resolved, this, + var result = CollisionTraversal.FindCollisions( + engine.DataCache!, cell, this, localCenter, sphereRadius, hasLocalSphere1, @@ -2348,7 +2352,10 @@ public sealed class Transition ? "poly=n/a" : System.FormattableString.Invariant( $"poly=0x{hit.Id:X4} n=({hit.Plane.Normal.X:F3},{hit.Plane.Normal.Y:F3},{hit.Plane.Normal.Z:F3}) d={hit.Plane.D:F3} sides={hit.SidesType}"); - var bs = cell.BSP.Root.BoundingSphere; + FlatCollisionSphere bs = + CollisionTraversal.RootBoundingSphere( + engine.DataCache, + cell); string bsDesc = System.FormattableString.Invariant( $"bs=({bs.Origin.X:F3},{bs.Origin.Y:F3},{bs.Origin.Z:F3}) br={bs.Radius:F3}"); Console.WriteLine(System.FormattableString.Invariant( @@ -2620,7 +2627,8 @@ public sealed class Transition if (cellLow >= 0x0100 && engine.DataCache is not null) { var cellPhysics = engine.DataCache.GetCellStruct(sp.CheckCellId); - if (cellPhysics?.BSP?.Root is not null) + if (cellPhysics is not null && + CollisionTraversal.HasPhysics(engine.DataCache!, cellPhysics)) { // Transform player sphere to cell-local space. var localCenter = Vector3.Transform(footCenter, cellPhysics.InverseWorldTransform); @@ -2677,9 +2685,9 @@ public sealed class Transition cellOrigin = cellPhysics.WorldTransform.Translation; } - var cellState = BSPQuery.FindCollisions( - cellPhysics.BSP.Root, - cellPhysics.Resolved, + var cellState = CollisionTraversal.FindCollisions( + engine.DataCache!, + cellPhysics, this, localCenter, sphereRadius, @@ -3134,12 +3142,14 @@ public sealed class Transition { Vector3 dxy = obj.Position - sp.GlobalCurrCenter[0].Origin; float distXY = MathF.Sqrt(dxy.X * dxy.X + dxy.Y * dxy.Y); - bool cacheHit = physics?.BSP?.Root is not null; + bool cacheHit = physics is not null && + CollisionTraversal.HasPhysics(engine.DataCache!, physics); Console.WriteLine(System.FormattableString.Invariant( $"[bsp-test] obj=0x{obj.EntityId:X8} gfx=0x{obj.GfxObjId:X8} state=0x{obj.State:X8} radius={obj.Radius:F3} pos=({obj.Position.X:F2},{obj.Position.Y:F2},{obj.Position.Z:F2}) distXY={distXY:F3} cacheHit={cacheHit}")); } - if (physics?.BSP?.Root is null) + if (physics is null || + !CollisionTraversal.HasPhysics(engine.DataCache!, physics)) { // Clear obstruction_ethereal before skipping — retail's per-object // clear (pc:276989) fires after shape tests; we clear early here to @@ -3179,9 +3189,9 @@ public sealed class Transition // Use the retail 6-path dispatcher with pre-resolved polygons. // Pass the object's scale so collision response offsets (in // unscaled local space) are multiplied back to world space. - result = BSPQuery.FindCollisions( - physics.BSP.Root, - physics.Resolved, + result = CollisionTraversal.FindCollisions( + engine.DataCache!, + physics, this, localSphere0Center, localSphere0Radius, @@ -3476,7 +3486,11 @@ public sealed class Transition // CacheBuilding site resolves 0x02 Setup models to their first part // — retail tests part_array->parts[0] only, 0x006b5320). var physics = engine.DataCache.GetGfxObj(building.ModelId); - if (physics?.BSP?.Root is null) return TransitionState.OK; + if (physics is null || + !CollisionTraversal.HasPhysics(engine.DataCache, physics)) + { + return TransitionState.OK; + } var sp = SpherePath; var ci = CollisionInfo; @@ -3509,9 +3523,9 @@ public sealed class Transition TransitionState result; try { - result = BSPQuery.FindCollisions( - physics.BSP.Root, - physics.Resolved, + result = CollisionTraversal.FindCollisions( + engine.DataCache!, + physics, this, localSphere0Center, localSphere0Radius, diff --git a/tests/AcDream.Core.Tests/Physics/FlatBspQueryDifferentialTests.cs b/tests/AcDream.Core.Tests/Physics/FlatBspQueryDifferentialTests.cs new file mode 100644 index 00000000..25354389 --- /dev/null +++ b/tests/AcDream.Core.Tests/Physics/FlatBspQueryDifferentialTests.cs @@ -0,0 +1,1249 @@ +using System.Collections; +using System.Numerics; +using System.Reflection; +using AcDream.Core.Physics; +using AcDream.Core.Tests.Conformance; +using DatReaderWriter; +using DatReaderWriter.Enums; +using DatReaderWriter.Options; +using DatReaderWriter.Types; + +namespace AcDream.Core.Tests.Physics; + +public sealed class FlatBspQueryDifferentialTests +{ + [Fact] + public void CellContainment_NullOnPlaneRadiusEqualityAndDeepChain_MatchGraph() + { + var leaf = new CellBSPNode + { + Type = BSPNodeType.Leaf, + LeafIndex = 77, + }; + CellBSPNode graph = leaf; + const int Depth = 256; + for (int i = 0; i < Depth; i++) + { + graph = new CellBSPNode + { + Type = BSPNodeType.BPIn, + SplittingPlane = new Plane( + Vector3.UnitX, + i == 0 ? 0f : 1_000f), + PosNode = graph, + }; + } + + FlatCellContainmentBsp flat = + FlatCollisionAssetBuilder.FlattenCellContainmentBsp(graph); + Vector3[] points = + [ + Vector3.Zero, + new Vector3(0.5f, 0f, 0f), + new Vector3(-0.5f, 0f, 0f), + new Vector3( + BitConverter.Int32BitsToSingle(1), + 0f, + 0f), + ]; + + foreach (Vector3 point in points) + { + Assert.Equal( + BSPQuery.PointInsideCellBsp(graph, point), + FlatBspQuery.PointInsideCellBsp(flat, point)); + } + + foreach ((Vector3 center, float radius) in new[] + { + (Vector3.Zero, 0f), + (new Vector3(-0.51f, 0f, 0f), 0.5f), + (new Vector3(-0.51f, 0f, 0f), 0.49999997f), + (new Vector3(-0.51f, 0f, 0f), 0.50000006f), + }) + { + Assert.Equal( + BSPQuery.SphereIntersectsCellBsp(graph, center, radius), + FlatBspQuery.SphereIntersectsCellBsp(flat, center, radius)); + } + + var empty = FlatCollisionAssetBuilder.FlattenCellContainmentBsp(null); + Assert.True(FlatBspQuery.PointInsideCellBsp(empty, Vector3.Zero)); + Assert.True(FlatBspQuery.SphereIntersectsCellBsp( + empty, + Vector3.Zero, + 0.5f)); + } + + [Fact] + public void StaticAndSweptOverlap_MultiPolygonLeafOrderAndBoundaryValues_MatchGraphBits() + { + (PhysicsBSPNode root, Dictionary resolved) = + BuildTwoWallLeaf(); + FlatPhysicsBsp flat = + FlatCollisionAssetBuilder.FlattenPhysicsBsp(root, resolved); + + var random = new Random(0x4B53_5034); + for (int i = 0; i < 10_000; i++) + { + Vector3 center = new( + NextFloat(random, -3f, 3f), + NextFloat(random, -1f, 1f), + NextFloat(random, -3f, 3f)); + float radius = i % 7 switch + { + 0 => 0f, + 1 => PhysicsGlobals.EPSILON, + 2 => 0.5f, + _ => NextFloat(random, 0.01f, 1.5f), + }; + Vector3 movement = new( + NextFloat(random, -1f, 1f), + NextFloat(random, -1f, 1f), + NextFloat(random, -1f, 1f)); + + bool graphStatic = BSPQuery.SphereIntersectsPoly( + root, + resolved, + center, + radius, + out ushort graphStaticId, + out Vector3 graphStaticNormal); + bool flatStatic = FlatBspQuery.SphereIntersectsPoly( + flat, + center, + radius, + out ushort flatStaticId, + out Vector3 flatStaticNormal); + + Assert.Equal(graphStatic, flatStatic); + Assert.Equal(graphStaticId, flatStaticId); + AssertVectorBits(graphStaticNormal, flatStaticNormal); + + bool graphSwept = BSPQuery.SphereIntersectsPolyWithTime( + root, + resolved, + center, + radius, + movement, + out ushort graphSweptId, + out Vector3 graphSweptNormal, + out float graphTime); + bool flatSwept = FlatBspQuery.SphereIntersectsPolyWithTime( + flat, + center, + radius, + movement, + out ushort flatSweptId, + out Vector3 flatSweptNormal, + out float flatTime); + + Assert.Equal(graphSwept, flatSwept); + Assert.Equal(graphSweptId, flatSweptId); + AssertVectorBits(graphSweptNormal, flatSweptNormal); + AssertFloatBits(graphTime, flatTime); + } + } + + [Fact] + public void FindWalkable_RandomizedAndEqualCandidateDistances_MatchesGraphBits() + { + (PhysicsBSPNode root, Dictionary resolved) = + BuildTwoFloorLeaf(); + FlatPhysicsBsp flat = + FlatCollisionAssetBuilder.FlattenPhysicsBsp(root, resolved); + var random = new Random(0x5741_4C4B); + + for (int i = 0; i < 5_000; i++) + { + Vector3 center = new( + NextFloat(random, -2.5f, 2.5f), + NextFloat(random, -2.5f, 2.5f), + NextFloat(random, -0.25f, 1.25f)); + float radius = i % 5 == 0 + ? 0.48f + : NextFloat(random, 0.05f, 0.8f); + float probe = i % 7 == 0 + ? 0.5f + : NextFloat(random, 0.01f, 1.5f); + + Transition graphTransition = NewTransition(); + Transition flatTransition = NewTransition(); + bool graphFound = BSPQuery.FindWalkableSphere( + root, + resolved, + graphTransition, + center, + radius, + probe, + Vector3.UnitZ, + out ResolvedPolygon? graphPolygon, + out ushort graphId, + out Vector3 graphCenter); + bool flatFound = FlatBspQuery.FindWalkableSphere( + flat, + flatTransition, + center, + radius, + probe, + Vector3.UnitZ, + out int flatPolygonIndex, + out ushort flatId, + out Vector3 flatCenter); + + Assert.Equal(graphFound, flatFound); + Assert.Equal(graphId, flatId); + Assert.Equal(graphPolygon is null, flatPolygonIndex < 0); + AssertVectorBits(graphCenter, flatCenter); + AssertTransitionEquivalent(graphTransition, flatTransition); + } + } + + [Fact] + public void PhysicsTraversal_NullChildRadiusEqualityEqualCandidatesAndDeepTree_Match() + { + ResolvedPolygon first = Polygon( + 9, + new Plane(Vector3.UnitZ, 0f), + new Vector3(-2f, -2f, 0f), + new Vector3(2f, -2f, 0f), + new Vector3(2f, 2f, 0f), + new Vector3(-2f, 2f, 0f)); + ResolvedPolygon second = Polygon( + 4, + first.Plane, + first.Vertices.ToArray()); + PhysicsBSPNode graph = Leaf(); + graph.Polygons.Add(first.Id); + graph.Polygons.Add(second.Id); + var resolved = new Dictionary + { + [second.Id] = second, + [first.Id] = first, + }; + + const int Depth = 128; + for (int i = 0; i < Depth; i++) + { + graph = new PhysicsBSPNode + { + Type = BSPNodeType.BPIn, + SplittingPlane = new Plane(Vector3.UnitX, 1_000f), + PosNode = graph, + BoundingSphere = new Sphere + { + Origin = Vector3.Zero, + Radius = 10_000f, + }, + }; + } + + FlatPhysicsBsp flat = + FlatCollisionAssetBuilder.FlattenPhysicsBsp(graph, resolved); + Vector3 equalityCenter = new(0f, 0f, 0.48f); + bool graphStatic = BSPQuery.SphereIntersectsPoly( + graph, + resolved, + equalityCenter, + 0.48f, + out ushort graphStaticId, + out Vector3 graphStaticNormal); + bool flatStatic = FlatBspQuery.SphereIntersectsPoly( + flat, + equalityCenter, + 0.48f, + out ushort flatStaticId, + out Vector3 flatStaticNormal); + Assert.Equal(graphStatic, flatStatic); + Assert.Equal(graphStaticId, flatStaticId); + AssertVectorBits(graphStaticNormal, flatStaticNormal); + + Transition graphTransition = NewTransition(); + Transition flatTransition = NewTransition(); + bool graphFound = BSPQuery.FindWalkableSphere( + graph, + resolved, + graphTransition, + new Vector3(0f, 0f, 0.4f), + 0.48f, + 0.5f, + Vector3.UnitZ, + out ResolvedPolygon? graphPolygon, + out ushort graphPolygonId, + out Vector3 graphAdjustedCenter); + bool flatFound = FlatBspQuery.FindWalkableSphere( + flat, + flatTransition, + new Vector3(0f, 0f, 0.4f), + 0.48f, + 0.5f, + Vector3.UnitZ, + out int flatPolygonIndex, + out ushort flatPolygonId, + out Vector3 flatAdjustedCenter); + Assert.Equal(graphFound, flatFound); + Assert.Equal(graphPolygonId, flatPolygonId); + Assert.Equal(graphPolygon is null, flatPolygonIndex < 0); + AssertVectorBits(graphAdjustedCenter, flatAdjustedCenter); + AssertTransitionEquivalent(graphTransition, flatTransition); + } + + [Fact] + public void FindCollisions_Path1Placement_MatchesGraphStateAndMutations() + { + (PhysicsBSPNode root, Dictionary resolved) = + BuildFloorLeaf(); + Sphere sphere = SphereAt(new Vector3(0f, 0f, 0.4f)); + AssertFindCollisionsEquivalent( + root, + resolved, + sphere, + null, + sphere.Origin, + transition => transition.SpherePath.InsertType = InsertType.Placement); + } + + [Fact] + public void FindCollisions_Path2CheckWalkable_MatchesGraphStateAndMutations() + { + (PhysicsBSPNode root, Dictionary resolved) = + BuildFloorLeaf(); + Sphere sphere = SphereAt(new Vector3(0f, 0f, 0.4f)); + AssertFindCollisionsEquivalent( + root, + resolved, + sphere, + null, + sphere.Origin, + transition => transition.SpherePath.CheckWalkable = true); + } + + [Fact] + public void FindCollisions_Path3StepDown_MatchesEveryTransitionOutputBit() + { + (PhysicsBSPNode root, Dictionary resolved) = + BuildFloorLeaf(); + Sphere sphere = SphereAt(new Vector3(0f, 0f, 0.4f)); + AssertFindCollisionsEquivalent( + root, + resolved, + sphere, + null, + sphere.Origin, + transition => + { + transition.SpherePath.StepDown = true; + transition.SpherePath.StepDownAmt = 0.5f; + }, + worldOrigin: new Vector3(17f, -23f, 94f)); + } + + [Fact] + public void FindCollisions_Path4Landing_MatchesEveryTransitionOutputBit() + { + (PhysicsBSPNode root, Dictionary resolved) = + BuildFloorLeaf(); + Sphere sphere = SphereAt(new Vector3(0f, 0f, 0.4f)); + AssertFindCollisionsEquivalent( + root, + resolved, + sphere, + null, + sphere.Origin + new Vector3(0f, 0f, 0.05f), + transition => transition.SpherePath.Collide = true, + worldOrigin: new Vector3(17f, -23f, 94f)); + } + + [Fact] + public void FindCollisions_Path5FullAndNearMiss_MatchEveryTransitionOutputBit() + { + (PhysicsBSPNode root, Dictionary resolved) = + BuildTwoWallLeaf(); + Sphere foot = SphereAt(new Vector3(0f, 0.3f, 0f)); + Sphere head = SphereAt(new Vector3(0f, 0.3f, 1f)); + + AssertFindCollisionsEquivalent( + root, + resolved, + foot, + head, + foot.Origin - new Vector3(0.05f, 0f, 0f), + transition => + transition.ObjectInfo.State = ObjectInfoState.Contact); + + AssertFindCollisionsEquivalent( + root, + resolved, + foot, + null, + foot.Origin - new Vector3(0f, -0.05f, 0f), + transition => + transition.ObjectInfo.State = ObjectInfoState.Contact); + } + + [Fact] + public void FindCollisions_Path6LandingSteepAndPerfectClip_MatchEveryOutputBit() + { + (PhysicsBSPNode floorRoot, Dictionary floorResolved) = + BuildFloorLeaf(); + Sphere floorSphere = SphereAt(new Vector3(0f, 0f, 0.4f)); + AssertFindCollisionsEquivalent( + floorRoot, + floorResolved, + floorSphere, + null, + floorSphere.Origin + new Vector3(0f, 0f, 0.05f), + configure: null); + + (PhysicsBSPNode wallRoot, Dictionary wallResolved) = + BuildTwoWallLeaf(); + Sphere wallSphere = SphereAt(new Vector3(0f, 0.3f, 0f)); + AssertFindCollisionsEquivalent( + wallRoot, + wallResolved, + wallSphere, + null, + wallSphere.Origin - new Vector3(0f, -0.05f, 0f), + transition => + { + transition.ObjectInfo.State = + ObjectInfoState.PathClipped | ObjectInfoState.PerfectClip; + }); + } + + [Fact] + public void InstalledDat_LargeRandomizedSweep_HasZeroBitMismatch() + { + string? datDirectory = ConformanceDats.ResolveDatDir(); + if (datDirectory is null) + return; + + using var dats = new DatCollection(datDirectory, DatAccessType.Read); + var random = new Random(0x4934_4253); + foreach (uint cellId in new[] + { + 0x8A02_016Eu, + 0x8A02_017Au, + 0xA9B4_013Fu, + }) + { + var cache = new PhysicsDataCache(); + ConformanceDats.LoadEnvCell(dats, cache, cellId); + CellPhysics source = Assert.IsType( + cache.GetCellStruct(cellId)); + FlatPhysicsBsp flat = + FlatCollisionAssetBuilder.FlattenPhysicsBsp( + source.BSP?.Root, + source.Resolved); + FlatCellContainmentBsp flatContainment = + FlatCollisionAssetBuilder.FlattenCellContainmentBsp( + source.CellBSP?.Root); + if (flat.PolygonTable.Polygons.Length == 0) + continue; + + for (int iteration = 0; iteration < 2_500; iteration++) + { + int polygonIndex = + random.Next(flat.PolygonTable.Polygons.Length); + FlatCollisionPolygon polygon = + flat.PolygonTable.Polygons[polygonIndex]; + Vector3 anchor = flat.PolygonTable.Vertices[ + polygon.VertexRange.Start + + random.Next(polygon.VertexRange.Count)]; + float radius = (iteration % 11) switch + { + 0 => PhysicsGlobals.EPSILON, + 1 => 0.01f, + 2 => 0.48f, + 3 => 1f, + _ => NextFloat(random, 0.05f, 1.25f), + }; + float signedOffset = (iteration % 13) switch + { + 0 => radius, + 1 => radius - PhysicsGlobals.EPSILON, + 2 => radius + PhysicsGlobals.EPSILON, + 3 => 0f, + _ => NextFloat(random, -radius * 1.5f, radius * 1.5f), + }; + Vector3 center = + anchor + polygon.Plane.Normal * signedOffset; + Vector3 movement = (iteration % 7) switch + { + 0 => Vector3.Zero, + 1 => -polygon.Plane.Normal * 0.05f, + 2 => polygon.Plane.Normal * 0.05f, + _ => new Vector3( + NextFloat(random, -0.5f, 0.5f), + NextFloat(random, -0.5f, 0.5f), + NextFloat(random, -0.5f, 0.5f)), + }; + + AssertStaticAndSweptEquivalent( + source.BSP?.Root, + source.Resolved, + flat, + center, + radius, + movement, + cellId, + iteration); + + bool graphInside = BSPQuery.PointInsideCellBsp( + source.CellBSP?.Root, + center); + bool flatInside = + FlatBspQuery.PointInsideCellBsp(flatContainment, center); + Assert.True( + graphInside == flatInside, + $"cell 0x{cellId:X8}, iteration {iteration}: point containment."); + + bool graphSphereInside = BSPQuery.SphereIntersectsCellBsp( + source.CellBSP?.Root, + center, + radius); + bool flatSphereInside = + FlatBspQuery.SphereIntersectsCellBsp( + flatContainment, + center, + radius); + Assert.True( + graphSphereInside == flatSphereInside, + $"cell 0x{cellId:X8}, iteration {iteration}: sphere containment."); + + Transition graphWalkable = NewTransition(); + Transition flatWalkable = NewTransition(); + float probeDistance = iteration % 5 == 0 + ? 0.5f + : NextFloat(random, 0.01f, 1.5f); + bool graphFound = BSPQuery.FindWalkableSphere( + source.BSP?.Root, + source.Resolved, + graphWalkable, + center, + radius, + probeDistance, + Vector3.UnitZ, + out ResolvedPolygon? graphPolygon, + out ushort graphPolygonId, + out Vector3 graphAdjustedCenter); + bool flatFound = FlatBspQuery.FindWalkableSphere( + flat, + flatWalkable, + center, + radius, + probeDistance, + Vector3.UnitZ, + out int flatPolygonIndex, + out ushort flatPolygonId, + out Vector3 flatAdjustedCenter); + Assert.True( + graphFound == flatFound, + $"cell 0x{cellId:X8}, iteration {iteration}: walkable result."); + Assert.Equal(graphPolygonId, flatPolygonId); + Assert.Equal(graphPolygon is null, flatPolygonIndex < 0); + AssertVectorBits(graphAdjustedCenter, flatAdjustedCenter); + AssertTransitionEquivalent(graphWalkable, flatWalkable); + + Sphere sphere0 = new() + { + Origin = center, + Radius = radius, + }; + Sphere? sphere1 = iteration % 2 == 0 + ? new Sphere + { + Origin = center + Vector3.UnitZ * (radius * 1.75f), + Radius = radius, + } + : null; + Vector3 currentCenter = center - movement; + Transition graphTransition = + SeedTransition(sphere0, sphere1, currentCenter); + Transition flatTransition = + SeedTransition(sphere0, sphere1, currentCenter); + ConfigureRandomPath(graphTransition, iteration); + ConfigureRandomPath(flatTransition, iteration); + float angle = + NextFloat(random, -MathF.PI, MathF.PI); + Quaternion localToWorld = + Quaternion.CreateFromAxisAngle(Vector3.UnitZ, angle); + Vector3 localSpaceZ = Vector3.Transform( + Vector3.UnitZ, + Quaternion.Conjugate(localToWorld)); + float scale = (iteration % 3) switch + { + 0 => 0.5f, + 1 => 1f, + _ => 2f, + }; + Vector3 worldOrigin = new( + NextFloat(random, -200f, 200f), + NextFloat(random, -200f, 200f), + NextFloat(random, -20f, 200f)); + + TransitionState graphState = BSPQuery.FindCollisions( + source.BSP?.Root, + source.Resolved, + graphTransition, + sphere0, + sphere1, + currentCenter, + localSpaceZ, + scale, + localToWorld, + engine: null, + worldOrigin); + TransitionState flatState = FlatBspQuery.FindCollisions( + flat, + flatTransition, + sphere0, + sphere1, + currentCenter, + localSpaceZ, + scale, + localToWorld, + engine: null, + worldOrigin); + Assert.True( + graphState == flatState, + $"cell 0x{cellId:X8}, iteration {iteration}: " + + $"collision state graph={graphState}, flat={flatState}."); + AssertTransitionEquivalent(graphTransition, flatTransition); + } + } + } + + [Fact] + public void CompleteResolver_FinalResultBodyAndCellMembership_MatchGraphBits() + { + (PhysicsBSPNode root, Dictionary resolved) = + BuildTwoWallLeaf(); + var containmentRoot = new CellBSPNode + { + Type = BSPNodeType.Leaf, + LeafIndex = 3, + }; + FlatPhysicsBsp flatPhysics = + FlatCollisionAssetBuilder.FlattenPhysicsBsp(root, resolved); + FlatCellContainmentBsp flatContainment = + FlatCollisionAssetBuilder.FlattenCellContainmentBsp( + containmentRoot); + const uint CellId = 0xA9B4_0157u; + + CellPhysics CreateCell() => new() + { + BSP = new PhysicsBSPTree { Root = root }, + Resolved = resolved, + WorldTransform = Matrix4x4.Identity, + InverseWorldTransform = Matrix4x4.Identity, + CellBSP = new CellBSPTree { Root = containmentRoot }, + FlatPhysicsBsp = flatPhysics, + FlatContainmentBsp = flatContainment, + }; + + PhysicsEngine graphEngine = CreateEngine( + CellId, + CreateCell(), + CollisionTraversalMode.Graph); + PhysicsEngine flatEngine = CreateEngine( + CellId, + CreateCell(), + CollisionTraversalMode.Flat); + var random = new Random(0x5245_534F); + + for (int sequence = 0; sequence < 100; sequence++) + { + PhysicsBody graphBody = CreateGroundedBody(CellId); + PhysicsBody flatBody = CreateGroundedBody(CellId); + Vector3 position = new( + NextFloat(random, -1.5f, 1.5f), + NextFloat(random, 0.15f, 0.8f), + 0f); + graphBody.SnapToCell(CellId, position, position); + flatBody.SnapToCell(CellId, position, position); + + for (int frame = 0; frame < 12; frame++) + { + Vector3 movement = (frame % 4) switch + { + 0 => new Vector3(0.08f, -0.12f, 0f), + 1 => new Vector3(-0.05f, -0.08f, 0f), + 2 => new Vector3(0.03f, 0.1f, 0f), + _ => new Vector3( + NextFloat(random, -0.1f, 0.1f), + NextFloat(random, -0.15f, 0.15f), + 0f), + }; + Vector3 target = position + movement; + + ResolveResult graphResult = graphEngine.ResolveWithTransition( + position, + target, + CellId, + sphereRadius: 0.48f, + sphereHeight: 1.835f, + stepUpHeight: 0.1f, + stepDownHeight: 0.04f, + isOnGround: true, + graphBody, + moverFlags: ObjectInfoState.IsPlayer); + ResolveResult flatResult = flatEngine.ResolveWithTransition( + position, + target, + CellId, + sphereRadius: 0.48f, + sphereHeight: 1.835f, + stepUpHeight: 0.1f, + stepDownHeight: 0.04f, + isOnGround: true, + flatBody, + moverFlags: ObjectInfoState.IsPlayer); + + AssertValueBits( + $"sequence[{sequence}].frame[{frame}].result", + graphResult, + flatResult); + AssertValueBits( + $"sequence[{sequence}].frame[{frame}].body", + graphBody, + flatBody); + Assert.Equal(graphResult.CellId, flatResult.CellId); + Assert.Equal(CellId, flatResult.CellId); + position = graphResult.Position; + } + } + } + + [Fact] + public void WarmedFlatTraversal_AllocatesZeroBytes() + { + (PhysicsBSPNode root, Dictionary resolved) = + BuildFloorLeaf(); + FlatPhysicsBsp flat = + FlatCollisionAssetBuilder.FlattenPhysicsBsp(root, resolved); + Sphere sphere = SphereAt(new Vector3(0f, 0f, 0.4f)); + var transition = new Transition(); + + void Execute() + { + transition.ResetForReuse(); + transition.SpherePath.WalkableAllowance = PhysicsGlobals.FloorZ; + transition.SpherePath.WalkInterp = 1f; + transition.SpherePath.StepDown = true; + transition.SpherePath.StepDownAmt = 0.5f; + transition.SpherePath.CheckCellId = 0xA9B4_0157u; + _ = FlatBspQuery.FindCollisions( + flat, + transition, + sphere, + null, + sphere.Origin, + Vector3.UnitZ, + 1f, + Quaternion.Identity, + engine: null, + worldOrigin: Vector3.Zero); + _ = FlatBspQuery.SphereIntersectsPoly( + flat, + sphere.Origin, + sphere.Radius, + out _, + out _); + _ = FlatBspQuery.SphereIntersectsPolyWithTime( + flat, + sphere.Origin, + sphere.Radius, + -Vector3.UnitZ * 0.1f, + out _, + out _, + out _); + } + + for (int i = 0; i < 100; i++) + Execute(); + + long before = GC.GetAllocatedBytesForCurrentThread(); + for (int i = 0; i < 10_000; i++) + Execute(); + long allocated = GC.GetAllocatedBytesForCurrentThread() - before; + + Assert.Equal(0, allocated); + } + + private static PhysicsEngine CreateEngine( + uint cellId, + CellPhysics cell, + CollisionTraversalMode mode) + { + var cache = new PhysicsDataCache + { + CollisionTraversalMode = mode, + }; + cache.RegisterCellStructForTest(cellId, cell); + var engine = new PhysicsEngine + { + DataCache = cache, + }; + var heights = new byte[81]; + var heightTable = new float[256]; + for (int i = 0; i < heightTable.Length; i++) + heightTable[i] = i; + engine.AddLandblock( + 0xA9B4_FFFFu, + new TerrainSurface(heights, heightTable), + Array.Empty(), + Array.Empty(), + 0f, + 0f); + return engine; + } + + private static PhysicsBody CreateGroundedBody(uint cellId) + { + var body = new PhysicsBody + { + ContactPlaneValid = true, + ContactPlane = new Plane(Vector3.UnitZ, 0f), + ContactPlaneCellId = cellId, + TransientState = + TransientStateFlags.Contact | + TransientStateFlags.OnWalkable, + State = PhysicsStateFlags.Gravity, + }; + return body; + } + + private static void AssertStaticAndSweptEquivalent( + PhysicsBSPNode? root, + Dictionary resolved, + FlatPhysicsBsp flat, + Vector3 center, + float radius, + Vector3 movement, + uint cellId, + int iteration) + { + bool graphStatic = BSPQuery.SphereIntersectsPoly( + root, + resolved, + center, + radius, + out ushort graphStaticId, + out Vector3 graphStaticNormal); + bool flatStatic = FlatBspQuery.SphereIntersectsPoly( + flat, + center, + radius, + out ushort flatStaticId, + out Vector3 flatStaticNormal); + Assert.True( + graphStatic == flatStatic, + $"cell 0x{cellId:X8}, iteration {iteration}: static result."); + Assert.Equal(graphStaticId, flatStaticId); + AssertVectorBits(graphStaticNormal, flatStaticNormal); + + bool graphSwept = BSPQuery.SphereIntersectsPolyWithTime( + root, + resolved, + center, + radius, + movement, + out ushort graphSweptId, + out Vector3 graphSweptNormal, + out float graphTime); + bool flatSwept = FlatBspQuery.SphereIntersectsPolyWithTime( + flat, + center, + radius, + movement, + out ushort flatSweptId, + out Vector3 flatSweptNormal, + out float flatTime); + Assert.True( + graphSwept == flatSwept, + $"cell 0x{cellId:X8}, iteration {iteration}: swept result."); + Assert.Equal(graphSweptId, flatSweptId); + AssertVectorBits(graphSweptNormal, flatSweptNormal); + AssertFloatBits(graphTime, flatTime); + } + + private static void ConfigureRandomPath( + Transition transition, + int iteration) + { + switch (iteration % 6) + { + case 0: + transition.SpherePath.InsertType = InsertType.Placement; + break; + case 1: + transition.SpherePath.CheckWalkable = true; + break; + case 2: + transition.SpherePath.StepDown = true; + transition.SpherePath.StepDownAmt = 0.5f; + break; + case 3: + transition.SpherePath.Collide = true; + break; + case 4: + transition.ObjectInfo.State = ObjectInfoState.Contact; + break; + default: + if ((iteration & 12) == 12) + { + transition.ObjectInfo.State = + ObjectInfoState.PathClipped | + ObjectInfoState.PerfectClip; + } + break; + } + } + + private static void AssertFindCollisionsEquivalent( + PhysicsBSPNode root, + Dictionary resolved, + Sphere sphere0, + Sphere? sphere1, + Vector3 localCurrentCenter, + Action? configure, + Vector3 worldOrigin = default) + { + FlatPhysicsBsp flat = + FlatCollisionAssetBuilder.FlattenPhysicsBsp(root, resolved); + Transition graphTransition = + SeedTransition(sphere0, sphere1, localCurrentCenter); + Transition flatTransition = + SeedTransition(sphere0, sphere1, localCurrentCenter); + configure?.Invoke(graphTransition); + configure?.Invoke(flatTransition); + + TransitionState graphState = BSPQuery.FindCollisions( + root, + resolved, + graphTransition, + sphere0, + sphere1, + localCurrentCenter, + Vector3.UnitZ, + 1f, + Quaternion.Identity, + engine: null, + worldOrigin); + TransitionState flatState = FlatBspQuery.FindCollisions( + flat, + flatTransition, + sphere0, + sphere1, + localCurrentCenter, + Vector3.UnitZ, + 1f, + Quaternion.Identity, + engine: null, + worldOrigin); + + Assert.Equal(graphState, flatState); + AssertTransitionEquivalent(graphTransition, flatTransition); + } + + private static Transition SeedTransition( + Sphere sphere0, + Sphere? sphere1, + Vector3 localCurrentCenter) + { + Transition transition = NewTransition(); + SpherePath path = transition.SpherePath; + path.NumSphere = sphere1 is null ? 1 : 2; + path.LocalSphere[0].Origin = sphere0.Origin; + path.LocalSphere[0].Radius = sphere0.Radius; + path.GlobalSphere[0].Origin = sphere0.Origin; + path.GlobalSphere[0].Radius = sphere0.Radius; + path.GlobalCurrCenter[0].Origin = localCurrentCenter; + path.GlobalCurrCenter[0].Radius = sphere0.Radius; + if (sphere1 is not null) + { + path.LocalSphere[1].Origin = sphere1.Origin; + path.LocalSphere[1].Radius = sphere1.Radius; + path.GlobalSphere[1].Origin = sphere1.Origin; + path.GlobalSphere[1].Radius = sphere1.Radius; + path.GlobalCurrCenter[1].Origin = + sphere1.Origin - (sphere0.Origin - localCurrentCenter); + path.GlobalCurrCenter[1].Radius = sphere1.Radius; + } + + path.CheckPos = sphere0.Origin; + path.CheckCellId = 0xA9B4_013Fu; + path.BackupCheckPos = new Vector3(3f, 5f, 7f); + path.BackupCheckCellId = 0x8A02_016Eu; + return transition; + } + + private static Transition NewTransition() + { + var transition = new Transition(); + transition.SpherePath.WalkableAllowance = PhysicsGlobals.FloorZ; + transition.SpherePath.WalkInterp = 1f; + return transition; + } + + private static (PhysicsBSPNode, Dictionary) + BuildFloorLeaf() + { + ResolvedPolygon floor = Polygon( + 7, + new Plane(Vector3.UnitZ, 0f), + new Vector3(-2f, -2f, 0f), + new Vector3(2f, -2f, 0f), + new Vector3(2f, 2f, 0f), + new Vector3(-2f, 2f, 0f)); + PhysicsBSPNode root = Leaf(); + root.Polygons.Add(floor.Id); + return (root, new Dictionary + { + [floor.Id] = floor, + }); + } + + private static (PhysicsBSPNode, Dictionary) + BuildTwoFloorLeaf() + { + ResolvedPolygon lower = Polygon( + 9, + new Plane(Vector3.UnitZ, 0f), + new Vector3(-2f, -2f, 0f), + new Vector3(2f, -2f, 0f), + new Vector3(2f, 2f, 0f), + new Vector3(-2f, 2f, 0f)); + ResolvedPolygon upper = Polygon( + 4, + new Plane(Vector3.UnitZ, -0.75f), + new Vector3(-2f, -2f, 0.75f), + new Vector3(2f, -2f, 0.75f), + new Vector3(2f, 2f, 0.75f), + new Vector3(-2f, 2f, 0.75f)); + PhysicsBSPNode root = Leaf(); + root.Polygons.Add(lower.Id); + root.Polygons.Add(upper.Id); + return (root, new Dictionary + { + [upper.Id] = upper, + [lower.Id] = lower, + }); + } + + private static (PhysicsBSPNode, Dictionary) + BuildTwoWallLeaf() + { + ResolvedPolygon first = Polygon( + 9, + new Plane(Vector3.UnitY, 0f), + new Vector3(-2f, 0f, -2f), + new Vector3(-2f, 0f, 2f), + new Vector3(2f, 0f, 2f), + new Vector3(2f, 0f, -2f)); + ResolvedPolygon second = Polygon( + 4, + new Plane(Vector3.UnitY, -0.1f), + new Vector3(-2f, 0.1f, -2f), + new Vector3(-2f, 0.1f, 2f), + new Vector3(2f, 0.1f, 2f), + new Vector3(2f, 0.1f, -2f)); + PhysicsBSPNode root = Leaf(); + root.Polygons.Add(first.Id); + root.Polygons.Add(second.Id); + return (root, new Dictionary + { + [second.Id] = second, + [first.Id] = first, + }); + } + + private static PhysicsBSPNode Leaf() => new() + { + Type = BSPNodeType.Leaf, + BoundingSphere = new Sphere + { + Origin = Vector3.Zero, + Radius = 10_000f, + }, + }; + + private static ResolvedPolygon Polygon( + ushort id, + Plane plane, + params Vector3[] vertices) => new() + { + Id = id, + Plane = plane, + SidesType = CullMode.None, + NumPoints = vertices.Length, + Vertices = vertices, + }; + + private static Sphere SphereAt(Vector3 center) => new() + { + Origin = center, + Radius = 0.48f, + }; + + private static float NextFloat(Random random, float minimum, float maximum) + => minimum + (float)random.NextDouble() * (maximum - minimum); + + private static void AssertTransitionEquivalent( + Transition expected, + Transition actual) + { + AssertValueBits("ObjectInfo", expected.ObjectInfo, actual.ObjectInfo); + AssertValueBits("SpherePath", expected.SpherePath, actual.SpherePath); + AssertValueBits("CollisionInfo", expected.CollisionInfo, actual.CollisionInfo); + Assert.Equal( + expected.CollisionInfo.ContactPlaneWriteCount, + actual.CollisionInfo.ContactPlaneWriteCount); + } + + private static void AssertValueBits( + string path, + object? expected, + object? actual) + { + if (expected is null || actual is null) + { + Assert.True( + expected is null && actual is null, + $"{path}: one value is null."); + return; + } + + Type expectedType = expected.GetType(); + Assert.Equal(expectedType, actual.GetType()); + + if (expected is float expectedFloat && + actual is float actualFloat) + { + AssertFloatBits(expectedFloat, actualFloat, path); + return; + } + + if (expected is Vector3 expectedVector && + actual is Vector3 actualVector) + { + AssertVectorBits(expectedVector, actualVector, path); + return; + } + + if (expected is Quaternion expectedQuaternion && + actual is Quaternion actualQuaternion) + { + AssertFloatBits( + expectedQuaternion.X, + actualQuaternion.X, + $"{path}.X"); + AssertFloatBits( + expectedQuaternion.Y, + actualQuaternion.Y, + $"{path}.Y"); + AssertFloatBits( + expectedQuaternion.Z, + actualQuaternion.Z, + $"{path}.Z"); + AssertFloatBits( + expectedQuaternion.W, + actualQuaternion.W, + $"{path}.W"); + return; + } + + if (expected is Plane expectedPlane && + actual is Plane actualPlane) + { + AssertVectorBits( + expectedPlane.Normal, + actualPlane.Normal, + $"{path}.Normal"); + AssertFloatBits(expectedPlane.D, actualPlane.D, $"{path}.D"); + return; + } + + if (expectedType.IsEnum || + expectedType.IsPrimitive || + expected is string || + expected is decimal) + { + Assert.True( + expected.Equals(actual), + $"{path}: expected {expected}, actual {actual}."); + return; + } + + if (expected is IEnumerable expectedEnumerable && + actual is IEnumerable actualEnumerable) + { + object?[] expectedItems = expectedEnumerable.Cast().ToArray(); + object?[] actualItems = actualEnumerable.Cast().ToArray(); + Assert.Equal(expectedItems.Length, actualItems.Length); + for (int i = 0; i < expectedItems.Length; i++) + { + AssertValueBits( + $"{path}[{i}]", + expectedItems[i], + actualItems[i]); + } + + return; + } + + FieldInfo[] fields = expectedType + .GetFields(BindingFlags.Instance | BindingFlags.Public) + .OrderBy(field => field.Name, StringComparer.Ordinal) + .ToArray(); + PropertyInfo[] properties = expectedType + .GetProperties(BindingFlags.Instance | BindingFlags.Public) + .Where(property => + property.CanRead && + property.GetIndexParameters().Length == 0) + .OrderBy(property => property.Name, StringComparer.Ordinal) + .ToArray(); + Assert.True( + fields.Length != 0 || properties.Length != 0, + $"{path}: unsupported comparison type {expectedType.FullName}."); + + foreach (FieldInfo field in fields) + { + AssertValueBits( + $"{path}.{field.Name}", + field.GetValue(expected), + field.GetValue(actual)); + } + + foreach (PropertyInfo property in properties) + { + AssertValueBits( + $"{path}.{property.Name}", + property.GetValue(expected), + property.GetValue(actual)); + } + } + + private static void AssertVectorBits( + Vector3 expected, + Vector3 actual, + string path = "vector") + { + AssertFloatBits(expected.X, actual.X, $"{path}.X"); + AssertFloatBits(expected.Y, actual.Y, $"{path}.Y"); + AssertFloatBits(expected.Z, actual.Z, $"{path}.Z"); + } + + private static void AssertFloatBits( + float expected, + float actual, + string path = "float") + { + Assert.True( + BitConverter.SingleToInt32Bits(expected) == + BitConverter.SingleToInt32Bits(actual), + $"{path}: expected 0x{BitConverter.SingleToInt32Bits(expected):X8}, " + + $"actual 0x{BitConverter.SingleToInt32Bits(actual):X8}."); + } +}