public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
IndexedVector3 separatingNormal = separatingNormal1.Normalized();
IndexedVector3 c0 = transA * hullA.m_localCenter;
IndexedVector3 c1 = transB * hullB.m_localCenter;
IndexedVector3 DeltaC2 = c0 - c1;
float curMaxDist = maxDist;
int closestFaceB = -1;
float dmax = float.MinValue;
{
for (int face = 0; face < hullB.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
IndexedVector3 WorldNormal = transB._basis * Normal;
float d = IndexedVector3.Dot(WorldNormal, separatingNormal);
if (d > dmax)
{
dmax = d;
closestFaceB = face;
}
}
}
// setup initial clip face (minimizing face from hull B)
ObjectArray<IndexedVector3> worldVertsB1 = new ObjectArray<IndexedVector3>();
{
Face polyB = hullB.m_faces[closestFaceB];
int numVertices = polyB.m_indices.Count;
for (int e0 = 0; e0 < numVertices; e0++)
{
IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]];
// check this to see if it is transposed version
worldVertsB1.Add(transB * b);
}
}
if (closestFaceB >= 0)
{
ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
}
}
