MoonTools.Bonk/Bonk/NarrowPhase/GJK2D.cs

78 lines
2.7 KiB
C#
Raw Normal View History

2019-10-25 10:46:47 +00:00
using Microsoft.Xna.Framework;
2019-09-06 08:11:58 +00:00
using MoonTools.Core.Structs;
2019-10-25 10:46:47 +00:00
using MoonTools.Core.Bonk.Extensions;
2019-09-06 08:11:58 +00:00
namespace MoonTools.Core.Bonk
{
public static class GJK2D
{
2019-10-25 10:46:47 +00:00
public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
2019-09-06 08:11:58 +00:00
{
2019-10-25 19:37:25 +00:00
var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB);
2019-10-25 20:09:03 +00:00
var a = minkowskiDifference.Support(Vector2.UnitX);
var b = minkowskiDifference.Support(-a);
2019-09-06 08:11:58 +00:00
2019-10-25 20:09:03 +00:00
return Vector2.Dot(a, b) > 0 ? false : CheckSimplex(new Simplex(minkowskiDifference, a, b));
2019-10-25 10:46:47 +00:00
}
2019-09-06 08:11:58 +00:00
2019-10-25 20:09:03 +00:00
private static bool CheckSimplex(Simplex simplex)
2019-10-25 10:46:47 +00:00
{
2019-10-25 20:09:03 +00:00
var a = simplex.DirectionA;
var b = simplex.DirectionB;
2019-09-06 08:11:58 +00:00
2019-10-25 20:09:03 +00:00
var axb = a.Cross(b);
var c = simplex.Support((b - a).Perpendicular());
var axc = a.Cross(c);
var bxc = b.Cross(c);
var cxb = -bxc;
2019-09-06 08:11:58 +00:00
2019-10-25 20:09:03 +00:00
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));
2019-10-25 10:46:47 +00:00
}
2019-09-06 08:11:58 +00:00
2019-10-25 20:09:03 +00:00
public static (bool, Simplex) FindCollisionSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
2019-10-25 10:46:47 +00:00
{
2019-10-25 20:09:03 +00:00
var minkowskiDifference = new MinkowskiDifference(shapeA, transformA, shapeB, transformB);
2019-10-25 19:37:25 +00:00
var a = minkowskiDifference.Support(Vector2.UnitX);
var b = minkowskiDifference.Support(-a);
2019-09-06 08:11:58 +00:00
2019-10-25 20:09:03 +00:00
return Vector2.Dot(a, b) > 0 ? (false, default(Simplex)) : Simplex(new Simplex(minkowskiDifference, a, b));
2019-10-25 10:46:47 +00:00
}
2019-09-06 08:11:58 +00:00
2019-10-25 20:09:03 +00:00
private static (bool, Simplex) Simplex(Simplex simplex)
2019-10-25 10:46:47 +00:00
{
2019-10-25 20:09:03 +00:00
var a = simplex.DirectionA;
var b = simplex.DirectionB;
2019-09-06 08:11:58 +00:00
2019-10-25 10:46:47 +00:00
if ((b - a) == Vector2.Zero)
{
2019-10-25 20:09:03 +00:00
return (false, simplex.WithDirections(a, b));
2019-10-25 10:46:47 +00:00
}
else
{
2019-10-25 20:09:03 +00:00
var c = simplex.Support((b - a).Perpendicular());
2019-10-25 10:46:47 +00:00
var axb = a.Cross(b);
var bxc = b.Cross(c);
if (axb.Y > 0 != bxc.Y > 0)
{
2019-10-25 20:09:03 +00:00
return Simplex(simplex.WithDirections(b, c));
2019-10-25 10:46:47 +00:00
}
else
{
var axc = a.Cross(c);
var cxb = -bxc;
if (axc.Y > 0 != cxb.Y > 0)
2019-09-06 08:11:58 +00:00
{
2019-10-25 20:09:03 +00:00
return Simplex(simplex.WithDirections(a, b));
2019-09-06 08:11:58 +00:00
}
else
{
2019-10-25 20:09:03 +00:00
return (true, simplex.WithDirections(a, b));
2019-09-06 08:11:58 +00:00
}
2019-10-25 10:46:47 +00:00
}
2019-09-06 08:11:58 +00:00
}
}
}
}