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>
483 lines
18 KiB
C#
483 lines
18 KiB
C#
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);
|
||
}
|
||
}
|