// 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); }