using System;
using System.Numerics;
namespace AcDream.Core.Physics;
///
/// Pure-static collision primitive routines ported from the retail acclient.exe.
///
///
/// Each method corresponds to a specific decompiled function from
/// chunk_00530000.c. Addresses are noted in the per-method XML doc so
/// they can be cross-checked against the ghidra listing.
///
///
///
/// "Polygon" in this context is an AC convex face stored in BSP leaves.
/// The polygon is described by an array of vertices
/// (wound counter-clockwise when viewed from the normal side) together with a
/// precomputed . The plane normal is the outward-facing
/// surface normal; Plane.D is the signed distance from the origin
/// (positive-D convention: dot(N,p) + D == 0 on the surface).
///
///
public static class CollisionPrimitives
{
// -----------------------------------------------------------------------
// Global-equivalent constants (pulled from retail binary DAT addresses)
// -----------------------------------------------------------------------
///
/// Floating-point epsilon used for "near-zero" ray-direction checks
/// (_DAT_007ca628 in the retail binary, ≈ 1×10⁻⁴).
///
public const float Epsilon = 1e-4f;
///
/// Relaxed epsilon for the squared-distance comparisons
/// (_DAT_007ca5d8 in the retail binary, ≈ 1×10⁻⁸).
///
public const float EpsilonSq = 1e-8f;
// -----------------------------------------------------------------------
// 1. SphereIntersectsRay — FUN_005384e0
// -----------------------------------------------------------------------
///
/// Tests whether a ray intersects a sphere and, if so, returns the
/// parametric time of the nearest intersection.
///
///
/// Decompiled from FUN_005384e0 @ 0x005384E0.
/// The sphere is represented by its centre
/// and . The ray is defined by a start
/// point and direction vector
/// (need not be unit length;
/// is in terms of that direction's length).
///
///
///
/// Returns when the origin is inside the
/// sphere (AC treats that as non-colliding, matching the retail binary).
///
///
/// Centre of the sphere.
/// Radius of the sphere.
/// Start point of the ray (param_1[0..2]).
/// Direction of the ray (param_2[3..5]).
///
/// On success: parametric intersection time ≥ 0.
/// Undefined on failure.
///
/// if the ray hits the sphere.
public static bool SphereIntersectsRay(
Vector3 sphereCenter, float sphereRadius,
Vector3 rayOrigin, Vector3 rayDir,
out double t)
{
t = 0.0;
// delta = rayOrigin − sphereCenter
float dx = rayOrigin.X - sphereCenter.X;
float dy = rayOrigin.Y - sphereCenter.Y;
float dz = rayOrigin.Z - sphereCenter.Z;
// c = |delta|² − r² (positive ⟹ origin is outside sphere)
float c = dx * dx + dy * dy + dz * dz - sphereRadius * sphereRadius;
if (c <= 0f)
return false; // origin is inside — not considered an intersection
// a = |rayDir|²
float a = rayDir.X * rayDir.X + rayDir.Y * rayDir.Y + rayDir.Z * rayDir.Z;
if (a < EpsilonSq)
return false; // degenerate ray
// b = −dot(delta, rayDir)
float b = -(dx * rayDir.X + dy * rayDir.Y + dz * rayDir.Z);
// discriminant = b² − c·a
float disc = b * b - c * a;
if (disc < 0f)
return false; // no real intersection
float sqrtDisc = MathF.Sqrt(disc);
// retail: if b < sqrtDisc return (b+sqrtDisc)/a else (b−sqrtDisc)/a
if (b < sqrtDisc)
t = (b + sqrtDisc) / a;
else
t = (b - sqrtDisc) / a;
return true;
}
// -----------------------------------------------------------------------
// 2. ray_plane_intersect — FUN_00539060
// -----------------------------------------------------------------------
///
/// Finds the parametric time at which a ray intersects a plane, returning
/// the result in .
///
///
/// Decompiled from FUN_00539060 @ 0x00539060.
/// The plane is stored as four floats: normal (XYZ) and D.
/// The ray is stored as six floats: origin (XYZ) then direction (XYZ).
///
///
///
/// Returns when the ray is parallel to the plane
/// (dot(dir, normal) ≈ 0) or when the intersection is behind the ray
/// origin (t < 0).
///
///
///
/// The plane to test. plane.D is the signed offset such that
/// dot(N,p) + D == 0 on the plane surface.
///
/// Start point of the ray.
/// Direction of the ray (need not be normalised).
/// Parametric intersection distance (≥ 0 on success).
/// if the ray hits the plane.
public static bool RayPlaneIntersect(
Plane plane,
Vector3 rayOrigin, Vector3 rayDir,
out double t)
{
t = 0.0;
// denom = dot(rayDir, plane.Normal)
float denom = Vector3.Dot(rayDir, plane.Normal);
if (MathF.Abs(denom) < Epsilon)
return false; // ray is (nearly) parallel to plane
// numerator = −(dot(rayOrigin, plane.Normal) + plane.D) / denom
// Note: retail uses a negated constant (_DAT_0079cc48 = −1.0) then
// multiplies, which is equivalent to flipping the sign of
// (dot(origin,N) + D).
float num = -(Vector3.Dot(rayOrigin, plane.Normal) + plane.D);
float tVal = num / denom;
t = tVal;
return tVal >= 0f;
}
// -----------------------------------------------------------------------
// 3. calc_normal — FUN_00539110
// -----------------------------------------------------------------------
///
/// Computes the face normal and plane-distance for an N-gon described by
/// an ordered list of vertex positions, storing the result in
/// and .
///
///
/// Decompiled from FUN_00539110 @ 0x00539110.
/// The algorithm accumulates a Newell-style cross-product sum across all
/// (fan) triangle pairs, then normalises to obtain the unit normal. The
/// plane-distance is the average dot-product of all vertices with the
/// normal, negated — matching how AC stores Plane.D.
///
///
/// Polygon vertices in order (≥ 3 required).
/// Computed unit normal (zero-vector if degenerate).
///
/// Plane constant D such that dot(N,p) + D == 0 on the surface.
///
public static void CalcNormal(
ReadOnlySpan vertices,
out Vector3 normal,
out float planeD)
{
normal = Vector3.Zero;
planeD = 0f;
int n = vertices.Length;
if (n < 3)
return;
// Accumulate cross-product contributions (Newell method)
float accX = 0f, accY = 0f, accZ = 0f;
var v0 = vertices[0];
for (int i = 1; i < n - 1; i++)
{
var vi = vertices[i];
var vi1 = vertices[i + 1];
// edge a = vi − v0, edge b = vi1 − v0
float ax = vi.X - v0.X, ay = vi.Y - v0.Y, az = vi.Z - v0.Z;
float bx = vi1.X - v0.X, by = vi1.Y - v0.Y, bz = vi1.Z - v0.Z;
accX += ay * bz - az * by;
accY += az * bx - ax * bz;
accZ += ax * by - ay * bx;
}
float len = MathF.Sqrt(accX * accX + accY * accY + accZ * accZ);
if (len < EpsilonSq)
return;
float invLen = 1f / len;
normal = new Vector3(accX * invLen, accY * invLen, accZ * invLen);
// planeD = −(average dot(normal, vertex))
float dotSum = 0f;
for (int i = 0; i < n; i++)
dotSum += Vector3.Dot(normal, vertices[i]);
planeD = -(dotSum / n);
}
// -----------------------------------------------------------------------
// 4. sphere_intersects_poly — FUN_00539500
// -----------------------------------------------------------------------
///
/// Returns when a sphere overlaps (or just touches)
/// a convex polygon on the positive-normal side.
///
///
/// Decompiled from FUN_00539500 @ 0x00539500.
/// On success the contact point projected onto the polygon plane is written
/// into .
///
///
///
/// The algorithm:
///
/// - Project sphere centre onto the polygon plane; if the signed
/// distance exceeds the radius the sphere cannot touch.
/// - Walk each directed edge; if the projected contact lies outside
/// an edge check whether it is within the sphere's "inflated" radius —
/// returning immediately when an edge vertex is inside the sphere.
///
///
///
/// Plane of the polygon (normal + D).
/// Polygon vertices in winding order.
/// Centre of the sphere.
/// Radius of the sphere.
///
/// Projected contact point on the polygon plane (valid when result is
/// ).
///
///
/// when the sphere touches the polygon face.
///
public static bool SphereIntersectsPoly(
Plane polyPlane,
ReadOnlySpan vertices,
Vector3 sphereCenter, float sphereRadius,
out Vector3 contactPoint)
{
contactPoint = Vector3.Zero;
// Signed distance from sphere centre to plane
float dist = Vector3.Dot(polyPlane.Normal, sphereCenter) + polyPlane.D;
float rad = sphereRadius - Epsilon;
if (MathF.Abs(dist) > rad)
return false;
// Project sphere centre onto the plane
contactPoint = sphereCenter - polyPlane.Normal * dist;
float radSq = rad * rad - dist * dist; // available slack² for edge tests
int numVerts = vertices.Length;
if (numVerts == 0)
return true;
bool inside = true; // tracks whether contact point is unambiguously inside all edges
int prevIdx = numVerts - 1;
for (int i = 0; i < numVerts; i++)
{
var v0 = vertices[prevIdx];
var v1 = vertices[i];
prevIdx = i;
// edge vector and cross-product with plane normal (perpendicular-to-edge in the plane)
var edge = v1 - v0;
var disp = contactPoint - v0;
// edgePerp = edge × normal projected into plane ≈ cross(normal, edge) negated
var edgePerp = new Vector3(
edge.Z * polyPlane.Normal.Y - edge.Y * polyPlane.Normal.Z,
edge.X * polyPlane.Normal.Z - edge.Z * polyPlane.Normal.X,
edge.Y * polyPlane.Normal.X - edge.X * polyPlane.Normal.Y);
float dp = Vector3.Dot(disp, edgePerp);
if (dp < 0f)
{
// Contact point is outside this edge
float edgePerpLenSq = edgePerp.LengthSquared();
if (edgePerpLenSq * radSq < dp * dp)
return false; // too far outside to be reached by sphere radius
// Check if closest edge vertex is within sphere
float dispEdge = Vector3.Dot(disp, edge);
float edgeLenSq = edge.LengthSquared();
if (dispEdge >= 0f && dispEdge < edgeLenSq)
return true; // closest point on edge is inside sphere
inside = false;
}
// Vertex distance check
if (disp.LengthSquared() <= radSq)
return true;
}
return inside;
}
// -----------------------------------------------------------------------
// 5. find_time_of_collision — FUN_00539ba0
// -----------------------------------------------------------------------
///
/// Finds the parametric time at which a moving sphere (radius embedded in
/// [3]) first intersects a convex polygon.
///
///
/// Decompiled from FUN_00539ba0 @ 0x00539BA0.
/// The sphere travels from [0..2] in direction
/// [0..2]. The radius is [3].
///
///
///
/// Internally:
///
/// - Compute the time t at which the sphere's centre reaches
/// the offset plane (plane pushed out by radius).
/// - Walk each directed edge to verify the intersection point
/// actually lies inside the polygon (with sphere-radius tolerance at
/// edges).
///
/// Returns when the ray is parallel to the plane,
/// when the polygon has zero vertices, or when the intersection is behind
/// the ray.
///
///
/// Plane of the polygon.
/// Polygon vertices in winding order.
/// Start position of the sphere centre.
/// Radius of the sphere.
/// Normalised movement direction of the sphere.
///
/// Parametric collision time on success. Sign convention matches the
/// retail binary: t = (dot(origin,N) + D) / dot(dir,N). When
/// the ray is approaching the surface from above (dir·N < 0) the
/// returned t is negative; apply as contact = origin − dir*t.
///
///
/// when a collision is found.
///
public static bool FindTimeOfCollision(
Plane polyPlane,
ReadOnlySpan vertices,
Vector3 sphereOrigin, float sphereRadius,
Vector3 rayDir,
out float t)
{
t = 0f;
// dot(rayDir, planeNormal) — denominator
float denom = Vector3.Dot(rayDir, polyPlane.Normal);
if (MathF.Abs(denom) < Epsilon)
return false;
int numVerts = vertices.Length;
// t = (dot(origin, N) + D) / dot(dir, N)
// Note: sign is such that contact = origin − dir*t lands on the plane.
float num = Vector3.Dot(sphereOrigin, polyPlane.Normal) + polyPlane.D;
t = num / denom;
// Pre-compute the contact centre (constant per-call)
var contact = sphereOrigin - rayDir * t;
float radSq = sphereRadius * sphereRadius;
bool inside = true;
int prevIdx = numVerts - 1;
for (int i = 0; i < numVerts; i++)
{
var v0 = vertices[prevIdx];
var v1 = vertices[i];
prevIdx = i;
var edge = v1 - v0;
var dispFromV0 = contact - v0;
// edgePerp = cross(N, edge) — inward-facing perpendicular to edge in the polygon plane
var edgePerp = new Vector3(
edge.Z * polyPlane.Normal.Y - edge.Y * polyPlane.Normal.Z,
edge.X * polyPlane.Normal.Z - edge.Z * polyPlane.Normal.X,
edge.Y * polyPlane.Normal.X - edge.X * polyPlane.Normal.Y);
float dp = Vector3.Dot(dispFromV0, edgePerp);
if (dp < 0f)
{
float edgePerpLenSq = edgePerp.LengthSquared();
if (edgePerpLenSq * radSq < dp * dp)
return false;
float dispEdge = Vector3.Dot(dispFromV0, edge);
float edgeLenSq = edge.LengthSquared();
if (dispEdge >= 0f && dispEdge < edgeLenSq)
return true;
inside = false;
}
float dispLenSq = dispFromV0.LengthSquared();
if (dispLenSq < radSq)
return true;
}
// Retail returns the value held in the "inside" tracking variable (1 or 0 pointer)
return inside;
}
// -----------------------------------------------------------------------
// 6. hits_walkable — FUN_0053a230
// -----------------------------------------------------------------------
///
/// Returns when a sphere touches a walkable polygon
/// and the movement direction has a positive component along the polygon
/// normal (i.e. the sphere is approaching from below / from the correct
/// side).
///
///
/// Decompiled from FUN_0053a230 @ 0x0053A230.
/// Delegates to ; the "walkable" check
/// additionally requires that dot(polyNormal, movementDir) ≥ 0.
///
///
/// Plane of the polygon.
/// Polygon vertices.
/// Centre of the sphere.
/// Radius of the sphere.
/// Normalised movement direction of the sphere.
///
/// when the polygon is hit and is walkable from
/// the movement direction.
///
public static bool HitsWalkable(
Plane polyPlane,
ReadOnlySpan vertices,
Vector3 sphereCenter, float sphereRadius,
Vector3 movementDir)
{
// The retail function checks sphere_intersects_solid (FUN_00539750)
// which is the two-sided version of sphere_intersects_poly. For our
// purposes (physics/walkable check) the single-sided version is
// sufficient. The walkable side-check is: dot(normal, movementDir) > 0.
if (Vector3.Dot(polyPlane.Normal, movementDir) < 0f)
return false;
return SphereIntersectsPoly(polyPlane, vertices, sphereCenter, sphereRadius, out _);
}
// -----------------------------------------------------------------------
// 7. find_walkable_collision — FUN_0053a040
// -----------------------------------------------------------------------
///
/// Finds a walkable surface collision for a moving sphere and, on hit,
/// writes the outward-facing edge normal of the first crossed edge into
/// .
///
///
/// Decompiled from FUN_0053a040 @ 0x0053A040.
/// Unlike (which returns the face
/// contact point), this variant is interested in which edge was
/// crossed during movement — useful for computing the slide direction.
///
///
///
/// The algorithm:
///
/// - Verify the movement direction has a non-zero component along
/// the plane normal.
/// - Compute the ray-plane intersection time and project the contact
/// centre onto the plane.
/// - Walk each edge; if the contact lies outside an edge, compute
/// and normalise the edge perpendicular and return it.
///
///
///
/// Plane of the polygon.
/// Polygon vertices.
/// Sphere start position.
/// Sphere movement direction.
///
/// Outward-facing (normalised) edge normal on a crossed edge.
/// Populated only when the method returns .
///
///
/// when a crossed edge was found (sphere would
/// slide off the polygon).
///
public static bool FindWalkableCollision(
Plane polyPlane,
ReadOnlySpan vertices,
Vector3 sphereOrigin,
Vector3 movementDir,
out Vector3 edgeNormal)
{
edgeNormal = Vector3.Zero;
float denom = Vector3.Dot(polyPlane.Normal, movementDir);
if (MathF.Abs(denom) < Epsilon)
return false;
int numVerts = vertices.Length;
// Ray-plane time
float t = (Vector3.Dot(polyPlane.Normal, sphereOrigin) + polyPlane.D) / denom;
// Contact centre projected onto polygon plane
var contact = sphereOrigin - movementDir * t;
int prevIdx = numVerts - 1;
for (int i = 0; i < numVerts; i++)
{
var v0 = vertices[prevIdx];
var v1 = vertices[i];
prevIdx = i;
var edge = v1 - v0;
var disp = contact - v0;
// edgePerp = cross(edge, normal) projected [= normal × edge in retail]
var nx = polyPlane.Normal.X;
var ny = polyPlane.Normal.Y;
var nz = polyPlane.Normal.Z;
var epX = edge.Z * ny - edge.Y * nz;
var epY = edge.X * nz - edge.Z * nx;
var epZ = edge.Y * nx - edge.X * ny;
float dp = disp.X * epX + disp.Y * epY + disp.Z * epZ;
if (dp < 0f)
{
// Contact point is outside this edge — this is the crossed edge
var raw = new Vector3(epX, epY, epZ);
float len = raw.Length();
if (len < EpsilonSq)
return false;
edgeNormal = raw / len;
return true;
}
}
return false;
}
// -----------------------------------------------------------------------
// 8. slide_sphere — FUN_00538eb0
// -----------------------------------------------------------------------
///
/// Computes the parametric distance a sphere must travel to reach a plane
/// when sliding along it, taking into account the sphere's radius.
///
///
/// Decompiled from FUN_00538eb0 @ 0x00538EB0.
/// Returns a positive value when the sphere should move toward the plane,
/// a negative value when it is moving away, or
/// / 0f for degenerate cases.
///
///
///
/// The retail code uses struct offsets into a "Plane" at +0x20 (normal
/// XYZ) and +0x2C (D). A second struct at +0x0C holds the sphere radius.
///
///
/// The plane to slide against (normal + D).
/// Radius of the sphere.
/// Current sphere centre.
/// Desired movement direction (unit vector).
///
/// Parametric distance along to the plane
/// surface, accounting for sphere radius.
/// Returns when the sphere is already flush
/// with the plane; returns 0f when the movement is perpendicular.
///
public static float SlideSphere(
Plane plane, float sphereRadius,
Vector3 sphereCenter, Vector3 movementDir)
{
// Signed distance from sphere centre to plane
float dist = Vector3.Dot(sphereCenter, plane.Normal) + plane.D;
if (MathF.Abs(dist) < sphereRadius)
return float.MaxValue; // already touching — no slide needed
float denom = Vector3.Dot(movementDir, plane.Normal);
if (MathF.Abs(denom) < Epsilon)
return 0f; // movement is parallel to plane
// offset = ±radius (sign chosen by which side the sphere is on)
float offset = dist <= 0f ? -sphereRadius : sphereRadius;
return (offset - dist) / denom;
}
// -----------------------------------------------------------------------
// 9. land_on_sphere — FUN_00538f50
// -----------------------------------------------------------------------
///
/// Steps a sphere "down" onto a plane surface, updating its centre in
/// place when the landing is valid.
///
///
/// Decompiled from FUN_00538f50 @ 0x00538F50.
/// The sphere tries to land on the plane by moving
/// −radius·normal (toward the surface). The move is accepted only
/// when:
///
/// - The movement direction has a non-zero component along the normal
/// (sphere is not travelling parallel to the surface).
/// - The resulting interpolation factor lies in the valid range
/// [−0.1, walkInterp).
///
/// When accepted, is modified and
/// is updated.
///
///
/// Surface plane (normal + D).
/// Radius of the sphere.
///
/// Sphere centre — updated in place when landing succeeds.
///
///
/// Current movement direction (updated in place).
///
///
/// Walk interpolation factor. Updated to the new interp value when
/// landing is accepted.
///
///
/// when the sphere successfully landed on the
/// plane.
///
public static bool LandOnSphere(
Plane plane, float sphereRadius,
ref Vector3 sphereCenter, ref Vector3 movementDir,
ref float walkInterp)
{
// Signed distance from sphere centre to plane
float distToPlane = Vector3.Dot(sphereCenter, plane.Normal) + plane.D;
float denom = Vector3.Dot(movementDir, plane.Normal);
// Retail logic (FUN_00538f50):
// if denom > +Eps → moving away from surface → use (-r - dist) / denom
// if denom in [−Eps, +Eps] → parallel, return 0
// if denom < −Eps → moving toward surface → use (dist - r) / denom
float tLand;
if (denom > Epsilon)
{
// Moving away from surface (along positive normal direction)
tLand = (-sphereRadius - distToPlane) / denom;
}
else if (denom >= -Epsilon)
{
// Parallel to plane
return false;
}
else
{
// Moving toward surface (against positive normal direction)
tLand = (distToPlane - sphereRadius) / denom;
}
// Retail check: newInterp must be in [−0.5, walkInterp)
// (_DAT_007ca630 ≈ −0.5 from usage context in FUN_00538f50)
float newInterp = (1f - tLand) * walkInterp;
if (newInterp >= walkInterp || newInterp < -0.5f)
return false;
// Apply the landing
sphereCenter -= movementDir * tLand;
walkInterp = newInterp;
return true;
}
}