public Fix64Vector2 Support(Fix64Vector2 d)
{
Fix64 maxDot = Fix64Vector2.Dot(vertex[0], d);
Fix64Vector2 point = vertex[0];
for (int i = 1; i < vertex.Length; i++)
{
Fix64 now = Fix64Vector2.Dot(vertex[i], d);
if (now > maxDot)
{
maxDot = now;
point = vertex[i];
}
}
return(point + GetPos());
}
private static bool ContainsOrigin(Simplex s, ref Fix64Vector2 dir)
{
var a = s.GetLast();
var AO = -a;
var b = s.GetB();
var AB = b - a;
if (s.Count() == 3)
{
var c = s.GetC();
var AC = c - a;
var abPerp = CalcNomal(AC, AB, AB);
var acPerp = CalcNomal(AB, AC, AC);
if (Fix64Vector2.Dot(abPerp, AO) > Fix64.Zero)
{
s.Remove(c);
dir = abPerp;
}
else
{
if (Fix64Vector2.Dot(acPerp, AO) > Fix64.Zero)
{
s.Remove(b);
dir = acPerp;
}
else
{
return(true);
}
}
}
else
{
var abPerp = CalcNomal(AB, AO, AB);
dir = abPerp;
}
return(false);
}
public static bool GJK(MGFObject a, MGFObject b)
{
Simplex s = new Simplex();
Fix64Vector2 dir = a.GetPos() - b.GetPos();
s.Add(Support(a, b, dir));
dir = -dir;
while (true)
{
s.Add(Support(a, b, dir));
if (Fix64Vector2.Dot(s.GetLast(), dir) <= Fix64.Zero)
{
return(false);
}
else
{
if (ContainsOrigin(s, ref dir))
{
return(true);
}
}
}
}
internal void CalcCollisionDir(MGFObject mgfObject)
{
if (IsTrigger == true)
{
CollisionDir = Forward;
return;
}
Fix64Vector2[] vertexO = mgfObject.GetVertex();
Fix64Vector2 v1 = Fix64Vector2.Rotate(vertexO[0], -(Fix64)90);
Fix64Vector2 v2 = Fix64Vector2.Rotate(vertexO[1], -(Fix64)90);
Fix64Vector2 v3 = -v1;
Fix64Vector2 v4 = -v2;
Fix64Vector2 dir = GetPos() - mgfObject.GetPos();
Fix64 d1 = Fix64Vector2.Dot(v1, dir);
Fix64 d2 = Fix64Vector2.Dot(v2, dir);
Fix64 d3 = Fix64Vector2.Dot(v3, dir);
Fix64 d4 = Fix64Vector2.Dot(v4, dir);
Fix64Vector2 collisionDirVertical = vertexO[0] - vertexO[1];
Fix64Vector2 collisionDirHorizontal = vertexO[1] - vertexO[2];
if (d1 >= Fix64.Zero && d4 >= Fix64.Zero)
{
if (Fix64Vector2.Dot(collisionDirHorizontal, Forward) > Fix64.Zero)
{
CollisionDir = Forward;
}
else
{
if (Fix64Vector2.Dot(collisionDirVertical, Forward) > Fix64.Zero)
{
CollisionDir = collisionDirVertical;
}
else
{
CollisionDir = -collisionDirVertical;
}
}
}
else if (d1 >= Fix64.Zero && d2 >= Fix64.Zero)
{
if (Fix64Vector2.Dot(collisionDirVertical, Forward) < Fix64.Zero)
{
CollisionDir = Forward;
}
else
{
if (Fix64Vector2.Dot(collisionDirHorizontal, Forward) > Fix64.Zero)
{
CollisionDir = collisionDirHorizontal;
}
else
{
CollisionDir = -collisionDirHorizontal;
}
}
}
else if (d2 >= Fix64.Zero && d3 >= Fix64.Zero)
{
if (Fix64Vector2.Dot(collisionDirHorizontal, Forward) < Fix64.Zero)
{
CollisionDir = Forward;
}
else
{
if (Fix64Vector2.Dot(collisionDirVertical, Forward) > Fix64.Zero)
{
CollisionDir = collisionDirVertical;
}
else
{
CollisionDir = -collisionDirVertical;
}
}
}
else if (d3 >= Fix64.Zero && d4 >= Fix64.Zero)
{
if (Fix64Vector2.Dot(collisionDirVertical, Forward) > Fix64.Zero)
{
CollisionDir = Forward;
}
else
{
if (Fix64Vector2.Dot(collisionDirHorizontal, Forward) > Fix64.Zero)
{
CollisionDir = collisionDirHorizontal;
}
else
{
CollisionDir = -collisionDirHorizontal;
}
}
}
}
