Thanks to Mårten Svanfeldt for the contribution:

- optionally disable collisions between bodies that are linked with constraints
- improved debug rendering with support for compounds, spheres, capsules

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -273,6 +273,8 @@ bool	btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO
 		
 	if ((!body0->isActive()) && (!body1->isActive()))
 		needsCollision = false;
+	else if (!body0->checkCollideWith(body1))
+		needsCollision = false;
 	
 	return needsCollision ;
 

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -24,7 +24,8 @@ btCollisionObject::btCollisionObject()
 		m_userObjectPointer(0),
 		m_hitFraction(btScalar(1.)),
 		m_ccdSweptSphereRadius(btScalar(0.)),
-		m_ccdSquareMotionThreshold(btScalar(0.))
+		m_ccdSquareMotionThreshold(btScalar(0.)),
+		m_checkCollideWith(false)
 {
 	
 }

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -77,7 +77,15 @@ protected:
 	/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
 	btScalar		m_ccdSquareMotionThreshold;
 	
-	char	m_pad[8];
+	/// If some object should have elaborate collision filtering by sub-classes
+	bool			m_checkCollideWith;
+
+	char	m_pad[7];
+
+	virtual bool	checkCollideWithOverride(btCollisionObject* co)
+	{
+		return true;
+	}
 
 public:
 
@@ -322,6 +330,15 @@ public:
 		m_userObjectPointer = userPointer;
 	}
 
+	inline bool checkCollideWith(btCollisionObject* co)
+	{
+		if (m_checkCollideWith)
+			return checkCollideWithOverride(co);
+
+		return true;
+	}
+
+
 }
 ;
 

src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

@@ -290,6 +290,8 @@ void	btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 		for(int x=startX; x<endX; x++)
 		{
 			btVector3 vertices[3];
+			if (!m_flipQuadEdges)
+			{
 				//first triangle
 				getVertex(x,j,vertices[0]);
 				getVertex(x,j+1,vertices[1]);
@@ -300,6 +302,20 @@ void	btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 				getVertex(x,j+1,vertices[1]);
 				getVertex(x+1,j+1,vertices[2]);
 				callback->processTriangle(vertices,x,j);
+			} else
+			{
+				//first triangle
+				getVertex(x,j,vertices[0]);
+				getVertex(x+1,j,vertices[1]);
+				getVertex(x+1,j+1,vertices[2]);
+				callback->processTriangle(vertices,x,j);
+				//second triangle
+				getVertex(x,j,vertices[0]);
+				getVertex(x+1,j+1,vertices[1]);
+				getVertex(x,j+1,vertices[2]);
+				callback->processTriangle(vertices,x,j);
+				
+			}
 		}
 	}
 

src/BulletCollision/CollisionShapes/btMultiSphereShape.h

@@ -46,6 +46,20 @@ public:
 
 	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
 	
+	int	getSphereCount() const
+	{
+		return m_numSpheres;
+	}
+
+	const btVector3&	getSpherePosition(int index) const
+	{
+		return m_localPositions[index];
+	}
+
+	btScalar	getSphereRadius(int index) const
+	{
+		return m_radi[index];
+	}
 
 	virtual int	getShapeType() const { return MULTI_SPHERE_SHAPE_PROXYTYPE; }
 

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -30,15 +30,17 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 
 //for debug rendering
-#include "BulletCollision/CollisionShapes/btCompoundShape.h"
-#include "BulletCollision/CollisionShapes/btSphereShape.h"
 #include "BulletCollision/CollisionShapes/btBoxShape.h"
-#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
 #include "BulletCollision/CollisionShapes/btConeShape.h"
-#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
-#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
 #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
 #include "LinearMath/btIDebugDraw.h"
 
 
@@ -370,14 +372,21 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 	END_PROFILE("updateActivationState");
 }
 
-void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint)
+void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
 {
 	m_constraints.push_back(constraint);
+	if (disableCollisionsBetweenLinkedBodies)
+	{
+		constraint->getRigidBodyA().addConstraintRef(constraint);
+		constraint->getRigidBodyB().addConstraintRef(constraint);
+	}
 }
 
 void	btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
 {
 	m_constraints.remove(constraint);
+	constraint->getRigidBodyA().removeConstraintRef(constraint);
+	constraint->getRigidBodyB().removeConstraintRef(constraint);
 }
 
 void	btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
@@ -726,10 +735,42 @@ public:
 	}
 };
 
+void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
+{
+	btVector3 start = transform.getOrigin();
 
+	const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
+	const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
+	const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
+
+	// XY 
+	getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
+	getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
+	getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
+	getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
+
+	// XZ
+	getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
+	getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
+	getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
+	getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
+
+	// YZ
+	getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
+	getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
+	getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
+	getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
+}
 
 void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
 {
+	// Draw a small simplex at the center of the object
+	{
+		btVector3 start = worldTransform.getOrigin();
+		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
+		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
+		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
+	}
 
 	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
 	{
@@ -750,14 +791,52 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 			{
 				const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
 				btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
-				btVector3 start = worldTransform.getOrigin();
+				
-				getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(radius,0,0),color);
+				debugDrawSphere(radius, worldTransform, color);
-				getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,radius,0),color);
-				getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,0,radius),color);
-				//drawSphere					
 				break;
 			}
 		case MULTI_SPHERE_SHAPE_PROXYTYPE:
+			{
+				const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
+
+				for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
+				{
+					btTransform childTransform = worldTransform;
+					childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
+					debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
+				}
+
+				break;
+			}
+		case CAPSULE_SHAPE_PROXYTYPE:
+			{
+				const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
+
+				btScalar radius = capsuleShape->getRadius();
+				btScalar halfHeight = capsuleShape->getHalfHeight();
+
+				// Draw the ends
+				{
+					btTransform childTransform = worldTransform;
+					childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
+					debugDrawSphere(radius, childTransform, color);
+				}
+
+				{
+					btTransform childTransform = worldTransform;
+					childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
+					debugDrawSphere(radius, childTransform, color);
+				}
+
+				// Draw some additional lines
+				btVector3 start = worldTransform.getOrigin();
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color);
+
+				break;
+			}
 		case CONE_SHAPE_PROXYTYPE:
 			{
 				const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
@@ -789,11 +868,9 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 		default:
 			{
 
-				if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+				if (shape->isConcave())
 				{
-					btTriangleMeshShape* concaveMesh = (btTriangleMeshShape*) shape;
+					btConcaveShape* concaveMesh = (btConcaveShape*) shape;
-					//btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-					//btVector3 aabbMax(100,100,100);//btScalar(1e30),btScalar(1e30),btScalar(1e30));
 					
 					//todo pass camera, for some culling
 					btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -80,6 +80,7 @@ protected:
 
 	void	saveKinematicState(btScalar timeStep);
 
+	void	debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
 
 public:
 
@@ -94,7 +95,7 @@ public:
 
 	virtual void	updateAabbs();
 
-	void	addConstraint(btTypedConstraint* constraint);
+	void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
 
 	void	removeConstraint(btTypedConstraint* constraint);
 

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -16,8 +16,9 @@ subject to the following restrictions:
 #include "btRigidBody.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "LinearMath/btMinMax.h"
-#include <LinearMath/btTransformUtil.h>
+#include "LinearMath/btTransformUtil.h"
-#include <LinearMath/btMotionState.h>
+#include "LinearMath/btMotionState.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 
 btScalar gLinearAirDamping = btScalar(1.);
 //'temporarily' global variables
@@ -305,4 +306,33 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 }
 
 
+bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
+{
+	btRigidBody* otherRb = btRigidBody::upcast(co);
+	if (!otherRb)
+		return true;
 
+	for (int i = 0; i < m_constraintRefs.size(); ++i)
+	{
+		btTypedConstraint* c = m_constraintRefs[i];
+		if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
+			return false;
+	}
+
+	return true;
+}
+
+void btRigidBody::addConstraintRef(btTypedConstraint* c)
+{
+	int index = m_constraintRefs.findLinearSearch(c);
+	if (index == m_constraintRefs.size())
+		m_constraintRefs.push_back(c); 
+
+	m_checkCollideWith = true;
+}
+
+void btRigidBody::removeConstraintRef(btTypedConstraint* c)
+{
+	m_constraintRefs.remove(c);
+	m_checkCollideWith = m_constraintRefs.size() > 0;
+}

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -16,6 +16,7 @@ subject to the following restrictions:
 #ifndef RIGIDBODY_H
 #define RIGIDBODY_H
 
+#include "../../LinearMath/btAlignedObjectArray.h"
 #include "../../LinearMath/btPoint3.h"
 #include "../../LinearMath/btTransform.h"
 #include "../../BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
@@ -23,7 +24,7 @@ subject to the following restrictions:
 
 class btCollisionShape;
 class btMotionState;
-
+class btTypedConstraint;
 
 
 extern btScalar gLinearAirDamping;
@@ -57,6 +58,9 @@ class btRigidBody  : public btCollisionObject
 	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
 	btMotionState*	m_optionalMotionState;
 
+	//keep track of typed constraints referencing this rigid body
+	btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
+
 public:
 
 #ifdef OBSOLETE_MOTIONSTATE_LESS
@@ -339,6 +343,11 @@ public:
 		return (getBroadphaseProxy() != 0);
 	}
 
+	virtual bool checkCollideWithOverride(btCollisionObject* co);
+
+	void addConstraintRef(btTypedConstraint* c);
+	void removeConstraintRef(btTypedConstraint* c);
+
 	int	m_debugBodyId;
 };