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

using System;
using System.Collections.Generic;
using System.Numerics;

namespace AcDream.Core.Physics;

/// <summary>
/// Indoor floor resolver for a single EnvCell. Projects an XY point
/// onto the cell's floor polygons and returns the Z at that point.
///
/// <para>
/// Uses a simplified constructor that takes pre-transformed vertex
/// positions (world-space) and polygon vertex-id lists. The caller
/// is responsible for transforming CellStruct vertices from cell-local
/// space to world space using EnvCell.Position before constructing
/// this surface.
/// </para>
///
/// <para>
/// Floor polygon iteration is brute-force (no BSP). Cell polygon
/// counts are typically &lt; 20, making this acceptable for the MVP.
/// Each polygon is fan-triangulated and tested via point-in-triangle
/// + barycentric Z interpolation.
/// </para>
/// </summary>
public sealed class CellSurface
{
    public uint CellId { get; }

    private readonly List<(Vector3 A, Vector3 B, Vector3 C)> _triangles;

    /// <summary>
    /// Construct a CellSurface from pre-transformed vertex positions
    /// and polygon definitions.
    /// </summary>
    /// <param name="cellId">The EnvCell dat id (e.g., 0xA9B40100).</param>
    /// <param name="vertices">Vertex id → world-space position map.</param>
    /// <param name="polygonVertexIds">
    /// List of polygons, each a list of vertex IDs. Polygons with fewer
    /// than 3 vertices are skipped. Quads and larger are fan-triangulated.
    /// </param>
    public CellSurface(
        uint cellId,
        Dictionary<ushort, Vector3> vertices,
        List<List<short>> polygonVertexIds)
    {
        CellId = cellId;
        _triangles = new List<(Vector3, Vector3, Vector3)>();

        foreach (var polyVerts in polygonVertexIds)
        {
            if (polyVerts.Count < 3) continue;

            // Resolve vertex positions.
            var positions = new List<Vector3>(polyVerts.Count);
            bool skip = false;
            foreach (var vid in polyVerts)
            {
                if (!vertices.TryGetValue((ushort)vid, out var pos))
                {
                    skip = true;
                    break;
                }
                positions.Add(pos);
            }
            if (skip) continue;

            // Fan triangulation: (v0, v1, v2), (v0, v2, v3), ...
            for (int i = 1; i < positions.Count - 1; i++)
            {
                _triangles.Add((positions[0], positions[i], positions[i + 1]));
            }
        }
    }

    /// <summary>
    /// Project (worldX, worldY) onto this cell's floor polygons and
    /// return the Z. Returns null if outside all floor polygons.
    /// </summary>
    public float? SampleFloorZ(float worldX, float worldY)
    {
        foreach (var (a, b, c) in _triangles)
        {
            if (PointInTriangleXY(worldX, worldY, a, b, c, out float z))
                return z;
        }
        return null;
    }

    /// <summary>
    /// Test if (px, py) falls inside triangle (a, b, c) projected onto
    /// the XY plane. If inside, computes the barycentric Z interpolation
    /// and returns it via <paramref name="z"/>.
    /// </summary>
    private static bool PointInTriangleXY(
        float px, float py,
        Vector3 a, Vector3 b, Vector3 c,
        out float z)
    {
        z = 0;

        // Barycentric coordinate computation in 2D (XY plane).
        float v0x = c.X - a.X, v0y = c.Y - a.Y;
        float v1x = b.X - a.X, v1y = b.Y - a.Y;
        float v2x = px - a.X, v2y = py - a.Y;

        float dot00 = v0x * v0x + v0y * v0y;
        float dot01 = v0x * v1x + v0y * v1y;
        float dot02 = v0x * v2x + v0y * v2y;
        float dot11 = v1x * v1x + v1y * v1y;
        float dot12 = v1x * v2x + v1y * v2y;

        float denom = dot00 * dot11 - dot01 * dot01;
        if (MathF.Abs(denom) < 1e-10f) return false;  // degenerate triangle

        float invDenom = 1f / denom;
        float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
        float v = (dot00 * dot12 - dot01 * dot02) * invDenom;

        if (u < -1e-6f || v < -1e-6f || u + v > 1f + 1e-6f)
            return false;

        // Barycentric Z interpolation.
        z = a.Z * (1 - u - v) + b.Z * v + c.Z * u;
        return true;
    }
}