tasty restructuring

pull/2/head
Evan Hemsley 2019-10-25 13:09:03 -07:00
parent a53677ba02
commit a3eec5c01c
3 changed files with 35 additions and 41 deletions

View File

@ -1,75 +1,61 @@
using Microsoft.Xna.Framework; using Microsoft.Xna.Framework;
using MoonTools.Core.Structs; using MoonTools.Core.Structs;
using System;
using MoonTools.Core.Bonk.Extensions; using MoonTools.Core.Bonk.Extensions;
namespace MoonTools.Core.Bonk namespace MoonTools.Core.Bonk
{ {
public static class GJK2D public static class GJK2D
{ {
public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
{ {
var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB); var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB);
return OriginInside(minkowskiDifference);
}
public static (bool, Simplex) CollisionAndSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
{
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 Vector2 MinkowskiDifference(Vector2 direction, IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
{
return shapeA.Support(direction, transformA) - shapeB.Support(-direction, transformB);
}
private static bool OriginInside(MinkowskiDifference minkowskiDifference)
{
var a = minkowskiDifference.Support(Vector2.UnitX); var a = minkowskiDifference.Support(Vector2.UnitX);
var b = minkowskiDifference.Support(-a); var b = minkowskiDifference.Support(-a);
return Vector2.Dot(a, b) > 0 ? false : CheckSimplex(minkowskiDifference.Support, a, b); return Vector2.Dot(a, b) > 0 ? false : CheckSimplex(new Simplex(minkowskiDifference, a, b));
} }
private static (bool, Vector2, Vector2) OriginInsideWithSimplex(MinkowskiDifference minkowskiDifference) private static bool CheckSimplex(Simplex simplex)
{ {
var a = minkowskiDifference.Support(Vector2.UnitX); var a = simplex.DirectionA;
var b = minkowskiDifference.Support(-a); var b = simplex.DirectionB;
return Vector2.Dot(a, b) > 0 ? (false, a, b) : Simplex(minkowskiDifference.Support, a, b);
}
private static bool CheckSimplex(Func<Vector2, Vector2> support, Vector2 a, Vector2 b)
{
var axb = a.Cross(b); var axb = a.Cross(b);
var c = support((b - a).Perpendicular()); var c = simplex.Support((b - a).Perpendicular());
var axc = a.Cross(c); var axc = a.Cross(c);
var bxc = b.Cross(c); var bxc = b.Cross(c);
var cxb = -bxc; 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)); return (b - a) == Vector2.Zero || (axb.Y > 0 != bxc.Y > 0 ? CheckSimplex(simplex.WithDirections(b, c)) : (axc.Y > 0 != cxb.Y > 0 ? CheckSimplex(simplex.WithDirections(a, c)) : true));
} }
private static (bool, Vector2, Vector2) Simplex(Func<Vector2, Vector2> support, Vector2 a, Vector2 b) public static (bool, Simplex) FindCollisionSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
{ {
var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB);
var a = minkowskiDifference.Support(Vector2.UnitX);
var b = minkowskiDifference.Support(-a);
return Vector2.Dot(a, b) > 0 ? (false, default(Simplex)) : Simplex(new Simplex(minkowskiDifference, a, b));
}
private static (bool, Simplex) Simplex(Simplex simplex)
{
var a = simplex.DirectionA;
var b = simplex.DirectionB;
if ((b - a) == Vector2.Zero) if ((b - a) == Vector2.Zero)
{ {
return (false, a, b); return (false, simplex.WithDirections(a, b));
} }
else else
{ {
var c = support((b - a).Perpendicular()); var c = simplex.Support((b - a).Perpendicular());
var axb = a.Cross(b); var axb = a.Cross(b);
var bxc = b.Cross(c); var bxc = b.Cross(c);
if (axb.Y > 0 != bxc.Y > 0) if (axb.Y > 0 != bxc.Y > 0)
{ {
return Simplex(support, b, c); return Simplex(simplex.WithDirections(b, c));
} }
else else
{ {
@ -78,11 +64,11 @@ namespace MoonTools.Core.Bonk
if (axc.Y > 0 != cxb.Y > 0) if (axc.Y > 0 != cxb.Y > 0)
{ {
return Simplex(support, a, b); return Simplex(simplex.WithDirections(a, b));
} }
else else
{ {
return (true, a, b); return (true, simplex.WithDirections(a, b));
} }
} }
} }

View File

@ -11,6 +11,9 @@ namespace MoonTools.Core.Bonk
Vector2 directionA; Vector2 directionA;
Vector2 directionB; Vector2 directionB;
public Vector2 DirectionA { get { return directionA; } }
public Vector2 DirectionB { get { return directionB; } }
public Simplex(MinkowskiDifference minkowskiDifference, Vector2 directionA, Vector2 directionB) public Simplex(MinkowskiDifference minkowskiDifference, Vector2 directionA, Vector2 directionB)
{ {
this.minkowskiDifference = minkowskiDifference; this.minkowskiDifference = minkowskiDifference;
@ -18,6 +21,11 @@ namespace MoonTools.Core.Bonk
this.directionB = directionB; this.directionB = directionB;
} }
public Simplex WithDirections(Vector2 a, Vector2 b)
{
return new Simplex(minkowskiDifference, a, b);
}
public IEnumerable<Position2D> Vertices public IEnumerable<Position2D> Vertices
{ {
get get

View File

@ -18,7 +18,7 @@ namespace Tests
var squareB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1); var squareB = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
var transformB = new Transform2D(new Vector2(1.5f, 0)); var transformB = new Transform2D(new Vector2(1.5f, 0));
var (result, simplex) = GJK2D.CollisionAndSimplex(squareA, transformA, squareB, transformB); var (result, simplex) = GJK2D.FindCollisionSimplex(squareA, transformA, squareB, transformB);
result.Should().BeTrue(); result.Should().BeTrue();
@ -36,7 +36,7 @@ namespace Tests
var circleB = new Circle(1); var circleB = new Circle(1);
var transformB = new Transform2D(new Vector2(1, 1)); var transformB = new Transform2D(new Vector2(1, 1));
var (result, simplex) = GJK2D.CollisionAndSimplex(circleA, transformA, circleB, transformB); var (result, simplex) = GJK2D.FindCollisionSimplex(circleA, transformA, circleB, transformB);
result.Should().BeTrue(); result.Should().BeTrue();
@ -57,7 +57,7 @@ namespace Tests
var square = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1); var square = new MoonTools.Core.Bonk.Rectangle(-1, -1, 1, 1);
var transformB = Transform2D.DefaultTransform; var transformB = Transform2D.DefaultTransform;
var (result, simplex) = GJK2D.CollisionAndSimplex(line, transformA, square, transformB); var (result, simplex) = GJK2D.FindCollisionSimplex(line, transformA, square, transformB);
result.Should().BeTrue(); result.Should().BeTrue();