using Microsoft.Xna.Framework; using System; // Design largely from https://noonat.github.io/intersect/. namespace SemiColinGames { // Math functions that return floats rather than doubles, for convenience. public static class FMath { public const float PI = (float) Math.PI; public static float DegToRad(float degrees) { return PI / 180 * degrees; } public static float Sin(double degrees) { return (float) Math.Sin(degrees); } public static float Cos(double degrees) { return (float) Math.Cos(degrees); } public static T Clamp(T value, T min, T max) where T : IComparable { if (value.CompareTo(min) == -1) { return min; } else if (value.CompareTo(max) == 1) { return max; } else { return value; } } } public readonly struct Hit { public readonly AABB Collider; public readonly Vector2 Position; public readonly Vector2 Delta; public readonly Vector2 Normal; public readonly float Time; // ranges from [0, 1]. public Hit(AABB collider, Vector2 position, Vector2 delta, Vector2 normal) : this(collider, position, delta, normal, 0.0f) { } public Hit(AABB collider, Vector2 position, Vector2 delta, Vector2 normal, float time) { Collider = collider; Position = position; Delta = delta; Normal = normal; Time = time; } } public readonly struct Sweep { public readonly Hit? Hit; public readonly Vector2 Position; public readonly float Time; public Sweep(Hit? hit, Vector2 position, float time) { Hit = hit; Position = position; Time = time; } } public readonly struct AABB { public readonly Vector2 Position; // centroid public readonly Vector2 HalfSize; public AABB(Vector2 position, Vector2 halfSize) { Position = position; HalfSize = halfSize; } public float Top { get { return Position.Y - HalfSize.Y; } } public float Bottom { get { return Position.Y + HalfSize.Y; } } public float Left { get { return Position.X - HalfSize.X; } } public float Right { get { return Position.X + HalfSize.X; } } public Vector2 TopLeft { get { return new Vector2(Left, Top); } } public Vector2 TopRight { get { return new Vector2(Right, Top); } } public Vector2 BottomLeft { get { return new Vector2(Left, Bottom); } } public Vector2 BottomRight { get { return new Vector2(Right, Bottom); } } public Hit? Intersect(AABB box) { float dx = box.Position.X - Position.X; float px = box.HalfSize.X + HalfSize.X - Math.Abs(dx); if (px <= 0) { return null; } float dy = box.Position.Y - Position.Y; float py = box.HalfSize.Y + HalfSize.Y - Math.Abs(dy); if (py <= 0) { return null; } // TODO: which of delta/normal/hitPos do we actually care about? if (px < py) { int sign = Math.Sign(dx); Vector2 delta = new Vector2(px * sign, 0); Vector2 normal = new Vector2(sign, 0); Vector2 hitPos = new Vector2(Position.X + HalfSize.X * sign, box.Position.Y); return new Hit(box, hitPos, delta, normal); } else { int sign = Math.Sign(dy); Vector2 delta = new Vector2(0, py * sign); Vector2 normal = new Vector2(0, sign); Vector2 hitPos = new Vector2(box.Position.X, Position.Y + HalfSize.Y * sign); return new Hit(this, hitPos, delta, normal); } } public Hit? IntersectSegment(Vector2 pos, Vector2 delta) { return IntersectSegment(pos, delta, Vector2.Zero); } public Hit? IntersectSegment(Vector2 pos, Vector2 delta, Vector2 padding) { float scaleX = 1.0f / delta.X; float scaleY = 1.0f / delta.Y; int signX = Math.Sign(scaleX); int signY = Math.Sign(scaleY); float nearTimeX = (Position.X - signX * (HalfSize.X + padding.X) - pos.X) * scaleX; float nearTimeY = (Position.Y - signY * (HalfSize.Y + padding.Y) - pos.Y) * scaleY; float farTimeX = (Position.X + signX * (HalfSize.X + padding.X) - pos.X) * scaleX; float farTimeY = (Position.Y + signY * (HalfSize.Y + padding.Y) - pos.Y) * scaleY; if (nearTimeX > farTimeY || nearTimeY > farTimeX) { return null; } float nearTime = Math.Max(nearTimeX, nearTimeY); float farTime = Math.Min(farTimeX, farTimeY); if (nearTime >= 1 || farTime <= 0) { return null; } // If we've gotten this far, a collision is happening. If the near time is greater than zero, // the segment starts outside and is entering the box. Otherwise, the segment starts inside // the box, so we set the hit time to zero. float hitTime = Math.Max(0, nearTime); Vector2 normal = nearTimeX > nearTimeY ? new Vector2(-signX, 0) : new Vector2(0, -signY); // TODO: replace these with Vector2.Multiply (etc) Vector2 hitDelta = new Vector2((1.0f - hitTime) * -delta.X, (1.0f - hitTime) * -delta.Y); Vector2 hitPos = new Vector2(pos.X + delta.X * hitTime, pos.Y + delta.Y * hitTime); return new Hit(this, hitPos, hitDelta, normal, hitTime); } public Sweep Sweep(AABB box, Vector2 delta) { // fast-path case if the other box is static if (delta.X == 0 && delta.Y == 0) { Hit? staticHit = Intersect(box); // TODO: I don't understand the original source here, but I think this is correct. return new Sweep(staticHit, box.Position, staticHit?.Time ?? 1); } Hit? maybeHit = IntersectSegment(box.Position, delta, box.HalfSize); if (maybeHit == null) { return new Sweep(null, Vector2.Add(box.Position, delta), 1); } Hit hit = (Hit) maybeHit; Vector2 hitPos = new Vector2( box.Position.X + delta.X * hit.Time, box.Position.Y + delta.Y * hit.Time); Vector2 direction = Vector2.Normalize(delta); // TODO: why is this calculation made, and then thrown away? Vector2 sweepHitPos = new Vector2( FMath.Clamp(hit.Position.X + direction.X * box.HalfSize.X, Position.X - HalfSize.X, Position.X + HalfSize.X), FMath.Clamp(hit.Position.Y + direction.Y * box.HalfSize.Y, Position.Y - HalfSize.Y, Position.Y + HalfSize.Y)); return new Sweep(hit, hitPos, hit.Time); } } }