diff --git a/src/AcDream.Core/Physics/WalkMissDiagnostic.cs b/src/AcDream.Core/Physics/WalkMissDiagnostic.cs new file mode 100644 index 0000000..35a7dcf --- /dev/null +++ b/src/AcDream.Core/Physics/WalkMissDiagnostic.cs @@ -0,0 +1,173 @@ +using System.Collections.Generic; +using System.Numerics; + +namespace AcDream.Core.Physics; + +/// +/// ISSUES #83 H-disambiguation spike (2026-05-21). Pure-function +/// aggregator over a dict — picks +/// the nearest walkable-eligible polygon to a given foot position +/// (cell-local space) and reports XY-containment + vertical gap so +/// the [walk-miss] emission site can disambiguate H1/H2/H3 +/// without re-walking the dictionary itself. +/// +/// +/// Also enumerates walkable polygons for the one-shot +/// [floor-polys] dump at cell-cache time. +/// +/// +/// +/// Spec: docs/superpowers/specs/2026-05-21-indoor-walk-miss-probe-design.md. +/// +/// +public static class WalkMissDiagnostic +{ + public readonly struct AggregateResult + { + public bool Found { get; init; } + public ushort PolyId { get; init; } + public bool ContainsFootXY { get; init; } + public float Dz { get; init; } + public float NormalZ { get; init; } + } + + public readonly struct WalkableEntry + { + public ushort PolyId { get; init; } + public float NormalZ { get; init; } + public Vector3 BboxMin { get; init; } + public Vector3 BboxMax { get; init; } + public float PlaneZAtBboxCenter { get; init; } + } + + /// + /// Walks , considering only polygons + /// whose plane normal Z is at least + /// (walkable slope). Selection rule: + /// + /// Polygons whose local-XY bounding box contains + /// 's XY are preferred. Among them, + /// the one with smallest |dz| wins. + /// If no poly contains the foot XY, the poly + /// with smallest |dz| across all walkable polys wins, + /// and is false. + /// + /// + public static AggregateResult AggregateNearestWalkable( + IReadOnlyDictionary resolved, + Vector3 footLocal, + float floorZ) + { + bool bestFound = false; + bool bestContainsFootXY = false; + ushort bestPolyId = 0; + float bestAbsDz = float.MaxValue; + float bestSignedDz = 0f; + float bestNormalZ = 0f; + + foreach (var kvp in resolved) + { + var poly = kvp.Value; + if (poly.Plane.Normal.Z < floorZ) continue; + if (poly.Vertices.Length < 3) continue; + + // Local-XY bounding box. + float minX = float.MaxValue, minY = float.MaxValue; + float maxX = float.MinValue, maxY = float.MinValue; + for (int i = 0; i < poly.Vertices.Length; i++) + { + var v = poly.Vertices[i]; + if (v.X < minX) minX = v.X; + if (v.Y < minY) minY = v.Y; + if (v.X > maxX) maxX = v.X; + if (v.Y > maxY) maxY = v.Y; + } + bool containsFootXY = + footLocal.X >= minX && footLocal.X <= maxX && + footLocal.Y >= minY && footLocal.Y <= maxY; + + // Signed vertical gap from foot to the polygon's plane at + // the foot's XY: plane.D + n.x*X + n.y*Y + n.z*Z = 0 + // => planeZ = -(D + n.x*X + n.y*Y) / n.z + // => dz = footZ - planeZ + float planeZ = -(poly.Plane.D + + poly.Plane.Normal.X * footLocal.X + + poly.Plane.Normal.Y * footLocal.Y) + / poly.Plane.Normal.Z; + float signedDz = footLocal.Z - planeZ; + float absDz = MathF.Abs(signedDz); + + // Preference: prefer XY-containing polys. Among the + // preferred set, smallest |dz| wins. + bool preferOver = !bestFound + || (containsFootXY && !bestContainsFootXY) + || (containsFootXY == bestContainsFootXY && absDz < bestAbsDz); + + if (preferOver) + { + bestFound = true; + bestContainsFootXY = containsFootXY; + bestPolyId = kvp.Key; + bestAbsDz = absDz; + bestSignedDz = signedDz; + bestNormalZ = poly.Plane.Normal.Z; + } + } + + return new AggregateResult + { + Found = bestFound, + PolyId = bestPolyId, + ContainsFootXY = bestContainsFootXY, + Dz = bestSignedDz, + NormalZ = bestNormalZ, + }; + } + + /// + /// Enumerates walkable-eligible polygons (normal Z >= floorZ) + /// with their local-XY bounding boxes and plane Z at the bbox + /// center. Used by the one-shot [floor-polys] cell-load + /// dump. + /// + public static IEnumerable EnumerateWalkable( + IReadOnlyDictionary resolved, + float floorZ) + { + foreach (var kvp in resolved) + { + var poly = kvp.Value; + if (poly.Plane.Normal.Z < floorZ) continue; + if (poly.Vertices.Length < 3) continue; + + float minX = float.MaxValue, minY = float.MaxValue, minZ = float.MaxValue; + float maxX = float.MinValue, maxY = float.MinValue, maxZ = float.MinValue; + for (int i = 0; i < poly.Vertices.Length; i++) + { + var v = poly.Vertices[i]; + if (v.X < minX) minX = v.X; + if (v.Y < minY) minY = v.Y; + if (v.Z < minZ) minZ = v.Z; + if (v.X > maxX) maxX = v.X; + if (v.Y > maxY) maxY = v.Y; + if (v.Z > maxZ) maxZ = v.Z; + } + + float cx = (minX + maxX) * 0.5f; + float cy = (minY + maxY) * 0.5f; + float planeZAtCenter = -(poly.Plane.D + + poly.Plane.Normal.X * cx + + poly.Plane.Normal.Y * cy) + / poly.Plane.Normal.Z; + + yield return new WalkableEntry + { + PolyId = kvp.Key, + NormalZ = poly.Plane.Normal.Z, + BboxMin = new Vector3(minX, minY, minZ), + BboxMax = new Vector3(maxX, maxY, maxZ), + PlaneZAtBboxCenter = planeZAtCenter, + }; + } + } +} diff --git a/tests/AcDream.Core.Tests/Physics/WalkMissDiagnosticTests.cs b/tests/AcDream.Core.Tests/Physics/WalkMissDiagnosticTests.cs index 91de8c4..5581c11 100644 --- a/tests/AcDream.Core.Tests/Physics/WalkMissDiagnosticTests.cs +++ b/tests/AcDream.Core.Tests/Physics/WalkMissDiagnosticTests.cs @@ -38,4 +38,82 @@ public class WalkMissDiagnosticTests PhysicsDiagnostics.ProbeWalkMissEnabled = initial; } } + + private static ResolvedPolygon MakeFloorPoly( + Vector3 v00, Vector3 v10, Vector3 v11, Vector3 v01) + { + var verts = new[] { v00, v10, v11, v01 }; + var normal = Vector3.Normalize(Vector3.Cross(v10 - v00, v01 - v00)); + float d = -Vector3.Dot(normal, v00); + return new ResolvedPolygon + { + Vertices = verts, + Plane = new System.Numerics.Plane(normal, d), + NumPoints = 4, + SidesType = CullMode.None, + }; + } + + /// + /// Foot at (0,0,1). Two walkable polys: a low one at Z=0 (foot is + /// 1 m above) and a high one at Z=0.8 (foot is 0.2 m above). + /// Aggregator picks the high one — smaller |dz|. + /// + [Fact] + public void AggregateNearestWalkable_PicksNearestByDz_WhenFootXYInsideMultiplePolys() + { + var lowFloor = MakeFloorPoly( + new Vector3(-5f, -5f, 0f), + new Vector3( 5f, -5f, 0f), + new Vector3( 5f, 5f, 0f), + new Vector3(-5f, 5f, 0f)); + var highFloor = MakeFloorPoly( + new Vector3(-2f, -2f, 0.8f), + new Vector3( 2f, -2f, 0.8f), + new Vector3( 2f, 2f, 0.8f), + new Vector3(-2f, 2f, 0.8f)); + + var resolved = new Dictionary + { + [1] = lowFloor, + [2] = highFloor, + }; + + var result = WalkMissDiagnostic.AggregateNearestWalkable( + resolved, + footLocal: new Vector3(0f, 0f, 1f), + floorZ: PhysicsGlobals.FloorZ); + + Assert.True(result.Found); + Assert.Equal((ushort)2, result.PolyId); + Assert.True(result.ContainsFootXY); + Assert.Equal(0.2f, result.Dz, precision: 5); + Assert.Equal(1.0f, result.NormalZ, precision: 5); + } + + /// + /// Foot at (10,10,1) — outside both poly XY bboxes. Aggregator + /// returns the poly with smallest |dz| but with ContainsFootXY=false. + /// + [Fact] + public void AggregateNearestWalkable_FallsBackByDz_WhenFootXYOutsideAllBboxes() + { + var poly = MakeFloorPoly( + new Vector3(-1f, -1f, 0.5f), + new Vector3( 1f, -1f, 0.5f), + new Vector3( 1f, 1f, 0.5f), + new Vector3(-1f, 1f, 0.5f)); + + var resolved = new Dictionary { [42] = poly }; + + var result = WalkMissDiagnostic.AggregateNearestWalkable( + resolved, + footLocal: new Vector3(10f, 10f, 1f), + floorZ: PhysicsGlobals.FloorZ); + + Assert.True(result.Found); + Assert.Equal((ushort)42, result.PolyId); + Assert.False(result.ContainsFootXY); + Assert.Equal(0.5f, result.Dz, precision: 5); + } }