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