initial commit

bullet/src/LinearMath/btAabbUtil2.h

@@ -0,0 +1,127 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 AABB_UTIL2
+#define AABB_UTIL2
+
+#include "btVector3.h"
+#include "btSimdMinMax.h"
+
+
+#define btMin(a,b) ((a < b ? a : b))
+#define btMax(a,b) ((a > b ? a : b))
+
+
+/// conservative test for overlap between two aabbs
+SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
+								const btVector3 &aabbMin2, const btVector3 &aabbMax2)
+{
+	bool overlap = true;
+	overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+	overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+	overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+	return overlap;
+}
+
+/// conservative test for overlap between triangle and aabb
+SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
+									const btVector3 &aabbMin, const btVector3 &aabbMax)
+{
+	const btVector3 &p1 = vertices[0];
+	const btVector3 &p2 = vertices[1];
+	const btVector3 &p3 = vertices[2];
+
+	if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
+	if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
+
+	if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
+	if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
+  
+	if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
+	if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
+	return true;
+}
+
+
+SIMD_FORCE_INLINE int	btOutcode(const btVector3& p,const btVector3& halfExtent) 
+{
+	return (p.getX()  < -halfExtent.getX() ? 0x01 : 0x0) |    
+		   (p.getX() >  halfExtent.getX() ? 0x08 : 0x0) |
+		   (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |    
+		   (p.getY() >  halfExtent.getY() ? 0x10 : 0x0) |
+		   (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |    
+		   (p.getZ() >  halfExtent.getZ() ? 0x20 : 0x0);
+}
+
+
+SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, 
+								 const btVector3& rayTo, 
+								 const btVector3& aabbMin, 
+								 const btVector3& aabbMax,
+					  btScalar& param, btVector3& normal) 
+{
+	btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5);
+	btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5);
+	btVector3	source = rayFrom - aabbCenter;
+	btVector3	target = rayTo - aabbCenter;
+	int	sourceOutcode = btOutcode(source,aabbHalfExtent);
+	int targetOutcode = btOutcode(target,aabbHalfExtent);
+	if ((sourceOutcode & targetOutcode) == 0x0)
+	{
+		btScalar lambda_enter = btScalar(0.0);
+		btScalar lambda_exit  = param;
+		btVector3 r = target - source;
+		int i;
+		btScalar	normSign = 1;
+		btVector3	hitNormal(0,0,0);
+		int bit=1;
+
+		for (int j=0;j<2;j++)
+		{
+			for (i = 0; i != 3; ++i)
+			{
+				if (sourceOutcode & bit)
+				{
+					btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+					if (lambda_enter <= lambda)
+					{
+						lambda_enter = lambda;
+						hitNormal.setValue(0,0,0);
+						hitNormal[i] = normSign;
+					}
+				}
+				else if (targetOutcode & bit) 
+				{
+					btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+					btSetMin(lambda_exit, lambda);
+				}
+				bit<<=1;
+			}
+			normSign = btScalar(-1.);
+		}
+		if (lambda_enter <= lambda_exit)
+		{
+			param = lambda_enter;
+			normal = hitNormal;
+			return true;
+		}
+	}
+	return false;
+}
+
+
+#endif
+

bullet/src/LinearMath/btAlignedAllocator.cpp

@@ -0,0 +1,70 @@
+/*
+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 "btAlignedAllocator.h"
+
+
+#if defined (BT_HAS_ALIGNED_ALOCATOR)
+
+#include <malloc.h>
+void*	btAlignedAlloc	(int size, int alignment)
+{
+	return _aligned_malloc(size,alignment);
+}
+
+void	btAlignedFree	(void* ptr)
+{
+	_aligned_free(ptr);
+}
+
+#else
+
+#ifdef __CELLOS_LV2__
+
+#include <stdlib.h>
+
+int numAllocs = 0;
+int numFree = 0;
+
+void*	btAlignedAlloc	(int size, int alignment)
+{
+	numAllocs++;
+	return memalign(alignment, size);
+}
+
+void	btAlignedFree	(void* ptr)
+{
+	numFree++;
+	free(ptr);
+}
+
+#else
+///todo
+///will add some multi-platform version that works without _aligned_malloc/_aligned_free
+
+void*	btAlignedAlloc	(int size, int alignment)
+{
+	return new char[size];
+}
+
+void	btAlignedFree	(void* ptr)
+{
+	delete [] (char*) ptr;
+}
+#endif //
+
+#endif
+
+

bullet/src/LinearMath/btAlignedAllocator.h

@@ -0,0 +1,80 @@
+/*
+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 BT_ALIGNED_ALLOCATOR
+#define BT_ALIGNED_ALLOCATOR
+
+///we probably replace this with our own aligned memory allocator
+///so we replace _aligned_malloc and _aligned_free with our own
+///that is better portable and more predictable
+
+#include "btScalar.h"
+
+void*	btAlignedAlloc	(int size, int alignment);
+
+void	btAlignedFree	(void* ptr);
+
+
+typedef int	size_type;
+
+
+template < typename T , unsigned Alignment >
+class btAlignedAllocator {
+	
+	typedef btAlignedAllocator< T , Alignment > self_type;
+	
+public:
+
+	//just going down a list:
+	btAlignedAllocator() {}
+	/*
+	btAlignedAllocator( const self_type & ) {}
+	*/
+
+	template < typename Other >
+	btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {}
+
+	typedef const T*         const_pointer;
+	typedef const T&         const_reference;
+	typedef T*               pointer;
+	typedef T&               reference;
+	typedef T                value_type;
+
+	pointer       address   ( reference        ref ) const                           { return &ref; }
+	const_pointer address   ( const_reference  ref ) const                           { return &ref; }
+	pointer       allocate  ( size_type        n   , const_pointer *      hint = 0 ) {
+		(void)hint;
+		return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment ));
+	}
+	void          construct ( pointer          ptr , const value_type &   value    ) { new (ptr) value_type( value ); }
+	void          deallocate( pointer          ptr ) {
+		btAlignedFree( reinterpret_cast< void * >( ptr ) );
+	}
+	void          destroy   ( pointer          ptr )                                 { ptr->~value_type(); }
+	
+
+	template < typename O > struct rebind {
+		typedef btAlignedAllocator< O , Alignment > other;
+	};
+	template < typename O >
+	self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; }
+
+	friend bool operator==( const self_type & , const self_type & ) { return true; }
+};
+
+
+
+#endif //BT_ALIGNED_ALLOCATOR
+

bullet/src/LinearMath/btAlignedObjectArray.h

@@ -0,0 +1,367 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef BT_OBJECT_ARRAY__
+#define BT_OBJECT_ARRAY__
+
+#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
+#include "btAlignedAllocator.h"
+
+///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW
+///then the btAlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
+///You can enable BT_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
+///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
+///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
+
+#define BT_USE_PLACEMENT_NEW 1
+//#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
+
+#ifdef BT_USE_MEMCPY
+#include <memory.h>
+#include <string.h>
+#endif //BT_USE_MEMCPY
+
+#ifdef BT_USE_PLACEMENT_NEW
+#include <new> //for placement new
+#endif //BT_USE_PLACEMENT_NEW
+
+
+///btAlignedObjectArray uses a subset of the stl::vector interface for its methods
+///It is developed to replace stl::vector to avoid STL alignment issues to add SIMD/SSE data
+template <typename T> 
+//template <class T> 
+class btAlignedObjectArray
+{
+	btAlignedAllocator<T , 16>	m_allocator;
+
+	int					m_size;
+	int					m_capacity;
+	T*					m_data;
+
+	protected:
+		SIMD_FORCE_INLINE	int	allocSize(int size)
+		{
+			return (size ? size*2 : 1);
+		}
+		SIMD_FORCE_INLINE	void	copy(int start,int end, T* dest)
+		{
+			int i;
+			for (i=start;i<end;++i)
+#ifdef BT_USE_PLACEMENT_NEW
+				new (&dest[i]) T(m_data[i]);
+#else
+				dest[i] = m_data[i];
+#endif //BT_USE_PLACEMENT_NEW
+		}
+
+		SIMD_FORCE_INLINE	void	init()
+		{
+			m_data = 0;
+			m_size = 0;
+			m_capacity = 0;
+		}
+		SIMD_FORCE_INLINE	void	destroy(int first,int last)
+		{
+			int i;
+			for (i=first; i<last;i++)
+			{
+				m_data[i].~T();
+			}
+		}
+
+		SIMD_FORCE_INLINE	void* allocate(int size)
+		{
+			if (size)
+				return m_allocator.allocate(size);
+			return 0;
+		}
+
+		SIMD_FORCE_INLINE	void	deallocate()
+		{
+			if(m_data)	{
+				m_allocator.deallocate(m_data);
+				m_data = 0;
+			}
+		}
+
+	
+
+
+	public:
+		
+		btAlignedObjectArray()
+		{
+			init();
+		}
+
+		~btAlignedObjectArray()
+		{
+			clear();
+		}
+
+		SIMD_FORCE_INLINE	int capacity() const
+		{	// return current length of allocated storage
+			return m_capacity;
+		}
+		
+		SIMD_FORCE_INLINE	int size() const
+		{	// return length of sequence
+			return m_size;
+		}
+		
+		SIMD_FORCE_INLINE const T& operator[](int n) const
+		{
+			return m_data[n];
+		}
+
+		SIMD_FORCE_INLINE T& operator[](int n)
+		{
+			return m_data[n];
+		}
+		
+
+		SIMD_FORCE_INLINE	void	clear()
+		{
+			destroy(0,size());
+			
+			deallocate();
+			
+			init();
+		}
+
+		SIMD_FORCE_INLINE	void	pop_back()
+		{
+			m_size--;
+			m_data[m_size].~T();
+		}
+
+		SIMD_FORCE_INLINE	void	resize(int newsize, const T& fillData=T())
+		{
+			int curSize = size();
+
+			if (newsize < size())
+			{
+				for(int i = curSize; i < newsize; i++)
+				{
+					m_data[i].~T();
+				}
+			} else
+			{
+				if (newsize > size())
+				{
+					reserve(newsize);
+				}
+#ifdef BT_USE_PLACEMENT_NEW
+				for (int i=curSize;i<newsize;i++)
+				{
+					new ( &m_data[i]) T(fillData);
+				}
+#endif //BT_USE_PLACEMENT_NEW
+
+			}
+
+			m_size = newsize;
+		}
+	
+
+		SIMD_FORCE_INLINE	T&  expand( const T& fillValue=T())
+		{	
+			int sz = size();
+			if( sz == capacity() )
+			{
+				reserve( allocSize(size()) );
+			}
+			m_size++;
+#ifdef BT_USE_PLACEMENT_NEW
+			new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
+#endif
+
+			return m_data[sz];		
+		}
+
+
+		SIMD_FORCE_INLINE	void push_back(const T& _Val)
+		{	
+			int sz = size();
+			if( sz == capacity() )
+			{
+				reserve( allocSize(size()) );
+			}
+			
+#ifdef BT_USE_PLACEMENT_NEW
+			new ( &m_data[m_size] ) T(_Val);
+#else
+			m_data[size()] = _Val;			
+#endif //BT_USE_PLACEMENT_NEW
+
+			m_size++;
+		}
+
+	
+		
+		SIMD_FORCE_INLINE	void reserve(int _Count)
+		{	// determine new minimum length of allocated storage
+			if (capacity() < _Count)
+			{	// not enough room, reallocate
+				T*	s = (T*)allocate(_Count);
+
+				copy(0, size(), s);
+
+				destroy(0,size());
+
+				deallocate();
+
+				m_data = s;
+				
+				m_capacity = _Count;
+
+			}
+		}
+
+
+		class less
+		{
+			public:
+
+				bool operator() ( const T& a, const T& b )
+				{
+					return ( a < b );
+				}
+		};
+	
+
+		///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+		template <typename L>
+		void downHeap(T *pArr, int k, int n,L CompareFunc)
+		{
+			/*  PRE: a[k+1..N] is a heap */
+			/* POST:  a[k..N]  is a heap */
+			
+			T temp = pArr[k - 1];
+			/* k has child(s) */
+			while (k <= n/2) 
+			{
+				int child = 2*k;
+				
+				if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child]))
+				{
+					child++;
+				}
+				/* pick larger child */
+				if (CompareFunc(temp , pArr[child - 1]))
+				{
+					/* move child up */
+					pArr[k - 1] = pArr[child - 1];
+					k = child;
+				}
+				else
+				{
+					break;
+				}
+			}
+			pArr[k - 1] = temp;
+		} /*downHeap*/
+
+		void	swap(int index0,int index1)
+		{
+#ifdef BT_USE_MEMCPY
+			char	temp[sizeof(T)];
+			memcpy(temp,&m_data[index0],sizeof(T));
+			memcpy(&m_data[index0],&m_data[index1],sizeof(T));
+			memcpy(&m_data[index1],temp,sizeof(T));
+#else
+			T temp = m_data[index0];
+			m_data[index0] = m_data[index1];
+			m_data[index1] = temp;
+#endif //BT_USE_PLACEMENT_NEW
+
+		}
+
+	template <typename L>
+	void heapSort(L CompareFunc)
+	{
+		/* sort a[0..N-1],  N.B. 0 to N-1 */
+		int k;
+		int n = m_size;
+		for (k = n/2; k > 0; k--) 
+		{
+			downHeap(m_data, k, n, CompareFunc);
+		}
+
+		/* a[1..N] is now a heap */
+		while ( n>=1 ) 
+		{
+			swap(0,n-1); /* largest of a[0..n-1] */
+
+
+			n = n - 1;
+			/* restore a[1..i-1] heap */
+			downHeap(m_data, 1, n, CompareFunc);
+		} 
+	}
+
+	///non-recursive binary search, assumes sorted array
+	int	findBinarySearch(const T& key) const
+	{
+		int first = 0;
+		int last = size();
+
+		//assume sorted array
+		while (first <= last) {
+			int mid = (first + last) / 2;  // compute mid point.
+			if (key > m_data[mid]) 
+				first = mid + 1;  // repeat search in top half.
+			else if (key < m_data[mid]) 
+				last = mid - 1; // repeat search in bottom half.
+			else
+				return mid;     // found it. return position /////
+		}
+		return size();    // failed to find key
+	}
+
+
+	int	findLinearSearch(const T& key) const
+	{
+		int index=size();
+		int i;
+
+		for (i=0;i<size();i++)
+		{
+			if (m_data[i] == key)
+			{
+				index = i;
+				break;
+			}
+		}
+		return index;
+	}
+
+	void	remove(const T& key)
+	{
+
+		int findIndex = findLinearSearch(key);
+		if (findIndex<size())
+		{
+			swap( findIndex,size()-1);
+			pop_back();
+		}
+	}
+
+};
+
+#endif //BT_OBJECT_ARRAY__
+
+

bullet/src/LinearMath/btDefaultMotionState.h

@@ -0,0 +1,38 @@
+#ifndef DEFAULT_MOTION_STATE_H
+#define DEFAULT_MOTION_STATE_H
+
+///btDefaultMotionState provides a common implementation to synchronize world transforms with offsets
+struct	btDefaultMotionState : public btMotionState
+{
+	btTransform m_graphicsWorldTrans;
+	btTransform	m_centerOfMassOffset;
+	btTransform m_startWorldTrans;
+	void*		m_userPointer;
+
+	btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity())
+		: m_graphicsWorldTrans(startTrans),
+		m_centerOfMassOffset(centerOfMassOffset),
+		m_startWorldTrans(startTrans),
+		m_userPointer(0)
+
+	{
+	}
+
+	///synchronizes world transform from user to physics
+	virtual void	getWorldTransform(btTransform& centerOfMassWorldTrans ) const 
+	{
+			centerOfMassWorldTrans = 	m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ;
+	}
+
+	///synchronizes world transform from physics to user
+	///Bullet only calls the update of worldtransform for active objects
+	virtual void	setWorldTransform(const btTransform& centerOfMassWorldTrans)
+	{
+			m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ;
+	}
+
+	
+
+};
+
+#endif //DEFAULT_MOTION_STATE_H

bullet/src/LinearMath/btGeometryUtil.cpp

@@ -0,0 +1,178 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 "btGeometryUtil.h"
+
+
+/*
+  Make sure this dummy function never changes so that it
+  can be used by probes that are checking whether the
+  library is actually installed.
+*/
+extern "C" void btBulletMathProbe () {}
+
+
+bool	btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar	margin)
+{
+	int numbrushes = planeEquations.size();
+	for (int i=0;i<numbrushes;i++)
+	{
+		const btVector3& N1 = planeEquations[i];
+		btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin;
+		if (dist>btScalar(0.))
+		{
+			return false;
+		}
+	}
+	return true;
+		
+}
+
+
+bool	btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar	margin)
+{
+	int numvertices = vertices.size();
+	for (int i=0;i<numvertices;i++)
+	{
+		const btVector3& N1 = vertices[i];
+		btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
+		if (dist>btScalar(0.))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
+{
+	int numbrushes = planeEquations.size();
+	for (int i=0;i<numbrushes;i++)
+	{
+		const btVector3& N1 = planeEquations[i];
+		if (planeEquation.dot(N1) > btScalar(0.999))
+		{
+			return false;
+		} 
+	}
+	return true;
+}
+
+void	btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
+{
+		const int numvertices = vertices.size();
+	// brute force:
+	for (int i=0;i<numvertices;i++)
+	{
+		const btVector3& N1 = vertices[i];
+		
+
+		for (int j=i+1;j<numvertices;j++)
+		{
+			const btVector3& N2 = vertices[j];
+				
+			for (int k=j+1;k<numvertices;k++)
+			{
+
+				const btVector3& N3 = vertices[k];
+
+				btVector3 planeEquation,edge0,edge1;
+				edge0 = N2-N1;
+				edge1 = N3-N1;
+				btScalar normalSign = btScalar(1.);
+				for (int ww=0;ww<2;ww++)
+				{
+					planeEquation = normalSign * edge0.cross(edge1);
+					if (planeEquation.length2() > btScalar(0.0001))
+					{
+						planeEquation.normalize();
+						if (notExist(planeEquation,planeEquationsOut))
+						{
+							planeEquation[3] = -planeEquation.dot(N1);
+							
+								//check if inside, and replace supportingVertexOut if needed
+								if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
+								{
+									planeEquationsOut.push_back(planeEquation);
+								}
+						}
+					}
+					normalSign = btScalar(-1.);
+				}
+			
+			}
+		}
+	}
+
+}
+
+void	btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
+{
+	const int numbrushes = planeEquations.size();
+	// brute force:
+	for (int i=0;i<numbrushes;i++)
+	{
+		const btVector3& N1 = planeEquations[i];
+		
+
+		for (int j=i+1;j<numbrushes;j++)
+		{
+			const btVector3& N2 = planeEquations[j];
+				
+			for (int k=j+1;k<numbrushes;k++)
+			{
+
+				const btVector3& N3 = planeEquations[k];
+
+				btVector3 n2n3; n2n3 = N2.cross(N3);
+				btVector3 n3n1; n3n1 = N3.cross(N1);
+				btVector3 n1n2; n1n2 = N1.cross(N2);
+				
+				if ( ( n2n3.length2() > btScalar(0.0001) ) &&
+					 ( n3n1.length2() > btScalar(0.0001) ) &&
+					 ( n1n2.length2() > btScalar(0.0001) ) )
+				{
+					//point P out of 3 plane equations:
+
+					//	d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
+					//P =  -------------------------------------------------------------------------  
+					//   N1 . ( N2 * N3 )  
+
+
+					btScalar quotient = (N1.dot(n2n3));
+					if (btFabs(quotient) > btScalar(0.000001))
+					{
+						quotient = btScalar(-1.) / quotient;
+						n2n3 *= N1[3];
+						n3n1 *= N2[3];
+						n1n2 *= N3[3];
+						btVector3 potentialVertex = n2n3;
+						potentialVertex += n3n1;
+						potentialVertex += n1n2;
+						potentialVertex *= quotient;
+
+						//check if inside, and replace supportingVertexOut if needed
+						if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
+						{
+							verticesOut.push_back(potentialVertex);
+						}
+					}
+				}
+			}
+		}
+	}
+}
+

bullet/src/LinearMath/btGeometryUtil.h

@@ -0,0 +1,41 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 BT_GEOMETRY_UTIL_H
+#define BT_GEOMETRY_UTIL_H
+
+#include "btVector3.h"
+#include "btAlignedObjectArray.h"
+
+class btGeometryUtil
+{
+	public:
+	
+	
+		static void	getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut );
+
+		static void	getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut );
+	
+		static bool	isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, btScalar	margin);
+		
+		static bool	isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar	margin);
+
+		static bool	areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar	margin);
+
+};
+
+
+#endif //BT_GEOMETRY_UTIL_H
+

bullet/src/LinearMath/btIDebugDraw.h

@@ -0,0 +1,100 @@
+/*
+Copyright (c) 2005 Gino van den Bergen / Erwin Coumans http://continuousphysics.com
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+*/
+
+
+#ifndef IDEBUG_DRAW__H
+#define IDEBUG_DRAW__H
+
+#include "btVector3.h"
+
+
+class	btIDebugDraw
+{
+	public:
+
+	enum	DebugDrawModes
+	{
+		DBG_NoDebug=0,
+		DBG_DrawWireframe = 1,
+		DBG_DrawAabb=2,
+		DBG_DrawFeaturesText=4,
+		DBG_DrawContactPoints=8,
+		DBG_NoDeactivation=16,
+		DBG_NoHelpText = 32,
+		DBG_DrawText=64,
+		DBG_ProfileTimings = 128,
+		DBG_EnableSatComparison = 256,
+		DBG_DisableBulletLCP = 512,
+		DBG_EnableCCD = 1024,
+		DBG_MAX_DEBUG_DRAW_MODE
+	};
+
+	virtual ~btIDebugDraw() {};
+
+	virtual void	drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
+
+	virtual void	drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0;
+
+	virtual void	reportErrorWarning(const char* warningString) = 0;
+
+	virtual void	setDebugMode(int debugMode) =0;
+	
+	virtual int		getDebugMode() const = 0;
+
+	inline void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
+	{
+
+		btVector3 halfExtents = (to-from)* 0.5f;
+		btVector3 center = (to+from) *0.5f;
+		int i,j;
+
+		btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
+		for (i=0;i<4;i++)
+		{
+			for (j=0;j<3;j++)
+			{
+				pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],		
+					edgecoord[2]*halfExtents[2]);
+				pa+=center;
+
+				int othercoord = j%3;
+				edgecoord[othercoord]*=-1.f;
+				pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],	
+					edgecoord[2]*halfExtents[2]);
+				pb+=center;
+
+				drawLine(pa,pb,color);
+			}
+			edgecoord = btVector3(-1.f,-1.f,-1.f);
+			if (i<3)
+				edgecoord[i]*=-1.f;
+		}
+	}
+};
+
+
+#endif //IDEBUG_DRAW__H
+

bullet/src/LinearMath/btList.h

@@ -0,0 +1,73 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 GEN_LIST_H
+#define GEN_LIST_H
+
+class btGEN_Link {
+public:
+    btGEN_Link() : m_next(0), m_prev(0) {}
+    btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {}
+    
+    btGEN_Link *getNext() const { return m_next; }  
+    btGEN_Link *getPrev() const { return m_prev; }  
+
+    bool isHead() const { return m_prev == 0; }
+    bool isTail() const { return m_next == 0; }
+
+    void insertBefore(btGEN_Link *link) {
+        m_next         = link;
+        m_prev         = link->m_prev;
+        m_next->m_prev = this;
+        m_prev->m_next = this;
+    } 
+
+    void insertAfter(btGEN_Link *link) {
+        m_next         = link->m_next;
+        m_prev         = link;
+        m_next->m_prev = this;
+        m_prev->m_next = this;
+    } 
+
+    void remove() { 
+        m_next->m_prev = m_prev; 
+        m_prev->m_next = m_next;
+    }
+
+private:  
+    btGEN_Link  *m_next;
+    btGEN_Link  *m_prev;
+};
+
+class btGEN_List {
+public:
+    btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
+
+    btGEN_Link *getHead() const { return m_head.getNext(); } 
+    btGEN_Link *getTail() const { return m_tail.getPrev(); } 
+
+    void addHead(btGEN_Link *link) { link->insertAfter(&m_head); }
+    void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); }
+    
+private:
+    btGEN_Link m_head;
+    btGEN_Link m_tail;
+};
+
+#endif
+
+
+

bullet/src/LinearMath/btMatrix3x3.h

@@ -0,0 +1,410 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 btMatrix3x3_H
+#define btMatrix3x3_H
+
+#include "btScalar.h"
+
+#include "btVector3.h"
+#include "btQuaternion.h"
+
+
+class btMatrix3x3 {
+	public:
+		btMatrix3x3 () {}
+		
+//		explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
+		
+		explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
+		/*
+		template <typename btScalar>
+		Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+		{ 
+			setEulerYPR(yaw, pitch, roll);
+		}
+		*/
+		btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
+				  const btScalar& yx, const btScalar& yy, const btScalar& yz,
+				  const btScalar& zx, const btScalar& zy, const btScalar& zz)
+		{ 
+			setValue(xx, xy, xz, 
+					 yx, yy, yz, 
+					 zx, zy, zz);
+		}
+		
+		SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
+		{
+			m_el[0] = other.m_el[0];
+			m_el[1] = other.m_el[1];
+			m_el[2] = other.m_el[2];
+		}
+
+		SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
+		{
+			m_el[0] = other.m_el[0];
+			m_el[1] = other.m_el[1];
+			m_el[2] = other.m_el[2];
+			return *this;
+		}
+
+		SIMD_FORCE_INLINE btVector3 getColumn(int i) const
+		{
+			return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
+		}
+		
+
+
+		SIMD_FORCE_INLINE const btVector3& getRow(int i) const
+		{
+			return m_el[i];
+		}
+
+
+		SIMD_FORCE_INLINE btVector3&  operator[](int i)
+		{ 
+			btFullAssert(0 <= i && i < 3);
+			return m_el[i]; 
+		}
+		
+		SIMD_FORCE_INLINE const btVector3& operator[](int i) const
+		{
+			btFullAssert(0 <= i && i < 3);
+			return m_el[i]; 
+		}
+		
+		btMatrix3x3& operator*=(const btMatrix3x3& m); 
+		
+	
+	void setFromOpenGLSubMatrix(const btScalar *m)
+		{
+			m_el[0].setValue(m[0],m[4],m[8]);
+			m_el[1].setValue(m[1],m[5],m[9]);
+			m_el[2].setValue(m[2],m[6],m[10]);
+
+		}
+
+		void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 
+					  const btScalar& yx, const btScalar& yy, const btScalar& yz, 
+					  const btScalar& zx, const btScalar& zy, const btScalar& zz)
+		{
+			m_el[0].setValue(xx,xy,xz);
+			m_el[1].setValue(yx,yy,yz);
+			m_el[2].setValue(zx,zy,zz);
+		}
+  
+		void setRotation(const btQuaternion& q) 
+		{
+			btScalar d = q.length2();
+			btFullAssert(d != btScalar(0.0));
+			btScalar s = btScalar(2.0) / d;
+			btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
+			btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
+			btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
+			btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
+			setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
+					 xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
+					 xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
+		}
+		
+
+
+		void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 
+		{
+
+			btScalar cy(btCos(yaw)); 
+			btScalar  sy(btSin(yaw)); 
+			btScalar  cp(btCos(pitch)); 
+			btScalar  sp(btSin(pitch)); 
+			btScalar  cr(btCos(roll));
+			btScalar  sr(btSin(roll));
+			btScalar  cc = cy * cr; 
+			btScalar  cs = cy * sr; 
+			btScalar  sc = sy * cr; 
+			btScalar  ss = sy * sr;
+			setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
+                     cp * sr,       cp * cr,      -sp,
+					 sc + sp * cs, -ss + sp * cc,  cy * cp);
+		
+		}
+
+	/**
+	 * setEulerZYX
+	 * @param euler a const reference to a btVector3 of euler angles
+	 * These angles are used to produce a rotation matrix. The euler
+	 * angles are applied in ZYX order. I.e a vector is first rotated 
+	 * about X then Y and then Z
+	 **/
+	
+	void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
+		btScalar ci ( btCos(eulerX)); 
+		btScalar cj ( btCos(eulerY)); 
+		btScalar ch ( btCos(eulerZ)); 
+		btScalar si ( btSin(eulerX)); 
+		btScalar sj ( btSin(eulerY)); 
+		btScalar sh ( btSin(eulerZ)); 
+		btScalar cc = ci * ch; 
+		btScalar cs = ci * sh; 
+		btScalar sc = si * ch; 
+		btScalar ss = si * sh;
+		
+		setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+				 cj * sh, sj * ss + cc, sj * cs - sc, 
+	       			 -sj,      cj * si,      cj * ci);
+	}
+
+		void setIdentity()
+		{ 
+			setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
+					 btScalar(0.0), btScalar(1.0), btScalar(0.0), 
+					 btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
+		}
+    
+		void getOpenGLSubMatrix(btScalar *m) const 
+		{
+			m[0]  = btScalar(m_el[0].x()); 
+			m[1]  = btScalar(m_el[1].x());
+			m[2]  = btScalar(m_el[2].x());
+			m[3]  = btScalar(0.0); 
+			m[4]  = btScalar(m_el[0].y());
+			m[5]  = btScalar(m_el[1].y());
+			m[6]  = btScalar(m_el[2].y());
+			m[7]  = btScalar(0.0); 
+			m[8]  = btScalar(m_el[0].z()); 
+			m[9]  = btScalar(m_el[1].z());
+			m[10] = btScalar(m_el[2].z());
+			m[11] = btScalar(0.0); 
+		}
+
+		void getRotation(btQuaternion& q) const
+		{
+			btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
+			btScalar temp[4];
+			
+			if (trace > btScalar(0.0)) 
+			{
+				btScalar s = btSqrt(trace + btScalar(1.0));
+				temp[3]=(s * btScalar(0.5));
+				s = btScalar(0.5) / s;
+				
+				temp[0]=((m_el[2].y() - m_el[1].z()) * s);
+				temp[1]=((m_el[0].z() - m_el[2].x()) * s);
+				temp[2]=((m_el[1].x() - m_el[0].y()) * s);
+			} 
+			else 
+			{
+				int i = m_el[0].x() < m_el[1].y() ? 
+					(m_el[1].y() < m_el[2].z() ? 2 : 1) :
+					(m_el[0].x() < m_el[2].z() ? 2 : 0); 
+				int j = (i + 1) % 3;  
+				int k = (i + 2) % 3;
+				
+				btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
+				temp[i] = s * btScalar(0.5);
+				s = btScalar(0.5) / s;
+				
+				temp[3] = (m_el[k][j] - m_el[j][k]) * s;
+				temp[j] = (m_el[j][i] + m_el[i][j]) * s;
+				temp[k] = (m_el[k][i] + m_el[i][k]) * s;
+			}
+			q.setValue(temp[0],temp[1],temp[2],temp[3]);
+		}
+	
+		void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const
+		{
+			
+			if (btScalar(m_el[1].z()) < btScalar(1))
+			{
+				if (btScalar(m_el[1].z()) > -btScalar(1))
+				{
+					yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
+					pitch = btScalar(btAsin(-m_el[1].y()));
+					roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
+				}
+				else 
+				{
+					yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z()));
+					pitch = SIMD_HALF_PI;
+					roll = btScalar(0.0);
+				}
+			}
+			else
+			{
+				yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z()));
+				pitch = -SIMD_HALF_PI;
+				roll = btScalar(0.0);
+			}
+		}
+		
+
+	
+		
+		btMatrix3x3 scaled(const btVector3& s) const
+		{
+			return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
+									 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
+									 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
+		}
+
+		btScalar            determinant() const;
+		btMatrix3x3 adjoint() const;
+		btMatrix3x3 absolute() const;
+		btMatrix3x3 transpose() const;
+		btMatrix3x3 inverse() const; 
+		
+		btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
+		btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
+		
+		SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 
+		{
+			return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
+		}
+		SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 
+		{
+			return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
+		}
+		SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 
+		{
+			return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
+		}
+		
+
+		
+	protected:
+		btScalar cofac(int r1, int c1, int r2, int c2) const 
+		{
+			return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
+		}
+
+		btVector3 m_el[3];
+	};
+	
+	SIMD_FORCE_INLINE btMatrix3x3& 
+	btMatrix3x3::operator*=(const btMatrix3x3& m)
+	{
+		setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
+				 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
+				 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
+		return *this;
+	}
+	
+	SIMD_FORCE_INLINE btScalar 
+	btMatrix3x3::determinant() const
+	{ 
+		return triple((*this)[0], (*this)[1], (*this)[2]);
+	}
+	
+
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::absolute() const
+	{
+		return btMatrix3x3(
+			btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
+			btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
+			btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
+	}
+
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::transpose() const 
+	{
+		return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
+								 m_el[0].y(), m_el[1].y(), m_el[2].y(),
+								 m_el[0].z(), m_el[1].z(), m_el[2].z());
+	}
+	
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::adjoint() const 
+	{
+		return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
+								 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
+								 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
+	}
+	
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::inverse() const
+	{
+		btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
+		btScalar det = (*this)[0].dot(co);
+		btFullAssert(det != btScalar(0.0));
+		btScalar s = btScalar(1.0) / det;
+		return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
+								 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+								 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
+	}
+	
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
+	{
+		return btMatrix3x3(
+			m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
+			m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
+			m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
+			m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
+			m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
+			m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
+			m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
+			m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
+			m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].x());
+	}
+	
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
+	{
+		return btMatrix3x3(
+			m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
+			m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
+			m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
+		
+	}
+
+	SIMD_FORCE_INLINE btVector3 
+	operator*(const btMatrix3x3& m, const btVector3& v) 
+	{
+		return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
+	}
+	
+
+	SIMD_FORCE_INLINE btVector3
+	operator*(const btVector3& v, const btMatrix3x3& m)
+	{
+		return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
+	}
+
+	SIMD_FORCE_INLINE btMatrix3x3 
+	operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
+	{
+		return btMatrix3x3(
+			m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
+			m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
+			m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
+	}
+
+/*
+	SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
+    return btMatrix3x3(
+        m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
+        m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
+        m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
+        m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
+        m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
+        m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
+        m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
+        m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
+        m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
+}
+*/
+
+
+#endif

bullet/src/LinearMath/btMinMax.h

@@ -0,0 +1,69 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 GEN_MINMAX_H
+#define GEN_MINMAX_H
+
+template <class T>
+SIMD_FORCE_INLINE const T& GEN_min(const T& a, const T& b) 
+{
+  return b < a ? b : a;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& GEN_max(const T& a, const T& b) 
+{
+  return  a < b ? b : a;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) 
+{
+	return a < lb ? lb : (ub < a ? ub : a); 
+}
+
+template <class T>
+SIMD_FORCE_INLINE void GEN_set_min(T& a, const T& b) 
+{
+    if (b < a) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+SIMD_FORCE_INLINE void GEN_set_max(T& a, const T& b) 
+{
+    if (a < b) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) 
+{
+	if (a < lb) 
+	{
+		a = lb; 
+	}
+	else if (ub < a) 
+	{
+		a = ub;
+	}
+}
+
+#endif

bullet/src/LinearMath/btMotionState.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 BT_MOTIONSTATE_H
+#define BT_MOTIONSTATE_H
+
+#include "btTransform.h"
+
+///btMotionState allows the dynamics world to synchronize the updated world transforms with graphics
+///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation)
+class	btMotionState
+{
+	public:
+		
+		virtual ~btMotionState()
+		{
+			
+		}
+		
+		virtual void	getWorldTransform(btTransform& worldTrans ) const =0;
+
+		//Bullet only calls the update of worldtransform for active objects
+		virtual void	setWorldTransform(const btTransform& worldTrans)=0;
+		
+	
+};
+
+#endif //BT_MOTIONSTATE_H

bullet/src/LinearMath/btPoint3.h

@@ -0,0 +1,24 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 btPoint3_H
+#define btPoint3_H
+
+#include "btVector3.h"
+
+typedef btVector3 btPoint3;
+
+#endif

bullet/src/LinearMath/btQuadWord.h

@@ -0,0 +1,130 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 SIMD_QUADWORD_H
+#define SIMD_QUADWORD_H
+
+#include "btScalar.h"
+#include "btSimdMinMax.h"
+
+class btQuadWordStorage
+{
+protected:
+	btScalar	m_x;
+	btScalar	m_y;
+	btScalar	m_z;
+	btScalar	m_unusedW;
+};
+
+
+///btQuadWord is base-class for vectors, points
+class	btQuadWord : public btQuadWordStorage
+{
+	public:
+	
+//		SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_x)[i];	}      
+//		SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; }
+
+		SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; }
+
+		SIMD_FORCE_INLINE const btScalar& getY() const { return m_y; }
+
+		SIMD_FORCE_INLINE const btScalar& getZ() const { return m_z; }
+
+		SIMD_FORCE_INLINE void	setX(btScalar x) { m_x = x;};
+
+		SIMD_FORCE_INLINE void	setY(btScalar y) { m_y = y;};
+
+		SIMD_FORCE_INLINE void	setZ(btScalar z) { m_z = z;};
+
+		SIMD_FORCE_INLINE void	setW(btScalar w) { m_unusedW = w;};
+
+		SIMD_FORCE_INLINE const btScalar& x() const { return m_x; }
+
+		SIMD_FORCE_INLINE const btScalar& y() const { return m_y; }
+
+		SIMD_FORCE_INLINE const btScalar& z() const { return m_z; }
+
+		SIMD_FORCE_INLINE const btScalar& w() const { return m_unusedW; }
+
+
+		SIMD_FORCE_INLINE	operator       btScalar *()       { return &m_x; }
+		SIMD_FORCE_INLINE	operator const btScalar *() const { return &m_x; }
+
+		SIMD_FORCE_INLINE void 	setValue(const btScalar& x, const btScalar& y, const btScalar& z)
+		{
+			m_x=x;
+			m_y=y;
+			m_z=z;
+			m_unusedW = 0.f;
+		}
+
+/*		void getValue(btScalar *m) const 
+		{
+			m[0] = m_x;
+			m[1] = m_y;
+			m[2] = m_z;
+		}
+*/
+		SIMD_FORCE_INLINE void	setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
+		{
+			m_x=x;
+			m_y=y;
+			m_z=z;
+			m_unusedW=w;
+		}
+
+		SIMD_FORCE_INLINE btQuadWord()
+		//	:m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.))
+		{
+		}
+
+		SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
+		{
+			*((btQuadWordStorage*)this) = q;
+		}
+
+		SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z)		
+		{
+			m_x = x, m_y = y, m_z = z, m_unusedW = 0.0f;
+		}
+
+		SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
+		{
+			m_x = x, m_y = y, m_z = z, m_unusedW = w;
+		}
+
+
+		SIMD_FORCE_INLINE void	setMax(const btQuadWord& other)
+		{
+			btSetMax(m_x, other.m_x);
+			btSetMax(m_y, other.m_y);
+			btSetMax(m_z, other.m_z);
+			btSetMax(m_unusedW, other.m_unusedW);
+		}
+
+		SIMD_FORCE_INLINE void	setMin(const btQuadWord& other)
+		{
+			btSetMin(m_x, other.m_x);
+			btSetMin(m_y, other.m_y);
+			btSetMin(m_z, other.m_z);
+			btSetMin(m_unusedW, other.m_unusedW);
+		}
+
+
+
+};
+
+#endif //SIMD_QUADWORD_H

bullet/src/LinearMath/btQuaternion.h

@@ -0,0 +1,321 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 SIMD__QUATERNION_H_
+#define SIMD__QUATERNION_H_
+
+#include "btVector3.h"
+
+class btQuaternion : public btQuadWord {
+public:
+	btQuaternion() {}
+
+	//		template <typename btScalar>
+	//		explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
+
+	btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) 
+		: btQuadWord(x, y, z, w) 
+	{}
+
+	btQuaternion(const btVector3& axis, const btScalar& angle) 
+	{ 
+		setRotation(axis, angle); 
+	}
+
+	btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+	{ 
+		setEuler(yaw, pitch, roll); 
+	}
+
+	void setRotation(const btVector3& axis, const btScalar& angle)
+	{
+		btScalar d = axis.length();
+		assert(d != btScalar(0.0));
+		btScalar s = btSin(angle * btScalar(0.5)) / d;
+		setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
+			btCos(angle * btScalar(0.5)));
+	}
+
+	void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+	{
+		btScalar halfYaw = btScalar(yaw) * btScalar(0.5);  
+		btScalar halfPitch = btScalar(pitch) * btScalar(0.5);  
+		btScalar halfRoll = btScalar(roll) * btScalar(0.5);  
+		btScalar cosYaw = btCos(halfYaw);
+		btScalar sinYaw = btSin(halfYaw);
+		btScalar cosPitch = btCos(halfPitch);
+		btScalar sinPitch = btSin(halfPitch);
+		btScalar cosRoll = btCos(halfRoll);
+		btScalar sinRoll = btSin(halfRoll);
+		setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
+			cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
+			sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
+			cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
+	}
+
+	btQuaternion& operator+=(const btQuaternion& q)
+	{
+		m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW;
+		return *this;
+	}
+
+	btQuaternion& operator-=(const btQuaternion& q) 
+	{
+		m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW;
+		return *this;
+	}
+
+	btQuaternion& operator*=(const btScalar& s)
+	{
+		m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
+		return *this;
+	}
+
+
+	btQuaternion& operator*=(const btQuaternion& q)
+	{
+		setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(),
+			m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(),
+			m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(),
+			m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z());
+		return *this;
+	}
+
+	btScalar dot(const btQuaternion& q) const
+	{
+		return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW;
+	}
+
+	btScalar length2() const
+	{
+		return dot(*this);
+	}
+
+	btScalar length() const
+	{
+		return btSqrt(length2());
+	}
+
+	btQuaternion& normalize() 
+	{
+		return *this /= length();
+	}
+
+	SIMD_FORCE_INLINE btQuaternion
+	operator*(const btScalar& s) const
+	{
+		return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
+	}
+
+
+
+	btQuaternion operator/(const btScalar& s) const
+	{
+		assert(s != btScalar(0.0));
+		return *this * (btScalar(1.0) / s);
+	}
+
+
+	btQuaternion& operator/=(const btScalar& s) 
+	{
+		assert(s != btScalar(0.0));
+		return *this *= btScalar(1.0) / s;
+	}
+
+
+	btQuaternion normalized() const 
+	{
+		return *this / length();
+	} 
+
+	btScalar angle(const btQuaternion& q) const 
+	{
+		btScalar s = btSqrt(length2() * q.length2());
+		assert(s != btScalar(0.0));
+		return btAcos(dot(q) / s);
+	}
+
+	btScalar getAngle() const 
+	{
+		btScalar s = btScalar(2.) * btAcos(m_unusedW);
+		return s;
+	}
+
+
+
+	btQuaternion inverse() const
+	{
+		return btQuaternion(m_x, m_y, m_z, -m_unusedW);
+	}
+
+	SIMD_FORCE_INLINE btQuaternion
+	operator+(const btQuaternion& q2) const
+	{
+		const btQuaternion& q1 = *this;
+		return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW);
+	}
+
+	SIMD_FORCE_INLINE btQuaternion
+	operator-(const btQuaternion& q2) const
+	{
+		const btQuaternion& q1 = *this;
+		return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW);
+	}
+
+	SIMD_FORCE_INLINE btQuaternion operator-() const
+	{
+		const btQuaternion& q2 = *this;
+		return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_unusedW);
+	}
+
+	SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const 
+	{
+		btQuaternion diff,sum;
+		diff = *this - qd;
+		sum = *this + qd;
+		if( diff.dot(diff) > sum.dot(sum) )
+			return qd;
+		return (-qd);
+	}
+
+	btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
+	{
+		btScalar theta = angle(q);
+		if (theta != btScalar(0.0))
+		{
+			btScalar d = btScalar(1.0) / btSin(theta);
+			btScalar s0 = btSin((btScalar(1.0) - t) * theta);
+			btScalar s1 = btSin(t * theta);   
+			return btQuaternion((m_x * s0 + q.x() * s1) * d,
+				(m_y * s0 + q.y() * s1) * d,
+				(m_z * s0 + q.z() * s1) * d,
+				(m_unusedW * s0 + q.m_unusedW * s1) * d);
+		}
+		else
+		{
+			return *this;
+		}
+	}
+
+	SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; }
+
+	
+};
+
+
+
+SIMD_FORCE_INLINE btQuaternion
+operator-(const btQuaternion& q)
+{
+	return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w());
+}
+
+
+
+
+SIMD_FORCE_INLINE btQuaternion
+operator*(const btQuaternion& q1, const btQuaternion& q2) {
+	return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
+		q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
+		q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
+		q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
+}
+
+SIMD_FORCE_INLINE btQuaternion
+operator*(const btQuaternion& q, const btVector3& w)
+{
+	return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
+		q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
+		q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
+		-q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
+}
+
+SIMD_FORCE_INLINE btQuaternion
+operator*(const btVector3& w, const btQuaternion& q)
+{
+	return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
+		w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
+		w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
+		-w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
+}
+
+SIMD_FORCE_INLINE btScalar 
+dot(const btQuaternion& q1, const btQuaternion& q2) 
+{ 
+	return q1.dot(q2); 
+}
+
+
+SIMD_FORCE_INLINE btScalar
+length(const btQuaternion& q) 
+{ 
+	return q.length(); 
+}
+
+SIMD_FORCE_INLINE btScalar
+angle(const btQuaternion& q1, const btQuaternion& q2) 
+{ 
+	return q1.angle(q2); 
+}
+
+
+SIMD_FORCE_INLINE btQuaternion
+inverse(const btQuaternion& q) 
+{
+	return q.inverse();
+}
+
+SIMD_FORCE_INLINE btQuaternion
+slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) 
+{
+	return q1.slerp(q2, t);
+}
+
+SIMD_FORCE_INLINE btVector3 
+quatRotate(const btQuaternion& rotation, const btVector3& v) 
+{
+	btQuaternion q = rotation * v;
+	q *= rotation.inverse();
+	return btVector3(q.getX(),q.getY(),q.getZ());
+}
+
+SIMD_FORCE_INLINE btQuaternion 
+shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
+{
+	btVector3 c = v0.cross(v1);
+	btScalar  d = v0.dot(v1);
+
+	if (d < -1.0 + SIMD_EPSILON)
+		return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector
+
+	btScalar  s = btSqrt((1.0f + d) * 2.0f);
+	btScalar rs = 1.0f / s;
+
+	return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
+}
+
+SIMD_FORCE_INLINE btQuaternion 
+shortestArcQuatNormalize(btVector3 v0,btVector3 v1)
+{
+	v0.normalize();
+	v1.normalize();
+	return shortestArcQuat(v0,v1);
+}
+
+#endif
+
+
+

bullet/src/LinearMath/btQuickprof.cpp

@@ -0,0 +1,38 @@
+/*
+Copyright (c) 2006 Tyler Streeter
+
+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.
+
+*/
+
+
+// Please visit the project website (http://quickprof.sourceforge.net) 
+// for usage instructions.
+
+// Credits: The Clock class was inspired by the Timer classes in 
+// Ogre (www.ogre3d.org).
+
+#include "LinearMath/btQuickprof.h"
+
+#ifdef USE_QUICKPROF
+
+// Note: We must declare these private static variables again here to 
+// avoid link errors.
+bool btProfiler::mEnabled = false;
+btClock btProfiler::mClock;
+unsigned long int btProfiler::mCurrentCycleStartMicroseconds = 0;
+unsigned long int btProfiler::mLastCycleDurationMicroseconds = 0;
+std::map<std::string, hidden::ProfileBlock*> btProfiler::mProfileBlocks;
+std::ofstream btProfiler::mOutputFile;
+bool btProfiler::mFirstFileOutput = true;
+btProfiler::BlockTimingMethod btProfiler::mFileOutputMethod;
+unsigned long int btProfiler::mCycleNumber = 0;
+#endif //USE_QUICKPROF

bullet/src/LinearMath/btQuickprof.h

@@ -0,0 +1,712 @@
+/*
+Copyright (c) 2006 Tyler Streeter
+
+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.
+
+*/
+	
+// Please visit the project website (http://quickprof.sourceforge.net) 
+// for usage instructions.
+
+// Credits: The Clock class was inspired by the Timer classes in 
+// Ogre (www.ogre3d.org).
+
+#ifndef QUICK_PROF_H
+#define QUICK_PROF_H
+
+#include "btScalar.h"
+
+//#define USE_QUICKPROF 1
+//Don't use quickprof for now, because it contains STL. TODO: replace STL by Bullet container classes.
+
+
+//if you don't need btClock, you can comment next line
+#define USE_BT_CLOCK 1
+
+#ifdef USE_BT_CLOCK
+#ifdef __CELLOS_LV2__
+#include <sys/sys_time.h>
+#include <stdio.h>
+typedef uint64_t __int64;
+#endif
+
+#if defined (SUNOS) || defined (__SUNOS__) 
+        #include <stdio.h> 
+#endif
+
+#if defined(WIN32) || defined(_WIN32)
+
+ #define USE_WINDOWS_TIMERS 
+   #define WIN32_LEAN_AND_MEAN 
+   #define NOWINRES 
+   #define NOMCX 
+   #define NOIME 
+#ifdef _XBOX
+	#include <Xtl.h>
+#else
+	#include <windows.h>
+#endif
+	#include <time.h>
+
+#else
+	#include <sys/time.h>
+#endif
+
+#define mymin(a,b) (a > b ? a : b)
+
+/// basic clock
+class btClock
+	{
+	public:
+		btClock()
+		{
+#ifdef USE_WINDOWS_TIMERS
+			QueryPerformanceFrequency(&mClockFrequency);
+#endif
+			reset();
+		}
+
+		~btClock()
+		{
+		}
+
+		/// Resets the initial reference time.
+		void reset()
+		{
+#ifdef USE_WINDOWS_TIMERS
+			QueryPerformanceCounter(&mStartTime);
+			mStartTick = GetTickCount();
+			mPrevElapsedTime = 0;
+#else
+#ifdef __CELLOS_LV2__
+
+	typedef uint64_t __int64;
+	typedef __int64  ClockSize;
+	ClockSize newTime;
+	__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+	mStartTime = newTime;
+#else
+			gettimeofday(&mStartTime, 0);
+#endif
+
+#endif
+		}
+
+		/// Returns the time in ms since the last call to reset or since 
+		/// the btClock was created.
+		unsigned long int getTimeMilliseconds()
+		{
+#ifdef USE_WINDOWS_TIMERS
+			LARGE_INTEGER currentTime;
+			QueryPerformanceCounter(&currentTime);
+			LONGLONG elapsedTime = currentTime.QuadPart - 
+				mStartTime.QuadPart;
+
+			// Compute the number of millisecond ticks elapsed.
+			unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / 
+				mClockFrequency.QuadPart);
+
+			// Check for unexpected leaps in the Win32 performance counter.  
+			// (This is caused by unexpected data across the PCI to ISA 
+			// bridge, aka south bridge.  See Microsoft KB274323.)
+			unsigned long elapsedTicks = GetTickCount() - mStartTick;
+			signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+			if (msecOff < -100 || msecOff > 100)
+			{
+				// Adjust the starting time forwards.
+				LONGLONG msecAdjustment = mymin(msecOff * 
+					mClockFrequency.QuadPart / 1000, elapsedTime - 
+					mPrevElapsedTime);
+				mStartTime.QuadPart += msecAdjustment;
+				elapsedTime -= msecAdjustment;
+
+				// Recompute the number of millisecond ticks elapsed.
+				msecTicks = (unsigned long)(1000 * elapsedTime / 
+					mClockFrequency.QuadPart);
+			}
+
+			// Store the current elapsed time for adjustments next time.
+			mPrevElapsedTime = elapsedTime;
+
+			return msecTicks;
+#else
+			
+#ifdef __CELLOS_LV2__
+	__int64 freq=sys_time_get_timebase_frequency();
+	 double dFreq=((double) freq) / 1000.0;
+	typedef uint64_t __int64;
+	typedef __int64  ClockSize;
+	ClockSize newTime;
+	__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+	
+	return (newTime-mStartTime) / dFreq;
+#else
+
+			struct timeval currentTime;
+			gettimeofday(&currentTime, 0);
+			return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 + 
+				(currentTime.tv_usec - mStartTime.tv_usec) / 1000;
+#endif //__CELLOS_LV2__
+#endif
+		}
+
+		/// Returns the time in us since the last call to reset or since 
+		/// the Clock was created.
+		unsigned long int getTimeMicroseconds()
+		{
+#ifdef USE_WINDOWS_TIMERS
+			LARGE_INTEGER currentTime;
+			QueryPerformanceCounter(&currentTime);
+			LONGLONG elapsedTime = currentTime.QuadPart - 
+				mStartTime.QuadPart;
+
+			// Compute the number of millisecond ticks elapsed.
+			unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / 
+				mClockFrequency.QuadPart);
+
+			// Check for unexpected leaps in the Win32 performance counter.  
+			// (This is caused by unexpected data across the PCI to ISA 
+			// bridge, aka south bridge.  See Microsoft KB274323.)
+			unsigned long elapsedTicks = GetTickCount() - mStartTick;
+			signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+			if (msecOff < -100 || msecOff > 100)
+			{
+				// Adjust the starting time forwards.
+				LONGLONG msecAdjustment = mymin(msecOff * 
+					mClockFrequency.QuadPart / 1000, elapsedTime - 
+					mPrevElapsedTime);
+				mStartTime.QuadPart += msecAdjustment;
+				elapsedTime -= msecAdjustment;
+			}
+
+			// Store the current elapsed time for adjustments next time.
+			mPrevElapsedTime = elapsedTime;
+
+			// Convert to microseconds.
+			unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / 
+				mClockFrequency.QuadPart);
+
+			return usecTicks;
+#else
+
+#ifdef __CELLOS_LV2__
+	__int64 freq=sys_time_get_timebase_frequency();
+	 double dFreq=((double) freq)/ 1000000.0;
+	typedef uint64_t __int64;
+	typedef __int64  ClockSize;
+	ClockSize newTime;
+	__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+	
+	return (newTime-mStartTime) / dFreq;
+#else
+
+			struct timeval currentTime;
+			gettimeofday(&currentTime, 0);
+			return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 + 
+				(currentTime.tv_usec - mStartTime.tv_usec);
+#endif//__CELLOS_LV2__
+#endif 
+		}
+
+	private:
+#ifdef USE_WINDOWS_TIMERS
+		LARGE_INTEGER mClockFrequency;
+		DWORD mStartTick;
+		LONGLONG mPrevElapsedTime;
+		LARGE_INTEGER mStartTime;
+#else
+#ifdef __CELLOS_LV2__
+		uint64_t	mStartTime;
+#else
+		struct timeval mStartTime;
+#endif
+#endif //__CELLOS_LV2__
+
+	};
+
+#endif //USE_BT_CLOCK
+
+
+#ifdef USE_QUICKPROF
+
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <map>
+
+
+
+
+namespace hidden
+{
+	/// A simple data structure representing a single timed block 
+	/// of code.
+	struct ProfileBlock
+	{
+		ProfileBlock()
+		{
+			currentBlockStartMicroseconds = 0;
+			currentCycleTotalMicroseconds = 0;
+			lastCycleTotalMicroseconds = 0;
+			totalMicroseconds = 0;
+		}
+
+		/// The starting time (in us) of the current block update.
+		unsigned long int currentBlockStartMicroseconds;
+
+		/// The accumulated time (in us) spent in this block during the 
+		/// current profiling cycle.
+		unsigned long int currentCycleTotalMicroseconds;
+
+		/// The accumulated time (in us) spent in this block during the 
+		/// past profiling cycle.
+		unsigned long int lastCycleTotalMicroseconds;
+
+		/// The total accumulated time (in us) spent in this block.
+		unsigned long int totalMicroseconds;
+	};
+
+};
+
+/// A static class that manages timing for a set of profiling blocks.
+class btProfiler
+{
+public:
+	/// A set of ways to retrieve block timing data.
+	enum BlockTimingMethod
+	{
+		/// The total time spent in the block (in seconds) since the 
+		/// profiler was initialized.
+		BLOCK_TOTAL_SECONDS,
+
+		/// The total time spent in the block (in ms) since the 
+		/// profiler was initialized.
+		BLOCK_TOTAL_MILLISECONDS,
+
+		/// The total time spent in the block (in us) since the 
+		/// profiler was initialized.
+		BLOCK_TOTAL_MICROSECONDS,
+
+		/// The total time spent in the block, as a % of the total 
+		/// elapsed time since the profiler was initialized.
+		BLOCK_TOTAL_PERCENT,
+
+		/// The time spent in the block (in seconds) in the most recent 
+		/// profiling cycle.
+		BLOCK_CYCLE_SECONDS,
+
+		/// The time spent in the block (in ms) in the most recent 
+		/// profiling cycle.
+		BLOCK_CYCLE_MILLISECONDS,
+
+		/// The time spent in the block (in us) in the most recent 
+		/// profiling cycle.
+		BLOCK_CYCLE_MICROSECONDS,
+
+		/// The time spent in the block (in seconds) in the most recent 
+		/// profiling cycle, as a % of the total cycle time.
+		BLOCK_CYCLE_PERCENT
+	};
+
+	/// Initializes the profiler.  This must be called first.  If this is 
+	/// never called, the profiler is effectively disabled; all other 
+	/// functions will return immediately.  The first parameter 
+	/// is the name of an output data file; if this string is not empty, 
+	/// data will be saved on every profiling cycle; if this string is 
+	/// empty, no data will be saved to a file.  The second parameter 
+	/// determines which timing method is used when printing data to the 
+	/// output file.
+	inline static void init(const std::string outputFilename="", 
+		BlockTimingMethod outputMethod=BLOCK_CYCLE_MILLISECONDS);
+
+	/// Cleans up allocated memory.
+	inline static void destroy();
+
+	/// Begins timing the named block of code.
+	inline static void beginBlock(const std::string& name);
+
+	/// Updates the accumulated time spent in the named block by adding 
+	/// the elapsed time since the last call to startBlock for this block 
+	/// name.
+	inline static void endBlock(const std::string& name);
+
+	/// Returns the time spent in the named block according to the 
+	/// given timing method.  See comments on BlockTimingMethod for details.
+	inline static double getBlockTime(const std::string& name, 
+		BlockTimingMethod method=BLOCK_CYCLE_MILLISECONDS);
+
+	/// Defines the end of a profiling cycle.  Use this regularly if you 
+	/// want to generate detailed timing information.  This must not be 
+	/// called within a timing block.
+	inline static void endProfilingCycle();
+
+	/// A helper function that creates a string of statistics for 
+	/// each timing block.  This is mainly for printing an overall 
+	/// summary to the command line.
+	inline static std::string createStatsString(
+		BlockTimingMethod method=BLOCK_TOTAL_PERCENT);
+
+//private:
+	inline btProfiler();
+
+	inline ~btProfiler();
+
+	/// Prints an error message to standard output.
+	inline static void printError(const std::string& msg)
+	{
+		//btAssert(0);
+		std::cout << "[QuickProf error] " << msg << std::endl;
+	}
+
+	/// Determines whether the profiler is enabled.
+	static bool mEnabled;
+
+	/// The clock used to time profile blocks.
+	static btClock mClock;
+
+	/// The starting time (in us) of the current profiling cycle.
+	static unsigned long int mCurrentCycleStartMicroseconds;
+
+	/// The duration (in us) of the most recent profiling cycle.
+	static unsigned long int mLastCycleDurationMicroseconds;
+
+	/// Internal map of named profile blocks.
+	static std::map<std::string, hidden::ProfileBlock*> mProfileBlocks;
+
+	/// The data file used if this feature is enabled in 'init.'
+	static std::ofstream mOutputFile;
+
+	/// Tracks whether we have begun print data to the output file.
+	static bool mFirstFileOutput;
+
+	/// The method used when printing timing data to an output file.
+	static BlockTimingMethod mFileOutputMethod;
+
+	/// The number of the current profiling cycle.
+	static unsigned long int mCycleNumber;
+};
+
+
+btProfiler::btProfiler()
+{
+	// This never gets called because a btProfiler instance is never 
+	// created.
+}
+
+btProfiler::~btProfiler()
+{
+	// This never gets called because a btProfiler instance is never 
+	// created.
+}
+
+void btProfiler::init(const std::string outputFilename, 
+	BlockTimingMethod outputMethod)
+{
+	mEnabled = true;
+	
+	if (!outputFilename.empty())
+	{
+		mOutputFile.open(outputFilename.c_str());
+	}
+
+	mFileOutputMethod = outputMethod;
+
+	mClock.reset();
+
+	// Set the start time for the first cycle.
+	mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds();
+}
+
+void btProfiler::destroy()
+{
+	if (!mEnabled)
+	{
+		return;
+	}
+
+	if (mOutputFile.is_open())
+	{
+		mOutputFile.close();
+	}
+
+	// Destroy all ProfileBlocks.
+	while (!mProfileBlocks.empty())
+	{
+		delete (*mProfileBlocks.begin()).second;
+		mProfileBlocks.erase(mProfileBlocks.begin());
+	}
+}
+
+void btProfiler::beginBlock(const std::string& name)
+{
+	if (!mEnabled)
+	{
+		return;
+	}
+
+	if (name.empty())
+	{
+		printError("Cannot allow unnamed profile blocks.");
+		return;
+	}
+
+	hidden::ProfileBlock* block = mProfileBlocks[name];
+
+	if (!block)
+	{
+		// Create a new ProfileBlock.
+		mProfileBlocks[name] = new hidden::ProfileBlock();
+		block = mProfileBlocks[name];
+	}
+
+	// We do this at the end to get more accurate results.
+	block->currentBlockStartMicroseconds = mClock.getTimeMicroseconds();
+}
+
+void btProfiler::endBlock(const std::string& name)
+{
+	if (!mEnabled)
+	{
+		return;
+	}
+
+	// We do this at the beginning to get more accurate results.
+	unsigned long int endTick = mClock.getTimeMicroseconds();
+
+	hidden::ProfileBlock* block = mProfileBlocks[name];
+
+	if (!block)
+	{
+		// The named block does not exist.  Print an error.
+		printError("The profile block named '" + name + 
+			"' does not exist.");
+		return;
+	}
+
+	unsigned long int blockDuration = endTick - 
+		block->currentBlockStartMicroseconds;
+	block->currentCycleTotalMicroseconds += blockDuration;
+	block->totalMicroseconds += blockDuration;
+}
+
+double btProfiler::getBlockTime(const std::string& name, 
+	BlockTimingMethod method)
+{
+	if (!mEnabled)
+	{
+		return 0;
+	}
+
+	hidden::ProfileBlock* block = mProfileBlocks[name];
+
+	if (!block)
+	{
+		// The named block does not exist.  Print an error.
+		printError("The profile block named '" + name + 
+			"' does not exist.");
+		return 0;
+	}
+
+	double result = 0;
+
+	switch(method)
+	{
+		case BLOCK_TOTAL_SECONDS:
+			result = (double)block->totalMicroseconds * (double)0.000001;
+			break;
+		case BLOCK_TOTAL_MILLISECONDS:
+			result = (double)block->totalMicroseconds * (double)0.001;
+			break;
+		case BLOCK_TOTAL_MICROSECONDS:
+			result = (double)block->totalMicroseconds;
+			break;
+		case BLOCK_TOTAL_PERCENT:
+		{
+			double timeSinceInit = (double)mClock.getTimeMicroseconds();
+			if (timeSinceInit <= 0)
+			{
+				result = 0;
+			}
+			else
+			{
+				result = 100.0 * (double)block->totalMicroseconds / 
+					timeSinceInit;
+			}
+			break;
+		}
+		case BLOCK_CYCLE_SECONDS:
+			result = (double)block->lastCycleTotalMicroseconds * 
+				(double)0.000001;
+			break;
+		case BLOCK_CYCLE_MILLISECONDS:
+			result = (double)block->lastCycleTotalMicroseconds * 
+				(double)0.001;
+			break;
+		case BLOCK_CYCLE_MICROSECONDS:
+			result = (double)block->lastCycleTotalMicroseconds;
+			break;
+		case BLOCK_CYCLE_PERCENT:
+		{
+			if (0 == mLastCycleDurationMicroseconds)
+			{
+				// We have not yet finished a cycle, so just return zero 
+				// percent to avoid a divide by zero error.
+				result = 0;
+			}
+			else
+			{
+				result = 100.0 * (double)block->lastCycleTotalMicroseconds / 
+					mLastCycleDurationMicroseconds;
+			}
+			break;
+		}
+		default:
+			break;
+	}
+
+	return result;
+}
+
+void btProfiler::endProfilingCycle()
+{
+	if (!mEnabled)
+	{
+		return;
+	}
+
+	// Store the duration of the cycle that just finished.
+	mLastCycleDurationMicroseconds = mClock.getTimeMicroseconds() - 
+		mCurrentCycleStartMicroseconds;
+
+	// For each block, update data for the cycle that just finished.
+	std::map<std::string, hidden::ProfileBlock*>::iterator iter;
+	for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); ++iter)
+	{
+		hidden::ProfileBlock* block = (*iter).second;
+		block->lastCycleTotalMicroseconds = 
+			block->currentCycleTotalMicroseconds;
+		block->currentCycleTotalMicroseconds = 0;
+	}
+
+	if (mOutputFile.is_open())
+	{
+		// Print data to the output file.
+		if (mFirstFileOutput)
+		{
+			// On the first iteration, print a header line that shows the 
+			// names of each profiling block.
+			mOutputFile << "#cycle, ";
+
+			for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); 
+				++iter)
+			{
+				mOutputFile << (*iter).first << ", ";
+			}
+
+			mOutputFile << std::endl;
+			mFirstFileOutput = false;
+		}
+
+		mOutputFile << mCycleNumber << ", ";
+
+		for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); 
+			++iter)
+		{
+			mOutputFile << getBlockTime((*iter).first, mFileOutputMethod) 
+				<< ", ";
+		}
+
+		mOutputFile << std::endl;
+	}
+
+	++mCycleNumber;
+	mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds();
+}
+
+std::string btProfiler::createStatsString(BlockTimingMethod method)
+{
+	if (!mEnabled)
+	{
+		return "";
+	}
+
+	std::string s;
+	std::string suffix;
+
+	switch(method)
+	{
+		case BLOCK_TOTAL_SECONDS:
+			suffix = "s";
+			break;
+		case BLOCK_TOTAL_MILLISECONDS:
+			suffix = "ms";
+			break;
+		case BLOCK_TOTAL_MICROSECONDS:
+			suffix = "us";
+			break;
+		case BLOCK_TOTAL_PERCENT:
+		{
+			suffix = "%";
+			break;
+		}
+		case BLOCK_CYCLE_SECONDS:
+			suffix = "s";
+			break;
+		case BLOCK_CYCLE_MILLISECONDS:
+			suffix = "ms";
+			break;
+		case BLOCK_CYCLE_MICROSECONDS:
+			suffix = "us";
+			break;
+		case BLOCK_CYCLE_PERCENT:
+		{
+			suffix = "%";
+			break;
+		}
+		default:
+			break;
+	}
+
+	std::map<std::string, hidden::ProfileBlock*>::iterator iter;
+	for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); ++iter)
+	{
+		if (iter != mProfileBlocks.begin())
+		{
+			s += "\n";
+		}
+
+		char blockTime[64];
+		sprintf(blockTime, "%lf", getBlockTime((*iter).first, method));
+
+		s += (*iter).first;
+		s += ": ";
+		s += blockTime;
+		s += " ";
+		s += suffix;
+	}
+
+	return s;
+}
+
+
+#define BEGIN_PROFILE(a) btProfiler::beginBlock(a)
+#define END_PROFILE(a) btProfiler::endBlock(a)
+
+#else //USE_QUICKPROF
+#define BEGIN_PROFILE(a)
+#define END_PROFILE(a)
+
+#endif //USE_QUICKPROF
+
+#endif //QUICK_PROF_H
+
+

bullet/src/LinearMath/btRandom.h

@@ -0,0 +1,42 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 GEN_RANDOM_H
+#define GEN_RANDOM_H
+
+#ifdef MT19937
+
+#include <limits.h>
+#include <mt19937.h>
+
+#define GEN_RAND_MAX UINT_MAX
+
+SIMD_FORCE_INLINE void         GEN_srand(unsigned int seed) { init_genrand(seed); }
+SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return genrand_int32(); }
+
+#else
+
+#include <stdlib.h>
+
+#define GEN_RAND_MAX RAND_MAX
+
+SIMD_FORCE_INLINE void         GEN_srand(unsigned int seed) { srand(seed); } 
+SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return rand(); }
+
+#endif
+
+#endif
+

bullet/src/LinearMath/btScalar.h

@@ -0,0 +1,192 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 SIMD___SCALAR_H
+#define SIMD___SCALAR_H
+
+#include <math.h>
+
+#include <cstdlib>
+#include <cfloat>
+#include <float.h>
+
+#ifdef WIN32
+
+		#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
+			#define SIMD_FORCE_INLINE inline
+			#define ATTRIBUTE_ALIGNED16(a) a
+		#else
+			#define BT_HAS_ALIGNED_ALOCATOR
+			#pragma warning(disable:4530)
+			#pragma warning(disable:4996)
+			#pragma warning(disable:4786)
+			#define SIMD_FORCE_INLINE __forceinline
+			#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+		#ifdef _XBOX
+			#define BT_USE_VMX128
+
+			#include <ppcintrinsics.h>
+ 			#define BT_HAVE_NATIVE_FSEL
+ 			#define btFsel(a,b,c) __fsel((a),(b),(c))
+		#else
+			#define BT_USE_SSE
+		#endif
+		#endif //__MINGW32__
+
+		#include <assert.h>
+		#define btAssert assert
+		//btFullAssert is optional, slows down a lot
+		#define btFullAssert(x)
+#else
+	
+#if defined	(__CELLOS_LV2__)
+		#define SIMD_FORCE_INLINE inline
+		#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+		#ifndef assert
+		#include <assert.h>
+		#endif
+		#define btAssert assert
+		//btFullAssert is optional, slows down a lot
+		#define btFullAssert(x)
+#else
+
+	//non-windows systems
+
+		#define SIMD_FORCE_INLINE inline
+		#define ATTRIBUTE_ALIGNED16(a) a
+		#ifndef assert
+		#include <assert.h>
+		#endif
+		#define btAssert assert
+		//btFullAssert is optional, slows down a lot
+		#define btFullAssert(x)
+#endif	//__CELLOS_LV2__
+#endif
+
+/// older compilers (gcc 3.x) and Sun needs double version of sqrt etc.
+/// exclude Apple Intel (i's assumed to be a Macbook or new Intel Dual Core Processor)
+#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__))
+//use slow double float precision operation on those platforms
+#ifndef BT_USE_DOUBLE_PRECISION
+#define BT_FORCE_DOUBLE_FUNCTIONS
+#endif
+#endif
+
+#if defined(BT_USE_DOUBLE_PRECISION)
+typedef double btScalar;
+#else
+typedef float btScalar;
+#endif
+
+
+#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
+		
+SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
+SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
+SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acos(x); }
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asin(x); }
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
+SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
+SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
+SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
+
+#else
+		
+SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrtf(x); }
+SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
+SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); }
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); }
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
+SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
+SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
+SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
+	
+#endif
+
+#define SIMD_2_PI         btScalar(6.283185307179586232)
+#define SIMD_PI           (SIMD_2_PI * btScalar(0.5))
+#define SIMD_HALF_PI      (SIMD_2_PI * btScalar(0.25))
+#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
+#define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define SIMD_EPSILON      DBL_EPSILON
+#define SIMD_INFINITY     DBL_MAX
+#else
+#define SIMD_EPSILON      FLT_EPSILON
+#define SIMD_INFINITY     FLT_MAX
+#endif
+
+SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) 
+{
+	btScalar coeff_1 = SIMD_PI / 4.0f;
+	btScalar coeff_2 = 3.0f * coeff_1;
+	btScalar abs_y = btFabs(y);
+	btScalar angle;
+	if (x >= 0.0f) {
+		btScalar r = (x - abs_y) / (x + abs_y);
+		angle = coeff_1 - coeff_1 * r;
+	} else {
+		btScalar r = (x + abs_y) / (abs_y - x);
+		angle = coeff_2 - coeff_1 * r;
+	}
+	return (y < 0.0f) ? -angle : angle;
+}
+
+SIMD_FORCE_INLINE bool      btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
+
+SIMD_FORCE_INLINE bool	btEqual(btScalar a, btScalar eps) {
+	return (((a) <= eps) && !((a) < -eps));
+}
+SIMD_FORCE_INLINE bool	btGreaterEqual (btScalar a, btScalar eps) {
+	return (!((a) <= eps));
+}
+
+/*SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); }
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); }
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
+*/
+
+SIMD_FORCE_INLINE int       btIsNegative(btScalar x) {
+    return x < btScalar(0.0) ? 1 : 0;
+}
+
+SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
+SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
+
+#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
+
+#ifndef btFsel
+SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
+{
+	return a >= 0 ? b : c;
+}
+#endif
+#define btFsels(a,b,c) (btScalar)btFsel(a,b,c)
+
+#endif //SIMD___SCALAR_H

bullet/src/LinearMath/btSimdMinMax.h

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 SIMD_MINMAX_H
+#define SIMD_MINMAX_H
+#include "btScalar.h"
+
+template <class T>
+SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) {
+  return b < a ? b : a;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) {
+  return  a < b ? b : a;
+}
+
+template <class T>
+SIMD_FORCE_INLINE void btSetMin(T& a, const T& b) {
+    if (a > b) a = b;
+}
+
+template <class T>
+SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) {
+    if (a < b) a = b;
+}
+
+// Specialize on float/double for platforms that have btFsel natively
+#ifdef BT_HAVE_NATIVE_FSEL
+SIMD_FORCE_INLINE  float btMin( float a,  float b) {
+	return (float)btFsel((a-b), b, a);
+}
+
+SIMD_FORCE_INLINE  float btMax( float a,  float b) {
+	return (float)btFsel((a-b), a, b);
+}
+
+SIMD_FORCE_INLINE void btSetMin(float& a,  float b) {
+	a = (float)btFsel((a-b), b, a);
+}
+
+SIMD_FORCE_INLINE void btSetMax(float& a,  float b) {
+	a = (float)btFsel((a-b), a, b);
+}
+
+SIMD_FORCE_INLINE  double btMin( double a,  double b) {
+	return btFsel((a-b), b, a);
+}
+
+SIMD_FORCE_INLINE  double btMax( double a,  double b) {
+	return btFsel((a-b), a, b);
+}
+
+SIMD_FORCE_INLINE void btSetMin(double& a, double b) {
+	a = btFsel((a-b), b, a);
+}
+
+SIMD_FORCE_INLINE void btSetMax(double& a,  double b) {
+	a = btFsel((a-b), a, b);
+}
+#endif
+
+#endif

bullet/src/LinearMath/btStackAlloc.h

@@ -0,0 +1,106 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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.
+*/
+
+/*
+StackAlloc extracted from GJK-EPA collision solver by Nathanael Presson
+Nov.2006
+*/
+
+#ifndef BT_STACK_ALLOC
+#define BT_STACK_ALLOC
+
+#include "btScalar.h" //for btAssert
+
+struct btBlock
+{
+	btBlock*			previous;
+	unsigned char*		address;
+};
+
+///StackAlloc provides some fast stack-based memory allocator (LIFO last-in first-out)
+class btStackAlloc
+{
+public:
+
+	btStackAlloc(unsigned int size)	{ ctor();create(size); }
+	~btStackAlloc()		{ destroy(); }
+	
+	inline void		create(unsigned int size)
+	{
+		destroy();
+		data		=	new unsigned char[size];
+		totalsize	=	size;
+	}
+	inline void		destroy()
+	{
+		btAssert(usedsize==0);
+		//Raise(L"StackAlloc is still in use");
+
+		if(usedsize==0)
+		{
+			if(!ischild)		delete[] data;
+			data				=	0;
+			usedsize			=	0;
+		}
+		
+	}
+	unsigned char*			allocate(unsigned int size)
+	{
+		const unsigned int	nus(usedsize+size);
+		if(nus<totalsize)
+		{
+			usedsize=nus;
+			return(data+(usedsize-size));
+		}
+		btAssert(0);
+		//&& (L"Not enough memory"));
+		
+		return(0);
+	}
+	inline btBlock*		beginBlock()
+	{
+		btBlock*	pb = (btBlock*)allocate(sizeof(btBlock));
+		pb->previous	=	current;
+		pb->address		=	data+usedsize;
+		current			=	pb;
+		return(pb);
+	}
+	inline void		endBlock(btBlock* block)
+	{
+		btAssert(block==current);
+		//Raise(L"Unmatched blocks");
+		if(block==current)
+		{
+			current		=	block->previous;
+			usedsize	=	(unsigned int)((block->address-data)-sizeof(btBlock));
+		}
+	}
+
+private:
+	void		ctor()
+	{
+		data		=	0;
+		totalsize	=	0;
+		usedsize	=	0;
+		current		=	0;
+		ischild		=	false;
+	}
+	unsigned char*		data;
+	unsigned int		totalsize;
+	unsigned int		usedsize;
+	btBlock*	current;
+	bool		ischild;
+};
+
+#endif //BT_STACK_ALLOC

bullet/src/LinearMath/btTransform.h

@@ -0,0 +1,206 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 btTransform_H
+#define btTransform_H
+
+#include "btVector3.h"
+#include "btMatrix3x3.h"
+
+
+///btTransform supports rigid transforms (only translation and rotation, no scaling/shear)
+class btTransform {
+	
+
+public:
+	
+	
+	btTransform() {}
+
+	explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q, 
+		const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) 
+		: m_basis(q),
+		m_origin(c)
+	{}
+
+	explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b, 
+		const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
+		: m_basis(b),
+		m_origin(c)
+	{}
+
+	SIMD_FORCE_INLINE btTransform (const btTransform& other)
+		: m_basis(other.m_basis),
+		m_origin(other.m_origin)
+	{
+	}
+
+	SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other)
+	{
+		m_basis = other.m_basis;
+		m_origin = other.m_origin;
+		return *this;
+	}
+
+
+		SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) {
+			m_basis = t1.m_basis * t2.m_basis;
+			m_origin = t1(t2.m_origin);
+		}
+
+/*		void multInverseLeft(const btTransform& t1, const btTransform& t2) {
+			btVector3 v = t2.m_origin - t1.m_origin;
+			m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
+			m_origin = v * t1.m_basis;
+		}
+		*/
+
+
+	SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
+	{
+		return btVector3(m_basis[0].dot(x) + m_origin.x(), 
+			m_basis[1].dot(x) + m_origin.y(), 
+			m_basis[2].dot(x) + m_origin.z());
+	}
+
+	SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const
+	{
+		return (*this)(x);
+	}
+
+	SIMD_FORCE_INLINE btMatrix3x3&       getBasis()          { return m_basis; }
+	SIMD_FORCE_INLINE const btMatrix3x3& getBasis()    const { return m_basis; }
+
+	SIMD_FORCE_INLINE btVector3&         getOrigin()         { return m_origin; }
+	SIMD_FORCE_INLINE const btVector3&   getOrigin()   const { return m_origin; }
+
+	btQuaternion getRotation() const { 
+		btQuaternion q;
+		m_basis.getRotation(q);
+		return q;
+	}
+	template <typename Scalar2>
+		void setValue(const Scalar2 *m) 
+	{
+		m_basis.setValue(m);
+		m_origin.setValue(&m[12]);
+	}
+
+	
+	void setFromOpenGLMatrix(const btScalar *m)
+	{
+		m_basis.setFromOpenGLSubMatrix(m);
+		m_origin.setValue(m[12],m[13],m[14]);
+	}
+
+	void getOpenGLMatrix(btScalar *m) const 
+	{
+		m_basis.getOpenGLSubMatrix(m);
+		m[12] = m_origin.x();
+		m[13] = m_origin.y();
+		m[14] = m_origin.z();
+		m[15] = btScalar(1.0);
+	}
+
+	SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) 
+	{ 
+		m_origin = origin;
+	}
+
+	SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const;
+
+
+
+	SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis)
+	{ 
+		m_basis = basis;
+	}
+
+	SIMD_FORCE_INLINE void setRotation(const btQuaternion& q)
+	{
+		m_basis.setRotation(q);
+	}
+
+
+
+	void setIdentity()
+	{
+		m_basis.setIdentity();
+		m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	}
+
+	
+	btTransform& operator*=(const btTransform& t) 
+	{
+		m_origin += m_basis * t.m_origin;
+		m_basis *= t.m_basis;
+		return *this;
+	}
+
+	btTransform inverse() const
+	{ 
+		btMatrix3x3 inv = m_basis.transpose();
+		return btTransform(inv, inv * -m_origin);
+	}
+
+	btTransform inverseTimes(const btTransform& t) const;  
+
+	btTransform operator*(const btTransform& t) const;
+
+	static btTransform	getIdentity()
+	{
+		btTransform tr;
+		tr.setIdentity();
+		return tr;
+	}
+	
+private:
+
+	btMatrix3x3 m_basis;
+	btVector3   m_origin;
+};
+
+
+SIMD_FORCE_INLINE btVector3
+btTransform::invXform(const btVector3& inVec) const
+{
+	btVector3 v = inVec - m_origin;
+	return (m_basis.transpose() * v);
+}
+
+SIMD_FORCE_INLINE btTransform 
+btTransform::inverseTimes(const btTransform& t) const  
+{
+	btVector3 v = t.getOrigin() - m_origin;
+		return btTransform(m_basis.transposeTimes(t.m_basis),
+			v * m_basis);
+}
+
+SIMD_FORCE_INLINE btTransform 
+btTransform::operator*(const btTransform& t) const
+{
+	return btTransform(m_basis * t.m_basis, 
+		(*this)(t.m_origin));
+}	
+
+
+
+#endif
+
+
+
+
+

bullet/src/LinearMath/btTransformUtil.h

@@ -0,0 +1,138 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 SIMD_TRANSFORM_UTIL_H
+#define SIMD_TRANSFORM_UTIL_H
+
+#include "btTransform.h"
+#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI
+
+
+
+#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
+
+#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))		/* reciprocal square root */
+
+inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
+{
+	return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
+      supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
+      supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); 
+}
+
+
+inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
+{
+  if (btFabs(n.z()) > SIMDSQRT12) {
+    // choose p in y-z plane
+    btScalar a = n[1]*n[1] + n[2]*n[2];
+    btScalar k = btRecipSqrt (a);
+    p.setValue(0,-n[2]*k,n[1]*k);
+    // set q = n x p
+    q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
+  }
+  else {
+    // choose p in x-y plane
+    btScalar a = n.x()*n.x() + n.y()*n.y();
+    btScalar k = btRecipSqrt (a);
+    p.setValue(-n.y()*k,n.x()*k,0);
+    // set q = n x p
+    q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
+  }
+}
+
+
+
+/// Utils related to temporal transforms
+class btTransformUtil
+{
+
+public:
+
+	static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
+	{
+		predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
+//	#define QUATERNION_DERIVATIVE
+	#ifdef QUATERNION_DERIVATIVE
+		btQuaternion predictedOrn = curTrans.getRotation();
+		predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
+		predictedOrn.normalize();
+	#else
+		//exponential map
+		btVector3 axis;
+		btScalar	fAngle = angvel.length(); 
+		//limit the angular motion
+		if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
+		{
+			fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
+		}
+
+		if ( fAngle < btScalar(0.001) )
+		{
+			// use Taylor's expansions of sync function
+			axis   = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
+		}
+		else
+		{
+			// sync(fAngle) = sin(c*fAngle)/t
+			axis   = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
+		}
+		btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
+		btQuaternion orn0 = curTrans.getRotation();
+
+		btQuaternion predictedOrn = dorn * orn0;
+		predictedOrn.normalize();
+	#endif
+		predictedTransform.setRotation(predictedOrn);
+	}
+
+	static void	calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
+	{
+		linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
+		btVector3 axis;
+		btScalar  angle;
+		calculateDiffAxisAngle(transform0,transform1,axis,angle);
+		angVel = axis * angle / timeStep;
+	}
+
+	static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
+	{
+	
+	#ifdef USE_QUATERNION_DIFF
+		btQuaternion orn0 = transform0.getRotation();
+		btQuaternion orn1a = transform1.getRotation();
+		btQuaternion orn1 = orn0.farthest(orn1a);
+		btQuaternion dorn = orn1 * orn0.inverse();
+#else
+		btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
+		btQuaternion dorn;
+		dmat.getRotation(dorn);
+#endif//USE_QUATERNION_DIFF
+	
+		angle = dorn.getAngle();
+		axis = btVector3(dorn.x(),dorn.y(),dorn.z());
+		axis[3] = btScalar(0.);
+		//check for axis length
+		btScalar len = axis.length2();
+		if (len < SIMD_EPSILON*SIMD_EPSILON)
+			axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
+		else
+			axis /= btSqrt(len);
+	}
+
+};
+
+#endif //SIMD_TRANSFORM_UTIL_H
+

bullet/src/LinearMath/btVector3.h

@@ -0,0 +1,406 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 SIMD__VECTOR3_H
+#define SIMD__VECTOR3_H
+
+#include "btQuadWord.h"
+
+///btVector3 can be used to represent 3D points and vectors.
+///It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
+///Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
+class	btVector3 : public btQuadWord {
+
+public:
+	SIMD_FORCE_INLINE btVector3() {}
+
+	SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) 
+		: btQuadWord(q)
+	{
+	}
+	
+
+	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) 
+		:btQuadWord(x,y,z,btScalar(0.))
+	{
+	}
+
+//	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
+//		: btQuadWord(x,y,z,w)
+//	{
+//	}
+
+	
+
+	SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
+	{
+		m_x += v.x(); m_y += v.y(); m_z += v.z();
+		return *this;
+	}
+
+
+
+	SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
+	{
+		m_x -= v.x(); m_y -= v.y(); m_z -= v.z();
+		return *this;
+	}
+
+	SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
+	{
+		m_x *= s; m_y *= s; m_z *= s;
+		return *this;
+	}
+
+	SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) 
+	{
+		btFullAssert(s != btScalar(0.0));
+		return *this *= btScalar(1.0) / s;
+	}
+
+	SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
+	{
+		return m_x * v.x() + m_y * v.y() + m_z * v.z();
+	}
+
+	SIMD_FORCE_INLINE btScalar length2() const
+	{
+		return dot(*this);
+	}
+
+	SIMD_FORCE_INLINE btScalar length() const
+	{
+		return btSqrt(length2());
+	}
+
+	SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
+
+	SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
+
+	SIMD_FORCE_INLINE btVector3& normalize() 
+	{
+		return *this /= length();
+	}
+
+	SIMD_FORCE_INLINE btVector3 normalized() const;
+
+	SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
+
+	SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const 
+	{
+		btScalar s = btSqrt(length2() * v.length2());
+		btFullAssert(s != btScalar(0.0));
+		return btAcos(dot(v) / s);
+	}
+
+	SIMD_FORCE_INLINE btVector3 absolute() const 
+	{
+		return btVector3(
+			btFabs(m_x), 
+			btFabs(m_y), 
+			btFabs(m_z));
+	}
+
+	SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
+	{
+		return btVector3(
+			m_y * v.z() - m_z * v.y(),
+			m_z * v.x() - m_x * v.z(),
+			m_x * v.y() - m_y * v.x());
+	}
+
+	SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
+	{
+		return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) + 
+			m_y * (v1.z() * v2.x() - v1.x() * v2.z()) + 
+			m_z * (v1.x() * v2.y() - v1.y() * v2.x());
+	}
+
+	SIMD_FORCE_INLINE int minAxis() const
+	{
+		return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2);
+	}
+
+	SIMD_FORCE_INLINE int maxAxis() const 
+	{
+		return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0);
+	}
+
+	SIMD_FORCE_INLINE int furthestAxis() const
+	{
+		return absolute().minAxis();
+	}
+
+	SIMD_FORCE_INLINE int closestAxis() const 
+	{
+		return absolute().maxAxis();
+	}
+
+	SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
+	{
+		btScalar s = btScalar(1.0) - rt;
+		m_x = s * v0.x() + rt * v1.x();
+		m_y = s * v0.y() + rt * v1.y();
+		m_z = s * v0.z() + rt * v1.z();
+		//don't do the unused w component
+		//		m_co[3] = s * v0[3] + rt * v1[3];
+	}
+
+	SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
+	{
+		return btVector3(m_x + (v.x() - m_x) * t,
+			m_y + (v.y() - m_y) * t,
+			m_z + (v.z() - m_z) * t);
+	}
+
+
+	SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
+	{
+		m_x *= v.x(); m_y *= v.y(); m_z *= v.z();
+		return *this;
+	}
+
+	
+
+};
+
+SIMD_FORCE_INLINE btVector3 
+operator+(const btVector3& v1, const btVector3& v2) 
+{
+	return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
+}
+
+SIMD_FORCE_INLINE btVector3 
+operator*(const btVector3& v1, const btVector3& v2) 
+{
+	return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
+}
+
+SIMD_FORCE_INLINE btVector3 
+operator-(const btVector3& v1, const btVector3& v2)
+{
+	return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
+}
+
+SIMD_FORCE_INLINE btVector3 
+operator-(const btVector3& v)
+{
+	return btVector3(-v.x(), -v.y(), -v.z());
+}
+
+SIMD_FORCE_INLINE btVector3 
+operator*(const btVector3& v, const btScalar& s)
+{
+	return btVector3(v.x() * s, v.y() * s, v.z() * s);
+}
+
+SIMD_FORCE_INLINE btVector3 
+operator*(const btScalar& s, const btVector3& v)
+{ 
+	return v * s; 
+}
+
+SIMD_FORCE_INLINE btVector3
+operator/(const btVector3& v, const btScalar& s)
+{
+	btFullAssert(s != btScalar(0.0));
+	return v * (btScalar(1.0) / s);
+}
+
+SIMD_FORCE_INLINE btVector3
+operator/(const btVector3& v1, const btVector3& v2)
+{
+	return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
+}
+
+SIMD_FORCE_INLINE btScalar 
+dot(const btVector3& v1, const btVector3& v2) 
+{ 
+	return v1.dot(v2); 
+}
+
+
+
+SIMD_FORCE_INLINE btScalar
+distance2(const btVector3& v1, const btVector3& v2) 
+{ 
+	return v1.distance2(v2); 
+}
+
+
+SIMD_FORCE_INLINE btScalar
+distance(const btVector3& v1, const btVector3& v2) 
+{ 
+	return v1.distance(v2); 
+}
+
+SIMD_FORCE_INLINE btScalar
+angle(const btVector3& v1, const btVector3& v2) 
+{ 
+	return v1.angle(v2); 
+}
+
+SIMD_FORCE_INLINE btVector3 
+cross(const btVector3& v1, const btVector3& v2) 
+{ 
+	return v1.cross(v2); 
+}
+
+SIMD_FORCE_INLINE btScalar
+triple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
+{
+	return v1.triple(v2, v3);
+}
+
+SIMD_FORCE_INLINE btVector3 
+lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
+{
+	return v1.lerp(v2, t);
+}
+
+
+SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) 
+{
+	return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
+}
+
+SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
+{
+	return (v - *this).length2();
+}
+
+SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
+{
+	return (v - *this).length();
+}
+
+SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
+{
+	return *this / length();
+} 
+
+SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle )
+{
+	// wAxis must be a unit lenght vector
+
+	btVector3 o = wAxis * wAxis.dot( *this );
+	btVector3 x = *this - o;
+	btVector3 y;
+
+	y = wAxis.cross( *this );
+
+	return ( o + x * btCos( angle ) + y * btSin( angle ) );
+}
+
+class btVector4 : public btVector3
+{
+public:
+
+	SIMD_FORCE_INLINE btVector4() {}
+
+
+	SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
+		: btVector3(x,y,z)
+	{
+		m_unusedW = w;
+	}
+
+
+	SIMD_FORCE_INLINE btVector4 absolute4() const 
+	{
+		return btVector4(
+			btFabs(m_x), 
+			btFabs(m_y), 
+			btFabs(m_z),
+			btFabs(m_unusedW));
+	}
+
+
+
+	btScalar	getW() const { return m_unusedW;}
+
+
+		SIMD_FORCE_INLINE int maxAxis4() const
+	{
+		int maxIndex = -1;
+		btScalar maxVal = btScalar(-1e30);
+		if (m_x > maxVal)
+		{
+			maxIndex = 0;
+			maxVal = m_x;
+		}
+		if (m_y > maxVal)
+		{
+			maxIndex = 1;
+			maxVal = m_y;
+		}
+		if (m_z > maxVal)
+		{
+			maxIndex = 2;
+			maxVal = m_z;
+		}
+		if (m_unusedW > maxVal)
+		{
+			maxIndex = 3;
+			maxVal = m_unusedW;
+		}
+		
+		
+		
+
+		return maxIndex;
+
+	}
+
+
+	SIMD_FORCE_INLINE int minAxis4() const
+	{
+		int minIndex = -1;
+		btScalar minVal = btScalar(1e30);
+		if (m_x < minVal)
+		{
+			minIndex = 0;
+			minVal = m_x;
+		}
+		if (m_y < minVal)
+		{
+			minIndex = 1;
+			minVal = m_y;
+		}
+		if (m_z < minVal)
+		{
+			minIndex = 2;
+			minVal = m_z;
+		}
+		if (m_unusedW < minVal)
+		{
+			minIndex = 3;
+			minVal = m_unusedW;
+		}
+		
+		return minIndex;
+
+	}
+
+
+	SIMD_FORCE_INLINE int closestAxis4() const 
+	{
+		return absolute4().maxAxis4();
+	}
+
+};
+
+#endif //SIMD__VECTOR3_H