initial commit

bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

@@ -0,0 +1,200 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btScalar.h"
+#include "SphereTriangleDetector.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+
+SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle)
+:m_sphere(sphere),
+m_triangle(triangle)
+{
+
+}
+
+void	SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+{
+
+	(void)debugDraw;
+	const btTransform& transformA = input.m_transformA;
+	const btTransform& transformB = input.m_transformB;
+
+	btVector3 point,normal;
+	btScalar timeOfImpact = btScalar(1.);
+	btScalar depth = btScalar(0.);
+//	output.m_distance = btScalar(1e30);
+	//move sphere into triangle space
+	btTransform	sphereInTr = transformB.inverseTimes(transformA);
+
+	if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact))
+	{
+		output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
+	}
+
+}
+
+#define MAX_OVERLAP btScalar(0.)
+
+
+
+// See also geometrictools.com
+// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
+	btVector3 diff = p - from;
+	btVector3 v = to - from;
+	btScalar t = v.dot(diff);
+	
+	if (t > 0) {
+		btScalar dotVV = v.dot(v);
+		if (t < dotVV) {
+			t /= dotVV;
+			diff -= t*v;
+		} else {
+			t = 1;
+			diff -= v;
+		}
+	} else
+		t = 0;
+
+	nearest = from + t*v;
+	return diff.dot(diff);	
+}
+
+bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
+	btVector3 lp(p);
+	btVector3 lnormal(normal);
+	
+	return pointInTriangle(vertices, lnormal, &lp);
+}
+
+///combined discrete/continuous sphere-triangle
+bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact)
+{
+
+	const btVector3* vertices = &m_triangle->getVertexPtr(0);
+	const btVector3& c = sphereCenter;
+	btScalar r = m_sphere->getRadius();
+
+	btVector3 delta (0,0,0);
+
+	btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
+	normal.normalize();
+	btVector3 p1ToCentre = c - vertices[0];
+	btScalar distanceFromPlane = p1ToCentre.dot(normal);
+
+	if (distanceFromPlane < btScalar(0.))
+	{
+		//triangle facing the other way
+	
+		distanceFromPlane *= btScalar(-1.);
+		normal *= btScalar(-1.);
+	}
+
+	///todo: move this gContactBreakingThreshold into a proper structure
+	extern btScalar gContactBreakingThreshold;
+
+	btScalar contactMargin = gContactBreakingThreshold;
+	bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
+	bool isInsideShellPlane = distanceFromPlane < r;
+	
+	btScalar deltaDotNormal = delta.dot(normal);
+	if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
+		return false;
+
+	// Check for contact / intersection
+	bool hasContact = false;
+	btVector3 contactPoint;
+	if (isInsideContactPlane) {
+		if (facecontains(c,vertices,normal)) {
+			// Inside the contact wedge - touches a point on the shell plane
+			hasContact = true;
+			contactPoint = c - normal*distanceFromPlane;
+		} else {
+			// Could be inside one of the contact capsules
+			btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
+			btVector3 nearestOnEdge;
+			for (int i = 0; i < m_triangle->getNumEdges(); i++) {
+				
+				btPoint3 pa;
+				btPoint3 pb;
+				
+				m_triangle->getEdge(i,pa,pb);
+
+				btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
+				if (distanceSqr < contactCapsuleRadiusSqr) {
+					// Yep, we're inside a capsule
+					hasContact = true;
+					contactPoint = nearestOnEdge;
+				}
+				
+			}
+		}
+	}
+
+	if (hasContact) {
+		btVector3 contactToCentre = c - contactPoint;
+		btScalar distanceSqr = contactToCentre.length2();
+		if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
+			btScalar distance = btSqrt(distanceSqr);
+			resultNormal = contactToCentre;
+			resultNormal.normalize();
+			point = contactPoint;
+			depth = -(r-distance);
+			return true;
+		}
+
+		if (delta.dot(contactToCentre) >= btScalar(0.0)) 
+			return false;
+		
+		// Moving towards the contact point -> collision
+		point = contactPoint;
+		timeOfImpact = btScalar(0.0);
+		return true;
+	}
+	
+	return false;
+}
+
+
+bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
+{
+	const btVector3* p1 = &vertices[0];
+	const btVector3* p2 = &vertices[1];
+	const btVector3* p3 = &vertices[2];
+
+	btVector3 edge1( *p2 - *p1 );
+	btVector3 edge2( *p3 - *p2 );
+	btVector3 edge3( *p1 - *p3 );
+
+	btVector3 p1_to_p( *p - *p1 );
+	btVector3 p2_to_p( *p - *p2 );
+	btVector3 p3_to_p( *p - *p3 );
+
+	btVector3 edge1_normal( edge1.cross(normal));
+	btVector3 edge2_normal( edge2.cross(normal));
+	btVector3 edge3_normal( edge3.cross(normal));
+	
+	btScalar r1, r2, r3;
+	r1 = edge1_normal.dot( p1_to_p );
+	r2 = edge2_normal.dot( p2_to_p );
+	r3 = edge3_normal.dot( p3_to_p );
+	if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
+	     ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
+		return true;
+	return false;
+
+}

bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h

@@ -0,0 +1,49 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_TRIANGLE_DETECTOR_H
+#define SPHERE_TRIANGLE_DETECTOR_H
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "LinearMath/btPoint3.h"
+
+
+class btSphereShape;
+class btTriangleShape;
+
+
+
+/// sphere-triangle to match the btDiscreteCollisionDetectorInterface
+struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface
+{
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);
+
+	SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle);
+
+	virtual ~SphereTriangleDetector() {};
+
+private:
+
+	bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact);
+	bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p );
+	bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal);
+
+	btSphereShape* m_sphere;
+	btTriangleShape* m_triangle;
+
+	
+};
+#endif //SPHERE_TRIANGLE_DETECTOR_H
+

bullet/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h

@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_CREATE_FUNC
+#define COLLISION_CREATE_FUNC
+
+#include "LinearMath/btAlignedObjectArray.h"
+typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
+class btCollisionAlgorithm;
+class btCollisionObject;
+
+struct btCollisionAlgorithmConstructionInfo;
+
+///Used by the btCollisionDispatcher to register and create instances for btCollisionAlgorithm
+struct btCollisionAlgorithmCreateFunc
+{
+	bool m_swapped;
+	
+	btCollisionAlgorithmCreateFunc()
+		:m_swapped(false)
+	{
+	}
+	virtual ~btCollisionAlgorithmCreateFunc(){};
+
+	virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , btCollisionObject* body0,btCollisionObject* body1)
+	{
+		
+		(void)body0;
+		(void)body1;
+		return 0;
+	}
+};
+#endif //COLLISION_CREATE_FUNC
+

bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -0,0 +1,367 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btCollisionDispatcher.h"
+
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+
+int gNumManifold = 0;
+
+#include <stdio.h>
+
+	
+btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms):
+m_count(0),
+m_useIslands(true),
+m_convexConvexCreateFunc(0),
+m_convexConcaveCreateFunc(0),
+m_swappedConvexConcaveCreateFunc(0),
+m_compoundCreateFunc(0),
+m_swappedCompoundCreateFunc(0),
+m_emptyCreateFunc(0)
+{
+	(void)noDefaultAlgorithms;
+	int i;
+
+	setNearCallback(defaultNearCallback);
+
+	m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc;
+	for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
+	{
+		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
+		{
+			m_doubleDispatch[i][j] = m_emptyCreateFunc;
+		}
+	}
+}
+//if you want to not link with the default collision algorithms, you can
+//define BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION 
+//in your Bullet library build system
+#ifndef BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION
+
+btCollisionDispatcher::btCollisionDispatcher (): 
+	m_count(0),
+	m_useIslands(true)
+{
+	int i;
+
+	setNearCallback(defaultNearCallback);
+	
+	//default CreationFunctions, filling the m_doubleDispatch table
+	m_convexConvexCreateFunc = new btConvexConvexAlgorithm::CreateFunc;
+	m_convexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::CreateFunc;
+	m_swappedConvexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
+	m_compoundCreateFunc = new btCompoundCollisionAlgorithm::CreateFunc;
+	m_swappedCompoundCreateFunc = new btCompoundCollisionAlgorithm::SwappedCreateFunc;
+	m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc;
+
+	for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
+	{
+		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
+		{
+			m_doubleDispatch[i][j] = internalFindCreateFunc(i,j);
+			assert(m_doubleDispatch[i][j]);
+		}
+	}
+	
+	
+};
+
+#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION
+
+
+void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
+{
+	m_doubleDispatch[proxyType0][proxyType1] = createFunc;
+}
+
+btCollisionDispatcher::~btCollisionDispatcher()
+{
+	delete m_convexConvexCreateFunc;
+	delete m_convexConcaveCreateFunc;
+	delete m_swappedConvexConcaveCreateFunc;
+	delete m_compoundCreateFunc;
+	delete m_swappedCompoundCreateFunc;
+	delete m_emptyCreateFunc;
+}
+
+btPersistentManifold*	btCollisionDispatcher::getNewManifold(void* b0,void* b1) 
+{ 
+	gNumManifold++;
+	
+	//btAssert(gNumManifold < 65535);
+	
+
+	btCollisionObject* body0 = (btCollisionObject*)b0;
+	btCollisionObject* body1 = (btCollisionObject*)b1;
+	
+	btPersistentManifold* manifold = new btPersistentManifold (body0,body1);
+	m_manifoldsPtr.push_back(manifold);
+
+	return manifold;
+}
+
+void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
+{
+	manifold->clearManifold();
+}
+
+	
+void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
+{
+	
+	gNumManifold--;
+
+	//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
+	clearManifold(manifold);
+
+	///todo: this can be improved a lot, linear search might be slow part!
+	int findIndex = m_manifoldsPtr.findLinearSearch(manifold);
+	if (findIndex < m_manifoldsPtr.size())
+	{
+		m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
+		m_manifoldsPtr.pop_back();
+		delete manifold;
+	}
+	
+}
+
+	
+
+btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
+{
+
+#ifdef USE_DISPATCH_REGISTRY_ARRAY
+	
+	btCollisionAlgorithmConstructionInfo ci;
+	ci.m_dispatcher = this;
+	ci.m_manifold = sharedManifold;
+	btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]
+	->CreateCollisionAlgorithm(ci,body0,body1);
+#else
+	btCollisionAlgorithm* algo = internalFindAlgorithm(body0,body1);
+#endif //USE_DISPATCH_REGISTRY_ARRAY
+	return algo;
+}
+
+
+#ifndef BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION
+
+btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(int proxyType0,int proxyType1)
+{
+	
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
+	{
+		return m_convexConvexCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
+	{
+		return m_convexConcaveCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
+	{
+		return m_swappedConvexConcaveCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isCompound(proxyType0))
+	{
+		return m_compoundCreateFunc;
+	} else
+	{
+		if (btBroadphaseProxy::isCompound(proxyType1))
+		{
+			return m_swappedCompoundCreateFunc;
+		}
+	}
+
+	//failed to find an algorithm
+	return m_emptyCreateFunc;
+}
+
+#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION
+
+
+#ifndef USE_DISPATCH_REGISTRY_ARRAY
+
+btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
+{
+	m_count++;
+	
+	btCollisionAlgorithmConstructionInfo ci;
+	ci.m_dispatcher = this;
+	
+	if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConvex() )
+	{
+		return new btConvexConvexAlgorithm(sharedManifold,ci,body0,body1);
+	}
+
+	if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConcave())
+	{
+		return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
+	}
+
+	if (body1->getCollisionShape()->isConvex() && body0->getCollisionShape()->isConcave())
+	{
+		return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
+	}
+
+	if (body0->getCollisionShape()->isCompound())
+	{
+		return new btCompoundCollisionAlgorithm(ci,body0,body1,false);
+	} else
+	{
+		if (body1->getCollisionShape()->isCompound())
+		{
+			return new btCompoundCollisionAlgorithm(ci,body0,body1,true);
+		}
+	}
+
+	//failed to find an algorithm
+	return new btEmptyAlgorithm(ci);
+	
+}
+#endif //USE_DISPATCH_REGISTRY_ARRAY
+
+bool	btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1)
+{
+	//here you can do filtering
+	bool hasResponse = 
+		(body0->hasContactResponse() && body1->hasContactResponse());
+	//no response between two static/kinematic bodies:
+	hasResponse = hasResponse &&
+		((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject()));
+	return hasResponse;
+}
+
+bool	btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
+{
+	assert(body0);
+	assert(body1);
+
+	bool needsCollision = true;
+
+	//broadphase filtering already deals with this
+	if ((body0->isStaticObject() || body0->isKinematicObject()) &&
+		(body1->isStaticObject() || body1->isKinematicObject()))
+	{
+		printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
+	}
+		
+	if ((!body0->isActive()) && (!body1->isActive()))
+		needsCollision = false;
+	else if (!body0->checkCollideWith(body1))
+		needsCollision = false;
+	
+	return needsCollision ;
+
+}
+
+
+
+///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
+///this is useful for the collision dispatcher.
+class btCollisionPairCallback : public btOverlapCallback
+{
+	btDispatcherInfo& m_dispatchInfo;
+	btCollisionDispatcher*	m_dispatcher;
+
+public:
+
+	btCollisionPairCallback(btDispatcherInfo& dispatchInfo,btCollisionDispatcher*	dispatcher)
+	:m_dispatchInfo(dispatchInfo),
+	m_dispatcher(dispatcher)
+	{
+	}
+
+	btCollisionPairCallback& operator=(btCollisionPairCallback& other)
+	{
+		m_dispatchInfo = other.m_dispatchInfo;
+		m_dispatcher = other.m_dispatcher;
+		return *this;
+	}
+
+	virtual ~btCollisionPairCallback() {}
+
+
+	virtual bool	processOverlap(btBroadphasePair& pair)
+	{
+		(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
+
+		return false;
+	}
+};
+
+
+void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)
+{
+	//m_blockedForChanges = true;
+
+	btCollisionPairCallback	collisionCallback(dispatchInfo,this);
+
+	pairCache->processAllOverlappingPairs(&collisionCallback);
+
+	//m_blockedForChanges = false;
+
+}
+
+
+
+
+//by default, Bullet will use this near callback
+void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, btDispatcherInfo& dispatchInfo)
+{
+		btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+		btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+		if (dispatcher.needsCollision(colObj0,colObj1))
+		{
+			//dispatcher will keep algorithms persistent in the collision pair
+			if (!collisionPair.m_algorithm)
+			{
+				collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1);
+			}
+
+			if (collisionPair.m_algorithm)
+			{
+				btManifoldResult contactPointResult(colObj0,colObj1);
+				
+				if (dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
+				{
+					//discrete collision detection query
+					collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult);
+				} else
+				{
+					//continuous collision detection query, time of impact (toi)
+					btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
+					if (dispatchInfo.m_timeOfImpact > toi)
+						dispatchInfo.m_timeOfImpact = toi;
+
+				}
+			}
+		}
+
+}

bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

@@ -0,0 +1,135 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION__DISPATCHER_H
+#define COLLISION__DISPATCHER_H
+
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class btIDebugDraw;
+class btOverlappingPairCache;
+
+
+#include "btCollisionCreateFunc.h"
+
+#define USE_DISPATCH_REGISTRY_ARRAY 1
+
+class btCollisionDispatcher;
+///user can override this nearcallback for collision filtering and more finegrained control over collision detection
+typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, btDispatcherInfo& dispatchInfo);
+
+
+///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
+///Time of Impact, Closest Points and Penetration Depth.
+class btCollisionDispatcher : public btDispatcher
+{
+	int m_count;
+	
+	btAlignedObjectArray<btPersistentManifold*>	m_manifoldsPtr;
+
+	bool m_useIslands;
+	
+	btManifoldResult	m_defaultManifoldResult;
+
+	btNearCallback		m_nearCallback;
+	
+	btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
+	
+	btCollisionAlgorithmCreateFunc* internalFindCreateFunc(int proxyType0,int proxyType1);
+
+	//default CreationFunctions, filling the m_doubleDispatch table
+	btCollisionAlgorithmCreateFunc*	m_convexConvexCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_convexConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_swappedConvexConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_compoundCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_swappedCompoundCreateFunc;
+	btCollisionAlgorithmCreateFunc*   m_emptyCreateFunc;
+
+#ifndef USE_DISPATCH_REGISTRY_ARRAY
+	btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0);
+#endif //USE_DISPATCH_REGISTRY_ARRAY
+
+public:
+
+	///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
+	void	registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
+
+	int	getNumManifolds() const
+	{ 
+		return int( m_manifoldsPtr.size());
+	}
+
+	btPersistentManifold**	getInternalManifoldPointer()
+	{
+		return &m_manifoldsPtr[0];
+	}
+
+	 btPersistentManifold* getManifoldByIndexInternal(int index)
+	{
+		return m_manifoldsPtr[index];
+	}
+
+	 const btPersistentManifold* getManifoldByIndexInternal(int index) const
+	{
+		return m_manifoldsPtr[index];
+	}
+
+	///the default constructor creates/register default collision algorithms, for convex, compound and concave shape support
+	btCollisionDispatcher ();
+
+	///a special constructor that doesn't create/register the default collision algorithms
+	btCollisionDispatcher(bool noDefaultAlgorithms);
+
+	virtual ~btCollisionDispatcher();
+
+	virtual btPersistentManifold*	getNewManifold(void* b0,void* b1);
+	
+	virtual void releaseManifold(btPersistentManifold* manifold);
+
+
+	virtual void clearManifold(btPersistentManifold* manifold);
+
+			
+	btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0);
+		
+	virtual bool	needsCollision(btCollisionObject* body0,btCollisionObject* body1);
+	
+	virtual bool	needsResponse(btCollisionObject* body0,btCollisionObject* body1);
+	
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo);
+
+	void	setNearCallback(btNearCallback	nearCallback)
+	{
+		m_nearCallback = nearCallback; 
+	}
+
+	btNearCallback	getNearCallback() const
+	{
+		return m_nearCallback;
+	}
+
+	//by default, Bullet will use this near callback
+	static void  defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, btDispatcherInfo& dispatchInfo);
+
+};
+
+#endif //COLLISION__DISPATCHER_H
+

bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -0,0 +1,61 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btCollisionObject.h"
+
+btCollisionObject::btCollisionObject()
+	:	m_broadphaseHandle(0),
+		m_collisionShape(0),
+		m_collisionFlags(0),
+		m_activationState1(1),
+		m_deactivationTime(btScalar(0.)),
+		m_userObjectPointer(0),
+		m_internalOwner(0),
+		m_hitFraction(btScalar(1.)),
+		m_ccdSweptSphereRadius(btScalar(0.)),
+		m_ccdSquareMotionThreshold(btScalar(0.)),
+		m_checkCollideWith(false),
+		m_islandTag1(-1),
+		m_companionId(-1)
+{
+	
+}
+
+btCollisionObject::~btCollisionObject()
+{
+}
+
+void btCollisionObject::setActivationState(int newState) 
+{ 
+	if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
+		m_activationState1 = newState;
+}
+
+void btCollisionObject::forceActivationState(int newState)
+{
+	m_activationState1 = newState;
+}
+
+void btCollisionObject::activate(bool forceActivation)
+{
+	if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT)))
+	{
+		setActivationState(ACTIVE_TAG);
+		m_deactivationTime = btScalar(0.);
+	}
+}
+
+
+

bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -0,0 +1,346 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_OBJECT_H
+#define COLLISION_OBJECT_H
+
+#include "LinearMath/btTransform.h"
+
+//island management, m_activationState1
+#define ACTIVE_TAG 1
+#define ISLAND_SLEEPING 2
+#define WANTS_DEACTIVATION 3
+#define DISABLE_DEACTIVATION 4
+#define DISABLE_SIMULATION 5
+
+struct	btBroadphaseProxy;
+class	btCollisionShape;
+#include "LinearMath/btMotionState.h"
+
+
+
+/// btCollisionObject can be used to manage collision detection objects. 
+/// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
+/// They can be added to the btCollisionWorld.
+ATTRIBUTE_ALIGNED16(class)	btCollisionObject
+{
+
+protected:
+
+	btTransform	m_worldTransform;
+
+	///m_interpolationWorldTransform is used for CCD and interpolation
+	///it can be either previous or future (predicted) transform
+	btTransform	m_interpolationWorldTransform;
+	//those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) 
+	//without destroying the continuous interpolated motion (which uses this interpolation velocities)
+	btVector3	m_interpolationLinearVelocity;
+	btVector3	m_interpolationAngularVelocity;
+	btBroadphaseProxy*		m_broadphaseHandle;
+	btCollisionShape*		m_collisionShape;
+
+	int				m_collisionFlags;
+
+	int				m_islandTag1;
+	int				m_companionId;
+
+	int				m_activationState1;
+	btScalar			m_deactivationTime;
+
+	btScalar		m_friction;
+	btScalar		m_restitution;
+
+	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
+	void*			m_userObjectPointer;
+
+	///m_internalOwner is reserved to point to Bullet's btRigidBody. Don't use this, use m_userObjectPointer instead.
+	void*			m_internalOwner;
+
+	///time of impact calculation
+	btScalar		m_hitFraction; 
+	
+	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	btScalar		m_ccdSweptSphereRadius;
+
+	/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
+	btScalar		m_ccdSquareMotionThreshold;
+	
+	/// 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:
+
+	enum CollisionFlags
+	{
+		CF_STATIC_OBJECT= 1,
+		CF_KINEMATIC_OBJECT= 2,
+		CF_NO_CONTACT_RESPONSE = 4,
+		CF_CUSTOM_MATERIAL_CALLBACK = 8//this allows per-triangle material (friction/restitution)
+	};
+
+
+	inline bool mergesSimulationIslands() const
+	{
+		///static objects, kinematic and object without contact response don't merge islands
+		return  ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
+	}
+
+
+	inline bool		isStaticObject() const {
+		return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
+	}
+
+	inline bool		isKinematicObject() const
+	{
+		return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
+	}
+
+	inline bool		isStaticOrKinematicObject() const
+	{
+		return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
+	}
+
+	inline bool		hasContactResponse() const {
+		return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0;
+	}
+
+	
+	btCollisionObject();
+
+	virtual ~btCollisionObject();
+
+	void	setCollisionShape(btCollisionShape* collisionShape)
+	{
+		m_collisionShape = collisionShape;
+	}
+
+	const btCollisionShape*	getCollisionShape() const
+	{
+		return m_collisionShape;
+	}
+
+	btCollisionShape*	getCollisionShape()
+	{
+		return m_collisionShape;
+	}
+
+	
+
+
+	int	getActivationState() const { return m_activationState1;}
+	
+	void setActivationState(int newState);
+
+	void	setDeactivationTime(btScalar time)
+	{
+		m_deactivationTime = time;
+	}
+	btScalar	getDeactivationTime() const
+	{
+		return m_deactivationTime;
+	}
+
+	void forceActivationState(int newState);
+
+	void	activate(bool forceActivation = false);
+
+	inline bool isActive() const
+	{
+		return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
+	}
+
+	void	setRestitution(btScalar rest)
+	{
+		m_restitution = rest;
+	}
+	btScalar	getRestitution() const
+	{
+		return m_restitution;
+	}
+	void	setFriction(btScalar frict)
+	{
+		m_friction = frict;
+	}
+	btScalar	getFriction() const
+	{
+		return m_friction;
+	}
+
+	///reserved for Bullet internal usage
+	void*	getInternalOwner()
+	{
+		return m_internalOwner;
+	}
+
+	const void*	getInternalOwner() const
+	{
+		return m_internalOwner;
+	}
+
+	btTransform&	getWorldTransform()
+	{
+		return m_worldTransform;
+	}
+
+	const btTransform&	getWorldTransform() const
+	{
+		return m_worldTransform;
+	}
+
+	void	setWorldTransform(const btTransform& worldTrans)
+	{
+		m_worldTransform = worldTrans;
+	}
+
+
+	btBroadphaseProxy*	getBroadphaseHandle()
+	{
+		return m_broadphaseHandle;
+	}
+
+	const btBroadphaseProxy*	getBroadphaseHandle() const
+	{
+		return m_broadphaseHandle;
+	}
+
+	void	setBroadphaseHandle(btBroadphaseProxy* handle)
+	{
+		m_broadphaseHandle = handle;
+	}
+
+
+	const btTransform&	getInterpolationWorldTransform() const
+	{
+		return m_interpolationWorldTransform;
+	}
+
+	btTransform&	getInterpolationWorldTransform()
+	{
+		return m_interpolationWorldTransform;
+	}
+
+	void	setInterpolationWorldTransform(const btTransform&	trans)
+	{
+		m_interpolationWorldTransform = trans;
+	}
+
+
+	const btVector3&	getInterpolationLinearVelocity() const
+	{
+		return m_interpolationLinearVelocity;
+	}
+
+	const btVector3&	getInterpolationAngularVelocity() const
+	{
+		return m_interpolationAngularVelocity;
+	}
+
+	const int getIslandTag() const
+	{
+		return	m_islandTag1;
+	}
+
+	void	setIslandTag(int tag)
+	{
+		m_islandTag1 = tag;
+	}
+
+	const int getCompanionId() const
+	{
+		return	m_companionId;
+	}
+
+	void	setCompanionId(int id)
+	{
+		m_companionId = id;
+	}
+
+	const btScalar			getHitFraction() const
+	{
+		return m_hitFraction; 
+	}
+
+	void	setHitFraction(btScalar hitFraction)
+	{
+		m_hitFraction = hitFraction;
+	}
+
+	
+	const int	getCollisionFlags() const
+	{
+		return m_collisionFlags;
+	}
+
+	void	setCollisionFlags(int flags)
+	{
+		m_collisionFlags = flags;
+	}
+	
+	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	btScalar			getCcdSweptSphereRadius() const
+	{
+		return m_ccdSweptSphereRadius;
+	}
+
+	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	void	setCcdSweptSphereRadius(btScalar radius)
+	{
+		m_ccdSweptSphereRadius = radius;
+	}
+
+	btScalar 	getCcdSquareMotionThreshold() const
+	{
+		return m_ccdSquareMotionThreshold;
+	}
+
+
+	/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
+	void	setCcdSquareMotionThreshold(btScalar ccdSquareMotionThreshold)
+	{
+		m_ccdSquareMotionThreshold = ccdSquareMotionThreshold;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void*	getUserPointer() const
+	{
+		return m_userObjectPointer;
+	}
+	
+	///users can point to their objects, userPointer is not used by Bullet
+	void	setUserPointer(void* userPointer)
+	{
+		m_userObjectPointer = userPointer;
+	}
+
+	inline bool checkCollideWith(btCollisionObject* co)
+	{
+		if (m_checkCollideWith)
+			return checkCollideWithOverride(co);
+
+		return true;
+	}
+
+
+}
+;
+
+#endif //COLLISION_OBJECT_H

bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -0,0 +1,367 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btCollisionWorld.h"
+#include "btCollisionDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" //for raycasting
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btStackAlloc.h"
+
+//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+
+
+
+btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, int stackSize)
+:m_dispatcher1(dispatcher),
+m_broadphasePairCache(pairCache),
+m_ownsDispatcher(false),
+m_ownsBroadphasePairCache(false)
+{
+	m_stackAlloc = new btStackAlloc(stackSize);
+	m_dispatchInfo.m_stackAllocator = m_stackAlloc;
+}
+
+
+btCollisionWorld::~btCollisionWorld()
+{
+	m_stackAlloc->destroy();
+	delete m_stackAlloc;
+
+	//clean up remaining objects
+	int i;
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* collisionObject= m_collisionObjects[i];
+		
+		btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
+		if (bp)
+		{
+			//
+			// only clear the cached algorithms
+			//
+			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp);
+			getBroadphase()->destroyProxy(bp);
+		}
+	}
+
+	if (m_ownsDispatcher)
+		delete m_dispatcher1;
+	if (m_ownsBroadphasePairCache)
+		delete m_broadphasePairCache;
+
+}
+
+
+
+
+
+
+
+
+
+
+void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+{
+
+	//check that the object isn't already added
+		btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
+
+		m_collisionObjects.push_back(collisionObject);
+
+		//calculate new AABB
+		btTransform trans = collisionObject->getWorldTransform();
+
+		btVector3	minAabb;
+		btVector3	maxAabb;
+		collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
+
+		int type = collisionObject->getCollisionShape()->getShapeType();
+		collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
+			minAabb,
+			maxAabb,
+			type,
+			collisionObject,
+			collisionFilterGroup,
+			collisionFilterMask
+			))	;
+
+		
+
+
+
+}
+
+
+
+
+void	btCollisionWorld::performDiscreteCollisionDetection()
+{
+	btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
+	BEGIN_PROFILE("perform Broadphase Collision Detection");
+
+
+	//update aabb (of all moved objects)
+
+	btVector3 aabbMin,aabbMax;
+	for (int i=0;i<m_collisionObjects.size();i++)
+	{
+		m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax);
+		m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax);
+	}
+
+	m_broadphasePairCache->calculateOverlappingPairs();
+	
+	END_PROFILE("perform Broadphase Collision Detection");
+
+	BEGIN_PROFILE("performDiscreteCollisionDetection");
+
+	btDispatcher* dispatcher = getDispatcher();
+	if (dispatcher)
+		dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo);
+
+	END_PROFILE("performDiscreteCollisionDetection");
+
+}
+
+
+void	btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+	
+	
+	//bool removeFromBroadphase = false;
+	
+	{
+		
+		btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
+		if (bp)
+		{
+			//
+			// only clear the cached algorithms
+			//
+			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp);
+			getBroadphase()->destroyProxy(bp);
+			collisionObject->setBroadphaseHandle(0);
+		}
+	}
+
+
+	//swapremove
+	m_collisionObjects.remove(collisionObject);
+
+}
+
+
+
+void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback,short int collisionFilterMask)
+{
+	
+	btSphereShape pointShape(btScalar(0.0));
+	pointShape.setMargin(0.f);
+
+	objectQuerySingle(&pointShape,rayFromTrans,rayToTrans,
+					  collisionObject,
+					  collisionShape,
+					  colObjWorldTransform,
+					  resultCallback,collisionFilterMask);
+}
+
+void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback,short int collisionFilterMask)
+{
+	
+
+	if (collisionShape->isConvex())
+			{
+				btConvexCast::CastResult castResult;
+				castResult.m_fraction = btScalar(1.);//??
+
+				btConvexShape* convexShape = (btConvexShape*) collisionShape;
+				btVoronoiSimplexSolver	simplexSolver;
+				btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
+				//btGjkConvexCast	convexCaster(castShape,convexShape,&simplexSolver);
+				//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
+				
+				if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
+				{
+					//add hit
+					if (castResult.m_normal.length2() > btScalar(0.0001))
+					{
+						castResult.m_normal.normalize();
+						if (castResult.m_fraction < resultCallback.m_closestHitFraction)
+						{
+
+							btCollisionWorld::LocalRayResult localRayResult
+								(
+									collisionObject, 
+									0,
+									castResult.m_normal,
+									castResult.m_fraction
+								);
+
+							bool normalInWorldSpace = true;
+							resultCallback.AddSingleResult(localRayResult, normalInWorldSpace);
+
+						}
+					}
+				}
+			}
+			else
+			{
+				
+				if (collisionShape->isConcave())
+					{
+
+						btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape;
+						
+						btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+
+						btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+						btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+						//ConvexCast::CastResult
+
+						struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback 
+						{
+							btCollisionWorld::RayResultCallback* m_resultCallback;
+							btCollisionObject*	m_collisionObject;
+							btTriangleMeshShape*	m_triangleMesh;
+
+							BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
+								btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh):
+								btTriangleRaycastCallback(from,to),
+									m_resultCallback(resultCallback),
+									m_collisionObject(collisionObject),
+									m_triangleMesh(triangleMesh)
+								{
+								}
+
+
+							virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
+							{
+								btCollisionWorld::LocalShapeInfo	shapeInfo;
+								shapeInfo.m_shapePart = partId;
+								shapeInfo.m_triangleIndex = triangleIndex;
+								
+								btCollisionWorld::LocalRayResult rayResult
+								(m_collisionObject, 
+									&shapeInfo,
+									hitNormalLocal,
+									hitFraction);
+								
+								bool	normalInWorldSpace = false;
+								return m_resultCallback->AddSingleResult(rayResult,normalInWorldSpace);
+								
+								
+							}
+	
+						};
+
+
+						BridgeTriangleRaycastCallback	rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
+						rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+
+						btVector3 rayAabbMinLocal = rayFromLocal;
+						rayAabbMinLocal.setMin(rayToLocal);
+						btVector3 rayAabbMaxLocal = rayFromLocal;
+						rayAabbMaxLocal.setMax(rayToLocal);
+
+						triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
+											
+					} else
+					{
+						//todo: use AABB tree or other BVH acceleration structure!
+						if (collisionShape->isCompound())
+						{
+							const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+							int i=0;
+							for (i=0;i<compoundShape->getNumChildShapes();i++)
+							{
+								btTransform childTrans = compoundShape->getChildTransform(i);
+								const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
+								btTransform childWorldTrans = colObjWorldTransform * childTrans;
+								objectQuerySingle(castShape, rayFromTrans,rayToTrans,
+									collisionObject,
+									childCollisionShape,
+									childWorldTrans,
+									resultCallback, collisionFilterMask);
+
+							}
+
+
+						}
+					}
+			}
+}
+
+void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback,short int collisionFilterMask)
+{
+
+	
+	btTransform	rayFromTrans,rayToTrans;
+	rayFromTrans.setIdentity();
+	rayFromTrans.setOrigin(rayFromWorld);
+	rayToTrans.setIdentity();
+	
+	rayToTrans.setOrigin(rayToWorld);
+
+	/// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
+	
+	int i;
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject*	collisionObject= m_collisionObjects[i];
+		//only perform raycast if filterMask matches
+		if(collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) { 
+			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+
+			btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
+			btVector3 hitNormal;
+			if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
+			{
+				rayTestSingle(rayFromTrans,rayToTrans,
+					collisionObject,
+						collisionObject->getCollisionShape(),
+						collisionObject->getWorldTransform(),
+						resultCallback);
+			}	
+		}
+	}
+
+}

bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -0,0 +1,262 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+/**
+ * @mainpage Bullet Documentation
+ *
+ * @section intro_sec Introduction
+ * Bullet Collision Detection & Physics SDK
+ *
+ * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
+ *
+ * There is the Physics Forum for Feedback and bteral Collision Detection and Physics discussions.
+ * Please visit http://www.continuousphysics.com/Bullet/phpBB2/index.php
+ *
+ * @section install_sec Installation
+ *
+ * @subsection step1 Step 1: Download
+ * You can download the Bullet Physics Library from our website: http://www.continuousphysics.com/Bullet/
+ * @subsection step2 Step 2: Building
+ * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8.
+ * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version).
+ * 
+ * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using cmake, http://www.cmake.org, or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet.
+ * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files.
+ * So if you are not using MSVC, you can run configure and jam .
+ * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/pub/jam/
+ * 
+ * @subsection step3 Step 3: Testing demos
+ * Try to run and experiment with CcdPhysicsDemo executable as a starting point.
+ * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector Library or Low Level / Snippets like the GJK Closest Point calculation.
+ * The Dependencies can be seen in this documentation under Directories
+ * 
+ * @subsection step4 Step 4: Integrating in your application, Full Rigid Body Simulation
+ * Check out CcdPhysicsDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform.
+ * PLEASE NOTE THE CcdPhysicsEnvironment and CcdPhysicsController is obsolete and will be removed. It has been replaced by classes derived frmo btDynamicsWorld and btRididBody
+ * @subsection step5 Step 5 : Integrate the Collision Detection Library (without Dynamics and other Extras)
+ * Bullet Collision Detection can also be used without the Dynamics/Extras.
+ * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo. Also in Extras/test_BulletOde.cpp there is a sample Collision Detection integration with Open Dynamics Engine, ODE, http://www.ode.org
+ * @subsection step6 Step 6 : Use Snippets like the GJK Closest Point calculation.
+ * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector.
+ *
+ * @section copyright Copyright
+ * Copyright (C) 2005-2007 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon
+ * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky,
+ * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren, Marten Svanfeldt.
+ * 
+ */
+ 
+ 
+
+#ifndef COLLISION_WORLD_H
+#define COLLISION_WORLD_H
+
+class btStackAlloc;
+class btCollisionShape;
+class btConvexShape;
+class btBroadphaseInterface;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "btCollisionObject.h"
+#include "btCollisionDispatcher.h" //for definition of btCollisionObjectArray
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+///CollisionWorld is interface and container for the collision detection
+class btCollisionWorld
+{
+
+	
+protected:
+
+	btAlignedObjectArray<btCollisionObject*>	m_collisionObjects;
+	
+	btDispatcher*	m_dispatcher1;
+
+	btDispatcherInfo	m_dispatchInfo;
+
+	btStackAlloc*	m_stackAlloc;
+
+	btBroadphaseInterface*	m_broadphasePairCache;
+	
+	bool	m_ownsDispatcher;
+	bool	m_ownsBroadphasePairCache;
+
+public:
+
+	//this constructor doesn't own the dispatcher and paircache/broadphase
+	btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, int stackSize = 2*1024*1024);
+
+	virtual ~btCollisionWorld();
+
+
+	btBroadphaseInterface*	getBroadphase()
+	{
+		return m_broadphasePairCache;
+	}
+
+	btOverlappingPairCache*	getPairCache()
+	{
+		return m_broadphasePairCache->getOverlappingPairCache();
+	}
+
+
+	btDispatcher*	getDispatcher()
+	{
+		return m_dispatcher1;
+	}
+
+	///LocalShapeInfo gives extra information for complex shapes
+	///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart
+	struct	LocalShapeInfo
+	{
+		int	m_shapePart;
+		int	m_triangleIndex;
+		
+		//const btCollisionShape*	m_shapeTemp;
+		//const btTransform*	m_shapeLocalTransform;
+	};
+
+	struct	LocalRayResult
+	{
+		LocalRayResult(btCollisionObject*	collisionObject, 
+			LocalShapeInfo*	localShapeInfo,
+			const btVector3&		hitNormalLocal,
+			btScalar hitFraction)
+		:m_collisionObject(collisionObject),
+		m_localShapeInfo(localShapeInfo),
+		m_hitNormalLocal(hitNormalLocal),
+		m_hitFraction(hitFraction)
+		{
+		}
+
+		btCollisionObject*		m_collisionObject;
+		LocalShapeInfo*			m_localShapeInfo;
+		btVector3				m_hitNormalLocal;
+		btScalar				m_hitFraction;
+
+	};
+
+	///RayResultCallback is used to report new raycast results
+	struct	RayResultCallback
+	{
+		virtual ~RayResultCallback()
+		{
+		}
+		btScalar	m_closestHitFraction;
+		bool	HasHit()
+		{
+			return (m_closestHitFraction < btScalar(1.));
+		}
+
+		RayResultCallback()
+			:m_closestHitFraction(btScalar(1.))
+		{
+		}
+		virtual	btScalar	AddSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0;
+	};
+
+	struct	ClosestRayResultCallback : public RayResultCallback
+	{
+		ClosestRayResultCallback(const btVector3&	rayFromWorld,const btVector3&	rayToWorld)
+		:m_rayFromWorld(rayFromWorld),
+		m_rayToWorld(rayToWorld),
+		m_collisionObject(0)
+		{
+		}
+
+		btVector3	m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
+		btVector3	m_rayToWorld;
+
+		btVector3	m_hitNormalWorld;
+		btVector3	m_hitPointWorld;
+		btCollisionObject*	m_collisionObject;
+		
+		virtual	btScalar	AddSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
+		{
+
+//caller already does the filter on the m_closestHitFraction
+			assert(rayResult.m_hitFraction <= m_closestHitFraction);
+			
+			m_closestHitFraction = rayResult.m_hitFraction;
+			m_collisionObject = rayResult.m_collisionObject;
+			if (normalInWorldSpace)
+			{
+				m_hitNormalWorld = rayResult.m_hitNormalLocal;
+			} else
+			{
+				///need to transform normal into worldspace
+				m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
+			}
+			m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+			return rayResult.m_hitFraction;
+		}
+	};
+
+
+	
+
+	int	getNumCollisionObjects() const
+	{
+		return int(m_collisionObjects.size());
+	}
+
+	/// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
+	/// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
+	void	rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback, short int collisionFilterMask=-1);
+
+	/// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
+	/// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
+	/// This allows more customization.
+	static void	rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback, short int collisionFilterMask=-1);
+
+	/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
+	static void	objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback, short int collisionFilterMask=-1);
+
+	void	addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=1,short int collisionFilterMask=1);
+
+	btCollisionObjectArray& getCollisionObjectArray()
+	{
+		return m_collisionObjects;
+	}
+
+	const btCollisionObjectArray& getCollisionObjectArray() const
+	{
+		return m_collisionObjects;
+	}
+
+
+	void	removeCollisionObject(btCollisionObject* collisionObject);
+
+	virtual void	performDiscreteCollisionDetection();
+
+	btDispatcherInfo& getDispatchInfo()
+	{
+		return m_dispatchInfo;
+	}
+
+};
+
+
+#endif //COLLISION_WORLD_H

bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -0,0 +1,140 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+
+
+btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+:m_isSwapped(isSwapped)
+{
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+	assert (colObj->getCollisionShape()->isCompound());
+	
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+	int numChildren = compoundShape->getNumChildShapes();
+	int i;
+	
+	m_childCollisionAlgorithms.resize(numChildren);
+	for (i=0;i<numChildren;i++)
+	{
+		btCollisionShape* childShape = compoundShape->getChildShape(i);
+		btCollisionShape* orgShape = colObj->getCollisionShape();
+		colObj->setCollisionShape( childShape );
+		m_childCollisionAlgorithms[i] = ci.m_dispatcher->findAlgorithm(colObj,otherObj);
+		colObj->setCollisionShape( orgShape );
+	}
+}
+
+
+btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
+{
+	int numChildren = m_childCollisionAlgorithms.size();
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		delete m_childCollisionAlgorithms[i];
+	}
+}
+
+void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
+	assert (colObj->getCollisionShape()->isCompound());
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
+	//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
+	//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
+	//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
+	//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
+	//then use each overlapping node AABB against Tree0
+	//and vise versa.
+
+	int numChildren = m_childCollisionAlgorithms.size();
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		//temporarily exchange parent btCollisionShape with childShape, and recurse
+		btCollisionShape* childShape = compoundShape->getChildShape(i);
+
+		//backup
+		btTransform	orgTrans = colObj->getWorldTransform();
+		btCollisionShape* orgShape = colObj->getCollisionShape();
+
+		const btTransform& childTrans = compoundShape->getChildTransform(i);
+		//btTransform	newChildWorldTrans = orgTrans*childTrans ;
+		colObj->setWorldTransform( orgTrans*childTrans );
+		//the contactpoint is still projected back using the original inverted worldtrans
+		colObj->setCollisionShape( childShape );
+		m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut);
+		//revert back
+		colObj->setCollisionShape( orgShape);
+		colObj->setWorldTransform(  orgTrans );
+	}
+}
+
+btScalar	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
+	assert (colObj->getCollisionShape()->isCompound());
+	
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
+	//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
+	//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
+	//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
+	//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
+	//then use each overlapping node AABB against Tree0
+	//and vise versa.
+
+	btScalar hitFraction = btScalar(1.);
+
+	int numChildren = m_childCollisionAlgorithms.size();
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		//temporarily exchange parent btCollisionShape with childShape, and recurse
+		btCollisionShape* childShape = compoundShape->getChildShape(i);
+
+		//backup
+		btTransform	orgTrans = colObj->getWorldTransform();
+		btCollisionShape* orgShape = colObj->getCollisionShape();
+
+		const btTransform& childTrans = compoundShape->getChildTransform(i);
+		//btTransform	newChildWorldTrans = orgTrans*childTrans ;
+		colObj->setWorldTransform( orgTrans*childTrans );
+
+		colObj->setCollisionShape( childShape );
+		btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
+		if (frac<hitFraction)
+		{
+			hitFraction = frac;
+		}
+		//revert back
+		colObj->setCollisionShape( orgShape);
+		colObj->setWorldTransform( orgTrans);
+	}
+	return hitFraction;
+
+}
+
+

bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h

@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COMPOUND_COLLISION_ALGORITHM_H
+#define COMPOUND_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "btCollisionCreateFunc.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+/// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
+/// Place holder, not fully implemented yet
+class btCompoundCollisionAlgorithm  : public btCollisionAlgorithm
+{
+	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
+	bool m_isSwapped;
+	
+public:
+
+	btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	virtual ~btCompoundCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			return new btCompoundCollisionAlgorithm(ci,body0,body1,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			return new btCompoundCollisionAlgorithm(ci,body0,body1,true);
+		}
+	};
+
+};
+
+#endif //COMPOUND_COLLISION_ALGORITHM_H

bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -0,0 +1,312 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btConvexConcaveCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionShapes/btConcaveShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+
+btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+: btCollisionAlgorithm(ci),
+m_isSwapped(isSwapped),
+m_btConvexTriangleCallback(ci.m_dispatcher,body0,body1,isSwapped)
+{
+}
+
+btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
+{
+}
+
+
+
+btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
+	  m_dispatcher(dispatcher),
+	m_dispatchInfoPtr(0)
+{
+	m_convexBody = isSwapped? body1:body0;
+	m_triBody = isSwapped? body0:body1;
+	
+	  //
+	  // create the manifold from the dispatcher 'manifold pool'
+	  //
+	  m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
+
+  	  clearCache();
+}
+
+btConvexTriangleCallback::~btConvexTriangleCallback()
+{
+	clearCache();
+	m_dispatcher->releaseManifold( m_manifoldPtr );
+  
+}
+  
+
+void	btConvexTriangleCallback::clearCache()
+{
+	m_dispatcher->clearManifold(m_manifoldPtr);
+};
+
+
+
+void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+{
+ 
+	//just for debugging purposes
+	//printf("triangle %d",m_triangleCount++);
+
+
+	//aabb filter is already applied!	
+
+	btCollisionAlgorithmConstructionInfo ci;
+	ci.m_dispatcher = m_dispatcher;
+
+	btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
+
+
+	
+	///debug drawing of the overlapping triangles
+	if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
+	{
+		btVector3 color(255,255,0);
+		btTransform& tr = ob->getWorldTransform();
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
+
+		//btVector3 center = triangle[0] + triangle[1]+triangle[2];
+		//center *= btScalar(0.333333);
+		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color);
+		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color);
+		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color);
+
+	}
+
+
+	//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
+	
+	if (m_convexBody->getCollisionShape()->isConvex())
+	{
+		btTriangleShape tm(triangle[0],triangle[1],triangle[2]);	
+		tm.setMargin(m_collisionMarginTriangle);
+	
+		
+		btCollisionShape* tmpShape = ob->getCollisionShape();
+		ob->setCollisionShape( &tm );
+		
+
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
+		///this should use the btDispatcher, so the actual registered algorithm is used
+		//		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
+
+		m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
+	//	cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
+//		cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		delete colAlgo;
+		ob->setCollisionShape( tmpShape );
+
+	}
+
+	
+
+}
+
+
+
+void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	m_dispatchInfoPtr = &dispatchInfo;
+	m_collisionMarginTriangle = collisionMarginTriangle;
+	m_resultOut = resultOut;
+
+	//recalc aabbs
+	btTransform convexInTriangleSpace;
+	convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform();
+	btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape());
+	//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
+	convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
+	btScalar extraMargin = collisionMarginTriangle;
+	btVector3 extra(extraMargin,extraMargin,extraMargin);
+
+	m_aabbMax += extra;
+	m_aabbMin -= extra;
+	
+}
+
+void btConvexConcaveCollisionAlgorithm::clearCache()
+{
+	m_btConvexTriangleCallback.clearCache();
+
+}
+
+void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	
+	
+	btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
+	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+	if (triBody->getCollisionShape()->isConcave())
+	{
+
+
+		btCollisionObject*	triOb = triBody;
+		btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
+		
+		if (convexBody->getCollisionShape()->isConvex())
+		{
+			btScalar collisionMarginTriangle = concaveShape->getMargin();
+					
+			resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+			m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
+
+			//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
+			//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+
+			m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
+
+			concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
+			
+	
+		}
+
+	}
+
+}
+
+
+btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
+	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+
+	//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
+
+	//only perform CCD above a certain threshold, this prevents blocking on the long run
+	//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
+	btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
+	if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
+	{
+		return btScalar(1.);
+	}
+
+	//const btVector3& from = convexbody->m_worldTransform.getOrigin();
+	//btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
+	//todo: only do if the motion exceeds the 'radius'
+
+	btTransform triInv = triBody->getWorldTransform().inverse();
+	btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
+	btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
+
+	struct LocalTriangleSphereCastCallback	: public btTriangleCallback
+	{
+		btTransform m_ccdSphereFromTrans;
+		btTransform m_ccdSphereToTrans;
+		btTransform	m_meshTransform;
+
+		btScalar	m_ccdSphereRadius;
+		btScalar	m_hitFraction;
+	
+
+		LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
+			:m_ccdSphereFromTrans(from),
+			m_ccdSphereToTrans(to),
+			m_ccdSphereRadius(ccdSphereRadius),
+			m_hitFraction(hitFraction)
+		{			
+		}
+		
+		
+		virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+		{
+			(void)partId;
+			(void)triangleIndex;
+			//do a swept sphere for now
+			btTransform ident;
+			ident.setIdentity();
+			btConvexCast::CastResult castResult;
+			castResult.m_fraction = m_hitFraction;
+			btSphereShape	pointShape(m_ccdSphereRadius);
+			btTriangleShape	triShape(triangle[0],triangle[1],triangle[2]);
+			btVoronoiSimplexSolver	simplexSolver;
+			btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
+			//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
+			//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+			//local space?
+
+			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
+				ident,ident,castResult))
+			{
+				if (m_hitFraction > castResult.m_fraction)
+					m_hitFraction = castResult.m_fraction;
+			}
+
+		}
+
+	};
+
+
+	
+
+	
+	if (triBody->getCollisionShape()->isConcave())
+	{
+		btVector3 rayAabbMin = convexFromLocal.getOrigin();
+		rayAabbMin.setMin(convexToLocal.getOrigin());
+		btVector3 rayAabbMax = convexFromLocal.getOrigin();
+		rayAabbMax.setMax(convexToLocal.getOrigin());
+		btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
+		rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+		rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+
+		btScalar curHitFraction = btScalar(1.); //is this available?
+		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
+			convexbody->getCcdSweptSphereRadius(),curHitFraction);
+
+		raycastCallback.m_hitFraction = convexbody->getHitFraction();
+
+		btCollisionObject* concavebody = triBody;
+
+		btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
+		
+		if (triangleMesh)
+		{
+			triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
+		}
+	
+
+
+		if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
+		{
+			convexbody->setHitFraction( raycastCallback.m_hitFraction);
+			return raycastCallback.m_hitFraction;
+		}
+	}
+
+	return btScalar(1.);
+
+}

bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h

@@ -0,0 +1,111 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_CONCAVE_COLLISION_ALGORITHM_H
+#define CONVEX_CONCAVE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "btCollisionCreateFunc.h"
+
+///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called.
+class btConvexTriangleCallback : public btTriangleCallback
+{
+	btCollisionObject* m_convexBody;
+	btCollisionObject* m_triBody;
+
+	btVector3	m_aabbMin;
+	btVector3	m_aabbMax ;
+
+	btManifoldResult* m_resultOut;
+
+	btDispatcher*	m_dispatcher;
+	const btDispatcherInfo* m_dispatchInfoPtr;
+	btScalar m_collisionMarginTriangle;
+	
+public:
+int	m_triangleCount;
+	
+	btPersistentManifold*	m_manifoldPtr;
+
+	btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	void	setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual ~btConvexTriangleCallback();
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+	
+	void clearCache();
+
+	inline const btVector3& getAabbMin() const
+	{
+		return m_aabbMin;
+	}
+	inline const btVector3& getAabbMax() const
+	{
+		return m_aabbMax;
+	}
+
+};
+
+
+
+
+/// btConvexConcaveCollisionAlgorithm  supports collision between convex shapes and (concave) trianges meshes.
+class btConvexConcaveCollisionAlgorithm  : public btCollisionAlgorithm
+{
+
+	bool	m_isSwapped;
+
+	btConvexTriangleCallback m_btConvexTriangleCallback;
+
+
+public:
+
+	btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	virtual ~btConvexConcaveCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	void	clearCache();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
+		}
+	};
+
+};
+
+#endif //CONVEX_CONCAVE_COLLISION_ALGORITHM_H

bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -0,0 +1,254 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btConvexConvexAlgorithm.h"
+
+#include <stdio.h>
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+
+
+
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+
+
+
+
+
+
+
+btConvexConvexAlgorithm::CreateFunc::CreateFunc()
+{
+	m_ownsSolvers = true;
+	m_simplexSolver = new btVoronoiSimplexSolver();
+	m_pdSolver = new btGjkEpaPenetrationDepthSolver;
+}
+
+btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+{
+	m_ownsSolvers = false;
+	m_simplexSolver = simplexSolver;
+	m_pdSolver = pdSolver;
+}
+
+btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
+{ 
+   if (m_ownsSolvers){ 
+      delete m_simplexSolver; 
+      delete m_pdSolver; 
+   } 
+}
+
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+: btCollisionAlgorithm(ci),
+m_gjkPairDetector(0,0,simplexSolver,pdSolver),
+m_ownManifold (false),
+m_manifoldPtr(mf),
+m_lowLevelOfDetail(false)
+{
+	(void)body0;
+	(void)body1;
+
+
+}
+
+
+
+
+btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void	btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+{
+	m_lowLevelOfDetail = useLowLevel;
+}
+
+
+
+
+
+//
+// Convex-Convex collision algorithm
+//
+void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	if (!m_manifoldPtr)
+	{
+		//swapped?
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+		m_ownManifold = true;
+	}
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+#ifdef USE_BT_GJKEPA
+	btConvexShape*				shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
+	btConvexShape*				shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
+	const btScalar				radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/);
+	btGjkEpaSolver::sResults	results;
+	if(btGjkEpaSolver::Collide(	shape0,body0->getWorldTransform(),
+								shape1,body1->getWorldTransform(),
+								radialmargin,results))
+		{
+		dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+		resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+		}
+#else
+
+	btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
+	btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
+	
+	btGjkPairDetector::ClosestPointInput input;
+
+	//TODO: if (dispatchInfo.m_useContinuous)
+	m_gjkPairDetector.setMinkowskiA(min0);
+	m_gjkPairDetector.setMinkowskiB(min1);
+	input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+	input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+	input.m_stackAlloc = dispatchInfo.m_stackAllocator;
+
+//	input.m_maximumDistanceSquared = btScalar(1e30);
+	
+	input.m_transformA = body0->getWorldTransform();
+	input.m_transformB = body1->getWorldTransform();
+	
+	m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+#endif
+
+}
+
+
+
+bool disableCcd = false;
+btScalar	btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
+    
+	///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
+	///col0->m_worldTransform,
+	btScalar resultFraction = btScalar(1.);
+
+
+	btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
+	btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
+    
+	if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
+		squareMot1 < col1->getCcdSquareMotionThreshold())
+		return resultFraction;
+
+	if (disableCcd)
+		return btScalar(1.);
+
+
+	//An adhoc way of testing the Continuous Collision Detection algorithms
+	//One object is approximated as a sphere, to simplify things
+	//Starting in penetration should report no time of impact
+	//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
+	//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
+
+		
+	/// Convex0 against sphere for Convex1
+	{
+		btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
+
+		btSphereShape	sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btConvexCast::CastResult result;
+		btVoronoiSimplexSolver voronoiSimplex;
+		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+		///Simplification, one object is simplified as a sphere
+		btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
+		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		{
+		
+			//store result.m_fraction in both bodies
+		
+			if (col0->getHitFraction()> result.m_fraction)
+				col0->setHitFraction( result.m_fraction );
+
+			if (col1->getHitFraction() > result.m_fraction)
+				col1->setHitFraction( result.m_fraction);
+
+			if (resultFraction > result.m_fraction)
+				resultFraction = result.m_fraction;
+
+		}
+		
+		
+
+
+	}
+
+	/// Sphere (for convex0) against Convex1
+	{
+		btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
+
+		btSphereShape	sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btConvexCast::CastResult result;
+		btVoronoiSimplexSolver voronoiSimplex;
+		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+		///Simplification, one object is simplified as a sphere
+		btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
+		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		{
+		
+			//store result.m_fraction in both bodies
+		
+			if (col0->getHitFraction()	> result.m_fraction)
+				col0->setHitFraction( result.m_fraction);
+
+			if (col1->getHitFraction() > result.m_fraction)
+				col1->setHitFraction( result.m_fraction);
+
+			if (resultFraction > result.m_fraction)
+				resultFraction = result.m_fraction;
+
+		}
+	}
+	
+	return resultFraction;
+
+}
+

bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

@@ -0,0 +1,76 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_CONVEX_ALGORITHM_H
+#define CONVEX_CONVEX_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "btCollisionCreateFunc.h"
+
+class btConvexPenetrationDepthSolver;
+
+///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
+class btConvexConvexAlgorithm : public btCollisionAlgorithm
+{
+	btGjkPairDetector m_gjkPairDetector;
+public:
+
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool			m_lowLevelOfDetail;
+	
+
+public:
+
+	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+
+	virtual ~btConvexConvexAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	void	setLowLevelOfDetail(bool useLowLevel);
+
+
+	const btPersistentManifold*	getManifold()
+	{
+		return m_manifoldPtr;
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		btConvexPenetrationDepthSolver*		m_pdSolver;
+		btSimplexSolverInterface*			m_simplexSolver;
+		bool	m_ownsSolvers;
+		
+		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+		CreateFunc();
+		virtual ~CreateFunc();
+
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			return new btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver);
+		}
+	};
+
+
+};
+
+#endif //CONVEX_CONVEX_ALGORITHM_H

bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp

@@ -0,0 +1,34 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btEmptyCollisionAlgorithm.h"
+
+
+
+btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+	: btCollisionAlgorithm(ci)
+{
+}
+
+void btEmptyAlgorithm::processCollision (btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
+{
+}
+
+btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
+{
+	return btScalar(1.);
+}
+
+

bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h

@@ -0,0 +1,48 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef EMPTY_ALGORITH
+#define EMPTY_ALGORITH
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "btCollisionCreateFunc.h"
+
+#define ATTRIBUTE_ALIGNED(a)
+
+///EmptyAlgorithm is a stub for unsupported collision pairs.
+///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame.
+class btEmptyAlgorithm : public btCollisionAlgorithm
+{
+
+public:
+	
+	btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			(void)body0;
+			(void)body1;
+			return new btEmptyAlgorithm(ci);
+		}
+	};
+
+} ATTRIBUTE_ALIGNED(16);
+
+#endif //EMPTY_ALGORITH

bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -0,0 +1,109 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btManifoldResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+
+///This is to allow MaterialCombiner/Custom Friction/Restitution values
+ContactAddedCallback		gContactAddedCallback=0;
+
+///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+inline btScalar	calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+	btScalar friction = body0->getFriction() * body1->getFriction();
+
+	const btScalar MAX_FRICTION  = btScalar(10.);
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
+inline btScalar	calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+	return body0->getRestitution() * body1->getRestitution();
+}
+
+
+
+btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* body1)
+		:m_manifoldPtr(0),
+		m_body0(body0),
+		m_body1(body1)
+{
+	m_rootTransA = body0->getWorldTransform();
+	m_rootTransB = body1->getWorldTransform();
+}
+
+
+void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+{
+	assert(m_manifoldPtr);
+	//order in manifold needs to match
+	
+	if (depth > m_manifoldPtr->getContactBreakingThreshold())
+		return;
+
+	bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+
+	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+
+	btVector3 localA;
+	btVector3 localB;
+	
+	if (isSwapped)
+	{
+		localA = m_rootTransB.invXform(pointA );
+		localB = m_rootTransA.invXform(pointInWorld);
+	} else
+	{
+		localA = m_rootTransA.invXform(pointA );
+		localB = m_rootTransB.invXform(pointInWorld);
+	}
+
+	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+
+	int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
+
+	newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);
+	newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1);
+
+	//User can override friction and/or restitution
+	if (gContactAddedCallback &&
+		//and if either of the two bodies requires custom material
+		 ((m_body0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
+		   (m_body1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
+	{
+		//experimental feature info, for per-triangle material etc.
+		btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
+		btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
+		(*gContactAddedCallback)(newPt,obj0,m_partId0,m_index0,obj1,m_partId1,m_index1);
+	}
+
+	if (insertIndex >= 0)
+	{
+		//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
+		m_manifoldPtr->replaceContactPoint(newPt,insertIndex);
+	} else
+	{
+		m_manifoldPtr->AddManifoldPoint(newPt);
+	}
+}
+

bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.h

@@ -0,0 +1,77 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef MANIFOLD_RESULT_H
+#define MANIFOLD_RESULT_H
+
+class btCollisionObject;
+class btPersistentManifold;
+class btManifoldPoint;
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
+#include "LinearMath/btTransform.h"
+
+typedef bool (*ContactAddedCallback)(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1);
+extern ContactAddedCallback		gContactAddedCallback;
+
+
+
+///btManifoldResult is a helper class to manage  contact results.
+class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
+{
+	btPersistentManifold* m_manifoldPtr;
+
+	//we need this for compounds
+	btTransform	m_rootTransA;
+	btTransform	m_rootTransB;
+
+	btCollisionObject* m_body0;
+	btCollisionObject* m_body1;
+	int	m_partId0;
+	int m_partId1;
+	int m_index0;
+	int m_index1;
+public:
+
+	btManifoldResult()
+	{
+	}
+
+	btManifoldResult(btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~btManifoldResult() {};
+
+	void	setPersistentManifold(btPersistentManifold* manifoldPtr)
+	{
+		m_manifoldPtr = manifoldPtr;
+	}
+
+	virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+	{
+			m_partId0=partId0;
+			m_partId1=partId1;
+			m_index0=index0;
+			m_index1=index1;		
+	}
+
+	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
+
+
+
+};
+
+#endif //MANIFOLD_RESULT_H

bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -0,0 +1,357 @@
+
+
+#include "LinearMath/btScalar.h"
+#include "btSimulationIslandManager.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+
+#include <stdio.h>
+#include "LinearMath/btQuickprof.h"
+
+btSimulationIslandManager::btSimulationIslandManager()
+{
+}
+
+btSimulationIslandManager::~btSimulationIslandManager()
+{
+}
+
+
+void btSimulationIslandManager::initUnionFind(int n)
+{
+		m_unionFind.reset(n);
+}
+		
+
+void btSimulationIslandManager::findUnions(btDispatcher* dispatcher)
+{
+	
+	{
+		for (int i=0;i<dispatcher->getNumManifolds();i++)
+		{
+			const btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+			//static objects (invmass btScalar(0.)) don't merge !
+
+			 const  btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
+			 const  btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
+
+			if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
+				((colObj1) && ((colObj1)->mergesSimulationIslands())))
+			{
+
+				m_unionFind.unite((colObj0)->getIslandTag(),
+					(colObj1)->getIslandTag());
+			}
+		}
+	}
+}
+
+
+void	btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+{
+	
+	initUnionFind( int (colWorld->getCollisionObjectArray().size()));
+	
+	// put the index into m_controllers into m_tag	
+	{
+		
+		int index = 0;
+		int i;
+		for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+		{
+			btCollisionObject*	collisionObject= colWorld->getCollisionObjectArray()[i];
+			collisionObject->setIslandTag(index);
+			collisionObject->setCompanionId(-1);
+			collisionObject->setHitFraction(btScalar(1.));
+			index++;
+			
+		}
+	}
+	// do the union find
+	
+	findUnions(dispatcher);
+	
+
+	
+}
+
+
+
+
+void	btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+{
+	// put the islandId ('find' value) into m_tag	
+	{
+		
+		
+		int index = 0;
+		int i;
+		for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+		{
+			btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+			if (collisionObject->mergesSimulationIslands())
+			{
+				collisionObject->setIslandTag( m_unionFind.find(index) );
+				collisionObject->setCompanionId(-1);
+			} else
+			{
+				collisionObject->setIslandTag(-1);
+				collisionObject->setCompanionId(-2);
+			}
+			index++;
+		}
+	}
+}
+
+inline	int	getIslandId(const btPersistentManifold* lhs)
+{
+	int islandId;
+	const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
+	const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
+	islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
+	return islandId;
+
+}
+
+
+
+/// function object that routes calls to operator<
+class btPersistentManifoldSortPredicate
+{
+	public:
+
+		SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
+		{
+			return getIslandId(lhs) < getIslandId(rhs);
+		}
+};
+
+
+
+
+
+//
+// todo: this is random access, it can be walked 'cache friendly'!
+//
+void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
+{
+
+	
+	
+	/*if (0)
+	{
+		int maxNumManifolds = dispatcher->getNumManifolds();
+		btCollisionDispatcher* colDis = (btCollisionDispatcher*)dispatcher;
+		btPersistentManifold** manifold = colDis->getInternalManifoldPointer();
+		callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, 0);
+		return;
+	}
+	*/
+
+
+	BEGIN_PROFILE("islandUnionFindAndHeapSort");
+	
+	//we are going to sort the unionfind array, and store the element id in the size
+	//afterwards, we clean unionfind, to make sure no-one uses it anymore
+	
+	getUnionFind().sortIslands();
+	int numElem = getUnionFind().getNumElements();
+
+	int endIslandIndex=1;
+	int startIslandIndex;
+
+
+	//update the sleeping state for bodies, if all are sleeping
+	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+	{
+		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+		for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+		{
+		}
+
+		//int numSleeping = 0;
+
+		bool allSleeping = true;
+
+		int idx;
+		for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+		{
+			int i = getUnionFind().getElement(idx).m_sz;
+
+			btCollisionObject* colObj0 = collisionObjects[i];
+			if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+			{
+				printf("error in island management\n");
+			}
+
+			assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+			if (colObj0->getIslandTag() == islandId)
+			{
+				if (colObj0->getActivationState()== ACTIVE_TAG)
+				{
+					allSleeping = false;
+				}
+				if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
+				{
+					allSleeping = false;
+				}
+			}
+		}
+			
+
+		if (allSleeping)
+		{
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+					printf("error in island management\n");
+				}
+
+				assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					colObj0->setActivationState( ISLAND_SLEEPING );
+				}
+			}
+		} else
+		{
+
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+					printf("error in island management\n");
+				}
+
+				assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+					{
+						colObj0->setActivationState( WANTS_DEACTIVATION);
+					}
+				}
+			}
+		}
+	}
+
+	btAlignedObjectArray<btPersistentManifold*>  islandmanifold;
+	int i;
+	int maxNumManifolds = dispatcher->getNumManifolds();
+	islandmanifold.reserve(maxNumManifolds);
+
+	for (i=0;i<maxNumManifolds ;i++)
+	{
+		 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+		 
+		 btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
+		 btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
+		
+		 //todo: check sleeping conditions!
+		 if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+			((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
+		{
+		
+			//kinematic objects don't merge islands, but wake up all connected objects
+			if (colObj0->isStaticOrKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
+			{
+				colObj1->activate();
+			}
+			if (colObj1->isStaticOrKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
+			{
+				colObj0->activate();
+			}
+
+			//filtering for response
+			if (dispatcher->needsResponse(colObj0,colObj1))
+				islandmanifold.push_back(manifold);
+		}
+	}
+
+	int numManifolds = int (islandmanifold.size());
+
+	// Sort manifolds, based on islands
+	// Sort the vector using predicate and std::sort
+	//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
+
+	//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
+	islandmanifold.heapSort(btPersistentManifoldSortPredicate());
+
+	//now process all active islands (sets of manifolds for now)
+
+	int startManifoldIndex = 0;
+	int endManifoldIndex = 1;
+
+	//int islandId;
+
+	END_PROFILE("islandUnionFindAndHeapSort");
+
+	btAlignedObjectArray<btCollisionObject*>	islandBodies;
+
+
+	//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
+	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+	{
+		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+
+
+	       bool islandSleeping = false;
+                
+                for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+                {
+                        int i = getUnionFind().getElement(endIslandIndex).m_sz;
+                        btCollisionObject* colObj0 = collisionObjects[i];
+						islandBodies.push_back(colObj0);
+                        if (!colObj0->isActive())
+                                islandSleeping = true;
+                }
+                
+
+		//find the accompanying contact manifold for this islandId
+		int numIslandManifolds = 0;
+		btPersistentManifold** startManifold = 0;
+
+		if (startManifoldIndex<numManifolds)
+		{
+			int curIslandId = getIslandId(islandmanifold[startManifoldIndex]);
+			if (curIslandId == islandId)
+			{
+				startManifold = &islandmanifold[startManifoldIndex];
+			
+				for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
+				{
+
+				}
+				/// Process the actual simulation, only if not sleeping/deactivated
+				numIslandManifolds = endManifoldIndex-startManifoldIndex;
+			}
+
+		}
+
+		if (!islandSleeping)
+		{
+			callback->ProcessIsland(&islandBodies[0],islandBodies.size(),startManifold,numIslandManifolds, islandId);
+		}
+		
+		if (numIslandManifolds)
+		{
+			startManifoldIndex = endManifoldIndex;
+		}
+
+		islandBodies.resize(0);
+	}
+
+	
+}

bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h

@@ -0,0 +1,61 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SIMULATION_ISLAND_MANAGER_H
+#define SIMULATION_ISLAND_MANAGER_H
+
+#include "BulletCollision/CollisionDispatch/btUnionFind.h"
+#include "btCollisionCreateFunc.h"
+
+class btCollisionObject;
+class btCollisionWorld;
+class btDispatcher;
+
+///SimulationIslandManager creates and handles simulation islands, using btUnionFind
+class btSimulationIslandManager
+{
+	btUnionFind m_unionFind;
+
+public:
+	btSimulationIslandManager();
+	virtual ~btSimulationIslandManager();
+
+
+	void initUnionFind(int n);	
+	
+		
+	btUnionFind& getUnionFind() { return m_unionFind;}
+
+	virtual	void	updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher);
+	virtual	void	storeIslandActivationState(btCollisionWorld* world);
+
+
+	void	findUnions(btDispatcher* dispatcher);
+
+	
+
+	struct	IslandCallback
+	{
+		virtual ~IslandCallback() {};
+
+		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold**	manifolds,int numManifolds, int islandId) = 0;
+	};
+
+	void	buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback);
+
+};
+
+#endif //SIMULATION_ISLAND_MANAGER_H
+

bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp

@@ -0,0 +1,249 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSphereBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+//#include <stdio.h>
+
+btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
+: btCollisionAlgorithm(ci),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_isSwapped(isSwapped)
+{
+	btCollisionObject* sphereObj = m_isSwapped? col1 : col0;
+	btCollisionObject* boxObj = m_isSwapped? col0 : col1;
+	
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj);
+		m_ownManifold = true;
+	}
+}
+
+
+btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+
+
+void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+	(void)resultOut;
+	if (!m_manifoldPtr)
+		return;
+
+	btCollisionObject* sphereObj = m_isSwapped? body1 : body0;
+	btCollisionObject* boxObj = m_isSwapped? body0 : body1;
+
+
+	btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape();
+
+	btVector3 normalOnSurfaceB;
+	btVector3 pOnBox,pOnSphere;
+	btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin();
+	btScalar radius = sphere0->getRadius();
+	
+	btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
+
+	if (dist < SIMD_EPSILON)
+	{
+		btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
+
+		/// report a contact. internally this will be kept persistent, and contact reduction is done
+
+		resultOut->setPersistentManifold(m_manifoldPtr);
+		resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
+		
+	}
+
+	
+
+}
+
+btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	(void)col0;
+	(void)col1;
+
+	//not yet
+	return btScalar(1.);
+}
+
+
+btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) 
+{
+
+	btScalar margins;
+	btVector3 bounds[2];
+	btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
+	
+	bounds[0] = -boxShape->getHalfExtents();
+	bounds[1] = boxShape->getHalfExtents();
+
+	margins = boxShape->getMargin();//also add sphereShape margin?
+
+	const btTransform&	m44T = boxObj->getWorldTransform();
+
+	btVector3	boundsVec[2];
+	btScalar	fPenetration;
+
+	boundsVec[0] = bounds[0];
+	boundsVec[1] = bounds[1];
+
+	btVector3	marginsVec( margins, margins, margins );
+
+	// add margins
+	bounds[0] += marginsVec;
+	bounds[1] -= marginsVec;
+
+	/////////////////////////////////////////////////
+
+	btVector3	tmp, prel, n[6], normal, v3P;
+	btScalar   fSep = btScalar(10000000.0), fSepThis;
+
+	n[0].setValue( btScalar(-1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[1].setValue(  btScalar(0.0), btScalar(-1.0),  btScalar(0.0) );
+	n[2].setValue(  btScalar(0.0),  btScalar(0.0), btScalar(-1.0) );
+	n[3].setValue(  btScalar(1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[4].setValue(  btScalar(0.0),  btScalar(1.0),  btScalar(0.0) );
+	n[5].setValue(  btScalar(0.0),  btScalar(0.0),  btScalar(1.0) );
+
+	// convert  point in local space
+	prel = m44T.invXform( sphereCenter);
+	
+	bool	bFound = false;
+
+	v3P = prel;
+
+	for (int i=0;i<6;i++)
+	{
+		int j = i<3? 0:1;
+		if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) )
+		{
+			v3P = v3P - n[i]*fSepThis;		
+			bFound = true;
+		}
+	}
+	
+	//
+
+	if ( bFound )
+	{
+		bounds[0] = boundsVec[0];
+		bounds[1] = boundsVec[1];
+
+		normal = (prel - v3P).normalize();
+		pointOnBox = v3P + normal*margins;
+		v3PointOnSphere = prel - normal*fRadius;
+
+		if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) )
+		{
+			return btScalar(1.0);
+		}
+
+		// transform back in world space
+		tmp = m44T( pointOnBox);
+		pointOnBox    = tmp;
+		tmp  = m44T( v3PointOnSphere);		
+		v3PointOnSphere = tmp;
+		btScalar fSeps2 = (pointOnBox-v3PointOnSphere).length2();
+		
+		//if this fails, fallback into deeper penetration case, below
+		if (fSeps2 > SIMD_EPSILON)
+		{
+			fSep = - btSqrt(fSeps2);
+			normal = (pointOnBox-v3PointOnSphere);
+			normal *= btScalar(1.)/fSep;
+		}
+
+		return fSep;
+	}
+
+	//////////////////////////////////////////////////
+	// Deep penetration case
+
+	fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] );
+
+	bounds[0] = boundsVec[0];
+	bounds[1] = boundsVec[1];
+
+	if ( fPenetration <= btScalar(0.0) )
+		return (fPenetration-margins);
+	else
+		return btScalar(1.0);
+}
+
+btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) 
+{
+
+	btVector3 bounds[2];
+
+	bounds[0] = aabbMin;
+	bounds[1] = aabbMax;
+
+	btVector3	p0, tmp, prel, n[6], normal;
+	btScalar   fSep = btScalar(-10000000.0), fSepThis;
+
+	n[0].setValue( btScalar(-1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[1].setValue(  btScalar(0.0), btScalar(-1.0),  btScalar(0.0) );
+	n[2].setValue(  btScalar(0.0),  btScalar(0.0), btScalar(-1.0) );
+	n[3].setValue(  btScalar(1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[4].setValue(  btScalar(0.0),  btScalar(1.0),  btScalar(0.0) );
+	n[5].setValue(  btScalar(0.0),  btScalar(0.0),  btScalar(1.0) );
+
+	const btTransform&	m44T = boxObj->getWorldTransform();
+
+	// convert  point in local space
+	prel = m44T.invXform( sphereCenter);
+
+	///////////
+
+	for (int i=0;i<6;i++)
+	{
+		int j = i<3 ? 0:1;
+		if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) )	return btScalar(1.0);
+		if ( fSepThis > fSep )
+		{
+			p0 = bounds[j];	normal = (btVector3&)n[i];
+			fSep = fSepThis;
+		}
+	}
+
+	pointOnBox = prel - normal*(normal.dot((prel-p0)));
+	v3PointOnSphere = pointOnBox + normal*fSep;
+
+	// transform back in world space
+	tmp  = m44T( pointOnBox);		
+	pointOnBox    = tmp;
+	tmp  = m44T( v3PointOnSphere);		v3PointOnSphere = tmp;
+	normal = (pointOnBox-v3PointOnSphere).normalize();
+
+	return fSep;
+
+}
+

bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h

@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_BOX_COLLISION_ALGORITHM_H
+#define SPHERE_BOX_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "LinearMath/btVector3.h"
+
+/// btSphereBoxCollisionAlgorithm  provides sphere-box collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+class btSphereBoxCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool	m_isSwapped;
+	
+public:
+
+	btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
+
+	virtual ~btSphereBoxCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius );
+
+	btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);
+	
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			if (!m_swapped)
+			{
+				return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false);
+			} else
+			{
+				return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true);
+			}
+		}
+	};
+
+};
+
+#endif //SPHERE_BOX_COLLISION_ALGORITHM_H
+

bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp

@@ -0,0 +1,85 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSphereSphereCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1)
+: btCollisionAlgorithm(ci),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+	if (!m_manifoldPtr)
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
+		m_ownManifold = true;
+	}
+}
+
+btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+
+	if (!m_manifoldPtr)
+		return;
+
+	btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape();
+	btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape();
+
+	btVector3 diff = col0->getWorldTransform().getOrigin()-  col1->getWorldTransform().getOrigin();
+	btScalar len = diff.length();
+	btScalar radius0 = sphere0->getRadius();
+	btScalar radius1 = sphere1->getRadius();
+
+	///iff distance positive, don't generate a new contact
+	if ( len > (radius0+radius1))
+		return;
+
+	///distance (negative means penetration)
+	btScalar dist = len - (radius0+radius1);
+
+	btVector3 normalOnSurfaceB = diff / len;
+	///point on A (worldspace)
+	btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
+	///point on B (worldspace)
+	btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
+
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	resultOut->setPersistentManifold(m_manifoldPtr);
+	resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
+
+}
+
+btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)col0;
+	(void)col1;
+	(void)dispatchInfo;
+	(void)resultOut;
+
+	//not yet
+	return btScalar(1.);
+}

bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h

@@ -0,0 +1,56 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H
+#define SPHERE_SPHERE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+
+/// btSphereSphereCollisionAlgorithm  provides sphere-sphere collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+/// Also provides the most basic sample for custom/user btCollisionAlgorithm
+class btSphereSphereCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	
+public:
+	btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	
+	virtual ~btSphereSphereCollisionAlgorithm();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			return new btSphereSphereCollisionAlgorithm(0,ci,body0,body1);
+		}
+	};
+
+};
+
+#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H
+

bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp

@@ -0,0 +1,76 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btSphereTriangleCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "SphereTriangleDetector.h"
+
+
+btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped)
+: btCollisionAlgorithm(ci),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_swapped(swapped)
+{
+	if (!m_manifoldPtr)
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
+		m_ownManifold = true;
+	}
+}
+
+btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	if (!m_manifoldPtr)
+		return;
+
+	btSphereShape* sphere = (btSphereShape*)col0->getCollisionShape();
+	btTriangleShape* triangle = (btTriangleShape*)col1->getCollisionShape();
+	
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	resultOut->setPersistentManifold(m_manifoldPtr);
+	SphereTriangleDetector detector(sphere,triangle);
+	
+	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
+	input.m_maximumDistanceSquared = btScalar(1e30);//todo: tighter bounds
+	input.m_transformA = col0->getWorldTransform();
+	input.m_transformB = col1->getWorldTransform();
+
+	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+}
+
+btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	(void)col0;
+	(void)col1;
+
+	//not yet
+	return btScalar(1.);
+}

bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h

@@ -0,0 +1,59 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+
+/// btSphereSphereCollisionAlgorithm  provides sphere-sphere collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+/// Also provides the most basic sample for custom/user btCollisionAlgorithm
+class btSphereTriangleCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool	m_swapped;
+	
+public:
+	btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped);
+
+	btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	
+	virtual ~btSphereTriangleCollisionAlgorithm();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			
+				return new btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped);
+		}
+	};
+
+};
+
+#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+

bullet/src/BulletCollision/CollisionDispatch/btUnionFind.cpp

@@ -0,0 +1,83 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btUnionFind.h"
+#include <assert.h>
+
+
+
+btUnionFind::~btUnionFind()
+{
+	Free();
+
+}
+
+btUnionFind::btUnionFind()
+{ 
+
+}
+
+void	btUnionFind::allocate(int N)
+{
+	m_elements.resize(N);
+}
+void	btUnionFind::Free()
+{
+	m_elements.clear();
+}
+
+
+void	btUnionFind::reset(int N)
+{
+	allocate(N);
+
+	for (int i = 0; i < N; i++) 
+	{ 
+		m_elements[i].m_id = i; m_elements[i].m_sz = 1; 
+	} 
+}
+
+
+class btUnionFindElementSortPredicate
+{
+	public:
+
+		bool operator() ( const btElement& lhs, const btElement& rhs )
+		{
+			return lhs.m_id < rhs.m_id;
+		}
+};
+
+///this is a special operation, destroying the content of btUnionFind.
+///it sorts the elements, based on island id, in order to make it easy to iterate over islands
+void	btUnionFind::sortIslands()
+{
+
+	//first store the original body index, and islandId
+	int numElements = m_elements.size();
+	
+	for (int i=0;i<numElements;i++)
+	{
+		m_elements[i].m_id = find(i);
+		m_elements[i].m_sz = i;
+	}
+	
+	 // Sort the vector using predicate and std::sort
+	  //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
+	//perhaps use radix sort?
+	  m_elements.heapSort(btUnionFindElementSortPredicate());
+
+}
+

bullet/src/BulletCollision/CollisionDispatch/btUnionFind.h

@@ -0,0 +1,124 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef UNION_FIND_H
+#define UNION_FIND_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+	#define USE_PATH_COMPRESSION 1
+
+struct	btElement
+{
+	int	m_id;
+	int	m_sz;
+};
+
+///UnionFind calculates connected subsets
+// Implements weighted Quick Union with path compression
+// optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable)
+class btUnionFind
+  {
+    private:
+		btAlignedObjectArray<btElement>	m_elements;
+
+    public:
+	  
+		btUnionFind();
+		~btUnionFind();
+
+	
+		//this is a special operation, destroying the content of btUnionFind.
+		//it sorts the elements, based on island id, in order to make it easy to iterate over islands
+		void	sortIslands();
+
+	  void	reset(int N);
+
+	  inline int	getNumElements() const
+	  {
+		  return int(m_elements.size());
+	  }
+	  inline bool  isRoot(int x) const
+	  {
+		  return (x == m_elements[x].m_id);
+	  }
+
+	  btElement&	getElement(int index)
+	  {
+		  return m_elements[index];
+	  }
+	  const btElement& getElement(int index) const
+	  {
+		  return m_elements[index];
+	  }
+   
+	  void	allocate(int N);
+	  void	Free();
+
+
+
+
+	  int find(int p, int q)
+		{ 
+			return (find(p) == find(q)); 
+		}
+
+		void unite(int p, int q)
+		{
+			int i = find(p), j = find(q);
+			if (i == j) 
+				return;
+
+#ifndef USE_PATH_COMPRESSION
+			//weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
+			if (m_elements[i].m_sz < m_elements[j].m_sz)
+			{ 
+				m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; 
+			}
+			else 
+			{ 
+				m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; 
+			}
+#else
+			m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; 
+#endif //USE_PATH_COMPRESSION
+		}
+
+		int find(int x)
+		{ 
+			//assert(x < m_N);
+			//assert(x >= 0);
+
+			while (x != m_elements[x].m_id) 
+			{
+		//not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically
+	
+		#ifdef USE_PATH_COMPRESSION
+				//
+				m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id;
+		#endif //
+				x = m_elements[x].m_id;
+				//assert(x < m_N);
+				//assert(x >= 0);
+
+			}
+			return x; 
+		}
+
+
+  };
+
+
+#endif //UNION_FIND_H