moved files around

.cvsignore

@@ -0,0 +1,22 @@
+*.exe
+*.ilk
+autom4te.cache
+bullet.pc
+config.cache
+config.h
+config.h.in~
+config.log
+config.status
+out
+Jamconfig
+Jamfile
+CcdPhysicsDemo
+CollisionDemo
+CollisionInterfaceDemo
+ConcaveDemo
+ConstraintDemo
+ContinuousConvexCollision
+ConvexHullDistance
+GjkConvexCastDemo
+Raytracer
+SimplexDemo

Bullet/BroadphaseCollision/AxisSweep3.cpp

@@ -0,0 +1,499 @@
+
+//Bullet Continuous Collision Detection and Physics Library
+//Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+
+//
+// AxisSweep3
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// 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 "AxisSweep3.h"
+
+#include <assert.h>
+
+BroadphaseProxy*	AxisSweep3::CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr )
+{
+		unsigned short handleId = AddHandle(min,max, userPtr);
+		
+		Handle* handle = GetHandle(handleId);
+		return handle;
+}
+
+void	AxisSweep3::DestroyProxy(BroadphaseProxy* proxy)
+{
+	Handle* handle = static_cast<Handle*>(proxy);
+	RemoveHandle(handle->m_handleId);
+}
+
+void	AxisSweep3::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax)
+{
+	Handle* handle = static_cast<Handle*>(proxy);
+	UpdateHandle(handle->m_handleId,aabbMin,aabbMax);
+}
+
+
+
+
+
+
+AxisSweep3::AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles, int maxOverlaps)
+{
+	//assert(bounds.HasVolume());
+
+	// 1 handle is reserved as sentinel
+	assert(maxHandles > 1 && maxHandles < 32767);
+
+	// doesn't need this limit right now, but I may want to use unsigned short indexes into overlaps array
+	assert(maxOverlaps > 0 && maxOverlaps < 65536);
+
+	// init bounds
+	m_worldAabbMin = worldAabbMin;
+	m_worldAabbMax = worldAabbMax;
+
+	SimdVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
+
+	m_quantize = SimdVector3(65535.0f,65535.0f,65535.0f) / aabbSize;
+
+	// allocate handles buffer and put all handles on free list
+	m_pHandles = new Handle[maxHandles];
+	m_maxHandles = maxHandles;
+	m_numHandles = 0;
+
+	// handle 0 is reserved as the null index, and is also used as the sentinel
+	m_firstFreeHandle = 1;
+	{
+		for (int i = m_firstFreeHandle; i < maxHandles; i++)
+			m_pHandles[i].SetNextFree(i + 1);
+		m_pHandles[maxHandles - 1].SetNextFree(0);
+	}
+
+	{
+	// allocate edge buffers
+	for (int i = 0; i < 3; i++)
+		m_pEdges[i] = new Edge[maxHandles * 2];
+	}
+	//removed overlap management
+
+	// make boundary sentinels
+	
+	m_pHandles[0].m_clientObject = 0;
+
+	for (int axis = 0; axis < 3; axis++)
+	{
+		m_pHandles[0].m_minEdges[axis] = 0;
+		m_pHandles[0].m_maxEdges[axis] = 1;
+
+		m_pEdges[axis][0].m_pos = 0;
+		m_pEdges[axis][0].m_handle = 0;
+		m_pEdges[axis][1].m_pos = 0xffff;
+		m_pEdges[axis][1].m_handle = 0;
+	}
+}
+
+AxisSweep3::~AxisSweep3()
+{
+	
+	for (int i = 2; i >= 0; i--)
+		delete[] m_pEdges[i];
+	delete[] m_pHandles;
+}
+
+void AxisSweep3::Quantize(unsigned short* out, const SimdPoint3& point, int isMax) const
+{
+	SimdPoint3 clampedPoint(point);
+	/*
+	if (isMax)
+		clampedPoint += SimdVector3(10,10,10);
+	else
+	{
+		clampedPoint -= SimdVector3(10,10,10);
+	}
+	*/
+
+
+	clampedPoint.setMax(m_worldAabbMin);
+	clampedPoint.setMin(m_worldAabbMax);
+
+	SimdVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
+	out[0] = (unsigned short)(((int)v.getX() & 0xfffc) | isMax);
+	out[1] = (unsigned short)(((int)v.getY() & 0xfffc) | isMax);
+	out[2] = (unsigned short)(((int)v.getZ() & 0xfffc) | isMax);
+	
+}
+
+
+
+unsigned short AxisSweep3::AllocHandle()
+{
+	assert(m_firstFreeHandle);
+
+	unsigned short handle = m_firstFreeHandle;
+	m_firstFreeHandle = GetHandle(handle)->GetNextFree();
+	m_numHandles++;
+
+	return handle;
+}
+
+void AxisSweep3::FreeHandle(unsigned short handle)
+{
+	assert(handle > 0 && handle < m_maxHandles);
+
+	GetHandle(handle)->SetNextFree(m_firstFreeHandle);
+	m_firstFreeHandle = handle;
+
+	m_numHandles--;
+}
+
+
+
+unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner)
+{
+	// quantize the bounds
+	unsigned short min[3], max[3];
+	Quantize(min, aabbMin, 0);
+	Quantize(max, aabbMax, 1);
+
+	// allocate a handle
+	unsigned short handle = AllocHandle();
+	assert(handle!= 0xcdcd);
+
+	Handle* pHandle = GetHandle(handle);
+	
+
+	pHandle->m_handleId = handle;
+	//pHandle->m_pOverlaps = 0;
+	pHandle->m_clientObject = pOwner;
+
+	// compute current limit of edge arrays
+	int limit = m_numHandles * 2;
+
+	// insert new edges just inside the max boundary edge
+	for (int axis = 0; axis < 3; axis++)
+	{
+		m_pHandles[0].m_maxEdges[axis] += 2;
+
+		m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
+
+		m_pEdges[axis][limit - 1].m_pos = min[axis];
+		m_pEdges[axis][limit - 1].m_handle = handle;
+
+		m_pEdges[axis][limit].m_pos = max[axis];
+		m_pEdges[axis][limit].m_handle = handle;
+
+		pHandle->m_minEdges[axis] = limit - 1;
+		pHandle->m_maxEdges[axis] = limit;
+	}
+
+	// now sort the new edges to their correct position
+	SortMinDown(0, pHandle->m_minEdges[0], false);
+	SortMaxDown(0, pHandle->m_maxEdges[0], false);
+	SortMinDown(1, pHandle->m_minEdges[1], false);
+	SortMaxDown(1, pHandle->m_maxEdges[1], false);
+	SortMinDown(2, pHandle->m_minEdges[2], true);
+	SortMaxDown(2, pHandle->m_maxEdges[2], true);
+
+	//PrintAxis(1);
+
+	return handle;
+}
+
+
+void AxisSweep3::RemoveHandle(unsigned short handle)
+{
+	Handle* pHandle = GetHandle(handle);
+
+	RemoveOverlappingPairsContainingProxy(pHandle);
+
+
+	// compute current limit of edge arrays
+	int limit = m_numHandles * 2;
+	int axis;
+
+	for (axis = 0;axis<3;axis++)
+	{
+		Edge* pEdges = m_pEdges[axis];
+		int maxEdge= pHandle->m_maxEdges[axis];
+		pEdges[maxEdge].m_pos = 0xffff;
+		int minEdge = pHandle->m_minEdges[axis];
+		pEdges[minEdge].m_pos = 0xffff;
+	}
+
+	// remove the edges by sorting them up to the end of the list
+	for ( axis = 0; axis < 3; axis++)
+	{
+		Edge* pEdges = m_pEdges[axis];
+		int max = pHandle->m_maxEdges[axis];
+		pEdges[max].m_pos = 0xffff;
+
+		SortMaxUp(axis,max,false);
+		
+		int i = pHandle->m_minEdges[axis];
+		pEdges[i].m_pos = 0xffff;
+
+		SortMinUp(axis,i,false);
+
+		pEdges[limit-1].m_handle = 0;
+		pEdges[limit-1].m_pos = 0xffff;
+
+	}
+
+	// free the handle
+	FreeHandle(handle);
+
+	
+}
+
+bool AxisSweep3::TestOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB)
+{
+	//optimization 1: check the array index (memory address), instead of the m_pos
+
+	for (int axis = 0; axis < 3; axis++)
+	{ 
+		if (axis != ignoreAxis)
+		{
+			if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
+				pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
+			{ 
+				return false; 
+			} 
+		}
+	} 
+
+	//optimization 2: only 2 axis need to be tested
+
+	/*for (int axis = 0; axis < 3; axis++)
+	{
+		if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos ||
+			m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos)
+		{
+			return false;
+		}
+	}
+	*/
+
+	return true;
+}
+
+void AxisSweep3::UpdateHandle(unsigned short handle, const SimdPoint3& aabbMin,const SimdPoint3& aabbMax)
+{
+//	assert(bounds.IsFinite());
+	//assert(bounds.HasVolume());
+
+	Handle* pHandle = GetHandle(handle);
+
+	// quantize the new bounds
+	unsigned short min[3], max[3];
+	Quantize(min, aabbMin, 0);
+	Quantize(max, aabbMax, 1);
+
+	// update changed edges
+	for (int axis = 0; axis < 3; axis++)
+	{
+		unsigned short emin = pHandle->m_minEdges[axis];
+		unsigned short emax = pHandle->m_maxEdges[axis];
+
+		int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
+		int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
+
+		m_pEdges[axis][emin].m_pos = min[axis];
+		m_pEdges[axis][emax].m_pos = max[axis];
+
+		// expand (only adds overlaps)
+		if (dmin < 0)
+			SortMinDown(axis, emin);
+
+		if (dmax > 0)
+			SortMaxUp(axis, emax);
+
+		// shrink (only removes overlaps)
+		if (dmin > 0)
+			SortMinUp(axis, emin);
+
+		if (dmax < 0)
+			SortMaxDown(axis, emax);
+	}
+
+	//PrintAxis(1);
+}
+
+// sorting a min edge downwards can only ever *add* overlaps
+void AxisSweep3::SortMinDown(int axis, unsigned short edge, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pPrev = pEdge - 1;
+	Handle* pHandleEdge = GetHandle(pEdge->m_handle);
+
+	while (pEdge->m_pos < pPrev->m_pos)
+	{
+		Handle* pHandlePrev = GetHandle(pPrev->m_handle);
+
+		if (pPrev->IsMax())
+		{
+			// if previous edge is a maximum check the bounds and add an overlap if necessary
+			if (updateOverlaps && TestOverlap(axis,pHandleEdge, pHandlePrev))
+			{
+				AddOverlappingPair(pHandleEdge,pHandlePrev);
+
+				//AddOverlap(pEdge->m_handle, pPrev->m_handle);
+
+			}
+
+			// update edge reference in other handle
+			pHandlePrev->m_maxEdges[axis]++;
+		}
+		else
+			pHandlePrev->m_minEdges[axis]++;
+
+		pHandleEdge->m_minEdges[axis]--;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pPrev;
+		*pPrev = swap;
+
+		// decrement
+		pEdge--;
+		pPrev--;
+	}
+}
+
+// sorting a min edge upwards can only ever *remove* overlaps
+void AxisSweep3::SortMinUp(int axis, unsigned short edge, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pNext = pEdge + 1;
+	Handle* pHandleEdge = GetHandle(pEdge->m_handle);
+
+	while (pEdge->m_pos > pNext->m_pos)
+	{
+		Handle* pHandleNext = GetHandle(pNext->m_handle);
+
+		if (pNext->IsMax())
+		{
+			// if next edge is maximum remove any overlap between the two handles
+			if (updateOverlaps)
+			{
+				Handle* handle0 = GetHandle(pEdge->m_handle);
+				Handle* handle1 = GetHandle(pNext->m_handle);
+				BroadphasePair* pair = FindPair(handle0,handle1);
+				//assert(pair);
+				if (pair)
+				{
+					RemoveOverlappingPair(*pair);
+				}
+			}
+
+			// update edge reference in other handle
+			pHandleNext->m_maxEdges[axis]--;
+		}
+		else
+			pHandleNext->m_minEdges[axis]--;
+
+		pHandleEdge->m_minEdges[axis]++;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pNext;
+		*pNext = swap;
+
+		// increment
+		pEdge++;
+		pNext++;
+	}
+}
+
+// sorting a max edge downwards can only ever *remove* overlaps
+void AxisSweep3::SortMaxDown(int axis, unsigned short edge, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pPrev = pEdge - 1;
+	Handle* pHandleEdge = GetHandle(pEdge->m_handle);
+
+	while (pEdge->m_pos < pPrev->m_pos)
+	{
+		Handle* pHandlePrev = GetHandle(pPrev->m_handle);
+
+		if (!pPrev->IsMax())
+		{
+			// if previous edge was a minimum remove any overlap between the two handles
+			if (updateOverlaps)
+			{
+				Handle* handle0 = GetHandle(pEdge->m_handle);
+				Handle* handle1 = GetHandle(pPrev->m_handle);
+				BroadphasePair* pair = FindPair(handle0,handle1);
+				//assert(pair);
+
+				if (pair)
+				{
+					RemoveOverlappingPair(*pair);
+				}
+			}
+
+			// update edge reference in other handle
+			pHandlePrev->m_minEdges[axis]++;;
+		}
+		else
+			pHandlePrev->m_maxEdges[axis]++;
+
+		pHandleEdge->m_maxEdges[axis]--;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pPrev;
+		*pPrev = swap;
+
+		// decrement
+		pEdge--;
+		pPrev--;
+	}
+}
+
+// sorting a max edge upwards can only ever *add* overlaps
+void AxisSweep3::SortMaxUp(int axis, unsigned short edge, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pNext = pEdge + 1;
+	Handle* pHandleEdge = GetHandle(pEdge->m_handle);
+
+	while (pEdge->m_pos > pNext->m_pos)
+	{
+		Handle* pHandleNext = GetHandle(pNext->m_handle);
+
+		if (!pNext->IsMax())
+		{
+			// if next edge is a minimum check the bounds and add an overlap if necessary
+			if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext))
+			{
+				Handle* handle0 = GetHandle(pEdge->m_handle);
+				Handle* handle1 = GetHandle(pNext->m_handle);
+				AddOverlappingPair(handle0,handle1);
+			}
+
+			// update edge reference in other handle
+			pHandleNext->m_minEdges[axis]--;
+		}
+		else
+			pHandleNext->m_maxEdges[axis]--;
+
+		pHandleEdge->m_maxEdges[axis]++;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pNext;
+		*pNext = swap;
+
+		// increment
+		pEdge++;
+		pNext++;
+	}
+}

Bullet/BroadphaseCollision/AxisSweep3.h

@@ -0,0 +1,115 @@
+//Bullet Continuous Collision Detection and Physics Library
+//Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+//
+// AxisSweep3.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// 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 AXIS_SWEEP_3_H
+#define AXIS_SWEEP_3_H
+
+#include "SimdPoint3.h"
+#include "SimdVector3.h"
+#include "SimpleBroadphase.h"
+#include "BroadphaseProxy.h"
+
+/// AxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
+/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using integer coordinates instead of floats.
+/// The TestOverlap check is optimized to check the array index, rather then the actual AABB coordinates/pos
+class AxisSweep3 : public SimpleBroadphase
+{
+
+public:
+	
+
+	class Edge
+	{
+	public:
+		unsigned short m_pos;			// low bit is min/max
+		unsigned short m_handle;
+
+		unsigned short IsMax() const {return m_pos & 1;}
+	};
+
+public:
+	class Handle : public BroadphaseProxy
+	{
+	public:
+		
+		// indexes into the edge arrays
+		unsigned short m_minEdges[3], m_maxEdges[3];		// 6 * 2 = 12
+		unsigned short m_handleId;
+		unsigned short m_pad;
+		
+		//void* m_pOwner; this is now in BroadphaseProxy.m_clientObject
+	
+		inline void SetNextFree(unsigned short next) {m_minEdges[0] = next;}
+		inline unsigned short GetNextFree() const {return m_minEdges[0];}
+	};		// 24 bytes + 24 for Edge structures = 44 bytes total per entry
+
+	
+private:
+	SimdPoint3 m_worldAabbMin;						// overall system bounds
+	SimdPoint3 m_worldAabbMax;						// overall system bounds
+
+	SimdVector3 m_quantize;						// scaling factor for quantization
+
+	int m_numHandles;						// number of active handles
+	int m_maxHandles;						// max number of handles
+	Handle* m_pHandles;						// handles pool
+	unsigned short m_firstFreeHandle;		// free handles list
+
+	Edge* m_pEdges[3];						// edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
+
+
+	// allocation/deallocation
+	unsigned short AllocHandle();
+	void FreeHandle(unsigned short handle);
+	
+
+	bool TestOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB);
+
+	//Overlap* AddOverlap(unsigned short handleA, unsigned short handleB);
+	//void RemoveOverlap(unsigned short handleA, unsigned short handleB);
+
+	void Quantize(unsigned short* out, const SimdPoint3& point, int isMax) const;
+
+	void SortMinDown(int axis, unsigned short edge, bool updateOverlaps = true);
+	void SortMinUp(int axis, unsigned short edge, bool updateOverlaps = true);
+	void SortMaxDown(int axis, unsigned short edge, bool updateOverlaps = true);
+	void SortMaxUp(int axis, unsigned short edge, bool updateOverlaps = true);
+
+public:
+	AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles = 1024, int maxOverlaps = 8192);
+	virtual	~AxisSweep3();
+
+	virtual void	RefreshOverlappingPairs()
+	{
+		//this is replace by sweep and prune
+	}
+	
+	unsigned short AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner);
+	void RemoveHandle(unsigned short handle);
+	void UpdateHandle(unsigned short handle, const SimdPoint3& aabbMin,const SimdPoint3& aabbMax);
+	inline Handle* GetHandle(unsigned short index) const {return m_pHandles + index;}
+
+
+	//Broadphase Interface
+	virtual BroadphaseProxy*	CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr );
+	virtual void	DestroyProxy(BroadphaseProxy* proxy);
+	virtual void	SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax);
+
+};
+
+#endif //AXIS_SWEEP_3_H

Bullet/BroadphaseCollision/BroadphaseInterface.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		BROADPHASE_INTERFACE_H
+#define 	BROADPHASE_INTERFACE_H
+
+
+
+struct DispatcherInfo;
+class Dispatcher;
+struct BroadphaseProxy;
+#include "SimdVector3.h"
+
+///BroadphaseInterface for aabb-overlapping object pairs
+class BroadphaseInterface
+{
+public:
+	virtual ~BroadphaseInterface() {}
+
+	virtual BroadphaseProxy*	CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr ) =0;
+	virtual void	DestroyProxy(BroadphaseProxy* proxy)=0;
+	virtual void	SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax)=0;
+	virtual void	CleanProxyFromPairs(BroadphaseProxy* proxy)=0;
+	virtual void	DispatchAllCollisionPairs(Dispatcher&	dispatcher,DispatcherInfo& dispatchInfo)=0;
+
+};
+
+#endif //BROADPHASE_INTERFACE_H

Bullet/BroadphaseCollision/BroadphaseProxy.cpp

@@ -0,0 +1,17 @@
+/*
+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 "BroadphaseProxy.h"
+

Bullet/BroadphaseCollision/BroadphaseProxy.h

@@ -0,0 +1,116 @@
+/*
+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 BROADPHASE_PROXY_H
+#define BROADPHASE_PROXY_H
+
+
+
+/// Dispatcher uses these types
+/// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
+/// to facilitate type checking
+enum BroadphaseNativeTypes
+{
+// polyhedral convex shapes
+	BOX_SHAPE_PROXYTYPE,
+	TRIANGLE_SHAPE_PROXYTYPE,
+	TETRAHEDRAL_SHAPE_PROXYTYPE,
+	CONVEX_HULL_SHAPE_PROXYTYPE,
+//implicit convex shapes
+IMPLICIT_CONVEX_SHAPES_START_HERE,
+	SPHERE_SHAPE_PROXYTYPE,
+	MULTI_SPHERE_SHAPE_PROXYTYPE,
+	CONE_SHAPE_PROXYTYPE,
+	CONVEX_SHAPE_PROXYTYPE,
+	CYLINDER_SHAPE_PROXYTYPE,
+	MINKOWSKI_SUM_SHAPE_PROXYTYPE,
+	MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE,
+//concave shapes
+CONCAVE_SHAPES_START_HERE,
+	//keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
+	TRIANGLE_MESH_SHAPE_PROXYTYPE,
+	EMPTY_SHAPE_PROXYTYPE,
+
+	MAX_BROADPHASE_COLLISION_TYPES
+};
+
+
+///BroadphaseProxy
+struct BroadphaseProxy
+{
+	
+	//Usually the client CollisionObject or Rigidbody class
+	void*	m_clientObject;
+
+
+	BroadphaseProxy() :m_clientObject(0){}
+	BroadphaseProxy(int shapeType,void* userPtr)
+		:m_clientObject(userPtr)
+		//m_clientObjectType(shapeType)
+	{
+	}
+	
+};
+
+class CollisionAlgorithm;
+
+struct BroadphaseProxy;
+
+#define SIMPLE_MAX_ALGORITHMS 2
+
+/// contains a pair of aabb-overlapping objects
+struct BroadphasePair
+{
+	BroadphasePair ()
+		:
+	m_pProxy0(0),
+		m_pProxy1(0)
+	{
+		for (int i=0;i<SIMPLE_MAX_ALGORITHMS;i++)
+		{
+			m_algorithms[i] = 0;
+		}
+	}
+
+	BroadphasePair(const BroadphasePair& other)
+		:		m_pProxy0(other.m_pProxy0),
+				m_pProxy1(other.m_pProxy1)
+	{
+		for (int i=0;i<SIMPLE_MAX_ALGORITHMS;i++)
+		{
+			m_algorithms[i] = other.m_algorithms[i];
+		}
+	}
+	BroadphasePair(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
+		:
+		m_pProxy0(&proxy0),
+		m_pProxy1(&proxy1)
+	{
+		for (int i=0;i<SIMPLE_MAX_ALGORITHMS;i++)
+	{
+			m_algorithms[i] = 0;
+		}
+
+	}
+
+	
+	BroadphaseProxy* m_pProxy0;
+	BroadphaseProxy* m_pProxy1;
+	
+	mutable CollisionAlgorithm* m_algorithms[SIMPLE_MAX_ALGORITHMS];
+};
+
+#endif //BROADPHASE_PROXY_H
+

Bullet/BroadphaseCollision/CollisionAlgorithm.cpp

@@ -0,0 +1,23 @@
+/*
+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 "CollisionAlgorithm.h"
+#include "Dispatcher.h"
+
+CollisionAlgorithm::CollisionAlgorithm(const CollisionAlgorithmConstructionInfo& ci)
+{
+	m_dispatcher = ci.m_dispatcher;
+}
+

Bullet/BroadphaseCollision/CollisionAlgorithm.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 COLLISION_ALGORITHM_H
+#define COLLISION_ALGORITHM_H
+
+struct BroadphaseProxy;
+class Dispatcher;
+
+struct CollisionAlgorithmConstructionInfo
+{
+	CollisionAlgorithmConstructionInfo()
+		:m_dispatcher(0)
+	{
+	}
+	CollisionAlgorithmConstructionInfo(Dispatcher* dispatcher,int temp)
+		:m_dispatcher(dispatcher)
+	{
+	}
+
+	Dispatcher*	m_dispatcher;
+
+	int	GetDispatcherId();
+
+};
+
+
+///CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher.
+///It is persistent over frames
+class CollisionAlgorithm
+{
+
+protected:
+
+	Dispatcher*	m_dispatcher;
+
+protected:
+	int	GetDispatcherId();
+	
+public:
+
+	CollisionAlgorithm() {};
+
+	CollisionAlgorithm(const CollisionAlgorithmConstructionInfo& ci);
+
+	virtual ~CollisionAlgorithm() {};
+
+	virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const struct DispatcherInfo& dispatchInfo) = 0;
+
+	virtual float CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const struct DispatcherInfo& dispatchInfo) = 0;
+
+};
+
+
+#endif //COLLISION_ALGORITHM_H

Bullet/BroadphaseCollision/Dispatcher.cpp

@@ -0,0 +1,22 @@
+/*
+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 "Dispatcher.h"
+
+Dispatcher::~Dispatcher()
+{
+
+}
+

Bullet/BroadphaseCollision/Dispatcher.h

@@ -0,0 +1,91 @@
+/*
+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 _DISPATCHER_H
+#define _DISPATCHER_H
+
+class CollisionAlgorithm;
+struct BroadphaseProxy;
+class RigidBody;
+struct	CollisionObject;
+class ManifoldResult;
+
+enum CollisionDispatcherId
+{
+	RIGIDBODY_DISPATCHER = 0,
+	USERCALLBACK_DISPATCHER
+};
+
+class PersistentManifold;
+
+struct DispatcherInfo
+{
+	enum DispatchFunc
+	{
+		DISPATCH_DISCRETE = 1,
+		DISPATCH_CONTINUOUS
+	};
+	DispatcherInfo()
+		:m_dispatchFunc(DISPATCH_DISCRETE),
+		m_timeOfImpact(1.f),
+		m_useContinuous(false),
+		m_debugDraw(0),
+		m_enableSatConvex(false)
+	{
+
+	}
+	float	m_timeStep;
+	int		m_stepCount;
+	int		m_dispatchFunc;
+	float	m_timeOfImpact;
+	bool	m_useContinuous;
+	class IDebugDraw*	m_debugDraw;
+	bool	m_enableSatConvex;
+	
+};
+
+/// Dispatcher can be used in combination with broadphase to dispatch overlapping pairs.
+/// For example for pairwise collision detection or user callbacks (game logic).
+class Dispatcher
+{
+
+
+public:
+	virtual ~Dispatcher() ;
+
+	virtual CollisionAlgorithm* FindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) = 0;
+
+	//
+	// asume dispatchers to have unique id's in the range [0..max dispacher]
+	//
+	virtual int GetUniqueId() = 0;
+
+	virtual PersistentManifold*	GetNewManifold(void* body0,void* body1)=0;
+
+	virtual void ReleaseManifold(PersistentManifold* manifold)=0;
+
+	virtual void ClearManifold(PersistentManifold* manifold)=0;
+
+	virtual bool	NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) = 0;
+
+	virtual	ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold) =0;
+
+	virtual	void	ReleaseManifoldResult(ManifoldResult*)=0;
+
+
+};
+
+
+#endif //_DISPATCHER_H

Bullet/BroadphaseCollision/SimpleBroadphase.cpp

@@ -0,0 +1,348 @@
+/*
+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 "SimpleBroadphase.h"
+#include "BroadphaseCollision/Dispatcher.h"
+#include "BroadphaseCollision/CollisionAlgorithm.h"
+
+#include "SimdVector3.h"
+#include "SimdTransform.h"
+#include "SimdMatrix3x3.h"
+#include <vector>
+
+
+void	SimpleBroadphase::validate()
+{
+	for (int i=0;i<m_numProxies;i++)
+	{
+		for (int j=i+1;j<m_numProxies;j++)
+		{
+			assert(m_pProxies[i] != m_pProxies[j]);
+		}
+	}
+	
+}
+
+SimpleBroadphase::SimpleBroadphase(int maxProxies,int maxOverlap)
+	:m_firstFreeProxy(0),
+	m_numProxies(0),
+	m_blockedForChanges(false),
+	m_NumOverlapBroadphasePair(0),
+	m_maxProxies(maxProxies),
+	m_maxOverlap(maxOverlap)
+{
+
+	m_proxies = new SimpleBroadphaseProxy[maxProxies];
+	m_freeProxies = new int[maxProxies];
+	m_pProxies = new SimpleBroadphaseProxy*[maxProxies];
+	m_OverlappingPairs = new BroadphasePair[maxOverlap];
+
+
+	int i;
+	for (i=0;i<m_maxProxies;i++)
+	{
+		m_freeProxies[i] = i;
+	}
+}
+
+SimpleBroadphase::~SimpleBroadphase()
+{
+	delete[] m_proxies;
+	delete []m_freeProxies;
+	delete [] m_pProxies;
+	delete [] m_OverlappingPairs;
+
+	/*int i;
+	for (i=m_numProxies-1;i>=0;i--)
+	{
+		BP_Proxy* proxy = m_pProxies[i]; 
+		destroyProxy(proxy);
+	}
+	*/
+}
+
+
+BroadphaseProxy*	SimpleBroadphase::CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr)
+{
+	if (m_numProxies >= m_maxProxies)
+	{
+		assert(0);
+		return 0; //should never happen, but don't let the game crash ;-)
+	}
+	assert(min[0]<= max[0] && min[1]<= max[1] && min[2]<= max[2]);
+
+	int freeIndex= m_freeProxies[m_firstFreeProxy];
+	SimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(min,max,shapeType,userPtr);
+	m_firstFreeProxy++;
+
+	SimpleBroadphaseProxy* proxy1 = &m_proxies[0];
+		
+	int index = proxy - proxy1;
+	assert(index == freeIndex);
+
+	m_pProxies[m_numProxies] = proxy;
+	m_numProxies++;
+	//validate();
+
+	return proxy;
+}
+
+void	SimpleBroadphase::RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy)
+{
+	int i;
+		for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--)
+		{
+			BroadphasePair& pair = m_OverlappingPairs[i];
+			if (pair.m_pProxy0 == proxy ||
+					pair.m_pProxy1 == proxy)
+			{
+				RemoveOverlappingPair(pair);
+			}
+		}
+}
+
+void	SimpleBroadphase::DestroyProxy(BroadphaseProxy* proxyOrg)
+{
+		
+		int i;
+		
+		SimpleBroadphaseProxy* proxy0 = static_cast<SimpleBroadphaseProxy*>(proxyOrg);
+		SimpleBroadphaseProxy* proxy1 = &m_proxies[0];
+	
+		int index = proxy0 - proxy1;
+		assert (index < m_maxProxies);
+		m_freeProxies[--m_firstFreeProxy] = index;
+
+		RemoveOverlappingPairsContainingProxy(proxyOrg);
+
+		
+		for (i=0;i<m_numProxies;i++)
+		{
+			if (m_pProxies[i] == proxyOrg)
+			{
+				m_pProxies[i] = m_pProxies[m_numProxies-1];
+				break;
+			}
+		}
+		m_numProxies--;
+		//validate();
+		
+}
+
+void	SimpleBroadphase::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax)
+{
+	SimpleBroadphaseProxy* sbp = GetSimpleProxyFromProxy(proxy);
+	sbp->m_min = aabbMin;
+	sbp->m_max = aabbMax;
+}
+
+void	SimpleBroadphase::CleanOverlappingPair(BroadphasePair& pair)
+{
+	for (int dispatcherId=0;dispatcherId<SIMPLE_MAX_ALGORITHMS;dispatcherId++)
+	{
+		if (pair.m_algorithms[dispatcherId])
+		{
+			{
+				delete pair.m_algorithms[dispatcherId];
+				pair.m_algorithms[dispatcherId]=0;
+			}
+		}
+	}
+}
+
+
+void	SimpleBroadphase::CleanProxyFromPairs(BroadphaseProxy* proxy)
+{
+	for (int i=0;i<m_NumOverlapBroadphasePair;i++)
+	{
+		BroadphasePair& pair = m_OverlappingPairs[i];
+		if (pair.m_pProxy0 == proxy ||
+				pair.m_pProxy1 == proxy)
+		{
+			CleanOverlappingPair(pair);
+		}
+	}
+}
+
+void	SimpleBroadphase::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
+{
+	//don't add overlap with own
+	assert(proxy0 != proxy1);
+
+	BroadphasePair pair(*proxy0,*proxy1);
+	m_OverlappingPairs[m_NumOverlapBroadphasePair] = pair;
+
+	int i;
+	for (i=0;i<SIMPLE_MAX_ALGORITHMS;i++)
+	{
+		assert(!m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i]);
+		m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i] = 0;
+	}
+
+	if (m_NumOverlapBroadphasePair >= m_maxOverlap)
+	{
+		//printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair);
+#ifdef DEBUG
+		assert(0);
+#endif 
+	} else
+	{
+		m_NumOverlapBroadphasePair++;
+	}
+
+	
+}
+	
+BroadphasePair*	SimpleBroadphase::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
+{
+	BroadphasePair* foundPair = 0;
+
+	int i;
+	for (i=m_NumOverlapBroadphasePair-1;i>=0;i--)
+	{
+		BroadphasePair& pair = m_OverlappingPairs[i];
+		if (((pair.m_pProxy0 == proxy0) && (pair.m_pProxy1 == proxy1)) ||
+			((pair.m_pProxy0 == proxy1) && (pair.m_pProxy1 == proxy0)))
+		{
+			foundPair = &pair;
+			return foundPair;
+		}
+	}	
+
+	return foundPair;
+}
+void	SimpleBroadphase::RemoveOverlappingPair(BroadphasePair& pair)
+{
+    CleanOverlappingPair(pair);
+	int	index = &pair - &m_OverlappingPairs[0];
+	//remove efficiently, swap with the last
+	m_OverlappingPairs[index] = m_OverlappingPairs[m_NumOverlapBroadphasePair-1];
+	m_NumOverlapBroadphasePair--;
+}
+
+bool	SimpleBroadphase::AabbOverlap(SimpleBroadphaseProxy* proxy0,SimpleBroadphaseProxy* proxy1)
+{
+	return proxy0->m_min[0] <= proxy1->m_max[0] && proxy1->m_min[0] <= proxy0->m_max[0] && 
+		   proxy0->m_min[1] <= proxy1->m_max[1] && proxy1->m_min[1] <= proxy0->m_max[1] &&
+		   proxy0->m_min[2] <= proxy1->m_max[2] && proxy1->m_min[2] <= proxy0->m_max[2];
+
+}
+void	SimpleBroadphase::RefreshOverlappingPairs()
+{
+	//first check for new overlapping pairs
+	int i,j;
+
+	for (i=0;i<m_numProxies;i++)
+	{
+		BroadphaseProxy* proxy0 = m_pProxies[i];
+		for (j=i+1;j<m_numProxies;j++)
+		{
+			BroadphaseProxy* proxy1 = m_pProxies[j];
+			SimpleBroadphaseProxy* p0 = GetSimpleProxyFromProxy(proxy0);
+			SimpleBroadphaseProxy* p1 = GetSimpleProxyFromProxy(proxy1);
+
+			if (AabbOverlap(p0,p1))
+			{
+				if ( !FindPair(proxy0,proxy1))
+				{
+					AddOverlappingPair(proxy0,proxy1);
+				}
+			}
+
+		}
+	}
+
+	//then remove non-overlapping ones
+	for (i=0;i<m_NumOverlapBroadphasePair;i++)
+	{
+		BroadphasePair& pair = m_OverlappingPairs[i];
+		SimpleBroadphaseProxy* proxy0 = GetSimpleProxyFromProxy(pair.m_pProxy0);
+		SimpleBroadphaseProxy* proxy1 = GetSimpleProxyFromProxy(pair.m_pProxy1);
+		if (!AabbOverlap(proxy0,proxy1))
+		{
+            RemoveOverlappingPair(pair);
+		}
+	}
+
+	
+
+}
+
+void	SimpleBroadphase::DispatchAllCollisionPairs(Dispatcher&	dispatcher,DispatcherInfo& dispatchInfo)
+{
+	m_blockedForChanges = true;
+
+	int i;
+	
+	int dispatcherId = dispatcher.GetUniqueId();
+
+	RefreshOverlappingPairs();
+
+	for (i=0;i<m_NumOverlapBroadphasePair;i++)
+	{
+		
+		BroadphasePair& pair = m_OverlappingPairs[i];
+		
+		if (dispatcherId>= 0)
+		{
+			//dispatcher will keep algorithms persistent in the collision pair
+			if (!pair.m_algorithms[dispatcherId])
+			{
+				pair.m_algorithms[dispatcherId] = dispatcher.FindAlgorithm(
+					*pair.m_pProxy0,
+					*pair.m_pProxy1);
+			}
+
+			if (pair.m_algorithms[dispatcherId])
+			{
+				if (dispatchInfo.m_dispatchFunc == 		DispatcherInfo::DISPATCH_DISCRETE)
+				{
+					pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+				} else
+				{
+					float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+					if (dispatchInfo.m_timeOfImpact > toi)
+						dispatchInfo.m_timeOfImpact = toi;
+
+				}
+			}
+		} else
+		{
+			//non-persistent algorithm dispatcher
+				CollisionAlgorithm* algo = dispatcher.FindAlgorithm(
+					*pair.m_pProxy0,
+					*pair.m_pProxy1);
+
+				if (algo)
+				{
+					if (dispatchInfo.m_dispatchFunc == 		DispatcherInfo::DISPATCH_DISCRETE)
+					{
+						algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+					} else
+					{
+						float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+						if (dispatchInfo.m_timeOfImpact > toi)
+							dispatchInfo.m_timeOfImpact = toi;
+					}
+				}
+		}
+
+	}
+
+	m_blockedForChanges = false;
+
+}
+
+

Bullet/BroadphaseCollision/SimpleBroadphase.h

@@ -0,0 +1,97 @@
+/*
+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 SIMPLE_BROADPHASE_H
+#define SIMPLE_BROADPHASE_H
+
+//#define SIMPLE_MAX_PROXIES 8192
+//#define SIMPLE_MAX_OVERLAP 4096
+
+#include "BroadphaseInterface.h"
+#include "BroadphaseProxy.h"
+#include "SimdPoint3.h"
+
+struct SimpleBroadphaseProxy : public BroadphaseProxy
+{
+	SimdVector3	m_min;
+	SimdVector3	m_max;
+	
+	SimpleBroadphaseProxy() {};
+
+	SimpleBroadphaseProxy(const SimdPoint3& minpt,const SimdPoint3& maxpt,int shapeType,void* userPtr)
+	:BroadphaseProxy(shapeType,userPtr),
+	m_min(minpt),m_max(maxpt)		
+	{
+	}
+	
+
+};
+
+///SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks
+class SimpleBroadphase : public BroadphaseInterface
+{
+
+	SimpleBroadphaseProxy*	m_proxies;
+	int*				m_freeProxies;
+	int				m_firstFreeProxy;
+
+	SimpleBroadphaseProxy** m_pProxies;
+	int				m_numProxies;
+
+	//during the dispatch, check that user doesn't destroy/create proxy
+	bool		m_blockedForChanges;
+
+	BroadphasePair*	m_OverlappingPairs;
+	int	m_NumOverlapBroadphasePair;
+
+	int m_maxProxies;
+	int m_maxOverlap;
+	
+	inline SimpleBroadphaseProxy*	GetSimpleProxyFromProxy(BroadphaseProxy* proxy)
+	{
+		SimpleBroadphaseProxy* proxy0 = static_cast<SimpleBroadphaseProxy*>(proxy);
+		return proxy0;
+	}
+
+	bool	AabbOverlap(SimpleBroadphaseProxy* proxy0,SimpleBroadphaseProxy* proxy1);
+
+	void	validate();
+
+protected:
+	void	RemoveOverlappingPair(BroadphasePair& pair);
+	void	CleanOverlappingPair(BroadphasePair& pair);
+
+	void	RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy);
+
+	void	AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
+	BroadphasePair*	FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
+	virtual void	RefreshOverlappingPairs();
+public:
+	SimpleBroadphase(int maxProxies=4096,int maxOverlap=8192);
+	virtual ~SimpleBroadphase();
+
+	virtual BroadphaseProxy*	CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr);
+
+	virtual void	DestroyProxy(BroadphaseProxy* proxy);
+	virtual void	SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax);
+	virtual void	CleanProxyFromPairs(BroadphaseProxy* proxy);
+	virtual void	DispatchAllCollisionPairs(Dispatcher&	dispatcher,DispatcherInfo& dispatchInfo);
+
+};
+
+
+
+#endif //SIMPLE_BROADPHASE_H
+

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

Bullet/CollisionShapes/BoxShape.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 "BoxShape.h"
+
+SimdVector3 BoxShape::GetHalfExtents() const
+{
+	return m_boxHalfExtents1 * m_localScaling;
+}
+//{ 
+
+
+void BoxShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
+{
+	SimdVector3 halfExtents = GetHalfExtents();
+
+	SimdMatrix3x3 abs_b = t.getBasis().absolute();  
+	SimdPoint3 center = t.getOrigin();
+	SimdVector3 extent = SimdVector3(abs_b[0].dot(halfExtents),
+		   abs_b[1].dot(halfExtents),
+		  abs_b[2].dot(halfExtents));
+	extent += SimdVector3(GetMargin(),GetMargin(),GetMargin());
+
+	aabbMin = center - extent;
+	aabbMax = center + extent;
+
+
+}
+
+
+void	BoxShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+	//float margin = 0.f;
+	SimdVector3 halfExtents = GetHalfExtents();
+
+	SimdScalar lx=2.f*(halfExtents.x());
+	SimdScalar ly=2.f*(halfExtents.y());
+	SimdScalar lz=2.f*(halfExtents.z());
+
+	inertia[0] = mass/(12.0f) * (ly*ly + lz*lz);
+	inertia[1] = mass/(12.0f) * (lx*lx + lz*lz);
+	inertia[2] = mass/(12.0f) * (lx*lx + ly*ly);
+
+
+}
+

Bullet/CollisionShapes/BoxShape.h

@@ -0,0 +1,279 @@
+/*
+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 OBB_BOX_MINKOWSKI_H
+#define OBB_BOX_MINKOWSKI_H
+
+#include "PolyhedralConvexShape.h"
+#include "CollisionShapes/CollisionMargin.h"
+#include "BroadphaseCollision/BroadphaseProxy.h"
+#include "SimdPoint3.h"
+#include "SimdMinMax.h"
+
+///BoxShape implements both a feature based (vertex/edge/plane) and implicit (getSupportingVertex) Box
+class BoxShape: public PolyhedralConvexShape
+{
+
+	SimdVector3	m_boxHalfExtents1;
+
+
+public:
+
+	virtual ~BoxShape()
+	{
+
+	}
+
+	SimdVector3 GetHalfExtents() const;
+	//{ return m_boxHalfExtents1 * m_localScaling;}
+ 	//const SimdVector3& GetHalfExtents() const{ return m_boxHalfExtents1;}
+
+
+	
+	virtual int	GetShapeType() const { return BOX_SHAPE_PROXYTYPE;}
+
+	virtual SimdVector3	LocalGetSupportingVertex(const SimdVector3& vec) const
+	{
+		
+		SimdVector3 halfExtents = GetHalfExtents();
+		SimdVector3 margin(GetMargin(),GetMargin(),GetMargin());
+		halfExtents -= margin;
+		
+		SimdVector3 supVertex;
+		supVertex = SimdPoint3(vec.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(),
+                     vec.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(),
+                     vec.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); 
+  
+		if ( GetMargin()!=0.f )
+		{
+			SimdVector3 vecnorm = vec;
+			if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+			{
+				vecnorm.setValue(-1.f,-1.f,-1.f);
+			} 
+			vecnorm.normalize();
+			supVertex+= GetMargin() * vecnorm;
+		}
+		return supVertex;
+	}
+
+	virtual inline SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const
+	{
+		SimdVector3 halfExtents = GetHalfExtents();
+		SimdVector3 margin(GetMargin(),GetMargin(),GetMargin());
+		halfExtents -= margin;
+
+		return SimdVector3(vec.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(),
+                    vec.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(),
+                    vec.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); 
+	}
+
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+	{
+		SimdVector3 halfExtents = GetHalfExtents();
+		SimdVector3 margin(GetMargin(),GetMargin(),GetMargin());
+		halfExtents -= margin;
+
+
+		for (int i=0;i<numVectors;i++)
+		{
+			const SimdVector3& vec = vectors[i];
+			supportVerticesOut[i].setValue(vec.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(),
+                    vec.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(),
+                    vec.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); 
+		}
+
+	}
+
+
+	BoxShape( const SimdVector3& boxHalfExtents) :  m_boxHalfExtents1(boxHalfExtents){};
+	
+	virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const;
+
+	
+
+	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+
+	virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const
+	{
+		//this plane might not be aligned...
+		SimdVector4 plane ;
+		GetPlaneEquation(plane,i);
+		planeNormal = SimdVector3(plane.getX(),plane.getY(),plane.getZ());
+		planeSupport = LocalGetSupportingVertex(-planeNormal);
+	}
+
+	
+	virtual int GetNumPlanes() const
+	{
+		return 6;
+	}	
+	
+	virtual int	GetNumVertices() const 
+	{
+		return 8;
+	}
+
+	virtual int GetNumEdges() const
+	{
+		return 12;
+	}
+
+
+	virtual void GetVertex(int i,SimdVector3& vtx) const
+	{
+		SimdVector3 halfExtents = GetHalfExtents();
+
+		vtx = SimdVector3(
+				halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
+				halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1),
+				halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2));
+	}
+	
+
+	virtual void	GetPlaneEquation(SimdVector4& plane,int i) const
+	{
+		SimdVector3 halfExtents = GetHalfExtents();
+
+		switch (i)
+		{
+		case 0:
+			plane.setValue(1.f,0.f,0.f);
+			plane[3] = -halfExtents.x();
+			break;
+		case 1:
+			plane.setValue(-1.f,0.f,0.f);
+			plane[3] = -halfExtents.x();
+			break;
+		case 2:
+			plane.setValue(0.f,1.f,0.f);
+			plane[3] = -halfExtents.y();
+			break;
+		case 3:
+			plane.setValue(0.f,-1.f,0.f);
+			plane[3] = -halfExtents.y();
+			break;
+		case 4:
+			plane.setValue(0.f,0.f,1.f);
+			plane[3] = -halfExtents.z();
+			break;
+		case 5:
+			plane.setValue(0.f,0.f,-1.f);
+			plane[3] = -halfExtents.z();
+			break;
+		default:
+			assert(0);
+		}
+	}
+
+	
+	virtual void GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const
+	//virtual void GetEdge(int i,Edge& edge) const
+	{
+		int edgeVert0 = 0;
+		int edgeVert1 = 0;
+
+		switch (i)
+		{
+		case 0:
+				edgeVert0 = 0;
+				edgeVert1 = 1;
+			break;
+		case 1:
+				edgeVert0 = 0;
+				edgeVert1 = 2;
+			break;
+		case 2:
+			edgeVert0 = 1;
+			edgeVert1 = 3;
+
+			break;
+		case 3:
+			edgeVert0 = 2;
+			edgeVert1 = 3;
+			break;
+		case 4:
+			edgeVert0 = 0;
+			edgeVert1 = 4;
+			break;
+		case 5:
+			edgeVert0 = 1;
+			edgeVert1 = 5;
+
+			break;
+		case 6:
+			edgeVert0 = 2;
+			edgeVert1 = 6;
+			break;
+		case 7:
+			edgeVert0 = 3;
+			edgeVert1 = 7;
+			break;
+		case 8:
+			edgeVert0 = 4;
+			edgeVert1 = 5;
+			break;
+		case 9:
+			edgeVert0 = 4;
+			edgeVert1 = 6;
+			break;
+		case 10:
+			edgeVert0 = 5;
+			edgeVert1 = 7;
+			break;
+		case 11:
+			edgeVert0 = 6;
+			edgeVert1 = 7;
+			break;
+		default:
+			ASSERT(0);
+
+		}
+
+		GetVertex(edgeVert0,pa );
+		GetVertex(edgeVert1,pb );
+	}
+
+
+
+
+	
+	virtual	bool IsInside(const SimdPoint3& pt,SimdScalar tolerance) const
+	{
+		SimdVector3 halfExtents = GetHalfExtents();
+
+		//SimdScalar minDist = 2*tolerance;
+		
+		bool result =	(pt.x() <= (halfExtents.x()+tolerance)) &&
+						(pt.x() >= (-halfExtents.x()-tolerance)) &&
+						(pt.y() <= (halfExtents.y()+tolerance)) &&
+						(pt.y() >= (-halfExtents.y()-tolerance)) &&
+						(pt.z() <= (halfExtents.z()+tolerance)) &&
+						(pt.z() >= (-halfExtents.z()-tolerance));
+		
+		return result;
+	}
+
+
+	//debugging
+	virtual char*	GetName()const
+	{
+		return "Box";
+	}
+
+
+};
+
+#endif //OBB_BOX_MINKOWSKI_H

Bullet/CollisionShapes/BvhTriangleMeshShape.cpp

@@ -0,0 +1,138 @@
+/*
+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.
+*/
+
+//#define DISABLE_BVH
+
+
+#include "CollisionShapes/BvhTriangleMeshShape.h"
+#include "CollisionShapes/OptimizedBvh.h"
+
+///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
+///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
+BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface* meshInterface)
+:TriangleMeshShape(meshInterface)
+{
+	//construct bvh from meshInterface
+#ifndef DISABLE_BVH
+
+	m_bvh = new OptimizedBvh();
+	m_bvh->Build(meshInterface);
+
+#endif //DISABLE_BVH
+
+}
+
+BvhTriangleMeshShape::~BvhTriangleMeshShape()
+{
+	delete m_bvh;
+}
+
+//perform bvh tree traversal and report overlapping triangles to 'callback'
+void	BvhTriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+
+#ifdef DISABLE_BVH
+	//brute force traverse all triangles
+	TriangleMeshShape::ProcessAllTriangles(callback,aabbMin,aabbMax);
+#else
+
+	//first get all the nodes
+
+	
+	struct	MyNodeOverlapCallback : public NodeOverlapCallback
+	{
+		StridingMeshInterface*	m_meshInterface;
+		TriangleCallback*		m_callback;
+		SimdVector3				m_triangle[3];
+
+
+		MyNodeOverlapCallback(TriangleCallback* callback,StridingMeshInterface* meshInterface)
+			:m_meshInterface(meshInterface),
+			m_callback(callback)
+		{
+		}
+				
+		virtual void ProcessNode(const OptimizedBvhNode* node)
+		{
+			const unsigned char *vertexbase;
+			int numverts;
+			PHY_ScalarType type;
+			int stride;
+			const unsigned char *indexbase;
+			int indexstride;
+			int numfaces;
+			PHY_ScalarType indicestype;
+			
+
+			m_meshInterface->getLockedReadOnlyVertexIndexBase(
+				&vertexbase,
+				numverts,
+				type,
+				stride,
+				&indexbase,
+				indexstride,
+				numfaces,
+				indicestype,
+				node->m_subPart);
+
+			int* gfxbase = (int*)(indexbase+node->m_triangleIndex*indexstride);
+			
+			const SimdVector3& meshScaling = m_meshInterface->getScaling();
+			for (int j=2;j>=0;j--)
+			{
+				
+				int graphicsindex = gfxbase[j];
+#ifdef DEBUG_TRIANGLE_MESH
+				printf("%d ,",graphicsindex);
+#endif //DEBUG_TRIANGLE_MESH
+				float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+
+				m_triangle[j] = SimdVector3(
+					graphicsbase[0]*meshScaling.getX(),
+					graphicsbase[1]*meshScaling.getY(),
+					graphicsbase[2]*meshScaling.getZ());
+#ifdef DEBUG_TRIANGLE_MESH
+				printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z());
+#endif //DEBUG_TRIANGLE_MESH
+			}
+
+			m_callback->ProcessTriangle(m_triangle,node->m_subPart,node->m_triangleIndex);
+			m_meshInterface->unLockReadOnlyVertexBase(node->m_subPart);
+		}
+
+	};
+
+	MyNodeOverlapCallback	myNodeCallback(callback,m_meshInterface);
+
+	m_bvh->ReportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
+
+
+#endif//DISABLE_BVH
+
+
+}
+
+
+void	BvhTriangleMeshShape::setLocalScaling(const SimdVector3& scaling)
+{
+	if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON)
+	{
+		TriangleMeshShape::setLocalScaling(scaling);
+		delete m_bvh;
+		m_bvh = new OptimizedBvh();
+		m_bvh->Build(m_meshInterface);
+		//rebuild the bvh...
+	}
+}

Bullet/CollisionShapes/BvhTriangleMeshShape.h

@@ -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.
+*/
+
+#ifndef BVH_TRIANGLE_MESH_SHAPE_H
+#define BVH_TRIANGLE_MESH_SHAPE_H
+
+#include "CollisionShapes/TriangleMeshShape.h"
+#include "CollisionShapes/OptimizedBvh.h"
+
+///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
+///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
+class BvhTriangleMeshShape : public TriangleMeshShape
+{
+
+	OptimizedBvh*	m_bvh;
+	
+	
+public:
+	BvhTriangleMeshShape(StridingMeshInterface* meshInterface);
+
+	virtual ~BvhTriangleMeshShape();
+
+	
+	/*
+	virtual int	GetShapeType() const
+	{
+		return TRIANGLE_MESH_SHAPE_PROXYTYPE;
+	}
+	*/
+
+
+
+	virtual void	ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+
+	//debugging
+	virtual char*	GetName()const {return "BVHTRIANGLEMESH";}
+
+
+	virtual void	setLocalScaling(const SimdVector3& scaling);
+	
+
+
+};
+
+#endif //BVH_TRIANGLE_MESH_SHAPE_H

Bullet/CollisionShapes/CollisionMargin.h

@@ -0,0 +1,26 @@
+/*
+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_MARGIN_H
+#define COLLISION_MARGIN_H
+
+//used by Gjk and some other algorithms
+
+#define CONVEX_DISTANCE_MARGIN 0.04f// 0.1f//;//0.01f
+
+
+
+#endif //COLLISION_MARGIN_H
+

Bullet/CollisionShapes/CollisionShape.cpp

@@ -0,0 +1,75 @@
+/*
+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 "CollisionShapes/CollisionShape.h"
+
+void	CollisionShape::GetBoundingSphere(SimdVector3& center,SimdScalar& radius) const
+{
+	SimdTransform tr;
+	tr.setIdentity();
+	SimdVector3 aabbMin,aabbMax;
+
+	GetAabb(tr,aabbMin,aabbMax);
+
+	radius = (aabbMax-aabbMin).length()*0.5f;
+	center = (aabbMin+aabbMax)*0.5f;
+}
+
+float	CollisionShape::GetAngularMotionDisc() const
+{
+	SimdVector3	center;
+	float disc;
+	GetBoundingSphere(center,disc);
+	disc += (center).length();
+	return disc;
+}
+
+void CollisionShape::CalculateTemporalAabb(const SimdTransform& curTrans,const SimdVector3& linvel,const SimdVector3& angvel,SimdScalar timeStep, SimdVector3& temporalAabbMin,SimdVector3& temporalAabbMax)
+{
+	//start with static aabb
+	GetAabb(curTrans,temporalAabbMin,temporalAabbMax);
+
+	float temporalAabbMaxx = temporalAabbMax.getX();
+	float temporalAabbMaxy = temporalAabbMax.getY();
+	float temporalAabbMaxz = temporalAabbMax.getZ();
+	float temporalAabbMinx = temporalAabbMin.getX();
+	float temporalAabbMiny = temporalAabbMin.getY();
+	float temporalAabbMinz = temporalAabbMin.getZ();
+
+	// add linear motion
+	SimdVector3 linMotion = linvel*timeStep;
+	//todo: simd would have a vector max/min operation, instead of per-element access
+	if (linMotion.x() > 0.f)
+		temporalAabbMaxx += linMotion.x(); 
+	else
+		temporalAabbMinx += linMotion.x();
+	if (linMotion.y() > 0.f)
+		temporalAabbMaxy += linMotion.y(); 
+	else
+		temporalAabbMiny += linMotion.y();
+	if (linMotion.z() > 0.f)
+		temporalAabbMaxz += linMotion.z(); 
+	else
+		temporalAabbMinz += linMotion.z();
+
+	//add conservative angular motion
+	SimdScalar angularMotion = angvel.length() * GetAngularMotionDisc() * timeStep;
+	SimdVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
+	temporalAabbMin = SimdVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
+	temporalAabbMax = SimdVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
+
+	temporalAabbMin -= angularMotion3d;
+	temporalAabbMax += angularMotion3d;
+}

Bullet/CollisionShapes/CollisionShape.h

@@ -0,0 +1,84 @@
+/*
+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_SHAPE_H
+#define COLLISION_SHAPE_H
+
+#include "SimdTransform.h"
+#include "SimdVector3.h"
+#include <SimdMatrix3x3.h>
+#include "SimdPoint3.h"
+#include "BroadphaseCollision/BroadphaseProxy.h" //for the shape types
+
+///CollisionShape provides generic interface for collidable objects
+class CollisionShape
+{
+
+public:
+
+	CollisionShape()
+	:m_tempDebug(0)
+	{
+	}
+	virtual ~CollisionShape()
+	{
+	}
+
+	virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const =0;
+
+	virtual void	GetBoundingSphere(SimdVector3& center,SimdScalar& radius) const;
+
+	virtual float	GetAngularMotionDisc() const;
+
+	virtual int		GetShapeType() const=0;
+
+	///CalculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
+	///result is conservative
+	void CalculateTemporalAabb(const SimdTransform& curTrans,const SimdVector3& linvel,const SimdVector3& angvel,SimdScalar timeStep, SimdVector3& temporalAabbMin,SimdVector3& temporalAabbMax);
+
+	bool	IsPolyhedral() const
+	{
+		return (GetShapeType() < IMPLICIT_CONVEX_SHAPES_START_HERE);
+	}
+
+	bool	IsConvex() const
+	{
+		return (GetShapeType() < CONCAVE_SHAPES_START_HERE);
+	}
+	bool	IsConcave() const
+	{
+		return (GetShapeType() > CONCAVE_SHAPES_START_HERE);
+	}
+
+
+	virtual void	setLocalScaling(const SimdVector3& scaling) =0;
+	virtual const SimdVector3& getLocalScaling() const =0;
+
+	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) = 0;
+
+//debugging support
+	virtual char*	GetName()const =0 ;
+	const char* GetExtraDebugInfo() const { return m_tempDebug;}
+	void  SetExtraDebugInfo(const char* extraDebugInfo) { m_tempDebug = extraDebugInfo;}
+	const char * m_tempDebug;
+//endif debugging support
+
+	virtual void	SetMargin(float margin) = 0;
+	virtual float	GetMargin() const = 0;
+
+};	
+
+#endif //COLLISION_SHAPE_H
+

Bullet/CollisionShapes/ConeShape.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 "ConeShape.h"
+#include "SimdPoint3.h"
+
+#ifdef WIN32
+static int coneindices[3] = {1,2,0};
+#else
+static int coneindices[3] = {2,1,0};
+#endif
+
+ConeShape::ConeShape (SimdScalar radius,SimdScalar height):
+m_radius (radius),
+m_height(height)
+{
+	SimdVector3 halfExtents;
+	m_sinAngle = (m_radius / sqrt(m_radius * m_radius + m_height * m_height));
+}
+
+
+SimdVector3 ConeShape::ConeLocalSupport(const SimdVector3& v) const
+{
+	
+	float halfHeight = m_height * 0.5f;
+
+ if (v[coneindices[1]] > v.length() * m_sinAngle)
+ {
+	SimdVector3 tmp;
+
+	tmp[coneindices[0]] = 0.f;
+	tmp[coneindices[1]] = halfHeight;
+	tmp[coneindices[2]] = 0.f;
+	return tmp;
+ }
+  else {
+    SimdScalar s = SimdSqrt(v[coneindices[0]] * v[coneindices[0]] + v[coneindices[2]] * v[coneindices[2]]);
+    if (s > SIMD_EPSILON) {
+      SimdScalar d = m_radius / s;
+	  SimdVector3 tmp;
+	  tmp[coneindices[0]] = v[coneindices[0]] * d;
+	  tmp[coneindices[1]] = -halfHeight;
+	  tmp[coneindices[2]] = v[coneindices[2]] * d;
+	  return tmp;
+    }
+    else  {
+		SimdVector3 tmp;
+		tmp[coneindices[0]] = 0.f;
+		tmp[coneindices[1]] = -halfHeight;
+		tmp[coneindices[2]] = 0.f;
+		return tmp;
+	}
+  }
+
+}
+
+SimdVector3	ConeShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec) const
+{
+		return ConeLocalSupport(vec);
+}
+
+void	ConeShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+{
+	for (int i=0;i<numVectors;i++)
+	{
+		const SimdVector3& vec = vectors[i];
+		supportVerticesOut[i] = ConeLocalSupport(vec);
+	}
+}
+
+
+SimdVector3	ConeShape::LocalGetSupportingVertex(const SimdVector3& vec)  const
+{
+	SimdVector3 supVertex = ConeLocalSupport(vec);
+	if ( GetMargin()!=0.f )
+	{
+		SimdVector3 vecnorm = vec;
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		{
+			vecnorm.setValue(-1.f,-1.f,-1.f);
+		} 
+		vecnorm.normalize();
+		supVertex+= GetMargin() * vecnorm;
+	}
+	return supVertex;
+}
+
+

Bullet/CollisionShapes/ConeShape.h

@@ -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.
+*/
+
+#ifndef CONE_MINKOWSKI_H
+#define CONE_MINKOWSKI_H
+
+#include "ConvexShape.h"
+#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
+
+/// implements cone shape interface
+class ConeShape : public ConvexShape
+
+{
+
+	float m_sinAngle;
+	float m_radius;
+	float m_height;
+	
+	SimdVector3 ConeLocalSupport(const SimdVector3& v) const;
+
+
+public:
+	ConeShape (SimdScalar radius,SimdScalar height);
+	
+	virtual SimdVector3	LocalGetSupportingVertex(const SimdVector3& vec) const;
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec) const;
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+
+	float GetRadius() const { return m_radius;}
+	float GetHeight() const { return m_height;}
+
+
+	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+	{
+		SimdTransform identity;
+		identity.setIdentity();
+		SimdVector3 aabbMin,aabbMax;
+		GetAabb(identity,aabbMin,aabbMax);
+
+		SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;
+
+		float margin = GetMargin();
+
+		SimdScalar lx=2.f*(halfExtents.x()+margin);
+		SimdScalar ly=2.f*(halfExtents.y()+margin);
+		SimdScalar lz=2.f*(halfExtents.z()+margin);
+		const SimdScalar x2 = lx*lx;
+		const SimdScalar y2 = ly*ly;
+		const SimdScalar z2 = lz*lz;
+		const SimdScalar scaledmass = mass * 0.08333333f;
+
+		inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));
+
+//		inertia.x() = scaledmass * (y2+z2);
+//		inertia.y() = scaledmass * (x2+z2);
+//		inertia.z() = scaledmass * (x2+y2);
+	}
+
+
+
+		virtual int	GetShapeType() const { return CONE_SHAPE_PROXYTYPE; }
+
+		virtual char*	GetName()const 
+		{
+			return "Cone";
+		}
+};
+
+
+#endif //CONE_MINKOWSKI_H
+

Bullet/CollisionShapes/ConvexHullShape.cpp

@@ -0,0 +1,160 @@
+/*
+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 "ConvexHullShape.h"
+#include "CollisionShapes/CollisionMargin.h"
+
+#include "SimdQuaternion.h"
+
+
+ConvexHullShape ::ConvexHullShape (SimdPoint3* points,int numPoints)
+{
+	m_points.resize(numPoints);
+	for (int i=0;i<numPoints;i++)
+		m_points[i] = points[i];
+}
+
+SimdVector3	ConvexHullShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
+{
+	SimdVector3 supVec(0.f,0.f,0.f);
+	SimdScalar newDot,maxDot = -1e30f;
+
+	SimdVector3 vec = vec0;
+	SimdScalar lenSqr = vec.length2();
+	if (lenSqr < 0.0001f)
+	{
+		vec.setValue(1,0,0);
+	} else
+	{
+		float rlen = 1.f / SimdSqrt(lenSqr );
+		vec *= rlen;
+	}
+
+
+	for (size_t i=0;i<m_points.size();i++)
+	{
+		SimdPoint3 vtx = m_points[i] * m_localScaling;
+
+		newDot = vec.dot(vtx);
+		if (newDot > maxDot)
+		{
+			maxDot = newDot;
+			supVec = vtx;
+		}
+	}
+	return supVec;
+}
+
+void	ConvexHullShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+{
+	SimdScalar newDot;
+	//use 'w' component of supportVerticesOut?
+	{
+		for (int i=0;i<numVectors;i++)
+		{
+			supportVerticesOut[i][3] = -1e30f;
+		}
+	}
+	for (size_t i=0;i<m_points.size();i++)
+	{
+		SimdPoint3 vtx = m_points[i] * m_localScaling;
+
+		for (int j=0;j<numVectors;j++)
+		{
+			const SimdVector3& vec = vectors[j];
+			
+			newDot = vec.dot(vtx);
+			if (newDot > supportVerticesOut[j][3])
+			{
+				//WARNING: don't swap next lines, the w component would get overwritten!
+				supportVerticesOut[j] = vtx;
+				supportVerticesOut[j][3] = newDot;
+			}
+		}
+	}
+
+
+
+}
+	
+
+
+SimdVector3	ConvexHullShape::LocalGetSupportingVertex(const SimdVector3& vec)const
+{
+	SimdVector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
+
+	if ( GetMargin()!=0.f )
+	{
+		SimdVector3 vecnorm = vec;
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		{
+			vecnorm.setValue(-1.f,-1.f,-1.f);
+		} 
+		vecnorm.normalize();
+		supVertex+= GetMargin() * vecnorm;
+	}
+	return supVertex;
+}
+
+
+
+
+
+
+
+
+
+//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
+//Please note that you can debug-draw ConvexHullShape with the Raytracer Demo
+int	ConvexHullShape::GetNumVertices() const
+{
+	return m_points.size();
+}
+
+int ConvexHullShape::GetNumEdges() const
+{
+	return m_points.size()*m_points.size();
+}
+
+void ConvexHullShape::GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const
+{
+
+	int index0 = i%m_points.size();
+	int index1 = i/m_points.size();
+	pa = m_points[index0]*m_localScaling;
+	pb = m_points[index1]*m_localScaling;
+}
+
+void ConvexHullShape::GetVertex(int i,SimdPoint3& vtx) const
+{
+	vtx = m_points[i]*m_localScaling;
+}
+
+int	ConvexHullShape::GetNumPlanes() const
+{
+	return 0;
+}
+
+void ConvexHullShape::GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const
+{
+	assert(0);
+}
+
+//not yet
+bool ConvexHullShape::IsInside(const SimdPoint3& pt,SimdScalar tolerance) const
+{
+	assert(0);
+	return false;
+}
+

Bullet/CollisionShapes/ConvexHullShape.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 CONVEX_HULL_SHAPE_H
+#define CONVEX_HULL_SHAPE_H
+
+#include "PolyhedralConvexShape.h"
+#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
+
+#include <vector>
+
+///ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices)
+///No connectivity is needed. LocalGetSupportingVertex iterates linearly though all vertices.
+///on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash.
+///(memory is much slower then the cpu)
+class ConvexHullShape : public PolyhedralConvexShape
+{
+	std::vector<SimdPoint3>	m_points;
+
+public:
+	ConvexHullShape(SimdPoint3* points,int numPoints);
+
+	void AddPoint(const SimdPoint3& point)
+	{
+		m_points.push_back(point);
+	}
+	virtual SimdVector3	LocalGetSupportingVertex(const SimdVector3& vec)const;
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+	
+
+	virtual int	GetShapeType()const { return CONVEX_HULL_SHAPE_PROXYTYPE; }
+
+	//debugging
+	virtual char*	GetName()const {return "Convex";}
+
+	
+	virtual int	GetNumVertices() const;
+	virtual int GetNumEdges() const;
+	virtual void GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const;
+	virtual void GetVertex(int i,SimdPoint3& vtx) const;
+	virtual int	GetNumPlanes() const;
+	virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const;
+	virtual	bool IsInside(const SimdPoint3& pt,SimdScalar tolerance) const;
+
+
+
+};
+
+
+#endif //CONVEX_HULL_SHAPE_H
+

Bullet/CollisionShapes/ConvexShape.cpp

@@ -0,0 +1,74 @@
+/*
+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 "ConvexShape.h"
+
+ConvexShape::~ConvexShape()
+{
+
+}
+
+ConvexShape::ConvexShape()
+:m_collisionMargin(CONVEX_DISTANCE_MARGIN),
+m_localScaling(1.f,1.f,1.f)
+{
+}
+
+
+void	ConvexShape::setLocalScaling(const SimdVector3& scaling)
+{
+	m_localScaling = scaling;
+}
+
+
+
+void	ConvexShape::GetAabbSlow(const SimdTransform& trans,SimdVector3&minAabb,SimdVector3&maxAabb) const
+{
+
+	SimdScalar margin = GetMargin();
+	for (int i=0;i<3;i++)
+	{
+		SimdVector3 vec(0.f,0.f,0.f);
+		vec[i] = 1.f;
+
+		SimdVector3 sv = LocalGetSupportingVertex(vec*trans.getBasis());
+
+		SimdVector3 tmp = trans(sv);
+		maxAabb[i] = tmp[i]+margin;
+		vec[i] = -1.f;
+		tmp = trans(LocalGetSupportingVertex(vec*trans.getBasis()));
+		minAabb[i] = tmp[i]-margin;
+	}
+};
+
+SimdVector3	ConvexShape::LocalGetSupportingVertex(const SimdVector3& vec)const
+ {
+	 SimdVector3	supVertex = LocalGetSupportingVertexWithoutMargin(vec);
+
+	if ( GetMargin()!=0.f )
+	{
+		SimdVector3 vecnorm = vec;
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		{
+			vecnorm.setValue(-1.f,-1.f,-1.f);
+		} 
+		vecnorm.normalize();
+		supVertex+= GetMargin() * vecnorm;
+	}
+	return supVertex;
+
+ }
+
+

Bullet/CollisionShapes/ConvexShape.h

@@ -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.
+*/
+
+#ifndef CONVEX_SHAPE_INTERFACE1
+#define CONVEX_SHAPE_INTERFACE1
+
+#include "CollisionShape.h"
+
+#include "SimdVector3.h"
+#include "SimdTransform.h"
+#include "SimdMatrix3x3.h"
+#include <vector>
+#include "CollisionShapes/CollisionMargin.h"
+
+//todo: get rid of this ConvexCastResult thing!
+struct ConvexCastResult;
+
+
+/// ConvexShape is an abstract shape interface.
+/// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface.
+/// used in combination with GJK or ConvexCast
+class ConvexShape : public CollisionShape
+{
+public:
+	ConvexShape();
+
+	virtual ~ConvexShape();
+
+	virtual SimdVector3	LocalGetSupportingVertex(const SimdVector3& vec)const;
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec) const= 0;
+	
+	//notice that the vectors should be unit length
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const= 0;
+
+	// testing for hullnode code
+
+	///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+	void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
+	{
+		GetAabbSlow(t,aabbMin,aabbMax);
+	}
+
+
+	
+	virtual void GetAabbSlow(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const;
+
+
+	virtual void	setLocalScaling(const SimdVector3& scaling);
+	virtual const SimdVector3& getLocalScaling() const 
+	{
+		return m_localScaling;
+	}
+
+
+	virtual void	SetMargin(float margin)
+	{
+		m_collisionMargin = margin;
+	}
+	virtual float	GetMargin() const
+	{
+		return m_collisionMargin;
+	}
+private:
+	SimdScalar	m_collisionMargin;
+	//local scaling. collisionMargin is not scaled !
+protected:
+	SimdVector3	m_localScaling;
+
+};
+
+
+
+#endif //CONVEX_SHAPE_INTERFACE1

Bullet/CollisionShapes/CylinderShape.cpp

@@ -0,0 +1,196 @@
+/*
+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 "CylinderShape.h"
+#include "SimdPoint3.h"
+
+CylinderShape::CylinderShape (const SimdVector3& halfExtents)
+:BoxShape(halfExtents)
+{
+
+}
+
+
+CylinderShapeX::CylinderShapeX (const SimdVector3& halfExtents)
+:CylinderShape(halfExtents)
+{
+}
+
+
+CylinderShapeZ::CylinderShapeZ (const SimdVector3& halfExtents)
+:CylinderShape(halfExtents)
+{
+}
+
+
+
+inline SimdVector3 CylinderLocalSupportX(const SimdVector3& halfExtents,const SimdVector3& v) 
+{
+const int cylinderUpAxis = 0;
+const int XX = 1;
+const int YY = 0;
+const int ZZ = 2;
+
+	//mapping depends on how cylinder local orientation is
+	// extents of the cylinder is: X,Y is for radius, and Z for height
+
+
+	float radius = halfExtents[XX];
+	float halfHeight = halfExtents[cylinderUpAxis];
+
+
+    SimdVector3 tmp;
+	SimdScalar d ;
+
+    SimdScalar s = SimdSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+    if (s != SimdScalar(0.0))
+	{
+        d = radius / s;  
+		tmp[XX] = v[XX] * d;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = v[ZZ] * d;
+		return tmp;
+	}
+    else
+	{
+	    tmp[XX] = radius;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = SimdScalar(0.0);
+		return tmp;
+    }
+
+
+}
+
+
+
+
+
+
+inline  SimdVector3 CylinderLocalSupportY(const SimdVector3& halfExtents,const SimdVector3& v) 
+{
+
+const int cylinderUpAxis = 1;
+const int XX = 0;
+const int YY = 1;
+const int ZZ = 2;
+
+
+	float radius = halfExtents[XX];
+	float halfHeight = halfExtents[cylinderUpAxis];
+
+
+    SimdVector3 tmp;
+	SimdScalar d ;
+
+    SimdScalar s = SimdSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+    if (s != SimdScalar(0.0))
+	{
+        d = radius / s;  
+		tmp[XX] = v[XX] * d;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = v[ZZ] * d;
+		return tmp;
+	}
+    else
+	{
+	    tmp[XX] = radius;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = SimdScalar(0.0);
+		return tmp;
+    }
+
+}
+
+inline SimdVector3 CylinderLocalSupportZ(const SimdVector3& halfExtents,const SimdVector3& v) 
+{
+const int cylinderUpAxis = 2;
+const int XX = 0;
+const int YY = 2;
+const int ZZ = 1;
+
+	//mapping depends on how cylinder local orientation is
+	// extents of the cylinder is: X,Y is for radius, and Z for height
+
+
+	float radius = halfExtents[XX];
+	float halfHeight = halfExtents[cylinderUpAxis];
+
+
+    SimdVector3 tmp;
+	SimdScalar d ;
+
+    SimdScalar s = SimdSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+    if (s != SimdScalar(0.0))
+	{
+        d = radius / s;  
+		tmp[XX] = v[XX] * d;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = v[ZZ] * d;
+		return tmp;
+	}
+    else
+	{
+	    tmp[XX] = radius;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = SimdScalar(0.0);
+		return tmp;
+    }
+
+
+}
+
+SimdVector3	CylinderShapeX::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const
+{
+	return CylinderLocalSupportX(GetHalfExtents(),vec);
+}
+
+
+SimdVector3	CylinderShapeZ::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const
+{
+	return CylinderLocalSupportZ(GetHalfExtents(),vec);
+}
+SimdVector3	CylinderShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const
+{
+	return CylinderLocalSupportY(GetHalfExtents(),vec);
+}
+
+void	CylinderShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+{
+	for (int i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i] = CylinderLocalSupportY(GetHalfExtents(),vectors[i]);
+	}
+}
+
+void	CylinderShapeZ::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+{
+	for (int i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i] = CylinderLocalSupportZ(GetHalfExtents(),vectors[i]);
+	}
+}
+
+
+
+
+void	CylinderShapeX::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+{
+	for (int i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i] = CylinderLocalSupportX(GetHalfExtents(),vectors[i]);
+	}
+}
+
+

Bullet/CollisionShapes/CylinderShape.h

@@ -0,0 +1,94 @@
+/*
+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 CYLINDER_MINKOWSKI_H
+#define CYLINDER_MINKOWSKI_H
+
+#include "BoxShape.h"
+#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
+#include "SimdVector3.h"
+
+/// implements cylinder shape interface
+class CylinderShape : public BoxShape
+
+{
+
+public:
+	CylinderShape (const SimdVector3& halfExtents);
+	
+	///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+	void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
+	{
+		GetAabbSlow(t,aabbMin,aabbMax);
+	}
+
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
+
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+
+	virtual SimdVector3	LocalGetSupportingVertex(const SimdVector3& vec) const
+	{
+
+		SimdVector3 supVertex;
+		supVertex = LocalGetSupportingVertexWithoutMargin(vec);
+		
+		if ( GetMargin()!=0.f )
+		{
+			SimdVector3 vecnorm = vec;
+			if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+			{
+				vecnorm.setValue(-1.f,-1.f,-1.f);
+			} 
+			vecnorm.normalize();
+			supVertex+= GetMargin() * vecnorm;
+		}
+		return supVertex;
+	}
+
+
+	//use box inertia
+	//	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+
+	virtual int	GetShapeType() const
+	{
+		return CYLINDER_SHAPE_PROXYTYPE;
+	}
+	
+
+
+};
+
+class CylinderShapeX : public CylinderShape
+{
+public:
+	CylinderShapeX (const SimdVector3& halfExtents);
+
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+};
+
+class CylinderShapeZ : public CylinderShape
+{
+public:
+	CylinderShapeZ (const SimdVector3& halfExtents);
+
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+
+};
+
+
+#endif //CYLINDER_MINKOWSKI_H
+

Bullet/CollisionShapes/EmptyShape.cpp

@@ -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.
+*/
+
+#include "EmptyShape.h"
+
+
+#include "CollisionShape.h"
+
+
+EmptyShape::EmptyShape()
+{
+}
+
+
+EmptyShape::~EmptyShape()
+{
+}
+
+
+	///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void EmptyShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
+{
+	SimdVector3 margin(GetMargin(),GetMargin(),GetMargin());
+
+	aabbMin = t.getOrigin() - margin;
+
+	aabbMax = t.getOrigin() + margin;
+
+}
+
+void	EmptyShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+	assert(0);
+}
+
+	
+	

Bullet/CollisionShapes/EmptyShape.h

@@ -0,0 +1,79 @@
+/*
+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_SHAPE_H
+#define EMPTY_SHAPE_H
+
+#include "CollisionShape.h"
+
+#include "SimdVector3.h"
+#include "SimdTransform.h"
+#include "SimdMatrix3x3.h"
+#include <vector>
+#include "CollisionShapes/CollisionMargin.h"
+
+
+
+
+/// EmptyShape is a collision shape without actual collision detection. It can be replaced by another shape during runtime
+class EmptyShape	: public CollisionShape
+{
+public:
+	EmptyShape();
+
+	virtual ~EmptyShape();
+
+
+	///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+	void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const;
+
+
+	virtual void	setLocalScaling(const SimdVector3& scaling)
+	{
+		m_localScaling = scaling;
+	}
+	virtual const SimdVector3& getLocalScaling() const 
+	{
+		return m_localScaling;
+	}
+
+	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+	
+	virtual int	GetShapeType() const { return EMPTY_SHAPE_PROXYTYPE;}
+
+	virtual void	SetMargin(float margin)
+	{
+		m_collisionMargin = margin;
+	}
+	virtual float	GetMargin() const
+	{
+		return m_collisionMargin;
+	}
+	virtual char*	GetName()const
+	{
+		return "Empty";
+	}
+
+
+private:
+	SimdScalar	m_collisionMargin;
+protected:
+	SimdVector3	m_localScaling;
+
+};
+
+
+
+#endif //EMPTY_SHAPE_H

Bullet/CollisionShapes/MinkowskiSumShape.cpp

@@ -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.
+*/
+
+#include "MinkowskiSumShape.h"
+
+
+MinkowskiSumShape::MinkowskiSumShape(ConvexShape* shapeA,ConvexShape* shapeB)
+:m_shapeA(shapeA),
+m_shapeB(shapeB)
+{
+	m_transA.setIdentity();
+	m_transB.setIdentity();
+}
+
+SimdVector3 MinkowskiSumShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const
+{
+	SimdVector3 supVertexA = m_transA(m_shapeA->LocalGetSupportingVertexWithoutMargin(vec*m_transA.getBasis()));
+	SimdVector3 supVertexB = m_transB(m_shapeB->LocalGetSupportingVertexWithoutMargin(vec*m_transB.getBasis()));
+	return supVertexA + supVertexB;
+}
+
+void	MinkowskiSumShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+{
+	//todo: could make recursive use of batching. probably this shape is not used frequently.
+	for (int i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]);
+	}
+
+}
+
+
+
+float	MinkowskiSumShape::GetMargin() const
+{
+	return m_shapeA->GetMargin() + m_shapeB->GetMargin();
+}
+
+
+void	MinkowskiSumShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+	assert(0);
+	inertia.setValue(0,0,0);
+}

Bullet/CollisionShapes/MinkowskiSumShape.h

@@ -0,0 +1,62 @@
+/*
+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 MINKOWSKI_SUM_SHAPE_H
+#define MINKOWSKI_SUM_SHAPE_H
+
+#include "ConvexShape.h"
+#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
+
+/// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes.
+class MinkowskiSumShape : public ConvexShape
+{
+
+	SimdTransform	m_transA;
+	SimdTransform	m_transB;
+	ConvexShape*	m_shapeA;
+	ConvexShape*	m_shapeB;
+
+public:
+
+	MinkowskiSumShape(ConvexShape* shapeA,ConvexShape* shapeB);
+
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
+
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+
+
+	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+
+	void	SetTransformA(const SimdTransform&	transA) { m_transA = transA;}
+	void	SetTransformB(const SimdTransform&	transB) { m_transB = transB;}
+
+	const SimdTransform& GetTransformA()const  { return m_transA;}
+	const SimdTransform& GetTransformB()const  { return m_transB;}
+
+
+	virtual int	GetShapeType() const { return MINKOWSKI_SUM_SHAPE_PROXYTYPE; }
+
+	virtual float	GetMargin() const;
+
+	const ConvexShape*	GetShapeA() const { return m_shapeA;}
+	const ConvexShape*	GetShapeB() const { return m_shapeB;}
+
+	virtual char*	GetName()const 
+	{
+		return "MinkowskiSum";
+	}
+};
+
+#endif //MINKOWSKI_SUM_SHAPE_H

Bullet/CollisionShapes/MultiSphereShape.cpp

@@ -0,0 +1,148 @@
+/*
+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 "MultiSphereShape.h"
+#include "CollisionShapes/CollisionMargin.h"
+#include "SimdQuaternion.h"
+
+MultiSphereShape::MultiSphereShape (const SimdVector3& inertiaHalfExtents,const SimdVector3* positions,const SimdScalar* radi,int numSpheres)
+:m_inertiaHalfExtents(inertiaHalfExtents)
+{
+	m_minRadius = 1e30f;
+
+	m_numSpheres = numSpheres;
+	for (int i=0;i<m_numSpheres;i++)
+	{
+		m_localPositions[i] = positions[i];
+		m_radi[i] = radi[i];
+		if (radi[i] < m_minRadius)
+			m_minRadius = radi[i];
+	}
+	SetMargin(m_minRadius);
+
+}
+
+
+
+ 
+ SimdVector3	MultiSphereShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
+{
+	int i;
+	SimdVector3 supVec(0,0,0);
+
+	SimdScalar maxDot(-1e30f);
+
+
+	SimdVector3 vec = vec0;
+	SimdScalar lenSqr = vec.length2();
+	if (lenSqr < 0.0001f)
+	{
+		vec.setValue(1,0,0);
+	} else
+	{
+		float rlen = 1.f / SimdSqrt(lenSqr );
+		vec *= rlen;
+	}
+
+	SimdVector3 vtx;
+	SimdScalar newDot;
+
+	const SimdVector3* pos = &m_localPositions[0];
+	const SimdScalar* rad = &m_radi[0];
+
+	for (i=0;i<m_numSpheres;i++)
+	{
+		vtx = (*pos) +vec*((*rad)-m_minRadius);
+		pos++;
+		rad++;
+		newDot = vec.dot(vtx);
+		if (newDot > maxDot)
+		{
+			maxDot = newDot;
+			supVec = vtx;
+		}
+	}
+
+	return supVec;
+
+}
+
+ void	MultiSphereShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+{
+
+	for (int j=0;j<numVectors;j++)
+	{
+		SimdScalar maxDot(-1e30f);
+
+		const SimdVector3& vec = vectors[j];
+
+		SimdVector3 vtx;
+		SimdScalar newDot;
+
+		const SimdVector3* pos = &m_localPositions[0];
+		const SimdScalar* rad = &m_radi[0];
+
+		for (int i=0;i<m_numSpheres;i++)
+		{
+			vtx = (*pos) +vec*((*rad)-m_minRadius);
+			pos++;
+			rad++;
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supportVerticesOut[j] = vtx;
+			}
+		}
+	}
+}
+
+
+
+
+
+
+
+
+void	MultiSphereShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+	//as an approximation, take the inertia of the box that bounds the spheres
+
+	SimdTransform ident;
+	ident.setIdentity();
+//	SimdVector3 aabbMin,aabbMax;
+
+//	GetAabb(ident,aabbMin,aabbMax);
+
+	SimdVector3 halfExtents = m_inertiaHalfExtents;//(aabbMax - aabbMin)* 0.5f;
+
+	float margin = CONVEX_DISTANCE_MARGIN;
+
+	SimdScalar lx=2.f*(halfExtents[0]+margin);
+	SimdScalar ly=2.f*(halfExtents[1]+margin);
+	SimdScalar lz=2.f*(halfExtents[2]+margin);
+	const SimdScalar x2 = lx*lx;
+	const SimdScalar y2 = ly*ly;
+	const SimdScalar z2 = lz*lz;
+	const SimdScalar scaledmass = mass * 0.08333333f;
+
+	inertia[0] = scaledmass * (y2+z2);
+	inertia[1] = scaledmass * (x2+z2);
+	inertia[2] = scaledmass * (x2+y2);
+
+}
+
+
+

Bullet/CollisionShapes/MultiSphereShape.h

@@ -0,0 +1,62 @@
+/*
+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 MULTI_SPHERE_MINKOWSKI_H
+#define MULTI_SPHERE_MINKOWSKI_H
+
+#include "ConvexShape.h"
+#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
+
+#define MAX_NUM_SPHERES 5
+
+///MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex)
+class MultiSphereShape : public ConvexShape
+
+{
+	
+	SimdVector3 m_localPositions[MAX_NUM_SPHERES];
+	SimdScalar  m_radi[MAX_NUM_SPHERES];
+	SimdVector3	m_inertiaHalfExtents;
+
+	int m_numSpheres;
+	float m_minRadius;
+
+
+
+
+
+public:
+	MultiSphereShape (const SimdVector3& inertiaHalfExtents,const SimdVector3* positions,const SimdScalar* radi,int numSpheres);
+
+	///CollisionShape Interface
+	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+
+	/// ConvexShape Interface
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
+
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+	
+
+	virtual int	GetShapeType() const { return MULTI_SPHERE_SHAPE_PROXYTYPE; }
+
+	virtual char*	GetName()const 
+	{
+		return "MultiSphere";
+	}
+
+};
+
+
+#endif //MULTI_SPHERE_MINKOWSKI_H

Bullet/CollisionShapes/OptimizedBvh.cpp

@@ -0,0 +1,276 @@
+/*
+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 "OptimizedBvh.h"
+#include "StridingMeshInterface.h"
+#include "AabbUtil2.h"
+
+
+
+void OptimizedBvh::Build(StridingMeshInterface* triangles)
+{
+	//int countTriangles = 0;
+
+	
+
+	// NodeArray	triangleNodes;
+
+	struct	NodeTriangleCallback : public InternalTriangleIndexCallback
+	{
+		NodeArray&	m_triangleNodes;
+
+		NodeTriangleCallback(NodeArray&	triangleNodes)
+			:m_triangleNodes(triangleNodes)
+		{
+
+		}
+
+		virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int  triangleIndex)
+		{
+
+			OptimizedBvhNode node;
+			node.m_aabbMin = SimdVector3(1e30f,1e30f,1e30f); 
+			node.m_aabbMax = SimdVector3(-1e30f,-1e30f,-1e30f); 
+			node.m_aabbMin.setMin(triangle[0]);
+			node.m_aabbMax.setMax(triangle[0]);
+			node.m_aabbMin.setMin(triangle[1]);
+			node.m_aabbMax.setMax(triangle[1]);
+			node.m_aabbMin.setMin(triangle[2]);
+			node.m_aabbMax.setMax(triangle[2]);
+
+			node.m_escapeIndex = -1;
+			node.m_leftChild = 0;
+			node.m_rightChild = 0;
+
+
+			//for child nodes
+			node.m_subPart = partId;
+			node.m_triangleIndex = triangleIndex;
+
+			
+			m_triangleNodes.push_back(node);
+		}
+	};
+
+	
+
+	NodeTriangleCallback	callback(m_leafNodes);
+
+	SimdVector3 aabbMin(-1e30f,-1e30f,-1e30f);
+	SimdVector3 aabbMax(1e30f,1e30f,1e30f);
+
+	triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
+
+	//now we have an array of leafnodes in m_leafNodes
+
+	m_contiguousNodes = new OptimizedBvhNode[2*m_leafNodes.size()];
+	m_curNodeIndex = 0;
+
+	m_rootNode1 = BuildTree(m_leafNodes,0,m_leafNodes.size());
+
+
+	///create the leafnodes first
+//	OptimizedBvhNode* leafNodes = new OptimizedBvhNode;
+}
+
+
+OptimizedBvhNode*	OptimizedBvh::BuildTree	(NodeArray&	leafNodes,int startIndex,int endIndex)
+{
+	OptimizedBvhNode* internalNode;
+
+	int splitAxis, splitIndex, i;
+	int numIndices =endIndex-startIndex;
+	int curIndex = m_curNodeIndex;
+
+	assert(numIndices>0);
+
+	if (numIndices==1)
+	{
+		return new (&m_contiguousNodes[m_curNodeIndex++]) OptimizedBvhNode(leafNodes[startIndex]);
+	}
+	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+	
+	splitAxis = CalcSplittingAxis(leafNodes,startIndex,endIndex);
+
+	splitIndex = SortAndCalcSplittingIndex(leafNodes,startIndex,endIndex,splitAxis);
+
+	internalNode = &m_contiguousNodes[m_curNodeIndex++];
+	
+	internalNode->m_aabbMax.setValue(-1e30f,-1e30f,-1e30f);
+	internalNode->m_aabbMin.setValue(1e30f,1e30f,1e30f);
+	
+	for (i=startIndex;i<endIndex;i++)
+	{
+		internalNode->m_aabbMax.setMax(leafNodes[i].m_aabbMax);
+		internalNode->m_aabbMin.setMin(leafNodes[i].m_aabbMin);
+	}
+
+	
+
+	//internalNode->m_escapeIndex;
+	internalNode->m_leftChild = BuildTree(leafNodes,startIndex,splitIndex);
+	internalNode->m_rightChild = BuildTree(leafNodes,splitIndex,endIndex);
+
+	internalNode->m_escapeIndex  = m_curNodeIndex - curIndex;
+	return internalNode;
+}
+
+int	OptimizedBvh::SortAndCalcSplittingIndex(NodeArray&	leafNodes,int startIndex,int endIndex,int splitAxis)
+{
+	int i;
+	int splitIndex =startIndex;
+	int numIndices = endIndex - startIndex;
+	float splitValue;
+
+	SimdVector3 means(0.f,0.f,0.f);
+	for (i=startIndex;i<endIndex;i++)
+	{
+		SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+		means+=center;
+	}
+	means *= (1.f/(float)numIndices);
+	
+	splitValue = means[splitAxis];
+	
+	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+	for (i=startIndex;i<endIndex;i++)
+	{
+		SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+		if (center[splitAxis] > splitValue)
+		{
+			//swap
+			OptimizedBvhNode tmp = leafNodes[i];
+			leafNodes[i] = leafNodes[splitIndex];
+			leafNodes[splitIndex] = tmp;
+			splitIndex++;
+		}
+	}
+	if ((splitIndex==startIndex) || (splitIndex == (endIndex-1)))
+	{
+		splitIndex = startIndex+ (numIndices>>1);
+	}
+	return splitIndex;
+}
+
+
+int	OptimizedBvh::CalcSplittingAxis(NodeArray&	leafNodes,int startIndex,int endIndex)
+{
+	int i;
+
+	SimdVector3 means(0.f,0.f,0.f);
+	SimdVector3 variance(0.f,0.f,0.f);
+	int numIndices = endIndex-startIndex;
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+		means+=center;
+	}
+	means *= (1.f/(float)numIndices);
+		
+	for (i=startIndex;i<endIndex;i++)
+	{
+		SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+		SimdVector3 diff2 = center-means;
+		diff2 = diff2 * diff2;
+		variance += diff2;
+	}
+	variance *= (1.f/	((float)numIndices-1)	);
+	
+	return variance.maxAxis();
+}
+
+
+	
+void	OptimizedBvh::ReportAabbOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+	if (aabbMin.length() > 1000.f)
+	{
+		for (size_t i=0;i<m_leafNodes.size();i++)
+		{
+			const OptimizedBvhNode&	node = m_leafNodes[i];
+			nodeCallback->ProcessNode(&node);
+		}
+	} else
+	{
+		//WalkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
+		WalkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
+	}
+}
+
+void	OptimizedBvh::WalkTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+	bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
+	if (aabbOverlap)
+	{
+		isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
+		if (isLeafNode)
+		{
+			nodeCallback->ProcessNode(rootNode);
+		} else
+		{
+			WalkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax);
+			WalkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax);
+		}
+	}
+
+}
+
+int maxIterations = 0;
+
+void	OptimizedBvh::WalkStacklessTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+	int escapeIndex, curIndex = 0;
+	int walkIterations = 0;
+	bool aabbOverlap, isLeafNode;
+
+	while (curIndex < m_curNodeIndex)
+	{
+		//catch bugs in tree data
+		assert (walkIterations < m_curNodeIndex);
+
+		walkIterations++;
+		aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
+		isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
+		
+		if (isLeafNode && aabbOverlap)
+		{
+			nodeCallback->ProcessNode(rootNode);
+		} 
+		
+		if (aabbOverlap || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->m_escapeIndex;
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+		
+	}
+
+	if (maxIterations < walkIterations)
+		maxIterations = walkIterations;
+
+}
+
+
+void	OptimizedBvh::ReportSphereOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+
+}
+

Bullet/CollisionShapes/OptimizedBvh.h

@@ -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.
+*/
+
+#ifndef OPTIMIZED_BVH_H
+#define OPTIMIZED_BVH_H
+#include "SimdVector3.h"
+#include <vector>
+
+class StridingMeshInterface;
+
+/// OptimizedBvhNode contains both internal and leaf node information.
+/// It hasn't been optimized yet for storage. Some obvious optimizations are:
+/// Removal of the pointers (can already be done, they are not used for traversal)
+/// and storing aabbmin/max as quantized integers.
+/// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle
+/// meshes stored in a non-uniform way (like batches/subparts of triangle-fans
+struct OptimizedBvhNode
+{
+
+	SimdVector3	m_aabbMin;
+	SimdVector3	m_aabbMax;
+
+//these 2 pointers are obsolete, the stackless traversal just uses the escape index
+	OptimizedBvhNode*	m_leftChild;
+	OptimizedBvhNode*	m_rightChild;
+
+	int	m_escapeIndex;
+
+	//for child nodes
+	int	m_subPart;
+	int	m_triangleIndex;
+
+};
+
+class NodeOverlapCallback
+{
+public:
+	virtual ~NodeOverlapCallback() {};
+
+	virtual void ProcessNode(const OptimizedBvhNode* node) = 0;
+};
+
+typedef std::vector<OptimizedBvhNode>	NodeArray;
+
+
+///OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future)
+class OptimizedBvh
+{
+	OptimizedBvhNode*	m_rootNode1;
+	
+	OptimizedBvhNode*	m_contiguousNodes;
+	int					m_curNodeIndex;
+
+	int					m_numNodes;
+
+	NodeArray			m_leafNodes;
+
+public:
+	OptimizedBvh() :m_rootNode1(0), m_numNodes(0) { }
+	virtual ~OptimizedBvh() {};
+	
+	void	Build(StridingMeshInterface* triangles);
+
+	OptimizedBvhNode*	BuildTree	(NodeArray&	leafNodes,int startIndex,int endIndex);
+
+	int	CalcSplittingAxis(NodeArray&	leafNodes,int startIndex,int endIndex);
+
+	int	SortAndCalcSplittingIndex(NodeArray&	leafNodes,int startIndex,int endIndex,int splitAxis);
+	
+	void	WalkTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+	
+	void	WalkStacklessTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+	
+
+	//OptimizedBvhNode*	GetRootNode() { return m_rootNode1;}
+
+	int					GetNumNodes() { return m_numNodes;}
+
+	void	ReportAabbOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+	void	ReportSphereOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+
+};
+
+
+#endif //OPTIMIZED_BVH_H
+

Bullet/CollisionShapes/PolyhedralConvexShape.cpp

@@ -0,0 +1,113 @@
+/*
+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 <CollisionShapes/PolyhedralConvexShape.h>
+
+PolyhedralConvexShape::PolyhedralConvexShape()
+:m_optionalHull(0)
+{
+
+}
+
+
+
+SimdVector3	PolyhedralConvexShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
+{
+	int i;
+	SimdVector3 supVec(0,0,0);
+
+	SimdScalar maxDot(-1e30f);
+
+	SimdVector3 vec = vec0;
+	SimdScalar lenSqr = vec.length2();
+	if (lenSqr < 0.0001f)
+	{
+		vec.setValue(1,0,0);
+	} else
+	{
+		float rlen = 1.f / SimdSqrt(lenSqr );
+		vec *= rlen;
+	}
+
+	SimdVector3 vtx;
+	SimdScalar newDot;
+
+	for (i=0;i<GetNumVertices();i++)
+	{
+		GetVertex(i,vtx);
+		newDot = vec.dot(vtx);
+		if (newDot > maxDot)
+		{
+			maxDot = newDot;
+			supVec = vtx;
+		}
+	}
+
+	return supVec;
+
+}
+
+void	PolyhedralConvexShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+{
+	int i;
+
+	SimdVector3 vtx;
+	SimdScalar newDot;
+
+	for (int j=0;j<numVectors;j++)
+	{
+		SimdScalar maxDot(-1e30f);
+
+		const SimdVector3& vec = vectors[j];
+
+		for (i=0;i<GetNumVertices();i++)
+		{
+			GetVertex(i,vtx);
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supportVerticesOut[i] = vtx;
+			}
+		}
+	}
+}
+
+
+
+void	PolyhedralConvexShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+	//not yet, return box inertia
+
+	float margin = GetMargin();
+
+	SimdTransform ident;
+	ident.setIdentity();
+	SimdVector3 aabbMin,aabbMax;
+	GetAabb(ident,aabbMin,aabbMax);
+	SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;
+
+	SimdScalar lx=2.f*(halfExtents.x()+margin);
+	SimdScalar ly=2.f*(halfExtents.y()+margin);
+	SimdScalar lz=2.f*(halfExtents.z()+margin);
+	const SimdScalar x2 = lx*lx;
+	const SimdScalar y2 = ly*ly;
+	const SimdScalar z2 = lz*lz;
+	const SimdScalar scaledmass = mass * 0.08333333f;
+
+	inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));
+
+}
+

Bullet/CollisionShapes/PolyhedralConvexShape.h

@@ -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.
+*/
+
+#ifndef BU_SHAPE
+#define BU_SHAPE
+
+#include <SimdPoint3.h>
+#include <SimdMatrix3x3.h>
+#include <CollisionShapes/ConvexShape.h>
+
+
+///PolyhedralConvexShape is an interface class for feature based (vertex/edge/face) convex shapes.
+class PolyhedralConvexShape : public ConvexShape
+{
+
+public:
+
+	PolyhedralConvexShape();
+	
+	//brute force implementations
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+	
+	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+
+
+
+	virtual int	GetNumVertices() const = 0 ;
+	virtual int GetNumEdges() const = 0;
+	virtual void GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const = 0;
+	virtual void GetVertex(int i,SimdPoint3& vtx) const = 0;
+	virtual int	GetNumPlanes() const = 0;
+	virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const = 0;
+//	virtual int GetIndex(int i) const = 0 ; 
+
+	virtual	bool IsInside(const SimdPoint3& pt,SimdScalar tolerance) const = 0;
+	
+	/// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp
+	class	Hull*	m_optionalHull;
+
+};
+
+#endif //BU_SHAPE

Bullet/CollisionShapes/Simplex1to4Shape.cpp

@@ -0,0 +1,193 @@
+
+/*
+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 "Simplex1to4Shape.h"
+#include "SimdMatrix3x3.h"
+
+BU_Simplex1to4::BU_Simplex1to4()
+:m_numVertices(0)
+{
+}
+
+BU_Simplex1to4::BU_Simplex1to4(const SimdPoint3& pt0)
+:m_numVertices(0)
+{
+	AddVertex(pt0);
+}
+
+BU_Simplex1to4::BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1)
+:m_numVertices(0)
+{
+	AddVertex(pt0);
+	AddVertex(pt1);
+}
+
+BU_Simplex1to4::BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1,const SimdPoint3& pt2)
+:m_numVertices(0)
+{
+	AddVertex(pt0);
+	AddVertex(pt1);
+	AddVertex(pt2);
+}
+
+BU_Simplex1to4::BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1,const SimdPoint3& pt2,const SimdPoint3& pt3)
+:m_numVertices(0)
+{
+	AddVertex(pt0);
+	AddVertex(pt1);
+	AddVertex(pt2);
+	AddVertex(pt3);
+}
+
+
+
+
+
+void BU_Simplex1to4::AddVertex(const SimdPoint3& pt)
+{
+	m_vertices[m_numVertices++] = pt;
+}
+
+
+int	BU_Simplex1to4::GetNumVertices() const
+{
+	return m_numVertices;
+}
+
+int BU_Simplex1to4::GetNumEdges() const
+{
+	//euler formula, F-E+V = 2, so E = F+V-2
+
+	switch (m_numVertices)
+	{
+	case 0:
+		return 0;
+	case 1: return 0;
+	case 2: return 1;
+	case 3: return 3;
+	case 4: return 6;
+
+
+	}
+
+	return 0;
+}
+
+void BU_Simplex1to4::GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const
+{
+	
+    switch (m_numVertices)
+	{
+
+	case 2: 
+		pa = m_vertices[0];
+		pb = m_vertices[1];
+		break;
+	case 3:  
+		switch (i)
+		{
+		case 0:
+			pa = m_vertices[0];
+			pb = m_vertices[1];
+			break;
+		case 1:
+			pa = m_vertices[1];
+			pb = m_vertices[2];
+			break;
+		case 2:
+			pa = m_vertices[2];
+			pb = m_vertices[0];
+			break;
+
+		}
+		break;
+	case 4: 
+		switch (i)
+		{
+		case 0:
+			pa = m_vertices[0];
+			pb = m_vertices[1];
+			break;
+		case 1:
+			pa = m_vertices[1];
+			pb = m_vertices[2];
+			break;
+		case 2:
+			pa = m_vertices[2];
+			pb = m_vertices[0];
+			break;
+		case 3:
+			pa = m_vertices[0];
+			pb = m_vertices[3];
+			break;
+		case 4:
+			pa = m_vertices[1];
+			pb = m_vertices[3];
+			break;
+		case 5:
+			pa = m_vertices[2];
+			pb = m_vertices[3];
+			break;
+		}
+
+	}
+
+
+
+
+}
+
+void BU_Simplex1to4::GetVertex(int i,SimdPoint3& vtx) const
+{
+	vtx = m_vertices[i];
+}
+
+int	BU_Simplex1to4::GetNumPlanes() const
+{
+	switch (m_numVertices)
+	{
+	case 0:
+			return 0;
+	case 1:
+			return 0;
+	case 2:
+			return 0;
+	case 3:
+			return 2;
+	case 4:
+			return 4;
+	default:
+		{
+		}
+	}
+	return 0;
+}
+
+
+void BU_Simplex1to4::GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i) const
+{
+	
+}
+
+int BU_Simplex1to4::GetIndex(int i) const
+{
+	return 0;
+}
+
+bool BU_Simplex1to4::IsInside(const SimdPoint3& pt,SimdScalar tolerance) const
+{
+	return false;
+}
+

Bullet/CollisionShapes/Simplex1to4Shape.h

@@ -0,0 +1,75 @@
+/*
+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 BU_SIMPLEX_1TO4_SHAPE
+#define BU_SIMPLEX_1TO4_SHAPE
+
+
+#include <CollisionShapes/PolyhedralConvexShape.h>
+#include "BroadphaseCollision/BroadphaseProxy.h"
+
+
+///BU_Simplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex).
+class BU_Simplex1to4 : public PolyhedralConvexShape
+{
+protected:
+
+	int	m_numVertices;
+	SimdPoint3	m_vertices[4];
+
+public:
+	BU_Simplex1to4();
+
+	BU_Simplex1to4(const SimdPoint3& pt0);
+	BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1);
+	BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1,const SimdPoint3& pt2);
+	BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1,const SimdPoint3& pt2,const SimdPoint3& pt3);
+
+    
+	void	Reset()
+	{
+		m_numVertices = 0;
+	}
+	
+
+	virtual int	GetShapeType() const{ return TETRAHEDRAL_SHAPE_PROXYTYPE; }
+
+	void AddVertex(const SimdPoint3& pt);
+
+	//PolyhedralConvexShape interface
+
+	virtual int	GetNumVertices() const;
+
+	virtual int GetNumEdges() const;
+
+	virtual void GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const;
+	
+	virtual void GetVertex(int i,SimdPoint3& vtx) const;
+
+	virtual int	GetNumPlanes() const;
+
+	virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i) const;
+
+	virtual int GetIndex(int i) const;
+
+	virtual	bool IsInside(const SimdPoint3& pt,SimdScalar tolerance) const;
+
+
+	///GetName is for debugging
+	virtual  char*	GetName()const { return "BU_Simplex1to4";}
+
+};
+
+#endif //BU_SIMPLEX_1TO4_SHAPE

Bullet/CollisionShapes/SphereShape.cpp

@@ -0,0 +1,74 @@
+/*
+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 "SphereShape.h"
+#include "CollisionShapes/CollisionMargin.h"
+
+#include "SimdQuaternion.h"
+
+
+SphereShape ::SphereShape (SimdScalar radius)
+: m_radius(radius)
+{	
+}
+
+SimdVector3	SphereShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const
+{
+	return SimdVector3(0.f,0.f,0.f);
+}
+
+void	SphereShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+{
+	for (int i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i].setValue(0.f,0.f,0.f);
+	}
+}
+
+
+SimdVector3	SphereShape::LocalGetSupportingVertex(const SimdVector3& vec)const
+{
+	SimdVector3 supVertex;
+	supVertex = LocalGetSupportingVertexWithoutMargin(vec);
+
+	SimdVector3 vecnorm = vec;
+	if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+	{
+		vecnorm.setValue(-1.f,-1.f,-1.f);
+	} 
+	vecnorm.normalize();
+	supVertex+= GetMargin() * vecnorm;
+	return supVertex;
+}
+
+
+//broken due to scaling
+void SphereShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
+{
+	const SimdVector3& center = t.getOrigin();
+	SimdVector3 extent(GetMargin(),GetMargin(),GetMargin());
+	aabbMin = center - extent;
+	aabbMax = center + extent;
+}
+
+
+
+void	SphereShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+	SimdScalar elem = 0.4f * mass * GetMargin()*GetMargin();
+	inertia[0] = inertia[1] = inertia[2] = elem;
+
+}
+

Bullet/CollisionShapes/SphereShape.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_MINKOWSKI_H
+#define SPHERE_MINKOWSKI_H
+
+#include "ConvexShape.h"
+#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
+
+///SphereShape implements an implicit (getSupportingVertex) Sphere
+class SphereShape : public ConvexShape
+
+{
+	SimdScalar m_radius;
+	
+public:
+	SphereShape (SimdScalar radius);
+	
+	
+	virtual SimdVector3	LocalGetSupportingVertex(const SimdVector3& vec)const;
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
+	//notice that the vectors should be unit length
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+
+
+	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+
+	virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const;
+
+	virtual int	GetShapeType() const { return SPHERE_SHAPE_PROXYTYPE; }
+
+	SimdScalar	GetRadius() const { return m_radius;}
+
+	//debugging
+	virtual char*	GetName()const {return "SPHERE";}
+
+	virtual void	SetMargin(float margin)
+	{
+		ConvexShape::SetMargin(margin);
+	}
+	virtual float	GetMargin() const
+	{
+		//to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case
+		//this means, non-uniform scaling is not supported anymore
+		return m_localScaling[0] * m_radius + ConvexShape::GetMargin();
+	}
+
+
+};
+
+
+#endif //SPHERE_MINKOWSKI_H

Bullet/CollisionShapes/StridingMeshInterface.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 "StridingMeshInterface.h"
+
+StridingMeshInterface::~StridingMeshInterface()
+{
+
+}
+
+
+void	StridingMeshInterface::InternalProcessAllTriangles(InternalTriangleIndexCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+
+	SimdVector3 meshScaling = getScaling();
+
+	int numtotalphysicsverts = 0;
+	int part,graphicssubparts = getNumSubParts();
+	for (part=0;part<graphicssubparts ;part++)
+	{
+		const unsigned char * vertexbase;
+		const unsigned char * indexbase;
+		int indexstride;
+		PHY_ScalarType type;
+		PHY_ScalarType gfxindextype;
+		int stride,numverts,numtriangles;
+		getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
+
+		numtotalphysicsverts+=numtriangles*3; //upper bound
+
+	
+		int gfxindex;
+		SimdVector3 triangle[3];
+
+		for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+		{
+		
+			int	graphicsindex=0;
+
+#ifdef DEBUG_TRIANGLE_MESH
+			printf("triangle indices:\n");
+#endif //DEBUG_TRIANGLE_MESH
+			ASSERT(gfxindextype == PHY_INTEGER);
+			int* gfxbase = (int*)(indexbase+gfxindex*indexstride);
+
+			for (int j=2;j>=0;j--)
+			{
+				
+				graphicsindex = gfxbase[j];
+#ifdef DEBUG_TRIANGLE_MESH
+				printf("%d ,",graphicsindex);
+#endif //DEBUG_TRIANGLE_MESH
+				float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+
+				triangle[j] = SimdVector3(
+					graphicsbase[0]*meshScaling.getX(),
+					graphicsbase[1]*meshScaling.getY(),
+					graphicsbase[2]*meshScaling.getZ());
+#ifdef DEBUG_TRIANGLE_MESH
+				printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z());
+#endif //DEBUG_TRIANGLE_MESH
+			}
+
+			
+			//check aabb in triangle-space, before doing this
+			callback->InternalProcessTriangleIndex(triangle,part,gfxindex);
+			
+		}
+		
+		unLockReadOnlyVertexBase(part);
+	}
+}
+

Bullet/CollisionShapes/StridingMeshInterface.h

@@ -0,0 +1,87 @@
+/*
+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 STRIDING_MESHINTERFACE_H
+#define STRIDING_MESHINTERFACE_H
+
+#include "SimdVector3.h"
+#include "TriangleCallback.h"
+
+/// PHY_ScalarType enumerates possible scalar types.
+/// See the StridingMeshInterface for its use
+typedef enum PHY_ScalarType {
+	PHY_FLOAT,
+	PHY_DOUBLE,
+	PHY_INTEGER,
+	PHY_SHORT,
+	PHY_FIXEDPOINT88
+} PHY_ScalarType;
+
+///	StridingMeshInterface is the interface class for high performance access to triangle meshes
+/// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory.
+class  StridingMeshInterface
+{
+	protected:
+	
+		SimdVector3 m_scaling;
+
+	public:
+		StridingMeshInterface() :m_scaling(1.f,1.f,1.f)
+		{
+
+		}
+
+		virtual ~StridingMeshInterface();
+
+
+
+		void	InternalProcessAllTriangles(InternalTriangleIndexCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+
+		/// get read and write access to a subpart of a triangle mesh
+		/// this subpart has a continuous array of vertices and indices
+		/// in this way the mesh can be handled as chunks of memory with striding
+		/// very similar to OpenGL vertexarray support
+		/// make a call to unLockVertexBase when the read and write access is finished	
+		virtual void	getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0)=0;
+		
+		virtual void	getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const=0;
+	
+		/// unLockVertexBase finishes the access to a subpart of the triangle mesh
+		/// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
+		virtual void	unLockVertexBase(int subpart)=0;
+
+		virtual void	unLockReadOnlyVertexBase(int subpart) const=0;
+
+
+		/// getNumSubParts returns the number of seperate subparts
+		/// each subpart has a continuous array of vertices and indices
+		virtual int		getNumSubParts() const=0;
+
+		virtual void	preallocateVertices(int numverts)=0;
+		virtual void	preallocateIndices(int numindices)=0;
+
+		const SimdVector3&	getScaling() const {
+			return m_scaling;
+		}
+		void	setScaling(const SimdVector3& scaling)
+		{
+			m_scaling = scaling;
+		}
+
+
+};
+
+#endif //STRIDING_MESHINTERFACE_H

Bullet/CollisionShapes/TriangleCallback.cpp

@@ -0,0 +1,28 @@
+/*
+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 "TriangleCallback.h"
+
+TriangleCallback::~TriangleCallback()
+{
+
+}
+
+
+InternalTriangleIndexCallback::~InternalTriangleIndexCallback()
+{
+
+}
+

Bullet/CollisionShapes/TriangleCallback.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 TRIANGLE_CALLBACK_H
+#define TRIANGLE_CALLBACK_H
+
+#include "SimdVector3.h"
+
+
+class TriangleCallback
+{
+public:
+
+	virtual ~TriangleCallback();
+	virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex) = 0;
+};
+
+class InternalTriangleIndexCallback
+{
+public:
+
+	virtual ~InternalTriangleIndexCallback();
+	virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int  triangleIndex) = 0;
+};
+
+
+
+#endif //TRIANGLE_CALLBACK_H

Bullet/CollisionShapes/TriangleIndexVertexArray.cpp

@@ -0,0 +1,53 @@
+/*
+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 "TriangleIndexVertexArray.h"
+
+TriangleIndexVertexArray::TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride)
+:m_numTriangleIndices(numTriangleIndices),
+m_triangleIndexBase(triangleIndexBase),
+m_triangleIndexStride(triangleIndexStride),
+m_numVertices(numVertices),
+m_vertexBase(vertexBase),
+m_vertexStride(vertexStride)
+{
+}
+
+void	TriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart)
+{
+	numverts = m_numVertices;
+	(*vertexbase) = (unsigned char *)m_vertexBase;
+	type = PHY_FLOAT;
+	vertexStride = m_vertexStride;
+
+	numfaces = m_numTriangleIndices;
+	(*indexbase) = (unsigned char *)m_triangleIndexBase;
+	indexstride = m_triangleIndexStride;
+	indicestype = PHY_INTEGER;
+}
+
+void	TriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const
+{
+	numverts = m_numVertices;
+	(*vertexbase) = (unsigned char *)m_vertexBase;
+	type = PHY_FLOAT;
+	vertexStride = m_vertexStride;
+
+	numfaces = m_numTriangleIndices;
+	(*indexbase) = (unsigned char *)m_triangleIndexBase;
+	indexstride = m_triangleIndexStride;
+	indicestype = PHY_INTEGER;
+}
+

Bullet/CollisionShapes/TriangleIndexVertexArray.h

@@ -0,0 +1,51 @@
+/*
+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 "StridingMeshInterface.h"
+
+
+class TriangleIndexVertexArray : public StridingMeshInterface
+{
+
+	int			m_numTriangleIndices;
+	int*		m_triangleIndexBase;
+	int			m_triangleIndexStride;
+	int			m_numVertices;
+	float*		m_vertexBase;
+	int			m_vertexStride;
+	
+public:
+
+	TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride);
+	
+	virtual void	getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0);
+
+	virtual void	getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const;
+
+	/// unLockVertexBase finishes the access to a subpart of the triangle mesh
+	/// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
+	virtual void	unLockVertexBase(int subpart) {}
+
+	virtual void	unLockReadOnlyVertexBase(int subpart) const {}
+
+	/// getNumSubParts returns the number of seperate subparts
+	/// each subpart has a continuous array of vertices and indices
+	virtual int		getNumSubParts() const { return 1;}
+	
+	virtual void	preallocateVertices(int numverts){}
+	virtual void	preallocateIndices(int numindices){}
+
+};
+

Bullet/CollisionShapes/TriangleMesh.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 "TriangleMesh.h"
+#include <assert.h>
+
+static int	myindices[3] = {0,1,2};
+
+TriangleMesh::TriangleMesh ()
+{
+
+}
+
+void	TriangleMesh::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart)
+{
+	numverts = 3;
+	*vertexbase = (unsigned char*)&m_triangles[subpart];
+	type = PHY_FLOAT;
+	stride = sizeof(SimdVector3);
+
+
+	numfaces = 1;
+	*indexbase = (unsigned char*) &myindices[0];
+	indicestype = PHY_INTEGER;
+	indexstride = sizeof(int);
+
+}
+
+void	TriangleMesh::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const
+{
+	numverts = 3;
+	*vertexbase = (unsigned char*)&m_triangles[subpart];
+	type = PHY_FLOAT;
+	stride = sizeof(SimdVector3);
+
+
+	numfaces = 1;
+	*indexbase = (unsigned char*) &myindices[0];
+	indicestype = PHY_INTEGER;
+	indexstride = sizeof(int);
+
+}
+
+
+
+int		TriangleMesh::getNumSubParts() const
+{
+	return m_triangles.size();
+}

Bullet/CollisionShapes/TriangleMesh.h

@@ -0,0 +1,73 @@
+/*
+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 TRIANGLE_MESH_H
+#define TRIANGLE_MESH_H
+
+#include "CollisionShapes/StridingMeshInterface.h"
+#include <vector>
+#include <SimdVector3.h>
+
+struct MyTriangle
+{
+	SimdVector3	m_vert0;
+	SimdVector3	m_vert1;
+	SimdVector3	m_vert2;
+};
+
+///TriangleMesh provides storage for a concave triangle mesh. It can be used as data for the TriangleMeshShape.
+class TriangleMesh : public StridingMeshInterface
+{
+	std::vector<MyTriangle>	m_triangles;
+	
+
+	public:
+		TriangleMesh ();
+
+		void	AddTriangle(const SimdVector3& vertex0,const SimdVector3& vertex1,const SimdVector3& vertex2)
+		{
+			MyTriangle tri;
+			tri.m_vert0 = vertex0;
+			tri.m_vert1 = vertex1;
+			tri.m_vert2 = vertex2;
+			m_triangles.push_back(tri);
+		}
+
+
+//StridingMeshInterface interface implementation
+
+		virtual void	getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0);
+
+		virtual void	getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const;
+
+		/// unLockVertexBase finishes the access to a subpart of the triangle mesh
+		/// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
+		virtual void	unLockVertexBase(int subpart) {}
+
+		virtual void	unLockReadOnlyVertexBase(int subpart) const {}
+
+		/// getNumSubParts returns the number of seperate subparts
+		/// each subpart has a continuous array of vertices and indices
+		virtual int		getNumSubParts() const;
+		
+		virtual void	preallocateVertices(int numverts){}
+		virtual void	preallocateIndices(int numindices){}
+
+		
+};
+
+#endif //TRIANGLE_MESH_H
+

Bullet/CollisionShapes/TriangleMeshShape.cpp

@@ -0,0 +1,202 @@
+/*
+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 "TriangleMeshShape.h"
+#include "SimdVector3.h"
+#include "SimdQuaternion.h"
+#include "StridingMeshInterface.h"
+#include "AabbUtil2.h"
+#include "CollisionShapes/CollisionMargin.h"
+
+#include "stdio.h"
+
+TriangleMeshShape::TriangleMeshShape(StridingMeshInterface* meshInterface)
+: m_meshInterface(meshInterface),
+m_collisionMargin(CONVEX_DISTANCE_MARGIN)
+{
+	RecalcLocalAabb();
+}
+
+
+TriangleMeshShape::~TriangleMeshShape()
+{
+		
+}
+
+
+
+
+void TriangleMeshShape::GetAabb(const SimdTransform& trans,SimdVector3& aabbMin,SimdVector3& aabbMax) const
+{
+
+	SimdVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin);
+	SimdVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin);
+	
+	SimdMatrix3x3 abs_b = trans.getBasis().absolute();  
+
+	SimdPoint3 center = trans(localCenter);
+
+	SimdVector3 extent = SimdVector3(abs_b[0].dot(localHalfExtents),
+		   abs_b[1].dot(localHalfExtents),
+		  abs_b[2].dot(localHalfExtents));
+	extent += SimdVector3(GetMargin(),GetMargin(),GetMargin());
+
+	aabbMin = center - extent;
+	aabbMax = center + extent;
+
+	
+}
+
+void	TriangleMeshShape::RecalcLocalAabb()
+{
+	for (int i=0;i<3;i++)
+	{
+		SimdVector3 vec(0.f,0.f,0.f);
+		vec[i] = 1.f;
+		SimdVector3 tmp = LocalGetSupportingVertex(vec);
+		m_localAabbMax[i] = tmp[i]+m_collisionMargin;
+		vec[i] = -1.f;
+		tmp = LocalGetSupportingVertex(vec);
+		m_localAabbMin[i] = tmp[i]-m_collisionMargin;
+	}
+}
+
+
+
+class SupportVertexCallback : public TriangleCallback
+{
+
+	SimdVector3 m_supportVertexLocal;
+public:
+
+	SimdTransform	m_worldTrans;
+	SimdScalar m_maxDot;
+	SimdVector3 m_supportVecLocal;
+
+	SupportVertexCallback(const SimdVector3& supportVecWorld,const SimdTransform& trans)
+		: m_supportVertexLocal(0.f,0.f,0.f), m_worldTrans(trans) ,m_maxDot(-1e30f)
+		
+	{
+		m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis();
+	}
+
+	virtual void ProcessTriangle( SimdVector3* triangle,int partId, int triangleIndex)
+	{
+		for (int i=0;i<3;i++)
+		{
+			SimdScalar dot = m_supportVecLocal.dot(triangle[i]);
+			if (dot > m_maxDot)
+			{
+				m_maxDot = dot;
+				m_supportVertexLocal = triangle[i];
+			}
+		}
+	}
+
+	SimdVector3 GetSupportVertexWorldSpace()
+	{
+		return m_worldTrans(m_supportVertexLocal);
+	}
+
+	SimdVector3	GetSupportVertexLocal()
+	{
+		return m_supportVertexLocal;
+	}
+
+};
+
+	
+void TriangleMeshShape::setLocalScaling(const SimdVector3& scaling)
+{
+	m_meshInterface->setScaling(scaling);
+	RecalcLocalAabb();
+}
+
+const SimdVector3& TriangleMeshShape::getLocalScaling() const
+{
+	return m_meshInterface->getScaling();
+}
+
+
+
+
+
+
+//#define DEBUG_TRIANGLE_MESH
+
+
+void	TriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+
+	struct FilteredCallback : public InternalTriangleIndexCallback
+	{
+		TriangleCallback* m_callback;
+		SimdVector3 m_aabbMin;
+		SimdVector3 m_aabbMax;
+
+		FilteredCallback(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax)
+			:m_callback(callback),
+			m_aabbMin(aabbMin),
+			m_aabbMax(aabbMax)
+		{
+		}
+
+		virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex)
+		{
+			if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax))
+			{
+				//check aabb in triangle-space, before doing this
+				m_callback->ProcessTriangle(triangle,partId,triangleIndex);
+			}
+			
+		}
+
+	};
+
+	FilteredCallback filterCallback(callback,aabbMin,aabbMax);
+
+	m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax);
+
+}
+
+
+
+
+
+void	TriangleMeshShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+	//moving concave objects not supported
+	assert(0);
+	inertia.setValue(0.f,0.f,0.f);
+}
+
+
+SimdVector3 TriangleMeshShape::LocalGetSupportingVertex(const SimdVector3& vec) const
+{
+	SimdVector3 supportVertex;
+
+	SimdTransform ident;
+	ident.setIdentity();
+
+	SupportVertexCallback supportCallback(vec,ident);
+
+	SimdVector3 aabbMax(1e30f,1e30f,1e30f);
+	
+	ProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+		
+	supportVertex = supportCallback.GetSupportVertexLocal();
+
+	return supportVertex;
+}

Bullet/CollisionShapes/TriangleMeshShape.h

@@ -0,0 +1,84 @@
+/*
+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 TRIANGLE_MESH_SHAPE_H
+#define TRIANGLE_MESH_SHAPE_H
+
+#include "CollisionShapes/CollisionShape.h"
+#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
+
+#include "StridingMeshInterface.h"
+#include "TriangleCallback.h"
+
+
+
+///Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
+class TriangleMeshShape : public CollisionShape
+{
+protected:
+	StridingMeshInterface* m_meshInterface;
+	SimdVector3	m_localAabbMin;
+	SimdVector3	m_localAabbMax;
+	float m_collisionMargin;
+
+public:
+	TriangleMeshShape(StridingMeshInterface* meshInterface);
+
+	virtual ~TriangleMeshShape();
+
+
+	
+	virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec) const;
+
+	virtual SimdVector3	LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const
+	{
+		assert(0);
+		return LocalGetSupportingVertex(vec);
+	}
+
+	void	RecalcLocalAabb();
+
+	virtual int	GetShapeType() const
+	{
+		return TRIANGLE_MESH_SHAPE_PROXYTYPE;
+	}
+
+	virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const;
+
+	virtual void	ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+
+	virtual void	setLocalScaling(const SimdVector3& scaling);
+	virtual const SimdVector3& getLocalScaling() const;
+	
+
+	//debugging
+	virtual char*	GetName()const {return "TRIANGLEMESH";}
+
+	
+	virtual float GetMargin() const {
+		return m_collisionMargin;
+	}
+	virtual void SetMargin(float collisionMargin)
+	{
+		m_collisionMargin = collisionMargin;
+	}
+
+
+
+};
+
+#endif //TRIANGLE_MESH_SHAPE_H

Bullet/CollisionShapes/TriangleShape.h

@@ -0,0 +1,164 @@
+/*
+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 OBB_TRIANGLE_MINKOWSKI_H
+#define OBB_TRIANGLE_MINKOWSKI_H
+
+#include "ConvexShape.h"
+#include "CollisionShapes/BoxShape.h"
+
+class TriangleShape : public PolyhedralConvexShape
+{
+
+
+public:
+
+	SimdVector3	m_vertices1[3];
+
+
+	virtual int GetNumVertices() const
+	{
+		return 3;
+	}
+
+	const SimdVector3& GetVertexPtr(int index) const
+	{
+		return m_vertices1[index];
+	}
+	virtual void GetVertex(int index,SimdVector3& vert) const
+	{
+		vert = m_vertices1[index];
+	}
+	virtual int	GetShapeType() const
+	{
+		return TRIANGLE_SHAPE_PROXYTYPE;
+	}
+
+	virtual int GetNumEdges() const
+	{
+		return 3;
+	}
+	
+	virtual void GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const
+	{
+		GetVertex(i,pa);
+		GetVertex((i+1)%3,pb);
+	}
+
+	virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax)const 
+	{
+//		ASSERT(0);
+		GetAabbSlow(t,aabbMin,aabbMax);
+	}
+
+	SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& dir)const 
+	{
+		SimdVector3 dots(dir.dot(m_vertices1[0]), dir.dot(m_vertices1[1]), dir.dot(m_vertices1[2]));
+	  	return m_vertices1[dots.maxAxis()];
+
+	}
+
+	virtual void	BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+	{
+		for (int i=0;i<numVectors;i++)
+		{
+			const SimdVector3& dir = vectors[i];
+			SimdVector3 dots(dir.dot(m_vertices1[0]), dir.dot(m_vertices1[1]), dir.dot(m_vertices1[2]));
+  			supportVerticesOut[i] = m_vertices1[dots.maxAxis()];
+		}
+
+	}
+
+
+
+	TriangleShape(const SimdVector3& p0,const SimdVector3& p1,const SimdVector3& p2)
+	{
+		m_vertices1[0] = p0;
+		m_vertices1[1] = p1;
+		m_vertices1[2] = p2;
+	}
+
+	
+
+	virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i) const
+	{
+		GetPlaneEquation(i,planeNormal,planeSupport);
+	}
+
+	virtual int	GetNumPlanes() const
+	{
+		return 1;
+	}
+
+	void CalcNormal(SimdVector3& normal) const
+	{
+		normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
+		normal.normalize();
+	}
+
+	virtual void GetPlaneEquation(int i, SimdVector3& planeNormal,SimdPoint3& planeSupport) const
+	{
+		CalcNormal(planeNormal);
+		planeSupport = m_vertices1[0];
+	}
+
+	virtual void	CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+	{
+		ASSERT(0);
+		inertia.setValue(0.f,0.f,0.f);
+	}
+
+		virtual	bool IsInside(const SimdPoint3& pt,SimdScalar tolerance) const
+	{
+		SimdVector3 normal;
+		CalcNormal(normal);
+		//distance to plane
+		SimdScalar dist = pt.dot(normal);
+		SimdScalar planeconst = m_vertices1[0].dot(normal);
+		dist -= planeconst;
+		if (dist >= -tolerance && dist <= tolerance)
+		{
+			//inside check on edge-planes
+			int i;
+			for (i=0;i<3;i++)
+			{
+				SimdPoint3 pa,pb;
+				GetEdge(i,pa,pb);
+				SimdVector3 edge = pb-pa;
+				SimdVector3 edgeNormal = edge.cross(normal);
+				edgeNormal.normalize();
+				SimdScalar dist = pt.dot( edgeNormal);
+				SimdScalar edgeConst = pa.dot(edgeNormal);
+				dist -= edgeConst;
+				if (dist < -tolerance)
+					return false;
+			}
+			
+			return true;
+		}
+
+		return false;
+	}
+		//debugging
+		virtual char*	GetName()const
+		{
+			return "Triangle";
+		}
+
+
+};
+
+#endif //OBB_TRIANGLE_MINKOWSKI_H
+

Bullet/Doxyfile

@@ -0,0 +1,746 @@
+# Doxyfile 1.2.4
+
+# This file describes the settings to be used by doxygen for a project
+#
+# All text after a hash (#) is considered a comment and will be ignored
+# The format is:
+#       TAG = value [value, ...]
+# For lists items can also be appended using:
+#       TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (" ")
+
+#---------------------------------------------------------------------------
+# General configuration options
+#---------------------------------------------------------------------------
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded 
+# by quotes) that should identify the project. 
+PROJECT_NAME           = "Bullet Continuous Collision Detection Library"
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. 
+# This could be handy for archiving the generated documentation or 
+# if some version control system is used.
+
+PROJECT_NUMBER         = 
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) 
+# base path where the generated documentation will be put. 
+# If a relative path is entered, it will be relative to the location 
+# where doxygen was started. If left blank the current directory will be used.
+
+OUTPUT_DIRECTORY       = 
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all 
+# documentation generated by doxygen is written. Doxygen will use this 
+# information to generate all constant output in the proper language. 
+# The default language is English, other supported languages are: 
+# Dutch, French, Italian, Czech, Swedish, German, Finnish, Japanese, 
+# Korean, Hungarian, Norwegian, Spanish, Romanian, Russian, Croatian, 
+# Polish, Portuguese and Slovene.
+
+OUTPUT_LANGUAGE        = English
+
+# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in 
+# documentation are documented, even if no documentation was available. 
+# Private class members and static file members will be hidden unless 
+# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES 
+
+EXTRACT_ALL            = YES
+
+# If the EXTRACT_PRIVATE tag is set to YES all private members of a class 
+# will be included in the documentation. 
+
+EXTRACT_PRIVATE        = YES
+
+# If the EXTRACT_STATIC tag is set to YES all static members of a file 
+# will be included in the documentation. 
+
+EXTRACT_STATIC         = YES
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all 
+# undocumented members of documented classes, files or namespaces. 
+# If set to NO (the default) these members will be included in the 
+# various overviews, but no documentation section is generated. 
+# This option has no effect if EXTRACT_ALL is enabled. 
+
+HIDE_UNDOC_MEMBERS     = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all 
+# undocumented classes that are normally visible in the class hierarchy. 
+# If set to NO (the default) these class will be included in the various 
+# overviews. This option has no effect if EXTRACT_ALL is enabled. 
+
+HIDE_UNDOC_CLASSES     = NO
+
+# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will 
+# include brief member descriptions after the members that are listed in 
+# the file and class documentation (similar to JavaDoc). 
+# Set to NO to disable this. 
+
+BRIEF_MEMBER_DESC      = YES
+
+# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend 
+# the brief description of a member or function before the detailed description. 
+# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the 
+# brief descriptions will be completely suppressed. 
+
+REPEAT_BRIEF           = YES
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then 
+# Doxygen will generate a detailed section even if there is only a brief 
+# description. 
+
+ALWAYS_DETAILED_SEC    = NO
+
+# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full 
+# path before files name in the file list and in the header files. If set 
+# to NO the shortest path that makes the file name unique will be used. 
+
+FULL_PATH_NAMES        = NO
+
+# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag 
+# can be used to strip a user defined part of the path. Stripping is 
+# only done if one of the specified strings matches the left-hand part of 
+# the path. It is allowed to use relative paths in the argument list.
+
+STRIP_FROM_PATH        = 
+
+# The INTERNAL_DOCS tag determines if documentation 
+# that is typed after a \internal command is included. If the tag is set 
+# to NO (the default) then the documentation will be excluded. 
+# Set it to YES to include the internal documentation. 
+
+INTERNAL_DOCS          = NO
+
+# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will 
+# generate a class diagram (in Html and LaTeX) for classes with base or 
+# super classes. Setting the tag to NO turns the diagrams off. 
+
+CLASS_DIAGRAMS         = YES
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will 
+# be generated. Documented entities will be cross-referenced with these sources. 
+
+SOURCE_BROWSER         = YES
+
+# Setting the INLINE_SOURCES tag to YES will include the body 
+# of functions and classes directly in the documentation. 
+
+INLINE_SOURCES         = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct 
+# doxygen to hide any special comment blocks from generated source code 
+# fragments. Normal C and C++ comments will always remain visible. 
+
+STRIP_CODE_COMMENTS    = YES
+
+# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate 
+# file names in lower case letters. If set to YES upper case letters are also 
+# allowed. This is useful if you have classes or files whose names only differ 
+# in case and if your file system supports case sensitive file names. Windows 
+# users are adviced to set this option to NO.
+
+CASE_SENSE_NAMES       = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen 
+# will show members with their full class and namespace scopes in the 
+# documentation. If set to YES the scope will be hidden. 
+
+HIDE_SCOPE_NAMES       = NO
+
+# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen 
+# will generate a verbatim copy of the header file for each class for 
+# which an include is specified. Set to NO to disable this. 
+
+VERBATIM_HEADERS       = YES
+
+# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen 
+# will put list of the files that are included by a file in the documentation 
+# of that file. 
+
+SHOW_INCLUDE_FILES     = YES
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen 
+# will interpret the first line (until the first dot) of a JavaDoc-style 
+# comment as the brief description. If set to NO, the JavaDoc 
+# comments  will behave just like the Qt-style comments (thus requiring an 
+# explict @brief command for a brief description. 
+
+JAVADOC_AUTOBRIEF      = YES
+
+# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented 
+# member inherits the documentation from any documented member that it 
+# reimplements. 
+
+INHERIT_DOCS           = YES
+
+# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] 
+# is inserted in the documentation for inline members. 
+
+INLINE_INFO            = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen 
+# will sort the (detailed) documentation of file and class members 
+# alphabetically by member name. If set to NO the members will appear in 
+# declaration order. 
+
+SORT_MEMBER_DOCS       = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC 
+# tag is set to YES, then doxygen will reuse the documentation of the first 
+# member in the group (if any) for the other members of the group. By default 
+# all members of a group must be documented explicitly.
+
+DISTRIBUTE_GROUP_DOC   = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. 
+# Doxygen uses this value to replace tabs by spaces in code fragments. 
+
+TAB_SIZE               = 8
+
+# The ENABLE_SECTIONS tag can be used to enable conditional 
+# documentation sections, marked by \if sectionname ... \endif. 
+
+ENABLED_SECTIONS       = 
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or 
+# disable (NO) the todo list. This list is created by putting \todo 
+# commands in the documentation.
+
+GENERATE_TODOLIST      = YES
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or 
+# disable (NO) the test list. This list is created by putting \test 
+# commands in the documentation.
+
+GENERATE_TESTLIST      = YES
+
+# This tag can be used to specify a number of aliases that acts 
+# as commands in the documentation. An alias has the form "name=value". 
+# For example adding "sideeffect=\par Side Effects:\n" will allow you to 
+# put the command \sideeffect (or @sideeffect) in the documentation, which 
+# will result in a user defined paragraph with heading "Side Effects:". 
+# You can put \n's in the value part of an alias to insert newlines. 
+
+ALIASES                = 
+
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated 
+# by doxygen. Possible values are YES and NO. If left blank NO is used. 
+
+QUIET                  = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are 
+# generated by doxygen. Possible values are YES and NO. If left blank 
+# NO is used. 
+
+WARNINGS               = YES
+
+# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings 
+# for undocumented members. If EXTRACT_ALL is set to YES then this flag will 
+# automatically be disabled. 
+
+WARN_IF_UNDOCUMENTED   = YES
+
+# The WARN_FORMAT tag determines the format of the warning messages that 
+# doxygen can produce. The string should contain the $file, $line, and $text 
+# tags, which will be replaced by the file and line number from which the 
+# warning originated and the warning text. 
+
+WARN_FORMAT            = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning 
+# and error messages should be written. If left blank the output is written 
+# to stderr. 
+
+WARN_LOGFILE           = 
+
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag can be used to specify the files and/or directories that contain 
+# documented source files. You may enter file names like "myfile.cpp" or 
+# directories like "/usr/src/myproject". Separate the files or directories 
+# with spaces. 
+
+INPUT                  = .
+
+
+# If the value of the INPUT tag contains directories, you can use the 
+# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp 
+# and *.h) to filter out the source-files in the directories. If left 
+# blank all files are included. 
+
+FILE_PATTERNS          = *.h *.cpp *.c
+
+# The RECURSIVE tag can be used to turn specify whether or not subdirectories 
+# should be searched for input files as well. Possible values are YES and NO. 
+# If left blank NO is used. 
+
+RECURSIVE              = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should 
+# excluded from the INPUT source files. This way you can easily exclude a 
+# subdirectory from a directory tree whose root is specified with the INPUT tag. 
+
+EXCLUDE                = 
+
+# If the value of the INPUT tag contains directories, you can use the 
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude 
+# certain files from those directories. 
+
+EXCLUDE_PATTERNS       = 
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or 
+# directories that contain example code fragments that are included (see 
+# the \include command). 
+
+EXAMPLE_PATH           = 
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the 
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp 
+# and *.h) to filter out the source-files in the directories. If left 
+# blank all files are included. 
+
+EXAMPLE_PATTERNS       = 
+
+# The IMAGE_PATH tag can be used to specify one or more files or 
+# directories that contain image that are included in the documentation (see 
+# the \image command). 
+
+IMAGE_PATH             = 
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should 
+# invoke to filter for each input file. Doxygen will invoke the filter program 
+# by executing (via popen()) the command <filter> <input-file>, where <filter> 
+# is the value of the INPUT_FILTER tag, and <input-file> is the name of an 
+# input file. Doxygen will then use the output that the filter program writes 
+# to standard output. 
+
+INPUT_FILTER           = 
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using 
+# INPUT_FILTER) will be used to filter the input files when producing source 
+# files to browse. 
+
+FILTER_SOURCE_FILES    = NO
+
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index 
+# of all compounds will be generated. Enable this if the project 
+# contains a lot of classes, structs, unions or interfaces. 
+
+ALPHABETICAL_INDEX     = NO
+
+# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then 
+# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns 
+# in which this list will be split (can be a number in the range [1..20]) 
+
+COLS_IN_ALPHA_INDEX    = 5
+
+# In case all classes in a project start with a common prefix, all 
+# classes will be put under the same header in the alphabetical index. 
+# The IGNORE_PREFIX tag can be used to specify one or more prefixes that 
+# should be ignored while generating the index headers. 
+
+IGNORE_PREFIX          = 
+
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES (the default) Doxygen will 
+# generate HTML output. 
+
+GENERATE_HTML          = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `html' will be used as the default path. 
+
+HTML_OUTPUT            = html
+
+# The HTML_HEADER tag can be used to specify a personal HTML header for 
+# each generated HTML page. If it is left blank doxygen will generate a 
+# standard header.
+
+HTML_HEADER            = 
+
+# The HTML_FOOTER tag can be used to specify a personal HTML footer for 
+# each generated HTML page. If it is left blank doxygen will generate a 
+# standard footer.
+
+HTML_FOOTER            = 
+
+# The HTML_STYLESHEET tag can be used to specify a user defined cascading 
+# style sheet that is used by each HTML page. It can be used to 
+# fine-tune the look of the HTML output. If the tag is left blank doxygen 
+# will generate a default style sheet 
+
+HTML_STYLESHEET        = 
+
+# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, 
+# files or namespaces will be aligned in HTML using tables. If set to 
+# NO a bullet list will be used. 
+
+HTML_ALIGN_MEMBERS     = YES
+
+# If the GENERATE_HTMLHELP tag is set to YES, additional index files 
+# will be generated that can be used as input for tools like the 
+# Microsoft HTML help workshop to generate a compressed HTML help file (.chm) 
+# of the generated HTML documentation. 
+
+GENERATE_HTMLHELP      = NO
+
+# The DISABLE_INDEX tag can be used to turn on/off the condensed index at 
+# top of each HTML page. The value NO (the default) enables the index and 
+# the value YES disables it. 
+
+DISABLE_INDEX          = NO
+
+# This tag can be used to set the number of enum values (range [1..20]) 
+# that doxygen will group on one line in the generated HTML documentation. 
+
+ENUM_VALUES_PER_LINE   = 4
+
+# If the GENERATE_TREEVIEW tag is set to YES, a side pannel will be
+# generated containing a tree-like index structure (just like the one that 
+# is generated for HTML Help). For this to work a browser that supports 
+# JavaScript and frames is required (for instance Netscape 4.0+ 
+# or Internet explorer 4.0+). 
+
+GENERATE_TREEVIEW      = NO
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be 
+# used to set the initial width (in pixels) of the frame in which the tree 
+# is shown. 
+
+TREEVIEW_WIDTH         = 250
+
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will 
+# generate Latex output. 
+
+GENERATE_LATEX         = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `latex' will be used as the default path. 
+
+LATEX_OUTPUT           = latex
+
+# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact 
+# LaTeX documents. This may be useful for small projects and may help to 
+# save some trees in general. 
+
+COMPACT_LATEX          = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used 
+# by the printer. Possible values are: a4, a4wide, letter, legal and 
+# executive. If left blank a4wide will be used. 
+
+PAPER_TYPE             = a4wide
+
+# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX 
+# packages that should be included in the LaTeX output. 
+
+EXTRA_PACKAGES         = 
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for 
+# the generated latex document. The header should contain everything until 
+# the first chapter. If it is left blank doxygen will generate a 
+# standard header. Notice: only use this tag if you know what you are doing! 
+
+LATEX_HEADER           = 
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated 
+# is prepared for conversion to pdf (using ps2pdf). The pdf file will 
+# contain links (just like the HTML output) instead of page references 
+# This makes the output suitable for online browsing using a pdf viewer. 
+
+PDF_HYPERLINKS         = NO
+
+# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of 
+# plain latex in the generated Makefile. Set this option to YES to get a 
+# higher quality PDF documentation. 
+
+USE_PDFLATEX           = NO
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. 
+# command to the generated LaTeX files. This will instruct LaTeX to keep 
+# running if errors occur, instead of asking the user for help. 
+# This option is also used when generating formulas in HTML. 
+
+LATEX_BATCHMODE        = NO
+
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output 
+# The RTF output is optimised for Word 97 and may not look very pretty with 
+# other RTF readers or editors.
+
+GENERATE_RTF           = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `rtf' will be used as the default path. 
+
+RTF_OUTPUT             = rtf
+
+# If the COMPACT_RTF tag is set to YES Doxygen generates more compact 
+# RTF documents. This may be useful for small projects and may help to 
+# save some trees in general. 
+
+COMPACT_RTF            = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated 
+# will contain hyperlink fields. The RTF file will 
+# contain links (just like the HTML output) instead of page references. 
+# This makes the output suitable for online browsing using a WORD or other. 
+# programs which support those fields. 
+# Note: wordpad (write) and others do not support links. 
+
+RTF_HYPERLINKS         = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's 
+# config file, i.e. a series of assigments. You only have to provide 
+# replacements, missing definitions are set to their default value. 
+
+RTF_STYLESHEET_FILE    = 
+
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES (the default) Doxygen will 
+# generate man pages 
+
+GENERATE_MAN           = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `man' will be used as the default path. 
+
+MAN_OUTPUT             = man
+
+# The MAN_EXTENSION tag determines the extension that is added to 
+# the generated man pages (default is the subroutine's section .3) 
+
+MAN_EXTENSION          = .3
+
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES Doxygen will 
+# generate an XML file that captures the structure of 
+# the code including all documentation. Warning: This feature 
+# is still experimental and very incomplete.
+
+GENERATE_XML           = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor   
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will 
+# evaluate all C-preprocessor directives found in the sources and include 
+# files. 
+
+ENABLE_PREPROCESSING   = YES
+
+# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro 
+# names in the source code. If set to NO (the default) only conditional 
+# compilation will be performed. Macro expansion can be done in a controlled 
+# way by setting EXPAND_ONLY_PREDEF to YES. 
+
+MACRO_EXPANSION        = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES 
+# then the macro expansion is limited to the macros specified with the 
+# PREDEFINED and EXPAND_AS_PREDEFINED tags. 
+
+EXPAND_ONLY_PREDEF     = NO
+
+# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files 
+# in the INCLUDE_PATH (see below) will be search if a #include is found. 
+
+SEARCH_INCLUDES        = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that 
+# contain include files that are not input files but should be processed by 
+# the preprocessor. 
+
+INCLUDE_PATH           = ../../generic/extern
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard 
+# patterns (like *.h and *.hpp) to filter out the header-files in the 
+# directories. If left blank, the patterns specified with FILE_PATTERNS will 
+# be used. 
+
+INCLUDE_FILE_PATTERNS  = 
+
+# The PREDEFINED tag can be used to specify one or more macro names that 
+# are defined before the preprocessor is started (similar to the -D option of 
+# gcc). The argument of the tag is a list of macros of the form: name 
+# or name=definition (no spaces). If the definition and the = are 
+# omitted =1 is assumed. 
+
+PREDEFINED             = 
+
+# If the MACRO_EXPANSION and EXPAND_PREDEF_ONLY tags are set to YES then 
+# this tag can be used to specify a list of macro names that should be expanded. 
+# The macro definition that is found in the sources will be used. 
+# Use the PREDEFINED tag if you want to use a different macro definition. 
+
+EXPAND_AS_DEFINED      = 
+
+#---------------------------------------------------------------------------
+# Configuration::addtions related to external references   
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tagfiles. 
+
+TAGFILES               = 
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create 
+# a tag file that is based on the input files it reads. 
+
+GENERATE_TAGFILE       = 
+
+# If the ALLEXTERNALS tag is set to YES all external classes will be listed 
+# in the class index. If set to NO only the inherited external classes 
+# will be listed. 
+
+ALLEXTERNALS           = NO
+
+# The PERL_PATH should be the absolute path and name of the perl script 
+# interpreter (i.e. the result of `which perl'). 
+
+PERL_PATH              = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool   
+#---------------------------------------------------------------------------
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is 
+# available from the path. This tool is part of Graphviz, a graph visualization 
+# toolkit from AT&T and Lucent Bell Labs. The other options in this section 
+# have no effect if this option is set to NO (the default) 
+
+HAVE_DOT               = YES
+
+# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen 
+# will generate a graph for each documented class showing the direct and 
+# indirect inheritance relations. Setting this tag to YES will force the 
+# the CLASS_DIAGRAMS tag to NO.
+
+CLASS_GRAPH            = YES
+
+# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen 
+# will generate a graph for each documented class showing the direct and 
+# indirect implementation dependencies (inheritance, containment, and 
+# class references variables) of the class with other documented classes. 
+
+COLLABORATION_GRAPH    = YES
+
+# If the ENABLE_PREPROCESSING, INCLUDE_GRAPH, and HAVE_DOT tags are set to 
+# YES then doxygen will generate a graph for each documented file showing 
+# the direct and indirect include dependencies of the file with other 
+# documented files. 
+
+INCLUDE_GRAPH          = YES
+
+# If the ENABLE_PREPROCESSING, INCLUDED_BY_GRAPH, and HAVE_DOT tags are set to 
+# YES then doxygen will generate a graph for each documented header file showing 
+# the documented files that directly or indirectly include this file 
+
+INCLUDED_BY_GRAPH      = YES
+
+# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen 
+# will graphical hierarchy of all classes instead of a textual one. 
+
+GRAPHICAL_HIERARCHY    = YES
+
+# The tag DOT_PATH can be used to specify the path where the dot tool can be 
+# found. If left blank, it is assumed the dot tool can be found on the path. 
+
+DOT_PATH               = 
+
+# The MAX_DOT_GRAPH_WIDTH tag can be used to set the maximum allowed width 
+# (in pixels) of the graphs generated by dot. If a graph becomes larger than 
+# this value, doxygen will try to truncate the graph, so that it fits within 
+# the specified constraint. Beware that most browsers cannot cope with very 
+# large images. 
+
+MAX_DOT_GRAPH_WIDTH    = 1024
+
+# The MAX_DOT_GRAPH_HEIGHT tag can be used to set the maximum allows height 
+# (in pixels) of the graphs generated by dot. If a graph becomes larger than 
+# this value, doxygen will try to truncate the graph, so that it fits within 
+# the specified constraint. Beware that most browsers cannot cope with very 
+# large images. 
+
+MAX_DOT_GRAPH_HEIGHT   = 1024
+
+# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will 
+# generate a legend page explaining the meaning of the various boxes and 
+# arrows in the dot generated graphs. 
+
+GENERATE_LEGEND        = YES
+
+#---------------------------------------------------------------------------
+# Configuration::addtions related to the search engine   
+#---------------------------------------------------------------------------
+
+# The SEARCHENGINE tag specifies whether or not a search engine should be 
+# used. If set to NO the values of all tags below this one will be ignored. 
+
+SEARCHENGINE           = NO
+
+# The CGI_NAME tag should be the name of the CGI script that 
+# starts the search engine (doxysearch) with the correct parameters. 
+# A script with this name will be generated by doxygen. 
+
+CGI_NAME               = search.cgi
+
+# The CGI_URL tag should be the absolute URL to the directory where the 
+# cgi binaries are located. See the documentation of your http daemon for 
+# details. 
+
+CGI_URL                = 
+
+# The DOC_URL tag should be the absolute URL to the directory where the 
+# documentation is located. If left blank the absolute path to the 
+# documentation, with file:// prepended to it, will be used. 
+
+DOC_URL                = 
+
+# The DOC_ABSPATH tag should be the absolute path to the directory where the 
+# documentation is located. If left blank the directory on the local machine 
+# will be used. 
+
+DOC_ABSPATH            = 
+
+# The BIN_ABSPATH tag must point to the directory where the doxysearch binary 
+# is installed. 
+
+BIN_ABSPATH            = c:\program files\doxygen\bin
+
+# The EXT_DOC_PATHS tag can be used to specify one or more paths to 
+# documentation generated for other projects. This allows doxysearch to search 
+# the documentation for these projects as well. 
+
+EXT_DOC_PATHS          = 

Bullet/Jamfile

@@ -0,0 +1,11 @@
+SubDir TOP Bullet ;
+
+
+Description bullet : "Bullet Continuous Collision Detection and Physics Library" ;
+Library bullet :
+  [ Wildcard BroadphaseCollision : *.h *.cpp ] 
+  [ Wildcard CollisionDispatch : *.h *.cpp ] 
+  [ Wildcard CollisionShapes : *.h *.cpp ] 
+  [ Wildcard NarrowPhaseCollision : *.h *.cpp ] 
+;
+Recurse InstallHeader : .h  ;

Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp

@@ -0,0 +1,360 @@
+/*
+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 "BU_AlgebraicPolynomialSolver.h"
+#include <math.h>
+#include <SimdMinMax.h>
+
+int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q) 
+{ 
+
+	SimdScalar basic_h_local;
+	SimdScalar basic_h_local_delta;
+	
+	basic_h_local       = p * 0.5f; 
+   basic_h_local_delta = basic_h_local * basic_h_local - q; 
+   if (basic_h_local_delta > 0.0f)  { 
+      basic_h_local_delta = SimdSqrt(basic_h_local_delta); 
+      m_roots[0]  = - basic_h_local + basic_h_local_delta; 
+      m_roots[1]  = - basic_h_local - basic_h_local_delta; 
+      return 2; 
+   } 
+   else if (SimdGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) { 
+      m_roots[0]  = - basic_h_local; 
+      return 1; 
+   } 
+   else { 
+      return 0; 
+   } 
+ }
+
+
+int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) 
+{ 
+    SimdScalar radical = b * b - 4.0f * a * c;
+    if(radical >= 0.f)
+    {
+        SimdScalar sqrtRadical = SimdSqrt(radical); 
+        SimdScalar idenom = 1.0f/(2.0f * a);
+        m_roots[0]=(-b + sqrtRadical) * idenom;
+        m_roots[1]=(-b - sqrtRadical) * idenom;
+		return 2;
+	}
+	return 0;
+}
+
+
+#define cubic_rt(x) \
+ ((x) > 0.0f ? SimdPow((SimdScalar)(x), 0.333333333333333333333333f) : \
+  ((x) < 0.0f ? -SimdPow((SimdScalar)-(x), 0.333333333333333333333333f) : 0.0f))
+
+
+
+/*                                                                           */
+/* this function solves the following cubic equation:                        */
+/*                                                                           */
+/*              3          2                                                */
+/*      lead * x   +  a * x   +  b * x  +  c  =  0.                          */
+/*                                                                           */
+/* it returns the number of different roots found, and stores the roots in   */
+/* roots[0,2]. it returns -1 for a degenerate equation 0 = 0.                */
+/*                                                                           */
+int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c)
+{ 
+   SimdScalar p, q, r;
+   SimdScalar delta, u, phi;
+   SimdScalar dummy;
+
+   if (lead != 1.0) {
+      /*                                                                     */
+      /* transform into normal form: x^3 + a x^2 + b x + c = 0               */
+      /*                                                                     */
+      if (SimdEqual(lead, SIMD_EPSILON)) {
+         /*                                                                  */
+         /* we have  a x^2 + b x + c = 0                                     */
+         /*                                                                  */
+         if (SimdEqual(a, SIMD_EPSILON)) {
+            /*                                                               */
+            /* we have  b x + c = 0                                          */
+            /*                                                               */
+            if (SimdEqual(b, SIMD_EPSILON)) {
+               if (SimdEqual(c, SIMD_EPSILON)) {
+                  return -1;
+               }
+               else {
+                  return 0;
+               }
+            }
+            else {
+               m_roots[0] = -c / b;
+               return 1;
+            }
+         }
+         else {
+            p = c / a;
+            q = b / a;
+            return Solve2QuadraticFull(a,b,c);
+         }
+      }
+      else {
+         a = a / lead;
+         b = b / lead;
+         c = c / lead;
+      }
+   }
+               
+   /*                                                                        */
+   /* we substitute  x = y - a / 3  in order to eliminate the quadric term.  */
+   /* we get   x^3 + p x + q = 0                                             */
+   /*                                                                        */
+   a /= 3.0f;
+   u  = a * a;
+   p  = b / 3.0f - u;
+   q  = a * (2.0f * u - b) + c;
+
+   /*                                                                        */
+   /* now use Cardano's formula                                              */
+   /*                                                                        */
+   if (SimdEqual(p, SIMD_EPSILON)) {
+      if (SimdEqual(q, SIMD_EPSILON)) {
+         /*                                                                  */
+         /* one triple root                                                  */
+         /*                                                                  */
+         m_roots[0] = -a;
+         return 1;
+      }
+      else {
+         /*                                                                  */
+         /* one real and two complex roots                                   */
+         /*                                                                  */
+         m_roots[0] = cubic_rt(-q) - a;
+         return 1;
+      }
+   }
+
+   q /= 2.0f;
+   delta = p * p * p + q * q;
+   if (delta > 0.0f) {
+      /*                                                                     */
+      /* one real and two complex roots. note that  v = -p / u.              */
+      /*                                                                     */
+      u = -q + SimdSqrt(delta);
+      u = cubic_rt(u);
+      m_roots[0] = u - p / u - a;
+      return 1;
+   }
+   else if (delta < 0.0) {
+      /*                                                                     */
+      /* Casus irreducibilis: we have three real roots                       */
+      /*                                                                     */
+      r        = SimdSqrt(-p);
+      p       *= -r;
+      r       *= 2.0;
+      phi      = SimdAcos(-q / p) / 3.0f;
+      dummy    = SIMD_2_PI / 3.0f; 
+      m_roots[0] = r * SimdCos(phi) - a;
+      m_roots[1] = r * SimdCos(phi + dummy) - a;
+      m_roots[2] = r * SimdCos(phi - dummy) - a;
+      return 3;
+   }
+   else {
+      /*                                                                     */
+      /* one single and one SimdScalar root                                      */
+      /*                                                                     */
+      r = cubic_rt(-q);
+      m_roots[0] = 2.0f * r - a;
+      m_roots[1] = -r - a;
+      return 2;
+   }
+}
+
+
+/*                                                                           */
+/* this function solves the following quartic equation:                      */
+/*                                                                           */
+/*             4           3          2                                      */
+/*     lead * x   +  a *  x   +  b * x   +  c * x  +  d = 0.                 */
+/*                                                                           */
+/* it returns the number of different roots found, and stores the roots in   */
+/* roots[0,3]. it returns -1 for a degenerate equation 0 = 0.                */
+/*                                                                           */
+int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d)
+{ 
+   SimdScalar p, q ,r;
+   SimdScalar u, v, w;
+   int i, num_roots, num_tmp;
+   //SimdScalar tmp[2];
+
+   if (lead != 1.0) {
+      /*                                                                     */
+      /* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0       */
+      /*                                                                     */
+      if (SimdEqual(lead, SIMD_EPSILON)) {
+         /*                                                                  */
+         /* we have  a x^3 + b x^2 + c x + d = 0                             */
+         /*                                                                  */
+         if (SimdEqual(a, SIMD_EPSILON)) { 
+            /*                                                               */
+            /* we have  b x^2 + c x + d = 0                                  */
+            /*                                                               */
+            if (SimdEqual(b, SIMD_EPSILON)) {
+               /*                                                            */
+               /* we have  c x + d = 0                                       */
+               /*                                                            */
+               if (SimdEqual(c, SIMD_EPSILON)) {
+                  if (SimdEqual(d, SIMD_EPSILON)) {
+                     return -1;
+                  }
+                  else {
+                     return 0;
+                  }
+               }
+               else {
+                  m_roots[0] = -d / c;
+                  return 1;
+               }
+            }
+            else {
+               p = c / b;
+               q = d / b;
+               return Solve2QuadraticFull(b,c,d);
+               
+            }
+         }
+         else { 
+            return Solve3Cubic(1.0, b / a, c / a, d / a);
+         }
+      }
+      else {
+         a = a / lead;
+         b = b / lead;
+         c = c / lead;
+         d = d / lead;
+      }
+   }
+
+   /*                                                                        */
+   /* we substitute  x = y - a / 4  in order to eliminate the cubic term.    */
+   /* we get:  y^4 + p y^2 + q y + r = 0.                                    */
+   /*                                                                        */
+   a /= 4.0f;
+   p  = b - 6.0f * a * a;
+   q  = a * (8.0f * a * a - 2.0f * b) + c;
+   r  = a * (a * (b - 3.f * a * a) - c) + d;
+   if (SimdEqual(q, SIMD_EPSILON)) {
+      /*                                                                     */
+      /* biquadratic equation:  y^4 + p y^2 + r = 0.                         */
+      /*                                                                     */
+      num_roots = Solve2Quadratic(p, r);
+      if (num_roots > 0) {                 
+         if (m_roots[0] > 0.0f) {
+            if (num_roots > 1)  {
+               if ((m_roots[1] > 0.0f)  &&  (m_roots[1] != m_roots[0])) {
+                  u        = SimdSqrt(m_roots[1]);
+                  m_roots[2] =  u - a;
+                  m_roots[3] = -u - a;
+                  u        = SimdSqrt(m_roots[0]);
+                  m_roots[0] =  u - a;
+                  m_roots[1] = -u - a;
+                  return 4;
+               }
+               else {
+                  u        = SimdSqrt(m_roots[0]);
+                  m_roots[0] =  u - a;
+                  m_roots[1] = -u - a;
+                  return 2;
+               }
+            }
+            else {
+               u        = SimdSqrt(m_roots[0]);
+               m_roots[0] =  u - a;
+               m_roots[1] = -u - a;
+               return 2;
+            }
+         }
+      }
+      return 0;
+   }
+   else if (SimdEqual(r, SIMD_EPSILON)) {
+      /*                                                                     */
+      /* no absolute term:  y (y^3 + p y + q) = 0.                           */
+      /*                                                                     */
+      num_roots = Solve3Cubic(1.0, 0.0, p, q);
+      for (i = 0;  i < num_roots;  ++i)  m_roots[i] -= a;
+      if (num_roots != -1) {
+         m_roots[num_roots] = -a;
+         ++num_roots;
+      }
+      else {
+         m_roots[0]  = -a;
+         num_roots = 1;;
+      }
+      return num_roots;
+   }
+   else {
+      /*                                                                     */
+      /* we solve the resolvent cubic equation                               */
+      /*                                                                     */
+      num_roots = Solve3Cubic(1.0f, -0.5f * p, -r, 0.5f * r * p - 0.125f * q * q);
+      if (num_roots == -1) {
+         num_roots = 1;
+         m_roots[0]  = 0.0f;
+      }
+
+      /*                                                                     */
+      /* build two quadric equations                                         */
+      /*                                                                     */
+      w = m_roots[0];
+      u = w * w - r;
+      v = 2.0f * w - p;
+
+      if (SimdEqual(u, SIMD_EPSILON))
+         u = 0.0;
+      else if (u > 0.0f)
+         u = SimdSqrt(u);
+      else
+         return 0;
+      
+      if (SimdEqual(v, SIMD_EPSILON))
+         v = 0.0;
+      else if (v > 0.0f)
+         v = SimdSqrt(v);
+      else
+         return 0;
+
+      if (q < 0.0f)  v = -v;
+      w -= u;
+      num_roots=Solve2Quadratic(v, w);
+      for (i = 0;  i < num_roots;  ++i)  
+	  {
+		  m_roots[i] -= a;
+	  }
+      w += 2.0f *u;
+	  SimdScalar tmp[2];
+	  tmp[0] = m_roots[0];
+	  tmp[1] = m_roots[1];
+
+      num_tmp = Solve2Quadratic(-v, w);
+      for (i = 0;  i < num_tmp;  ++i)
+	  {
+		 m_roots[i + num_roots] = tmp[i] - a;
+		 m_roots[i]=tmp[i];
+	  }
+
+      return  (num_tmp + num_roots);
+   }
+}
+

Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.h

@@ -0,0 +1,45 @@
+/*
+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 BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H
+#define BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H
+
+#include "BU_PolynomialSolverInterface.h"
+
+/// BU_AlgebraicPolynomialSolver implements polynomial root finding by analytically solving algebraic equations.
+/// Polynomials up to 4rd degree are supported, Cardano's formula is used for 3rd degree
+class BU_AlgebraicPolynomialSolver : public BUM_PolynomialSolverInterface
+{
+public:
+	BU_AlgebraicPolynomialSolver() {};
+
+	int Solve2Quadratic(SimdScalar p, SimdScalar q);
+	int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c);
+	int	Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c);
+	int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d);
+	
+
+	SimdScalar GetRoot(int i) const 
+	{
+		return m_roots[i];
+	}
+
+private:
+	SimdScalar	m_roots[4];
+
+};
+
+#endif //BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H

Bullet/NarrowPhaseCollision/BU_Collidable.cpp

@@ -0,0 +1,25 @@
+/*
+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 "BU_Collidable.h"
+#include "CollisionShapes/CollisionShape.h"
+#include <SimdTransform.h>
+#include "BU_MotionStateInterface.h"
+
+BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape,void* userPointer )
+:m_motionState(motion),m_shape(shape),m_userPointer(userPointer)
+{
+}

Bullet/NarrowPhaseCollision/BU_Collidable.h

@@ -0,0 +1,57 @@
+/*
+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 BU_COLLIDABLE
+#define BU_COLLIDABLE
+
+
+class PolyhedralConvexShape;
+class BU_MotionStateInterface;
+#include <SimdPoint3.h>
+
+class BU_Collidable
+{
+public:
+	BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape, void* userPointer);
+
+	void*		GetUserPointer() const
+	{
+		return m_userPointer;
+	}
+
+	BU_MotionStateInterface&	GetMotionState()
+	{
+		return m_motionState;
+	}
+	inline const BU_MotionStateInterface&	GetMotionState() const
+	{
+		return m_motionState;
+	}
+	
+	inline const PolyhedralConvexShape&	GetShape() const
+	{
+		return m_shape;
+	};
+
+
+private:
+	BU_MotionStateInterface& m_motionState;
+	PolyhedralConvexShape&	m_shape;
+	void*		m_userPointer;
+
+};
+
+#endif //BU_COLLIDABLE

Bullet/NarrowPhaseCollision/BU_CollisionPair.cpp

@@ -0,0 +1,581 @@
+/*
+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 "BU_CollisionPair.h"
+#include "NarrowPhaseCollision/BU_VertexPoly.h"
+#include "NarrowPhaseCollision/BU_EdgeEdge.h"
+#include "BU_Collidable.h"
+
+
+#include "BU_MotionStateInterface.h"
+#include "CollisionShapes/PolyhedralConvexShape.h"
+#include <SimdMinMax.h>
+#include "SimdTransformUtil.h"
+
+
+
+BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance)
+: m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)),
+m_tolerance(tolerance)
+{
+
+}
+
+// if there exists a time-of-impact between any feature_pair (edgeA,edgeB),
+// (vertexA,faceB) or (vertexB,faceA) in [0..1], report true and smallest time
+
+
+/*
+bool BU_CollisionPair::GetTimeOfImpact(const SimdVector3& linearMotionA,const SimdQuaternion& angularMotionA,const SimdVector3& linearMotionB,const SimdQuaternion& angularMotionB, SimdScalar& toi,SimdTransform& impactTransA,SimdTransform& impactTransB)
+
+*/
+
+bool BU_CollisionPair::calcTimeOfImpact(
+					const SimdTransform& fromA,
+					const SimdTransform& toA,
+					const SimdTransform& fromB,
+					const SimdTransform& toB,
+					CastResult& result)
+{
+
+
+
+	
+	SimdVector3 linvelA,angvelA;
+	SimdVector3 linvelB,angvelB;
+
+	SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
+	SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);
+
+
+	SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
+	SimdQuaternion angularMotionA(0,0,0,1.f);
+	SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
+	SimdQuaternion angularMotionB(0,0,0,1);
+	
+
+
+	result.m_fraction = 1.f;
+
+	SimdTransform impactTransA;
+	SimdTransform impactTransB;
+
+	int index=0;
+
+	SimdScalar toiUnscaled=result.m_fraction;
+	const SimdScalar toiUnscaledLimit = result.m_fraction;
+
+	SimdTransform a2w;
+	a2w = fromA;
+	SimdTransform b2w = fromB;
+
+/* debugging code
+	{
+		const int numvertsB = m_convexB->GetNumVertices();
+		for (int v=0;v<numvertsB;v++)
+		{
+			SimdPoint3 pt;
+			m_convexB->GetVertex(v,pt);
+			pt = b2w * pt;
+			char buf[1000];
+
+			if (pt.y() < 0.)
+			{
+				sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
+				if (debugFile)
+					fwrite(buf,1,strlen(buf),debugFile);
+			} else
+			{
+				sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
+				if (debugFile)
+					fwrite(buf,1,strlen(buf),debugFile);
+
+			}
+		}
+	}
+*/
+
+
+	SimdTransform b2wp = b2w;
+	
+	b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
+	b2wp.setRotation( b2w.getRotation() + angularMotionB);
+
+	impactTransB = b2wp;
+	
+	SimdTransform a2wp;
+	a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
+	a2wp.setRotation(a2w.getRotation()+angularMotionA);
+
+	impactTransA = a2wp;
+
+	SimdTransform a2winv;
+	a2winv = a2w.inverse();
+
+	SimdTransform b2wpinv;
+	b2wpinv = b2wp.inverse();
+
+	SimdTransform b2winv;
+	b2winv = b2w.inverse();
+
+	SimdTransform a2wpinv;
+	a2wpinv = a2wp.inverse();
+
+		//Redon's version with concatenated transforms
+
+	SimdTransform relative;
+
+	relative = b2w * b2wpinv * a2wp * a2winv;
+
+	//relative = a2winv * a2wp  * b2wpinv * b2w;
+
+	SimdQuaternion qrel;
+	relative.getBasis().getRotation(qrel);
+
+	SimdVector3 linvel = relative.getOrigin();
+
+	if (linvel.length() < SCREWEPSILON)
+	{
+		linvel.setValue(0.,0.,0.);
+	}
+	SimdVector3 angvel;
+	angvel[0] = 2.f * SimdAsin (qrel[0]);
+	angvel[1] = 2.f * SimdAsin (qrel[1]);
+	angvel[2] = 2.f * SimdAsin (qrel[2]);
+	
+	if (angvel.length() < SCREWEPSILON)
+	{
+		angvel.setValue(0.f,0.f,0.f);
+	}
+
+	//Redon's version with concatenated transforms
+	m_screwing = BU_Screwing(linvel,angvel);
+	
+	SimdTransform w2s;
+	m_screwing.LocalMatrix(w2s);
+
+	SimdTransform s2w;
+	s2w = w2s.inverse();
+
+	//impactTransA = a2w;
+	//impactTransB = b2w;
+
+	bool hit = false;
+	
+	if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW()))
+	{
+		//W = 0 , S = 0 , no collision
+		//toi = 0;
+	/*	
+		{
+			const int numvertsB = m_convexB->GetNumVertices();
+			for (int v=0;v<numvertsB;v++)
+			{
+				SimdPoint3 pt;
+				m_convexB->GetVertex(v,pt);
+				pt = impactTransB * pt;
+				char buf[1000];
+				
+				if (pt.y() < 0.)
+				{
+					sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
+					if (debugFile)
+						fwrite(buf,1,strlen(buf),debugFile);
+				}
+				else
+				{
+					sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
+					if (debugFile)
+						fwrite(buf,1,strlen(buf),debugFile);
+				}
+			}
+		}
+	*/	
+		
+		return false;//don't continue moving within epsilon
+	}
+
+#define EDGEEDGE
+#ifdef EDGEEDGE
+
+	BU_EdgeEdge edgeEdge;
+
+	//for all edged in A check agains all edges in B
+	for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
+	{
+		SimdPoint3 pA0,pA1;
+
+		m_convexA->GetEdge(ea,pA0,pA1);
+
+		pA0= a2w * pA0;//in world space
+		pA0 = w2s * pA0;//in screwing space
+
+		pA1= a2w * pA1;//in world space
+		pA1 = w2s * pA1;//in screwing space
+
+		int numedgesB = m_convexB->GetNumEdges();
+		for (int eb = 0; eb < numedgesB;eb++)
+		{
+			{
+				SimdPoint3 pB0,pB1;
+				m_convexB->GetEdge(eb,pB0,pB1);
+
+				pB0= b2w * pB0;//in world space
+				pB0 = w2s * pB0;//in screwing space
+
+				pB1= b2w * pB1;//in world space
+				pB1 = w2s * pB1;//in screwing space
+
+
+				SimdScalar lambda,mu;
+				
+				toiUnscaled = 1.;
+
+				SimdVector3 edgeDirA(pA1-pA0);
+				SimdVector3 edgeDirB(pB1-pB0);
+
+				if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
+				{
+					//printf("edgeedge potential hit\n");
+					if (toiUnscaled>=0)
+					{
+						if (toiUnscaled < toiUnscaledLimit)							
+						{
+		
+							//inside check is already done by checking the mu and gamma !
+
+							SimdPoint3 vtx  = pA0+lambda * (pA1-pA0);
+							SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
+							
+							SimdPoint3 hitptWorld =   s2w * hitpt;
+							{
+
+								if (toiUnscaled < result.m_fraction)
+									result.m_fraction = toiUnscaled;
+
+								hit = true;
+
+								SimdVector3 hitNormal = edgeDirB.cross(edgeDirA);
+								
+								hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
+							
+
+								hitNormal.normalize();
+								
+								//an approximated normal can be calculated by taking the cross product of both edges
+								//take care of the sign !
+								
+								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
+						
+								SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld);
+								if (dist > 0)
+									hitNormalWorld *= -1;
+								
+								//todo: this is the wrong point, because b2winv is still at begin of motion
+								// not at time-of-impact location!
+								//bhitpt = b2winv * hitptWorld;
+
+//								m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld);
+							}
+					
+						}
+					}
+				}
+			}
+
+			index++;
+		}
+	};
+#endif //EDGEEDGE
+
+#define VERTEXFACE
+#ifdef VERTEXFACE
+
+	// for all vertices in A, for each face in B,do vertex-face
+	{
+		const int numvertsA = m_convexA->GetNumVertices();
+		for (int v=0;v<numvertsA;v++)
+		//int v=3;
+
+		{
+			SimdPoint3 vtx;
+			m_convexA->GetVertex(v,vtx);
+
+			vtx = a2w * vtx;//in world space
+			vtx = w2s * vtx;//in screwing space
+
+			const int numplanesB = m_convexB->GetNumPlanes();
+
+			for (int p = 0 ; p < numplanesB; p++)
+			//int p=2;
+			{
+
+				{
+				
+					SimdVector3 planeNorm;
+					SimdPoint3 planeSupport;
+
+					m_convexB->GetPlane(planeNorm,planeSupport,p);
+
+
+					planeSupport = b2w * planeSupport;//transform to world space
+					SimdVector3 planeNormWorld =  b2w.getBasis() * planeNorm;
+				
+					planeSupport =  w2s * planeSupport  ; //transform to screwing space
+					planeNorm =  w2s.getBasis() * planeNormWorld;
+
+					planeNorm.normalize();
+
+					SimdScalar d = planeSupport.dot(planeNorm);
+					
+					SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
+				
+					BU_VertexPoly vtxApolyB;
+
+					toiUnscaled = 1.;
+
+					if ((p==2) && (v==6))
+					{
+//						printf("%f toiUnscaled\n",toiUnscaled);
+
+					}
+					if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false))
+					{
+					
+
+
+						
+						if (toiUnscaled >= 0. )
+						{
+							//not only collect the first point, get every contactpoint, later we have to check the
+							//manifold properly!
+
+							if (toiUnscaled <= toiUnscaledLimit)
+							{
+	//							printf("toiUnscaled %f\n",toiUnscaled );
+
+								SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
+								SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
+
+								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
+								SimdPoint3 hitptWorld = s2w * hitpt;
+
+
+								hitpt = b2winv * hitptWorld;
+								//vertex has to be 'within' the facet's boundary
+								if (m_convexB->IsInside(hitpt,m_tolerance))
+								{
+//									m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld);
+									
+									if (toiUnscaled < result.m_fraction)
+										result.m_fraction= toiUnscaled;
+									hit = true;
+
+								}
+							}
+						}
+					}
+					
+				}
+
+				index++;
+			}
+		}
+	}
+
+	//
+	// for all vertices in B, for each face in A,do vertex-face
+	//copy and pasted from all verts A -> all planes B so potential typos!
+	//todo: make this into one method with a kind of 'swapped' logic
+	//
+	{
+		const int numvertsB = m_convexB->GetNumVertices();
+		for (int v=0;v<numvertsB;v++)
+		//int v=0;
+
+		{
+			SimdPoint3 vtx;
+			m_convexB->GetVertex(v,vtx);
+
+			vtx = b2w * vtx;//in world space
+/*
+			
+			char buf[1000];
+
+			if (vtx.y() < 0.)
+			{
+				sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z());
+				if (debugFile)
+					fwrite(buf,1,strlen(buf),debugFile);
+			}
+			sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z());
+			if (debugFile)
+				fwrite(buf,1,strlen(buf),debugFile);
+
+*/			
+			vtx = w2s * vtx;//in screwing space
+
+			const int numplanesA = m_convexA->GetNumPlanes();
+
+			for (int p = 0 ; p < numplanesA; p++)
+			//int p=2;
+			{
+
+				{
+					SimdVector3 planeNorm;
+					SimdPoint3 planeSupport;
+
+					m_convexA->GetPlane(planeNorm,planeSupport,p);
+
+
+					planeSupport = a2w * planeSupport;//transform to world space
+					SimdVector3 planeNormWorld =  a2w.getBasis() * planeNorm;
+				
+					planeSupport =  w2s * planeSupport  ; //transform to screwing space
+					planeNorm =  w2s.getBasis() * planeNormWorld;
+
+					planeNorm.normalize();
+
+					SimdScalar d = planeSupport.dot(planeNorm);
+					
+					SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
+				
+					BU_VertexPoly vtxBpolyA;
+
+					toiUnscaled = 1.;
+
+					if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true))
+					{
+						if (toiUnscaled>=0.)
+						{
+							if (toiUnscaled < toiUnscaledLimit)
+							{
+								SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
+								SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
+								//SimdScalar len =  hitNormal.length()-1;
+
+								//assert( SimdFuzzyZero(len) );
+
+								
+								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
+								SimdPoint3 hitptWorld = s2w * hitpt;
+								hitpt = a2winv * hitptWorld;
+							
+							
+								//vertex has to be 'within' the facet's boundary
+								if (m_convexA->IsInside(hitpt,m_tolerance))
+								{
+									
+//									m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld);
+									if (toiUnscaled <result.m_fraction)
+										result.m_fraction = toiUnscaled;
+									hit = true;
+								}
+							}
+						
+						}
+					
+					}
+					}
+
+			}
+		
+			index++;
+		}
+	}
+	
+
+#endif// VERTEXFACE
+
+	//the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame
+	//in addition there are contact points from previous frames
+	//we have to cleanup the manifold, using an additional epsilon/tolerance
+	//as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point
+	//else throw it away
+	
+
+	if (hit)
+	{
+
+		//try to avoid numerical drift on close contact
+		
+		if (result.m_fraction < 0.00001)
+		{
+//			printf("toiUnscaledMin< 0.00001\n");
+			impactTransA = a2w;
+			impactTransB = b2w;
+
+		} else
+		{
+
+			//SimdScalar vel = linearMotionB.length();
+			
+			//todo: check this margin
+			result.m_fraction *= 0.99f;
+
+			//move B to new position
+			impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB);
+			SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
+			ornB.normalize();
+			impactTransB.setRotation(ornB);
+
+			//now transform A
+			SimdTransform a2s,a2b;
+			a2s.mult( w2s , a2w);
+			a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
+			a2s.multInverseLeft(w2s,a2s);
+			a2b.multInverseLeft(b2w, a2s);
+
+			//transform by motion B
+			impactTransA.mult(impactTransB, a2b);
+			//normalize rotation
+			SimdQuaternion orn;
+			impactTransA.getBasis().getRotation(orn);
+			orn.normalize();
+			impactTransA.setBasis(SimdMatrix3x3(orn));
+		}
+	}
+
+/*
+	{
+		const int numvertsB = m_convexB->GetNumVertices();
+		for (int v=0;v<numvertsB;v++)
+		{
+			SimdPoint3 pt;
+			m_convexB->GetVertex(v,pt);
+			pt = impactTransB * pt;
+			char buf[1000];
+
+			if (pt.y() < 0.)
+			{
+				sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
+				if (debugFile)
+					fwrite(buf,1,strlen(buf),debugFile);
+			}
+			else
+			{
+				sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
+				if (debugFile)
+					fwrite(buf,1,strlen(buf),debugFile);
+			}
+		}
+	}
+*/
+	return hit;
+}
+
+

Bullet/NarrowPhaseCollision/BU_CollisionPair.h

@@ -0,0 +1,54 @@
+/*
+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 BU_COLLISIONPAIR
+#define BU_COLLISIONPAIR
+
+#include <NarrowPhaseCollision/BU_Screwing.h>
+#include <NarrowPhaseCollision/ConvexCast.h>
+
+
+#include <SimdQuaternion.h>
+
+class PolyhedralConvexShape;
+
+
+///BU_CollisionPair implements collision algorithm for algebraic time of impact calculation of feature based shapes.
+class BU_CollisionPair : public ConvexCast
+{
+	
+public:
+	BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance=0.2f);
+	//toi
+
+	virtual bool	calcTimeOfImpact(
+					const SimdTransform& fromA,
+					const SimdTransform& toA,
+					const SimdTransform& fromB,
+					const SimdTransform& toB,
+					CastResult& result);
+
+	
+	
+
+private:
+	const PolyhedralConvexShape*	m_convexA;
+	const PolyhedralConvexShape*	m_convexB;
+	BU_Screwing	m_screwing;
+	SimdScalar	m_tolerance;
+	
+};
+#endif //BU_COLLISIONPAIR

Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp

@@ -0,0 +1,578 @@
+/*
+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 "BU_EdgeEdge.h"
+#include "BU_Screwing.h"
+#include <SimdPoint3.h>
+#include <SimdPoint3.h>
+
+//#include "BU_IntervalArithmeticPolynomialSolver.h"
+#include "BU_AlgebraicPolynomialSolver.h"
+
+#define USE_ALGEBRAIC
+#ifdef USE_ALGEBRAIC	
+#define BU_Polynomial BU_AlgebraicPolynomialSolver
+#else	
+#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
+#endif
+
+BU_EdgeEdge::BU_EdgeEdge()
+{
+}
+
+
+bool BU_EdgeEdge::GetTimeOfImpact(
+								  const BU_Screwing& screwAB,
+								  const SimdPoint3& a,//edge in object A
+								  const SimdVector3& u,
+								  const SimdPoint3& c,//edge in object B
+								  const SimdVector3& v,
+								  SimdScalar &minTime,
+								  SimdScalar &lambda1,
+								  SimdScalar& mu1
+								  
+								  )
+{
+	bool hit=false;
+	
+	SimdScalar lambda;
+	SimdScalar mu;
+	
+	const SimdScalar w=screwAB.GetW();
+	const SimdScalar s=screwAB.GetS();
+	
+	if (SimdFuzzyZero(s) &&
+		SimdFuzzyZero(w))
+	{
+		//no motion, no collision
+		return false;
+	}
+	
+	if (SimdFuzzyZero(w) )
+	{
+		//pure translation W=0, S <> 0
+		//no trig, f(t)=t
+		SimdScalar det = u.y()*v.x()-u.x()*v.y();
+		if (!SimdFuzzyZero(det))
+		{		
+			lambda = (a.x()*v.y() - c.x() * v.y() - v.x() * a.y() + v.x() * c.y()) / det;
+			mu = (u.y() * a.x() - u.y() * c.x() - u.x() * a.y() + u.x() * c.y()) / det;
+
+			if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1)
+			{
+				// single potential collision is
+				SimdScalar t = (c.z()-a.z()+mu*v.z()-lambda*u.z())/s;
+				//if this is on the edge, and time t within [0..1] report hit
+				if (t>=0 && t <= minTime)
+				{
+					hit = true;
+					lambda1 = lambda;
+					mu1 = mu;
+					minTime=t;
+				}
+			}
+			
+		} else
+		{
+			//parallel case, not yet
+		}
+	} else
+	{
+		if (SimdFuzzyZero(s) )
+		{
+			if (SimdFuzzyZero(u.z()) )
+			{
+				if (SimdFuzzyZero(v.z()) )
+				{
+					//u.z()=0,v.z()=0
+					if (SimdFuzzyZero(a.z()-c.z()))
+					{
+						//printf("NOT YET planar problem, 4 vertex=edge cases\n");
+						
+					} else
+					{
+						//printf("parallel but distinct planes, no collision\n");
+						return false;
+					}
+					
+				} else
+				{
+					SimdScalar mu = (a.z() - c.z())/v.z();
+					if (0<=mu && mu <= 1)
+					{
+					//	printf("NOT YET//u.z()=0,v.z()<>0\n");
+					} else
+					{
+						return false;
+					}
+					
+				}
+			} else
+			{
+				//u.z()<>0
+				
+				if (SimdFuzzyZero(v.z()) )
+				{
+					//printf("u.z()<>0,v.z()=0\n");
+					lambda =  (c.z() - a.z())/u.z();
+					if (0<=lambda && lambda <= 1)
+					{
+						//printf("u.z()<>0,v.z()=0\n");
+						SimdPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
+						SimdScalar r2 = rotPt.length2();//px*px + py*py;
+						
+						//either y=a*x+b, or x = a*x+b...
+						//depends on whether value v.x() is zero or not
+						SimdScalar aa;
+						SimdScalar bb;
+						
+						if (SimdFuzzyZero(v.x()))
+						{
+							aa = v.x()/v.y();
+							bb= c.x()+  (-c.y() /v.y()) *v.x();
+						} else
+						{
+							//line is c+mu*v;
+							//x = c.x()+mu*v.x();
+							//mu = ((x-c.x())/v.x());
+							//y = c.y()+((x-c.x())/v.x())*v.y();
+							//y = c.y()+  (-c.x() /v.x()) *v.y() + (x /v.x())   *v.y();
+							//y = a*x+b,where a = v.y()/v.x(), b= c.y()+  (-c.x() /v.x()) *v.y();
+							aa = v.y()/v.x();
+							bb= c.y()+  (-c.x() /v.x()) *v.y();
+						}
+						
+						SimdScalar disc = aa*aa*r2 + r2 - bb*bb;
+						if (disc <0)
+						{
+							//edge doesn't intersect the circle (motion of the vertex)
+							return false;
+						}
+						SimdScalar rad = SimdSqrt(r2);
+						
+						if (SimdFuzzyZero(disc))
+						{
+							SimdPoint3 intersectPt;
+							
+							SimdScalar mu;
+							//intersectionPoint edge with circle;
+							if (SimdFuzzyZero(v.x()))
+							{
+								intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1)));
+								intersectPt.setX( aa*intersectPt.y()+bb );
+								mu = ((intersectPt.y()-c.y())/v.y());
+							} else
+							{
+								intersectPt.setX((-2*aa*bb)/(2*(aa*aa+1)));
+								intersectPt.setY(aa*intersectPt.x()+bb);
+								mu = ((intersectPt.getX()-c.getX())/v.getX());
+								
+							}
+							
+							if (0 <= mu && mu <= 1)
+							{
+								hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
+							}
+							//only one solution
+						} else
+						{
+							//two points...
+							//intersectionPoint edge with circle;
+							SimdPoint3 intersectPt;
+							//intersectionPoint edge with circle;
+							if (SimdFuzzyZero(v.x()))
+							{
+								SimdScalar mu;
+								
+								intersectPt.setY((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
+								intersectPt.setX(aa*intersectPt.y()+bb);
+								mu = ((intersectPt.getY()-c.getY())/v.getY());
+								if (0.f <= mu && mu <= 1.f)
+								{
+									hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
+								}
+								intersectPt.setY((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
+								intersectPt.setX(aa*intersectPt.y()+bb);
+								mu = ((intersectPt.getY()-c.getY())/v.getY());
+								if (0 <= mu && mu <= 1)
+								{
+									hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
+								}
+								
+							} else
+							{
+								SimdScalar mu;
+								
+								intersectPt.setX((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2*(aa*aa+1.f)));
+								intersectPt.setY(aa*intersectPt.x()+bb);
+								mu = ((intersectPt.getX()-c.getX())/v.getX());
+								if (0 <= mu && mu <= 1)
+								{
+									hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
+								}
+								intersectPt.setX((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
+								intersectPt.setY(aa*intersectPt.x()+bb);
+								mu = ((intersectPt.getX()-c.getX())/v.getX());
+								if (0.f <= mu && mu <= 1.f)
+								{
+									hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
+								}
+							}
+						}
+						
+						
+						
+						//int k=0;
+						
+					} else
+					{
+						return false;
+					}
+					
+					
+				} else
+				{
+					//u.z()<>0,v.z()<>0
+					//printf("general case with s=0\n");
+					hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
+					if (hit)
+					{
+						lambda1 = lambda;
+						mu1 = mu;
+						
+					}
+				}
+			}
+			
+		} else
+		{
+			//printf("general case, W<>0,S<>0\n");
+			hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
+			if (hit)
+			{
+				lambda1 = lambda;
+				mu1 = mu;
+			}
+			
+		}
+		
+		
+		//W <> 0,pure rotation
+	}
+	
+	return hit;
+}
+
+
+bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
+											 const BU_Screwing& screwAB,
+											 const SimdPoint3& a,//edge in object A
+											 const SimdVector3& u,
+											 const SimdPoint3& c,//edge in object B
+											 const SimdVector3& v,
+											 SimdScalar &minTime,
+											 SimdScalar &lambda,
+											 SimdScalar& mu
+											 
+											 )
+{
+	bool hit = false;
+	
+	SimdScalar coefs[4]={0.f,0.f,0.f,0.f};
+	BU_Polynomial polynomialSolver;
+	int numroots = 0;
+	
+	//SimdScalar eps=1e-15f;
+	//SimdScalar eps2=1e-20f;
+	SimdScalar s=screwAB.GetS();
+	SimdScalar w = screwAB.GetW();
+	
+	SimdScalar ax = a.x();
+	SimdScalar ay = a.y();
+	SimdScalar az = a.z();
+	SimdScalar cx = c.x();
+	SimdScalar cy = c.y();
+	SimdScalar cz = c.z();
+	SimdScalar vx = v.x();
+	SimdScalar vy = v.y();
+	SimdScalar vz = v.z();
+	SimdScalar ux = u.x();
+	SimdScalar uy = u.y();
+	SimdScalar uz = u.z();
+	
+	
+	if (!SimdFuzzyZero(v.z()))
+	{
+		
+		//Maple Autogenerated C code
+		SimdScalar t1,t2,t3,t4,t7,t8,t10;
+		SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
+		SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
+		SimdScalar t31,t32,t33,t34,t35,t36,t39,t40;
+		SimdScalar t41,t43,t48;
+		SimdScalar t63;
+		
+		SimdScalar aa,bb,cc,dd;//the coefficients
+		
+		t1 = v.y()*s;      t2 = t1*u.x();
+		t3 = v.x()*s;
+		t4 = t3*u.y();
+		t7 = SimdTan(w/2.0f);
+		t8 = 1.0f/t7;
+		t10 = 1.0f/v.z();
+		aa = (t2-t4)*t8*t10;
+		t13 = a.x()*t7;
+		t14 = u.z()*v.y();
+		t15 = t13*t14;
+		t16 = u.x()*v.z();
+		t17 = a.y()*t7;
+		t18 = t16*t17;
+		t19 = u.y()*v.z();
+		t20 = t13*t19;
+		t21 = v.y()*u.x();
+		t22 = c.z()*t7;
+		t23 = t21*t22;
+		t24 = v.x()*a.z();
+		t25 = t7*u.y();
+		t26 = t24*t25;
+		t27 = c.y()*t7;
+		t28 = t16*t27;
+		t29 = a.z()*t7;
+		t30 = t21*t29;
+		t31 = u.z()*v.x();
+		t32 = t31*t27;
+		t33 = t31*t17;
+		t34 = c.x()*t7;
+		t35 = t34*t19;
+		t36 = t34*t14;
+		t39 = v.x()*c.z();
+		t40 = t39*t25;
+		t41 = 2.0f*t1*u.y()-t15+t18-t20-t23-t26+t28+t30+t32+t33-t35-t36+2.0f*t3*u.x()+t40;
+		bb = t41*t8*t10;
+		t43 = t7*u.x();
+		t48 = u.y()*v.y();
+		cc = (-2.0f*t39*t43+2.0f*t24*t43+t4-2.0f*t48*t22+2.0f*t34*t16-2.0f*t31*t13-t2
+			-2.0f*t17*t14+2.0f*t19*t27+2.0f*t48*t29)*t8*t10;
+		t63 = -t36+t26+t32-t40+t23+t35-t20+t18-t28-t33+t15-t30;
+		dd = t63*t8*t10;
+		
+		coefs[0]=aa;
+		coefs[1]=bb;
+		coefs[2]=cc;
+		coefs[3]=dd;
+		
+	} else
+	{
+		
+		SimdScalar t1,t2,t3,t4,t7,t8,t10;
+		SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
+		SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
+		SimdScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
+		SimdScalar p1,p2,p3,p4;
+
+	  t1 = uy*s;
+      t2 = t1*vx;
+      t3 = ux*s;
+      t4 = t3*vy;
+      t7 = SimdTan(w/2.0f);
+      t8 = 1/t7;
+      t10 = 1/uz;
+      t13 = ux*az;
+      t14 = t7*vy;
+      t15 = t13*t14;
+      t16 = ax*t7;
+      t17 = uy*vz;
+      t18 = t16*t17;
+      t19 = cx*t7;
+      t20 = t19*t17;
+      t21 = vy*uz;
+      t22 = t19*t21;
+      t23 = ay*t7;
+      t24 = vx*uz;
+      t25 = t23*t24;
+      t26 = uy*cz;
+      t27 = t7*vx;
+      t28 = t26*t27;
+      t29 = t16*t21;
+      t30 = cy*t7;
+      t31 = ux*vz;
+      t32 = t30*t31;
+      t33 = ux*cz;
+      t34 = t33*t14;
+      t35 = t23*t31;
+      t36 = t30*t24;
+      t37 = uy*az;
+      t38 = t37*t27;
+
+	  p4 = (-t2+t4)*t8*t10;
+      p3 = 2.0f*t1*vy+t15-t18-t20-t22+t25+t28-t29+t32-t34+t35+t36-t38+2.0f*t3*vx;
+      p2 = -2.0f*t33*t27-2.0f*t26*t14-2.0f*t23*t21+2.0f*t37*t14+2.0f*t30*t17+2.0f*t13
+*t27+t2-t4+2.0f*t19*t31-2.0f*t16*t24;
+      t57 = -t22+t29+t36-t25-t32+t34+t35-t28-t15+t20-t18+t38;
+      p1 = t57*t8*t10;
+
+	coefs[0] = p4;
+	coefs[1] = p3;
+	coefs[2] = p2;
+	coefs[1] = p1;
+		
+	}
+	
+	numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
+	
+	for (int i=0;i<numroots;i++)
+	{
+		//SimdScalar tau = roots[i];//polynomialSolver.GetRoot(i);
+		SimdScalar tau = polynomialSolver.GetRoot(i);
+		
+		//check whether mu and lambda are in range [0..1]
+		
+		if (!SimdFuzzyZero(v.z()))
+		{
+			SimdScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz);
+			SimdScalar B1=((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+
+				vx*az*SimdTan(1.f/2.f*w)-vx*cz*SimdTan(1.f/2.f*w)+
+				vx*s*tau)/SimdTan(1.f/2.f*w)/vz)-(ax-ax*tau*tau-2.f*tau*ay);
+			lambda = B1/A1;
+			
+			mu = (a.z()-c.z()+lambda*u.z()+(s*tau)/(SimdTan(w/2.f)))/v.z();
+			
+			
+			//double check in original equation
+			
+			SimdScalar lhs = (a.x()+lambda*u.x())
+				*((1.f-tau*tau)/(1.f+tau*tau))-
+				(a.y()+lambda*u.y())*((2.f*tau)/(1.f+tau*tau));
+			
+			lhs = lambda*((ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz));
+			
+			SimdScalar rhs = c.x()+mu*v.x();
+			
+			rhs = ((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+vx*az*SimdTan(1.f/2.f*w)-
+				vx*cz*SimdTan(1.f/2.f*w)+vx*s*tau)/(SimdTan(1.f/2.f*w)*vz))-
+				
+				(ax-ax*tau*tau-2.f*tau*ay);
+			
+			/*SimdScalar res = coefs[0]*tau*tau*tau+
+				coefs[1]*tau*tau+
+				coefs[2]*tau+
+				coefs[3];*/
+			
+			//lhs should be rhs !
+			
+			if (0.<= mu && mu <=1 && 0.<=lambda && lambda <= 1)
+			{
+				
+			} else
+			{
+				//skip this solution, not really touching
+				continue;				
+			}
+			
+		}
+		
+		SimdScalar t = 2.f*SimdAtan(tau)/screwAB.GetW();
+		//tau = tan (wt/2) so 2*atan (tau)/w
+		if (t>=0.f && t<minTime)
+		{
+#ifdef STATS_EDGE_EDGE
+			printf(" ax = %12.12f\n ay = %12.12f\n az = %12.12f\n",a.x(),a.y(),a.z());
+			printf(" ux = %12.12f\n uy = %12.12f\n uz = %12.12f\n",u.x(),u.y(),u.z());
+			printf(" cx = %12.12f\n cy = %12.12f\n cz = %12.12f\n",c.x(),c.y(),c.z());
+			printf(" vx = %12.12f\n vy = %12.12f\n vz = %12.12f\n",v.x(),v.y(),v.z());
+			printf(" s  = %12.12f\n w  = %12.12f\n",       s,     w);
+			
+			printf(" tau = %12.12f \n lambda = %12.12f \n mu = %f\n",tau,lambda,mu); 
+			printf(" ---------------------------------------------\n"); 
+			
+#endif
+			
+			//	v,u,a,c,s,w
+			
+			//	BU_IntervalArithmeticPolynomialSolver iaSolver;
+			//	int numroots2 = iaSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
+			
+			minTime = t;
+			hit = true;
+		}
+	}
+	
+	return hit;
+}
+
+
+//C -S
+//S C
+
+bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime)
+{
+	bool hit = false;
+	
+	// now calculate the planeEquation for the vertex motion,
+	// and check if the intersectionpoint is at the positive side
+	SimdPoint3 rotPt1(SimdCos(rotW)*rotPt.x()-SimdSin(rotW)*rotPt.y(),
+	SimdSin(rotW)*rotPt.x()+SimdCos(rotW)*rotPt.y(),
+	0.f);
+
+	SimdVector3 rotVec = rotPt1-rotPt;
+	
+	SimdVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f);
+	
+	//SimdPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
+	SimdScalar planeD = planeNormal.dot(rotPt1);
+	
+	SimdScalar dist = (planeNormal.dot(intersectPt)-planeD);
+	hit = (dist >= -0.001);
+	
+	//if (hit)
+	{
+		//		minTime = 0;
+		//calculate the time of impact, using the fact of
+		//toi = alpha / screwAB.getW();
+		// cos (alpha) = adjacent/hypothenuse;
+		//adjacent = dotproduct(ipedge,point);
+		//hypothenuse = sqrt(r2);
+		SimdScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
+		SimdScalar hypo = rotRadius;
+		SimdScalar alpha = SimdAcos(adjacent/hypo);
+		SimdScalar t = alpha / rotW;
+		if (t >= 0 && t < minTime)
+		{
+			hit = true;
+			minTime = t;
+		} else
+		{
+			hit = false;
+		}
+		
+	}
+	return hit;
+}
+
+bool BU_EdgeEdge::GetTimeOfImpactVertexEdge(
+											const BU_Screwing& screwAB,
+											const SimdPoint3& a,//edge in object A
+											const SimdVector3& u,
+											const SimdPoint3& c,//edge in object B
+											const SimdVector3& v,
+											SimdScalar &minTime,
+											SimdScalar &lamda,
+											SimdScalar& mu
+											
+											)
+{
+	return false;
+}

Bullet/NarrowPhaseCollision/BU_EdgeEdge.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 BU_EDGEEDGE
+#define BU_EDGEEDGE
+
+class BU_Screwing;
+#include <SimdTransform.h>
+#include <SimdPoint3.h>
+#include <SimdVector3.h>
+
+//class BUM_Point2;
+
+#include <SimdScalar.h>
+
+///BU_EdgeEdge implements algebraic time of impact calculation between two (angular + linear) moving edges.
+class BU_EdgeEdge
+{
+public:
+	
+
+	BU_EdgeEdge();
+	bool GetTimeOfImpact(
+		const BU_Screwing& screwAB,
+		const SimdPoint3& a,//edge in object A
+		const SimdVector3& u,
+		const SimdPoint3& c,//edge in object B
+		const SimdVector3& v,
+		SimdScalar &minTime,
+		SimdScalar &lamda,
+		SimdScalar& mu
+		);
+private:
+
+	bool Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime);
+	bool GetTimeOfImpactGeneralCase(
+		const BU_Screwing& screwAB,
+		const SimdPoint3& a,//edge in object A
+		const SimdVector3& u,
+		const SimdPoint3& c,//edge in object B
+		const SimdVector3& v,
+		SimdScalar &minTime,
+		SimdScalar &lamda,
+		SimdScalar& mu
+
+		);
+
+	
+	bool GetTimeOfImpactVertexEdge(
+		const BU_Screwing& screwAB,
+		const SimdPoint3& a,//edge in object A
+		const SimdVector3& u,
+		const SimdPoint3& c,//edge in object B
+		const SimdVector3& v,
+		SimdScalar &minTime,
+		SimdScalar &lamda,
+		SimdScalar& mu
+
+		);
+
+};
+
+#endif //BU_EDGEEDGE

Bullet/NarrowPhaseCollision/BU_MotionStateInterface.h

@@ -0,0 +1,50 @@
+/*
+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 BU_MOTIONSTATE
+#define BU_MOTIONSTATE
+
+
+#include <SimdTransform.h>
+#include <SimdPoint3.h>
+#include <SimdQuaternion.h>
+
+class BU_MotionStateInterface
+{
+public:
+	virtual ~BU_MotionStateInterface(){};
+
+	virtual void	SetTransform(const SimdTransform& trans) = 0;
+	virtual void	GetTransform(SimdTransform& trans) const = 0; 
+
+	virtual void	SetPosition(const SimdPoint3& position) = 0;
+	virtual void	GetPosition(SimdPoint3& position) const = 0; 
+
+	virtual void	SetOrientation(const SimdQuaternion& orientation) = 0;
+	virtual void	GetOrientation(SimdQuaternion& orientation) const = 0; 
+
+	virtual void	SetBasis(const SimdMatrix3x3& basis) = 0;
+	virtual void	GetBasis(SimdMatrix3x3& basis) const = 0; 
+
+	virtual void	SetLinearVelocity(const SimdVector3& linvel) = 0;
+	virtual void	GetLinearVelocity(SimdVector3& linvel) const = 0;
+	
+	virtual void	GetAngularVelocity(SimdVector3& angvel) const = 0;
+	virtual void	SetAngularVelocity(const SimdVector3& angvel) = 0;
+
+};
+
+#endif //BU_MOTIONSTATE

Bullet/NarrowPhaseCollision/BU_PolynomialSolverInterface.h

@@ -0,0 +1,39 @@
+/*
+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 BUM_POLYNOMIAL_SOLVER_INTERFACE
+#define BUM_POLYNOMIAL_SOLVER_INTERFACE
+
+#include <SimdScalar.h>
+//
+//BUM_PolynomialSolverInterface is interface class for polynomial root finding.
+//The number of roots is returned as a result, query GetRoot to get the actual solution.
+//
+class BUM_PolynomialSolverInterface
+{
+public:
+	virtual ~BUM_PolynomialSolverInterface() {};
+
+	
+//	virtual int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) = 0;
+	
+	virtual int	Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c) = 0;
+	
+	virtual int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d) = 0;
+
+	virtual SimdScalar GetRoot(int i) const = 0;
+
+};
+
+#endif //BUM_POLYNOMIAL_SOLVER_INTERFACE

Bullet/NarrowPhaseCollision/BU_Screwing.cpp

@@ -0,0 +1,200 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Stephane Redon / 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 "BU_Screwing.h"
+
+
+BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel) {
+
+
+	const SimdScalar dx=relLinVel[0];
+	const SimdScalar dy=relLinVel[1];
+	const SimdScalar dz=relLinVel[2];
+	const SimdScalar wx=relAngVel[0];
+	const SimdScalar wy=relAngVel[1];
+	const SimdScalar wz=relAngVel[2];
+
+	// Compute the screwing parameters :
+	// w : total amount of rotation
+	// s : total amount of translation
+	// u : vector along the screwing axis (||u||=1)
+	// o : point on the screwing axis
+	
+	m_w=SimdSqrt(wx*wx+wy*wy+wz*wz);
+	//if (!w) {
+	if (fabs(m_w)<SCREWEPSILON ) {
+
+		assert(m_w == 0.f);
+
+		m_w=0.;
+		m_s=SimdSqrt(dx*dx+dy*dy+dz*dz);
+		if (fabs(m_s)<SCREWEPSILON ) {
+			assert(m_s == 0.);
+
+			m_s=0.;
+			m_u=SimdPoint3(0.,0.,1.);
+			m_o=SimdPoint3(0.,0.,0.);
+		}
+		else {
+			float t=1.f/m_s;
+			m_u=SimdPoint3(dx*t,dy*t,dz*t);
+			m_o=SimdPoint3(0.f,0.f,0.f);
+		}
+	}
+	else { // there is some rotation
+		
+		// we compute u
+		
+		float v(1.f/m_w);
+		m_u=SimdPoint3(wx*v,wy*v,wz*v); // normalization
+		
+		// decomposition of the translation along u and one orthogonal vector
+		
+		SimdPoint3 t(dx,dy,dz);
+		m_s=t.dot(m_u); // component along u
+		if (fabs(m_s)<SCREWEPSILON)
+		{
+			//printf("m_s component along u < SCREWEPSILION\n");
+			m_s=0.f;
+		}
+		SimdPoint3 n1(t-(m_s*m_u)); // the remaining part (which is orthogonal to u)
+		
+		// now we have to compute o
+		
+		//SimdScalar len = n1.length2();
+		//(len >= BUM_EPSILON2) {
+		if (n1[0] || n1[1] || n1[2]) { // n1 is not the zero vector
+			n1.normalize();
+			SimdVector3 n1orth=m_u.cross(n1);
+
+			float n2x=SimdCos(0.5f*m_w);
+			float n2y=SimdSin(0.5f*m_w);
+			
+			m_o=0.5f*t.dot(n1)*(n1+n2x/n2y*n1orth);
+		}
+		else 
+		{
+			m_o=SimdPoint3(0.f,0.f,0.f);
+		}
+		
+	}
+	
+}
+
+//Then, I need to compute Pa, the matrix from the reference (global) frame to
+//the screwing frame :
+
+
+void BU_Screwing::LocalMatrix(SimdTransform &t) const {
+//So the whole computations do this : align the Oz axis along the
+//	screwing axis (thanks to u), and then find two others orthogonal axes to
+//	complete the basis.
+		
+		if ((m_u[0]>SCREWEPSILON)||(m_u[0]<-SCREWEPSILON)||(m_u[1]>SCREWEPSILON)||(m_u[1]<-SCREWEPSILON)) 
+		{ 
+			// to avoid numerical problems
+			float n=SimdSqrt(m_u[0]*m_u[0]+m_u[1]*m_u[1]);
+			float invn=1.0f/n;
+			SimdMatrix3x3 mat;
+
+	  		mat[0][0]=-m_u[1]*invn;
+			mat[0][1]=m_u[0]*invn;
+			mat[0][2]=0.f;
+	
+			mat[1][0]=-m_u[0]*invn*m_u[2];
+			mat[1][1]=-m_u[1]*invn*m_u[2];
+			mat[1][2]=n;
+
+			mat[2][0]=m_u[0];
+			mat[2][1]=m_u[1];
+			mat[2][2]=m_u[2];
+			
+			t.setOrigin(SimdPoint3(
+				  m_o[0]*m_u[1]*invn-m_o[1]*m_u[0]*invn,
+				-(m_o[0]*mat[1][0]+m_o[1]*mat[1][1]+m_o[2]*n),
+				-(m_o[0]*m_u[0]+m_o[1]*m_u[1]+m_o[2]*m_u[2])));
+
+			t.setBasis(mat);
+
+		}
+		else {
+
+			SimdMatrix3x3 m;
+
+			m[0][0]=1.;
+			m[1][0]=0.;
+			m[2][0]=0.;
+
+			m[0][1]=0.f;
+			m[1][1]=float(SimdSign(m_u[2]));
+			m[2][1]=0.f;
+
+			m[0][2]=0.f;
+			m[1][2]=0.f;
+			m[2][2]=float(SimdSign(m_u[2]));
+
+			t.setOrigin(SimdPoint3(
+					-m_o[0],
+					-SimdSign(m_u[2])*m_o[1],
+					-SimdSign(m_u[2])*m_o[2]
+				));
+			t.setBasis(m);
+
+		}
+}
+
+//gives interpolated transform for time in [0..1] in screwing frame
+SimdTransform	BU_Screwing::InBetweenTransform(const SimdTransform& tr,SimdScalar t) const
+{
+	SimdPoint3 org = tr.getOrigin();
+
+	SimdPoint3 neworg (
+	org.x()*SimdCos(m_w*t)-org.y()*SimdSin(m_w*t),
+	org.x()*SimdSin(m_w*t)+org.y()*SimdCos(m_w*t),
+	org.z()+m_s*CalculateF(t));
+		
+	SimdTransform newtr;
+	newtr.setOrigin(neworg);
+	SimdMatrix3x3 basis = tr.getBasis();
+	SimdMatrix3x3 basisorg = tr.getBasis();
+
+	SimdQuaternion rot(SimdVector3(0.,0.,1.),m_w*t);
+	SimdQuaternion tmpOrn;
+	tr.getBasis().getRotation(tmpOrn);
+	rot = rot *  tmpOrn;
+
+	//to avoid numerical drift, normalize quaternion
+	rot.normalize();
+	newtr.setBasis(SimdMatrix3x3(rot));
+	return newtr;
+
+}
+
+
+SimdScalar BU_Screwing::CalculateF(SimdScalar t) const
+{
+	SimdScalar result;
+	if (!m_w)
+	{
+		result = t;
+	} else
+	{
+		result = ( SimdTan((m_w*t)/2.f) / SimdTan(m_w/2.f));
+	}
+	return result;
+}
+	

Bullet/NarrowPhaseCollision/BU_Screwing.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 B_SCREWING_H
+#define B_SCREWING_H
+
+
+#include <SimdVector3.h>
+#include <SimdPoint3.h>
+#include <SimdTransform.h>
+
+
+#define SCREWEPSILON 0.00001f
+
+///BU_Screwing implements screwing motion interpolation.
+class BU_Screwing
+{
+public:
+
+	
+	BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel);
+
+	~BU_Screwing() {
+	};
+	
+	SimdScalar CalculateF(SimdScalar t) const;
+	//gives interpolated position for time in [0..1] in screwing frame
+
+	inline SimdPoint3	InBetweenPosition(const SimdPoint3& pt,SimdScalar t) const
+	{
+		return SimdPoint3(
+		pt.x()*SimdCos(m_w*t)-pt.y()*SimdSin(m_w*t),
+		pt.x()*SimdSin(m_w*t)+pt.y()*SimdCos(m_w*t),
+		pt.z()+m_s*CalculateF(t));
+	}
+
+	inline SimdVector3	InBetweenVector(const SimdVector3& vec,SimdScalar t) const
+	{
+		return SimdVector3(
+		vec.x()*SimdCos(m_w*t)-vec.y()*SimdSin(m_w*t),
+		vec.x()*SimdSin(m_w*t)+vec.y()*SimdCos(m_w*t),
+		vec.z());
+	}
+
+	//gives interpolated transform for time in [0..1] in screwing frame
+	SimdTransform	InBetweenTransform(const SimdTransform& tr,SimdScalar t) const;
+
+	
+	//gives matrix from global frame into screwing frame
+	void	LocalMatrix(SimdTransform &t) const;
+
+	inline const SimdVector3& GetU() const {	return m_u;}
+	inline const SimdVector3& GetO() const {return m_o;}
+	inline const SimdScalar GetS() const{ return m_s;}
+	inline const SimdScalar GetW() const { return m_w;}
+	
+private:
+	float		m_w;
+	float		m_s;
+	SimdVector3 m_u;
+	SimdVector3	m_o;
+};
+
+#endif //B_SCREWING_H

Bullet/NarrowPhaseCollision/BU_StaticMotionState.h

@@ -0,0 +1,91 @@
+/*
+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 BU_STATIC_MOTIONSTATE
+#define BU_STATIC_MOTIONSTATE
+
+
+#include <CollisionShapes/BU_MotionStateInterface.h>
+
+class BU_StaticMotionState :public BU_MotionStateInterface
+{
+public:
+	virtual ~BU_StaticMotionState(){};
+
+	virtual void	SetTransform(const SimdTransform& trans)
+	{
+		m_trans = trans;
+	}
+	virtual void	GetTransform(SimdTransform& trans) const
+	{
+		trans = m_trans;
+	}
+	virtual void	SetPosition(const SimdPoint3& position)
+	{
+		m_trans.setOrigin( position );
+	}
+	virtual void	GetPosition(SimdPoint3& position) const
+	{
+		position = m_trans.getOrigin();
+	}
+
+	virtual void	SetOrientation(const SimdQuaternion& orientation)
+	{
+		m_trans.setRotation( orientation);
+	}
+	virtual void	GetOrientation(SimdQuaternion& orientation) const
+	{
+		orientation = m_trans.getRotation();
+	}
+
+	virtual void	SetBasis(const SimdMatrix3x3& basis)
+	{
+		m_trans.setBasis( basis);
+	}
+	virtual void	GetBasis(SimdMatrix3x3& basis) const
+	{ 
+		basis = m_trans.getBasis();
+	}
+
+	virtual void	SetLinearVelocity(const SimdVector3& linvel)
+	{
+		m_linearVelocity = linvel;
+	}
+	virtual void	GetLinearVelocity(SimdVector3& linvel) const
+	{
+		linvel = m_linearVelocity;
+	}
+	
+	virtual void	SetAngularVelocity(const SimdVector3& angvel)
+	{
+		m_angularVelocity = angvel;
+	}
+	virtual void	GetAngularVelocity(SimdVector3& angvel) const
+	{
+		angvel = m_angularVelocity;
+	}
+
+
+
+protected:
+
+	SimdTransform	m_trans;
+	SimdVector3		m_angularVelocity;
+	SimdVector3		m_linearVelocity;
+
+};
+
+#endif //BU_STATIC_MOTIONSTATE

Bullet/NarrowPhaseCollision/BU_VertexPoly.cpp

@@ -0,0 +1,159 @@
+/*
+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 "BU_VertexPoly.h"
+#include "BU_Screwing.h"
+#include <SimdTransform.h>
+#include <SimdPoint3.h>
+#include <SimdVector3.h>
+
+#define USE_ALGEBRAIC
+#ifdef USE_ALGEBRAIC
+	#include "BU_AlgebraicPolynomialSolver.h"
+	#define BU_Polynomial BU_AlgebraicPolynomialSolver
+#else
+	#include "BU_IntervalArithmeticPolynomialSolver.h"
+	#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
+#endif
+
+inline bool      TestFuzzyZero(SimdScalar x) { return SimdFabs(x) < 0.0001f; }
+
+
+BU_VertexPoly::BU_VertexPoly()
+{
+
+}
+//return true if a collision will occur between [0..1]
+//false otherwise. If true, minTime contains the time of impact
+bool BU_VertexPoly::GetTimeOfImpact(
+		const BU_Screwing& screwAB,
+		const SimdPoint3& a,
+		const SimdVector4& planeEq,
+		SimdScalar &minTime,bool swapAB)
+{
+
+	bool hit = false;
+
+	// precondition: s=0 and w= 0 is catched by caller!
+	if (TestFuzzyZero(screwAB.GetS()) &&
+		TestFuzzyZero(screwAB.GetW()))
+	{
+		return false;
+	}
+
+
+	//case w<>0 and s<> 0
+	const SimdScalar w=screwAB.GetW();
+	const SimdScalar s=screwAB.GetS();
+
+	SimdScalar coefs[4];
+	const SimdScalar p=planeEq[0];
+	const SimdScalar q=planeEq[1];
+	const SimdScalar r=planeEq[2];
+	const SimdScalar d=planeEq[3];
+
+	const SimdVector3 norm(p,q,r);
+	BU_Polynomial polynomialSolver;
+	int numroots = 0;
+
+	//SimdScalar eps=1e-80f;
+	//SimdScalar eps2=1e-100f;
+	
+	if (TestFuzzyZero(screwAB.GetS()) )	
+	{
+		//S = 0 , W <> 0
+		
+		//ax^3+bx^2+cx+d=0
+		coefs[0]=0.;
+		coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d);
+		coefs[2]=-2*p*a.y()+2*q*a.x();
+		coefs[3]=p*a.x()+q*a.y()+r*a.z()-d;
+
+//		numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
+		numroots = polynomialSolver.Solve2QuadraticFull(coefs[1],coefs[2],coefs[3]);
+
+	} else
+	{
+		if (TestFuzzyZero(screwAB.GetW()))
+		{
+			// W = 0 , S <> 0
+			//pax+qay+r(az+st)=d
+			
+			SimdScalar dist = (d - a.dot(norm));
+
+			if (TestFuzzyZero(r))
+			{
+				if (TestFuzzyZero(dist))
+				{
+					// no hit
+				} else
+				{
+					// todo a a' might hit sides of polygon T
+					//printf("unhandled case, w=0,s<>0,r<>0, a a' might hit sides of polygon T \n");
+				}
+				
+			} else
+			{
+				SimdScalar etoi = (dist)/(r*screwAB.GetS());
+				if (swapAB)
+					etoi *= -1;
+
+				if (etoi >= 0. && etoi <= minTime)
+				{
+					minTime = etoi;
+					hit = true;
+				}
+			}
+
+		} else
+		{
+				//ax^3+bx^2+cx+d=0
+
+				//degenerate coefficients mess things up :(
+				SimdScalar ietsje = (r*s)/SimdTan(w/2.f);
+				if (ietsje*ietsje < 0.01f)
+					ietsje = 0.f;
+
+				coefs[0]=ietsje;//(r*s)/tan(w/2.);
+				coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d);
+				coefs[2]=-2.f*p*a.y()+2.f*q*a.x()+ietsje;//((r*s)/(tan(w/2.)));
+				coefs[3]=p*a.x()+q*a.y()+r*a.z()-d;
+
+				numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
+		}
+	}
+
+
+	for (int i=0;i<numroots;i++)
+	{
+		SimdScalar tau = polynomialSolver.GetRoot(i);
+
+		SimdScalar t = 2.f*SimdAtan(tau)/w;
+		//tau = tan (wt/2) so 2*atan (tau)/w
+		if (swapAB)
+		{
+			t *= -1.;
+		}
+		if (t>=0 && t<minTime)
+		{
+			minTime = t;
+			hit = true;
+		}
+	}
+	
+	return hit;
+}

Bullet/NarrowPhaseCollision/BU_VertexPoly.h

@@ -0,0 +1,43 @@
+/*
+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 VERTEX_POLY_H
+#define VERTEX_POLY_H
+
+
+class BU_Screwing;
+#include <SimdTransform.h>
+#include <SimdPoint3.h>
+#include <SimdScalar.h>
+
+///BU_VertexPoly implements algebraic time of impact calculation between vertex and a plane.
+class BU_VertexPoly
+{
+public:
+	BU_VertexPoly();
+	bool GetTimeOfImpact(
+		const BU_Screwing& screwAB,
+		const SimdPoint3& vtx,
+		const SimdVector4& planeEq,
+		SimdScalar &minTime,
+		bool swapAB);
+
+private:
+
+	//cached data (frame coherency etc.) here
+
+};
+#endif //VERTEX_POLY_H

Bullet/NarrowPhaseCollision/CollisionMargin.h

@@ -0,0 +1,26 @@
+/*
+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_MARGIN_H
+#define COLLISION_MARGIN_H
+
+//used by Gjk and some other algorithms
+
+#define CONVEX_DISTANCE_MARGIN 0.04f// 0.1f//;//0.01f
+
+
+
+#endif //COLLISION_MARGIN_H
+

Bullet/NarrowPhaseCollision/ContinuousConvexCollision.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 "ContinuousConvexCollision.h"
+#include "CollisionShapes/ConvexShape.h"
+#include "CollisionShapes/MinkowskiSumShape.h"
+#include "NarrowPhaseCollision/SimplexSolverInterface.h"
+#include "SimdTransformUtil.h"
+#include "CollisionShapes/SphereShape.h"
+
+#include "GjkPairDetector.h"
+#include "PointCollector.h"
+
+
+
+ContinuousConvexCollision::ContinuousConvexCollision ( ConvexShape*	convexA,ConvexShape*	convexB,SimplexSolverInterface* simplexSolver, ConvexPenetrationDepthSolver* penetrationDepthSolver)
+:m_simplexSolver(simplexSolver),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
+/// You don't want your game ever to lock-up.
+#define MAX_ITERATIONS 1000
+
+bool	ContinuousConvexCollision::calcTimeOfImpact(
+				const SimdTransform& fromA,
+				const SimdTransform& toA,
+				const SimdTransform& fromB,
+				const SimdTransform& toB,
+				CastResult& result)
+{
+
+	m_simplexSolver->reset();
+
+	/// compute linear and angular velocity for this interval, to interpolate
+	SimdVector3 linVelA,angVelA,linVelB,angVelB;
+	SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linVelA,angVelA);
+	SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linVelB,angVelB);
+
+	SimdScalar boundingRadiusA = m_convexA->GetAngularMotionDisc();
+	SimdScalar boundingRadiusB = m_convexB->GetAngularMotionDisc();
+
+	SimdScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
+
+	float radius = 0.001f;
+
+	SimdScalar lambda = 0.f;
+	SimdVector3 v(1,0,0);
+
+	int maxIter = MAX_ITERATIONS;
+
+	SimdVector3 n;
+	n.setValue(0.f,0.f,0.f);
+	bool hasResult = false;
+	SimdVector3 c;
+
+	float lastLambda = lambda;
+	//float epsilon = 0.001f;
+
+	int numIter = 0;
+	//first solution, using GJK
+
+
+	SimdTransform identityTrans;
+	identityTrans.setIdentity();
+
+	SphereShape	raySphere(0.0f);
+	raySphere.SetMargin(0.f);
+
+
+//	result.DrawCoordSystem(sphereTr);
+
+	PointCollector	pointCollector1;
+
+	{
+		
+		GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);		
+		GjkPairDetector::ClosestPointInput input;
+	
+		//we don't use margins during CCD
+		gjk.SetIgnoreMargin(true);
+
+		input.m_transformA = fromA;
+		input.m_transformB = fromB;
+		gjk.GetClosestPoints(input,pointCollector1,0);
+
+		hasResult = pointCollector1.m_hasResult;
+		c = pointCollector1.m_pointInWorld;
+	}
+
+	if (hasResult)
+	{
+		SimdScalar dist;
+		dist = pointCollector1.m_distance;
+		n = pointCollector1.m_normalOnBInWorld;
+		
+		//not close enough
+		while (dist > radius)
+		{
+			numIter++;
+			if (numIter > maxIter)
+				return false; //todo: report a failure
+
+			float dLambda = 0.f;
+
+			//calculate safe moving fraction from distance / (linear+rotational velocity)
+			
+			//float clippedDist  = GEN_min(angularConservativeRadius,dist);
+			//float clippedDist  = dist;
+			
+			float projectedLinearVelocity = (linVelB-linVelA).dot(n);
+			
+			dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
+
+			lambda = lambda + dLambda;
+
+			if (lambda > 1.f)
+				return false;
+
+			if (lambda < 0.f)
+				return false;
+
+			//todo: next check with relative epsilon
+			if (lambda <= lastLambda)
+				break;
+			lastLambda = lambda;
+
+			
+
+			//interpolate to next lambda
+			SimdTransform interpolatedTransA,interpolatedTransB,relativeTrans;
+
+			SimdTransformUtil::IntegrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
+			SimdTransformUtil::IntegrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
+			relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
+
+			result.DebugDraw( lambda );
+
+			PointCollector	pointCollector;
+			GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
+			GjkPairDetector::ClosestPointInput input;
+			input.m_transformA = interpolatedTransA;
+			input.m_transformB = interpolatedTransB;
+			gjk.GetClosestPoints(input,pointCollector,0);
+			if (pointCollector.m_hasResult)
+			{
+				if (pointCollector.m_distance < 0.f)
+				{
+					//degenerate ?!
+					result.m_fraction = lastLambda;
+					result.m_normal = n;
+					return true;
+				}
+				c = pointCollector.m_pointInWorld;		
+				
+				dist = pointCollector.m_distance;
+			} else
+			{
+				//??
+				return false;
+			}
+
+		}
+
+		result.m_fraction = lambda;
+		result.m_normal = n;
+		return true;
+	}
+
+	return false;
+
+/*
+//todo:
+	//if movement away from normal, discard result
+	SimdVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
+	if (result.m_fraction < 1.f)
+	{
+		if (move.dot(result.m_normal) <= 0.f)
+		{
+		}
+	}
+*/
+
+}
+

Bullet/NarrowPhaseCollision/ContinuousConvexCollision.h

@@ -0,0 +1,52 @@
+/*
+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 CONTINUOUS_COLLISION_CONVEX_CAST_H
+#define CONTINUOUS_COLLISION_CONVEX_CAST_H
+
+#include "ConvexCast.h"
+#include "SimplexSolverInterface.h"
+class ConvexPenetrationDepthSolver;
+class ConvexShape;
+
+/// ContinuousConvexCollision implements angular and linear time of impact for convex objects.
+/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
+/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
+/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
+class ContinuousConvexCollision : public ConvexCast
+{
+	SimplexSolverInterface* m_simplexSolver;
+	ConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	ConvexShape*	m_convexA;
+	ConvexShape*	m_convexB;
+
+
+public:
+
+	ContinuousConvexCollision (ConvexShape*	shapeA,ConvexShape*	shapeB ,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver);
+
+	virtual bool	calcTimeOfImpact(
+				const SimdTransform& fromA,
+				const SimdTransform& toA,
+				const SimdTransform& fromB,
+				const SimdTransform& toB,
+				CastResult& result);
+
+
+};
+
+#endif //CONTINUOUS_COLLISION_CONVEX_CAST_H
+

Bullet/NarrowPhaseCollision/ConvexCast.cpp

@@ -0,0 +1,20 @@
+/*
+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 "ConvexCast.h"
+
+ConvexCast::~ConvexCast()
+{
+}

Bullet/NarrowPhaseCollision/ConvexCast.h

@@ -0,0 +1,71 @@
+/*
+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_CAST_H
+#define CONVEX_CAST_H
+
+#include <SimdTransform.h>
+#include <SimdVector3.h>
+#include <SimdScalar.h>
+class MinkowskiSumShape;
+#include "IDebugDraw.h"
+
+/// ConvexCast is an interface for Casting
+class ConvexCast
+{
+public:
+
+
+	virtual ~ConvexCast();
+
+	///RayResult stores the closest result
+	/// alternatively, add a callback method to decide about closest/all results
+	struct	CastResult
+	{
+		//virtual bool	addRayResult(const SimdVector3& normal,SimdScalar	fraction) = 0;
+				
+		virtual void	DebugDraw(SimdScalar	fraction) {}
+		virtual void	DrawCoordSystem(const SimdTransform& trans) {}
+
+		CastResult()
+			:m_fraction(1e30f),
+			m_debugDrawer(0)
+		{
+		}
+
+
+		virtual ~CastResult() {};
+
+		SimdVector3	m_normal;
+		SimdScalar	m_fraction;
+		SimdTransform	m_hitTransformA;
+		SimdTransform	m_hitTransformB;
+
+		IDebugDraw* m_debugDrawer;
+
+	};
+
+
+	/// cast a convex against another convex object
+	virtual bool	calcTimeOfImpact(
+					const SimdTransform& fromA,
+					const SimdTransform& toA,
+					const SimdTransform& fromB,
+					const SimdTransform& toB,
+					CastResult& result) = 0;
+};
+
+#endif //CONVEX_CAST_H

Bullet/NarrowPhaseCollision/ConvexPenetrationDepthSolver.h

@@ -0,0 +1,42 @@
+/*
+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_PENETRATION_DEPTH_H
+#define CONVEX_PENETRATION_DEPTH_H
+
+class SimdVector3;
+#include "SimplexSolverInterface.h"
+class ConvexShape;
+#include "SimdPoint3.h"
+class SimdTransform;
+
+///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
+class ConvexPenetrationDepthSolver
+{
+public:	
+	
+	virtual ~ConvexPenetrationDepthSolver() {};
+	virtual bool CalcPenDepth( SimplexSolverInterface& simplexSolver,
+		ConvexShape* convexA,ConvexShape* convexB,
+					const SimdTransform& transA,const SimdTransform& transB,
+				SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb,
+				class IDebugDraw* debugDraw
+				) = 0;
+
+
+};
+#endif //CONVEX_PENETRATION_DEPTH_H
+

Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h

@@ -0,0 +1,86 @@
+/*
+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 DISCRETE_COLLISION_DETECTOR_INTERFACE_H
+#define DISCRETE_COLLISION_DETECTOR_INTERFACE_H
+#include "SimdTransform.h"
+#include "SimdVector3.h"
+
+
+/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
+/// This interface allows to query for closest points and penetration depth between two (convex) objects
+/// the closest point is on the second object (B), and the normal points from the surface on B towards A.
+/// distance is between closest points on B and closest point on A. So you can calculate closest point on A
+/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
+struct DiscreteCollisionDetectorInterface
+{
+	void operator delete(void* ptr) {};
+
+	struct Result
+	{
+		void operator delete(void* ptr) {};
+	
+		virtual ~Result(){}	
+		virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)=0;
+	};
+
+	struct ClosestPointInput
+	{
+		ClosestPointInput()
+			:m_maximumDistanceSquared(1e30f)
+		{
+		}
+
+		SimdTransform m_transformA;
+		SimdTransform m_transformB;
+		SimdScalar	m_maximumDistanceSquared;
+	};
+
+	virtual ~DiscreteCollisionDetectorInterface() {};
+
+	//
+	// give either closest points (distance > 0) or penetration (distance)
+	// the normal always points from B towards A
+	//
+	virtual void	GetClosestPoints(const ClosestPointInput& input,Result& output,class IDebugDraw* debugDraw) = 0;
+
+	SimdScalar	getCollisionMargin() { return 0.2f;}
+};
+
+struct StorageResult : public DiscreteCollisionDetectorInterface::Result
+{
+		SimdVector3	m_normalOnSurfaceB;
+		SimdVector3	m_closestPointInB;
+		SimdScalar	m_distance; //negative means penetration !
+
+		StorageResult() : m_distance(1e30f)
+		{
+
+		}
+		virtual ~StorageResult() {};
+
+		virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+		{
+			if (depth < m_distance)
+			{
+				m_normalOnSurfaceB = normalOnBInWorld;
+				m_closestPointInB = pointInWorld;
+				m_distance = depth;
+			}
+		}
+};
+
+#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE_H

Bullet/NarrowPhaseCollision/Epa.cpp

@@ -0,0 +1,560 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 "SimdScalar.h"
+#include "SimdVector3.h"
+#include "SimdPoint3.h"
+#include "SimdTransform.h"
+#include "SimdMinMax.h"
+
+#include "CollisionShapes/ConvexShape.h"
+
+#include <vector>
+#include <list>
+#include <algorithm>
+
+#include "NarrowPhaseCollision/SimplexSolverInterface.h"
+
+#include "NarrowPhaseCollision/EpaCommon.h"
+
+#include "NarrowPhaseCollision/EpaVertex.h"
+#include "NarrowPhaseCollision/EpaHalfEdge.h"
+#include "NarrowPhaseCollision/EpaFace.h"
+#include "NarrowPhaseCollision/EpaPolyhedron.h"
+#include "NarrowPhaseCollision/Epa.h"
+
+const SimdScalar EPA_MAX_RELATIVE_ERROR = 1e-2f;
+const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR;
+
+Epa::Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
+		  const SimdTransform& transformA, const SimdTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ),
+																			   m_pConvexShapeB( pConvexShapeB ),
+																			   m_transformA( transformA ),
+																			   m_transformB( transformB )
+{
+	m_faceEntries.reserve( EPA_MAX_FACE_ENTRIES );
+}
+
+Epa::~Epa()
+{
+}
+
+bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
+{
+	// Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO
+
+	SimdVector3 v( 1, 0, 0 );
+	SimdScalar squaredDistance = SIMD_INFINITY;
+
+	SimdScalar delta = 0.f;
+
+	simplexSolver.reset();
+
+	int nbIterations = 0;
+
+	while ( true )
+	{
+		assert( ( v.length2() > 0 ) && "Warning : v has zero magnitude!" );
+
+		SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis();
+		SimdVector3 seperatingAxisInB =  v * m_transformB.getBasis();
+
+		SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+		SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+		SimdPoint3  pWorld = m_transformA( pInA );
+		SimdPoint3  qWorld = m_transformB( qInB );
+
+		SimdVector3 w = pWorld - qWorld;
+		delta = v.dot( w );
+
+		assert( ( delta <= 0 ) && "Shapes are disjoint, EPA should have never    been called!" );
+		assert( !simplexSolver.inSimplex( w ) && "Shapes are disjoint, EPA should have never been called!" );
+
+		// Add support point to simplex
+		simplexSolver.addVertex( w, pWorld, qWorld );
+
+		bool closestOk = simplexSolver.closest( v );
+		assert( closestOk && "Shapes are disjoint, EPA should have never been called!" );
+
+		SimdScalar prevVSqrd = squaredDistance;
+		squaredDistance = v.length2();
+
+		// Is v converging to v(A-B) ?
+		assert( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) && 
+				"Shapes are disjoint, EPA should have never been called!" );
+
+		if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
+		{
+			break;
+		}
+
+		++nbIterations;
+	}
+
+	SimdPoint3 simplexPoints[ 5 ];
+	SimdPoint3 wSupportPointsOnA[ 5 ];
+	SimdPoint3 wSupportPointsOnB[ 5 ];
+
+	int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints );
+
+	// nbSimplexPoints can't be one because cases where the origin is on the boundary are handled
+	// by hybrid penetration depth
+	assert( ( ( nbSimplexPoints > 1 ) && ( nbSimplexPoints <= 4 ) ) &&
+		    "Hybrid Penetration Depth algorithm failed!" );
+
+	int nbPolyhedronPoints = nbSimplexPoints;
+
+#ifndef EPA_POLYHEDRON_USE_PLANES
+	int initTetraIndices[ 4 ] = { 0, 1, 2, 3 };
+#endif
+
+	//	Prepare initial polyhedron to start EPA from
+	if ( nbSimplexPoints == 1 )
+	{
+		return false;
+	}
+	else if ( nbSimplexPoints == 2 )
+	{
+		// We have a line segment inside the CSO that contains the origin
+		// Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points
+
+		SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
+		d.normalize();
+
+		SimdVector3 v1;
+		SimdVector3 v2;
+		SimdVector3 v3;
+
+		SimdVector3 e1;
+
+		SimdScalar absx = abs( d.getX() );
+		SimdScalar absy = abs( d.getY() );
+		SimdScalar absz = abs( d.getZ() );
+
+		if ( absx < absy )
+		{
+			if ( absx < absz )
+			{
+				e1.setX( 1 );
+			}
+			else
+			{
+				e1.setZ( 1 );
+			}
+		}
+		else
+		{
+			if ( absy < absz )
+			{
+				e1.setY( 1 );
+			}
+			else
+			{
+				e1.setZ( 1 );
+			}
+		}
+
+		v1 = d.cross( e1 );
+		v1.normalize();
+
+		v2 = v1.rotate( d, 120 * SIMD_RADS_PER_DEG );
+		v3 = v2.rotate( d, 120 * SIMD_RADS_PER_DEG );
+
+		nbPolyhedronPoints = 5;
+
+		SimdVector3 seperatingAxisInA =  v1 * m_transformA.getBasis();
+		SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
+
+		SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+		SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+		SimdPoint3 pWorld = m_transformA( p );
+		SimdPoint3 qWorld = m_transformB( q );
+
+		wSupportPointsOnA[ 2 ] = pWorld;
+		wSupportPointsOnB[ 2 ] = qWorld;
+		simplexPoints[ 2 ]	   = wSupportPointsOnA[ 2 ] - wSupportPointsOnB[ 2 ];
+
+		seperatingAxisInA =  v2 * m_transformA.getBasis();
+		seperatingAxisInB = -v2 * m_transformB.getBasis();
+
+		p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+		q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+		pWorld = m_transformA( p );
+		qWorld = m_transformB( q );
+
+		wSupportPointsOnA[ 3 ] = pWorld;
+		wSupportPointsOnB[ 3 ] = qWorld;
+		simplexPoints[ 3 ]	   = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
+
+		seperatingAxisInA =  v3 * m_transformA.getBasis();
+		seperatingAxisInB = -v3 * m_transformB.getBasis();
+
+		p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+		q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+		pWorld = m_transformA( p );
+		qWorld = m_transformB( q );
+
+		wSupportPointsOnA[ 4 ] = pWorld;
+		wSupportPointsOnB[ 4 ] = qWorld;
+		simplexPoints[ 4 ]	   = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
+
+#ifndef EPA_POLYHEDRON_USE_PLANES
+		if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
+		{
+			initTetraIndices[ 1 ] = 2;
+			initTetraIndices[ 2 ] = 3;
+			initTetraIndices[ 3 ] = 4;
+		}
+		else
+		{
+			if ( TetrahedronContainsOrigin( simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
+			{
+				initTetraIndices[ 0 ] = 1;
+				initTetraIndices[ 1 ] = 2;
+				initTetraIndices[ 2 ] = 3;
+				initTetraIndices[ 3 ] = 4;
+			}
+			else
+			{
+				// No tetrahedron contains the origin
+				assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
+				return false;
+			}
+		}
+#endif
+	}
+	else if ( nbSimplexPoints == 3 )
+	{
+		// We have a triangle inside the CSO that contains the origin
+		// Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points
+
+		SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
+		SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
+		SimdVector3 triangleNormal = v0.cross( v1 );
+		triangleNormal.normalize();
+
+		nbPolyhedronPoints = 5;
+
+		SimdVector3 seperatingAxisInA =  triangleNormal * m_transformA.getBasis();
+		SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
+
+		SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+		SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+		SimdPoint3 pWorld = m_transformA( p );
+		SimdPoint3 qWorld = m_transformB( q );
+
+		wSupportPointsOnA[ 3 ] = pWorld;
+		wSupportPointsOnB[ 3 ] = qWorld;
+		simplexPoints[ 3 ]	   = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
+
+#ifndef EPA_POLYHEDRON_USE_PLANES
+		// We place this check here because if the tetrahedron contains the origin
+		// there is no need to sample another support point
+		if ( !TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ] ) )
+		{
+#endif
+			seperatingAxisInA = -triangleNormal * m_transformA.getBasis();
+			seperatingAxisInB =  triangleNormal * m_transformB.getBasis();
+
+			p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+			q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+			pWorld = m_transformA( p );
+			qWorld = m_transformB( q );
+
+			wSupportPointsOnA[ 4 ] = pWorld;
+			wSupportPointsOnB[ 4 ] = qWorld;
+			simplexPoints[ 4 ]	   = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
+
+#ifndef EPA_POLYHEDRON_USE_PLANES
+			if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 4 ] ) )
+			{
+				initTetraIndices[ 3 ] = 4;
+			}
+			else
+			{
+				// No tetrahedron contains the origin
+				assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
+				return false;
+			}
+		}
+#endif
+	}
+#ifdef _DEBUG
+	else if ( nbSimplexPoints == 4 )
+	{
+		assert( TetrahedronContainsOrigin( simplexPoints ) && "Initial tetrahedron does not contain the origin!" );
+	}
+#endif
+
+#ifndef EPA_POLYHEDRON_USE_PLANES
+	SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
+									 simplexPoints[ initTetraIndices[ 1 ] ],
+									 simplexPoints[ initTetraIndices[ 2 ] ],
+									 simplexPoints[ initTetraIndices[ 3 ] ] };
+
+	SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
+											   wSupportPointsOnA[ initTetraIndices[ 1 ] ],
+											   wSupportPointsOnA[ initTetraIndices[ 2 ] ],
+											   wSupportPointsOnA[ initTetraIndices[ 3 ] ] };
+
+	SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
+											   wSupportPointsOnB[ initTetraIndices[ 1 ] ],
+											   wSupportPointsOnB[ initTetraIndices[ 2 ] ],
+											   wSupportPointsOnB[ initTetraIndices[ 3 ] ] };
+#endif
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+	if ( !m_polyhedron.Create( simplexPoints, wSupportPointsOnA, wSupportPointsOnB, nbPolyhedronPoints ) )
+#else
+	if ( !m_polyhedron.Create( wTetraPoints, wTetraSupportPointsOnA, wTetraSupportPointsOnB, 4 ) )
+#endif
+	{
+		// Failed to create initial polyhedron
+		assert( false && "Failed to create initial polyhedron!" );
+		return false;
+	}
+
+	// Add initial faces to priority queue
+
+#ifdef _DEBUG
+	//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
+#endif
+
+	std::list< EpaFace* >& faces = m_polyhedron.GetFaces();
+
+	std::list< EpaFace* >::iterator facesItr( faces.begin() );
+
+	while ( facesItr != faces.end() )
+	{
+		EpaFace* pFace = *facesItr;
+
+		if ( !pFace->m_deleted )
+		{
+//#ifdef EPA_POLYHEDRON_USE_PLANES
+//			if ( pFace->m_planeDistance >= 0 )
+//			{
+//				m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
+//				assert( false && "Face's plane distance equal or greater than 0!" );
+//			}
+//#endif
+
+			if ( pFace->IsAffinelyDependent() )
+			{
+				assert( false && "One initial face is affinely dependent!" );
+				return false;
+			}
+
+			if ( pFace->m_vSqrd <= 0 )
+			{
+				assert( false && "Face containing the origin!" );
+				return false;
+			}
+
+			if ( pFace->IsClosestPointInternal() )
+			{
+				m_faceEntries.push_back( pFace );
+				std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
+			}
+		}
+
+		++facesItr;
+	}
+
+#ifdef _DEBUG
+	//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
+#endif
+
+	assert( !m_faceEntries.empty() && "No faces added to heap!" );
+
+	return true;
+}
+
+SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
+{
+	SimdVector3 v;
+
+	SimdScalar upperBoundSqrd = SIMD_INFINITY;
+	SimdScalar vSqrd = 0;
+#ifdef _DEBUG
+	SimdScalar prevVSqrd;
+#endif
+	SimdScalar delta;
+
+	bool isCloseEnough = false;
+
+	EpaFace* pEpaFace = NULL;
+
+	int nbIterations = 0;
+	//int nbMaxIterations = 1000;
+
+	do
+	{
+		pEpaFace = m_faceEntries.front();
+		std::pop_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
+		m_faceEntries.pop_back();
+
+		if ( !pEpaFace->m_deleted )
+		{
+#ifdef _DEBUG
+			prevVSqrd = vSqrd;
+#endif
+
+			vSqrd = pEpaFace->m_vSqrd;
+
+			if ( pEpaFace->m_planeDistance >= 0 )
+			{
+				v = pEpaFace->m_planeNormal;
+			}
+			else
+			{
+				v = pEpaFace->m_v;
+			}
+
+#ifdef _DEBUG
+			//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
+			assert( ( vSqrd >= prevVSqrd ) && "vSqrd decreased!" );
+#endif //_DEBUG
+			assert( ( v.length2() > 0 ) && "Zero vector not allowed!" );
+
+			SimdVector3 seperatingAxisInA =  v * m_transformA.getBasis();
+			SimdVector3 seperatingAxisInB = -v * m_transformB.getBasis();
+
+			SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
+			SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
+
+			SimdPoint3 pWorld = m_transformA( p );
+			SimdPoint3 qWorld = m_transformB( q );
+
+			SimdPoint3 w = pWorld - qWorld;
+			delta = v.dot( w );
+
+			// Keep tighest upper bound
+			upperBoundSqrd = SimdMin( upperBoundSqrd, delta * delta / vSqrd );
+			//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
+
+			isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd );
+
+			if ( !isCloseEnough )
+			{
+				std::list< EpaFace* > newFaces;
+				bool expandOk = m_polyhedron.Expand( w, pWorld, qWorld, pEpaFace, newFaces );
+
+				if ( expandOk )
+				{
+					assert( !newFaces.empty() && "EPA polyhedron not expanding ?" );
+
+					bool check    = true;
+					bool areEqual = false;
+
+					while ( !newFaces.empty() )
+					{
+						EpaFace* pNewFace = newFaces.front();
+						assert( !pNewFace->m_deleted && "New face is deleted!" );
+
+						if ( !pNewFace->m_deleted )
+						{
+							assert( ( pNewFace->m_vSqrd > 0 ) && "Face containing the origin!" );
+							assert( !pNewFace->IsAffinelyDependent() && "Face is affinely dependent!" );
+
+//#ifdef EPA_POLYHEDRON_USE_PLANES
+////							if ( pNewFace->m_planeDistance >= 0 )
+////							{
+//								// assert( false && "Face's plane distance greater than 0!" );
+//#ifdef _DEBUG
+////								m_polyhedron._dbgSaveToFile( "epa_beforeFix.dbg" );
+//#endif
+//								//pNewFace->FixOrder();
+//#ifdef _DEBUG
+//								//m_polyhedron._dbgSaveToFile( "epa_afterFix.dbg" );
+//#endif
+////							}
+//#endif
+//
+//#ifdef EPA_POLYHEDRON_USE_PLANES
+//							//assert( ( pNewFace->m_planeDistance < 0 ) && "Face's plane distance equal or greater than 0!" );
+//#endif
+
+							if ( pNewFace->IsClosestPointInternal() && ( vSqrd <= pNewFace->m_vSqrd ) && ( pNewFace->m_vSqrd <= upperBoundSqrd ) )
+							{
+								m_faceEntries.push_back( pNewFace );
+								std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
+							}
+						}
+
+						newFaces.pop_front();
+					}
+				}
+				else
+				{
+					pEpaFace->CalcClosestPointOnA( wWitnessOnA );
+					pEpaFace->CalcClosestPointOnB( wWitnessOnB );
+
+#ifdef _DEBUG
+					//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
+#endif
+
+					return v.length();
+				}
+			}
+		}
+
+		++nbIterations;
+	}
+	while ( ( m_polyhedron.GetNbFaces() < EPA_MAX_FACE_ENTRIES ) &&/*( nbIterations < nbMaxIterations ) &&*/
+			!isCloseEnough && ( m_faceEntries.size() > 0 ) && ( m_faceEntries[ 0 ]->m_vSqrd <= upperBoundSqrd ) );
+
+#ifdef _DEBUG
+	//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
+#endif
+
+	assert( pEpaFace && "Invalid epa face!" );
+
+	pEpaFace->CalcClosestPointOnA( wWitnessOnA );
+	pEpaFace->CalcClosestPointOnB( wWitnessOnB );
+
+	return v.length();
+}
+
+bool Epa::TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
+									 const SimdPoint3& point2, const SimdPoint3& point3 )
+{
+	SimdVector3 facesNormals[ 4 ] = { ( point1 - point0 ).cross( point2 - point0 ),
+									  ( point2 - point1 ).cross( point3 - point1 ),
+									  ( point3 - point2 ).cross( point0 - point2 ),
+									  ( point0 - point3 ).cross( point1 - point3 ) };
+
+	return ( ( facesNormals[ 0 ].dot( point0 ) > 0 ) != ( facesNormals[ 0 ].dot( point3 ) > 0 ) ) &&
+		   ( ( facesNormals[ 1 ].dot( point1 ) > 0 ) != ( facesNormals[ 1 ].dot( point0 ) > 0 ) ) &&
+		   ( ( facesNormals[ 2 ].dot( point2 ) > 0 ) != ( facesNormals[ 2 ].dot( point1 ) > 0 ) ) &&
+		   ( ( facesNormals[ 3 ].dot( point3 ) > 0 ) != ( facesNormals[ 3 ].dot( point2 ) > 0 ) );
+}
+
+bool Epa::TetrahedronContainsOrigin( SimdPoint3* pPoints )
+{
+	return TetrahedronContainsOrigin( pPoints[ 0 ], pPoints[ 1 ], pPoints[ 2 ], pPoints[ 3 ] );
+}
+
+bool CompareEpaFaceEntries( EpaFace* pFaceA, EpaFace* pFaceB )
+{
+	return ( pFaceA->m_vSqrd > pFaceB->m_vSqrd );
+}

Bullet/NarrowPhaseCollision/Epa.h

@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_H
+#define EPA_H
+
+#define EPA_MAX_FACE_ENTRIES 256
+
+extern const SimdScalar EPA_MAX_RELATIVE_ERROR;
+extern const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD;
+
+class Epa
+{
+	private :
+
+		//! Prevents copying objects from this class
+		Epa( const Epa& epa );
+		const Epa&			operator = ( const Epa& epa );
+
+	public :
+
+		Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
+			 const SimdTransform& transformA, const SimdTransform& transformB );
+		~Epa();
+
+		bool				Initialize( SimplexSolverInterface& simplexSolver );
+
+		SimdScalar			CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB );
+
+	private :
+
+		bool				TetrahedronContainsOrigin( SimdPoint3* pPoints );
+		bool				TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
+													   const SimdPoint3& point2, const SimdPoint3& point3 );
+
+	private :
+
+		//! Priority queue
+		std::vector< EpaFace* >	m_faceEntries;
+
+		ConvexShape*			m_pConvexShapeA;
+		ConvexShape*			m_pConvexShapeB;
+
+		SimdTransform			m_transformA;
+		SimdTransform			m_transformB;
+
+		EpaPolyhedron			m_polyhedron;
+};
+
+extern bool CompareEpaFaceEntries( EpaFace* pFaceA, EpaFace* pFaceB );
+
+#endif
+

Bullet/NarrowPhaseCollision/EpaCommon.h

@@ -0,0 +1,25 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_COMMON_H
+#define EPA_COMMON_H
+
+#define EPA_POLYHEDRON_USE_PLANES
+
+//#define EPA_USE_HYBRID
+
+#endif
+

Bullet/NarrowPhaseCollision/EpaFace.cpp

@@ -0,0 +1,254 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 "SimdScalar.h"
+#include "SimdVector3.h"
+#include "SimdPoint3.h"
+
+#include "NarrowPhaseCollision/EpaCommon.h"
+
+#include "NarrowPhaseCollision/EpaVertex.h"
+#include "NarrowPhaseCollision/EpaHalfEdge.h"
+#include "NarrowPhaseCollision/EpaFace.h"
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+SimdScalar PLANE_THICKNESS = 1e-5f;
+#endif
+
+EpaFace::EpaFace() : m_pHalfEdge( 0 ), m_deleted( false )
+{
+	m_pVertices[ 0 ] = m_pVertices[ 1 ] = m_pVertices[ 2 ] = 0;
+}
+
+EpaFace::~EpaFace()
+{
+}
+
+bool EpaFace::Initialize()
+{
+	assert( m_pHalfEdge && "Must setup half-edge first!" );
+
+	CollectVertices( m_pVertices );
+	
+	const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
+	const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
+
+	const SimdScalar e0Sqrd = e0.length2();
+	const SimdScalar e1Sqrd = e1.length2();
+	const SimdScalar e0e1   = e0.dot( e1 );
+
+	m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;
+
+	const SimdScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
+	const SimdScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );
+
+	m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0;
+	m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;
+
+	if ( IsAffinelyDependent() )
+	{
+		return false;
+	}
+
+	CalcClosestPoint();
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+	if ( !CalculatePlane() )
+	{
+		return false;
+	}
+#endif
+
+	return true;
+}
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+bool EpaFace::CalculatePlane()
+{
+	assert( ( m_pVertices[ 0 ] && m_pVertices[ 1 ] && m_pVertices[ 2 ] )
+			&& "Must setup vertices pointers first!" );
+
+	// Traditional method
+
+	const SimdVector3 v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
+	const SimdVector3 v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
+
+	m_planeNormal = v2.cross( v1 );
+
+	if ( m_planeNormal.length2() == 0 )
+	{
+		return false;
+	}
+
+	m_planeNormal.normalize();
+
+	m_planeDistance = m_pVertices[ 0 ]->m_point.dot( -m_planeNormal );
+
+	// Robust method
+
+	//SimdVector3 _v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
+	//SimdVector3 _v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
+
+	//SimdVector3 n;
+
+	//n = _v2.cross( _v1 );
+
+	//_v1 = m_pVertices[ 0 ]->m_point - m_pVertices[ 1 ]->m_point;
+	//_v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 1 ]->m_point;
+
+	//n += ( _v1.cross( _v2 ) );
+
+	//_v1 = m_pVertices[ 0 ]->m_point - m_pVertices[ 2 ]->m_point;
+	//_v2 = m_pVertices[ 1 ]->m_point - m_pVertices[ 2 ]->m_point;
+
+	//n += ( _v2.cross( _v1 ) );
+
+	//n /= 3;	
+	//n.normalize();
+
+	//SimdVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3;
+	//SimdScalar d  = c.dot( -n );
+
+	//m_robustPlaneNormal   = n;
+	//m_robustPlaneDistance = d;
+
+	// Compare results from both methods and check whether they disagree
+
+	//if ( d < 0 )
+	//{
+	//	assert( ( m_planeDistance < 0 ) && "He he! Busted!" );
+	//}
+	//else
+	//{
+	//	assert( ( m_planeDistance >= 0 ) && "He he! Busted!" );
+	//}
+
+	return true;
+}
+#endif
+
+void EpaFace::CalcClosestPoint()
+{
+	const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
+	const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
+
+	m_v =  m_pVertices[ 0 ]->m_point +
+		 ( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / m_determinant;
+
+	m_vSqrd = m_v.length2();
+}
+
+void EpaFace::CalcClosestPointOnA( SimdVector3& closestPointOnA )
+{
+	const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
+	const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
+
+	closestPointOnA =  m_pVertices[ 0 ]->m_wSupportPointOnA +
+					 ( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) /
+					   m_determinant;
+}
+
+void EpaFace::CalcClosestPointOnB( SimdVector3& closestPointOnB )
+{
+	const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
+	const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
+
+	closestPointOnB =  m_pVertices[ 0 ]->m_wSupportPointOnB +
+					 ( e0 * m_lambdas[  0 ] + e1 * m_lambdas[ 1 ] ) /
+					   m_determinant;
+}
+
+bool EpaFace::IsAffinelyDependent() const
+{
+	return ( m_determinant <= SIMD_EPSILON );
+}
+
+bool EpaFace::IsClosestPointInternal() const
+{
+	return ( ( m_lambdas[ 0 ] >= 0 ) && ( m_lambdas[ 1 ] >= 0 ) && ( ( m_lambdas[ 0 ] + m_lambdas[ 1 ] <= m_determinant ) ) );
+}
+
+void EpaFace::CollectVertices( EpaVertex** ppVertices )
+{
+	assert( m_pHalfEdge && "Invalid half-edge pointer!" );
+
+	int vertexIndex = 0;
+
+	EpaHalfEdge* pCurrentHalfEdge = m_pHalfEdge;
+
+	do
+	{
+		assert( ( ( vertexIndex >= 0 ) && ( vertexIndex < 3 ) ) &&
+				"Face is not a triangle!" );
+
+		assert( pCurrentHalfEdge->m_pVertex && "Half-edge has an invalid vertex pointer!" );
+
+		ppVertices[ vertexIndex++ ] = pCurrentHalfEdge->m_pVertex;
+
+		pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW;
+				
+	}
+	while( pCurrentHalfEdge != m_pHalfEdge );
+}
+
+//void EpaFace::FixOrder()
+//{
+//	EpaHalfEdge* pHalfEdges[ 3 ];
+//
+//	int halfEdgeIndex = 0;
+//
+//	EpaHalfEdge* pCurrentHalfEdge = m_pHalfEdge;
+//
+//	do
+//	{
+//		assert( ( ( halfEdgeIndex >= 0 ) && ( halfEdgeIndex < 3 ) ) &&
+//				"Face is not a triangle!" );
+//
+//		pHalfEdges[ halfEdgeIndex++ ] = pCurrentHalfEdge;
+//
+//		pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW;
+//	}
+//	while( pCurrentHalfEdge != m_pHalfEdge );
+//
+//	EpaVertex* pVertices[ 3 ] = { pHalfEdges[ 0 ]->m_pVertex,
+//								  pHalfEdges[ 1 ]->m_pVertex,
+//								  pHalfEdges[ 2 ]->m_pVertex };
+//	
+//	// Make them run in the opposite direction
+//	pHalfEdges[ 0 ]->m_pNextCCW = pHalfEdges[ 2 ];
+//	pHalfEdges[ 1 ]->m_pNextCCW = pHalfEdges[ 0 ];
+//	pHalfEdges[ 2 ]->m_pNextCCW = pHalfEdges[ 1 ];
+//
+//	// Make half-edges point to their correct origin vertices
+//
+//	pHalfEdges[ 1 ]->m_pVertex = pVertices[ 2 ];
+//	pHalfEdges[ 2 ]->m_pVertex = pVertices[ 0 ];
+//	pHalfEdges[ 0 ]->m_pVertex = pVertices[ 1 ];
+//
+//	// Make vertices point to the correct half-edges
+//
+//	//pHalfEdges[ 0 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 0 ];
+//	//pHalfEdges[ 1 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 1 ];
+//	//pHalfEdges[ 2 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 2 ];
+//
+//	// Flip normal and change the sign of plane distance
+//
+//#ifdef EPA_POLYHEDRON_USE_PLANES
+//	m_planeNormal	= -m_planeNormal;
+//	m_planeDistance = -m_planeDistance;
+//#endif
+//}
+

Bullet/NarrowPhaseCollision/EpaFace.h

@@ -0,0 +1,83 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_FACE_H
+#define EPA_FACE_H
+
+class EpaVertex;
+class EpaHalfEdge;
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+extern SimdScalar PLANE_THICKNESS;
+#endif
+
+//! Note : This class is not supposed to be a base class
+class EpaFace
+{
+	private :
+
+		//! Prevents copying objects from this class
+		EpaFace( const EpaFace& epaFace );
+		const EpaFace&	operator = ( const EpaFace& epaFace );
+
+	public :
+
+		EpaFace();
+		~EpaFace();
+
+		bool			Initialize();
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+		bool			CalculatePlane();
+#endif
+		void			CalcClosestPoint();
+		void			CalcClosestPointOnA( SimdVector3& closestPointOnA );
+		void			CalcClosestPointOnB( SimdVector3& closestPointOnB );
+
+		bool			IsAffinelyDependent()							const;
+		bool			IsClosestPointInternal()						const;
+
+		void			CollectVertices( EpaVertex** ppVertices );
+
+		//void			FixOrder();
+
+	public :
+
+		EpaHalfEdge*	m_pHalfEdge;
+
+		// We keep vertices here so we don't need to call CollectVertices
+		// every time we need them
+		EpaVertex*		m_pVertices[ 3 ];
+
+#ifdef EPA_POLYHEDRON_USE_PLANES
+		SimdVector3		m_planeNormal;
+		SimdScalar		m_planeDistance;
+			
+		//SimdVector3		m_robustPlaneNormal;
+		//SimdScalar		m_robustPlaneDistance;
+#endif
+
+		SimdVector3		m_v;
+		SimdScalar		m_vSqrd;
+
+		SimdScalar		m_determinant;
+		SimdScalar		m_lambdas[ 2 ];
+
+		bool			m_deleted;
+};
+
+#endif
+

Bullet/NarrowPhaseCollision/EpaHalfEdge.h

@@ -0,0 +1,58 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_HALF_EDGE_H
+#define EPA_HALF_EDGE_H
+
+class EpaFace;
+class EpaVertex;
+
+//! Note : This class is not supposed to be a base class
+class EpaHalfEdge
+{
+	private :
+
+		//! Prevents copying objects from this class
+		EpaHalfEdge( const EpaHalfEdge& epaHalfEdge );
+		const EpaHalfEdge&		operator = ( const EpaHalfEdge& epaHalfEdge );
+
+	public :
+
+		EpaHalfEdge() : m_pTwin( 0 ), m_pNextCCW( 0 ), m_pFace( 0 ), m_pVertex( 0 )
+		{
+		}
+
+		~EpaHalfEdge()
+		{
+		}
+
+	public :
+
+		//! Twin half-edge link
+		EpaHalfEdge*			m_pTwin;
+
+		//! Next half-edge in counter clock-wise ( CCW ) order
+		EpaHalfEdge*			m_pNextCCW;
+
+		//! Parent face link
+		EpaFace*				m_pFace;
+
+		//! Origin vertex link
+		EpaVertex*				m_pVertex;
+};
+
+#endif
+

Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.cpp

@@ -0,0 +1,202 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006
+
+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 "SimdScalar.h"
+#include "SimdVector3.h"
+#include "SimdPoint3.h"
+#include "SimdTransform.h"
+#include "SimdMinMax.h"
+
+#include <list>
+
+#include "CollisionShapes/ConvexShape.h"
+
+#include "NarrowPhaseCollision/SimplexSolverInterface.h"
+
+#include "NarrowPhaseCollision/EpaCommon.h"
+
+#include "NarrowPhaseCollision/EpaVertex.h"
+#include "NarrowPhaseCollision/EpaHalfEdge.h"
+#include "NarrowPhaseCollision/EpaFace.h"
+#include "NarrowPhaseCollision/EpaPolyhedron.h"
+#include "NarrowPhaseCollision/Epa.h"
+#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h"
+#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h"
+
+SimdScalar	g_GJKMaxRelError = 1e-3f;
+SimdScalar	g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError;
+
+bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSolver,
+											  ConvexShape* pConvexA, ConvexShape* pConvexB,
+											  const SimdTransform& transformA, const SimdTransform& transformB,
+											  SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
+											  class IDebugDraw* debugDraw )
+{
+	assert( pConvexA && "Convex shape A is invalid!" );
+	assert( pConvexB && "Convex shape B is invalid!" );
+
+	SimdScalar penDepth;
+
+#ifdef EPA_USE_HYBRID
+	bool needsEPA = !HybridPenDepth( simplexSolver, pConvexA, pConvexB, transformA, transformB,
+									 wWitnessOnA, wWitnessOnB, penDepth, v );
+
+	if ( needsEPA )
+	{
+#endif
+		penDepth = EpaPenDepth( simplexSolver, pConvexA, pConvexB,
+								transformA, transformB,
+								wWitnessOnA, wWitnessOnB );
+		assert( ( penDepth > 0 ) &&  "EPA or Hybrid Technique failed to calculate penetration depth!" );
+
+#ifdef EPA_USE_HYBRID
+	}
+#endif
+
+	return ( penDepth > 0 );
+}
+
+#ifdef EPA_USE_HYBRID
+bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver,
+												ConvexShape* pConvexA, ConvexShape* pConvexB,
+												const SimdTransform& transformA, const SimdTransform& transformB,
+												SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
+											    SimdScalar& penDepth, SimdVector3& v )
+{
+	SimdScalar squaredDistance = SIMD_INFINITY;
+	SimdScalar delta = 0.f;
+
+	const SimdScalar margin     = pConvexA->GetMargin() + pConvexB->GetMargin();
+	const SimdScalar marginSqrd = margin * margin;
+
+	simplexSolver.reset();
+
+	int nbIterations = 0;
+
+	while ( true )
+	{
+		assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
+
+		SimdVector3 seperatingAxisInA = -v * transformA.getBasis();
+		SimdVector3 seperatingAxisInB =  v * transformB.getBasis();
+
+		SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
+		SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
+
+		SimdPoint3  pWorld = transformA( pInA );
+		SimdPoint3  qWorld = transformB( qInB );
+
+		SimdVector3 w = pWorld - qWorld;
+		delta = v.dot( w );
+
+		// potential exit, they don't overlap
+		if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) )
+		{
+			// Convex shapes do not overlap
+			// Returning true means that Hybrid's result is ok and there's no need to run EPA
+			penDepth = 0;
+			return true;
+		}
+
+		//exit 0: the new point is already in the simplex, or we didn't come any closer
+		if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) )
+		{
+			simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
+
+			assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
+			SimdScalar vLength = sqrt( squaredDistance );
+
+			wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
+			wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
+
+			penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
+
+			// Returning true means that Hybrid's result is ok and there's no need to run EPA
+			return true;
+		}
+
+		//add current vertex to simplex
+		simplexSolver.addVertex( w, pWorld, qWorld );
+
+		//calculate the closest point to the origin (update vector v)
+		if ( !simplexSolver.closest( v ) )
+		{
+			simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
+
+			assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
+			SimdScalar vLength = sqrt( squaredDistance );
+
+			wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
+			wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
+
+			penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
+
+			// Returning true means that Hybrid's result is ok and there's no need to run EPA
+			return true;
+		}
+
+		SimdScalar previousSquaredDistance = squaredDistance;
+		squaredDistance = v.length2();
+
+		//are we getting any closer ?
+		if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance ) 
+		{ 
+			simplexSolver.backup_closest( v );
+			squaredDistance = v.length2();
+
+			simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
+
+			assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
+			SimdScalar vLength = sqrt( squaredDistance );
+
+			wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
+			wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
+
+			penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
+
+			// Returning true means that Hybrid's result is ok and there's no need to run EPA
+			return true;
+		}
+
+		if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
+		{
+			// Convex Shapes intersect - we need to run EPA
+			// Returning false means that Hybrid couldn't do anything for us
+			// and that we need to run EPA to calculate the pen depth
+			return false;
+		}
+
+		++nbIterations;
+	}
+}
+#endif
+
+SimdScalar EpaPenetrationDepthSolver::EpaPenDepth( SimplexSolverInterface& simplexSolver,
+												   ConvexShape* pConvexA, ConvexShape* pConvexB,
+												   const SimdTransform& transformA, const SimdTransform& transformB,
+												   SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
+{
+	Epa epa( pConvexA, pConvexB, transformA, transformB );
+
+	if ( !epa.Initialize( simplexSolver ) )
+	{
+		assert( false && "Epa failed to initialize!" );
+		return 0;
+	}
+
+	return epa.CalcPenDepth( wWitnessOnA, wWitnessOnB );
+}
+

Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.h

@@ -0,0 +1,56 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+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 EPA_PENETRATION_DEPTH_H
+#define EPA_PENETRATION_DEPTH_H
+
+/**
+*	EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
+*	calculate the penetration depth between two convex shapes.
+*/
+
+extern SimdScalar	g_GJKMaxRelError;
+extern SimdScalar	g_GJKMaxRelErrorSqrd;
+
+//! Note : This class is not supposed to be a base class
+class EpaPenetrationDepthSolver : public ConvexPenetrationDepthSolver
+{
+	public :
+
+		bool			CalcPenDepth( SimplexSolverInterface& simplexSolver,
+									  ConvexShape* pConvexA, ConvexShape* pConvexB,
+									  const SimdTransform& transformA, const SimdTransform& transformB,
+									  SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
+									  class IDebugDraw* debugDraw );
+
+	private :
+
+#ifdef EPA_USE_HYBRID
+		bool			HybridPenDepth( SimplexSolverInterface& simplexSolver,
+										ConvexShape* pConvexA, ConvexShape* pConvexB,
+										const SimdTransform& transformA, const SimdTransform& transformB,
+										SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
+										SimdScalar& penDepth, SimdVector3& v );
+#endif
+
+		SimdScalar		EpaPenDepth( SimplexSolverInterface& simplexSolver,
+									 ConvexShape* pConvexA, ConvexShape* pConvexB,
+									 const SimdTransform& transformA, const SimdTransform& transformB,
+									 SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB );
+};
+
+#endif	// EPA_PENETRATION_DEPTH_H
+