From 874bcc8690614cde88d41d42e8da1bdedfa20afb Mon Sep 17 00:00:00 2001 From: Erik Date: Tue, 14 Apr 2026 16:18:43 +0200 Subject: [PATCH] feat(physics): retail-faithful collision system port from ACE MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace the patched collision system (~60-70% retail) with a faithful port of ACE's BSPTree/BSPNode/BSPLeaf/Polygon collision pipeline. BSPQuery.cs completely rewritten (1808 lines): - Polygon-level: polygon_hits_sphere_precise (retail two-loop test), pos_hits_sphere, hits_sphere, walkable_hits_sphere, check_walkable, adjust_sphere_to_plane, find_crossed_edge, adjust_to_placement_poly - BSP traversal: sphere_intersects_poly, find_walkable, hits_walkable, sphere_intersects_solid, sphere_intersects_solid_poly - BSP tree-level: find_collisions (6-path dispatcher), step_sphere_up, step_sphere_down, slide_sphere, collide_with_pt, adjust_to_plane, placement_insert PhysicsDataCache.cs: Added ResolvedPolygon type with pre-computed vertex positions and face planes (matching ACE's Polygon constructor which calls make_plane() at load time). Populated at cache time to avoid per-collision-test vertex lookups. TransitionTypes.cs: FindObjCollisions rewritten to use the retail per-object FindCollisions 6-path dispatcher instead of the old "find earliest t, then apply custom response" approach. BSP objects now go through the same collision paths as indoor cell BSP. The previous approach was explicitly rejected by the user after ~10 iterations of patches. This port follows the CLAUDE.md mandatory workflow: decompile first → cross-reference ACE → port faithfully. Co-Authored-By: Claude Opus 4.6 (1M context) --- src/AcDream.Core/Physics/BSPQuery.cs | 2643 +++++++++++------- src/AcDream.Core/Physics/PhysicsDataCache.cs | 83 + src/AcDream.Core/Physics/TransitionTypes.cs | 266 +- 3 files changed, 1781 insertions(+), 1211 deletions(-) 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); } // -----------------------------------------------------------------------