From 1b305562be117c32bbcadd62c9b1201cbadad463 Mon Sep 17 00:00:00 2001
From: "erwin.coumans" <erwin.coumans@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Fri, 15 Apr 2011 18:37:28 +0000
Subject: [PATCH] Added Pierre Terdiman's 'internal object' optimization to
 improve performance for separating axis tests. 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)

---
 .../CollisionDispatch/btCollisionWorld.cpp    | 283 +++++++++---------
 .../btConvexConvexAlgorithm.cpp               |  63 +++-
 .../CollisionShapes/btConvexHullShape.cpp     |  44 +++
 .../CollisionShapes/btConvexHullShape.h       |   2 +
 .../CollisionShapes/btConvexPolyhedron.cpp    | 122 +++++++-
 .../CollisionShapes/btConvexPolyhedron.h      |  10 +-
 .../CollisionShapes/btConvexShape.cpp         |  17 ++
 .../CollisionShapes/btConvexShape.h           |   2 +
 .../btPolyhedralConvexShape.cpp               |  34 +--
 .../btPolyhedralContactClipping.cpp           |  94 +++++-
 src/LinearMath/btConvexHullComputer.cpp       |  22 +-
 src/LinearMath/btConvexHullComputer.h         |   4 +-
 12 files changed, 488 insertions(+), 209 deletions(-)

diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index 807b3d66b..dc8829dde 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -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);
 					}
 
 
+					
 				}
 			}
-		}
+			}
 	}
 }
 
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
index 79353a477..d575c427b 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
@@ -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)
diff --git a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
index 69bc67caf..d253562fc 100644
--- a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
@@ -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;
+	}
+
+
+}
+
 
diff --git a/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/src/BulletCollision/CollisionShapes/btConvexHullShape.h
index 1f7c89dea..cea379d29 100644
--- a/src/BulletCollision/CollisionShapes/btConvexHullShape.h
+++ b/src/BulletCollision/CollisionShapes/btConvexHullShape.h
@@ -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";}
diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
index 6fceb6f25..2404164eb 100644
--- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
+++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
@@ -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;
 	}
diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
index a64cdbfd7..d17a30535 100644
--- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
+++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
@@ -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;
 };
 
 	
diff --git a/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/src/BulletCollision/CollisionShapes/btConvexShape.cpp
index f5f3aa58a..8e7d2cb29 100644
--- a/src/BulletCollision/CollisionShapes/btConvexShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btConvexShape.cpp
@@ -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)
 {	
diff --git a/src/BulletCollision/CollisionShapes/btConvexShape.h b/src/BulletCollision/CollisionShapes/btConvexShape.h
index cab4bb69b..de4a991ee 100644
--- a/src/BulletCollision/CollisionShapes/btConvexShape.h
+++ b/src/BulletCollision/CollisionShapes/btConvexShape.h
@@ -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;
diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
index e84767eae..f9043f7d4 100644
--- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
@@ -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]);
-				}
-			}
-		}
-	}
+	
 
 	
 
diff --git a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
index 31fe7da87..070226f98 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
+++ b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
@@ -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
diff --git a/src/LinearMath/btConvexHullComputer.cpp b/src/LinearMath/btConvexHullComputer.cpp
index 10316b406..2d7c22975 100644
--- a/src/LinearMath/btConvexHullComputer.cpp
+++ b/src/LinearMath/btConvexHullComputer.cpp
@@ -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];
 	}
diff --git a/src/LinearMath/btConvexHullComputer.h b/src/LinearMath/btConvexHullComputer.h
index f1d263071..da96a494d 100644
--- a/src/LinearMath/btConvexHullComputer.h
+++ b/src/LinearMath/btConvexHullComputer.h
@@ -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();
 				}