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