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);
}
}
public static bool TestSepAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 sep_axis, out float depth)
{
float Min0, Max0;
float Min1, Max1;
hullA.Project(ref transA, ref sep_axis, out Min0, out Max0);
hullB.Project(ref transB, ref sep_axis, out Min1, out Max1);
if (Max0 < Min1 || Max1 < Min0)
{
depth = 0;
return(false);
}
float d0 = Max0 - Min1;
Debug.Assert(d0 >= 0.0f);
float d1 = Max1 - Min0;
Debug.Assert(d1 >= 0.0f);
depth = d0 < d1 ? d0 : d1;
return(true);
}
///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges)
public virtual bool InitializePolyhedralFeatures()
{
#if USE_CONVEX_HULL_COMPUTER
if (m_polyhedron != null)
{
m_polyhedron = null;
}
m_polyhedron = new ConvexPolyhedron();
ObjectArray <IndexedVector3> tmpVertices = new ObjectArray <IndexedVector3>();
for (int i = 0; i < GetNumVertices(); i++)
{
IndexedVector3 newVertex;
GetVertex(i, out newVertex);
tmpVertices.Add(newVertex);
}
ConvexHullComputer conv = new ConvexHullComputer();
//conv.compute(&tmpVertices[0].getX(), sizeof(IndexedVector3),tmpVertices.Count,0.0f,0.0f);
conv.Compute(tmpVertices, 0, tmpVertices.Count, 0.0f, 0.0f);
ObjectArray <IndexedVector3> faceNormals = new ObjectArray <IndexedVector3>();
int numFaces = conv.faces.size();
faceNormals.Resize(numFaces);
ConvexHullComputer convexUtil = conv;
m_polyhedron.m_faces.Resize(numFaces);
int numVertices = convexUtil.vertices.Count;
m_polyhedron.m_vertices.Resize(numVertices);
for (int p = 0; p < numVertices; p++)
{
m_polyhedron.m_vertices[p] = convexUtil.vertices[p];
}
for (int i = 0; i < numFaces; i++)
{
int face = convexUtil.faces[i];
//printf("face=%d\n",face);
Edge firstEdge = convexUtil.edges[face];
Edge edge = firstEdge;
IndexedVector3[] edges = new IndexedVector3[3];
int numEdges = 0;
//compute face normals
float maxCross2 = 0.0f;
int chosenEdge = -1;
do
{
int src = edge.GetSourceVertex();
m_polyhedron.m_faces[i].m_indices.Add(src);
int targ = edge.GetTargetVertex();
IndexedVector3 wa = convexUtil.vertices[src];
IndexedVector3 wb = convexUtil.vertices[targ];
IndexedVector3 newEdge = wb - wa;
newEdge.Normalize();
if (numEdges < 2)
{
edges[numEdges++] = newEdge;
}
edge = edge.GetNextEdgeOfFace();
} while (edge != firstEdge);
float planeEq = 1e30f;
if (numEdges == 2)
{
faceNormals[i] = IndexedVector3.Cross(edges[0], edges[1]);
faceNormals[i].Normalize();
m_polyhedron.m_faces[i].m_plane[0] = -faceNormals[i].X;
m_polyhedron.m_faces[i].m_plane[1] = -faceNormals[i].Y;
m_polyhedron.m_faces[i].m_plane[2] = -faceNormals[i].Z;
m_polyhedron.m_faces[i].m_plane[3] = planeEq;
}
else
{
Debug.Assert(false);//degenerate?
faceNormals[i] = IndexedVector3.Zero;
}
for (int v = 0; v < m_polyhedron.m_faces[i].m_indices.Count; v++)
{
float eq = IndexedVector3.Dot(m_polyhedron.m_vertices[m_polyhedron.m_faces[i].m_indices[v]], faceNormals[i]);
if (planeEq > eq)
{
planeEq = eq;
}
}
m_polyhedron.m_faces[i].m_plane[3] = planeEq;
}
if (m_polyhedron.m_faces.Count > 0 && conv.vertices.Count > 0)
{
for (int f = 0; f < m_polyhedron.m_faces.Count; f++)
{
IndexedVector3 planeNormal = new IndexedVector3(m_polyhedron.m_faces[f].m_plane[0], m_polyhedron.m_faces[f].m_plane[1], m_polyhedron.m_faces[f].m_plane[2]);
float planeEq = m_polyhedron.m_faces[f].m_plane[3];
IndexedVector3 supVec = LocalGetSupportingVertex(-planeNormal);
if (IndexedVector3.Dot(supVec, planeNormal) < planeEq)
{
m_polyhedron.m_faces[f].m_plane[0] *= -1;
m_polyhedron.m_faces[f].m_plane[1] *= -1;
m_polyhedron.m_faces[f].m_plane[2] *= -1;
m_polyhedron.m_faces[f].m_plane[3] *= -1;
int numVerts = m_polyhedron.m_faces[f].m_indices.Count;
for (int v = 0; v < numVerts / 2; v++)
{
int temp = m_polyhedron.m_faces[f].m_indices[v];
m_polyhedron.m_faces[f].m_indices[v] = m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v];
m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v] = temp;
}
}
}
}
m_polyhedron.Initialize();
#endif
return(true);
}
public static bool TestInternalObjects(ref IndexedMatrix trans0, ref IndexedMatrix trans1, ref IndexedVector3 delta_c, ref IndexedVector3 axis, ConvexPolyhedron convex0, ConvexPolyhedron convex1, float dmin)
{
float dp = delta_c.Dot(ref axis);
IndexedVector3 localAxis0;
InverseTransformPoint3x3(out localAxis0, ref axis, ref trans0);
IndexedVector3 localAxis1;
InverseTransformPoint3x3(out localAxis1, ref axis, ref trans1);
IndexedVector3 p0;
BoxSupport(ref convex0.m_extents, ref localAxis0, out p0);
IndexedVector3 p1;
BoxSupport(ref convex1.m_extents, ref localAxis1, out p1);
float Radius0 = p0.X * localAxis0.X + p0.Y * localAxis0.Y + p0.Z * localAxis0.Z;
float Radius1 = p1.X * localAxis1.X + p1.Y * localAxis1.Y + p1.Z * localAxis1.Z;
float MinRadius = Radius0 > convex0.m_radius ? Radius0 : convex0.m_radius;
float MaxRadius = Radius1 > convex1.m_radius ? Radius1 : convex1.m_radius;
float MinMaxRadius = MaxRadius + MinRadius;
float d0 = MinMaxRadius + dp;
float d1 = MinMaxRadius - dp;
float depth = d0 < d1 ? d0:d1;
if (depth > dmin)
{
return(false);
}
return(true);
}
public static bool FindSeparatingAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, out IndexedVector3 sep)
{
gActualSATPairTests++;
// dummy value to satisfy exit points.
sep = new IndexedVector3(0, 1, 0);
#if TEST_INTERNAL_OBJECTS
IndexedVector3 c0 = transA * hullA.m_localCenter;
IndexedVector3 c1 = transB * hullB.m_localCenter;
IndexedVector3 DeltaC2 = c0 - c1;
#endif
float dmin = float.MaxValue;
int curPlaneTests = 0;
int numFacesA = hullA.m_faces.Count;
// Test normals from hullA
for (int i = 0; i < numFacesA; i++)
{
IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
IndexedVector3 faceANormalWS = transA._basis * Normal;
curPlaneTests++;
#if TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if (gUseInternalObject && !TestInternalObjects(ref transA, ref transB, ref DeltaC2, ref faceANormalWS, hullA, hullB, dmin))
{
continue;
}
gActualNbTests++;
#endif
float d;
if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref faceANormalWS, out d))
{
return(false);
}
if (d < dmin)
{
dmin = d;
sep = faceANormalWS;
}
}
int numFacesB = hullB.m_faces.Count;
// Test normals from hullB
for (int i = 0; i < numFacesB; i++)
{
IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
IndexedVector3 WorldNormal = transB._basis * Normal;
curPlaneTests++;
#if TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if (gUseInternalObject && !TestInternalObjects(ref transA, ref transB, ref DeltaC2, ref WorldNormal, hullA, hullB, dmin))
{
continue;
}
gActualNbTests++;
#endif
float d;
if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref WorldNormal, out d))
{
return(false);
}
if (d < dmin)
{
dmin = d;
sep = WorldNormal;
}
}
IndexedVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
int curEdgeEdge = 0;
// Test edges
for (int e0 = 0; e0 < hullA.m_uniqueEdges.Count; e0++)
{
IndexedVector3 edge0 = hullA.m_uniqueEdges[e0];
IndexedVector3 WorldEdge0 = transA._basis * edge0;
for (int e1 = 0; e1 < hullB.m_uniqueEdges.Count; e1++)
{
IndexedVector3 edge1 = hullB.m_uniqueEdges[e1];
IndexedVector3 WorldEdge1 = transB._basis * edge1;
IndexedVector3 Cross = IndexedVector3.Cross(WorldEdge0, WorldEdge1);
curEdgeEdge++;
if (!MathUtil.IsAlmostZero(ref Cross))
{
Cross.Normalize();
#if TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if (gUseInternalObject && !TestInternalObjects(ref transA, ref transB, ref DeltaC2, ref Cross, hullA, hullB, dmin))
{
continue;
}
gActualNbTests++;
#endif
float dist;
if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref Cross, out dist))
{
return(false);
}
if (dist < dmin)
{
dmin = dist;
sep = Cross;
}
}
}
}
IndexedVector3 deltaC = transB._origin - transA._origin;
if ((IndexedVector3.Dot(deltaC, sep)) > 0.0f)
{
sep = -sep;
}
return(true);
}
public static bool FindSeparatingAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, out IndexedVector3 sep)
{
return(FindSeparatingAxis(hullA, hullB, ref transA, ref transB, out sep));
}
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
}
public static void ClipFaceAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, IndexedMatrix transA, ObjectArray <IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
}
public static void ClipHullAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ClipHullAgainstHull(ref separatingNormal, hullA, hullB, ref transA, ref transB, minDist, maxDist, resultOut);
}
