// Used for debugging
public int GJK_Step(RigidBody m1, RigidBody m2, List <SupportPoint> simplex, ref vec3 Direction, ref int counter)
{
if (counter == 0)
{
simplex.Add(SumSupport(m1, m2, vec3.UnitY));
Direction = -simplex[0].Sup;
}
SupportPoint P = SumSupport(m1, m2, Direction);
if (counter == 1)
{
simplex.Add(P);
}
else if (counter == 2 && vec3.Dot(P.Sup.NormalizedSafe, Direction.NormalizedSafe) < 0)
{
return(2);
}
else if (counter == 2 && DoSimplex(simplex))
{
return(1);
}
else if (counter == 3)
{
Direction = GetNewDirection(simplex);
}
counter++;
if (counter >= 4)
{
counter = 1;
}
return(0);
}
public CollisionResult GJKResult(RigidBody m1, RigidBody m2)
{
List <SupportPoint> simplex = new List <SupportPoint> {
SumSupport(m1, m2, vec3.UnitY)
};
vec3 Direction = -simplex[0].Sup;
CollisionResult result = new CollisionResult(m1, m2, simplex);
for (int i = 0; i < 1000; i++)
{
SupportPoint P = SumSupport(m1, m2, Direction);
if (vec3.Dot(P.Sup.NormalizedSafe, Direction.NormalizedSafe) < 0)
{
return(result);
}
simplex.Add(P);
if (DoSimplex(simplex))
{
result.Colliding = true; return(result);
}
//if (simplex.Find(x => x.Length <= Error).Length > 0 || Direction.Length <= Error) return result;
Direction = GetNewDirection(simplex);
}
Console.WriteLine("GJK exceeded max steps!");
return(result);
}
