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

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

namespace AcDream.Core.Physics;

/// <summary>
/// Top-level physics resolver that combines <see cref="TerrainSurface"/> and
/// <see cref="CellSurface"/> to resolve entity movement with step-height
/// enforcement and outdoor/indoor cell transitions.
///
/// <para>
/// Landblocks are registered via <see cref="AddLandblock"/> with their
/// terrain, indoor cells, and world-space offsets. <see cref="Resolve"/>
/// takes a current position, the entity's current cell ID, a movement delta,
/// and a step-up height limit; it returns the validated new position, the
/// updated cell ID, and whether the entity is standing on a surface.
/// </para>
/// </summary>
public sealed class PhysicsEngine
{
    private readonly Dictionary<uint, LandblockPhysics> _landblocks = new();

    /// <summary>Number of registered landblocks (diagnostic).</summary>
    public int LandblockCount => _landblocks.Count;

    private sealed record LandblockPhysics(
        TerrainSurface Terrain,
        IReadOnlyList<CellSurface> Cells,
        float WorldOffsetX,
        float WorldOffsetY);

    /// <summary>
    /// Register a landblock with its terrain surface, indoor cells, and
    /// world-space origin offset.
    /// </summary>
    public void AddLandblock(uint landblockId, TerrainSurface terrain,
        IReadOnlyList<CellSurface> cells, float worldOffsetX, float worldOffsetY)
    {
        _landblocks[landblockId] = new LandblockPhysics(terrain, cells, worldOffsetX, worldOffsetY);
    }

    /// <summary>
    /// Remove a previously registered landblock.
    /// </summary>
    public void RemoveLandblock(uint landblockId) => _landblocks.Remove(landblockId);

    /// <summary>
    /// Resolve an entity's movement from <paramref name="currentPos"/> by
    /// applying <paramref name="delta"/> (XY only) and computing the correct Z
    /// from the terrain or indoor cell floor beneath the candidate position.
    ///
    /// <para>
    /// Step-height enforcement rejects horizontal movement when the upward Z
    /// change exceeds <paramref name="stepUpHeight"/>. Downhill movement is
    /// always accepted. Returns <see cref="ResolveResult.IsOnGround"/> false
    /// when no loaded landblock covers the candidate position.
    /// </para>
    /// </summary>
    public ResolveResult Resolve(Vector3 currentPos, uint cellId, Vector3 delta, float stepUpHeight)
    {
        var candidatePos = currentPos + new Vector3(delta.X, delta.Y, 0f);

        // Find the landblock this candidate position falls in.
        LandblockPhysics? physics = null;
        foreach (var kvp in _landblocks)
        {
            var lb = kvp.Value;
            float localX = candidatePos.X - lb.WorldOffsetX;
            float localY = candidatePos.Y - lb.WorldOffsetY;
            if (localX >= 0 && localX < 192f && localY >= 0 && localY < 192f)
            {
                physics = lb;
                break;
            }
        }

        if (physics is null)
            return new ResolveResult(candidatePos, cellId, IsOnGround: false);

        float localCandX = candidatePos.X - physics.WorldOffsetX;
        float localCandY = candidatePos.Y - physics.WorldOffsetY;

        // Check if the candidate position falls on any indoor cell floor.
        // Pick the cell whose floor Z is closest to the entity's current Z.
        CellSurface? bestCell = null;
        float? bestCellZ = null;
        float bestZDist = float.MaxValue;

        foreach (var cell in physics.Cells)
        {
            float? floorZ = cell.SampleFloorZ(candidatePos.X, candidatePos.Y);
            if (floorZ is not null)
            {
                float dist = MathF.Abs(floorZ.Value - currentPos.Z);
                if (dist < bestZDist)
                {
                    bestCell = cell;
                    bestCellZ = floorZ;
                    bestZDist = dist;
                }
            }
        }

        // Determine target surface Z and cell.
        float terrainZ = physics.Terrain.SampleZ(localCandX, localCandY);
        float targetZ;
        uint targetCellId;

        // Only the low 16 bits of cellId carry the cell index. Outdoor
        // cells are 0x0001–0x0040; indoor (EnvCell) cells are 0x0100+.
        // The full 32-bit cellId includes the landblock prefix in the
        // high 16 bits (e.g., 0xA9B40001), so we MUST mask before
        // comparing. Without the mask, every cell looks "indoor" because
        // 0xA9B40001 >= 0x0100 → the engine always takes the "stay
        // indoors" path and snaps Z to an EnvCell floor 28m below.
        bool currentlyIndoor = (cellId & 0xFFFFu) >= 0x0100;

        if (currentlyIndoor && bestCellZ is not null)
        {
            // Stay indoors on the best cell's floor.
            targetZ = bestCellZ.Value;
            targetCellId = bestCell!.CellId & 0xFFFFu;
        }
        else if (currentlyIndoor && bestCellZ is null)
        {
            // Walked out of the current cell — transition to outdoor.
            targetZ = terrainZ;
            targetCellId = physics.Terrain.ComputeOutdoorCellId(localCandX, localCandY);
        }
        else if (!currentlyIndoor && bestCellZ is not null
                 && MathF.Abs(bestCellZ.Value - currentPos.Z) < stepUpHeight + 2f
                 && bestCellZ.Value < terrainZ - 1f)
        {
            // Walked into an indoor cell from outdoor — transition to indoor.
            // The extra guard `bestCellZ < terrainZ - 1` prevents transitioning
            // into cells whose floor is AT or ABOVE terrain level — those are
            // typically roofs, upper floors, or building footprints that overlap
            // the outdoor terrain. A genuine indoor transition is into a cell
            // whose floor is BELOW the terrain surface (basements, ground floors
            // of buildings that sit on elevated terrain). Without this guard,
            // standing near any building with a floor polygon covering the
            // player's XY triggers an indoor transition and snaps Z to the
            // cell's floor — which for multi-story buildings can be 30+ units
            // below the outdoor terrain.
            targetZ = bestCellZ.Value;
            targetCellId = bestCell!.CellId & 0xFFFFu;
        }
        else
        {
            // Stay outdoors on terrain.
            targetZ = terrainZ;
            targetCellId = physics.Terrain.ComputeOutdoorCellId(localCandX, localCandY);
        }

        // Step-height enforcement: block upward movement that exceeds the limit.
        float zDelta = targetZ - currentPos.Z;
        if (zDelta > stepUpHeight)
        {
            // Too steep to step up — reject horizontal movement.
            return new ResolveResult(currentPos, cellId, IsOnGround: true);
        }

        return new ResolveResult(
            new Vector3(candidatePos.X, candidatePos.Y, targetZ),
            targetCellId,
            IsOnGround: true);
    }
}