MoonsideGames
/
MoonWorks
1
0
Fork 1
Code Issues 4 Pull Requests Projects Releases 1 Activity

line test + improving AABB sweep

evan 2023-01-15 16:41:36 -08:00
parent 27d7e23b3f
commit 4b783b22b3
2 changed files with 258 additions and 194 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
src/Collision/Fixed/AABB2D.cs
View File

@ -66,6 +66,14 @@ namespace MoonWorks.Collision.Fixed
);
}
// When you don't know which values are smaller!
public static AABB2D Create(Fix64 x1, Fix64 y1, Fix64 x2, Fix64 y2)
{
var min = new Vector2(Fix64.Min(x1, x2), Fix64.Min(y1, y2));
var max = new Vector2(Fix64.Max(x1, x2), Fix64.Max(y1, y2));
return new AABB2D(min, max);
}
static readonly Fix64 Half = Fix64.FromFraction(1, 2);
/// <summary>
@ -185,67 +193,25 @@ namespace MoonWorks.Collision.Fixed
return a.Left < b.Right && a.Right > b.Left && a.Top < b.Bottom && a.Bottom > b.Top;
}
// FIXME: this is broken
public static bool SweepTest(AABB2D a, AABB2D b, Vector2 aMovement, Vector2 bMovement, out Fix64 normalizedTime)
private static AABB2D RotateAroundOrigin(AABB2D aabb)
{
if (TestOverlap(a, b))
{
normalizedTime = Fix64.Zero;
return true;
return Create(-aabb.Min.X, -aabb.Min.Y, -aabb.Max.X, -aabb.Max.Y);
}
normalizedTime = Fix64.One;
var relativeVelocity = bMovement - aMovement;
Vector2 entry = Vector2.Zero;
if (a.Max.X < b.Min.X && relativeVelocity.X < 0)
private static AABB2D MinkowskiSum(AABB2D a, AABB2D b)
{
entry.X = (a.Max.X - b.Min.X) / relativeVelocity.X;
}
else if (b.Max.X < a.Min.X && relativeVelocity.X > 0)
{
entry.X = (a.Min.X - b.Max.X) / relativeVelocity.X;
return new AABB2D(a.Min.X + b.Min.X, a.Min.Y + b.Min.Y, a.Max.X + b.Max.X, a.Max.Y + b.Max.Y);
}
if (a.Max.Y < b.Min.Y && relativeVelocity.Y < 0)
public static bool SweepTest(AABB2D a, AABB2D b, Vector2 aMovement, Vector2 bMovement, out Fix64 entry, out Fix64 exit)
{
entry.Y = (a.Max.Y - b.Min.Y) / relativeVelocity.Y;
}
else if (b.Max.Y > a.Min.Y && relativeVelocity.Y > 0)
{
entry.Y = (a.Min.Y - b.Max.Y) / relativeVelocity.Y;
}
var rotatedA = RotateAroundOrigin(a);
var sum = MinkowskiSum(rotatedA, b);
Vector2 exit = new Vector2(Fix64.MaxValue, Fix64.MaxValue);
if (b.Max.X > a.Min.X && relativeVelocity.X < 0)
{
exit.X = (a.Min.X - b.Max.X) / relativeVelocity.X;
}
else if (a.Max.X > b.Min.X && relativeVelocity.X > 0)
{
exit.Y = (a.Max.X - b.Min.X) / relativeVelocity.X;
}
var relativeMovement = aMovement - bMovement;
var line = new Line(Vector2.Zero, relativeMovement);
if (b.Max.Y > a.Min.Y && relativeVelocity.Y < 0)
{
exit.Y = (a.Min.Y - b.Max.Y) / relativeVelocity.Y;
}
else if (a.Max.Y > b.Min.Y && relativeVelocity.Y > 0)
{
exit.Y = (a.Max.Y - b.Min.Y) / relativeVelocity.Y;
}
Fix64 firstTime = Fix64.Max(entry.X, entry.Y);
Fix64 lastTime = Fix64.Min(exit.X, exit.Y);
if (firstTime <= lastTime && firstTime > 0)
{
normalizedTime = firstTime;
return true;
}
return false;
return NarrowPhase.TestLineAABBOverlap(line, sum, out entry, out exit);
}
public override bool Equals(object obj)

98
src/Collision/Fixed/NarrowPhase.cs
View File

@ -93,6 +93,13 @@ namespace MoonWorks.Collision.Fixed
return TestCollision(rectangle, transformA, rectangleB, transformB);
}
}
else if (shapeA is Line line)
{
if (shapeB is Line lineB)
{
return TestCollision(line, transformA, lineB, transformB);
}
}
return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1;
}
@ -172,6 +179,11 @@ namespace MoonWorks.Collision.Fixed
}
}
public static bool TestCollision(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB)
{
return TestLineOverlap(lineA, transformA, lineB, transformB);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static bool TestRectangleOverlap(in Rectangle rectangleA, in Transform2D transformA, in Rectangle rectangleB, in Transform2D transformB)
{
@ -237,6 +249,92 @@ namespace MoonWorks.Collision.Fixed
return distanceSquared < radiusSumSquared;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static bool TestLineOverlap(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB)
{
var b = lineA.End - lineA.Start;
var d = lineB.End - lineB.Start;
var bDotDPerp = b.X * d.Y - b.Y * d.X;
if (bDotDPerp == Fix64.Zero)
{
return false;
}
var c = lineB.Start - lineA.Start;
var t = (c.X * d.Y - c.Y * d.X) / bDotDPerp;
if (t < 0 || t > 1)
{
return false;
}
var u = (c.X * b.Y - c.Y * b.X) / bDotDPerp;
if (u < 0 || u > 1)
{
return false;
}
return true;
}
public static bool TestLineOverlap(in Line lineA, in Transform2D transformA, in Line lineB, in Transform2D transformB, out Vector2 intersection)
{
intersection = Vector2.Zero;
var b = lineA.End - lineA.Start;
var d = lineB.End - lineB.Start;
var bDotDPerp = b.X * d.Y - b.Y * d.X;
if (bDotDPerp == Fix64.Zero)
{
return false;
}
var c = lineB.Start - lineA.Start;
var t = (c.X * d.Y - c.Y * d.X) / bDotDPerp;
if (t < 0 || t > 1)
{
return false;
}
var u = (c.X * b.Y - c.Y * b.X) / bDotDPerp;
if (u < 0 || u > 1)
{
return false;
}
intersection = lineA.Start + t * b;
return true;
}
public static bool TestLineAABBOverlap(in Line line, in AABB2D aabb, out Fix64 entry, out Fix64 exit)
{
entry = Fix64.MinValue;
exit = Fix64.MaxValue;
var lineDirection = line.End - line.Start;
if (lineDirection.X != Fix64.Zero)
{
var tx1 = (aabb.Min.X - line.Start.X) / lineDirection.X;
var tx2 = (aabb.Max.X - line.Start.X) / lineDirection.X;
entry = Fix64.Max(entry, Fix64.Min(tx1, tx2));
exit = Fix64.Min(exit, Fix64.Max(tx1, tx2));
}
if (lineDirection.Y != Fix64.Zero)
{
var ty1 = (aabb.Min.Y - line.Start.Y) / lineDirection.Y;
var ty2 = (aabb.Max.Y - line.Start.Y) / lineDirection.Y;
entry = Fix64.Max(entry, Fix64.Min(ty1, ty2));
exit = Fix64.Min(exit, Fix64.Max(ty1, ty2));
}
return exit >= entry;
}
public static bool TestPointOverlap(in Point pointA, in Transform2D transformA, in Point pointB, in Transform2D transformB)
{
return transformA.Position == transformB.Position;