diff --git a/src/AcDream.Core/Physics/BSPQuery.cs b/src/AcDream.Core/Physics/BSPQuery.cs index fd8d8cc..7d7a37b 100644 --- a/src/AcDream.Core/Physics/BSPQuery.cs +++ b/src/AcDream.Core/Physics/BSPQuery.cs @@ -1,480 +1,147 @@ using System.Numerics; using DatReaderWriter.Enums; using DatReaderWriter.Types; +using Plane = System.Numerics.Plane; namespace AcDream.Core.Physics; /// -/// BSP tree collision queries ported from decompiled retail client. +/// BSP tree collision queries faithfully ported from the decompiled retail AC client. /// Cross-referenced against ACE BSPTree.cs, BSPNode.cs, BSPLeaf.cs, Polygon.cs. /// /// -/// The retail system has 6 dispatch paths in find_collisions based on -/// SpherePath flags. Each path uses different BSP traversal and -/// collision response strategies. +/// All methods operate in object-local space. The caller (e.g. FindObjCollisions +/// or FindEnvCollisions in Transition) transforms player spheres into object-local +/// space before calling, and transforms collision normals back to world space after. /// /// /// -/// Methods operate in object-local space. The caller (e.g. FindObjCollisions -/// in Transition) is responsible for transforming player spheres into object- -/// local space before calling, and transforming hit normals back to world space. +/// Polygons are passed as pre-resolved dictionaries +/// (vertex positions + face plane computed once at cache time) to avoid per-test +/// vertex lookups. See . +/// +/// +/// +/// ACE references: BSPTree.cs, BSPNode.cs, BSPLeaf.cs, Polygon.cs. +/// Decompiled client: chunk_00530000.c, chunk_00539000.c. /// /// public static class BSPQuery { - // ----------------------------------------------------------------------- - // Polygon plane computation helper - // ----------------------------------------------------------------------- + // ========================================================================= + // Internal mutable sphere helper + // + // ACE's Sphere class is mutable (Center + Radius). We mirror that with a + // small internal class so adjust_sphere_to_plane can mutate validPos in place, + // exactly as ACE does when passing Sphere by reference through find_walkable. + // ========================================================================= /// - /// Compute the polygon plane from vertices, using the retail CalcNormal - /// algorithm. Returns false if the polygon is degenerate. + /// Mutable sphere used internally during BSP traversal where the position + /// is adjusted as walkable surfaces are found (e.g. find_walkable, step_sphere_down). + /// Mirrors ACE's Sphere(Center, Radius) which is passed by reference through + /// the BSP recursive calls. /// - private static bool TryGetPolyPlane( - Polygon poly, VertexArray vertices, - out Plane plane, out Vector3[] polyVerts) + internal sealed class CollisionSphere { - plane = default; - polyVerts = Array.Empty(); + public Vector3 Center; + public float Radius; - if (poly.VertexIds.Count < 3) return false; - - polyVerts = new Vector3[poly.VertexIds.Count]; - for (int i = 0; i < poly.VertexIds.Count; i++) + public CollisionSphere(Vector3 center, float radius) { - ushort vid = (ushort)poly.VertexIds[i]; - if (!vertices.Vertices.TryGetValue(vid, out var sv)) - return false; - polyVerts[i] = sv.Origin; + Center = center; + Radius = radius; } - CollisionPrimitives.CalcNormal(polyVerts, out var normal, out float planeD); - if (normal.LengthSquared() < CollisionPrimitives.EpsilonSq) - return false; - - plane = new Plane(normal, planeD); - return true; + public CollisionSphere(CollisionSphere other) + { + Center = other.Center; + Radius = other.Radius; + } } - // ----------------------------------------------------------------------- - // 1. FindCollisions — BSPTree.find_collisions dispatcher - // ACE: BSPTree.cs line ~131. 6 dispatch paths. - // ----------------------------------------------------------------------- + // ========================================================================= + // Bounding-sphere intersection helper + // + // ACE: Sphere.Intersects(Sphere) — LengthSquared < (r1+r2)^2. + // Used by every BSP traversal method for broad-phase rejection. + // ========================================================================= /// - /// BSPTree.find_collisions -- the 6-path dispatcher. - /// Determines how collision is tested against BSP geometry based on - /// SpherePath flags. - /// - /// - /// ACE: BSPTree.cs find_collisions. Decompiled: chunk_00530000.c. - /// The 6 paths are: - /// 1. Placement/ObstructionEthereal -> sphere_intersects_solid - /// 2. CheckWalkable -> check_walkable (hits_walkable) - /// 3. StepDown -> step_sphere_down (find_walkable) - /// 4. Collide -> find_walkable - /// 5. Contact|OnWalkable -> sphere_intersects_poly + step_up/slide - /// 6. Default (not in contact) -> sphere_intersects_poly + collide_with_pt - /// + /// Test whether a query sphere intersects a BSP node's bounding sphere. + /// ACE: Sphere.Intersects(Sphere other) — d.LengthSquared < (r1+r2)^2. /// - /// Root of the physics BSP tree. - /// Physics polygon dictionary from the GfxObj. - /// Vertex array from the GfxObj. - /// The current transition state. - /// The sphere in object-local space (index 0). - /// Second sphere in local space (index 1), or null. - /// Previous position of the sphere center in local space. - /// The up vector in local space. - /// Scale factor for the object. - public static TransitionState FindCollisions( - PhysicsBSPNode? root, - Dictionary polygons, - VertexArray vertices, - Transition transition, - DatReaderWriter.Types.Sphere localSphere, - DatReaderWriter.Types.Sphere? localSphere1, - Vector3 localCurrCenter, - Vector3 localSpaceZ, - float scale) + private static bool NodeIntersects(PhysicsBSPNode node, CollisionSphere sphere) { - if (root is null) return TransitionState.OK; - - var path = transition.SpherePath; - var collisions = transition.CollisionInfo; - var obj = transition.ObjectInfo; - - var center = localCurrCenter; - var movement = localSphere.Origin - center; - - // ---------------------------------------------------------------- - // Path 1: Placement or ObstructionEthereal - // ACE: sphere_intersects_solid - // ---------------------------------------------------------------- - if (path.InsertType == InsertType.Placement || obj.Ethereal) - { - if (SphereIntersectsSolid(root, polygons, vertices, - localSphere.Origin, localSphere.Radius, true)) - return TransitionState.Collided; - - if (localSphere1 is not null) - { - if (SphereIntersectsSolid(root, polygons, vertices, - localSphere1.Origin, localSphere1.Radius, true)) - return TransitionState.Collided; - } - return TransitionState.OK; - } - - // ---------------------------------------------------------------- - // Path 2: CheckWalkable - // ACE: check_walkable -> hits_walkable - // ---------------------------------------------------------------- - if (path.CheckWalkable) - { - return HitsWalkable(root, polygons, vertices, - localSphere.Origin, localSphere.Radius, - path, localSpaceZ) - ? TransitionState.Collided - : TransitionState.OK; - } - - // ---------------------------------------------------------------- - // Path 3: StepDown - // ACE: step_sphere_down -> find_walkable - // ---------------------------------------------------------------- - if (path.StepDown) - { - return StepSphereDown(root, polygons, vertices, - transition, localSphere, localSpaceZ, scale); - } - - // ---------------------------------------------------------------- - // Path 4: Collide (stepping onto walkable surfaces) - // ACE: find_walkable - // ---------------------------------------------------------------- - if (path.Collide) - { - var validCenter = localSphere.Origin; - Polygon? hitPoly = null; - Plane hitPlane = default; - bool changed = false; - - FindWalkable(root, polygons, vertices, - path, ref validCenter, localSphere.Radius, - ref hitPoly, ref hitPlane, movement, localSpaceZ, ref changed); - - if (changed && hitPoly is not null) - { - var offset = validCenter - localSphere.Origin; - // Transform offset back and apply - var collisionNormal = offset * scale; - path.AddOffsetToCheckPos(collisionNormal); - - collisions.SetContactPlane( - new Plane(hitPlane.Normal, hitPlane.D * scale), - path.CheckCellId, false); - - // SetWalkable state - path.WalkableValid = true; - path.WalkablePlane = new Plane(hitPlane.Normal, hitPlane.D * scale); - path.WalkableAllowance = PhysicsGlobals.FloorZ; - - return TransitionState.Adjusted; - } - return TransitionState.OK; - } - - // ---------------------------------------------------------------- - // Path 5 & 6: sphere_intersects_poly with movement vector - // ---------------------------------------------------------------- - Polygon? hitPoly5 = null; - Plane hitPlane5 = default; - Vector3 contactPoint5 = Vector3.Zero; - - // ---------------------------------------------------------------- - // Path 5: Contact | OnWalkable - // ACE: sphere_intersects_poly + step_sphere_up / slide_sphere - // ---------------------------------------------------------------- - if (obj.State.HasFlag(ObjectInfoState.Contact)) - { - if (SphereIntersectsPolyWithMovement(root, polygons, vertices, - localSphere.Origin, localSphere.Radius, movement, - ref hitPoly5, ref hitPlane5, ref contactPoint5)) - { - // Step up over the obstacle - var globNormal = hitPlane5.Normal * scale; - if (globNormal.LengthSquared() > PhysicsGlobals.EpsilonSq) - globNormal = Vector3.Normalize(globNormal); - return StepSphereUp(transition, globNormal); - } - - if (localSphere1 is not null) - { - Polygon? hitPoly1 = null; - Plane hitPlane1 = default; - Vector3 contactPoint1 = Vector3.Zero; - - if (SphereIntersectsPolyWithMovement(root, polygons, vertices, - localSphere1.Origin, localSphere1.Radius, movement, - ref hitPoly1, ref hitPlane1, ref contactPoint1)) - { - // Slide sphere for second sphere hit - var globNormal = hitPlane1.Normal * scale; - if (globNormal.LengthSquared() > PhysicsGlobals.EpsilonSq) - globNormal = Vector3.Normalize(globNormal); - collisions.SetCollisionNormal(globNormal); - return TransitionState.Collided; - } - - if (hitPoly1 is not null) - { - return NegPolyHit(path, hitPlane1.Normal, false); - } - if (hitPoly5 is not null) - { - return NegPolyHit(path, hitPlane5.Normal, true); - } - } - return TransitionState.OK; - } - - // ---------------------------------------------------------------- - // Path 6: Default (not in contact) - // ACE: sphere_intersects_poly → collide / land - // ---------------------------------------------------------------- - if (SphereIntersectsPolyWithMovement(root, polygons, vertices, - localSphere.Origin, localSphere.Radius, movement, - ref hitPoly5, ref hitPlane5, ref contactPoint5) - || hitPoly5 is not null) - { - if (obj.State.HasFlag(ObjectInfoState.PathClipped)) - { - // PerfectClip / PathClipped collision response - var collisionNormal = hitPlane5.Normal; - collisions.SetCollisionNormal(collisionNormal); - return TransitionState.Collided; - } - - var globNormal5 = hitPlane5.Normal; - if (globNormal5.LengthSquared() > PhysicsGlobals.EpsilonSq) - globNormal5 = Vector3.Normalize(globNormal5); - - path.WalkableAllowance = PhysicsGlobals.LandingZ; - path.Collide = true; - collisions.SetCollisionNormal(globNormal5); - return TransitionState.Adjusted; - } - - if (localSphere1 is not null) - { - Polygon? hitPoly1b = null; - Plane hitPlane1b = default; - Vector3 contactPoint1b = Vector3.Zero; - - if (SphereIntersectsPolyWithMovement(root, polygons, vertices, - localSphere1.Origin, localSphere1.Radius, movement, - ref hitPoly1b, ref hitPlane1b, ref contactPoint1b) - || hitPoly1b is not null) - { - var globNormal = hitPlane1b.Normal; - if (globNormal.LengthSquared() > PhysicsGlobals.EpsilonSq) - globNormal = Vector3.Normalize(globNormal); - collisions.SetCollisionNormal(globNormal); - return TransitionState.Collided; - } - } - - return TransitionState.OK; + var bs = node.BoundingSphere; + var d = sphere.Center - bs.Origin; + float r = sphere.Radius + bs.Radius; + return d.LengthSquared() < r * r; } - // ----------------------------------------------------------------------- - // 2. SphereIntersectsPolyWithMovement — BSPNode.sphere_intersects_poly - // with movement vector (ACE BSPNode.cs + BSPLeaf.cs) - // Uses pos_hits_sphere for front-face culling. - // ----------------------------------------------------------------------- + // ========================================================================= + // POLYGON-LEVEL METHODS + // + // Ported from ACE Polygon.cs. These operate on a single ResolvedPolygon. + // ========================================================================= + + // ------------------------------------------------------------------------- + // polygon_hits_sphere_precise + // ACE: Polygon.cs polygon_hits_sphere_precise + // ------------------------------------------------------------------------- /// - /// BSPNode.sphere_intersects_poly -- tree traversal with movement vector. - /// Recursively descends the BSP tree testing sphere+movement against - /// the splitting plane, then tests polygons at leaves using pos_hits_sphere - /// (front-face culling by movement direction). + /// Polygon.polygon_hits_sphere_precise — full retail polygon-sphere overlap. /// /// - /// ACE: BSPNode.cs sphere_intersects_poly (internal nodes), - /// BSPLeaf.cs sphere_intersects_poly (leaf nodes). - /// Decompiled: FUN_00539270. + /// The outer loop checks if the contact point projected onto the plane lies + /// inside all edge half-planes. If it falls outside an edge the inner loop + /// checks whether the sphere still touches that edge or vertex within radius. /// - /// - public static bool SphereIntersectsPolyWithMovement( - PhysicsBSPNode? node, - Dictionary polygons, - VertexArray vertices, - Vector3 sphereCenter, float sphereRadius, - Vector3 movement, - ref Polygon? hitPoly, - ref Plane hitPlane, - ref Vector3 contactPoint) - { - if (node is null) return false; - - // Broad phase: bounding sphere rejection - float dist = Vector3.Distance(sphereCenter, node.BoundingSphere.Origin); - if (dist > sphereRadius + node.BoundingSphere.Radius + CollisionPrimitives.Epsilon) - return false; - - // Leaf node: test each polygon with pos_hits_sphere - if (node.Type == BSPNodeType.Leaf) - { - if (node.Polygons.Count == 0) return false; - - foreach (var polyIdx in node.Polygons) - { - if (!polygons.TryGetValue(polyIdx, out var poly)) continue; - if (!TryGetPolyPlane(poly, vertices, out var polyPlane, out var polyVerts)) - continue; - - if (PosHitsSphere(polyPlane, polyVerts, sphereCenter, sphereRadius, - movement, out var cp)) - { - hitPoly = poly; - hitPlane = polyPlane; - contactPoint = cp; - return true; - } - } - return false; - } - - // Internal node: classify against splitting plane - float splitDist = Vector3.Dot(node.SplittingPlane.Normal, sphereCenter) - + node.SplittingPlane.D; - float reach = sphereRadius - PhysicsGlobals.EPSILON; - - if (splitDist >= reach) - { - return SphereIntersectsPolyWithMovement( - node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, movement, - ref hitPoly, ref hitPlane, ref contactPoint); - } - - if (splitDist <= -reach) - { - return SphereIntersectsPolyWithMovement( - node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, movement, - ref hitPoly, ref hitPlane, ref contactPoint); - } - - // Straddles: check both sides - if (SphereIntersectsPolyWithMovement( - node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, movement, - ref hitPoly, ref hitPlane, ref contactPoint)) - return true; - - return SphereIntersectsPolyWithMovement( - node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, movement, - ref hitPoly, ref hitPlane, ref contactPoint); - } - - // ----------------------------------------------------------------------- - // 3. PosHitsSphere — Polygon.pos_hits_sphere - // ACE: Polygon.cs pos_hits_sphere - // The critical front-face culling test using movement direction. - // ----------------------------------------------------------------------- - - /// - /// Polygon.pos_hits_sphere -- per-polygon test with movement direction. - /// Only detects collisions when the sphere is MOVING TOWARD the polygon - /// face (dot(movement, normal) < 0). This front-face culling prevents - /// the "walking into walls" problem and is the key difference from the - /// static SphereIntersectsPoly. /// - /// - /// ACE: Polygon.cs pos_hits_sphere. - /// Decompiled: retail client polygon collision test. - /// + /// ACE: Polygon.cs polygon_hits_sphere_precise. /// - public static bool PosHitsSphere( - Plane polyPlane, - ReadOnlySpan polyVerts, - Vector3 sphereCenter, float sphereRadius, - Vector3 movement, - out Vector3 contactPoint) + private static bool PolygonHitsSpherePrecise( + in Plane polyPlane, + ReadOnlySpan verts, + Vector3 sphereCenter, + float sphereRadius, + ref Vector3 contactPoint) { - contactPoint = Vector3.Zero; - - // Test sphere-polygon overlap using the precise retail algorithm - bool hit = PolygonHitsSpherePrecise(polyPlane, polyVerts, - sphereCenter, sphereRadius, ref contactPoint); - - // Front-face cull: only count hits where movement is toward the face - // ACE: dist = Vector3.Dot(movement, Plane.Normal); if dist >= 0 return false; - float moveDot = Vector3.Dot(movement, polyPlane.Normal); - if (moveDot >= 0f) - return false; - - return hit; - } - - // ----------------------------------------------------------------------- - // 4. PolygonHitsSpherePrecise — Polygon.polygon_hits_sphere_precise - // ACE: Polygon.cs polygon_hits_sphere_precise - // Full retail precision polygon-sphere overlap test. - // ----------------------------------------------------------------------- - - /// - /// Polygon.polygon_hits_sphere_precise -- the precise retail polygon-sphere - /// intersection test. Unlike the simpler SphereIntersectsPoly, this has an - /// inner loop for edge/vertex proximity testing that matches the decompiled - /// client exactly. - /// - /// - /// ACE: Polygon.cs polygon_hits_sphere_precise. - /// - /// - public static bool PolygonHitsSpherePrecise( - Plane polyPlane, - ReadOnlySpan polyVerts, - Vector3 sphereCenter, float sphereRadius, - ref Vector3 contactPoint) - { - int numVerts = polyVerts.Length; - if (numVerts == 0) return true; + int n = verts.Length; + if (n == 0) return true; float dist = Vector3.Dot(polyPlane.Normal, sphereCenter) + polyPlane.D; - float rad = sphereRadius - PhysicsGlobals.EPSILON; + float rad = sphereRadius - PhysicsGlobals.EPSILON; if (MathF.Abs(dist) > rad) return false; - float diff = rad * rad - dist * dist; + float diff = rad * rad - dist * dist; contactPoint = sphereCenter - polyPlane.Normal * dist; - int prevIdx = numVerts - 1; - for (int i = 0; i < numVerts; i++) + int prevIdx = n - 1; + for (int i = 0; i < n; i++) { - var vertex = polyVerts[i]; - var lastVertex = polyVerts[prevIdx]; + var v = verts[i]; + var lv = verts[prevIdx]; prevIdx = i; - var edge = vertex - lastVertex; - var disp = contactPoint - lastVertex; + var edge = v - lv; + var disp = contactPoint - lv; var cross = Vector3.Cross(polyPlane.Normal, edge); if (Vector3.Dot(disp, cross) >= 0f) continue; - // Inner loop: re-check all edges for closest feature - prevIdx = numVerts - 1; - for (int j = 0; j < numVerts; j++) + // Contact point is outside this edge — run inner loop. + prevIdx = n - 1; + for (int j = 0; j < n; j++) { - vertex = polyVerts[j]; - lastVertex = polyVerts[prevIdx]; + v = verts[j]; + lv = verts[prevIdx]; prevIdx = j; - edge = vertex - lastVertex; - disp = contactPoint - lastVertex; + edge = v - lv; + disp = contactPoint - lv; cross = Vector3.Cross(polyPlane.Normal, edge); float dispDot = Vector3.Dot(disp, cross); @@ -496,452 +163,135 @@ public static class BSPQuery return true; } - // ----------------------------------------------------------------------- - // 5. SphereIntersectsSolid — BSPNode.sphere_intersects_solid - // ACE: BSPNode.cs + BSPLeaf.cs sphere_intersects_solid - // Used for placement/ethereal checks. - // ----------------------------------------------------------------------- + // ------------------------------------------------------------------------- + // pos_hits_sphere + // ACE: Polygon.cs pos_hits_sphere + // ------------------------------------------------------------------------- /// - /// BSPNode.sphere_intersects_solid -- checks if a sphere intersects any - /// solid geometry in the BSP tree. Used for placement and ethereal checks. + /// Polygon.pos_hits_sphere — polygon hit filtered by movement direction. /// /// - /// ACE: BSPNode.cs sphere_intersects_solid (internal node), - /// BSPLeaf.cs sphere_intersects_solid (leaf). + /// Rejects the hit if dot(movement, normal) ≥ 0 (moving away from or + /// parallel to the polygon face). This front-face cull prevents back-face + /// hits after a sphere has already penetrated past a polygon. /// + /// + /// ACE: Polygon.cs pos_hits_sphere. /// - public static bool SphereIntersectsSolid( - PhysicsBSPNode? node, - Dictionary polygons, - VertexArray vertices, - Vector3 sphereCenter, float sphereRadius, - bool centerCheck) + private static bool PosHitsSphere( + ResolvedPolygon poly, + CollisionSphere sphere, + Vector3 movement, + ref Vector3 contactPoint, + ref ResolvedPolygon? hitPoly) { - if (node is null) return false; + bool hit = PolygonHitsSpherePrecise( + poly.Plane, poly.Vertices, + sphere.Center, sphere.Radius, + ref contactPoint); - // Broad phase - float dist = Vector3.Distance(sphereCenter, node.BoundingSphere.Origin); - if (dist > sphereRadius + node.BoundingSphere.Radius + CollisionPrimitives.Epsilon) - return false; + // ACE: dist = Dot(movement, Plane.Normal); if dist >= 0 return false; + float moveDot = Vector3.Dot(movement, poly.Plane.Normal); + if (moveDot >= 0f) return false; - // Leaf node - if (node.Type == BSPNodeType.Leaf) - { - if (node.Polygons.Count == 0) return false; - if (centerCheck && node.Solid != 0) return true; - - foreach (var polyIdx in node.Polygons) - { - if (!polygons.TryGetValue(polyIdx, out var poly)) continue; - if (!TryGetPolyPlane(poly, vertices, out var polyPlane, out var polyVerts)) - continue; - - if (CollisionPrimitives.SphereIntersectsPoly( - polyPlane, polyVerts, sphereCenter, sphereRadius, out _)) - return true; - } - return false; - } - - // Internal node - float splitDist = Vector3.Dot(node.SplittingPlane.Normal, sphereCenter) - + node.SplittingPlane.D; - float reach = sphereRadius - PhysicsGlobals.EPSILON; - - if (splitDist >= reach) - return SphereIntersectsSolid(node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, centerCheck); - - if (splitDist <= -reach) - return SphereIntersectsSolid(node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, centerCheck); - - // Straddles: check both sides with appropriate centerCheck flags - if (splitDist < 0f) - { - if (SphereIntersectsSolid(node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, false)) - return true; - return SphereIntersectsSolid(node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, centerCheck); - } - else - { - if (SphereIntersectsSolid(node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, centerCheck)) - return true; - return SphereIntersectsSolid(node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, false); - } + if (hit) hitPoly = poly; + return hit; } - // ----------------------------------------------------------------------- - // 6. HitsWalkable — BSPNode.hits_walkable + BSPLeaf.hits_walkable - // ACE: BSPNode.cs hits_walkable, BSPLeaf.cs hits_walkable - // Used for check_walkable path. - // ----------------------------------------------------------------------- + // ------------------------------------------------------------------------- + // hits_sphere + // ACE: Polygon.cs hits_sphere + // ------------------------------------------------------------------------- /// - /// BSPNode.hits_walkable -- checks if a sphere touches any walkable polygon. - /// Used by the CheckWalkable dispatch path. - /// - /// - /// ACE: BSPNode.cs hits_walkable (internal), BSPLeaf.cs hits_walkable (leaf). - /// + /// Polygon.hits_sphere — static sphere-polygon overlap (no movement culling). + /// ACE: Polygon.cs hits_sphere. /// - public static bool HitsWalkable( - PhysicsBSPNode? node, - Dictionary polygons, - VertexArray vertices, - Vector3 sphereCenter, float sphereRadius, - SpherePath path, - Vector3 up) + private static bool HitsSphere(ResolvedPolygon poly, CollisionSphere sphere) { - if (node is null) return false; - - // Broad phase - float dist = Vector3.Distance(sphereCenter, node.BoundingSphere.Origin); - if (dist > sphereRadius + node.BoundingSphere.Radius + CollisionPrimitives.Epsilon) - return false; - - // Leaf node - if (node.Type == BSPNodeType.Leaf) - { - if (node.Polygons.Count == 0) return false; - - foreach (var polyIdx in node.Polygons) - { - if (!polygons.TryGetValue(polyIdx, out var poly)) continue; - if (!TryGetPolyPlane(poly, vertices, out var polyPlane, out var polyVerts)) - continue; - - // walkable_hits_sphere: check normal faces up + sphere intersection - float dp = Vector3.Dot(up, polyPlane.Normal); - if (dp <= path.WalkableAllowance) continue; - - if (CollisionPrimitives.SphereIntersectsPoly( - polyPlane, polyVerts, sphereCenter, sphereRadius, out _)) - { - // check_small_walkable - if (CheckWalkable(polyPlane, polyVerts, sphereCenter, sphereRadius, up, true)) - return true; - } - } - return false; - } - - // Internal node - float splitDist = Vector3.Dot(node.SplittingPlane.Normal, sphereCenter) - + node.SplittingPlane.D; - float reach = sphereRadius - PhysicsGlobals.EPSILON; - - if (splitDist >= reach) - return HitsWalkable(node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, path, up); - - if (splitDist <= -reach) - return HitsWalkable(node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, path, up); - - if (HitsWalkable(node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, path, up)) - return true; - - return HitsWalkable(node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, path, up); + Vector3 cp = Vector3.Zero; + return PolygonHitsSpherePrecise( + poly.Plane, poly.Vertices, + sphere.Center, sphere.Radius, + ref cp); } - // ----------------------------------------------------------------------- - // 7. FindWalkable — BSPNode.find_walkable + BSPLeaf.find_walkable - // ACE: BSPNode.cs find_walkable, BSPLeaf.cs find_walkable - // Used for Collide and StepDown paths. - // ----------------------------------------------------------------------- + // ------------------------------------------------------------------------- + // walkable_hits_sphere + // ACE: Polygon.cs walkable_hits_sphere + // ------------------------------------------------------------------------- /// - /// BSPNode.find_walkable -- finds walkable surfaces and adjusts the sphere - /// to rest on them. Used by the Collide and StepDown dispatch paths. + /// Polygon.walkable_hits_sphere — walkable surface test. /// /// - /// ACE: BSPNode.cs find_walkable (internal), BSPLeaf.cs find_walkable (leaf). + /// A polygon is walkable only when dot(up, normal) > WalkableAllowance. + /// polygon_hits_sphere_precise is then called for the overlap. /// + /// + /// ACE: Polygon.cs walkable_hits_sphere. /// - public static void FindWalkable( - PhysicsBSPNode? node, - Dictionary polygons, - VertexArray vertices, - SpherePath path, - ref Vector3 validCenter, float sphereRadius, - ref Polygon? hitPoly, - ref Plane hitPlane, - Vector3 movement, - Vector3 up, - ref bool changed) + private static bool WalkableHitsSphere( + ResolvedPolygon poly, + SpherePath path, + CollisionSphere sphere, + Vector3 up) { - if (node is null) return; + float dp = Vector3.Dot(up, poly.Plane.Normal); + if (dp <= path.WalkableAllowance) return false; - // Broad phase - float dist = Vector3.Distance(validCenter, node.BoundingSphere.Origin); - if (dist > sphereRadius + node.BoundingSphere.Radius + CollisionPrimitives.Epsilon) - return; - - // Leaf node - if (node.Type == BSPNodeType.Leaf) - { - if (node.Polygons.Count == 0) return; - - foreach (var polyIdx in node.Polygons) - { - if (!polygons.TryGetValue(polyIdx, out var poly)) continue; - if (!TryGetPolyPlane(poly, vertices, out var polyPlane, out var polyVerts)) - continue; - - // walkable_hits_sphere check - float dpUp = Vector3.Dot(up, polyPlane.Normal); - if (dpUp <= path.WalkableAllowance) continue; - - // polygon_hits_sphere_precise - Vector3 contactPoint = Vector3.Zero; - if (!PolygonHitsSpherePrecise(polyPlane, polyVerts, - validCenter, sphereRadius, ref contactPoint)) - continue; - - // adjust_sphere_to_plane - if (AdjustSphereToPlane(polyPlane, ref validCenter, sphereRadius, - movement, ref path)) - { - changed = true; - hitPoly = poly; - hitPlane = polyPlane; - } - } - return; - } - - // Internal node - float splitDist = Vector3.Dot(node.SplittingPlane.Normal, validCenter) - + node.SplittingPlane.D; - float reach = sphereRadius - PhysicsGlobals.EPSILON; - - if (splitDist >= reach) - { - FindWalkable(node.PosNode, polygons, vertices, path, - ref validCenter, sphereRadius, ref hitPoly, ref hitPlane, - movement, up, ref changed); - return; - } - - if (splitDist <= -reach) - { - FindWalkable(node.NegNode, polygons, vertices, path, - ref validCenter, sphereRadius, ref hitPoly, ref hitPlane, - movement, up, ref changed); - return; - } - - // Straddles - FindWalkable(node.PosNode, polygons, vertices, path, - ref validCenter, sphereRadius, ref hitPoly, ref hitPlane, - movement, up, ref changed); - FindWalkable(node.NegNode, polygons, vertices, path, - ref validCenter, sphereRadius, ref hitPoly, ref hitPlane, - movement, up, ref changed); + Vector3 cp = Vector3.Zero; + return PolygonHitsSpherePrecise( + poly.Plane, poly.Vertices, + sphere.Center, sphere.Radius, + ref cp); } - // ----------------------------------------------------------------------- - // 8. StepSphereDown — BSPTree.step_sphere_down - // ACE: BSPTree.cs step_sphere_down - // ----------------------------------------------------------------------- + // ------------------------------------------------------------------------- + // check_walkable / check_small_walkable + // ACE: Polygon.cs check_walkable (small=false), check_small_walkable (small=true) + // ------------------------------------------------------------------------- /// - /// BSPTree.step_sphere_down -- BSP-based step-down ground search. - /// Probes downward along localSpaceZ to find a walkable surface. + /// Polygon.check_walkable — check if the sphere projects onto the walkable area. /// /// - /// ACE: BSPTree.cs step_sphere_down. + /// Projects sphere center onto polygon plane along the up vector, then checks + /// that projected point is inside (or close enough to) all polygon edges. + /// When is true the effective radius is halved + /// (check_small_walkable variant for step-down detection). /// - /// - public static TransitionState StepSphereDown( - PhysicsBSPNode root, - Dictionary polygons, - VertexArray vertices, - Transition transition, - DatReaderWriter.Types.Sphere localSphere, - Vector3 localSpaceZ, - float scale) - { - var path = transition.SpherePath; - var collisions = transition.CollisionInfo; - - float stepDownAmount = -(path.StepDownAmt * path.WalkInterp); - var movement = localSpaceZ * stepDownAmount * (1f / scale); - var validCenter = localSphere.Origin; - bool changed = false; - Polygon? polyHit = null; - Plane hitPlane = default; - - FindWalkable(root, polygons, vertices, path, - ref validCenter, localSphere.Radius, - ref polyHit, ref hitPlane, movement, localSpaceZ, ref changed); - - if (changed && polyHit is not null) - { - var adjusted = validCenter - localSphere.Origin; - var offset = adjusted * scale; - path.AddOffsetToCheckPos(offset); - - collisions.SetContactPlane( - new Plane(hitPlane.Normal, hitPlane.D * scale), - path.CheckCellId, false); - - path.WalkableValid = true; - path.WalkablePlane = new Plane(hitPlane.Normal, hitPlane.D * scale); - path.WalkableAllowance = PhysicsGlobals.FloorZ; - - return TransitionState.Adjusted; - } - - return TransitionState.OK; - } - - // ----------------------------------------------------------------------- - // 9. StepSphereUp — BSPTree.step_sphere_up - // ACE: BSPTree.cs step_sphere_up - // ----------------------------------------------------------------------- - - /// - /// BSPTree.step_sphere_up -- attempts to step over an obstacle. /// - /// - /// ACE: BSPTree.cs step_sphere_up. - /// - /// - private static TransitionState StepSphereUp( - Transition transition, Vector3 collisionNormal) - { - // ACE: if (transition.StepUp(globNormal)) return OK; else StepUpSlide - // For now, simplified: just report collision and let the main - // transition system handle step-up via its existing mechanism. - var path = transition.SpherePath; - transition.CollisionInfo.SetCollisionNormal(collisionNormal); - - // Attempt step-up: set the flag for the transition system - path.StepUp = true; - path.StepUpNormal = collisionNormal; - - return TransitionState.OK; - } - - // ----------------------------------------------------------------------- - // 10. NegPolyHit — BSPTree.NegPolyHit - // ACE: BSPTree.cs NegPolyHit - // ----------------------------------------------------------------------- - - /// - /// BSPTree.NegPolyHit -- records a negative polygon hit for later processing. - /// The collision normal is stored but the transition is not blocked. - /// - /// - /// ACE: BSPTree.cs NegPolyHit. - /// - /// - private static TransitionState NegPolyHit( - SpherePath path, Vector3 planeNormal, bool stepUp) - { - path.NegPolyHit = true; - path.NegStepUp = stepUp; - path.NegCollisionNormal = planeNormal; - return TransitionState.OK; - } - - // ----------------------------------------------------------------------- - // 11. AdjustSphereToPlane — Polygon.adjust_sphere_to_plane - // ACE: Polygon.cs adjust_sphere_to_plane - // ----------------------------------------------------------------------- - - /// - /// Polygon.adjust_sphere_to_plane -- adjusts a sphere's position to rest - /// on a polygon plane, used during find_walkable traversal. - /// - /// - /// ACE: Polygon.cs adjust_sphere_to_plane. - /// - /// - private static bool AdjustSphereToPlane( - Plane polyPlane, - ref Vector3 validCenter, float sphereRadius, - Vector3 movement, - ref SpherePath path) - { - float dpPos = Vector3.Dot(validCenter, polyPlane.Normal) + polyPlane.D; - float dpMove = Vector3.Dot(movement, polyPlane.Normal); - - float dist; - if (dpMove <= PhysicsGlobals.EPSILON) - { - if (dpMove >= -PhysicsGlobals.EPSILON) - return false; - - dist = dpPos - sphereRadius; - } - else - { - dist = -sphereRadius - dpPos; - } - - float iDist = dist / dpMove; - float interp = (1f - iDist) * path.WalkInterp; - if (interp >= path.WalkInterp || interp < -0.5f) - return false; - - validCenter -= movement * iDist; - path.WalkInterp = interp; - return true; - } - - // ----------------------------------------------------------------------- - // 12. CheckWalkable — Polygon.check_walkable - // ACE: Polygon.cs check_walkable / check_small_walkable - // ----------------------------------------------------------------------- - - /// - /// Polygon.check_walkable -- checks if a sphere is over the walkable - /// area of a polygon (accounting for sphere radius tolerance at edges). - /// - /// - /// ACE: Polygon.cs check_walkable / check_small_walkable. - /// + /// ACE: Polygon.cs check_walkable / check_small_walkable. /// private static bool CheckWalkable( - Plane polyPlane, - ReadOnlySpan polyVerts, - Vector3 sphereCenter, float sphereRadius, - Vector3 up, - bool small) + ResolvedPolygon poly, + CollisionSphere sphere, + Vector3 up, + bool small) { - float angleUp = Vector3.Dot(polyPlane.Normal, up); + float angleUp = Vector3.Dot(poly.Plane.Normal, up); if (angleUp < PhysicsGlobals.EPSILON) return false; - // Project sphere center onto the polygon plane along the up vector - float dpDist = (Vector3.Dot(polyPlane.Normal, sphereCenter) + polyPlane.D) / angleUp; - var center = sphereCenter - up * dpDist; + float angle = (Vector3.Dot(poly.Plane.Normal, sphere.Center) + poly.Plane.D) / angleUp; + var center = sphere.Center - up * angle; - float radsum = sphereRadius * sphereRadius; + float radsum = sphere.Radius * sphere.Radius; if (small) radsum *= 0.25f; - int numVerts = polyVerts.Length; - int prevIdx = numVerts - 1; - bool result = true; + int n = poly.Vertices.Length; + int prevIdx = n - 1; - for (int i = 0; i < numVerts; i++) + for (int i = 0; i < n; i++) { - var vertex = polyVerts[i]; - var lastVertex = polyVerts[prevIdx]; + var v = poly.Vertices[i]; + var lv = poly.Vertices[prevIdx]; prevIdx = i; - var edge = vertex - lastVertex; - var disp = center - lastVertex; - var cross = Vector3.Cross(polyPlane.Normal, edge); + var edge = v - lv; + var disp = center - lv; + var cross = Vector3.Cross(poly.Plane.Normal, edge); float diff = Vector3.Dot(disp, cross); if (diff < 0f) @@ -959,228 +309,1366 @@ public static class BSPQuery if (disp.LengthSquared() <= radsum) return true; } - - return result; + return true; } - // ======================================================================= - // LEGACY: Static overlap SphereIntersectsPoly (no movement vector) + // ------------------------------------------------------------------------- + // adjust_sphere_to_plane + // ACE: Polygon.cs adjust_sphere_to_plane + // ------------------------------------------------------------------------- + + /// + /// Polygon.adjust_sphere_to_plane — slide sphere to rest on polygon plane. + /// + /// + /// Computes the parametric distance along at which + /// the sphere first contacts the plane, moves the sphere to that position, + /// and updates . + /// Returns false if the contact is outside the interp window. + /// + /// + /// ACE: Polygon.cs adjust_sphere_to_plane. + /// + private static bool AdjustSphereToPlane( + ResolvedPolygon poly, + SpherePath path, + CollisionSphere validPos, + Vector3 movement) + { + float dpPos = Vector3.Dot(validPos.Center, poly.Plane.Normal) + poly.Plane.D; + float dpMove = Vector3.Dot(movement, poly.Plane.Normal); + float dist; + + if (dpMove <= PhysicsGlobals.EPSILON) + { + if (dpMove >= -PhysicsGlobals.EPSILON) + return false; + dist = dpPos - validPos.Radius; + } + else + { + dist = -validPos.Radius - dpPos; + } + + float iDist = dist / dpMove; + float interp = (1f - iDist) * path.WalkInterp; + + if (interp >= path.WalkInterp || interp < -0.5f) + return false; + + validPos.Center -= movement * iDist; + path.WalkInterp = interp; + return true; + } + + // ------------------------------------------------------------------------- + // find_crossed_edge + // ACE: Polygon.cs find_crossed_edge + // ------------------------------------------------------------------------- + + /// + /// Polygon.find_crossed_edge — find the edge the sphere center has crossed. + /// + /// + /// Projects sphere center onto polygon plane along the up vector and finds + /// the first edge for which the projected point is on the outside. + /// Returns the normalised outward edge normal via . + /// + /// + /// ACE: Polygon.cs find_crossed_edge. + /// + private static bool FindCrossedEdge( + ResolvedPolygon poly, + CollisionSphere sphere, + Vector3 up, + ref Vector3 normal) + { + float angleUp = Vector3.Dot(poly.Plane.Normal, up); + if (MathF.Abs(angleUp) < PhysicsGlobals.EPSILON) return false; + + float angle = (Vector3.Dot(poly.Plane.Normal, sphere.Center) + poly.Plane.D) / angleUp; + var center = sphere.Center - up * angle; + + int n = poly.Vertices.Length; + int prevIdx = n - 1; + + for (int i = 0; i < n; i++) + { + var v = poly.Vertices[i]; + var lv = poly.Vertices[prevIdx]; + prevIdx = i; + + var edge = v - lv; + var disp = center - lv; + var cross = Vector3.Cross(poly.Plane.Normal, edge); + + if (Vector3.Dot(disp, cross) < 0f) + { + float crossLen = cross.Length(); + normal = crossLen > 0f ? cross * (1f / crossLen) : Vector3.Zero; + return true; + } + } + return false; + } + + // ------------------------------------------------------------------------- + // adjust_to_placement_poly + // ACE: Polygon.cs adjust_to_placement_poly + // ------------------------------------------------------------------------- + + /// + /// Polygon.adjust_to_placement_poly — push sphere(s) out of a solid polygon. + /// + /// + /// If the center is solid the sphere is pushed along the polygon normal; + /// otherwise find_crossed_edge locates the nearest boundary edge and pushes + /// along that. The second sphere (if provided) is displaced by the same offset. + /// + /// + /// ACE: Polygon.cs adjust_to_placement_poly. + /// + private static void AdjustToPlacementPoly( + ResolvedPolygon poly, + CollisionSphere validPos, + CollisionSphere? validPos2, + float radius, + bool centerSolid, + bool clearCell) + { + Vector3 moveDir = Vector3.Zero; + + if (centerSolid) + { + moveDir = poly.Plane.Normal; + } + else + { + var up = Vector3.UnitZ; + if (!FindCrossedEdge(poly, validPos, up, ref moveDir)) + moveDir = poly.Plane.Normal; + } + + float dist = Vector3.Dot(validPos.Center, poly.Plane.Normal) + poly.Plane.D; + float pushAmt = radius - dist; + if (pushAmt <= 0f) pushAmt = PhysicsGlobals.EPSILON; + + var offset = moveDir * pushAmt; + validPos.Center += offset; + + if (validPos2 is not null) + validPos2.Center += offset; + } + + // ------------------------------------------------------------------------- + // adjust_sphere_to_poly + // ACE: Polygon.cs adjust_sphere_to_poly + // ------------------------------------------------------------------------- + + /// + /// Polygon.adjust_sphere_to_poly — compute parametric contact time. + /// + /// + /// Returns 1.0 if the sphere currently intersects the polygon (needs further + /// back-off), or the parametric time [0,1] of first contact along the movement + /// vector. Used by adjust_to_plane binary-search loop. + /// + /// + /// ACE: Polygon.cs adjust_sphere_to_poly. + /// + private static float AdjustSphereToPoly( + ResolvedPolygon poly, + CollisionSphere checkPos, + Vector3 curPos, + Vector3 movement) + { + Vector3 cp = Vector3.Zero; + if (PolygonHitsSpherePrecise( + poly.Plane, poly.Vertices, + checkPos.Center, checkPos.Radius, + ref cp)) + return 1f; + + float dpPos = Vector3.Dot(curPos, poly.Plane.Normal) + poly.Plane.D; + float dpMove = Vector3.Dot(movement, poly.Plane.Normal); + if (MathF.Abs(dpMove) < PhysicsGlobals.EPSILON) return 0f; + + float t = (-checkPos.Radius - dpPos) / dpMove; + return Math.Clamp(t, 0f, 1f); + } + + // ========================================================================= + // BSP TRAVERSAL METHODS // - // This is the original method used by FindObjCollisions in - // TransitionTypes.cs. It does NOT use movement-based front-face culling. - // Kept for backward compatibility with the existing object collision - // system until it is upgraded to use FindCollisions. - // ======================================================================= + // Ported from ACE BSPNode.cs (internal nodes) and BSPLeaf.cs (leaf nodes). + // DatReaderWriter uses a single PhysicsBSPNode type distinguished by + // Type == BSPNodeType.Leaf; we dispatch via if/else instead of virtual calls. + // ========================================================================= + + // ------------------------------------------------------------------------- + // sphere_intersects_poly + // ACE: BSPNode.sphere_intersects_poly (internal) + BSPLeaf.sphere_intersects_poly (leaf) + // ------------------------------------------------------------------------- /// - /// Test if a sphere intersects any polygon in the physics BSP tree. - /// Returns on the first hit, populating - /// and . + /// BSPNode.sphere_intersects_poly — tree traversal to find a polygon hit. /// /// - /// This is the static overlap test (no movement vector). For the - /// movement-aware version with front-face culling, use - /// . + /// Descends the BSP tree classifying the sphere against each splitting plane. + /// At leaves tests each polygon with pos_hits_sphere (front-face culled by + /// movement direction). Returns true on the first positive hit. /// /// /// - /// Ported from FUN_00539270; cross-ref ACE BSPNode.sphere_intersects_poly. + /// ACE: BSPNode.cs sphere_intersects_poly (internal), + /// BSPLeaf.cs sphere_intersects_poly (leaf). /// /// - public static bool SphereIntersectsPoly( - PhysicsBSPNode? node, - Dictionary polygons, - VertexArray vertices, - Vector3 sphereCenter, - float sphereRadius, - out ushort hitPolyId, - out Vector3 hitNormal) + private static bool SphereIntersectsPolyInternal( + PhysicsBSPNode? node, + Dictionary resolved, + CollisionSphere sphere, + Vector3 movement, + ref ResolvedPolygon? hitPoly, + ref Vector3 contactPoint) { - hitPolyId = 0; - hitNormal = Vector3.Zero; + if (node is null) return false; + if (!NodeIntersects(node, sphere)) return false; + // Leaf: test each polygon. + if (node.Type == BSPNodeType.Leaf) + { + if (node.Polygons.Count == 0) return false; + + foreach (ushort polyId in node.Polygons) + { + if (!resolved.TryGetValue(polyId, out var poly)) continue; + + if (PosHitsSphere(poly, sphere, movement, ref contactPoint, ref hitPoly)) + return true; + } + return false; + } + + // Internal: classify against splitting plane. + float dist = Vector3.Dot(node.SplittingPlane.Normal, sphere.Center) + + node.SplittingPlane.D; + float reach = sphere.Radius - PhysicsGlobals.EPSILON; + + if (dist >= reach) + return SphereIntersectsPolyInternal(node.PosNode, resolved, sphere, movement, + ref hitPoly, ref contactPoint); + + if (dist <= -reach) + return SphereIntersectsPolyInternal(node.NegNode, resolved, sphere, movement, + ref hitPoly, ref contactPoint); + + // Straddles: check both children. + if (node.PosNode is not null && + SphereIntersectsPolyInternal(node.PosNode, resolved, sphere, movement, + ref hitPoly, ref contactPoint)) + return true; + + if (node.NegNode is not null && + SphereIntersectsPolyInternal(node.NegNode, resolved, sphere, movement, + ref hitPoly, ref contactPoint)) + return true; + + return false; + } + + // ------------------------------------------------------------------------- + // find_walkable + // ACE: BSPNode.find_walkable (internal) + BSPLeaf.find_walkable (leaf) + // ------------------------------------------------------------------------- + + /// + /// BSPNode.find_walkable — traverse BSP to find walkable surfaces and adjust sphere. + /// + /// + /// At each leaf tests every polygon with walkable_hits_sphere then + /// adjust_sphere_to_plane. If both succeed the sphere is repositioned to rest + /// on the surface and is set true. + /// + /// + /// + /// Note: validPos is a reference type so mutations propagate back to the caller + /// automatically, mirroring ACE's by-ref Sphere passing. + /// + /// + /// + /// ACE: BSPNode.cs find_walkable (internal), BSPLeaf.cs find_walkable (leaf). + /// + /// + private static void FindWalkableInternal( + PhysicsBSPNode? node, + Dictionary resolved, + SpherePath path, + CollisionSphere validPos, + Vector3 movement, + Vector3 up, + ref ResolvedPolygon? hitPoly, + ref bool changed) + { + if (node is null) return; + if (!NodeIntersects(node, validPos)) return; + + // Leaf. + if (node.Type == BSPNodeType.Leaf) + { + if (node.Polygons.Count == 0) return; + + foreach (ushort polyId in node.Polygons) + { + if (!resolved.TryGetValue(polyId, out var poly)) continue; + + bool walkable = WalkableHitsSphere(poly, path, validPos, up); + bool adjusted = walkable && AdjustSphereToPlane(poly, path, validPos, movement); + + if (walkable && adjusted) + { + changed = true; + hitPoly = poly; + } + } + return; + } + + // Internal: classify against splitting plane. + float dist = Vector3.Dot(node.SplittingPlane.Normal, validPos.Center) + + node.SplittingPlane.D; + float reach = validPos.Radius - PhysicsGlobals.EPSILON; + + if (dist >= reach) + { + FindWalkableInternal(node.PosNode, resolved, path, validPos, movement, up, + ref hitPoly, ref changed); + return; + } + + if (dist <= -reach) + { + FindWalkableInternal(node.NegNode, resolved, path, validPos, movement, up, + ref hitPoly, ref changed); + return; + } + + // Straddles. + FindWalkableInternal(node.PosNode, resolved, path, validPos, movement, up, + ref hitPoly, ref changed); + FindWalkableInternal(node.NegNode, resolved, path, validPos, movement, up, + ref hitPoly, ref changed); + } + + // ------------------------------------------------------------------------- + // hits_walkable + // ACE: BSPNode.hits_walkable (internal) + BSPLeaf.hits_walkable (leaf) + // ------------------------------------------------------------------------- + + /// + /// BSPNode.hits_walkable — check if sphere touches any walkable polygon. + /// + /// + /// At leaves combines walkable_hits_sphere + check_small_walkable. + /// Used by the CheckWalkable dispatch path. + /// + /// + /// + /// ACE: BSPNode.cs hits_walkable (internal), BSPLeaf.cs hits_walkable (leaf). + /// + /// + private static bool HitsWalkableInternal( + PhysicsBSPNode? node, + Dictionary resolved, + SpherePath path, + CollisionSphere sphere, + Vector3 up) + { + if (node is null) return false; + if (!NodeIntersects(node, sphere)) return false; + + // Leaf. + if (node.Type == BSPNodeType.Leaf) + { + if (node.Polygons.Count == 0) return false; + + foreach (ushort polyId in node.Polygons) + { + if (!resolved.TryGetValue(polyId, out var poly)) continue; + + if (WalkableHitsSphere(poly, path, sphere, up) && + CheckWalkable(poly, sphere, up, small: true)) + return true; + } + return false; + } + + // Internal. + float dist = Vector3.Dot(node.SplittingPlane.Normal, sphere.Center) + + node.SplittingPlane.D; + float reach = sphere.Radius - PhysicsGlobals.EPSILON; + + if (dist >= reach) + return HitsWalkableInternal(node.PosNode, resolved, path, sphere, up); + + if (dist <= -reach) + return HitsWalkableInternal(node.NegNode, resolved, path, sphere, up); + + if (HitsWalkableInternal(node.PosNode, resolved, path, sphere, up)) return true; + return HitsWalkableInternal(node.NegNode, resolved, path, sphere, up); + } + + // ------------------------------------------------------------------------- + // sphere_intersects_solid + // ACE: BSPNode.sphere_intersects_solid (internal) + BSPLeaf.sphere_intersects_solid (leaf) + // ------------------------------------------------------------------------- + + /// + /// BSPNode.sphere_intersects_solid — check if sphere overlaps solid geometry. + /// + /// + /// At leaves: if centerCheck is true and the leaf is marked Solid, returns true + /// immediately. Otherwise tests each polygon with hits_sphere. + /// + /// + /// + /// The centerCheck flag tracks which side of each splitting plane the sphere + /// center is on; propagated faithfully per ACE's logic. + /// + /// + /// + /// ACE: BSPNode.cs sphere_intersects_solid (internal), + /// BSPLeaf.cs sphere_intersects_solid (leaf). + /// + /// + private static bool SphereIntersectsSolidInternal( + PhysicsBSPNode? node, + Dictionary resolved, + CollisionSphere sphere, + bool centerCheck) + { if (node is null) return false; - // Broad phase: bounding sphere rejection + // Leaf. + if (node.Type == BSPNodeType.Leaf) { - float dist = Vector3.Distance(sphereCenter, node.BoundingSphere.Origin); - if (dist > sphereRadius + node.BoundingSphere.Radius + CollisionPrimitives.Epsilon) + if (node.Polygons.Count == 0) return false; + if (centerCheck && node.Solid != 0) return true; + if (!NodeIntersects(node, sphere)) return false; + + foreach (ushort polyId in node.Polygons) + { + if (!resolved.TryGetValue(polyId, out var poly)) continue; + if (HitsSphere(poly, sphere)) return true; + } + return false; + } + + if (!NodeIntersects(node, sphere)) return false; + + // Internal. + float dist = Vector3.Dot(node.SplittingPlane.Normal, sphere.Center) + + node.SplittingPlane.D; + float reach = sphere.Radius - PhysicsGlobals.EPSILON; + + if (dist >= reach) + return SphereIntersectsSolidInternal(node.PosNode, resolved, sphere, centerCheck); + + if (dist <= -reach) + return SphereIntersectsSolidInternal(node.NegNode, resolved, sphere, centerCheck); + + // Straddles: propagate centerCheck to the side the center is on. + if (dist < 0f) + { + if (SphereIntersectsSolidInternal(node.PosNode, resolved, sphere, false)) + return true; + return SphereIntersectsSolidInternal(node.NegNode, resolved, sphere, centerCheck); + } + else + { + if (SphereIntersectsSolidInternal(node.PosNode, resolved, sphere, centerCheck)) + return true; + return SphereIntersectsSolidInternal(node.NegNode, resolved, sphere, false); + } + } + + // ------------------------------------------------------------------------- + // sphere_intersects_solid_poly + // ACE: BSPNode.sphere_intersects_solid_poly + BSPLeaf.sphere_intersects_solid_poly + // ------------------------------------------------------------------------- + + /// + /// BSPNode.sphere_intersects_solid_poly — find solid polygon for placement. + /// + /// + /// Like sphere_intersects_solid but additionally records the specific polygon + /// hit so adjust_to_placement_poly can push the sphere out of it. + /// + /// + /// + /// ACE: BSPNode.cs sphere_intersects_solid_poly (internal), + /// BSPLeaf.cs sphere_intersects_solid_poly (leaf). + /// + /// + private static bool SphereIntersectsSolidPolyInternal( + PhysicsBSPNode? node, + Dictionary resolved, + CollisionSphere sphere, + float radius, + ref bool centerSolid, + ref ResolvedPolygon? hitPoly, + bool centerCheck) + { + if (node is null) return centerSolid; + + // Leaf. + if (node.Type == BSPNodeType.Leaf) + { + if (node.Polygons.Count == 0) return false; + + if (centerCheck && node.Solid != 0) + centerSolid = true; + + if (!NodeIntersects(node, sphere)) + return centerSolid; + + foreach (ushort polyId in node.Polygons) + { + if (!resolved.TryGetValue(polyId, out var poly)) continue; + if (HitsSphere(poly, sphere)) + { + hitPoly = poly; + return true; + } + } + return centerSolid; + } + + if (!NodeIntersects(node, sphere)) return centerSolid; + + // Internal. + float dist = Vector3.Dot(node.SplittingPlane.Normal, sphere.Center) + + node.SplittingPlane.D; + float reach = radius - PhysicsGlobals.EPSILON; + + if (dist >= reach) + return SphereIntersectsSolidPolyInternal(node.PosNode, resolved, sphere, radius, + ref centerSolid, ref hitPoly, centerCheck); + + if (dist <= -reach) + return SphereIntersectsSolidPolyInternal(node.NegNode, resolved, sphere, radius, + ref centerSolid, ref hitPoly, centerCheck); + + // Straddles. + if (dist <= 0f) + { + SphereIntersectsSolidPolyInternal(node.NegNode, resolved, sphere, radius, + ref centerSolid, ref hitPoly, centerCheck); + if (hitPoly is not null) return centerSolid; + return SphereIntersectsSolidPolyInternal(node.PosNode, resolved, sphere, radius, + ref centerSolid, ref hitPoly, false); + } + else + { + SphereIntersectsSolidPolyInternal(node.PosNode, resolved, sphere, radius, + ref centerSolid, ref hitPoly, centerCheck); + if (hitPoly is not null) return centerSolid; + return SphereIntersectsSolidPolyInternal(node.NegNode, resolved, sphere, radius, + ref centerSolid, ref hitPoly, false); + } + } + + // ========================================================================= + // PUBLIC: point_inside_cell_bsp + // ACE: BSPNode.point_inside_cell_bsp + // ========================================================================= + + /// + /// BSPNode.point_inside_cell_bsp — test if a 3D point is inside the cell BSP. + /// + /// + /// Follows the front side of each splitting plane. A point is inside when it + /// reaches a front leaf or null PosNode (solid interior). + /// + /// + /// ACE: BSPNode.cs point_inside_cell_bsp. + /// + public static bool PointInsideCellBsp(PhysicsBSPNode? node, Vector3 point) + { + if (node is null) return true; + if (node.Type == BSPNodeType.Leaf) return true; + + float dist = Vector3.Dot(node.SplittingPlane.Normal, point) + node.SplittingPlane.D; + + // Front or on-plane → follow positive child (inside). + if (dist >= 0f) + return node.PosNode is not null ? PointInsideCellBsp(node.PosNode, point) : true; + + // Behind → outside. + return false; + } + + // ========================================================================= + // BSP TREE-LEVEL HELPERS + // + // Ported from ACE BSPTree.cs. Wrap the recursive traversals with + // transition-level logic. + // ========================================================================= + + // ------------------------------------------------------------------------- + // adjust_to_plane — BSPTree method + // ACE: BSPTree.cs adjust_to_plane + // ------------------------------------------------------------------------- + + /// + /// BSPTree.adjust_to_plane — binary-search for non-penetrating sphere position. + /// + /// + /// Runs up to 15 forward iterations until touching, then up to 15 binary-search + /// iterations to narrow the touch point. Modifies checkPos.Center in place. + /// Returns false if convergence fails. + /// + /// + /// ACE: BSPTree.cs adjust_to_plane. + /// + private static bool AdjustToPlane( + PhysicsBSPNode root, + Dictionary resolved, + CollisionSphere checkPos, + Vector3 curPos, + ResolvedPolygon hitPoly, + Vector3 contactPoint) + { + var movement = checkPos.Center - curPos; + + double lowerTime = 0.0; + double upperTime = 1.0; + + const int MaxIter = 15; + + // Phase 1: step forward until non-intersecting. + for (int i = 0; i < MaxIter; i++) + { + float touchTime = AdjustSphereToPoly(hitPoly, checkPos, curPos, movement); + + if (touchTime == 1f) + { + checkPos.Center = curPos + movement * (float)touchTime; + + ResolvedPolygon? hp2 = null; + Vector3 cp2 = Vector3.Zero; + if (!SphereIntersectsPolyInternal(root, resolved, checkPos, movement, + ref hp2, ref cp2)) + { + lowerTime = touchTime; + break; + } + upperTime = touchTime; + } + + if (i == MaxIter - 1) return false; + } + + // Phase 2: binary-search. + for (int j = 0; j < MaxIter; j++) + { + double average = (lowerTime + upperTime) * 0.5; + checkPos.Center = curPos + movement * (float)average; + + ResolvedPolygon? hp2 = null; + Vector3 cp2 = Vector3.Zero; + + if (!SphereIntersectsPolyInternal(root, resolved, checkPos, movement, + ref hp2, ref cp2)) + upperTime = (lowerTime + upperTime) * 0.5; + else + lowerTime = (lowerTime + upperTime) * 0.5; + + if (upperTime - lowerTime < 0.02) return false; } - // Leaf node: test each polygon + return true; + } + + // ------------------------------------------------------------------------- + // check_walkable — BSPTree level + // ACE: BSPTree.cs check_walkable + // ------------------------------------------------------------------------- + + /// + /// BSPTree.check_walkable — CheckWalkable dispatch path entry point. + /// Returns Collided if the sphere is on a walkable surface, OK otherwise. + /// ACE: BSPTree.cs check_walkable. + /// + private static TransitionState CheckWalkableDispatch( + PhysicsBSPNode root, + Dictionary resolved, + SpherePath path, + CollisionSphere checkPos, + Vector3 up) + { + var validPos = new CollisionSphere(checkPos); + return HitsWalkableInternal(root, resolved, path, validPos, up) + ? TransitionState.Collided + : TransitionState.OK; + } + + // ------------------------------------------------------------------------- + // step_sphere_down — BSPTree level + // ACE: BSPTree.cs step_sphere_down + // ------------------------------------------------------------------------- + + /// + /// BSPTree.step_sphere_down — probe downward to land on a walkable surface. + /// + /// + /// Computes a downward movement from StepDownAmt × WalkInterp, runs + /// find_walkable to locate a surface, updates CheckPos and CollisionInfo + /// contact plane if one is found. + /// + /// + /// ACE: BSPTree.cs step_sphere_down. + /// + private static TransitionState StepSphereDown( + PhysicsBSPNode root, + Dictionary resolved, + Transition transition, + CollisionSphere checkPos, + Vector3 up, + float scale) + { + var path = transition.SpherePath; + var collisions = transition.CollisionInfo; + + float stepDownAmount = -(path.StepDownAmt * path.WalkInterp); + var movement = up * stepDownAmount * (1f / scale); + + var validPos = new CollisionSphere(checkPos); + bool changed = false; + ResolvedPolygon? polyHit = null; + + FindWalkableInternal(root, resolved, path, validPos, movement, up, + ref polyHit, ref changed); + + if (changed && polyHit is not null) + { + var adjusted = validPos.Center - checkPos.Center; + var offset = adjusted * scale; + path.AddOffsetToCheckPos(offset); + + collisions.SetContactPlane( + new Plane(polyHit.Plane.Normal, polyHit.Plane.D * scale), + path.CheckCellId, false); + + path.WalkableValid = true; + path.WalkablePlane = new Plane(polyHit.Plane.Normal, polyHit.Plane.D * scale); + path.WalkableAllowance = PhysicsGlobals.FloorZ; + + return TransitionState.Adjusted; + } + + return TransitionState.OK; + } + + // ------------------------------------------------------------------------- + // step_sphere_up — BSPTree level + // ACE: BSPTree.cs step_sphere_up + // ------------------------------------------------------------------------- + + /// + /// BSPTree.step_sphere_up — attempt to step over a low obstacle. + /// + /// + /// Sets the StepUp flag on SpherePath with the collision normal. + /// The Transition's outer loop will pick this up and attempt the step. + /// If StepUp is already pending, falls back to setting the collision normal + /// directly (StepUpSlide equivalent). + /// + /// + /// ACE: BSPTree.cs step_sphere_up. + /// + private static TransitionState StepSphereUp( + Transition transition, + Vector3 collisionNormal) + { + var path = transition.SpherePath; + var ci = transition.CollisionInfo; + + // ACE calls transition.StepUp(globNormal); if false -> path.StepUpSlide(transition). + // In acdream, StepUp is a flag field on SpherePath. + // If no StepUp is pending yet, request one. + if (!path.StepUp) + { + path.StepUp = true; + path.StepUpNormal = collisionNormal; + return TransitionState.OK; + } + + // StepUpSlide: can't step up, set collision normal and report adjusted. + ci.SetCollisionNormal(collisionNormal); + return TransitionState.Adjusted; + } + + // ------------------------------------------------------------------------- + // slide_sphere — BSPTree level + // ACE: BSPTree.cs slide_sphere + // ------------------------------------------------------------------------- + + /// + /// BSPTree.slide_sphere — apply sliding collision response. + /// + /// + /// Sets the sliding normal on CollisionInfo so the outer transition loop + /// applies a wall-slide projection. + /// + /// + /// ACE: BSPTree.cs slide_sphere — calls GlobalSphere[0].SlideSphere. + /// + private static TransitionState SlideSphere( + Transition transition, + Vector3 collisionNormal) + { + transition.CollisionInfo.SetSlidingNormal(collisionNormal); + return TransitionState.Slid; + } + + // ------------------------------------------------------------------------- + // collide_with_pt — BSPTree level + // ACE: BSPTree.cs collide_with_pt + // ------------------------------------------------------------------------- + + /// + /// BSPTree.collide_with_pt — PerfectClip collision with binary-search adjustment. + /// + /// + /// When ObjectInfo.State has PerfectClip, runs adjust_to_plane to find the + /// exact non-penetrating position, applies the resulting offset to CheckPos, + /// and returns Adjusted. Without PerfectClip just sets the collision normal. + /// + /// + /// ACE: BSPTree.cs collide_with_pt. + /// + private static TransitionState CollideWithPt( + PhysicsBSPNode root, + Dictionary resolved, + Transition transition, + CollisionSphere checkPos, + Vector3 curPos, + ResolvedPolygon hitPoly, + Vector3 contactPoint, + float scale) + { + var obj = transition.ObjectInfo; + var path = transition.SpherePath; + var collisions = transition.CollisionInfo; + + var collisionNormal = hitPoly.Plane.Normal; + + if (!obj.State.HasFlag(ObjectInfoState.PerfectClip)) + { + collisions.SetCollisionNormal(collisionNormal); + return TransitionState.Collided; + } + + var validPos = new CollisionSphere(checkPos); + + if (!AdjustToPlane(root, resolved, validPos, curPos, hitPoly, contactPoint)) + return TransitionState.Collided; + + collisions.SetCollisionNormal(collisionNormal); + + var adjusted = validPos.Center - checkPos.Center; + var offset = adjusted * scale; + path.AddOffsetToCheckPos(offset); + + return TransitionState.Adjusted; + } + + // ------------------------------------------------------------------------- + // NegPolyHit — BSPTree level + // ACE: BSPTree.cs NegPolyHit + // ------------------------------------------------------------------------- + + /// + /// BSPTree.NegPolyHit — record a negative-polygon hit for deferred processing. + /// ACE: BSPTree.cs NegPolyHit — calls path.SetNegPolyHit(stepUp, collisionNormal). + /// + private static TransitionState NegPolyHitDispatch( + SpherePath path, + ResolvedPolygon hitPoly, + bool stepUp) + { + path.NegPolyHit = true; + path.NegStepUp = stepUp; + path.NegCollisionNormal = hitPoly.Plane.Normal; + return TransitionState.OK; + } + + // ------------------------------------------------------------------------- + // placement_insert — BSPTree level + // ACE: BSPTree.cs placement_insert + // ------------------------------------------------------------------------- + + /// + /// BSPTree.placement_insert — iterative placement that pushes sphere(s) out of solids. + /// + /// + /// Up to 20 iterations of sphere_intersects_solid_poly. When a penetrating + /// polygon is found, adjust_to_placement_poly pushes the sphere(s) out. + /// When fitting without penetration, accumulated offset is applied to CheckPos. + /// + /// + /// ACE: BSPTree.cs placement_insert. + /// + private static TransitionState PlacementInsert( + PhysicsBSPNode root, + Dictionary resolved, + Transition transition, + bool clearCell) + { + var path = transition.SpherePath; + + var s0 = new CollisionSphere( + path.LocalSphere[0].Origin, + path.LocalSphere[0].Radius); + + float rad = s0.Radius; + + CollisionSphere? s1 = null; + if (path.NumSphere > 1) + s1 = new CollisionSphere( + path.LocalSphere[1].Origin, + path.LocalSphere[1].Radius); + + ResolvedPolygon? hitPoly = null; + + const int MaxIter = 20; + for (int i = 0; i < MaxIter; i++) + { + bool centerSolid = false; + hitPoly = null; + + if (SphereIntersectsSolidPolyInternal(root, resolved, s0, rad, + ref centerSolid, ref hitPoly, clearCell)) + { + if (hitPoly is not null) + { + AdjustToPlacementPoly(hitPoly, s0, s1, rad, centerSolid, clearCell); + continue; + } + } + else + { + if (path.NumSphere >= 2 && s1 is not null) + { + centerSolid = false; + hitPoly = null; + + if (SphereIntersectsSolidPolyInternal(root, resolved, s1, rad, + ref centerSolid, ref hitPoly, clearCell)) + { + if (hitPoly is not null) + { + AdjustToPlacementPoly(hitPoly, s1, s0, rad, centerSolid, clearCell); + continue; + } + } + else + { + return PlacementInsertInner(s0, path, i); + } + } + else + { + return PlacementInsertInner(s0, path, i); + } + } + + rad *= 2f; + } + return TransitionState.Collided; + } + + private static TransitionState PlacementInsertInner( + CollisionSphere s0, + SpherePath path, + int iteration) + { + if (iteration == 0) return TransitionState.OK; + + var adjust = s0.Center - path.LocalSphere[0].Origin; + path.AddOffsetToCheckPos(adjust); + return TransitionState.Adjusted; + } + + // ========================================================================= + // PUBLIC ENTRY POINT: FindCollisions + // + // ACE: BSPTree.cs find_collisions — the 6-path dispatcher. + // ========================================================================= + + /// + /// BSPTree.find_collisions — the 6-path BSP collision dispatcher. + /// + /// + /// Dispatches to one of six collision paths depending on SpherePath flags: + /// + /// Placement / Ethereal → sphere_intersects_solid + /// CheckWalkable → hits_walkable + /// StepDown → step_sphere_down (find_walkable) + /// Collide → find_walkable (land on surface) + /// Contact → sphere_intersects_poly + step_sphere_up / slide + /// Default → sphere_intersects_poly + collide_with_pt / land + /// + /// + /// + /// + /// ACE: BSPTree.cs find_collisions. + /// Decompiled: chunk_00530000.c. + /// + /// + /// Root of the physics BSP tree. + /// Pre-resolved polygon dictionary (vertices + plane). + /// Current transition state. + /// Primary sphere in object-local space (index 0). + /// Second sphere in object-local space (head), or null. + /// Previous center of the primary sphere in local space. + /// Up vector in object-local space (usually Vector3.UnitZ). + /// Scale factor for the collision object. + public static TransitionState FindCollisions( + PhysicsBSPNode? root, + Dictionary resolved, + Transition transition, + DatReaderWriter.Types.Sphere localSphere, + DatReaderWriter.Types.Sphere? localSphere1, + Vector3 localCurrCenter, + Vector3 localSpaceZ, + float scale) + { + if (root is null) return TransitionState.OK; + + var path = transition.SpherePath; + var collisions = transition.CollisionInfo; + var obj = transition.ObjectInfo; + + var sphere0 = new CollisionSphere(localSphere.Origin, localSphere.Radius); + CollisionSphere? sphere1 = localSphere1 is not null + ? new CollisionSphere(localSphere1.Origin, localSphere1.Radius) + : null; + + var movement = sphere0.Center - localCurrCenter; + + // ---------------------------------------------------------------- + // Path 1: Placement or Ethereal → sphere_intersects_solid + // ACE: if (path.InsertType == InsertType.Placement || path.ObstructionEthereal) + // ---------------------------------------------------------------- + if (path.InsertType == InsertType.Placement || obj.Ethereal) + { + // clearCell=true unless BuildingCheck && HitsInteriorCell (not exposed here). + const bool clearCell = true; + + if (SphereIntersectsSolidInternal(root, resolved, sphere0, clearCell)) + return TransitionState.Collided; + + if (sphere1 is not null && + SphereIntersectsSolidInternal(root, resolved, sphere1, clearCell)) + return TransitionState.Collided; + + return TransitionState.OK; + } + + // ---------------------------------------------------------------- + // Path 2: CheckWalkable → hits_walkable + // ACE: if (path.CheckWalkable) return check_walkable(path, localSphere, scale); + // ---------------------------------------------------------------- + if (path.CheckWalkable) + { + return CheckWalkableDispatch(root, resolved, path, sphere0, localSpaceZ); + } + + // ---------------------------------------------------------------- + // Path 3: StepDown → step_sphere_down + // ACE: if (path.StepDown) return step_sphere_down(transition, localSphere, scale); + // ---------------------------------------------------------------- + if (path.StepDown) + { + return StepSphereDown(root, resolved, transition, sphere0, localSpaceZ, scale); + } + + // ---------------------------------------------------------------- + // Path 4: Collide → find_walkable (land on surface) + // ACE: RootNode.find_walkable(path, validPos, ref hitPoly, movement, Z, ref changed) + // ---------------------------------------------------------------- + if (path.Collide) + { + var validPos = new CollisionSphere(sphere0); + ResolvedPolygon? hitPoly = null; + bool changed = false; + + FindWalkableInternal(root, resolved, path, validPos, movement, localSpaceZ, + ref hitPoly, ref changed); + + if (changed && hitPoly is not null) + { + var offset = (validPos.Center - sphere0.Center) * scale; + path.AddOffsetToCheckPos(offset); + + collisions.SetContactPlane( + new Plane(hitPoly.Plane.Normal, hitPoly.Plane.D * scale), + path.CheckCellId, false); + + path.WalkableValid = true; + path.WalkablePlane = new Plane(hitPoly.Plane.Normal, hitPoly.Plane.D * scale); + path.WalkableAllowance = PhysicsGlobals.FloorZ; + + return TransitionState.Adjusted; + } + return TransitionState.OK; + } + + // ---------------------------------------------------------------- + // Path 5: Contact — sphere_intersects_poly + step_sphere_up / slide + // ACE: if (obj.State.HasFlag(ObjectInfoState.Contact)) + // ---------------------------------------------------------------- + if (obj.State.HasFlag(ObjectInfoState.Contact)) + { + ResolvedPolygon? hitPoly0 = null; + Vector3 contact0 = Vector3.Zero; + + if (SphereIntersectsPolyInternal(root, resolved, sphere0, movement, + ref hitPoly0, ref contact0)) + { + return StepSphereUp(transition, hitPoly0!.Plane.Normal); + } + + if (sphere1 is not null) + { + ResolvedPolygon? hitPoly1 = null; + Vector3 contact1 = Vector3.Zero; + + if (SphereIntersectsPolyInternal(root, resolved, sphere1, movement, + ref hitPoly1, ref contact1)) + { + return SlideSphere(transition, hitPoly1!.Plane.Normal); + } + + // ACE checks hitPoly1 != null and hitPoly0 != null after the calls + // (the traversal may record a hitPoly even when pos_hits_sphere + // returned false, because the movement dot filtered it out). + if (hitPoly1 is not null) + return NegPolyHitDispatch(path, hitPoly1, false); + if (hitPoly0 is not null) + return NegPolyHitDispatch(path, hitPoly0, true); + } + + return TransitionState.OK; + } + + // ---------------------------------------------------------------- + // Path 6: Default — sphere_intersects_poly → collide_with_pt / land + // ACE: RootNode.sphere_intersects_poly(localSphere, movement, ref hitPoly, ref cp) || hitPoly != null + // ---------------------------------------------------------------- + { + ResolvedPolygon? hitPoly0 = null; + Vector3 contact0 = Vector3.Zero; + + bool hit0 = SphereIntersectsPolyInternal(root, resolved, sphere0, movement, + ref hitPoly0, ref contact0); + + if (hit0 || hitPoly0 is not null) + { + if (obj.State.HasFlag(ObjectInfoState.PathClipped)) + { + return CollideWithPt(root, resolved, transition, + sphere0, localCurrCenter, + hitPoly0!, contact0, scale); + } + + var normal = hitPoly0!.Plane.Normal; + path.WalkableAllowance = PhysicsGlobals.LandingZ; + // ACE: path.SetCollide(collisionNormal) — sets Collide=true + stores normal. + path.Collide = true; + collisions.SetCollisionNormal(normal); + return TransitionState.Adjusted; + } + + if (sphere1 is not null) + { + ResolvedPolygon? hitPoly1 = null; + Vector3 contact1 = Vector3.Zero; + + bool hit1 = SphereIntersectsPolyInternal(root, resolved, sphere1, movement, + ref hitPoly1, ref contact1); + + if (hit1 || hitPoly1 is not null) + { + collisions.SetCollisionNormal(hitPoly1!.Plane.Normal); + return TransitionState.Collided; + } + } + } + + return TransitionState.OK; + } + + // ========================================================================= + // LEGACY OVERLOADS + // + // The existing call sites in TransitionTypes.cs and BSPQueryTests pass + // raw (Polygon dictionary + VertexArray). These thin wrappers preserve + // that interface by building a resolved dictionary on the fly. + // + // Performance note: the resolved dictionary is rebuilt on every call. + // Callers that have pre-resolved data from PhysicsDataCache.Resolved should + // use the primary overloads directly to avoid this overhead. + // ========================================================================= + + // ------------------------------------------------------------------------- + // Legacy FindCollisions — wraps the new resolved-polygon version. + // Used by TransitionTypes.cs FindEnvCollisions. + // ------------------------------------------------------------------------- + + /// + /// Legacy overload: BSPTree.find_collisions accepting raw Polygon + VertexArray. + /// Prefer calling + /// with pre-resolved data when available. + /// + public static TransitionState FindCollisions( + PhysicsBSPNode? root, + Dictionary polygons, + DatReaderWriter.Types.VertexArray vertices, + Transition transition, + DatReaderWriter.Types.Sphere localSphere, + DatReaderWriter.Types.Sphere? localSphere1, + Vector3 localCurrCenter, + Vector3 localSpaceZ, + float scale) + { + var resolved = BuildResolved(polygons, vertices); + return FindCollisions(root, resolved, transition, + localSphere, localSphere1, localCurrCenter, localSpaceZ, scale); + } + + // ------------------------------------------------------------------------- + // Legacy SphereIntersectsPoly — static overlap, no movement culling. + // Used by BSPQueryTests and the legacy FindObjCollisions path. + // ------------------------------------------------------------------------- + + /// + /// Static sphere-BSP overlap test with no movement-direction culling. + /// + /// + /// This is the original method used by unit tests and the legacy object-collision + /// path. It does NOT use pos_hits_sphere movement culling — any polygon whose + /// plane is within sphere radius returns true regardless of movement direction. + /// For the retail-faithful movement-aware version use . + /// + /// + public static bool SphereIntersectsPoly( + PhysicsBSPNode? node, + Dictionary polygons, + DatReaderWriter.Types.VertexArray vertices, + Vector3 sphereCenter, + float sphereRadius, + out ushort hitPolyId, + out Vector3 hitNormal) + { + hitPolyId = 0; + hitNormal = Vector3.Zero; + if (node is null) return false; + + var resolved = BuildResolved(polygons, vertices); + return SphereIntersectsPolyStaticRecurse(node, resolved, sphereCenter, sphereRadius, + ref hitPolyId, ref hitNormal); + } + + private static bool SphereIntersectsPolyStaticRecurse( + PhysicsBSPNode? node, + Dictionary resolved, + Vector3 center, + float radius, + ref ushort hitPolyId, + ref Vector3 hitNormal) + { + if (node is null) return false; + + // Broad phase. + var bs = node.BoundingSphere; + var d = center - bs.Origin; + float r = radius + bs.Radius; + if (d.LengthSquared() >= r * r) return false; + if (node.Type == BSPNodeType.Leaf) { - foreach (var polyIdx in node.Polygons) + foreach (ushort polyId in node.Polygons) { - if (!polygons.TryGetValue(polyIdx, out var poly)) continue; - if (!TryGetPolyPlane(poly, vertices, out var polyPlane, out var polyVerts)) - continue; + if (!resolved.TryGetValue(polyId, out var poly)) continue; - if (CollisionPrimitives.SphereIntersectsPoly( - polyPlane, polyVerts, sphereCenter, sphereRadius, out _)) + Vector3 cp = Vector3.Zero; + if (PolygonHitsSpherePrecise(poly.Plane, poly.Vertices, + center, radius, ref cp)) { - hitPolyId = polyIdx; - hitNormal = polyPlane.Normal; + hitPolyId = polyId; + hitNormal = poly.Plane.Normal; return true; } } return false; } - // Internal node: classify against splitting plane - float splitDist = Vector3.Dot(node.SplittingPlane.Normal, sphereCenter) - + node.SplittingPlane.D; - float reach = sphereRadius - CollisionPrimitives.Epsilon; + float splitDist = Vector3.Dot(node.SplittingPlane.Normal, center) + node.SplittingPlane.D; + float reach = radius - PhysicsGlobals.EPSILON; if (splitDist >= reach) - { - return SphereIntersectsPoly( - node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, - out hitPolyId, out hitNormal); - } + return SphereIntersectsPolyStaticRecurse(node.PosNode, resolved, + center, radius, ref hitPolyId, ref hitNormal); if (splitDist <= -reach) - { - return SphereIntersectsPoly( - node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, - out hitPolyId, out hitNormal); - } + return SphereIntersectsPolyStaticRecurse(node.NegNode, resolved, + center, radius, ref hitPolyId, ref hitNormal); - // Straddles - if (SphereIntersectsPoly(node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, out hitPolyId, out hitNormal)) + if (SphereIntersectsPolyStaticRecurse(node.PosNode, resolved, + center, radius, ref hitPolyId, ref hitNormal)) return true; - return SphereIntersectsPoly(node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, out hitPolyId, out hitNormal); + return SphereIntersectsPolyStaticRecurse(node.NegNode, resolved, + center, radius, ref hitPolyId, ref hitNormal); } - /// - /// Movement-aware sphere-BSP intersection with front-face culling. - /// Only detects collisions against polygon faces the sphere is moving TOWARD - /// (dot(movement, normal) < 0). This prevents detecting collisions after - /// the sphere has already penetrated past a face. - /// - /// Matches retail Polygon.pos_hits_sphere behavior. - /// - public static bool SphereIntersectsPoly( - PhysicsBSPNode? node, - Dictionary polygons, - VertexArray vertices, - Vector3 sphereCenter, - float sphereRadius, - Vector3 movement, - out ushort hitPolyId, - out Vector3 hitNormal) - { - hitPolyId = 0; - hitNormal = Vector3.Zero; - - if (node is null) return false; - - // Broad phase: bounding sphere - { - float dist = Vector3.Distance(sphereCenter, node.BoundingSphere.Origin); - if (dist > sphereRadius + node.BoundingSphere.Radius + movement.Length() + 0.1f) - return false; - } - - if (node.Type == BSPNodeType.Leaf) - { - foreach (var polyIdx in node.Polygons) - { - if (!polygons.TryGetValue(polyIdx, out var poly)) continue; - if (!TryGetPolyPlane(poly, vertices, out var polyPlane, out var polyVerts)) - continue; - - // Front-face culling: only collide if moving TOWARD this face. - // This is the critical retail behavior from Polygon.pos_hits_sphere. - if (Vector3.Dot(movement, polyPlane.Normal) >= 0f) - continue; // moving away from or parallel to face - - if (CollisionPrimitives.SphereIntersectsPoly( - polyPlane, polyVerts, sphereCenter, sphereRadius, out _)) - { - hitPolyId = polyIdx; - hitNormal = polyPlane.Normal; - return true; - } - - // Also check at the end-of-movement position. - Vector3 endCenter = sphereCenter + movement; - if (CollisionPrimitives.SphereIntersectsPoly( - polyPlane, polyVerts, endCenter, sphereRadius, out _)) - { - hitPolyId = polyIdx; - hitNormal = polyPlane.Normal; - return true; - } - } - return false; - } - - // Internal node: same split logic - float splitDist = Vector3.Dot(node.SplittingPlane.Normal, sphereCenter) - + node.SplittingPlane.D; - float reach = sphereRadius + movement.Length(); - - if (splitDist >= reach) - return SphereIntersectsPoly(node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, movement, out hitPolyId, out hitNormal); - - if (splitDist <= -reach) - return SphereIntersectsPoly(node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, movement, out hitPolyId, out hitNormal); - - if (SphereIntersectsPoly(node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, movement, out hitPolyId, out hitNormal)) - return true; - - return SphereIntersectsPoly(node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, movement, out hitPolyId, out hitNormal); - } - - // ----------------------------------------------------------------------- - // 14. SphereIntersectsPolyWithTime — swept-sphere BSP query using - // FindTimeOfCollision for exact parametric contact time. - // Fix 4: replaces static overlap + ad-hoc t computation. - // ----------------------------------------------------------------------- + // ------------------------------------------------------------------------- + // Legacy SphereIntersectsPolyWithTime — swept-sphere BSP query. + // Used by FindObjCollisions in TransitionTypes.cs. + // ------------------------------------------------------------------------- /// - /// Movement-aware sphere-BSP intersection that uses - /// to compute the - /// exact parametric time of first contact. Returns the earliest collision - /// across all polygons in the BSP tree. + /// Movement-aware sphere-BSP intersection that returns the earliest hit time. /// /// - /// Unlike which - /// tests static overlap at start and end positions, this method finds the - /// precise contact time via swept-sphere analysis. + /// Uses front-face culling (dot(movement, normal) < 0) and tests at both + /// start and end positions to find the parametric time t ∈ [0,1] of first + /// contact. Returns true if any polygon was hit. /// /// public static bool SphereIntersectsPolyWithTime( - PhysicsBSPNode? node, - Dictionary polygons, - VertexArray vertices, - Vector3 sphereCenter, - float sphereRadius, - Vector3 movement, - out ushort hitPolyId, - out Vector3 hitNormal, - out float hitTime) + PhysicsBSPNode? node, + Dictionary polygons, + DatReaderWriter.Types.VertexArray vertices, + Vector3 sphereCenter, + float sphereRadius, + Vector3 movement, + out ushort hitPolyId, + out Vector3 hitNormal, + out float hitTime) { hitPolyId = 0; hitNormal = Vector3.Zero; - hitTime = float.MaxValue; - + hitTime = float.MaxValue; if (node is null) return false; - SphereIntersectsPolyWithTimeRecurse( - node, polygons, vertices, + var resolved = BuildResolved(polygons, vertices); + + SphereIntersectsPolyWithTimeRecurse(node, resolved, sphereCenter, sphereRadius, movement, ref hitPolyId, ref hitNormal, ref hitTime); @@ -1188,118 +1676,133 @@ public static class BSPQuery } private static void SphereIntersectsPolyWithTimeRecurse( - PhysicsBSPNode? node, - Dictionary polygons, - VertexArray vertices, - Vector3 sphereCenter, - float sphereRadius, - Vector3 movement, - ref ushort hitPolyId, - ref Vector3 hitNormal, - ref float bestTime) + PhysicsBSPNode? node, + Dictionary resolved, + Vector3 center, + float radius, + Vector3 movement, + ref ushort hitPolyId, + ref Vector3 hitNormal, + ref float bestTime) { if (node is null) return; - // Broad phase: bounding sphere + movement extent - float dist = Vector3.Distance(sphereCenter, node.BoundingSphere.Origin); - if (dist > sphereRadius + node.BoundingSphere.Radius + movement.Length() + 0.1f) - return; + // Broad phase. + var bs = node.BoundingSphere; + var d = center - bs.Origin; + float r = radius + bs.Radius + movement.Length() + 0.1f; + if (d.LengthSquared() >= r * r) return; - // Leaf node: test each polygon with FindTimeOfCollision if (node.Type == BSPNodeType.Leaf) { - foreach (var polyIdx in node.Polygons) + foreach (ushort polyId in node.Polygons) { - if (!polygons.TryGetValue(polyIdx, out var poly)) continue; - if (!TryGetPolyPlane(poly, vertices, out var polyPlane, out var polyVerts)) - continue; + if (!resolved.TryGetValue(polyId, out var poly)) continue; - // Front-face culling: only collide if moving toward this face. - if (Vector3.Dot(movement, polyPlane.Normal) >= 0f) - continue; + // Front-face cull. + if (Vector3.Dot(movement, poly.Plane.Normal) >= 0f) continue; - // Use FindTimeOfCollision for exact parametric contact time. - if (CollisionPrimitives.FindTimeOfCollision( - polyPlane, polyVerts, - sphereCenter, sphereRadius, - movement, out float t)) + // Test at start position. + Vector3 cp = Vector3.Zero; + if (PolygonHitsSpherePrecise(poly.Plane, poly.Vertices, center, radius, ref cp)) { - // FindTimeOfCollision returns t such that contact = origin - movement*t. - // For our purposes, a positive t means the sphere reaches the polygon - // when travelling along 'movement'. We want the absolute value as - // our parametric time (0=start, 1=end of movement). - float absT = MathF.Abs(t); - if (absT < bestTime) + if (0f < bestTime) { - bestTime = absT; - hitPolyId = polyIdx; - hitNormal = polyPlane.Normal; + bestTime = 0f; + hitPolyId = polyId; + hitNormal = poly.Plane.Normal; } + continue; } - else + + // Test at end position. + var endCenter = center + movement; + if (PolygonHitsSpherePrecise(poly.Plane, poly.Vertices, endCenter, radius, ref cp)) { - // Fallback: static overlap test at start and end positions. - if (CollisionPrimitives.SphereIntersectsPoly( - polyPlane, polyVerts, sphereCenter, sphereRadius, out _)) + if (1f < bestTime) { - if (0f < bestTime) - { - bestTime = 0f; - hitPolyId = polyIdx; - hitNormal = polyPlane.Normal; - } - } - else - { - Vector3 endCenter = sphereCenter + movement; - if (CollisionPrimitives.SphereIntersectsPoly( - polyPlane, polyVerts, endCenter, sphereRadius, out _)) - { - if (1f < bestTime) - { - bestTime = 1f; - hitPolyId = polyIdx; - hitNormal = polyPlane.Normal; - } - } + bestTime = 1f; + hitPolyId = polyId; + hitNormal = poly.Plane.Normal; } } } return; } - // Internal node: classify against splitting plane - float splitDist = Vector3.Dot(node.SplittingPlane.Normal, sphereCenter) - + node.SplittingPlane.D; - float reach = sphereRadius + movement.Length(); + float splitDist = Vector3.Dot(node.SplittingPlane.Normal, center) + node.SplittingPlane.D; + float reach = radius + movement.Length(); if (splitDist >= reach) { - SphereIntersectsPolyWithTimeRecurse( - node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, movement, - ref hitPolyId, ref hitNormal, ref bestTime); + SphereIntersectsPolyWithTimeRecurse(node.PosNode, resolved, + center, radius, movement, ref hitPolyId, ref hitNormal, ref bestTime); return; } if (splitDist <= -reach) { - SphereIntersectsPolyWithTimeRecurse( - node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, movement, - ref hitPolyId, ref hitNormal, ref bestTime); + SphereIntersectsPolyWithTimeRecurse(node.NegNode, resolved, + center, radius, movement, ref hitPolyId, ref hitNormal, ref bestTime); return; } - // Straddles: check both sides to find the earliest collision. - SphereIntersectsPolyWithTimeRecurse( - node.PosNode, polygons, vertices, - sphereCenter, sphereRadius, movement, - ref hitPolyId, ref hitNormal, ref bestTime); + SphereIntersectsPolyWithTimeRecurse(node.PosNode, resolved, + center, radius, movement, ref hitPolyId, ref hitNormal, ref bestTime); + SphereIntersectsPolyWithTimeRecurse(node.NegNode, resolved, + center, radius, movement, ref hitPolyId, ref hitNormal, ref bestTime); + } - SphereIntersectsPolyWithTimeRecurse( - node.NegNode, polygons, vertices, - sphereCenter, sphereRadius, movement, - ref hitPolyId, ref hitNormal, ref bestTime); + // ========================================================================= + // UTILITY: Build ResolvedPolygon dictionary from raw Polygon + VertexArray + // + // Used by legacy overloads. Callers with pre-resolved data from + // PhysicsDataCache should use the primary overloads to avoid this overhead. + // ========================================================================= + + private static Dictionary BuildResolved( + Dictionary polygons, + DatReaderWriter.Types.VertexArray vertices) + { + var resolved = new Dictionary(polygons.Count); + foreach (var (id, poly) in polygons) + { + int n = poly.VertexIds.Count; + if (n < 3) continue; + + var verts = new Vector3[n]; + bool valid = true; + for (int i = 0; i < n; i++) + { + ushort vid = (ushort)poly.VertexIds[i]; + if (!vertices.Vertices.TryGetValue(vid, out var sv)) + { valid = false; break; } + verts[i] = sv.Origin; + } + if (!valid) continue; + + // Fan cross-product normal accumulation (ACE make_plane). + var normal = Vector3.Zero; + for (int i = 1; i < n - 1; i++) + normal += Vector3.Cross(verts[i] - verts[0], verts[i + 1] - verts[0]); + + float len = normal.Length(); + if (len < 1e-8f) continue; + normal /= len; + + float dotSum = 0f; + for (int i = 0; i < n; i++) + dotSum += Vector3.Dot(normal, verts[i]); + float planeD = -(dotSum / n); + + resolved[id] = new ResolvedPolygon + { + Vertices = verts, + Plane = new Plane(normal, planeD), + NumPoints = n, + SidesType = poly.SidesType, + }; + } + return resolved; } } diff --git a/src/AcDream.Core/Physics/PhysicsDataCache.cs b/src/AcDream.Core/Physics/PhysicsDataCache.cs index 688bba7..5efeab6 100644 --- a/src/AcDream.Core/Physics/PhysicsDataCache.cs +++ b/src/AcDream.Core/Physics/PhysicsDataCache.cs @@ -3,6 +3,7 @@ using System.Numerics; using DatReaderWriter.DBObjs; using DatReaderWriter.Enums; using DatReaderWriter.Types; +using Plane = System.Numerics.Plane; namespace AcDream.Core.Physics; @@ -34,6 +35,7 @@ public sealed class PhysicsDataCache PhysicsPolygons = gfxObj.PhysicsPolygons, BoundingSphere = gfxObj.PhysicsBSP.Root.BoundingSphere, Vertices = gfxObj.VertexArray, + Resolved = ResolvePolygons(gfxObj.PhysicsPolygons, gfxObj.VertexArray), }; } @@ -75,9 +77,66 @@ public sealed class PhysicsDataCache Vertices = cellStruct.VertexArray, WorldTransform = worldTransform, InverseWorldTransform = inverseTransform, + Resolved = ResolvePolygons(cellStruct.PhysicsPolygons, cellStruct.VertexArray), }; } + /// + /// 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. + /// + private static Dictionary ResolvePolygons( + Dictionary polys, + VertexArray vertexArray) + { + var resolved = new Dictionary(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; @@ -86,6 +145,19 @@ public sealed class PhysicsDataCache public int CellStructCount => _cellStruct.Count; } +/// +/// 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. +/// +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; } +} + /// Cached physics data for a single GfxObj part. public sealed class GfxObjPhysics { @@ -93,6 +165,12 @@ public sealed class GfxObjPhysics public required Dictionary PhysicsPolygons { get; init; } public Sphere? BoundingSphere { get; init; } public required VertexArray Vertices { get; init; } + + /// + /// 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. + /// + public required Dictionary Resolved { get; init; } } /// Cached collision shape data for a Setup (character/creature capsule). @@ -118,4 +196,9 @@ public sealed class CellPhysics public required VertexArray Vertices { get; init; } public Matrix4x4 WorldTransform { get; init; } public Matrix4x4 InverseWorldTransform { get; init; } + + /// + /// Pre-resolved polygon data with vertex positions and computed planes. + /// + public required Dictionary Resolved { get; init; } } diff --git a/src/AcDream.Core/Physics/TransitionTypes.cs b/src/AcDream.Core/Physics/TransitionTypes.cs index 16f7fcf..b6a500f 100644 --- a/src/AcDream.Core/Physics/TransitionTypes.cs +++ b/src/AcDream.Core/Physics/TransitionTypes.cs @@ -547,10 +547,10 @@ public sealed class Transition } // Use the full 6-path BSP dispatcher for retail-faithful collision. + // Use pre-resolved polygons (vertices+planes computed at cache time). var cellState = BSPQuery.FindCollisions( cellPhysics.BSP.Root, - cellPhysics.PhysicsPolygons, - cellPhysics.Vertices, + cellPhysics.Resolved, this, localSphere, localSphere1, @@ -665,27 +665,28 @@ public sealed class Transition /// /// Query the ShadowObjectRegistry for nearby static objects and run - /// sphere-vs-BSP collision against each. On hit, calls SlideSphere to - /// compute a wall-slide offset and returns the result. + /// collision against each using the retail BSPTree.find_collisions 6-path + /// dispatcher. /// - /// Object-local transform: the player sphere is mapped into each object's - /// local space via the inverse of (Rotation, Position) before the BSP query. - /// The hit normal is then rotated back to world space. + /// ACE: ObjCell.FindObjCollisions iterates objects, calling + /// PhysicsObj.FindObjCollisions on each. For BSP objects, this transforms + /// to object-local space and calls BSPTree.find_collisions (the 6-path + /// dispatcher that handles step-up, slide, collide-with-point, etc.). /// - /// Ported from pseudocode section 4 (ObjCell.FindObjCollisions) and - /// section 6 (SlideSphere). + /// The retail approach processes objects sequentially — the first non-OK + /// result modifies SpherePath and is returned. This differs from the + /// previous "find earliest t" approach. /// private TransitionState FindObjCollisions(PhysicsEngine engine) { if (engine.DataCache is null) return TransitionState.OK; var sp = SpherePath; - var ci = CollisionInfo; Vector3 checkPos = sp.GlobalSphere[0].Origin; Vector3 currPos = sp.GlobalCurrCenter[0].Origin; float sphereRadius = sp.GlobalSphere[0].Radius; - Vector3 movement = checkPos - currPos; // this step's movement vector + Vector3 movement = checkPos - currPos; if (!engine.TryGetLandblockContext(checkPos.X, checkPos.Y, out uint landblockId, out float worldOffsetX, out float worldOffsetY)) @@ -697,154 +698,137 @@ public sealed class Transition worldOffsetX, worldOffsetY, landblockId, _nearbyObjs); - // Find the EARLIEST collision along the movement path. - // Test both foot sphere (index 0) and head sphere (index 1) if present. - float bestT = float.MaxValue; - Vector3 bestNormal = Vector3.Zero; - bool bestIsHeadSphere = false; - - for (int sphereIdx = 0; sphereIdx < sp.NumSphere; sphereIdx++) + foreach (var obj in _nearbyObjs) { - Vector3 sphereCheckPos = sp.GlobalSphere[sphereIdx].Origin; - Vector3 sphereCurrPos = sp.GlobalCurrCenter[sphereIdx].Origin; - float sphRadius = sp.GlobalSphere[sphereIdx].Radius; - Vector3 sphMovement = sphereCheckPos - sphereCurrPos; + // Broad-phase: can the moving sphere reach this object? + Vector3 deltaToCurr = currPos - obj.Position; + float distToCurr; + if (obj.CollisionType == ShadowCollisionType.Cylinder) + distToCurr = MathF.Sqrt(deltaToCurr.X * deltaToCurr.X + deltaToCurr.Y * deltaToCurr.Y); + else + distToCurr = deltaToCurr.Length(); + float maxReach = sphereRadius + obj.Radius + movement.Length() + 2f; + if (distToCurr > maxReach) + continue; - foreach (var obj in _nearbyObjs) + TransitionState result; + + if (obj.CollisionType == ShadowCollisionType.BSP) { - // Broad-phase: can the moving sphere reach this object? - Vector3 deltaToCurr = sphereCurrPos - obj.Position; - float distToCurr; - if (obj.CollisionType == ShadowCollisionType.Cylinder) - distToCurr = MathF.Sqrt(deltaToCurr.X * deltaToCurr.X + deltaToCurr.Y * deltaToCurr.Y); - else - distToCurr = deltaToCurr.Length(); - float maxReach = sphRadius + obj.Radius + sphMovement.Length() + 2f; - if (distToCurr > maxReach) - continue; + // ── BSP object: use the full 6-path retail dispatcher ──── + // ACE: PhysicsObj.FindObjCollisions → Setup.BSP.find_collisions + var physics = engine.DataCache.GetGfxObj(obj.GfxObjId); + if (physics?.BSP?.Root is null) continue; - float t; - Vector3 worldHitNormal; + // Transform player spheres to object-local space. + var invRot = Quaternion.Inverse(obj.Rotation); - if (obj.CollisionType == ShadowCollisionType.BSP) + var localSphere0 = new DatReaderWriter.Types.Sphere { - var physics = engine.DataCache.GetGfxObj(obj.GfxObjId); - if (physics?.BSP?.Root is null) continue; + Origin = Vector3.Transform(sp.GlobalSphere[0].Origin - obj.Position, invRot), + Radius = sp.GlobalSphere[0].Radius, + }; + var localCurrCenter = Vector3.Transform( + sp.GlobalCurrCenter[0].Origin - obj.Position, invRot); - // Transform to object-local space. - var invRot = Quaternion.Inverse(obj.Rotation); - Vector3 localCurrPos = Vector3.Transform(sphereCurrPos - obj.Position, invRot); - Vector3 localMovement = Vector3.Transform(sphMovement, invRot); - - // Use movement-aware BSP query with front-face culling. - if (!BSPQuery.SphereIntersectsPolyWithTime( - physics.BSP.Root, - physics.PhysicsPolygons, - physics.Vertices, - localCurrPos, sphRadius, - localMovement, - out _, out Vector3 localHitNormal, out t)) - continue; - - worldHitNormal = Vector3.Transform(localHitNormal, obj.Rotation); - t = Math.Clamp(t, 0f, 1f); - } - else + DatReaderWriter.Types.Sphere? localSphere1 = null; + if (sp.NumSphere > 1) { - // Cylinder swept-sphere test. - Vector3 deltaCurr = sphereCurrPos - obj.Position; - float dx = deltaCurr.X, dy = deltaCurr.Y; - float mx = sphMovement.X, my = sphMovement.Y; - float combinedR = sphRadius + obj.Radius; - - float a = mx * mx + my * my; - float b = 2f * (dx * mx + dy * my); - float c = dx * dx + dy * dy - combinedR * combinedR; - - if (a < PhysicsGlobals.EPSILON) + localSphere1 = new DatReaderWriter.Types.Sphere { - if (c > 0f) continue; - t = 0f; - } - else - { - float disc = b * b - 4f * a * c; - if (disc < 0f) continue; - float sqrtDisc = MathF.Sqrt(disc); - t = (-b - sqrtDisc) / (2f * a); - if (t > 1f) continue; - if (t < 0f) t = 0f; - } - - // Vertical check at contact time. - Vector3 contactPos = sphereCurrPos + sphMovement * t; - float cylTop = obj.CylHeight > 0 ? obj.CylHeight : obj.Radius * 4f; - float playerBottom = contactPos.Z - sphRadius; - float playerTop = contactPos.Z + sphRadius; - if (playerBottom > obj.Position.Z + cylTop || playerTop < obj.Position.Z) - continue; - - // Normal: radial at contact point. - Vector3 contactDelta = contactPos - obj.Position; - float hDist = MathF.Sqrt(contactDelta.X * contactDelta.X + contactDelta.Y * contactDelta.Y); - if (hDist < PhysicsGlobals.EPSILON) - worldHitNormal = Vector3.UnitX; - else - worldHitNormal = Vector3.Normalize(new Vector3(contactDelta.X, contactDelta.Y, 0f)); + Origin = Vector3.Transform(sp.GlobalSphere[1].Origin - obj.Position, invRot), + Radius = sp.GlobalSphere[1].Radius, + }; } - if (t < bestT && worldHitNormal.LengthSquared() > PhysicsGlobals.EpsilonSq) - { - bestT = t; - bestNormal = Vector3.Normalize(worldHitNormal); - bestIsHeadSphere = (sphereIdx == 1); - } + // Local-space Z (up direction rotated into object space). + var localSpaceZ = Vector3.Transform(Vector3.UnitZ, invRot); + + // Use the retail 6-path dispatcher with pre-resolved polygons. + result = BSPQuery.FindCollisions( + physics.BSP.Root, + physics.Resolved, + this, + localSphere0, + localSphere1, + localCurrCenter, + localSpaceZ, + 1.0f); // scale = 1.0 for object geometry } + else + { + // ── Cylinder object: swept-sphere cylinder test ────────── + // ACE: Sphere.IntersectsSphere handles CylSphere objects via + // the same 6-path dispatcher. For now we keep the swept-sphere + // cylinder test which matches the retail CylSphere behavior. + result = CylinderCollision(obj, sp); + } + + if (result != TransitionState.OK) + return result; } - if (bestT >= float.MaxValue) + return TransitionState.OK; + } + + /// + /// Cylinder swept-sphere collision test for CylSphere objects (trees, rocks, etc.). + /// Performs a 2D ray-circle intersection to find contact time, then applies + /// a wall-slide response. + /// + private TransitionState CylinderCollision(ShadowEntry obj, SpherePath sp) + { + var ci = CollisionInfo; + Vector3 sphereCurrPos = sp.GlobalCurrCenter[0].Origin; + Vector3 sphereCheckPos = sp.GlobalSphere[0].Origin; + float sphRadius = sp.GlobalSphere[0].Radius; + Vector3 sphMovement = sphereCheckPos - sphereCurrPos; + + Vector3 deltaCurr = sphereCurrPos - obj.Position; + float dx = deltaCurr.X, dy = deltaCurr.Y; + float mx = sphMovement.X, my = sphMovement.Y; + float combinedR = sphRadius + obj.Radius; + + float a = mx * mx + my * my; + float b = 2f * (dx * mx + dy * my); + float c = dx * dx + dy * dy - combinedR * combinedR; + + float t; + if (a < PhysicsGlobals.EPSILON) { + if (c > 0f) return TransitionState.OK; + t = 0f; + } + else + { + float disc = b * b - 4f * a * c; + if (disc < 0f) return TransitionState.OK; + float sqrtDisc = MathF.Sqrt(disc); + t = (-b - sqrtDisc) / (2f * a); + if (t > 1f) return TransitionState.OK; + if (t < 0f) t = 0f; + } + + // Vertical check at contact time. + Vector3 contactPos = sphereCurrPos + sphMovement * t; + float cylTop = obj.CylHeight > 0 ? obj.CylHeight : obj.Radius * 4f; + float playerBottom = contactPos.Z - sphRadius; + float playerTop = contactPos.Z + sphRadius; + if (playerBottom > obj.Position.Z + cylTop || playerTop < obj.Position.Z) return TransitionState.OK; - } - // ── Fix 3: Contact-path step-up attempt ───────────────────────── - // When in contact with ground and hitting a low obstacle (not the head - // sphere), try stepping up before falling back to slide. - // ACE: BSPTree.find_collisions path 5 — Contact|OnWalkable → step_sphere_up. - if (!bestIsHeadSphere - && ObjectInfo.Contact - && bestNormal.Z > PhysicsGlobals.EPSILON - && bestNormal.Z < PhysicsGlobals.FloorZ) - { - // The surface is angled (not a vertical wall, not a floor) — - // attempt step-up. Set the flag for the transition system. - sp.StepUp = true; - sp.StepUpNormal = bestNormal; - ci.SetCollisionNormal(bestNormal); - return TransitionState.OK; - } + // Collision normal: radial from cylinder axis. + Vector3 contactDelta = contactPos - obj.Position; + float hDist = MathF.Sqrt(contactDelta.X * contactDelta.X + contactDelta.Y * contactDelta.Y); + Vector3 collisionNormal; + if (hDist < PhysicsGlobals.EPSILON) + collisionNormal = Vector3.UnitX; + else + collisionNormal = Vector3.Normalize(new Vector3(contactDelta.X, contactDelta.Y, 0f)); - // Already overlapping at the START of the step (bestT == 0 or very small). - if (bestT <= PhysicsGlobals.EPSILON) - { - Vector3 pushOut = bestNormal * (sphereRadius * 0.5f + 0.01f); - sp.AddOffsetToCheckPos(pushOut); - ci.SetCollisionNormal(bestNormal); - ci.SetSlidingNormal(bestNormal); - return TransitionState.Adjusted; - } - - // Rewind the sphere to just BEFORE the contact point. - if (bestT < 1f) - { - float safeT = MathF.Max(0f, bestT - 0.02f); - Vector3 contactPos = currPos + movement * safeT; - contactPos += bestNormal * 0.02f; - sp.SetCheckPos(contactPos, sp.CheckCellId); - } - - // Apply wall-slide from the contact point. - return SlideSphere(bestNormal, currPos); + // Apply collision response via wall-slide. + ci.SetCollisionNormal(collisionNormal); + return SlideSphere(collisionNormal, sphereCurrPos); } // -----------------------------------------------------------------------