diff --git a/Bonk/EPA2D.cs b/Bonk/EPA2D.cs index e4edad5..6b3e1a5 100644 --- a/Bonk/EPA2D.cs +++ b/Bonk/EPA2D.cs @@ -22,15 +22,14 @@ 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, (Func, Vector2, Vector2) givenSimplexVertices) + public static Vector2 Intersect(IShape2D shapeA, Transform2D Transform2DA, IShape2D shapeB, Transform2D Transform2DB, Simplex givenSimplex) { var simplexVertices = new SimplexVertices(new Vector2?[36]); - var (simplexSupport, a, b) = givenSimplexVertices; - simplexVertices.Add(simplexSupport(a)); - simplexVertices.Add(simplexSupport(b)); - simplexVertices.Add(simplexSupport(-a)); - simplexVertices.Add(simplexSupport(-b)); + foreach (var vertex in givenSimplex.Vertices) + { + simplexVertices.Add(vertex); + } 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 e6c06a9..31a8783 100644 --- a/Bonk/GJK2D.cs +++ b/Bonk/GJK2D.cs @@ -2,43 +2,126 @@ using MoonTools.Core.Structs; using System; using MoonTools.Core.Bonk.Extensions; +using System.Collections.Generic; namespace MoonTools.Core.Bonk { - // TODO: get rid of minkowski closure for GC purposes + public struct MinkowskiDifference : IEquatable + { + private IShape2D shapeA; + private Transform2D transformA; + private IShape2D shapeB; + private Transform2D transformB; + + public MinkowskiDifference(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) + { + this.shapeA = shapeA; + this.transformA = transformA; + this.shapeB = shapeB; + this.transformB = transformB; + } + + public bool Equals(MinkowskiDifference other) + { + return + shapeA == other.shapeA && + transformA.Equals(other.transformA) && + shapeB == other.shapeB && + transformB.Equals(other.transformB); + } + + public Vector2 Support(Vector2 direction) + { + return shapeA.Support(direction, transformA) - shapeB.Support(-direction, transformB); + } + } + + public struct Simplex : IShape2D + { + MinkowskiDifference minkowskiDifference; + Vector2 directionA; + Vector2 directionB; + + public Simplex(MinkowskiDifference minkowskiDifference, Vector2 directionA, Vector2 directionB) + { + this.minkowskiDifference = minkowskiDifference; + this.directionA = directionA; + this.directionB = directionB; + } + + public IEnumerable Vertices + { + get + { + yield return (Position2D)Support(directionA); + yield return (Position2D)Support(directionB); + yield return (Position2D)Support(-(directionB - directionA).Perpendicular()); + } + } + + public AABB AABB(Transform2D transform) + { + return Bonk.AABB.FromTransformedVertices(Vertices, transform); + } + + public bool Equals(IShape2D other) + { + if (other is Simplex polytope) + { + return minkowskiDifference.Equals(polytope.minkowskiDifference) && + directionA == polytope.directionA && + directionB == polytope.directionB; + } + + return false; + } + + public Vector2 Support(Vector2 direction) + { + return minkowskiDifference.Support(direction); + } + + public Vector2 Support(Vector2 direction, Transform2D transform) + { + return Vector2.Transform(Support(direction), transform.TransformMatrix); + } + } + public static class GJK2D { public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) { - return OriginInside(MinkowskiDifference(shapeA, transformA, shapeB, transformB)); + var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB); + return OriginInside(minkowskiDifference); } - public static (bool, (Func, Vector2, Vector2)) CollisionAndSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) + public static (bool, Simplex) CollisionAndSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) { - var support = MinkowskiDifference(shapeA, transformA, shapeB, transformB); - var result = OriginInsideWithSimplex(support); - return (result.Item1, (support, result.Item2, result.Item3)); + var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB); + var (collision, a, b) = OriginInsideWithSimplex(minkowskiDifference); + var polytope = new Simplex(minkowskiDifference, a, b); + return (collision, polytope); } - private static Func MinkowskiDifference(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) + private static Vector2 MinkowskiDifference(Vector2 direction, IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) { - return direction => shapeA.Support(direction, transformA) - shapeB.Support(-direction, transformB); + return shapeA.Support(direction, transformA) - shapeB.Support(-direction, transformB); } - private static bool OriginInside(Func support) + private static bool OriginInside(MinkowskiDifference minkowskiDifference) { - var a = support(Vector2.UnitX); - var b = support(-a); + var a = minkowskiDifference.Support(Vector2.UnitX); + var b = minkowskiDifference.Support(-a); - return Vector2.Dot(a, b) > 0 ? false : CheckSimplex(support, a, b); + return Vector2.Dot(a, b) > 0 ? false : CheckSimplex(minkowskiDifference.Support, a, b); } - private static (bool, Vector2, Vector2) OriginInsideWithSimplex(Func support) + private static (bool, Vector2, Vector2) OriginInsideWithSimplex(MinkowskiDifference minkowskiDifference) { - var a = support(Vector2.UnitX); - var b = support(-a); + var a = minkowskiDifference.Support(Vector2.UnitX); + var b = minkowskiDifference.Support(-a); - return Vector2.Dot(a, b) > 0 ? (false, a, b) : Simplex(support, a, b); + return Vector2.Dot(a, b) > 0 ? (false, a, b) : Simplex(minkowskiDifference.Support, a, b); } private static bool CheckSimplex(Func support, Vector2 a, Vector2 b)