MoonTools.Bonk/Bonk/SweepTest/SweepTest.cs

130 lines
4.6 KiB
C#
Raw Normal View History

2020-01-05 07:17:29 +00:00
using System;
using System.Numerics;
using MoonTools.Core.Structs;
namespace MoonTools.Core.Bonk
{
public static class SweepTest
{
/// <summary>
2020-01-05 21:10:52 +00:00
/// Performs a sweep test on rectangles. Returns the position 1 pixel before overlap occurs.
2020-01-05 07:17:29 +00:00
/// </summary>
/// <typeparam name="T"></typeparam>
/// <param name="spatialHash">A spatial hash.</param>
/// <param name="rectangle"></param>
/// <param name="transform">A transform by which to transform the IHasAABB2D.</param>
/// <param name="ray">Given in world-space.</param>
/// <returns></returns>
public static SweepResult<T, Rectangle> Rectangle<T>(SpatialHash<T> spatialHash, Rectangle rectangle, Transform2D transform, Vector2 ray) where T : IEquatable<T>
{
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))
{
2020-01-05 21:10:52 +00:00
Rectangle otherRectangle;
Transform2D otherTransform;
AABB otherTransformedAABB;
if (shape is Rectangle)
2020-01-05 07:17:29 +00:00
{
2020-01-05 21:10:52 +00:00
otherRectangle = (Rectangle)shape;
otherTransformedAABB = shape.TransformedAABB(shapeTransform);
otherTransform = shapeTransform;
}
else if (shape is MultiShape multiShape && multiShape.IsSingleShape<Rectangle>())
{
Transform2D rectangleOffset;
(otherRectangle, rectangleOffset) = multiShape.ShapeTransformPair<Rectangle>();
otherTransform = shapeTransform.Compose(rectangleOffset);
otherTransformedAABB = shape.TransformedAABB(otherTransform);
}
else
{
continue;
}
2020-01-05 07:17:29 +00:00
2020-01-05 21:10:52 +00:00
float xInvEntry, yInvEntry;
2020-01-05 07:17:29 +00:00
2020-01-05 21:10:52 +00:00
if (ray.X > 0)
{
xInvEntry = otherTransformedAABB.Left - (transformedAABB.Right);
}
else
{
xInvEntry = (otherTransformedAABB.Right) - transformedAABB.Left;
}
2020-01-05 07:17:29 +00:00
2020-01-05 21:10:52 +00:00
if (ray.Y > 0)
{
yInvEntry = otherTransformedAABB.Top - (transformedAABB.Bottom);
}
else
{
yInvEntry = (otherTransformedAABB.Bottom) - transformedAABB.Top;
}
2020-01-05 07:17:29 +00:00
2020-01-05 21:10:52 +00:00
float xEntry, yEntry;
2020-01-05 07:17:29 +00:00
2020-01-05 21:10:52 +00:00
if (ray.X == 0)
{
xEntry = float.MinValue;
}
else
{
xEntry = xInvEntry / ray.X;
}
2020-01-05 07:17:29 +00:00
2020-01-05 21:10:52 +00:00
if (ray.Y == 0)
{
yEntry = float.MinValue;
}
else
{
yEntry = yInvEntry / ray.Y;
}
2020-01-05 07:17:29 +00:00
2020-01-05 21:10:52 +00:00
var entryTime = Math.Max(xEntry, yEntry);
if (entryTime >= 0 && entryTime <= 1)
{
if (entryTime < shortestDistance)
2020-01-05 07:17:29 +00:00
{
2020-01-05 21:10:52 +00:00
shortestDistance = entryTime;
nearestID = id;
nearestRectangle = otherRectangle;
nearestTransform = shapeTransform;
2020-01-05 07:17:29 +00:00
}
}
2020-01-05 21:10:52 +00:00
2020-01-05 07:17:29 +00:00
}
if (nearestRectangle.HasValue)
{
2020-01-05 21:10:52 +00:00
var overlapPosition = ray * shortestDistance;
2020-01-05 21:14:06 +00:00
var correctionX = -Math.Sign(ray.X);
var correctionY = -Math.Sign(ray.Y);
2020-01-05 21:10:52 +00:00
return new SweepResult<T, Rectangle>(true, new Position2D((int)overlapPosition.X + correctionX, (int)overlapPosition.Y + correctionY), nearestID, nearestRectangle.Value, nearestTransform.Value);
2020-01-05 07:17:29 +00:00
}
else
{
return SweepResult<T, Rectangle>.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)
);
}
}
}