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

using System.Collections.Concurrent;
using System.Numerics;
using DatReaderWriter.DBObjs;
using DatReaderWriter.Enums;
using DatReaderWriter.Types;
using Plane = System.Numerics.Plane;

namespace AcDream.Core.Physics;

/// <summary>
/// Thread-safe cache of physics-relevant data extracted from GfxObj and Setup
/// dat objects during streaming. Populated by the streaming worker thread;
/// read by the physics engine on the game/render thread. ConcurrentDictionary
/// makes cross-thread access safe without a global lock.
/// </summary>
public sealed class PhysicsDataCache
{
    private readonly ConcurrentDictionary<uint, GfxObjPhysics> _gfxObj = new();
    private readonly ConcurrentDictionary<uint, GfxObjVisualBounds> _visualBounds = new();
    private readonly ConcurrentDictionary<uint, SetupPhysics> _setup = new();
    private readonly ConcurrentDictionary<uint, CellPhysics> _cellStruct = new();

    /// <summary>
    /// Extract and cache the physics BSP + polygon data from a GfxObj,
    /// PLUS always cache a visual AABB from the vertex data regardless of
    /// the HasPhysics flag. The visual AABB is used as a collision fallback
    /// for entities whose Setup has no retail physics data — it lets the
    /// user collide with decorative meshes that don't have a CylSphere or
    /// per-part BSP.
    /// </summary>
    public void CacheGfxObj(uint gfxObjId, GfxObj gfxObj)
    {
        // Always cache a visual AABB from the mesh vertices — this is cheap
        // and fed by the mesh data that's already loaded. It serves as the
        // fallback collision shape for pure-visual entities.
        if (!_visualBounds.ContainsKey(gfxObjId) && gfxObj.VertexArray != null)
        {
            _visualBounds[gfxObjId] = ComputeVisualBounds(gfxObj.VertexArray);
        }

        if (_gfxObj.ContainsKey(gfxObjId)) return;
        if (!gfxObj.Flags.HasFlag(GfxObjFlags.HasPhysics)) return;
        if (gfxObj.PhysicsBSP?.Root is null) return;
        if (gfxObj.VertexArray is null) return;

        _gfxObj[gfxObjId] = new GfxObjPhysics
        {
            BSP = gfxObj.PhysicsBSP,
            PhysicsPolygons = gfxObj.PhysicsPolygons,
            BoundingSphere = gfxObj.PhysicsBSP.Root.BoundingSphere,
            Vertices = gfxObj.VertexArray,
            Resolved = ResolvePolygons(gfxObj.PhysicsPolygons, gfxObj.VertexArray),
        };
    }

    /// <summary>
    /// Get the cached visual AABB for a GfxObj, or null if not cached.
    /// </summary>
    public GfxObjVisualBounds? GetVisualBounds(uint gfxObjId) =>
        _visualBounds.TryGetValue(gfxObjId, out var vb) ? vb : null;

    /// <summary>
    /// Compute a tight axis-aligned bounding box over all vertices in the mesh.
    /// Used as a fallback collision shape for entities whose Setup has no
    /// physics data — we approximate collision using the visual extent.
    /// </summary>
    private static GfxObjVisualBounds ComputeVisualBounds(VertexArray vertexArray)
    {
        if (vertexArray.Vertices == null || vertexArray.Vertices.Count == 0)
        {
            return new GfxObjVisualBounds
            {
                Min = Vector3.Zero,
                Max = Vector3.Zero,
                Center = Vector3.Zero,
                Radius = 0f,
                HalfExtents = Vector3.Zero,
            };
        }

        var min = new Vector3(float.MaxValue);
        var max = new Vector3(float.MinValue);
        foreach (var kv in vertexArray.Vertices)
        {
            var p = kv.Value.Origin;
            if (p.X < min.X) min.X = p.X;
            if (p.Y < min.Y) min.Y = p.Y;
            if (p.Z < min.Z) min.Z = p.Z;
            if (p.X > max.X) max.X = p.X;
            if (p.Y > max.Y) max.Y = p.Y;
            if (p.Z > max.Z) max.Z = p.Z;
        }

        var center = (min + max) * 0.5f;
        var halfExt = (max - min) * 0.5f;
        float radius = halfExt.Length();

        return new GfxObjVisualBounds
        {
            Min = min,
            Max = max,
            Center = center,
            Radius = radius,
            HalfExtents = halfExt,
        };
    }

    /// <summary>
    /// Extract and cache the collision shape data from a Setup.
    /// No-ops if the id is already cached.
    /// </summary>
    public void CacheSetup(uint setupId, Setup setup)
    {
        if (_setup.ContainsKey(setupId)) return;
        _setup[setupId] = new SetupPhysics
        {
            CylSpheres = setup.CylSpheres ?? new(),
            Spheres = setup.Spheres ?? new(),
            Height = setup.Height,
            Radius = setup.Radius,
            StepUpHeight = setup.StepUpHeight,
            StepDownHeight = setup.StepDownHeight,
        };
    }

    /// <summary>
    /// Extract and cache the physics BSP + polygon data from a CellStruct
    /// (indoor room geometry). No-ops if the id is already cached or the
    /// CellStruct has no physics BSP.
    /// </summary>
    public void CacheCellStruct(uint envCellId, CellStruct cellStruct,
                                 Matrix4x4 worldTransform)
    {
        if (_cellStruct.ContainsKey(envCellId)) return;
        if (cellStruct.PhysicsBSP?.Root is null) return;

        Matrix4x4.Invert(worldTransform, out var inverseTransform);

        var resolved = ResolvePolygons(cellStruct.PhysicsPolygons, cellStruct.VertexArray);

        // Indoor walking Phase D (2026-05-19): compute a tight local AABB from
        // the resolved polygon vertices. Computed once at cache time so the
        // per-frame TryFindContainingCell check only does AABB point tests.
        var aabbMin = new Vector3(float.MaxValue);
        var aabbMax = new Vector3(float.MinValue);
        foreach (var (_, poly) in resolved)
        {
            if (poly.Vertices is null) continue;
            foreach (var v in poly.Vertices)
            {
                if (v.X < aabbMin.X) aabbMin.X = v.X;
                if (v.Y < aabbMin.Y) aabbMin.Y = v.Y;
                if (v.Z < aabbMin.Z) aabbMin.Z = v.Z;
                if (v.X > aabbMax.X) aabbMax.X = v.X;
                if (v.Y > aabbMax.Y) aabbMax.Y = v.Y;
                if (v.Z > aabbMax.Z) aabbMax.Z = v.Z;
            }
        }

        _cellStruct[envCellId] = new CellPhysics
        {
            BSP = cellStruct.PhysicsBSP,
            PhysicsPolygons = cellStruct.PhysicsPolygons,
            Vertices = cellStruct.VertexArray,
            WorldTransform = worldTransform,
            InverseWorldTransform = inverseTransform,
            Resolved = resolved,
            LocalAabbMin = aabbMin,
            LocalAabbMax = aabbMax,
        };

        if (PhysicsDiagnostics.ProbeCellCacheEnabled)
        {
            var root = cellStruct.PhysicsBSP?.Root;
            int bspRootPolyCount   = root?.Polygons?.Count ?? 0;
            bool bspRootHasChildren = root?.PosNode is not null || root?.NegNode is not null;
            Console.WriteLine(System.FormattableString.Invariant(
                $"[cell-cache] envCellId=0x{envCellId:X8} physicsPolyCount={cellStruct.PhysicsPolygons?.Count ?? 0} resolvedCount={resolved.Count} bspRootPolyCount={bspRootPolyCount} bspRootHasChildren={bspRootHasChildren}"));
        }
    }

    /// <summary>
    /// Pre-resolve all physics polygons: lookup vertex positions from VertexArray
    /// and compute the face plane. Matches ACE's Polygon constructor which calls
    /// make_plane() and resolves Vertices from VertexIDs at load time.
    /// </summary>
    private static Dictionary<ushort, ResolvedPolygon> ResolvePolygons(
        Dictionary<ushort, DatReaderWriter.Types.Polygon> polys,
        VertexArray vertexArray)
    {
        var resolved = new Dictionary<ushort, ResolvedPolygon>(polys.Count);
        foreach (var (id, poly) in polys)
        {
            int numVerts = poly.VertexIds.Count;
            if (numVerts < 3) continue;

            var verts = new Vector3[numVerts];
            bool valid = true;
            for (int i = 0; i < numVerts; i++)
            {
                ushort vid = (ushort)poly.VertexIds[i];
                if (!vertexArray.Vertices.TryGetValue(vid, out var sv))
                { valid = false; break; }
                verts[i] = sv.Origin;
            }
            if (!valid) continue;

            // Compute plane normal using ACE's make_plane algorithm:
            // fan cross-product accumulation + normalization.
            var normal = Vector3.Zero;
            for (int i = 1; i < numVerts - 1; i++)
            {
                var v1 = verts[i] - verts[0];
                var v2 = verts[i + 1] - verts[0];
                normal += Vector3.Cross(v1, v2);
            }
            float len = normal.Length();
            if (len < 1e-8f) continue;
            normal /= len;

            // D = -(average dot(normal, vertex))
            float dotSum = 0f;
            for (int i = 0; i < numVerts; i++)
                dotSum += Vector3.Dot(normal, verts[i]);
            float d = -(dotSum / numVerts);

            resolved[id] = new ResolvedPolygon
            {
                Vertices = verts,
                Plane = new Plane(normal, d),
                NumPoints = numVerts,
                SidesType = poly.SidesType,
            };
        }
        return resolved;
    }

    public GfxObjPhysics? GetGfxObj(uint id) => _gfxObj.TryGetValue(id, out var p) ? p : null;
    public SetupPhysics? GetSetup(uint id) => _setup.TryGetValue(id, out var p) ? p : null;
    public CellPhysics? GetCellStruct(uint id) => _cellStruct.TryGetValue(id, out var p) ? p : null;
    public int GfxObjCount => _gfxObj.Count;
    public int SetupCount => _setup.Count;
    public int CellStructCount => _cellStruct.Count;

    /// <summary>
    /// Indoor walking Phase 1 (2026-05-19). Snapshot of currently-cached
    /// EnvCell ids — used by <see cref="AcDream.Core.Selection.WorldPicker"/>
    /// to enumerate occluder candidates without exposing the underlying
    /// dictionary. Returns the live key-set; callers should snapshot the
    /// collection if they need stability across frames.
    /// </summary>
    public IReadOnlyCollection<uint> CellStructIds => (IReadOnlyCollection<uint>)_cellStruct.Keys;

    /// <summary>
    /// Indoor walking Phase D (2026-05-19). Returns the full id of the first
    /// cached EnvCell whose local AABB contains <paramref name="worldPos"/>,
    /// or false if no cached EnvCell contains it. Used by
    /// <see cref="PhysicsEngine.ResolveOutdoorCellId"/> to promote the player's
    /// CellId to an indoor EnvCell when the player is geometrically inside one.
    ///
    /// <para>
    /// AABBs are pre-computed in <see cref="CacheCellStruct"/> from each
    /// cell's resolved polygon vertices, transformed into local space via
    /// <see cref="CellPhysics.InverseWorldTransform"/>. Iteration is O(N) over
    /// cached cells; N is bounded by the streaming radius (~80 cells at
    /// radius 4).
    /// </para>
    ///
    /// <para>
    /// Local AABB is a tight bound around the cell's geometry. EnvCells in
    /// Holtburg are roughly room-sized cuboids; the local AABB is therefore
    /// a reasonable proxy for "is the player in this cell." For cells with
    /// concave shapes or non-room geometry, the AABB will over-approximate;
    /// this only matters if two cells' AABBs overlap and the player is in
    /// the overlap region (rare in practice; if it becomes an issue, switch
    /// to a BSP point-in-cell test).
    /// </para>
    /// </summary>
    public bool TryFindContainingCell(Vector3 worldPos, out uint envCellId)
    {
        foreach (var (id, cp) in _cellStruct)
        {
            // Guard: if the AABB was never populated (no vertices in the cell),
            // LocalAabbMin stays at float.MaxValue — the containment test will
            // always fail, so we skip the cell silently.
            if (cp.LocalAabbMin.X == float.MaxValue) continue;

            var local = Vector3.Transform(worldPos, cp.InverseWorldTransform);
            if (local.X >= cp.LocalAabbMin.X && local.X <= cp.LocalAabbMax.X &&
                local.Y >= cp.LocalAabbMin.Y && local.Y <= cp.LocalAabbMax.Y &&
                local.Z >= cp.LocalAabbMin.Z && local.Z <= cp.LocalAabbMax.Z)
            {
                envCellId = id;
                return true;
            }
        }
        envCellId = 0;
        return false;
    }

    /// <summary>
    /// Register a pre-built <see cref="GfxObjPhysics"/> directly.
    /// Intended for unit-test fixtures that construct synthetic BSP trees
    /// without needing real DAT content.
    /// </summary>
    public void RegisterGfxObjForTest(uint gfxObjId, GfxObjPhysics physics)
        => _gfxObj[gfxObjId] = physics;

    /// <summary>
    /// Register a pre-built <see cref="CellPhysics"/> directly. Intended for
    /// unit-test fixtures that construct synthetic cells without going through
    /// dat-driven <see cref="CacheCellStruct"/>.
    /// </summary>
    public void RegisterCellStructForTest(uint envCellId, CellPhysics physics)
        => _cellStruct[envCellId] = physics;
}

/// <summary>
/// Visual AABB of a GfxObj mesh — populated for every cached GfxObj regardless
/// of whether it has physics data. Used as a collision fallback shape for
/// entities whose Setup has no CylSpheres/Spheres/Radius (pure decorative
/// meshes). Provides an approximate cylinder matching the visible mesh extent.
/// </summary>
public sealed class GfxObjVisualBounds
{
    /// <summary>Local-space minimum corner of the mesh AABB.</summary>
    public required Vector3 Min { get; init; }
    /// <summary>Local-space maximum corner of the mesh AABB.</summary>
    public required Vector3 Max { get; init; }
    /// <summary>Center of the local-space AABB.</summary>
    public required Vector3 Center { get; init; }
    /// <summary>Local-space radius (diagonal half-length) — loose bound.</summary>
    public required float Radius { get; init; }
    /// <summary>Local-space half-extents ((Max - Min) * 0.5).</summary>
    public required Vector3 HalfExtents { get; init; }
}

/// <summary>
/// A physics polygon with pre-resolved vertex positions and pre-computed plane.
/// ACE pre-computes these in its Polygon constructor; we do it at cache time
/// to avoid per-collision-test vertex lookups.
/// </summary>
public sealed class ResolvedPolygon
{
    public required Vector3[] Vertices { get; init; }
    public required Plane Plane { get; init; }
    public required int NumPoints { get; init; }
    public required CullMode SidesType { get; init; }
}

/// <summary>Cached physics data for a single GfxObj part.</summary>
public sealed class GfxObjPhysics
{
    public required PhysicsBSPTree BSP { get; init; }
    public required Dictionary<ushort, Polygon> PhysicsPolygons { get; init; }
    public Sphere? BoundingSphere { get; init; }
    public required VertexArray Vertices { get; init; }

    /// <summary>
    /// Pre-resolved polygon data with vertex positions and computed planes.
    /// Populated once at cache time so BSP queries don't pay per-test lookup cost.
    /// </summary>
    public required Dictionary<ushort, ResolvedPolygon> Resolved { get; init; }
}

/// <summary>Cached collision shape data for a Setup (character/creature capsule).</summary>
public sealed class SetupPhysics
{
    public List<CylSphere> CylSpheres { get; init; } = new();
    public List<Sphere> Spheres { get; init; } = new();
    public float Height { get; init; }
    public float Radius { get; init; }
    public float StepUpHeight { get; init; }
    public float StepDownHeight { get; init; }
}

/// <summary>
/// Cached physics data for an indoor cell's room geometry (CellStruct).
/// Used for wall/floor/ceiling collision in EnvCells.
/// ACE: EnvCell.find_env_collisions queries CellStructure.PhysicsBSP.
/// </summary>
public sealed class CellPhysics
{
    /// <summary>
    /// The physics BSP tree for this cell. Nullable so that test fixtures
    /// can construct a <see cref="CellPhysics"/> from <see cref="Resolved"/>
    /// alone without needing a real DAT BSP object. Production code must
    /// null-check before traversal: <c>cell.BSP?.Root is not null</c>.
    /// </summary>
    public PhysicsBSPTree? BSP { get; init; }
    public Dictionary<ushort, Polygon>? PhysicsPolygons { get; init; }
    public VertexArray? Vertices { get; init; }
    public Matrix4x4 WorldTransform { get; init; }
    public Matrix4x4 InverseWorldTransform { get; init; }

    /// <summary>
    /// Pre-resolved polygon data with vertex positions and computed planes.
    /// </summary>
    public required Dictionary<ushort, ResolvedPolygon> Resolved { get; init; }

    /// <summary>
    /// Indoor walking Phase D (2026-05-19). Local-space AABB minimum corner,
    /// computed from the resolved polygon vertices at <see cref="PhysicsDataCache.CacheCellStruct"/>
    /// time. Initialized to <c>float.MaxValue</c> so that
    /// <see cref="PhysicsDataCache.TryFindContainingCell"/> silently skips
    /// cells with no vertex data.
    /// </summary>
    public Vector3 LocalAabbMin { get; init; } = new Vector3(float.MaxValue);

    /// <summary>
    /// Indoor walking Phase D (2026-05-19). Local-space AABB maximum corner,
    /// computed from the resolved polygon vertices at <see cref="PhysicsDataCache.CacheCellStruct"/>
    /// time. Initialized to <c>float.MinValue</c> so that
    /// <see cref="PhysicsDataCache.TryFindContainingCell"/> silently skips
    /// cells with no vertex data.
    /// </summary>
    public Vector3 LocalAabbMax { get; init; } = new Vector3(float.MinValue);
}