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

281
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);
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
}
} 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,9 +471,6 @@ 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;
@@ -467,10 +478,46 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
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)

44
src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
View File

@@ -208,4 +208,48 @@ const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* seriali
return "btConvexHullShapeData";
}
void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, float& min, float& max) const
{
#if 1
min = FLT_MAX;
max = -FLT_MAX;
btVector3 witnesPtMin;
btVector3 witnesPtMax;
int numVerts = m_unscaledPoints.size();
for(int i=0;i<numVerts;i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
btVector3 pt = trans * vtx;
float dp = pt.dot(dir);
if(dp < min)
{
min = dp;
witnesPtMin = pt;
}
if(dp > max)
{
max = dp;
witnesPtMax=pt;
}
}
#else
btVector3 localAxis = dir*trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
min = vtx1.dot(dir);
max = vtx2.dot(dir);
#endif
if(min>max)
{
float tmp = min;
min = max;
max = tmp;
}
}

2
src/BulletCollision/CollisionShapes/btConvexHullShape.h
View File

@@ -73,6 +73,8 @@ public:
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
virtual void project(const btTransform& trans, const btVector3& dir, float& min, float& max) const;
//debugging
virtual const char* getName()const {return "Convex";}

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

@@ -17,7 +17,7 @@ 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"
@@ -72,11 +72,39 @@ struct btInternalEdge
//
#ifdef TEST_INTERNAL_OBJECTS
bool btConvexPolyhedron::testContainment() const
{
for(int p=0;p<8;p++)
{
btVector3 LocalPt;
if(p==0) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]);
else if(p==1) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]);
else if(p==2) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]);
else if(p==3) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]);
else if(p==4) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]);
else if(p==5) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]);
else if(p==6) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]);
else if(p==7) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]);
for(int i=0;i<m_faces.size();i++)
{
const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
if(d>0.0f)
return false;
}
}
return true;
}
#endif
void btConvexPolyhedron::initialize()
{
btHashMap<btInternalVertexPair,btInternalEdge> edges;
float TotalArea = 0.0f;
btScalar TotalArea = 0.0f;
m_localCenter.setValue(0, 0, 0);
for(int i=0;i<m_faces.size();i++)
@@ -153,7 +181,7 @@ void btConvexPolyhedron::initialize()
int k = (j+1)%numVertices;
const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
float Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
btVector3 Center = (p0+p1+p2)/3.0f;
m_localCenter += Area * Center;
TotalArea += Area;
@@ -161,10 +189,92 @@ void btConvexPolyhedron::initialize()
}
m_localCenter /= TotalArea;
#ifdef TEST_INTERNAL_OBJECTS
if(1)
{
m_radius = FLT_MAX;
for(int i=0;i<m_faces.size();i++)
{
const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
if(dist<m_radius)
m_radius = dist;
}
btScalar MinX = FLT_MAX;
btScalar MinY = FLT_MAX;
btScalar MinZ = FLT_MAX;
btScalar MaxX = -FLT_MAX;
btScalar MaxY = -FLT_MAX;
btScalar MaxZ = -FLT_MAX;
for(int i=0; i<m_vertices.size(); i++)
{
const btVector3& pt = m_vertices[i];
if(pt.x()<MinX) MinX = pt.x();
if(pt.x()>MaxX) MaxX = pt.x();
if(pt.y()<MinY) MinY = pt.y();
if(pt.y()>MaxY) MaxY = pt.y();
if(pt.z()<MinZ) MinZ = pt.z();
if(pt.z()>MaxZ) MaxZ = pt.z();
}
mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
// const btScalar r = m_radius / sqrtf(2.0f);
const btScalar r = m_radius / sqrtf(3.0f);
const int LargestExtent = mE.maxAxis();
const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
m_extents[0] = m_extents[1] = m_extents[2] = r;
m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
bool FoundBox = false;
for(int j=0;j<1024;j++)
{
if(testContainment())
{
FoundBox = true;
break;
}
m_extents[LargestExtent] -= Step;
}
if(!FoundBox)
{
m_extents[0] = m_extents[1] = m_extents[2] = r;
}
else
{
// Refine the box
const btScalar Step = (m_radius - r)/1024.0f;
const int e0 = (1<<LargestExtent) & 3;
const int e1 = (1<<e0) & 3;
for(int j=0;j<1024;j++)
{
const btScalar Saved0 = m_extents[e0];
const btScalar Saved1 = m_extents[e1];
m_extents[e0] += Step;
m_extents[e1] += Step;
if(!testContainment())
{
m_extents[e0] = Saved0;
m_extents[e1] = Saved1;
break;
}
}
}
}
#endif
}
void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, float& min, float& max) const
void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
{
min = FLT_MAX;
max = -FLT_MAX;
@@ -172,13 +282,13 @@ void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir,
for(int i=0;i<numVerts;i++)
{
btVector3 pt = trans * m_vertices[i];
float dp = pt.dot(dir);
btScalar dp = pt.dot(dir);
if(dp < min) min = dp;
if(dp > max) max = dp;
}
if(min>max)
{
float tmp = min;
btScalar tmp = min;
min = max;
max = tmp;
}

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

@@ -28,7 +28,7 @@ struct btFace
{
btAlignedObjectArray<int> m_indices;
btAlignedObjectArray<int> m_connectedFaces;
float m_plane[4];
btScalar m_plane[4];
};
@@ -41,11 +41,17 @@ class btConvexPolyhedron
btAlignedObjectArray<btVector3> m_vertices;
btAlignedObjectArray<btFace> m_faces;
btAlignedObjectArray<btVector3> m_uniqueEdges;
btVector3 m_localCenter;
btVector3 m_extents;
btScalar m_radius;
btVector3 mC;
btVector3 mE;
void initialize();
bool testContainment() const;
void project(const btTransform& trans, const btVector3& dir, float& min, float& max) const;
void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const;
};

17
src/BulletCollision/CollisionShapes/btConvexShape.cpp
View File

@@ -43,6 +43,23 @@ btConvexShape::~btConvexShape()
}
void btConvexShape::project(const btTransform& trans, const btVector3& dir, float& min, float& max) const
{
btVector3 localAxis = dir*trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
min = vtx1.dot(dir);
max = vtx2.dot(dir);
if(min>max)
{
float tmp = min;
min = max;
max = tmp;
}
}
static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling)
{

2
src/BulletCollision/CollisionShapes/btConvexShape.h
View File

@@ -52,6 +52,8 @@ public:
btScalar getMarginNonVirtual () const;
void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
virtual void project(const btTransform& trans, const btVector3& dir, float& min, float& max) const;
//notice that the vectors should be unit length
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;

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

@@ -105,9 +105,9 @@ 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[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;
}
@@ -125,35 +125,11 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures()
planeEq=eq;
}
}
m_polyhedron->m_faces[i].m_plane[3] = planeEq;
m_polyhedron->m_faces[i].m_plane[3] = -planeEq;
}
if (m_polyhedron->m_faces.size() && conv.vertices.size())
{
for (int f=0;f<m_polyhedron->m_faces.size();f++)
{
btVector3 planeNormal(m_polyhedron->m_faces[f].m_plane[0],m_polyhedron->m_faces[f].m_plane[1],m_polyhedron->m_faces[f].m_plane[2]);
btScalar planeEq = m_polyhedron->m_faces[f].m_plane[3];
btVector3 supVec = localGetSupportingVertex(-planeNormal);
if (supVec.dot(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.size();
for (int v=0;v<numVerts/2;v++)
{
btSwap(m_polyhedron->m_faces[f].m_indices[v],m_polyhedron->m_faces[f].m_indices[numVerts-1-v]);
}
}
}
}

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

@@ -24,6 +24,9 @@ subject to the following restrictions:
#include <float.h> //for FLT_MAX
int gExpectedNbTests=0;
int gActualNbTests = 0;
bool gUseInternalObject = true;
// Clips a face to the back of a plane
void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
@@ -75,19 +78,19 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
#include <stdio.h>
static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, float& depth)
static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth)
{
float Min0,Max0;
float Min1,Max1;
btScalar Min0,Max0;
btScalar Min1,Max1;
hullA.project(transA,sep_axis, Min0, Max0);
hullB.project(transB, sep_axis, Min1, Max1);
if(Max0<Min1 || Max1<Min0)
return false;
float d0 = Max0 - Min1;
btScalar d0 = Max0 - Min1;
assert(d0>=0.0f);
float d1 = Max1 - Min0;
btScalar d1 = Max1 - Min0;
assert(d1>=0.0f);
depth = d0<d1 ? d0:d1;
return true;
@@ -103,18 +106,77 @@ inline bool IsAlmostZero(const btVector3& v)
return true;
}
#define TEST_INTERNAL_OBJECTS 1
#ifdef TEST_INTERNAL_OBJECTS
__forceinline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3])
{
// This version is ~11.000 cycles (4%) faster overall in one of the tests.
// IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK);
// IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK);
// IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK);
p[0] = sv[0] < 0.0f ? -extents[0] : extents[0];
p[1] = sv[1] < 0.0f ? -extents[1] : extents[1];
p[2] = sv[2] < 0.0f ? -extents[2] : extents[2];
}
void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
{
const btMatrix3x3& rot = tr.getBasis();
const btVector3& r0 = rot[0];
const btVector3& r1 = rot[1];
const btVector3& r2 = rot[2];
const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();
out.setValue(x, y, z);
}
bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
{
const btScalar dp = delta_c.dot(axis);
btVector3 localAxis0;
InverseTransformPoint3x3(localAxis0, axis,trans0);
btVector3 localAxis1;
InverseTransformPoint3x3(localAxis1, axis,trans1);
btScalar p0[3];
BoxSupport(convex0.m_extents, localAxis0, p0);
btScalar p1[3];
BoxSupport(convex1.m_extents, localAxis1, p1);
const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z();
const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z();
const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;
const btScalar MinMaxRadius = MaxRadius + MinRadius;
const btScalar d0 = MinMaxRadius + dp;
const btScalar d1 = MinMaxRadius - dp;
const btScalar depth = d0<d1 ? d0:d1;
if(depth>dmin)
return false;
return true;
}
#endif //TEST_INTERNAL_OBJECTS
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep)
{
gActualSATPairTests++;
#ifdef TEST_INTERNAL_OBJECTS
const btVector3 c0 = transA * hullA.mLocalCenter;
const btVector3 c1 = transB * hullB.mLocalCenter;
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
#endif
float dmin = FLT_MAX;
btScalar dmin = FLT_MAX;
int curPlaneTests=0;
int numFacesA = hullA.m_faces.size();
@@ -127,12 +189,12 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, faceANormalWS, hullA, hullB, dmin))
if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
float d;
btScalar d;
if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d))
return false;
@@ -158,7 +220,7 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
gActualNbTests++;
#endif
float d;
btScalar d;
if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d))
return false;
@@ -195,7 +257,7 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
gActualNbTests++;
#endif
float dist;
btScalar dist;
if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist))
return false;
@@ -285,7 +347,13 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
{
btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS;
if (depth <=maxDist && depth >=minDist)
if (depth <=minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <=maxDist)
{
btVector3 point = pVtxIn->at(i);
#ifdef ONLY_REPORT_DEEPEST_POINT

22
src/LinearMath/btConvexHullComputer.cpp
View File

@@ -1974,17 +1974,21 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
medAxis = 3 - maxAxis - minAxis;
s /= btScalar(10216);
if (((medAxis + 1) % 3) != maxAxis)
{
s *= -1;
}
scaling = s;
if (s[0] > 0)
if (s[0] != 0)
{
s[0] = btScalar(1) / s[0];
}
if (s[1] > 0)
if (s[1] != 0)
{
s[1] = btScalar(1) / s[1];
}
if (s[2] > 0)
if (s[2] != 0)
{
s[2] = btScalar(1) / s[2];
}
@@ -2065,9 +2069,7 @@ btVector3 btConvexHullInternal::toBtVector(const Point32& v)
btVector3 btConvexHullInternal::getBtNormal(Face* face)
{
btVector3 normal = toBtVector(face->dir0).cross(toBtVector(face->dir1));
normal /= ((medAxis + 1 == maxAxis) || (medAxis - 2 == maxAxis)) ? normal.length() : -normal.length();
return normal;
return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
}
btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
@@ -2203,15 +2205,15 @@ btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
{
btVector3 origShift = getBtNormal(face) * -amount;
if (scaling[0] > 0)
if (scaling[0] != 0)
{
origShift[0] /= scaling[0];
}
if (scaling[1] > 0)
if (scaling[1] != 0)
{
origShift[1] /= scaling[1];
}
if (scaling[2] > 0)
if (scaling[2] != 0)
{
origShift[2] /= scaling[2];
}

4
src/LinearMath/btConvexHullComputer.h
View File

@@ -48,12 +48,12 @@ class btConvexHullComputer
return targetVertex;
}
const Edge* getNextEdgeOfVertex() const // counter-clockwise list of all edges of a vertex
const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
{
return this + next;
}
const Edge* getNextEdgeOfFace() const // clockwise list of all edges of a face
const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
{
return (this + reverse)->getNextEdgeOfVertex();
}