sneak/Shared/Geometry.cs

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;

    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>(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);
    }
  }
}