|
|
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;
private readonly static float[] degToRad = new float[360];
static FMath() { for (int i = 0; i < degToRad.Length; i++) { degToRad[i] = PI / 180 * i; } }
// Converts degrees to radians using a look-up table. Expects the input to be near [0, 360)
// and will loop for potentially a long while if that's not the case.
public static float DegToRad(int degrees) { while (degrees < 0) { degrees += 360; } while (degrees >= 360) { degrees -= 360; } return degToRad[degrees]; }
public static float Sin(double radians) { return (float) Math.Sin(radians); }
public static float Cos(double radians) { return (float) Math.Cos(radians); }
public static T Clamp<T>(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 static bool operator ==(Hit a, Hit b) { return a.Position == b.Position && a.Delta == b.Delta && a.Normal == b.Normal && a.Time == b.Time && a.Collider == b.Collider; }
public static bool operator !=(Hit a, Hit b) { return !(a == b); }
public override bool Equals(object o) { return o is Hit h && this == h; }
public override int GetHashCode() { return (Collider, Position, Delta, Normal, Time).GetHashCode(); } }
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 static bool operator ==(Sweep a, Sweep b) { return a.Hit == b.Hit && a.Position == b.Position && a.Time == b.Time; }
public static bool operator !=(Sweep a, Sweep b) { return !(a == b); }
public override bool Equals(object o) { return o is Sweep s && this == s; }
public override int GetHashCode() { return (Hit, Position, Time).GetHashCode(); } }
public readonly struct AABB { public readonly Vector2 Position; // centroid
public readonly Vector2 HalfSize; public readonly Tile Tile;
public AABB(Vector2 position, Vector2 halfSize) : this(position, halfSize, null) { }
public AABB(Vector2 position, Vector2 halfSize, Tile tile) { Position = position; HalfSize = halfSize; Tile = tile; }
public static bool operator ==(AABB a, AABB b) { return a.Position == b.Position && a.HalfSize == b.HalfSize && a.Tile == b.Tile; }
public static bool operator !=(AABB a, AABB b) { return !(a == b); }
public override bool Equals(object o) { return o is AABB a && this == a; }
public override int GetHashCode() { return (Position, HalfSize, Tile).GetHashCode(); }
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; }
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); Vector2 hitDelta = Vector2.Multiply(delta, -(1.0f - hitTime)); Vector2 hitPos = Vector2.Add(pos, Vector2.Multiply(delta, 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 = Vector2.Add(box.Position, Vector2.Multiply(delta, hit.Time)); // TODO: why is this calculation made, and then thrown away?
// Vector2 direction = Vector2.Normalize(delta);
// 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); } } }
|