improvements in separating axis test / polyhedral clipping support.

improved debug rendering for polyhedra
allow to dynamically switch between gjk and sat test to compute separating axis (independent from the polyhedral clipping)

src/BulletCollision/BroadphaseCollision/btDispatcher.h

@@ -42,7 +42,7 @@ struct btDispatcherInfo
 		m_timeOfImpact(btScalar(1.)),
 		m_useContinuous(false),
 		m_debugDraw(0),
-		m_enableSatConvex(false),
+		m_enableSatConvex(true),
 		m_enableSPU(true),
 		m_useEpa(true),
 		m_allowedCcdPenetration(btScalar(0.04)),

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -33,6 +33,7 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btStackAlloc.h"
 #include "LinearMath/btSerializer.h"
+#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
 
 //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 
@@ -1339,6 +1340,36 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
 					btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
 
 					int i;
+					if (polyshape->getConvexPolyhedron())
+					{
+						const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
+						for (i=0;i<poly->m_faces.size();i++)
+						{
+							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);
+							
+							
+						}
+
+						
+					} else
+					{
 						for (i=0;i<polyshape->getNumEdges();i++)
 						{
 							btVector3 a,b;
@@ -1346,7 +1377,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
 							btVector3 wa = worldTransform * a;
 							btVector3 wb = worldTransform * b;
 							getDebugDrawer()->drawLine(wa,wb,color);
-
+						}
 					}
 
 

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -332,6 +332,9 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	}
 #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
 
+
+
+
 #ifdef USE_SEPDISTANCE_UTIL2
 	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
 	{
@@ -390,7 +393,6 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	}
 #endif //USE_SEPDISTANCE_UTIL2
 
-
 	if (min0->isPolyhedral() && min1->isPolyhedral())
 	{
 		btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
@@ -399,38 +401,40 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 		{
 			btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
 
+			btScalar minDist = 0.f;
 			btVector3 sepNormalWorldSpace;
-//#define USE_SAT_TEST
+			bool foundSepAxis  = true;
-#ifdef USE_SAT_TEST
+
-			bool foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
+			if (dispatchInfo.m_enableSatConvex)
+			{
+				foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
 					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
 					body0->getWorldTransform(), 
 					body1->getWorldTransform(),
 					sepNormalWorldSpace);
-#else
+			} else
-			bool foundSepAxis  = true;
+			{
 				sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
-#endif //USE_SAT_TEST
+				minDist = gjkPairDetector.getCachedSeparatingDistance();
+			}
 			if (foundSepAxis)
 			{
-				btScalar minDist = gjkPairDetector.getCachedSeparatingDistance();
+//				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
 
 				btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
 					body0->getWorldTransform(), 
 					body1->getWorldTransform(), minDist-threshold, threshold, *resultOut);
  				
+			}
 			if (m_ownManifold)
 			{
 				resultOut->refreshContactPoints();
 			}
 			return;
-			}
-
 
 		} else
 		{
 			//we can also deal with convex versus triangle (without connectivity data)
-
 			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
 			{
 
@@ -446,13 +450,17 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 				btScalar minDist = gjkPairDetector.getCachedSeparatingDistance();
 				btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
 					body0->getWorldTransform(), vertices, minDist-threshold, threshold, *resultOut);
+				
+				
+			}
 			if (m_ownManifold)
 			{
 				resultOut->refreshContactPoints();
 			}
+		}
+
 		return;
-			}
+
-		}
 	}
 
 	//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects

src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp

@@ -50,6 +50,8 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 	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);
@@ -89,25 +91,9 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 
 			btVector3 wb = convexUtil->vertices[targ];
 			btVector3 newEdge = wb-wa;
-			if (!newEdge.fuzzyZero())
-			{
 			newEdge.normalize();
-				if (!numEdges)
+			if (numEdges<2)
-				{
 				edges[numEdges++] = newEdge;
-				} else
-				{
-					btVector3 cr = (edges[0].cross(newEdge));
-					btScalar cr2 = cr.length2();
-					if (cr2 > maxCross2)
-					{
-						chosenEdge = m_polyhedron->m_faces[i].m_indices.size();
-						numEdges=1;//replace current edge
-						edges[numEdges++] = newEdge;
-						maxCross2=cr2;
-					}
-				}
-			}
 
 			edge = edge->getNextEdgeOfFace();
 		} while (edge!=firstEdge);
@@ -143,6 +129,34 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 	}
 
 
+	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]);
+				}
+			}
+		}
+	}
+
+	
+
 	m_polyhedron->initialize();
 
 	return true;

src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp

@@ -208,6 +208,11 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 		}
 
 	}
+
+	const btVector3 deltaC = transB.getOrigin() - transA.getOrigin();
+	if((deltaC.dot(sep))>0.0f)
+		sep = -sep;
+
 	return true;
 }
 
@@ -244,10 +249,12 @@ 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;
+		*/
+
 		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];