mrgameeng/src/MrGameEng.Collisions/CollisionWorld.cs

using Friflo.Engine.ECS;
using Microsoft.Xna.Framework;
using MrGameEng.Graphics;

namespace MrGameEng.Collisions;

/// <summary>A pair of entities whose colliders overlap this frame.</summary>
public readonly record struct CollisionPair(Entity A, Entity B);

/// <summary>Result of a <see cref="CollisionWorld.Raycast"/>.</summary>
public readonly record struct RaycastHit(Entity Entity, Vector2 Point, float Fraction);

/// <summary>
/// Broad + narrow phase collision detection over a uniform spatial hash, rebuilt from
/// scratch every tick by <see cref="CollisionSystem"/> — O(n) for moving entities, flat
/// arrays only, no allocations after warm-up. Overlapping pairs are collected into
/// <see cref="Pairs"/> (deterministic order); ad-hoc area/ray queries are available to
/// game systems at any point after the rebuild.
/// </summary>
public sealed class CollisionWorld
{
    private struct Entry
    {
        public Entity Entity;
        public Vector2 Center;
        public RectF Aabb;
        public float Radius;
        public Vector2 HalfExtents;
        public uint Layer;
        public uint CollidesWith;
        public ColliderShape Shape;
    }

    private readonly float _cellSize;
    private Entry[] _entries = new Entry[256];
    private int _count;

    // Spatial hash: головы бакетов + связные списки вставок (по индексу записи на ячейку).
    private int[] _bucketHeads = new int[512];
    private int[] _cellNext = new int[1024];
    private int[] _cellEntry = new int[1024];
    private int _cellCount;

    private int[] _testedStamp = new int[256];
    private CollisionPair[] _pairs = new CollisionPair[256];
    private int _pairCount;

    /// <summary>Creates a world. <paramref name="cellSize"/> should match typical collider size.</summary>
    public CollisionWorld(float cellSize = 64f)
    {
        if (cellSize <= 0f)
        {
            throw new ArgumentOutOfRangeException(nameof(cellSize));
        }

        _cellSize = cellSize;
        Array.Fill(_bucketHeads, -1); // пустой хэш до первого ребилда: обход бакета сразу завершается
    }

    /// <summary>
    /// Pairs found by the last rebuild. Order is deterministic for identical registration
    /// history; it may differ between sessions whose peak collider count differed
    /// (the hash table only grows and its size affects bucket iteration order).
    /// </summary>
    public ReadOnlySpan<CollisionPair> Pairs => _pairs.AsSpan(0, _pairCount);

    /// <summary>Colliders registered in the last rebuild.</summary>
    public int Count => _count;

    /// <summary>Starts a rebuild. Called by <see cref="CollisionSystem"/> once per tick.</summary>
    public void BeginRebuild()
    {
        _count = 0;
        _cellCount = 0;
        _pairCount = 0;
    }

    /// <summary>Registers one collider. Order of registration defines pair order (keep it deterministic).</summary>
    public void Add(Entity entity, in Transform2D transform, in Collider collider)
    {
        if (_count == _entries.Length)
        {
            Array.Resize(ref _entries, _entries.Length * 2);
            Array.Resize(ref _testedStamp, _entries.Length);
        }

        var center = transform.Position + collider.Offset;
        var half = collider.Shape == ColliderShape.Circle
            ? new Vector2(collider.Radius)
            : collider.HalfExtents;

        _entries[_count++] = new Entry
        {
            Entity = entity,
            Center = center,
            Aabb = new RectF(center.X - half.X, center.Y - half.Y, half.X * 2f, half.Y * 2f),
            Radius = collider.Radius,
            HalfExtents = collider.HalfExtents,
            Layer = collider.Layer,
            CollidesWith = collider.CollidesWith,
            Shape = collider.Shape,
        };
    }

    /// <summary>Builds the hash and collects all overlapping pairs.</summary>
    public void EndRebuild()
    {
        BuildHash();
        CollectPairs();
    }

    /// <summary>
    /// Writes entities whose colliders overlap <paramref name="area"/> into
    /// <paramref name="results"/>; returns the count (truncated to the span length).
    /// </summary>
    public int QueryAabb(in RectF area, Span<Entity> results)
    {
        var found = 0;
        var minX = CellOf(area.Left);
        var maxX = CellOf(area.Right);
        var minY = CellOf(area.Top);
        var maxY = CellOf(area.Bottom);
        var stamp = -1; // запросы используют отрицательные штампы, чтобы не портить пары

        for (var cy = minY; cy <= maxY; cy++)
        {
            for (var cx = minX; cx <= maxX; cx++)
            {
                for (var i = _bucketHeads[Bucket(cx, cy)]; i >= 0; i = _cellNext[i])
                {
                    var entryIndex = _cellEntry[i];
                    if (_testedStamp[entryIndex] == stamp)
                    {
                        continue;
                    }

                    _testedStamp[entryIndex] = stamp;
                    if (_entries[entryIndex].Aabb.Intersects(area))
                    {
                        if (found == results.Length)
                        {
                            ResetQueryStamps(); // иначе следующий запрос пропустит помеченные записи
                            return found;
                        }

                        results[found++] = _entries[entryIndex].Entity;
                    }
                }
            }
        }

        ResetQueryStamps();
        return found;
    }

    /// <summary>
    /// Casts a segment and returns the closest hit among colliders whose
    /// <see cref="Collider.Layer"/> intersects <paramref name="mask"/>.
    /// A ray starting inside a collider hits it at fraction 0. Linear scan over all
    /// registered colliders — fine for occasional rays, not for thousands per tick.
    /// </summary>
    public bool Raycast(Vector2 from, Vector2 to, out RaycastHit hit, uint mask = uint.MaxValue)
    {
        hit = default;
        var bestFraction = float.MaxValue;

        for (var i = 0; i < _count; i++)
        {
            ref readonly var entry = ref _entries[i];
            if ((entry.Layer & mask) == 0)
            {
                continue;
            }

            float fraction;
            var found = entry.Shape == ColliderShape.Circle
                ? RaySegmentCircle(from, to, entry.Center, entry.Radius, out fraction)
                : RaySegmentAabb(from, to, entry.Aabb, out fraction);

            if (found && fraction < bestFraction)
            {
                bestFraction = fraction;
                hit = new RaycastHit(entry.Entity, Vector2.Lerp(from, to, fraction), fraction);
            }
        }

        return bestFraction <= 1f;
    }

    private void BuildHash()
    {
        var buckets = _bucketHeads.Length;
        while (buckets < _count * 2)
        {
            buckets *= 2;
        }

        if (buckets != _bucketHeads.Length)
        {
            _bucketHeads = new int[buckets];
        }

        Array.Fill(_bucketHeads, -1);

        for (var i = 0; i < _count; i++)
        {
            ref readonly var aabb = ref _entries[i].Aabb;
            var minX = CellOf(aabb.Left);
            var maxX = CellOf(aabb.Right);
            var minY = CellOf(aabb.Top);
            var maxY = CellOf(aabb.Bottom);
            for (var cy = minY; cy <= maxY; cy++)
            {
                for (var cx = minX; cx <= maxX; cx++)
                {
                    if (_cellCount == _cellNext.Length)
                    {
                        Array.Resize(ref _cellNext, _cellNext.Length * 2);
                        Array.Resize(ref _cellEntry, _cellEntry.Length * 2);
                    }

                    var bucket = Bucket(cx, cy);
                    _cellEntry[_cellCount] = i;
                    _cellNext[_cellCount] = _bucketHeads[bucket];
                    _bucketHeads[bucket] = _cellCount;
                    _cellCount++;
                }
            }
        }

        for (var i = 0; i < _count; i++)
        {
            _testedStamp[i] = int.MinValue;
        }
    }

    private void CollectPairs()
    {
        for (var i = 0; i < _count; i++)
        {
            ref readonly var a = ref _entries[i];
            var minX = CellOf(a.Aabb.Left);
            var maxX = CellOf(a.Aabb.Right);
            var minY = CellOf(a.Aabb.Top);
            var maxY = CellOf(a.Aabb.Bottom);

            for (var cy = minY; cy <= maxY; cy++)
            {
                for (var cx = minX; cx <= maxX; cx++)
                {
                    for (var c = _bucketHeads[Bucket(cx, cy)]; c >= 0; c = _cellNext[c])
                    {
                        var j = _cellEntry[c];
                        if (j <= i || _testedStamp[j] == i)
                        {
                            continue; // только пары (i, j>i), каждая один раз
                        }

                        _testedStamp[j] = i;
                        ref readonly var b = ref _entries[j];
                        if ((a.Layer & b.CollidesWith) == 0 || (b.Layer & a.CollidesWith) == 0)
                        {
                            continue;
                        }

                        if (Overlaps(in a, in b))
                        {
                            if (_pairCount == _pairs.Length)
                            {
                                Array.Resize(ref _pairs, _pairs.Length * 2);
                            }

                            _pairs[_pairCount++] = new CollisionPair(a.Entity, b.Entity);
                        }
                    }
                }
            }
        }
    }

    private static bool Overlaps(in Entry a, in Entry b)
    {
        if (a.Shape == ColliderShape.Circle && b.Shape == ColliderShape.Circle)
        {
            var sum = a.Radius + b.Radius;
            return Vector2.DistanceSquared(a.Center, b.Center) <= sum * sum;
        }

        if (a.Shape == ColliderShape.Box && b.Shape == ColliderShape.Box)
        {
            // Включительно (касание = пара) — единообразно с кругами; RectF.Intersects строгий.
            return a.Aabb.Left <= b.Aabb.Right && b.Aabb.Left <= a.Aabb.Right &&
                   a.Aabb.Top <= b.Aabb.Bottom && b.Aabb.Top <= a.Aabb.Bottom;
        }

        // circle vs box
        ref readonly var circle = ref a.Shape == ColliderShape.Circle ? ref a : ref b;
        ref readonly var box = ref a.Shape == ColliderShape.Circle ? ref b : ref a;
        var nearest = new Vector2(
            Math.Clamp(circle.Center.X, box.Aabb.Left, box.Aabb.Right),
            Math.Clamp(circle.Center.Y, box.Aabb.Top, box.Aabb.Bottom));
        return Vector2.DistanceSquared(circle.Center, nearest) <= circle.Radius * circle.Radius;
    }

    private static bool RaySegmentCircle(Vector2 from, Vector2 to, Vector2 center, float radius, out float fraction)
    {
        fraction = 0f;
        var d = to - from;
        var f = from - center;
        var a = Vector2.Dot(d, d);
        if (a <= float.Epsilon)
        {
            return f.LengthSquared() <= radius * radius;
        }

        var b = 2f * Vector2.Dot(f, d);
        var c = Vector2.Dot(f, f) - radius * radius;
        if (c <= 0f)
        {
            return true; // старт внутри круга — попадание в точке старта (как и у AABB)
        }

        var discriminant = b * b - 4f * a * c;
        if (discriminant < 0f)
        {
            return false;
        }

        var sqrt = MathF.Sqrt(discriminant);
        var t = (-b - sqrt) / (2f * a);
        if (t < 0f || t > 1f)
        {
            return false;
        }

        fraction = t;
        return true;
    }

    private static bool RaySegmentAabb(Vector2 from, Vector2 to, in RectF aabb, out float fraction)
    {
        fraction = 0f;
        var d = to - from;
        var tMin = 0f;
        var tMax = 1f;

        for (var axis = 0; axis < 2; axis++)
        {
            var origin = axis == 0 ? from.X : from.Y;
            var direction = axis == 0 ? d.X : d.Y;
            var min = axis == 0 ? aabb.Left : aabb.Top;
            var max = axis == 0 ? aabb.Right : aabb.Bottom;

            if (Math.Abs(direction) < float.Epsilon)
            {
                if (origin < min || origin > max)
                {
                    return false;
                }
            }
            else
            {
                var t1 = (min - origin) / direction;
                var t2 = (max - origin) / direction;
                if (t1 > t2)
                {
                    (t1, t2) = (t2, t1);
                }

                tMin = Math.Max(tMin, t1);
                tMax = Math.Min(tMax, t2);
                if (tMin > tMax)
                {
                    return false;
                }
            }
        }

        fraction = tMin;
        return true;
    }

    private void ResetQueryStamps()
    {
        for (var i = 0; i < _count; i++)
        {
            if (_testedStamp[i] < 0)
            {
                _testedStamp[i] = int.MinValue;
            }
        }
    }

    private int CellOf(float coordinate) => (int)MathF.Floor(coordinate / _cellSize);

    private int Bucket(int cellX, int cellY) =>
        (int)(((uint)(cellX * 73856093 ^ cellY * 19349663)) & (uint)(_bucketHeads.Length - 1));
}