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