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

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

namespace AcDream.Core.Tests.Physics;

/// <summary>
/// Synthetic BSP tree fixtures for step-up and roof-landing conformance tests.
///
/// <para>
/// These fixtures construct minimal <see cref="PhysicsBSPNode"/> trees plus
/// matching <see cref="ResolvedPolygon"/> dictionaries that represent canonical
/// AC collision shapes without needing real DAT content.  The shapes cover every
/// interesting branch in <see cref="BSPQuery.FindCollisions"/> Path 5 and Path 6.
/// </para>
///
/// <para>
/// Coordinate convention: +Z is up, all geometry is expressed in object-local
/// space (identity rotation, scale = 1.0) with objects at world origin so that
/// <c>localSphere.Origin == worldPosition</c>.
/// </para>
///
/// <para>
/// Retail references:
///   BSPTREE::find_collisions Path 5 — acclient_2013_pseudo_c.txt:323849 /
///     ACE BSPTree.cs:192-196.
///   BSPTREE::find_collisions Path 6 / set_collide —
///     acclient_2013_pseudo_c.txt:323819 / ACE BSPTree.cs:210-219.
///   CTransition::step_up — acclient_2013_pseudo_c.txt:273099-273133 /
///     ACE Transition.cs:746-777.
///   SPHEREPATH::set_collide — acclient_2013_pseudo_c.txt:321594-321607 /
///     ACE SpherePath.cs:279-286.
/// </para>
/// </summary>
public static class BSPStepUpFixtures
{
    // -------------------------------------------------------------------------
    // Polygon ID constants — each fixture uses a distinct range so the
    // resolved-polygon dictionary is unambiguous when fixtures are composed.
    // -------------------------------------------------------------------------
    public const ushort LowStep_FloorId     = 10;
    public const ushort LowStep_WallId      = 11;
    public const ushort LowStep_UpperFloorId = 12;

    public const ushort TallWall_FloorId    = 20;
    public const ushort TallWall_WallId     = 21;

    public const ushort FlatRoof_FloorId    = 30;
    public const ushort FlatRoof_RoofId     = 31;

    public const ushort SlopedUnwalkable_FloorId = 40;
    public const ushort SlopedUnwalkable_SlopeId = 41;

    // -------------------------------------------------------------------------
    // Sphere radius used in every test.
    // -------------------------------------------------------------------------
    public const float SphereRadius = 0.2f;

    // =========================================================================
    // Fixture 1 — Low step (25 cm)
    //
    //  Schema (side view, XZ plane):
    //
    //       +X ──────────────────►
    //    Z
    //    0.5 ┆           ┌───────  ← UpperFloor at z=0.25 (vert 8..11)
    //    0.25├───────────┤
    //        ┆   Wall    ┆ (x=0.5, z=[0,0.25])
    //    0.0 ┆═══════════┘
    //                               ← Floor at z=0 (vert 0..3)
    //
    //  The mover starts grounded at x=-0.5, z=SphereRadius and walks toward +X.
    //  Expected: step-up succeeds when Contact is set; sphere lifts to z=0.25+eps.
    // =========================================================================

    /// <summary>
    /// Constructs a BSP tree and resolved-polygon dict representing a 25 cm step.
    ///
    /// <para>Geometry (object-local space):</para>
    /// <list type="bullet">
    ///   <item>Floor polygon at z = 0, x ∈ [-2, 0.5], y ∈ [-1, 1].</item>
    ///   <item>Vertical wall polygon at x = 0.5, z ∈ [0, 0.25], y ∈ [-1, 1], facing -X.</item>
    ///   <item>Upper floor polygon at z = 0.25, x ∈ [0.2, 2], y ∈ [-1, 1] — extends
    ///     left of the wall face so the vertical step-down probe finds it when the
    ///     sphere is at x ≈ 0.3–0.5 (the wall contact zone).</item>
    /// </list>
    /// </summary>
    public static (PhysicsBSPNode Root, Dictionary<ushort, ResolvedPolygon> Resolved)
        LowStep()
    {
        var resolved = new Dictionary<ushort, ResolvedPolygon>();

        // Lower floor: z=0, x∈[-2,0.5], y∈[-1,1], normal = +Z
        resolved[LowStep_FloorId] = MakeFloor(
            new Vector3(-2f, -1f, 0f), new Vector3(0.5f, -1f, 0f),
            new Vector3(0.5f,  1f, 0f), new Vector3(-2f,  1f, 0f));

        // Vertical wall facing -X at x=0.5, z∈[0,0.25], normal = -X
        // For normal=(-1,0,0), the winding that makes cross(normal,edge)·disp > 0
        // for interior points is: (y=-1,z=0)→(y=-1,z=0.25)→(y=1,z=0.25)→(y=1,z=0).
        resolved[LowStep_WallId] = MakeQuad(
            new Vector3(0.5f, -1f, 0f),
            new Vector3(0.5f, -1f, 0.25f),
            new Vector3(0.5f,  1f, 0.25f),
            new Vector3(0.5f,  1f, 0f),
            expectedNormal: new Vector3(-1f, 0f, 0f));

        // Upper floor at z=0.25, x∈[0.2,2], y∈[-1,1], normal = +Z.
        // The upper floor extends slightly left of the wall face (x=0.5)
        // so the step-down probe (vertical, from the wall-contact XY) can
        // find it when the sphere is at x≈0.3-0.5.  Retail BSPs have the
        // same overlap because geometry is continuous across the step.
        resolved[LowStep_UpperFloorId] = MakeFloor(
            new Vector3(0.2f, -1f, 0.25f), new Vector3(2f, -1f, 0.25f),
            new Vector3(2f,   1f, 0.25f), new Vector3(0.2f, 1f, 0.25f));

        // Build a flat BSP tree: one internal node with all three polys in a leaf.
        // The bounding sphere covers everything.
        var leaf = new PhysicsBSPNode
        {
            Type = BSPNodeType.Leaf,
            BoundingSphere = new Sphere { Origin = Vector3.Zero, Radius = 10f },
        };
        leaf.Polygons.Add(LowStep_FloorId);
        leaf.Polygons.Add(LowStep_WallId);
        leaf.Polygons.Add(LowStep_UpperFloorId);

        return (leaf, resolved);
    }

    // =========================================================================
    // Fixture 2 — Too-tall wall (5 m)
    //
    //  A floor at z=0 and a 5 m wall at x=0.5 with no floor on the other side.
    //  Expected: step-up fails (wall too tall), mover slides along wall.
    // =========================================================================

    /// <summary>
    /// Constructs a BSP tree and resolved-polygon dict representing a wall that
    /// is too tall to step over (5 m), so step-up should fail.
    /// </summary>
    public static (PhysicsBSPNode Root, Dictionary<ushort, ResolvedPolygon> Resolved)
        TallWall()
    {
        var resolved = new Dictionary<ushort, ResolvedPolygon>();

        // Floor at z=0
        resolved[TallWall_FloorId] = MakeFloor(
            new Vector3(-2f, -1f, 0f), new Vector3(0.5f, -1f, 0f),
            new Vector3(0.5f,  1f, 0f), new Vector3(-2f,  1f, 0f));

        // Tall wall at x=0.5, z∈[0,5], normal = -X
        // Winding for normal=(-1,0,0): (y=-1,z=0)→(y=-1,z=5)→(y=1,z=5)→(y=1,z=0).
        resolved[TallWall_WallId] = MakeQuad(
            new Vector3(0.5f, -1f, 0f),
            new Vector3(0.5f, -1f, 5f),
            new Vector3(0.5f,  1f, 5f),
            new Vector3(0.5f,  1f, 0f),
            expectedNormal: new Vector3(-1f, 0f, 0f));

        var leaf = new PhysicsBSPNode
        {
            Type = BSPNodeType.Leaf,
            BoundingSphere = new Sphere { Origin = new Vector3(0f, 0f, 2.5f), Radius = 10f },
        };
        leaf.Polygons.Add(TallWall_FloorId);
        leaf.Polygons.Add(TallWall_WallId);

        return (leaf, resolved);
    }

    // =========================================================================
    // Fixture 3 — Flat roof (3 m)
    //
    //  A horizontal polygon at z=3 representing a building rooftop.
    //  The mover is airborne (no Contact flag) descending toward the roof.
    //  Expected (after L.2.2): Path 6 sets Collide flag; the Collide-flag handler
    //  re-tests as Placement; ContactPlane is set; OnWalkable is established.
    // =========================================================================

    /// <summary>
    /// Constructs a BSP tree and resolved-polygon dict representing a 3 m flat roof.
    /// </summary>
    public static (PhysicsBSPNode Root, Dictionary<ushort, ResolvedPolygon> Resolved)
        FlatRoof()
    {
        var resolved = new Dictionary<ushort, ResolvedPolygon>();

        // Ground floor for reference (not involved in landing test)
        resolved[FlatRoof_FloorId] = MakeFloor(
            new Vector3(-2f, -1f, 0f), new Vector3(2f, -1f, 0f),
            new Vector3(2f,   1f, 0f), new Vector3(-2f, 1f, 0f));

        // Roof at z=3.0, x∈[-2,2], y∈[-1,1], normal = +Z
        resolved[FlatRoof_RoofId] = MakeFloor(
            new Vector3(-2f, -1f, 3f), new Vector3(2f, -1f, 3f),
            new Vector3(2f,   1f, 3f), new Vector3(-2f, 1f, 3f));

        var leaf = new PhysicsBSPNode
        {
            Type = BSPNodeType.Leaf,
            BoundingSphere = new Sphere { Origin = new Vector3(0f, 0f, 1.5f), Radius = 10f },
        };
        leaf.Polygons.Add(FlatRoof_FloorId);
        leaf.Polygons.Add(FlatRoof_RoofId);

        return (leaf, resolved);
    }

    // =========================================================================
    // Fixture 4 — Sloped unwalkable surface (60°)
    //
    //  A flat reference floor plus an angled slope at ~60° from horizontal.
    //  normal.Z = cos(60°) ≈ 0.5 < PhysicsGlobals.FloorZ (0.6642).
    //  Expected: no contact plane set; mover slides off.
    // =========================================================================

    /// <summary>
    /// Constructs a BSP tree and resolved-polygon dict representing a steep (60°)
    /// slope whose normal.Z is below the walkable threshold.
    /// </summary>
    public static (PhysicsBSPNode Root, Dictionary<ushort, ResolvedPolygon> Resolved)
        SlopedUnwalkable()
    {
        var resolved = new Dictionary<ushort, ResolvedPolygon>();

        // Reference floor at z=0
        resolved[SlopedUnwalkable_FloorId] = MakeFloor(
            new Vector3(-2f, -1f, 0f), new Vector3(0f, -1f, 0f),
            new Vector3(0f,   1f, 0f), new Vector3(-2f, 1f, 0f));

        // Steep slope: rises 2 m over 1 m horizontal run (63.4° from horizontal).
        // Vertices: (0,-1,0), (1,-1,2), (1,1,2), (0,1,0)
        // Normal direction: cross((1,0,2)-(0,0,0), (0,1,0)-(0,0,0)) ∝ (-2,0,1) normalised
        // After normalisation: (-0.894, 0, 0.447) — normal.Z ≈ 0.447 < FloorZ.
        // We point the normal outward (-X side) so it represents a wall-like slope.
        var v0 = new Vector3(0f, -1f, 0f);
        var v1 = new Vector3(1f, -1f, 2f);
        var v2 = new Vector3(1f,  1f, 2f);
        var v3 = new Vector3(0f,  1f, 0f);
        var raw = Vector3.Cross(v1 - v0, v3 - v0);
        var slopeNormal = Vector3.Normalize(raw);
        // Ensure the normal faces away from the approach side (-X direction).
        if (slopeNormal.X > 0) slopeNormal = -slopeNormal;

        var vertices = new[] { v0, v1, v2, v3 };
        float dotSum = 0f;
        foreach (var v in vertices) dotSum += Vector3.Dot(slopeNormal, v);
        float d = -(dotSum / vertices.Length);

        resolved[SlopedUnwalkable_SlopeId] = new ResolvedPolygon
        {
            Vertices  = vertices,
            Plane     = new Plane(slopeNormal, d),
            NumPoints = 4,
            SidesType = CullMode.None,
        };

        var leaf = new PhysicsBSPNode
        {
            Type = BSPNodeType.Leaf,
            BoundingSphere = new Sphere { Origin = new Vector3(0.5f, 0f, 1f), Radius = 10f },
        };
        leaf.Polygons.Add(SlopedUnwalkable_FloorId);
        leaf.Polygons.Add(SlopedUnwalkable_SlopeId);

        return (leaf, resolved);
    }

    // =========================================================================
    // Transition builder helpers
    // =========================================================================

    /// <summary>
    /// Build a <see cref="Transition"/> for a grounded mover (Contact + OnWalkable set).
    ///
    /// <para>
    /// The mover's foot sphere starts at <paramref name="from"/> and is headed
    /// toward <paramref name="to"/>.  <see cref="ObjectInfo.StepUpHeight"/> is
    /// set to <paramref name="stepUpHeight"/> so the test can control which step
    /// heights succeed.
    /// </para>
    /// </summary>
    public static Transition MakeGroundedTransition(
        Vector3 from,
        Vector3 to,
        float   stepUpHeight = 0.30f,
        uint    cellId       = 0xA9B40001u)
    {
        var t = new Transition();
        t.SpherePath.InitPath(from, to, cellId, SphereRadius);
        t.ObjectInfo.State          = ObjectInfoState.Contact | ObjectInfoState.OnWalkable;
        t.ObjectInfo.StepUpHeight   = stepUpHeight;
        t.ObjectInfo.StepDownHeight = 0.04f;
        t.ObjectInfo.StepDown       = true;
        // Seed LastKnownContactPlane so the mover is "on the floor".
        t.CollisionInfo.LastKnownContactPlane      = new Plane(Vector3.UnitZ, 0f);
        t.CollisionInfo.LastKnownContactPlaneValid = true;
        return t;
    }

    /// <summary>
    /// Build a <see cref="Transition"/> for an airborne mover (no Contact, no OnWalkable).
    ///
    /// <para>
    /// Represents a character that has just jumped or fallen and is now moving
    /// downward to land on a surface.
    /// </para>
    /// </summary>
    public static Transition MakeAirborneTransition(
        Vector3 from,
        Vector3 to,
        uint    cellId = 0xA9B40001u)
    {
        var t = new Transition();
        t.SpherePath.InitPath(from, to, cellId, SphereRadius);
        t.ObjectInfo.State          = ObjectInfoState.None;
        t.ObjectInfo.StepUpHeight   = 0.04f;
        t.ObjectInfo.StepDownHeight = 0.04f;
        t.ObjectInfo.StepDown       = false;
        return t;
    }

    // =========================================================================
    // Internal polygon builders
    // =========================================================================

    // Build a horizontal floor polygon (normal = +Z) from four CCW vertices
    // (as viewed from above).
    private static ResolvedPolygon MakeFloor(
        Vector3 v0, Vector3 v1, Vector3 v2, Vector3 v3)
    {
        var verts = new[] { v0, v1, v2, v3 };
        var normal = Vector3.UnitZ;
        float dotSum = 0f;
        foreach (var v in verts) dotSum += Vector3.Dot(normal, v);
        float d = -(dotSum / verts.Length);
        return new ResolvedPolygon
        {
            Vertices  = verts,
            Plane     = new Plane(normal, d),
            NumPoints = 4,
            SidesType = CullMode.None,
        };
    }

    // Build a quad polygon with a specified outward normal.
    // Vertices should be ordered so that the cross-product of two edges aligns
    // with expectedNormal; we explicitly override the computed plane so the test
    // is deterministic regardless of winding order.
    private static ResolvedPolygon MakeQuad(
        Vector3 v0, Vector3 v1, Vector3 v2, Vector3 v3,
        Vector3 expectedNormal)
    {
        var verts = new[] { v0, v1, v2, v3 };
        float dotSum = 0f;
        foreach (var v in verts) dotSum += Vector3.Dot(expectedNormal, v);
        float d = -(dotSum / verts.Length);
        return new ResolvedPolygon
        {
            Vertices  = verts,
            Plane     = new Plane(expectedNormal, d),
            NumPoints = 4,
            SidesType = CullMode.None,
        };
    }
}