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
Demos/OpenGL/DemoApplication.cpp
View File

@@ -786,7 +786,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
btVector3 pickPos = rayCallback.m_hitPointWorld;
printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;

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

138
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;
btAlignedObjectArray<btFace> tmpFaces;
tmpFaces.resize(numFaces);
m_polyhedron->m_faces.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,21 +140,108 @@ 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();

74
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);
}

7
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -675,7 +675,12 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
if (m_manifolds.size() + m_constraints.size()>0)
{
m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
}
m_bodies.resize(0);
m_manifolds.resize(0);

9
src/LinearMath/btAlignedObjectArray.h
View File

@@ -140,21 +140,29 @@ class btAlignedObjectArray
SIMD_FORCE_INLINE const T& at(int n) const
{
btAssert(n>=0);
btAssert(n<size());
return m_data[n];
}
SIMD_FORCE_INLINE T& at(int n)
{
btAssert(n>=0);
btAssert(n<size());
return m_data[n];
}
SIMD_FORCE_INLINE const T& operator[](int n) const
{
btAssert(n>=0);
btAssert(n<size());
return m_data[n];
}
SIMD_FORCE_INLINE T& operator[](int n)
{
btAssert(n>=0);
btAssert(n<size());
return m_data[n];
}
@@ -171,6 +179,7 @@ class btAlignedObjectArray
SIMD_FORCE_INLINE void pop_back()
{
btAssert(m_size>0);
m_size--;
m_data[m_size].~T();
}

110
src/LinearMath/btGrahamScan2dConvexHull.h Normal file
View File

@@ -0,0 +1,110 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef GRAHAM_SCAN_2D_CONVEX_HULL_H
#define GRAHAM_SCAN_2D_CONVEX_HULL_H
#include "btVector3.h"
#include "btAlignedObjectArray.h"
struct GrahamVector2 : public btVector3
{
GrahamVector2(const btVector3& org, int orgIndex)
:btVector3(org),
m_orgIndex(orgIndex)
{
}
btScalar m_angle;
int m_orgIndex;
};
struct btAngleCompareFunc {
btVector3 m_anchor;
btAngleCompareFunc(const btVector3& anchor)
: m_anchor(anchor)
{
}
bool operator()(const GrahamVector2& a, const GrahamVector2& b) {
if (a.m_angle != b.m_angle)
return a.m_angle < b.m_angle;
else
{
btScalar al = (a-m_anchor).length2();
btScalar bl = (b-m_anchor).length2();
if (al != bl)
return al < bl;
else
{
return a.uid < b.uid;
}
}
}
};
inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector2>& originalPoints, btAlignedObjectArray<btVector3>& hull)
{
if (originalPoints.size()<=1)
{
for (int i=0;i<originalPoints.size();i++)
hull.push_back(originalPoints[0]);
return;
}
//step1 : find anchor point with smallest x/y and move it to first location
//also precompute angles
for (int i=0;i<originalPoints.size();i++)
{
const btVector3& left = originalPoints[i];
const btVector3& right = originalPoints[0];
if (left.x() < right.x() || !(right.x() < left.x()) && left.y() < right.y())
{
originalPoints.swap(0,i);
}
}
for (int i=0;i<originalPoints.size();i++)
{
btVector3 xvec(1,0,0);
btVector3 ar = originalPoints[i]-originalPoints[0];
originalPoints[i].m_angle = btCross(xvec, ar).dot(btVector3(0,0,1)) / ar.length();
}
//step 2: sort all points, based on 'angle' with this anchor
btAngleCompareFunc comp(originalPoints[0]);
originalPoints.quickSortInternal(comp,1,originalPoints.size()-1);
int i;
for (i = 0; i<2; i++)
hull.push_back(originalPoints[i]);
//step 3: keep all 'convex' points and discard concave points (using back tracking)
for (; i != originalPoints.size(); i++)
{
bool isConvex = false;
while (!isConvex&& hull.size()>1) {
btVector3& a = hull[hull.size()-2];
btVector3& b = hull[hull.size()-1];
isConvex = btCross(a-b,a-originalPoints[i]).dot(btVector3(0,0,1))> 0;
if (!isConvex)
hull.pop_back();
else
hull.push_back(originalPoints[i]);
}
}
}
#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H