MoonTools.Bonk/Bonk/GJK2D.cs

89 lines
3.1 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;
using System;
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
{
2019-10-25 11:07:13 +00:00
// TODO: get rid of minkowski closure for GC purposes
2019-09-06 08:11:58 +00:00
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 10:46:47 +00:00
return OriginInside(MinkowskiDifference(shapeA, transformA, shapeB, transformB));
2019-09-06 08:11:58 +00:00
}
2019-10-25 10:46:47 +00:00
public static (bool, (Func<Vector2, Vector2>, Vector2, Vector2)) CollisionAndSimplex(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
2019-09-06 08:11:58 +00:00
{
2019-10-25 10:46:47 +00:00
var support = MinkowskiDifference(shapeA, transformA, shapeB, transformB);
var result = OriginInsideWithSimplex(support);
return (result.Item1, (support, result.Item2, result.Item3));
}
2019-09-06 08:11:58 +00:00
2019-10-25 10:46:47 +00:00
private static Func<Vector2, Vector2> MinkowskiDifference(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB)
{
return direction => shapeA.Support(direction, transformA) - shapeB.Support(-direction, transformB);
2019-09-06 08:11:58 +00:00
}
2019-10-25 10:46:47 +00:00
private static bool OriginInside(Func<Vector2, Vector2> support)
2019-09-06 08:11:58 +00:00
{
2019-10-25 10:46:47 +00:00
var a = support(Vector2.UnitX);
var b = support(-a);
2019-09-06 08:11:58 +00:00
2019-10-25 10:46:47 +00:00
return Vector2.Dot(a, b) > 0 ? false : CheckSimplex(support, a, b);
}
2019-09-06 08:11:58 +00:00
2019-10-25 10:46:47 +00:00
private static (bool, Vector2, Vector2) OriginInsideWithSimplex(Func<Vector2, Vector2> support)
{
var a = support(Vector2.UnitX);
var b = support(-a);
2019-09-06 08:11:58 +00:00
2019-10-25 10:46:47 +00:00
return Vector2.Dot(a, b) > 0 ? (false, a, b) : Simplex(support, a, b);
}
2019-09-06 08:11:58 +00:00
2019-10-25 10:46:47 +00:00
private static bool CheckSimplex(Func<Vector2, Vector2> 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;
2019-09-06 08:11:58 +00:00
2019-10-25 10:46:47 +00:00
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));
}
2019-09-06 08:11:58 +00:00
2019-10-25 10:46:47 +00:00
private static (bool, Vector2, Vector2) Simplex(Func<Vector2, Vector2> support, Vector2 a, Vector2 b)
{
if ((b - a) == Vector2.Zero)
{
return (false, a, b);
}
else
{
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)
2019-09-06 08:11:58 +00:00
{
2019-10-25 10:46:47 +00:00
return Simplex(support, a, b);
2019-09-06 08:11:58 +00:00
}
else
{
2019-10-25 10:46:47 +00:00
return (true, 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
}
}
}
}