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