public CSOFace(CSOVertex A, CSOVertex B, CSOVertex C)
{
a = A;
b = B;
c = C;
normal = Vector3.Cross(b.vertCSO - a.vertCSO, c.vertCSO - a.vertCSO).normalized;
distance = Vector3.Dot(a.vertCSO, normal);
}
private static bool GJK(CustomCollider colliderA, CustomCollider colliderB)
{
/*Find the first point of the simplex*/
searchDir = colliderA.transform.position - colliderB.transform.position;
third = new CSOVertex(-Support(colliderA, -searchDir), Support(colliderB, searchDir));
/*Find the second point of the simplex*/
searchDir = -third.vertCSO;
second = new CSOVertex(-Support(colliderA, -searchDir), Support(colliderB, searchDir));
/*If the new simplex point have negative dot value, then the CSO must NOT contain the origin*/
if (Vector3.Dot(second.vertCSO, searchDir) < 0)
{
return(false);
}
searchDir = Vector3.Cross(Vector3.Cross(third.vertCSO - second.vertCSO, -second.vertCSO), third.vertCSO - second.vertCSO);
if (searchDir == Vector3.zero)
{
searchDir = Vector3.Cross(third.vertCSO - second.vertCSO, Vector3.right); //normal with x-axis
if (searchDir == Vector3.zero)
{
searchDir = Vector3.Cross(third.vertCSO - second.vertCSO, Vector3.forward); //normal with z-axis
}
}
simplexCount = 2;
/*run through the iteration.*/
for (int i = 0; i < maxGJKLoop; i++)
{
first = new CSOVertex(-Support(colliderA, -searchDir), Support(colliderB, searchDir));
if (Vector3.Dot(first.vertCSO, searchDir) < 0)
{
return(false);
}
simplexCount++;
if (simplexCount == 3)
{
UpdateTriangle();
}
else if (TetrahedralContainOrigin())
{
return(true);
}
}
return(false);
}
private static void EPA(CustomCollider colliderA, CustomCollider colliderB)
{
CSOFace[] faces = new CSOFace[maxEPAFaces];
faces[0] = new CSOFace(first, second, third);
faces[1] = new CSOFace(first, third, last);
faces[2] = new CSOFace(first, last, second);
faces[3] = new CSOFace(second, last, third);
int numFaces = 4;
int closestFace = 0;
for (int iterations = 0; iterations < maxEPALoop; iterations++)
{
float min_dist = faces[0].distance;
closestFace = 0;
for (int i = 1; i < numFaces; i++)
{
float dist = faces[i].distance;
if (dist < min_dist)
{
min_dist = dist;
closestFace = i;
}
}
searchDir = faces[closestFace].normal;
CSOVertex newVert = new CSOVertex(-Support(colliderA, -searchDir), Support(colliderB, searchDir));
if (Vector3.Dot(newVert.vertCSO, searchDir) - min_dist < epaThreshold)
{
CalculateContactInfo(colliderA, colliderB, faces[closestFace]);
return;
}
CSOVertex[,] looseEdges = new CSOVertex[maxEPALooseEdges, 2];
int looseEdgeNum = 0;
for (int i = 0; i < numFaces; i++)
{
if (Vector3.Dot(faces[i].normal, newVert.vertCSO - faces[i].a.vertCSO) > 0)
{
for (int j = 0; j < 3; j++)
{
CSOVertex[] currentEdge = new CSOVertex[2];
if (j == 0)
{
currentEdge[0] = faces[i].a;
currentEdge[1] = faces[i].b;
}
else if (j == 1)
{
currentEdge[0] = faces[i].b;
currentEdge[1] = faces[i].c;
}
else
{
currentEdge[0] = faces[i].c;
currentEdge[1] = faces[i].a;
}
bool foundEdge = false;
for (int k = 0; k < looseEdgeNum; k++)
{
if (looseEdges[k, 1].vertCSO == currentEdge[0].vertCSO && looseEdges[k, 0].vertCSO == currentEdge[1].vertCSO)
{
looseEdges[k, 0] = looseEdges[looseEdgeNum - 1, 0];
looseEdges[k, 1] = looseEdges[looseEdgeNum - 1, 1];
looseEdgeNum--;
foundEdge = true;
k = looseEdgeNum;
}
}
if (!foundEdge)
{
if (looseEdgeNum >= maxEPALooseEdges)
{
break;
}
looseEdges[looseEdgeNum, 0] = currentEdge[0];
looseEdges[looseEdgeNum, 1] = currentEdge[1];
looseEdgeNum++;
}
}
faces[i] = faces[numFaces - 1];
numFaces--;
i--;
}
}
for (int i = 0; i < looseEdgeNum; i++)
{
if (numFaces >= maxEPAFaces)
{
break;
}
faces[numFaces] = new CSOFace(looseEdges[i, 0], looseEdges[i, 1], newVert);
float bias = 0.0001f;
if (faces[numFaces].distance + bias < 0) //Check the counter-clockwiseness of the vertices
{
faces[numFaces] = new CSOFace(looseEdges[i, 1], looseEdges[i, 0], newVert);
}
numFaces++;
}
if (iterations == maxEPALoop - 1)
{
CalculateContactInfo(colliderA, colliderB, faces[closestFace]);
return;
}
}
}
