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) <noreply@anthropic.com>
This commit is contained in:
Erik
parent
3997839d1a
commit
8e1230c53b
2 changed files with 176 additions and 39 deletions
|
|
@ -1057,4 +1057,89 @@ public static class BSPQuery
|
|||
return SphereIntersectsPoly(node.NegNode, polygons, vertices,
|
||||
sphereCenter, sphereRadius, out hitPolyId, out hitNormal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 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.
|
||||
/// </summary>
|
||||
public static bool SphereIntersectsPoly(
|
||||
PhysicsBSPNode? node,
|
||||
Dictionary<ushort, Polygon> 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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue