acdream/src/AcDream.Core/Physics/WalkMissDiagnostic.cs

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 &gt;= 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,
            };
        }
    }
}