diff --git a/Bonk/SweepTest/SweepResult.cs b/Bonk/SweepTest/SweepResult.cs new file mode 100644 index 0000000..2575090 --- /dev/null +++ b/Bonk/SweepTest/SweepResult.cs @@ -0,0 +1,26 @@ +using System; +using System.Numerics; +using MoonTools.Core.Structs; + +namespace MoonTools.Core.Bonk +{ + public struct SweepResult where T : IEquatable where U : struct, IShape2D + { + public static SweepResult False = new SweepResult(); + + public bool Hit { get; } + public Vector2 Motion { get; } + public T ID { get; } + public U Shape { get; } + public Transform2D Transform { get; } + + public SweepResult(bool hit, Vector2 motion, T id, U shape, Transform2D transform) + { + Hit = hit; + Motion = motion; + ID = id; + Shape = shape; + Transform = transform; + } + } +} diff --git a/Bonk/SweepTest/SweepTest.cs b/Bonk/SweepTest/SweepTest.cs new file mode 100644 index 0000000..5001188 --- /dev/null +++ b/Bonk/SweepTest/SweepTest.cs @@ -0,0 +1,108 @@ +using System; +using System.Numerics; +using MoonTools.Core.Structs; + +namespace MoonTools.Core.Bonk +{ + public static class SweepTest + { + /// + /// Performs a sweep test on rectangles. + /// + /// + /// A spatial hash. + /// + /// A transform by which to transform the IHasAABB2D. + /// Given in world-space. + /// + public static SweepResult Rectangle(SpatialHash spatialHash, Rectangle rectangle, Transform2D transform, Vector2 ray) where T : IEquatable + { + var transformedAABB = rectangle.TransformedAABB(transform); + var sweepBox = SweepBox(transformedAABB, ray); + + var shortestDistance = float.MaxValue; + var nearestID = default(T); + Rectangle? nearestRectangle = null; + Transform2D? nearestTransform = null; + + foreach (var (id, shape, shapeTransform) in spatialHash.Retrieve(sweepBox)) + { + if (shape is Rectangle otherRectangle) + { + var otherTransformedAABB = otherRectangle.TransformedAABB(shapeTransform); + float xInvEntry, yInvEntry; + + if (ray.X > 0) + { + xInvEntry = shapeTransform.Position.X - (transform.Position.X + transformedAABB.Width); + } + else + { + xInvEntry = (shapeTransform.Position.X + otherTransformedAABB.Width) - transform.Position.X; + } + + if (ray.Y > 0) + { + yInvEntry = shapeTransform.Position.Y - (transform.Position.Y + transformedAABB.Height); + } + else + { + yInvEntry = (shapeTransform.Position.Y + otherTransformedAABB.Height) - shapeTransform.Position.Y; + } + + float xEntry, yEntry; + + if (ray.X == 0) + { + xEntry = float.MinValue; + } + else + { + xEntry = xInvEntry / ray.X; + } + + if (ray.Y == 0) + { + yEntry = float.MinValue; + } + else + { + yEntry = yInvEntry / ray.Y; + } + + var entryTime = Math.Max(xEntry, yEntry); + + if (entryTime > 0 && entryTime < 1) + { + if (entryTime < shortestDistance) + { + shortestDistance = entryTime; + nearestID = id; + nearestRectangle = rectangle; + nearestTransform = shapeTransform; + } + } + } + } + + if (nearestRectangle.HasValue) + { + return new SweepResult(true, ray * shortestDistance, nearestID, nearestRectangle.Value, nearestTransform.Value); + } + else + { + return SweepResult.False; + } + } + + private static AABB SweepBox(AABB aabb, Vector2 ray) + { + return new AABB( + Math.Min(aabb.Min.X, aabb.Min.X + ray.X), + Math.Min(aabb.Min.Y, aabb.Min.Y + ray.Y), + Math.Max(aabb.Max.X, aabb.Max.X + ray.X), + Math.Max(aabb.Max.Y, aabb.Max.Y + ray.Y) + ); + } + } +} diff --git a/Test/SweepTestTest.cs b/Test/SweepTestTest.cs new file mode 100644 index 0000000..cd7b1d4 --- /dev/null +++ b/Test/SweepTestTest.cs @@ -0,0 +1,34 @@ +using System.Numerics; +using FluentAssertions; +using MoonTools.Core.Bonk; +using MoonTools.Core.Structs; +using NUnit.Framework; + +namespace Tests +{ + class SweepTestTest + { + [Test] + public void SweepsThrough() + { + var rectangle = new Rectangle(4, 4); + var transform = new Transform2D(new Position2D(-6, 0)); + + var otherRectangle = new Rectangle(4, 4); + var otherTransform = new Transform2D(new Position2D(6, 0)); + + var farthestRectangle = new Rectangle(4, 4); + var farthestTransform = new Transform2D(new Position2D(12, 0)); + + var spatialHash = new SpatialHash(16); + spatialHash.Insert(1, otherRectangle, otherTransform); + spatialHash.Insert(2, farthestRectangle, farthestTransform); + + SweepTest.Rectangle(spatialHash, rectangle, transform, new Vector2(12, 0)).Should().Be( + new SweepResult(true, new Vector2(8, 0), 1, otherRectangle, otherTransform) + ); + + SweepTest.Rectangle(spatialHash, rectangle, transform, new Vector2(-12, 0)).Hit.Should().BeFalse(); + } + } +}