diff --git a/src/AcDream.Core/Physics/BSPQuery.cs b/src/AcDream.Core/Physics/BSPQuery.cs index 469572e..4618352 100644 --- a/src/AcDream.Core/Physics/BSPQuery.cs +++ b/src/AcDream.Core/Physics/BSPQuery.cs @@ -5,46 +5,988 @@ using DatReaderWriter.Types; namespace AcDream.Core.Physics; /// -/// BSP tree traversal for sphere-polygon collision detection. +/// BSP tree collision queries ported from decompiled retail client. +/// Cross-referenced against ACE BSPTree.cs, BSPNode.cs, BSPLeaf.cs, Polygon.cs. /// /// -/// Ported from decompiled FUN_00539270 (chunk_00539000.c), cross-referenced -/// against ACE's BSPNode.sphere_intersects_poly() in -/// Source/ACE.Server/Physics/BSP/BSPNode.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. /// /// /// -/// The algorithm is a recursive descent through the BSP tree: -/// -/// Broad phase: discard the subtree if the sphere cannot reach the -/// node's bounding sphere. -/// Leaf: test each polygon using the existing retail-ported -/// . -/// Internal: classify the sphere against the splitting plane and -/// recurse into the positive half, the negative half, or both when the -/// sphere straddles the plane. -/// +/// 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. /// /// public static class BSPQuery { + // ----------------------------------------------------------------------- + // Polygon plane computation helper + // ----------------------------------------------------------------------- + + /// + /// Compute the polygon plane from vertices, using the retail CalcNormal + /// algorithm. Returns false if the polygon is degenerate. + /// + private static bool TryGetPolyPlane( + Polygon poly, VertexArray vertices, + out Plane plane, out Vector3[] polyVerts) + { + plane = default; + polyVerts = Array.Empty(); + + if (poly.VertexIds.Count < 3) return false; + + polyVerts = new Vector3[poly.VertexIds.Count]; + for (int i = 0; i < poly.VertexIds.Count; i++) + { + ushort vid = (ushort)poly.VertexIds[i]; + if (!vertices.Vertices.TryGetValue(vid, out var sv)) + return false; + polyVerts[i] = sv.Origin; + } + + CollisionPrimitives.CalcNormal(polyVerts, out var normal, out float planeD); + if (normal.LengthSquared() < CollisionPrimitives.EpsilonSq) + return false; + + plane = new Plane(normal, planeD); + return true; + } + + // ----------------------------------------------------------------------- + // 1. FindCollisions — BSPTree.find_collisions dispatcher + // ACE: BSPTree.cs line ~131. 6 dispatch paths. + // ----------------------------------------------------------------------- + + /// + /// 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 + /// + /// + /// 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) + { + 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; + } + + // ----------------------------------------------------------------------- + // 2. SphereIntersectsPolyWithMovement — BSPNode.sphere_intersects_poly + // with movement vector (ACE BSPNode.cs + BSPLeaf.cs) + // Uses pos_hits_sphere for front-face culling. + // ----------------------------------------------------------------------- + + /// + /// 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). + /// + /// + /// ACE: BSPNode.cs sphere_intersects_poly (internal nodes), + /// BSPLeaf.cs sphere_intersects_poly (leaf nodes). + /// Decompiled: FUN_00539270. + /// + /// + 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. + /// + /// + public static bool PosHitsSphere( + Plane polyPlane, + ReadOnlySpan polyVerts, + Vector3 sphereCenter, float sphereRadius, + Vector3 movement, + out 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; + + float dist = Vector3.Dot(polyPlane.Normal, sphereCenter) + polyPlane.D; + float rad = sphereRadius - PhysicsGlobals.EPSILON; + + if (MathF.Abs(dist) > rad) return false; + + float diff = rad * rad - dist * dist; + contactPoint = sphereCenter - polyPlane.Normal * dist; + + int prevIdx = numVerts - 1; + for (int i = 0; i < numVerts; i++) + { + var vertex = polyVerts[i]; + var lastVertex = polyVerts[prevIdx]; + prevIdx = i; + + var edge = vertex - lastVertex; + var disp = contactPoint - lastVertex; + 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++) + { + vertex = polyVerts[j]; + lastVertex = polyVerts[prevIdx]; + prevIdx = j; + + edge = vertex - lastVertex; + disp = contactPoint - lastVertex; + cross = Vector3.Cross(polyPlane.Normal, edge); + float dispDot = Vector3.Dot(disp, cross); + + if (dispDot < 0f) + { + if (cross.LengthSquared() * diff < dispDot * dispDot) + return false; + + float dispEdge = Vector3.Dot(disp, edge); + if (dispEdge >= 0f && dispEdge <= edge.LengthSquared()) + return true; + } + + if (disp.LengthSquared() <= diff) + return true; + } + return false; + } + return true; + } + + // ----------------------------------------------------------------------- + // 5. SphereIntersectsSolid — BSPNode.sphere_intersects_solid + // ACE: BSPNode.cs + BSPLeaf.cs sphere_intersects_solid + // Used for placement/ethereal checks. + // ----------------------------------------------------------------------- + + /// + /// BSPNode.sphere_intersects_solid -- checks if a sphere intersects any + /// solid geometry in the BSP tree. Used for placement and ethereal checks. + /// + /// + /// ACE: BSPNode.cs sphere_intersects_solid (internal node), + /// BSPLeaf.cs sphere_intersects_solid (leaf). + /// + /// + public static bool SphereIntersectsSolid( + PhysicsBSPNode? node, + Dictionary polygons, + VertexArray vertices, + Vector3 sphereCenter, float sphereRadius, + bool centerCheck) + { + 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; + 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); + } + } + + // ----------------------------------------------------------------------- + // 6. HitsWalkable — BSPNode.hits_walkable + BSPLeaf.hits_walkable + // ACE: BSPNode.cs hits_walkable, BSPLeaf.cs hits_walkable + // Used for check_walkable path. + // ----------------------------------------------------------------------- + + /// + /// 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). + /// + /// + public static bool HitsWalkable( + PhysicsBSPNode? node, + Dictionary polygons, + VertexArray vertices, + Vector3 sphereCenter, float sphereRadius, + SpherePath path, + Vector3 up) + { + 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); + } + + // ----------------------------------------------------------------------- + // 7. FindWalkable — BSPNode.find_walkable + BSPLeaf.find_walkable + // ACE: BSPNode.cs find_walkable, BSPLeaf.cs find_walkable + // Used for Collide and StepDown paths. + // ----------------------------------------------------------------------- + + /// + /// BSPNode.find_walkable -- finds walkable surfaces and adjusts the sphere + /// to rest on them. Used by the Collide and StepDown dispatch paths. + /// + /// + /// ACE: BSPNode.cs find_walkable (internal), BSPLeaf.cs find_walkable (leaf). + /// + /// + 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) + { + if (node is null) return; + + // 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); + } + + // ----------------------------------------------------------------------- + // 8. StepSphereDown — BSPTree.step_sphere_down + // ACE: BSPTree.cs step_sphere_down + // ----------------------------------------------------------------------- + + /// + /// BSPTree.step_sphere_down -- BSP-based step-down ground search. + /// Probes downward along localSpaceZ to find a walkable surface. + /// + /// + /// ACE: BSPTree.cs step_sphere_down. + /// + /// + 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. + /// + /// + private static bool CheckWalkable( + Plane polyPlane, + ReadOnlySpan polyVerts, + Vector3 sphereCenter, float sphereRadius, + Vector3 up, + bool small) + { + float angleUp = Vector3.Dot(polyPlane.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 radsum = sphereRadius * sphereRadius; + if (small) radsum *= 0.25f; + + int numVerts = polyVerts.Length; + int prevIdx = numVerts - 1; + bool result = true; + + for (int i = 0; i < numVerts; i++) + { + var vertex = polyVerts[i]; + var lastVertex = polyVerts[prevIdx]; + prevIdx = i; + + var edge = vertex - lastVertex; + var disp = center - lastVertex; + var cross = Vector3.Cross(polyPlane.Normal, edge); + float diff = Vector3.Dot(disp, cross); + + if (diff < 0f) + { + if (cross.LengthSquared() * radsum < diff * diff) + return false; + + float dispEdge = Vector3.Dot(disp, edge); + if (dispEdge >= 0f && dispEdge <= edge.LengthSquared()) + return true; + + return false; + } + + if (disp.LengthSquared() <= radsum) + return true; + } + + return result; + } + + // ======================================================================= + // LEGACY: Static overlap SphereIntersectsPoly (no movement vector) + // + // 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. + // ======================================================================= + /// /// Test if a sphere intersects any polygon in the physics BSP tree. /// Returns on the first hit, populating /// and . /// /// + /// This is the static overlap test (no movement vector). For the + /// movement-aware version with front-face culling, use + /// . + /// + /// + /// /// Ported from FUN_00539270; cross-ref ACE BSPNode.sphere_intersects_poly. /// /// - /// Current BSP node (null-safe). - /// Physics polygon dictionary from the GfxObj. - /// Vertex array from the GfxObj. - /// Sphere centre in object-local space. - /// Sphere radius. - /// Polygon id of the first intersecting polygon (0 on miss). - /// Outward normal at the hit point (zero on miss). - /// if any polygon is hit. public static bool SphereIntersectsPoly( PhysicsBSPNode? node, Dictionary polygons, @@ -59,72 +1001,40 @@ public static class BSPQuery if (node is null) return false; - // ---------------------------------------------------------------- - // Broad phase: reject the whole subtree when the sphere cannot - // reach the node's bounding sphere. Both Leaf and internal nodes - // carry a BoundingSphere in the retail format. - // ---------------------------------------------------------------- + // 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 referenced polygon against the sphere using - // the retail-ported CollisionPrimitives.SphereIntersectsPoly. - // ---------------------------------------------------------------- + // Leaf node: test each polygon if (node.Type == BSPNodeType.Leaf) { foreach (var polyIdx in node.Polygons) { if (!polygons.TryGetValue(polyIdx, out var poly)) continue; - if (poly.VertexIds.Count < 3) continue; - - // Gather polygon vertices from the vertex array. - var polyVerts = new Vector3[poly.VertexIds.Count]; - bool allFound = true; - for (int i = 0; i < poly.VertexIds.Count; i++) - { - ushort vid = (ushort)poly.VertexIds[i]; - if (vertices.Vertices.TryGetValue(vid, out var sv)) - polyVerts[i] = sv.Origin; - else { allFound = false; break; } - } - if (!allFound) continue; - - // Compute the polygon plane using the retail CalcNormal port. - CollisionPrimitives.CalcNormal(polyVerts, out var normal, out float planeD); - if (normal.LengthSquared() < CollisionPrimitives.EpsilonSq) continue; - - var polyPlane = new Plane(normal, planeD); + if (!TryGetPolyPlane(poly, vertices, out var polyPlane, out var polyVerts)) + continue; if (CollisionPrimitives.SphereIntersectsPoly( polyPlane, polyVerts, sphereCenter, sphereRadius, out _)) { hitPolyId = polyIdx; - hitNormal = normal; + hitNormal = polyPlane.Normal; return true; } } return false; } - // ---------------------------------------------------------------- - // Internal node: classify sphere against splitting plane and - // recurse into the positive side, negative side, or both. - // - // System.Numerics.Plane convention: dot(N, p) + D = 0 on the - // surface, so signed distance = dot(N, center) + D. - // FUN_00539270 uses the same sign convention. - // ---------------------------------------------------------------- + // Internal node: classify against splitting plane float splitDist = Vector3.Dot(node.SplittingPlane.Normal, sphereCenter) + node.SplittingPlane.D; float reach = sphereRadius - CollisionPrimitives.Epsilon; if (splitDist >= reach) { - // Sphere entirely on the positive side. return SphereIntersectsPoly( node.PosNode, polygons, vertices, sphereCenter, sphereRadius, @@ -133,14 +1043,13 @@ public static class BSPQuery if (splitDist <= -reach) { - // Sphere entirely on the negative side. return SphereIntersectsPoly( node.NegNode, polygons, vertices, sphereCenter, sphereRadius, out hitPolyId, out hitNormal); } - // Sphere straddles the plane — check both sides, return on first hit. + // Straddles if (SphereIntersectsPoly(node.PosNode, polygons, vertices, sphereCenter, sphereRadius, out hitPolyId, out hitNormal)) return true;