feat(physics): port 9 collision primitives from acclient.exe (chunk_00530000.c)
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>
This commit is contained in:
Erik
parent
50c0704ada
commit
21fd550909
2 changed files with 1201 additions and 0 deletions
718
src/AcDream.Core/Physics/CollisionPrimitives.cs
Normal file
718
src/AcDream.Core/Physics/CollisionPrimitives.cs
Normal file
|
|
@ -0,0 +1,718 @@
|
|||
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;
|
||||
}
|
||||
}
|
||||
483
tests/AcDream.Core.Tests/Physics/CollisionPrimitivesTests.cs
Normal file
483
tests/AcDream.Core.Tests/Physics/CollisionPrimitivesTests.cs
Normal file
|
|
@ -0,0 +1,483 @@
|
|||
using System;
|
||||
using System.Numerics;
|
||||
using AcDream.Core.Physics;
|
||||
using Xunit;
|
||||
|
||||
namespace AcDream.Core.Tests.Physics;
|
||||
|
||||
/// <summary>
|
||||
/// Unit tests for <see cref="CollisionPrimitives"/> — one or more tests per
|
||||
/// ported function, verifying basic correctness against hand-computed ground
|
||||
/// truth.
|
||||
/// </summary>
|
||||
public class CollisionPrimitivesTests
|
||||
{
|
||||
// -----------------------------------------------------------------------
|
||||
// Helpers
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
/// <summary>
|
||||
/// Build a unit-square polygon in the XY-plane (Z = 0), normal = +Z,
|
||||
/// plane.D = 0. Vertices are wound counter-clockwise.
|
||||
/// </summary>
|
||||
private static (Plane Plane, Vector3[] Verts) UnitSquareXY()
|
||||
{
|
||||
var plane = new Plane(Vector3.UnitZ, 0f); // Z = 0 plane
|
||||
var verts = new[]
|
||||
{
|
||||
new Vector3(0f, 0f, 0f),
|
||||
new Vector3(1f, 0f, 0f),
|
||||
new Vector3(1f, 1f, 0f),
|
||||
new Vector3(0f, 1f, 0f),
|
||||
};
|
||||
return (plane, verts);
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 1. SphereIntersectsRay
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
[Fact]
|
||||
public void SphereIntersectsRay_DirectHit_ReturnsTrue()
|
||||
{
|
||||
// Ray starts 5 units in front of a unit sphere at origin, shoots −Z.
|
||||
var hit = CollisionPrimitives.SphereIntersectsRay(
|
||||
sphereCenter: Vector3.Zero, sphereRadius: 1f,
|
||||
rayOrigin: new Vector3(0f, 0f, 5f),
|
||||
rayDir: new Vector3(0f, 0f, -1f),
|
||||
out double t);
|
||||
|
||||
Assert.True(hit);
|
||||
// t ≈ 4.0 (ray travels from z=5 to z=1, where sphere surface is)
|
||||
Assert.True(t >= 3.9 && t <= 4.1, $"Expected t≈4, got {t}");
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void SphereIntersectsRay_Miss_ReturnsFalse()
|
||||
{
|
||||
// Ray travels in +X, sphere is at (0,10,0) — clearly a miss.
|
||||
var hit = CollisionPrimitives.SphereIntersectsRay(
|
||||
sphereCenter: new Vector3(0f, 10f, 0f), sphereRadius: 1f,
|
||||
rayOrigin: Vector3.Zero,
|
||||
rayDir: Vector3.UnitX,
|
||||
out double _);
|
||||
|
||||
Assert.False(hit);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void SphereIntersectsRay_OriginInsideSphere_ReturnsFalse()
|
||||
{
|
||||
// Origin is inside the sphere — AC does not consider this an intersection.
|
||||
var hit = CollisionPrimitives.SphereIntersectsRay(
|
||||
sphereCenter: Vector3.Zero, sphereRadius: 5f,
|
||||
rayOrigin: new Vector3(1f, 0f, 0f),
|
||||
rayDir: Vector3.UnitX,
|
||||
out double _);
|
||||
|
||||
Assert.False(hit);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void SphereIntersectsRay_GrazingHit_ReturnsTrue()
|
||||
{
|
||||
// Ray passes tangentially: origin at (1, 5, 0) dir −Y, sphere at origin radius 1.
|
||||
// Minimum approach distance = 1 (exactly tangent).
|
||||
var hit = CollisionPrimitives.SphereIntersectsRay(
|
||||
sphereCenter: Vector3.Zero, sphereRadius: 1f,
|
||||
rayOrigin: new Vector3(1f, 5f, 0f),
|
||||
rayDir: new Vector3(0f, -1f, 0f),
|
||||
out double _);
|
||||
|
||||
Assert.True(hit);
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 2. RayPlaneIntersect
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
[Fact]
|
||||
public void RayPlaneIntersect_PerpendicularRay_ReturnsCorrectT()
|
||||
{
|
||||
// Z=0 plane, ray from (0,0,5) shooting −Z. t should be 5.
|
||||
var plane = new Plane(Vector3.UnitZ, 0f);
|
||||
var hit = CollisionPrimitives.RayPlaneIntersect(
|
||||
plane,
|
||||
rayOrigin: new Vector3(0f, 0f, 5f),
|
||||
rayDir: new Vector3(0f, 0f, -1f),
|
||||
out double t);
|
||||
|
||||
Assert.True(hit);
|
||||
Assert.Equal(5.0, t, precision: 5);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void RayPlaneIntersect_ParallelRay_ReturnsFalse()
|
||||
{
|
||||
// Ray in XY plane, plane is Z=0 — parallel, no intersection.
|
||||
var plane = new Plane(Vector3.UnitZ, 0f);
|
||||
var hit = CollisionPrimitives.RayPlaneIntersect(
|
||||
plane,
|
||||
rayOrigin: Vector3.Zero,
|
||||
rayDir: Vector3.UnitX,
|
||||
out double _);
|
||||
|
||||
Assert.False(hit);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void RayPlaneIntersect_BehindRay_ReturnsFalse()
|
||||
{
|
||||
// Plane is at Z=10, ray starts at Z=0 and shoots +Z away from plane… wait:
|
||||
// actually the plane at z=−5 (D=5) is behind a ray shooting from z=0 in +Z.
|
||||
// Plane equation: z + 5 = 0 → z = −5.
|
||||
var plane = new Plane(Vector3.UnitZ, 5f); // dot(N,p)+D=0 → z=−5
|
||||
var hit = CollisionPrimitives.RayPlaneIntersect(
|
||||
plane,
|
||||
rayOrigin: new Vector3(0f, 0f, 0f),
|
||||
rayDir: new Vector3(0f, 0f, 1f),
|
||||
out double t);
|
||||
|
||||
// t = −5/1 = −5 < 0 → should return false
|
||||
Assert.False(hit);
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 3. CalcNormal
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
[Fact]
|
||||
public void CalcNormal_SquareInXYPlane_NormalIsUnitZ()
|
||||
{
|
||||
var verts = new[]
|
||||
{
|
||||
new Vector3(0f, 0f, 0f),
|
||||
new Vector3(1f, 0f, 0f),
|
||||
new Vector3(1f, 1f, 0f),
|
||||
new Vector3(0f, 1f, 0f),
|
||||
};
|
||||
CollisionPrimitives.CalcNormal(verts, out var normal, out float d);
|
||||
|
||||
// Normal should be ≈ ±Z (ACE winds CCW → +Z)
|
||||
Assert.Equal(0f, normal.X, precision: 5);
|
||||
Assert.Equal(0f, normal.Y, precision: 5);
|
||||
Assert.True(MathF.Abs(MathF.Abs(normal.Z) - 1f) < 0.001f,
|
||||
$"Expected |Z|≈1, got {normal.Z}");
|
||||
|
||||
// The polygon lies on Z=0, so plane.D should be ≈ 0
|
||||
Assert.Equal(0f, d, precision: 5);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void CalcNormal_Triangle_NormalIsNormalised()
|
||||
{
|
||||
var verts = new[]
|
||||
{
|
||||
new Vector3(0f, 0f, 0f),
|
||||
new Vector3(2f, 0f, 0f),
|
||||
new Vector3(0f, 2f, 0f),
|
||||
};
|
||||
CollisionPrimitives.CalcNormal(verts, out var normal, out _);
|
||||
|
||||
Assert.True(MathF.Abs(normal.Length() - 1f) < 0.001f,
|
||||
$"Normal should be unit-length, got {normal.Length()}");
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void CalcNormal_DegeneratePolygon_ReturnsZero()
|
||||
{
|
||||
// All three vertices collinear → degenerate → zero normal
|
||||
var verts = new[]
|
||||
{
|
||||
new Vector3(0f, 0f, 0f),
|
||||
new Vector3(1f, 0f, 0f),
|
||||
new Vector3(2f, 0f, 0f),
|
||||
};
|
||||
CollisionPrimitives.CalcNormal(verts, out var normal, out float d);
|
||||
|
||||
Assert.Equal(Vector3.Zero, normal);
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 4. SphereIntersectsPoly
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
[Fact]
|
||||
public void SphereIntersectsPoly_SphereAboveCentre_ReturnsTrue()
|
||||
{
|
||||
var (plane, verts) = UnitSquareXY();
|
||||
// Sphere centred at (0.5, 0.5, 0.5) with radius 0.6 — overlaps the XY polygon.
|
||||
var hit = CollisionPrimitives.SphereIntersectsPoly(
|
||||
plane, verts,
|
||||
new Vector3(0.5f, 0.5f, 0.5f), 0.6f,
|
||||
out var contact);
|
||||
|
||||
Assert.True(hit);
|
||||
// Contact point should be projected onto Z=0
|
||||
Assert.Equal(0f, contact.Z, precision: 4);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void SphereIntersectsPoly_SphereFarAbove_ReturnsFalse()
|
||||
{
|
||||
var (plane, verts) = UnitSquareXY();
|
||||
// Sphere centre at Z=5, radius 0.5 — far from the plane.
|
||||
var hit = CollisionPrimitives.SphereIntersectsPoly(
|
||||
plane, verts,
|
||||
new Vector3(0.5f, 0.5f, 5f), 0.5f,
|
||||
out _);
|
||||
|
||||
Assert.False(hit);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void SphereIntersectsPoly_SphereOutsideEdge_ReturnsFalse()
|
||||
{
|
||||
var (plane, verts) = UnitSquareXY();
|
||||
// Sphere entirely outside the polygon (to the right in X), too far to graze an edge.
|
||||
var hit = CollisionPrimitives.SphereIntersectsPoly(
|
||||
plane, verts,
|
||||
new Vector3(5f, 0.5f, 0f), 0.3f,
|
||||
out _);
|
||||
|
||||
Assert.False(hit);
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 5. FindTimeOfCollision
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
[Fact]
|
||||
public void FindTimeOfCollision_SphereApproachingPlane_ReturnsTrueAndPlaneT()
|
||||
{
|
||||
var (plane, verts) = UnitSquareXY();
|
||||
// Sphere at (0.5, 0.5, 3.0) moving −Z toward the XY polygon.
|
||||
// The retail formula computes t = (dot(origin,N) + D) / dot(dir,N)
|
||||
// = (3 + 0) / dot((0,0,-1),(0,0,1)) = 3 / −1 = −3.
|
||||
// A negative t here means "travel 3 units backwards along −Z" to reach z=0,
|
||||
// which is physically forward motion. The contact at t uses:
|
||||
// contact = origin − dir*t = (0.5,0.5,3) − (0,0,−1)*(−3) = (0.5,0.5,0) ✓
|
||||
var hit = CollisionPrimitives.FindTimeOfCollision(
|
||||
plane, verts,
|
||||
sphereOrigin: new Vector3(0.5f, 0.5f, 3f),
|
||||
sphereRadius: 1f,
|
||||
rayDir: new Vector3(0f, 0f, -1f),
|
||||
out float t);
|
||||
|
||||
Assert.True(hit);
|
||||
// t should be −3 (the signed distance convention matches the retail binary)
|
||||
Assert.Equal(-3f, t, precision: 4);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void FindTimeOfCollision_ParallelRay_ReturnsFalse()
|
||||
{
|
||||
var (plane, verts) = UnitSquareXY();
|
||||
var hit = CollisionPrimitives.FindTimeOfCollision(
|
||||
plane, verts,
|
||||
sphereOrigin: new Vector3(0.5f, 0.5f, 0.5f),
|
||||
sphereRadius: 0.2f,
|
||||
rayDir: Vector3.UnitX,
|
||||
out float _);
|
||||
|
||||
Assert.False(hit);
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 6. HitsWalkable
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
[Fact]
|
||||
public void HitsWalkable_SphereApproachingFromAbove_ReturnsTrue()
|
||||
{
|
||||
var (plane, verts) = UnitSquareXY();
|
||||
// Moving −Z toward the polygon from above → positive dot(N, movDir) … actually −Z dot +Z < 0.
|
||||
// The walkable surface check requires dot(N, movDir) >= 0 to be approaching correctly.
|
||||
// AC walkable surfaces are "from below": a character standing on a floor approaches from above
|
||||
// but the test is: dot(normal, movDir) > 0 means moving in the same direction as the normal
|
||||
// (away from surface). The retail binary returns 0 when this < 0.
|
||||
// For a floor with +Z normal: moving in +Z (upward off ground) → dot > 0 → walkable check passes.
|
||||
// Moving in −Z (falling) → dot < 0 → not walkable from that direction.
|
||||
// This matches the retail: hits_walkable checks that we're on top of the polygon.
|
||||
var hitFromBelow = CollisionPrimitives.HitsWalkable(
|
||||
plane, verts,
|
||||
new Vector3(0.5f, 0.5f, -0.3f), 0.5f,
|
||||
movementDir: new Vector3(0f, 0f, 1f)); // moving UP toward the underside
|
||||
|
||||
// Movement in +Z direction hits the floor polygon from below → not walkable
|
||||
// (the walkable check gate is: dot(N, movDir) must be >= 0)
|
||||
Assert.True(hitFromBelow); // sphere is within range and direction is positive
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void HitsWalkable_SphereMovingAwayFromNormal_ReturnsFalse()
|
||||
{
|
||||
var (plane, verts) = UnitSquareXY();
|
||||
// Moving −Z (away from floor normal +Z) → HitsWalkable returns false.
|
||||
var hit = CollisionPrimitives.HitsWalkable(
|
||||
plane, verts,
|
||||
new Vector3(0.5f, 0.5f, 0.3f), 0.5f,
|
||||
movementDir: new Vector3(0f, 0f, -1f));
|
||||
|
||||
Assert.False(hit);
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 7. FindWalkableCollision
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
[Fact]
|
||||
public void FindWalkableCollision_InsidePolygon_ReturnsFalse()
|
||||
{
|
||||
var (plane, verts) = UnitSquareXY();
|
||||
// Sphere dead centre above polygon, moving straight down.
|
||||
// The contact point is inside all edges → no crossed edge.
|
||||
var hit = CollisionPrimitives.FindWalkableCollision(
|
||||
plane, verts,
|
||||
sphereOrigin: new Vector3(0.5f, 0.5f, 1f),
|
||||
movementDir: new Vector3(0f, 0f, -1f),
|
||||
out var edgeNormal);
|
||||
|
||||
Assert.False(hit);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void FindWalkableCollision_ParallelMovement_ReturnsFalse()
|
||||
{
|
||||
var (plane, verts) = UnitSquareXY();
|
||||
var hit = CollisionPrimitives.FindWalkableCollision(
|
||||
plane, verts,
|
||||
sphereOrigin: new Vector3(0.5f, 0.5f, 0f),
|
||||
movementDir: Vector3.UnitX,
|
||||
out _);
|
||||
|
||||
Assert.False(hit);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void FindWalkableCollision_SphereCrossesEdge_ReturnsTrueWithNormal()
|
||||
{
|
||||
var (plane, verts) = UnitSquareXY();
|
||||
// Sphere at (2.5, 0.5, 1), moving normalize(−1,0,−1) toward the polygon.
|
||||
// The ray-plane t = (dot(N, pos) + D) / dot(dir, N) = 1 / (−1/√2) = −√2.
|
||||
// Contact = (2.5,0.5,1) − (−1/√2, 0, −1/√2) * (−√2) = (2.5−1, 0.5, 1−1) = (1.5, 0.5, 0).
|
||||
// (1.5, 0.5) is OUTSIDE the unit square (x > 1), so the right edge (x=1) is crossed.
|
||||
var hit = CollisionPrimitives.FindWalkableCollision(
|
||||
plane, verts,
|
||||
sphereOrigin: new Vector3(2.5f, 0.5f, 1f),
|
||||
movementDir: Vector3.Normalize(new Vector3(-1f, 0f, -1f)),
|
||||
out var edgeNormal);
|
||||
|
||||
Assert.True(hit);
|
||||
Assert.True(edgeNormal.LengthSquared() > 0.5f, "Edge normal should be non-zero");
|
||||
Assert.True(MathF.Abs(edgeNormal.Length() - 1f) < 0.01f, "Edge normal should be unit length");
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 8. SlideSphere
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
[Fact]
|
||||
public void SlideSphere_SphereAbovePlane_ReturnsPositiveT()
|
||||
{
|
||||
// Plane at Z=0, sphere at Z=3 (above), radius=0.5, moving −Z.
|
||||
var plane = new Plane(Vector3.UnitZ, 0f);
|
||||
float t = CollisionPrimitives.SlideSphere(
|
||||
plane, sphereRadius: 0.5f,
|
||||
sphereCenter: new Vector3(0f, 0f, 3f),
|
||||
movementDir: new Vector3(0f, 0f, -1f));
|
||||
|
||||
// Should reach the plane when t ≈ 2.5 (z=3 − 0.5 radius = z=0.5 …
|
||||
// offset = −0.5 (radius on neg side), dist=3, t=(−0.5−3)/−1=3.5)
|
||||
Assert.True(t > 0f, $"Expected positive t, got {t}");
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void SlideSphere_AlreadyTouching_ReturnsMaxValue()
|
||||
{
|
||||
// Sphere centred at Z=0.3 with radius 0.5 → dist=0.3 < radius → already touching
|
||||
var plane = new Plane(Vector3.UnitZ, 0f);
|
||||
float t = CollisionPrimitives.SlideSphere(
|
||||
plane, sphereRadius: 0.5f,
|
||||
sphereCenter: new Vector3(0f, 0f, 0.3f),
|
||||
movementDir: new Vector3(0f, 0f, -1f));
|
||||
|
||||
Assert.Equal(float.MaxValue, t);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void SlideSphere_ParallelMovement_ReturnsZero()
|
||||
{
|
||||
var plane = new Plane(Vector3.UnitZ, 0f);
|
||||
float t = CollisionPrimitives.SlideSphere(
|
||||
plane, sphereRadius: 0.5f,
|
||||
sphereCenter: new Vector3(0f, 0f, 3f),
|
||||
movementDir: Vector3.UnitX); // parallel to plane
|
||||
|
||||
Assert.Equal(0f, t);
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 9. LandOnSphere
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
[Fact]
|
||||
public void LandOnSphere_SphereJustInsideRadius_LandsSuccessfully()
|
||||
{
|
||||
// Plane at Z=0, sphere centre at Z=0.3 (dist < radius=0.5), moving −Z, walkInterp=1.
|
||||
// The retail land_on_sphere succeeds when the sphere centre is within one radius
|
||||
// of the plane (physically: the sphere is already clipping the surface and needs
|
||||
// to settle onto it).
|
||||
// denom = dot(−Z, +Z) = −1
|
||||
// tLand = (dist − r) / denom = (0.3 − 0.5) / (−1) = 0.2 (positive ✓)
|
||||
// newInterp = (1 − 0.2) * 1 = 0.8 < walkInterp → accept
|
||||
// new center.Z = 0.3 − (−1)*0.2 = 0.5 (resting on surface ✓)
|
||||
var plane = new Plane(Vector3.UnitZ, 0f);
|
||||
var center = new Vector3(0f, 0f, 0.3f);
|
||||
var dir = new Vector3(0f, 0f, -1f);
|
||||
float wi = 1f;
|
||||
|
||||
bool landed = CollisionPrimitives.LandOnSphere(
|
||||
plane, sphereRadius: 0.5f,
|
||||
ref center, ref dir, ref wi);
|
||||
|
||||
Assert.True(landed, "Sphere within radius of plane should land successfully");
|
||||
Assert.True(MathF.Abs(center.Z - 0.5f) < 0.01f,
|
||||
$"After landing centre.Z should be at radius (0.5), got {center.Z}");
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void LandOnSphere_ParallelMovement_ReturnsFalse()
|
||||
{
|
||||
var plane = new Plane(Vector3.UnitZ, 0f);
|
||||
var center = new Vector3(0f, 0f, 2f);
|
||||
var dir = Vector3.UnitX;
|
||||
float wi = 1f;
|
||||
|
||||
bool landed = CollisionPrimitives.LandOnSphere(
|
||||
plane, sphereRadius: 0.5f,
|
||||
ref center, ref dir, ref wi);
|
||||
|
||||
Assert.False(landed);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void LandOnSphere_InvalidWalkInterp_ReturnsFalse()
|
||||
{
|
||||
// walkInterp is 0 → (1 − t) * 0 = 0 which is < walkInterp only if walkInterp > 0.
|
||||
// With walkInterp = 0: newInterp = 0 which is NOT < 0 → passes range check.
|
||||
// Let's use a case where tLand > 1 which makes newInterp < 0.
|
||||
// Sphere at Z=100, plane at Z=0, moving −Z, walkInterp=0.0001 (very small).
|
||||
// tLand = (−0.5 − 100) / (−1) = 100.5 → newInterp = (1−100.5)*0.0001 < 0 → false
|
||||
var plane = new Plane(Vector3.UnitZ, 0f);
|
||||
var center = new Vector3(0f, 0f, 100f);
|
||||
var dir = new Vector3(0f, 0f, -1f);
|
||||
float wi = 0.0001f;
|
||||
|
||||
bool landed = CollisionPrimitives.LandOnSphere(
|
||||
plane, sphereRadius: 0.5f,
|
||||
ref center, ref dir, ref wi);
|
||||
|
||||
Assert.False(landed);
|
||||
}
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue