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