Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Remove co-planar faces from convex hull, using 2d Graham scan

Browse Source 
Improve SAT performance, by skipping back-facing features
Add assert in array class (probably fires in places)
This commit is contained in:
erwin.coumans
2011-05-20 12:29:24 +00:00
parent 20e95be9cd
commit b80e5fd167
10 changed files with 306 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -1260,7 +1260,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
btVector3 normalColor(1,1,0);
btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
//getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
}

3
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
View File

@@ -47,6 +47,8 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl
{
btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
if (numOverlappingPairs)
{
btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
for (int i=0;i<numOverlappingPairs;i++)
@@ -63,6 +65,7 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl
(colObj1)->getIslandTag());
}
}
}
}
}

3
src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
View File

@@ -17,7 +17,6 @@ subject to the following restrictions:
///This file was written by Erwin Coumans
///Separating axis rest based on work from Pierre Terdiman, see
///And contact clipping based on work from Simon Hobbs
#define TEST_INTERNAL_OBJECTS 1
#include "btConvexPolyhedron.h"
#include "LinearMath/btHashMap.h"
@@ -151,6 +150,7 @@ void btConvexPolyhedron::initialize()
}
}
#ifdef USE_CONNECTED_FACES
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
@@ -169,6 +169,7 @@ void btConvexPolyhedron::initialize()
m_faces[i].m_connectedFaces[j] = connectedFace;
}
}
#endif//USE_CONNECTED_FACES
for(int i=0;i<m_faces.size();i++)
{

4
src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
View File

@@ -23,11 +23,13 @@ subject to the following restrictions:
#include "LinearMath/btTransform.h"
#include "LinearMath/btAlignedObjectArray.h"
#define TEST_INTERNAL_OBJECTS 1
struct btFace
{
btAlignedObjectArray<int> m_indices;
btAlignedObjectArray<int> m_connectedFaces;
// btAlignedObjectArray<int> m_connectedFaces;
btScalar m_plane[4];
};

144
src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
View File

@@ -17,6 +17,9 @@ subject to the following restrictions:
#include "btConvexPolyhedron.h"
#include "LinearMath/btConvexHullComputer.h"
#include <new>
#include "LinearMath/btGeometryUtil.h"
#include "LinearMath/btGrahamScan2dConvexHull.h"
btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
m_polyhedron(0)
@@ -32,34 +35,53 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
}
}
bool btPolyhedralConvexShape::initializePolyhedralFeatures()
{
if (m_polyhedron)
btAlignedFree(m_polyhedron);
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
m_polyhedron = new (mem) btConvexPolyhedron;
btAlignedObjectArray<btVector3> tmpVertices;
btAlignedObjectArray<btVector3> orgVertices;
for (int i=0;i<getNumVertices();i++)
{
btVector3& newVertex = tmpVertices.expand();
btVector3& newVertex = orgVertices.expand();
getVertex(i,newVertex);
}
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);
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
faceNormals.resize(numFaces);
btConvexHullComputer* convexUtil = &conv;
m_polyhedron->m_faces.resize(numFaces);
btAlignedObjectArray<btFace> tmpFaces;
tmpFaces.resize(numFaces);
int numVertices = convexUtil->vertices.size();
m_polyhedron->m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
@@ -67,6 +89,7 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures()
m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
}
for (int i=0;i<numFaces;i++)
{
int face = convexUtil->faces[i];
@@ -85,7 +108,7 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures()
{
int src = edge->getSourceVertex();
m_polyhedron->m_faces[i].m_indices.push_back(src);
tmpFaces[i].m_indices.push_back(src);
int targ = edge->getTargetVertex();
btVector3 wa = convexUtil->vertices[src];
@@ -105,10 +128,10 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures()
{
faceNormals[i] = edges[0].cross(edges[1]);
faceNormals[i].normalize();
m_polyhedron->m_faces[i].m_plane[0] = faceNormals[i].getX();
m_polyhedron->m_faces[i].m_plane[1] = faceNormals[i].getY();
m_polyhedron->m_faces[i].m_plane[2] = faceNormals[i].getZ();
m_polyhedron->m_faces[i].m_plane[3] = planeEq;
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
@@ -117,22 +140,109 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures()
faceNormals[i].setZero();
}
for (int v=0;v<m_polyhedron->m_faces[i].m_indices.size();v++)
for (int v=0;v<tmpFaces[i].m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[m_polyhedron->m_faces[i].m_indices[v]].dot(faceNormals[i]);
btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
if (planeEq>eq)
{
planeEq=eq;
}
}
m_polyhedron->m_faces[i].m_plane[3] = -planeEq;
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);
}
}
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 ((rotatedPt-orgpoints[i]).length2()<0.001)
{
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);
}
m_polyhedron->m_faces.push_back(combinedFace);
} else
{
for (int i=0;i<coplanarFaceGroup.size();i++)
{
m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
}
}
}
m_polyhedron->initialize();
return true;

76
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
View File

@@ -75,7 +75,6 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
ds = de;
}
}
#include <stdio.h>
static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth)
@@ -106,7 +105,6 @@ inline bool IsAlmostZero(const btVector3& v)
return true;
}
#define TEST_INTERNAL_OBJECTS 1
#ifdef TEST_INTERNAL_OBJECTS
inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3])
@@ -170,11 +168,11 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
{
gActualSATPairTests++;
#ifdef TEST_INTERNAL_OBJECTS
//#ifdef TEST_INTERNAL_OBJECTS
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
#endif
//#endif
btScalar dmin = FLT_MAX;
int curPlaneTests=0;
@@ -185,6 +183,8 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
{
const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
const btVector3 faceANormalWS = transA.getBasis() * Normal;
if (DeltaC2.dot(faceANormalWS)<0)
continue;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
@@ -211,6 +211,8 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
{
const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
const btVector3 WorldNormal = transB.getBasis() * Normal;
if (DeltaC2.dot(WorldNormal)<0)
continue;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
@@ -249,6 +251,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
if(!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
continue;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
@@ -311,18 +316,29 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
int numVerticesA = polyA.m_indices.size();
for(int e0=0;e0<numVerticesA;e0++)
{
/*const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
const btVector3 edge0 = a - b;
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
*/
btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
btVector3 worldA1 = transA*a;
btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
//int otherFace=0;
#ifdef BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
#else
btVector3 planeNormalWS = planeNormalWS1;
btScalar planeEqWS=planeEqWS1;
#endif
//clip face
clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
@@ -380,19 +396,24 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
}
void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
btVector3 separatingNormal = separatingNormal1.normalized();
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
btScalar curMaxDist=maxDist;
int closestFaceB=-1;
btScalar dmax = -FLT_MAX;
{
btScalar dmax = -FLT_MAX;
for(int face=0;face<hullB.m_faces.size();face++)
{
const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
const btVector3 WorldNormal = transB.getBasis() * Normal;
btScalar d = WorldNormal.dot(separatingNormal);
if (d > dmax)
{
@@ -401,28 +422,19 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
}
}
}
btVertexArray worldVertsB1;
{
const btFace& polyB = hullB.m_faces[closestFaceB];
const int numVertices = polyB.m_indices.size();
for(int e0=0;e0<numVertices;e0++)
{
const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
worldVertsB1.push_back(transB*b);
}
}
if (closestFaceB<0)
{
return;
}
// setup initial clip face (minimizing face from hull B)
btVertexArray worldVertsB1;
{
const btFace& polyB = hullB.m_faces[closestFaceB];
const int numVertices = polyB.m_indices.size();
for(int e0=0;e0<numVertices;e0++)
{
const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
worldVertsB1.push_back(transB*b);
}
}
clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
if (closestFaceB>=0)
clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
}