6b4e7569a3
The BSP collision detection runs in object-local space, but the collision response (normals, push offsets) was being applied directly to world-space SpherePath without rotating back to world space. For rotated objects (trees, rocks, buildings), this caused the push direction to be wrong — pushing the player sideways or into the object instead of away from it. Added localToWorld quaternion parameter to FindCollisions and all helper methods (StepSphereDown, CollideWithPt, NegPolyHitDispatch). All normals and offsets are now transformed via Vector3.Transform(v, localToWorld) before being applied to SpherePath, matching ACE's path.LocalSpacePos.LocalToGlobalVec() pattern. Indoor cell collision uses Quaternion.Identity (cell-local = world). Object collision passes obj.Rotation. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
1832 lines
70 KiB
C#
1832 lines
70 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>
|
||
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
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <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)
|
||
{
|
||
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 = Vector3.Transform(polyHit.Plane.Normal, localToWorld);
|
||
collisions.SetContactPlane(
|
||
new Plane(worldNormal, polyHit.Plane.D * scale),
|
||
path.CheckCellId, false);
|
||
|
||
path.WalkableValid = true;
|
||
path.WalkablePlane = new Plane(worldNormal, 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
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// BSPTree.step_sphere_up — attempt to step over a low obstacle.
|
||
///
|
||
/// <para>
|
||
/// 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).
|
||
/// </para>
|
||
///
|
||
/// <para>ACE: BSPTree.cs step_sphere_up.</para>
|
||
/// </summary>
|
||
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
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <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)
|
||
{
|
||
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);
|
||
}
|
||
|
||
// ----------------------------------------------------------------
|
||
// 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 = L2W(hitPoly.Plane.Normal);
|
||
collisions.SetContactPlane(
|
||
new Plane(worldNormal, hitPoly.Plane.D * scale),
|
||
path.CheckCellId, false);
|
||
|
||
path.WalkableValid = true;
|
||
path.WalkablePlane = new Plane(worldNormal, 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 transforms collision normal from local→global before step_up/slide
|
||
// ----------------------------------------------------------------
|
||
if (obj.State.HasFlag(ObjectInfoState.Contact))
|
||
{
|
||
ResolvedPolygon? hitPoly0 = null;
|
||
Vector3 contact0 = Vector3.Zero;
|
||
|
||
if (SphereIntersectsPolyInternal(root, resolved, sphere0, movement,
|
||
ref hitPoly0, ref contact0))
|
||
{
|
||
var worldNormal = L2W(hitPoly0!.Plane.Normal);
|
||
return StepSphereUp(transition, worldNormal);
|
||
}
|
||
|
||
if (sphere1 is not null)
|
||
{
|
||
ResolvedPolygon? hitPoly1 = null;
|
||
Vector3 contact1 = Vector3.Zero;
|
||
|
||
if (SphereIntersectsPolyInternal(root, resolved, sphere1, movement,
|
||
ref hitPoly1, ref contact1))
|
||
{
|
||
var worldNormal = L2W(hitPoly1!.Plane.Normal);
|
||
return SlideSphere(transition, worldNormal);
|
||
}
|
||
|
||
if (hitPoly1 is not null)
|
||
return NegPolyHitDispatch(path, hitPoly1, false, localToWorld);
|
||
if (hitPoly0 is not null)
|
||
return NegPolyHitDispatch(path, hitPoly0, true, localToWorld);
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
var worldNormal = L2W(hitPoly0!.Plane.Normal);
|
||
path.WalkableAllowance = PhysicsGlobals.LandingZ;
|
||
path.Collide = true;
|
||
collisions.SetCollisionNormal(worldNormal);
|
||
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)
|
||
{
|
||
var worldNormal = L2W(hitPoly1!.Plane.Normal);
|
||
collisions.SetCollisionNormal(worldNormal);
|
||
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.
|
||
// -------------------------------------------------------------------------
|
||
|
||
/// <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;
|
||
}
|
||
}
|