diff --git a/.gitignore b/.gitignore index cbbd0b5..5ed3d3e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ bin/ -obj/ \ No newline at end of file +obj/ +.vscode \ No newline at end of file diff --git a/Bonk/Bonk.csproj b/Bonk/Bonk.csproj index 4a5064c..7999b43 100644 --- a/Bonk/Bonk.csproj +++ b/Bonk/Bonk.csproj @@ -16,5 +16,6 @@ + \ No newline at end of file diff --git a/Bonk/EPA2D.cs b/Bonk/EPA2D.cs index 2219f26..931be03 100644 --- a/Bonk/EPA2D.cs +++ b/Bonk/EPA2D.cs @@ -5,6 +5,7 @@ */ using Microsoft.Xna.Framework; +using MoonTools.Core.Bonk.Extensions; using MoonTools.Core.Structs; using System; using System.Collections.Generic; @@ -20,14 +21,15 @@ namespace MoonTools.Core.Bonk public static class EPA2D { // vector returned gives direction from A to B - public static Vector2 Intersect(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, IEnumerable givenSimplexVertices) + public static Vector2 Intersect(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, (Func, Vector2, Vector2) givenSimplexVertices) { var simplexVertices = new SimplexVertices(new Vector2?[36]); - foreach (var vertex in givenSimplexVertices) - { - simplexVertices.Add(vertex); - } + var (simplexSupport, a, b) = givenSimplexVertices; + simplexVertices.Add(simplexSupport(a)); + simplexVertices.Add(simplexSupport(b)); + simplexVertices.Add(simplexSupport(-a)); + simplexVertices.Add(simplexSupport(-b)); var e0 = (simplexVertices[1].X - simplexVertices[0].X) * (simplexVertices[1].Y + simplexVertices[0].Y); var e1 = (simplexVertices[2].X - simplexVertices[1].X) * (simplexVertices[2].Y + simplexVertices[1].Y); diff --git a/Bonk/GJK2D.cs b/Bonk/GJK2D.cs index 76b23e2..ea814bc 100644 --- a/Bonk/GJK2D.cs +++ b/Bonk/GJK2D.cs @@ -1,126 +1,87 @@ -/* - * Implementation of the GJK collision algorithm - * Based on some math blogs - * https://blog.hamaluik.ca/posts/building-a-collision-engine-part-1-2d-gjk-collision-detection/ - * and some code from https://github.com/kroitor/gjk.c - */ - -using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework; using MoonTools.Core.Structs; using System; +using MoonTools.Core.Bonk.Extensions; namespace MoonTools.Core.Bonk { public static class GJK2D { - private enum SolutionStatus + public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) { - NoIntersection, - Intersection, - StillSolving + return OriginInside(MinkowskiDifference(shapeA, transformA, shapeB, transformB)); } - public static ValueTuple TestCollision(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB) + public static (bool, (Func, Vector2, Vector2)) CollisionAndSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) { - var vertices = new SimplexVertices(new Vector2?[] { null, null, null, null }); + var support = MinkowskiDifference(shapeA, transformA, shapeB, transformB); + var result = OriginInsideWithSimplex(support); + return (result.Item1, (support, result.Item2, result.Item3)); + } - const SolutionStatus solutionStatus = SolutionStatus.StillSolving; - var direction = Transform2DB.Position - Transform2DA.Position; + private static Func MinkowskiDifference(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) + { + return direction => shapeA.Support(direction, transformA) - shapeB.Support(-direction, transformB); + } - var result = (solutionStatus, direction); + private static bool OriginInside(Func support) + { + var a = support(Vector2.UnitX); + var b = support(-a); - while (result.solutionStatus == SolutionStatus.StillSolving) + return Vector2.Dot(a, b) > 0 ? false : CheckSimplex(support, a, b); + } + + private static (bool, Vector2, Vector2) OriginInsideWithSimplex(Func support) + { + var a = support(Vector2.UnitX); + var b = support(-a); + + return Vector2.Dot(a, b) > 0 ? (false, a, b) : Simplex(support, a, b); + } + + private static bool CheckSimplex(Func support, Vector2 a, Vector2 b) + { + var axb = a.Cross(b); + var c = support((b - a).Perpendicular()); + var axc = a.Cross(c); + var bxc = b.Cross(c); + var cxb = -bxc; + + return (b - a) == Vector2.Zero || (axb.Y > 0 != bxc.Y > 0 ? CheckSimplex(support, b, c) : (axc.Y > 0 != cxb.Y > 0 ? CheckSimplex(support, a, c) : true)); + } + + private static (bool, Vector2, Vector2) Simplex(Func support, Vector2 a, Vector2 b) + { + if ((b - a) == Vector2.Zero) { - result = EvolveSimplex(shapeA, Transform2DA, shapeB, Transform2DB, vertices, result.direction); + return (false, a, b); } - - return ValueTuple.Create(result.solutionStatus == SolutionStatus.Intersection, vertices); - } - - private static (SolutionStatus, Vector2) EvolveSimplex(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, SimplexVertices vertices, Vector2 direction) - { - switch(vertices.Count) + else { - case 0: - if (direction == Vector2.Zero) + var c = support((b - a).Perpendicular()); + var axb = a.Cross(b); + var bxc = b.Cross(c); + + if (axb.Y > 0 != bxc.Y > 0) + { + return Simplex(support, b, c); + } + else + { + var axc = a.Cross(c); + var cxb = -bxc; + + if (axc.Y > 0 != cxb.Y > 0) { - direction = Vector2.UnitX; + return Simplex(support, a, b); } - break; - - case 1: - direction *= -1; - break; - - case 2: - var ab = vertices[1] - vertices[0]; - var a0 = vertices[0] * -1; - - direction = TripleProduct(ab, a0, ab); - if (direction == Vector2.Zero) - { - direction = Perpendicular(ab); - } - break; - - case 3: - var c0 = vertices[2] * -1; - var bc = vertices[1] - vertices[2]; - var ca = vertices[0] - vertices[2]; - - var bcNorm = TripleProduct(ca, bc, bc); - var caNorm = TripleProduct(bc, ca, ca); - - // the origin is outside line bc - // get rid of a and add a new support in the direction of bcNorm - if (Vector2.Dot(bcNorm, c0) > 0) - { - vertices.RemoveAt(0); - direction = bcNorm; - } - // the origin is outside line ca - // get rid of b and add a new support in the direction of caNorm - else if (Vector2.Dot(caNorm, c0) > 0) - { - vertices.RemoveAt(1); - direction = caNorm; - } - // the origin is inside both ab and ac, - // so it must be inside the triangle! else { - return (SolutionStatus.Intersection, direction); + return (true, a, b); } - break; + } } - - return (AddSupport(shapeA, Transform2DA, shapeB, Transform2DB, vertices, direction) ? - SolutionStatus.StillSolving : - SolutionStatus.NoIntersection, direction); - } - - private static bool AddSupport(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, SimplexVertices vertices, Vector2 direction) - { - var newVertex = shapeA.Support(direction, Transform2DA) - shapeB.Support(-direction, Transform2DB); - vertices.Add(newVertex); - return Vector2.Dot(direction, newVertex) >= 0; - } - - private static Vector2 TripleProduct(Vector2 a, Vector2 b, Vector2 c) - { - var A = new Vector3(a.X, a.Y, 0); - var B = new Vector3(b.X, b.Y, 0); - var C = new Vector3(c.X, c.Y, 0); - - var first = Vector3.Cross(A, B); - var second = Vector3.Cross(first, C); - - return new Vector2(second.X, second.Y); - } - - private static Vector2 Perpendicular(Vector2 v) - { - return new Vector2(v.Y, -v.X); } } } diff --git a/Bonk/Line.cs b/Bonk/Line.cs index e35dd6e..62456d3 100644 --- a/Bonk/Line.cs +++ b/Bonk/Line.cs @@ -1,4 +1,5 @@ using System; +using System.Collections.Generic; using Microsoft.Xna.Framework; using MoonTools.Core.Structs; @@ -6,17 +7,28 @@ namespace MoonTools.Core.Bonk { public struct Line : IShape2D, IEquatable { - private Position2D[] vertices; + private Position2D v0; + private Position2D v1; + + private IEnumerable vertices + { + get + { + yield return v0; + yield return v0; + } + } public Line(Position2D start, Position2D end) { - vertices = new Position2D[2] { start, end }; + v0 = start; + v1 = end; } public Vector2 Support(Vector2 direction, Transform2D transform) { - var TransformedStart = Vector2.Transform(vertices[0], transform.TransformMatrix); - var TransformedEnd = Vector2.Transform(vertices[1], transform.TransformMatrix); + var TransformedStart = Vector2.Transform(v0, transform.TransformMatrix); + var TransformedEnd = Vector2.Transform(v1, transform.TransformMatrix); return Vector2.Dot(TransformedStart, direction) > Vector2.Dot(TransformedEnd, direction) ? TransformedStart : TransformedEnd; @@ -32,7 +44,7 @@ namespace MoonTools.Core.Bonk if (other is Line) { var otherLine = (Line)other; - return vertices[0].ToVector2() == otherLine.vertices[0].ToVector2() && vertices[1].ToVector2() == otherLine.vertices[1].ToVector2(); + return v0.ToVector2() == otherLine.v0.ToVector2() && v1.ToVector2() == otherLine.v1.ToVector2(); } return false; diff --git a/Bonk/Rectangle.cs b/Bonk/Rectangle.cs index e880119..fb44184 100644 --- a/Bonk/Rectangle.cs +++ b/Bonk/Rectangle.cs @@ -1,6 +1,9 @@ using System; +using System.Collections.Generic; +using System.Linq; using Microsoft.Xna.Framework; using MoonTools.Core.Structs; +using MoreLinq; namespace MoonTools.Core.Bonk { @@ -11,7 +14,16 @@ namespace MoonTools.Core.Bonk public int MaxX { get; } public int MaxY { get; } - private Position2D[] vertices; + private IEnumerable vertices + { + get + { + yield return new Position2D(MinX, MinY); + yield return new Position2D(MinX, MaxY); + yield return new Position2D(MaxX, MinY); + yield return new Position2D(MaxX, MaxY); + } + } public Rectangle(int minX, int minY, int maxX, int maxY) { @@ -19,33 +31,11 @@ namespace MoonTools.Core.Bonk MinY = minY; MaxX = maxX; MaxY = maxY; - - vertices = new Position2D[4] - { - new Position2D(minX, minY), - new Position2D(minX, maxY), - new Position2D(maxX, minY), - new Position2D(maxX, maxY) - }; } public Vector2 Support(Vector2 direction, Transform2D transform) { - var furthestDistance = float.NegativeInfinity; - var furthestVertex = Vector2.Transform(vertices[0], transform.TransformMatrix); - - foreach (var v in vertices) - { - var TransformedVertex = Vector2.Transform(v, transform.TransformMatrix); - var distance = Vector2.Dot(TransformedVertex, direction); - if (distance > furthestDistance) - { - furthestDistance = distance; - furthestVertex = TransformedVertex; - } - } - - return furthestVertex; + return vertices.Select(vertex => Vector2.Transform(vertex, transform.TransformMatrix)).MaxBy(transformed => Vector2.Dot(transformed, direction)).First(); } public AABB AABB(Transform2D Transform2D) diff --git a/Bonk/Vector2Extensions.cs b/Bonk/Vector2Extensions.cs new file mode 100644 index 0000000..f411b4a --- /dev/null +++ b/Bonk/Vector2Extensions.cs @@ -0,0 +1,18 @@ +using Microsoft.Xna.Framework; + +namespace MoonTools.Core.Bonk.Extensions +{ + public static class Vector2Extensions + { + public static Vector2 Cross(this Vector2 a, Vector2 b) + { + var vec3 = Vector3.Cross(new Vector3(a.X, a.Y, 0), new Vector3(b.X, b.Y, 0)); + return new Vector2(vec3.X, vec3.Y); + } + + public static Vector2 Perpendicular(this Vector2 v) + { + return new Vector2(v.Y, -v.X); + } + } +} \ No newline at end of file diff --git a/Test/EPA2DTest.cs b/Test/EPA2DTest.cs index df35325..4459b9c 100644 --- a/Test/EPA2DTest.cs +++ b/Test/EPA2DTest.cs @@ -18,11 +18,11 @@ namespace Tests var squareB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1); var transformB = new Transform2D(new Vector2(1.5f, 0)); - var test = GJK2D.TestCollision(squareA, transformA, squareB, transformB); + var (result, simplex) = GJK2D.CollisionAndSimplex(squareA, transformA, squareB, transformB); - Assert.That(test.Item1, Is.True); + result.Should().BeTrue(); - var intersection = EPA2D.Intersect(squareA, transformA, squareB, transformB, test.Item2); + var intersection = EPA2D.Intersect(squareA, transformA, squareB, transformB, simplex); intersection.X.Should().Be(1f); intersection.Y.Should().Be(0); @@ -36,11 +36,11 @@ namespace Tests var circleB = new Circle(1); var transformB = new Transform2D(new Vector2(1, 1)); - var test = GJK2D.TestCollision(circleA, transformA, circleB, transformB); + var (result, simplex) = GJK2D.CollisionAndSimplex(circleA, transformA, circleB, transformB); - Assert.That(test.Item1, Is.True); + result.Should().BeTrue(); - var intersection = EPA2D.Intersect(circleA, transformA, circleB, transformB, test.Item2); + var intersection = EPA2D.Intersect(circleA, transformA, circleB, transformB, simplex); var ix = circleA.Radius * (float)Math.Cos(Math.PI / 4) - (circleB.Radius * (float)Math.Cos(5 * Math.PI / 4) + transformB.Position.X); var iy = circleA.Radius * (float)Math.Sin(Math.PI / 4) - (circleB.Radius * (float)Math.Sin(5 * Math.PI / 4) + transformB.Position.Y); @@ -57,11 +57,11 @@ namespace Tests var square = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1); var transformB = Transform2D.DefaultTransform; - var test = GJK2D.TestCollision(line, transformA, square, transformB); + var (result, simplex) = GJK2D.CollisionAndSimplex(line, transformA, square, transformB); - Assert.That(test.Item1, Is.True); + result.Should().BeTrue(); - var intersection = EPA2D.Intersect(line, transformA, square, transformB, test.Item2); + var intersection = EPA2D.Intersect(line, transformA, square, transformB, simplex); intersection.X.Should().Be(-1); intersection.Y.Should().Be(1); diff --git a/Test/GJK2DTest.cs b/Test/GJK2DTest.cs index 81a868a..b054e15 100644 --- a/Test/GJK2DTest.cs +++ b/Test/GJK2DTest.cs @@ -2,6 +2,7 @@ using MoonTools.Core.Bonk; using MoonTools.Core.Structs; using Microsoft.Xna.Framework; +using FluentAssertions; namespace Tests { @@ -13,7 +14,7 @@ namespace Tests var lineA = new Line(new Position2D(-1, -1), new Position2D(1, 1)); var lineB = new Line(new Position2D(-1, 1), new Position2D(1, -1)); - Assert.IsTrue(GJK2D.TestCollision(lineA, Transform2D.DefaultTransform, lineB, Transform2D.DefaultTransform).Item1); + GJK2D.TestCollision(lineA, Transform2D.DefaultTransform, lineB, Transform2D.DefaultTransform).Should().BeTrue(); } [Test] @@ -22,7 +23,7 @@ namespace Tests var lineA = new Line(new Position2D(0, 1), new Position2D(1, 0)); var lineB = new Line(new Position2D(-1, -1), new Position2D(-2, -2)); - Assert.IsFalse(GJK2D.TestCollision(lineA, Transform2D.DefaultTransform, lineB, Transform2D.DefaultTransform).Item1); + GJK2D.TestCollision(lineA, Transform2D.DefaultTransform, lineB, Transform2D.DefaultTransform).Should().BeFalse(); } [Test] @@ -33,7 +34,7 @@ namespace Tests var circleB = new Circle(2); var transformB = new Transform2D(new Vector2(1, 1)); - Assert.IsTrue(GJK2D.TestCollision(circleA, transformA, circleB, transformB).Item1); + Assert.IsTrue(GJK2D.TestCollision(circleA, transformA, circleB, transformB)); } [Test] @@ -44,7 +45,7 @@ namespace Tests var circleB = new Circle(2); var transformB = new Transform2D(new Vector2(5, 5)); - Assert.IsFalse(GJK2D.TestCollision(circleA, transformA, circleB, transformB).Item1); + Assert.IsFalse(GJK2D.TestCollision(circleA, transformA, circleB, transformB)); } [Test] @@ -64,7 +65,7 @@ namespace Tests var transformB = new Transform2D(new Vector2(0.5f, 0.5f)); - Assert.IsTrue(GJK2D.TestCollision(shapeA, transformA, shapeB, transformB).Item1); + Assert.IsTrue(GJK2D.TestCollision(shapeA, transformA, shapeB, transformB)); } [Test] @@ -84,7 +85,7 @@ namespace Tests var transformB = new Transform2D(new Vector2(5, 0)); - Assert.IsFalse(GJK2D.TestCollision(shapeA, transformA, shapeB, transformB).Item1); + Assert.IsFalse(GJK2D.TestCollision(shapeA, transformA, shapeB, transformB)); } [Test] @@ -101,7 +102,7 @@ namespace Tests var transformB = Transform2D.DefaultTransform; - Assert.IsTrue(GJK2D.TestCollision(line, transformA, polygon, transformB).Item1); + Assert.IsTrue(GJK2D.TestCollision(line, transformA, polygon, transformB)); } [Test] @@ -118,7 +119,7 @@ namespace Tests var transformB = Transform2D.DefaultTransform; - Assert.IsFalse(GJK2D.TestCollision(line, transformA, polygon, transformB).Item1); + Assert.IsFalse(GJK2D.TestCollision(line, transformA, polygon, transformB)); } [Test] @@ -129,7 +130,7 @@ namespace Tests var circle = new Circle(1); var transformB = Transform2D.DefaultTransform; - Assert.IsTrue(GJK2D.TestCollision(line, transformA, circle, transformB).Item1); + Assert.IsTrue(GJK2D.TestCollision(line, transformA, circle, transformB)); } [Test] @@ -140,7 +141,7 @@ namespace Tests var circle = new Circle(1); var transformB = Transform2D.DefaultTransform; - Assert.IsFalse(GJK2D.TestCollision(line, transformA, circle, transformB).Item1); + Assert.IsFalse(GJK2D.TestCollision(line, transformA, circle, transformB)); } [Test] @@ -156,7 +157,7 @@ namespace Tests var transformB = Transform2D.DefaultTransform; - Assert.IsTrue(GJK2D.TestCollision(circle, transformA, square, transformB).Item1); + Assert.IsTrue(GJK2D.TestCollision(circle, transformA, square, transformB)); } [Test] @@ -171,7 +172,7 @@ namespace Tests ); var squareTransform = Transform2D.DefaultTransform; - Assert.IsFalse(GJK2D.TestCollision(circle, circleTransform, square, squareTransform).Item1); + Assert.IsFalse(GJK2D.TestCollision(circle, circleTransform, square, squareTransform)); } [Test] @@ -183,7 +184,7 @@ namespace Tests var rectangleB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1); var transformB = new Transform2D(new Vector2(1, 0)); - Assert.IsTrue(GJK2D.TestCollision(rectangleA, transformA, rectangleB, transformB).Item1); + Assert.IsTrue(GJK2D.TestCollision(rectangleA, transformA, rectangleB, transformB)); } [Test] @@ -195,7 +196,7 @@ namespace Tests var rectangleB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1); var transformB = new Transform2D(new Vector2(1, 0)); - Assert.IsTrue(GJK2D.TestCollision(rectangleA, transformA, rectangleB, transformB).Item1); + Assert.IsTrue(GJK2D.TestCollision(rectangleA, transformA, rectangleB, transformB)); } } } diff --git a/Test/Test.csproj b/Test/Test.csproj index 505ac85..31e8e2e 100644 --- a/Test/Test.csproj +++ b/Test/Test.csproj @@ -1,6 +1,6 @@ - netcoreapp2.2 + netcoreapp3.0 false