diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp')
-rw-r--r-- | extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp | 288 |
1 files changed, 285 insertions, 3 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index b1ecb3e432c..82def79cf55 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -14,10 +14,289 @@ subject to the following restrictions: */ #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" +#include "btConvexPolyhedron.h" +#include "LinearMath/btConvexHullComputer.h" +#include <new> +#include "LinearMath/btGeometryUtil.h" +#include "LinearMath/btGrahamScan2dConvexHull.h" -btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape() + +btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(), +m_polyhedron(0) +{ + +} + +btPolyhedralConvexShape::~btPolyhedralConvexShape() +{ + if (m_polyhedron) + { + btAlignedFree(m_polyhedron); + } +} + + +bool btPolyhedralConvexShape::initializePolyhedralFeatures() { + if (m_polyhedron) + btAlignedFree(m_polyhedron); + + void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16); + m_polyhedron = new (mem) btConvexPolyhedron; + + btAlignedObjectArray<btVector3> orgVertices; + + for (int i=0;i<getNumVertices();i++) + { + btVector3& newVertex = orgVertices.expand(); + getVertex(i,newVertex); + } + +#if 0 + btAlignedObjectArray<btVector3> planeEquations; + btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); + + btAlignedObjectArray<btVector3> shiftedPlaneEquations; + for (int p=0;p<planeEquations.size();p++) + { + btVector3 plane = planeEquations[p]; + plane[3] -= getMargin(); + shiftedPlaneEquations.push_back(plane); + } + + btAlignedObjectArray<btVector3> tmpVertices; + + btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); + btConvexHullComputer conv; + conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); + +#else + btConvexHullComputer conv; + conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); + +#endif + + + + btAlignedObjectArray<btVector3> faceNormals; + int numFaces = conv.faces.size(); + faceNormals.resize(numFaces); + btConvexHullComputer* convexUtil = &conv; + + + btAlignedObjectArray<btFace> tmpFaces; + tmpFaces.resize(numFaces); + + int numVertices = convexUtil->vertices.size(); + 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); + const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; + const btConvexHullComputer::Edge* edge = firstEdge; + + btVector3 edges[3]; + int numEdges = 0; + //compute face normals + + btScalar maxCross2 = 0.f; + int chosenEdge = -1; + + do + { + + int src = edge->getSourceVertex(); + tmpFaces[i].m_indices.push_back(src); + int targ = edge->getTargetVertex(); + btVector3 wa = convexUtil->vertices[src]; + + btVector3 wb = convexUtil->vertices[targ]; + btVector3 newEdge = wb-wa; + newEdge.normalize(); + if (numEdges<2) + edges[numEdges++] = newEdge; + + edge = edge->getNextEdgeOfFace(); + } while (edge!=firstEdge); + + btScalar planeEq = 1e30f; + + + if (numEdges==2) + { + faceNormals[i] = edges[0].cross(edges[1]); + faceNormals[i].normalize(); + tmpFaces[i].m_plane[0] = faceNormals[i].getX(); + tmpFaces[i].m_plane[1] = faceNormals[i].getY(); + tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); + tmpFaces[i].m_plane[3] = planeEq; + + } + else + { + btAssert(0);//degenerate? + faceNormals[i].setZero(); + } + + for (int v=0;v<tmpFaces[i].m_indices.size();v++) + { + btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]); + if (planeEq>eq) + { + planeEq=eq; + } + } + tmpFaces[i].m_plane[3] = -planeEq; + } + + //merge coplanar faces and copy them to m_polyhedron + + btScalar faceWeldThreshold= 0.999f; + btAlignedObjectArray<int> todoFaces; + for (int i=0;i<tmpFaces.size();i++) + todoFaces.push_back(i); + + while (todoFaces.size()) + { + btAlignedObjectArray<int> coplanarFaceGroup; + int refFace = todoFaces[todoFaces.size()-1]; + + coplanarFaceGroup.push_back(refFace); + btFace& faceA = tmpFaces[refFace]; + todoFaces.pop_back(); + + btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); + for (int j=todoFaces.size()-1;j>=0;j--) + { + int i = todoFaces[j]; + btFace& faceB = tmpFaces[i]; + btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); + if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) + { + coplanarFaceGroup.push_back(i); + todoFaces.remove(i); + } + } + + + bool did_merge = false; + if (coplanarFaceGroup.size()>1) + { + //do the merge: use Graham Scan 2d convex hull + + btAlignedObjectArray<GrahamVector2> orgpoints; + + for (int i=0;i<coplanarFaceGroup.size();i++) + { +// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + + btFace& face = tmpFaces[coplanarFaceGroup[i]]; + btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]); + btVector3 xyPlaneNormal(0,0,1); + + btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal); + + for (int f=0;f<face.m_indices.size();f++) + { + int orgIndex = face.m_indices[f]; + btVector3 pt = m_polyhedron->m_vertices[orgIndex]; + btVector3 rotatedPt = quatRotate(rotationArc,pt); + rotatedPt.setZ(0); + bool found = false; + + for (int i=0;i<orgpoints.size();i++) + { + //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001)) + if (orgpoints[i].m_orgIndex == orgIndex) + { + found=true; + break; + } + } + if (!found) + orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex)); + } + } + + btFace combinedFace; + for (int i=0;i<4;i++) + combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i]; + + btAlignedObjectArray<GrahamVector2> hull; + GrahamScanConvexHull2D(orgpoints,hull); + + for (int i=0;i<hull.size();i++) + { + combinedFace.m_indices.push_back(hull[i].m_orgIndex); + for(int k = 0; k < orgpoints.size(); 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.size(); 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.size(); j++) { + 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.size(); 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.size(); 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.push_back(combinedFace); + } + } + if(!did_merge) + { + for (int i=0;i<coplanarFaceGroup.size();i++) + { + m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + } + + } + + + + } + + m_polyhedron->initialize(); + + return true; } @@ -183,11 +462,14 @@ void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]+m_collisionMargin; + m_localAabbMax[i] = tmp[i]; vec[i] = btScalar(-1.); tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]-m_collisionMargin; + m_localAabbMin[i] = tmp[i]; } #endif } + + + |