acdream/tests/AcDream.Core.Tests/Physics/WalkMissDiagnosticTests.cs

using AcDream.Core.Physics;
using DatReaderWriter.Enums;
using System.Collections.Generic;
using System.Numerics;
using Xunit;

namespace AcDream.Core.Tests.Physics;

/// <summary>
/// Tests for the ISSUES #83 H-disambiguation probe spike (spec
/// 2026-05-21-indoor-walk-miss-probe-design.md).
///
/// Covers:
/// 1. PhysicsDiagnostics.ProbeWalkMissEnabled flag get/set roundtrip.
/// 2. WalkMissDiagnostic.AggregateNearestWalkable selects the nearest
///    walkable polygon by |dz| when the foot XY lies inside a poly's
///    local XY bounding box.
/// 3. WalkMissDiagnostic.AggregateNearestWalkable falls back to the
///    nearest poly by |dz| when no walkable poly XY-contains the foot,
///    reporting ContainsFootXY=false.
/// </summary>
public class WalkMissDiagnosticTests
{
    [Fact]
    public void ProbeWalkMiss_StaticApi_Roundtrip()
    {
        bool initial = PhysicsDiagnostics.ProbeWalkMissEnabled;
        try
        {
            PhysicsDiagnostics.ProbeWalkMissEnabled = true;
            Assert.True(PhysicsDiagnostics.ProbeWalkMissEnabled);

            PhysicsDiagnostics.ProbeWalkMissEnabled = false;
            Assert.False(PhysicsDiagnostics.ProbeWalkMissEnabled);
        }
        finally
        {
            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);
    }
}