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

using System.Numerics;
using DatReaderWriter.Enums;
using DatReaderWriter.Types;

namespace AcDream.Core.Physics;

/// <summary>
/// BSP tree traversal for sphere-polygon collision detection.
///
/// <para>
/// Ported from decompiled FUN_00539270 (chunk_00539000.c), cross-referenced
/// against ACE's <c>BSPNode.sphere_intersects_poly()</c> in
/// <c>Source/ACE.Server/Physics/BSP/BSPNode.cs</c>.
/// </para>
///
/// <para>
/// The algorithm is a recursive descent through the BSP tree:
/// <list type="number">
///   <item>Broad phase: discard the subtree if the sphere cannot reach the
///     node's bounding sphere.</item>
///   <item>Leaf: test each polygon using the existing retail-ported
///     <see cref="CollisionPrimitives.SphereIntersectsPoly"/>.</item>
///   <item>Internal: classify the sphere against the splitting plane and
///     recurse into the positive half, the negative half, or both when the
///     sphere straddles the plane.</item>
/// </list>
/// </para>
/// </summary>
public static class BSPQuery
{
    /// <summary>
    /// Test if a sphere intersects any polygon in the physics BSP tree.
    /// Returns <see langword="true"/> on the first hit, populating
    /// <paramref name="hitPolyId"/> and <paramref name="hitNormal"/>.
    ///
    /// <para>
    /// Ported from FUN_00539270; cross-ref ACE BSPNode.sphere_intersects_poly.
    /// </para>
    /// </summary>
    /// <param name="node">Current BSP node (null-safe).</param>
    /// <param name="polygons">Physics polygon dictionary from the GfxObj.</param>
    /// <param name="vertices">Vertex array from the GfxObj.</param>
    /// <param name="sphereCenter">Sphere centre in object-local space.</param>
    /// <param name="sphereRadius">Sphere radius.</param>
    /// <param name="hitPolyId">Polygon id of the first intersecting polygon (0 on miss).</param>
    /// <param name="hitNormal">Outward normal at the hit point (zero on miss).</param>
    /// <returns><see langword="true"/> if any polygon is hit.</returns>
    public static bool SphereIntersectsPoly(
        PhysicsBSPNode? node,
        Dictionary<ushort, Polygon> polygons,
        VertexArray vertices,
        Vector3 sphereCenter,
        float sphereRadius,
        out ushort hitPolyId,
        out Vector3 hitNormal)
    {
        hitPolyId = 0;
        hitNormal = Vector3.Zero;

        if (node is null) return false;

        // ----------------------------------------------------------------
        // Broad phase: reject the whole subtree when the sphere cannot
        // reach the node's bounding sphere. Both Leaf and internal nodes
        // carry a BoundingSphere in the retail format.
        // ----------------------------------------------------------------
        {
            float dist = Vector3.Distance(sphereCenter, node.BoundingSphere.Origin);
            if (dist > sphereRadius + node.BoundingSphere.Radius + CollisionPrimitives.Epsilon)
                return false;
        }

        // ----------------------------------------------------------------
        // Leaf node: test each referenced polygon against the sphere using
        // the retail-ported CollisionPrimitives.SphereIntersectsPoly.
        // ----------------------------------------------------------------
        if (node.Type == BSPNodeType.Leaf)
        {
            foreach (var polyIdx in node.Polygons)
            {
                if (!polygons.TryGetValue(polyIdx, out var poly)) continue;
                if (poly.VertexIds.Count < 3) continue;

                // Gather polygon vertices from the vertex array.
                var polyVerts = new Vector3[poly.VertexIds.Count];
                bool allFound = true;
                for (int i = 0; i < poly.VertexIds.Count; i++)
                {
                    ushort vid = (ushort)poly.VertexIds[i];
                    if (vertices.Vertices.TryGetValue(vid, out var sv))
                        polyVerts[i] = sv.Origin;
                    else { allFound = false; break; }
                }
                if (!allFound) continue;

                // Compute the polygon plane using the retail CalcNormal port.
                CollisionPrimitives.CalcNormal(polyVerts, out var normal, out float planeD);
                if (normal.LengthSquared() < CollisionPrimitives.EpsilonSq) continue;

                var polyPlane = new Plane(normal, planeD);

                if (CollisionPrimitives.SphereIntersectsPoly(
                    polyPlane, polyVerts, sphereCenter, sphereRadius, out _))
                {
                    hitPolyId = polyIdx;
                    hitNormal = normal;
                    return true;
                }
            }
            return false;
        }

        // ----------------------------------------------------------------
        // Internal node: classify sphere against splitting plane and
        // recurse into the positive side, negative side, or both.
        //
        // System.Numerics.Plane convention: dot(N, p) + D = 0 on the
        // surface, so signed distance = dot(N, center) + D.
        // FUN_00539270 uses the same sign convention.
        // ----------------------------------------------------------------
        float splitDist = Vector3.Dot(node.SplittingPlane.Normal, sphereCenter)
                          + node.SplittingPlane.D;
        float reach = sphereRadius - CollisionPrimitives.Epsilon;

        if (splitDist >= reach)
        {
            // Sphere entirely on the positive side.
            return SphereIntersectsPoly(
                node.PosNode, polygons, vertices,
                sphereCenter, sphereRadius,
                out hitPolyId, out hitNormal);
        }

        if (splitDist <= -reach)
        {
            // Sphere entirely on the negative side.
            return SphereIntersectsPoly(
                node.NegNode, polygons, vertices,
                sphereCenter, sphereRadius,
                out hitPolyId, out hitNormal);
        }

        // Sphere straddles the plane — check both sides, return on first hit.
        if (SphereIntersectsPoly(node.PosNode, polygons, vertices,
                sphereCenter, sphereRadius, out hitPolyId, out hitNormal))
            return true;

        return SphereIntersectsPoly(node.NegNode, polygons, vertices,
            sphereCenter, sphereRadius, out hitPolyId, out hitNormal);
    }
}