public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray<IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ObjectArray<IndexedVector3> worldVertsB2 = new ObjectArray<IndexedVector3>();
ObjectArray<IndexedVector3> pVtxIn = worldVertsB1;
ObjectArray<IndexedVector3> pVtxOut = worldVertsB2;
pVtxOut.Capacity = pVtxIn.Count;
int closestFaceA = -1;
{
float dmin = float.MaxValue;
for (int face = 0; face < hullA.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
IndexedVector3 faceANormalWS = transA._basis * Normal;
float d = IndexedVector3.Dot(faceANormalWS, separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
}
}
}
if (closestFaceA < 0)
return;
Face polyA = hullA.m_faces[closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
int numContacts = pVtxIn.Count;
int numVerticesA = polyA.m_indices.Count;
for (int e0 = 0; e0 < numVerticesA; e0++)
{
IndexedVector3 a = hullA.m_vertices[polyA.m_indices[e0]];
IndexedVector3 b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
IndexedVector3 edge0 = a - b;
IndexedVector3 WorldEdge0 = transA._basis * edge0;
IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0);
IndexedVector3 worldA1 = transA * a;
float planeEqWS1 = -worldA1.Dot(planeNormalWS1);
//int otherFace=0;
#if BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
#else
IndexedVector3 planeNormalWS = planeNormalWS1;
float planeEqWS=planeEqWS1;
#endif //clip face
ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS);
//btSwap(pVtxIn,pVtxOut);
ObjectArray<IndexedVector3> temp = pVtxIn;
pVtxIn = pVtxOut;
pVtxOut = temp;
pVtxOut.Clear();
}
//#define ONLY_REPORT_DEEPEST_POINT
IndexedVector3 point;
// only keep points that are behind the witness face
{
IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
float localPlaneEq = polyA.m_plane[3];
IndexedVector3 planeNormalWS = transA._basis * localPlaneNormal;
float planeEqWS = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin);
for (int i = 0; i < pVtxIn.Count; i++)
{
float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
if (depth <= minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <= maxDist && depth >= minDist)
{
IndexedVector3 point2 = pVtxIn[i];
#if ONLY_REPORT_DEEPEST_POINT
curMaxDist = depth;
#else
#if false
if (depth<-3)
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.AddContactPoint(ref separatingNormal, ref point2, depth);
#endif
}
}
}
#if ONLY_REPORT_DEEPEST_POINT
if (curMaxDist<maxDist)
{
resultOut.AddContactPoint(ref separatingNormal,ref point,curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
}