339 lines
17 KiB
C#
339 lines
17 KiB
C#
using System.Numerics;
|
|
using GamePort.Assets;
|
|
|
|
namespace GamePort.Game;
|
|
|
|
/// <summary>
|
|
/// Provisional geometric reconstruction for the controllable-scene checkpoint.
|
|
/// Uses source terrain-eligible triangles, including invisible proxies. NC shape
|
|
/// exclusion is recovered; remaining masks/state rules and solver are provisional.
|
|
/// Positions are the bottom of a vertical capsule in original Y-up world units.
|
|
/// </summary>
|
|
public sealed class SceneCollision
|
|
{
|
|
public const float Skin = 0.05f;
|
|
public const float MinimumGroundNormal = 0.65f;
|
|
private const float Tolerance = 0.005f;
|
|
private readonly Triangle[] triangles;
|
|
public int TriangleCount => triangles.Length;
|
|
|
|
/// <summary>Modern debug exit check against the current capsule adapter.</summary>
|
|
public bool CanOccupy(Vector3 feet, float radius, float height)
|
|
{
|
|
Validate(feet, Vector3.Zero, radius, height);
|
|
CapsuleBounds(feet, radius, height, out var min, out var max);
|
|
foreach (var triangle in triangles)
|
|
if (Overlaps(min, max, triangle)
|
|
&& Closest(feet, radius, height, triangle).DistanceSquared < (radius - Tolerance) * (radius - Tolerance))
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
private readonly record struct Triangle(Vector3 A, Vector3 B, Vector3 C,
|
|
Vector3 Normal, Vector3 Min, Vector3 Max);
|
|
private readonly record struct Hit(float Time, Vector3 Normal);
|
|
private readonly record struct Pair(Vector3 Capsule, Vector3 Surface)
|
|
{
|
|
public float DistanceSquared => Vector3.DistanceSquared(Capsule, Surface);
|
|
}
|
|
|
|
public SceneCollision(Mesh mesh, bool wallsAndCeilingsOnly = false)
|
|
{
|
|
if (mesh.Indices.Length % 3 != 0) throw new ArgumentException("Collision indices must be triangles", nameof(mesh));
|
|
if (wallsAndCeilingsOnly && mesh.CollisionPolygons.Length != mesh.Indices.Length / 3)
|
|
throw new InvalidDataException("Original wall grid requires prepared source polygon normals");
|
|
var result = new List<Triangle>();
|
|
// Render visibility is not a proven collision mask. Original null-material
|
|
// proxies remain present and block movement in this provisional policy.
|
|
for (var i = 0; i < mesh.Indices.Length; i += 3)
|
|
{
|
|
if (mesh.CollisionPolygons.Length == mesh.Indices.Length / 3
|
|
&& !mesh.CollisionPolygons[i / 3].TerrainCollision) continue;
|
|
// EXE4798B3 registers only stored normal.Y<.7 in the wall/ceiling
|
|
// grid. Render normals and a recomputed cross product differ.
|
|
if (wallsAndCeilingsOnly && mesh.CollisionPolygons[i / 3].Normal.Y >= .7f) continue;
|
|
if (wallsAndCeilingsOnly && mesh.CollisionPolygons[i / 3].MaterialName is { } material
|
|
&& (material.Equals("cstart", StringComparison.OrdinalIgnoreCase)
|
|
|| material.Length == 7 && material.StartsWith("ccheck", StringComparison.OrdinalIgnoreCase) && material[6] is >= '1' and <= '9')) continue;
|
|
if (mesh.Indices[i] >= mesh.Vertices.Length || mesh.Indices[i + 1] >= mesh.Vertices.Length
|
|
|| mesh.Indices[i + 2] >= mesh.Vertices.Length)
|
|
throw new ArgumentException("Collision index outside vertex table", nameof(mesh));
|
|
var a = mesh.Vertices[mesh.Indices[i]].Position;
|
|
var b = mesh.Vertices[mesh.Indices[i + 1]].Position;
|
|
var c = mesh.Vertices[mesh.Indices[i + 2]].Position;
|
|
if (!Finite(a) || !Finite(b) || !Finite(c)) throw new ArgumentException("Nonfinite collision coordinate", nameof(mesh));
|
|
var normal = Vector3.Cross(b - a, c - a);
|
|
var normalLengthSquared = normal.LengthSquared();
|
|
if (!float.IsFinite(normalLengthSquared)) throw new ArgumentException("Collision triangle exceeds finite arithmetic range", nameof(mesh));
|
|
if (normalLengthSquared < 0.000001f) continue;
|
|
result.Add(new Triangle(a, b, c, Vector3.Normalize(normal),
|
|
Vector3.Min(a, Vector3.Min(b, c)), Vector3.Max(a, Vector3.Max(b, c))));
|
|
}
|
|
triangles = result.ToArray();
|
|
}
|
|
|
|
/// <summary>Highest walkable surface in the requested vertical interval.</summary>
|
|
public float? GroundHeight(Vector3 point, float maxDrop, float maxRise = 0)
|
|
{
|
|
if (!Finite(point) || !float.IsFinite(maxDrop) || !float.IsFinite(maxRise) || maxDrop < 0 || maxRise < 0)
|
|
throw new ArgumentOutOfRangeException(nameof(point));
|
|
float? highest = null;
|
|
var low = point.Y - maxDrop;
|
|
var high = point.Y + maxRise;
|
|
foreach (var triangle in triangles)
|
|
{
|
|
if (MathF.Abs(triangle.Normal.Y) < MinimumGroundNormal || point.X < triangle.Min.X - Tolerance
|
|
|| point.X > triangle.Max.X + Tolerance || point.Z < triangle.Min.Z - Tolerance
|
|
|| point.Z > triangle.Max.Z + Tolerance || triangle.Min.Y > high || triangle.Max.Y < low) continue;
|
|
var t = triangle;
|
|
var denominator = (t.B.Z - t.C.Z) * (t.A.X - t.C.X) + (t.C.X - t.B.X) * (t.A.Z - t.C.Z);
|
|
if (MathF.Abs(denominator) < 0.000001f) continue;
|
|
var u = ((t.B.Z - t.C.Z) * (point.X - t.C.X) + (t.C.X - t.B.X) * (point.Z - t.C.Z)) / denominator;
|
|
var v = ((t.C.Z - t.A.Z) * (point.X - t.C.X) + (t.A.X - t.C.X) * (point.Z - t.C.Z)) / denominator;
|
|
if (u < -0.00001f || v < -0.00001f || u + v > 1.00001f) continue;
|
|
var y = u * t.A.Y + v * t.B.Y + (1 - u - v) * t.C.Y;
|
|
if (y < low - Tolerance || y > high + Tolerance || highest.HasValue && y <= highest.Value) continue;
|
|
highest = y;
|
|
}
|
|
return highest;
|
|
}
|
|
|
|
/// <summary>Resolve initial overlaps and sweep/slide a capsule continuously.</summary>
|
|
public CollisionMove Move(Vector3 feetPosition, Vector3 displacement, float radius, float height)
|
|
{
|
|
Validate(feetPosition, displacement, radius, height);
|
|
var position = feetPosition;
|
|
var grounded = false;
|
|
var ceiling = false;
|
|
var wall = false;
|
|
// Recover gently from a spawn touching/intersecting source triangles. A
|
|
// finite iteration bound also handles impossible narrow-gap input safely.
|
|
for (var iteration = 0; iteration < 12; iteration++)
|
|
{
|
|
var bestDepth = Tolerance;
|
|
var bestNormal = Vector3.Zero;
|
|
CapsuleBounds(position, radius + Skin, height, out var min, out var max);
|
|
foreach (var triangle in triangles)
|
|
{
|
|
if (!Overlaps(min, max, triangle)) continue;
|
|
var pair = Closest(position, radius, height, triangle);
|
|
var distance = MathF.Sqrt(pair.DistanceSquared);
|
|
var depth = radius + Skin - distance;
|
|
if (depth <= bestDepth) continue;
|
|
bestDepth = depth;
|
|
bestNormal = ContactNormal(pair, triangle, position, height, displacement);
|
|
}
|
|
if (bestNormal == Vector3.Zero) break;
|
|
position += bestNormal * bestDepth;
|
|
Classify(bestNormal, ref grounded, ref ceiling, ref wall);
|
|
}
|
|
var remaining = displacement;
|
|
for (var iteration = 0; iteration < 8 && remaining.LengthSquared() > 0.00000001f; iteration++)
|
|
{
|
|
var hit = Sweep(position, remaining, radius, height);
|
|
if (!hit.HasValue) { position += remaining; remaining = Vector3.Zero; break; }
|
|
position += remaining * Math.Clamp(hit.Value.Time, 0, 1);
|
|
Classify(hit.Value.Normal, ref grounded, ref ceiling, ref wall);
|
|
remaining *= 1 - Math.Clamp(hit.Value.Time, 0, 1);
|
|
var into = Vector3.Dot(remaining, hit.Value.Normal);
|
|
if (into < 0) remaining -= hit.Value.Normal * into;
|
|
else break;
|
|
}
|
|
// Contact persistence matters for a stationary character and tangential
|
|
// motion. This query does not snap a falling or jumping actor to a floor.
|
|
// Grounded describes support at the final position, not a floor crossed
|
|
// earlier in this move (for example after walking over a ledge).
|
|
grounded = false;
|
|
CapsuleBounds(position, radius + Skin + Tolerance, height, out var contactMin, out var contactMax);
|
|
foreach (var triangle in triangles)
|
|
{
|
|
if (!Overlaps(contactMin, contactMax, triangle)) continue;
|
|
var pair = Closest(position, radius, height, triangle);
|
|
if (pair.DistanceSquared <= (radius + Skin + Tolerance) * (radius + Skin + Tolerance))
|
|
Classify(ContactNormal(pair, triangle, position, height, displacement), ref grounded, ref ceiling, ref wall);
|
|
}
|
|
if (!Finite(position)) throw new InvalidOperationException("Collision produced nonfinite position");
|
|
return new CollisionMove(position, grounded, ceiling, wall);
|
|
}
|
|
|
|
/// <summary>Sphere sweep for a follow camera; stops before source geometry.</summary>
|
|
public Vector3 ClipCamera(Vector3 target, Vector3 desiredEye, float radius = 8)
|
|
{
|
|
Validate(target - Vector3.UnitY * radius, desiredEye - target, radius, radius * 2);
|
|
var movement = desiredEye - target;
|
|
var hit = Sweep(target - Vector3.UnitY * radius, movement, radius, radius * 2);
|
|
return hit.HasValue ? target + movement * Math.Clamp(hit.Value.Time, 0, 1) : desiredEye;
|
|
}
|
|
|
|
private Hit? Sweep(Vector3 position, Vector3 movement, float radius, float height)
|
|
{
|
|
CapsuleBounds(position, radius + Skin, height, out var startMin, out var startMax);
|
|
CapsuleBounds(position + movement, radius + Skin, height, out var endMin, out var endMax);
|
|
var min = Vector3.Min(startMin, endMin);
|
|
var max = Vector3.Max(startMax, endMax);
|
|
Hit? closest = null;
|
|
foreach (var triangle in triangles)
|
|
{
|
|
if (!Overlaps(min, max, triangle)) continue;
|
|
var limit = closest?.Time ?? 1f;
|
|
var time = 0f;
|
|
for (var iteration = 0; iteration < 40; iteration++)
|
|
{
|
|
var current = position + movement * time;
|
|
var pair = Closest(current, radius, height, triangle);
|
|
var distance = MathF.Sqrt(pair.DistanceSquared);
|
|
var normal = ContactNormal(pair, triangle, current, height, movement);
|
|
var closing = -Vector3.Dot(movement, normal);
|
|
if (distance <= radius + Skin + Tolerance)
|
|
{
|
|
// A tangent or departing contact must not lock movement.
|
|
if (closing > 0.000001f || distance < radius - Tolerance)
|
|
closest = new Hit(time, normal);
|
|
break;
|
|
}
|
|
if (closing <= 0.000001f) break;
|
|
// The closest-pair plane separates two convex shapes. Advancing
|
|
// to where that plane's gap closes is conservative even when the
|
|
// closest feature later changes, unlike endpoint-only sampling.
|
|
var advance = (distance - radius - Skin) / closing;
|
|
if (advance <= 0.0000001f)
|
|
{
|
|
closest = new Hit(time, normal);
|
|
break;
|
|
}
|
|
time += advance;
|
|
if (time > limit || time > 1) break;
|
|
if (iteration == 39) closest = new Hit(time, normal);
|
|
}
|
|
}
|
|
return closest;
|
|
}
|
|
|
|
private static Pair Closest(Vector3 feet, float radius, float height, Triangle triangle)
|
|
{
|
|
var p = feet + Vector3.UnitY * radius;
|
|
var q = feet + Vector3.UnitY * (height - radius);
|
|
var denominator = Vector3.Dot(triangle.Normal, q - p);
|
|
if (MathF.Abs(denominator) > 0.000001f)
|
|
{
|
|
var time = Vector3.Dot(triangle.Normal, triangle.A - p) / denominator;
|
|
if (time is >= 0 and <= 1)
|
|
{
|
|
var intersection = p + (q - p) * time;
|
|
if (Vector3.DistanceSquared(intersection, ClosestPoint(intersection, triangle)) < 0.000001f)
|
|
return new Pair(intersection, intersection);
|
|
}
|
|
}
|
|
var best = new Pair(p, ClosestPoint(p, triangle));
|
|
var candidate = new Pair(q, ClosestPoint(q, triangle));
|
|
if (candidate.DistanceSquared < best.DistanceSquared) best = candidate;
|
|
candidate = ClosestSegments(p, q, triangle.A, triangle.B);
|
|
if (candidate.DistanceSquared < best.DistanceSquared) best = candidate;
|
|
candidate = ClosestSegments(p, q, triangle.B, triangle.C);
|
|
if (candidate.DistanceSquared < best.DistanceSquared) best = candidate;
|
|
candidate = ClosestSegments(p, q, triangle.C, triangle.A);
|
|
if (candidate.DistanceSquared < best.DistanceSquared) best = candidate;
|
|
return best;
|
|
}
|
|
|
|
private static Vector3 ClosestPoint(Vector3 p, Triangle t)
|
|
{
|
|
var ab = t.B - t.A;
|
|
var ac = t.C - t.A;
|
|
var ap = p - t.A;
|
|
var d1 = Vector3.Dot(ab, ap);
|
|
var d2 = Vector3.Dot(ac, ap);
|
|
if (d1 <= 0 && d2 <= 0) return t.A;
|
|
var bp = p - t.B;
|
|
var d3 = Vector3.Dot(ab, bp);
|
|
var d4 = Vector3.Dot(ac, bp);
|
|
if (d3 >= 0 && d4 <= d3) return t.B;
|
|
var vc = d1 * d4 - d3 * d2;
|
|
if (vc <= 0 && d1 >= 0 && d3 <= 0) return t.A + ab * (d1 / (d1 - d3));
|
|
var cp = p - t.C;
|
|
var d5 = Vector3.Dot(ab, cp);
|
|
var d6 = Vector3.Dot(ac, cp);
|
|
if (d6 >= 0 && d5 <= d6) return t.C;
|
|
var vb = d5 * d2 - d1 * d6;
|
|
if (vb <= 0 && d2 >= 0 && d6 <= 0) return t.A + ac * (d2 / (d2 - d6));
|
|
var va = d3 * d6 - d5 * d4;
|
|
if (va <= 0 && d4 - d3 >= 0 && d5 - d6 >= 0)
|
|
return t.B + (t.C - t.B) * ((d4 - d3) / (d4 - d3 + d5 - d6));
|
|
// Once the face region is known, project onto its plane directly. A
|
|
// barycentric sum of far-apart vertices loses enough precision to give
|
|
// an axis-aligned floor a false horizontal normal at original scales.
|
|
return p - t.Normal * Vector3.Dot(p - t.A, t.Normal);
|
|
}
|
|
|
|
private static Pair ClosestSegments(Vector3 p, Vector3 q, Vector3 a, Vector3 b)
|
|
{
|
|
var d1 = q - p;
|
|
var d2 = b - a;
|
|
var r = p - a;
|
|
var aa = Vector3.Dot(d1, d1);
|
|
var ee = Vector3.Dot(d2, d2);
|
|
var f = Vector3.Dot(d2, r);
|
|
float s, t;
|
|
if (aa <= 0.00000001f && ee <= 0.00000001f) return new Pair(p, a);
|
|
if (aa <= 0.00000001f) { s = 0; t = Math.Clamp(f / ee, 0, 1); }
|
|
else
|
|
{
|
|
var c = Vector3.Dot(d1, r);
|
|
if (ee <= 0.00000001f) { t = 0; s = Math.Clamp(-c / aa, 0, 1); }
|
|
else
|
|
{
|
|
var dot = Vector3.Dot(d1, d2);
|
|
var denominator = aa * ee - dot * dot;
|
|
s = MathF.Abs(denominator) > 0.00000001f ? Math.Clamp((dot * f - c * ee) / denominator, 0, 1) : 0;
|
|
t = (dot * s + f) / ee;
|
|
if (t < 0) { t = 0; s = Math.Clamp(-c / aa, 0, 1); }
|
|
else if (t > 1) { t = 1; s = Math.Clamp((dot - c) / aa, 0, 1); }
|
|
}
|
|
}
|
|
return new Pair(p + d1 * s, a + d2 * t);
|
|
}
|
|
|
|
private static Vector3 ContactNormal(Pair pair, Triangle triangle, Vector3 feet, float height, Vector3 movement)
|
|
{
|
|
var difference = pair.Capsule - pair.Surface;
|
|
if (difference.LengthSquared() > 0.00000001f)
|
|
{
|
|
var distance = difference.Length();
|
|
var planeDistance = Vector3.Dot(pair.Capsule - triangle.A, triangle.Normal);
|
|
// A contact indistinguishable from the face plane at our world-unit
|
|
// tolerance uses its exact normal. Otherwise tiny closest-edge
|
|
// rounding on a coplanar triangle seam can turn a tangent sweep into
|
|
// a large sideways/upward correction at high velocity.
|
|
if (MathF.Abs(planeDistance) >= distance - Tolerance && MathF.Abs(planeDistance) > Tolerance)
|
|
return planeDistance > 0 ? triangle.Normal : -triangle.Normal;
|
|
return difference / distance;
|
|
}
|
|
var side = Vector3.Dot(feet + Vector3.UnitY * (height * 0.5f) - triangle.A, triangle.Normal);
|
|
if (MathF.Abs(side) > 0.0001f) return side > 0 ? triangle.Normal : -triangle.Normal;
|
|
return Vector3.Dot(movement, triangle.Normal) > 0 ? -triangle.Normal : triangle.Normal;
|
|
}
|
|
|
|
private static void CapsuleBounds(Vector3 feet, float radius, float height, out Vector3 min, out Vector3 max)
|
|
{
|
|
min = feet - new Vector3(radius, Skin + Tolerance, radius);
|
|
max = feet + new Vector3(radius, height + Skin + Tolerance, radius);
|
|
}
|
|
private static bool Overlaps(Vector3 min, Vector3 max, Triangle t) =>
|
|
min.X <= t.Max.X && max.X >= t.Min.X && min.Y <= t.Max.Y && max.Y >= t.Min.Y && min.Z <= t.Max.Z && max.Z >= t.Min.Z;
|
|
private static void Classify(Vector3 normal, ref bool ground, ref bool ceiling, ref bool wall)
|
|
{
|
|
if (normal.Y >= MinimumGroundNormal) ground = true;
|
|
else if (normal.Y <= -MinimumGroundNormal) ceiling = true;
|
|
else wall = true;
|
|
}
|
|
private static bool Finite(Vector3 p) => float.IsFinite(p.X) && float.IsFinite(p.Y) && float.IsFinite(p.Z);
|
|
private static void Validate(Vector3 position, Vector3 movement, float radius, float height)
|
|
{
|
|
if (!Finite(position) || !Finite(movement) || !float.IsFinite(radius) || !float.IsFinite(height)
|
|
|| radius <= 0 || height < radius * 2)
|
|
throw new ArgumentOutOfRangeException(nameof(radius), "Finite capsule coordinates, positive radius and height >= diameter are required");
|
|
}
|
|
}
|
|
|
|
public readonly record struct CollisionMove(Vector3 Position, bool Grounded, bool HitCeiling, bool HitWall);
|