226 lines
7.1 KiB
C#
226 lines
7.1 KiB
C#
using Microsoft.Xna.Framework;
|
|
using System;
|
|
|
|
// Design largely from https://noonat.github.io/intersect/.
|
|
|
|
namespace SemiColinGames {
|
|
|
|
public static class Geometry {
|
|
public static Vector2 Rotate(this Vector2 point, float angle) {
|
|
float cos = FMath.Cos(angle);
|
|
float sin = FMath.Sin(angle);
|
|
return new Vector2(
|
|
point.X * cos - point.Y * sin,
|
|
point.Y * cos + point.X * sin);
|
|
}
|
|
}
|
|
|
|
// 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 degrees) {
|
|
return (float) Math.Sin(degrees);
|
|
}
|
|
|
|
public static float Cos(double degrees) {
|
|
return (float) Math.Cos(degrees);
|
|
}
|
|
|
|
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 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);
|
|
}
|
|
}
|
|
} |