moved files around

Bullet/CollisionDispatch/CollisionDispatcher.cpp

@@ -0,0 +1,300 @@
+/*
+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 "CollisionDispatcher.h"
+
+
+#include "BroadphaseCollision/CollisionAlgorithm.h"
+#include "CollisionDispatch/ConvexConvexAlgorithm.h"
+#include "CollisionDispatch/EmptyCollisionAlgorithm.h"
+#include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h"
+#include "CollisionShapes/CollisionShape.h"
+#include "CollisionDispatch/CollisionObject.h"
+#include <algorithm>
+
+int gNumManifold = 0;
+
+void CollisionDispatcher::FindUnions()
+{
+	if (m_useIslands)
+	{
+		for (int i=0;i<GetNumManifolds();i++)
+		{
+			const PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
+			//static objects (invmass 0.f) don't merge !
+
+			 const  CollisionObject* colObj0 = static_cast<const CollisionObject*>(manifold->GetBody0());
+			 const  CollisionObject* colObj1 = static_cast<const CollisionObject*>(manifold->GetBody1());
+
+			 if (colObj0 && colObj1 && NeedsResponse(*colObj0,*colObj1))
+			 {
+				if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
+					((colObj1) && ((colObj1)->mergesSimulationIslands())))
+				{
+
+					m_unionFind.unite((colObj0)->m_islandTag1,
+						(colObj1)->m_islandTag1);
+				}
+			 }
+			
+			
+		}
+	}
+	
+}
+	
+
+	
+CollisionDispatcher::CollisionDispatcher (): 
+	m_useIslands(true),
+		m_defaultManifoldResult(0,0,0),
+		m_count(0)
+{
+	int i;
+	
+	for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
+	{
+		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
+		{
+			m_doubleDispatch[i][j] = 0;
+		}
+	}
+	
+	
+};
+	
+PersistentManifold*	CollisionDispatcher::GetNewManifold(void* b0,void* b1) 
+{ 
+	gNumManifold++;
+	//printf("GetNewManifoldResult: gNumManifold %d\n",gNumManifold);
+
+	CollisionObject* body0 = (CollisionObject*)b0;
+	CollisionObject* body1 = (CollisionObject*)b1;
+	
+	PersistentManifold* manifold = new PersistentManifold (body0,body1);
+	m_manifoldsPtr.push_back(manifold);
+
+	return manifold;
+}
+
+void CollisionDispatcher::ClearManifold(PersistentManifold* manifold)
+{
+	manifold->ClearManifold();
+}
+
+	
+void CollisionDispatcher::ReleaseManifold(PersistentManifold* manifold)
+{
+	
+	gNumManifold--;
+
+	//printf("ReleaseManifold: gNumManifold %d\n",gNumManifold);
+
+	ClearManifold(manifold);
+
+	std::vector<PersistentManifold*>::iterator i =
+		std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold);
+	if (!(i == m_manifoldsPtr.end()))
+	{
+		std::swap(*i, m_manifoldsPtr.back());
+		m_manifoldsPtr.pop_back();
+		delete manifold;
+
+	}
+	
+	
+}
+
+	
+//
+// todo: this is random access, it can be walked 'cache friendly'!
+//
+void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback* callback)
+{
+	for (int islandId=0;islandId<numBodies;islandId++)
+	{
+
+		std::vector<PersistentManifold*>  islandmanifold;
+		
+		//int numSleeping = 0;
+
+		bool allSleeping = true;
+
+		for (int i=0;i<GetNumManifolds();i++)
+		{
+			 PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
+			 
+			 //filtering for response
+
+			 CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
+			 CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
+
+
+			 
+			 {
+				if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
+					((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
+				{
+
+					if (((colObj0) && (colObj0)->GetActivationState()== ACTIVE_TAG) ||
+						((colObj1) && (colObj1)->GetActivationState() == ACTIVE_TAG))
+					{
+						allSleeping = false;
+					}
+					if (((colObj0) && (colObj0)->GetActivationState()== DISABLE_DEACTIVATION) ||
+						((colObj1) && (colObj1)->GetActivationState() == DISABLE_DEACTIVATION))
+					{
+						allSleeping = false;
+					}
+
+					if (NeedsResponse(*colObj0,*colObj1))
+						islandmanifold.push_back(manifold);
+				}
+			 }
+		}
+		if (allSleeping)
+		{
+			//tag all as 'ISLAND_SLEEPING'
+			for (size_t i=0;i<islandmanifold.size();i++)
+			{
+				PersistentManifold* manifold = islandmanifold[i];
+
+				CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
+				CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
+
+				if ((colObj0))	
+				{
+					(colObj0)->SetActivationState( ISLAND_SLEEPING );
+				}
+				if ((colObj1))	
+				{
+					(colObj1)->SetActivationState( ISLAND_SLEEPING);
+				}
+
+			}
+		} else
+		{
+
+			//tag all as 'ISLAND_SLEEPING'
+			for (size_t i=0;i<islandmanifold.size();i++)
+			{
+				 PersistentManifold* manifold = islandmanifold[i];
+				 CollisionObject* body0 = (CollisionObject*)manifold->GetBody0();
+				 CollisionObject* body1 = (CollisionObject*)manifold->GetBody1();
+
+				if (body0)	
+				{
+					if ( body0->GetActivationState() == ISLAND_SLEEPING)
+					{
+						body0->SetActivationState( WANTS_DEACTIVATION);
+					}
+				}
+				if (body1)	
+				{
+					if ( body1->GetActivationState() == ISLAND_SLEEPING)
+					{
+						body1->SetActivationState(WANTS_DEACTIVATION);
+					}
+				}
+
+			}
+
+			if (islandmanifold.size())
+			{
+				callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());
+			}
+
+		}
+	}
+}
+
+
+
+CollisionAlgorithm* CollisionDispatcher::InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
+{
+	m_count++;
+	CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject;
+	CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject;
+
+	CollisionAlgorithmConstructionInfo ci;
+	ci.m_dispatcher = this;
+	
+	if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConvex() )
+	{
+		return new ConvexConvexAlgorithm(0,ci,&proxy0,&proxy1);			
+	}
+
+	if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConcave())
+	{
+		return new ConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1);
+	}
+
+	if (body1->m_collisionShape->IsConvex() && body0->m_collisionShape->IsConcave())
+	{
+		return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0);
+	}
+
+	//failed to find an algorithm
+	return new EmptyAlgorithm(ci);
+	
+}
+
+bool	CollisionDispatcher::NeedsResponse(const  CollisionObject& colObj0,const CollisionObject& colObj1)
+{
+	//here you can do filtering
+	bool hasResponse = 
+		(!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) &&
+		(!(colObj1.m_collisionFlags & CollisionObject::noContactResponse));
+	return hasResponse;
+}
+
+bool	CollisionDispatcher::NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
+{
+
+	CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject;
+	CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject;
+
+	assert(body0);
+	assert(body1);
+
+	bool needsCollision = true;
+
+	if ((body0->m_collisionFlags & CollisionObject::isStatic) && 
+		(body1->m_collisionFlags & CollisionObject::isStatic))
+		needsCollision = false;
+	
+	if ((body0->GetActivationState() == 2) &&(body1->GetActivationState() == 2))
+		needsCollision = false;
+	
+	return needsCollision ;
+
+}
+
+///allows the user to get contact point callbacks 
+ManifoldResult*	CollisionDispatcher::GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold)
+{
+
+
+	//in-place, this prevents parallel dispatching, but just adding a list would fix that.
+	ManifoldResult* manifoldResult = new (&m_defaultManifoldResult)	ManifoldResult(obj0,obj1,manifold);
+	return manifoldResult;
+}
+	
+///allows the user to get contact point callbacks 
+void	CollisionDispatcher::ReleaseManifoldResult(ManifoldResult*)
+{
+
+}

Bullet/CollisionDispatch/CollisionDispatcher.h

@@ -0,0 +1,139 @@
+/*
+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 "BroadphaseCollision/Dispatcher.h"
+#include "NarrowPhaseCollision/PersistentManifold.h"
+#include "CollisionDispatch/UnionFind.h"
+#include "CollisionDispatch/ManifoldResult.h"
+
+#include "BroadphaseCollision/BroadphaseProxy.h"
+#include <vector>
+
+class IDebugDraw;
+
+
+
+
+struct CollisionAlgorithmCreateFunc
+{
+	bool m_swapped;
+	
+	CollisionAlgorithmCreateFunc()
+		:m_swapped(false)
+	{
+	}
+	virtual ~CollisionAlgorithmCreateFunc(){};
+
+	virtual	CollisionAlgorithm* CreateCollisionAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
+	{
+		return 0;
+	}
+};
+
+
+///CollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
+///Time of Impact, Closest Points and Penetration Depth.
+class CollisionDispatcher : public Dispatcher
+{
+	
+	std::vector<PersistentManifold*>	m_manifoldsPtr;
+
+	UnionFind m_unionFind;
+
+	bool m_useIslands;
+	
+	ManifoldResult	m_defaultManifoldResult;
+	
+	CollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
+	
+public:
+	
+	UnionFind& GetUnionFind() { return m_unionFind;}
+
+	struct	IslandCallback
+	{
+		virtual ~IslandCallback() {};
+
+		virtual	void	ProcessIsland(PersistentManifold**	manifolds,int numManifolds) = 0;
+	};
+
+
+	int	GetNumManifolds() const
+	{ 
+		return m_manifoldsPtr.size();
+	}
+
+	 PersistentManifold* GetManifoldByIndexInternal(int index)
+	{
+		return m_manifoldsPtr[index];
+	}
+
+	 const PersistentManifold* GetManifoldByIndexInternal(int index) const
+	{
+		return m_manifoldsPtr[index];
+	}
+
+	void InitUnionFind(int n)
+	{
+		if (m_useIslands)
+			m_unionFind.reset(n);
+	}
+	
+	void FindUnions();
+	
+	int m_count;
+	
+	CollisionDispatcher ();
+	virtual ~CollisionDispatcher() {};
+
+	virtual PersistentManifold*	GetNewManifold(void* b0,void* b1);
+	
+	virtual void ReleaseManifold(PersistentManifold* manifold);
+
+	
+	virtual void BuildAndProcessIslands(int numBodies, IslandCallback* callback);
+
+	///allows the user to get contact point callbacks 
+	virtual	ManifoldResult*	GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold);
+
+	///allows the user to get contact point callbacks 
+	virtual	void	ReleaseManifoldResult(ManifoldResult*);
+
+	virtual void ClearManifold(PersistentManifold* manifold);
+
+			
+	CollisionAlgorithm* FindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
+	{
+		CollisionAlgorithm* algo = InternalFindAlgorithm(proxy0,proxy1);
+		return algo;
+	}
+	
+	CollisionAlgorithm* InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1);
+	
+	virtual bool	NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1);
+	
+	virtual bool	NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1);
+
+	virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;}
+	
+	
+
+};
+
+#endif //COLLISION__DISPATCHER_H
+

Bullet/CollisionDispatch/CollisionObject.cpp

@@ -0,0 +1,55 @@
+/*
+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 "CollisionObject.h"
+
+CollisionObject::CollisionObject()
+	:	m_collisionFlags(0),
+		m_activationState1(1),
+		m_deactivationTime(0.f),
+		m_broadphaseHandle(0),
+		m_collisionShape(0),
+		m_hitFraction(1.f),
+		m_ccdSweptShereRadius(0.f),
+		m_ccdSquareMotionTreshold(0.f)
+{
+
+}
+
+
+void CollisionObject::SetActivationState(int newState) 
+{ 
+	if (m_activationState1 != DISABLE_DEACTIVATION)
+		m_activationState1 = newState;
+}
+
+void CollisionObject::ForceActivationState(int newState)
+{
+	m_activationState1 = newState;
+}
+
+void CollisionObject::activate()
+{
+	if (!(m_collisionFlags & isStatic))
+	{
+		SetActivationState(1);
+		m_deactivationTime = 0.f;
+	}
+}
+
+bool CollisionObject::mergesSimulationIslands() const
+{
+	return ( !(m_collisionFlags & isStatic));
+}

Bullet/CollisionDispatch/CollisionObject.h

@@ -0,0 +1,101 @@
+/*
+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 "SimdTransform.h"
+
+//island management, m_activationState1
+#define ACTIVE_TAG 1
+#define ISLAND_SLEEPING 2
+#define WANTS_DEACTIVATION 3
+#define DISABLE_DEACTIVATION 4
+
+struct	BroadphaseProxy;
+class	CollisionShape;
+
+/// CollisionObject can be used to manage collision detection objects. 
+/// CollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
+/// They can be added to the CollisionWorld.
+struct	CollisionObject
+{
+	SimdTransform	m_worldTransform;
+	
+	//m_interpolationWorldTransform is used for CCD and interpolation
+	//it can be either previous or future (predicted) transform
+	SimdTransform	m_interpolationWorldTransform;
+	
+	enum CollisionFlags
+	{
+		isStatic = 1,
+		noContactResponse = 2,
+
+	};
+
+	int				m_collisionFlags;
+
+	int				m_islandTag1;
+	int				m_activationState1;
+	float			m_deactivationTime;
+
+	BroadphaseProxy*	m_broadphaseHandle;
+	CollisionShape*		m_collisionShape;
+
+	void*			m_userPointer;//not use by Bullet internally
+
+	///time of impact calculation
+	float			m_hitFraction; 
+	
+	///Swept sphere radius (0.0 by default), see ConvexConvexAlgorithm::
+	float			m_ccdSweptShereRadius;
+
+	/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold
+	float			m_ccdSquareMotionTreshold;
+
+	bool			mergesSimulationIslands() const;
+
+	inline bool		IsStatic() const {
+		return m_collisionFlags & isStatic;
+	}
+
+	inline bool		HasContactResponse() {
+		return !(m_collisionFlags & noContactResponse);
+	}
+
+	
+
+
+	CollisionObject();
+
+
+	void	SetCollisionShape(CollisionShape* collisionShape)
+	{
+		m_collisionShape = collisionShape;
+	}
+
+	int	GetActivationState() const { return m_activationState1;}
+	
+	void SetActivationState(int newState);
+
+	void ForceActivationState(int newState);
+
+	void	activate();
+
+
+
+};
+
+#endif //COLLISION_OBJECT_H

Bullet/CollisionDispatch/CollisionWorld.cpp

@@ -0,0 +1,323 @@
+/*
+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 "CollisionWorld.h"
+#include "CollisionDispatcher.h"
+#include "CollisionDispatch/CollisionObject.h"
+#include "CollisionShapes/CollisionShape.h"
+#include "CollisionShapes/SphereShape.h" //for raycasting
+#include "CollisionShapes/TriangleMeshShape.h" //for raycasting
+#include "NarrowPhaseCollision/RaycastCallback.h"
+
+#include "NarrowPhaseCollision/SubSimplexConvexCast.h"
+#include "BroadphaseCollision/BroadphaseInterface.h"
+#include "AabbUtil2.h"
+
+#include <algorithm>
+
+CollisionWorld::~CollisionWorld()
+{
+	//clean up remaining objects
+	std::vector<CollisionObject*>::iterator i;
+
+	int index = 0;
+	for (i=m_collisionObjects.begin();
+	!(i==m_collisionObjects.end()); i++)
+
+	{
+		CollisionObject* collisionObject= (*i);
+		
+		BroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
+		if (bp)
+		{
+			//
+			// only clear the cached algorithms
+			//
+			GetBroadphase()->CleanProxyFromPairs(bp);
+			GetBroadphase()->DestroyProxy(bp);
+		}
+	}
+
+}
+
+void	CollisionWorld::UpdateActivationState()
+{
+	m_dispatcher->InitUnionFind(m_collisionObjects.size());
+	
+	// put the index into m_controllers into m_tag	
+	{
+		std::vector<CollisionObject*>::iterator i;
+		
+		int index = 0;
+		for (i=m_collisionObjects.begin();
+		!(i==m_collisionObjects.end()); i++)
+		{
+			
+			CollisionObject*	collisionObject= (*i);
+			collisionObject->m_islandTag1 = index;
+			collisionObject->m_hitFraction = 1.f;
+			index++;
+			
+		}
+	}
+	// do the union find
+	
+	m_dispatcher->FindUnions();
+	
+	// put the islandId ('find' value) into m_tag	
+	{
+		UnionFind& unionFind = m_dispatcher->GetUnionFind();
+		
+		std::vector<CollisionObject*>::iterator i;
+		
+		int index = 0;
+		for (i=m_collisionObjects.begin();
+		!(i==m_collisionObjects.end()); i++)
+		{
+			CollisionObject* collisionObject= (*i);
+			
+			if (collisionObject->mergesSimulationIslands())
+			{
+				collisionObject->m_islandTag1 = unionFind.find(index);
+			} else
+			{
+				collisionObject->m_islandTag1 = -1;
+			}
+			index++;
+		}
+	}
+	
+}
+
+
+
+
+
+void	CollisionWorld::AddCollisionObject(CollisionObject* collisionObject)
+{
+		m_collisionObjects.push_back(collisionObject);
+
+		//calculate new AABB
+		SimdTransform trans = collisionObject->m_worldTransform;
+
+		SimdVector3	minAabb;
+		SimdVector3	maxAabb;
+		collisionObject->m_collisionShape->GetAabb(trans,minAabb,maxAabb);
+
+		int type = collisionObject->m_collisionShape->GetShapeType();
+		collisionObject->m_broadphaseHandle = GetBroadphase()->CreateProxy(
+			minAabb,
+			maxAabb,
+			type,
+			collisionObject
+			);
+		
+
+
+
+}
+
+void	CollisionWorld::PerformDiscreteCollisionDetection()
+{
+	DispatcherInfo	dispatchInfo;
+	dispatchInfo.m_timeStep = 0.f;
+	dispatchInfo.m_stepCount = 0;
+
+	//update aabb (of all moved objects)
+
+	SimdVector3 aabbMin,aabbMax;
+	for (size_t i=0;i<m_collisionObjects.size();i++)
+	{
+		m_collisionObjects[i]->m_collisionShape->GetAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax);
+		m_broadphase->SetAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax);
+	}
+
+	m_broadphase->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);
+}
+
+
+void	CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject)
+{
+	
+	
+	//bool removeFromBroadphase = false;
+	
+	{
+		
+		BroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
+		if (bp)
+		{
+			//
+			// only clear the cached algorithms
+			//
+			GetBroadphase()->CleanProxyFromPairs(bp);
+			GetBroadphase()->DestroyProxy(bp);
+		}
+	}
+
+
+	std::vector<CollisionObject*>::iterator i =	std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject);
+		
+	if (!(i == m_collisionObjects.end()))
+		{
+			std::swap(*i, m_collisionObjects.back());
+			m_collisionObjects.pop_back();
+		}
+}
+
+
+
+void	CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
+{
+
+	
+	SimdTransform	rayFromTrans,rayToTrans;
+	rayFromTrans.setIdentity();
+	rayFromTrans.setOrigin(rayFromWorld);
+	rayToTrans.setIdentity();
+	
+	rayToTrans.setOrigin(rayToWorld);
+
+	//do culling based on aabb (rayFrom/rayTo)
+	SimdVector3 rayAabbMin = rayFromWorld;
+	SimdVector3 rayAabbMax = rayFromWorld;
+	rayAabbMin.setMin(rayToWorld);
+	rayAabbMax.setMax(rayToWorld);
+
+	SphereShape pointShape(0.0f);
+
+	/// brute force go over all objects. Once there is a broadphase, use that, or
+	/// add a raycast against aabb first.
+	
+	std::vector<CollisionObject*>::iterator iter;
+	
+	for (iter=m_collisionObjects.begin();
+	!(iter==m_collisionObjects.end()); iter++)
+	{
+		
+		CollisionObject*	collisionObject= (*iter);
+
+		//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+		SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+		collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
+
+		//check aabb overlap
+
+		if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
+		{
+			if (collisionObject->m_collisionShape->IsConvex())
+			{
+				ConvexCast::CastResult castResult;
+				castResult.m_fraction = 1.f;//??
+
+				ConvexShape* convexShape = (ConvexShape*) collisionObject->m_collisionShape;
+				VoronoiSimplexSolver	simplexSolver;
+				SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
+				//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
+				//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+				
+				if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,collisionObject->m_worldTransform,collisionObject->m_worldTransform,castResult))
+				{
+					//add hit
+					if (castResult.m_normal.length2() > 0.0001f)
+					{
+						castResult.m_normal.normalize();
+						if (castResult.m_fraction < resultCallback.m_closestHitFraction)
+						{
+							
+
+							CollisionWorld::LocalRayResult localRayResult
+								(
+									collisionObject, 
+									0,
+									castResult.m_normal,
+									castResult.m_fraction
+								);
+
+							resultCallback.AddSingleResult(localRayResult);
+
+						}
+					}
+				}
+			}
+			else
+			{
+				
+				if (collisionObject->m_collisionShape->IsConcave())
+					{
+
+						TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionObject->m_collisionShape;
+						
+						SimdTransform worldTocollisionObject = collisionObject->m_worldTransform.inverse();
+
+						SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+						SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+						//ConvexCast::CastResult
+
+						struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback 
+						{
+							RayResultCallback* m_resultCallback;
+							CollisionObject*	m_collisionObject;
+							TriangleMeshShape*	m_triangleMesh;
+
+							BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to,
+								RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape*	triangleMesh):
+								TriangleRaycastCallback(from,to),
+									m_resultCallback(resultCallback),
+									m_collisionObject(collisionObject),
+									m_triangleMesh(triangleMesh)
+								{
+								}
+
+
+							virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
+							{
+								CollisionWorld::LocalShapeInfo	shapeInfo;
+								shapeInfo.m_shapePart = partId;
+								shapeInfo.m_triangleIndex = triangleIndex;
+								
+								CollisionWorld::LocalRayResult rayResult
+								(m_collisionObject, 
+									&shapeInfo,
+									hitNormalLocal,
+									hitFraction);
+								
+								return m_resultCallback->AddSingleResult(rayResult);
+								
+								
+							}
+	
+						};
+
+
+						BridgeTriangleRaycastCallback	rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
+						rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+
+						SimdVector3 rayAabbMinLocal = rayFromLocal;
+						rayAabbMinLocal.setMin(rayToLocal);
+						SimdVector3 rayAabbMaxLocal = rayFromLocal;
+						rayAabbMaxLocal.setMax(rayToLocal);
+
+						triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
+											
+					}
+					
+
+			}
+		}
+	}
+
+}

Bullet/CollisionDispatch/CollisionWorld.h

@@ -0,0 +1,207 @@
+/*
+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 General 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 jam, http://www.perforce.com/jam/jam.html .
+ * 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 CcdPhysicsEnvironment , CollisionShape and RigidBody, Stepping the simulation and synchronizing your derived version of the PHY_IMotionState class.
+ * @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 CollisionWorld and CollisionObject, 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 GjkPairDetector.
+ *
+ * @section copyright Copyright
+ * Copyright (C) 2005-2006 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.
+ * 
+ */
+ 
+#ifndef COLLISION_WORLD_H
+#define COLLISION_WORLD_H
+
+
+class CollisionShape;
+class CollisionDispatcher;
+class BroadphaseInterface;
+#include "SimdVector3.h"
+#include "SimdTransform.h"
+#include "CollisionObject.h"
+
+#include <vector>
+
+///CollisionWorld is interface and container for the collision detection
+class CollisionWorld
+{
+
+	
+
+	std::vector<CollisionObject*>	m_collisionObjects;
+	
+	CollisionDispatcher*	m_dispatcher;
+
+	BroadphaseInterface*	m_broadphase;
+
+public:
+
+	CollisionWorld(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
+		:m_dispatcher(dispatcher),
+		m_broadphase(broadphase)
+	{
+
+	}
+	virtual ~CollisionWorld();
+
+	virtual	void	UpdateActivationState();
+
+	BroadphaseInterface*	GetBroadphase()
+	{
+		return m_broadphase;
+	}
+
+	CollisionDispatcher*	GetDispatcher()
+	{
+		return m_dispatcher;
+	}
+
+	///LocalShapeInfo gives extra information for complex shapes
+	///Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart
+	struct	LocalShapeInfo
+	{
+		int	m_shapePart;
+		int	m_triangleIndex;
+		
+		//const CollisionShape*	m_shapeTemp;
+		//const SimdTransform*	m_shapeLocalTransform;
+	};
+
+	struct	LocalRayResult
+	{
+		LocalRayResult(const CollisionObject*	collisionObject, 
+			LocalShapeInfo*	localShapeInfo,
+			const SimdVector3&		hitNormalLocal,
+			float hitFraction)
+		:m_collisionObject(collisionObject),
+		m_localShapeInfo(m_localShapeInfo),
+		m_hitNormalLocal(hitNormalLocal),
+		m_hitFraction(hitFraction)
+		{
+		}
+
+		const CollisionObject*	m_collisionObject;
+		LocalShapeInfo*			m_localShapeInfo;
+		const SimdVector3&		m_hitNormalLocal;
+		float					m_hitFraction;
+
+	};
+
+	///RayResultCallback is used to report new raycast results
+	struct	RayResultCallback
+	{
+		virtual ~RayResultCallback()
+		{
+		}
+		float	m_closestHitFraction;
+		bool	HasHit()
+		{
+			return (m_closestHitFraction < 1.f);
+		}
+
+		RayResultCallback()
+			:m_closestHitFraction(1.f)
+		{
+		}
+		virtual	float	AddSingleResult(const LocalRayResult& rayResult) = 0;
+	};
+
+	struct	ClosestRayResultCallback : public RayResultCallback
+	{
+		ClosestRayResultCallback(SimdVector3	rayFromWorld,SimdVector3	rayToWorld)
+		:m_rayFromWorld(rayFromWorld),
+		m_rayToWorld(rayToWorld),
+		m_collisionObject(0)
+		{
+		}
+
+		SimdVector3	m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
+		SimdVector3	m_rayToWorld;
+
+		SimdVector3	m_hitNormalWorld;
+		SimdVector3	m_hitPointWorld;
+		const CollisionObject*	m_collisionObject;
+		
+		virtual	float	AddSingleResult(const LocalRayResult& rayResult)
+		{
+
+//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;
+			m_hitNormalWorld = m_collisionObject->m_worldTransform.getBasis()*rayResult.m_hitNormalLocal;
+			m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+			return rayResult.m_hitFraction;
+		}
+	};
+
+
+	
+
+	int	GetNumCollisionObjects() const
+	{
+		return m_collisionObjects.size();
+	}
+
+	void	RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback);
+
+
+	void	AddCollisionObject(CollisionObject* collisionObject);
+
+	void	RemoveCollisionObject(CollisionObject* collisionObject);
+
+	virtual void	PerformDiscreteCollisionDetection();
+
+};
+
+
+#endif //COLLISION_WORLD_H

Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp

@@ -0,0 +1,234 @@
+/*
+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 "ConvexConcaveCollisionAlgorithm.h"
+#include "CollisionDispatch/CollisionObject.h"
+#include "CollisionShapes/MultiSphereShape.h"
+#include "CollisionShapes/BoxShape.h"
+#include "ConvexConvexAlgorithm.h"
+#include "BroadphaseCollision/BroadphaseProxy.h"
+#include "CollisionShapes/TriangleShape.h"
+#include "CollisionDispatch/ManifoldResult.h"
+#include "NarrowPhaseCollision/RaycastCallback.h"
+#include "CollisionShapes/TriangleMeshShape.h"
+
+
+ConvexConcaveCollisionAlgorithm::ConvexConcaveCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
+: CollisionAlgorithm(ci),m_convex(*proxy0),m_concave(*proxy1),
+m_boxTriangleCallback(ci.m_dispatcher,proxy0,proxy1)
+
+{
+}
+
+ConvexConcaveCollisionAlgorithm::~ConvexConcaveCollisionAlgorithm()
+{
+}
+
+
+
+BoxTriangleCallback::BoxTriangleCallback(Dispatcher*  dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1):
+  m_boxProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher),
+	m_dispatchInfoPtr(0)
+{
+
+	  //
+	  // create the manifold from the dispatcher 'manifold pool'
+	  //
+	  m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
+
+  	  ClearCache();
+}
+
+BoxTriangleCallback::~BoxTriangleCallback()
+{
+	ClearCache();
+	m_dispatcher->ReleaseManifold( m_manifoldPtr );
+  
+}
+  
+
+void	BoxTriangleCallback::ClearCache()
+{
+	m_dispatcher->ClearManifold(m_manifoldPtr);
+};
+
+
+
+void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex)
+{
+ 
+	//just for debugging purposes
+	//printf("triangle %d",m_triangleCount++);
+
+
+	//aabb filter is already applied!	
+
+	CollisionAlgorithmConstructionInfo ci;
+	ci.m_dispatcher = m_dispatcher;
+
+	
+
+	CollisionObject* colObj = static_cast<CollisionObject*>(m_boxProxy->m_clientObject);
+	
+	if (colObj->m_collisionShape->IsConvex())
+	{
+		TriangleShape tm(triangle[0],triangle[1],triangle[2]);	
+		tm.SetMargin(m_collisionMarginTriangle);
+	
+		CollisionObject* ob = static_cast<CollisionObject*>(m_triangleProxy.m_clientObject);
+
+		CollisionShape* tmpShape = ob->m_collisionShape;
+		ob->m_collisionShape = &tm;
+		
+		ConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_boxProxy,&m_triangleProxy);
+		cvxcvxalgo.ProcessCollision(m_boxProxy,&m_triangleProxy,*m_dispatchInfoPtr);
+		ob->m_collisionShape = tmpShape;
+
+	}
+
+	
+
+}
+
+
+
+void	BoxTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo)
+{
+	m_dispatchInfoPtr = &dispatchInfo;
+	m_collisionMarginTriangle = collisionMarginTriangle;
+
+	//recalc aabbs
+	CollisionObject* boxBody = (CollisionObject* )m_boxProxy->m_clientObject;
+	CollisionObject* triBody = (CollisionObject* )m_triangleProxy.m_clientObject;
+
+	SimdTransform boxInTriangleSpace;
+	boxInTriangleSpace = triBody->m_worldTransform.inverse() * boxBody->m_worldTransform;
+
+	CollisionShape* boxshape = static_cast<CollisionShape*>(boxBody->m_collisionShape);
+	//CollisionShape* triangleShape = static_cast<CollisionShape*>(triBody->m_collisionShape);
+
+	boxshape->GetAabb(boxInTriangleSpace,m_aabbMin,m_aabbMax);
+
+	float extraMargin = collisionMarginTriangle;//CONVEX_DISTANCE_MARGIN;//+0.1f;
+
+	SimdVector3 extra(extraMargin,extraMargin,extraMargin);
+
+	m_aabbMax += extra;
+	m_aabbMin -= extra;
+	
+}
+
+void ConvexConcaveCollisionAlgorithm::ClearCache()
+{
+	m_boxTriangleCallback.ClearCache();
+
+}
+
+void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
+{
+
+	CollisionObject* boxBody = static_cast<CollisionObject* >(m_convex.m_clientObject);
+	CollisionObject* triBody = static_cast<CollisionObject* >(m_concave.m_clientObject);
+
+	if (triBody->m_collisionShape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+	{
+
+		if (!m_dispatcher->NeedsCollision(m_convex,m_concave))
+			return;
+
+		
+
+		CollisionObject*	triOb = static_cast<CollisionObject*>(m_concave.m_clientObject);
+		TriangleMeshShape* triangleMesh = static_cast<TriangleMeshShape*>( triOb->m_collisionShape);
+		
+		if (boxBody->m_collisionShape->IsConvex())
+		{
+			float collisionMarginTriangle = triangleMesh->GetMargin();
+					
+			m_boxTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle,dispatchInfo);
+#ifdef USE_BOX_TRIANGLE
+			m_dispatcher->ClearManifold(m_boxTriangleCallback.m_manifoldPtr);
+#endif
+			m_boxTriangleCallback.m_manifoldPtr->SetBodies(m_convex.m_clientObject,m_concave.m_clientObject);
+
+			triangleMesh->ProcessAllTriangles( &m_boxTriangleCallback,m_boxTriangleCallback.GetAabbMin(),m_boxTriangleCallback.GetAabbMax());
+			
+	
+		}
+
+	}
+
+}
+
+
+float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
+{
+
+	//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the ConvexCast)
+	CollisionObject* convexbody = (CollisionObject* )m_convex.m_clientObject;
+	CollisionObject* triBody = static_cast<CollisionObject* >(m_concave.m_clientObject);
+
+	const SimdVector3& from = convexbody->m_worldTransform.getOrigin();
+	
+	SimdVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
+	//todo: only do if the motion exceeds the 'radius'
+
+	struct LocalTriangleRaycastCallback	: public TriangleRaycastCallback
+	{
+		LocalTriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to)
+			:TriangleRaycastCallback(from,to)
+		{
+		}
+		
+		virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
+		{
+			//todo: handle ccd here
+			return 0.f;
+
+		}
+	};
+
+
+	LocalTriangleRaycastCallback raycastCallback(from,to);
+
+	raycastCallback.m_hitFraction = convexbody->m_hitFraction;
+
+	SimdVector3 aabbMin (-1e30f,-1e30f,-1e30f);
+	SimdVector3 aabbMax (SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
+
+	if (triBody->m_collisionShape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+	{
+
+		CollisionObject* concavebody = (CollisionObject* )m_concave.m_clientObject;
+
+		TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->m_collisionShape;
+		
+		if (triangleMesh)
+		{
+			triangleMesh->ProcessAllTriangles(&raycastCallback,aabbMin,aabbMax);
+		}
+	}
+
+
+	if (raycastCallback.m_hitFraction < convexbody->m_hitFraction)
+	{
+		convexbody->m_hitFraction = raycastCallback.m_hitFraction;
+		return raycastCallback.m_hitFraction;
+	}
+
+	return 1.f;
+
+}

Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h

@@ -0,0 +1,95 @@
+/*
+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 "BroadphaseCollision/CollisionAlgorithm.h"
+#include "BroadphaseCollision/Dispatcher.h"
+#include "BroadphaseCollision/BroadphaseInterface.h"
+#include "CollisionShapes/TriangleCallback.h"
+#include "NarrowPhaseCollision/PersistentManifold.h"
+class Dispatcher;
+#include "BroadphaseCollision/BroadphaseProxy.h"
+
+
+
+class BoxTriangleCallback : public TriangleCallback
+{
+	BroadphaseProxy* m_boxProxy;
+	BroadphaseProxy m_triangleProxy;
+
+	SimdVector3	m_aabbMin;
+	SimdVector3	m_aabbMax ;
+
+	Dispatcher*	m_dispatcher;
+	const DispatcherInfo* m_dispatchInfoPtr;
+	float m_collisionMarginTriangle;
+	
+public:
+int	m_triangleCount;
+	
+	PersistentManifold*	m_manifoldPtr;
+
+	BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
+
+	void	SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo);
+
+	virtual ~BoxTriangleCallback();
+
+	virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex);
+	
+	void ClearCache();
+
+	inline const SimdVector3& GetAabbMin() const
+	{
+		return m_aabbMin;
+	}
+	inline const SimdVector3& GetAabbMax() const
+	{
+		return m_aabbMax;
+	}
+
+};
+
+
+
+
+/// ConvexConcaveCollisionAlgorithm  supports collision between convex shapes and (concave) trianges meshes.
+class ConvexConcaveCollisionAlgorithm  : public CollisionAlgorithm
+{
+
+	BroadphaseProxy m_convex;
+
+	BroadphaseProxy m_concave;
+
+	BoxTriangleCallback m_boxTriangleCallback;
+
+
+public:
+
+	ConvexConcaveCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
+
+	virtual ~ConvexConcaveCollisionAlgorithm();
+
+	virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
+
+	float	CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
+
+	void	ClearCache();
+
+};
+
+#endif //CONVEX_CONCAVE_COLLISION_ALGORITHM_H

Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp

@@ -0,0 +1,432 @@
+/*
+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 "ConvexConvexAlgorithm.h"
+
+#include <stdio.h>
+#include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h"
+#include "BroadphaseCollision/BroadphaseInterface.h"
+#include "CollisionDispatch/CollisionObject.h"
+#include "CollisionShapes/ConvexShape.h"
+#include "NarrowPhaseCollision/GjkPairDetector.h"
+#include "BroadphaseCollision/BroadphaseProxy.h"
+#include "CollisionDispatch/CollisionDispatcher.h"
+#include "CollisionShapes/BoxShape.h"
+#include "CollisionDispatch/ManifoldResult.h"
+
+#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h"
+#include "NarrowPhaseCollision/ContinuousConvexCollision.h"
+#include "NarrowPhaseCollision/SubSimplexConvexCast.h"
+#include "NarrowPhaseCollision/GjkConvexCast.h"
+
+
+
+#include "CollisionShapes/MinkowskiSumShape.h"
+#include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
+#include "CollisionShapes/SphereShape.h"
+
+#include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h"
+
+//#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h"
+
+#ifdef WIN32
+#if _MSC_VER >= 1310
+//only use SIMD Hull code under Win32
+#ifdef TEST_HULL
+#define USE_HULL 1
+#endif //TEST_HULL
+#endif //_MSC_VER 
+#endif //WIN32
+
+
+#ifdef USE_HULL
+
+#include "NarrowPhaseCollision/Hull.h"
+#include "NarrowPhaseCollision/HullContactCollector.h"
+
+
+#endif //USE_HULL
+
+bool gUseEpa = false;
+
+
+#ifdef WIN32
+void DrawRasterizerLine(const float* from,const float* to,int color);
+#endif
+
+
+
+
+//#define PROCESS_SINGLE_CONTACT
+#ifdef WIN32
+bool gForceBoxBox = false;//false;//true;
+
+#else
+bool gForceBoxBox = false;//false;//true;
+#endif
+bool gBoxBoxUseGjk = true;//true;//false;
+bool gDisableConvexCollision = false;
+
+
+
+ConvexConvexAlgorithm::ConvexConvexAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
+: CollisionAlgorithm(ci),
+m_gjkPairDetector(0,0,&m_simplexSolver,0),
+m_useEpa(!gUseEpa),
+m_box0(*proxy0),
+m_box1(*proxy1),
+m_ownManifold (false),
+m_manifoldPtr(mf),
+m_lowLevelOfDetail(false)
+{
+	CheckPenetrationDepthSolver();
+
+	{
+		if (!m_manifoldPtr && m_dispatcher->NeedsCollision(m_box0,m_box1))
+		{
+			m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
+			m_ownManifold = true;
+		}
+	}
+
+}
+
+
+
+ConvexConvexAlgorithm::~ConvexConvexAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->ReleaseManifold(m_manifoldPtr);
+	}
+}
+
+void	ConvexConvexAlgorithm ::SetLowLevelOfDetail(bool useLowLevel)
+{
+	m_lowLevelOfDetail = useLowLevel;
+}
+
+
+
+class FlippedContactResult : public DiscreteCollisionDetectorInterface::Result
+{
+	DiscreteCollisionDetectorInterface::Result* m_org;
+
+public:
+
+	FlippedContactResult(DiscreteCollisionDetectorInterface::Result* org)
+		: m_org(org)
+	{
+
+	}
+
+	virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+	{
+		SimdVector3 flippedNormal = -normalOnBInWorld;
+
+		m_org->AddContactPoint(flippedNormal,pointInWorld,depth);
+	}
+
+};
+
+static MinkowskiPenetrationDepthSolver	gPenetrationDepthSolver;
+
+//static EpaPenetrationDepthSolver	gEpaPenetrationDepthSolver;
+
+#ifdef USE_EPA
+Solid3EpaPenetrationDepth	gSolidEpaPenetrationSolver;
+#endif //USE_EPA
+
+void	ConvexConvexAlgorithm::CheckPenetrationDepthSolver()
+{
+	if (m_useEpa != gUseEpa)
+	{
+		m_useEpa  = gUseEpa;
+		if (m_useEpa)
+		{
+			
+		//	m_gjkPairDetector.SetPenetrationDepthSolver(&gEpaPenetrationDepthSolver);
+						
+			
+		} else
+		{
+			m_gjkPairDetector.SetPenetrationDepthSolver(&gPenetrationDepthSolver);
+		}
+	}
+	
+}
+
+#ifdef USE_HULL
+
+Transform	GetTransformFromSimdTransform(const SimdTransform& trans)
+{
+			//const SimdVector3& rowA0 = trans.getBasis().getRow(0);
+			////const SimdVector3& rowA1 = trans.getBasis().getRow(1);
+			//const SimdVector3& rowA2 = trans.getBasis().getRow(2);
+
+			SimdVector3 rowA0 = trans.getBasis().getColumn(0);
+			SimdVector3 rowA1 = trans.getBasis().getColumn(1);
+			SimdVector3 rowA2 = trans.getBasis().getColumn(2);
+
+
+			Vector3	x(rowA0.getX(),rowA0.getY(),rowA0.getZ());
+			Vector3	y(rowA1.getX(),rowA1.getY(),rowA1.getZ());
+			Vector3	z(rowA2.getX(),rowA2.getY(),rowA2.getZ());
+			
+			Matrix33 ornA(x,y,z);
+	
+			Point3 transA(
+				trans.getOrigin().getX(),
+				trans.getOrigin().getY(),
+				trans.getOrigin().getZ());
+
+			return Transform(ornA,transA);
+}
+
+class ManifoldResultCollector : public HullContactCollector
+{
+public:
+	ManifoldResult& m_manifoldResult;
+
+	ManifoldResultCollector(ManifoldResult& manifoldResult)
+		:m_manifoldResult(manifoldResult)
+	{
+
+	}
+	
+
+	virtual ~ManifoldResultCollector() {};
+
+	virtual int	BatchAddContactGroup(const Separation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths)
+	{
+		for (int i=0;i<numContacts;i++)
+		{
+			//printf("numContacts = %i\n",numContacts);
+			SimdVector3 normalOnBInWorld(sep.m_axis.GetX(),sep.m_axis.GetY(),sep.m_axis.GetZ());
+			//normalOnBInWorld.normalize();
+			SimdVector3 pointInWorld(positionsWorld[i].GetX(),positionsWorld[i].GetY(),positionsWorld[i].GetZ());
+			float depth = -depths[i];
+			m_manifoldResult.AddContactPoint(normalOnBInWorld,pointInWorld,depth);
+
+		}
+		return 0;
+	}
+
+	virtual int		GetMaxNumContacts() const
+	{
+		return 4;
+	}
+
+};
+#endif //USE_HULL
+
+
+//
+// Convex-Convex collision algorithm
+//
+void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
+{
+
+	if (!m_manifoldPtr)
+		return;
+
+	CheckPenetrationDepthSolver();
+
+//	printf("ConvexConvexAlgorithm::ProcessCollision\n");
+
+	bool needsCollision = m_dispatcher->NeedsCollision(m_box0,m_box1);
+	if (!needsCollision)
+		return;
+	
+	CollisionObject*	col0 = static_cast<CollisionObject*>(m_box0.m_clientObject);
+	CollisionObject*	col1 = static_cast<CollisionObject*>(m_box1.m_clientObject);
+
+#ifdef USE_HULL
+
+
+	if (dispatchInfo.m_enableSatConvex)
+	{
+		if ((col0->m_collisionShape->IsPolyhedral()) &&
+			(col1->m_collisionShape->IsPolyhedral()))
+		{
+		
+			
+			PolyhedralConvexShape* polyhedron0 = static_cast<PolyhedralConvexShape*>(col0->m_collisionShape);
+			PolyhedralConvexShape* polyhedron1 = static_cast<PolyhedralConvexShape*>(col1->m_collisionShape);
+			if (polyhedron0->m_optionalHull && polyhedron1->m_optionalHull)
+			{
+				//printf("Hull-Hull");
+
+				//todo: cache this information, rather then initialize
+				Separation sep;
+				sep.m_featureA = 0;
+				sep.m_featureB = 0;
+				sep.m_contact = -1;
+				sep.m_separator = 0;
+
+				//convert from SimdTransform to Transform
+				
+				Transform trA = GetTransformFromSimdTransform(col0->m_worldTransform);
+				Transform trB = GetTransformFromSimdTransform(col1->m_worldTransform);
+
+				//either use persistent manifold or clear it every time
+				m_dispatcher->ClearManifold(m_manifoldPtr);
+				ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr);
+
+				ManifoldResultCollector hullContactCollector(*resultOut);
+				
+				Hull::ProcessHullHull(sep,*polyhedron0->m_optionalHull,*polyhedron1->m_optionalHull,
+					trA,trB,&hullContactCollector);
+
+				
+				//user provided hull's, so we use SAT Hull collision detection
+				return;
+			}
+		}
+	}
+
+#endif //USE_HULL
+
+	
+	ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr);
+	
+	ConvexShape* min0 = static_cast<ConvexShape*>(col0->m_collisionShape);
+	ConvexShape* min1 = static_cast<ConvexShape*>(col1->m_collisionShape);
+	
+	GjkPairDetector::ClosestPointInput input;
+
+
+	//TODO: if (dispatchInfo.m_useContinuous)
+	m_gjkPairDetector.SetMinkowskiA(min0);
+	m_gjkPairDetector.SetMinkowskiB(min1);
+	input.m_maximumDistanceSquared = min0->GetMargin() + min1->GetMargin() + m_manifoldPtr->GetContactBreakingTreshold();
+	input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+	
+	input.m_maximumDistanceSquared = 1e30f;
+	
+	input.m_transformA = col0->m_worldTransform;
+	input.m_transformB = col1->m_worldTransform;
+    
+	m_gjkPairDetector.GetClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+	m_dispatcher->ReleaseManifoldResult(resultOut);
+}
+
+
+
+bool disableCcd = false;
+float	ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)
+{
+	///Rather then checking ALL pairs, only calculate TOI when motion exceeds treshold
+    
+	///Linear motion for one of objects needs to exceed m_ccdSquareMotionTreshold
+	///col0->m_worldTransform,
+	float resultFraction = 1.f;
+
+	CollisionObject* col1 = static_cast<CollisionObject*>(m_box1.m_clientObject);
+	CollisionObject* col0 = static_cast<CollisionObject*>(m_box0.m_clientObject);
+
+	float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2();
+	float squareMot1 = (col1->m_interpolationWorldTransform.getOrigin() - col1->m_worldTransform.getOrigin()).length2();
+    
+	if (squareMot0 < col0->m_ccdSquareMotionTreshold &&
+		squareMot0 < col0->m_ccdSquareMotionTreshold)
+		return resultFraction;
+
+
+
+	if (disableCcd)
+		return 1.f;
+
+	CheckPenetrationDepthSolver();
+
+	//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)
+
+	bool needsCollision = m_dispatcher->NeedsCollision(m_box0,m_box1);
+
+	if (!needsCollision)
+		return 1.f;
+	
+		
+	/// Convex0 against sphere for Convex1
+	{
+		ConvexShape* convex0 = static_cast<ConvexShape*>(col0->m_collisionShape);
+
+		SphereShape	sphere1(col1->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation
+		ConvexCast::CastResult result;
+		VoronoiSimplexSolver voronoiSimplex;
+		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+		///Simplification, one object is simplified as a sphere
+		GjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
+		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+		if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform,
+			col1->m_worldTransform,col1->m_interpolationWorldTransform,result))
+		{
+		
+			//store result.m_fraction in both bodies
+		
+			if (col0->m_hitFraction	> result.m_fraction)
+				col0->m_hitFraction  = result.m_fraction;
+
+			if (col1->m_hitFraction > result.m_fraction)
+				col1->m_hitFraction  = result.m_fraction;
+
+			if (resultFraction > result.m_fraction)
+				resultFraction = result.m_fraction;
+
+		}
+		
+		
+
+
+	}
+
+	/// Sphere (for convex0) against Convex1
+	{
+		ConvexShape* convex1 = static_cast<ConvexShape*>(col1->m_collisionShape);
+
+		SphereShape	sphere0(col0->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation
+		ConvexCast::CastResult result;
+		VoronoiSimplexSolver voronoiSimplex;
+		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+		///Simplification, one object is simplified as a sphere
+		GjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
+		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+		if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform,
+			col1->m_worldTransform,col1->m_interpolationWorldTransform,result))
+		{
+		
+			//store result.m_fraction in both bodies
+		
+			if (col0->m_hitFraction	> result.m_fraction)
+				col0->m_hitFraction  = result.m_fraction;
+
+			if (col1->m_hitFraction > result.m_fraction)
+				col1->m_hitFraction  = result.m_fraction;
+
+			if (resultFraction > result.m_fraction)
+				resultFraction = result.m_fraction;
+
+		}
+	}
+	
+	return resultFraction;
+
+}

Bullet/CollisionDispatch/ConvexConvexAlgorithm.h

@@ -0,0 +1,67 @@
+/*
+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 "BroadphaseCollision/CollisionAlgorithm.h"
+#include "NarrowPhaseCollision/GjkPairDetector.h"
+#include "NarrowPhaseCollision/PersistentManifold.h"
+#include "BroadphaseCollision/BroadphaseProxy.h"
+#include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
+
+class ConvexPenetrationDepthSolver;
+
+///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
+class ConvexConvexAlgorithm : public CollisionAlgorithm
+{
+	//ConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	VoronoiSimplexSolver	m_simplexSolver;
+	GjkPairDetector m_gjkPairDetector;
+	bool	m_useEpa;
+public:
+	BroadphaseProxy	m_box0;
+	BroadphaseProxy	m_box1;
+
+	bool	m_ownManifold;
+	PersistentManifold*	m_manifoldPtr;
+	bool			m_lowLevelOfDetail;
+
+	void	CheckPenetrationDepthSolver();
+
+	
+
+public:
+
+	ConvexConvexAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
+
+	virtual ~ConvexConvexAlgorithm();
+
+	virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
+
+	virtual float CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
+
+	void	SetLowLevelOfDetail(bool useLowLevel);
+
+	
+
+	const PersistentManifold*	GetManifold()
+	{
+		return m_manifoldPtr;
+	}
+
+};
+
+#endif //CONVEX_CONVEX_ALGORITHM_H

Bullet/CollisionDispatch/EmptyCollisionAlgorithm.cpp

@@ -0,0 +1,35 @@
+/*
+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 "EmptyCollisionAlgorithm.h"
+
+
+
+EmptyAlgorithm::EmptyAlgorithm(const CollisionAlgorithmConstructionInfo& ci)
+	: CollisionAlgorithm(ci)
+{
+}
+
+void EmptyAlgorithm::ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)
+{
+
+}
+
+float EmptyAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)
+{
+	return 1.f;
+}
+
+

Bullet/CollisionDispatch/EmptyCollisionAlgorithm.h

@@ -0,0 +1,40 @@
+/*
+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 "BroadphaseCollision/CollisionAlgorithm.h"
+
+#define ATTRIBUTE_ALIGNED(a)
+
+///EmptyAlgorithm is a stub for unsupported collision pairs.
+///The dispatcher can dispatch a persistent EmptyAlgorithm to avoid a search every frame.
+class EmptyAlgorithm : public CollisionAlgorithm
+{
+
+public:
+	
+	EmptyAlgorithm(const CollisionAlgorithmConstructionInfo& ci);
+
+	virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
+
+	virtual float CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
+
+
+
+
+} ATTRIBUTE_ALIGNED(16);
+
+#endif //EMPTY_ALGORITH

Bullet/CollisionDispatch/ManifoldResult.cpp

@@ -0,0 +1,58 @@
+/*
+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 "ManifoldResult.h"
+#include "NarrowPhaseCollision/PersistentManifold.h"
+#include "CollisionDispatch/CollisionObject.h"
+
+ManifoldResult::ManifoldResult(CollisionObject* body0,CollisionObject* body1,PersistentManifold* manifoldPtr)
+		:m_manifoldPtr(manifoldPtr),
+		m_body0(body0),
+		m_body1(body1)
+	{
+	}
+
+
+void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+{
+	if (depth > m_manifoldPtr->GetContactBreakingTreshold())
+		return;
+
+	SimdTransform transAInv = m_body0->m_worldTransform.inverse();
+	SimdTransform transBInv= m_body1->m_worldTransform.inverse();
+	SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+	SimdVector3 localA = transAInv(pointA );
+	SimdVector3 localB = transBInv(pointInWorld);
+	ManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+
+	
+
+	int insertIndex = m_manifoldPtr->GetCacheEntry(newPt);
+	if (insertIndex >= 0)
+	{
+
+// This is not needed, just use the old info!
+//		const ManifoldPoint& oldPoint = m_manifoldPtr->GetContactPoint(insertIndex);
+//		newPt.CopyPersistentInformation(oldPoint);
+//		m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex);
+
+
+	} else
+	{
+		m_manifoldPtr->AddManifoldPoint(newPt);
+	}
+}
+

Bullet/CollisionDispatch/ManifoldResult.h

@@ -0,0 +1,41 @@
+/*
+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
+
+#include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h"
+struct CollisionObject;
+class PersistentManifold;
+
+///ManifoldResult is a helper class to manage  contact results.
+class ManifoldResult : public DiscreteCollisionDetectorInterface::Result
+{
+	PersistentManifold* m_manifoldPtr;
+	CollisionObject* m_body0;
+	CollisionObject* m_body1;
+
+public:
+
+	ManifoldResult(CollisionObject* body0,CollisionObject* body1,PersistentManifold* manifoldPtr);
+
+	virtual ~ManifoldResult() {};
+
+	virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth);
+
+};
+
+#endif //MANIFOLD_RESULT_H

Bullet/CollisionDispatch/UnionFind.cpp

@@ -0,0 +1,100 @@
+/*
+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 "UnionFind.h"
+#include <assert.h>
+
+
+int UnionFind::find(int x)
+{ 
+	assert(x < m_N);
+	assert(x >= 0);
+
+	while (x != m_id[x]) 
+	{
+		x = m_id[x];
+		assert(x < m_N);
+		assert(x >= 0);
+
+	}
+	return x; 
+}
+
+UnionFind::~UnionFind()
+{
+	Free();
+
+}
+
+UnionFind::UnionFind()
+:m_id(0),
+m_sz(0),
+m_N(0)
+{ 
+
+}
+
+void	UnionFind::Allocate(int N)
+{
+	if (m_N < N)
+	{
+		Free();
+
+		m_N = N;
+		m_id = new int[N]; 
+		m_sz = new int[N];
+	}
+}
+void	UnionFind::Free()
+{
+	if (m_N)
+	{
+		m_N=0;
+		delete m_id;
+		delete m_sz;
+	}
+}
+
+
+void	UnionFind::reset(int N)
+{
+	Allocate(N);
+
+	for (int i = 0; i < m_N; i++) 
+	{ 
+		m_id[i] = i; m_sz[i] = 1; 
+	} 
+}
+
+
+int UnionFind ::find(int p, int q)
+{ 
+	return (find(p) == find(q)); 
+}
+
+void UnionFind ::unite(int p, int q)
+{
+	int i = find(p), j = find(q);
+	if (i == j) 
+		return;
+	if (m_sz[i] < m_sz[j])
+	{ 
+		m_id[i] = j; m_sz[j] += m_sz[i]; 
+	}
+	else 
+	{ 
+		m_id[j] = i; m_sz[i] += m_sz[j]; 
+	}
+}

Bullet/CollisionDispatch/UnionFind.h

@@ -0,0 +1,44 @@
+/*
+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
+
+///UnionFind calculates connected subsets
+class UnionFind
+  {
+    private:
+		int*	m_id;
+		int*	m_sz;
+		int m_N;
+
+    public:
+		int find(int x);
+	  
+		UnionFind();
+		~UnionFind();
+
+	  void	reset(int N);
+
+      int find(int p, int q);
+      void unite(int p, int q);
+
+	  void	Allocate(int N);
+	  void	Free();
+
+  };
+
+
+#endif //UNION_FIND_H