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

Added Pierre Terdiman's 'internal object' optimization to improve performance for separating axis tests.

Browse Source 
Make the winding consistent in btConvexHullComputer (and related fixes in btPolyhedralConvexShape), thanks to Ole!
Some fixes in the btPolyhedralContactClipping implementation (never report a penetration deeper than GJK/EPA found, to avoid issues due to its approximate contact normal directions)
Properly visualize btPolyhedralConvexHullShape that have a  btConvexPolyhedron (by calling initializePolyhedralFeatures() method)
This commit is contained in:
erwin.coumans
2011-04-15 18:37:28 +00:00
parent 7d37b3c472
commit 1b305562be
12 changed files with 488 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1231,159 +1231,164 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
} else
{
switch (shape->getShapeType())
/// for polyhedral shapes
if (shape->isPolyhedral())
{
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
case BOX_SHAPE_PROXYTYPE:
int i;
if (polyshape->getConvexPolyhedron())
{
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
break;
}
case SPHERE_SHAPE_PROXYTYPE:
{
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
getDebugDrawer()->drawSphere(radius, worldTransform, color);
break;
}
case MULTI_SPHERE_SHAPE_PROXYTYPE:
{
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
btTransform childTransform;
childTransform.setIdentity();
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
for (i=0;i<poly->m_faces.size();i++)
{
childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
}
break;
}
case CAPSULE_SHAPE_PROXYTYPE:
{
const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
btScalar radius = capsuleShape->getRadius();
btScalar halfHeight = capsuleShape->getHalfHeight();
int upAxis = capsuleShape->getUpAxis();
getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
break;
}
case CONE_SHAPE_PROXYTYPE:
{
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
btScalar height = coneShape->getHeight();//+coneShape->getMargin();
int upAxis= coneShape->getConeUpIndex();
getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
break;
}
case CYLINDER_SHAPE_PROXYTYPE:
{
const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
int upAxis = cylinder->getUpAxis();
btScalar radius = cylinder->getRadius();
btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
break;
}
case STATIC_PLANE_PROXYTYPE:
{
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
btScalar planeConst = staticPlaneShape->getPlaneConstant();
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
break;
}
default:
{
if (shape->isConcave())
{
btConcaveShape* concaveMesh = (btConcaveShape*) shape;
///@todo pass camera, for some culling? no -> we are not a graphics lib
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
}
if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{
btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
//todo: pass camera for some culling
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
//DebugDrawcallback drawCallback;
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
}
/// for polyhedral shapes
if (shape->isPolyhedral())
{
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
int i;
if (polyshape->getConvexPolyhedron())
btVector3 centroid(0,0,0);
int numVerts = poly->m_faces[i].m_indices.size();
if (numVerts)
{
const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
for (i=0;i<poly->m_faces.size();i++)
int lastV = poly->m_faces[i].m_indices[numVerts-1];
for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
{
btVector3 centroid(0,0,0);
int numVerts = poly->m_faces[i].m_indices.size();
if (numVerts)
{
int lastV = poly->m_faces[i].m_indices[numVerts-1];
for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
{
int curVert = poly->m_faces[i].m_indices[v];
centroid+=poly->m_vertices[curVert];
getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color);
lastV = curVert;
}
}
centroid*= 1./btScalar(numVerts);
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);
int curVert = poly->m_faces[i].m_indices[v];
centroid+=poly->m_vertices[curVert];
getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color);
lastV = curVert;
}
}
centroid*= btScalar(1.f)/btScalar(numVerts);
} else
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);
}
} else
{
for (i=0;i<polyshape->getNumEdges();i++)
{
btVector3 a,b;
polyshape->getEdge(i,a,b);
btVector3 wa = worldTransform * a;
btVector3 wb = worldTransform * b;
getDebugDrawer()->drawLine(wa,wb,color);
}
}
}
else
{
switch (shape->getShapeType())
{
case BOX_SHAPE_PROXYTYPE:
{
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
break;
}
case SPHERE_SHAPE_PROXYTYPE:
{
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
getDebugDrawer()->drawSphere(radius, worldTransform, color);
break;
}
case MULTI_SPHERE_SHAPE_PROXYTYPE:
{
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
btTransform childTransform;
childTransform.setIdentity();
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
{
for (i=0;i<polyshape->getNumEdges();i++)
{
btVector3 a,b;
polyshape->getEdge(i,a,b);
btVector3 wa = worldTransform * a;
btVector3 wb = worldTransform * b;
getDebugDrawer()->drawLine(wa,wb,color);
}
childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
}
break;
}
case CAPSULE_SHAPE_PROXYTYPE:
{
const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
btScalar radius = capsuleShape->getRadius();
btScalar halfHeight = capsuleShape->getHalfHeight();
int upAxis = capsuleShape->getUpAxis();
getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
break;
}
case CONE_SHAPE_PROXYTYPE:
{
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
btScalar height = coneShape->getHeight();//+coneShape->getMargin();
int upAxis= coneShape->getConeUpIndex();
getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
break;
}
case CYLINDER_SHAPE_PROXYTYPE:
{
const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
int upAxis = cylinder->getUpAxis();
btScalar radius = cylinder->getRadius();
btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
break;
}
case STATIC_PLANE_PROXYTYPE:
{
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
btScalar planeConst = staticPlaneShape->getPlaneConstant();
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
break;
}
default:
{
if (shape->isConcave())
{
btConcaveShape* concaveMesh = (btConcaveShape*) shape;
///@todo pass camera, for some culling? no -> we are not a graphics lib
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
}
if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{
btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
//todo: pass camera for some culling
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
//DebugDrawcallback drawCallback;
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
}
}
}
}
}
}
}

63
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -17,6 +17,7 @@ subject to the following restrictions:
///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
///with reproduction case
//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
//#define ZERO_MARGIN
#include "btConvexConvexAlgorithm.h"
@@ -416,12 +417,11 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
{
gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
btScalar minDist = 0.f;
btScalar minDist = -1e30f;
btVector3 sepNormalWorldSpace;
bool foundSepAxis = true;
@@ -434,8 +434,22 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
sepNormalWorldSpace);
} else
{
#ifdef ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
//gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif //ZERO_MARGIN
sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
minDist = gjkPairDetector.getCachedSeparatingDistance();
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
#ifdef ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(min0->getMargin()+min1->getMargin());
#endif
}
if (foundSepAxis)
{
@@ -457,20 +471,53 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
{
gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
btVertexArray vertices;
btTriangleShape* tri = (btTriangleShape*)polyhedronB;
vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[0]);
vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[1]);
vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[2]);
//tri->initializePolyhedralFeatures();
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
btScalar minDist = gjkPairDetector.getCachedSeparatingDistance();
btVector3 sepNormalWorldSpace;
btScalar minDist =-1e30f;
btScalar maxDist = threshold;
bool foundSepAxis = true;
if (0)
{
polyhedronB->initializePolyhedralFeatures();
foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0->getWorldTransform(),
body1->getWorldTransform(),
sepNormalWorldSpace);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
} else
{
#ifdef ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
}
if (foundSepAxis)
{
btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
body0->getWorldTransform(), vertices, minDist-threshold, threshold, *resultOut);
body0->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
}
if (m_ownManifold)