///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges)
///experimental/work-in-progress
public virtual bool initializePolyhedralFeatures( bool shiftVerticesByMargin = false )
{
if( m_polyhedron != null )
{
m_polyhedron = null;
}
m_polyhedron = new btConvexPolyhedron();
int count = getNumVertices();
btList<btVector3> orgVertices = new btList<btVector3>( count );
btVector3[] arrVertices = orgVertices.InternalArray;
for( int i = 0; i < count; i++ )
{
getVertex( i, out arrVertices[i] );
}
btConvexHullComputer conv = new btConvexHullComputer();
if( shiftVerticesByMargin )
{
btList<btVector3> planeEquations = new btList<btVector3>();
btGeometryUtil.getPlaneEquationsFromVertices( orgVertices, planeEquations );
btList<btVector3> shiftedPlaneEquations = new btList<btVector3>();
for( int p = 0; p < planeEquations.Count; p++ )
{
btVector3 plane = planeEquations[p];
// double margin = getMargin();
plane[3] -= getMargin();
shiftedPlaneEquations.Add( plane );
}
btList<btVector3> tmpVertices = new btList<btVector3>();
btGeometryUtil.getVerticesFromPlaneEquations( shiftedPlaneEquations, tmpVertices );
conv.compute( tmpVertices, tmpVertices.Count, 0, 0 );
}
else
{
conv.compute( orgVertices, orgVertices.Count, 0, 0 );
}
btList<btVector3> faceNormals = new btList<btVector3>( conv.faces.Count );
int numFaces = conv.faces.Count;
btConvexHullComputer convexUtil = conv;
btVector3[] arr_faceNormals = faceNormals.InternalArray;
btList<btConvexPolyhedron.btFace> tmpFaces = new btList<btConvexPolyhedron.btFace>( numFaces );
int numVertices = convexUtil.vertices.Count;
m_polyhedron.m_vertices.Count = m_polyhedron.m_vertices.Capacity = ( numVertices );
for( int p = 0; p < numVertices; p++ )
{
m_polyhedron.m_vertices[p] = convexUtil.vertices[p];
}
for( int i = 0; i < numFaces; i++ )
{
btConvexHullComputer.Edge face = convexUtil.faces[i];
//Console.WriteLine("face=%d\n",face);
//btConvexHullComputer::Edge* firstEdge = &convexUtil.edges[face];
btConvexHullComputer.Edge edge = face;
btVector3[] edges = new btVector3[3];
int numEdges = 0;
//compute face normals
do
{
int src = edge.getSourceVertex();
tmpFaces[i].m_indices.Add( (short)src );
int targ = edge.getTargetVertex();
btVector3 wa = convexUtil.vertices[src];
btVector3 wb = convexUtil.vertices[targ];
btVector3 newEdge; wb.Sub( ref wa, out newEdge );
newEdge.normalize();
if( numEdges < 2 )
edges[numEdges++] = newEdge;
edge = edge.getNextEdgeOfFace();
} while( edge != face );
double planeEq = btScalar.BT_LARGE_FLOAT;
if( numEdges == 2 )
{
//faceNormals[i]
edges[0].cross( ref edges[1], out faceNormals.InternalArray[i] );
faceNormals[i].normalize();
tmpFaces[i].m_plane[0] = faceNormals[i].x;
tmpFaces[i].m_plane[1] = faceNormals[i].y;
tmpFaces[i].m_plane[2] = faceNormals[i].z;
tmpFaces[i].m_plane[3] = planeEq;
}
else
{
Debug.Assert( false );//degenerate?
faceNormals[i].setZero();
}
for( int v = 0; v < tmpFaces[i].m_indices.Count; v++ )
{
double eq = m_polyhedron.m_vertices[tmpFaces[i].m_indices[v]].dot( ref arr_faceNormals[i] );
if( planeEq > eq )
{
planeEq = eq;
}
}
tmpFaces[i].m_plane[3] = -planeEq;
}
//merge coplanar faces and copy them to m_polyhedron
double faceWeldThreshold = 0.999f;
btList<int> todoFaces = new btList<int>();
for( int i = 0; i < tmpFaces.Count; i++ )
todoFaces.Add( i );
btList<int> coplanarFaceGroup = new btList<int>();
while( todoFaces.Count > 0 )
{
int refFace = todoFaces[todoFaces.Count - 1];
coplanarFaceGroup.Add( refFace );
btConvexPolyhedron.btFace faceA = tmpFaces[refFace];
todoFaces.Count--;
btVector3 faceNormalA = new btVector3( faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2] );
for( int j = todoFaces.Count - 1; j >= 0; j-- )
{
int i = todoFaces[j];
btConvexPolyhedron.btFace faceB = tmpFaces[i];
btVector3 faceNormalB = new btVector3( faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2] );
if( faceNormalA.dot( ref faceNormalB ) > faceWeldThreshold )
{
coplanarFaceGroup.Add( i );
todoFaces.RemoveAt( i );
}
}
bool did_merge = false;
if( coplanarFaceGroup.Count > 1 )
{
//do the merge: use Graham Scan 2d convex hull
btList<GrahamVector3> orgpoints = new btList<GrahamVector3>();
btVector3 averageFaceNormal = btVector3.Zero;
for( int i = 0; i < coplanarFaceGroup.Count; i++ )
{
// m_polyhedron.m_faces.Add(tmpFaces[coplanarFaceGroup[i]]);
btConvexPolyhedron.btFace face = tmpFaces[coplanarFaceGroup[i]];
btVector3 faceNormal = new btVector3( face.m_plane[0], face.m_plane[1], face.m_plane[2] );
averageFaceNormal.Add( ref faceNormal, out averageFaceNormal );
for( int f = 0; f < face.m_indices.Count; f++ )
{
int orgIndex = face.m_indices[f];
btVector3 pt = m_polyhedron.m_vertices[orgIndex];
bool found = false;
for( int j = 0; j < orgpoints.Count; j++ )
{
//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
if( orgpoints[j].m_orgIndex == orgIndex )
{
found = true;
break;
}
}
if( !found )
orgpoints.Add( new GrahamVector3( ref pt, orgIndex ) );
}
}
btConvexPolyhedron.btFace combinedFace = new btConvexPolyhedron.btFace();
for( int i = 0; i < 4; i++ )
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
btList<GrahamVector3> hull = new btList<GrahamVector3>();
averageFaceNormal.normalize();
GrahamVector3.GrahamScanConvexHull2D( orgpoints, hull, ref averageFaceNormal );
for( int i = 0; i < hull.Count; i++ )
{
combinedFace.m_indices.Add( hull[i].m_orgIndex );
for( int k = 0; k < orgpoints.Count; k++ )
{
if( orgpoints[k].m_orgIndex == hull[i].m_orgIndex )
{
orgpoints[k].m_orgIndex = -1; // invalidate...
break;
}
}
}
// are there rejected vertices?
bool reject_merge = false;
for( int i = 0; i < orgpoints.Count; i++ )
{
if( orgpoints[i].m_orgIndex == -1 )
continue; // this is in the hull...
// this vertex is rejected -- is anybody else using this vertex?
for( int j = 0; j < tmpFaces.Count; j++ )
{
btConvexPolyhedron.btFace face = tmpFaces[j];
// is this a face of the current coplanar group?
bool is_in_current_group = false;
for( int k = 0; k < coplanarFaceGroup.Count; k++ )
{
if( coplanarFaceGroup[k] == j )
{
is_in_current_group = true;
break;
}
}
if( is_in_current_group ) // ignore this face...
continue;
// does this face use this rejected vertex?
for( int v = 0; v < face.m_indices.Count; v++ )
{
if( face.m_indices[v] == orgpoints[i].m_orgIndex )
{
// this rejected vertex is used in another face -- reject merge
reject_merge = true;
break;
}
}
if( reject_merge )
break;
}
if( reject_merge )
break;
}
if( !reject_merge )
{
// do this merge!
did_merge = true;
m_polyhedron.m_faces.Add( combinedFace );
}
}
if( !did_merge )
{
for( int i = 0; i < coplanarFaceGroup.Count; i++ )
{
btConvexPolyhedron.btFace face = tmpFaces[coplanarFaceGroup[i]];
m_polyhedron.m_faces.Add( face );
}
}
}
m_polyhedron.initialize();
return true;
}
public btPolyhedralConvexShape()
{
m_polyhedron = null;
}
