261322b48e
Port the first retail precipice-slide slice from named retail/ACE: terrain and BSP walkable hits now preserve polygon vertices, failed step-down edges back-probe to rediscover the walkable polygon, and edge-slide can run precipice/cliff slide instead of only hard-stopping. Adds pseudocode anchors plus regression coverage for terrain polygon context and loaded-terrain boundary edge-slide. Co-authored-by: Codex <codex@openai.com>
1905 lines
74 KiB
C#
1905 lines
74 KiB
C#
using System.Numerics;
|
||
using DatReaderWriter.Enums;
|
||
using DatReaderWriter.Types;
|
||
using Plane = System.Numerics.Plane;
|
||
|
||
namespace AcDream.Core.Physics;
|
||
|
||
/// <summary>
|
||
/// BSP tree collision queries faithfully ported from the decompiled retail AC client.
|
||
/// Cross-referenced against ACE BSPTree.cs, BSPNode.cs, BSPLeaf.cs, Polygon.cs.
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// Polygons are passed as pre-resolved <see cref="ResolvedPolygon"/> dictionaries
|
||
/// (vertex positions + face plane computed once at cache time) to avoid per-test
|
||
/// vertex lookups. See <see cref="PhysicsDataCache"/>.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// ACE references: BSPTree.cs, BSPNode.cs, BSPLeaf.cs, Polygon.cs.
|
||
/// Decompiled client: chunk_00530000.c, chunk_00539000.c.
|
||
/// </para>
|
||
/// </summary>
|
||
public static class BSPQuery
|
||
{
|
||
// =========================================================================
|
||
// 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.
|
||
// =========================================================================
|
||
|
||
/// <summary>
|
||
/// 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.
|
||
/// </summary>
|
||
internal sealed class CollisionSphere
|
||
{
|
||
public Vector3 Center;
|
||
public float Radius;
|
||
|
||
public CollisionSphere(Vector3 center, float radius)
|
||
{
|
||
Center = center;
|
||
Radius = radius;
|
||
}
|
||
|
||
public CollisionSphere(CollisionSphere other)
|
||
{
|
||
Center = other.Center;
|
||
Radius = other.Radius;
|
||
}
|
||
}
|
||
|
||
// =========================================================================
|
||
// Bounding-sphere intersection helper
|
||
//
|
||
// ACE: Sphere.Intersects(Sphere) — LengthSquared < (r1+r2)^2.
|
||
// Used by every BSP traversal method for broad-phase rejection.
|
||
// =========================================================================
|
||
|
||
/// <summary>
|
||
/// Test whether a query sphere intersects a BSP node's bounding sphere.
|
||
/// ACE: Sphere.Intersects(Sphere other) — d.LengthSquared < (r1+r2)^2.
|
||
/// </summary>
|
||
private static bool NodeIntersects(PhysicsBSPNode node, CollisionSphere sphere)
|
||
{
|
||
var bs = node.BoundingSphere;
|
||
var d = sphere.Center - bs.Origin;
|
||
float r = sphere.Radius + bs.Radius;
|
||
return d.LengthSquared() < r * r;
|
||
}
|
||
|
||
// =========================================================================
|
||
// 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
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Polygon.polygon_hits_sphere_precise — full retail polygon-sphere overlap.
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: Polygon.cs polygon_hits_sphere_precise.</para>
|
||
/// </summary>
|
||
private static bool PolygonHitsSpherePrecise(
|
||
in Plane polyPlane,
|
||
ReadOnlySpan<Vector3> verts,
|
||
Vector3 sphereCenter,
|
||
float sphereRadius,
|
||
ref Vector3 contactPoint)
|
||
{
|
||
int n = verts.Length;
|
||
if (n == 0) return true;
|
||
|
||
float dist = Vector3.Dot(polyPlane.Normal, sphereCenter) + polyPlane.D;
|
||
float rad = sphereRadius - PhysicsGlobals.EPSILON;
|
||
|
||
if (MathF.Abs(dist) > rad) return false;
|
||
|
||
float diff = rad * rad - dist * dist;
|
||
contactPoint = sphereCenter - polyPlane.Normal * dist;
|
||
|
||
int prevIdx = n - 1;
|
||
for (int i = 0; i < n; i++)
|
||
{
|
||
var v = verts[i];
|
||
var lv = verts[prevIdx];
|
||
prevIdx = i;
|
||
|
||
var edge = v - lv;
|
||
var disp = contactPoint - lv;
|
||
var cross = Vector3.Cross(polyPlane.Normal, edge);
|
||
|
||
if (Vector3.Dot(disp, cross) >= 0f) continue;
|
||
|
||
// Contact point is outside this edge — run inner loop.
|
||
prevIdx = n - 1;
|
||
for (int j = 0; j < n; j++)
|
||
{
|
||
v = verts[j];
|
||
lv = verts[prevIdx];
|
||
prevIdx = j;
|
||
|
||
edge = v - lv;
|
||
disp = contactPoint - lv;
|
||
cross = Vector3.Cross(polyPlane.Normal, edge);
|
||
float dispDot = Vector3.Dot(disp, cross);
|
||
|
||
if (dispDot < 0f)
|
||
{
|
||
if (cross.LengthSquared() * diff < dispDot * dispDot)
|
||
return false;
|
||
|
||
float dispEdge = Vector3.Dot(disp, edge);
|
||
if (dispEdge >= 0f && dispEdge <= edge.LengthSquared())
|
||
return true;
|
||
}
|
||
|
||
if (disp.LengthSquared() <= diff)
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// pos_hits_sphere
|
||
// ACE: Polygon.cs pos_hits_sphere
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Polygon.pos_hits_sphere — polygon hit filtered by movement direction.
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: Polygon.cs pos_hits_sphere.</para>
|
||
/// </summary>
|
||
private static bool PosHitsSphere(
|
||
ResolvedPolygon poly,
|
||
CollisionSphere sphere,
|
||
Vector3 movement,
|
||
ref Vector3 contactPoint,
|
||
ref ResolvedPolygon? hitPoly)
|
||
{
|
||
bool hit = PolygonHitsSpherePrecise(
|
||
poly.Plane, poly.Vertices,
|
||
sphere.Center, sphere.Radius,
|
||
ref contactPoint);
|
||
|
||
// ACE: dist = Dot(movement, Plane.Normal); if dist >= 0 return false;
|
||
float moveDot = Vector3.Dot(movement, poly.Plane.Normal);
|
||
if (moveDot >= 0f) return false;
|
||
|
||
if (hit) hitPoly = poly;
|
||
return hit;
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// hits_sphere
|
||
// ACE: Polygon.cs hits_sphere
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Polygon.hits_sphere — static sphere-polygon overlap (no movement culling).
|
||
/// ACE: Polygon.cs hits_sphere.
|
||
/// </summary>
|
||
private static bool HitsSphere(ResolvedPolygon poly, CollisionSphere sphere)
|
||
{
|
||
Vector3 cp = Vector3.Zero;
|
||
return PolygonHitsSpherePrecise(
|
||
poly.Plane, poly.Vertices,
|
||
sphere.Center, sphere.Radius,
|
||
ref cp);
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// walkable_hits_sphere
|
||
// ACE: Polygon.cs walkable_hits_sphere
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Polygon.walkable_hits_sphere — walkable surface test.
|
||
///
|
||
/// <para>
|
||
/// A polygon is walkable only when dot(up, normal) > WalkableAllowance.
|
||
/// polygon_hits_sphere_precise is then called for the overlap.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: Polygon.cs walkable_hits_sphere.</para>
|
||
/// </summary>
|
||
private static bool WalkableHitsSphere(
|
||
ResolvedPolygon poly,
|
||
SpherePath path,
|
||
CollisionSphere sphere,
|
||
Vector3 up)
|
||
{
|
||
float dp = Vector3.Dot(up, poly.Plane.Normal);
|
||
if (dp <= path.WalkableAllowance) return false;
|
||
|
||
Vector3 cp = Vector3.Zero;
|
||
return PolygonHitsSpherePrecise(
|
||
poly.Plane, poly.Vertices,
|
||
sphere.Center, sphere.Radius,
|
||
ref cp);
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// check_walkable / check_small_walkable
|
||
// ACE: Polygon.cs check_walkable (small=false), check_small_walkable (small=true)
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Polygon.check_walkable — check if the sphere projects onto the walkable area.
|
||
///
|
||
/// <para>
|
||
/// 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 <paramref name="small"/> is true the effective radius is halved
|
||
/// (check_small_walkable variant for step-down detection).
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: Polygon.cs check_walkable / check_small_walkable.</para>
|
||
/// </summary>
|
||
private static bool CheckWalkable(
|
||
ResolvedPolygon poly,
|
||
CollisionSphere sphere,
|
||
Vector3 up,
|
||
bool small)
|
||
{
|
||
float angleUp = Vector3.Dot(poly.Plane.Normal, up);
|
||
if (angleUp < PhysicsGlobals.EPSILON) return false;
|
||
|
||
float angle = (Vector3.Dot(poly.Plane.Normal, sphere.Center) + poly.Plane.D) / angleUp;
|
||
var center = sphere.Center - up * angle;
|
||
|
||
float radsum = sphere.Radius * sphere.Radius;
|
||
if (small) radsum *= 0.25f;
|
||
|
||
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);
|
||
float diff = Vector3.Dot(disp, cross);
|
||
|
||
if (diff < 0f)
|
||
{
|
||
if (cross.LengthSquared() * radsum < diff * diff)
|
||
return false;
|
||
|
||
float dispEdge = Vector3.Dot(disp, edge);
|
||
if (dispEdge >= 0f && dispEdge <= edge.LengthSquared())
|
||
return true;
|
||
|
||
return false;
|
||
}
|
||
|
||
if (disp.LengthSquared() <= radsum)
|
||
return true;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// adjust_sphere_to_plane
|
||
// ACE: Polygon.cs adjust_sphere_to_plane
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Polygon.adjust_sphere_to_plane — slide sphere to rest on polygon plane.
|
||
///
|
||
/// <para>
|
||
/// Computes the parametric distance along <paramref name="movement"/> at which
|
||
/// the sphere first contacts the plane, moves the sphere to that position,
|
||
/// and updates <see cref="SpherePath.WalkInterp"/>.
|
||
/// Returns false if the contact is outside the interp window.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: Polygon.cs adjust_sphere_to_plane.</para>
|
||
/// </summary>
|
||
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
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Polygon.find_crossed_edge — find the edge the sphere center has crossed.
|
||
///
|
||
/// <para>
|
||
/// 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 <paramref name="normal"/>.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: Polygon.cs find_crossed_edge.</para>
|
||
/// </summary>
|
||
internal static bool FindCrossedEdge(
|
||
Plane polyPlane,
|
||
ReadOnlySpan<Vector3> verts,
|
||
Vector3 sphereCenter,
|
||
Vector3 up,
|
||
out Vector3 normal)
|
||
{
|
||
normal = Vector3.Zero;
|
||
|
||
float angleUp = Vector3.Dot(polyPlane.Normal, up);
|
||
if (MathF.Abs(angleUp) < PhysicsGlobals.EPSILON) return false;
|
||
|
||
float angle = (Vector3.Dot(polyPlane.Normal, sphereCenter) + polyPlane.D) / angleUp;
|
||
var center = sphereCenter - up * angle;
|
||
|
||
int n = verts.Length;
|
||
int prevIdx = n - 1;
|
||
|
||
for (int i = 0; i < n; i++)
|
||
{
|
||
var v = verts[i];
|
||
var lv = verts[prevIdx];
|
||
prevIdx = i;
|
||
|
||
var edge = v - lv;
|
||
var disp = center - lv;
|
||
var cross = Vector3.Cross(polyPlane.Normal, edge);
|
||
|
||
if (Vector3.Dot(disp, cross) < 0f)
|
||
{
|
||
float crossLen = cross.Length();
|
||
normal = crossLen > 0f ? cross * (1f / crossLen) : Vector3.Zero;
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
private static bool FindCrossedEdge(
|
||
ResolvedPolygon poly,
|
||
CollisionSphere sphere,
|
||
Vector3 up,
|
||
ref Vector3 normal)
|
||
{
|
||
if (!FindCrossedEdge(poly.Plane, poly.Vertices, sphere.Center, up, out var crossedNormal))
|
||
return false;
|
||
|
||
normal = crossedNormal;
|
||
return true;
|
||
}
|
||
|
||
private static Vector3 TransformNormal(Vector3 normal, Quaternion localToWorld)
|
||
{
|
||
var worldNormal = Vector3.Transform(normal, localToWorld);
|
||
return worldNormal.LengthSquared() > PhysicsGlobals.EpsilonSq
|
||
? Vector3.Normalize(worldNormal)
|
||
: Vector3.UnitZ;
|
||
}
|
||
|
||
private static Vector3[] TransformVertices(
|
||
ReadOnlySpan<Vector3> vertices,
|
||
Quaternion localToWorld,
|
||
float scale,
|
||
Vector3 worldOrigin)
|
||
{
|
||
var result = new Vector3[vertices.Length];
|
||
for (int i = 0; i < vertices.Length; i++)
|
||
result[i] = Vector3.Transform(vertices[i] * scale, localToWorld) + worldOrigin;
|
||
return result;
|
||
}
|
||
|
||
private static Plane BuildWorldPlane(Vector3 worldNormal, ReadOnlySpan<Vector3> worldVertices)
|
||
{
|
||
float d = worldVertices.Length > 0
|
||
? -Vector3.Dot(worldNormal, worldVertices[0])
|
||
: 0f;
|
||
return new Plane(worldNormal, d);
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// adjust_to_placement_poly
|
||
// ACE: Polygon.cs adjust_to_placement_poly
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Polygon.adjust_to_placement_poly — push sphere(s) out of a solid polygon.
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: Polygon.cs adjust_to_placement_poly.</para>
|
||
/// </summary>
|
||
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
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Polygon.adjust_sphere_to_poly — compute parametric contact time.
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: Polygon.cs adjust_sphere_to_poly.</para>
|
||
/// </summary>
|
||
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
|
||
//
|
||
// 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)
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPNode.sphere_intersects_poly — tree traversal to find a polygon hit.
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// ACE: BSPNode.cs sphere_intersects_poly (internal),
|
||
/// BSPLeaf.cs sphere_intersects_poly (leaf).
|
||
/// </para>
|
||
/// </summary>
|
||
private static bool SphereIntersectsPolyInternal(
|
||
PhysicsBSPNode? node,
|
||
Dictionary<ushort, ResolvedPolygon> resolved,
|
||
CollisionSphere sphere,
|
||
Vector3 movement,
|
||
ref ResolvedPolygon? hitPoly,
|
||
ref Vector3 contactPoint)
|
||
{
|
||
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)
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPNode.find_walkable — traverse BSP to find walkable surfaces and adjust sphere.
|
||
///
|
||
/// <para>
|
||
/// 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 <paramref name="changed"/> is set true.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// Note: validPos is a reference type so mutations propagate back to the caller
|
||
/// automatically, mirroring ACE's by-ref Sphere passing.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// ACE: BSPNode.cs find_walkable (internal), BSPLeaf.cs find_walkable (leaf).
|
||
/// </para>
|
||
/// </summary>
|
||
private static void FindWalkableInternal(
|
||
PhysicsBSPNode? node,
|
||
Dictionary<ushort, ResolvedPolygon> 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)
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPNode.hits_walkable — check if sphere touches any walkable polygon.
|
||
///
|
||
/// <para>
|
||
/// At leaves combines walkable_hits_sphere + check_small_walkable.
|
||
/// Used by the CheckWalkable dispatch path.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// ACE: BSPNode.cs hits_walkable (internal), BSPLeaf.cs hits_walkable (leaf).
|
||
/// </para>
|
||
/// </summary>
|
||
private static bool HitsWalkableInternal(
|
||
PhysicsBSPNode? node,
|
||
Dictionary<ushort, ResolvedPolygon> 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)
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPNode.sphere_intersects_solid — check if sphere overlaps solid geometry.
|
||
///
|
||
/// <para>
|
||
/// At leaves: if centerCheck is true and the leaf is marked Solid, returns true
|
||
/// immediately. Otherwise tests each polygon with hits_sphere.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// The centerCheck flag tracks which side of each splitting plane the sphere
|
||
/// center is on; propagated faithfully per ACE's logic.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// ACE: BSPNode.cs sphere_intersects_solid (internal),
|
||
/// BSPLeaf.cs sphere_intersects_solid (leaf).
|
||
/// </para>
|
||
/// </summary>
|
||
private static bool SphereIntersectsSolidInternal(
|
||
PhysicsBSPNode? node,
|
||
Dictionary<ushort, ResolvedPolygon> resolved,
|
||
CollisionSphere sphere,
|
||
bool centerCheck)
|
||
{
|
||
if (node is null) return false;
|
||
|
||
// Leaf.
|
||
if (node.Type == BSPNodeType.Leaf)
|
||
{
|
||
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
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPNode.sphere_intersects_solid_poly — find solid polygon for placement.
|
||
///
|
||
/// <para>
|
||
/// Like sphere_intersects_solid but additionally records the specific polygon
|
||
/// hit so adjust_to_placement_poly can push the sphere out of it.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// ACE: BSPNode.cs sphere_intersects_solid_poly (internal),
|
||
/// BSPLeaf.cs sphere_intersects_solid_poly (leaf).
|
||
/// </para>
|
||
/// </summary>
|
||
private static bool SphereIntersectsSolidPolyInternal(
|
||
PhysicsBSPNode? node,
|
||
Dictionary<ushort, ResolvedPolygon> 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
|
||
// =========================================================================
|
||
|
||
/// <summary>
|
||
/// BSPNode.point_inside_cell_bsp — test if a 3D point is inside the cell BSP.
|
||
///
|
||
/// <para>
|
||
/// Follows the front side of each splitting plane. A point is inside when it
|
||
/// reaches a front leaf or null PosNode (solid interior).
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: BSPNode.cs point_inside_cell_bsp.</para>
|
||
/// </summary>
|
||
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
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPTree.adjust_to_plane — binary-search for non-penetrating sphere position.
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: BSPTree.cs adjust_to_plane.</para>
|
||
/// </summary>
|
||
private static bool AdjustToPlane(
|
||
PhysicsBSPNode root,
|
||
Dictionary<ushort, ResolvedPolygon> 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;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// check_walkable — BSPTree level
|
||
// ACE: BSPTree.cs check_walkable
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// 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.
|
||
/// </summary>
|
||
private static TransitionState CheckWalkableDispatch(
|
||
PhysicsBSPNode root,
|
||
Dictionary<ushort, ResolvedPolygon> 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
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPTree.step_sphere_down — probe downward to land on a walkable surface.
|
||
///
|
||
/// <para>
|
||
/// Computes a downward movement from StepDownAmt × WalkInterp, runs
|
||
/// find_walkable to locate a surface, updates CheckPos and CollisionInfo
|
||
/// contact plane if one is found.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: BSPTree.cs step_sphere_down.</para>
|
||
/// </summary>
|
||
private static TransitionState StepSphereDown(
|
||
PhysicsBSPNode root,
|
||
Dictionary<ushort, ResolvedPolygon> resolved,
|
||
Transition transition,
|
||
CollisionSphere checkPos,
|
||
Vector3 up,
|
||
float scale,
|
||
Quaternion localToWorld = default,
|
||
Vector3 worldOrigin = default)
|
||
{
|
||
if (localToWorld == default) localToWorld = Quaternion.Identity;
|
||
|
||
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)
|
||
{
|
||
// ACE: path.LocalSpacePos.LocalToGlobalVec(adjusted) * scale
|
||
var adjusted = validPos.Center - checkPos.Center;
|
||
var offset = Vector3.Transform(adjusted, localToWorld) * scale;
|
||
path.AddOffsetToCheckPos(offset);
|
||
|
||
var worldNormal = TransformNormal(polyHit.Plane.Normal, localToWorld);
|
||
var worldVertices = TransformVertices(polyHit.Vertices, localToWorld, scale, worldOrigin);
|
||
var worldPlane = BuildWorldPlane(worldNormal, worldVertices);
|
||
collisions.SetContactPlane(worldPlane, path.CheckCellId, false);
|
||
|
||
path.SetWalkable(worldPlane, worldVertices, Vector3.UnitZ);
|
||
|
||
return TransitionState.Adjusted;
|
||
}
|
||
|
||
return TransitionState.OK;
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// step_sphere_up — BSPTree level
|
||
// ACE: BSPTree.cs step_sphere_up
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPTree.step_sphere_up — attempt to step over a low obstacle.
|
||
///
|
||
/// <para>
|
||
/// Calls <see cref="Transition.DoStepUp"/> which probes upward then steps
|
||
/// down to find a walkable landing surface. If the step-up succeeds the
|
||
/// sphere's CheckPos is already updated and we return OK. If it fails we
|
||
/// fall back to StepUpSlide: clear the contact plane and slide along the
|
||
/// collision normal.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// ACE: BSPTree.step_sphere_up calls transition.StepUp(globNormal);
|
||
/// on false → SpherePath.StepUpSlide(transition).
|
||
/// Named-retail: BSPTREE::step_sphere_up.
|
||
/// </para>
|
||
/// </summary>
|
||
private static TransitionState StepSphereUp(
|
||
Transition transition,
|
||
Vector3 collisionNormal,
|
||
PhysicsEngine engine)
|
||
{
|
||
if (transition.DoStepUp(collisionNormal, engine!))
|
||
return TransitionState.OK;
|
||
|
||
return transition.SpherePath.StepUpSlide(transition);
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// slide_sphere — BSPTree level
|
||
// ACE: BSPTree.cs slide_sphere
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPTree.slide_sphere — apply sliding collision response.
|
||
///
|
||
/// <para>
|
||
/// Sets the sliding normal on CollisionInfo so the outer transition loop
|
||
/// applies a wall-slide projection.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: BSPTree.cs slide_sphere — calls GlobalSphere[0].SlideSphere.</para>
|
||
/// </summary>
|
||
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
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPTree.collide_with_pt — PerfectClip collision with binary-search adjustment.
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: BSPTree.cs collide_with_pt.</para>
|
||
/// </summary>
|
||
private static TransitionState CollideWithPt(
|
||
PhysicsBSPNode root,
|
||
Dictionary<ushort, ResolvedPolygon> resolved,
|
||
Transition transition,
|
||
CollisionSphere checkPos,
|
||
Vector3 curPos,
|
||
ResolvedPolygon hitPoly,
|
||
Vector3 contactPoint,
|
||
float scale,
|
||
Quaternion localToWorld = default)
|
||
{
|
||
if (localToWorld == default) localToWorld = Quaternion.Identity;
|
||
|
||
var obj = transition.ObjectInfo;
|
||
var path = transition.SpherePath;
|
||
var collisions = transition.CollisionInfo;
|
||
|
||
// ACE: path.LocalSpacePos.LocalToGlobalVec(hitPoly.Plane.Normal)
|
||
var collisionNormal = Vector3.Transform(hitPoly.Plane.Normal, localToWorld);
|
||
|
||
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;
|
||
// ACE: path.LocalSpacePos.LocalToGlobalVec(adjusted) * scale
|
||
var offset = Vector3.Transform(adjusted, localToWorld) * scale;
|
||
path.AddOffsetToCheckPos(offset);
|
||
|
||
return TransitionState.Adjusted;
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// NegPolyHit — BSPTree level
|
||
// ACE: BSPTree.cs NegPolyHit
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPTree.NegPolyHit — record a negative-polygon hit for deferred processing.
|
||
/// ACE: BSPTree.cs NegPolyHit — calls path.SetNegPolyHit(stepUp, collisionNormal).
|
||
/// </summary>
|
||
private static TransitionState NegPolyHitDispatch(
|
||
SpherePath path,
|
||
ResolvedPolygon hitPoly,
|
||
bool stepUp,
|
||
Quaternion localToWorld = default)
|
||
{
|
||
if (localToWorld == default) localToWorld = Quaternion.Identity;
|
||
path.NegPolyHit = true;
|
||
path.NegStepUp = stepUp;
|
||
// ACE: path.LocalSpacePos.LocalToGlobalVec(hitPoly.Plane.Normal)
|
||
path.NegCollisionNormal = Vector3.Transform(hitPoly.Plane.Normal, localToWorld);
|
||
return TransitionState.OK;
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// placement_insert — BSPTree level
|
||
// ACE: BSPTree.cs placement_insert
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPTree.placement_insert — iterative placement that pushes sphere(s) out of solids.
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: BSPTree.cs placement_insert.</para>
|
||
/// </summary>
|
||
private static TransitionState PlacementInsert(
|
||
PhysicsBSPNode root,
|
||
Dictionary<ushort, ResolvedPolygon> 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.
|
||
// =========================================================================
|
||
|
||
/// <summary>
|
||
/// BSPTree.find_collisions — the 6-path BSP collision dispatcher.
|
||
///
|
||
/// <para>
|
||
/// Dispatches to one of six collision paths depending on SpherePath flags:
|
||
/// <list type="number">
|
||
/// <item>Placement / Ethereal → sphere_intersects_solid</item>
|
||
/// <item>CheckWalkable → hits_walkable</item>
|
||
/// <item>StepDown → step_sphere_down (find_walkable)</item>
|
||
/// <item>Collide → find_walkable (land on surface)</item>
|
||
/// <item>Contact → sphere_intersects_poly + step_sphere_up / slide</item>
|
||
/// <item>Default → sphere_intersects_poly + collide_with_pt / land</item>
|
||
/// </list>
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// ACE: BSPTree.cs find_collisions.
|
||
/// Decompiled: chunk_00530000.c.
|
||
/// </para>
|
||
/// </summary>
|
||
/// <param name="root">Root of the physics BSP tree.</param>
|
||
/// <param name="resolved">Pre-resolved polygon dictionary (vertices + plane).</param>
|
||
/// <param name="transition">Current transition state.</param>
|
||
/// <param name="localSphere">Primary sphere in object-local space (index 0).</param>
|
||
/// <param name="localSphere1">Second sphere in object-local space (head), or null.</param>
|
||
/// <param name="localCurrCenter">Previous center of the primary sphere in local space.</param>
|
||
/// <param name="localSpaceZ">Up vector in object-local space (usually Vector3.UnitZ).</param>
|
||
/// <param name="scale">Scale factor for the collision object.</param>
|
||
/// <param name="localToWorld">
|
||
/// Rotation that transforms vectors from object-local space back to world space.
|
||
/// ACE: path.LocalSpacePos.LocalToGlobalVec(). For indoor cells with identity
|
||
/// transform, pass Quaternion.Identity. For rotated objects, pass the object's
|
||
/// rotation quaternion.
|
||
/// </param>
|
||
public static TransitionState FindCollisions(
|
||
PhysicsBSPNode? root,
|
||
Dictionary<ushort, ResolvedPolygon> resolved,
|
||
Transition transition,
|
||
DatReaderWriter.Types.Sphere localSphere,
|
||
DatReaderWriter.Types.Sphere? localSphere1,
|
||
Vector3 localCurrCenter,
|
||
Vector3 localSpaceZ,
|
||
float scale,
|
||
Quaternion localToWorld = default,
|
||
PhysicsEngine? engine = null,
|
||
Vector3 worldOrigin = default)
|
||
{
|
||
if (root is null) return TransitionState.OK;
|
||
// Default quaternion (0,0,0,0) → treat as identity
|
||
if (localToWorld == default) localToWorld = Quaternion.Identity;
|
||
|
||
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;
|
||
|
||
// Helper: transform a local-space vector to world space.
|
||
// ACE: path.LocalSpacePos.LocalToGlobalVec(v)
|
||
Vector3 L2W(Vector3 v) => Vector3.Transform(v, localToWorld);
|
||
|
||
// ----------------------------------------------------------------
|
||
// Path 1: Placement or Ethereal → sphere_intersects_solid
|
||
// ----------------------------------------------------------------
|
||
if (path.InsertType == InsertType.Placement || obj.Ethereal)
|
||
{
|
||
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
|
||
// ----------------------------------------------------------------
|
||
if (path.CheckWalkable)
|
||
{
|
||
return CheckWalkableDispatch(root, resolved, path, sphere0, localSpaceZ);
|
||
}
|
||
|
||
// ----------------------------------------------------------------
|
||
// Path 3: StepDown → step_sphere_down
|
||
// ----------------------------------------------------------------
|
||
if (path.StepDown)
|
||
{
|
||
return StepSphereDown(root, resolved, transition, sphere0, localSpaceZ, scale, localToWorld, worldOrigin);
|
||
}
|
||
|
||
// ----------------------------------------------------------------
|
||
// Path 4: Collide → find_walkable (land on surface)
|
||
// ACE transforms offset and plane normal from local→global
|
||
// ----------------------------------------------------------------
|
||
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)
|
||
{
|
||
// ACE: var offset = LocalToGlobalVec(validPos.Center - localSphere.Center) * scale
|
||
var localOffset = validPos.Center - sphere0.Center;
|
||
var worldOffset = L2W(localOffset) * scale;
|
||
path.AddOffsetToCheckPos(worldOffset);
|
||
|
||
var worldNormal = TransformNormal(hitPoly.Plane.Normal, localToWorld);
|
||
var worldVertices = TransformVertices(hitPoly.Vertices, localToWorld, scale, worldOrigin);
|
||
var worldPlane = BuildWorldPlane(worldNormal, worldVertices);
|
||
collisions.SetContactPlane(worldPlane, path.CheckCellId, false);
|
||
|
||
path.SetWalkable(worldPlane, worldVertices, Vector3.UnitZ);
|
||
|
||
return TransitionState.Adjusted;
|
||
}
|
||
return TransitionState.OK;
|
||
}
|
||
|
||
// ----------------------------------------------------------------
|
||
// Path 5: Contact (grounded) — sphere_intersects_poly + step_sphere_up
|
||
//
|
||
// A grounded mover hits a polygon. Retail calls BSPTREE::step_sphere_up,
|
||
// which runs CTransition::step_up (upward probe + step-down scan). If the
|
||
// obstacle is short enough the sphere climbs it; if too tall, it falls back
|
||
// to StepUpSlide (clear contact-plane, slide along StepUpNormal).
|
||
//
|
||
// ACE: BSPTree.find_collisions → step_sphere_up (BSPTree.cs, path 5 branch).
|
||
// Named-retail: BSPTREE::find_collisions Contact branch → step_sphere_up.
|
||
// ----------------------------------------------------------------
|
||
if (obj.State.HasFlag(ObjectInfoState.Contact))
|
||
{
|
||
ResolvedPolygon? hitPoly0 = null;
|
||
Vector3 contact0 = Vector3.Zero;
|
||
|
||
bool hit0 = SphereIntersectsPolyInternal(root, resolved, sphere0, movement,
|
||
ref hitPoly0, ref contact0);
|
||
|
||
if (hit0 || hitPoly0 is not null)
|
||
{
|
||
var worldNormal = L2W(hitPoly0!.Plane.Normal);
|
||
// L.2.3b (2026-04-29): recursion guard. Retail
|
||
// (acclient_2013_pseudo_c.txt:272954) gates step_sphere_up on
|
||
// `if (sp.step_up == 0 && sp.step_down == 0)`. Without this,
|
||
// the inner TransitionalInsert spawned by DoStepDown re-enters
|
||
// FindObjCollisions, hits the same wall, and recursively
|
||
// re-invokes step-up — churning the contact plane until
|
||
// numAttempts decays. Mid-recursion we fall back to wall-slide.
|
||
if (engine is not null && !path.StepUp && !path.StepDown)
|
||
return StepSphereUp(transition, worldNormal, engine);
|
||
|
||
// No engine OR step-up/step-down already in progress — fall
|
||
// back to wall-slide so the inner sphere doesn't recurse.
|
||
collisions.SetCollisionNormal(worldNormal);
|
||
collisions.SetSlidingNormal(worldNormal);
|
||
return TransitionState.Slid;
|
||
}
|
||
|
||
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)
|
||
{
|
||
var worldNormal = L2W(hitPoly1!.Plane.Normal);
|
||
// L.2.3b: same recursion guard as the foot-sphere branch.
|
||
if (engine is not null && !path.StepUp && !path.StepDown)
|
||
return StepSphereUp(transition, worldNormal, engine);
|
||
|
||
collisions.SetCollisionNormal(worldNormal);
|
||
collisions.SetSlidingNormal(worldNormal);
|
||
return TransitionState.Slid;
|
||
}
|
||
}
|
||
|
||
return TransitionState.OK;
|
||
}
|
||
|
||
// ----------------------------------------------------------------
|
||
// Path 6: Default — sphere_intersects_poly → collide_with_pt / land
|
||
// ACE transforms normals from local→global
|
||
// ----------------------------------------------------------------
|
||
{
|
||
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, localToWorld);
|
||
}
|
||
|
||
// ─── SetCollide response ─────────────────────────────────
|
||
// Airborne sphere hits a polygon. Per retail, call SetCollide
|
||
// which saves backup position, records StepUpNormal = worldNormal,
|
||
// and sets WalkInterp=1. TransitionalInsert's Collide branch will
|
||
// then re-test as Placement to confirm we can land on the surface.
|
||
//
|
||
// ACE: BSPTree.find_collisions default branch → SpherePath.SetCollide
|
||
// + return Adjusted.
|
||
// Named-retail: BSPTREE::find_collisions airborne branch → set_collide.
|
||
var worldNormal0 = L2W(hitPoly0!.Plane.Normal);
|
||
path.SetCollide(worldNormal0);
|
||
path.WalkableAllowance = PhysicsGlobals.LandingZ;
|
||
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)
|
||
{
|
||
// Head sphere hit: same SetCollide response.
|
||
var worldNormal1 = L2W(hitPoly1!.Plane.Normal);
|
||
path.SetCollide(worldNormal1);
|
||
path.WalkableAllowance = PhysicsGlobals.LandingZ;
|
||
return TransitionState.Adjusted;
|
||
}
|
||
}
|
||
}
|
||
|
||
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.
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Legacy overload: BSPTree.find_collisions accepting raw Polygon + VertexArray.
|
||
/// Prefer calling <see cref="FindCollisions(PhysicsBSPNode?, Dictionary{ushort, ResolvedPolygon}, Transition, Sphere, Sphere?, Vector3, Vector3, float)"/>
|
||
/// with pre-resolved data when available.
|
||
/// </summary>
|
||
public static TransitionState FindCollisions(
|
||
PhysicsBSPNode? root,
|
||
Dictionary<ushort, DatReaderWriter.Types.Polygon> 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.
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Static sphere-BSP overlap test with no movement-direction culling.
|
||
///
|
||
/// <para>
|
||
/// 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 <see cref="FindCollisions"/>.
|
||
/// </para>
|
||
/// </summary>
|
||
public static bool SphereIntersectsPoly(
|
||
PhysicsBSPNode? node,
|
||
Dictionary<ushort, DatReaderWriter.Types.Polygon> 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<ushort, ResolvedPolygon> 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 (ushort polyId in node.Polygons)
|
||
{
|
||
if (!resolved.TryGetValue(polyId, out var poly)) continue;
|
||
|
||
Vector3 cp = Vector3.Zero;
|
||
if (PolygonHitsSpherePrecise(poly.Plane, poly.Vertices,
|
||
center, radius, ref cp))
|
||
{
|
||
hitPolyId = polyId;
|
||
hitNormal = poly.Plane.Normal;
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
float splitDist = Vector3.Dot(node.SplittingPlane.Normal, center) + node.SplittingPlane.D;
|
||
float reach = radius - PhysicsGlobals.EPSILON;
|
||
|
||
if (splitDist >= reach)
|
||
return SphereIntersectsPolyStaticRecurse(node.PosNode, resolved,
|
||
center, radius, ref hitPolyId, ref hitNormal);
|
||
|
||
if (splitDist <= -reach)
|
||
return SphereIntersectsPolyStaticRecurse(node.NegNode, resolved,
|
||
center, radius, ref hitPolyId, ref hitNormal);
|
||
|
||
if (SphereIntersectsPolyStaticRecurse(node.PosNode, resolved,
|
||
center, radius, ref hitPolyId, ref hitNormal))
|
||
return true;
|
||
|
||
return SphereIntersectsPolyStaticRecurse(node.NegNode, resolved,
|
||
center, radius, ref hitPolyId, ref hitNormal);
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// Legacy SphereIntersectsPolyWithTime — swept-sphere BSP query.
|
||
// Used by FindObjCollisions in TransitionTypes.cs.
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Movement-aware sphere-BSP intersection that returns the earliest hit time.
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
/// </summary>
|
||
public static bool SphereIntersectsPolyWithTime(
|
||
PhysicsBSPNode? node,
|
||
Dictionary<ushort, DatReaderWriter.Types.Polygon> 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;
|
||
if (node is null) return false;
|
||
|
||
var resolved = BuildResolved(polygons, vertices);
|
||
|
||
SphereIntersectsPolyWithTimeRecurse(node, resolved,
|
||
sphereCenter, sphereRadius, movement,
|
||
ref hitPolyId, ref hitNormal, ref hitTime);
|
||
|
||
return hitTime < float.MaxValue;
|
||
}
|
||
|
||
private static void SphereIntersectsPolyWithTimeRecurse(
|
||
PhysicsBSPNode? node,
|
||
Dictionary<ushort, ResolvedPolygon> resolved,
|
||
Vector3 center,
|
||
float radius,
|
||
Vector3 movement,
|
||
ref ushort hitPolyId,
|
||
ref Vector3 hitNormal,
|
||
ref float bestTime)
|
||
{
|
||
if (node is null) 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;
|
||
|
||
if (node.Type == BSPNodeType.Leaf)
|
||
{
|
||
foreach (ushort polyId in node.Polygons)
|
||
{
|
||
if (!resolved.TryGetValue(polyId, out var poly)) continue;
|
||
|
||
// Front-face cull.
|
||
if (Vector3.Dot(movement, poly.Plane.Normal) >= 0f) continue;
|
||
|
||
// Test at start position.
|
||
Vector3 cp = Vector3.Zero;
|
||
if (PolygonHitsSpherePrecise(poly.Plane, poly.Vertices, center, radius, ref cp))
|
||
{
|
||
if (0f < bestTime)
|
||
{
|
||
bestTime = 0f;
|
||
hitPolyId = polyId;
|
||
hitNormal = poly.Plane.Normal;
|
||
}
|
||
continue;
|
||
}
|
||
|
||
// Test at end position.
|
||
var endCenter = center + movement;
|
||
if (PolygonHitsSpherePrecise(poly.Plane, poly.Vertices, endCenter, radius, ref cp))
|
||
{
|
||
if (1f < bestTime)
|
||
{
|
||
bestTime = 1f;
|
||
hitPolyId = polyId;
|
||
hitNormal = poly.Plane.Normal;
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
float splitDist = Vector3.Dot(node.SplittingPlane.Normal, center) + node.SplittingPlane.D;
|
||
float reach = radius + movement.Length();
|
||
|
||
if (splitDist >= reach)
|
||
{
|
||
SphereIntersectsPolyWithTimeRecurse(node.PosNode, resolved,
|
||
center, radius, movement, ref hitPolyId, ref hitNormal, ref bestTime);
|
||
return;
|
||
}
|
||
|
||
if (splitDist <= -reach)
|
||
{
|
||
SphereIntersectsPolyWithTimeRecurse(node.NegNode, resolved,
|
||
center, radius, movement, ref hitPolyId, ref hitNormal, ref bestTime);
|
||
return;
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
// =========================================================================
|
||
// 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<ushort, ResolvedPolygon> BuildResolved(
|
||
Dictionary<ushort, DatReaderWriter.Types.Polygon> polygons,
|
||
DatReaderWriter.Types.VertexArray vertices)
|
||
{
|
||
var resolved = new Dictionary<ushort, ResolvedPolygon>(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;
|
||
}
|
||
}
|