private static void CalculateContactInfo(CustomCollider a, CustomCollider b, CSOFace face)
{
currentContact.contactNormal = face.normal;
currentContact.penetrationDepth = face.distance;
Vector3 closestPoint = face.normal * face.distance;
/* Calculate the barycentric coordinate of the closest point. We're using the Cramer's rule to solve coordinates. */
/* The method of calulating coordinates is based on this website :
* https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
*/
Vector3 v0 = face.b.vertCSO - face.a.vertCSO, v1 = face.c.vertCSO - face.a.vertCSO, v2 = closestPoint - face.a.vertCSO;
float d00 = Vector3.Dot(v0, v0);
float d01 = Vector3.Dot(v0, v1);
float d11 = Vector3.Dot(v1, v1);
float d20 = Vector3.Dot(v2, v0);
float d21 = Vector3.Dot(v2, v1);
float denom = d00 * d11 - d01 * d01;
float v = (d11 * d20 - d01 * d21) / denom;
float w = (d00 * d21 - d01 * d20) / denom;
float u = 1.0f - v - w;
currentContact.globalContactA = u * -face.a.vertA + v * -face.b.vertA + w * -face.c.vertA;
currentContact.localContactA = a.gameObject.transform.worldToLocalMatrix.MultiplyPoint3x4(currentContact.globalContactA);
currentContact.globalContactB = u * face.a.vertB + v * face.b.vertB + w * face.c.vertB;
currentContact.localContactB = b.gameObject.transform.worldToLocalMatrix.MultiplyPoint3x4(currentContact.globalContactB);
/* The method of calulating orthonormal basis is based on this website :
* http://allenchou.net/2013/12/game-physics-contact-generation-epa/
*/
currentContact.contactTangent1 = currentContact.contactNormal.x >= 0.55735f ?
new Vector3(currentContact.contactNormal.y, -currentContact.contactNormal.x, 0).normalized:
new Vector3(0, currentContact.contactNormal.z, -currentContact.contactNormal.y).normalized;
currentContact.contactTangent2 = Vector3.Cross(currentContact.contactNormal, currentContact.contactTangent1);
return;
}
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;
}
}
}
