private bool Gjk(Collider colliderA, Collider colliderB, out ContactData contactData) { contactData = default; Vec3f a, b, c, d; Vec3f Asa, Asb, Asc, Asd, Bsa, Bsb, Bsc, Bsd; a = b = c = d = Vec3f.Zero; Asa = Asb = Asc = Asd = Vec3f.Zero; Bsa = Bsb = Bsc = Bsd = Vec3f.Zero; Vec3f searchDir = colliderA.localCentroid - colliderB.localCentroid; CsoSupport(colliderB, colliderA, searchDir, out c, out Asc, out Bsc); searchDir = -c; CsoSupport(colliderB, colliderA, searchDir, out b, out Asb, out Bsb); if (Vec3f.Dot(b, searchDir) < 0) { return(false); } Vec3f bc = c - b; searchDir = Vec3f.Cross(Vec3f.Cross(bc, -b), bc); //search perpendicular to line segment towards origin if (searchDir.SqrMagnitude < MathUtils.Epsilon) { searchDir = Vec3f.Cross(bc, Vec3f.Right); //normal with x-axis if (Vec3f.Aproximates(searchDir, Vec3f.Zero)) { searchDir = Vec3f.Cross(bc, -Vec3f.Forward); //normal with z-axis } } int simplexDimension = 2; for (int i = 0; i < 64; i++) { CsoSupport(colliderB, colliderA, searchDir, out a, out Asa, out Bsa); if (Vec3f.Dot(a, searchDir) < 0) { return(false); } simplexDimension++; if (simplexDimension == 3) { UpdateSimplex3(ref a, ref b, ref c, ref d, ref simplexDimension, ref searchDir, ref Asa, ref Asb, ref Asc, ref Asd , ref Bsa, ref Bsb, ref Bsc, ref Bsd); } else if (UpdateSimplex4(ref a, ref b, ref c, ref d, ref simplexDimension, ref searchDir, ref Asa, ref Asb, ref Asc, ref Asd , ref Bsa, ref Bsb, ref Bsc, ref Bsd)) { contactData = Epa(colliderA, colliderB, a, b, c, d, Asa, Asb, Asc, Asd, Bsa, Bsb, Bsc, Bsd); return(true); } } return(false); }