21fd550909
Adds CollisionPrimitives.cs with C# ports of FUN_005384e0 / FUN_00539500 / FUN_00539ba0 / FUN_00539110 / FUN_00539060 / FUN_0053a230 / FUN_0053a040 / FUN_00538eb0 / FUN_00538f50 — covering ray-sphere, sphere-poly contact, find-time-of-collision, face-normal computation, ray-plane intersection, walkable checks, edge-normal slide, and sphere landing. Key findings from cross-referencing with ACE's Polygon.cs: - The edge-perpendicular formula is cross(N, edge) (normal × edge), matching the retail param_1[9/10/8] order in the decompiled loops. - find_time_of_collision uses t = (dot(origin,N)+D) / dot(dir,N); the sign is negative when approaching from above — contact = origin − dir*t. - land_on_sphere only succeeds when the sphere centre is within one radius of the plane (dist < r), which is the "settling onto ground" scenario. 26 new tests green; full suite 367/367 green. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
718 lines
28 KiB
C#
718 lines
28 KiB
C#
using System;
|
||
using System.Numerics;
|
||
|
||
namespace AcDream.Core.Physics;
|
||
|
||
/// <summary>
|
||
/// Pure-static collision primitive routines ported from the retail acclient.exe.
|
||
///
|
||
/// <para>
|
||
/// Each method corresponds to a specific decompiled function from
|
||
/// <c>chunk_00530000.c</c>. Addresses are noted in the per-method XML doc so
|
||
/// they can be cross-checked against the ghidra listing.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// "Polygon" in this context is an AC convex face stored in BSP leaves.
|
||
/// The polygon is described by an array of <see cref="Vector3"/> vertices
|
||
/// (wound counter-clockwise when viewed from the normal side) together with a
|
||
/// precomputed <see cref="Plane"/>. The plane normal is the outward-facing
|
||
/// surface normal; <c>Plane.D</c> is the signed distance from the origin
|
||
/// (positive-D convention: <c>dot(N,p) + D == 0</c> on the surface).
|
||
/// </para>
|
||
/// </summary>
|
||
public static class CollisionPrimitives
|
||
{
|
||
// -----------------------------------------------------------------------
|
||
// Global-equivalent constants (pulled from retail binary DAT addresses)
|
||
// -----------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Floating-point epsilon used for "near-zero" ray-direction checks
|
||
/// (<c>_DAT_007ca628</c> in the retail binary, ≈ 1×10⁻⁴).
|
||
/// </summary>
|
||
public const float Epsilon = 1e-4f;
|
||
|
||
/// <summary>
|
||
/// Relaxed epsilon for the squared-distance comparisons
|
||
/// (<c>_DAT_007ca5d8</c> in the retail binary, ≈ 1×10⁻⁸).
|
||
/// </summary>
|
||
public const float EpsilonSq = 1e-8f;
|
||
|
||
// -----------------------------------------------------------------------
|
||
// 1. SphereIntersectsRay — FUN_005384e0
|
||
// -----------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Tests whether a ray intersects a sphere and, if so, returns the
|
||
/// parametric time <paramref name="t"/> of the nearest intersection.
|
||
///
|
||
/// <para>
|
||
/// Decompiled from <c>FUN_005384e0 @ 0x005384E0</c>.
|
||
/// The sphere is represented by its centre <paramref name="sphereCenter"/>
|
||
/// and <paramref name="sphereRadius"/>. The ray is defined by a start
|
||
/// point <paramref name="rayOrigin"/> and direction vector
|
||
/// <paramref name="rayDir"/> (need not be unit length; <paramref name="t"/>
|
||
/// is in terms of that direction's length).
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// Returns <see langword="false"/> when the origin is <em>inside</em> the
|
||
/// sphere (AC treats that as non-colliding, matching the retail binary).
|
||
/// </para>
|
||
/// </summary>
|
||
/// <param name="sphereCenter">Centre of the sphere.</param>
|
||
/// <param name="sphereRadius">Radius of the sphere.</param>
|
||
/// <param name="rayOrigin">Start point of the ray (param_1[0..2]).</param>
|
||
/// <param name="rayDir">Direction of the ray (param_2[3..5]).</param>
|
||
/// <param name="t">
|
||
/// On success: parametric intersection time ≥ 0.
|
||
/// Undefined on failure.
|
||
/// </param>
|
||
/// <returns><see langword="true"/> if the ray hits the sphere.</returns>
|
||
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
|
||
// -----------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Finds the parametric time at which a ray intersects a plane, returning
|
||
/// the result in <paramref name="t"/>.
|
||
///
|
||
/// <para>
|
||
/// Decompiled from <c>FUN_00539060 @ 0x00539060</c>.
|
||
/// The plane is stored as four floats: normal (XYZ) and D.
|
||
/// The ray is stored as six floats: origin (XYZ) then direction (XYZ).
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// Returns <see langword="false"/> when the ray is parallel to the plane
|
||
/// (dot(dir, normal) ≈ 0) or when the intersection is behind the ray
|
||
/// origin (<c>t < 0</c>).
|
||
/// </para>
|
||
/// </summary>
|
||
/// <param name="plane">
|
||
/// The plane to test. <c>plane.D</c> is the signed offset such that
|
||
/// <c>dot(N,p) + D == 0</c> on the plane surface.
|
||
/// </param>
|
||
/// <param name="rayOrigin">Start point of the ray.</param>
|
||
/// <param name="rayDir">Direction of the ray (need not be normalised).</param>
|
||
/// <param name="t">Parametric intersection distance (≥ 0 on success).</param>
|
||
/// <returns><see langword="true"/> if the ray hits the plane.</returns>
|
||
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
|
||
// -----------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Computes the face normal and plane-distance for an N-gon described by
|
||
/// an ordered list of vertex positions, storing the result in
|
||
/// <paramref name="normal"/> and <paramref name="planeD"/>.
|
||
///
|
||
/// <para>
|
||
/// Decompiled from <c>FUN_00539110 @ 0x00539110</c>.
|
||
/// 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 <c>Plane.D</c>.
|
||
/// </para>
|
||
/// </summary>
|
||
/// <param name="vertices">Polygon vertices in order (≥ 3 required).</param>
|
||
/// <param name="normal">Computed unit normal (zero-vector if degenerate).</param>
|
||
/// <param name="planeD">
|
||
/// Plane constant D such that <c>dot(N,p) + D == 0</c> on the surface.
|
||
/// </param>
|
||
public static void CalcNormal(
|
||
ReadOnlySpan<Vector3> 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
|
||
// -----------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Returns <see langword="true"/> when a sphere overlaps (or just touches)
|
||
/// a convex polygon on the positive-normal side.
|
||
///
|
||
/// <para>
|
||
/// Decompiled from <c>FUN_00539500 @ 0x00539500</c>.
|
||
/// On success the contact point projected onto the polygon plane is written
|
||
/// into <paramref name="contactPoint"/>.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// The algorithm:
|
||
/// <list type="number">
|
||
/// <item>Project sphere centre onto the polygon plane; if the signed
|
||
/// distance exceeds the radius the sphere cannot touch.</item>
|
||
/// <item>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.</item>
|
||
/// </list>
|
||
/// </para>
|
||
/// </summary>
|
||
/// <param name="polyPlane">Plane of the polygon (normal + D).</param>
|
||
/// <param name="vertices">Polygon vertices in winding order.</param>
|
||
/// <param name="sphereCenter">Centre of the sphere.</param>
|
||
/// <param name="sphereRadius">Radius of the sphere.</param>
|
||
/// <param name="contactPoint">
|
||
/// Projected contact point on the polygon plane (valid when result is
|
||
/// <see langword="true"/>).
|
||
/// </param>
|
||
/// <returns>
|
||
/// <see langword="true"/> when the sphere touches the polygon face.
|
||
/// </returns>
|
||
public static bool SphereIntersectsPoly(
|
||
Plane polyPlane,
|
||
ReadOnlySpan<Vector3> 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
|
||
// -----------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Finds the parametric time at which a moving sphere (radius embedded in
|
||
/// <paramref name="sphere"/>[3]) first intersects a convex polygon.
|
||
///
|
||
/// <para>
|
||
/// Decompiled from <c>FUN_00539ba0 @ 0x00539BA0</c>.
|
||
/// The sphere travels from <paramref name="sphere"/>[0..2] in direction
|
||
/// <paramref name="rayDir"/>[0..2]. The radius is <paramref name="sphere"/>[3].
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// Internally:
|
||
/// <list type="number">
|
||
/// <item>Compute the time <c>t</c> at which the sphere's centre reaches
|
||
/// the offset plane (plane pushed out by radius).</item>
|
||
/// <item>Walk each directed edge to verify the intersection point
|
||
/// actually lies inside the polygon (with sphere-radius tolerance at
|
||
/// edges).</item>
|
||
/// </list>
|
||
/// Returns <see langword="false"/> when the ray is parallel to the plane,
|
||
/// when the polygon has zero vertices, or when the intersection is behind
|
||
/// the ray.
|
||
/// </para>
|
||
/// </summary>
|
||
/// <param name="polyPlane">Plane of the polygon.</param>
|
||
/// <param name="vertices">Polygon vertices in winding order.</param>
|
||
/// <param name="sphereOrigin">Start position of the sphere centre.</param>
|
||
/// <param name="sphereRadius">Radius of the sphere.</param>
|
||
/// <param name="rayDir">Normalised movement direction of the sphere.</param>
|
||
/// <param name="t">
|
||
/// Parametric collision time on success. Sign convention matches the
|
||
/// retail binary: <c>t = (dot(origin,N) + D) / dot(dir,N)</c>. When
|
||
/// the ray is approaching the surface from above (dir·N < 0) the
|
||
/// returned t is negative; apply as <c>contact = origin − dir*t</c>.
|
||
/// </param>
|
||
/// <returns>
|
||
/// <see langword="true"/> when a collision is found.
|
||
/// </returns>
|
||
public static bool FindTimeOfCollision(
|
||
Plane polyPlane,
|
||
ReadOnlySpan<Vector3> 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
|
||
// -----------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Returns <see langword="true"/> 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).
|
||
///
|
||
/// <para>
|
||
/// Decompiled from <c>FUN_0053a230 @ 0x0053A230</c>.
|
||
/// Delegates to <see cref="SphereIntersectsPoly"/>; the "walkable" check
|
||
/// additionally requires that <c>dot(polyNormal, movementDir) ≥ 0</c>.
|
||
/// </para>
|
||
/// </summary>
|
||
/// <param name="polyPlane">Plane of the polygon.</param>
|
||
/// <param name="vertices">Polygon vertices.</param>
|
||
/// <param name="sphereCenter">Centre of the sphere.</param>
|
||
/// <param name="sphereRadius">Radius of the sphere.</param>
|
||
/// <param name="movementDir">Normalised movement direction of the sphere.</param>
|
||
/// <returns>
|
||
/// <see langword="true"/> when the polygon is hit and is walkable from
|
||
/// the movement direction.
|
||
/// </returns>
|
||
public static bool HitsWalkable(
|
||
Plane polyPlane,
|
||
ReadOnlySpan<Vector3> 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
|
||
// -----------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Finds a walkable surface collision for a moving sphere and, on hit,
|
||
/// writes the outward-facing edge normal of the first crossed edge into
|
||
/// <paramref name="edgeNormal"/>.
|
||
///
|
||
/// <para>
|
||
/// Decompiled from <c>FUN_0053a040 @ 0x0053A040</c>.
|
||
/// Unlike <see cref="SphereIntersectsPoly"/> (which returns the face
|
||
/// contact point), this variant is interested in <em>which edge</em> was
|
||
/// crossed during movement — useful for computing the slide direction.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// The algorithm:
|
||
/// <list type="number">
|
||
/// <item>Verify the movement direction has a non-zero component along
|
||
/// the plane normal.</item>
|
||
/// <item>Compute the ray-plane intersection time and project the contact
|
||
/// centre onto the plane.</item>
|
||
/// <item>Walk each edge; if the contact lies outside an edge, compute
|
||
/// and normalise the edge perpendicular and return it.</item>
|
||
/// </list>
|
||
/// </para>
|
||
/// </summary>
|
||
/// <param name="polyPlane">Plane of the polygon.</param>
|
||
/// <param name="vertices">Polygon vertices.</param>
|
||
/// <param name="sphereOrigin">Sphere start position.</param>
|
||
/// <param name="movementDir">Sphere movement direction.</param>
|
||
/// <param name="edgeNormal">
|
||
/// Outward-facing (normalised) edge normal on a crossed edge.
|
||
/// Populated only when the method returns <see langword="true"/>.
|
||
/// </param>
|
||
/// <returns>
|
||
/// <see langword="true"/> when a crossed edge was found (sphere would
|
||
/// slide off the polygon).
|
||
/// </returns>
|
||
public static bool FindWalkableCollision(
|
||
Plane polyPlane,
|
||
ReadOnlySpan<Vector3> 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
|
||
// -----------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Computes the parametric distance a sphere must travel to reach a plane
|
||
/// when sliding along it, taking into account the sphere's radius.
|
||
///
|
||
/// <para>
|
||
/// Decompiled from <c>FUN_00538eb0 @ 0x00538EB0</c>.
|
||
/// Returns a positive value when the sphere should move toward the plane,
|
||
/// a negative value when it is moving away, or
|
||
/// <see cref="float.MaxValue"/> / <c>0f</c> for degenerate cases.
|
||
/// </para>
|
||
///
|
||
/// <para>
|
||
/// 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.
|
||
/// </para>
|
||
/// </summary>
|
||
/// <param name="plane">The plane to slide against (normal + D).</param>
|
||
/// <param name="sphereRadius">Radius of the sphere.</param>
|
||
/// <param name="sphereCenter">Current sphere centre.</param>
|
||
/// <param name="movementDir">Desired movement direction (unit vector).</param>
|
||
/// <returns>
|
||
/// Parametric distance along <paramref name="movementDir"/> to the plane
|
||
/// surface, accounting for sphere radius.
|
||
/// Returns <see cref="float.MaxValue"/> when the sphere is already flush
|
||
/// with the plane; returns <c>0f</c> when the movement is perpendicular.
|
||
/// </returns>
|
||
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
|
||
// -----------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// Steps a sphere "down" onto a plane surface, updating its centre in
|
||
/// place when the landing is valid.
|
||
///
|
||
/// <para>
|
||
/// Decompiled from <c>FUN_00538f50 @ 0x00538F50</c>.
|
||
/// The sphere tries to land on the plane by moving
|
||
/// <c>−radius·normal</c> (toward the surface). The move is accepted only
|
||
/// when:
|
||
/// <list type="bullet">
|
||
/// <item>The movement direction has a non-zero component along the normal
|
||
/// (sphere is not travelling parallel to the surface).</item>
|
||
/// <item>The resulting interpolation factor lies in the valid range
|
||
/// <c>[−0.1, walkInterp)</c>.</item>
|
||
/// </list>
|
||
/// When accepted, <paramref name="sphereCenter"/> is modified and
|
||
/// <paramref name="walkInterp"/> is updated.
|
||
/// </para>
|
||
/// </summary>
|
||
/// <param name="plane">Surface plane (normal + D).</param>
|
||
/// <param name="sphereRadius">Radius of the sphere.</param>
|
||
/// <param name="sphereCenter">
|
||
/// Sphere centre — updated in place when landing succeeds.
|
||
/// </param>
|
||
/// <param name="movementDir">
|
||
/// Current movement direction (updated in place).
|
||
/// </param>
|
||
/// <param name="walkInterp">
|
||
/// Walk interpolation factor. Updated to the new interp value when
|
||
/// landing is accepted.
|
||
/// </param>
|
||
/// <returns>
|
||
/// <see langword="true"/> when the sphere successfully landed on the
|
||
/// plane.
|
||
/// </returns>
|
||
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;
|
||
}
|
||
}
|