feat(physics): WalkMissDiagnostic aggregator for ISSUES #83 probe spike
Pure-function aggregator that, given a CellPhysics.Resolved dict and a foot local position, picks the nearest walkable-eligible polygon (normal Z >= FloorZ) and reports XY-containment + signed vertical gap. Also enumerates walkable polys with local-XY bboxes for the one-shot [floor-polys] cell-load dump. Pure-function, no behavior change. Wiring to emission sites lands in the next commit. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Erik
parent
27c728484d
commit
31da57c94c
2 changed files with 251 additions and 0 deletions
173
src/AcDream.Core/Physics/WalkMissDiagnostic.cs
Normal file
173
src/AcDream.Core/Physics/WalkMissDiagnostic.cs
Normal file
|
|
@ -0,0 +1,173 @@
|
|||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
|
||||
namespace AcDream.Core.Physics;
|
||||
|
||||
/// <summary>
|
||||
/// ISSUES #83 H-disambiguation spike (2026-05-21). Pure-function
|
||||
/// aggregator over a <see cref="CellPhysics.Resolved"/> dict — picks
|
||||
/// the nearest walkable-eligible polygon to a given foot position
|
||||
/// (cell-local space) and reports XY-containment + vertical gap so
|
||||
/// the <c>[walk-miss]</c> emission site can disambiguate H1/H2/H3
|
||||
/// without re-walking the dictionary itself.
|
||||
///
|
||||
/// <para>
|
||||
/// Also enumerates walkable polygons for the one-shot
|
||||
/// <c>[floor-polys]</c> dump at cell-cache time.
|
||||
/// </para>
|
||||
///
|
||||
/// <para>
|
||||
/// Spec: <c>docs/superpowers/specs/2026-05-21-indoor-walk-miss-probe-design.md</c>.
|
||||
/// </para>
|
||||
/// </summary>
|
||||
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; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Walks <paramref name="resolved"/>, considering only polygons
|
||||
/// whose plane normal Z is at least <paramref name="floorZ"/>
|
||||
/// (walkable slope). Selection rule:
|
||||
/// <list type="number">
|
||||
/// <item><description>Polygons whose local-XY bounding box contains
|
||||
/// <paramref name="footLocal"/>'s XY are preferred. Among them,
|
||||
/// the one with smallest <c>|dz|</c> wins.</description></item>
|
||||
/// <item><description>If no poly contains the foot XY, the poly
|
||||
/// with smallest <c>|dz|</c> across all walkable polys wins,
|
||||
/// and <see cref="AggregateResult.ContainsFootXY"/> is false.</description></item>
|
||||
/// </list>
|
||||
/// </summary>
|
||||
public static AggregateResult AggregateNearestWalkable(
|
||||
IReadOnlyDictionary<ushort, ResolvedPolygon> 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,
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 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 <c>[floor-polys]</c> cell-load
|
||||
/// dump.
|
||||
/// </summary>
|
||||
public static IEnumerable<WalkableEntry> EnumerateWalkable(
|
||||
IReadOnlyDictionary<ushort, ResolvedPolygon> 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,
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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,
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 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|.
|
||||
/// </summary>
|
||||
[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<ushort, ResolvedPolygon>
|
||||
{
|
||||
[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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Foot at (10,10,1) — outside both poly XY bboxes. Aggregator
|
||||
/// returns the poly with smallest |dz| but with ContainsFootXY=false.
|
||||
/// </summary>
|
||||
[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<ushort, ResolvedPolygon> { [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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue