From 8e1230c53bb591d3715343fcf6f0e9763c19554c Mon Sep 17 00:00:00 2001 From: Erik Date: Tue, 14 Apr 2026 12:23:10 +0200 Subject: [PATCH] fix(physics): swept-sphere collision prevents penetration Restructure FindObjCollisions to compute collision ALONG the movement path instead of at the final position: BSP: movement-aware SphereIntersectsPoly with front-face culling (dot(movement, normal) < 0). Only detects faces the sphere is approaching, matching retail Polygon.pos_hits_sphere. Cylinder: quadratic ray-cylinder intersection computes parametric contact time t. If t < 1.0, sphere is rewound to the contact point. Both: find the EARLIEST collision (minimum t), rewind sphere to contact point + small epsilon along normal, then SlideSphere. This prevents the "walking into walls" penetration (BUG-005). Co-Authored-By: Claude Opus 4.6 (1M context) --- src/AcDream.Core/Physics/BSPQuery.cs | 85 +++++++++++++ src/AcDream.Core/Physics/TransitionTypes.cs | 130 ++++++++++++++------ 2 files changed, 176 insertions(+), 39 deletions(-) diff --git a/src/AcDream.Core/Physics/BSPQuery.cs b/src/AcDream.Core/Physics/BSPQuery.cs index 4618352..97f3419 100644 --- a/src/AcDream.Core/Physics/BSPQuery.cs +++ b/src/AcDream.Core/Physics/BSPQuery.cs @@ -1057,4 +1057,89 @@ public static class BSPQuery return SphereIntersectsPoly(node.NegNode, polygons, vertices, sphereCenter, sphereRadius, out hitPolyId, out 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); + } } diff --git a/src/AcDream.Core/Physics/TransitionTypes.cs b/src/AcDream.Core/Physics/TransitionTypes.cs index bc7d987..b919790 100644 --- a/src/AcDream.Core/Physics/TransitionTypes.cs +++ b/src/AcDream.Core/Physics/TransitionTypes.cs @@ -629,88 +629,140 @@ public sealed class Transition if (engine.DataCache is null) return TransitionState.OK; var sp = SpherePath; + var ci = CollisionInfo; - Vector3 footCenter = sp.GlobalSphere[0].Origin; + 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 - if (!engine.TryGetLandblockContext(footCenter.X, footCenter.Y, + if (!engine.TryGetLandblockContext(checkPos.X, checkPos.Y, out uint landblockId, out float worldOffsetX, out float worldOffsetY)) return TransitionState.OK; - float queryRadius = sphereRadius + 10f; + float queryRadius = sphereRadius + movement.Length() + 5f; engine.ShadowObjects.GetNearbyObjects( - footCenter, queryRadius, + currPos, queryRadius, worldOffsetX, worldOffsetY, landblockId, _nearbyObjs); + // Find the EARLIEST collision along the movement path. + float bestT = float.MaxValue; + Vector3 bestNormal = Vector3.Zero; + foreach (var obj in _nearbyObjs) { - // Broad-phase: sphere-sphere distance check. - float dist = Vector3.Distance(footCenter, obj.Position); - if (dist > sphereRadius + obj.Radius + 1f) + // Broad-phase: can the moving sphere reach this object? + float distToCurr = Vector3.Distance(currPos, obj.Position); + float maxReach = sphereRadius + obj.Radius + movement.Length() + 1f; + if (distToCurr > maxReach) continue; + float t; Vector3 worldHitNormal; if (obj.CollisionType == ShadowCollisionType.BSP) { - // BSP narrow phase: full polygon collision. var physics = engine.DataCache.GetGfxObj(obj.GfxObjId); if (physics?.BSP?.Root is null) continue; + // Transform to object-local space. var invRot = Quaternion.Inverse(obj.Rotation); - Vector3 localSphereCenter = Vector3.Transform(footCenter - obj.Position, invRot); + Vector3 localCurrPos = Vector3.Transform(currPos - obj.Position, invRot); + Vector3 localMovement = Vector3.Transform(movement, invRot); + // Use movement-aware BSP query with front-face culling. if (!BSPQuery.SphereIntersectsPoly( physics.BSP.Root, physics.PhysicsPolygons, physics.Vertices, - localSphereCenter, sphereRadius, + localCurrPos, sphereRadius, + localMovement, out _, out Vector3 localHitNormal)) continue; worldHitNormal = Vector3.Transform(localHitNormal, obj.Rotation); + + // Compute parametric contact time: how far along the movement + // does the sphere first touch this polygon? + // Project the center-to-plane distance onto the movement direction. + float planeDist = Vector3.Dot(localHitNormal, localCurrPos) - + Vector3.Dot(localHitNormal, Vector3.Zero); // plane through origin in local + float approach = -Vector3.Dot(localHitNormal, localMovement); + if (approach > PhysicsGlobals.EPSILON) + t = (planeDist - sphereRadius) / approach; + else + t = 0f; // already touching or parallel + t = Math.Clamp(t, 0f, 1f); } else { - // Cylinder/Sphere narrow phase: simple radial collision. - // Retail uses CylSphere::IntershectsSphere for trees, rocks, NPCs. - // The cylinder extends vertically from obj.Position.Z to Z+Height. - // We test if the player sphere overlaps the cylinder radially AND - // is within the vertical extent. - Vector3 delta = footCenter - obj.Position; - float horizontalDist = MathF.Sqrt(delta.X * delta.X + delta.Y * delta.Y); - float combinedRadius = sphereRadius + obj.Radius; + // Cylinder swept-sphere test. + // Find parametric time when moving sphere first contacts the cylinder. + Vector3 deltaCurr = currPos - obj.Position; + float dx = deltaCurr.X, dy = deltaCurr.Y; + float mx = movement.X, my = movement.Y; + float combinedR = sphereRadius + obj.Radius; - if (horizontalDist >= combinedRadius) - continue; // no radial overlap + // Quadratic: |curr_xy + t*move_xy|^2 = combinedR^2 + float a = mx * mx + my * my; + float b = 2f * (dx * mx + dy * my); + float c = dx * dx + dy * dy - combinedR * combinedR; - // Vertical check: player sphere must overlap the cylinder height range. - float cylTop = obj.CylHeight > 0 ? obj.CylHeight : obj.Radius * 4f; - float playerBottom = footCenter.Z - sphereRadius; - float playerTop = footCenter.Z + sphereRadius; - float objBottom = obj.Position.Z; - float objTop = obj.Position.Z + cylTop; - - if (playerBottom > objTop || playerTop < objBottom) - continue; // vertically separated - - // Collision normal: push player out radially (XY only). - if (horizontalDist < PhysicsGlobals.EPSILON) - worldHitNormal = Vector3.UnitX; // degenerate: directly on top + if (a < PhysicsGlobals.EPSILON) + { + // Not moving horizontally — check static overlap. + if (c > 0f) continue; + t = 0f; + } else - worldHitNormal = Vector3.Normalize(new Vector3(delta.X, delta.Y, 0f)); + { + float disc = b * b - 4f * a * c; + if (disc < 0f) continue; // no intersection + float sqrtDisc = MathF.Sqrt(disc); + t = (-b - sqrtDisc) / (2f * a); // first contact time + if (t > 1f) continue; // contact is past this step + if (t < 0f) t = 0f; // already overlapping + } + + // Vertical check at contact time. + Vector3 contactPos = currPos + movement * t; + float cylTop = obj.CylHeight > 0 ? obj.CylHeight : obj.Radius * 4f; + float playerBottom = contactPos.Z - sphereRadius; + float playerTop = contactPos.Z + sphereRadius; + 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)); } - if (worldHitNormal.LengthSquared() > PhysicsGlobals.EpsilonSq) + if (t < bestT && worldHitNormal.LengthSquared() > PhysicsGlobals.EpsilonSq) { - worldHitNormal = Vector3.Normalize(worldHitNormal); - Vector3 currPos = sp.GlobalCurrCenter[0].Origin; - return SlideSphere(worldHitNormal, currPos); + bestT = t; + bestNormal = Vector3.Normalize(worldHitNormal); } } - return TransitionState.OK; + if (bestT >= float.MaxValue) + return TransitionState.OK; // no collision + + // Rewind the sphere to the contact point (before penetration). + if (bestT < 1f) + { + Vector3 contactPos = currPos + movement * bestT; + // Push slightly back along normal to prevent sitting exactly on the surface. + contactPos += bestNormal * 0.005f; + sp.SetCheckPos(contactPos, sp.CheckCellId); + } + + // Apply wall-slide from the contact point. + return SlideSphere(bestNormal, currPos); } // -----------------------------------------------------------------------