moved Extras/Gimpact to src/BulletCollision/Gimpact

moved Extras/BulletMultiThreaded to src/BulletMultiThreaded
(build systems will be updated soon)

src/BulletCollision/Gimpact/btBoxCollision.h

@@ -0,0 +1,651 @@
+#ifndef BT_BOX_COLLISION_H_INCLUDED
+#define BT_BOX_COLLISION_H_INCLUDED
+
+/*! \file gim_box_collision.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+
+/*! \defgroup BOUND_AABB_OPERATIONS
+*/
+//! @{
+
+///Swap numbers
+#define BT_SWAP_NUMBERS(a,b){ \
+    a = a+b; \
+    b = a-b; \
+    a = a-b; \
+}\
+
+
+#define BT_MAX(a,b) (a<b?b:a)
+#define BT_MIN(a,b) (a>b?b:a)
+
+#define BT_GREATER(x, y)	fabsf(x) > (y)
+
+#define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c))
+#define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c))
+
+
+
+
+
+
+enum eBT_PLANE_INTERSECTION_TYPE
+{
+	BT_CONST_BACK_PLANE = 0,
+	BT_CONST_COLLIDE_PLANE,
+	BT_CONST_FRONT_PLANE
+};
+
+//SIMD_FORCE_INLINE bool test_cross_edge_box(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, const btVector3 & extend,
+//	int dir_index0,
+//	int dir_index1
+//	int component_index0,
+//	int component_index1)
+//{
+//	// dir coords are -z and y
+//
+//	const btScalar dir0 = -edge[dir_index0];
+//	const btScalar dir1 = edge[dir_index1];
+//	btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
+//	btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
+//	//find minmax
+//	if(pmin>pmax)
+//	{
+//		BT_SWAP_NUMBERS(pmin,pmax);
+//	}
+//	//find extends
+//	const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
+//					extend[component_index1] * absolute_edge[dir_index1];
+//
+//	if(pmin>rad || -rad>pmax) return false;
+//	return true;
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
+//}
+//
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
+//}
+
+
+#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
+{\
+	const btScalar dir0 = -edge[i_dir_0];\
+	const btScalar dir1 = edge[i_dir_1];\
+	btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
+	btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
+	if(pmin>pmax)\
+	{\
+		BT_SWAP_NUMBERS(pmin,pmax); \
+	}\
+	const btScalar abs_dir0 = absolute_edge[i_dir_0];\
+	const btScalar abs_dir1 = absolute_edge[i_dir_1];\
+	const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
+	if(pmin>rad || -rad>pmax) return false;\
+}\
+
+
+#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
+}\
+
+
+//! Returns the dot product between a vec3f and the col of a matrix
+SIMD_FORCE_INLINE btScalar bt_mat3_dot_col(
+const btMatrix3x3 & mat, const btVector3 & vec3, int colindex)
+{
+	return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex];
+}
+
+
+//!  Class for transforming a model1 to the space of model0
+ATTRIBUTE_ALIGNED16	(class) BT_BOX_BOX_TRANSFORM_CACHE
+{
+public:
+    btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
+	btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
+	btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
+
+	SIMD_FORCE_INLINE void calc_absolute_matrix()
+	{
+//		static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
+//		m_AR[0] = vepsi + m_R1to0[0].absolute();
+//		m_AR[1] = vepsi + m_R1to0[1].absolute();
+//		m_AR[2] = vepsi + m_R1to0[2].absolute();
+
+		int i,j;
+
+        for(i=0;i<3;i++)
+        {
+            for(j=0;j<3;j++ )
+            {
+            	m_AR[i][j] = 1e-6f + fabsf(m_R1to0[i][j]);
+            }
+        }
+
+	}
+
+	BT_BOX_BOX_TRANSFORM_CACHE()
+	{
+	}
+
+
+
+	//! Calc the transformation relative  1 to 0. Inverts matrics by transposing
+	SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
+	{
+
+		btTransform temp_trans = trans0.inverse();
+		temp_trans = temp_trans * trans1;
+
+		m_T1to0 = temp_trans.getOrigin();
+		m_R1to0 = temp_trans.getBasis();
+
+
+		calc_absolute_matrix();
+	}
+
+	//! Calcs the full invertion of the matrices. Useful for scaling matrices
+	SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
+	{
+		m_R1to0 = trans0.getBasis().inverse();
+		m_T1to0 = m_R1to0 * (-trans0.getOrigin());
+
+		m_T1to0 += m_R1to0*trans1.getOrigin();
+		m_R1to0 *= trans1.getBasis();
+
+		calc_absolute_matrix();
+	}
+
+	SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const
+	{
+		return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(),
+			m_R1to0[1].dot(point) + m_T1to0.y(),
+			m_R1to0[2].dot(point) + m_T1to0.z());
+	}
+};
+
+
+#define BOX_PLANE_EPSILON 0.000001f
+
+//! Axis aligned box
+ATTRIBUTE_ALIGNED16	(class) btAABB
+{
+public:
+	btVector3 m_min;
+	btVector3 m_max;
+
+	btAABB()
+	{}
+
+
+	btAABB(const btVector3 & V1,
+			 const btVector3 & V2,
+			 const btVector3 & V3)
+	{
+		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+	}
+
+	btAABB(const btVector3 & V1,
+			 const btVector3 & V2,
+			 const btVector3 & V3,
+			 btScalar margin)
+	{
+		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	btAABB(const btAABB &other):
+		m_min(other.m_min),m_max(other.m_max)
+	{
+	}
+
+	btAABB(const btAABB &other,btScalar margin ):
+		m_min(other.m_min),m_max(other.m_max)
+	{
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	SIMD_FORCE_INLINE void invalidate()
+	{
+		m_min[0] = SIMD_INFINITY;
+		m_min[1] = SIMD_INFINITY;
+		m_min[2] = SIMD_INFINITY;
+		m_max[0] = -SIMD_INFINITY;
+		m_max[1] = -SIMD_INFINITY;
+		m_max[2] = -SIMD_INFINITY;
+	}
+
+	SIMD_FORCE_INLINE void increment_margin(btScalar margin)
+	{
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	SIMD_FORCE_INLINE void copy_with_margin(const btAABB &other, btScalar margin)
+	{
+		m_min[0] = other.m_min[0] - margin;
+		m_min[1] = other.m_min[1] - margin;
+		m_min[2] = other.m_min[2] - margin;
+
+		m_max[0] = other.m_max[0] + margin;
+		m_max[1] = other.m_max[1] + margin;
+		m_max[2] = other.m_max[2] + margin;
+	}
+
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void calc_from_triangle(
+							const CLASS_POINT & V1,
+							const CLASS_POINT & V2,
+							const CLASS_POINT & V3)
+	{
+		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+	}
+
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void calc_from_triangle_margin(
+							const CLASS_POINT & V1,
+							const CLASS_POINT & V2,
+							const CLASS_POINT & V3, btScalar margin)
+	{
+		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	//! Apply a transform to an AABB
+	SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
+	{
+		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 extends = m_max - center;
+		// Compute new center
+		center = trans(center);
+
+		btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()),
+ 				 extends.dot(trans.getBasis().getRow(1).absolute()),
+				 extends.dot(trans.getBasis().getRow(2).absolute()));
+
+		m_min = center - textends;
+		m_max = center + textends;
+	}
+
+
+	//! Apply a transform to an AABB
+	SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans)
+	{
+		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 extends = m_max - center;
+		// Compute new center
+		center = trans.transform(center);
+
+		btVector3 textends(extends.dot(trans.m_R1to0.getRow(0).absolute()),
+ 				 extends.dot(trans.m_R1to0.getRow(1).absolute()),
+				 extends.dot(trans.m_R1to0.getRow(2).absolute()));
+
+		m_min = center - textends;
+		m_max = center + textends;
+	}
+
+	//! Merges a Box
+	SIMD_FORCE_INLINE void merge(const btAABB & box)
+	{
+		m_min[0] = BT_MIN(m_min[0],box.m_min[0]);
+		m_min[1] = BT_MIN(m_min[1],box.m_min[1]);
+		m_min[2] = BT_MIN(m_min[2],box.m_min[2]);
+
+		m_max[0] = BT_MAX(m_max[0],box.m_max[0]);
+		m_max[1] = BT_MAX(m_max[1],box.m_max[1]);
+		m_max[2] = BT_MAX(m_max[2],box.m_max[2]);
+	}
+
+	//! Merges a point
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
+	{
+		m_min[0] = BT_MIN(m_min[0],point[0]);
+		m_min[1] = BT_MIN(m_min[1],point[1]);
+		m_min[2] = BT_MIN(m_min[2],point[2]);
+
+		m_max[0] = BT_MAX(m_max[0],point[0]);
+		m_max[1] = BT_MAX(m_max[1],point[1]);
+		m_max[2] = BT_MAX(m_max[2],point[2]);
+	}
+
+	//! Gets the extend and center
+	SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
+	{
+		center = (m_max+m_min)*0.5f;
+		extend = m_max - center;
+	}
+
+	//! Finds the intersecting box between this box and the other.
+	SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection)  const
+	{
+		intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]);
+		intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]);
+		intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]);
+
+		intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]);
+		intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]);
+		intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]);
+	}
+
+
+	SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const
+	{
+		if(m_min[0] > other.m_max[0] ||
+		   m_max[0] < other.m_min[0] ||
+		   m_min[1] > other.m_max[1] ||
+		   m_max[1] < other.m_min[1] ||
+		   m_min[2] > other.m_max[2] ||
+		   m_max[2] < other.m_min[2])
+		{
+			return false;
+		}
+		return true;
+	}
+
+	/*! \brief Finds the Ray intersection parameter.
+	\param aabb Aligned box
+	\param vorigin A vec3f with the origin of the ray
+	\param vdir A vec3f with the direction of the ray
+	*/
+	SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)  const
+	{
+		btVector3 extents,center;
+		this->get_center_extend(center,extents);;
+
+		btScalar Dx = vorigin[0] - center[0];
+		if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)	return false;
+		btScalar Dy = vorigin[1] - center[1];
+		if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)	return false;
+		btScalar Dz = vorigin[2] - center[2];
+		if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)	return false;
+
+
+		btScalar f = vdir[1] * Dz - vdir[2] * Dy;
+		if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
+		f = vdir[2] * Dx - vdir[0] * Dz;
+		if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
+		f = vdir[0] * Dy - vdir[1] * Dx;
+		if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
+		return true;
+	}
+
+
+	SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
+	{
+		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 extend = m_max-center;
+
+		btScalar _fOrigin =  direction.dot(center);
+		btScalar _fMaximumExtent = extend.dot(direction.absolute());
+		vmin = _fOrigin - _fMaximumExtent;
+		vmax = _fOrigin + _fMaximumExtent;
+	}
+
+	SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
+	{
+		btScalar _fmin,_fmax;
+		this->projection_interval(plane,_fmin,_fmax);
+
+		if(plane[3] > _fmax + BOX_PLANE_EPSILON)
+		{
+			return BT_CONST_BACK_PLANE; // 0
+		}
+
+		if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
+		{
+			return BT_CONST_COLLIDE_PLANE; //1
+		}
+		return BT_CONST_FRONT_PLANE;//2
+	}
+
+	SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const
+	{
+		btAABB tbox = box;
+		tbox.appy_transform(trans1_to_0);
+		return has_collision(tbox);
+	}
+
+	SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box,
+		const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const
+	{
+		btAABB tbox = box;
+		tbox.appy_transform_trans_cache(trans1_to_0);
+		return has_collision(tbox);
+	}
+
+	//! transcache is the transformation cache from box to this AABB
+	SIMD_FORCE_INLINE bool overlapping_trans_cache(
+		const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const
+	{
+
+		//Taken from OPCODE
+		btVector3 ea,eb;//extends
+		btVector3 ca,cb;//extends
+		get_center_extend(ca,ea);
+		box.get_center_extend(cb,eb);
+
+
+		btVector3 T;
+		btScalar t,t2;
+		int i;
+
+		// Class I : A's basis vectors
+		for(i=0;i<3;i++)
+		{
+			T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
+			t = transcache.m_AR[i].dot(eb) + ea[i];
+			if(BT_GREATER(T[i], t))	return false;
+		}
+		// Class II : B's basis vectors
+		for(i=0;i<3;i++)
+		{
+			t = bt_mat3_dot_col(transcache.m_R1to0,T,i);
+			t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i];
+			if(BT_GREATER(t,t2))	return false;
+		}
+		// Class III : 9 cross products
+		if(fulltest)
+		{
+			int j,m,n,o,p,q,r;
+			for(i=0;i<3;i++)
+			{
+				m = (i+1)%3;
+				n = (i+2)%3;
+				o = i==0?1:0;
+				p = i==2?1:2;
+				for(j=0;j<3;j++)
+				{
+					q = j==2?1:2;
+					r = j==0?1:0;
+					t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
+					t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
+						eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
+					if(BT_GREATER(t,t2))	return false;
+				}
+			}
+		}
+		return true;
+	}
+
+	//! Simple test for planes.
+	SIMD_FORCE_INLINE bool collide_plane(
+		const btVector4 & plane) const
+	{
+		eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane);
+		return (classify == BT_CONST_COLLIDE_PLANE);
+	}
+
+	//! test for a triangle, with edges
+	SIMD_FORCE_INLINE bool collide_triangle_exact(
+		const btVector3 & p1,
+		const btVector3 & p2,
+		const btVector3 & p3,
+		const btVector4 & triangle_plane) const
+	{
+		if(!collide_plane(triangle_plane)) return false;
+
+		btVector3 center,extends;
+		this->get_center_extend(center,extends);
+
+		const btVector3 v1(p1 - center);
+		const btVector3 v2(p2 - center);
+		const btVector3 v3(p3 - center);
+
+		//First axis
+		btVector3 diff(v2 - v1);
+		btVector3 abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+
+
+		diff = v3 - v2;
+		abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+
+		diff = v1 - v3;
+		abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+
+		return true;
+	}
+};
+
+
+//! Compairison of transformation objects
+SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
+{
+	if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
+
+	if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
+	if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
+	if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
+	return true;
+}
+
+
+//! @}
+
+#endif // GIM_BOX_COLLISION_H_INCLUDED

src/BulletCollision/Gimpact/btClipPolygon.h

@@ -0,0 +1,186 @@
+#ifndef BT_CLIP_POLYGON_H_INCLUDED
+#define BT_CLIP_POLYGON_H_INCLUDED
+
+/*! \file btClipPolygon.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btGeometryUtil.h"
+
+/*! \addtogroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point)
+{
+	return point.dot(plane) - plane[3];
+}
+
+/*! Vector blending
+Takes two vectors a, b, blends them together*/
+SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va,const btVector3 &vb, btScalar blend_factor)
+{
+	vr = (1-blend_factor)*va + blend_factor*vb;
+}
+
+//! This function calcs the distance from a 3D plane
+SIMD_FORCE_INLINE void bt_plane_clip_polygon_collect(
+						const btVector3 & point0,
+						const btVector3 & point1,
+						btScalar dist0,
+						btScalar dist1,
+						btVector3 * clipped,
+						int & clipped_count)
+{
+	bool _prevclassif = (dist0>SIMD_EPSILON);
+	bool _classif = (dist1>SIMD_EPSILON);
+	if(_classif!=_prevclassif)
+	{
+		btScalar blendfactor = -dist0/(dist1-dist0);
+		bt_vec_blend(clipped[clipped_count],point0,point1,blendfactor);
+		clipped_count++;
+	}
+	if(!_classif)
+	{
+		clipped[clipped_count] = point1;
+		clipped_count++;
+	}
+}
+
+
+//! Clips a polygon by a plane
+/*!
+*\return The count of the clipped counts
+*/
+SIMD_FORCE_INLINE int bt_plane_clip_polygon(
+						const btVector4 & plane,
+						const btVector3 * polygon_points,
+						int polygon_point_count,
+						btVector3 * clipped)
+{
+    int clipped_count = 0;
+
+
+    //clip first point
+	btScalar firstdist = bt_distance_point_plane(plane,polygon_points[0]);;
+	if(!(firstdist>SIMD_EPSILON))
+	{
+		clipped[clipped_count] = polygon_points[0];
+		clipped_count++;
+	}
+
+	btScalar olddist = firstdist;
+	for(int i=1;i<polygon_point_count;i++)
+	{
+		btScalar dist = bt_distance_point_plane(plane,polygon_points[i]);
+
+		bt_plane_clip_polygon_collect(
+						polygon_points[i-1],polygon_points[i],
+						olddist,
+						dist,
+						clipped,
+						clipped_count);
+
+
+		olddist = dist;
+	}
+
+	//RETURN TO FIRST  point
+
+	bt_plane_clip_polygon_collect(
+					polygon_points[polygon_point_count-1],polygon_points[0],
+					olddist,
+					firstdist,
+					clipped,
+					clipped_count);
+
+	return clipped_count;
+}
+
+//! Clips a polygon by a plane
+/*!
+*\param clipped must be an array of 16 points.
+*\return The count of the clipped counts
+*/
+SIMD_FORCE_INLINE int bt_plane_clip_triangle(
+						const btVector4 & plane,
+						const btVector3 & point0,
+						const btVector3 & point1,
+						const btVector3& point2,
+						btVector3 * clipped // an allocated array of 16 points at least
+						)
+{
+    int clipped_count = 0;
+
+    //clip first point0
+	btScalar firstdist = bt_distance_point_plane(plane,point0);;
+	if(!(firstdist>SIMD_EPSILON))
+	{
+		clipped[clipped_count] = point0;
+		clipped_count++;
+	}
+
+	// point 1
+	btScalar olddist = firstdist;
+	btScalar dist = bt_distance_point_plane(plane,point1);
+
+	bt_plane_clip_polygon_collect(
+					point0,point1,
+					olddist,
+					dist,
+					clipped,
+					clipped_count);
+
+	olddist = dist;
+
+
+	// point 2
+	dist = bt_distance_point_plane(plane,point2);
+
+	bt_plane_clip_polygon_collect(
+					point1,point2,
+					olddist,
+					dist,
+					clipped,
+					clipped_count);
+	olddist = dist;
+
+
+
+	//RETURN TO FIRST  point0
+	bt_plane_clip_polygon_collect(
+					point2,point0,
+					olddist,
+					firstdist,
+					clipped,
+					clipped_count);
+
+	return clipped_count;
+}
+
+
+
+
+//! @}
+
+#endif // GIM_TRI_COLLISION_H_INCLUDED

src/BulletCollision/Gimpact/btContactProcessing.cpp

@@ -0,0 +1,181 @@
+
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 "btContactProcessing.h"
+
+#define MAX_COINCIDENT 8
+
+struct CONTACT_KEY_TOKEN
+{
+	unsigned int m_key;
+	int m_value;
+	CONTACT_KEY_TOKEN()
+    {
+    }
+
+    CONTACT_KEY_TOKEN(unsigned int key,int token)
+    {
+    	m_key = key;
+    	m_value =  token;
+    }
+
+    CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken)
+    {
+    	m_key = rtoken.m_key;
+    	m_value = rtoken.m_value;
+    }
+
+    inline bool operator <(const CONTACT_KEY_TOKEN& other) const
+	{
+		return (m_key < other.m_key);
+	}
+
+	inline bool operator >(const CONTACT_KEY_TOKEN& other) const
+	{
+		return (m_key > other.m_key);
+	}
+
+};
+
+class CONTACT_KEY_TOKEN_COMP
+{
+	public:
+
+		bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b )
+		{
+			return ( a < b );
+		}
+};
+
+
+void btContactArray::merge_contacts(
+	const btContactArray & contacts, bool normal_contact_average)
+{
+	clear();
+
+	int i;
+	if(contacts.size()==0) return;
+
+
+	if(contacts.size()==1)
+	{
+		push_back(contacts[0]);
+		return;
+	}
+
+	btAlignedObjectArray<CONTACT_KEY_TOKEN> keycontacts;
+
+	keycontacts.reserve(contacts.size());
+
+	//fill key contacts
+
+	for ( i = 0;i<contacts.size() ;i++ )
+	{
+		keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i));
+	}
+
+	//sort keys
+	keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP());
+
+	// Merge contacts
+	int coincident_count=0;
+	btVector3 coincident_normals[MAX_COINCIDENT];
+
+	unsigned int last_key = keycontacts[0].m_key;
+	unsigned int key = 0;
+
+	push_back(contacts[keycontacts[0].m_value]);
+
+	BT_CONTACT * pcontact = &(*this)[0];
+
+	for( i=1;i<keycontacts.size();i++)
+	{
+	    key = keycontacts[i].m_key;
+		const BT_CONTACT * scontact = &contacts[keycontacts[i].m_value];
+
+		if(last_key ==  key)//same points
+		{
+			//merge contact
+			if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
+			{
+				*pcontact = *scontact;
+                coincident_count = 0;
+			}
+			else if(normal_contact_average)
+			{
+				if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
+                {
+                    if(coincident_count<MAX_COINCIDENT)
+                    {
+                    	coincident_normals[coincident_count] = scontact->m_normal;
+                        coincident_count++;
+                    }
+                }
+			}
+		}
+		else
+		{//add new contact
+
+		    if(normal_contact_average && coincident_count>0)
+		    {
+		    	pcontact->interpolate_normals(coincident_normals,coincident_count);
+		        coincident_count = 0;
+		    }
+
+		    push_back(*scontact);
+		    pcontact = &(*this)[this->size()-1];
+        }
+		last_key = key;
+	}
+}
+
+void btContactArray::merge_contacts_unique(const btContactArray & contacts)
+{
+	clear();
+
+	if(contacts.size()==0) return;
+
+	if(contacts.size()==1)
+	{
+		push_back(contacts[0]);
+		return;
+	}
+
+	BT_CONTACT average_contact = contacts[0];
+
+	for (int i=1;i<contacts.size() ;i++ )
+	{
+		average_contact.m_point += contacts[i].m_point;
+		average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
+	}
+
+	//divide
+	btScalar divide_average = 1.0f/((btScalar)contacts.size());
+
+	average_contact.m_point *= divide_average;
+
+	average_contact.m_normal *= divide_average;
+
+	average_contact.m_depth = average_contact.m_normal.length();
+
+	average_contact.m_normal /= average_contact.m_depth;
+
+}
+

src/BulletCollision/Gimpact/btContactProcessing.h

@@ -0,0 +1,149 @@
+#ifndef BT_CONTACT_H_INCLUDED
+#define BT_CONTACT_H_INCLUDED
+
+/*! \file gim_contact.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btTriangleShapeEx.h"
+
+
+/*! \defgroup CONTACTS
+\brief
+Functions for managing and sorting contacts resulting from a collision query.
+*/
+//! @{
+
+/**
+Configuration var for applying interpolation of  contact normals
+*/
+#define NORMAL_CONTACT_AVERAGE 1
+
+#define CONTACT_DIFF_EPSILON 0.00001f
+
+/// Structure for collision results
+class BT_CONTACT
+{
+public:
+    btVector3 m_point;
+    btVector3 m_normal;
+    btScalar m_depth;//Positive value indicates interpenetration
+    btScalar m_distance;//Padding not for use
+    int m_feature1;//Face number
+    int m_feature2;//Face number
+public:
+    BT_CONTACT()
+    {
+    }
+
+    BT_CONTACT(const BT_CONTACT & contact):
+				m_point(contact.m_point),
+				m_normal(contact.m_normal),
+				m_depth(contact.m_depth),
+				m_feature1(contact.m_feature1),
+				m_feature2(contact.m_feature2)
+    {
+    }
+
+    BT_CONTACT(const btVector3 &point,const btVector3 & normal,
+    	 			btScalar depth, int feature1, int feature2):
+				m_point(point),
+				m_normal(normal),
+				m_depth(depth),
+				m_feature1(feature1),
+				m_feature2(feature2)
+    {
+    }
+
+	//! Calcs key for coord classification
+    SIMD_FORCE_INLINE unsigned int calc_key_contact() const
+    {
+    	int _coords[] = {
+    		(int)(m_point[0]*1000.0f+1.0f),
+    		(int)(m_point[1]*1333.0f),
+    		(int)(m_point[2]*2133.0f+3.0f)};
+		unsigned int _hash=0;
+		unsigned int *_uitmp = (unsigned int *)(&_coords[0]);
+		_hash = *_uitmp;
+		_uitmp++;
+		_hash += (*_uitmp)<<4;
+		_uitmp++;
+		_hash += (*_uitmp)<<8;
+		return _hash;
+    }
+
+    SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count)
+    {
+    	btVector3 vec_sum(m_normal);
+		for(int i=0;i<normal_count;i++)
+		{
+			vec_sum += normals[i];
+		}
+
+		btScalar vec_sum_len = vec_sum.length2();
+		if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
+
+		//GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
+
+		m_normal = vec_sum/btSqrt(vec_sum_len);
+    }
+
+};
+
+
+class btContactArray:public btAlignedObjectArray<BT_CONTACT>
+{
+public:
+	btContactArray()
+	{
+		reserve(64);
+	}
+
+	SIMD_FORCE_INLINE void push_contact(
+		const btVector3 &point,const btVector3 & normal,
+		btScalar depth, int feature1, int feature2)
+	{
+		push_back( BT_CONTACT(point,normal,depth,feature1,feature2) );
+	}
+
+	SIMD_FORCE_INLINE void push_triangle_contacts(
+		const BT_TRIANGLE_CONTACT & tricontact,
+		int feature1,int feature2)
+	{
+		for(int i = 0;i<tricontact.m_point_count ;i++ )
+		{
+			push_contact(
+				tricontact.m_points[i],
+				tricontact.m_separating_normal,
+				tricontact.m_penetration_depth,feature1,feature2);
+		}
+	}
+
+	void merge_contacts(const btContactArray & contacts, bool normal_contact_average = true);
+
+	void merge_contacts_unique(const btContactArray & contacts);
+};
+
+//! @}
+#endif // GIM_CONTACT_H_INCLUDED

src/BulletCollision/Gimpact/btGImpactBvh.cpp

@@ -0,0 +1,499 @@
+/*! \file gim_box_set.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 "btGImpactBvh.h"
+#include "LinearMath/btQuickprof.h"
+
+#ifdef TRI_COLLISION_PROFILING
+
+btClock g_tree_clock;
+
+float g_accum_tree_collision_time = 0;
+int g_count_traversing = 0;
+
+
+void bt_begin_gim02_tree_time()
+{
+	g_tree_clock.reset();
+}
+
+void bt_end_gim02_tree_time()
+{
+	g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds();
+	g_count_traversing++;
+}
+
+//! Gets the average time in miliseconds of tree collisions
+float btGImpactBvh::getAverageTreeCollisionTime()
+{
+	if(g_count_traversing == 0) return 0;
+
+	float avgtime = g_accum_tree_collision_time;
+	avgtime /= (float)g_count_traversing;
+
+	g_accum_tree_collision_time = 0;
+	g_count_traversing = 0;
+	return avgtime;
+
+//	float avgtime = g_count_traversing;
+//	g_count_traversing = 0;
+//	return avgtime;
+
+}
+
+#endif //TRI_COLLISION_PROFILING
+
+/////////////////////// btBvhTree /////////////////////////////////
+
+int btBvhTree::_calc_splitting_axis(
+	BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+{
+
+	int i;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+	int numIndices = endIndex-startIndex;
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center-means;
+		diff2 = diff2 * diff2;
+		variance += diff2;
+	}
+	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+
+	return variance.maxAxis();
+}
+
+
+int btBvhTree::_sort_and_calc_splitting_index(
+	BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
+	int endIndex, int splitAxis)
+{
+	int i;
+	int splitIndex =startIndex;
+	int numIndices = endIndex - startIndex;
+
+	// average of centers
+	btScalar splitValue = 0.0f;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	splitValue = means[splitAxis];
+
+
+	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		if (center[splitAxis] > splitValue)
+		{
+			//swap
+			primitive_boxes.swap(i,splitIndex);
+			//swapLeafNodes(i,splitIndex);
+			splitIndex++;
+		}
+	}
+
+	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+	//otherwise the tree-building might fail due to stack-overflows in certain cases.
+	//unbalanced1 is unsafe: it can cause stack overflows
+	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+	//unbalanced2 should work too: always use center (perfect balanced trees)
+	//bool unbalanced2 = true;
+
+	//this should be safe too:
+	int rangeBalancedIndices = numIndices/3;
+	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+	if (unbalanced)
+	{
+		splitIndex = startIndex+ (numIndices>>1);
+	}
+
+	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
+	btAssert(!unbal);
+
+	return splitIndex;
+
+}
+
+
+void btBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+{
+	int curIndex = m_num_nodes;
+	m_num_nodes++;
+
+	btAssert((endIndex-startIndex)>0);
+
+	if ((endIndex-startIndex)==1)
+	{
+	    //We have a leaf node
+	    setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
+		m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
+
+		return;
+	}
+	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+	//split axis
+	int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+	splitIndex = _sort_and_calc_splitting_index(
+			primitive_boxes,startIndex,endIndex,
+			splitIndex//split axis
+			);
+
+
+	//calc this node bounding box
+
+	btAABB node_bound;
+	node_bound.invalidate();
+
+	for (int i=startIndex;i<endIndex;i++)
+	{
+		node_bound.merge(primitive_boxes[i].m_bound);
+	}
+
+	setNodeBound(curIndex,node_bound);
+
+
+	//build left branch
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+
+	//build right branch
+	 _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+	m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
+
+
+}
+
+//! stackless build tree
+void btBvhTree::build_tree(
+	BT_BVH_DATA_ARRAY & primitive_boxes)
+{
+	// initialize node count to 0
+	m_num_nodes = 0;
+	// allocate nodes
+	m_node_array.resize(primitive_boxes.size()*2);
+
+	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+
+////////////////////////////////////class btGImpactBvh
+
+void btGImpactBvh::refit()
+{
+	int nodecount = getNodeCount();
+	while(nodecount--)
+	{
+		if(isLeafNode(nodecount))
+		{
+			btAABB leafbox;
+			m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
+			setNodeBound(nodecount,leafbox);
+		}
+		else
+		{
+			//const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
+			//get left bound
+			btAABB bound;
+			bound.invalidate();
+
+			btAABB temp_box;
+
+			int child_node = getLeftNode(nodecount);
+			if(child_node)
+			{
+				getNodeBound(child_node,temp_box);
+				bound.merge(temp_box);
+			}
+
+			child_node = getRightNode(nodecount);
+			if(child_node)
+			{
+				getNodeBound(child_node,temp_box);
+				bound.merge(temp_box);
+			}
+
+			setNodeBound(nodecount,bound);
+		}
+	}
+}
+
+//! this rebuild the entire set
+void btGImpactBvh::buildSet()
+{
+	//obtain primitive boxes
+	BT_BVH_DATA_ARRAY primitive_boxes;
+	primitive_boxes.resize(m_primitive_manager->get_primitive_count());
+
+	for (int i = 0;i<primitive_boxes.size() ;i++ )
+	{
+		 m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
+		 primitive_boxes[i].m_data = i;
+	}
+
+	m_box_tree.build_tree(primitive_boxes);
+}
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
+{
+	int curIndex = 0;
+	int numNodes = getNodeCount();
+
+	while (curIndex < numNodes)
+	{
+		btAABB bound;
+		getNodeBound(curIndex,bound);
+
+		//catch bugs in tree data
+
+		bool aabbOverlap = bound.has_collision(box);
+		bool isleafnode = isLeafNode(curIndex);
+
+		if (isleafnode && aabbOverlap)
+		{
+			collided_results.push_back(getNodeData(curIndex));
+		}
+
+		if (aabbOverlap || isleafnode)
+		{
+			//next subnode
+			curIndex++;
+		}
+		else
+		{
+			//skip node
+			curIndex+= getEscapeNodeIndex(curIndex);
+		}
+	}
+	if(collided_results.size()>0) return true;
+	return false;
+}
+
+
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactBvh::rayQuery(
+	const btVector3 & ray_dir,const btVector3 & ray_origin ,
+	btAlignedObjectArray<int> & collided_results) const
+{
+	int curIndex = 0;
+	int numNodes = getNodeCount();
+
+	while (curIndex < numNodes)
+	{
+		btAABB bound;
+		getNodeBound(curIndex,bound);
+
+		//catch bugs in tree data
+
+		bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+		bool isleafnode = isLeafNode(curIndex);
+
+		if (isleafnode && aabbOverlap)
+		{
+			collided_results.push_back(getNodeData( curIndex));
+		}
+
+		if (aabbOverlap || isleafnode)
+		{
+			//next subnode
+			curIndex++;
+		}
+		else
+		{
+			//skip node
+			curIndex+= getEscapeNodeIndex(curIndex);
+		}
+	}
+	if(collided_results.size()>0) return true;
+	return false;
+}
+
+
+SIMD_FORCE_INLINE bool _node_collision(
+	btGImpactBvh * boxset0, btGImpactBvh * boxset1,
+	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	int node0 ,int node1, bool complete_primitive_tests)
+{
+	btAABB box0;
+	boxset0->getNodeBound(node0,box0);
+	btAABB box1;
+	boxset1->getNodeBound(node1,box1);
+
+	return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
+//	box1.appy_transform_trans_cache(trans_cache_1to0);
+//	return box0.has_collision(box1);
+
+}
+
+
+//stackless recursive collision routine
+static void _find_collision_pairs_recursive(
+	btGImpactBvh * boxset0, btGImpactBvh * boxset1,
+	btPairSet * collision_pairs,
+	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	int node0, int node1, bool complete_primitive_tests)
+{
+
+
+
+	if( _node_collision(
+		boxset0,boxset1,trans_cache_1to0,
+		node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
+
+	if(boxset0->isLeafNode(node0))
+	{
+		if(boxset1->isLeafNode(node1))
+		{
+			// collision result
+			collision_pairs->push_pair(
+				boxset0->getNodeData(node0),boxset1->getNodeData(node1));
+			return;
+		}
+		else
+		{
+
+			//collide left recursive
+
+			_find_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								node0,boxset1->getLeftNode(node1),false);
+
+			//collide right recursive
+			_find_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								node0,boxset1->getRightNode(node1),false);
+
+
+		}
+	}
+	else
+	{
+		if(boxset1->isLeafNode(node1))
+		{
+
+			//collide left recursive
+			_find_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								boxset0->getLeftNode(node0),node1,false);
+
+
+			//collide right recursive
+
+			_find_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								boxset0->getRightNode(node0),node1,false);
+
+
+		}
+		else
+		{
+			//collide left0 left1
+
+
+
+			_find_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
+
+			//collide left0 right1
+
+			_find_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
+
+
+			//collide right0 left1
+
+			_find_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
+
+			//collide right0 right1
+
+			_find_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
+
+		}// else if node1 is not a leaf
+	}// else if node0 is not a leaf
+}
+
+
+void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0,
+		btGImpactBvh * boxset1, const btTransform & trans1,
+		btPairSet & collision_pairs)
+{
+
+	if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
+
+	BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
+
+	trans_cache_1to0.calc_from_homogenic(trans0,trans1);
+
+#ifdef TRI_COLLISION_PROFILING
+	bt_begin_gim02_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+	_find_collision_pairs_recursive(
+		boxset0,boxset1,
+		&collision_pairs,trans_cache_1to0,0,0,true);
+#ifdef TRI_COLLISION_PROFILING
+	bt_end_gim02_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+}
+

src/BulletCollision/Gimpact/btGImpactBvh.h

@@ -0,0 +1,401 @@
+#ifndef GIM_BOX_SET_H_INCLUDED
+#define GIM_BOX_SET_H_INCLUDED
+
+/*! \file gim_box_set.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btBoxCollision.h"
+#include "btTriangleShapeEx.h"
+
+
+
+/*! \defgroup BOX_TREES
+
+
+
+*/
+//! @{
+
+
+//! Overlapping pair
+struct BT_PAIR
+{
+    int m_index1;
+    int m_index2;
+    BT_PAIR()
+    {}
+
+    BT_PAIR(const BT_PAIR & p)
+    {
+    	m_index1 = p.m_index1;
+    	m_index2 = p.m_index2;
+	}
+
+	BT_PAIR(int index1, int index2)
+    {
+    	m_index1 = index1;
+    	m_index2 = index2;
+	}
+};
+
+//! A pairset array
+class btPairSet: public btAlignedObjectArray<BT_PAIR>
+{
+public:
+	btPairSet()
+	{
+		reserve(32);
+	}
+	inline void push_pair(int index1,int index2)
+	{
+		push_back(BT_PAIR(index1,index2));
+	}
+
+	inline void push_pair_inv(int index1,int index2)
+	{
+		push_back(BT_PAIR(index2,index1));
+	}
+};
+
+
+
+struct BT_BVH_DATA
+{
+	btAABB m_bound;
+	int m_data;
+};
+
+//! Node Structure for trees
+class BT_BVH_TREE_NODE
+{
+public:
+	btAABB m_bound;
+protected:
+	int	m_escapeIndexOrDataIndex;
+public:
+	BT_BVH_TREE_NODE()
+	{
+		m_escapeIndexOrDataIndex = 0;
+	}
+
+	SIMD_FORCE_INLINE bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrDataIndex>=0);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex < 0);
+		return -m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setEscapeIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = -index;
+	}
+
+	SIMD_FORCE_INLINE int getDataIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex >= 0);
+
+		return m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setDataIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = index;
+	}
+
+};
+
+
+class BT_BVH_DATA_ARRAY:public btAlignedObjectArray<BT_BVH_DATA>
+{
+};
+
+
+class BT_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<BT_BVH_TREE_NODE>
+{
+};
+
+
+
+
+//! Basic Box tree structure
+class btBvhTree
+{
+protected:
+	int m_num_nodes;
+	BT_BVH_TREE_NODE_ARRAY m_node_array;
+protected:
+	int _sort_and_calc_splitting_index(
+		BT_BVH_DATA_ARRAY & primitive_boxes,
+		 int startIndex,  int endIndex, int splitAxis);
+
+	int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
+
+	void _build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
+public:
+	btBvhTree()
+	{
+		m_num_nodes = 0;
+	}
+
+	//! prototype functions for box tree management
+	//!@{
+	void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes);
+
+	SIMD_FORCE_INLINE void clearNodes()
+	{
+		m_node_array.clear();
+		m_num_nodes = 0;
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE int getNodeCount() const
+	{
+		return m_num_nodes;
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+	{
+		return m_node_array[nodeindex].isLeafNode();
+	}
+
+	SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+	{
+		return m_node_array[nodeindex].getDataIndex();
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
+	{
+		bound = m_node_array[nodeindex].m_bound;
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	{
+		m_node_array[nodeindex].m_bound = bound;
+	}
+
+	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+	{
+		return nodeindex+1;
+	}
+
+	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+	{
+		if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
+		return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
+	}
+
+	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+	{
+		return m_node_array[nodeindex].getEscapeIndex();
+	}
+
+	SIMD_FORCE_INLINE const BT_BVH_TREE_NODE * get_node_pointer(int index = 0) const
+	{
+		return &m_node_array[index];
+	}
+
+	//!@}
+};
+
+
+//! Prototype Base class for primitive classification
+/*!
+This class is a wrapper for primitive collections.
+This tells relevant info for the Bounding Box set classes, which take care of space classification.
+This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the  Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons.
+*/
+class btPrimitiveManagerBase
+{
+public:
+
+	//! determines if this manager consist on only triangles, which special case will be optimized
+	virtual bool is_trimesh() const = 0;
+	virtual int get_primitive_count() const = 0;
+	virtual void get_primitive_box(int prim_index ,btAABB & primbox) const = 0;
+	//! retrieves only the points of the triangle, and the collision margin
+	virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const= 0;
+};
+
+
+//! Structure for containing Boxes
+/*!
+This class offers an structure for managing a box tree of primitives.
+Requires a Primitive prototype (like btPrimitiveManagerBase )
+*/
+class btGImpactBvh
+{
+protected:
+	btBvhTree m_box_tree;
+	btPrimitiveManagerBase * m_primitive_manager;
+
+protected:
+	//stackless refit
+	void refit();
+public:
+
+	//! this constructor doesn't build the tree. you must call	buildSet
+	btGImpactBvh()
+	{
+		m_primitive_manager = NULL;
+	}
+
+	//! this constructor doesn't build the tree. you must call	buildSet
+	btGImpactBvh(btPrimitiveManagerBase * primitive_manager)
+	{
+		m_primitive_manager = primitive_manager;
+	}
+
+	SIMD_FORCE_INLINE btAABB getGlobalBox()  const
+	{
+		btAABB totalbox;
+		getNodeBound(0, totalbox);
+		return totalbox;
+	}
+
+	SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
+	{
+		m_primitive_manager = primitive_manager;
+	}
+
+	SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
+	{
+		return m_primitive_manager;
+	}
+
+
+//! node manager prototype functions
+///@{
+
+	//! this attemps to refit the box set.
+	SIMD_FORCE_INLINE void update()
+	{
+		refit();
+	}
+
+	//! this rebuild the entire set
+	void buildSet();
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box,
+		 const btTransform & transform, btAlignedObjectArray<int> & collided_results) const
+	{
+		btAABB transbox=box;
+		transbox.appy_transform(transform);
+		return boxQuery(transbox,collided_results);
+	}
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	bool rayQuery(
+		const btVector3 & ray_dir,const btVector3 & ray_origin ,
+		btAlignedObjectArray<int> & collided_results) const;
+
+	//! tells if this set has hierarcht
+	SIMD_FORCE_INLINE bool hasHierarchy() const
+	{
+		return true;
+	}
+
+	//! tells if this set is a trimesh
+	SIMD_FORCE_INLINE bool isTrimesh()  const
+	{
+		return m_primitive_manager->is_trimesh();
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE int getNodeCount() const
+	{
+		return m_box_tree.getNodeCount();
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+	{
+		return m_box_tree.isLeafNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+	{
+		return m_box_tree.getNodeData(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound)  const
+	{
+		m_box_tree.getNodeBound(nodeindex, bound);
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	{
+		m_box_tree.setNodeBound(nodeindex, bound);
+	}
+
+
+	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+	{
+		return m_box_tree.getLeftNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+	{
+		return m_box_tree.getRightNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+	{
+		return m_box_tree.getEscapeNodeIndex(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
+	{
+		m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
+	}
+
+
+	SIMD_FORCE_INLINE const BT_BVH_TREE_NODE * get_node_pointer(int index = 0) const
+	{
+		return m_box_tree.get_node_pointer(index);
+	}
+
+//! @}
+
+	static float getAverageTreeCollisionTime();
+
+
+	static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1,
+		btGImpactBvh * boxset2, const btTransform & trans2,
+		btPairSet & collision_pairs);
+};
+
+
+#endif // GIM_BOXPRUNING_H_INCLUDED

src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp

@@ -0,0 +1,896 @@
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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.
+*/
+/*
+Author: Francisco Len N<>jera
+Concave-Concave Collision
+
+*/
+
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "btGImpactCollisionAlgorithm.h"
+#include "btContactProcessing.h"
+#include "LinearMath/btQuickprof.h"
+
+
+//! Class for accessing the plane equation
+class btPlaneShape : public btStaticPlaneShape
+{
+public:
+
+	btPlaneShape(const btVector3& v, float f)
+		:btStaticPlaneShape(v,f)
+	{
+	}
+
+	void get_plane_equation(btVector4 &equation)
+	{
+		equation[0] = m_planeNormal[0];
+		equation[1] = m_planeNormal[1];
+		equation[2] = m_planeNormal[2];
+		equation[3] = m_planeConstant;
+	}
+
+
+	void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation)
+	{
+		equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal);
+		equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal);
+		equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal);
+		equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant;
+	}
+};
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+#ifdef TRI_COLLISION_PROFILING
+
+btClock g_triangle_clock;
+
+float g_accum_triangle_collision_time = 0;
+int g_count_triangle_collision = 0;
+
+void bt_begin_gim02_tri_time()
+{
+	g_triangle_clock.reset();
+}
+
+void bt_end_gim02_tri_time()
+{
+	g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds();
+	g_count_triangle_collision++;
+}
+#endif //TRI_COLLISION_PROFILING
+//! Retrieving shapes shapes
+/*!
+Declared here due of insuficent space on Pool allocators
+*/
+//!@{
+class GIM_ShapeRetriever
+{
+public:
+	btGImpactShapeInterface * m_gim_shape;
+	btTriangleShapeEx m_trishape;
+	btTetrahedronShapeEx m_tetrashape;
+
+public:
+	class ChildShapeRetriever
+	{
+	public:
+		GIM_ShapeRetriever * m_parent;
+		virtual btCollisionShape * getChildShape(int index)
+		{
+			return m_parent->m_gim_shape->getChildShape(index);
+		}
+	};
+
+	class TriangleShapeRetriever:public ChildShapeRetriever
+	{
+	public:
+
+		virtual btCollisionShape * getChildShape(int index)
+		{
+			m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape);
+			return &m_parent->m_trishape;
+		}
+	};
+
+	class TetraShapeRetriever:public ChildShapeRetriever
+	{
+	public:
+
+		virtual btCollisionShape * getChildShape(int index)
+		{
+			m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape);
+			return &m_parent->m_tetrashape;
+		}
+	};
+public:
+	ChildShapeRetriever m_child_retriever;
+	TriangleShapeRetriever m_tri_retriever;
+	TetraShapeRetriever  m_tetra_retriever;
+	ChildShapeRetriever * m_current_retriever;
+
+	GIM_ShapeRetriever(btGImpactShapeInterface * gim_shape)
+	{
+		m_gim_shape = gim_shape;
+		//select retriever
+		if(m_gim_shape->needsRetrieveTriangles())
+		{
+			m_current_retriever = &m_tri_retriever;
+		}
+		else if(m_gim_shape->needsRetrieveTetrahedrons())
+		{
+			m_current_retriever = &m_tetra_retriever;
+		}
+		else
+		{
+			m_current_retriever = &m_child_retriever;
+		}
+
+		m_current_retriever->m_parent = this;
+	}
+
+	btCollisionShape * getChildShape(int index)
+	{
+		return m_current_retriever->getChildShape(index);
+	}
+
+
+};
+
+
+
+//!@}
+
+
+#ifdef TRI_COLLISION_PROFILING
+
+//! Gets the average time in miliseconds of tree collisions
+float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime()
+{
+	return btGImpactBoxSet::getAverageTreeCollisionTime();
+
+}
+
+//! Gets the average time in miliseconds of triangle collisions
+float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime()
+{
+	if(g_count_triangle_collision == 0) return 0;
+
+	float avgtime = g_accum_triangle_collision_time;
+	avgtime /= (float)g_count_triangle_collision;
+
+	g_accum_triangle_collision_time = 0;
+	g_count_triangle_collision = 0;
+
+	return avgtime;
+}
+
+#endif //TRI_COLLISION_PROFILING
+
+
+
+btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+: btCollisionAlgorithm(ci)
+{
+	m_manifoldPtr = NULL;
+	m_convex_algorithm = NULL;
+}
+
+btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm()
+{
+	clearCache();
+}
+
+
+
+
+
+void btGImpactCollisionAlgorithm::addContactPoint(btCollisionObject * body0,
+				btCollisionObject * body1,
+				const btVector3 & point,
+				const btVector3 & normal,
+				btScalar distance)
+{
+	m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1);
+	checkManifold(body0,body1);
+	m_resultOut->addContactPoint(normal,point,distance);
+}
+
+
+void btGImpactCollisionAlgorithm::shape_vs_shape_collision(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btCollisionShape * shape0,
+					  btCollisionShape * shape1)
+{
+
+	btCollisionShape* tmpShape0 = body0->getCollisionShape();
+	btCollisionShape* tmpShape1 = body1->getCollisionShape();
+	
+	body0->internalSetTemporaryCollisionShape(shape0);
+	body1->internalSetTemporaryCollisionShape(shape1);
+
+	{
+		btCollisionAlgorithm* algor = newAlgorithm(body0,body1);
+		// post :	checkManifold is called
+
+		m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1);
+
+		algor->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
+
+		algor->~btCollisionAlgorithm();
+		m_dispatcher->freeCollisionAlgorithm(algor);
+	}
+
+	body0->internalSetTemporaryCollisionShape(tmpShape0);
+	body1->internalSetTemporaryCollisionShape(tmpShape1);
+}
+
+void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btCollisionShape * shape0,
+					  btCollisionShape * shape1)
+{
+
+	btCollisionShape* tmpShape0 = body0->getCollisionShape();
+	btCollisionShape* tmpShape1 = body1->getCollisionShape();
+	
+	body0->internalSetTemporaryCollisionShape(shape0);
+	body1->internalSetTemporaryCollisionShape(shape1);
+
+
+	m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1);
+
+	checkConvexAlgorithm(body0,body1);
+	m_convex_algorithm->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
+
+	body0->internalSetTemporaryCollisionShape(tmpShape0);
+	body1->internalSetTemporaryCollisionShape(tmpShape1);
+
+}
+
+
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
+					  const btTransform & trans0,
+					  const btTransform & trans1,
+					  btGImpactShapeInterface * shape0,
+					  btGImpactShapeInterface * shape1,btPairSet & pairset)
+{
+	if(shape0->hasBoxSet() && shape1->hasBoxSet())
+	{
+		btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset);
+	}
+	else
+	{
+		btAABB boxshape0;
+		btAABB boxshape1;
+		int i = shape0->getNumChildShapes();
+
+		while(i--)
+		{
+			shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
+
+			int j = shape1->getNumChildShapes();
+			while(j--)
+			{
+				shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max);
+
+				if(boxshape1.has_collision(boxshape0))
+				{
+					pairset.push_pair(i,j);
+				}
+			}
+		}
+	}
+
+
+}
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
+					  const btTransform & trans0,
+					  const btTransform & trans1,
+					  btGImpactShapeInterface * shape0,
+					  btCollisionShape * shape1,
+					  btAlignedObjectArray<int> & collided_primitives)
+{
+
+	btAABB boxshape;
+
+
+	if(shape0->hasBoxSet())
+	{
+		btTransform trans1to0 = trans0.inverse();
+		trans1to0 *= trans1;
+
+		shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
+
+		shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
+	}
+	else
+	{
+		shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);
+
+		btAABB boxshape0;
+		int i = shape0->getNumChildShapes();
+
+		while(i--)
+		{
+			shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
+
+			if(boxshape.has_collision(boxshape0))
+			{
+				collided_primitives.push_back(i);
+			}
+		}
+
+	}
+
+}
+
+
+void btGImpactCollisionAlgorithm::collide_gjk_triangles(btCollisionObject * body0,
+				  btCollisionObject * body1,
+				  btGImpactMeshShapePart * shape0,
+				  btGImpactMeshShapePart * shape1,
+				  const int * pairs, int pair_count)
+{
+	btTriangleShapeEx tri0;
+	btTriangleShapeEx tri1;
+
+	shape0->lockChildShapes();
+	shape1->lockChildShapes();
+
+	const int * pair_pointer = pairs;
+
+	while(pair_count--)
+	{
+
+		m_triface0 = *(pair_pointer);
+		m_triface1 = *(pair_pointer+1);
+		pair_pointer+=2;
+
+
+
+		shape0->getBulletTriangle(m_triface0,tri0);
+		shape1->getBulletTriangle(m_triface1,tri1);
+
+
+		//collide two convex shapes
+		if(tri0.overlap_test_conservative(tri1))
+		{
+			convex_vs_convex_collision(body0,body1,&tri0,&tri1);
+		}
+
+	}
+
+	shape0->unlockChildShapes();
+	shape1->unlockChildShapes();
+}
+
+void btGImpactCollisionAlgorithm::collide_sat_triangles(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactMeshShapePart * shape0,
+					  btGImpactMeshShapePart * shape1,
+					  const int * pairs, int pair_count)
+{
+	btTransform orgtrans0 = body0->getWorldTransform();
+	btTransform orgtrans1 = body1->getWorldTransform();
+
+	btPrimitiveTriangle ptri0;
+	btPrimitiveTriangle ptri1;
+	BT_TRIANGLE_CONTACT contact_data;
+
+	shape0->lockChildShapes();
+	shape1->lockChildShapes();
+
+	const int * pair_pointer = pairs;
+
+	while(pair_count--)
+	{
+
+		m_triface0 = *(pair_pointer);
+		m_triface1 = *(pair_pointer+1);
+		pair_pointer+=2;
+
+
+		shape0->getPrimitiveTriangle(m_triface0,ptri0);
+		shape1->getPrimitiveTriangle(m_triface1,ptri1);
+
+		#ifdef TRI_COLLISION_PROFILING
+		bt_begin_gim02_tri_time();
+		#endif
+
+		ptri0.applyTransform(orgtrans0);
+		ptri1.applyTransform(orgtrans1);
+
+
+		//build planes
+		ptri0.buildTriPlane();
+		ptri1.buildTriPlane();
+		// test conservative
+
+
+
+		if(ptri0.overlap_test_conservative(ptri1))
+		{
+			if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data))
+			{
+
+				int j = contact_data.m_point_count;
+				while(j--)
+				{
+
+					addContactPoint(body0, body1,
+								contact_data.m_points[j],
+								contact_data.m_separating_normal,
+								-contact_data.m_penetration_depth);
+				}
+			}
+		}
+
+		#ifdef TRI_COLLISION_PROFILING
+		bt_end_gim02_tri_time();
+		#endif
+
+	}
+
+	shape0->unlockChildShapes();
+	shape1->unlockChildShapes();
+
+}
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
+						btCollisionObject * body0,
+					   	btCollisionObject * body1,
+					  	btGImpactShapeInterface * shape0,
+					  	btGImpactShapeInterface * shape1)
+{
+
+	if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+	{
+		btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0);
+		m_part0 = meshshape0->getMeshPartCount();
+
+		while(m_part0--)
+		{
+			gimpact_vs_gimpact(body0,body1,meshshape0->getMeshPart(m_part0),shape1);
+		}
+
+		return;
+	}
+
+	if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+	{
+		btGImpactMeshShape * meshshape1 = static_cast<btGImpactMeshShape *>(shape1);
+		m_part1 = meshshape1->getMeshPartCount();
+
+		while(m_part1--)
+		{
+
+			gimpact_vs_gimpact(body0,body1,shape0,meshshape1->getMeshPart(m_part1));
+
+		}
+
+		return;
+	}
+
+
+	btTransform orgtrans0 = body0->getWorldTransform();
+	btTransform orgtrans1 = body1->getWorldTransform();
+
+	btPairSet pairset;
+
+	gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
+
+	if(pairset.size()== 0) return;
+
+	if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
+		shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART)
+	{
+		btGImpactMeshShapePart * shapepart0 = static_cast<btGImpactMeshShapePart * >(shape0);
+		btGImpactMeshShapePart * shapepart1 = static_cast<btGImpactMeshShapePart * >(shape1);
+		//specialized function
+		#ifdef BULLET_TRIANGLE_COLLISION
+		collide_gjk_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
+		#else
+		collide_sat_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
+		#endif
+
+		return;
+	}
+
+	//general function
+
+	shape0->lockChildShapes();
+	shape1->lockChildShapes();
+
+	GIM_ShapeRetriever retriever0(shape0);
+	GIM_ShapeRetriever retriever1(shape1);
+
+	bool child_has_transform0 = shape0->childrenHasTransform();
+	bool child_has_transform1 = shape1->childrenHasTransform();
+
+	int i = pairset.size();
+	while(i--)
+	{
+		BT_PAIR * pair = &pairset[i];
+		m_triface0 = pair->m_index1;
+		m_triface1 = pair->m_index2;
+		btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0);
+		btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1);
+
+		if(child_has_transform0)
+		{
+			body0->setWorldTransform(orgtrans0*shape0->getChildTransform(m_triface0));
+		}
+
+		if(child_has_transform1)
+		{
+			body1->setWorldTransform(orgtrans1*shape1->getChildTransform(m_triface1));
+		}
+
+		//collide two convex shapes
+		convex_vs_convex_collision(body0,body1,colshape0,colshape1);
+
+
+		if(child_has_transform0)
+		{
+			body0->setWorldTransform(orgtrans0);
+		}
+
+		if(child_has_transform1)
+		{
+			body1->setWorldTransform(orgtrans1);
+		}
+
+	}
+
+	shape0->unlockChildShapes();
+	shape1->unlockChildShapes();
+}
+
+void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0,
+				  btCollisionObject * body1,
+				  btGImpactShapeInterface * shape0,
+				  btCollisionShape * shape1,bool swapped)
+{
+	if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+	{
+		btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0);
+		m_part0 = meshshape0->getMeshPartCount();
+
+		while(m_part0--)
+		{
+
+			gimpact_vs_shape(body0,
+				  body1,
+				  meshshape0->getMeshPart(m_part0),
+				  shape1,swapped);
+
+		}
+
+		return;
+	}
+
+	#ifdef GIMPACT_VS_PLANE_COLLISION
+	if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
+		shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
+	{
+		btGImpactMeshShapePart * shapepart = static_cast<btGImpactMeshShapePart *>(shape0);
+		btStaticPlaneShape * planeshape = static_cast<btStaticPlaneShape * >(shape1);
+		gimpacttrimeshpart_vs_plane_collision(body0,body1,shapepart,planeshape,swapped);
+		return;
+	}
+
+	#endif
+
+
+
+	if(shape1->isCompound())
+	{
+		btCompoundShape * compoundshape = static_cast<btCompoundShape *>(shape1);
+		gimpact_vs_compoundshape(body0,body1,shape0,compoundshape,swapped);
+		return;
+	}
+	else if(shape1->isConcave())
+	{
+		btConcaveShape * concaveshape = static_cast<btConcaveShape *>(shape1);
+		gimpact_vs_concave(body0,body1,shape0,concaveshape,swapped);
+		return;
+	}
+
+
+	btTransform orgtrans0 = body0->getWorldTransform();
+
+	btTransform orgtrans1 = body1->getWorldTransform();
+
+	btAlignedObjectArray<int> collided_results;
+
+	gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results);
+
+	if(collided_results.size() == 0) return;
+
+
+	shape0->lockChildShapes();
+
+	GIM_ShapeRetriever retriever0(shape0);
+
+
+	bool child_has_transform0 = shape0->childrenHasTransform();
+
+
+	int i = collided_results.size();
+
+	while(i--)
+	{
+		int child_index = collided_results[i];
+        if(swapped)
+    		m_triface1 = child_index;
+        else
+            m_triface0 = child_index;
+
+		btCollisionShape * colshape0 = retriever0.getChildShape(child_index);
+
+		if(child_has_transform0)
+		{
+			body0->setWorldTransform(orgtrans0*shape0->getChildTransform(child_index));
+		}
+
+		//collide two shapes
+		if(swapped)
+		{
+			shape_vs_shape_collision(body1,body0,shape1,colshape0);
+		}
+		else
+		{
+			shape_vs_shape_collision(body0,body1,colshape0,shape1);
+		}
+
+		//restore transforms
+		if(child_has_transform0)
+		{
+			body0->setWorldTransform(orgtrans0);
+		}
+
+	}
+
+	shape0->unlockChildShapes();
+
+}
+
+void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(btCollisionObject * body0,
+				  btCollisionObject * body1,
+				  btGImpactShapeInterface * shape0,
+				  btCompoundShape * shape1,bool swapped)
+{
+	btTransform orgtrans1 = body1->getWorldTransform();
+
+	int i = shape1->getNumChildShapes();
+	while(i--)
+	{
+
+		btCollisionShape * colshape1 = shape1->getChildShape(i);
+		btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i);
+
+		body1->setWorldTransform(childtrans1);
+
+		//collide child shape
+		gimpact_vs_shape(body0, body1,
+					  shape0,colshape1,swapped);
+
+
+		//restore transforms
+		body1->setWorldTransform(orgtrans1);
+	}
+}
+
+void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactMeshShapePart * shape0,
+					  btStaticPlaneShape * shape1,bool swapped)
+{
+
+
+	btTransform orgtrans0 = body0->getWorldTransform();
+	btTransform orgtrans1 = body1->getWorldTransform();
+
+	btPlaneShape * planeshape = static_cast<btPlaneShape *>(shape1);
+	btVector4 plane;
+	planeshape->get_plane_equation_transformed(orgtrans1,plane);
+
+	//test box against plane
+
+	btAABB tribox;
+	shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max);
+	tribox.increment_margin(planeshape->getMargin());
+
+	if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return;
+
+	shape0->lockChildShapes();
+
+	btScalar margin = shape0->getMargin() + planeshape->getMargin();
+
+	btVector3 vertex;
+	int vi = shape0->getVertexCount();
+	while(vi--)
+	{
+		shape0->getVertex(vi,vertex);
+		vertex = orgtrans0(vertex);
+
+		btScalar distance = vertex.dot(plane) - plane[3] - margin;
+
+		if(distance<0.0)//add contact
+		{
+			if(swapped)
+			{
+				addContactPoint(body1, body0,
+					vertex,
+					-plane,
+					distance);
+			}
+			else
+			{
+				addContactPoint(body0, body1,
+					vertex,
+					plane,
+					distance);
+			}
+		}
+	}
+
+	shape0->unlockChildShapes();
+}
+
+
+
+
+class btGImpactTriangleCallback: public btTriangleCallback
+{
+public:
+	btGImpactCollisionAlgorithm * algorithm;
+	btCollisionObject * body0;
+	btCollisionObject * body1;
+	btGImpactShapeInterface * gimpactshape0;
+	bool swapped;
+	btScalar margin;
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+	{
+		btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]);
+		tri1.setMargin(margin);
+        if(swapped)
+        {
+            algorithm->setPart0(partId);
+            algorithm->setFace0(triangleIndex);
+        }
+        else
+        {
+            algorithm->setPart1(partId);
+            algorithm->setFace1(triangleIndex);
+        }
+		algorithm->gimpact_vs_shape(
+							body0,body1,gimpactshape0,&tri1,swapped);
+	}
+};
+
+
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_concave(
+				  btCollisionObject * body0,
+				  btCollisionObject * body1,
+				  btGImpactShapeInterface * shape0,
+				  btConcaveShape * shape1,bool swapped)
+{
+	//create the callback
+	btGImpactTriangleCallback tricallback;
+	tricallback.algorithm = this;
+	tricallback.body0 = body0;
+	tricallback.body1 = body1;
+	tricallback.gimpactshape0 = shape0;
+	tricallback.swapped = swapped;
+	tricallback.margin = shape1->getMargin();
+
+	//getting the trimesh AABB
+	btTransform gimpactInConcaveSpace;
+
+	gimpactInConcaveSpace = body1->getWorldTransform().inverse() * body0->getWorldTransform();
+
+	btVector3 minAABB,maxAABB;
+	shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB);
+
+	shape1->processAllTriangles(&tricallback,minAABB,maxAABB);
+
+}
+
+
+
+void btGImpactCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+    clearCache();
+
+    m_resultOut = resultOut;
+	m_dispatchInfo = &dispatchInfo;
+    btGImpactShapeInterface * gimpactshape0;
+    btGImpactShapeInterface * gimpactshape1;
+
+	if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
+	{
+		gimpactshape0 = static_cast<btGImpactShapeInterface *>(body0->getCollisionShape());
+
+		if( body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
+		{
+			gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
+
+			gimpact_vs_gimpact(body0,body1,gimpactshape0,gimpactshape1);
+		}
+		else
+		{
+			gimpact_vs_shape(body0,body1,gimpactshape0,body1->getCollisionShape(),false);
+		}
+
+	}
+	else if (body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
+	{
+		gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
+
+		gimpact_vs_shape(body1,body0,gimpactshape1,body0->getCollisionShape(),true);
+	}
+}
+
+
+btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	return 1.f;
+
+}
+
+///////////////////////////////////// REGISTERING ALGORITHM //////////////////////////////////////////////
+
+btGImpactCollisionAlgorithm::CreateFunc g_gimpact_cf;
+
+//! Use this function for register the algorithm externally
+void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher)
+{
+
+	int i;
+
+	for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
+	{
+		dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&g_gimpact_cf);
+	}
+
+	for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
+	{
+		dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&g_gimpact_cf);
+	}
+
+}

src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h

@@ -0,0 +1,306 @@
+/*! \file btGImpactShape.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BVH_CONCAVE_COLLISION_ALGORITHM_H
+#define BVH_CONCAVE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btGImpactShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
+#include "LinearMath/btIDebugDraw.h"
+
+
+
+//! Collision Algorithm for GImpact Shapes
+/*!
+For register this algorithm in Bullet, proceed as following:
+ \code
+btCollisionDispatcher * dispatcher = static_cast<btCollisionDispatcher *>(m_dynamicsWorld ->getDispatcher());
+btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);
+ \endcode
+*/
+class btGImpactCollisionAlgorithm : public btCollisionAlgorithm
+{
+protected:
+	btCollisionAlgorithm * m_convex_algorithm;
+    btPersistentManifold * m_manifoldPtr;
+	btManifoldResult* m_resultOut;
+	const btDispatcherInfo * m_dispatchInfo;
+	int m_triface0;
+	int m_part0;
+	int m_triface1;
+	int m_part1;
+
+
+	//! Creates a new contact point
+	SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(btCollisionObject* body0,btCollisionObject* body1)
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);		
+		return m_manifoldPtr;
+	}
+
+	SIMD_FORCE_INLINE void destroyConvexAlgorithm()
+	{
+		if(m_convex_algorithm)
+		{
+			m_convex_algorithm->~btCollisionAlgorithm();
+			m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm);
+			m_convex_algorithm = NULL;
+		}
+	}
+
+	SIMD_FORCE_INLINE void destroyContactManifolds()
+	{
+		if(m_manifoldPtr == NULL) return;
+		m_dispatcher->releaseManifold(m_manifoldPtr);
+		m_manifoldPtr = NULL;
+	}
+
+	SIMD_FORCE_INLINE void clearCache()
+	{
+		destroyContactManifolds();
+		destroyConvexAlgorithm();
+
+		m_triface0 = -1;
+		m_part0 = -1;
+		m_triface1 = -1;
+		m_part1 = -1;
+	}
+
+	SIMD_FORCE_INLINE btPersistentManifold* getLastManifold()
+	{
+		return m_manifoldPtr;
+	}
+
+
+	// Call before process collision
+	SIMD_FORCE_INLINE void checkManifold(btCollisionObject* body0,btCollisionObject* body1)
+	{
+		if(getLastManifold() == 0)
+		{
+			newContactManifold(body0,body1);
+		}
+
+		m_resultOut->setPersistentManifold(getLastManifold());
+	}
+
+	// Call before process collision
+	SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(btCollisionObject* body0,btCollisionObject* body1)
+	{
+		checkManifold(body0,body1);
+
+		btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(
+				body0,body1,getLastManifold());
+		return convex_algorithm ;
+	}
+
+	// Call before process collision
+	SIMD_FORCE_INLINE void checkConvexAlgorithm(btCollisionObject* body0,btCollisionObject* body1)
+	{
+		if(m_convex_algorithm) return;
+		m_convex_algorithm = newAlgorithm(body0,body1);
+	}
+
+
+
+
+	void addContactPoint(btCollisionObject * body0,
+					btCollisionObject * body1,
+					const btVector3 & point,
+					const btVector3 & normal,
+					btScalar distance);
+
+//! Collision routines
+//!@{
+
+	void collide_gjk_triangles(btCollisionObject * body0,
+				  btCollisionObject * body1,
+				  btGImpactMeshShapePart * shape0,
+				  btGImpactMeshShapePart * shape1,
+				  const int * pairs, int pair_count);
+
+	void collide_sat_triangles(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactMeshShapePart * shape0,
+					  btGImpactMeshShapePart * shape1,
+					  const int * pairs, int pair_count);
+
+
+
+
+	void shape_vs_shape_collision(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btCollisionShape * shape0,
+					  btCollisionShape * shape1);
+
+	void convex_vs_convex_collision(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btCollisionShape * shape0,
+					  btCollisionShape * shape1);
+
+
+
+	void gimpact_vs_gimpact_find_pairs(
+					  const btTransform & trans0,
+					  const btTransform & trans1,
+					  btGImpactShapeInterface * shape0,
+					  btGImpactShapeInterface * shape1,btPairSet & pairset);
+
+	void gimpact_vs_shape_find_pairs(
+					  const btTransform & trans0,
+					  const btTransform & trans1,
+					  btGImpactShapeInterface * shape0,
+					  btCollisionShape * shape1,
+					  btAlignedObjectArray<int> & collided_primitives);
+
+
+	void gimpacttrimeshpart_vs_plane_collision(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactMeshShapePart * shape0,
+					  btStaticPlaneShape * shape1,bool swapped);
+
+
+public:
+
+	btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~btGImpactCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm));
+			return new(mem) btGImpactCollisionAlgorithm(ci,body0,body1);
+		}
+	};
+
+	//! Use this function for register the algorithm externally
+	static void registerAlgorithm(btCollisionDispatcher * dispatcher);
+
+	//! Gets the average time in miliseconds of tree collisions
+	static float getAverageTreeCollisionTime();
+
+	//! Gets the average time in miliseconds of triangle collisions
+	static float getAverageTriangleCollisionTime();
+
+
+	//! Collides two gimpact shapes
+	/*!
+	\pre shape0 and shape1 couldn't be btGImpactMeshShape objects
+	*/
+
+
+	void gimpact_vs_gimpact(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactShapeInterface * shape0,
+					  btGImpactShapeInterface * shape1);
+
+	void gimpact_vs_shape(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactShapeInterface * shape0,
+					  btCollisionShape * shape1,bool swapped);
+
+	void gimpact_vs_compoundshape(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactShapeInterface * shape0,
+					  btCompoundShape * shape1,bool swapped);
+
+	void gimpact_vs_concave(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactShapeInterface * shape0,
+					  btConcaveShape * shape1,bool swapped);
+
+
+
+
+		/// Accessor/Mutator pairs for Part and triangleID
+    void 	setFace0(int value) 
+    { 
+    	m_triface0 = value; 
+    }
+    int getFace0() 
+    { 
+    	return m_triface0; 
+    }
+    void setFace1(int value) 
+    { 
+    	m_triface1 = value; 
+    }
+    int getFace1() 
+    { 
+    	return m_triface1; 
+    }
+    void setPart0(int value) 
+    { 
+    	m_part0 = value; 
+    }
+    int getPart0() 
+    { 
+    	return m_part0; 
+    }
+    void setPart1(int value) 
+    { 
+    	m_part1 = value; 
+		}
+    int getPart1() 
+    { 
+    	return m_part1; 
+    }
+
+};
+
+
+//algorithm details
+//#define BULLET_TRIANGLE_COLLISION 1
+#define GIMPACT_VS_PLANE_COLLISION 1
+
+
+
+#endif //BVH_CONCAVE_COLLISION_ALGORITHM_H

src/BulletCollision/Gimpact/btGImpactMassUtil.h

@@ -0,0 +1,60 @@
+/*! \file btGImpactMassUtil.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 GIMPACT_MASS_UTIL_H
+#define GIMPACT_MASS_UTIL_H
+
+#include "LinearMath/btTransform.h"
+
+
+
+SIMD_FORCE_INLINE btVector3 gim_inertia_add_transformed(
+	const btVector3 & source_inertia, const btVector3 & added_inertia, const btTransform & transform)
+{
+	btMatrix3x3  rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose();
+
+	btScalar x2 = transform.getOrigin()[0];
+	x2*= x2;
+	btScalar y2 = transform.getOrigin()[1];
+	y2*= y2;
+	btScalar z2 = transform.getOrigin()[2];
+	z2*= z2;
+
+	btScalar ix = rotatedTensor[0][0]*(y2+z2);
+	btScalar iy = rotatedTensor[1][1]*(x2+z2);
+	btScalar iz = rotatedTensor[2][2]*(x2+y2);
+
+	return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz);
+}
+
+SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3 & point, btScalar mass)
+{
+	btScalar x2 = point[0]*point[0];
+	btScalar y2 = point[1]*point[1];
+	btScalar z2 = point[2]*point[2];
+	return btVector3(mass*(y2+z2),mass*(x2+z2),mass*(x2+y2));
+}
+
+
+#endif //GIMPACT_MESH_SHAPE_H

src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp

@@ -0,0 +1,529 @@
+/*! \file gim_box_set.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 "btGImpactQuantizedBvh.h"
+#include "LinearMath/btQuickprof.h"
+
+#ifdef TRI_COLLISION_PROFILING
+btClock g_q_tree_clock;
+
+
+float g_q_accum_tree_collision_time = 0;
+int g_q_count_traversing = 0;
+
+
+void bt_begin_gim02_q_tree_time()
+{
+	g_q_tree_clock.reset();
+}
+
+void bt_end_gim02_q_tree_time()
+{
+	g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds();
+	g_q_count_traversing++;
+}
+
+
+//! Gets the average time in miliseconds of tree collisions
+float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
+{
+	if(g_q_count_traversing == 0) return 0;
+
+	float avgtime = g_q_accum_tree_collision_time;
+	avgtime /= (float)g_q_count_traversing;
+
+	g_q_accum_tree_collision_time = 0;
+	g_q_count_traversing = 0;
+	return avgtime;
+
+//	float avgtime = g_q_count_traversing;
+//	g_q_count_traversing = 0;
+//	return avgtime;
+
+}
+
+#endif //TRI_COLLISION_PROFILING
+
+/////////////////////// btQuantizedBvhTree /////////////////////////////////
+
+void btQuantizedBvhTree::calc_quantization(
+	BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin)
+{
+	//calc globa box
+	btAABB global_bound;
+	global_bound.invalidate();
+
+	for (int i=0;i<primitive_boxes.size() ;i++ )
+	{
+		global_bound.merge(primitive_boxes[i].m_bound);
+	}
+
+	bt_calc_quantization_parameters(
+		m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin);
+
+}
+
+
+
+int btQuantizedBvhTree::_calc_splitting_axis(
+	BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+{
+
+	int i;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+	int numIndices = endIndex-startIndex;
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center-means;
+		diff2 = diff2 * diff2;
+		variance += diff2;
+	}
+	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+
+	return variance.maxAxis();
+}
+
+
+int btQuantizedBvhTree::_sort_and_calc_splitting_index(
+	BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
+	int endIndex, int splitAxis)
+{
+	int i;
+	int splitIndex =startIndex;
+	int numIndices = endIndex - startIndex;
+
+	// average of centers
+	btScalar splitValue = 0.0f;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	splitValue = means[splitAxis];
+
+
+	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		if (center[splitAxis] > splitValue)
+		{
+			//swap
+			primitive_boxes.swap(i,splitIndex);
+			//swapLeafNodes(i,splitIndex);
+			splitIndex++;
+		}
+	}
+
+	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+	//otherwise the tree-building might fail due to stack-overflows in certain cases.
+	//unbalanced1 is unsafe: it can cause stack overflows
+	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+	//unbalanced2 should work too: always use center (perfect balanced trees)
+	//bool unbalanced2 = true;
+
+	//this should be safe too:
+	int rangeBalancedIndices = numIndices/3;
+	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+	if (unbalanced)
+	{
+		splitIndex = startIndex+ (numIndices>>1);
+	}
+
+	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
+	btAssert(!unbal);
+
+	return splitIndex;
+
+}
+
+
+void btQuantizedBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+{
+	int curIndex = m_num_nodes;
+	m_num_nodes++;
+
+	btAssert((endIndex-startIndex)>0);
+
+	if ((endIndex-startIndex)==1)
+	{
+	    //We have a leaf node
+	    setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
+		m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
+
+		return;
+	}
+	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+	//split axis
+	int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+	splitIndex = _sort_and_calc_splitting_index(
+			primitive_boxes,startIndex,endIndex,
+			splitIndex//split axis
+			);
+
+
+	//calc this node bounding box
+
+	btAABB node_bound;
+	node_bound.invalidate();
+
+	for (int i=startIndex;i<endIndex;i++)
+	{
+		node_bound.merge(primitive_boxes[i].m_bound);
+	}
+
+	setNodeBound(curIndex,node_bound);
+
+
+	//build left branch
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+
+	//build right branch
+	 _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+	m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
+
+
+}
+
+//! stackless build tree
+void btQuantizedBvhTree::build_tree(
+	BT_BVH_DATA_ARRAY & primitive_boxes)
+{
+	calc_quantization(primitive_boxes);
+	// initialize node count to 0
+	m_num_nodes = 0;
+	// allocate nodes
+	m_node_array.resize(primitive_boxes.size()*2);
+
+	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+
+////////////////////////////////////class btGImpactQuantizedBvh
+
+void btGImpactQuantizedBvh::refit()
+{
+	int nodecount = getNodeCount();
+	while(nodecount--)
+	{
+		if(isLeafNode(nodecount))
+		{
+			btAABB leafbox;
+			m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
+			setNodeBound(nodecount,leafbox);
+		}
+		else
+		{
+			//const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
+			//get left bound
+			btAABB bound;
+			bound.invalidate();
+
+			btAABB temp_box;
+
+			int child_node = getLeftNode(nodecount);
+			if(child_node)
+			{
+				getNodeBound(child_node,temp_box);
+				bound.merge(temp_box);
+			}
+
+			child_node = getRightNode(nodecount);
+			if(child_node)
+			{
+				getNodeBound(child_node,temp_box);
+				bound.merge(temp_box);
+			}
+
+			setNodeBound(nodecount,bound);
+		}
+	}
+}
+
+//! this rebuild the entire set
+void btGImpactQuantizedBvh::buildSet()
+{
+	//obtain primitive boxes
+	BT_BVH_DATA_ARRAY primitive_boxes;
+	primitive_boxes.resize(m_primitive_manager->get_primitive_count());
+
+	for (int i = 0;i<primitive_boxes.size() ;i++ )
+	{
+		 m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
+		 primitive_boxes[i].m_data = i;
+	}
+
+	m_box_tree.build_tree(primitive_boxes);
+}
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
+{
+	int curIndex = 0;
+	int numNodes = getNodeCount();
+
+	//quantize box
+
+	unsigned short quantizedMin[3];
+	unsigned short quantizedMax[3];
+
+	m_box_tree.quantizePoint(quantizedMin,box.m_min);
+	m_box_tree.quantizePoint(quantizedMax,box.m_max);
+
+
+	while (curIndex < numNodes)
+	{
+
+		//catch bugs in tree data
+
+		bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax);
+		bool isleafnode = isLeafNode(curIndex);
+
+		if (isleafnode && aabbOverlap)
+		{
+			collided_results.push_back(getNodeData(curIndex));
+		}
+
+		if (aabbOverlap || isleafnode)
+		{
+			//next subnode
+			curIndex++;
+		}
+		else
+		{
+			//skip node
+			curIndex+= getEscapeNodeIndex(curIndex);
+		}
+	}
+	if(collided_results.size()>0) return true;
+	return false;
+}
+
+
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactQuantizedBvh::rayQuery(
+	const btVector3 & ray_dir,const btVector3 & ray_origin ,
+	btAlignedObjectArray<int> & collided_results) const
+{
+	int curIndex = 0;
+	int numNodes = getNodeCount();
+
+	while (curIndex < numNodes)
+	{
+		btAABB bound;
+		getNodeBound(curIndex,bound);
+
+		//catch bugs in tree data
+
+		bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+		bool isleafnode = isLeafNode(curIndex);
+
+		if (isleafnode && aabbOverlap)
+		{
+			collided_results.push_back(getNodeData( curIndex));
+		}
+
+		if (aabbOverlap || isleafnode)
+		{
+			//next subnode
+			curIndex++;
+		}
+		else
+		{
+			//skip node
+			curIndex+= getEscapeNodeIndex(curIndex);
+		}
+	}
+	if(collided_results.size()>0) return true;
+	return false;
+}
+
+
+SIMD_FORCE_INLINE bool _quantized_node_collision(
+	btGImpactQuantizedBvh * boxset0, btGImpactQuantizedBvh * boxset1,
+	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	int node0 ,int node1, bool complete_primitive_tests)
+{
+	btAABB box0;
+	boxset0->getNodeBound(node0,box0);
+	btAABB box1;
+	boxset1->getNodeBound(node1,box1);
+
+	return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
+//	box1.appy_transform_trans_cache(trans_cache_1to0);
+//	return box0.has_collision(box1);
+
+}
+
+
+//stackless recursive collision routine
+static void _find_quantized_collision_pairs_recursive(
+	btGImpactQuantizedBvh * boxset0, btGImpactQuantizedBvh * boxset1,
+	btPairSet * collision_pairs,
+	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	int node0, int node1, bool complete_primitive_tests)
+{
+
+
+
+	if( _quantized_node_collision(
+		boxset0,boxset1,trans_cache_1to0,
+		node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
+
+	if(boxset0->isLeafNode(node0))
+	{
+		if(boxset1->isLeafNode(node1))
+		{
+			// collision result
+			collision_pairs->push_pair(
+				boxset0->getNodeData(node0),boxset1->getNodeData(node1));
+			return;
+		}
+		else
+		{
+
+			//collide left recursive
+
+			_find_quantized_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								node0,boxset1->getLeftNode(node1),false);
+
+			//collide right recursive
+			_find_quantized_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								node0,boxset1->getRightNode(node1),false);
+
+
+		}
+	}
+	else
+	{
+		if(boxset1->isLeafNode(node1))
+		{
+
+			//collide left recursive
+			_find_quantized_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								boxset0->getLeftNode(node0),node1,false);
+
+
+			//collide right recursive
+
+			_find_quantized_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								boxset0->getRightNode(node0),node1,false);
+
+
+		}
+		else
+		{
+			//collide left0 left1
+
+
+
+			_find_quantized_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
+
+			//collide left0 right1
+
+			_find_quantized_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
+
+
+			//collide right0 left1
+
+			_find_quantized_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
+
+			//collide right0 right1
+
+			_find_quantized_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
+
+		}// else if node1 is not a leaf
+	}// else if node0 is not a leaf
+}
+
+
+void btGImpactQuantizedBvh::find_collision(btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
+		btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
+		btPairSet & collision_pairs)
+{
+
+	if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
+
+	BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
+
+	trans_cache_1to0.calc_from_homogenic(trans0,trans1);
+
+#ifdef TRI_COLLISION_PROFILING
+	bt_begin_gim02_q_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+	_find_quantized_collision_pairs_recursive(
+		boxset0,boxset1,
+		&collision_pairs,trans_cache_1to0,0,0,true);
+#ifdef TRI_COLLISION_PROFILING
+	bt_end_gim02_q_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+}
+
+

src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h

@@ -0,0 +1,379 @@
+#ifndef GIM_QUANTIZED_SET_H_INCLUDED
+#define GIM_QUANTIZED_SET_H_INCLUDED
+
+/*! \file btGImpactQuantizedBvh.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 "btGImpactBvh.h"
+#include "btQuantization.h"
+
+
+
+/*! \defgroup BOX_TREES
+
+
+
+*/
+//! @{
+
+
+///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+ATTRIBUTE_ALIGNED16	(struct) BT_QUANTIZED_BVH_NODE
+{
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes
+	int	m_escapeIndexOrDataIndex;
+
+	BT_QUANTIZED_BVH_NODE()
+	{
+		m_escapeIndexOrDataIndex = 0;
+	}
+
+	SIMD_FORCE_INLINE bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrDataIndex>=0);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex < 0);
+		return -m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setEscapeIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = -index;
+	}
+
+	SIMD_FORCE_INLINE int getDataIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex >= 0);
+
+		return m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setDataIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = index;
+	}
+
+	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
+		unsigned short * quantizedMin,unsigned short * quantizedMax) const
+	{
+		if(m_quantizedAabbMin[0] > quantizedMax[0] ||
+		   m_quantizedAabbMax[0] < quantizedMin[0] ||
+		   m_quantizedAabbMin[1] > quantizedMax[1] ||
+		   m_quantizedAabbMax[1] < quantizedMin[1] ||
+		   m_quantizedAabbMin[2] > quantizedMax[2] ||
+		   m_quantizedAabbMax[2] < quantizedMin[2])
+		{
+			return false;
+		}
+		return true;
+	}
+
+};
+
+
+
+class BT_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE>
+{
+};
+
+
+
+
+//! Basic Box tree structure
+class btQuantizedBvhTree
+{
+protected:
+	int m_num_nodes;
+	BT_QUANTIZED_BVH_NODE_ARRAY m_node_array;
+	btAABB m_global_bound;
+	btVector3 m_bvhQuantization;
+protected:
+	void calc_quantization(BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) );
+
+	int _sort_and_calc_splitting_index(
+		BT_BVH_DATA_ARRAY & primitive_boxes,
+		 int startIndex,  int endIndex, int splitAxis);
+
+	int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
+
+	void _build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
+public:
+	btQuantizedBvhTree()
+	{
+		m_num_nodes = 0;
+	}
+
+	//! prototype functions for box tree management
+	//!@{
+	void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes);
+
+	SIMD_FORCE_INLINE void quantizePoint(
+		unsigned short * quantizedpoint, const btVector3 & point) const
+	{
+		bt_quantize_clamp(quantizedpoint,point,m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization);
+	}
+
+
+	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
+		int node_index,
+		unsigned short * quantizedMin,unsigned short * quantizedMax) const
+	{
+		return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax);
+	}
+
+	SIMD_FORCE_INLINE void clearNodes()
+	{
+		m_node_array.clear();
+		m_num_nodes = 0;
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE int getNodeCount() const
+	{
+		return m_num_nodes;
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+	{
+		return m_node_array[nodeindex].isLeafNode();
+	}
+
+	SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+	{
+		return m_node_array[nodeindex].getDataIndex();
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
+	{
+		bound.m_min = bt_unquantize(
+			m_node_array[nodeindex].m_quantizedAabbMin,
+			m_global_bound.m_min,m_bvhQuantization);
+
+		bound.m_max = bt_unquantize(
+			m_node_array[nodeindex].m_quantizedAabbMax,
+			m_global_bound.m_min,m_bvhQuantization);
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	{
+		bt_quantize_clamp(	m_node_array[nodeindex].m_quantizedAabbMin,
+							bound.m_min,
+							m_global_bound.m_min,
+							m_global_bound.m_max,
+							m_bvhQuantization);
+
+		bt_quantize_clamp(	m_node_array[nodeindex].m_quantizedAabbMax,
+							bound.m_max,
+							m_global_bound.m_min,
+							m_global_bound.m_max,
+							m_bvhQuantization);
+	}
+
+	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+	{
+		return nodeindex+1;
+	}
+
+	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+	{
+		if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
+		return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
+	}
+
+	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+	{
+		return m_node_array[nodeindex].getEscapeIndex();
+	}
+
+	SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
+	{
+		return &m_node_array[index];
+	}
+
+	//!@}
+};
+
+
+
+//! Structure for containing Boxes
+/*!
+This class offers an structure for managing a box tree of primitives.
+Requires a Primitive prototype (like btPrimitiveManagerBase )
+*/
+class btGImpactQuantizedBvh
+{
+protected:
+	btQuantizedBvhTree m_box_tree;
+	btPrimitiveManagerBase * m_primitive_manager;
+
+protected:
+	//stackless refit
+	void refit();
+public:
+
+	//! this constructor doesn't build the tree. you must call	buildSet
+	btGImpactQuantizedBvh()
+	{
+		m_primitive_manager = NULL;
+	}
+
+	//! this constructor doesn't build the tree. you must call	buildSet
+	btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager)
+	{
+		m_primitive_manager = primitive_manager;
+	}
+
+	SIMD_FORCE_INLINE btAABB getGlobalBox()  const
+	{
+		btAABB totalbox;
+		getNodeBound(0, totalbox);
+		return totalbox;
+	}
+
+	SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
+	{
+		m_primitive_manager = primitive_manager;
+	}
+
+	SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
+	{
+		return m_primitive_manager;
+	}
+
+
+//! node manager prototype functions
+///@{
+
+	//! this attemps to refit the box set.
+	SIMD_FORCE_INLINE void update()
+	{
+		refit();
+	}
+
+	//! this rebuild the entire set
+	void buildSet();
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box,
+		 const btTransform & transform, btAlignedObjectArray<int> & collided_results) const
+	{
+		btAABB transbox=box;
+		transbox.appy_transform(transform);
+		return boxQuery(transbox,collided_results);
+	}
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	bool rayQuery(
+		const btVector3 & ray_dir,const btVector3 & ray_origin ,
+		btAlignedObjectArray<int> & collided_results) const;
+
+	//! tells if this set has hierarcht
+	SIMD_FORCE_INLINE bool hasHierarchy() const
+	{
+		return true;
+	}
+
+	//! tells if this set is a trimesh
+	SIMD_FORCE_INLINE bool isTrimesh()  const
+	{
+		return m_primitive_manager->is_trimesh();
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE int getNodeCount() const
+	{
+		return m_box_tree.getNodeCount();
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+	{
+		return m_box_tree.isLeafNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+	{
+		return m_box_tree.getNodeData(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound)  const
+	{
+		m_box_tree.getNodeBound(nodeindex, bound);
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	{
+		m_box_tree.setNodeBound(nodeindex, bound);
+	}
+
+
+	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+	{
+		return m_box_tree.getLeftNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+	{
+		return m_box_tree.getRightNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+	{
+		return m_box_tree.getEscapeNodeIndex(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
+	{
+		m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
+	}
+
+
+	SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
+	{
+		return m_box_tree.get_node_pointer(index);
+	}
+
+//! @}
+
+	static float getAverageTreeCollisionTime();
+
+
+	static void find_collision(btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
+		btGImpactQuantizedBvh * boxset2, const btTransform & trans2,
+		btPairSet & collision_pairs);
+};
+
+
+#endif // GIM_BOXPRUNING_H_INCLUDED

src/BulletCollision/Gimpact/btGImpactShape.cpp

@@ -0,0 +1,183 @@
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 "btGImpactShape.h"
+#include "btGImpactMassUtil.h"
+
+
+#define CALC_EXACT_INERTIA 1
+
+void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	lockChildShapes();
+#ifdef CALC_EXACT_INERTIA
+	inertia.setValue(0.f,0.f,0.f);
+
+	int i = this->getNumChildShapes();
+	btScalar shapemass = mass/btScalar(i);
+
+	while(i--)
+	{
+		btVector3 temp_inertia;
+		m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia);
+		if(childrenHasTransform())
+		{
+			inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]);
+		}
+		else
+		{
+			inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity());
+		}
+
+	}
+
+#else
+
+	// Calc box inertia
+
+	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
+	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
+	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
+	const btScalar x2 = lx*lx;
+	const btScalar y2 = ly*ly;
+	const btScalar z2 = lz*lz;
+	const btScalar scaledmass = mass * btScalar(0.08333333);
+
+	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+#endif
+	unlockChildShapes();
+}
+
+
+
+void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	lockChildShapes();
+
+
+#ifdef CALC_EXACT_INERTIA
+	inertia.setValue(0.f,0.f,0.f);
+
+	int i = this->getVertexCount();
+	btScalar pointmass = mass/btScalar(i);
+
+	while(i--)
+	{
+		btVector3 pointintertia;
+		this->getVertex(i,pointintertia);
+		pointintertia = gim_get_point_inertia(pointintertia,pointmass);
+		inertia+=pointintertia;
+	}
+
+#else
+
+	// Calc box inertia
+
+	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
+	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
+	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
+	const btScalar x2 = lx*lx;
+	const btScalar y2 = ly*ly;
+	const btScalar z2 = lz*lz;
+	const btScalar scaledmass = mass * btScalar(0.08333333);
+
+	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+#endif
+
+	unlockChildShapes();
+}
+
+void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+
+#ifdef CALC_EXACT_INERTIA
+	inertia.setValue(0.f,0.f,0.f);
+
+	int i = this->getMeshPartCount();
+	btScalar partmass = mass/btScalar(i);
+
+	while(i--)
+	{
+		btVector3 partinertia;
+		getMeshPart(i)->calculateLocalInertia(partmass,partinertia);
+		inertia+=partinertia;
+	}
+
+#else
+
+	// Calc box inertia
+
+	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
+	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
+	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
+	const btScalar x2 = lx*lx;
+	const btScalar y2 = ly*ly;
+	const btScalar z2 = lz*lz;
+	const btScalar scaledmass = mass * btScalar(0.08333333);
+
+	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+#endif
+}
+
+void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
+{
+}
+
+
+void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	lockChildShapes();
+	btAABB box;
+	box.m_min = aabbMin;
+	box.m_max = aabbMax;
+
+	btAlignedObjectArray<int> collided;
+	m_box_set.boxQuery(box,collided);
+
+	if(collided.size()==0)
+	{
+		unlockChildShapes();
+		return;
+	}
+
+	int part = (int)getPart();
+	btPrimitiveTriangle triangle;
+	int i = collided.size();
+	while(i--)
+	{
+		this->getPrimitiveTriangle(collided[i],triangle);
+		callback->processTriangle(triangle.m_vertices,part,collided[i]);
+	}
+	unlockChildShapes();
+
+}
+
+void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	int i = m_mesh_parts.size();
+	while(i--)
+	{
+		m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax);
+	}
+}

src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp

@@ -0,0 +1,283 @@
+/*! \file btGenericPoolAllocator.cpp
+\author Francisco Len N<>jera. email projectileman@yahoo.com
+
+General purpose allocator class
+*/
+/*
+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 "btGenericPoolAllocator.h"
+
+
+
+/// *************** btGenericMemoryPool ******************///////////
+
+size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements)
+{
+	size_t ptr = BT_UINT_MAX;
+
+	if(m_free_nodes_count == 0) return BT_UINT_MAX;
+	// find an avaliable free node with the correct size
+	size_t revindex = m_free_nodes_count;
+
+	while(revindex-- && ptr == BT_UINT_MAX)
+	{
+		if(m_allocated_sizes[m_free_nodes[revindex]]>=num_elements)
+		{
+			ptr = revindex;
+		}
+	}
+	if(ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found
+
+
+	revindex = ptr;
+	ptr = m_free_nodes[revindex];
+	// post: ptr contains the node index, and revindex the index in m_free_nodes
+
+	size_t  finalsize = m_allocated_sizes[ptr];
+	finalsize -= num_elements;
+
+	m_allocated_sizes[ptr] = num_elements;
+
+	// post: finalsize>=0, m_allocated_sizes[ptr] has the requested size
+
+	if(finalsize>0) // preserve free node, there are some free memory
+	{
+		m_free_nodes[revindex] = ptr + num_elements;
+		m_allocated_sizes[ptr + num_elements] = finalsize;
+	}
+	else // delete free node
+	{
+		// swap with end
+		m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count-1];
+		m_free_nodes_count--;
+	}
+
+	return ptr;
+}
+
+size_t btGenericMemoryPool::allocate_from_pool(size_t num_elements)
+{
+	if(m_allocated_count+num_elements>m_max_element_count) return BT_UINT_MAX;
+
+	size_t ptr = m_allocated_count;
+
+	m_allocated_sizes[m_allocated_count] = num_elements;
+	m_allocated_count+=num_elements;
+
+	return ptr;
+}
+
+
+void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count)
+{
+	m_allocated_count = 0;
+	m_free_nodes_count = 0;
+
+	m_element_size = element_size;
+	m_max_element_count = element_count;
+
+
+
+
+	m_pool = (unsigned char *) btAlignedAlloc(m_element_size*m_max_element_count,16);
+	m_free_nodes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16);
+	m_allocated_sizes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16);
+
+	for (size_t i = 0;i< m_max_element_count;i++ )
+	{
+		m_allocated_sizes[i] = 0;
+	}
+}
+
+void btGenericMemoryPool::end_pool()
+{
+	btAlignedFree(m_pool);
+	btAlignedFree(m_free_nodes);
+	btAlignedFree(m_allocated_sizes);
+	m_allocated_count = 0;
+	m_free_nodes_count = 0;
+}
+
+
+//! Allocates memory in pool
+/*!
+\param size_bytes size in bytes of the buffer
+*/
+void * btGenericMemoryPool::allocate(size_t size_bytes)
+{
+
+	size_t module = size_bytes%m_element_size;
+	size_t element_count = size_bytes/m_element_size;
+	if(module>0) element_count++;
+
+	size_t alloc_pos = allocate_from_free_nodes(element_count);
+	// a free node is found
+	if(alloc_pos != BT_UINT_MAX)
+	{
+		return get_element_data(alloc_pos);
+	}
+	// allocate directly on pool
+	alloc_pos = allocate_from_pool(element_count);
+
+	if(alloc_pos == BT_UINT_MAX) return NULL; // not space
+	return get_element_data(alloc_pos);
+}
+
+bool btGenericMemoryPool::freeMemory(void * pointer)
+{
+	unsigned char * pointer_pos = (unsigned char *)pointer;
+	unsigned char * pool_pos = (unsigned char *)m_pool;
+	// calc offset
+	if(pointer_pos<pool_pos) return false;//other pool
+	size_t offset = size_t(pointer_pos - pool_pos);
+	if(offset>=get_pool_capacity()) return false;// far away
+
+	// find free position
+	m_free_nodes[m_free_nodes_count] = offset/m_element_size;
+	m_free_nodes_count++;
+	return true;
+}
+
+
+/// *******************! btGenericPoolAllocator *******************!///
+
+
+btGenericPoolAllocator::~btGenericPoolAllocator()
+{
+	// destroy pools
+	size_t i;
+	for (i=0;i<m_pool_count;i++)
+	{
+		m_pools[i]->end_pool();
+		btAlignedFree(m_pools[i]);
+	}
+}
+
+
+// creates a pool
+btGenericMemoryPool * btGenericPoolAllocator::push_new_pool()
+{
+	if(m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL;
+
+	btGenericMemoryPool * newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool),16);
+
+	m_pools[m_pool_count] = newptr;
+
+	m_pools[m_pool_count]->init_pool(m_pool_element_size,m_pool_element_count);
+
+	m_pool_count++;
+	return newptr;
+}
+
+void * btGenericPoolAllocator::failback_alloc(size_t size_bytes)
+{
+
+	btGenericMemoryPool * pool = NULL;
+
+
+	if(size_bytes<=get_pool_capacity())
+	{
+		pool = 	push_new_pool();
+	}
+
+	if(pool==NULL) // failback
+	{
+		return btAlignedAlloc(size_bytes,16);
+	}
+
+	return pool->allocate(size_bytes);
+}
+
+bool btGenericPoolAllocator::failback_free(void * pointer)
+{
+	btAlignedFree(pointer);
+	return true;
+}
+
+
+//! Allocates memory in pool
+/*!
+\param size_bytes size in bytes of the buffer
+*/
+void * btGenericPoolAllocator::allocate(size_t size_bytes)
+{
+	void * ptr = NULL;
+
+	size_t i = 0;
+	while(i<m_pool_count && ptr == NULL)
+	{
+		ptr = m_pools[i]->allocate(size_bytes);
+		++i;
+	}
+
+	if(ptr) return ptr;
+
+	return failback_alloc(size_bytes);
+}
+
+bool btGenericPoolAllocator::freeMemory(void * pointer)
+{
+	bool result = false;
+
+	size_t i = 0;
+	while(i<m_pool_count && result == false)
+	{
+		result = m_pools[i]->freeMemory(pointer);
+		++i;
+	}
+
+	if(result) return true;
+
+	return failback_free(pointer);
+}
+
+/// ************** STANDARD ALLOCATOR ***************************///
+
+
+#define BT_DEFAULT_POOL_SIZE 32768
+#define BT_DEFAULT_POOL_ELEMENT_SIZE 8
+
+// main allocator
+class BT_STANDARD_ALLOCATOR: public btGenericPoolAllocator
+{
+public:
+	BT_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE)
+	{
+	}
+};
+
+// global allocator
+BT_STANDARD_ALLOCATOR g_main_allocator;
+
+
+void * btPoolAlloc(size_t size)
+{
+	return g_main_allocator.allocate(size);
+}
+
+void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize)
+{
+	void * newptr = btPoolAlloc(newsize);
+    size_t copysize = oldsize<newsize?oldsize:newsize;
+    memcpy(newptr,ptr,copysize);
+    btPoolFree(ptr);
+    return newptr;
+}
+
+void btPoolFree(void *ptr)
+{
+	g_main_allocator.freeMemory(ptr);
+}

src/BulletCollision/Gimpact/btGenericPoolAllocator.h

@@ -0,0 +1,168 @@
+/*! \file btGenericPoolAllocator.h
+\author Francisco Len N<>jera. email projectileman@yahoo.com
+
+General purpose allocator class
+*/
+/*
+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_GENERIC_POOL_ALLOCATOR_H
+#define BT_GENERIC_POOL_ALLOCATOR_H
+
+#include <limits.h>
+#include <stdio.h>
+#include <memory.h>
+#include "LinearMath/btAlignedAllocator.h"
+
+#define BT_UINT_MAX UINT_MAX
+#define BT_DEFAULT_MAX_POOLS 16
+
+
+//! Generic Pool class
+class btGenericMemoryPool
+{
+public:
+	unsigned char * m_pool; //[m_element_size*m_max_element_count];
+	size_t * m_free_nodes; //[m_max_element_count];//! free nodes
+	size_t * m_allocated_sizes;//[m_max_element_count];//! Number of elements allocated per node
+	size_t m_allocated_count;
+	size_t m_free_nodes_count;
+protected:
+	size_t m_element_size;
+	size_t m_max_element_count;
+
+	size_t allocate_from_free_nodes(size_t num_elements);
+	size_t allocate_from_pool(size_t num_elements);
+
+public:
+
+	void init_pool(size_t element_size, size_t element_count);
+
+	void end_pool();
+
+
+	btGenericMemoryPool(size_t element_size, size_t element_count)
+	{
+		init_pool(element_size, element_count);
+	}
+
+	~btGenericMemoryPool()
+	{
+		end_pool();
+	}
+
+
+	inline size_t get_pool_capacity()
+	{
+		return m_element_size*m_max_element_count;
+	}
+
+	inline size_t gem_element_size()
+	{
+		return m_element_size;
+	}
+
+	inline size_t get_max_element_count()
+	{
+		return m_max_element_count;
+	}
+
+	inline size_t get_allocated_count()
+	{
+		return m_allocated_count;
+	}
+
+	inline size_t get_free_positions_count()
+	{
+		return m_free_nodes_count;
+	}
+
+	inline void * get_element_data(size_t element_index)
+	{
+		return &m_pool[element_index*m_element_size];
+	}
+
+	//! Allocates memory in pool
+	/*!
+	\param size_bytes size in bytes of the buffer
+	*/
+	void * allocate(size_t size_bytes);
+
+	bool freeMemory(void * pointer);
+};
+
+
+
+
+//! Generic Allocator with pools
+/*!
+General purpose Allocator which can create Memory Pools dynamiacally as needed.
+*/
+class btGenericPoolAllocator
+{
+protected:
+	size_t m_pool_element_size;
+	size_t m_pool_element_count;
+public:
+	btGenericMemoryPool * m_pools[BT_DEFAULT_MAX_POOLS];
+	size_t m_pool_count;
+
+
+	inline size_t get_pool_capacity()
+	{
+		return m_pool_element_size*m_pool_element_count;
+	}
+
+
+protected:
+	// creates a pool
+	btGenericMemoryPool * push_new_pool();
+
+	void * failback_alloc(size_t size_bytes);
+
+	bool failback_free(void * pointer);
+public:
+
+	btGenericPoolAllocator(size_t pool_element_size, size_t pool_element_count)
+	{
+		m_pool_count = 0;
+		m_pool_element_size = pool_element_size;
+		m_pool_element_count = pool_element_count;
+	}
+
+	virtual ~btGenericPoolAllocator();
+
+	//! Allocates memory in pool
+	/*!
+	\param size_bytes size in bytes of the buffer
+	*/
+	void * allocate(size_t size_bytes);
+
+	bool freeMemory(void * pointer);
+};
+
+
+
+/*! \defgroup POOL_MEMORY_FUNCTIONS
+standar managed Memory functions. Memory pools are used.
+*/
+//! @{
+void * btPoolAlloc(size_t size);
+void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize);
+void btPoolFree(void *ptr);
+//! @}
+
+
+#endif

src/BulletCollision/Gimpact/btGeometryOperations.h

@@ -0,0 +1,216 @@
+#ifndef BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+#define BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+
+/*! \file btGeometryOperations.h
+*\author Francisco Len N<>jera
+
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 "btBoxCollision.h"
+
+
+
+/*! \defgroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+
+#define PLANEDIREPSILON 0.0000001f
+#define PARALELENORMALS 0.000001f
+
+
+#define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
+
+/// Calc a plane from a triangle edge an a normal. plane is a vec4f
+SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 &  e2, const btVector3 & normal,btVector4 & plane)
+{
+	btVector3 planenormal = (e2-e1).cross(normal);
+	planenormal.normalize();
+	plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal));
+}
+
+
+
+//***************** SEGMENT and LINE FUNCTIONS **********************************///
+
+/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
+ */
+SIMD_FORCE_INLINE void bt_closest_point_on_segment(
+	btVector3 & cp, const btVector3 & v,
+	const btVector3  &e1,const btVector3 &e2)
+{
+    btVector3 n = e2-e1;
+    cp = v - e1;
+	btScalar _scalar = cp.dot(n)/n.dot(n);
+	if(_scalar <0.0f)
+	{
+	    cp = e1;
+	}
+	else if(_scalar >1.0f)
+	{
+	    cp = e2;
+	}
+	else
+	{
+		cp = _scalar*n + e1;
+	}
+}
+
+
+//! line plane collision
+/*!
+*\return
+	-0  if the ray never intersects
+	-1 if the ray collides in front
+	-2 if the ray collides in back
+*/
+
+SIMD_FORCE_INLINE int bt_line_plane_collision(
+	const btVector4 & plane,
+	const btVector3 & vDir,
+	const btVector3 & vPoint,
+	btVector3 & pout,
+	btScalar &tparam,
+	btScalar tmin, btScalar tmax)
+{
+
+	btScalar _dotdir = vDir.dot(plane);
+
+	if(btFabs(_dotdir)<PLANEDIREPSILON)
+	{
+		tparam = tmax;
+	    return 0;
+	}
+
+	btScalar _dis = bt_distance_point_plane(plane,vPoint);
+	char returnvalue = _dis<0.0f? 2:1;
+	tparam = -_dis/_dotdir;
+
+	if(tparam<tmin)
+	{
+		returnvalue = 0;
+		tparam = tmin;
+	}
+	else if(tparam>tmax)
+	{
+		returnvalue = 0;
+		tparam = tmax;
+	}
+	pout = tparam*vDir + vPoint;
+	return returnvalue;
+}
+
+
+//! Find closest points on segments
+SIMD_FORCE_INLINE void bt_segment_collision(
+	const btVector3 & vA1,
+	const btVector3 & vA2,
+	const btVector3 & vB1,
+	const btVector3 & vB2,
+	btVector3 & vPointA,
+	btVector3 & vPointB)
+{
+    btVector3 AD = vA2 - vA1;
+    btVector3 BD = vB2 - vB1;
+    btVector3 N = AD.cross(BD);
+    btScalar tp = N.length2();
+
+    btVector4 _M;//plane
+
+    if(tp<SIMD_EPSILON)//ARE PARALELE
+    {
+    	//project B over A
+    	bool invert_b_order = false;
+    	_M[0] = vB1.dot(AD);
+    	_M[1] = vB2.dot(AD);
+
+    	if(_M[0]>_M[1])
+    	{
+    		invert_b_order  = true;
+    		BT_SWAP_NUMBERS(_M[0],_M[1]);
+    	}
+    	_M[2] = vA1.dot(AD);
+    	_M[3] = vA2.dot(AD);
+    	//mid points
+    	N[0] = (_M[0]+_M[1])*0.5f;
+    	N[1] = (_M[2]+_M[3])*0.5f;
+
+    	if(N[0]<N[1])
+    	{
+    		if(_M[1]<_M[2])
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			vPointA = vA1;
+    		}
+    		else if(_M[1]<_M[3])
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
+    		}
+    		else
+    		{
+    			vPointA = vA2;
+    			bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
+    		}
+    	}
+    	else
+    	{
+    		if(_M[3]<_M[0])
+    		{
+    			vPointB = invert_b_order?vB2:vB1;
+    			vPointA = vA2;
+    		}
+    		else if(_M[3]<_M[1])
+    		{
+    			vPointA = vA2;
+    			bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
+    		}
+    		else
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
+    		}
+    	}
+    	return;
+    }
+
+    N = N.cross(BD);
+    _M.setValue(N[0],N[1],N[2],vB1.dot(N));
+
+	// get point A as the plane collision point
+    bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1));
+
+    /*Closest point on segment*/
+    vPointB = vPointA - vB1;
+	tp = vPointB.dot(BD);
+	tp/= BD.dot(BD);
+	tp = BT_CLAMP(tp,0.0f,1.0f);
+
+	vPointB = tp*BD + vB1;
+}
+
+
+
+//! @}
+
+
+#endif // GIM_VECTOR_H_INCLUDED

src/BulletCollision/Gimpact/btQuantization.h

@@ -0,0 +1,92 @@
+#ifndef BT_QUANTIZATION_H_INCLUDED
+#define BT_QUANTIZATION_H_INCLUDED
+
+/*! \file btQuantization.h
+*\author Francisco Len N<>jera
+
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+
+
+
+/*! \defgroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+
+
+SIMD_FORCE_INLINE void bt_calc_quantization_parameters(
+	btVector3 & outMinBound,
+	btVector3 & outMaxBound,
+	btVector3 & bvhQuantization,
+	const btVector3& srcMinBound,const btVector3& srcMaxBound,
+	btScalar quantizationMargin)
+{
+	//enlarge the AABB to avoid division by zero when initializing the quantization values
+	btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
+	outMinBound = srcMinBound - clampValue;
+	outMaxBound = srcMaxBound + clampValue;
+	btVector3 aabbSize = outMaxBound - outMinBound;
+	bvhQuantization = btVector3(btScalar(65535.0),
+								btScalar(65535.0),
+								btScalar(65535.0)) / aabbSize;
+}
+
+
+SIMD_FORCE_INLINE void bt_quantize_clamp(
+	unsigned short* out,
+	const btVector3& point,
+	const btVector3 & min_bound,
+	const btVector3 & max_bound,
+	const btVector3 & bvhQuantization)
+{
+
+	btVector3 clampedPoint(point);
+	clampedPoint.setMax(min_bound);
+	clampedPoint.setMin(max_bound);
+
+	btVector3 v = (clampedPoint - min_bound) * bvhQuantization;
+	out[0] = (unsigned short)(v.getX()+0.5f);
+	out[1] = (unsigned short)(v.getY()+0.5f);
+	out[2] = (unsigned short)(v.getZ()+0.5f);
+}
+
+
+SIMD_FORCE_INLINE btVector3 bt_unquantize(
+	const unsigned short* vecIn,
+	const btVector3 & offset,
+	const btVector3 & bvhQuantization)
+{
+	btVector3	vecOut;
+	vecOut.setValue(
+		(btScalar)(vecIn[0]) / (bvhQuantization.getX()),
+		(btScalar)(vecIn[1]) / (bvhQuantization.getY()),
+		(btScalar)(vecIn[2]) / (bvhQuantization.getZ()));
+	vecOut += offset;
+	return vecOut;
+}
+
+//! @}
+
+
+#endif // GIM_VECTOR_H_INCLUDED

src/BulletCollision/Gimpact/btTriangleShapeEx.cpp

@@ -0,0 +1,218 @@
+/*! \file btGImpactTriangleShape.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+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 "btTriangleShapeEx.h"
+
+
+
+void BT_TRIANGLE_CONTACT::merge_points(const btVector4 & plane,
+                                       btScalar margin, const btVector3 * points, int point_count)
+{
+    m_point_count = 0;
+    m_penetration_depth= -1000.0f;
+
+    int point_indices[MAX_TRI_CLIPPING];
+
+	int _k;
+
+    for ( _k=0;_k<point_count;_k++)
+    {
+        btScalar _dist = - bt_distance_point_plane(plane,points[_k]) + margin;
+
+        if (_dist>=0.0f)
+        {
+            if (_dist>m_penetration_depth)
+            {
+                m_penetration_depth = _dist;
+                point_indices[0] = _k;
+                m_point_count=1;
+            }
+            else if ((_dist+SIMD_EPSILON)>=m_penetration_depth)
+            {
+                point_indices[m_point_count] = _k;
+                m_point_count++;
+            }
+        }
+    }
+
+    for ( _k=0;_k<m_point_count;_k++)
+    {
+        m_points[_k] = points[point_indices[_k]];
+    }
+}
+
+///class btPrimitiveTriangle
+bool btPrimitiveTriangle::overlap_test_conservative(const btPrimitiveTriangle& other)
+{
+    btScalar total_margin = m_margin + other.m_margin;
+    // classify points on other triangle
+    btScalar dis0 = bt_distance_point_plane(m_plane,other.m_vertices[0]) - total_margin;
+
+    btScalar dis1 = bt_distance_point_plane(m_plane,other.m_vertices[1]) - total_margin;
+
+    btScalar dis2 = bt_distance_point_plane(m_plane,other.m_vertices[2]) - total_margin;
+
+    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+    // classify points on this triangle
+    dis0 = bt_distance_point_plane(other.m_plane,m_vertices[0]) - total_margin;
+
+    dis1 = bt_distance_point_plane(other.m_plane,m_vertices[1]) - total_margin;
+
+    dis2 = bt_distance_point_plane(other.m_plane,m_vertices[2]) - total_margin;
+
+    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+    return true;
+}
+
+int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points )
+{
+    // edge 0
+
+    btVector3 temp_points[MAX_TRI_CLIPPING];
+
+
+    btVector4 edgeplane;
+
+    get_edge_plane(0,edgeplane);
+
+
+    int clipped_count = bt_plane_clip_triangle(
+                            edgeplane,other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],temp_points);
+
+    if (clipped_count == 0) return 0;
+
+    btVector3 temp_points1[MAX_TRI_CLIPPING];
+
+
+    // edge 1
+    get_edge_plane(1,edgeplane);
+
+
+    clipped_count = bt_plane_clip_polygon(edgeplane,temp_points,clipped_count,temp_points1);
+
+    if (clipped_count == 0) return 0;
+
+    // edge 2
+    get_edge_plane(2,edgeplane);
+
+    clipped_count = bt_plane_clip_polygon(
+                        edgeplane,temp_points1,clipped_count,clipped_points);
+
+    return clipped_count;
+}
+
+bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts)
+{
+    btScalar margin = m_margin + other.m_margin;
+
+    btVector3 clipped_points[MAX_TRI_CLIPPING];
+    int clipped_count;
+    //create planes
+    // plane v vs U points
+
+    BT_TRIANGLE_CONTACT contacts1;
+
+    contacts1.m_separating_normal = m_plane;
+
+
+    clipped_count = clip_triangle(other,clipped_points);
+
+    if (clipped_count == 0 )
+    {
+        return false;//Reject
+    }
+
+    //find most deep interval face1
+    contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
+    if (contacts1.m_point_count == 0) return false; // too far
+    //Normal pointing to this triangle
+    contacts1.m_separating_normal *= -1.f;
+
+
+    //Clip tri1 by tri2 edges
+    BT_TRIANGLE_CONTACT contacts2;
+    contacts2.m_separating_normal = other.m_plane;
+
+    clipped_count = other.clip_triangle(*this,clipped_points);
+
+    if (clipped_count == 0 )
+    {
+        return false;//Reject
+    }
+
+    //find most deep interval face1
+    contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
+    if (contacts2.m_point_count == 0) return false; // too far
+
+
+
+
+    ////check most dir for contacts
+    if (contacts2.m_penetration_depth<contacts1.m_penetration_depth)
+    {
+        contacts.copy_from(contacts2);
+    }
+    else
+    {
+        contacts.copy_from(contacts1);
+    }
+    return true;
+}
+
+
+
+///class btTriangleShapeEx: public btTriangleShape
+
+bool btTriangleShapeEx::overlap_test_conservative(const btTriangleShapeEx& other)
+{
+    btScalar total_margin = getMargin() + other.getMargin();
+
+    btVector4 plane0;
+    buildTriPlane(plane0);
+    btVector4 plane1;
+    other.buildTriPlane(plane1);
+
+    // classify points on other triangle
+    btScalar dis0 = bt_distance_point_plane(plane0,other.m_vertices1[0]) - total_margin;
+
+    btScalar dis1 = bt_distance_point_plane(plane0,other.m_vertices1[1]) - total_margin;
+
+    btScalar dis2 = bt_distance_point_plane(plane0,other.m_vertices1[2]) - total_margin;
+
+    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+    // classify points on this triangle
+    dis0 = bt_distance_point_plane(plane1,m_vertices1[0]) - total_margin;
+
+    dis1 = bt_distance_point_plane(plane1,m_vertices1[1]) - total_margin;
+
+    dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - total_margin;
+
+    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+    return true;
+}
+
+

src/BulletCollision/Gimpact/btTriangleShapeEx.h

@@ -0,0 +1,180 @@
+/*! \file btGImpactShape.h
+\author Francisco Len N<>jera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef TRIANGLE_SHAPE_EX_H
+#define TRIANGLE_SHAPE_EX_H
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "btBoxCollision.h"
+#include "btClipPolygon.h"
+#include "btGeometryOperations.h"
+
+
+#define MAX_TRI_CLIPPING 16
+
+//! Structure for collision
+struct BT_TRIANGLE_CONTACT
+{
+    btScalar m_penetration_depth;
+    int m_point_count;
+    btVector4 m_separating_normal;
+    btVector3 m_points[MAX_TRI_CLIPPING];
+
+	SIMD_FORCE_INLINE void copy_from(const BT_TRIANGLE_CONTACT& other)
+	{
+		m_penetration_depth = other.m_penetration_depth;
+		m_separating_normal = other.m_separating_normal;
+		m_point_count = other.m_point_count;
+		int i = m_point_count;
+		while(i--)
+		{
+			m_points[i] = other.m_points[i];
+		}
+	}
+
+	BT_TRIANGLE_CONTACT()
+	{
+	}
+
+	BT_TRIANGLE_CONTACT(const BT_TRIANGLE_CONTACT& other)
+	{
+		copy_from(other);
+	}
+
+    //! classify points that are closer
+    void merge_points(const btVector4 & plane,
+    				btScalar margin, const btVector3 * points, int point_count);
+
+};
+
+
+
+class btPrimitiveTriangle
+{
+public:
+	btVector3 m_vertices[3];
+	btVector4 m_plane;
+	btScalar m_margin;
+	btScalar m_dummy;
+	btPrimitiveTriangle():m_margin(0.01f)
+	{
+
+	}
+
+
+	SIMD_FORCE_INLINE void buildTriPlane()
+	{
+		btVector3 normal = (m_vertices[1]-m_vertices[0]).cross(m_vertices[2]-m_vertices[0]);
+		normal.normalize();
+		m_plane.setValue(normal[0],normal[1],normal[2],m_vertices[0].dot(normal));
+	}
+
+	//! Test if triangles could collide
+	bool overlap_test_conservative(const btPrimitiveTriangle& other);
+
+	//! Calcs the plane which is paralele to the edge and perpendicular to the triangle plane
+	/*!
+	\pre this triangle must have its plane calculated.
+	*/
+	SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4 &plane)  const
+    {
+		const btVector3 & e0 = m_vertices[edge_index];
+		const btVector3 & e1 = m_vertices[(edge_index+1)%3];
+		bt_edge_plane(e0,e1,m_plane,plane);
+    }
+
+    void applyTransform(const btTransform& t)
+	{
+		m_vertices[0] = t(m_vertices[0]);
+		m_vertices[1] = t(m_vertices[1]);
+		m_vertices[2] = t(m_vertices[2]);
+	}
+
+	//! Clips the triangle against this
+	/*!
+	\pre clipped_points must have MAX_TRI_CLIPPING size, and this triangle must have its plane calculated.
+	\return the number of clipped points
+	*/
+    int clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points );
+
+	//! Find collision using the clipping method
+	/*!
+	\pre this triangle and other must have their triangles calculated
+	*/
+    bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts);
+};
+
+
+
+//! Helper class for colliding Bullet Triangle Shapes
+/*!
+This class implements a better getAabb method than the previous btTriangleShape class
+*/
+class btTriangleShapeEx: public btTriangleShape
+{
+public:
+
+	btTriangleShapeEx():btTriangleShape(btVector3(0,0,0),btVector3(0,0,0),btVector3(0,0,0))
+	{
+	}
+
+	btTriangleShapeEx(const btVector3& p0,const btVector3& p1,const btVector3& p2):	btTriangleShape(p0,p1,p2)
+	{
+	}
+
+	btTriangleShapeEx(const btTriangleShapeEx & other):	btTriangleShape(other.m_vertices1[0],other.m_vertices1[1],other.m_vertices1[2])
+	{
+	}
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const
+	{
+		btVector3 tv0 = t(m_vertices1[0]);
+		btVector3 tv1 = t(m_vertices1[1]);
+		btVector3 tv2 = t(m_vertices1[2]);
+
+		btAABB trianglebox(tv0,tv1,tv2,m_collisionMargin);
+		aabbMin = trianglebox.m_min;
+		aabbMax = trianglebox.m_max;
+	}
+
+	void applyTransform(const btTransform& t)
+	{
+		m_vertices1[0] = t(m_vertices1[0]);
+		m_vertices1[1] = t(m_vertices1[1]);
+		m_vertices1[2] = t(m_vertices1[2]);
+	}
+
+	SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const
+	{
+		btVector3 normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
+		normal.normalize();
+		plane.setValue(normal[0],normal[1],normal[2],m_vertices1[0].dot(normal));
+	}
+
+	bool overlap_test_conservative(const btTriangleShapeEx& other);
+};
+
+
+#endif //TRIANGLE_MESH_SHAPE_H

src/BulletCollision/Gimpact/gim_array.h

@@ -0,0 +1,332 @@
+#ifndef GIM_ARRAY_H_INCLUDED
+#define GIM_ARRAY_H_INCLUDED
+/*! \file gim_array.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_memory.h"
+
+/*! \addtogroup CONTAINERS
+\brief
+Abstract class for template containers
+*/
+//! @{
+
+#define GIM_ARRAY_GROW_INCREMENT 2
+#define GIM_ARRAY_GROW_FACTOR 2
+
+//!	Very simple array container with fast access and simd memory
+template<typename T>
+class gim_array
+{
+public:
+//! properties
+//!@{
+    T *m_data;
+    GUINT m_size;
+    GUINT m_allocated_size;
+//!@}
+//! protected operations
+//!@{
+
+    inline void destroyData()
+	{
+	    m_allocated_size = 0;
+		if(m_data==NULL) return;
+		gim_free(m_data);
+		m_data = NULL;
+	}
+
+	inline bool resizeData(GUINT newsize)
+	{
+		if(newsize==0)
+		{
+			destroyData();
+			return true;
+		}
+
+		if(m_size>0)
+		{
+            m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T));
+		}
+		else
+		{
+		    m_data = (T*)gim_alloc(newsize*sizeof(T));
+		}
+		m_allocated_size = newsize;
+		return true;
+	}
+
+	inline bool growingCheck()
+	{
+		if(m_allocated_size<=m_size)
+		{
+		    GUINT requestsize = m_size;
+		    m_size = m_allocated_size;
+			if(resizeData((requestsize+GIM_ARRAY_GROW_INCREMENT)*GIM_ARRAY_GROW_FACTOR)==false) return false;
+		}
+		return true;
+	}
+
+//!@}
+//! public operations
+//!@{
+    inline  bool reserve(GUINT size)
+    {
+        if(m_allocated_size>=size) return false;
+        return resizeData(size);
+    }
+
+    inline void clear_range(GUINT start_range)
+    {
+        while(m_size>start_range)
+        {
+            m_data[--m_size].~T();
+        }
+    }
+
+    inline void clear()
+    {
+        if(m_size==0)return;
+        clear_range(0);
+    }
+
+    inline void clear_memory()
+    {
+        clear();
+        destroyData();
+    }
+
+    gim_array()
+    {
+        m_data = 0;
+        m_size = 0;
+        m_allocated_size = 0;
+    }
+
+    gim_array(GUINT reservesize)
+    {
+        m_data = 0;
+        m_size = 0;
+
+        m_allocated_size = 0;
+        reserve(reservesize);
+    }
+
+    ~gim_array()
+    {
+        clear_memory();
+    }
+
+    inline GUINT size() const
+    {
+        return m_size;
+    }
+
+    inline GUINT max_size() const
+    {
+        return m_allocated_size;
+    }
+
+    inline T & operator[](size_t i)
+	{
+		return m_data[i];
+	}
+	inline  const T & operator[](size_t i) const
+	{
+		return m_data[i];
+	}
+
+    inline T * pointer(){ return m_data;}
+    inline const T * pointer() const
+    { return m_data;}
+
+
+    inline T * get_pointer_at(GUINT i)
+	{
+		return m_data + i;
+	}
+
+	inline const T * get_pointer_at(GUINT i) const
+	{
+		return m_data + i;
+	}
+
+	inline T & at(GUINT i)
+	{
+		return m_data[i];
+	}
+
+	inline const T & at(GUINT i) const
+	{
+		return m_data[i];
+	}
+
+	inline T & front()
+	{
+		return *m_data;
+	}
+
+	inline const T & front() const
+	{
+		return *m_data;
+	}
+
+	inline T & back()
+	{
+		return m_data[m_size-1];
+	}
+
+	inline const T & back() const
+	{
+		return m_data[m_size-1];
+	}
+
+
+	inline void swap(GUINT i, GUINT j)
+	{
+	    gim_swap_elements(m_data,i,j);
+	}
+
+	inline void push_back(const T & obj)
+	{
+	    this->growingCheck();
+	    m_data[m_size] = obj;
+	    m_size++;
+	}
+
+	//!Simply increase the m_size, doesn't call the new element constructor
+	inline void push_back_mem()
+	{
+	    this->growingCheck();
+	    m_size++;
+	}
+
+	inline void push_back_memcpy(const T & obj)
+	{
+	    this->growingCheck();
+	    irr_simd_memcpy(&m_data[m_size],&obj,sizeof(T));
+	    m_size++;
+	}
+
+	inline void pop_back()
+	{
+	    m_size--;
+        m_data[m_size].~T();
+	}
+
+	//!Simply decrease the m_size, doesn't call the deleted element destructor
+	inline void pop_back_mem()
+	{
+	    m_size--;
+	}
+
+    //! fast erase
+	inline void erase(GUINT index)
+	{
+	    if(index<m_size-1)
+	    {
+	        swap(index,m_size-1);
+	    }
+	    pop_back();
+	}
+
+	inline void erase_sorted_mem(GUINT index)
+	{
+	    m_size--;
+	    for(GUINT i = index;i<m_size;i++)
+	    {
+	        gim_simd_memcpy(m_data+i,m_data+i+1,sizeof(T));
+	    }
+	}
+
+	inline void erase_sorted(GUINT index)
+	{
+	    m_data[index].~T();
+	    erase_sorted_mem(index);
+	}
+
+	inline void insert_mem(GUINT index)
+	{
+	    this->growingCheck();
+	    for(GUINT i = m_size;i>index;i--)
+	    {
+	        gim_simd_memcpy(m_data+i,m_data+i-1,sizeof(T));
+	    }
+	    m_size++;
+	}
+
+	inline void insert(const T & obj,GUINT index)
+	{
+	    insert_mem(index);
+	    m_data[index] = obj;
+	}
+
+	inline void resize(GUINT size, bool call_constructor = true)
+	{
+
+	    if(size>m_size)
+	    {
+            reserve(size);
+            if(call_constructor)
+            {
+            	T obj;
+                while(m_size<size)
+                {
+                    m_data[m_size] = obj;
+                    m_size++;
+                }
+            }
+            else
+            {
+            	m_size = size;
+            }
+	    }
+	    else if(size<m_size)
+	    {
+	        if(call_constructor) clear_range(size);
+	        m_size = size;
+	    }
+	}
+
+	inline void refit()
+	{
+	    resizeData(m_size);
+	}
+//!@}
+};
+
+
+//! @}
+
+
+
+#endif // GIM_CONTAINERS_H_INCLUDED

src/BulletCollision/Gimpact/gim_basic_geometry_operations.h

@@ -0,0 +1,547 @@
+#ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+#define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+
+/*! \file gim_basic_geometry_operations.h
+*\author Francisco Len N<>jera
+type independant geometry routines
+
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_linear_math.h"
+
+
+
+/*! \defgroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+
+#define PLANEDIREPSILON 0.0000001f
+#define PARALELENORMALS 0.000001f
+
+
+#define TRIANGLE_NORMAL(v1,v2,v3,n)\
+{\
+	vec3f _dif1,_dif2;\
+    VEC_DIFF(_dif1,v2,v1);\
+    VEC_DIFF(_dif2,v3,v1);\
+    VEC_CROSS(n,_dif1,_dif2);\
+    VEC_NORMALIZE(n);\
+}\
+
+#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
+    vec3f _dif1,_dif2; \
+    VEC_DIFF(_dif1,v2,v1); \
+    VEC_DIFF(_dif2,v3,v1); \
+    VEC_CROSS(n,_dif1,_dif2); \
+}\
+
+/// plane is a vec4f
+#define TRIANGLE_PLANE(v1,v2,v3,plane) {\
+    TRIANGLE_NORMAL(v1,v2,v3,plane);\
+    plane[3] = VEC_DOT(v1,plane);\
+}\
+
+/// plane is a vec4f
+#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
+    TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
+    plane[3] = VEC_DOT(v1,plane);\
+}\
+
+/// Calc a plane from an edge an a normal. plane is a vec4f
+#define EDGE_PLANE(e1,e2,n,plane) {\
+    vec3f _dif; \
+    VEC_DIFF(_dif,e2,e1); \
+    VEC_CROSS(plane,_dif,n); \
+    VEC_NORMALIZE(plane); \
+    plane[3] = VEC_DOT(e1,plane);\
+}\
+
+#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])
+
+#define PROJECT_POINT_PLANE(point,plane,projected) {\
+	GREAL _dis;\
+	_dis = DISTANCE_PLANE_POINT(plane,point);\
+	VEC_SCALE(projected,-_dis,plane);\
+	VEC_SUM(projected,projected,point);	\
+}\
+
+//! Verifies if a point is in the plane hull
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE bool POINT_IN_HULL(
+	const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
+{
+	GREAL _dis;
+	for (GUINT _i = 0;_i< plane_count;++_i)
+	{
+		_dis = DISTANCE_PLANE_POINT(planes[_i],point);
+	    if(_dis>0.0f) return false;
+	}
+	return true;
+}
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
+	const CLASS_POINT& s1,
+	const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
+{
+	GREAL _dis1,_dis2;
+	_dis1 = DISTANCE_PLANE_POINT(plane,s1);
+	VEC_DIFF(clipped,s2,s1);
+	_dis2 = VEC_DOT(clipped,plane);
+	VEC_SCALE(clipped,-_dis1/_dis2,clipped);
+	VEC_SUM(clipped,clipped,s1);
+}
+
+enum ePLANE_INTERSECTION_TYPE
+{
+	G_BACK_PLANE = 0,
+	G_COLLIDE_PLANE,
+	G_FRONT_PLANE
+};
+
+enum eLINE_PLANE_INTERSECTION_TYPE
+{
+	G_FRONT_PLANE_S1 = 0,
+	G_FRONT_PLANE_S2,
+	G_BACK_PLANE_S1,
+	G_BACK_PLANE_S2,
+	G_COLLIDE_PLANE_S1,
+	G_COLLIDE_PLANE_S2
+};
+
+//! Confirms if the plane intersect the edge or nor
+/*!
+intersection type must have the following values
+<ul>
+<li> 0 : Segment in front of plane, s1 closest
+<li> 1 : Segment in front of plane, s2 closest
+<li> 2 : Segment in back of plane, s1 closest
+<li> 3 : Segment in back of plane, s2 closest
+<li> 4 : Segment collides plane, s1 in back
+<li> 5 : Segment collides plane, s2 in back
+</ul>
+*/
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
+	const CLASS_POINT& s1,
+	const CLASS_POINT &s2,
+	const CLASS_PLANE &plane,CLASS_POINT &clipped)
+{
+	GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
+	GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
+	if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
+	{
+	    if(_dis1<_dis2) return G_FRONT_PLANE_S1;
+	    return G_FRONT_PLANE_S2;
+	}
+	else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
+	{
+	    if(_dis1>_dis2) return G_BACK_PLANE_S1;
+	    return G_BACK_PLANE_S2;
+	}
+
+	VEC_DIFF(clipped,s2,s1);
+	_dis2 = VEC_DOT(clipped,plane);
+	VEC_SCALE(clipped,-_dis1/_dis2,clipped);
+	VEC_SUM(clipped,clipped,s1);
+	if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
+	return G_COLLIDE_PLANE_S2;
+}
+
+//! Confirms if the plane intersect the edge or not
+/*!
+clipped1 and clipped2 are the vertices behind the plane.
+clipped1 is the closest
+
+intersection_type must have the following values
+<ul>
+<li> 0 : Segment in front of plane, s1 closest
+<li> 1 : Segment in front of plane, s2 closest
+<li> 2 : Segment in back of plane, s1 closest
+<li> 3 : Segment in back of plane, s2 closest
+<li> 4 : Segment collides plane, s1 in back
+<li> 5 : Segment collides plane, s2 in back
+</ul>
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
+	const CLASS_POINT& s1,
+	const CLASS_POINT &s2,
+	const CLASS_PLANE &plane,
+	CLASS_POINT &clipped1,CLASS_POINT &clipped2)
+{
+	eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
+	switch(intersection_type)
+	{
+	case G_FRONT_PLANE_S1:
+		VEC_COPY(clipped1,s1);
+	    VEC_COPY(clipped2,s2);
+		break;
+	case G_FRONT_PLANE_S2:
+		VEC_COPY(clipped1,s2);
+	    VEC_COPY(clipped2,s1);
+		break;
+	case G_BACK_PLANE_S1:
+		VEC_COPY(clipped1,s1);
+	    VEC_COPY(clipped2,s2);
+		break;
+	case G_BACK_PLANE_S2:
+		VEC_COPY(clipped1,s2);
+	    VEC_COPY(clipped2,s1);
+		break;
+	case G_COLLIDE_PLANE_S1:
+		VEC_COPY(clipped2,s1);
+		break;
+	case G_COLLIDE_PLANE_S2:
+		VEC_COPY(clipped2,s2);
+		break;
+	}
+	return intersection_type;
+}
+
+
+//! Finds the 2 smallest cartesian coordinates of a plane normal
+#define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
+
+//! Ray plane collision in one way
+/*!
+Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
+It uses the PLANEDIREPSILON constant.
+*/
+template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
+	const CLASS_PLANE & plane,
+	const CLASS_POINT & vDir,
+	const CLASS_POINT & vPoint,
+	CLASS_POINT & pout,T &tparam)
+{
+	GREAL _dis,_dotdir;
+	_dotdir = VEC_DOT(plane,vDir);
+	if(_dotdir<PLANEDIREPSILON)
+	{
+	    return false;
+	}
+	_dis = DISTANCE_PLANE_POINT(plane,vPoint);
+	tparam = -_dis/_dotdir;
+	VEC_SCALE(pout,tparam,vDir);
+	VEC_SUM(pout,vPoint,pout);
+	return true;
+}
+
+//! line collision
+/*!
+*\return
+	-0  if the ray never intersects
+	-1 if the ray collides in front
+	-2 if the ray collides in back
+*/
+template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
+	const CLASS_PLANE & plane,
+	const CLASS_POINT & vDir,
+	const CLASS_POINT & vPoint,
+	CLASS_POINT & pout,
+	T &tparam,
+	T tmin, T tmax)
+{
+	GREAL _dis,_dotdir;
+	_dotdir = VEC_DOT(plane,vDir);
+	if(btFabs(_dotdir)<PLANEDIREPSILON)
+	{
+		tparam = tmax;
+	    return 0;
+	}
+	_dis = DISTANCE_PLANE_POINT(plane,vPoint);
+	char returnvalue = _dis<0.0f?2:1;
+	tparam = -_dis/_dotdir;
+
+	if(tparam<tmin)
+	{
+		returnvalue = 0;
+		tparam = tmin;
+	}
+	else if(tparam>tmax)
+	{
+		returnvalue = 0;
+		tparam = tmax;
+	}
+
+	VEC_SCALE(pout,tparam,vDir);
+	VEC_SUM(pout,vPoint,pout);
+	return returnvalue;
+}
+
+/*! \brief Returns the Ray on which 2 planes intersect if they do.
+    Written by Rodrigo Hernandez on ODE convex collision
+
+  \param p1 Plane 1
+  \param p2 Plane 2
+  \param p Contains the origin of the ray upon returning if planes intersect
+  \param d Contains the direction of the ray upon returning if planes intersect
+  \return true if the planes intersect, 0 if paralell.
+
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE bool INTERSECT_PLANES(
+		const CLASS_PLANE &p1,
+		const CLASS_PLANE &p2,
+		CLASS_POINT &p,
+		CLASS_POINT &d)
+{
+	VEC_CROSS(d,p1,p2);
+  	GREAL denom = VEC_DOT(d, d);
+  	if(GIM_IS_ZERO(denom)) return false;
+	vec3f _n;
+	_n[0]=p1[3]*p2[0] - p2[3]*p1[0];
+	_n[1]=p1[3]*p2[1] - p2[3]*p1[1];
+	_n[2]=p1[3]*p2[2] - p2[3]*p1[2];
+	VEC_CROSS(p,_n,d);
+	p[0]/=denom;
+	p[1]/=denom;
+	p[2]/=denom;
+	return true;
+}
+
+//***************** SEGMENT and LINE FUNCTIONS **********************************///
+
+/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
+ */
+template<typename CLASS_POINT>
+SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
+	CLASS_POINT & cp, const CLASS_POINT & v,
+	const CLASS_POINT &e1,const CLASS_POINT &e2)
+{
+    vec3f _n;
+    VEC_DIFF(_n,e2,e1);
+    VEC_DIFF(cp,v,e1);
+	GREAL _scalar = VEC_DOT(cp, _n);
+	_scalar/= VEC_DOT(_n, _n);
+	if(_scalar <0.0f)
+	{
+	    VEC_COPY(cp,e1);
+	}
+	else if(_scalar >1.0f)
+	{
+	    VEC_COPY(cp,e2);
+	}
+	else
+	{
+        VEC_SCALE(cp,_scalar,_n);
+        VEC_SUM(cp,cp,e1);
+	}
+}
+
+
+/*! \brief Finds the line params where these lines intersect.
+
+\param dir1 Direction of line 1
+\param point1 Point of line 1
+\param dir2 Direction of line 2
+\param point2 Point of line 2
+\param t1 Result Parameter for line 1
+\param t2 Result Parameter for line 2
+\param dointersect  0  if the lines won't intersect, else 1
+
+*/
+template<typename T,typename CLASS_POINT>
+SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
+	const CLASS_POINT & dir1,
+	CLASS_POINT & point1,
+	const CLASS_POINT & dir2,
+	CLASS_POINT &  point2,
+	T& t1,T& t2)
+{
+    GREAL det;
+	GREAL e1e1 = VEC_DOT(dir1,dir1);
+	GREAL e1e2 = VEC_DOT(dir1,dir2);
+	GREAL e2e2 = VEC_DOT(dir2,dir2);
+	vec3f p1p2;
+    VEC_DIFF(p1p2,point1,point2);
+    GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
+	GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
+	det = e1e2*e1e2 - e1e1*e2e2;
+	if(GIM_IS_ZERO(det)) return false;
+	t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
+	t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
+	return true;
+}
+
+//! Find closest points on segments
+template<typename CLASS_POINT>
+SIMD_FORCE_INLINE void SEGMENT_COLLISION(
+	const CLASS_POINT & vA1,
+	const CLASS_POINT & vA2,
+	const CLASS_POINT & vB1,
+	const CLASS_POINT & vB2,
+	CLASS_POINT & vPointA,
+	CLASS_POINT & vPointB)
+{
+    CLASS_POINT _AD,_BD,_N;
+    vec4f _M;//plane
+    VEC_DIFF(_AD,vA2,vA1);
+    VEC_DIFF(_BD,vB2,vB1);
+    VEC_CROSS(_N,_AD,_BD);
+    GREAL _tp = VEC_DOT(_N,_N);
+    if(_tp<G_EPSILON)//ARE PARALELE
+    {
+    	//project B over A
+    	bool invert_b_order = false;
+    	_M[0] = VEC_DOT(vB1,_AD);
+    	_M[1] = VEC_DOT(vB2,_AD);
+    	if(_M[0]>_M[1])
+    	{
+    		invert_b_order  = true;
+    		GIM_SWAP_NUMBERS(_M[0],_M[1]);
+    	}
+    	_M[2] = VEC_DOT(vA1,_AD);
+    	_M[3] = VEC_DOT(vA2,_AD);
+    	//mid points
+    	_N[0] = (_M[0]+_M[1])*0.5f;
+    	_N[1] = (_M[2]+_M[3])*0.5f;
+
+    	if(_N[0]<_N[1])
+    	{
+    		if(_M[1]<_M[2])
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			vPointA = vA1;
+    		}
+    		else if(_M[1]<_M[3])
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
+    		}
+    		else
+    		{
+    			vPointA = vA2;
+    			CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
+    		}
+    	}
+    	else
+    	{
+    		if(_M[3]<_M[0])
+    		{
+    			vPointB = invert_b_order?vB2:vB1;
+    			vPointA = vA2;
+    		}
+    		else if(_M[3]<_M[1])
+    		{
+    			vPointA = vA2;
+    			CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
+    		}
+    		else
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
+    		}
+    	}
+    	return;
+    }
+
+
+    VEC_CROSS(_M,_N,_BD);
+    _M[3] = VEC_DOT(_M,vB1);
+
+    LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
+    /*Closest point on segment*/
+    VEC_DIFF(vPointB,vPointA,vB1);
+	_tp = VEC_DOT(vPointB, _BD);
+	_tp/= VEC_DOT(_BD, _BD);
+	_tp = GIM_CLAMP(_tp,0.0f,1.0f);
+    VEC_SCALE(vPointB,_tp,_BD);
+    VEC_SUM(vPointB,vPointB,vB1);
+}
+
+
+
+
+//! Line box intersection in one dimension
+/*!
+
+*\param pos Position of the ray
+*\param dir Projection of the Direction of the ray
+*\param bmin Minimum bound of the box
+*\param bmax Maximum bound of the box
+*\param tfirst the minimum projection. Assign to 0 at first.
+*\param tlast the maximum projection. Assign to INFINITY at first.
+*\return true if there is an intersection.
+*/
+template<typename T>
+SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
+{
+	if(GIM_IS_ZERO(dir))
+	{
+        return !(pos < bmin || pos > bmax);
+	}
+	GREAL a0 = (bmin - pos) / dir;
+	GREAL a1 = (bmax - pos) / dir;
+	if(a0 > a1)   GIM_SWAP_NUMBERS(a0, a1);
+	tfirst = GIM_MAX(a0, tfirst);
+	tlast = GIM_MIN(a1, tlast);
+	if (tlast < tfirst) return false;
+	return true;
+}
+
+
+//! Sorts 3 componets
+template<typename T>
+SIMD_FORCE_INLINE void SORT_3_INDICES(
+		const T * values,
+		GUINT * order_indices)
+{
+	//get minimum
+	order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
+
+	//get second and third
+	GUINT i0 = (order_indices[0] + 1)%3;
+	GUINT i1 = (i0 + 1)%3;
+
+	if(values[i0] < values[i1])
+	{
+		order_indices[1] = i0;
+		order_indices[2] = i1;
+	}
+	else
+	{
+		order_indices[1] = i1;
+		order_indices[2] = i0;
+	}
+}
+
+
+
+//! @}
+
+
+#endif // GIM_VECTOR_H_INCLUDED

src/BulletCollision/Gimpact/gim_bitset.h

@@ -0,0 +1,129 @@
+#ifndef GIM_BITSET_H_INCLUDED
+#define GIM_BITSET_H_INCLUDED
+/*! \file gim_bitset.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_array.h"
+
+/*! \addtogroup CONTAINERS
+\brief
+Abstract class for template containers
+*/
+//! @{
+
+#define GUINT_BIT_COUNT 32
+#define GUINT_EXPONENT 5
+
+class gim_bitset
+{
+public:
+    gim_array<GUINT> m_container;
+
+    gim_bitset()
+    {
+
+    }
+
+    gim_bitset(GUINT bits_count)
+    {
+        resize(bits_count);
+    }
+
+    ~gim_bitset()
+    {
+    }
+
+	inline bool resize(GUINT newsize)
+	{
+		GUINT oldsize = m_container.size();
+		m_container.resize(newsize/GUINT_BIT_COUNT + 1,false);
+		while(oldsize<m_container.size())
+		{
+			m_container[oldsize] = 0;
+		}
+		return true;
+	}
+
+	inline GUINT size()
+	{
+		return m_container.size()*GUINT_BIT_COUNT;
+	}
+
+	inline void set_all()
+	{
+		for(GUINT i = 0;i<m_container.size();++i)
+		{
+			m_container[i] = 0xffffffff;
+		}
+	}
+
+	inline void clear_all()
+	{
+	    for(GUINT i = 0;i<m_container.size();++i)
+		{
+			m_container[i] = 0;
+		}
+	}
+
+	inline void set(GUINT bit_index)
+	{
+		if(bit_index>=size())
+		{
+			resize(bit_index);
+		}
+		m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT-1)));
+	}
+
+	///Return 0 or 1
+	inline char get(GUINT bit_index)
+	{
+		if(bit_index>=size())
+		{
+			return 0;
+		}
+		char value = m_container[bit_index >> GUINT_EXPONENT] &
+					 (1 << (bit_index & (GUINT_BIT_COUNT-1)));
+		return value;
+	}
+
+	inline void clear(GUINT bit_index)
+	{
+	    m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT-1)));
+	}
+};
+
+
+//! @}
+
+
+
+#endif // GIM_CONTAINERS_H_INCLUDED

src/BulletCollision/Gimpact/gim_box_collision.h

@@ -0,0 +1,594 @@
+#ifndef GIM_BOX_COLLISION_H_INCLUDED
+#define GIM_BOX_COLLISION_H_INCLUDED
+
+/*! \file gim_box_collision.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+#include "gim_basic_geometry_operations.h"
+#include "LinearMath/btTransform.h"
+
+/*! \defgroup BOUND_AABB_OPERATIONS
+*/
+//! @{
+
+
+//SIMD_FORCE_INLINE bool test_cross_edge_box(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, const btVector3 & extend,
+//	int dir_index0,
+//	int dir_index1
+//	int component_index0,
+//	int component_index1)
+//{
+//	// dir coords are -z and y
+//
+//	const btScalar dir0 = -edge[dir_index0];
+//	const btScalar dir1 = edge[dir_index1];
+//	btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
+//	btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
+//	//find minmax
+//	if(pmin>pmax)
+//	{
+//		GIM_SWAP_NUMBERS(pmin,pmax);
+//	}
+//	//find extends
+//	const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
+//					extend[component_index1] * absolute_edge[dir_index1];
+//
+//	if(pmin>rad || -rad>pmax) return false;
+//	return true;
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
+//}
+//
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
+//}
+
+#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
+{\
+	const btScalar dir0 = -edge[i_dir_0];\
+	const btScalar dir1 = edge[i_dir_1];\
+	btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
+	btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
+	if(pmin>pmax)\
+	{\
+		GIM_SWAP_NUMBERS(pmin,pmax); \
+	}\
+	const btScalar abs_dir0 = absolute_edge[i_dir_0];\
+	const btScalar abs_dir1 = absolute_edge[i_dir_1];\
+	const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
+	if(pmin>rad || -rad>pmax) return false;\
+}\
+
+
+#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
+}\
+
+
+
+//!  Class for transforming a model1 to the space of model0
+class GIM_BOX_BOX_TRANSFORM_CACHE
+{
+public:
+    btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
+	btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
+	btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
+
+	SIMD_FORCE_INLINE void calc_absolute_matrix()
+	{
+		static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
+		m_AR[0] = vepsi + m_R1to0[0].absolute();
+		m_AR[1] = vepsi + m_R1to0[1].absolute();
+		m_AR[2] = vepsi + m_R1to0[2].absolute();
+	}
+
+	GIM_BOX_BOX_TRANSFORM_CACHE()
+	{
+	}
+
+
+	GIM_BOX_BOX_TRANSFORM_CACHE(mat4f  trans1_to_0)
+	{
+		COPY_MATRIX_3X3(m_R1to0,trans1_to_0)
+        MAT_GET_TRANSLATION(trans1_to_0,m_T1to0)
+		calc_absolute_matrix();
+	}
+
+	//! Calc the transformation relative  1 to 0. Inverts matrics by transposing
+	SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
+	{
+
+		m_R1to0 = trans0.getBasis().transpose();
+		m_T1to0 = m_R1to0 * (-trans0.getOrigin());
+
+		m_T1to0 += m_R1to0*trans1.getOrigin();
+		m_R1to0 *= trans1.getBasis();
+
+		calc_absolute_matrix();
+	}
+
+	//! Calcs the full invertion of the matrices. Useful for scaling matrices
+	SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
+	{
+		m_R1to0 = trans0.getBasis().inverse();
+		m_T1to0 = m_R1to0 * (-trans0.getOrigin());
+
+		m_T1to0 += m_R1to0*trans1.getOrigin();
+		m_R1to0 *= trans1.getBasis();
+
+		calc_absolute_matrix();
+	}
+
+	SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point)
+	{
+		return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(),
+			m_R1to0[1].dot(point) + m_T1to0.y(),
+			m_R1to0[2].dot(point) + m_T1to0.z());
+	}
+};
+
+
+#define BOX_PLANE_EPSILON 0.000001f
+
+//! Axis aligned box
+class GIM_AABB
+{
+public:
+	btVector3 m_min;
+	btVector3 m_max;
+
+	GIM_AABB()
+	{}
+
+
+	GIM_AABB(const btVector3 & V1,
+			 const btVector3 & V2,
+			 const btVector3 & V3)
+	{
+		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+	}
+
+	GIM_AABB(const btVector3 & V1,
+			 const btVector3 & V2,
+			 const btVector3 & V3,
+			 GREAL margin)
+	{
+		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	GIM_AABB(const GIM_AABB &other):
+		m_min(other.m_min),m_max(other.m_max)
+	{
+	}
+
+	GIM_AABB(const GIM_AABB &other,btScalar margin ):
+		m_min(other.m_min),m_max(other.m_max)
+	{
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	SIMD_FORCE_INLINE void invalidate()
+	{
+		m_min[0] = G_REAL_INFINITY;
+		m_min[1] = G_REAL_INFINITY;
+		m_min[2] = G_REAL_INFINITY;
+		m_max[0] = -G_REAL_INFINITY;
+		m_max[1] = -G_REAL_INFINITY;
+		m_max[2] = -G_REAL_INFINITY;
+	}
+
+	SIMD_FORCE_INLINE void increment_margin(btScalar margin)
+	{
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	SIMD_FORCE_INLINE void copy_with_margin(const GIM_AABB &other, btScalar margin)
+	{
+		m_min[0] = other.m_min[0] - margin;
+		m_min[1] = other.m_min[1] - margin;
+		m_min[2] = other.m_min[2] - margin;
+
+		m_max[0] = other.m_max[0] + margin;
+		m_max[1] = other.m_max[1] + margin;
+		m_max[2] = other.m_max[2] + margin;
+	}
+
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void calc_from_triangle(
+							const CLASS_POINT & V1,
+							const CLASS_POINT & V2,
+							const CLASS_POINT & V3)
+	{
+		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+	}
+
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void calc_from_triangle_margin(
+							const CLASS_POINT & V1,
+							const CLASS_POINT & V2,
+							const CLASS_POINT & V3, btScalar margin)
+	{
+		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	//! Apply a transform to an AABB
+	SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
+	{
+		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 extends = m_max - center;
+		// Compute new center
+		center = trans(center);
+
+		btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()),
+ 				 extends.dot(trans.getBasis().getRow(1).absolute()),
+				 extends.dot(trans.getBasis().getRow(2).absolute()));
+
+		m_min = center - textends;
+		m_max = center + textends;
+	}
+
+	//! Merges a Box
+	SIMD_FORCE_INLINE void merge(const GIM_AABB & box)
+	{
+		m_min[0] = GIM_MIN(m_min[0],box.m_min[0]);
+		m_min[1] = GIM_MIN(m_min[1],box.m_min[1]);
+		m_min[2] = GIM_MIN(m_min[2],box.m_min[2]);
+
+		m_max[0] = GIM_MAX(m_max[0],box.m_max[0]);
+		m_max[1] = GIM_MAX(m_max[1],box.m_max[1]);
+		m_max[2] = GIM_MAX(m_max[2],box.m_max[2]);
+	}
+
+	//! Merges a point
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
+	{
+		m_min[0] = GIM_MIN(m_min[0],point[0]);
+		m_min[1] = GIM_MIN(m_min[1],point[1]);
+		m_min[2] = GIM_MIN(m_min[2],point[2]);
+
+		m_max[0] = GIM_MAX(m_max[0],point[0]);
+		m_max[1] = GIM_MAX(m_max[1],point[1]);
+		m_max[2] = GIM_MAX(m_max[2],point[2]);
+	}
+
+	//! Gets the extend and center
+	SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
+	{
+		center = (m_max+m_min)*0.5f;
+		extend = m_max - center;
+	}
+
+	//! Finds the intersecting box between this box and the other.
+	SIMD_FORCE_INLINE void find_intersection(const GIM_AABB & other, GIM_AABB & intersection)  const
+	{
+		intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]);
+		intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]);
+		intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]);
+
+		intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]);
+		intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]);
+		intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]);
+	}
+
+
+	SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const
+	{
+		if(m_min[0] > other.m_max[0] ||
+		   m_max[0] < other.m_min[0] ||
+		   m_min[1] > other.m_max[1] ||
+		   m_max[1] < other.m_min[1] ||
+		   m_min[2] > other.m_max[2] ||
+		   m_max[2] < other.m_min[2])
+		{
+			return false;
+		}
+		return true;
+	}
+
+	/*! \brief Finds the Ray intersection parameter.
+	\param aabb Aligned box
+	\param vorigin A vec3f with the origin of the ray
+	\param vdir A vec3f with the direction of the ray
+	*/
+	SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)
+	{
+		btVector3 extents,center;
+		this->get_center_extend(center,extents);;
+
+		btScalar Dx = vorigin[0] - center[0];
+		if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)	return false;
+		btScalar Dy = vorigin[1] - center[1];
+		if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)	return false;
+		btScalar Dz = vorigin[2] - center[2];
+		if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)	return false;
+
+
+		btScalar f = vdir[1] * Dz - vdir[2] * Dy;
+		if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
+		f = vdir[2] * Dx - vdir[0] * Dz;
+		if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
+		f = vdir[0] * Dy - vdir[1] * Dx;
+		if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
+		return true;
+	}
+
+
+	SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
+	{
+		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 extend = m_max-center;
+
+		btScalar _fOrigin =  direction.dot(center);
+		btScalar _fMaximumExtent = extend.dot(direction.absolute());
+		vmin = _fOrigin - _fMaximumExtent;
+		vmax = _fOrigin + _fMaximumExtent;
+	}
+
+	SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
+	{
+		btScalar _fmin,_fmax;
+		this->projection_interval(plane,_fmin,_fmax);
+
+		if(plane[3] > _fmax + BOX_PLANE_EPSILON)
+		{
+			return G_BACK_PLANE; // 0
+		}
+
+		if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
+		{
+			return G_COLLIDE_PLANE; //1
+		}
+		return G_FRONT_PLANE;//2
+	}
+
+	SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0)
+	{
+		GIM_AABB tbox = box;
+		tbox.appy_transform(trans1_to_0);
+		return has_collision(tbox);
+	}
+
+	//! transcache is the transformation cache from box to this AABB
+	SIMD_FORCE_INLINE bool overlapping_trans_cache(
+		const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest)
+	{
+
+		//Taken from OPCODE
+		btVector3 ea,eb;//extends
+		btVector3 ca,cb;//extends
+		get_center_extend(ca,ea);
+		box.get_center_extend(cb,eb);
+
+
+		btVector3 T;
+		btScalar t,t2;
+		int i;
+
+		// Class I : A's basis vectors
+		for(i=0;i<3;i++)
+		{
+			T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
+			t = transcache.m_AR[i].dot(eb) + ea[i];
+			if(GIM_GREATER(T[i], t))	return false;
+		}
+		// Class II : B's basis vectors
+		for(i=0;i<3;i++)
+		{
+			t = MAT_DOT_COL(transcache.m_R1to0,T,i);
+			t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i];
+			if(GIM_GREATER(t,t2))	return false;
+		}
+		// Class III : 9 cross products
+		if(fulltest)
+		{
+			int j,m,n,o,p,q,r;
+			for(i=0;i<3;i++)
+			{
+				m = (i+1)%3;
+				n = (i+2)%3;
+				o = i==0?1:0;
+				p = i==2?1:2;
+				for(j=0;j<3;j++)
+				{
+					q = j==2?1:2;
+					r = j==0?1:0;
+					t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
+					t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
+						eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
+					if(GIM_GREATER(t,t2))	return false;
+				}
+			}
+		}
+		return true;
+	}
+
+	//! Simple test for planes.
+	SIMD_FORCE_INLINE bool collide_plane(
+		const btVector4 & plane)
+	{
+		ePLANE_INTERSECTION_TYPE classify = plane_classify(plane);
+		return (classify == G_COLLIDE_PLANE);
+	}
+
+	//! test for a triangle, with edges
+	SIMD_FORCE_INLINE bool collide_triangle_exact(
+		const btVector3 & p1,
+		const btVector3 & p2,
+		const btVector3 & p3,
+		const btVector4 & triangle_plane)
+	{
+		if(!collide_plane(triangle_plane)) return false;
+
+		btVector3 center,extends;
+		this->get_center_extend(center,extends);
+
+		const btVector3 v1(p1 - center);
+		const btVector3 v2(p2 - center);
+		const btVector3 v3(p3 - center);
+
+		//First axis
+		btVector3 diff(v2 - v1);
+		btVector3 abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+
+
+		diff = v3 - v2;
+		abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+
+		diff = v1 - v3;
+		abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+
+		return true;
+	}
+};
+
+
+//! Compairison of transformation objects
+SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
+{
+	if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
+
+	if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
+	if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
+	if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
+	return true;
+}
+
+
+//! @}
+
+#endif // GIM_BOX_COLLISION_H_INCLUDED

src/BulletCollision/Gimpact/gim_box_set.cpp

@@ -0,0 +1,182 @@
+
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_box_set.h"
+
+
+GUINT GIM_BOX_TREE::_calc_splitting_axis(
+	gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex)
+{
+	GUINT i;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+	GUINT numIndices = endIndex-startIndex;
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center-means;
+		diff2 = diff2 * diff2;
+		variance += diff2;
+	}
+	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+
+	return variance.maxAxis();
+}
+
+
+GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index(
+	gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,
+	GUINT endIndex, GUINT splitAxis)
+{
+	GUINT i;
+	GUINT splitIndex =startIndex;
+	GUINT numIndices = endIndex - startIndex;
+
+	// average of centers
+	btScalar splitValue = 0.0f;
+	for (i=startIndex;i<endIndex;i++)
+	{
+		splitValue+= 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
+					 primitive_boxes[i].m_bound.m_min[splitAxis]);
+	}
+	splitValue /= (btScalar)numIndices;
+
+	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btScalar center = 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
+					 primitive_boxes[i].m_bound.m_min[splitAxis]);
+		if (center > splitValue)
+		{
+			//swap
+			primitive_boxes.swap(i,splitIndex);
+			splitIndex++;
+		}
+	}
+
+	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+	//otherwise the tree-building might fail due to stack-overflows in certain cases.
+	//unbalanced1 is unsafe: it can cause stack overflows
+	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+	//unbalanced2 should work too: always use center (perfect balanced trees)
+	//bool unbalanced2 = true;
+
+	//this should be safe too:
+	GUINT rangeBalancedIndices = numIndices/3;
+	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+	if (unbalanced)
+	{
+		splitIndex = startIndex+ (numIndices>>1);
+	}
+
+	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
+	btAssert(!unbal);
+
+	return splitIndex;
+}
+
+
+void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex)
+{
+	GUINT current_index = m_num_nodes++;
+
+	btAssert((endIndex-startIndex)>0);
+
+	if((endIndex-startIndex) == 1) //we got a leaf
+	{		
+		m_node_array[current_index].m_left = 0;
+		m_node_array[current_index].m_right = 0;
+		m_node_array[current_index].m_escapeIndex = 0;
+
+		m_node_array[current_index].m_bound = primitive_boxes[startIndex].m_bound;
+		m_node_array[current_index].m_data = primitive_boxes[startIndex].m_data;
+		return;
+	}
+
+	//configure inner node
+
+	GUINT splitIndex;
+
+	//calc this node bounding box
+	m_node_array[current_index].m_bound.invalidate();	
+	for (splitIndex=startIndex;splitIndex<endIndex;splitIndex++)
+	{
+		m_node_array[current_index].m_bound.merge(primitive_boxes[splitIndex].m_bound);
+	}
+
+	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+	//split axis
+	splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+	splitIndex = _sort_and_calc_splitting_index(
+			primitive_boxes,startIndex,endIndex,splitIndex);
+
+	//configure this inner node : the left node index
+	m_node_array[current_index].m_left = m_num_nodes;
+	//build left child tree
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+	//configure this inner node : the right node index
+	m_node_array[current_index].m_right = m_num_nodes;
+
+	//build right child tree
+	_build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+	//configure this inner node : the escape index
+	m_node_array[current_index].m_escapeIndex  = m_num_nodes - current_index;
+}
+
+//! stackless build tree
+void GIM_BOX_TREE::build_tree(
+	gim_array<GIM_AABB_DATA> & primitive_boxes)
+{
+	// initialize node count to 0
+	m_num_nodes = 0;
+	// allocate nodes
+	m_node_array.resize(primitive_boxes.size()*2);
+	
+	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+

src/BulletCollision/Gimpact/gim_box_set.h

@@ -0,0 +1,678 @@
+#ifndef GIM_BOX_SET_H_INCLUDED
+#define GIM_BOX_SET_H_INCLUDED
+
+/*! \file gim_box_set.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_array.h"
+#include "gim_radixsort.h"
+#include "gim_box_collision.h"
+#include "gim_tri_collision.h"
+
+
+/*! \defgroup BOX_PRUNNING
+
+
+
+*/
+//! @{
+
+//! Overlapping pair
+struct GIM_PAIR
+{
+    GUINT m_index1;
+    GUINT m_index2;
+    GIM_PAIR()
+    {}
+
+    GIM_PAIR(const GIM_PAIR & p)
+    {
+    	m_index1 = p.m_index1;
+    	m_index2 = p.m_index2;
+	}
+
+	GIM_PAIR(GUINT index1, GUINT index2)
+    {
+    	m_index1 = index1;
+    	m_index2 = index2;
+	}
+};
+
+//! A pairset array
+class gim_pair_set: public gim_array<GIM_PAIR>
+{
+public:
+	gim_pair_set():gim_array<GIM_PAIR>(32)
+	{
+	}
+	inline void push_pair(GUINT index1,GUINT index2)
+	{
+		push_back(GIM_PAIR(index1,index2));
+	}
+
+	inline void push_pair_inv(GUINT index1,GUINT index2)
+	{
+		push_back(GIM_PAIR(index2,index1));
+	}
+};
+
+
+//! Prototype Base class for primitive classification
+/*!
+This class is a wrapper for primitive collections.
+This tells relevant info for the Bounding Box set classes, which take care of space classification.
+This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the  Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons.
+*/
+class GIM_PRIMITIVE_MANAGER_PROTOTYPE
+{
+public:
+
+	//! determines if this manager consist on only triangles, which special case will be optimized
+	virtual bool is_trimesh() = 0;
+	virtual GUINT get_primitive_count() = 0;
+	virtual void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) = 0;
+	virtual void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) = 0;
+};
+
+
+struct GIM_AABB_DATA
+{
+	GIM_AABB m_bound;
+	GUINT m_data;
+};
+
+//! Node Structure for trees
+struct GIM_BOX_TREE_NODE
+{
+	GIM_AABB m_bound;
+	GUINT m_left;//!< Left subtree
+	GUINT m_right;//!< Right subtree
+	GUINT m_escapeIndex;//!< Scape index for traversing
+	GUINT m_data;//!< primitive index if apply
+
+	GIM_BOX_TREE_NODE()
+	{
+	    m_left = 0;
+	    m_right = 0;
+	    m_escapeIndex = 0;
+	    m_data = 0;
+	}
+
+	SIMD_FORCE_INLINE bool is_leaf_node() const
+	{
+	    return  (!m_left && !m_right);
+	}
+};
+
+//! Basic Box tree structure
+class GIM_BOX_TREE
+{
+protected:
+	GUINT m_num_nodes;
+	gim_array<GIM_BOX_TREE_NODE> m_node_array;
+protected:
+	GUINT _sort_and_calc_splitting_index(
+		gim_array<GIM_AABB_DATA> & primitive_boxes,
+		 GUINT startIndex,  GUINT endIndex, GUINT splitAxis);
+
+	GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex);
+
+	void _build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex);
+public:
+	GIM_BOX_TREE()
+	{
+		m_num_nodes = 0;
+	}
+
+	//! prototype functions for box tree management
+	//!@{
+	void build_tree(gim_array<GIM_AABB_DATA> & primitive_boxes);
+
+	SIMD_FORCE_INLINE void clearNodes()
+	{
+		m_node_array.clear();
+		m_num_nodes = 0;
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE GUINT getNodeCount() const
+	{
+		return m_num_nodes;
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const
+	{
+		return m_node_array[nodeindex].is_leaf_node();
+	}
+
+	SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const
+	{
+		return m_node_array[nodeindex].m_data;
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const
+	{
+		bound = m_node_array[nodeindex].m_bound;
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
+	{
+		m_node_array[nodeindex].m_bound = bound;
+	}
+
+	SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex)  const
+	{
+		return m_node_array[nodeindex].m_left;
+	}
+
+	SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex)  const
+	{
+		return m_node_array[nodeindex].m_right;
+	}
+
+	SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const
+	{
+		return m_node_array[nodeindex].m_escapeIndex;
+	}
+
+	//!@}
+};
+
+
+//! Generic Box Tree Template
+/*!
+This class offers an structure for managing a box tree of primitives.
+Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and
+a Box tree structure ( like GIM_BOX_TREE).
+*/
+template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE>
+class GIM_BOX_TREE_TEMPLATE_SET
+{
+protected:
+	_GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager;
+	_GIM_BOX_TREE_PROTOTYPE m_box_tree;
+protected:
+	//stackless refit
+	SIMD_FORCE_INLINE void refit()
+	{
+		GUINT nodecount = getNodeCount();
+		while(nodecount--)
+		{
+			if(isLeafNode(nodecount))
+			{
+				GIM_AABB leafbox;
+				m_primitive_manager.get_primitive_box(getNodeData(nodecount),leafbox);
+				setNodeBound(nodecount,leafbox);
+			}
+			else
+			{
+				//get left bound
+				GUINT childindex = getLeftNodeIndex(nodecount);
+				GIM_AABB bound;
+				getNodeBound(childindex,bound);
+				//get right bound
+				childindex = getRightNodeIndex(nodecount);
+				GIM_AABB bound2;
+				getNodeBound(childindex,bound2);
+				bound.merge(bound2);
+
+				setNodeBound(nodecount,bound);
+			}
+		}
+	}
+public:
+
+	GIM_BOX_TREE_TEMPLATE_SET()
+	{
+	}
+
+	SIMD_FORCE_INLINE GIM_AABB getGlobalBox()  const
+	{
+		GIM_AABB totalbox;
+		getNodeBound(0, totalbox);
+		return totalbox;
+	}
+
+	SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & primitive_manager)
+	{
+		m_primitive_manager = primitive_manager;
+	}
+
+	const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const
+	{
+		return m_primitive_manager;
+	}
+
+	_GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager()
+	{
+		return m_primitive_manager;
+	}
+
+//! node manager prototype functions
+///@{
+
+	//! this attemps to refit the box set.
+	SIMD_FORCE_INLINE void update()
+	{
+		refit();
+	}
+
+	//! this rebuild the entire set
+	SIMD_FORCE_INLINE void buildSet()
+	{
+		//obtain primitive boxes
+		gim_array<GIM_AABB_DATA> primitive_boxes;
+		primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false);
+
+		for (GUINT i = 0;i<primitive_boxes.size() ;i++ )
+		{
+			 m_primitive_manager.get_primitive_box(i,primitive_boxes[i].m_bound);
+			 primitive_boxes[i].m_data = i;
+		}
+
+		m_box_tree.build_tree(primitive_boxes);
+	}
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB & box, gim_array<GUINT> & collided_results) const
+	{
+		GUINT curIndex = 0;
+		GUINT numNodes = getNodeCount();
+
+		while (curIndex < numNodes)
+		{
+			GIM_AABB bound;
+			getNodeBound(curIndex,bound);
+
+			//catch bugs in tree data
+
+			bool aabbOverlap = bound.has_collision(box);
+			bool isleafnode = isLeafNode(curIndex);
+
+			if (isleafnode && aabbOverlap)
+			{
+				collided_results.push_back(getNodeData(curIndex));
+			}
+
+			if (aabbOverlap || isleafnode)
+			{
+				//next subnode
+				curIndex++;
+			}
+			else
+			{
+				//skip node
+				curIndex+= getScapeNodeIndex(curIndex);
+			}
+		}
+		if(collided_results.size()>0) return true;
+		return false;
+	}
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB & box,
+		 const btTransform & transform, gim_array<GUINT> & collided_results) const
+	{
+		GIM_AABB transbox=box;
+		transbox.appy_transform(transform);
+		return boxQuery(transbox,collided_results);
+	}
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	SIMD_FORCE_INLINE bool rayQuery(
+		const btVector3 & ray_dir,const btVector3 & ray_origin ,
+		gim_array<GUINT> & collided_results) const
+	{
+		GUINT curIndex = 0;
+		GUINT numNodes = getNodeCount();
+
+		while (curIndex < numNodes)
+		{
+			GIM_AABB bound;
+			getNodeBound(curIndex,bound);
+
+			//catch bugs in tree data
+
+			bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+			bool isleafnode = isLeafNode(curIndex);
+
+			if (isleafnode && aabbOverlap)
+			{
+				collided_results.push_back(getNodeData( curIndex));
+			}
+
+			if (aabbOverlap || isleafnode)
+			{
+				//next subnode
+				curIndex++;
+			}
+			else
+			{
+				//skip node
+				curIndex+= getScapeNodeIndex(curIndex);
+			}
+		}
+		if(collided_results.size()>0) return true;
+		return false;
+	}
+
+	//! tells if this set has hierarcht
+	SIMD_FORCE_INLINE bool hasHierarchy() const
+	{
+		return true;
+	}
+
+	//! tells if this set is a trimesh
+	SIMD_FORCE_INLINE bool isTrimesh()  const
+	{
+		return m_primitive_manager.is_trimesh();
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE GUINT getNodeCount() const
+	{
+		return m_box_tree.getNodeCount();
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const
+	{
+		return m_box_tree.isLeafNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const
+	{
+		return m_box_tree.getNodeData(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound)  const
+	{
+		m_box_tree.getNodeBound(nodeindex, bound);
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
+	{
+		m_box_tree.setNodeBound(nodeindex, bound);
+	}
+
+	SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const
+	{
+		return m_box_tree.getLeftNodeIndex(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const
+	{
+		return m_box_tree.getRightNodeIndex(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const
+	{
+		return m_box_tree.getScapeNodeIndex(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const
+	{
+		m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle);
+	}
+
+//! @}
+};
+
+//! Class for Box Tree Sets
+/*!
+this has the GIM_BOX_TREE implementation for bounding boxes.
+*/
+template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE>
+class GIM_BOX_TREE_SET: public GIM_BOX_TREE_TEMPLATE_SET< _GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE>
+{
+public:
+
+};
+
+
+
+
+
+/// GIM_BOX_SET collision methods
+template<typename BOX_SET_CLASS0,typename BOX_SET_CLASS1>
+class GIM_TREE_TREE_COLLIDER
+{
+public:
+	gim_pair_set * m_collision_pairs;
+	BOX_SET_CLASS0 * m_boxset0;
+	BOX_SET_CLASS1 * m_boxset1;
+	GUINT current_node0;
+	GUINT current_node1;
+	bool node0_is_leaf;
+	bool node1_is_leaf;
+	bool t0_is_trimesh;
+	bool t1_is_trimesh;
+	bool node0_has_triangle;
+	bool node1_has_triangle;
+	GIM_AABB m_box0;
+	GIM_AABB m_box1;
+	GIM_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
+	btTransform trans_cache_0to1;
+	GIM_TRIANGLE m_tri0;
+	btVector4 m_tri0_plane;
+	GIM_TRIANGLE m_tri1;
+	btVector4 m_tri1_plane;
+
+
+public:
+	GIM_TREE_TREE_COLLIDER()
+	{
+		current_node0 = G_UINT_INFINITY;
+		current_node1 = G_UINT_INFINITY;
+	}
+protected:
+	SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0)
+	{
+		if(node0_has_triangle) return;
+		m_boxset0->getNodeTriangle(node0,m_tri0);
+		//transform triangle
+		m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]);
+		m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]);
+		m_tri0.m_vertices[2] = trans_cache_0to1(m_tri0.m_vertices[2]);
+		m_tri0.get_plane(m_tri0_plane);
+
+		node0_has_triangle = true;
+	}
+
+	SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1)
+	{
+		if(node1_has_triangle) return;
+		m_boxset1->getNodeTriangle(node1,m_tri1);
+		//transform triangle
+		m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]);
+		m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]);
+		m_tri1.m_vertices[2] = trans_cache_1to0.transform(m_tri1.m_vertices[2]);
+		m_tri1.get_plane(m_tri1_plane);
+
+		node1_has_triangle = true;
+	}
+
+	SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0)
+	{
+		if(node0 == current_node0) return;
+		m_boxset0->getNodeBound(node0,m_box0);
+		node0_is_leaf = m_boxset0->isLeafNode(node0);
+		node0_has_triangle = false;
+		current_node0 = node0;
+	}
+
+	SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1)
+	{
+		if(node1 == current_node1) return;
+		m_boxset1->getNodeBound(node1,m_box1);
+		node1_is_leaf = m_boxset1->isLeafNode(node1);
+		node1_has_triangle = false;
+		current_node1 = node1;
+	}
+
+	SIMD_FORCE_INLINE bool node_collision(GUINT node0 ,GUINT node1)
+	{
+		retrieve_node0_info(node0);
+		retrieve_node1_info(node1);
+		bool result = m_box0.overlapping_trans_cache(m_box1,trans_cache_1to0,true);
+		if(!result) return false;
+
+		if(t0_is_trimesh && node0_is_leaf)
+		{
+			//perform primitive vs box collision
+			retrieve_node0_triangle(node0);
+			//do triangle vs box collision
+			m_box1.increment_margin(m_tri0.m_margin);
+
+			result = m_box1.collide_triangle_exact(
+				m_tri0.m_vertices[0],m_tri0.m_vertices[1],m_tri0.m_vertices[2],m_tri0_plane);
+
+			m_box1.increment_margin(-m_tri0.m_margin);
+
+			if(!result) return false;
+			return true;
+		}
+		else if(t1_is_trimesh && node1_is_leaf)
+		{
+			//perform primitive vs box collision
+			retrieve_node1_triangle(node1);
+			//do triangle vs box collision
+			m_box0.increment_margin(m_tri1.m_margin);
+
+			result = m_box0.collide_triangle_exact(
+				m_tri1.m_vertices[0],m_tri1.m_vertices[1],m_tri1.m_vertices[2],m_tri1_plane);
+
+			m_box0.increment_margin(-m_tri1.m_margin);
+
+			if(!result) return false;
+			return true;
+		}
+		return true;
+	}
+
+	//stackless collision routine
+	void find_collision_pairs()
+	{
+		gim_pair_set stack_collisions;
+		stack_collisions.reserve(32);
+
+		//add the first pair
+		stack_collisions.push_pair(0,0);
+
+
+		while(stack_collisions.size())
+		{
+			//retrieve the last pair and pop
+			GUINT node0 = stack_collisions.back().m_index1;
+			GUINT node1 = stack_collisions.back().m_index2;
+			stack_collisions.pop_back();
+			if(node_collision(node0,node1)) // a collision is found
+			{
+				if(node0_is_leaf)
+				{
+					if(node1_is_leaf)
+					{
+						m_collision_pairs->push_pair(m_boxset0->getNodeData(node0),m_boxset1->getNodeData(node1));
+					}
+					else
+					{
+						//collide left
+						stack_collisions.push_pair(node0,m_boxset1->getLeftNodeIndex(node1));
+
+						//collide right
+						stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1));
+					}
+				}
+				else
+				{
+					if(node1_is_leaf)
+					{
+						//collide left
+						stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1);
+						//collide right
+						stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1);
+					}
+					else
+					{
+						GUINT left0 = m_boxset0->getLeftNodeIndex(node0);
+						GUINT right0 = m_boxset0->getRightNodeIndex(node0);
+						GUINT left1 = m_boxset1->getLeftNodeIndex(node1);
+						GUINT right1 = m_boxset1->getRightNodeIndex(node1);
+						//collide left
+						stack_collisions.push_pair(left0,left1);
+						//collide right
+						stack_collisions.push_pair(left0,right1);
+						//collide left
+						stack_collisions.push_pair(right0,left1);
+						//collide right
+						stack_collisions.push_pair(right0,right1);
+
+					}// else if node1 is not a leaf
+				}// else if node0 is not a leaf
+
+			}// if(node_collision(node0,node1))
+		}//while(stack_collisions.size())
+	}
+public:
+	void find_collision(BOX_SET_CLASS0 * boxset1, const btTransform & trans1,
+		BOX_SET_CLASS1 * boxset2, const btTransform & trans2,
+		gim_pair_set & collision_pairs, bool complete_primitive_tests = true)
+	{
+		m_collision_pairs = &collision_pairs;
+		m_boxset0 = boxset1;
+		m_boxset1 = boxset2;
+
+		trans_cache_1to0.calc_from_homogenic(trans1,trans2);
+
+		trans_cache_0to1 =  trans2.inverse();
+		trans_cache_0to1 *= trans1;
+
+
+		if(complete_primitive_tests)
+		{
+			t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh();
+			t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh();
+		}
+		else
+		{
+			t0_is_trimesh = false;
+			t1_is_trimesh = false;
+		}
+
+		find_collision_pairs();
+	}
+};
+
+
+#endif // GIM_BOXPRUNING_H_INCLUDED

src/BulletCollision/Gimpact/gim_clip_polygon.h

@@ -0,0 +1,214 @@
+#ifndef GIM_CLIP_POLYGON_H_INCLUDED
+#define GIM_CLIP_POLYGON_H_INCLUDED
+
+/*! \file gim_tri_collision.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+/*! \addtogroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+//! This function calcs the distance from a 3D plane
+class DISTANCE_PLANE_3D_FUNC
+{
+public:
+	template<typename CLASS_POINT,typename CLASS_PLANE>
+	inline GREAL operator()(const CLASS_PLANE & plane, const CLASS_POINT & point)
+	{
+		return DISTANCE_PLANE_POINT(plane, point);
+	}
+};
+
+
+
+template<typename CLASS_POINT>
+SIMD_FORCE_INLINE void PLANE_CLIP_POLYGON_COLLECT(
+						const CLASS_POINT & point0,
+						const CLASS_POINT & point1,
+						GREAL dist0,
+						GREAL dist1,
+						CLASS_POINT * clipped,
+						GUINT & clipped_count)
+{
+	GUINT _prevclassif = (dist0>G_EPSILON);
+	GUINT _classif = (dist1>G_EPSILON);
+	if(_classif!=_prevclassif)
+	{
+		GREAL blendfactor = -dist0/(dist1-dist0);
+		VEC_BLEND(clipped[clipped_count],point0,point1,blendfactor);
+		clipped_count++;
+	}
+	if(!_classif)
+	{
+		VEC_COPY(clipped[clipped_count],point1);
+		clipped_count++;
+	}
+}
+
+
+//! Clips a polygon by a plane
+/*!
+*\return The count of the clipped counts
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC(
+						const CLASS_PLANE & plane,
+						const CLASS_POINT * polygon_points,
+						GUINT polygon_point_count,
+						CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
+{
+    GUINT clipped_count = 0;
+
+
+    //clip first point
+	GREAL firstdist = distance_func(plane,polygon_points[0]);;
+	if(!(firstdist>G_EPSILON))
+	{
+		VEC_COPY(clipped[clipped_count],polygon_points[0]);
+		clipped_count++;
+	}
+
+	GREAL olddist = firstdist;
+	for(GUINT _i=1;_i<polygon_point_count;_i++)
+	{		
+		GREAL dist = distance_func(plane,polygon_points[_i]);
+
+		PLANE_CLIP_POLYGON_COLLECT(
+						polygon_points[_i-1],polygon_points[_i],
+						olddist,
+						dist,
+						clipped,
+						clipped_count);
+
+
+		olddist = dist;		
+	}
+
+	//RETURN TO FIRST  point	
+
+	PLANE_CLIP_POLYGON_COLLECT(
+					polygon_points[polygon_point_count-1],polygon_points[0],
+					olddist,
+					firstdist,
+					clipped,
+					clipped_count);
+
+	return clipped_count;
+}
+
+//! Clips a polygon by a plane
+/*!
+*\return The count of the clipped counts
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE_GENERIC(
+						const CLASS_PLANE & plane,
+						const CLASS_POINT & point0,
+						const CLASS_POINT & point1,
+						const CLASS_POINT & point2,
+						CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
+{
+    GUINT clipped_count = 0;
+
+    //clip first point
+	GREAL firstdist = distance_func(plane,point0);;
+	if(!(firstdist>G_EPSILON))
+	{
+		VEC_COPY(clipped[clipped_count],point0);
+		clipped_count++;
+	}
+
+	// point 1
+	GREAL olddist = firstdist;
+	GREAL dist = distance_func(plane,point1);
+
+	PLANE_CLIP_POLYGON_COLLECT(
+					point0,point1,
+					olddist,
+					dist,
+					clipped,
+					clipped_count);
+
+	olddist = dist;
+
+
+	// point 2
+	dist = distance_func(plane,point2);
+
+	PLANE_CLIP_POLYGON_COLLECT(
+					point1,point2,
+					olddist,
+					dist,
+					clipped,
+					clipped_count);
+	olddist = dist;
+
+
+
+	//RETURN TO FIRST  point
+	PLANE_CLIP_POLYGON_COLLECT(
+					point2,point0,
+					olddist,
+					firstdist,
+					clipped,
+					clipped_count);
+
+	return clipped_count;
+}
+
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON3D(
+						const CLASS_PLANE & plane,
+						const CLASS_POINT * polygon_points,
+						GUINT polygon_point_count,
+						CLASS_POINT * clipped)
+{
+	return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,polygon_points,polygon_point_count,clipped,DISTANCE_PLANE_3D_FUNC());
+}
+
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D(
+						const CLASS_PLANE & plane,
+						const CLASS_POINT & point0,
+						const CLASS_POINT & point1,
+						const CLASS_POINT & point2,
+						CLASS_POINT * clipped)
+{
+	return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,point0,point1,point2,clipped,DISTANCE_PLANE_3D_FUNC());
+}
+
+
+//! @}
+
+#endif // GIM_TRI_COLLISION_H_INCLUDED

src/BulletCollision/Gimpact/gim_contact.cpp

@@ -0,0 +1,146 @@
+
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_contact.h"
+
+#define MAX_COINCIDENT 8
+
+void gim_contact_array::merge_contacts(
+	const gim_contact_array & contacts, bool normal_contact_average)
+{
+	clear();
+
+	if(contacts.size()==1)
+	{
+		push_back(contacts.back());
+		return;
+	}
+
+	gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size());
+	keycontacts.resize(contacts.size(),false);
+
+	//fill key contacts
+
+	GUINT i;
+
+	for (i = 0;i<contacts.size() ;i++ )
+	{
+		keycontacts[i].m_key = contacts[i].calc_key_contact();
+		keycontacts[i].m_value = i;
+	}
+
+	//sort keys
+	gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR());
+
+	// Merge contacts
+
+	GUINT coincident_count=0;
+	btVector3 coincident_normals[MAX_COINCIDENT];
+
+	GUINT last_key = keycontacts[0].m_key;
+	GUINT key = 0;
+
+	push_back(contacts[keycontacts[0].m_value]);
+	GIM_CONTACT * pcontact = &back();
+
+
+
+	for( i=1;i<keycontacts.size();i++)
+	{
+	    key = keycontacts[i].m_key;
+		const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
+
+		if(last_key ==  key)//same points
+		{
+			//merge contact
+			if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
+			{
+				*pcontact = *scontact;
+                coincident_count = 0;
+			}
+			else if(normal_contact_average)
+			{
+				if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
+                {
+                    if(coincident_count<MAX_COINCIDENT)
+                    {
+                    	coincident_normals[coincident_count] = scontact->m_normal;
+                        coincident_count++;
+                    }
+                }
+			}
+		}
+		else
+		{//add new contact
+
+		    if(normal_contact_average && coincident_count>0)
+		    {
+		    	pcontact->interpolate_normals(coincident_normals,coincident_count);
+		        coincident_count = 0;
+		    }
+
+		    push_back(*scontact);
+		    pcontact = &back();
+        }
+		last_key = key;
+	}
+}
+
+void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts)
+{
+	clear();
+
+	if(contacts.size()==1)
+	{
+		push_back(contacts.back());
+		return;
+	}
+
+	GIM_CONTACT average_contact = contacts.back();
+
+	for (GUINT i=1;i<contacts.size() ;i++ )
+	{
+		average_contact.m_point += contacts[i].m_point;
+		average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
+	}
+
+	//divide
+	GREAL divide_average = 1.0f/((GREAL)contacts.size());
+
+	average_contact.m_point *= divide_average;
+
+	average_contact.m_normal *= divide_average;
+
+	average_contact.m_depth = average_contact.m_normal.length();
+
+	average_contact.m_normal /= average_contact.m_depth;
+
+}
+

src/BulletCollision/Gimpact/gim_contact.h

@@ -0,0 +1,172 @@
+#ifndef GIM_CONTACT_H_INCLUDED
+#define GIM_CONTACT_H_INCLUDED
+
+/*! \file gim_contact.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+#include "gim_geometry.h"
+#include "gim_radixsort.h"
+#include "gim_array.h"
+
+/*! \defgroup CONTACTS
+\brief
+Functions for managing and sorting contacts resulting from a collision query.
+<ul>
+<li> Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST
+<li> After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY
+<li> Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts
+</ul>
+
+*/
+//! @{
+
+/**
+Configuration var for applying interpolation of  contact normals
+*/
+#define NORMAL_CONTACT_AVERAGE 1
+#define CONTACT_DIFF_EPSILON 0.00001f
+
+/// Structure for collision results
+class GIM_CONTACT
+{
+public:
+    btVector3 m_point;
+    btVector3 m_normal;
+    GREAL m_depth;//Positive value indicates interpenetration
+    GREAL m_distance;//Padding not for use
+    GUINT m_feature1;//Face number
+    GUINT m_feature2;//Face number
+public:
+    GIM_CONTACT()
+    {
+    }
+
+    GIM_CONTACT(const GIM_CONTACT & contact):
+				m_point(contact.m_point),
+				m_normal(contact.m_normal),
+				m_depth(contact.m_depth),
+				m_feature1(contact.m_feature1),
+				m_feature2(contact.m_feature2)
+    {
+    	m_point = contact.m_point;
+    	m_normal = contact.m_normal;
+    	m_depth = contact.m_depth;
+    	m_feature1 = contact.m_feature1;
+    	m_feature2 = contact.m_feature2;
+    }
+
+    GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
+    	 			GREAL depth, GUINT feature1, GUINT feature2):
+				m_point(point),
+				m_normal(normal),
+				m_depth(depth),
+				m_feature1(feature1),
+				m_feature2(feature2)
+    {
+    }
+
+	//! Calcs key for coord classification
+    SIMD_FORCE_INLINE GUINT calc_key_contact() const
+    {
+    	GINT _coords[] = {
+    		(GINT)(m_point[0]*1000.0f+1.0f),
+    		(GINT)(m_point[1]*1333.0f),
+    		(GINT)(m_point[2]*2133.0f+3.0f)};
+		GUINT _hash=0;
+		GUINT *_uitmp = (GUINT *)(&_coords[0]);
+		_hash = *_uitmp;
+		_uitmp++;
+		_hash += (*_uitmp)<<4;
+		_uitmp++;
+		_hash += (*_uitmp)<<8;
+		return _hash;
+    }
+
+    SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,GUINT normal_count)
+    {
+    	btVector3 vec_sum(m_normal);
+		for(GUINT i=0;i<normal_count;i++)
+		{
+			vec_sum += normals[i];
+		}
+
+		GREAL vec_sum_len = vec_sum.length2();
+		if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
+
+		GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
+
+		m_normal = vec_sum*vec_sum_len;
+    }
+
+};
+
+
+class gim_contact_array:public gim_array<GIM_CONTACT>
+{
+public:
+	gim_contact_array():gim_array<GIM_CONTACT>(64)
+	{
+	}
+
+	SIMD_FORCE_INLINE void push_contact(const btVector3 &point,const btVector3 & normal,
+    	 			GREAL depth, GUINT feature1, GUINT feature2)
+	{
+		push_back_mem();
+		GIM_CONTACT & newele = back();
+		newele.m_point = point;
+		newele.m_normal = normal;
+		newele.m_depth = depth;
+		newele.m_feature1 = feature1;
+		newele.m_feature2 = feature2;
+	}
+
+	SIMD_FORCE_INLINE void push_triangle_contacts(
+		const GIM_TRIANGLE_CONTACT_DATA & tricontact,
+		GUINT feature1,GUINT feature2)
+	{
+		for(GUINT i = 0;i<tricontact.m_point_count ;i++ )
+		{
+			push_back_mem();
+			GIM_CONTACT & newele = back();
+			newele.m_point = tricontact.m_points[i];
+			newele.m_normal = tricontact.m_separating_normal;
+			newele.m_depth = tricontact.m_penetration_depth;
+			newele.m_feature1 = feature1;
+			newele.m_feature2 = feature2;
+		}
+	}
+
+	void merge_contacts(const gim_contact_array & contacts, bool normal_contact_average = true);
+	void merge_contacts_unique(const gim_contact_array & contacts);
+};
+
+//! @}
+#endif // GIM_CONTACT_H_INCLUDED

src/BulletCollision/Gimpact/gim_geom_types.h

@@ -0,0 +1,102 @@
+#ifndef GIM_GEOM_TYPES_H_INCLUDED
+#define GIM_GEOM_TYPES_H_INCLUDED
+
+/*! \file gim_geom_types.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_math.h"
+
+/*! \defgroup GEOMETRIC_TYPES
+\brief
+Basic types and constants for geometry
+*/
+//! @{
+
+//! Short Integer vector 2D
+typedef GSHORT vec2s[2];
+//! Integer vector 3D
+typedef GSHORT vec3s[3];
+//! Integer vector 4D
+typedef GSHORT vec4s[4];
+
+//! Short Integer vector 2D
+typedef GUSHORT vec2us[2];
+//! Integer vector 3D
+typedef GUSHORT vec3us[3];
+//! Integer vector 4D
+typedef GUSHORT vec4us[4];
+
+//! Integer vector 2D
+typedef GINT vec2i[2];
+//! Integer vector 3D
+typedef GINT vec3i[3];
+//! Integer vector 4D
+typedef GINT vec4i[4];
+
+//! Unsigned Integer vector 2D
+typedef GUINT vec2ui[2];
+//! Unsigned Integer vector 3D
+typedef GUINT vec3ui[3];
+//! Unsigned Integer vector 4D
+typedef GUINT vec4ui[4];
+
+//! Float vector 2D
+typedef GREAL vec2f[2];
+//! Float vector 3D
+typedef GREAL vec3f[3];
+//! Float vector 4D
+typedef GREAL vec4f[4];
+
+//! Double vector 2D
+typedef GREAL2 vec2d[2];
+//! Float vector 3D
+typedef GREAL2 vec3d[3];
+//! Float vector 4D
+typedef GREAL2 vec4d[4];
+
+//! Matrix 2D, row ordered
+typedef GREAL mat2f[2][2];
+//! Matrix 3D, row ordered
+typedef GREAL mat3f[3][3];
+//! Matrix 4D, row ordered
+typedef GREAL mat4f[4][4];
+
+//! Quaternion
+typedef GREAL quatf[4];
+
+//typedef struct _aabb3f aabb3f;
+//! @}
+
+
+
+#endif // GIM_GEOM_TYPES_H_INCLUDED

src/BulletCollision/Gimpact/gim_geometry.h

@@ -0,0 +1,42 @@
+#ifndef GIM_GEOMETRY_H_INCLUDED
+#define GIM_GEOMETRY_H_INCLUDED
+
+/*! \file gim_geometry.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+///Additional Headers for Collision
+#include "gim_basic_geometry_operations.h"
+#include "gim_clip_polygon.h"
+#include "gim_box_collision.h"
+#include "gim_tri_collision.h"
+
+#endif // GIM_VECTOR_H_INCLUDED

src/BulletCollision/Gimpact/gim_hash_table.h

@@ -0,0 +1,920 @@
+#ifndef GIM_HASH_TABLE_H_INCLUDED
+#define GIM_HASH_TABLE_H_INCLUDED
+/*! \file gim_trimesh_data.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_radixsort.h"
+
+/*! \addtogroup CONTAINERS
+\brief
+Abstract class for collision objects
+*/
+//! @{
+
+#define GIM_INVALID_HASH 0xffffffff //!< A very very high value
+#define GIM_DEFAULT_HASH_TABLE_SIZE 380
+#define GIM_DEFAULT_HASH_TABLE_NODE_SIZE 4
+#define GIM_HASH_TABLE_GROW_FACTOR 2
+
+#define GIM_MIN_RADIX_SORT_SIZE 860 //!< calibrated on a PIII
+
+template<typename T>
+struct GIM_HASH_TABLE_NODE
+{
+    GUINT m_key;
+    T m_data;
+    GIM_HASH_TABLE_NODE()
+    {
+    }
+
+    GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE & value)
+    {
+        m_key = value.m_key;
+        m_data = value.m_data;
+    }
+
+    GIM_HASH_TABLE_NODE(GUINT key, const T & data)
+    {
+        m_key = key;
+        m_data = data;
+    }
+
+    bool operator <(const GIM_HASH_TABLE_NODE<T> & other) const
+	{
+		///inverse order, further objects are first
+		if(m_key <  other.m_key) return true;
+		return false;
+	}
+
+	bool operator >(const GIM_HASH_TABLE_NODE<T> & other) const
+	{
+		///inverse order, further objects are first
+		if(m_key >  other.m_key) return true;
+		return false;
+	}
+
+	bool operator ==(const GIM_HASH_TABLE_NODE<T> & other) const
+	{
+		///inverse order, further objects are first
+		if(m_key ==  other.m_key) return true;
+		return false;
+	}
+};
+
+///Macro for getting the key
+class GIM_HASH_NODE_GET_KEY
+{
+public:
+	template<class T>
+	inline GUINT operator()( const T& a)
+	{
+		return a.m_key;
+	}
+};
+
+
+
+///Macro for comparing the key and the element
+class GIM_HASH_NODE_CMP_KEY_MACRO
+{
+public:
+	template<class T>
+	inline int operator() ( const T& a, GUINT key)
+	{
+		return ((int)(a.m_key - key));
+	}
+};
+
+///Macro for comparing Hash nodes
+class GIM_HASH_NODE_CMP_MACRO
+{
+public:
+	template<class T>
+	inline int operator() ( const T& a, const T& b )
+	{
+		return ((int)(a.m_key - b.m_key));
+	}
+};
+
+
+
+
+
+//! Sorting for hash table
+/*!
+switch automatically between quicksort and radixsort
+*/
+template<typename T>
+void gim_sort_hash_node_array(T * array, GUINT array_count)
+{
+    if(array_count<GIM_MIN_RADIX_SORT_SIZE)
+    {
+    	gim_heap_sort(array,array_count,GIM_HASH_NODE_CMP_MACRO());
+    }
+    else
+    {
+    	memcopy_elements_func cmpfunc;
+    	gim_radix_sort(array,array_count,GIM_HASH_NODE_GET_KEY(),cmpfunc);
+    }
+}
+
+
+
+
+
+
+// Note: assumes long is at least 32 bits.
+#define GIM_NUM_PRIME 28
+
+static const GUINT gim_prime_list[GIM_NUM_PRIME] =
+{
+  53ul,         97ul,         193ul,       389ul,       769ul,
+  1543ul,       3079ul,       6151ul,      12289ul,     24593ul,
+  49157ul,      98317ul,      196613ul,    393241ul,    786433ul,
+  1572869ul,    3145739ul,    6291469ul,   12582917ul,  25165843ul,
+  50331653ul,   100663319ul,  201326611ul, 402653189ul, 805306457ul,
+  1610612741ul, 3221225473ul, 4294967291ul
+};
+
+inline GUINT gim_next_prime(GUINT number)
+{
+    //Find nearest upper prime
+    GUINT result_ind = 0;
+    gim_binary_search(gim_prime_list,0,(GIM_NUM_PRIME-2),number,result_ind);
+
+    // inv: result_ind < 28
+    return gim_prime_list[result_ind];
+}
+
+
+
+//! A compact hash table implementation
+/*!
+A memory aligned compact hash table that coud be treated as an array.
+It could be a simple sorted array without the overhead of the hash key bucked, or could
+be a formely hash table with an array of keys.
+You can use switch_to_hashtable() and switch_to_sorted_array for saving space or increase speed.
+</br>
+
+<ul>
+<li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes.
+When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable.
+<li> If node_size != 0, then this container becomes a hash table for ever
+</ul>
+
+*/
+template<class T>
+class gim_hash_table
+{
+protected:
+    typedef GIM_HASH_TABLE_NODE<T> _node_type;
+
+    //!The nodes
+    //array< _node_type, SuperAllocator<_node_type> > m_nodes;
+    gim_array< _node_type > m_nodes;
+    //SuperBufferedArray< _node_type > m_nodes;
+    bool m_sorted;
+
+    /*! \defgroup HASH_TABLE_STRUCTURES
+    \brief
+    Hash table data management. The hash table has the indices to the corresponding m_nodes array
+    */
+    //! @{
+
+    GUINT * m_hash_table;//!<
+    GUINT m_table_size;//!<
+    GUINT m_node_size;//!<
+    GUINT m_min_hash_table_size;
+
+
+
+    //! Returns the cell index
+    inline GUINT _find_cell(GUINT hashkey)
+    {
+        _node_type * nodesptr = m_nodes.pointer();
+        GUINT start_index = (hashkey%m_table_size)*m_node_size;
+        GUINT end_index = start_index + m_node_size;
+
+        while(start_index<end_index)
+        {
+            GUINT value = m_hash_table[start_index];
+            if(value != GIM_INVALID_HASH)
+            {
+                if(nodesptr[value].m_key == hashkey) return start_index;
+            }
+            start_index++;
+        }
+        return GIM_INVALID_HASH;
+    }
+
+    //! Find the avaliable cell for the hashkey, and return an existing cell if it has the same hash key
+    inline GUINT _find_avaliable_cell(GUINT hashkey)
+    {
+        _node_type * nodesptr = m_nodes.pointer();
+        GUINT avaliable_index = GIM_INVALID_HASH;
+        GUINT start_index = (hashkey%m_table_size)*m_node_size;
+        GUINT end_index = start_index + m_node_size;
+
+        while(start_index<end_index)
+        {
+            GUINT value = m_hash_table[start_index];
+            if(value == GIM_INVALID_HASH)
+            {
+                if(avaliable_index==GIM_INVALID_HASH)
+                {
+                    avaliable_index = start_index;
+                }
+            }
+            else if(nodesptr[value].m_key == hashkey)
+            {
+                return start_index;
+            }
+            start_index++;
+        }
+        return avaliable_index;
+    }
+
+
+
+    //! reserves the memory for the hash table.
+    /*!
+    \pre hash table must be empty
+    \post reserves the memory for the hash table, an initializes all elements to GIM_INVALID_HASH.
+    */
+    inline void _reserve_table_memory(GUINT newtablesize)
+    {
+        if(newtablesize==0) return;
+        if(m_node_size==0) return;
+
+        //Get a Prime size
+
+        m_table_size = gim_next_prime(newtablesize);
+
+        GUINT datasize = m_table_size*m_node_size;
+        //Alloc the data buffer
+        m_hash_table =  (GUINT *)gim_alloc(datasize*sizeof(GUINT));
+    }
+
+    inline void _invalidate_keys()
+    {
+        GUINT datasize = m_table_size*m_node_size;
+        for(GUINT i=0;i<datasize;i++)
+        {
+            m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys
+        }
+    }
+
+    //! Clear all memory for the hash table
+    inline void _clear_table_memory()
+    {
+        if(m_hash_table==NULL) return;
+        gim_free(m_hash_table);
+        m_hash_table = NULL;
+        m_table_size = 0;
+    }
+
+    //! Invalidates the keys (Assigning GIM_INVALID_HASH to all) Reorders the hash keys
+    inline void _rehash()
+    {
+        _invalidate_keys();
+
+        _node_type * nodesptr = m_nodes.pointer();
+        for(GUINT i=0;i<(GUINT)m_nodes.size();i++)
+        {
+            GUINT nodekey = nodesptr[i].m_key;
+            if(nodekey != GIM_INVALID_HASH)
+            {
+                //Search for the avaliable cell in buffer
+                GUINT index = _find_avaliable_cell(nodekey);
+
+
+				if(m_hash_table[index]!=GIM_INVALID_HASH)
+				{//The new index is alreade used... discard this new incomming object, repeated key
+				    btAssert(m_hash_table[index]==nodekey);
+					nodesptr[i].m_key = GIM_INVALID_HASH;
+				}
+				else
+				{
+					//;
+					//Assign the value for alloc
+					m_hash_table[index] = i;
+				}
+            }
+        }
+    }
+
+    //! Resize hash table indices
+    inline void _resize_table(GUINT newsize)
+    {
+        //Clear memory
+        _clear_table_memory();
+        //Alloc the data
+        _reserve_table_memory(newsize);
+        //Invalidate keys and rehash
+        _rehash();
+    }
+
+    //! Destroy hash table memory
+    inline void _destroy()
+    {
+        if(m_hash_table==NULL) return;
+        _clear_table_memory();
+    }
+
+    //! Finds an avaliable hash table cell, and resizes the table if there isn't space
+    inline GUINT _assign_hash_table_cell(GUINT hashkey)
+    {
+        GUINT cell_index = _find_avaliable_cell(hashkey);
+
+        if(cell_index==GIM_INVALID_HASH)
+        {
+            //rehashing
+            _resize_table(m_table_size+1);
+            GUINT cell_index = _find_avaliable_cell(hashkey);
+            btAssert(cell_index!=GIM_INVALID_HASH);
+        }
+        return cell_index;
+    }
+
+    //! erase by index in hash table
+    inline bool _erase_by_index_hash_table(GUINT index)
+    {
+        if(index >= m_nodes.size()) return false;
+        if(m_nodes[index].m_key != GIM_INVALID_HASH)
+        {
+            //Search for the avaliable cell in buffer
+            GUINT cell_index = _find_cell(m_nodes[index].m_key);
+
+            btAssert(cell_index!=GIM_INVALID_HASH);
+            btAssert(m_hash_table[cell_index]==index);
+
+            m_hash_table[cell_index] = GIM_INVALID_HASH;
+        }
+
+        return this->_erase_unsorted(index);
+    }
+
+    //! erase by key in hash table
+    inline bool _erase_hash_table(GUINT hashkey)
+    {
+        if(hashkey == GIM_INVALID_HASH) return false;
+
+        //Search for the avaliable cell in buffer
+        GUINT cell_index = _find_cell(hashkey);
+        if(cell_index ==GIM_INVALID_HASH) return false;
+
+        GUINT index = m_hash_table[cell_index];
+        m_hash_table[cell_index] = GIM_INVALID_HASH;
+
+        return this->_erase_unsorted(index);
+    }
+
+
+
+    //! insert an element in hash table
+    /*!
+    If the element exists, this won't insert the element
+    \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
+    If so, the element has been inserted at the last position of the array.
+    */
+    inline GUINT _insert_hash_table(GUINT hashkey, const T & value)
+    {
+        if(hashkey==GIM_INVALID_HASH)
+        {
+            //Insert anyway
+            _insert_unsorted(hashkey,value);
+            return GIM_INVALID_HASH;
+        }
+
+        GUINT cell_index = _assign_hash_table_cell(hashkey);
+
+        GUINT value_key = m_hash_table[cell_index];
+
+        if(value_key!= GIM_INVALID_HASH) return value_key;// Not overrited
+
+        m_hash_table[cell_index] = m_nodes.size();
+
+        _insert_unsorted(hashkey,value);
+        return GIM_INVALID_HASH;
+    }
+
+    //! insert an element in hash table.
+    /*!
+    If the element exists, this replaces the element.
+    \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
+    If so, the element has been inserted at the last position of the array.
+    */
+    inline GUINT _insert_hash_table_replace(GUINT hashkey, const T & value)
+    {
+        if(hashkey==GIM_INVALID_HASH)
+        {
+            //Insert anyway
+            _insert_unsorted(hashkey,value);
+            return GIM_INVALID_HASH;
+        }
+
+        GUINT cell_index = _assign_hash_table_cell(hashkey);
+
+        GUINT value_key = m_hash_table[cell_index];
+
+        if(value_key!= GIM_INVALID_HASH)
+        {//replaces the existing
+            m_nodes[value_key] = _node_type(hashkey,value);
+            return value_key;// index of the replaced element
+        }
+
+        m_hash_table[cell_index] = m_nodes.size();
+
+        _insert_unsorted(hashkey,value);
+        return GIM_INVALID_HASH;
+
+    }
+
+    //! @}
+
+    /*! \defgroup SORTED_ARRAY_STRUCTURES
+    \brief
+    Sorted array data management. The hash table has the indices to the corresponding m_nodes array
+    */
+    //! @{
+    inline bool _erase_sorted(GUINT index)
+    {
+        if(index>=(GUINT)m_nodes.size()) return false;
+        m_nodes.erase_sorted(index);
+		if(m_nodes.size()<2) m_sorted = false;
+        return true;
+    }
+
+    //! faster, but unsorted
+    inline bool _erase_unsorted(GUINT index)
+    {
+        if(index>=m_nodes.size()) return false;
+
+        GUINT lastindex = m_nodes.size()-1;
+        if(index<lastindex && m_hash_table!=0)
+        {
+			GUINT hashkey =  m_nodes[lastindex].m_key;
+			if(hashkey!=GIM_INVALID_HASH)
+			{
+				//update the new position of the last element
+				GUINT cell_index = _find_cell(hashkey);
+				btAssert(cell_index!=GIM_INVALID_HASH);
+				//new position of the last element which will be swaped
+				m_hash_table[cell_index] = index;
+			}
+        }
+        m_nodes.erase(index);
+        m_sorted = false;
+        return true;
+    }
+
+    //! Insert in position ordered
+    /*!
+    Also checks if it is needed to transform this container to a hash table, by calling check_for_switching_to_hashtable
+    */
+    inline void _insert_in_pos(GUINT hashkey, const T & value, GUINT pos)
+    {
+        m_nodes.insert(_node_type(hashkey,value),pos);
+        this->check_for_switching_to_hashtable();
+    }
+
+    //! Insert an element in an ordered array
+    inline GUINT _insert_sorted(GUINT hashkey, const T & value)
+    {
+        if(hashkey==GIM_INVALID_HASH || size()==0)
+        {
+            m_nodes.push_back(_node_type(hashkey,value));
+            return GIM_INVALID_HASH;
+        }
+        //Insert at last position
+        //Sort element
+
+
+        GUINT result_ind=0;
+        GUINT last_index = m_nodes.size()-1;
+        _node_type * ptr = m_nodes.pointer();
+
+        bool found = gim_binary_search_ex(
+        	ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
+
+
+        //Insert before found index
+        if(found)
+        {
+            return result_ind;
+        }
+        else
+        {
+            _insert_in_pos(hashkey, value, result_ind);
+        }
+        return GIM_INVALID_HASH;
+    }
+
+    inline GUINT _insert_sorted_replace(GUINT hashkey, const T & value)
+    {
+        if(hashkey==GIM_INVALID_HASH || size()==0)
+        {
+            m_nodes.push_back(_node_type(hashkey,value));
+            return GIM_INVALID_HASH;
+        }
+        //Insert at last position
+        //Sort element
+        GUINT result_ind;
+        GUINT last_index = m_nodes.size()-1;
+        _node_type * ptr = m_nodes.pointer();
+
+        bool found = gim_binary_search_ex(
+        	ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
+
+        //Insert before found index
+        if(found)
+        {
+            m_nodes[result_ind] = _node_type(hashkey,value);
+        }
+        else
+        {
+            _insert_in_pos(hashkey, value, result_ind);
+        }
+        return result_ind;
+    }
+
+    //! Fast insertion in m_nodes array
+    inline GUINT  _insert_unsorted(GUINT hashkey, const T & value)
+    {
+        m_nodes.push_back(_node_type(hashkey,value));
+        m_sorted = false;
+        return GIM_INVALID_HASH;
+    }
+
+    //! @}
+
+
+public:
+
+    /*!
+        <li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes.
+        When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable.
+        <li> If node_size != 0, then this container becomes a hash table for ever
+        </ul>
+    */
+    gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE,
+                     GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE,
+                     GUINT min_hash_table_size = GIM_INVALID_HASH)
+    {
+        m_hash_table = NULL;
+        m_table_size = 0;
+        m_sorted = false;
+        m_node_size = node_size;
+        m_min_hash_table_size = min_hash_table_size;
+
+        if(m_node_size!=0)
+        {
+            if(reserve_size!=0)
+            {
+                m_nodes.reserve(reserve_size);
+                _reserve_table_memory(reserve_size);
+                _invalidate_keys();
+            }
+            else
+            {
+                m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE);
+                _reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE);
+                _invalidate_keys();
+            }
+        }
+        else if(reserve_size!=0)
+        {
+            m_nodes.reserve(reserve_size);
+        }
+
+    }
+
+    ~gim_hash_table()
+    {
+        _destroy();
+    }
+
+    inline bool is_hash_table()
+    {
+        if(m_hash_table) return true;
+        return false;
+    }
+
+    inline bool is_sorted()
+    {
+        if(size()<2) return true;
+        return m_sorted;
+    }
+
+    bool sort()
+    {
+        if(is_sorted()) return true;
+        if(m_nodes.size()<2) return false;
+
+
+        _node_type * ptr = m_nodes.pointer();
+        GUINT siz = m_nodes.size();
+        gim_sort_hash_node_array(ptr,siz);
+        m_sorted=true;
+
+
+
+        if(m_hash_table)
+        {
+            _rehash();
+        }
+        return true;
+    }
+
+    bool switch_to_hashtable()
+    {
+        if(m_hash_table) return false;
+        if(m_node_size==0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
+        if(m_nodes.size()<GIM_DEFAULT_HASH_TABLE_SIZE)
+        {
+            _resize_table(GIM_DEFAULT_HASH_TABLE_SIZE);
+        }
+        else
+        {
+            _resize_table(m_nodes.size()+1);
+        }
+
+        return true;
+    }
+
+    bool switch_to_sorted_array()
+    {
+        if(m_hash_table==NULL) return true;
+        _clear_table_memory();
+        return sort();
+    }
+
+    //!If the container reaches the
+    bool check_for_switching_to_hashtable()
+    {
+        if(this->m_hash_table) return true;
+
+        if(!(m_nodes.size()< m_min_hash_table_size))
+        {
+            if(m_node_size == 0)
+            {
+                m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
+            }
+
+            _resize_table(m_nodes.size()+1);
+            return true;
+        }
+        return false;
+    }
+
+    inline void set_sorted(bool value)
+    {
+    	m_sorted = value;
+    }
+
+    //! Retrieves the amount of keys.
+    inline GUINT size() const
+    {
+        return m_nodes.size();
+    }
+
+    //! Retrieves the hash key.
+    inline GUINT get_key(GUINT index) const
+    {
+        return m_nodes[index].m_key;
+    }
+
+    //! Retrieves the value by index
+    /*!
+    */
+    inline T * get_value_by_index(GUINT index)
+    {
+        return &m_nodes[index].m_data;
+    }
+
+    inline const T& operator[](GUINT index) const
+    {
+        return m_nodes[index].m_data;
+    }
+
+    inline T& operator[](GUINT index)
+    {
+        return m_nodes[index].m_data;
+    }
+
+    //! Finds the index of the element with the key
+    /*!
+    \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
+    If so, the element has been inserted at the last position of the array.
+    */
+    inline GUINT find(GUINT hashkey)
+    {
+        if(m_hash_table)
+        {
+            GUINT cell_index = _find_cell(hashkey);
+            if(cell_index==GIM_INVALID_HASH) return GIM_INVALID_HASH;
+            return m_hash_table[cell_index];
+        }
+		GUINT last_index = m_nodes.size();
+        if(last_index<2)
+        {
+			if(last_index==0) return GIM_INVALID_HASH;
+            if(m_nodes[0].m_key == hashkey) return 0;
+            return GIM_INVALID_HASH;
+        }
+        else if(m_sorted)
+        {
+            //Binary search
+            GUINT result_ind = 0;
+			last_index--;
+            _node_type *  ptr =  m_nodes.pointer();
+
+            bool found = gim_binary_search_ex(ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
+
+
+            if(found) return result_ind;
+        }
+        return GIM_INVALID_HASH;
+    }
+
+    //! Retrieves the value associated with the index
+    /*!
+    \return the found element, or null
+    */
+    inline T * get_value(GUINT hashkey)
+    {
+        GUINT index = find(hashkey);
+        if(index == GIM_INVALID_HASH) return NULL;
+        return &m_nodes[index].m_data;
+    }
+
+
+    /*!
+    */
+    inline bool erase_by_index(GUINT index)
+    {
+        if(index > m_nodes.size()) return false;
+
+        if(m_hash_table == NULL)
+        {
+            if(is_sorted())
+            {
+                return this->_erase_sorted(index);
+            }
+            else
+            {
+                return this->_erase_unsorted(index);
+            }
+        }
+        else
+        {
+            return this->_erase_by_index_hash_table(index);
+        }
+        return false;
+    }
+
+
+
+    inline bool erase_by_index_unsorted(GUINT index)
+    {
+        if(index > m_nodes.size()) return false;
+
+        if(m_hash_table == NULL)
+        {
+            return this->_erase_unsorted(index);
+        }
+        else
+        {
+            return this->_erase_by_index_hash_table(index);
+        }
+        return false;
+    }
+
+
+
+    /*!
+
+    */
+    inline bool erase_by_key(GUINT hashkey)
+    {
+        if(size()==0) return false;
+
+        if(m_hash_table)
+        {
+            return this->_erase_hash_table(hashkey);
+        }
+        //Binary search
+
+        if(is_sorted()==false) return false;
+
+        GUINT result_ind = find(hashkey);
+        if(result_ind!= GIM_INVALID_HASH)
+        {
+            return this->_erase_sorted(result_ind);
+        }
+        return false;
+    }
+
+    void clear()
+    {
+        m_nodes.clear();
+
+        if(m_hash_table==NULL) return;
+        GUINT datasize = m_table_size*m_node_size;
+        //Initialize the hashkeys.
+        GUINT i;
+        for(i=0;i<datasize;i++)
+        {
+            m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys
+        }
+		m_sorted = false;
+    }
+
+    //! Insert an element into the hash
+    /*!
+    \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position
+    of the existing element.
+    */
+    inline GUINT insert(GUINT hashkey, const T & element)
+    {
+        if(m_hash_table)
+        {
+            return this->_insert_hash_table(hashkey,element);
+        }
+        if(this->is_sorted())
+        {
+            return this->_insert_sorted(hashkey,element);
+        }
+        return this->_insert_unsorted(hashkey,element);
+    }
+
+    //! Insert an element into the hash, and could overrite an existing object with the same hash.
+    /*!
+    \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position
+    of the replaced element.
+    */
+    inline GUINT insert_override(GUINT hashkey, const T & element)
+    {
+        if(m_hash_table)
+        {
+            return this->_insert_hash_table_replace(hashkey,element);
+        }
+        if(this->is_sorted())
+        {
+            return this->_insert_sorted_replace(hashkey,element);
+        }
+        this->_insert_unsorted(hashkey,element);
+        return m_nodes.size();
+    }
+
+
+
+    //! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted
+    /*!
+    */
+    inline GUINT insert_unsorted(GUINT hashkey,const T & element)
+    {
+        if(m_hash_table)
+        {
+            return this->_insert_hash_table(hashkey,element);
+        }
+        return this->_insert_unsorted(hashkey,element);
+    }
+
+
+};
+
+//! @}
+
+
+
+#endif // GIM_CONTAINERS_H_INCLUDED

src/BulletCollision/Gimpact/gim_math.h

@@ -0,0 +1,181 @@
+#ifndef GIM_MATH_H_INCLUDED
+#define GIM_MATH_H_INCLUDED
+/*! \file gim_math.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "LinearMath/btScalar.h"
+
+
+/*! \defgroup BASIC_TYPES
+Basic types and constants
+Conventions:
+Types starting with G
+Constants starting with G_
+*/
+//! @{
+
+#define GREAL btScalar
+#define GREAL2 double
+#define GINT int
+#define GUINT unsigned int
+#define GSHORT short
+#define GUSHORT unsigned short
+#define GINT64 long long
+#define GUINT64 unsigned long long
+
+//! @}
+
+/*! \defgroup BASIC_CONSTANTS
+Basic constants
+Conventions:
+Types starting with G
+Constants starting with G_
+*/
+//! @{
+
+#define G_PI 3.14159265358979f
+#define G_HALF_PI 1.5707963f
+//267948966
+#define G_TWO_PI 6.28318530f
+//71795864
+#define G_ROOT3 1.73205f
+#define G_ROOT2 1.41421f
+#define G_UINT_INFINITY 0xffffffff //!< A very very high value
+#define G_REAL_INFINITY FLT_MAX
+#define	G_SIGN_BITMASK			0x80000000
+#define G_EPSILON SIMD_EPSILON
+//! @}
+
+
+/*! \defgroup SCALAR_TYPES
+\brief
+Precision type constants
+*/
+//! @{
+enum GIM_SCALAR_TYPES
+{
+	G_STYPE_REAL =0,
+	G_STYPE_REAL2,
+	G_STYPE_SHORT,
+	G_STYPE_USHORT,
+	G_STYPE_INT,
+	G_STYPE_UINT,
+	G_STYPE_INT64,
+	G_STYPE_UINT64
+};
+//! @}
+
+/*! \defgroup MATH_FUNCTIONS
+mathematical functions
+*/
+//! @{
+
+#define G_DEGTORAD(X) ((X)*3.1415926f/180.0f)
+#define G_RADTODEG(X) ((X)*180.0f/3.1415926f)
+
+//! Integer representation of a floating-point value.
+#define GIM_IR(x)					((GUINT&)(x))
+
+//! Signed integer representation of a floating-point value.
+#define GIM_SIR(x)					((GINT&)(x))
+
+//! Absolute integer representation of a floating-point value
+#define GIM_AIR(x)					(GIM_IR(x)&0x7fffffff)
+
+//! Floating-point representation of an integer value.
+#define GIM_FR(x)					((GREAL&)(x))
+
+#define GIM_MAX(a,b) (a<b?b:a)
+#define GIM_MIN(a,b) (a>b?b:a)
+
+#define GIM_MAX3(a,b,c) GIM_MAX(a,GIM_MAX(b,c))
+#define GIM_MIN3(a,b,c) GIM_MIN(a,GIM_MIN(b,c))
+
+#define GIM_IS_ZERO(value) (value < G_EPSILON &&  value > -G_EPSILON)
+
+#define GIM_IS_NEGATIVE(value) (value <= -G_EPSILON)
+
+#define GIM_IS_POSISITVE(value) (value >= G_EPSILON)
+
+#define GIM_NEAR_EQUAL(v1,v2) GIM_IS_ZERO((v1-v2))
+
+///returns a clamped number
+#define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
+
+#define GIM_GREATER(x, y)	fabsf(x) > (y)
+
+///Swap numbers
+#define GIM_SWAP_NUMBERS(a,b){ \
+    a = a+b; \
+    b = a-b; \
+    a = a-b; \
+}\
+
+#define GIM_INV_SQRT(va,isva)\
+{\
+    if(va<=0.0000001f)\
+    {\
+        isva = G_REAL_INFINITY;\
+    }\
+    else\
+    {\
+        GREAL _x = va * 0.5f;\
+        GUINT _y = 0x5f3759df - ( GIM_IR(va) >> 1);\
+        isva = GIM_FR(_y);\
+        isva  = isva * ( 1.5f - ( _x * isva * isva ) );\
+    }\
+}\
+
+#define GIM_SQRT(va,sva)\
+{\
+    GIM_INV_SQRT(va,sva);\
+    sva = 1.0f/sva;\
+}\
+
+//! Computes 1.0f / sqrtf(x). Comes from Quake3. See http://www.magic-software.com/3DGEDInvSqrt.html
+inline GREAL gim_inv_sqrt(GREAL f)
+{
+    GREAL r;
+    GIM_INV_SQRT(f,r);
+    return r;
+}
+
+inline GREAL gim_sqrt(GREAL f)
+{
+    GREAL r;
+    GIM_SQRT(f,r);
+    return r;
+}
+
+//! @}
+
+#endif // GIM_MATH_H_INCLUDED

src/BulletCollision/Gimpact/gim_memory.cpp

@@ -0,0 +1,135 @@
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_memory.h"
+#include "stdlib.h"
+
+#ifdef GIM_SIMD_MEMORY
+#include "LinearMath/btAlignedAllocator.h"
+#endif
+
+static gim_alloc_function *g_allocfn = 0;
+static gim_alloca_function *g_allocafn = 0;
+static gim_realloc_function *g_reallocfn = 0;
+static gim_free_function *g_freefn = 0;
+
+void gim_set_alloc_handler (gim_alloc_function *fn)
+{
+  g_allocfn = fn;
+}
+
+void gim_set_alloca_handler (gim_alloca_function *fn)
+{
+  g_allocafn = fn;
+}
+
+void gim_set_realloc_handler (gim_realloc_function *fn)
+{
+  g_reallocfn = fn;
+}
+
+void gim_set_free_handler (gim_free_function *fn)
+{
+  g_freefn = fn;
+}
+
+gim_alloc_function *gim_get_alloc_handler()
+{
+  return g_allocfn;
+}
+
+gim_alloca_function *gim_get_alloca_handler()
+{
+  return g_allocafn;
+}
+
+
+gim_realloc_function *gim_get_realloc_handler ()
+{
+  return g_reallocfn;
+}
+
+
+gim_free_function  *gim_get_free_handler ()
+{
+  return g_freefn;
+}
+
+
+void * gim_alloc(size_t size)
+{
+	void * ptr;
+	if (g_allocfn)
+	{
+		ptr = g_allocfn(size);
+	}
+	else
+	{
+#ifdef GIM_SIMD_MEMORY
+		ptr = btAlignedAlloc(size,16);
+#else
+		ptr = malloc(size);
+#endif
+	}
+  	return ptr;
+}
+
+void * gim_alloca(size_t size)
+{
+  if (g_allocafn) return g_allocafn(size); else return gim_alloc(size);
+}
+
+
+void * gim_realloc(void *ptr, size_t oldsize, size_t newsize)
+{
+ 	void * newptr = gim_alloc(newsize);
+    size_t copysize = oldsize<newsize?oldsize:newsize;
+    gim_simd_memcpy(newptr,ptr,copysize);
+    gim_free(ptr);
+    return newptr;
+}
+
+void gim_free(void *ptr)
+{
+	if (!ptr) return;
+	if (g_freefn)
+	{
+	   g_freefn(ptr);
+	}
+	else
+	{
+	#ifdef GIM_SIMD_MEMORY
+		btAlignedFree(ptr);
+	#else
+		free(ptr);
+	#endif
+	}
+}
+

src/BulletCollision/Gimpact/gim_memory.h

@@ -0,0 +1,211 @@
+#ifndef GIM_MEMORY_H_INCLUDED
+#define GIM_MEMORY_H_INCLUDED
+/*! \file gim_memory.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_math.h"
+#include <memory.h>
+
+//#define PREFETCH 1
+//! \defgroup PREFETCH
+//! @{
+#ifdef PREFETCH
+#include <xmmintrin.h>	// for prefetch
+#define pfval	64
+#define pfval2	128
+//! Prefetch 64
+#define pf(_x,_i)	_mm_prefetch((void *)(_x + _i + pfval), 0)
+//! Prefetch 128
+#define pf2(_x,_i)	_mm_prefetch((void *)(_x + _i + pfval2), 0)
+#else
+//! Prefetch 64
+#define pf(_x,_i)
+//! Prefetch 128
+#define pf2(_x,_i)
+#endif
+//! @}
+
+/*! \defgroup ARRAY_UTILITIES
+\brief
+Functions for manip packed arrays of numbers
+*/
+//! @{
+#define GIM_COPY_ARRAYS(dest_array,source_array,element_count)\
+{\
+    for (GUINT _i_=0;_i_<element_count ;++_i_)\
+    {\
+    	dest_array[_i_] = source_array[_i_];\
+    }\
+}\
+
+#define GIM_COPY_ARRAYS_1(dest_array,source_array,element_count,copy_macro)\
+{\
+    for (GUINT _i_=0;_i_<element_count ;++_i_)\
+    {\
+    	copy_macro(dest_array[_i_],source_array[_i_]);\
+    }\
+}\
+
+
+#define GIM_ZERO_ARRAY(array,element_count)\
+{\
+    for (GUINT _i_=0;_i_<element_count ;++_i_)\
+    {\
+    	array[_i_] = 0;\
+    }\
+}\
+
+#define GIM_CONSTANT_ARRAY(array,element_count,constant)\
+{\
+    for (GUINT _i_=0;_i_<element_count ;++_i_)\
+    {\
+    	array[_i_] = constant;\
+    }\
+}\
+//! @}
+
+/*! \defgroup MEMORY_FUNCTION_PROTOTYPES
+Function prototypes to allocate and free memory.
+*/
+//! @{
+typedef void * gim_alloc_function (size_t size);
+typedef void * gim_alloca_function (size_t size);//Allocs on the heap
+typedef void * gim_realloc_function (void *ptr, size_t oldsize, size_t newsize);
+typedef void gim_free_function (void *ptr);
+//! @}
+
+/*! \defgroup MEMORY_FUNCTION_HANDLERS
+\brief
+Memory Function Handlers
+ set new memory management functions. if fn is 0, the default handlers are
+  used. */
+//! @{
+void gim_set_alloc_handler (gim_alloc_function *fn);
+void gim_set_alloca_handler (gim_alloca_function *fn);
+void gim_set_realloc_handler (gim_realloc_function *fn);
+void gim_set_free_handler (gim_free_function *fn);
+//! @}
+
+/*! \defgroup MEMORY_FUNCTION_GET_HANDLERS
+\brief
+get current memory management functions.
+*/
+//! @{
+gim_alloc_function *gim_get_alloc_handler (void);
+gim_alloca_function *gim_get_alloca_handler(void);
+gim_realloc_function *gim_get_realloc_handler (void);
+gim_free_function  *gim_get_free_handler (void);
+//! @}
+
+/*! \defgroup MEMORY_FUNCTIONS
+Standar Memory functions
+*/
+//! @{
+void * gim_alloc(size_t size);
+void * gim_alloca(size_t size);
+void * gim_realloc(void *ptr, size_t oldsize, size_t newsize);
+void gim_free(void *ptr);
+//! @}
+
+
+#if defined (WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__)
+    #define GIM_SIMD_MEMORY 1
+#endif
+
+//! SIMD POINTER INTEGER
+#define SIMD_T GUINT64
+//! SIMD INTEGER SIZE
+#define SIMD_T_SIZE sizeof(SIMD_T)
+
+
+inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize)
+{
+#ifdef GIM_SIMD_MEMORY
+/*
+//'long long int' is incompatible with visual studio 6...
+    //copy words
+    SIMD_T * ui_src_ptr = (SIMD_T *)src;
+    SIMD_T * ui_dst_ptr = (SIMD_T *)dst;
+    while(copysize>=SIMD_T_SIZE)
+    {
+        *(ui_dst_ptr++) = *(ui_src_ptr++);
+        copysize-=SIMD_T_SIZE;
+    }
+    if(copysize==0) return;
+*/
+
+    char * c_src_ptr = (char *)src;
+    char * c_dst_ptr = (char *)dst;
+    while(copysize>0)
+    {
+        *(c_dst_ptr++) = *(c_src_ptr++);
+        copysize--;
+    }
+    return;
+#else
+    memcpy(dst,src,copysize);
+#endif
+}
+
+
+
+template<class T>
+inline void gim_swap_elements(T* _array,size_t _i,size_t _j)
+{
+	T _e_tmp_ = _array[_i];
+	_array[_i] = _array[_j];
+	_array[_j] = _e_tmp_;
+}
+
+
+template<class T>
+inline void gim_swap_elements_memcpy(T* _array,size_t _i,size_t _j)
+{
+	char _e_tmp_[sizeof(T)];
+	gim_simd_memcpy(_e_tmp_,&_array[_i],sizeof(T));
+	gim_simd_memcpy(&_array[_i],&_array[_j],sizeof(T));
+	gim_simd_memcpy(&_array[_j],_e_tmp_,sizeof(T));
+}
+
+template <int SIZE>
+inline void gim_swap_elements_ptr(char * _array,size_t _i,size_t _j)
+{
+	char _e_tmp_[SIZE];
+	_i*=SIZE;
+	_j*=SIZE;
+	gim_simd_memcpy(_e_tmp_,_array+_i,SIZE);
+	gim_simd_memcpy(_array+_i,_array+_j,SIZE);
+	gim_simd_memcpy(_array+_j,_e_tmp_,SIZE);
+}
+
+#endif // GIM_MEMORY_H_INCLUDED

src/BulletCollision/Gimpact/gim_radixsort.h

@@ -0,0 +1,410 @@
+#ifndef GIM_RADIXSORT_H_INCLUDED
+#define GIM_RADIXSORT_H_INCLUDED
+/*! \file gim_radixsort.h
+\author Francisco Len N<>jera.
+Based on the work of Michael Herf : "fast floating-point radix sort"
+Avaliable on http://www.stereopsis.com/radix.html
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_memory.h"
+
+/*! \defgroup SORTING
+\brief
+Macros for sorting.
+*/
+
+//! Prototype for comparators
+class less_comparator
+{
+	public:
+
+	template<class T,class Z>
+	inline int operator() ( const T& a, const Z& b )
+	{
+		return ( a<b?-1:(a>b?1:0));
+	}
+};
+
+//! Prototype for comparators
+class integer_comparator
+{
+	public:
+
+	template<class T>
+	inline int operator() ( const T& a, const T& b )
+	{
+		return (int)(a-b);
+	}
+};
+
+//!Prototype for getting the integer representation of an object
+class uint_key_func
+{
+public:
+	template<class T>
+	inline GUINT operator()( const T& a)
+	{
+		return (GUINT)a;
+	}
+};
+
+
+//!Prototype for copying elements
+class copy_elements_func
+{
+public:
+	template<class T>
+	inline void operator()(T& a,T& b)
+	{
+		a = b;
+	}
+};
+
+//!Prototype for copying elements
+class memcopy_elements_func
+{
+public:
+	template<class T>
+	inline void operator()(T& a,T& b)
+	{
+		gim_simd_memcpy(&a,&b,sizeof(T));
+	}
+};
+
+
+//! @{
+struct GIM_RSORT_TOKEN
+{
+    GUINT m_key;
+    GUINT m_value;
+    GIM_RSORT_TOKEN()
+    {
+    }
+    GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken)
+    {
+    	m_key = rtoken.m_key;
+    	m_value = rtoken.m_value;
+    }
+
+    inline bool operator <(const GIM_RSORT_TOKEN& other) const
+	{
+		return (m_key < other.m_key);
+	}
+
+	inline bool operator >(const GIM_RSORT_TOKEN& other) const
+	{
+		return (m_key > other.m_key);
+	}
+};
+
+//! Prototype for comparators
+class GIM_RSORT_TOKEN_COMPARATOR
+{
+	public:
+
+	inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b )
+	{
+		return (int)((a.m_key) - (b.m_key));
+	}
+};
+
+
+
+#define kHist 2048
+// ---- utils for accessing 11-bit quantities
+#define D11_0(x)	(x & 0x7FF)
+#define D11_1(x)	(x >> 11 & 0x7FF)
+#define D11_2(x)	(x >> 22 )
+
+
+
+///Radix sort for unsigned integer keys
+inline void gim_radix_sort_rtokens(
+				GIM_RSORT_TOKEN * array,
+				GIM_RSORT_TOKEN * sorted, GUINT element_count)
+{
+	GUINT i;
+	GUINT b0[kHist * 3];
+	GUINT *b1 = b0 + kHist;
+	GUINT *b2 = b1 + kHist;
+	for (i = 0; i < kHist * 3; ++i)
+	{
+		b0[i] = 0;
+	}
+	GUINT fi;
+	GUINT pos;
+	for (i = 0; i < element_count; ++i)
+	{
+	    fi = array[i].m_key;
+		b0[D11_0(fi)] ++;
+		b1[D11_1(fi)] ++;
+		b2[D11_2(fi)] ++;
+	}
+	{
+		GUINT sum0 = 0, sum1 = 0, sum2 = 0;
+		GUINT tsum;
+		for (i = 0; i < kHist; ++i)
+		{
+			tsum = b0[i] + sum0;
+			b0[i] = sum0 - 1;
+			sum0 = tsum;
+			tsum = b1[i] + sum1;
+			b1[i] = sum1 - 1;
+			sum1 = tsum;
+			tsum = b2[i] + sum2;
+			b2[i] = sum2 - 1;
+			sum2 = tsum;
+		}
+	}
+	for (i = 0; i < element_count; ++i)
+	{
+        fi = array[i].m_key;
+		pos = D11_0(fi);
+		pos = ++b0[pos];
+		sorted[pos].m_key = array[i].m_key;
+		sorted[pos].m_value = array[i].m_value;
+	}
+	for (i = 0; i < element_count; ++i)
+	{
+        fi = sorted[i].m_key;
+		pos = D11_1(fi);
+		pos = ++b1[pos];
+		array[pos].m_key = sorted[i].m_key;
+		array[pos].m_value = sorted[i].m_value;
+	}
+	for (i = 0; i < element_count; ++i)
+	{
+        fi = array[i].m_key;
+		pos = D11_2(fi);
+		pos = ++b2[pos];
+		sorted[pos].m_key = array[i].m_key;
+		sorted[pos].m_value = array[i].m_value;
+	}
+}
+
+
+
+
+/// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN
+/*!
+*\param array Array of elements to sort
+*\param sorted_tokens Tokens of sorted elements
+*\param element_count element count
+*\param uintkey_macro Functor which retrieves the integer representation of an array element
+*/
+template<typename T, class GETKEY_CLASS>
+void gim_radix_sort_array_tokens(
+			T* array ,
+			GIM_RSORT_TOKEN * sorted_tokens,
+			GUINT element_count,GETKEY_CLASS uintkey_macro)
+{
+	GIM_RSORT_TOKEN * _unsorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
+    for (GUINT _i=0;_i<element_count;++_i)
+    {
+        _unsorted[_i].m_key = uintkey_macro(array[_i]);
+        _unsorted[_i].m_value = _i;
+    }
+    gim_radix_sort_rtokens(_unsorted,sorted_tokens,element_count);
+    gim_free(_unsorted);
+    gim_free(_unsorted);
+}
+
+/// Sorts array in place. For generic use
+/*!
+\param type Type of the array
+\param array
+\param element_count
+\param get_uintkey_macro Macro for extract the Integer value of the element. Similar to SIMPLE_GET_UINTKEY
+\param copy_elements_macro Macro for copy elements, similar to SIMPLE_COPY_ELEMENTS
+*/
+template<typename T, class GETKEY_CLASS, class COPY_CLASS>
+void gim_radix_sort(
+	T * array, GUINT element_count,
+	GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro)
+{
+	GIM_RSORT_TOKEN * _sorted = (GIM_RSORT_TOKEN  *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
+    gim_radix_sort_array_tokens(array,_sorted,element_count,get_uintkey_macro);
+    T * _original_array = (T *) gim_alloc(sizeof(T)*element_count);
+    gim_simd_memcpy(_original_array,array,sizeof(T)*element_count);
+    for (GUINT _i=0;_i<element_count;++_i)
+    {
+        copy_elements_macro(array[_i],_original_array[_sorted[_i].m_value]);
+    }
+    gim_free(_original_array);
+    gim_free(_sorted);
+}
+
+//! Failsafe Iterative binary search,
+/*!
+If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
+\param _array
+\param _start_i the beginning of the array
+\param _end_i the ending  index of the array
+\param _search_key Value to find
+\param _comp_macro macro for comparing elements
+\param _found If true the value has found. Boolean
+\param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
+*/
+template<class T, typename KEYCLASS, typename COMP_CLASS>
+bool  gim_binary_search_ex(
+		const T* _array, GUINT _start_i,
+		GUINT _end_i,GUINT & _result_index,
+		const KEYCLASS & _search_key,
+		COMP_CLASS _comp_macro)
+{
+	GUINT _k;
+	int _comp_result;
+	GUINT _i = _start_i;
+	GUINT _j = _end_i+1;
+	while (_i < _j)
+	{
+		_k = (_j+_i-1)/2;
+		_comp_result = _comp_macro(_array[_k], _search_key);
+		if (_comp_result == 0)
+		{
+			_result_index = _k;
+			return true;
+		}
+		else if (_comp_result < 0)
+		{
+			_i = _k+1;
+		}
+		else
+		{
+			_j = _k;
+		}
+	}
+	_result_index = _i;
+	return false;
+}
+
+
+
+//! Failsafe Iterative binary search,Template version
+/*!
+If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
+\param _array
+\param _start_i the beginning of the array
+\param _end_i the ending  index of the array
+\param _search_key Value to find
+\param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
+\return true if found, else false
+*/
+template<class T>
+bool gim_binary_search(
+	const T*_array,GUINT _start_i,
+	GUINT _end_i,const T & _search_key,
+	GUINT & _result_index)
+{
+	GUINT _i = _start_i;
+	GUINT _j = _end_i+1;
+	GUINT _k;
+	while(_i < _j)
+	{
+		_k = (_j+_i-1)/2;
+		if(_array[_k]==_search_key)
+		{
+			_result_index = _k;
+			return true;
+		}
+		else if (_array[_k]<_search_key)
+		{
+			_i = _k+1;
+		}
+		else
+		{
+			_j = _k;
+		}
+	}
+	_result_index = _i;
+	return false;
+}
+
+
+
+///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+template <typename T, typename COMP_CLASS>
+void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS 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 < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0)
+		{
+			child++;
+		}
+		/* pick larger child */
+		if (CompareFunc(temp , pArr[child - 1])<0)
+		{
+			/* move child up */
+			pArr[k - 1] = pArr[child - 1];
+			k = child;
+		}
+		else
+		{
+			break;
+		}
+	}
+	pArr[k - 1] = temp;
+} /*downHeap*/
+
+
+template <typename T, typename COMP_CLASS>
+void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc)
+{
+	/* sort a[0..N-1],  N.B. 0 to N-1 */
+	GUINT k;
+	GUINT n = element_count;
+	for (k = n/2; k > 0; k--)
+	{
+		gim_down_heap(pArr, k, n, CompareFunc);
+	}
+
+	/* a[1..N] is now a heap */
+	while ( n>=2 )
+	{
+		gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */
+		--n;
+		/* restore a[1..i-1] heap */
+		gim_down_heap(pArr, 1, n, CompareFunc);
+	}
+}
+
+
+
+//! @}
+#endif // GIM_RADIXSORT_H_INCLUDED

src/BulletCollision/Gimpact/gim_tri_collision.cpp

@@ -0,0 +1,640 @@
+
+/*! \file gim_tri_collision.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_tri_collision.h"
+
+
+#define TRI_LOCAL_EPSILON 0.000001f
+#define MIN_EDGE_EDGE_DIS 0.00001f
+
+
+class _GIM_TRIANGLE_CALCULATION_CACHE
+{
+public:
+	GREAL margin;	
+	btVector3 tu_vertices[3];
+	btVector3 tv_vertices[3];
+	btVector4 tu_plane;
+	btVector4 tv_plane;
+	btVector3 closest_point_u;
+	btVector3 closest_point_v;
+	btVector3 edge_edge_dir;
+	btVector3 distances;
+	GREAL du[4];
+	GREAL du0du1;
+	GREAL du0du2;
+	GREAL dv[4];
+	GREAL dv0dv1;
+	GREAL dv0dv2;	
+	btVector3 temp_points[MAX_TRI_CLIPPING];
+	btVector3 temp_points1[MAX_TRI_CLIPPING];
+	btVector3 contact_points[MAX_TRI_CLIPPING];
+	
+
+
+	//! if returns false, the faces are paralele
+	SIMD_FORCE_INLINE bool compute_intervals(
+					const GREAL &D0,
+					const GREAL &D1,
+					const GREAL &D2,
+					const GREAL &D0D1,
+					const GREAL &D0D2,
+					GREAL & scale_edge0,
+					GREAL & scale_edge1,
+					GUINT &edge_index0,
+					GUINT &edge_index1)
+	{
+		if(D0D1>0.0f)
+		{
+			/* here we know that D0D2<=0.0 */
+			/* that is D0, D1 are on the same side, D2 on the other or on the plane */
+			scale_edge0 = -D2/(D0-D2);
+			scale_edge1 = -D1/(D2-D1);
+			edge_index0 = 2;edge_index1 = 1;
+		}
+		else if(D0D2>0.0f)
+		{
+			/* here we know that d0d1<=0.0 */
+			scale_edge0 = -D0/(D1-D0);
+			scale_edge1 = -D1/(D2-D1);
+			edge_index0 = 0;edge_index1 = 1;
+		}
+		else if(D1*D2>0.0f || D0!=0.0f)
+		{
+			/* here we know that d0d1<=0.0 or that D0!=0.0 */
+			scale_edge0 = -D0/(D1-D0);
+			scale_edge1 = -D2/(D0-D2);
+			edge_index0 = 0 ;edge_index1 = 2;
+		}
+		else
+		{
+			return false;
+		}
+		return true;
+	}
+
+
+	//! clip triangle
+	/*!
+	*/
+	SIMD_FORCE_INLINE GUINT clip_triangle(
+		const btVector4 & tri_plane,
+		const btVector3 * tripoints,
+		const btVector3 * srcpoints,
+		btVector3 * clip_points)
+	{
+		// edge 0
+
+		btVector4 edgeplane;
+
+		EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
+
+		GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
+			edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
+
+		if(clipped_count == 0) return 0;
+
+		// edge 1
+
+		EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
+
+		clipped_count = PLANE_CLIP_POLYGON3D(
+			edgeplane,temp_points,clipped_count,temp_points1);
+
+		if(clipped_count == 0) return 0;
+
+		// edge 2
+
+		EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
+
+		clipped_count = PLANE_CLIP_POLYGON3D(
+			edgeplane,temp_points1,clipped_count,clip_points);
+
+		return clipped_count;
+
+
+		/*GUINT i0 = (tri_plane.closestAxis()+1)%3;
+		GUINT i1 = (i0+1)%3;
+		// edge 0
+		btVector3 temp_points[MAX_TRI_CLIPPING];
+		btVector3 temp_points1[MAX_TRI_CLIPPING];
+
+		GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC(
+			0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points,
+			DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1));
+		
+		
+		if(clipped_count == 0) return 0;
+
+		// edge 1
+		clipped_count = PLANE_CLIP_POLYGON_GENERIC(
+			0,temp_points,clipped_count,temp_points1,
+			DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1));
+
+		if(clipped_count == 0) return 0;
+
+		// edge 2
+		clipped_count = PLANE_CLIP_POLYGON_GENERIC(
+			0,temp_points1,clipped_count,clipped_points,
+			DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1));
+
+		return clipped_count;*/
+	}
+
+	SIMD_FORCE_INLINE void sort_isect(
+		GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
+	{
+		if(isect1<isect0)
+		{
+			//swap
+			GIM_SWAP_NUMBERS(isect0,isect1);
+			GIM_SWAP_NUMBERS(e0,e1);
+			btVector3 tmp = vec0;
+			vec0 = vec1;
+			vec1 = tmp;
+		}
+	}
+
+	//! Test verifying interval intersection with the direction between planes
+	/*!
+	\pre tv_plane and tu_plane must be set
+	\post
+	distances[2] is set with the distance
+	closest_point_u, closest_point_v, edge_edge_dir are set too
+	\return
+	- 0: faces are paralele
+	- 1: face U casts face V
+	- 2: face V casts face U
+	- 3: nearest edges
+	*/
+	SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
+	{
+		// Compute direction of intersection line
+		edge_edge_dir = tu_plane.cross(tv_plane);
+		GREAL Dlen;
+		VEC_LENGTH(edge_edge_dir,Dlen);
+
+		if(Dlen<0.0001)
+		{
+			return 0; //faces near paralele
+		}
+
+		edge_edge_dir*= 1/Dlen;//normalize
+
+
+		// Compute interval for triangle 1
+		GUINT tu_e0,tu_e1;//edge indices
+		GREAL tu_scale_e0,tu_scale_e1;//edge scale
+		if(!compute_intervals(du[0],du[1],du[2],
+			du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
+
+		// Compute interval for triangle 2
+		GUINT tv_e0,tv_e1;//edge indices
+		GREAL tv_scale_e0,tv_scale_e1;//edge scale
+
+		if(!compute_intervals(dv[0],dv[1],dv[2],
+			dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
+
+		//proyected vertices
+		btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
+		btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
+
+		btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
+		btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
+
+		//proyected intervals
+		GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
+		GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
+
+		sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
+		sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
+
+		const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
+		const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
+
+		if(midpoint_u<midpoint_v)
+		{
+			if(isect_u[1]>=isect_v[1]) // face U casts face V
+			{
+				return 1;
+			}
+			else if(isect_v[0]<=isect_u[0]) // face V casts face U
+			{
+				return 2;
+			}
+			// closest points
+			closest_point_u = up_e1;
+			closest_point_v = vp_e0;
+			// calc edges and separation
+
+			if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
+			{
+				SEGMENT_COLLISION(
+					tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
+					tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
+					closest_point_u,
+					closest_point_v);
+
+				edge_edge_dir = closest_point_u-closest_point_v;
+				VEC_LENGTH(edge_edge_dir,distances[2]);
+				edge_edge_dir *= 1.0f/distances[2];// normalize
+			}
+			else
+			{
+				distances[2] = isect_v[0]-isect_u[1];//distance negative
+				//edge_edge_dir *= -1.0f; //normal pointing from V to U
+			}
+
+		}
+		else
+		{
+			if(isect_v[1]>=isect_u[1]) // face V casts face U
+			{
+				return 2;
+			}
+			else if(isect_u[0]<=isect_v[0]) // face U casts face V
+			{
+				return 1;
+			}
+			// closest points
+			closest_point_u = up_e0;
+			closest_point_v = vp_e1;
+			// calc edges and separation
+
+			if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
+			{
+				SEGMENT_COLLISION(
+					tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
+					tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
+					closest_point_u,
+					closest_point_v);
+
+				edge_edge_dir = closest_point_u-closest_point_v;
+				VEC_LENGTH(edge_edge_dir,distances[2]);
+				edge_edge_dir *= 1.0f/distances[2];// normalize
+			}
+			else
+			{
+				distances[2] = isect_u[0]-isect_v[1];//distance negative
+				//edge_edge_dir *= -1.0f; //normal pointing from V to U
+			}
+		}
+		return 3;
+	}
+
+
+	//! collides by two sides
+	SIMD_FORCE_INLINE bool triangle_collision(
+					const btVector3 & u0,
+					const btVector3 & u1,
+					const btVector3 & u2,
+					GREAL margin_u,
+					const btVector3 & v0,
+					const btVector3 & v1,
+					const btVector3 & v2,
+					GREAL margin_v,
+					GIM_TRIANGLE_CONTACT_DATA & contacts)
+	{
+
+		margin = margin_u + margin_v;
+
+		tu_vertices[0] = u0;
+		tu_vertices[1] = u1;
+		tu_vertices[2] = u2;
+
+		tv_vertices[0] = v0;
+		tv_vertices[1] = v1;
+		tv_vertices[2] = v2;
+
+		//create planes
+		// plane v vs U points
+
+		TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
+
+		du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
+		du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
+		du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);
+
+
+		du0du1 = du[0] * du[1];
+		du0du2 = du[0] * du[2];
+
+
+		if(du0du1>0.0f && du0du2>0.0f)	// same sign on all of them + not equal 0 ?
+		{
+			if(du[0]<0) //we need test behind the triangle plane
+			{
+				distances[0] = GIM_MAX3(du[0],du[1],du[2]);
+				distances[0] = -distances[0];
+				if(distances[0]>margin) return false; //never intersect
+
+				//reorder triangle v
+				VEC_SWAP(tv_vertices[0],tv_vertices[1]);
+				VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
+			}
+			else
+			{
+				distances[0] = GIM_MIN3(du[0],du[1],du[2]);
+				if(distances[0]>margin) return false; //never intersect
+			}
+		}
+		else
+		{
+			//Look if we need to invert the triangle
+			distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
+
+			if(distances[0]<0.0f)
+			{
+				//reorder triangle v
+				VEC_SWAP(tv_vertices[0],tv_vertices[1]);
+				VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
+
+				distances[0] = GIM_MAX3(du[0],du[1],du[2]);
+				distances[0] = -distances[0];
+			}
+			else
+			{
+				distances[0] = GIM_MIN3(du[0],du[1],du[2]);
+			}
+		}
+
+
+		// plane U vs V points
+
+		TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
+
+		dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
+		dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
+		dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);
+
+		dv0dv1 = dv[0] * dv[1];
+		dv0dv2 = dv[0] * dv[2];
+
+
+		if(dv0dv1>0.0f && dv0dv2>0.0f)	// same sign on all of them + not equal 0 ?
+		{
+			if(dv[0]<0) //we need test behind the triangle plane
+			{
+				distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
+				distances[1] = -distances[1];
+				if(distances[1]>margin) return false; //never intersect
+
+				//reorder triangle u
+				VEC_SWAP(tu_vertices[0],tu_vertices[1]);
+				VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
+			}
+			else
+			{
+				distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
+				if(distances[1]>margin) return false; //never intersect
+			}
+		}
+		else
+		{
+			//Look if we need to invert the triangle
+			distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
+
+			if(distances[1]<0.0f)
+			{
+				//reorder triangle v
+				VEC_SWAP(tu_vertices[0],tu_vertices[1]);
+				VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
+
+				distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
+				distances[1] = -distances[1];
+			}
+			else
+			{
+				distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
+			}
+		}
+
+		GUINT bl;
+		/* bl = cross_line_intersection_test();
+		if(bl==3)
+		{
+			//take edge direction too
+			bl = distances.maxAxis();
+		}
+		else
+		{*/
+			bl = 0;
+			if(distances[0]<distances[1]) bl = 1;
+		//}
+
+		if(bl==2) //edge edge separation
+		{
+			if(distances[2]>margin) return false;
+
+			contacts.m_penetration_depth = -distances[2] + margin;
+			contacts.m_points[0] = closest_point_v;
+			contacts.m_point_count = 1;
+			VEC_COPY(contacts.m_separating_normal,edge_edge_dir);
+
+			return true;
+		}
+
+		//clip face against other
+
+		
+		GUINT point_count;
+		//TODO
+		if(bl == 0) //clip U points against V
+		{
+			point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
+			if(point_count == 0) return false;						
+			contacts.merge_points(tv_plane,margin,contact_points,point_count);			
+		}
+		else //clip V points against U
+		{
+			point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
+			if(point_count == 0) return false;			
+			contacts.merge_points(tu_plane,margin,contact_points,point_count);
+			contacts.m_separating_normal *= -1.f;
+		}
+		if(contacts.m_point_count == 0) return false;
+		return true;
+	}
+
+};
+
+
+/*class _GIM_TRIANGLE_CALCULATION_CACHE
+{
+public:
+	GREAL margin;
+	GUINT clipped_count;
+	btVector3 tu_vertices[3];
+	btVector3 tv_vertices[3];
+	btVector3 temp_points[MAX_TRI_CLIPPING];
+	btVector3 temp_points1[MAX_TRI_CLIPPING];
+	btVector3 clipped_points[MAX_TRI_CLIPPING];
+	GIM_TRIANGLE_CONTACT_DATA contacts1;
+	GIM_TRIANGLE_CONTACT_DATA contacts2;
+
+
+	//! clip triangle
+	GUINT clip_triangle(
+		const btVector4 & tri_plane,
+		const btVector3 * tripoints,
+		const btVector3 * srcpoints,
+		btVector3 * clipped_points)
+	{
+		// edge 0
+
+		btVector4 edgeplane;
+
+		EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
+
+		GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
+			edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
+
+		if(clipped_count == 0) return 0;
+
+		// edge 1
+
+		EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
+
+		clipped_count = PLANE_CLIP_POLYGON3D(
+			edgeplane,temp_points,clipped_count,temp_points1);
+
+		if(clipped_count == 0) return 0;
+
+		// edge 2
+
+		EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
+
+		clipped_count = PLANE_CLIP_POLYGON3D(
+			edgeplane,temp_points1,clipped_count,clipped_points);
+
+		return clipped_count;
+	}
+
+
+
+
+	//! collides only on one side
+	bool triangle_collision(
+					const btVector3 & u0,
+					const btVector3 & u1,
+					const btVector3 & u2,
+					GREAL margin_u,
+					const btVector3 & v0,
+					const btVector3 & v1,
+					const btVector3 & v2,
+					GREAL margin_v,
+					GIM_TRIANGLE_CONTACT_DATA & contacts)
+	{
+
+		margin = margin_u + margin_v;
+
+		
+		tu_vertices[0] = u0;
+		tu_vertices[1] = u1;
+		tu_vertices[2] = u2;
+
+		tv_vertices[0] = v0;
+		tv_vertices[1] = v1;
+		tv_vertices[2] = v2;
+
+		//create planes
+		// plane v vs U points
+
+
+		TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal);
+
+		clipped_count = clip_triangle(
+			contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points);
+
+		if(clipped_count == 0 )
+		{
+			 return false;//Reject
+		}
+
+		//find most deep interval face1
+		contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
+		if(contacts1.m_point_count == 0) return false; // too far
+
+		//Normal pointing to triangle1
+		//contacts1.m_separating_normal *= -1.f;
+
+		//Clip tri1 by tri2 edges
+
+		TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal);
+
+		clipped_count = clip_triangle(
+			contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points);
+
+		if(clipped_count == 0 )
+		{
+			 return false;//Reject
+		}
+
+		//find most deep interval face1
+		contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
+		if(contacts2.m_point_count == 0) return false; // too far
+
+		contacts2.m_separating_normal *= -1.f;
+
+		////check most dir for contacts
+		if(contacts2.m_penetration_depth<contacts1.m_penetration_depth)
+		{
+			contacts.copy_from(contacts2);
+		}
+		else
+		{
+			contacts.copy_from(contacts1);
+		}
+		return true;
+	}
+
+
+};*/
+
+
+
+bool GIM_TRIANGLE::collide_triangle_hard_test(
+		const GIM_TRIANGLE & other,
+		GIM_TRIANGLE_CONTACT_DATA & contact_data) const
+{
+	_GIM_TRIANGLE_CALCULATION_CACHE calc_cache;	
+	return calc_cache.triangle_collision(
+					m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
+					other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
+					contact_data);
+
+}
+
+
+
+

src/BulletCollision/Gimpact/gim_tri_collision.h

@@ -0,0 +1,383 @@
+#ifndef GIM_TRI_COLLISION_H_INCLUDED
+#define GIM_TRI_COLLISION_H_INCLUDED
+
+/*! \file gim_tri_collision.h
+\author Francisco Len N<>jera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_box_collision.h"
+#include "gim_clip_polygon.h"
+
+/*! \addtogroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+
+
+#define MAX_TRI_CLIPPING 16
+
+//! Structure for collision
+struct GIM_TRIANGLE_CONTACT_DATA
+{
+    GREAL m_penetration_depth;
+    GUINT m_point_count;
+    btVector4 m_separating_normal;
+    btVector3 m_points[MAX_TRI_CLIPPING];
+
+	SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other)
+	{
+		m_penetration_depth = other.m_penetration_depth;
+		m_separating_normal = other.m_separating_normal;
+		m_point_count = other.m_point_count;
+		GUINT i = m_point_count;
+		while(i--)
+		{
+			m_points[i] = other.m_points[i];
+		}
+	}
+
+	GIM_TRIANGLE_CONTACT_DATA()
+	{
+	}
+
+	GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other)
+	{
+		copy_from(other);
+	}
+
+	
+	
+
+    //! classify points that are closer
+    template<typename DISTANCE_FUNC,typename CLASS_PLANE>
+    SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane,
+    				GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func)
+    {	
+    	m_point_count = 0;
+    	m_penetration_depth= -1000.0f;
+
+		GUINT point_indices[MAX_TRI_CLIPPING];
+
+		GUINT _k;
+
+		for(_k=0;_k<point_count;_k++)
+		{
+			GREAL _dist = -distance_func(plane,points[_k]) + margin;
+
+			if(_dist>=0.0f)
+			{
+				if(_dist>m_penetration_depth)
+				{
+					m_penetration_depth = _dist;
+					point_indices[0] = _k;
+					m_point_count=1;
+				}
+				else if((_dist+G_EPSILON)>=m_penetration_depth)
+				{
+					point_indices[m_point_count] = _k;
+					m_point_count++;
+				}
+			}
+		}
+
+		for( _k=0;_k<m_point_count;_k++)
+		{
+			m_points[_k] = points[point_indices[_k]];
+		}
+	}
+
+	//! classify points that are closer
+	SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin,
+										 const btVector3 * points, GUINT point_count)
+	{
+		m_separating_normal = plane;
+		mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC());
+	}
+};
+
+
+//! Class for colliding triangles
+class GIM_TRIANGLE
+{
+public:
+	btScalar m_margin;
+    btVector3 m_vertices[3];
+
+    GIM_TRIANGLE():m_margin(0.1f)
+    {
+    }
+
+    SIMD_FORCE_INLINE GIM_AABB get_box()  const
+    {
+    	return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
+    }
+
+    SIMD_FORCE_INLINE void get_normal(btVector3 &normal)  const
+    {
+    	TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal);
+    }
+
+    SIMD_FORCE_INLINE void get_plane(btVector4 &plane)  const
+    {
+    	TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);;
+    }
+
+    SIMD_FORCE_INLINE void apply_transform(const btTransform & trans)
+    {
+    	m_vertices[0] = trans(m_vertices[0]);
+    	m_vertices[1] = trans(m_vertices[1]);
+    	m_vertices[2] = trans(m_vertices[2]);
+    }
+
+    SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane)  const
+    {
+		const btVector3 & e0 = m_vertices[edge_index];
+		const btVector3 & e1 = m_vertices[(edge_index+1)%3];
+		EDGE_PLANE(e0,e1,triangle_normal,plane);
+    }
+
+    //! Gets the relative transformation of this triangle
+    /*!
+    The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0:
+    - triangle normal corresponds to Z axis.
+    - 1st normalized edge corresponds to X axis,
+
+    */
+    SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform)  const
+    {
+    	btMatrix3x3 & matrix = triangle_transform.getBasis();
+
+    	btVector3 zaxis;
+    	get_normal(zaxis);
+    	MAT_SET_Z(matrix,zaxis);
+
+    	btVector3 xaxis = m_vertices[1] - m_vertices[0];
+    	VEC_NORMALIZE(xaxis);
+    	MAT_SET_X(matrix,xaxis);
+
+    	//y axis
+    	xaxis = zaxis.cross(xaxis);
+    	MAT_SET_Y(matrix,xaxis);
+
+    	triangle_transform.setOrigin(m_vertices[0]);
+    }
+
+
+	//! Test triangles by finding separating axis
+	/*!
+	\param other Triangle for collide
+	\param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
+	*/
+	bool collide_triangle_hard_test(
+		const GIM_TRIANGLE & other,
+		GIM_TRIANGLE_CONTACT_DATA & contact_data) const;
+
+	//! Test boxes before doing hard test
+	/*!
+	\param other Triangle for collide
+	\param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
+	\
+	*/
+	SIMD_FORCE_INLINE bool collide_triangle(
+		const GIM_TRIANGLE & other,
+		GIM_TRIANGLE_CONTACT_DATA & contact_data) const
+	{
+		//test box collisioin
+		GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
+		GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin);
+		if(!boxu.has_collision(boxv)) return false;
+
+		//do hard test
+		return collide_triangle_hard_test(other,contact_data);
+	}
+
+	/*!
+
+	Solve the System for u,v parameters:
+
+	u*axe1[i1] + v*axe2[i1] = vecproj[i1]
+	u*axe1[i2] + v*axe2[i2] = vecproj[i2]
+
+	sustitute:
+	v = (vecproj[i2] - u*axe1[i2])/axe2[i2]
+
+	then the first equation in terms of 'u':
+
+	--> u*axe1[i1] + ((vecproj[i2] - u*axe1[i2])/axe2[i2])*axe2[i1] = vecproj[i1]
+
+	--> u*axe1[i1] + vecproj[i2]*axe2[i1]/axe2[i2] - u*axe1[i2]*axe2[i1]/axe2[i2] = vecproj[i1]
+
+	--> u*(axe1[i1]  - axe1[i2]*axe2[i1]/axe2[i2]) = vecproj[i1] - vecproj[i2]*axe2[i1]/axe2[i2]
+
+	--> u*((axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1])/axe2[i2]) = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1])/axe2[i2]
+
+	--> u*(axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1]) = vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]
+
+	--> u = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]) /(axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1])
+
+if 0.0<= u+v <=1.0 then they are inside of triangle
+
+	\return false if the point is outside of triangle.This function  doesn't take the margin
+	*/
+	SIMD_FORCE_INLINE bool get_uv_parameters(
+			const btVector3 & point,
+			const btVector3 & tri_plane,
+			GREAL & u, GREAL & v) const
+	{
+		btVector3 _axe1 = m_vertices[1]-m_vertices[0];
+		btVector3 _axe2 = m_vertices[2]-m_vertices[0];
+		btVector3 _vecproj = point - m_vertices[0];
+		GUINT _i1 = (tri_plane.closestAxis()+1)%3;
+		GUINT _i2 = (_i1+1)%3;
+		if(btFabs(_axe2[_i2])<G_EPSILON)
+		{
+			u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1]  - _axe1[_i1]*_axe2[_i2]);
+			v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1];
+		}
+		else
+		{
+			u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2]  - _axe1[_i2]*_axe2[_i1]);
+			v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2];
+		}
+
+		if(u<-G_EPSILON)
+		{
+			return false;
+		}
+		else if(v<-G_EPSILON)
+		{
+			return false;
+		}
+		else
+		{
+			float sumuv;
+			sumuv = u+v;
+			if(sumuv<-G_EPSILON)
+			{
+				return false;
+			}
+			else if(sumuv-1.0f>G_EPSILON)
+			{
+				return false;
+			}
+		}
+		return true;
+	}
+
+	//! is point in triangle beam?
+	/*!
+	Test if point is in triangle, with m_margin tolerance
+	*/
+	SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const
+	{
+		//Test with edge 0
+		btVector4 edge_plane;
+		this->get_edge_plane(0,tri_normal,edge_plane);
+		GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point);
+		if(dist-m_margin>0.0f) return false; // outside plane
+
+		this->get_edge_plane(1,tri_normal,edge_plane);
+		dist = DISTANCE_PLANE_POINT(edge_plane,point);
+		if(dist-m_margin>0.0f) return false; // outside plane
+
+		this->get_edge_plane(2,tri_normal,edge_plane);
+		dist = DISTANCE_PLANE_POINT(edge_plane,point);
+		if(dist-m_margin>0.0f) return false; // outside plane
+		return true;
+	}
+
+
+	//! Bidireccional ray collision
+	SIMD_FORCE_INLINE bool ray_collision(
+		const btVector3 & vPoint,
+		const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
+		GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
+	{
+		btVector4 faceplane;
+		{
+			btVector3 dif1 = m_vertices[1] - m_vertices[0];
+			btVector3 dif2 = m_vertices[2] - m_vertices[0];
+    		VEC_CROSS(faceplane,dif1,dif2);
+    		faceplane[3] = m_vertices[0].dot(faceplane);
+		}
+
+		GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
+		if(res == 0) return false;
+		if(! is_point_inside(pout,faceplane)) return false;
+
+		if(res==2) //invert normal
+		{
+			triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]);
+		}
+		else
+		{
+			triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
+		}
+
+		VEC_NORMALIZE(triangle_normal);
+
+		return true;
+	}
+
+
+	//! one direccion ray collision
+	SIMD_FORCE_INLINE bool ray_collision_front_side(
+		const btVector3 & vPoint,
+		const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
+		GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
+	{
+		btVector4 faceplane;
+		{
+			btVector3 dif1 = m_vertices[1] - m_vertices[0];
+			btVector3 dif2 = m_vertices[2] - m_vertices[0];
+    		VEC_CROSS(faceplane,dif1,dif2);
+    		faceplane[3] = m_vertices[0].dot(faceplane);
+		}
+
+		GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
+		if(res != 1) return false;
+
+		if(!is_point_inside(pout,faceplane)) return false;
+
+		triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
+
+		VEC_NORMALIZE(triangle_normal);
+
+		return true;
+	}
+
+};
+
+
+
+//! @}
+
+#endif // GIM_TRI_COLLISION_H_INCLUDED

src/BulletMultiThreaded/CMakeLists.txt

@@ -0,0 +1,66 @@
+INCLUDE_DIRECTORIES(
+${BULLET_PHYSICS_SOURCE_DIR}/Extras/BulletMultiThreaded/
+${BULLET_PHYSICS_SOURCE_DIR}/src
+)
+
+ADD_LIBRARY(LibBulletMultiThreaded
+		PlatformDefinitions.h
+		SpuFakeDma.cpp
+		SpuFakeDma.h
+		SpuSync.h
+		SpuDoubleBuffer.h
+		SpuLibspe2Support.cpp
+		SpuLibspe2Support.h
+		btThreadSupportInterface.cpp
+		btThreadSupportInterface.h
+		
+		Win32ThreadSupport.cpp
+		Win32ThreadSupport.h
+		PosixThreadSupport.cpp
+		PosixThreadSupport.h
+		SequentialThreadSupport.cpp
+		SequentialThreadSupport.h
+		SpuSampleTaskProcess.h
+		SpuSampleTaskProcess.cpp
+
+		SpuCollisionObjectWrapper.cpp 
+		SpuCollisionObjectWrapper.h 
+		SpuCollisionTaskProcess.h
+		SpuCollisionTaskProcess.cpp
+		SpuGatheringCollisionDispatcher.h
+		SpuGatheringCollisionDispatcher.cpp
+		SpuContactManifoldCollisionAlgorithm.cpp
+		SpuContactManifoldCollisionAlgorithm.h
+		SpuNarrowPhaseCollisionTask/SpuContactResult.cpp
+		SpuNarrowPhaseCollisionTask/SpuContactResult.h
+		SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp
+		SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp
+		SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h
+		SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp
+		SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h
+		SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h
+		SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h
+		SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h
+		SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp
+		SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h
+		SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp
+		SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h
+		SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp
+		SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h
+		SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp
+		SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h
+
+		SpuParallelSolver.cpp
+		SpuParallelSolver.h
+		SpuSolverTask/SpuParallellSolverTask.cpp
+		SpuSolverTask/SpuParallellSolverTask.h
+
+		SpuBatchRaycaster.cpp
+		SpuBatchRaycaster.h
+		SpuRaycastTaskProcess.cpp
+		SpuRaycastTaskProcess.h
+		SpuRaycastTask/SpuRaycastTask.cpp
+		SpuRaycastTask/SpuRaycastTask.h
+		SpuRaycastTask/SpuSubSimplexConvexCast.cpp
+		SpuRaycastTask/SpuSubSimplexConvexCast.h
+)

src/BulletMultiThreaded/Jamfile

@@ -0,0 +1,12 @@
+SubDir TOP Extras BulletMultiThreaded ;
+
+#IncludeDir Extras/BulletMultiThreaded ;
+
+Library bulletmultithreaded : [ Wildcard . : */.h *.cpp ] [ Wildcard SpuNarrowPhaseCollisionTask : *.h *.cpp  ] [ Wildcard SpuSolverTask : *.h *.cpp  ] : noinstall ;
+CFlags bulletmultithreaded : [ FIncludes $(TOP)/Extras/BulletMultiThreaded ] ;
+LibDepends bulletmultithreaded :  ;
+
+   MsvcIncDirs bulletmultithreaded : 
+	"../../Extras/BulletMultiThreaded"  ;
+
+InstallHeader [ Wildcard *.h ] : bulletmultithreaded ;

src/BulletMultiThreaded/Makefile.original

@@ -0,0 +1,177 @@
+__ARCH_BITS__ := 32
+
+# define macros
+NARROWPHASEDIR=./SpuNarrowPhaseCollisionTask
+SPU_TASKFILE=$(NARROWPHASEDIR)/SpuGatheringCollisionTask
+
+IBM_CELLSDK_VERSION := $(shell if [ -d /opt/cell ]; then echo "3.0"; fi)
+
+ifeq ("$(IBM_CELLSDK_VERSION)","3.0")
+        CELL_TOP ?= /opt/cell/sdk
+	CELL_SYSROOT := /opt/cell/sysroot
+else
+        CELL_TOP ?= /opt/ibm/cell-sdk/prototype
+	CELL_SYSROOT := $(CELL_TOP)/sysroot
+endif
+
+
+USE_CCACHE=ccache
+RM=rm -f 
+OUTDIR=./out
+DEBUGFLAG=-DNDEBUG
+LIBOUTDIR=../../lib/ibmsdk
+COLLISIONDIR=../../src/BulletCollision
+MATHDIR=../../src/LinearMath
+ARCHITECTUREFLAG=-m$(__ARCH_BITS__)
+ifeq "$(__ARCH_BITS__)" "64"
+  SPU_DEFFLAGS= -DUSE_LIBSPE2 -D__SPU__ -DUSE_ADDR64
+else
+  SPU_DEFFLAGS= -DUSE_LIBSPE2 -D__SPU__
+endif
+SPU_GCC=$(USE_CCACHE) /usr/bin/spu-gcc
+SPU_INCLUDEDIR= -I. -I$(CELL_SYSROOT)/usr/spu/include -I../../src -I$(NARROWPHASEDIR)
+#SPU_CFLAGS= $(DEBUGFLAG) -W -Wall -Winline -Os -c -include spu_intrinsics.h -include stdbool.h
+SPU_CFLAGS= $(DEBUGFLAG) -W -Wall -Winline -O3 -mbranch-hints -fomit-frame-pointer -ftree-vectorize -finline-functions -ftree-vect-loop-version -ftree-loop-optimize -ffast-math -fno-rtti -fno-exceptions -c -include spu_intrinsics.h -include stdbool.h
+
+SPU_LFLAGS= -Wl,-N
+SPU_LIBRARIES=-lstdc++
+SPU_EMBED=/usr/bin/ppu-embedspu
+SPU_AR=/usr/bin/ar
+SYMBOLNAME=spu_program
+
+ifeq "$(__ARCH_BITS__)" "64"
+  PPU_DEFFLAGS= -DUSE_LIBSPE2 -DUSE_ADDR64
+  PPU_GCC=$(USE_CCACHE) /usr/bin/ppu-gcc
+else
+  PPU_DEFFLAGS= -DUSE_LIBSPE2
+  PPU_GCC=$(USE_CCACHE) /usr/bin/ppu32-gcc
+endif
+
+PPU_CFLAGS= $(ARCHITECTUREFLAG) $(DEBUGFLAG) -W -Wall -Winline -O3 -c -mabi=altivec -maltivec -include altivec.h -include stdbool.h
+PPU_INCLUDEDIR= -I. -I$(CELL_SYSROOT)/usr/include -I../../src -I$(NARROWPHASEDIR)
+PPU_LFLAGS= $(ARCHITECTUREFLAG) -Wl,-m,elf$(__ARCH_BITS__)ppc
+PPU_LIBRARIES= -lstdc++ -lsupc++ -lgcc -lgcov -lspe2 -lpthread -L../../lib/ibmsdk -lbulletcollision -lbulletdynamics -lbulletmath -L$(CELL_SYSROOT)/usr/lib$(__ARCH_BITS__) -R$(CELL_SYSROOT)/usr/lib
+PPU_AR=/usr/bin/ar
+
+MakeOut :
+#	rm -f -R $(OUTDIR) ; mkdir $(OUTDIR)
+	@echo "usage: make spu, make ppu, make all, or make clean"
+# SPU
+SpuTaskFile : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/SpuTaskFile.o $(SPU_TASKFILE).cpp
+
+SpuFakeDma : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $@.cpp
+
+SpuContactManifoldCollisionAlgorithm_spu : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o SpuContactManifoldCollisionAlgorithm.cpp
+
+SpuCollisionShapes : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(NARROWPHASEDIR)/$@.cpp
+
+SpuContactResult : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(NARROWPHASEDIR)/$@.cpp
+
+#SpuGatheringCollisionTask : MakeOut
+#	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(NARROWPHASEDIR)/$@.cpp
+
+SpuGjkPairDetector: MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(NARROWPHASEDIR)/$@.cpp
+
+SpuMinkowskiPenetrationDepthSolver : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(NARROWPHASEDIR)/$@.cpp
+
+SpuVoronoiSimplexSolver : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(NARROWPHASEDIR)/$@.cpp
+
+#SpuLibspe2Support_spu : MakeOut
+#	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o SpuLibspe2Support.cpp
+
+## SPU-Bullet
+btPersistentManifold : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(COLLISIONDIR)/NarrowPhaseCollision/$@.cpp
+
+btOptimizedBvh : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(COLLISIONDIR)/CollisionShapes/$@.cpp
+
+btCollisionObject : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(COLLISIONDIR)/CollisionDispatch/$@.cpp
+
+btTriangleCallback : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(COLLISIONDIR)/CollisionShapes/$@.cpp
+
+btTriangleIndexVertexArray : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(COLLISIONDIR)/CollisionShapes/$@.cpp
+
+btStridingMeshInterface : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(COLLISIONDIR)/CollisionShapes/$@.cpp
+
+btAlignedAllocator : MakeOut
+	$(SPU_GCC) $(SPU_DEFFLAGS) $(SPU_CFLAGS) $(SPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $(MATHDIR)/$@.cpp
+
+
+# PPU
+SpuGatheringCollisionDispatcher : MakeOut
+	$(PPU_GCC) $(PPU_DEFFLAGS) $(PPU_CFLAGS) $(PPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $@.cpp
+
+SpuLibspe2Support: MakeOut
+	$(PPU_GCC) $(PPU_DEFFLAGS) $(PPU_CFLAGS) $(PPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $@.cpp
+
+btThreadSupportInterface: MakeOut
+	$(PPU_GCC) $(PPU_DEFFLAGS) $(PPU_CFLAGS) $(PPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $@.cpp
+
+SpuCollisionTaskProcess : MakeOut
+	$(PPU_GCC) $(PPU_DEFFLAGS) $(PPU_CFLAGS) $(PPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $@.cpp
+
+SpuContactManifoldCollisionAlgorithm : MakeOut
+	$(PPU_GCC) $(PPU_DEFFLAGS) $(PPU_CFLAGS) $(PPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $@.cpp
+	
+SpuSampleTaskProcess : MakeOut
+	$(PPU_GCC) $(PPU_DEFFLAGS) $(PPU_CFLAGS) $(PPU_INCLUDEDIR) -o $(OUTDIR)/$@.o $@.cpp
+	
+
+
+spu : SpuFakeDma SpuContactManifoldCollisionAlgorithm_spu SpuContactResult SpuTaskFile \
+      SpuGjkPairDetector SpuMinkowskiPenetrationDepthSolver SpuVoronoiSimplexSolver SpuCollisionShapes \
+      btPersistentManifold btOptimizedBvh btCollisionObject btTriangleCallback btTriangleIndexVertexArray \
+      btStridingMeshInterface btAlignedAllocator
+	$(SPU_GCC) -o $(OUTDIR)/spuCollision.elf \
+                            $(OUTDIR)/SpuTaskFile.o \
+                            $(OUTDIR)/SpuFakeDma.o \
+                            $(OUTDIR)/SpuContactManifoldCollisionAlgorithm_spu.o \
+                            $(OUTDIR)/SpuContactResult.o \
+                            $(OUTDIR)/SpuCollisionShapes.o \
+                            $(OUTDIR)/SpuGjkPairDetector.o \
+                            $(OUTDIR)/SpuMinkowskiPenetrationDepthSolver.o \
+                            $(OUTDIR)/SpuVoronoiSimplexSolver.o \
+                            $(OUTDIR)/btPersistentManifold.o \
+                            $(OUTDIR)/btTriangleCallback.o \
+                            $(OUTDIR)/btTriangleIndexVertexArray.o \
+                            $(OUTDIR)/btStridingMeshInterface.o \
+                            $(OUTDIR)/btAlignedAllocator.o \
+                            $(SPU_LFLAGS) $(SPU_LIBRARIES)
+
+spu-embed : spu
+	$(SPU_EMBED) $(ARCHITECTUREFLAG) $(SYMBOLNAME) $(OUTDIR)/spuCollision.elf $(OUTDIR)/$@.o
+	$(SPU_AR) -qcs $(LIBOUTDIR)/libspu.a $(OUTDIR)/$@.o
+
+
+
+ppu : SpuGatheringCollisionDispatcher SpuCollisionTaskProcess btThreadSupportInterface \
+      SpuLibspe2Support SpuContactManifoldCollisionAlgorithm SpuSampleTaskProcess
+	$(PPU_AR) -qcs $(LIBOUTDIR)/bulletmultithreaded.a \
+                                                          $(OUTDIR)/SpuCollisionTaskProcess.o \
+                                                          $(OUTDIR)/SpuSampleTaskProcess.o \
+                                                          $(OUTDIR)/SpuGatheringCollisionDispatcher.o \
+                                                          $(OUTDIR)/SpuLibspe2Support.o \
+                                                          $(OUTDIR)/btThreadSupportInterface.o \
+							  $(OUTDIR)/SpuContactManifoldCollisionAlgorithm.o
+
+all : spu-embed ppu 
+
+clean:
+	$(RM) $(OUTDIR)/* ; $(RM) $(LIBOUTDIR)/libspu.a ; $(RM) $(LIBOUTDIR)/bulletmultithreaded.a
+
+
+
+

src/BulletMultiThreaded/PlatformDefinitions.h

@@ -0,0 +1,82 @@
+#ifndef TYPE_DEFINITIONS_H
+#define TYPE_DEFINITIONS_H
+
+///This file provides some platform/compiler checks for common definitions
+
+#ifdef WIN32
+
+typedef union
+{
+  unsigned int u;
+  void *p;
+} addr64;
+
+#define USE_WIN32_THREADING 1
+
+		#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
+		#else
+		#endif //__MINGW32__
+
+		typedef unsigned char     uint8_t;
+#ifndef __PHYSICS_COMMON_H__
+		typedef unsigned long int uint64_t;
+		typedef unsigned int      uint32_t;
+#endif //__PHYSICS_COMMON_H__
+		typedef unsigned short    uint16_t;
+
+		#include <malloc.h>
+		#define memalign(alignment, size) malloc(size);
+			
+#include <string.h> //memcpy
+
+		
+
+		#include <stdio.h>		
+		#define spu_printf printf
+		
+#else
+		#include <stdint.h>
+		#include <stdlib.h>
+		#include <string.h> //for memcpy
+
+#if defined	(__CELLOS_LV2__)
+	// Playstation 3 Cell SDK
+#include <spu_printf.h>
+		
+#else
+	// posix system
+
+#define USE_PTHREADS    (1)
+
+#ifdef USE_LIBSPE2
+#include <stdio.h>		
+#define spu_printf printf	
+#define DWORD unsigned int
+		
+			typedef union
+			{
+			  unsigned long long ull;
+			  unsigned int ui[2];
+			  void *p;
+			} addr64;
+		
+		
+#else
+
+#include <stdio.h>		
+#define spu_printf printf	
+
+#endif // USE_LIBSPE2
+	
+#endif	//__CELLOS_LV2__
+	
+#endif
+
+
+/* Included here because we need uint*_t typedefs */
+#include "PpuAddressSpace.h"
+
+#endif //TYPE_DEFINITIONS_H
+
+
+

src/BulletMultiThreaded/PosixThreadSupport.cpp

@@ -0,0 +1,211 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 <stdio.h>
+#include "PosixThreadSupport.h"
+
+
+#ifdef USE_PTHREADS
+
+#include "SpuCollisionTaskProcess.h"
+#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+
+#define checkPThreadFunction(returnValue) \
+    if(0 != returnValue) { \
+        printf("PThread problem at line %i in file %s: %i\n", __LINE__, __FILE__, returnValue); \
+    }
+
+// The number of threads should be equal to the number of available cores
+// Todo: each worker should be linked to a single core, using SetThreadIdealProcessor.
+
+// PosixThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+// Setup and initialize SPU/CELL/Libspe2
+PosixThreadSupport::PosixThreadSupport(ThreadConstructionInfo& threadConstructionInfo)
+{
+	startThreads(threadConstructionInfo);
+}
+
+// cleanup/shutdown Libspe2
+PosixThreadSupport::~PosixThreadSupport()
+{
+	stopSPU();
+}
+
+// this semaphore will signal, if and how many threads are finished with their work
+static sem_t mainSemaphore;
+
+static void *threadFunction(void *argument) 
+{
+
+	PosixThreadSupport::btSpuStatus* status = (PosixThreadSupport::btSpuStatus*)argument;
+
+	
+	while (1)
+	{
+            checkPThreadFunction(sem_wait(&status->startSemaphore));
+		
+		void* userPtr = status->m_userPtr;
+
+		if (userPtr)
+		{
+			btAssert(status->m_status);
+			status->m_userThreadFunc(userPtr,status->m_lsMemory);
+			status->m_status = 2;
+			checkPThreadFunction(sem_post(&mainSemaphore));
+
+            status->threadUsed++;
+		} else {
+			//exit Thread
+			status->m_status = 3;
+			checkPThreadFunction(sem_post(&mainSemaphore));
+			printf("Thread with taskId %i exiting\n",status->m_taskId);
+			break;
+		}
+		
+	}
+
+	printf("Thread TERMINATED\n");
+	return 0;
+
+}
+
+///send messages to SPUs
+void PosixThreadSupport::sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t taskId)
+{
+	///	gMidphaseSPU.sendRequest(CMD_GATHER_AND_PROCESS_PAIRLIST, (uint32_t) &taskDesc);
+	
+	///we should spawn an SPU task here, and in 'waitForResponse' it should wait for response of the (one of) the first tasks that finished
+	
+
+
+	switch (uiCommand)
+	{
+	case 	CMD_GATHER_AND_PROCESS_PAIRLIST:
+		{
+			btSpuStatus&	spuStatus = m_activeSpuStatus[taskId];
+			btAssert(taskId >= 0);
+			btAssert(taskId < m_activeSpuStatus.size());
+
+			spuStatus.m_commandId = uiCommand;
+			spuStatus.m_status = 1;
+			spuStatus.m_userPtr = (void*)uiArgument0;
+
+			// fire event to start new task
+            checkPThreadFunction(sem_post(&spuStatus.startSemaphore));
+			break;
+		}
+	default:
+		{
+			///not implemented
+			btAssert(0);
+		}
+
+	};
+
+
+}
+
+
+///check for messages from SPUs
+void PosixThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
+{
+	///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response
+	
+	///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback'
+
+
+	btAssert(m_activeSpuStatus.size());
+
+        // wait for any of the threads to finish
+        checkPThreadFunction(sem_wait(&mainSemaphore));
+        
+	// get at least one thread which has finished
+        size_t last = -1;
+        
+        for(size_t t=0; t < m_activeSpuStatus.size(); ++t) {
+            if(2 == m_activeSpuStatus[t].m_status) {
+                last = t;
+                break;
+            }
+        }
+
+	btSpuStatus& spuStatus = m_activeSpuStatus[last];
+
+	btAssert(spuStatus.m_status > 1);
+	spuStatus.m_status = 0;
+
+	// need to find an active spu
+	btAssert(last >= 0);
+
+	*puiArgument0 = spuStatus.m_taskId;
+	*puiArgument1 = spuStatus.m_status;
+}
+
+
+
+void PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructionInfo)
+{
+        printf("%s creating %i threads.\n", __FUNCTION__, threadConstructionInfo.m_numThreads);
+	m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads);
+        
+        checkPThreadFunction(sem_init(&mainSemaphore, 0, 0));
+
+	for (int i=0;i < threadConstructionInfo.m_numThreads;i++)
+	{
+		printf("starting thread %d\n",i);
+
+		btSpuStatus&	spuStatus = m_activeSpuStatus[i];
+                
+                checkPThreadFunction(sem_init(&spuStatus.startSemaphore, 0, 0));
+                checkPThreadFunction(pthread_create(&spuStatus.thread, NULL, &threadFunction, (void*)&spuStatus));
+
+		spuStatus.m_userPtr=0;
+
+		spuStatus.m_taskId = i;
+		spuStatus.m_commandId = 0;
+		spuStatus.m_status = 0;
+		spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
+		spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
+        spuStatus.threadUsed = 0;
+
+		printf("started thread %d \n",i);
+		
+	}
+
+}
+
+void PosixThreadSupport::startSPU()
+{
+}
+
+
+///tell the task scheduler we are done with the SPU tasks
+void PosixThreadSupport::stopSPU()
+{
+	for(size_t t=0; t < m_activeSpuStatus.size(); ++t) {
+            btSpuStatus&	spuStatus = m_activeSpuStatus[t];
+        printf("%s: Thread %i used: %ld\n", __FUNCTION__, t, spuStatus.threadUsed);
+        
+        
+            checkPThreadFunction(sem_destroy(&spuStatus.startSemaphore));
+            checkPThreadFunction(pthread_cancel(spuStatus.thread));
+        }
+        checkPThreadFunction(sem_destroy(&mainSemaphore));
+
+	m_activeSpuStatus.clear();
+}
+
+#endif // USE_PTHREADS
+

src/BulletMultiThreaded/PosixThreadSupport.h

@@ -0,0 +1,118 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "LinearMath/btScalar.h"
+#include "PlatformDefinitions.h"
+
+#ifdef USE_PTHREADS  //platform specific defines are defined in PlatformDefinitions.h
+#include <pthread.h>
+#include <semaphore.h>
+
+#ifndef POSIX_THREAD_SUPPORT_H
+#define POSIX_THREAD_SUPPORT_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btThreadSupportInterface.h"
+
+
+typedef void (*PosixThreadFunc)(void* userPtr,void* lsMemory);
+typedef void* (*PosixlsMemorySetupFunc)();
+
+// PosixThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+class PosixThreadSupport : public btThreadSupportInterface 
+{
+public:
+    typedef enum sStatus {
+        STATUS_BUSY,
+        STATUS_READY,
+        STATUS_FINISHED
+    } Status;
+
+	// placeholder, until libspe2 support is there
+	struct	btSpuStatus
+	{
+		uint32_t	m_taskId;
+		uint32_t	m_commandId;
+		uint32_t	m_status;
+
+		PosixThreadFunc	m_userThreadFunc;
+		void*	m_userPtr; //for taskDesc etc
+		void*	m_lsMemory; //initialized using PosixLocalStoreMemorySetupFunc
+
+                pthread_t thread;
+                sem_t startSemaphore;
+
+        unsigned long threadUsed;
+	};
+private:
+
+	btAlignedObjectArray<btSpuStatus>	m_activeSpuStatus;
+public:
+	///Setup and initialize SPU/CELL/Libspe2
+
+	
+
+	struct	ThreadConstructionInfo
+	{
+		ThreadConstructionInfo(char* uniqueName,
+									PosixThreadFunc userThreadFunc,
+									PosixlsMemorySetupFunc	lsMemoryFunc,
+									int numThreads=1,
+									int threadStackSize=65535
+									)
+									:m_uniqueName(uniqueName),
+									m_userThreadFunc(userThreadFunc),
+									m_lsMemoryFunc(lsMemoryFunc),
+									m_numThreads(numThreads),
+									m_threadStackSize(threadStackSize)
+		{
+
+		}
+
+		char*					m_uniqueName;
+		PosixThreadFunc			m_userThreadFunc;
+		PosixlsMemorySetupFunc	m_lsMemoryFunc;
+		int						m_numThreads;
+		int						m_threadStackSize;
+
+	};
+
+	PosixThreadSupport(ThreadConstructionInfo& threadConstructionInfo);
+
+///cleanup/shutdown Libspe2
+	virtual	~PosixThreadSupport();
+
+	void	startThreads(ThreadConstructionInfo&	threadInfo);
+
+
+///send messages to SPUs
+	virtual	void sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t uiArgument1);
+
+///check for messages from SPUs
+	virtual	void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
+
+///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+	virtual	void startSPU();
+
+///tell the task scheduler we are done with the SPU tasks
+	virtual	void stopSPU();
+
+};
+
+#endif // POSIX_THREAD_SUPPORT_H
+
+#endif // USE_PTHREADS

src/BulletMultiThreaded/PpuAddressSpace.h

@@ -0,0 +1,11 @@
+#ifndef __PPU_ADDRESS_SPACE_H
+#define __PPU_ADDRESS_SPACE_H
+
+#ifdef USE_ADDR64
+typedef uint64_t ppu_address_t;
+#else
+typedef uint32_t ppu_address_t;
+#endif
+
+#endif
+

src/BulletMultiThreaded/SequentialThreadSupport.cpp

@@ -0,0 +1,89 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 "SequentialThreadSupport.h"
+
+
+#include "SpuCollisionTaskProcess.h"
+#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+
+SequentialThreadSupport::SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo)
+{
+	startThreads(threadConstructionInfo);
+}
+
+///cleanup/shutdown Libspe2
+SequentialThreadSupport::~SequentialThreadSupport()
+{
+	stopSPU();
+}
+
+#include <stdio.h>
+
+///send messages to SPUs
+void SequentialThreadSupport::sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t taskId)
+{
+	switch (uiCommand)
+	{
+	case 	CMD_GATHER_AND_PROCESS_PAIRLIST:
+		{
+			btSpuStatus&	spuStatus = m_activeSpuStatus[0];
+			spuStatus.m_userPtr=(void*)uiArgument0;
+			spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory);
+		}
+	break;
+	default:
+		{
+			///not implemented
+			btAssert(0 && "Not implemented");
+		}
+
+	};
+
+
+}
+
+///check for messages from SPUs
+void SequentialThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
+{
+	btAssert(m_activeSpuStatus.size());
+	btSpuStatus& spuStatus = m_activeSpuStatus[0];
+	*puiArgument0 = spuStatus.m_taskId;
+	*puiArgument1 = spuStatus.m_status;
+}
+
+void SequentialThreadSupport::startThreads(SequentialThreadConstructionInfo& threadConstructionInfo)
+{
+	m_activeSpuStatus.resize(1);
+	printf("STS: Not starting any threads\n");
+	btSpuStatus& spuStatus = m_activeSpuStatus[0];
+	spuStatus.m_userPtr = 0;
+	spuStatus.m_taskId = 0;
+	spuStatus.m_commandId = 0;
+	spuStatus.m_status = 0;
+	spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
+	spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
+	printf("STS: Created local store at %p for task %s\n", spuStatus.m_lsMemory, threadConstructionInfo.m_uniqueName);
+}
+
+void SequentialThreadSupport::startSPU()
+{
+}
+
+void SequentialThreadSupport::stopSPU()
+{
+	m_activeSpuStatus.clear();
+}
+

src/BulletMultiThreaded/SequentialThreadSupport.h

@@ -0,0 +1,84 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btScalar.h"
+#include "PlatformDefinitions.h"
+
+
+#ifndef SEQUENTIAL_THREAD_SUPPORT_H
+#define SEQUENTIAL_THREAD_SUPPORT_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btThreadSupportInterface.h"
+
+typedef void (*SequentialThreadFunc)(void* userPtr,void* lsMemory);
+typedef void* (*SequentiallsMemorySetupFunc)();
+
+
+
+///SequentialThreadSupport is a portable non-parallel implementation of the btThreadSupportInterface
+class SequentialThreadSupport : public btThreadSupportInterface 
+{
+public:
+	struct	btSpuStatus
+	{
+		uint32_t	m_taskId;
+		uint32_t	m_commandId;
+		uint32_t	m_status;
+
+		SequentialThreadFunc	m_userThreadFunc;
+
+		void*	m_userPtr; //for taskDesc etc
+		void*	m_lsMemory; //initialized using SequentiallsMemorySetupFunc
+	};
+private:
+	btAlignedObjectArray<btSpuStatus>	m_activeSpuStatus;
+	btAlignedObjectArray<void*>			m_completeHandles;	
+public:
+	struct	SequentialThreadConstructionInfo
+	{
+		SequentialThreadConstructionInfo (char* uniqueName,
+									SequentialThreadFunc userThreadFunc,
+									SequentiallsMemorySetupFunc	lsMemoryFunc
+									)
+									:m_uniqueName(uniqueName),
+									m_userThreadFunc(userThreadFunc),
+									m_lsMemoryFunc(lsMemoryFunc)
+		{
+
+		}
+
+		char*						m_uniqueName;
+		SequentialThreadFunc		m_userThreadFunc;
+		SequentiallsMemorySetupFunc	m_lsMemoryFunc;
+	};
+
+	SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo);
+	virtual	~SequentialThreadSupport();
+	void	startThreads(SequentialThreadConstructionInfo&	threadInfo);
+///send messages to SPUs
+	virtual	void sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t uiArgument1);
+///check for messages from SPUs
+	virtual	void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
+///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+	virtual	void startSPU();
+///tell the task scheduler we are done with the SPU tasks
+	virtual	void stopSPU();
+
+};
+
+#endif //SEQUENTIAL_THREAD_SUPPORT_H
+

src/BulletMultiThreaded/SpuBatchRaycaster.cpp

@@ -0,0 +1,151 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 <new>
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "SpuBatchRaycaster.h"
+
+SpuBatchRaycaster::SpuBatchRaycaster (class	btThreadSupportInterface* threadInterface, int maxNumOutstandingTasks)
+{
+	m_threadInterface = threadInterface;
+
+	castUponObjectWrappers = NULL;
+	numCastUponObjectWrappers = 0;
+
+	m_spuRaycastTaskProcess = new SpuRaycastTaskProcess(m_threadInterface,maxNumOutstandingTasks); // FIXME non constant
+}
+
+SpuBatchRaycaster::~SpuBatchRaycaster ()
+{
+	if (castUponObjectWrappers)
+	{
+		btAlignedFree (castUponObjectWrappers);
+		castUponObjectWrappers = NULL;
+	}
+}
+
+void
+SpuBatchRaycaster::setCollisionObjects (btCollisionObjectArray& castUponObjects, int numCastUponObjects)
+{
+	if (castUponObjectWrappers)
+	{
+		btAlignedFree (castUponObjectWrappers);
+		castUponObjectWrappers = NULL;
+	}
+
+	castUponObjectWrappers = (SpuCollisionObjectWrapper*)btAlignedAlloc (sizeof(SpuCollisionObjectWrapper) * numCastUponObjects,16);
+	numCastUponObjectWrappers = numCastUponObjects;
+
+	for (int i = 0; i < numCastUponObjectWrappers; i++)
+	{
+		castUponObjectWrappers[i] = SpuCollisionObjectWrapper(castUponObjects[i]);
+	}
+}
+
+void
+SpuBatchRaycaster::setCollisionObjectsSkipPE (btCollisionObjectArray& castUponObjects, int numCastUponObjects)
+{
+	if (castUponObjectWrappers)
+	{
+		btAlignedFree (castUponObjectWrappers);
+		castUponObjectWrappers = NULL;
+	}
+
+	int numNonPEShapes = 0;
+	for (int i = 0; i < numCastUponObjects; i++)
+	{
+		const btCollisionShape* shape = castUponObjects[i]->getCollisionShape();
+
+		if (shape->getShapeType () == BOX_SHAPE_PROXYTYPE ||
+			shape->getShapeType () == SPHERE_SHAPE_PROXYTYPE ||
+			shape->getShapeType () == CAPSULE_SHAPE_PROXYTYPE)
+		{
+			continue;
+		}
+
+		numNonPEShapes++;
+	}
+
+	castUponObjectWrappers = (SpuCollisionObjectWrapper*)btAlignedAlloc (sizeof(SpuCollisionObjectWrapper) * numNonPEShapes,16);
+	numCastUponObjectWrappers = numNonPEShapes;
+
+	int index = 0;
+	for (int i = 0; i < numCastUponObjects; i++)
+	{
+		const btCollisionShape* shape = castUponObjects[i]->getCollisionShape();
+
+		if (shape->getShapeType () == BOX_SHAPE_PROXYTYPE ||
+			shape->getShapeType () == SPHERE_SHAPE_PROXYTYPE ||
+			shape->getShapeType () == CAPSULE_SHAPE_PROXYTYPE)
+		{
+			continue;
+		}
+
+		castUponObjectWrappers[index] = SpuCollisionObjectWrapper(castUponObjects[i]);
+		index++;
+	}
+
+//	printf("Number of shapes bullet is casting against: %d\n", numNonPEShapes);
+	btAssert (index == numNonPEShapes);
+}
+
+void
+SpuBatchRaycaster::addRay (const btVector3& rayFrom, const btVector3& rayTo, const btScalar hitFraction)
+{
+	SpuRaycastTaskWorkUnitOut workUnitOut;
+	workUnitOut.hitFraction = hitFraction;
+	workUnitOut.hitNormal = btVector3(0.0, 1.0, 0.0);
+
+	rayBatchOutput.push_back (workUnitOut);
+
+	SpuRaycastTaskWorkUnit workUnit;
+	workUnit.rayFrom = rayFrom;
+	workUnit.rayTo = rayTo;
+	rayBatch.push_back (workUnit);
+}
+
+void
+SpuBatchRaycaster::clearRays ()
+{
+	rayBatch.clear ();
+	rayBatchOutput.clear ();
+}
+
+void
+SpuBatchRaycaster::performBatchRaycast ()
+{
+	m_spuRaycastTaskProcess->initialize2 (castUponObjectWrappers, numCastUponObjectWrappers);
+
+	for (int i = 0; i < rayBatch.size(); i++)
+	{
+		rayBatch[i].output = &rayBatchOutput[i]; // assign output memory location
+		m_spuRaycastTaskProcess->addWorkToTask(rayBatch[i]);
+	}
+
+	m_spuRaycastTaskProcess->flush2 ();
+}
+
+const SpuRaycastTaskWorkUnitOut&
+SpuBatchRaycaster::operator [] (int i) const
+{
+	return rayBatchOutput[i];
+}
+
+int
+SpuBatchRaycaster::getNumRays () const
+{
+	return rayBatchOutput.size();
+}

src/BulletMultiThreaded/SpuBatchRaycaster.h

@@ -0,0 +1,49 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 SPU_BATCH_RAYCASTER_H
+#define SPU_BATCH_RAYCASTER_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "SpuRaycastTaskProcess.h"
+#include "SpuRaycastTask/SpuRaycastTask.h"
+#include "SpuCollisionObjectWrapper.h"
+
+/* FIXME:
+ * Need to decide how callbacks are performed...
+ */
+class SpuBatchRaycaster 
+{
+protected:
+	SpuCollisionObjectWrapper* castUponObjectWrappers;
+	int numCastUponObjectWrappers;
+	btAlignedObjectArray<SpuRaycastTaskWorkUnit> rayBatch;
+	btAlignedObjectArray<SpuRaycastTaskWorkUnitOut> rayBatchOutput;
+	SpuRaycastTaskProcess* m_spuRaycastTaskProcess;
+	class	btThreadSupportInterface*	m_threadInterface;
+public:
+	SpuBatchRaycaster (class btThreadSupportInterface* threadInterface, int maxNumOutstandingTasks);
+	~SpuBatchRaycaster ();
+	void setCollisionObjects (btCollisionObjectArray& castUponObjects, int numCastUponObjects);
+	void setCollisionObjectsSkipPE (btCollisionObjectArray& castUponObjects, int numCastUponObjects);
+	void addRay (const btVector3& rayFrom, const btVector3& rayTo, const btScalar hitFraction = 1.0);
+	void clearRays ();
+	void performBatchRaycast ();
+	const SpuRaycastTaskWorkUnitOut& operator [] (int i) const;
+	int getNumRays () const;
+};
+
+#endif

src/BulletMultiThreaded/SpuCollisionObjectWrapper.cpp

@@ -0,0 +1,48 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 "SpuCollisionObjectWrapper.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+
+SpuCollisionObjectWrapper::SpuCollisionObjectWrapper ()
+{
+}
+
+#ifndef __SPU__
+SpuCollisionObjectWrapper::SpuCollisionObjectWrapper (const btCollisionObject* collisionObject)
+{
+	m_shapeType = collisionObject->getCollisionShape()->getShapeType ();
+	m_collisionObjectPtr = (ppu_address_t)collisionObject;
+	m_margin = collisionObject->getCollisionShape()->getMargin ();
+}
+#endif
+
+int
+SpuCollisionObjectWrapper::getShapeType () const
+{
+	return m_shapeType;
+}
+
+float
+SpuCollisionObjectWrapper::getCollisionMargin () const
+{
+	return m_margin;
+}
+
+ppu_address_t
+SpuCollisionObjectWrapper::getCollisionObjectPtr () const
+{
+	return m_collisionObjectPtr;
+}

src/BulletMultiThreaded/SpuCollisionObjectWrapper.h

@@ -0,0 +1,35 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 "PlatformDefinitions.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+ATTRIBUTE_ALIGNED16(class) SpuCollisionObjectWrapper
+{
+protected:
+	int m_shapeType;
+	float m_margin;
+	ppu_address_t m_collisionObjectPtr;
+
+public:
+	SpuCollisionObjectWrapper ();
+
+	SpuCollisionObjectWrapper (const btCollisionObject* collisionObject);
+
+	int           getShapeType () const;
+	float         getCollisionMargin () const;
+	ppu_address_t getCollisionObjectPtr () const;
+};
+

src/BulletMultiThreaded/SpuCollisionTaskProcess.cpp

@@ -0,0 +1,304 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+//#define DEBUG_SPU_TASK_SCHEDULING 1
+
+
+//class OptimizedBvhNode;
+
+#include "SpuCollisionTaskProcess.h"
+
+
+
+
+
+
+SpuCollisionTaskProcess::SpuCollisionTaskProcess(class	btThreadSupportInterface*	threadInterface, unsigned int	maxNumOutstandingTasks)
+:m_threadInterface(threadInterface),
+m_maxNumOutstandingTasks(maxNumOutstandingTasks)
+{
+	m_workUnitTaskBuffers = (unsigned char *)0;
+	m_taskBusy.resize(m_maxNumOutstandingTasks);
+	m_spuGatherTaskDesc.resize(m_maxNumOutstandingTasks);
+
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+	m_currentPage = 0;
+	m_currentPageEntry = 0;
+
+#ifdef DEBUG_SpuCollisionTaskProcess
+	m_initialized = false;
+#endif
+
+	m_threadInterface->startSPU();
+
+	//printf("sizeof vec_float4: %d\n", sizeof(vec_float4));
+	printf("sizeof SpuGatherAndProcessWorkUnitInput: %d\n", sizeof(SpuGatherAndProcessWorkUnitInput));
+
+}
+
+SpuCollisionTaskProcess::~SpuCollisionTaskProcess()
+{
+	
+	if (m_workUnitTaskBuffers != 0)
+	{
+		btAlignedFree(m_workUnitTaskBuffers);
+		m_workUnitTaskBuffers = 0;
+	}
+	
+
+
+	m_threadInterface->stopSPU();
+	
+}
+
+
+
+void SpuCollisionTaskProcess::initialize2(bool useEpa)
+{
+
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("SpuCollisionTaskProcess::initialize()\n");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	if (!m_workUnitTaskBuffers)
+	{
+		m_workUnitTaskBuffers = (unsigned char *)btAlignedAlloc(MIDPHASE_WORKUNIT_TASK_SIZE*m_maxNumOutstandingTasks, 128);
+	}
+
+	
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+	m_currentPage = 0;
+	m_currentPageEntry = 0;
+	m_useEpa = useEpa;
+
+#ifdef DEBUG_SpuCollisionTaskProcess
+	m_initialized = true;
+	assert(MIDPHASE_NUM_WORKUNITS_PER_TASK*sizeof(SpuGatherAndProcessWorkUnitInput) <= MIDPHASE_WORKUNIT_TASK_SIZE);
+#endif
+}
+
+
+void SpuCollisionTaskProcess::issueTask2()
+{
+
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("SpuCollisionTaskProcess::issueTask (m_currentTask= %d\n)", m_currentTask);
+#endif //DEBUG_SPU_TASK_SCHEDULING
+
+	m_taskBusy[m_currentTask] = true;
+	m_numBusyTasks++;
+
+
+	SpuGatherAndProcessPairsTaskDesc& taskDesc = m_spuGatherTaskDesc[m_currentTask];
+	taskDesc.m_useEpa = m_useEpa;
+
+	{
+		// send task description in event message
+		// no error checking here...
+		// but, currently, event queue can be no larger than NUM_WORKUNIT_TASKS.
+	
+		taskDesc.inPtr = reinterpret_cast<uint64_t>(MIDPHASE_TASK_PTR(m_currentTask));
+	
+		taskDesc.taskId = m_currentTask;
+		taskDesc.numPages = m_currentPage+1;
+		taskDesc.numOnLastPage = m_currentPageEntry;
+	}
+
+
+
+	m_threadInterface->sendRequest(CMD_GATHER_AND_PROCESS_PAIRLIST, (uint32_t) &taskDesc,m_currentTask);
+
+	// if all tasks busy, wait for spu event to clear the task.
+	
+
+	if (m_numBusyTasks >= m_maxNumOutstandingTasks)
+	{
+		unsigned int taskId;
+		unsigned int outputSize;
+
+		
+		for (int i=0;i<m_maxNumOutstandingTasks;i++)
+		  {
+			  if (m_taskBusy[i])
+			  {
+				  taskId = i;
+				  break;
+			  }
+		  }
+
+	  btAssert(taskId>=0);
+
+	  
+		m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+//		printf("issueTask taskId %d completed, numBusy=%d\n",taskId,m_numBusyTasks);
+
+		//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
+
+		//postProcess(taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	}
+	
+}
+
+void SpuCollisionTaskProcess::addWorkToTask(void* pairArrayPtr,int startIndex,int endIndex)
+{
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("#");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	
+#ifdef DEBUG_SpuCollisionTaskProcess
+	assert(m_initialized);
+	assert(m_workUnitTaskBuffers);
+
+#endif
+
+	bool batch = true;
+
+	if (batch)
+	{
+		if (m_currentPageEntry == MIDPHASE_NUM_WORKUNITS_PER_PAGE)
+		{
+			if (m_currentPage == MIDPHASE_NUM_WORKUNIT_PAGES-1)
+			{
+				// task buffer is full, issue current task.
+				// if all task buffers busy, this waits until SPU is done.
+				issueTask2();
+
+				// find new task buffer
+				for (unsigned int i = 0; i < m_maxNumOutstandingTasks; i++)
+				{
+					if (!m_taskBusy[i])
+					{
+						m_currentTask = i;
+						//init the task data
+
+						break;
+					}
+				}
+
+				m_currentPage = 0;
+			}
+			else
+			{
+				m_currentPage++;
+			}
+
+			m_currentPageEntry = 0;
+		}
+	}
+
+	{
+
+
+
+		SpuGatherAndProcessWorkUnitInput &wuInput = 
+			*(reinterpret_cast<SpuGatherAndProcessWorkUnitInput*>
+			(MIDPHASE_ENTRY_PTR(m_currentTask, m_currentPage, m_currentPageEntry)));
+		
+		wuInput.m_pairArrayPtr = reinterpret_cast<uint64_t>(pairArrayPtr);
+		wuInput.m_startIndex = startIndex;
+		wuInput.m_endIndex = endIndex;
+
+		
+	
+		m_currentPageEntry++;
+
+		if (!batch)
+		{
+			issueTask2();
+
+			// find new task buffer
+			for (unsigned int i = 0; i < m_maxNumOutstandingTasks; i++)
+			{
+				if (!m_taskBusy[i])
+				{
+					m_currentTask = i;
+					//init the task data
+
+					break;
+				}
+			}
+
+			m_currentPage = 0;
+			m_currentPageEntry =0;
+		}
+	}
+}
+
+
+void 
+SpuCollisionTaskProcess::flush2()
+{
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("\nSpuCollisionTaskProcess::flush()\n");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	
+	// if there's a partially filled task buffer, submit that task
+	if (m_currentPage > 0 || m_currentPageEntry > 0)
+	{
+		issueTask2();
+	}
+
+
+	// all tasks are issued, wait for all tasks to be complete
+	while(m_numBusyTasks > 0)
+	{
+	  // Consolidating SPU code
+	  unsigned int taskId=-1;
+	  unsigned int outputSize;
+	  
+	  for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+
+	  btAssert(taskId>=0);
+
+	
+	  {
+			
+		// SPURS support.
+		  m_threadInterface->waitForResponse(&taskId, &outputSize);
+	  }
+//		 printf("flush2 taskId %d completed, numBusy =%d \n",taskId,m_numBusyTasks);
+		//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
+
+		//postProcess(taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	}
+
+
+}

src/BulletMultiThreaded/SpuCollisionTaskProcess.h

@@ -0,0 +1,154 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 SPU_COLLISION_TASK_PROCESS_H
+#define SPU_COLLISION_TASK_PROCESS_H
+
+#include <assert.h>
+
+#include <LinearMath/btScalar.h>
+
+#include "PlatformDefinitions.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h" // for definitions processCollisionTask and createCollisionLocalStoreMemory
+
+#include "btThreadSupportInterface.h"
+
+
+//#include "SPUAssert.h"
+#include <string.h>
+
+
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+#include <LinearMath/btAlignedAllocator.h>
+
+#include <stdio.h>
+
+
+#define DEBUG_SpuCollisionTaskProcess 1
+
+
+#define CMD_GATHER_AND_PROCESS_PAIRLIST	1
+
+class btCollisionObject;
+class btPersistentManifold;
+class btDispatcher;
+
+
+/////Task Description for SPU collision detection
+//struct SpuGatherAndProcessPairsTaskDesc
+//{
+//	uint64_t	inPtr;//m_pairArrayPtr;
+//	//mutex variable
+//	uint32_t	m_someMutexVariableInMainMemory;
+//
+//	uint64_t	m_dispatcher;
+//
+//	uint32_t	numOnLastPage;
+//
+//	uint16_t numPages;
+//	uint16_t taskId;
+//
+//	struct	CollisionTask_LocalStoreMemory*	m_lsMemory; 
+//}
+//
+//#if  defined(__CELLOS_LV2__) || defined(USE_LIBSPE2)
+//__attribute__ ((aligned (16)))
+//#endif
+//;
+
+
+///MidphaseWorkUnitInput stores individual primitive versus mesh collision detection input, to be processed by the SPU.
+struct SpuGatherAndProcessWorkUnitInput
+{
+	uint64_t m_pairArrayPtr;
+	int		m_startIndex;
+	int		m_endIndex;
+};
+
+
+
+
+/// SpuCollisionTaskProcess handles SPU processing of collision pairs.
+/// Maintains a set of task buffers.
+/// When the task is full, the task is issued for SPUs to process.  Contact output goes into btPersistentManifold
+/// associated with each task.
+/// When PPU issues a task, it will look for completed task buffers
+/// PPU will do postprocessing, dependent on workunit output (not likely)
+class SpuCollisionTaskProcess
+{
+
+  unsigned char  *m_workUnitTaskBuffers;
+
+
+	// track task buffers that are being used, and total busy tasks
+	btAlignedObjectArray<bool>	m_taskBusy;
+	btAlignedObjectArray<SpuGatherAndProcessPairsTaskDesc>	m_spuGatherTaskDesc;
+
+	class	btThreadSupportInterface*	m_threadInterface;
+
+	unsigned int	m_maxNumOutstandingTasks;
+
+	unsigned int   m_numBusyTasks;
+
+	// the current task and the current entry to insert a new work unit
+	unsigned int   m_currentTask;
+	unsigned int   m_currentPage;
+	unsigned int   m_currentPageEntry;
+
+	bool m_useEpa;
+
+#ifdef DEBUG_SpuCollisionTaskProcess
+	bool m_initialized;
+#endif
+	void issueTask2();
+	//void postProcess(unsigned int taskId, int outputSize);
+
+public:
+	SpuCollisionTaskProcess(btThreadSupportInterface*	threadInterface, unsigned int maxNumOutstandingTasks);
+	
+	~SpuCollisionTaskProcess();
+	
+	///call initialize in the beginning of the frame, before addCollisionPairToTask
+	void initialize2(bool useEpa = false);
+
+	///batch up additional work to a current task for SPU processing. When batch is full, it issues the task.
+	void addWorkToTask(void* pairArrayPtr,int startIndex,int endIndex);
+
+	///call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished
+	void flush2();
+};
+
+
+
+#define MIDPHASE_TASK_PTR(task) (&m_workUnitTaskBuffers[0] + MIDPHASE_WORKUNIT_TASK_SIZE*task)
+#define MIDPHASE_ENTRY_PTR(task,page,entry) (MIDPHASE_TASK_PTR(task) + MIDPHASE_WORKUNIT_PAGE_SIZE*page + sizeof(SpuGatherAndProcessWorkUnitInput)*entry)
+#define MIDPHASE_OUTPUT_PTR(task) (&m_contactOutputBuffers[0] + MIDPHASE_MAX_CONTACT_BUFFER_SIZE*task)
+#define MIDPHASE_TREENODES_PTR(task) (&m_complexShapeBuffers[0] + MIDPHASE_COMPLEX_SHAPE_BUFFER_SIZE*task)
+
+
+#define MIDPHASE_WORKUNIT_PAGE_SIZE (16)
+
+#define MIDPHASE_NUM_WORKUNIT_PAGES 1
+#define MIDPHASE_WORKUNIT_TASK_SIZE (MIDPHASE_WORKUNIT_PAGE_SIZE*MIDPHASE_NUM_WORKUNIT_PAGES)
+#define MIDPHASE_NUM_WORKUNITS_PER_PAGE (MIDPHASE_WORKUNIT_PAGE_SIZE / sizeof(SpuGatherAndProcessWorkUnitInput))
+#define MIDPHASE_NUM_WORKUNITS_PER_TASK (MIDPHASE_NUM_WORKUNITS_PER_PAGE*MIDPHASE_NUM_WORKUNIT_PAGES)
+
+
+#endif // SPU_COLLISION_TASK_PROCESS_H
+

src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp

@@ -0,0 +1,58 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 "SpuContactManifoldCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+
+
+SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm()
+:m_manifoldPtr(0)
+{
+	
+}
+
+
+void SpuContactManifoldCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	btAssert(0);
+}
+
+btScalar SpuContactManifoldCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	btAssert(0);
+	return 1.f;
+}
+
+#ifndef __SPU__
+SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1)
+:btCollisionAlgorithm(ci)
+{
+	m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+	m_shapeType0 = body0->getCollisionShape()->getShapeType();
+	m_shapeType1 = body1->getCollisionShape()->getShapeType();
+	m_collisionMargin0 = body0->getCollisionShape()->getMargin();
+	m_collisionMargin1 = body1->getCollisionShape()->getMargin();
+
+}
+#endif //__SPU__
+
+
+SpuContactManifoldCollisionAlgorithm::~SpuContactManifoldCollisionAlgorithm()
+{
+	if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+}

src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h

@@ -0,0 +1,89 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
+#define SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+
+class btPersistentManifold;
+
+/// SpuContactManifoldCollisionAlgorithm  provides contact manifold and should be processed on SPU.
+ATTRIBUTE_ALIGNED16(class) SpuContactManifoldCollisionAlgorithm : public btCollisionAlgorithm
+{
+
+	btPersistentManifold*	m_manifoldPtr;
+	int		m_shapeType0;
+	int		m_shapeType1;
+	float	m_collisionMargin0;
+	float	m_collisionMargin1;
+
+	
+public:
+	
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	SpuContactManifoldCollisionAlgorithm();
+
+	SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~SpuContactManifoldCollisionAlgorithm();
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
+	btPersistentManifold*	getContactManifoldPtr()
+	{
+		return m_manifoldPtr;
+	}
+
+	int		getShapeType0() const
+	{
+		return m_shapeType0;
+	}
+
+	int		getShapeType1() const
+	{
+		return m_shapeType1;
+	}
+	float	getCollisionMargin0() const
+	{
+		return m_collisionMargin0;
+	}
+	float	getCollisionMargin1() const
+	{
+		return m_collisionMargin1;
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm));
+			return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1);
+		}
+	};
+
+};
+
+#endif //SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H

src/BulletMultiThreaded/SpuDoubleBuffer.h

@@ -0,0 +1,107 @@
+#ifndef DOUBLE_BUFFER_H
+#define DOUBLE_BUFFER_H
+
+#include "SpuFakeDma.h"
+#include <LinearMath/btScalar.h>
+
+
+///DoubleBuffer
+template<class T, int size>
+class DoubleBuffer
+{
+#if defined(__SPU__) || defined(USE_LIBSPE2)
+	ATTRIBUTE_ALIGNED128( T m_buffer0[size] ) ;
+	ATTRIBUTE_ALIGNED128( T m_buffer1[size] ) ;
+#else
+	T m_buffer0[size];
+	T m_buffer1[size];
+#endif
+	
+	T *m_frontBuffer;
+	T *m_backBuffer;
+
+	unsigned int m_dmaTag;
+	bool m_dmaPending;
+public:
+	bool	isPending() const { return m_dmaPending;}
+	DoubleBuffer();
+
+	void init ();
+
+	// dma get and put commands
+	void backBufferDmaGet(uint64_t ea, unsigned int numBytes, unsigned int tag);
+	void backBufferDmaPut(uint64_t ea, unsigned int numBytes, unsigned int tag);
+
+	// gets pointer to a buffer
+	T *getFront();
+	T *getBack();
+
+	// if back buffer dma was started, wait for it to complete
+	// then move back to front and vice versa
+	T *swapBuffers();
+};
+
+template<class T, int size>
+DoubleBuffer<T,size>::DoubleBuffer()
+{
+	init ();
+}
+
+template<class T, int size>
+void DoubleBuffer<T,size>::init()
+{
+	this->m_dmaPending = false;
+	this->m_frontBuffer = &this->m_buffer0[0];
+	this->m_backBuffer = &this->m_buffer1[0];
+}
+
+template<class T, int size>
+void
+DoubleBuffer<T,size>::backBufferDmaGet(uint64_t ea, unsigned int numBytes, unsigned int tag)
+{
+	m_dmaPending = true;
+	m_dmaTag = tag;
+	cellDmaLargeGet(m_backBuffer, ea, numBytes, tag, 0, 0);
+}
+
+template<class T, int size>
+void
+DoubleBuffer<T,size>::backBufferDmaPut(uint64_t ea, unsigned int numBytes, unsigned int tag)
+{
+	m_dmaPending = true;
+	m_dmaTag = tag;
+	cellDmaLargePut(m_backBuffer, ea, numBytes, tag, 0, 0);
+}
+
+template<class T, int size>
+T *
+DoubleBuffer<T,size>::getFront()
+{
+	return m_frontBuffer;
+}
+
+template<class T, int size>
+T *
+DoubleBuffer<T,size>::getBack()
+{
+	return m_backBuffer;
+}
+
+template<class T, int size>
+T *
+DoubleBuffer<T,size>::swapBuffers()
+{
+	if (m_dmaPending)
+	{
+		cellDmaWaitTagStatusAll(1<<m_dmaTag);
+		m_dmaPending = false;
+	}
+
+	T *tmp = m_backBuffer;
+	m_backBuffer = m_frontBuffer;
+	m_frontBuffer = tmp;
+
+	return m_frontBuffer;
+}
+
+#endif

src/BulletMultiThreaded/SpuFakeDma.cpp

@@ -0,0 +1,195 @@
+
+#include "SpuFakeDma.h"
+#include <LinearMath/btScalar.h> //for btAssert
+//Disabling memcpy sometimes helps debugging DMA
+
+#define USE_MEMCPY 1
+#ifdef USE_MEMCPY
+
+#endif
+
+
+void*	cellDmaLargeGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+
+#if defined (__CELLOS_LV2__) || defined (USE_LIBSPE2)
+	cellDmaLargeGet(ls,ea,size,tag,tid,rid);
+	return ls;
+#else
+	return (void*)(uint32_t)ea;
+#endif
+}
+
+void*	cellDmaSmallGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+#if defined (__SPU__) || defined (USE_LIBSPE2)
+	mfc_get(ls,ea,size,tag,0,0);
+	return ls;
+#else
+	return (void*)(uint32_t)ea;
+#endif
+}
+
+
+
+
+void*	cellDmaGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+#if defined (__SPU__) || defined (USE_LIBSPE2)
+	cellDmaGet(ls,ea,size,tag,tid,rid);
+	return ls;
+#else
+	return (void*)(uint32_t)ea;
+#endif
+}
+
+
+///this unalignedDma should not be frequently used, only for small data. It handles alignment and performs check on size (<16 bytes)
+int stallingUnalignedDmaSmallGet(void *ls, uint64_t ea, uint32_t size)
+{
+	
+	btAssert(size<32);
+	
+	ATTRIBUTE_ALIGNED16(char	tmpBuffer[32]);
+
+	char* mainMem = (char*)ea;
+	char* localStore = (char*)ls;
+	uint32_t i;
+	
+
+	///make sure last 4 bits are the same, for cellDmaSmallGet
+	uint32_t last4BitsOffset = ea & 0x0f;
+	char* tmpTarget = tmpBuffer + last4BitsOffset;
+	
+#if defined (__SPU__) || defined (USE_LIBSPE2)
+	
+	int remainingSize = size;
+
+//#define FORCE_cellDmaUnalignedGet 1
+#ifdef FORCE_cellDmaUnalignedGet
+	cellDmaUnalignedGet(tmpTarget,ea,size,DMA_TAG(1),0,0);
+#else
+	char* remainingTmpTarget = tmpTarget;
+	uint64_t remainingEa = ea;
+
+	while (remainingSize)
+	{
+		switch (remainingSize)
+		{
+		case 1:
+		case 2:
+		case 4:
+		case 8:
+		case 16:
+			{
+				mfc_get(remainingTmpTarget,remainingEa,remainingSize,DMA_TAG(1),0,0);
+				remainingSize=0;
+				break;
+			}
+		default:
+			{
+				//spu_printf("unaligned DMA with non-natural size:%d\n",remainingSize);
+				int actualSize = 0;
+
+				if (remainingSize > 16)
+					actualSize = 16;
+				else
+					if (remainingSize >8)
+						actualSize=8;
+					else
+						if (remainingSize >4)
+							actualSize=4;
+						else
+							if (remainingSize >2)
+								actualSize=2;
+				mfc_get(remainingTmpTarget,remainingEa,actualSize,DMA_TAG(1),0,0);
+				remainingSize-=actualSize;
+				remainingTmpTarget+=actualSize;
+				remainingEa += actualSize;
+			}
+		}
+	}
+#endif//FORCE_cellDmaUnalignedGet
+
+#else
+	//copy into final destination
+#ifdef USE_MEMCPY
+		memcpy(tmpTarget,mainMem,size);
+#else
+		for ( i=0;i<size;i++)
+		{
+			tmpTarget[i] = mainMem[i];
+		}
+#endif //USE_MEMCPY
+
+#endif
+
+	cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+	//this is slowish, perhaps memcpy on SPU is smarter?
+	for (i=0; btLikely( i<size );i++)
+	{
+		localStore[i] = tmpTarget[i];
+	}
+
+	return 0;
+}
+
+#if defined (__SPU__) || defined (USE_LIBSPE2)
+#else
+
+int	cellDmaLargeGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+	char* mainMem = (char*)ea;
+	char* localStore = (char*)ls;
+
+#ifdef USE_MEMCPY
+	memcpy(localStore,mainMem,size);
+#else
+	for (uint32_t i=0;i<size;i++)
+	{
+		localStore[i] = mainMem[i];
+	}
+#endif
+	return 0;
+}
+
+int	cellDmaGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+	char* mainMem = (char*)ea;
+	char* localStore = (char*)ls;
+#ifdef USE_MEMCPY
+	memcpy(localStore,mainMem,size);
+#else
+	for (uint32_t i=0;i<size;i++)
+	{
+		localStore[i] = mainMem[i];
+	}	
+#endif //#ifdef USE_MEMCPY
+	return 0;
+}
+
+int cellDmaLargePut(const void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
+{
+	char* mainMem = (char*)ea;
+	const char* localStore = (const char*)ls;
+#ifdef USE_MEMCPY
+	memcpy(mainMem,localStore,size);
+#else
+	for (uint32_t i=0;i<size;i++)
+	{
+		mainMem[i] = localStore[i];
+	}	
+#endif //#ifdef USE_MEMCPY
+
+	return 0;
+}
+
+
+
+void	cellDmaWaitTagStatusAll(int ignore)
+{
+
+}
+
+#endif

src/BulletMultiThreaded/SpuFakeDma.h

@@ -0,0 +1,121 @@
+
+#ifndef FAKE_DMA_H
+#define FAKE_DMA_H
+
+
+#include "PlatformDefinitions.h"
+#include "LinearMath/btScalar.h"
+
+
+#ifdef __SPU__
+
+#ifndef USE_LIBSPE2
+
+#include <cell/dma.h>
+#include <stdint.h>
+
+#define DMA_TAG(xfer) (xfer + 1)
+#define DMA_MASK(xfer) (1 << DMA_TAG(xfer))
+
+#else // !USE_LIBSPE2
+
+#define DMA_TAG(xfer) (xfer + 1)
+#define DMA_MASK(xfer) (1 << DMA_TAG(xfer))
+		
+#include <spu_mfcio.h>		
+		
+#define DEBUG_DMA		
+#ifdef DEBUG_DMA
+#define dUASSERT(a,b) if (!(a)) { printf(b);}
+#define uintsize ppu_address_t
+		
+#define cellDmaLargeGet(ls, ea, size, tag, tid, rid) if (  (((uintsize)ls%16) != ((uintsize)ea%16)) || ((((uintsize)ea%16) || ((uintsize)ls%16)) && (( ((uintsize)ls%16) != ((uintsize)size%16) ) || ( ((uintsize)ea%16) != ((uintsize)size%16) ) ) ) || ( ((uintsize)size%16) && ((uintsize)size!=1) && ((uintsize)size!=2) && ((uintsize)size!=4) && ((uintsize)size!=8) ) || (size >= 16384) || !(uintsize)ls || !(uintsize)ea) { \
+															dUASSERT( (((uintsize)ea % 16) == 0) || (size < 16), "XDR Address not aligned: "); \
+															dUASSERT( (((uintsize)ls % 16) == 0) || (size < 16), "LS Address not aligned: "); \
+															dUASSERT( ((((uintsize)ls % size) == 0) && (((uintsize)ea % size) == 0))  || (size > 16), "Not naturally aligned: "); \
+															dUASSERT((size == 1) || (size == 2) || (size == 4) || (size == 8) || ((size % 16) == 0), "size not a multiple of 16byte: "); \
+															dUASSERT(size < 16384, "size too big: "); \
+															dUASSERT( ((uintsize)ea%16)==((uintsize)ls%16), "wrong Quadword alignment of LS and EA: "); \
+	    													dUASSERT(ea != 0, "Nullpointer EA: "); dUASSERT(ls != 0, "Nullpointer LS: ");\
+															printf("GET %s:%d from: 0x%x, to: 0x%x - %d bytes\n", __FILE__, __LINE__, (unsigned int)ea,(unsigned int)ls,(unsigned int)size);\
+															} \
+															mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaGet(ls, ea, size, tag, tid, rid) if (  (((uintsize)ls%16) != ((uintsize)ea%16)) || ((((uintsize)ea%16) || ((uintsize)ls%16)) && (( ((uintsize)ls%16) != ((uintsize)size%16) ) || ( ((uintsize)ea%16) != ((uintsize)size%16) ) ) ) || ( ((uintsize)size%16) && ((uintsize)size!=1) && ((uintsize)size!=2) && ((uintsize)size!=4) && ((uintsize)size!=8) ) || (size >= 16384) || !(uintsize)ls || !(uintsize)ea) { \
+														dUASSERT( (((uintsize)ea % 16) == 0) || (size < 16), "XDR Address not aligned: "); \
+														dUASSERT( (((uintsize)ls % 16) == 0) || (size < 16), "LS Address not aligned: "); \
+														dUASSERT( ((((uintsize)ls % size) == 0) && (((uintsize)ea % size) == 0))  || (size > 16), "Not naturally aligned: "); \
+														dUASSERT((size == 1) || (size == 2) || (size == 4) || (size == 8) || ((size % 16) == 0), "size not a multiple of 16byte: "); \
+    													dUASSERT(size < 16384, "size too big: "); \
+														dUASSERT( ((uintsize)ea%16)==((uintsize)ls%16), "wrong Quadword alignment of LS and EA: "); \
+    													dUASSERT(ea != 0, "Nullpointer EA: "); dUASSERT(ls != 0, "Nullpointer LS: ");\
+    													printf("GET %s:%d from: 0x%x, to: 0x%x - %d bytes\n", __FILE__, __LINE__, (unsigned int)ea,(unsigned int)ls,(unsigned int)size);\
+														} \
+														mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaLargePut(ls, ea, size, tag, tid, rid) if (  (((uintsize)ls%16) != ((uintsize)ea%16)) || ((((uintsize)ea%16) || ((uintsize)ls%16)) && (( ((uintsize)ls%16) != ((uintsize)size%16) ) || ( ((uintsize)ea%16) != ((uintsize)size%16) ) ) ) || ( ((uintsize)size%16) && ((uintsize)size!=1) && ((uintsize)size!=2) && ((uintsize)size!=4) && ((uintsize)size!=8) ) || (size >= 16384) || !(uintsize)ls || !(uintsize)ea) { \
+															dUASSERT( (((uintsize)ea % 16) == 0) || (size < 16), "XDR Address not aligned: "); \
+															dUASSERT( (((uintsize)ls % 16) == 0) || (size < 16), "LS Address not aligned: "); \
+															dUASSERT( ((((uintsize)ls % size) == 0) && (((uintsize)ea % size) == 0))  || (size > 16), "Not naturally aligned: "); \
+															dUASSERT((size == 1) || (size == 2) || (size == 4) || (size == 8) || ((size % 16) == 0), "size not a multiple of 16byte: "); \
+        													dUASSERT(size < 16384, "size too big: "); \
+															dUASSERT( ((uintsize)ea%16)==((uintsize)ls%16), "wrong Quadword alignment of LS and EA: "); \
+        													dUASSERT(ea != 0, "Nullpointer EA: "); dUASSERT(ls != 0, "Nullpointer LS: ");\
+    														printf("PUT %s:%d from: 0x%x, to: 0x%x - %d bytes\n", __FILE__, __LINE__, (unsigned int)ls,(unsigned int)ea,(unsigned int)size); \
+															} \
+															mfc_put(ls, ea, size, tag, tid, rid)
+#define cellDmaSmallGet(ls, ea, size, tag, tid, rid) if (  (((uintsize)ls%16) != ((uintsize)ea%16)) || ((((uintsize)ea%16) || ((uintsize)ls%16)) && (( ((uintsize)ls%16) != ((uintsize)size%16) ) || ( ((uintsize)ea%16) != ((uintsize)size%16) ) ) ) || ( ((uintsize)size%16) && ((uintsize)size!=1) && ((uintsize)size!=2) && ((uintsize)size!=4) && ((uintsize)size!=8) ) || (size >= 16384) || !(uintsize)ls || !(uintsize)ea) { \
+																dUASSERT( (((uintsize)ea % 16) == 0) || (size < 16), "XDR Address not aligned: "); \
+																dUASSERT( (((uintsize)ls % 16) == 0) || (size < 16), "LS Address not aligned: "); \
+																dUASSERT( ((((uintsize)ls % size) == 0) && (((uintsize)ea % size) == 0))  || (size > 16), "Not naturally aligned: "); \
+    															dUASSERT((size == 1) || (size == 2) || (size == 4) || (size == 8) || ((size % 16) == 0), "size not a multiple of 16byte: "); \
+    															dUASSERT(size < 16384, "size too big: "); \
+    															dUASSERT( ((uintsize)ea%16)==((uintsize)ls%16), "wrong Quadword alignment of LS and EA: "); \
+    	    													dUASSERT(ea != 0, "Nullpointer EA: "); dUASSERT(ls != 0, "Nullpointer LS: ");\
+    															printf("GET %s:%d from: 0x%x, to: 0x%x - %d bytes\n", __FILE__, __LINE__, (unsigned int)ea,(unsigned int)ls,(unsigned int)size);\
+																} \
+																mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaWaitTagStatusAll(ignore) mfc_write_tag_mask(ignore) ; mfc_read_tag_status_all()
+
+#else
+#define cellDmaLargeGet(ls, ea, size, tag, tid, rid) mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaGet(ls, ea, size, tag, tid, rid) mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaLargePut(ls, ea, size, tag, tid, rid) mfc_put(ls, ea, size, tag, tid, rid)
+#define cellDmaSmallGet(ls, ea, size, tag, tid, rid) mfc_get(ls, ea, size, tag, tid, rid)
+#define cellDmaWaitTagStatusAll(ignore) mfc_write_tag_mask(ignore) ; mfc_read_tag_status_all()
+#endif // DEBUG_DMA
+
+		
+		
+		
+		
+		
+		
+		
+#endif // USE_LIBSPE2
+#else // !__SPU__
+//Simulate DMA using memcpy or direct access on non-CELL platforms that don't have DMAs and SPUs (Win32, Mac, Linux etc)
+//Potential to add networked simulation using this interface
+
+#define DMA_TAG(a) (a)
+#define DMA_MASK(a) (a)
+
+		/// cellDmaLargeGet Win32 replacements for Cell DMA to allow simulating most of the SPU code (just memcpy)
+		int	cellDmaLargeGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+		int	cellDmaGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+		/// cellDmaLargePut Win32 replacements for Cell DMA to allow simulating most of the SPU code (just memcpy)
+		int cellDmaLargePut(const void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+		/// cellDmaWaitTagStatusAll Win32 replacements for Cell DMA to allow simulating most of the SPU code (just memcpy)
+		void	cellDmaWaitTagStatusAll(int ignore);
+
+
+#endif //__CELLOS_LV2__
+
+///stallingUnalignedDmaSmallGet internally uses DMA_TAG(1)
+int	stallingUnalignedDmaSmallGet(void *ls, uint64_t ea, uint32_t size);
+
+
+void*	cellDmaLargeGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+void*	cellDmaGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+void*	cellDmaSmallGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid);
+
+
+#endif //FAKE_DMA_H

src/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp

@@ -0,0 +1,216 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 "SpuGatheringCollisionDispatcher.h"
+#include "SpuCollisionTaskProcess.h"
+
+
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
+#include "SpuContactManifoldCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+
+
+
+
+SpuGatheringCollisionDispatcher::SpuGatheringCollisionDispatcher(class	btThreadSupportInterface*	threadInterface, unsigned int	maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration)
+:btCollisionDispatcher(collisionConfiguration),
+m_spuCollisionTaskProcess(0),
+m_threadInterface(threadInterface),
+m_maxNumOutstandingTasks(maxNumOutstandingTasks)
+{
+	
+}
+
+
+bool	SpuGatheringCollisionDispatcher::supportsDispatchPairOnSpu(int proxyType0,int proxyType1)
+{
+	bool supported0 = (
+		(proxyType0 == BOX_SHAPE_PROXYTYPE) ||
+		(proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) ||
+		(proxyType0 == SPHERE_SHAPE_PROXYTYPE) ||
+		(proxyType0 == CAPSULE_SHAPE_PROXYTYPE) ||
+		(proxyType0 == CYLINDER_SHAPE_PROXYTYPE) ||
+//		(proxyType0 == CONE_SHAPE_PROXYTYPE) ||
+		(proxyType0 == TRIANGLE_MESH_SHAPE_PROXYTYPE) ||
+		(proxyType0 == CONVEX_HULL_SHAPE_PROXYTYPE)||
+		(proxyType0 == COMPOUND_SHAPE_PROXYTYPE)
+		);
+
+	bool supported1 = (
+		(proxyType1 == BOX_SHAPE_PROXYTYPE) ||
+		(proxyType1 == TRIANGLE_SHAPE_PROXYTYPE) ||
+		(proxyType1 == SPHERE_SHAPE_PROXYTYPE) ||
+		(proxyType1 == CAPSULE_SHAPE_PROXYTYPE) ||
+		(proxyType1 == CYLINDER_SHAPE_PROXYTYPE) ||
+//		(proxyType1 == CONE_SHAPE_PROXYTYPE) ||
+		(proxyType1 == TRIANGLE_MESH_SHAPE_PROXYTYPE) ||
+		(proxyType1 == CONVEX_HULL_SHAPE_PROXYTYPE) ||
+		(proxyType1 == COMPOUND_SHAPE_PROXYTYPE)
+		);
+
+	return supported0 && supported1;
+}
+
+
+
+SpuGatheringCollisionDispatcher::~SpuGatheringCollisionDispatcher()
+{
+	if (m_spuCollisionTaskProcess)
+		delete m_spuCollisionTaskProcess;
+	
+}
+
+#include "stdio.h"
+
+
+
+///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
+///this is useful for the collision dispatcher.
+class btSpuCollisionPairCallback : public btOverlapCallback
+{
+	const btDispatcherInfo& m_dispatchInfo;
+	SpuGatheringCollisionDispatcher*	m_dispatcher;
+
+public:
+
+	btSpuCollisionPairCallback(const btDispatcherInfo& dispatchInfo, SpuGatheringCollisionDispatcher*	dispatcher)
+	:m_dispatchInfo(dispatchInfo),
+	m_dispatcher(dispatcher)
+	{
+	}
+
+	virtual bool	processOverlap(btBroadphasePair& collisionPair)
+	{
+
+
+		//PPU version
+		//(*m_dispatcher->getNearCallback())(collisionPair,*m_dispatcher,m_dispatchInfo);
+
+		//only support discrete collision detection for now, we could fallback on PPU/unoptimized version for TOI/CCD
+		btAssert(m_dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE);
+
+		//by default, Bullet will use this near callback
+		{
+			///userInfo is used to determine if the SPU has to handle this case or not (skip PPU tasks)
+			if (!collisionPair.m_userInfo)
+			{
+				collisionPair.m_userInfo = (void*) 1;
+			}
+			if (!collisionPair.m_algorithm)
+			{
+				btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+				btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+				btCollisionAlgorithmConstructionInfo ci;
+				ci.m_dispatcher1 = m_dispatcher;
+				ci.m_manifold = 0;
+
+				if (m_dispatcher->needsCollision(colObj0,colObj1))
+				{
+					int	proxyType0 = colObj0->getCollisionShape()->getShapeType();
+					int	proxyType1 = colObj1->getCollisionShape()->getShapeType();
+					if (m_dispatcher->supportsDispatchPairOnSpu(proxyType0,proxyType1))
+					{
+						int so = sizeof(SpuContactManifoldCollisionAlgorithm);
+						void* mem = m_dispatcher->allocateCollisionAlgorithm(so);
+						collisionPair.m_algorithm = new(mem) SpuContactManifoldCollisionAlgorithm(ci,colObj0,colObj1);
+						collisionPair.m_userInfo = (void*) 2;
+					} else
+					{
+						collisionPair.m_algorithm = m_dispatcher->findAlgorithm(colObj0,colObj1);
+						collisionPair.m_userInfo = (void*)3;
+					}
+				} 
+			}
+		}
+		return false;
+	}
+};
+
+void	SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) 
+{
+
+	if (dispatchInfo.m_enableSPU)
+	{
+		if (!m_spuCollisionTaskProcess)
+			m_spuCollisionTaskProcess = new SpuCollisionTaskProcess(m_threadInterface,m_maxNumOutstandingTasks);
+	
+		m_spuCollisionTaskProcess->initialize2(dispatchInfo.m_useEpa);
+	
+		///modified version of btCollisionDispatcher::dispatchAllCollisionPairs:
+		{
+			btSpuCollisionPairCallback	collisionCallback(dispatchInfo,this);
+
+			pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
+		}
+
+		//send one big batch
+		int numTotalPairs = pairCache->getNumOverlappingPairs();
+		btBroadphasePair* pairPtr = pairCache->getOverlappingPairArrayPtr();
+		int i;
+		for (i=0;i<numTotalPairs;)
+		{
+			//Performance Hint: tweak this number during benchmarking
+			static const int pairRange = SPU_BATCHSIZE_BROADPHASE_PAIRS;
+			int endIndex = (i+pairRange) < numTotalPairs ? i+pairRange : numTotalPairs;
+			m_spuCollisionTaskProcess->addWorkToTask(pairPtr,i,endIndex);
+			i = endIndex;
+		}
+
+		//handle PPU fallback pairs
+		for (i=0;i<numTotalPairs;i++)
+		{
+			btBroadphasePair& collisionPair = pairPtr[i];
+			if (collisionPair.m_userInfo == (void*)3)
+			{
+				if (collisionPair.m_algorithm)
+				{
+					btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+					btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+					if (dispatcher->needsCollision(colObj0,colObj1))
+					{
+						btManifoldResult contactPointResult(colObj0,colObj1);
+						
+						if (dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
+						{
+							//discrete collision detection query
+							collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult);
+						} else
+						{
+							//continuous collision detection query, time of impact (toi)
+							btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
+							if (dispatchInfo.m_timeOfImpact > toi)
+								dispatchInfo.m_timeOfImpact = toi;
+
+						}
+					}
+				}
+			}
+		}
+
+		//make sure all SPU work is done
+		m_spuCollisionTaskProcess->flush2();
+
+	} else
+	{
+		///PPU fallback
+		///!Need to make sure to clear all 'algorithms' when switching between SPU and PPU
+		btCollisionDispatcher::dispatchAllCollisionPairs(pairCache,dispatchInfo,dispatcher);
+	}
+}
+

src/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h

@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 SPU_GATHERING_COLLISION__DISPATCHER_H
+#define SPU_GATHERING_COLLISION__DISPATCHER_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+
+
+///Tuning value to optimized SPU utilization 
+///Too small value means Task overhead is large compared to computation (too fine granularity)
+///Too big value might render some SPUs are idle, while a few other SPUs are doing all work.
+#define SPU_BATCHSIZE_BROADPHASE_PAIRS 16
+//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 256
+
+
+class SpuCollisionTaskProcess;
+
+///SpuGatheringCollisionDispatcher can use SPU to gather and calculate collision detection
+///Time of Impact, Closest Points and Penetration Depth.
+class SpuGatheringCollisionDispatcher : public btCollisionDispatcher
+{
+	
+	SpuCollisionTaskProcess*	m_spuCollisionTaskProcess;
+	
+protected:
+
+	class	btThreadSupportInterface*	m_threadInterface;
+
+	unsigned int	m_maxNumOutstandingTasks;
+	
+
+public:
+
+	//can be used by SPU collision algorithms	
+	SpuCollisionTaskProcess*	getSpuCollisionTaskProcess()
+	{
+			return m_spuCollisionTaskProcess;
+	}
+	
+	SpuGatheringCollisionDispatcher (class	btThreadSupportInterface*	threadInterface, unsigned int	maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration);
+	
+	virtual ~SpuGatheringCollisionDispatcher();
+
+	bool	supportsDispatchPairOnSpu(int proxyType0,int proxyType1);
+
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
+
+};
+
+
+
+#endif //SPU_GATHERING_COLLISION__DISPATCHER_H

src/BulletMultiThreaded/SpuIntegrationTask/readme.txt

@@ -0,0 +1 @@
+Empty placeholder for future Libspe2 SPU task

src/BulletMultiThreaded/SpuLibspe2Support.cpp

@@ -0,0 +1,257 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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.
+*/
+
+#ifdef USE_LIBSPE2
+
+#include "SpuLibspe2Support.h"
+
+
+
+
+//SpuLibspe2Support helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+///Setup and initialize SPU/CELL/Libspe2
+SpuLibspe2Support::SpuLibspe2Support(spe_program_handle_t *speprog, int numThreads)
+{
+	this->program = speprog;
+	this->numThreads =  ((numThreads <= spe_cpu_info_get(SPE_COUNT_PHYSICAL_SPES, -1)) ? numThreads : spe_cpu_info_get(SPE_COUNT_PHYSICAL_SPES, -1));
+}
+
+///cleanup/shutdown Libspe2
+SpuLibspe2Support::~SpuLibspe2Support()
+{
+	
+	stopSPU();
+}
+
+
+
+///send messages to SPUs
+void SpuLibspe2Support::sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t uiArgument1)
+{
+	spe_context_ptr_t context;
+	
+	switch (uiCommand)
+	{
+	case CMD_SAMPLE_TASK_COMMAND:
+	{
+		//get taskdescription
+		SpuSampleTaskDesc* taskDesc = (SpuSampleTaskDesc*) uiArgument0;
+
+		btAssert(taskDesc->m_taskId<m_activeSpuStatus.size());
+
+		//get status of SPU on which task should run
+		btSpuStatus&	spuStatus = m_activeSpuStatus[taskDesc->m_taskId];
+
+		//set data for spuStatus
+		spuStatus.m_commandId = uiCommand;
+		spuStatus.m_status = Spu_Status_Occupied; //set SPU as "occupied"
+		spuStatus.m_taskDesc.p = taskDesc; 
+		
+		//get context
+		context = data[taskDesc->m_taskId].context;
+		
+		
+		taskDesc->m_mainMemoryPtr = reinterpret_cast<uint64_t> (spuStatus.m_lsMemory.p);
+		
+
+		break;
+	}
+	case CMD_GATHER_AND_PROCESS_PAIRLIST:
+		{
+			//get taskdescription
+			SpuGatherAndProcessPairsTaskDesc* taskDesc = (SpuGatherAndProcessPairsTaskDesc*) uiArgument0;
+
+			btAssert(taskDesc->taskId<m_activeSpuStatus.size());
+
+			//get status of SPU on which task should run
+			btSpuStatus&	spuStatus = m_activeSpuStatus[taskDesc->taskId];
+
+			//set data for spuStatus
+			spuStatus.m_commandId = uiCommand;
+			spuStatus.m_status = Spu_Status_Occupied; //set SPU as "occupied"
+			spuStatus.m_taskDesc.p = taskDesc; 
+			
+			//get context
+			context = data[taskDesc->taskId].context;
+			
+			
+			taskDesc->m_lsMemory = (CollisionTask_LocalStoreMemory*)spuStatus.m_lsMemory.p;
+			
+			break;
+		}
+	default:
+		{
+			///not implemented
+			btAssert(0);
+		}
+
+	};
+
+	
+	//write taskdescription in mailbox
+	unsigned int event = Spu_Mailbox_Event_Task;
+	spe_in_mbox_write(context, &event, 1, SPE_MBOX_ANY_NONBLOCKING);
+
+}
+
+///check for messages from SPUs
+void SpuLibspe2Support::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
+{
+	///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response
+	
+	///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback'
+	
+	btAssert(m_activeSpuStatus.size());
+
+	
+	int last = -1;
+	
+	//find an active spu/thread
+	while(last < 0)
+	{
+		for (int i=0;i<m_activeSpuStatus.size();i++)
+		{
+			if ( m_activeSpuStatus[i].m_status == Spu_Status_Free)
+			{
+				last = i;
+				break;
+			}
+		}
+		if(last < 0)
+			sched_yield();
+	}
+
+
+
+	btSpuStatus& spuStatus = m_activeSpuStatus[last];
+
+	///need to find an active spu
+	btAssert(last>=0);
+
+	
+
+	*puiArgument0 = spuStatus.m_taskId;
+	*puiArgument1 = spuStatus.m_status;
+
+
+}
+
+
+void SpuLibspe2Support::startSPU()
+{
+	this->internal_startSPU();
+}
+
+
+
+///start the spus group (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+void SpuLibspe2Support::internal_startSPU()
+{
+	m_activeSpuStatus.resize(numThreads);
+	
+	
+	for (int i=0; i < numThreads; i++)
+	{
+		
+		if(data[i].context == NULL) 
+		{
+					
+			 /* Create context */
+			if ((data[i].context = spe_context_create(0, NULL)) == NULL)
+			{
+			      perror ("Failed creating context");
+		          exit(1);
+			}
+	
+			/* Load program into context */
+			if(spe_program_load(data[i].context, this->program))
+			{
+			      perror ("Failed loading program");
+		          exit(1);
+			}
+			
+			m_activeSpuStatus[i].m_status = Spu_Status_Startup; 
+			m_activeSpuStatus[i].m_taskId = i; 
+			m_activeSpuStatus[i].m_commandId = 0; 
+			m_activeSpuStatus[i].m_lsMemory.p = NULL; 
+			
+			
+			data[i].entry = SPE_DEFAULT_ENTRY;
+			data[i].flags = 0;
+			data[i].argp.p = &m_activeSpuStatus[i];
+			data[i].envp.p = NULL;
+			
+		    /* Create thread for each SPE context */
+			if (pthread_create(&data[i].pthread, NULL, &ppu_pthread_function, &(data[i]) ))
+			{
+			      perror ("Failed creating thread");
+		          exit(1);
+			}
+			/*
+			else
+			{
+				printf("started thread %d\n",i);
+			}*/
+		}		
+	}
+	
+	
+	for (int i=0; i < numThreads; i++)
+	{
+		if(data[i].context != NULL) 
+		{
+			while( m_activeSpuStatus[i].m_status == Spu_Status_Startup)
+			{
+				// wait for spu to set up
+				sched_yield();
+			}
+			printf("Spu %d is ready\n", i);
+		}
+	}
+}
+
+///tell the task scheduler we are done with the SPU tasks
+void SpuLibspe2Support::stopSPU()
+{
+	// wait for all threads to finish 
+	int i;
+	for ( i = 0; i < this->numThreads; i++ ) 
+	{ 
+		
+		unsigned int event = Spu_Mailbox_Event_Shutdown;
+		spe_context_ptr_t context = data[i].context;
+		spe_in_mbox_write(context, &event, 1, SPE_MBOX_ALL_BLOCKING);
+		pthread_join (data[i].pthread, NULL); 
+		
+	} 
+	// close SPE program 
+	spe_image_close(program); 
+	// destroy SPE contexts 
+	for ( i = 0; i < this->numThreads; i++ ) 
+	{ 
+		if(data[i].context != NULL)
+		{
+			spe_context_destroy (data[i].context);
+		}
+	} 
+	
+	m_activeSpuStatus.clear();
+	
+}
+
+
+
+#endif //USE_LIBSPE2
+

src/BulletMultiThreaded/SpuLibspe2Support.h

@@ -0,0 +1,173 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 SPU_LIBSPE2_SUPPORT_H
+#define SPU_LIBSPE2_SUPPORT_H
+
+#include <LinearMath/btScalar.h> //for uint32_t etc.
+
+#ifdef USE_LIBSPE2
+
+#include <stdlib.h>
+#include <stdio.h>
+//#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+#include "PlatformDefinitions.h"
+
+
+//extern struct SpuGatherAndProcessPairsTaskDesc;
+
+enum
+{
+	Spu_Mailbox_Event_Nothing = 0,
+	Spu_Mailbox_Event_Task = 1,
+	Spu_Mailbox_Event_Shutdown = 2,
+	
+	Spu_Mailbox_Event_ForceDword = 0xFFFFFFFF
+	
+};
+
+enum
+{
+	Spu_Status_Free = 0,
+	Spu_Status_Occupied = 1,
+	Spu_Status_Startup = 2,
+	
+	Spu_Status_ForceDword = 0xFFFFFFFF
+	
+};
+
+
+struct btSpuStatus
+{
+	uint32_t	m_taskId;
+	uint32_t	m_commandId;
+	uint32_t	m_status;
+
+	addr64 m_taskDesc;
+	addr64 m_lsMemory;
+	
+}
+__attribute__ ((aligned (128)))
+;
+
+
+
+#ifndef __SPU__
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "SpuCollisionTaskProcess.h"
+#include "SpuSampleTaskProcess.h"
+#include "btThreadSupportInterface.h"
+#include <libspe2.h>
+#include <pthread.h>
+#include <sched.h>
+
+#define MAX_SPUS 4 
+
+typedef struct ppu_pthread_data 
+{
+	spe_context_ptr_t context;
+	pthread_t pthread;
+	unsigned int entry;
+	unsigned int flags;
+	addr64 argp;
+	addr64 envp;
+	spe_stop_info_t stopinfo;
+} ppu_pthread_data_t;
+
+
+static void *ppu_pthread_function(void *arg)
+{
+    ppu_pthread_data_t * datap = (ppu_pthread_data_t *)arg;
+    /*
+    int rc;
+    do 
+    {*/
+        spe_context_run(datap->context, &datap->entry, datap->flags, datap->argp.p, datap->envp.p, &datap->stopinfo);
+        if (datap->stopinfo.stop_reason == SPE_EXIT) 
+        {
+           if (datap->stopinfo.result.spe_exit_code != 0) 
+           {
+             perror("FAILED: SPE returned a non-zero exit status: \n");
+             exit(1);
+           }
+         } 
+        else 
+         {
+           perror("FAILED: SPE abnormally terminated\n");
+           exit(1);
+         }
+        
+        
+    //} while (rc > 0); // loop until exit or error, and while any stop & signal
+    pthread_exit(NULL);
+}
+
+
+
+
+
+
+///SpuLibspe2Support helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+class SpuLibspe2Support : public btThreadSupportInterface
+{
+
+	btAlignedObjectArray<btSpuStatus>	m_activeSpuStatus;
+	
+public:
+	//Setup and initialize SPU/CELL/Libspe2
+	SpuLibspe2Support(spe_program_handle_t *speprog,int numThreads);
+	
+	// SPE program handle ptr.
+	spe_program_handle_t *program;
+	
+	// SPE program data
+	ppu_pthread_data_t data[MAX_SPUS];
+	
+	//cleanup/shutdown Libspe2
+	~SpuLibspe2Support();
+
+	///send messages to SPUs
+	void sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t uiArgument1=0);
+
+	//check for messages from SPUs
+	void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
+
+	//start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+	virtual void startSPU();
+
+	//tell the task scheduler we are done with the SPU tasks
+	virtual void stopSPU();
+
+private:
+	
+	///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+	void internal_startSPU();
+	
+	
+	int numThreads;
+
+};
+
+#endif // NOT __SPU__
+
+#endif //USE_LIBSPE2
+
+#endif //SPU_LIBSPE2_SUPPORT_H
+
+
+
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp

@@ -0,0 +1,487 @@
+/*
+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 "SpuCollisionShapes.h"
+
+btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct	SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex)
+{
+    switch (shapeType)
+    {
+    case SPHERE_SHAPE_PROXYTYPE:
+        {
+            return btPoint3(0,0,0);
+        }
+	case BOX_SHAPE_PROXYTYPE:
+		{
+//			spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n");
+			btConvexInternalShape* convexShape = (btConvexInternalShape*)shape;
+			const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
+			
+			return btPoint3(
+				localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(),
+							localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(),
+							localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z());
+		}
+
+	case TRIANGLE_SHAPE_PROXYTYPE:
+		{
+
+			btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
+			btVector3* vertices = (btVector3*)shape;
+			btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
+	  		btVector3 sup = vertices[dots.maxAxis()];
+			return btPoint3(sup.getX(),sup.getY(),sup.getZ());
+			break;
+		}
+
+	case CYLINDER_SHAPE_PROXYTYPE:
+		{
+			btCylinderShape* cylShape = (btCylinderShape*)shape;
+
+			//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
+
+			btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
+			btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
+			
+			int cylinderUpAxis = cylShape->getUpAxis();
+			int XX(1),YY(0),ZZ(2);
+
+			switch (cylinderUpAxis)
+			{
+			case 0:
+				{
+					XX = 1;
+					YY = 0;
+					ZZ = 2;
+					break;
+				}
+			case 1:
+				{
+					XX = 0;
+					YY = 1;
+					ZZ = 2;
+				break;
+				}
+			case 2:
+				{
+					XX = 0;
+					YY = 2;
+					ZZ = 1;
+					break;
+				}
+			default:
+				btAssert(0);
+				//printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n");
+			};
+
+			btScalar radius = halfExtents[XX];
+			btScalar halfHeight = halfExtents[cylinderUpAxis];
+
+			btVector3 tmp;
+			btScalar d ;
+
+			btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+			if (s != btScalar(0.0))
+			{
+				d = radius / s;  
+				tmp[XX] = v[XX] * d;
+				tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+				tmp[ZZ] = v[ZZ] * d;
+				return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ());
+			}
+			else
+			{
+				tmp[XX] = radius;
+				tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+				tmp[ZZ] = btScalar(0.0);
+				return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ());
+			}
+		}
+
+	case CAPSULE_SHAPE_PROXYTYPE:
+	{
+		//spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n");
+		btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
+
+		btCapsuleShape* capsuleShape = (btCapsuleShape*)shape;
+		btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
+		btScalar halfHeight = capsuleShape->getHalfHeight();
+		int capsuleUpAxis = capsuleShape->getUpAxis();
+
+		btScalar radius = capsuleShape->getRadius();
+		btVector3 supVec(0,0,0);
+
+		btScalar maxDot(btScalar(-1e30));
+
+		btVector3 vec = vec0;
+		btScalar lenSqr = vec.length2();
+		if (lenSqr < btScalar(0.0001))
+		{
+			vec.setValue(1,0,0);
+		} else
+		{
+			btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+			vec *= rlen;
+		}
+		btVector3 vtx;
+		btScalar newDot;
+		{
+			btVector3 pos(0,0,0);
+			pos[capsuleUpAxis] = halfHeight;
+
+			vtx = pos +vec*(radius);
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supVec = vtx;
+			}
+		}
+		{
+			btVector3 pos(0,0,0);
+			pos[capsuleUpAxis] = -halfHeight;
+
+			vtx = pos +vec*(radius);
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supVec = vtx;
+			}
+		}
+		return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ());
+		break;
+	};
+
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+		{
+			//spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n");
+
+		
+
+			btPoint3* points = 0;
+			int numPoints = 0;
+			points = convexVertexData->gConvexPoints;
+			numPoints = convexVertexData->gNumConvexPoints;
+
+		//	spu_printf("numPoints = %d\n",numPoints);
+
+			btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+			btScalar newDot,maxDot = btScalar(-1e30);
+
+			btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
+			btVector3 vec = vec0;
+			btScalar lenSqr = vec.length2();
+			if (lenSqr < btScalar(0.0001))
+			{
+				vec.setValue(1,0,0);
+			} else
+			{
+				btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+				vec *= rlen;
+			}
+
+
+			for (int i=0;i<numPoints;i++)
+			{
+				btPoint3 vtx = points[i];// * m_localScaling;
+
+				newDot = vec.dot(vtx);
+				if (newDot > maxDot)
+				{
+					maxDot = newDot;
+					supVec = vtx;
+				}
+			}
+			return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ());
+
+			break;
+		};
+
+    default:
+
+		//spu_printf("SPU:(type %i) missing support function\n",shapeType);
+
+		
+#if __ASSERT
+       // spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType);
+#endif // __ASSERT
+        return btPoint3(0.f, 0.f, 0.f);
+    }
+}
+
+void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, btTransform xform)
+{
+	//calculate the aabb, given the types...
+	switch (shapeType)
+	{
+	case CYLINDER_SHAPE_PROXYTYPE:
+		/* fall through */
+	case BOX_SHAPE_PROXYTYPE:
+	{
+		float margin=convexShape->getMarginNV();
+		btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
+		halfExtents += btVector3(margin,margin,margin);
+		btTransform& t = xform;
+		btMatrix3x3 abs_b = t.getBasis().absolute();  
+		btPoint3 center = t.getOrigin();
+		btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
+		
+		aabbMin = center - extent;
+		aabbMax = center + extent;
+		break;
+	}
+	case CAPSULE_SHAPE_PROXYTYPE:
+	{
+		float margin=convexShape->getMarginNV();
+		btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
+		//add the radius to y-axis to get full height
+		btScalar radius = halfExtents[0];
+		halfExtents[1] += radius;
+		halfExtents += btVector3(margin,margin,margin);
+#if 0
+		int capsuleUpAxis = convexShape->getUpAxis();
+		btScalar halfHeight = convexShape->getHalfHeight();
+		btScalar radius = convexShape->getRadius();
+		halfExtents[capsuleUpAxis] = radius + halfHeight;
+#endif
+		btTransform& t = xform;
+		btMatrix3x3 abs_b = t.getBasis().absolute();  
+		btPoint3 center = t.getOrigin();
+		btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
+		
+		aabbMin = center - extent;
+		aabbMax = center + extent;
+		break;
+	}
+	case SPHERE_SHAPE_PROXYTYPE:
+	{
+		float radius = convexShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
+		float margin = radius + convexShape->getMarginNV();
+		btTransform& t = xform;
+		const btVector3& center = t.getOrigin();
+		btVector3 extent(margin,margin,margin);
+		aabbMin = center - extent;
+		aabbMax = center + extent;
+		break;
+	}
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+	{
+		ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
+		cellDmaGet(&convexHullShape0, convexShapePtr  , sizeof(btConvexHullShape), DMA_TAG(1), 0, 0);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+		btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0;
+		btTransform& t = xform;
+		btScalar margin = convexShape->getMarginNV();
+		localPtr->getNonvirtualAabb(t,aabbMin,aabbMax,margin);
+		//spu_printf("SPU convex aabbMin=%f,%f,%f=\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ());
+		//spu_printf("SPU convex aabbMax=%f,%f,%f=\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ());
+		break;
+	}
+	default:
+		{
+	//	spu_printf("SPU: unsupported shapetype %d in AABB calculation\n");
+		}
+	};
+}
+
+void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape)
+{
+	register int dmaSize;
+	register ppu_address_t	dmaPpuAddress2;
+
+	dmaSize = sizeof(btTriangleIndexVertexArray);
+	dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getMeshInterface());
+	//	spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2);
+#ifdef __SPU__
+	cellDmaGet(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
+	bvhMeshShape->gTriangleMeshInterfacePtr = &bvhMeshShape->gTriangleMeshInterfaceStorage;
+#else
+	bvhMeshShape->gTriangleMeshInterfacePtr = (btTriangleIndexVertexArray*)cellDmaGetReadOnly(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);
+#endif
+
+	//cellDmaWaitTagStatusAll(DMA_MASK(1));
+	
+	///now DMA over the BVH
+	
+	dmaSize = sizeof(btOptimizedBvh);
+	dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getOptimizedBvh());
+	//spu_printf("trimeshShape->getOptimizedBvh() == %llx\n",dmaPpuAddress2);
+	cellDmaGet(&bvhMeshShape->gOptimizedBvh, dmaPpuAddress2  , dmaSize, DMA_TAG(2), 0, 0);
+	//cellDmaWaitTagStatusAll(DMA_MASK(2));
+	cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
+}
+
+void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag)
+{		
+	cellDmaGet(IndexMesh, (ppu_address_t)&indexArray[index]  , sizeof(btIndexedMesh), DMA_TAG(dmaTag), 0, 0);
+	
+}
+
+void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag)
+{
+	cellDmaGet(subTreeHeaders, subTreePtr, batchSize * sizeof(btBvhSubtreeInfo), DMA_TAG(dmaTag), 0, 0);
+}
+
+void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray&	nodeArray, int dmaTag)
+{
+	cellDmaGet(nodes, reinterpret_cast<ppu_address_t>(&nodeArray[subtree.m_rootNodeIndex]) , subtree.m_subtreeSize* sizeof(btQuantizedBvhNode), DMA_TAG(2), 0, 0);
+}
+
+///getShapeTypeSize could easily be optimized, but it is not likely a bottleneck
+int		getShapeTypeSize(int shapeType)
+{
+
+
+	switch (shapeType)
+	{
+	case CYLINDER_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btCylinderShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+	case BOX_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btBoxShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+	case SPHERE_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btSphereShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+	case TRIANGLE_MESH_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btBvhTriangleMeshShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+	case CAPSULE_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btCapsuleShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btConvexHullShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+
+	case COMPOUND_SHAPE_PROXYTYPE:
+		{
+			int shapeSize = sizeof(btCompoundShape);
+			btAssert(shapeSize < MAX_SHAPE_SIZE);
+			return shapeSize;
+		}
+
+	default:
+		btAssert(0);
+		//unsupported shapetype, please add here
+		return 0;
+	}
+}
+
+void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU)
+{
+	convexVertexData->gNumConvexPoints = convexShapeSPU->getNumPoints();
+	if (convexVertexData->gNumConvexPoints>MAX_NUM_SPU_CONVEX_POINTS)
+	{
+		btAssert(0);
+	//	spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,convexVertexData->gNumConvexPoints);
+		return;
+	}
+			
+	register int dmaSize = convexVertexData->gNumConvexPoints*sizeof(btPoint3);
+	ppu_address_t pointsPPU = (ppu_address_t) convexShapeSPU->getPoints();
+	cellDmaGet(&convexVertexData->g_convexPointBuffer[0], pointsPPU  , dmaSize, DMA_TAG(2), 0, 0);
+}
+
+void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType)
+{
+	register int dmaSize = getShapeTypeSize(shapeType);
+	cellDmaGet(collisionShapeLocation, collisionShapePtr  , dmaSize, DMA_TAG(dmaTag), 0, 0);
+	//cellDmaWaitTagStatusAll(DMA_MASK(dmaTag));
+}
+
+void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag)
+{
+	register int dmaSize;
+	register	ppu_address_t	dmaPpuAddress2;
+	int childShapeCount = spuCompoundShape->getNumChildShapes();
+	dmaSize = childShapeCount * sizeof(btCompoundShapeChild);
+	dmaPpuAddress2 = (ppu_address_t)spuCompoundShape->getChildList();
+	cellDmaGet(&compoundShapeLocation->gSubshapes[0], dmaPpuAddress2, dmaSize, DMA_TAG(dmaTag), 0, 0);
+}
+
+void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag)
+{
+	int childShapeCount = spuCompoundShape->getNumChildShapes();
+	int i;
+	// DMA all the subshapes 
+	for ( i = 0; i < childShapeCount; ++i)
+	{
+		btCompoundShapeChild& childShape = compoundShapeLocation->gSubshapes[i];
+		dmaCollisionShape (&compoundShapeLocation->gSubshapeShape[i],(ppu_address_t)childShape.m_childShape, dmaTag, childShape.m_childShapeType);
+	}
+}
+
+
+void	spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex)
+{
+
+	int curIndex = startNodeIndex;
+	int walkIterations = 0;
+	int subTreeSize = endNodeIndex - startNodeIndex;
+
+	int escapeIndex;
+
+	unsigned int aabbOverlap, isLeafNode;
+
+	while (curIndex < endNodeIndex)
+	{
+		//catch bugs in tree data
+		assert (walkIterations < subTreeSize);
+
+		walkIterations++;
+		aabbOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+		isLeafNode = rootNode->isLeafNode();
+
+		if (isLeafNode && aabbOverlap)
+		{
+			//printf("overlap with node %d\n",rootNode->getTriangleIndex());
+			nodeCallback->processNode(0,rootNode->getTriangleIndex());
+			//			spu_printf("SPU: overlap detected with triangleIndex:%d\n",rootNode->getTriangleIndex());
+		} 
+
+		if (aabbOverlap || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->getEscapeIndex();
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+
+}

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h

@@ -0,0 +1,102 @@
+#ifndef __SPU_COLLISION_SHAPES_H
+#define __SPU_COLLISION_SHAPES_H
+
+#include "../SpuDoubleBuffer.h"
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+
+#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+
+#define MAX_NUM_SPU_CONVEX_POINTS 128
+
+struct	SpuConvexPolyhedronVertexData
+{
+	void*	gSpuConvexShapePtr;
+	btPoint3* gConvexPoints;
+	int gNumConvexPoints;
+	ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]);
+};
+
+#define MAX_SHAPE_SIZE 256
+
+struct CollisionShape_LocalStoreMemory
+{
+	ATTRIBUTE_ALIGNED16(char collisionShape[MAX_SHAPE_SIZE]);
+};
+
+struct CompoundShape_LocalStoreMemory
+{
+	// Compound data
+#define MAX_SPU_COMPOUND_SUBSHAPES 16
+	ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]);
+	ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]);
+};
+
+struct bvhMeshShape_LocalStoreMemory
+{
+	//ATTRIBUTE_ALIGNED16(btOptimizedBvh	gOptimizedBvh);
+	ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]);
+	btOptimizedBvh*	getOptimizedBvh()
+	{
+		return (btOptimizedBvh*) gOptimizedBvh;
+	}
+
+	ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray	gTriangleMeshInterfaceStorage);
+	btTriangleIndexVertexArray*	gTriangleMeshInterfacePtr;
+	///only a single mesh part for now, we can add support for multiple parts, but quantized trees don't support this at the moment 
+	ATTRIBUTE_ALIGNED16(btIndexedMesh	gIndexMesh);
+	#define MAX_SPU_SUBTREE_HEADERS 32
+	//1024
+	ATTRIBUTE_ALIGNED16(btBvhSubtreeInfo	gSubtreeHeaders[MAX_SPU_SUBTREE_HEADERS]);
+	ATTRIBUTE_ALIGNED16(btQuantizedBvhNode	gSubtreeNodes[MAX_SUBTREE_SIZE_IN_BYTES/sizeof(btQuantizedBvhNode)]);
+};
+
+
+btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct	SpuConvexPolyhedronVertexData* convexVertexData);//, int *featureIndex)
+void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, btTransform xform);
+void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape);
+void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag);
+void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag);
+void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray&	nodeArray, int dmaTag);
+
+int  getShapeTypeSize(int shapeType);
+void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU);
+void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType);
+void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
+void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
+
+#define USE_BRANCHFREE_TEST 1
+#ifdef USE_BRANCHFREE_TEST
+SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
+{		
+	return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
+		& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
+		& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
+		1, 0);
+}
+#else
+
+unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int*  aabbMax2)
+{
+	unsigned int overlap = 1;
+	overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap;
+	overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap;
+	overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap;
+	return overlap;
+}
+#endif
+
+void	spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex);
+
+#endif

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp

@@ -0,0 +1,227 @@
+/*
+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 "SpuContactResult.h"
+
+//#define DEBUG_SPU_COLLISION_DETECTION 1
+
+
+SpuContactResult::SpuContactResult()
+{
+	m_manifoldAddress = 0;
+	m_spuManifold = NULL;
+	m_RequiresWriteBack = false;
+}
+
+ SpuContactResult::~SpuContactResult()
+{
+	g_manifoldDmaExport.swapBuffers();
+}
+
+ 	///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+inline btScalar	calculateCombinedFriction(btScalar friction0,btScalar friction1)
+{
+	btScalar friction = friction0*friction1;
+
+	const btScalar MAX_FRICTION  = btScalar(10.);
+
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
+inline btScalar	calculateCombinedRestitution(btScalar restitution0,btScalar restitution1)
+{
+	return restitution0*restitution1;
+}
+
+
+
+ void	SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, ppu_address_t	manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction1, bool isSwapped)
+ {
+	//spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress);
+	m_rootWorldTransform0 = worldTrans0;
+	m_rootWorldTransform1 = worldTrans1;
+	m_manifoldAddress = manifoldAddress;    
+	m_spuManifold = spuManifold;
+
+	m_combinedFriction = calculateCombinedFriction(friction0,friction1);
+	m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1);
+	m_isSwapped = isSwapped;
+ }
+
+ void SpuContactResult::setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+ {
+	
+ }
+	
+
+
+ ///return true if it requires a dma transfer back
+bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
+								   const btVector3& pointInWorld,
+								   float depth,
+								   btPersistentManifold* manifoldPtr,
+								   btTransform& transA,
+								   btTransform& transB,
+									btScalar	combinedFriction,
+									btScalar	combinedRestitution,
+								   bool isSwapped)
+{
+	
+	float contactTreshold = manifoldPtr->getContactBreakingThreshold();
+
+	//spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr);
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+	spu_printf("SPU: contactTreshold %f\n",contactTreshold);
+#endif //DEBUG_SPU_COLLISION_DETECTION
+	if (depth > manifoldPtr->getContactBreakingThreshold())
+		return false;
+
+	//provide inverses or just calculate?
+	btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform;
+	btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform;
+
+	btVector3 pointA;
+	btVector3 localA;
+	btVector3 localB;
+	btVector3 normal;
+
+	if (isSwapped)
+	{
+		normal = normalOnBInWorld * -1;
+		pointA = pointInWorld + normal * depth;
+		localA = transAInv(pointA );
+		localB = transBInv(pointInWorld);
+		/*localA = transBInv(pointA );
+		localB = transAInv(pointInWorld);*/
+	}
+	else
+	{
+		normal = normalOnBInWorld;
+		pointA = pointInWorld + normal * depth;
+		localA = transAInv(pointA );
+		localB = transBInv(pointInWorld);
+	}
+
+	btManifoldPoint newPt(localA,localB,normal,depth);
+
+	int insertIndex = manifoldPtr->getCacheEntry(newPt);
+	if (insertIndex >= 0)
+	{
+//		manifoldPtr->replaceContactPoint(newPt,insertIndex);
+//		return true;
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("SPU: same contact detected, nothing done\n");
+#endif //DEBUG_SPU_COLLISION_DETECTION
+		// This is not needed, just use the old info! saves a DMA transfer as well
+	} else
+	{
+
+		newPt.m_combinedFriction = combinedFriction;
+		newPt.m_combinedRestitution = combinedRestitution;
+
+		/*
+		//potential TODO: SPU callbacks, either immediate (local on the SPU), or deferred
+		//User can override friction and/or restitution
+		if (gContactAddedCallback &&
+			//and if either of the two bodies requires custom material
+			 ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
+			   (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
+		{
+			//experimental feature info, for per-triangle material etc.
+			(*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
+		}
+		*/
+		manifoldPtr->addManifoldPoint(newPt);
+		return true;
+
+	}
+	return false;
+	
+}
+
+
+void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold)
+{
+    memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold));
+
+    g_manifoldDmaExport.swapBuffers();
+    uint64_t mmAddr = (uint32_t)mmManifold;
+    g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9));
+	// Should there be any kind of wait here?  What if somebody tries to use this tag again?  What if we call this function again really soon?
+	//no, the swapBuffers does the wait
+}
+
+void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth)
+{
+	//spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth);
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+ //   int sman = sizeof(rage::phManifold);
+//	spu_printf("sizeof_manifold = %i\n",sman);
+#endif //DEBUG_SPU_COLLISION_DETECTION
+
+	btPersistentManifold* localManifold = m_spuManifold;
+
+	btVector3	normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
+	btVector3	pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
+
+	//process the contact point
+	const bool retVal = ManifoldResultAddContactPoint(normalB,
+		pointWrld,
+		depth,
+		localManifold,
+		m_rootWorldTransform0,
+		m_rootWorldTransform1,
+		m_combinedFriction,
+		m_combinedRestitution,
+		m_isSwapped);
+	m_RequiresWriteBack = m_RequiresWriteBack || retVal;
+}
+
+void SpuContactResult::flush()
+{
+
+	if (m_spuManifold && m_spuManifold->getNumContacts())
+	{
+		m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1);
+		m_RequiresWriteBack = true;
+	}
+
+
+	if (m_RequiresWriteBack)
+	{
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n");
+		spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts());
+		spu_printf("Manifold address: %llu\n", m_manifoldAddress);
+#endif //DEBUG_SPU_COLLISION_DETECTION
+	//	spu_printf("writeDoubleBufferedManifold\n");
+		writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress);
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("SPU: Finished (Put) DMA\n");
+#endif //DEBUG_SPU_COLLISION_DETECTION
+	}
+	m_spuManifold = NULL;
+	m_RequiresWriteBack = false;
+}
+
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.h

@@ -0,0 +1,113 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPU_CONTACT_RESULT2_H
+#define SPU_CONTACT_RESULT2_H
+
+
+#ifndef WIN32
+#include <stdint.h>
+#endif
+
+
+
+#include "../SpuDoubleBuffer.h"
+
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btPoint3.h"
+
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+
+struct SpuCollisionPairInput
+{
+	ppu_address_t m_collisionShapes[2];
+	void*	m_spuCollisionShapes[2];
+
+	ppu_address_t m_persistentManifoldPtr;
+	btVector3	m_primitiveDimensions0;
+	btVector3	m_primitiveDimensions1;
+	int		m_shapeType0;
+	int		m_shapeType1;	
+	float	m_collisionMargin0;
+	float	m_collisionMargin1;
+
+	btTransform	m_worldTransform0;
+	btTransform m_worldTransform1;
+	
+	bool	m_isSwapped;
+	bool    m_useEpa;
+};
+
+
+struct SpuClosestPointInput
+{
+    SpuClosestPointInput()
+        :m_maximumDistanceSquared(float(1e30)),
+        m_stackAlloc(0)
+    {
+    }
+
+    btTransform m_transformA;
+    btTransform m_transformB;
+    float	m_maximumDistanceSquared;
+    class	btStackAlloc* m_stackAlloc;
+	struct SpuConvexPolyhedronVertexData* m_convexVertexData[2];
+};
+
+///SpuContactResult exports the contact points using double-buffered DMA transfers, only when needed
+///So when an existing contact point is duplicated, no transfer/refresh is performed.
+class SpuContactResult
+{
+    btTransform		m_rootWorldTransform0;
+	btTransform		m_rootWorldTransform1;
+	ppu_address_t	m_manifoldAddress;
+
+    btPersistentManifold* m_spuManifold;
+	bool m_RequiresWriteBack;
+	btScalar	m_combinedFriction;
+	btScalar	m_combinedRestitution;
+	
+	bool m_isSwapped;
+
+	DoubleBuffer<btPersistentManifold, 1> g_manifoldDmaExport;
+
+	public:
+		SpuContactResult();
+		virtual ~SpuContactResult();
+
+		btPersistentManifold*	GetSpuManifold() const
+		{
+			return m_spuManifold;
+		}
+
+		virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1);
+
+		void	setContactInfo(btPersistentManifold* spuManifold, ppu_address_t	manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction01, bool isSwapped);
+
+
+        void writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold);
+
+        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth);
+
+		void flush();
+};
+
+
+
+#endif //SPU_CONTACT_RESULT2_H
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h

@@ -0,0 +1,52 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef CONVEX_PENETRATION_DEPTH_H
+#define CONVEX_PENETRATION_DEPTH_H
+
+
+
+class btStackAlloc;
+class btIDebugDraw;
+class SpuVoronoiSimplexSolver;
+
+#include <LinearMath/btTransform.h>
+#include <LinearMath/btPoint3.h>
+
+
+///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
+class SpuConvexPenetrationDepthSolver
+{
+public:	
+	
+	virtual ~SpuConvexPenetrationDepthSolver() {};
+	virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
+	        void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
+            btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataA,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataB
+			) const = 0;
+
+
+};
+
+
+
+#endif //CONVEX_PENETRATION_DEPTH_H
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.cpp

@@ -0,0 +1,37 @@
+/*
+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 "SpuEpaPenetrationDepthSolver.h"
+#include "SpuVoronoiSimplexSolver.h"
+#include "SpuGjkPairDetector.h"
+#include "SpuContactResult.h"
+#include "SpuGjkEpa2.h"
+
+bool SpuEpaPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
+	        void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
+		btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataA,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataB
+			) const
+{
+	bool r;
+	SpuGjkEpaSolver2::sResults results;
+	r = SpuGjkEpaSolver2::Penetration (convexA, convexVertexDataA, shapeTypeA, marginA, transA, convexB, convexVertexDataB, shapeTypeB, marginB, transB, btVector3(1.0f, 0.0f, 0.0f), results);
+	pa = results.witnesses[0];
+	pb = results.witnesses[1];
+	return r;
+}

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuEpaPenetrationDepthSolver.h

@@ -0,0 +1,47 @@
+
+/*
+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 SPU_EPA_PENETRATION_DEPTH_SOLVER_H
+#define SPU_EPA_PENETRATION_DEPTH_SOLVER_H
+
+
+#include "SpuConvexPenetrationDepthSolver.h"
+
+class btStackAlloc;
+class btIDebugDraw;
+class SpuVoronoiSimplexSolver;
+
+///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
+///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
+class SpuEpaPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver
+{
+public:
+
+	virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
+	        void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
+            btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataA,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataB
+			) const;
+
+
+};
+
+
+#endif //SPU_EPA_PENETRATION_DEPTH_SOLVER_H
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h

@@ -0,0 +1,140 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPU_GATHERING_COLLISION_TASK_H
+#define SPU_GATHERING_COLLISION_TASK_H
+
+#include "../PlatformDefinitions.h"
+//#define DEBUG_SPU_COLLISION_DETECTION 1
+
+
+///Task Description for SPU collision detection
+struct SpuGatherAndProcessPairsTaskDesc 
+{
+	ppu_address_t	inPtr;//m_pairArrayPtr;
+	//mutex variable
+	uint32_t	m_someMutexVariableInMainMemory;
+
+	ppu_address_t	m_dispatcher;
+
+	uint32_t	numOnLastPage;
+
+	uint16_t numPages;
+	uint16_t taskId;
+	bool m_useEpa;
+
+	struct	CollisionTask_LocalStoreMemory*	m_lsMemory; 
+}
+
+#if  defined(__CELLOS_LV2__) || defined(USE_LIBSPE2)
+__attribute__ ((aligned (128)))
+#endif
+;
+
+
+void	processCollisionTask(void* userPtr, void* lsMemory);
+
+void*	createCollisionLocalStoreMemory();
+
+
+#if defined(USE_LIBSPE2) && defined(__SPU__)
+#include "../SpuLibspe2Support.h"
+#include <spu_intrinsics.h>
+#include <spu_mfcio.h>
+#include <SpuFakeDma.h>
+
+//#define DEBUG_LIBSPE2_SPU_TASK
+
+
+
+int main(unsigned long long speid, addr64 argp, addr64 envp)
+{
+	printf("SPU: hello \n");
+	
+	ATTRIBUTE_ALIGNED128(btSpuStatus status);
+	ATTRIBUTE_ALIGNED16( SpuGatherAndProcessPairsTaskDesc taskDesc ) ;
+	unsigned int received_message = Spu_Mailbox_Event_Nothing;
+    bool shutdown = false;
+
+	cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(3));
+
+	status.m_status = Spu_Status_Free;
+	status.m_lsMemory.p = createCollisionLocalStoreMemory();
+
+	cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(3));
+	
+	
+	while ( btLikely( !shutdown ) )
+	{
+		
+		received_message = spu_read_in_mbox();
+		
+		if( btLikely( received_message == Spu_Mailbox_Event_Task ))
+		{
+#ifdef DEBUG_LIBSPE2_SPU_TASK
+			printf("SPU: received Spu_Mailbox_Event_Task\n");
+#endif //DEBUG_LIBSPE2_SPU_TASK
+
+			// refresh the status
+			cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+			cellDmaWaitTagStatusAll(DMA_MASK(3));
+		
+			btAssert(status.m_status==Spu_Status_Occupied);
+			
+			cellDmaGet(&taskDesc, status.m_taskDesc.p, sizeof(SpuGatherAndProcessPairsTaskDesc), DMA_TAG(3), 0, 0);
+			cellDmaWaitTagStatusAll(DMA_MASK(3));
+#ifdef DEBUG_LIBSPE2_SPU_TASK		
+			printf("SPU:processCollisionTask\n");	
+#endif //DEBUG_LIBSPE2_SPU_TASK
+			processCollisionTask((void*)&taskDesc, taskDesc.m_lsMemory);
+			
+#ifdef DEBUG_LIBSPE2_SPU_TASK
+			printf("SPU:finished processCollisionTask\n");
+#endif //DEBUG_LIBSPE2_SPU_TASK
+		}
+		else
+		{
+#ifdef DEBUG_LIBSPE2_SPU_TASK
+			printf("SPU: received ShutDown\n");
+#endif //DEBUG_LIBSPE2_SPU_TASK
+			if( btLikely( received_message == Spu_Mailbox_Event_Shutdown ) )
+			{
+				shutdown = true;
+			}
+			else
+			{
+				//printf("SPU - Sth. recieved\n");
+			}
+		}
+
+		// set to status free and wait for next task
+		status.m_status = Spu_Status_Free;
+		cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+		cellDmaWaitTagStatusAll(DMA_MASK(3));		
+				
+		
+  	}
+
+	printf("SPU: shutdown\n");
+  	return 0;
+}
+#endif // USE_LIBSPE2
+
+
+#endif //SPU_GATHERING_COLLISION_TASK_H
+
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp

@@ -0,0 +1,850 @@
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "SpuCollisionShapes.h"
+#include "SpuGjkEpa2.h"
+
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#endif //__SPU__
+#endif
+
+namespace gjkepa2_spu_impl
+{
+
+// Config
+
+	/* GJK	*/ 
+#define GJK_MAX_ITERATIONS	128
+#define GJK_ACCURARY		((btScalar)0.0001)
+#define GJK_MIN_DISTANCE	((btScalar)0.0001)
+#define GJK_DUPLICATED_EPS	((btScalar)0.0001)
+#define GJK_SIMPLEX2_EPS	((btScalar)0.0)
+#define GJK_SIMPLEX3_EPS	((btScalar)0.0)
+#define GJK_SIMPLEX4_EPS	((btScalar)0.0)
+
+	/* EPA	*/ 
+#define EPA_MAX_VERTICES	64
+#define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
+#define EPA_MAX_ITERATIONS	255
+#define EPA_ACCURACY		((btScalar)0.0001)
+#define EPA_FALLBACK		(10*EPA_ACCURACY)
+#define EPA_PLANE_EPS		((btScalar)0.00001)
+#define EPA_INSIDE_EPS		((btScalar)0.01)
+
+
+// Shorthands
+typedef unsigned int	U;
+typedef unsigned char	U1;
+
+struct convexShape
+{
+	void* shape;
+	SpuConvexPolyhedronVertexData* convexData;
+	int shapeType;
+	float margin;
+};
+
+// MinkowskiDiff
+struct	MinkowskiDiff
+	{
+	convexShape m_shapes[2];
+	btMatrix3x3				m_toshape1;
+	btTransform				m_toshape0;
+	btVector3				(btConvexShape::*Ls)(const btVector3&) const;
+	void					EnableMargin(bool enable)
+		{
+#if 0
+		if(enable)
+			Ls=&btConvexShape::localGetSupportingVertex;
+			else
+			Ls=&btConvexShape::localGetSupportingVertexWithoutMargin;
+#endif
+		}	
+	inline btVector3		Support0(const btVector3& d) const
+		{
+			btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[0].shapeType, m_shapes[0].shape, d, m_shapes[0].convexData);
+			btVector3 ud = d;
+			ud.normalize();
+			sp += ud * m_shapes[0].margin;
+			return sp;
+	//	return(((m_shapes[0])->*(Ls))(d));
+		}
+	inline btVector3		Support1(const btVector3& d) const
+		{
+			btVector3 nd = m_toshape1*d;
+			btVector3 ud = nd;
+			ud.normalize ();
+			btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[1].shapeType, m_shapes[1].shape, nd, m_shapes[1].convexData);
+			sp += ud * m_shapes[1].margin;
+			return m_toshape0 * sp;
+	//	return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
+		}
+	inline btVector3		Support(const btVector3& d) const
+		{
+			return(Support0(d)-Support1(-d));
+		}
+	btVector3				Support(const btVector3& d,U index) const
+		{
+
+		if(index)
+			return(Support1(d));
+			else
+			return(Support0(d));
+
+		}
+	};
+
+typedef	MinkowskiDiff	tShape;
+
+// GJK
+struct	GJK
+{
+/* Types		*/ 
+struct	sSV
+	{
+	btVector3	d,w;
+	};
+struct	sSimplex
+	{
+	sSV*		c[4];
+	btScalar	p[4];
+	U			rank;
+	};
+struct	eStatus	{ enum _ {
+	Valid,
+	Inside,
+	Failed		};};
+/* Fields		*/ 
+tShape			m_shape;
+btVector3		m_ray;
+btScalar		m_distance;
+sSimplex		m_simplices[2];
+sSV				m_store[4];
+sSV*			m_free[4];
+U				m_nfree;
+U				m_current;
+sSimplex*		m_simplex;
+eStatus::_		m_status;
+/* Methods		*/ 
+					GJK()
+	{
+	Initialize();
+	}
+void				Initialize()
+	{
+	m_ray		=	btVector3(0,0,0);
+	m_nfree		=	0;
+	m_status	=	eStatus::Failed;
+	m_current	=	0;
+	m_distance	=	0;
+	}
+eStatus::_			Evaluate(const tShape& shapearg,const btVector3& guess)
+	{
+	U			iterations=0;
+	btScalar	sqdist=0;
+	btScalar	alpha=0;
+	btVector3	lastw[4];
+	U			clastw=0;
+	/* Initialize solver		*/ 
+	m_free[0]			=	&m_store[0];
+	m_free[1]			=	&m_store[1];
+	m_free[2]			=	&m_store[2];
+	m_free[3]			=	&m_store[3];
+	m_nfree				=	4;
+	m_current			=	0;
+	m_status			=	eStatus::Valid;
+	m_shape				=	shapearg;
+	m_distance			=	0;
+	/* Initialize simplex		*/ 
+	m_simplices[0].rank	=	0;
+	m_ray				=	guess;
+	const btScalar	sqrl=	m_ray.length2();
+	appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
+	m_simplices[0].p[0]	=	1;
+	m_ray				=	m_simplices[0].c[0]->w;	
+	sqdist				=	sqrl;
+	lastw[0]			=
+	lastw[1]			=
+	lastw[2]			=
+	lastw[3]			=	m_ray;
+	/* Loop						*/ 
+	do	{
+		const U		next=1-m_current;
+		sSimplex&	cs=m_simplices[m_current];
+		sSimplex&	ns=m_simplices[next];
+		/* Check zero							*/ 
+		const btScalar	rl=m_ray.length();
+		if(rl<GJK_MIN_DISTANCE)
+			{/* Touching or inside				*/ 
+			m_status=eStatus::Inside;
+			break;
+			}
+		/* Append new vertice in -'v' direction	*/ 
+		appendvertice(cs,-m_ray);
+		const btVector3&	w=cs.c[cs.rank-1]->w;
+		bool				found=false;
+		for(U i=0;i<4;++i)
+			{
+			if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
+				{ found=true;break; }
+			}
+		if(found)
+			{/* Return old simplex				*/ 
+			removevertice(m_simplices[m_current]);
+			break;
+			}
+			else
+			{/* Update lastw					*/ 
+			lastw[clastw=(clastw+1)&3]=w;
+			}
+		/* Check for termination				*/ 
+		const btScalar	omega=dot(m_ray,w)/rl;
+		alpha=btMax(omega,alpha);
+		if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+			{/* Return old simplex				*/ 
+			removevertice(m_simplices[m_current]);
+			break;
+			}		
+		/* Reduce simplex						*/ 
+		btScalar	weights[4];
+		U			mask=0;
+		switch(cs.rank)
+			{
+			case	2:	sqdist=projectorigin(	cs.c[0]->w,
+												cs.c[1]->w,
+												weights,mask);break;
+			case	3:	sqdist=projectorigin(	cs.c[0]->w,
+												cs.c[1]->w,
+												cs.c[2]->w,
+												weights,mask);break;
+			case	4:	sqdist=projectorigin(	cs.c[0]->w,
+												cs.c[1]->w,
+												cs.c[2]->w,
+												cs.c[3]->w,
+												weights,mask);break;
+			}
+		if(sqdist>=0)
+			{/* Valid	*/ 
+			ns.rank		=	0;
+			m_ray		=	btVector3(0,0,0);
+			m_current	=	next;
+			for(U i=0,ni=cs.rank;i<ni;++i)
+				{
+				if(mask&(1<<i))
+					{
+					ns.c[ns.rank]		=	cs.c[i];
+					ns.p[ns.rank++]		=	weights[i];
+					m_ray				+=	cs.c[i]->w*weights[i];
+					}
+					else
+					{
+					m_free[m_nfree++]	=	cs.c[i];
+					}
+				}
+			if(mask==15) m_status=eStatus::Inside;
+			}
+			else
+			{/* Return old simplex				*/ 
+			removevertice(m_simplices[m_current]);
+			break;
+			}
+		m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
+		} while(m_status==eStatus::Valid);
+	m_simplex=&m_simplices[m_current];
+	switch(m_status)
+		{
+		case	eStatus::Valid:		m_distance=m_ray.length();break;
+		case	eStatus::Inside:	m_distance=0;break;
+		}	
+	return(m_status);
+	}
+bool					EncloseOrigin()
+	{
+	switch(m_simplex->rank)
+		{
+		case	1:
+			{
+			for(U i=0;i<3;++i)
+				{
+				btVector3		axis=btVector3(0,0,0);
+				axis[i]=1;
+				appendvertice(*m_simplex, axis);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				appendvertice(*m_simplex,-axis);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				}
+			}
+		break;
+		case	2:
+			{
+			const btVector3	d=m_simplex->c[1]->w-m_simplex->c[0]->w;
+			for(U i=0;i<3;++i)
+				{
+				btVector3		axis=btVector3(0,0,0);
+				axis[i]=1;
+				if(btFabs(dot(axis,d))>0)
+					{
+					const btVector3	p=cross(d,axis);
+					appendvertice(*m_simplex, p);
+					if(EncloseOrigin())	return(true);
+					removevertice(*m_simplex);
+					appendvertice(*m_simplex,-p);
+					if(EncloseOrigin())	return(true);
+					removevertice(*m_simplex);
+					}
+				}
+			}
+		break;
+		case	3:
+			{
+			const btVector3	n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
+									m_simplex->c[2]->w-m_simplex->c[0]->w);
+			const btScalar	l=n.length();
+			if(l>0)
+				{
+				appendvertice(*m_simplex,n);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				appendvertice(*m_simplex,-n);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				}
+			}
+		break;
+		case	4:
+			{
+			if(btFabs(det(	m_simplex->c[0]->w-m_simplex->c[3]->w,
+							m_simplex->c[1]->w-m_simplex->c[3]->w,
+							m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
+				return(true);
+			}
+		break;
+		}
+	return(false);
+	}
+/* Internals	*/ 
+void				getsupport(const btVector3& d,sSV& sv) const
+	{
+	sv.d	=	d/d.length();
+	sv.w	=	m_shape.Support(sv.d);
+	}
+void				removevertice(sSimplex& simplex)
+	{
+	m_free[m_nfree++]=simplex.c[--simplex.rank];
+	}
+void				appendvertice(sSimplex& simplex,const btVector3& v)
+	{
+	simplex.p[simplex.rank]=0;
+	simplex.c[simplex.rank]=m_free[--m_nfree];
+	getsupport(v,*simplex.c[simplex.rank++]);
+	}
+static btScalar		det(const btVector3& a,const btVector3& b,const btVector3& c)
+	{
+	return(	a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
+			a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
+			a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
+	}
+static btScalar		projectorigin(	const btVector3& a,
+									const btVector3& b,
+									btScalar* w,U& m)
+	{
+	const btVector3	d=b-a;
+	const btScalar	l=d.length2();
+	if(l>GJK_SIMPLEX2_EPS)
+		{
+		const btScalar	t(l>0?-dot(a,d)/l:0);
+		if(t>=1)		{ w[0]=0;w[1]=1;m=2;return(b.length2()); }
+		else if(t<=0)	{ w[0]=1;w[1]=0;m=1;return(a.length2()); }
+		else			{ w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
+		}
+	return(-1);
+	}
+static btScalar		projectorigin(	const btVector3& a,
+									const btVector3& b,
+									const btVector3& c,
+									btScalar* w,U& m)
+	{
+	static const U		imd3[]={1,2,0};
+	const btVector3*	vt[]={&a,&b,&c};
+	const btVector3		dl[]={a-b,b-c,c-a};
+	const btVector3		n=cross(dl[0],dl[1]);
+	const btScalar		l=n.length2();
+	if(l>GJK_SIMPLEX3_EPS)
+		{
+		btScalar	mindist=-1;
+		btScalar	subw[2] = { btScalar(0.0f), btScalar(0.0f) };
+		U			subm;
+		for(U i=0;i<3;++i)
+			{
+			if(dot(*vt[i],cross(dl[i],n))>0)
+				{
+				const U			j=imd3[i];
+				const btScalar	subd(projectorigin(*vt[i],*vt[j],subw,subm));
+				if((mindist<0)||(subd<mindist))
+					{
+					mindist		=	subd;
+					m			=	((subm&1)?1<<i:0)+((subm&2)?1<<j:0);
+					w[i]		=	subw[0];
+					w[j]		=	subw[1];
+					w[imd3[j]]	=	0;				
+					}
+				}
+			}
+		if(mindist<0)
+			{
+			const btScalar	d=dot(a,n);	
+			const btScalar	s=btSqrt(l);
+			const btVector3	p=n*(d/l);
+			mindist	=	p.length2();
+			m		=	7;
+			w[0]	=	(cross(dl[1],b-p)).length()/s;
+			w[1]	=	(cross(dl[2],c-p)).length()/s;
+			w[2]	=	1-(w[0]+w[1]);
+			}
+		return(mindist);
+		}
+	return(-1);
+	}
+static btScalar		projectorigin(	const btVector3& a,
+									const btVector3& b,
+									const btVector3& c,
+									const btVector3& d,
+									btScalar* w,U& m)
+	{
+	static const U		imd3[]={1,2,0};
+	const btVector3*	vt[]={&a,&b,&c,&d};
+	const btVector3		dl[]={a-d,b-d,c-d};
+	const btScalar		vl=det(dl[0],dl[1],dl[2]);
+	const bool			ng=(vl*dot(a,cross(b-c,a-b)))<=0;
+	if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
+		{
+		btScalar	mindist=-1;
+		btScalar	subw[3];
+		U			subm;
+		for(U i=0;i<3;++i)
+			{
+			const U			j=imd3[i];
+			const btScalar	s=vl*dot(d,cross(dl[i],dl[j]));
+			if(s>0)
+				{
+				const btScalar	subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
+				if((mindist<0)||(subd<mindist))
+					{
+					mindist		=	subd;
+					m			=	(subm&1?1<<i:0)+
+									(subm&2?1<<j:0)+
+									(subm&4?8:0);
+					w[i]		=	subw[0];
+					w[j]		=	subw[1];
+					w[imd3[j]]	=	0;
+					w[3]		=	subw[2];
+					}
+				}
+			}
+		if(mindist<0)
+			{
+			mindist	=	0;
+			m		=	15;
+			w[0]	=	det(c,b,d)/vl;
+			w[1]	=	det(a,c,d)/vl;
+			w[2]	=	det(b,a,d)/vl;
+			w[3]	=	1-(w[0]+w[1]+w[2]);
+			}
+		return(mindist);
+		}
+	return(-1);
+	}
+};
+
+// EPA
+struct	EPA
+{
+/* Types		*/ 
+typedef	GJK::sSV	sSV;
+struct	sFace
+	{
+	btVector3	n;
+	btScalar	d;
+	btScalar	p;
+	sSV*		c[3];
+	sFace*		f[3];
+	sFace*		l[2];
+	U1			e[3];
+	U1			pass;
+	};
+struct	sList
+	{
+	sFace*		root;
+	U			count;
+				sList() : root(0),count(0)	{}
+	};
+struct	sHorizon
+	{
+	sFace*		cf;
+	sFace*		ff;
+	U			nf;
+				sHorizon() : cf(0),ff(0),nf(0)	{}
+	};
+struct	eStatus { enum _ {
+	Valid,
+	Touching,
+	Degenerated,
+	NonConvex,
+	InvalidHull,		
+	OutOfFaces,
+	OutOfVertices,
+	AccuraryReached,
+	FallBack,
+	Failed,		};};
+/* Fields		*/ 
+eStatus::_		m_status;
+GJK::sSimplex	m_result;
+btVector3		m_normal;
+btScalar		m_depth;
+sSV				m_sv_store[EPA_MAX_VERTICES];
+sFace			m_fc_store[EPA_MAX_FACES];
+U				m_nextsv;
+sList			m_hull;
+sList			m_stock;
+/* Methods		*/ 
+					EPA()
+	{
+	Initialize();	
+	}
+void				Initialize()
+	{
+	m_status	=	eStatus::Failed;
+	m_normal	=	btVector3(0,0,0);
+	m_depth		=	0;
+	m_nextsv	=	0;
+	for(U i=0;i<EPA_MAX_FACES;++i)
+		{
+		append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
+		}
+	}
+eStatus::_			Evaluate(GJK& gjk,const btVector3& guess)
+	{
+	GJK::sSimplex&	simplex=*gjk.m_simplex;
+	if((simplex.rank>1)&&gjk.EncloseOrigin())
+		{
+		/* Clean up				*/ 
+		while(m_hull.root)
+			{
+			sFace*	f(m_hull.root);
+			remove(m_hull,f);
+			append(m_stock,f);
+			}
+		m_status	=	eStatus::Valid;
+		m_nextsv	=	0;
+		/* Orient simplex		*/ 
+		if(gjk.det(	simplex.c[0]->w-simplex.c[3]->w,
+					simplex.c[1]->w-simplex.c[3]->w,
+					simplex.c[2]->w-simplex.c[3]->w)<0)
+			{
+			btSwap(simplex.c[0],simplex.c[1]);
+			btSwap(simplex.p[0],simplex.p[1]);
+			}
+		/* Build initial hull	*/ 
+		sFace*	tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
+						newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
+						newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
+						newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
+		if(m_hull.count==4)
+			{
+			sFace*		best=findbest();
+			sFace		outer=*best;
+			U			pass=0;
+			U			iterations=0;
+			bind(tetra[0],0,tetra[1],0);
+			bind(tetra[0],1,tetra[2],0);
+			bind(tetra[0],2,tetra[3],0);
+			bind(tetra[1],1,tetra[3],2);
+			bind(tetra[1],2,tetra[2],1);
+			bind(tetra[2],2,tetra[3],1);
+			m_status=eStatus::Valid;
+			for(;iterations<EPA_MAX_ITERATIONS;++iterations)
+				{
+				if(m_nextsv<EPA_MAX_VERTICES)
+					{	
+					sHorizon		horizon;
+					sSV*			w=&m_sv_store[m_nextsv++];
+					bool			valid=true;					
+					best->pass	=	(U1)(++pass);
+					gjk.getsupport(best->n,*w);
+					const btScalar	wdist=dot(best->n,w->w)-best->d;
+					if(wdist>EPA_ACCURACY)
+						{
+						for(U j=0;(j<3)&&valid;++j)
+							{
+							valid&=expand(	pass,w,
+											best->f[j],best->e[j],
+											horizon);
+							}
+						if(valid&&(horizon.nf>=3))
+							{
+							bind(horizon.cf,1,horizon.ff,2);
+							remove(m_hull,best);
+							append(m_stock,best);
+							best=findbest();
+							if(best->p>=outer.p) outer=*best;
+							} else { m_status=eStatus::InvalidHull;break; }
+						} else { m_status=eStatus::AccuraryReached;break; }
+					} else { m_status=eStatus::OutOfVertices;break; }
+				}
+			const btVector3	projection=outer.n*outer.d;
+			m_normal	=	outer.n;
+			m_depth		=	outer.d;
+			m_result.rank	=	3;
+			m_result.c[0]	=	outer.c[0];
+			m_result.c[1]	=	outer.c[1];
+			m_result.c[2]	=	outer.c[2];
+			m_result.p[0]	=	cross(	outer.c[1]->w-projection,
+										outer.c[2]->w-projection).length();
+			m_result.p[1]	=	cross(	outer.c[2]->w-projection,
+										outer.c[0]->w-projection).length();
+			m_result.p[2]	=	cross(	outer.c[0]->w-projection,
+										outer.c[1]->w-projection).length();
+			const btScalar	sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
+			m_result.p[0]	/=	sum;
+			m_result.p[1]	/=	sum;
+			m_result.p[2]	/=	sum;
+			return(m_status);
+			}
+		}
+	/* Fallback		*/ 
+	m_status	=	eStatus::FallBack;
+	m_normal	=	-guess;
+	const btScalar	nl=m_normal.length();
+	if(nl>0)
+		m_normal	=	m_normal/nl;
+		else
+		m_normal	=	btVector3(1,0,0);
+	m_depth	=	0;
+	m_result.rank=1;
+	m_result.c[0]=simplex.c[0];
+	m_result.p[0]=1;	
+	return(m_status);
+	}
+sFace*				newface(sSV* a,sSV* b,sSV* c,bool forced)
+	{
+	if(m_stock.root)
+		{
+		sFace*	face=m_stock.root;
+		remove(m_stock,face);
+		append(m_hull,face);
+		face->pass	=	0;
+		face->c[0]	=	a;
+		face->c[1]	=	b;
+		face->c[2]	=	c;
+		face->n		=	cross(b->w-a->w,c->w-a->w);
+		const btScalar	l=face->n.length();
+		const bool		v=l>EPA_ACCURACY;
+		face->p		=	btMin(btMin(
+							dot(a->w,cross(face->n,a->w-b->w)),
+							dot(b->w,cross(face->n,b->w-c->w))),
+							dot(c->w,cross(face->n,c->w-a->w)))	/
+							(v?l:1);
+		face->p		=	face->p>=-EPA_INSIDE_EPS?0:face->p;
+		if(v)
+			{
+			face->d		=	dot(a->w,face->n)/l;
+			face->n		/=	l;
+			if(forced||(face->d>=-EPA_PLANE_EPS))
+				{
+				return(face);
+				} else m_status=eStatus::NonConvex;
+			} else m_status=eStatus::Degenerated;
+		remove(m_hull,face);
+		append(m_stock,face);
+		return(0);
+		}
+	m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
+	return(0);
+	}
+sFace*				findbest()
+	{
+	sFace*		minf=m_hull.root;
+	btScalar	mind=minf->d*minf->d;
+	btScalar	maxp=minf->p;
+	for(sFace* f=minf->l[1];f;f=f->l[1])
+		{
+		const btScalar	sqd=f->d*f->d;
+		if((f->p>=maxp)&&(sqd<mind))
+			{
+			minf=f;
+			mind=sqd;
+			maxp=f->p;
+			}
+		}
+	return(minf);
+	}
+bool				expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
+	{
+	static const U	i1m3[]={1,2,0};
+	static const U	i2m3[]={2,0,1};
+	if(f->pass!=pass)
+		{
+		const U	e1=i1m3[e];
+		if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
+			{
+			sFace*	nf=newface(f->c[e1],f->c[e],w,false);
+			if(nf)
+				{
+				bind(nf,0,f,e);
+				if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
+				horizon.cf=nf;
+				++horizon.nf;
+				return(true);
+				}
+			}
+			else
+			{
+			const U	e2=i2m3[e];
+			f->pass		=	(U1)pass;
+			if(	expand(pass,w,f->f[e1],f->e[e1],horizon)&&
+				expand(pass,w,f->f[e2],f->e[e2],horizon))
+				{
+				remove(m_hull,f);
+				append(m_stock,f);
+				return(true);
+				}
+			}
+		}
+	return(false);
+	}
+static inline void		bind(sFace* fa,U ea,sFace* fb,U eb)
+	{
+	fa->e[ea]=(U1)eb;fa->f[ea]=fb;
+	fb->e[eb]=(U1)ea;fb->f[eb]=fa;
+	}
+static inline void		append(sList& list,sFace* face)
+	{
+	face->l[0]	=	0;
+	face->l[1]	=	list.root;
+	if(list.root) list.root->l[0]=face;
+	list.root	=	face;
+	++list.count;
+	}
+static inline void		remove(sList& list,sFace* face)
+	{
+	if(face->l[1]) face->l[1]->l[0]=face->l[0];
+	if(face->l[0]) face->l[0]->l[1]=face->l[1];
+	if(face==list.root) list.root=face->l[1];
+	--list.count;
+	}
+};
+
+//
+static void	Initialize(void* shapeA,
+						SpuConvexPolyhedronVertexData* convexDataA,
+						int shapeTypeA,
+						float marginA,
+						const btTransform& wtrs0,
+						void* shapeB,
+						SpuConvexPolyhedronVertexData* convexDataB,
+						int shapeTypeB,
+						float marginB,
+						const btTransform& wtrs1,	
+						SpuGjkEpaSolver2::sResults& results,
+						tShape& shape,
+						bool withmargins)
+{
+/* Results		*/ 
+results.witnesses[0]	=
+results.witnesses[1]	=	btVector3(0,0,0);
+results.status			=	SpuGjkEpaSolver2::sResults::Separated;
+/* Shape		*/ 
+shape.m_shapes[0].margin = marginA;
+shape.m_shapes[0].shape = shapeA;
+shape.m_shapes[0].shapeType = shapeTypeA;
+shape.m_shapes[0].convexData = convexDataA;
+shape.m_shapes[1].margin = marginB;
+shape.m_shapes[1].shape = shapeB;
+shape.m_shapes[1].shapeType = shapeTypeB;
+shape.m_shapes[1].convexData = convexDataB;
+shape.m_toshape1		=	wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
+shape.m_toshape0		=	wtrs0.inverseTimes(wtrs1);
+shape.EnableMargin(withmargins);
+}
+
+}
+
+//
+// Api
+//
+
+using namespace	gjkepa2_spu_impl;
+
+//
+int			SpuGjkEpaSolver2::StackSizeRequirement()
+{
+return(sizeof(GJK)+sizeof(EPA));
+}
+
+//
+bool   SpuGjkEpaSolver2::Penetration(void* shapeA,
+									SpuConvexPolyhedronVertexData* convexDataA,
+									int shapeTypeA,
+									float marginA,
+									const btTransform& wtrs0,
+									void* shapeB,
+									SpuConvexPolyhedronVertexData* convexDataB,
+									int shapeTypeB,
+									float marginB,
+									const btTransform& wtrs1,
+									const btVector3&		guess,
+									sResults&				results)
+{
+tShape			shape;
+Initialize(shapeA, convexDataA, shapeTypeA, marginA, wtrs0, shapeB, convexDataB, shapeTypeB, marginB, wtrs1, results,shape,true);
+GJK				gjk;	
+GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,-guess);
+switch(gjk_status)
+	{
+	case	GJK::eStatus::Inside:
+		{
+		EPA				epa;
+		EPA::eStatus::_	epa_status=epa.Evaluate(gjk,-guess);
+		if(epa_status!=EPA::eStatus::Failed)
+			{
+			btVector3	w0=btVector3(0,0,0);
+			for(U i=0;i<epa.m_result.rank;++i)
+				{
+				w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
+				}
+			results.status			=	sResults::Penetrating;
+			results.witnesses[0]	=	wtrs0*w0;
+			results.witnesses[1]	=	wtrs0*(w0-epa.m_normal*epa.m_depth);
+			return(true);
+			} else results.status=sResults::EPA_Failed;
+		}
+	break;
+	case	GJK::eStatus::Failed:
+	results.status=sResults::GJK_Failed;
+	break;
+	}
+return(false);
+}
+
+/* Symbols cleanup		*/ 
+
+#undef GJK_MAX_ITERATIONS
+#undef GJK_ACCURARY
+#undef GJK_MIN_DISTANCE
+#undef GJK_DUPLICATED_EPS
+#undef GJK_SIMPLEX2_EPS
+#undef GJK_SIMPLEX3_EPS
+#undef GJK_SIMPLEX4_EPS
+
+#undef EPA_MAX_VERTICES
+#undef EPA_MAX_FACES
+#undef EPA_MAX_ITERATIONS
+#undef EPA_ACCURACY
+#undef EPA_FALLBACK
+#undef EPA_PLANE_EPS
+#undef EPA_INSIDE_EPS

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.h

@@ -0,0 +1,38 @@
+#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
+#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+///btGjkEpaSolver contributed under zlib by Nathanael Presson
+struct	SpuGjkEpaSolver2
+{
+struct	sResults
+	{
+	enum eStatus
+		{
+		Separated,		/* Shapes doesnt penetrate												*/ 
+		Penetrating,	/* Shapes are penetrating												*/ 
+		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
+		EPA_Failed,		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+		}		status;
+	btVector3	witnesses[2];
+	btVector3	normal;
+	};
+
+static int		StackSizeRequirement();
+
+
+static bool		Penetration(void* shapeA,
+							SpuConvexPolyhedronVertexData* convexDataA,
+							int shapeTypeA,
+							float marginA,
+							const btTransform& xformA,
+							void* shapeB,
+							SpuConvexPolyhedronVertexData* convexDataB,
+							int shapeTypeB,
+							float marginB,
+							const btTransform& xformB,
+							const btVector3& guess,
+							sResults&	results);
+};
+
+#endif

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp

@@ -0,0 +1,311 @@
+/*
+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 "SpuGjkPairDetector.h"
+#include "SpuConvexPenetrationDepthSolver.h"
+#include "SpuCollisionShapes.h"
+
+
+
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#endif //__SPU__
+#endif
+
+//must be above the machine epsilon
+#define REL_ERROR2 btScalar(1.0e-6)
+
+//temp globals, to improve GJK/EPA/penetration calculations
+int gSpuNumDeepPenetrationChecks = 0;
+int gSpuNumGjkChecks = 0;
+
+
+
+SpuGjkPairDetector::SpuGjkPairDetector(void* objectA,void* objectB,int shapeTypeA, int shapeTypeB, float marginA,float marginB,SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver*	penetrationDepthSolver)
+:m_cachedSeparatingAxis(float(0.),float(0.),float(1.)),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_shapeTypeA(shapeTypeA),
+m_shapeTypeB(shapeTypeB),
+m_marginA(marginA),
+m_marginB(marginB),
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(1)
+{
+}
+
+void SpuGjkPairDetector::getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output)
+{
+	btScalar distance=btScalar(0.);
+	btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 pointOnA,pointOnB;
+	btTransform	localTransA = input.m_transformA;
+	btTransform localTransB = input.m_transformB;
+	btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
+	localTransA.getOrigin() -= positionOffset;
+	localTransB.getOrigin() -= positionOffset;
+
+	btScalar marginA = m_marginA;
+	btScalar marginB = m_marginB;
+
+	gSpuNumGjkChecks++;
+
+	//for CCD we don't use margins
+	if (m_ignoreMargin)
+	{
+		marginA = btScalar(0.);
+		marginB = btScalar(0.);
+	}
+
+	m_curIter = 0;
+	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
+	m_cachedSeparatingAxis.setValue(0,1,0);
+
+	bool isValid = false;
+	bool checkSimplex = false;
+	bool checkPenetration = true;
+	m_degenerateSimplex = 0;
+
+	m_lastUsedMethod = -1;
+
+	{
+		btScalar squaredDistance = SIMD_INFINITY;
+		btScalar delta = btScalar(0.);
+		
+		btScalar margin = marginA + marginB;
+		
+		
+
+		m_simplexSolver->reset();
+		
+		for ( ; ; )
+		//while (true)
+		{
+
+			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
+			btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+
+//			btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
+//			btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
+
+			btVector3 pInA  = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
+			btVector3 qInB  = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
+
+
+			btPoint3  pWorld = localTransA(pInA);	
+			btPoint3  qWorld = localTransB(qInB);
+			
+			btVector3 w	= pWorld - qWorld;
+			delta = m_cachedSeparatingAxis.dot(w);
+
+			// potential exit, they don't overlap
+			if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
+			{
+				checkPenetration = false;
+				break;
+			}
+
+			//exit 0: the new point is already in the simplex, or we didn't come any closer
+			if (m_simplexSolver->inSimplex(w))
+			{
+				m_degenerateSimplex = 1;
+				checkSimplex = true;
+				break;
+			}
+			// are we getting any closer ?
+			btScalar f0 = squaredDistance - delta;
+			btScalar f1 = squaredDistance * REL_ERROR2;
+
+			if (f0 <= f1)
+			{
+				if (f0 <= btScalar(0.))
+				{
+					m_degenerateSimplex = 2;
+				}
+				checkSimplex = true;
+				break;
+			}
+			//add current vertex to simplex
+			m_simplexSolver->addVertex(w, pWorld, qWorld);
+
+			//calculate the closest point to the origin (update vector v)
+			if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
+			{
+				m_degenerateSimplex = 3;
+				checkSimplex = true;
+				break;
+			}
+
+			btScalar previousSquaredDistance = squaredDistance;
+			squaredDistance = m_cachedSeparatingAxis.length2();
+			
+			//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+
+			//are we getting any closer ?
+			if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
+			{ 
+				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+				checkSimplex = true;
+				break;
+			}
+
+			  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
+              if (m_curIter++ > gGjkMaxIter)   
+              {   
+                      #if defined(DEBUG) || defined (_DEBUG)   
+
+                              printf("SpuGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
+                              printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
+                              m_cachedSeparatingAxis.getX(),   
+                              m_cachedSeparatingAxis.getY(),   
+                              m_cachedSeparatingAxis.getZ(),   
+                              squaredDistance,   
+							  m_shapeTypeA,   
+                              m_shapeTypeB);
+
+                      #endif   
+                      break;   
+
+              } 
+
+
+			bool check = (!m_simplexSolver->fullSimplex());
+			//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+
+			if (!check)
+			{
+				//do we need this backup_closest here ?
+				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+				break;
+			}
+		}
+
+		if (checkSimplex)
+		{
+			m_simplexSolver->compute_points(pointOnA, pointOnB);
+			normalInB = pointOnA-pointOnB;
+			btScalar lenSqr = m_cachedSeparatingAxis.length2();
+			//valid normal
+			if (lenSqr < 0.0001)
+			{
+				m_degenerateSimplex = 5;
+			} 
+			if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
+			{
+				btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+				normalInB *= rlen; //normalize
+				btScalar s = btSqrt(squaredDistance);
+			
+				btAssert(s > btScalar(0.0));
+				pointOnA -= m_cachedSeparatingAxis * (marginA / s);
+				pointOnB += m_cachedSeparatingAxis * (marginB / s);
+				distance = ((btScalar(1.)/rlen) - margin);
+				isValid = true;
+				
+				m_lastUsedMethod = 1;
+			} else
+			{
+				m_lastUsedMethod = 2;
+			}
+		}
+
+		bool catchDegeneratePenetrationCase = 
+			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
+
+		//if (checkPenetration && !isValid)
+		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
+		{
+			//penetration case
+		
+			//if there is no way to handle penetrations, bail out
+			if (m_penetrationDepthSolver)
+			{
+				// Penetration depth case.
+				btVector3 tmpPointOnA,tmpPointOnB;
+				
+				gSpuNumDeepPenetrationChecks++;
+
+				bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
+					*m_simplexSolver, 
+					m_minkowskiA,m_minkowskiB,
+                    m_shapeTypeA, m_shapeTypeB,
+                    marginA, marginB,
+					localTransA,localTransB,
+					m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
+					0,input.m_stackAlloc,input.m_convexVertexData[0], input.m_convexVertexData[1]
+					);
+
+				if (isValid2)
+				{
+					btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
+					btScalar lenSqr = tmpNormalInB.length2();
+					if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+					{
+						tmpNormalInB /= btSqrt(lenSqr);
+						btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
+						//only replace valid penetrations when the result is deeper (check)
+						if (!isValid || (distance2 < distance))
+						{
+							distance = distance2;
+							pointOnA = tmpPointOnA;
+							pointOnB = tmpPointOnB;
+							normalInB = tmpNormalInB;
+							isValid = true;
+							m_lastUsedMethod = 3;
+						} else
+						{
+							
+						}
+					} else
+					{
+						//isValid = false;
+						m_lastUsedMethod = 4;
+					}
+				} else
+				{
+					m_lastUsedMethod = 5;
+				}
+				
+			}
+		}
+	}
+
+	if (isValid)
+	{
+#ifdef __SPU__
+		//spu_printf("distance\n");
+#endif //__SPU__
+
+
+		output.addContactPoint(
+			normalInB,
+			pointOnB+positionOffset,
+			distance);
+		//printf("gjk add:%f",distance);
+	}
+
+
+}
+
+
+
+
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h

@@ -0,0 +1,93 @@
+
+/*
+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 SPU_GJK_PAIR_DETECTOR_H
+#define SPU_GJK_PAIR_DETECTOR_H
+
+
+
+#include "SpuContactResult.h"
+
+
+#include "SpuVoronoiSimplexSolver.h"
+class SpuConvexPenetrationDepthSolver;
+
+/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
+class SpuGjkPairDetector 
+{
+	
+
+	btVector3	m_cachedSeparatingAxis;
+	const SpuConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	SpuVoronoiSimplexSolver* m_simplexSolver;
+	void* m_minkowskiA;
+	void* m_minkowskiB;
+    int m_shapeTypeA;
+    int m_shapeTypeB;
+    float m_marginA;
+    float m_marginB;
+	bool		m_ignoreMargin;
+	
+
+public:
+
+	//some debugging to fix degeneracy problems
+	int			m_lastUsedMethod;
+	int			m_curIter;
+	int			m_degenerateSimplex;
+	int			m_catchDegeneracies;
+
+
+	SpuGjkPairDetector(void* objectA,void* objectB,int m_shapeTypeA, int m_shapeTypeB, float marginA, float marginB, SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver*	penetrationDepthSolver);
+	virtual ~SpuGjkPairDetector() {};
+
+	virtual void	getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output);
+
+	void setMinkowskiA(void* minkA)
+	{
+		m_minkowskiA = minkA;
+	}
+
+	void setMinkowskiB(void* minkB)
+	{
+		m_minkowskiB = minkB;
+	}
+
+	void setCachedSeperatingAxis(const btVector3& seperatingAxis)
+	{
+		m_cachedSeparatingAxis = seperatingAxis;
+	}
+
+	void	setPenetrationDepthSolver(SpuConvexPenetrationDepthSolver*	penetrationDepthSolver)
+	{
+		m_penetrationDepthSolver = penetrationDepthSolver;
+	}
+
+	///don't use setIgnoreMargin, it's for Bullet's internal use
+	void	setIgnoreMargin(bool ignoreMargin)
+	{
+		m_ignoreMargin = ignoreMargin;
+	}
+
+
+};
+
+
+
+#endif //SPU_GJK_PAIR_DETECTOR_H

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h

@@ -0,0 +1,19 @@
+/*
+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.
+*/
+
+
+
+
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp

@@ -0,0 +1,347 @@
+/*
+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 "SpuMinkowskiPenetrationDepthSolver.h"
+#include "SpuVoronoiSimplexSolver.h"
+#include "SpuGjkPairDetector.h"
+#include "SpuContactResult.h"
+#include "SpuPreferredPenetrationDirections.h"
+
+
+#include "SpuCollisionShapes.h"
+
+#define NUM_UNITSPHERE_POINTS 42
+static btVector3	sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
+{
+btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
+btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
+btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
+btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
+btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
+btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
+btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
+btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
+btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
+btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
+btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
+btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
+btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
+btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
+btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
+btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
+btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
+btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
+btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
+btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
+btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
+btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
+btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
+btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
+btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
+btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
+btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
+btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
+btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
+btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
+btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
+btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
+btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
+btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
+btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
+btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
+btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
+btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
+};
+
+bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
+	        void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
+            btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataA,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataB
+			) const
+{
+
+	(void)stackAlloc;
+	(void)v;
+	
+
+	struct btIntermediateResult : public SpuContactResult
+	{
+
+		btIntermediateResult():m_hasResult(false)
+		{
+		}
+		
+		btVector3 m_normalOnBInWorld;
+		btVector3 m_pointInWorld;
+		btScalar m_depth;
+		bool	m_hasResult;
+
+		virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+		{
+			(void)partId0;
+			(void)index0;
+			(void)partId1;
+			(void)index1;
+		}
+		void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+		{
+			m_normalOnBInWorld = normalOnBInWorld;
+			m_pointInWorld = pointInWorld;
+			m_depth = depth;
+			m_hasResult = true;
+		}
+	};
+
+	//just take fixed number of orientation, and sample the penetration depth in that direction
+	btScalar minProj = btScalar(1e30);
+	btVector3 minNorm;
+	btVector3 minVertex;
+	btVector3 minA,minB;
+	btVector3 seperatingAxisInA,seperatingAxisInB;
+	btVector3 pInA,qInB,pWorld,qWorld,w;
+
+//#define USE_BATCHED_SUPPORT 1
+#ifdef USE_BATCHED_SUPPORT
+
+	btVector3	supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	int i;
+
+	int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+	for (i=0;i<numSampleDirections;i++)
+	{
+		const btVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
+		seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
+	}
+
+	{
+		int numPDA = convexA->getNumPreferredPenetrationDirections();
+		if (numPDA)
+		{
+			for (int i=0;i<numPDA;i++)
+			{
+				btVector3 norm;
+				convexA->getPreferredPenetrationDirection(i,norm);
+				norm  = transA.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+				numSampleDirections++;
+			}
+		}
+	}
+
+	{
+		int numPDB = convexB->getNumPreferredPenetrationDirections();
+		if (numPDB)
+		{
+			for (int i=0;i<numPDB;i++)
+			{
+				btVector3 norm;
+				convexB->getPreferredPenetrationDirection(i,norm);
+				norm  = transB.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+				numSampleDirections++;
+			}
+		}
+	}
+
+
+
+	convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
+	convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
+
+	for (i=0;i<numSampleDirections;i++)
+	{
+		const btVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInA = seperatingAxisInABatch[i];
+		seperatingAxisInB = seperatingAxisInBBatch[i];
+
+		pInA = supportVerticesABatch[i];
+		qInB = supportVerticesBBatch[i];
+
+		pWorld = transA(pInA);	
+		qWorld = transB(qInB);
+		w	= qWorld - pWorld;
+		btScalar delta = norm.dot(w);
+		//find smallest delta
+		if (delta < minProj)
+		{
+			minProj = delta;
+			minNorm = norm;
+			minA = pWorld;
+			minB = qWorld;
+		}
+	}	
+#else
+
+	int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+///this is necessary, otherwise the normal is not correct, and sphere will rotate forever on a sloped triangle mesh
+#define DO_PREFERRED_DIRECTIONS 1
+#ifdef DO_PREFERRED_DIRECTIONS
+	{
+		int numPDA = spuGetNumPreferredPenetrationDirections(shapeTypeA,convexA);
+		if (numPDA)
+		{
+			for (int i=0;i<numPDA;i++)
+			{
+				btVector3 norm;
+				spuGetPreferredPenetrationDirection(shapeTypeA,convexA,i,norm);
+				norm  = transA.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+
+	{
+		int numPDB = spuGetNumPreferredPenetrationDirections(shapeTypeB,convexB);
+		if (numPDB)
+		{
+			for (int i=0;i<numPDB;i++)
+			{
+				btVector3 norm;
+				spuGetPreferredPenetrationDirection(shapeTypeB,convexB,i,norm);
+				norm  = transB.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+#endif //DO_PREFERRED_DIRECTIONS
+
+	for (int i=0;i<numSampleDirections;i++)
+	{
+		const btVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInA = (-norm)* transA.getBasis();
+		seperatingAxisInB = norm* transB.getBasis();
+
+		pInA = localGetSupportingVertexWithoutMargin(shapeTypeA, convexA, seperatingAxisInA,convexVertexDataA);//, NULL);
+		qInB = localGetSupportingVertexWithoutMargin(shapeTypeB, convexB, seperatingAxisInB,convexVertexDataB);//, NULL);
+
+	//	pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
+	//	qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
+
+		pWorld = transA(pInA);	
+		qWorld = transB(qInB);
+		w	= qWorld - pWorld;
+		btScalar delta = norm.dot(w);
+		//find smallest delta
+		if (delta < minProj)
+		{
+			minProj = delta;
+			minNorm = norm;
+			minA = pWorld;
+			minB = qWorld;
+		}
+	}
+#endif //USE_BATCHED_SUPPORT
+
+	//add the margins
+
+	minA += minNorm*marginA;
+	minB -= minNorm*marginB;
+	//no penetration
+	if (minProj < btScalar(0.))
+		return false;
+
+	minProj += (marginA + marginB) + btScalar(1.00);
+
+
+
+
+
+//#define DEBUG_DRAW 1
+#ifdef DEBUG_DRAW
+	if (debugDraw)
+	{
+		btVector3 color(0,1,0);
+		debugDraw->drawLine(minA,minB,color);
+		color = btVector3 (1,1,1);
+		btVector3 vec = minB-minA;
+		btScalar prj2 = minNorm.dot(vec);
+		debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
+
+	}
+#endif //DEBUG_DRAW
+
+	
+
+	SpuGjkPairDetector gjkdet(convexA,convexB,shapeTypeA,shapeTypeB,marginA,marginB,&simplexSolver,0);
+
+	btScalar offsetDist = minProj;
+	btVector3 offset = minNorm * offsetDist;
+	
+
+	SpuClosestPointInput input;
+	input.m_convexVertexData[0] = convexVertexDataA;
+	input.m_convexVertexData[1] = convexVertexDataB;
+	btVector3 newOrg = transA.getOrigin() + offset;
+
+	btTransform displacedTrans = transA;
+	displacedTrans.setOrigin(newOrg);
+
+	input.m_transformA = displacedTrans;
+	input.m_transformB = transB;
+	input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
+	
+	btIntermediateResult res;
+	gjkdet.getClosestPoints(input,res);
+
+	btScalar correctedMinNorm = minProj - res.m_depth;
+
+
+	//the penetration depth is over-estimated, relax it
+	btScalar penetration_relaxation= btScalar(1.);
+	minNorm*=penetration_relaxation;
+
+	if (res.m_hasResult)
+	{
+
+		pa = res.m_pointInWorld - minNorm * correctedMinNorm;
+		pb = res.m_pointInWorld;
+		
+#ifdef DEBUG_DRAW
+		if (debugDraw)
+		{
+			btVector3 color(1,0,0);
+			debugDraw->drawLine(pa,pb,color);
+		}
+#endif//DEBUG_DRAW
+
+
+	} else {
+		// could not seperate shapes
+		btAssert (false);
+	}
+	return res.m_hasResult;
+}
+
+
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h

@@ -0,0 +1,47 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
+
+#include "SpuConvexPenetrationDepthSolver.h"
+
+class btStackAlloc;
+class btIDebugDraw;
+class SpuVoronoiSimplexSolver;
+
+///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
+///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
+class SpuMinkowskiPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver
+{
+public:
+
+	virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
+	        void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
+            btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataA,
+			struct SpuConvexPolyhedronVertexData* convexVertexDataB
+			) const;
+
+
+};
+
+
+#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h

@@ -0,0 +1,70 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 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 _SPU_PREFERRED_PENETRATION_DIRECTIONS_H
+#define _SPU_PREFERRED_PENETRATION_DIRECTIONS_H
+
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+
+int		spuGetNumPreferredPenetrationDirections(int shapeType, void* shape)
+{
+	switch (shapeType)
+    {
+		case TRIANGLE_SHAPE_PROXYTYPE:
+		{
+			return 2;
+			//spu_printf("2\n");
+			break;
+		}
+		default:
+			{
+#if __ASSERT
+        spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType);
+#endif // __ASSERT
+			}
+	}
+
+	return 0;	
+}	
+
+void	spuGetPreferredPenetrationDirection(int shapeType, void* shape, int index, btVector3& penetrationVector)
+{
+
+
+	switch (shapeType)
+    {
+		case TRIANGLE_SHAPE_PROXYTYPE:
+		{
+			btVector3* vertices = (btVector3*)shape;
+			///calcNormal
+			penetrationVector = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
+			penetrationVector.normalize();
+			if (index)
+				penetrationVector *= btScalar(-1.);
+			break;
+		}
+		default:
+			{
+					
+#if __ASSERT
+        spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType);
+#endif // __ASSERT
+			}
+	}
+		
+}
+
+#endif //_SPU_PREFERRED_PENETRATION_DIRECTIONS_H

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp

@@ -0,0 +1,606 @@
+
+/*
+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.
+	
+	Elsevier CDROM license agreements grants nonexclusive license to use the software
+	for any purpose, commercial or non-commercial as long as the following credit is included
+	identifying the original source of the software:
+
+	Parts of the source are "from the book Real-Time Collision Detection by
+	Christer Ericson, published by Morgan Kaufmann Publishers,
+	(c) 2005 Elsevier Inc."
+		
+*/
+
+
+#include "SpuVoronoiSimplexSolver.h"
+#include <assert.h>
+#include <stdio.h>
+
+#define VERTA  0
+#define VERTB  1
+#define VERTC  2
+#define VERTD  3
+
+#define CATCH_DEGENERATE_TETRAHEDRON 1
+void	SpuVoronoiSimplexSolver::removeVertex(int index)
+{
+	
+	assert(m_numVertices>0);
+	m_numVertices--;
+	m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
+	m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
+	m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
+}
+
+void	SpuVoronoiSimplexSolver::reduceVertices (const SpuUsageBitfield& usedVerts)
+{
+	if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
+		removeVertex(3);
+
+	if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
+		removeVertex(2);
+
+	if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
+		removeVertex(1);
+	
+	if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
+		removeVertex(0);
+
+}
+
+
+
+
+
+//clear the simplex, remove all the vertices
+void SpuVoronoiSimplexSolver::reset()
+{
+	m_cachedValidClosest = false;
+	m_numVertices = 0;
+	m_needsUpdate = true;
+	m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	m_cachedBC.reset();
+}
+
+
+
+	//add a vertex
+void SpuVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
+{
+	m_lastW = w;
+	m_needsUpdate = true;
+
+	m_simplexVectorW[m_numVertices] = w;
+	m_simplexPointsP[m_numVertices] = p;
+	m_simplexPointsQ[m_numVertices] = q;
+
+	m_numVertices++;
+}
+
+bool	SpuVoronoiSimplexSolver::updateClosestVectorAndPoints()
+{
+	
+	if (m_needsUpdate)
+	{
+		m_cachedBC.reset();
+
+		m_needsUpdate = false;
+
+		switch (numVertices())
+		{
+		case 0:
+				m_cachedValidClosest = false;
+				break;
+		case 1:
+			{
+				m_cachedP1 = m_simplexPointsP[0];
+				m_cachedP2 = m_simplexPointsQ[0];
+				m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
+				m_cachedBC.reset();
+				m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
+				m_cachedValidClosest = m_cachedBC.isValid();
+				break;
+			};
+		case 2:
+			{
+			//closest point origin from line segment
+					const btVector3& from = m_simplexVectorW[0];
+					const btVector3& to = m_simplexVectorW[1];
+					btVector3 nearest;
+
+					btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+					btVector3 diff = p - from;
+					btVector3 v = to - from;
+					btScalar t = v.dot(diff);
+					
+					if (t > 0) {
+						btScalar dotVV = v.dot(v);
+						if (t < dotVV) {
+							t /= dotVV;
+							diff -= t*v;
+							m_cachedBC.m_usedVertices.usedVertexA = true;
+							m_cachedBC.m_usedVertices.usedVertexB = true;
+						} else {
+							t = 1;
+							diff -= v;
+							//reduce to 1 point
+							m_cachedBC.m_usedVertices.usedVertexB = true;
+						}
+					} else
+					{
+						t = 0;
+						//reduce to 1 point
+						m_cachedBC.m_usedVertices.usedVertexA = true;
+					}
+					m_cachedBC.setBarycentricCoordinates(1-t,t);
+					nearest = from + t*v;
+
+					m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
+					m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
+					m_cachedV = m_cachedP1 - m_cachedP2;
+					
+					reduceVertices(m_cachedBC.m_usedVertices);
+
+					m_cachedValidClosest = m_cachedBC.isValid();
+					break;
+			}
+		case 3:
+			{
+				//closest point origin from triangle
+				btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+				
+				const btVector3& a = m_simplexVectorW[0];
+				const btVector3& b = m_simplexVectorW[1];
+				const btVector3& c = m_simplexVectorW[2];
+
+				closestPtPointTriangle(p,a,b,c,m_cachedBC);
+				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+								m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+								m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+								m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+					m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+					m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+					m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+				m_cachedV = m_cachedP1-m_cachedP2;
+
+				reduceVertices (m_cachedBC.m_usedVertices);
+				m_cachedValidClosest =  m_cachedBC.isValid();
+
+				break;
+			}
+		case 4:
+			{
+
+				
+				btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+				
+				const btVector3& a = m_simplexVectorW[0];
+				const btVector3& b = m_simplexVectorW[1];
+				const btVector3& c = m_simplexVectorW[2];
+				const btVector3& d = m_simplexVectorW[3];
+
+				bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
+
+				if (hasSeperation)
+				{
+
+					m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+						m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+						m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+						m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+					m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+						m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+						m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+						m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+					m_cachedV = m_cachedP1-m_cachedP2;
+					reduceVertices (m_cachedBC.m_usedVertices);
+				} else
+				{
+//					printf("sub distance got penetration\n");
+
+					if (m_cachedBC.m_degenerate)
+					{
+						m_cachedValidClosest = false;
+					} else
+					{
+						m_cachedValidClosest = true;
+						//degenerate case == false, penetration = true + zero
+						m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+					}
+					break;
+				}
+
+				m_cachedValidClosest = m_cachedBC.isValid();
+
+				//closest point origin from tetrahedron
+				break;
+			}
+		default:
+			{
+				m_cachedValidClosest = false;
+			}
+		};
+	}
+
+	return m_cachedValidClosest;
+
+}
+
+//return/calculate the closest vertex
+bool SpuVoronoiSimplexSolver::closest(btVector3& v)
+{
+	bool succes = updateClosestVectorAndPoints();
+	v = m_cachedV;
+	return succes;
+}
+
+
+
+btScalar SpuVoronoiSimplexSolver::maxVertex()
+{
+	int i, numverts = numVertices();
+	btScalar maxV = btScalar(0.);
+	for (i=0;i<numverts;i++)
+	{
+		btScalar curLen2 = m_simplexVectorW[i].length2();
+		if (maxV < curLen2)
+			maxV = curLen2;
+	}
+	return maxV;
+}
+
+
+
+	//return the current simplex
+int SpuVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
+{
+	int i;
+	for (i=0;i<numVertices();i++)
+	{
+		yBuf[i] = m_simplexVectorW[i];
+		pBuf[i] = m_simplexPointsP[i];
+		qBuf[i] = m_simplexPointsQ[i];
+	}
+	return numVertices();
+}
+
+
+
+
+bool SpuVoronoiSimplexSolver::inSimplex(const btVector3& w)
+{
+	bool found = false;
+	int i, numverts = numVertices();
+	//btScalar maxV = btScalar(0.);
+	
+	//w is in the current (reduced) simplex
+	for (i=0;i<numverts;i++)
+	{
+		if (m_simplexVectorW[i] == w)
+			found = true;
+	}
+
+	//check in case lastW is already removed
+	if (w == m_lastW)
+		return true;
+    	
+	return found;
+}
+
+void SpuVoronoiSimplexSolver::backup_closest(btVector3& v) 
+{
+	v = m_cachedV;
+}
+
+
+bool SpuVoronoiSimplexSolver::emptySimplex() const 
+{
+	return (numVertices() == 0);
+
+}
+
+void SpuVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2) 
+{
+	updateClosestVectorAndPoints();
+	p1 = m_cachedP1;
+	p2 = m_cachedP2;
+
+}
+
+
+
+
+bool	SpuVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,SpuSubSimplexClosestResult& result)
+{
+	result.m_usedVertices.reset();
+
+    // Check if P in vertex region outside A
+    btVector3 ab = b - a;
+    btVector3 ac = c - a;
+    btVector3 ap = p - a;
+    btScalar d1 = ab.dot(ap);
+    btScalar d2 = ac.dot(ap);
+    if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) 
+	{
+		result.m_closestPointOnSimplex = a;
+		result.m_usedVertices.usedVertexA = true;
+		result.setBarycentricCoordinates(1,0,0);
+		return true;// a; // barycentric coordinates (1,0,0)
+	}
+
+    // Check if P in vertex region outside B
+    btVector3 bp = p - b;
+    btScalar d3 = ab.dot(bp);
+    btScalar d4 = ac.dot(bp);
+    if (d3 >= btScalar(0.0) && d4 <= d3) 
+	{
+		result.m_closestPointOnSimplex = b;
+		result.m_usedVertices.usedVertexB = true;
+		result.setBarycentricCoordinates(0,1,0);
+
+		return true; // b; // barycentric coordinates (0,1,0)
+	}
+    // Check if P in edge region of AB, if so return projection of P onto AB
+    btScalar vc = d1*d4 - d3*d2;
+    if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
+        btScalar v = d1 / (d1 - d3);
+		result.m_closestPointOnSimplex = a + v * ab;
+		result.m_usedVertices.usedVertexA = true;
+		result.m_usedVertices.usedVertexB = true;
+		result.setBarycentricCoordinates(1-v,v,0);
+		return true;
+        //return a + v * ab; // barycentric coordinates (1-v,v,0)
+    }
+
+    // Check if P in vertex region outside C
+    btVector3 cp = p - c;
+    btScalar d5 = ab.dot(cp);
+    btScalar d6 = ac.dot(cp);
+    if (d6 >= btScalar(0.0) && d5 <= d6) 
+	{
+		result.m_closestPointOnSimplex = c;
+		result.m_usedVertices.usedVertexC = true;
+		result.setBarycentricCoordinates(0,0,1);
+		return true;//c; // barycentric coordinates (0,0,1)
+	}
+
+    // Check if P in edge region of AC, if so return projection of P onto AC
+    btScalar vb = d5*d2 - d1*d6;
+    if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
+        btScalar w = d2 / (d2 - d6);
+		result.m_closestPointOnSimplex = a + w * ac;
+		result.m_usedVertices.usedVertexA = true;
+		result.m_usedVertices.usedVertexC = true;
+		result.setBarycentricCoordinates(1-w,0,w);
+		return true;
+        //return a + w * ac; // barycentric coordinates (1-w,0,w)
+    }
+
+    // Check if P in edge region of BC, if so return projection of P onto BC
+    btScalar va = d3*d6 - d5*d4;
+    if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
+        btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+		
+		result.m_closestPointOnSimplex = b + w * (c - b);
+		result.m_usedVertices.usedVertexB = true;
+		result.m_usedVertices.usedVertexC = true;
+		result.setBarycentricCoordinates(0,1-w,w);
+		return true;		
+       // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+    }
+
+    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+    btScalar denom = btScalar(1.0) / (va + vb + vc);
+    btScalar v = vb * denom;
+    btScalar w = vc * denom;
+    
+	result.m_closestPointOnSimplex = a + ab * v + ac * w;
+	result.m_usedVertices.usedVertexA = true;
+	result.m_usedVertices.usedVertexB = true;
+	result.m_usedVertices.usedVertexC = true;
+	result.setBarycentricCoordinates(1-v-w,v,w);
+	
+	return true;
+//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
+
+}
+
+
+
+
+
+/// Test if point p and d lie on opposite sides of plane through abc
+int SpuVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d)
+{
+	btVector3 normal = (b-a).cross(c-a);
+
+    btScalar signp = (p - a).dot(normal); // [AP AB AC]
+    btScalar signd = (d - a).dot( normal); // [AD AB AC]
+
+#ifdef CATCH_DEGENERATE_TETRAHEDRON
+#ifdef BT_USE_DOUBLE_PRECISION
+if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
+	{
+		return -1;
+	}
+#else
+	if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
+	{
+//		printf("affine dependent/degenerate\n");//
+		return -1;
+	}
+#endif
+
+#endif
+	// Points on opposite sides if expression signs are opposite
+    return signp * signd < btScalar(0.);
+}
+
+
+bool	SpuVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, SpuSubSimplexClosestResult& finalResult)
+{
+	SpuSubSimplexClosestResult tempResult;
+
+    // Start out assuming point inside all halfspaces, so closest to itself
+	finalResult.m_closestPointOnSimplex = p;
+	finalResult.m_usedVertices.reset();
+    finalResult.m_usedVertices.usedVertexA = true;
+	finalResult.m_usedVertices.usedVertexB = true;
+	finalResult.m_usedVertices.usedVertexC = true;
+	finalResult.m_usedVertices.usedVertexD = true;
+
+    int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
+	int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
+  	int	pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
+	int	pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
+
+   if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
+   {
+	   finalResult.m_degenerate = true;
+	   return false;
+   }
+
+   if (!pointOutsideABC  && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
+	 {
+		 return false;
+	 }
+
+
+    btScalar bestSqDist = FLT_MAX;
+    // If point outside face abc then compute closest point on abc
+	if (pointOutsideABC) 
+	{
+        closestPtPointTriangle(p, a, b, c,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		
+        btScalar sqDist = (q - p).dot( q - p);
+        // Update best closest point if (squared) distance is less than current best
+        if (sqDist < bestSqDist) {
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			//convert result bitmask!
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+			finalResult.setBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTB],
+					tempResult.m_barycentricCoords[VERTC],
+					0
+			);
+
+		}
+    }
+  
+
+	// Repeat test for face acd
+	if (pointOutsideACD) 
+	{
+        closestPtPointTriangle(p, a, c, d,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        btScalar sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
+			finalResult.setBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					0,
+					tempResult.m_barycentricCoords[VERTB],
+					tempResult.m_barycentricCoords[VERTC]
+			);
+
+		}
+    }
+    // Repeat test for face adb
+
+	
+	if (pointOutsideADB)
+	{
+		closestPtPointTriangle(p, a, d, b,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        btScalar sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
+			finalResult.setBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTC],
+					0,
+					tempResult.m_barycentricCoords[VERTB]
+			);
+
+		}
+    }
+    // Repeat test for face bdc
+    
+
+	if (pointOutsideBDC)
+	{
+        closestPtPointTriangle(p, b, d, c,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+        btScalar sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+
+			finalResult.setBarycentricCoordinates(
+					0,
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTC],
+					tempResult.m_barycentricCoords[VERTB]
+			);
+
+		}
+    }
+
+	//help! we ended up full !
+	
+	if (finalResult.m_usedVertices.usedVertexA &&
+		finalResult.m_usedVertices.usedVertexB &&
+		finalResult.m_usedVertices.usedVertexC &&
+		finalResult.m_usedVertices.usedVertexD) 
+	{
+		return true;
+	}
+
+    return true;
+}
+

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h

@@ -0,0 +1,156 @@
+
+/*
+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 SPUVoronoiSimplexSolver_H
+#define SPUVoronoiSimplexSolver_H
+
+#include <LinearMath/btTransform.h>
+#include <LinearMath/btPoint3.h>
+
+#define VORONOI_SIMPLEX_MAX_VERTS 5
+
+struct SpuUsageBitfield{
+	SpuUsageBitfield()
+	{
+		reset();
+	}
+
+	void reset()
+	{
+		usedVertexA = false;
+		usedVertexB = false;
+		usedVertexC = false;
+		usedVertexD = false;
+	}
+	unsigned short usedVertexA	: 1;
+	unsigned short usedVertexB	: 1;
+	unsigned short usedVertexC	: 1;
+	unsigned short usedVertexD	: 1;
+	unsigned short unused1		: 1;
+	unsigned short unused2		: 1;
+	unsigned short unused3		: 1;
+	unsigned short unused4		: 1;
+};
+
+
+struct	SpuSubSimplexClosestResult
+{
+	btVector3	m_closestPointOnSimplex;
+	//MASK for m_usedVertices
+	//stores the simplex vertex-usage, using the MASK, 
+	// if m_usedVertices & MASK then the related vertex is used
+	SpuUsageBitfield	m_usedVertices;
+	float	m_barycentricCoords[4];
+	bool m_degenerate;
+
+	void	reset()
+	{
+		m_degenerate = false;
+		setBarycentricCoordinates();
+		m_usedVertices.reset();
+	}
+	bool	isValid()
+	{
+		bool valid = (m_barycentricCoords[0] >= float(0.)) &&
+			(m_barycentricCoords[1] >= float(0.)) &&
+			(m_barycentricCoords[2] >= float(0.)) &&
+			(m_barycentricCoords[3] >= float(0.));
+
+
+		return valid;
+	}
+	void	setBarycentricCoordinates(float a=float(0.),float b=float(0.),float c=float(0.),float d=float(0.))
+	{
+		m_barycentricCoords[0] = a;
+		m_barycentricCoords[1] = b;
+		m_barycentricCoords[2] = c;
+		m_barycentricCoords[3] = d;
+	}
+
+};
+
+/// SpuVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
+/// Can be used with GJK, as an alternative to Johnson distance algorithm.
+class SpuVoronoiSimplexSolver
+{
+public:
+
+	int	m_numVertices;
+
+	btVector3	m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
+	btVector3	m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
+	btVector3	m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
+
+	int m_VertexIndexA[VORONOI_SIMPLEX_MAX_VERTS];
+	int m_VertexIndexB[VORONOI_SIMPLEX_MAX_VERTS];
+
+	btVector3	m_cachedP1;
+	btVector3	m_cachedP2;
+	btVector3	m_cachedV;
+	btVector3	m_lastW;
+	bool		m_cachedValidClosest;
+
+	SpuSubSimplexClosestResult m_cachedBC;
+
+	bool	m_needsUpdate;
+
+	void	removeVertex(int index);
+	void	reduceVertices (const SpuUsageBitfield& usedVerts);
+	bool	updateClosestVectorAndPoints();
+
+	bool	closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, SpuSubSimplexClosestResult& finalResult);
+	int		pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d);
+	bool	closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,SpuSubSimplexClosestResult& result);
+
+	int RemoveDegenerateIndices (const int *inArray, int numIndices, int *outArray) const;
+
+public:
+
+	void reset();
+
+	void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
+
+
+	bool closest(btVector3& v);
+
+	btScalar maxVertex();
+
+	bool fullSimplex() const
+	{
+		return (m_numVertices == 4);
+	}
+
+	int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;
+
+	bool inSimplex(const btVector3& w);
+
+	void backup_closest(btVector3& v) ;
+
+	bool emptySimplex() const ;
+
+	void compute_points(btVector3& p1, btVector3& p2) ;
+
+	int numVertices() const 
+	{
+		return m_numVertices;
+	}
+};
+
+
+
+#endif //SpuVoronoiSimplexSolver

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/readme.txt

@@ -0,0 +1 @@
+Empty placeholder for future Libspe2 SPU task

src/BulletMultiThreaded/SpuParallelSolver.cpp

@@ -0,0 +1,604 @@
+/*
+Bullet Continuous Collision Detection and Physics Library - Parallel solver
+Copyright (c) 2007 Starbreeze Studios
+
+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.
+
+Written by: Marten Svanfeldt
+*/
+
+#include "SpuParallelSolver.h"
+
+//#include "SpuFakeDma.h"
+#include "SpuSync.h"
+
+#include "LinearMath/btVector3.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "LinearMath/btQuickprof.h"
+
+#include "SpuSolverTask/SpuParallellSolverTask.h"
+
+#include <stdio.h>
+
+enum
+{
+	PARALLEL_SOLVER_BODIES_PER_TASK = 64,
+	PARALLEL_SOLVER_CELLS_PER_TASK = SPU_HASH_NUMCELLS >> 3
+};
+
+
+//-- Hash handling
+static void recordDependency(SpuSolverHash* hash, unsigned int i, unsigned int j)
+{
+	hash->m_dependencyMatrix[i][j >> 5] |= (1 << (j & 31));
+	hash->m_dependencyMatrix[j][i >> 5] |= (1 << (i & 31));
+}
+
+
+// Clear the given hash
+static void clearHash (SpuSolverHash* hash)
+{
+	size_t hashSize = sizeof(SpuSolverHash);
+	memset(hash, 0, hashSize);
+	int i;
+
+	// Setup basic dependency
+	for ( i = 0; i < SPU_HASH_NUMCELLS; ++i)
+	{
+		hash->m_dependencyMatrix[i][i >> 5] |= (1 << (i & 31));
+	}
+
+	// Set some ones to "unused cells"
+	for ( i = SPU_HASH_WORDWIDTH-SPU_HASH_NUMUNUSEDBITS; i < SPU_HASH_WORDWIDTH; ++i)
+	{
+		hash->m_currentMask[0][SPU_HASH_NUMCELLDWORDS-1] |= (1 << i);
+	}
+}
+/*
+static bool getDependency(SpuSolverHash* hash, unsigned int i, unsigned int j)
+{
+	return (hash->m_dependencyMatrix[i][j >> 5] & (1 << (j & 31))) != 0;
+}
+*/
+
+
+static unsigned int getObjectIndex (btCollisionObject* object)
+{
+	btVector3 center = object->getWorldTransform().getOrigin();
+	int cx = (int)floorf(center.x() / SPU_HASH_PHYSSIZE);
+	int cy = (int)floorf(center.y() / SPU_HASH_PHYSSIZE);
+	int cz = (int)floorf(center.z() / SPU_HASH_PHYSSIZE);
+
+	return spuGetHashCellIndex(cx, cy, cz);
+};
+
+
+
+
+
+btParallelSequentialImpulseSolver::btParallelSequentialImpulseSolver (btThreadSupportInterface* threadIf, int maxOutstandingTasks)
+: m_numberOfContacts(0), m_taskScheduler (threadIf, maxOutstandingTasks)
+{
+	m_solverHash = new SpuSolverHash;
+	clearHash(m_solverHash);
+}
+
+btParallelSequentialImpulseSolver::~btParallelSequentialImpulseSolver ()
+{
+	delete m_solverHash;
+}
+
+
+void btParallelSequentialImpulseSolver::prepareSolve(int numBodies, int numManifolds)
+{
+	m_sortedManifolds.reserve(numManifolds);
+	m_allObjects.reserve(numBodies);
+}
+
+btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher)
+{
+	BT_PROFILE("parallel_solveGroup");
+
+	if (!numManifolds && !numConstraints)
+		return 0;
+	int i;
+
+///refresh contact points is not needed anymore, it has been moved into the processCollision detection part.
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
+	for ( i = 0; i < numManifolds; ++i)
+	{
+		btPersistentManifold* currManifold = manifold[i];
+		btRigidBody* rb0 = (btRigidBody*)currManifold->getBody0();
+		btRigidBody* rb1 = (btRigidBody*)currManifold->getBody1();
+
+		currManifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
+	}
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS
+
+	// Record and mark the manifolds to the cells
+	for ( i = 0; i < numManifolds; ++i)
+	{
+		// Compute a hash cell for this manifold
+		btPersistentManifold* currManifold = manifold[i];
+
+		btCollisionObject *ownerObject, *otherObject;
+
+		btRigidBody* rb0 = (btRigidBody*)currManifold->getBody0();
+		btRigidBody* rb1 = (btRigidBody*)currManifold->getBody1();
+
+		if (rb0->getIslandTag() >= 0)
+		{
+			ownerObject = rb0;
+			otherObject = rb1;
+		}
+		else
+		{
+			ownerObject = rb1;
+			otherObject = rb0;
+		}
+
+		// Save the cell
+		unsigned int ownerCellIdx = getObjectIndex(ownerObject);
+		ManifoldCellHolder holder = {ownerCellIdx, currManifold};
+		m_sortedManifolds.push_back(holder);
+		m_solverHash->m_Hash[ownerCellIdx].m_numManifolds++;
+
+		// Record dependency
+		if (rb0->getIslandTag() >= 0 && rb1->getIslandTag() >= 0)
+		{
+			unsigned int otherCellIdx = getObjectIndex(otherObject);
+			recordDependency(m_solverHash, ownerCellIdx, otherCellIdx);
+		}
+		
+		// Save statistics
+		int numContacts = currManifold->getNumContacts();
+		m_solverHash->m_Hash[ownerCellIdx].m_numContacts += numContacts;
+		m_numberOfContacts += numContacts;
+	}
+
+	// Record and mark constraints to the cells
+	for ( i = 0; i < numConstraints; ++i)
+	{
+		// Compute a hash cell for this manifold
+		btTypedConstraint* currConstraint = constraints[i];
+
+		if (!constraintTypeSupported(currConstraint->getConstraintType()))
+			continue;
+
+		btCollisionObject *ownerObject, *otherObject;
+
+		btRigidBody* rb0 = &currConstraint->getRigidBodyA();
+		btRigidBody* rb1 = &currConstraint->getRigidBodyB();
+
+		if (rb0->getIslandTag() >= 0)
+		{
+			ownerObject = rb0;
+			otherObject = rb1;
+		}
+		else
+		{
+			ownerObject = rb1;
+			otherObject = rb0;
+		}
+
+		// Save the cell
+		unsigned int ownerCellIdx = getObjectIndex(ownerObject);
+		ConstraintCellHolder holder = {ownerCellIdx, currConstraint->getConstraintType(), currConstraint};
+		m_sortedConstraints.push_back(holder);
+		m_solverHash->m_Hash[ownerCellIdx].m_numConstraints++;
+
+		// Record dependency
+		if (rb0 && rb1 && rb0->getIslandTag() >= 0 && rb1->getIslandTag() >= 0)
+		{
+			unsigned int otherCellIdx = getObjectIndex(otherObject);
+			recordDependency(m_solverHash, ownerCellIdx, otherCellIdx);
+		}
+	}
+
+	// Save all RBs
+	for ( i = 0; i < numBodies; ++i)
+	{
+		btCollisionObject* obj = bodies[i];
+		//unsigned int cellIdx = getObjectIndex(obj);
+
+		btRigidBody* rb = btRigidBody::upcast(obj);
+		m_allObjects.push_back(rb);
+	}
+
+	return 0;
+}
+
+template<typename T>
+class CellHolderPredicate
+{
+public:
+	SIMD_FORCE_INLINE bool operator() ( const T& lhs, const T& rhs )
+	{
+		return lhs.m_hashCellIndex < rhs.m_hashCellIndex;
+	}
+};
+
+
+/*static void printDependencyMatrix(SpuSolverHash* hash)
+{
+	for (int r = 0; r < SPU_HASH_NUMCELLS; ++r)
+	{
+		for (int c = 0; c < SPU_HASH_NUMCELLS; ++c)
+		{
+			if (getDependency(hash, r, c))
+			{
+				printf("1");
+			}
+			else
+			{
+				printf("0");
+			}
+		}
+
+		printf("\n");
+	}
+	printf("\n");
+	fflush(stdout);
+}
+*/
+
+// Solver caches
+btAlignedObjectArray<SpuSolverBody> solverBodyPool_persist;
+btAlignedObjectArray<uint32_t> solverBodyOffsetList_persist;
+btAlignedObjectArray<SpuSolverInternalConstraint> solverInternalConstraintPool_persist;
+btAlignedObjectArray<SpuSolverConstraint> solverConstraintPool_persist;
+
+
+void btParallelSequentialImpulseSolver::allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc)
+{
+	BT_PROFILE("parallel_allSolved");
+
+	if (!m_numberOfContacts && !m_sortedConstraints.size())
+	{
+		m_sortedManifolds.clear();
+		m_sortedConstraints.clear();
+		m_allObjects.clear();
+		clearHash(m_solverHash);
+		return;
+	}
+
+
+	//printDependencyMatrix(m_solverHash);
+
+	// Sort the manifolds list
+	int numManifolds = m_sortedManifolds.size();
+	m_sortedManifolds.quickSort(CellHolderPredicate<ManifoldCellHolder>());
+
+	// Sort the constraint list
+	int numConstraints = m_sortedConstraints.size();
+	m_sortedConstraints.quickSort(CellHolderPredicate<ConstraintCellHolder>());
+
+
+	// Sort the body list
+	int numBodies = m_allObjects.size();
+	
+	// Reassign the hash offset
+	uint32_t emptyCellMask[SPU_HASH_NUMCELLDWORDS] = {0};
+	int numBodyOffsets = 0;
+	{
+		int manifoldRunner = 0;
+		int bodyOffsetRunner = 0;
+		int internalConstraintRunner = 0;
+		int constraintRunner = 0;
+		
+		for (int i = 0; i < SPU_HASH_NUMCELLS; ++i)
+		{
+			bool empty = true;
+
+			SpuSolverHashCell& hashCell = m_solverHash->m_Hash[i];
+			hashCell.m_solverBodyOffsetListOffset = bodyOffsetRunner;
+
+			if (hashCell.m_numManifolds)
+			{
+				hashCell.m_manifoldListOffset = manifoldRunner;
+				manifoldRunner += hashCell.m_numManifolds;
+				
+				bodyOffsetRunner += hashCell.m_numManifolds*2;
+			}			
+			if (hashCell.m_numContacts)
+			{
+				hashCell.m_internalConstraintListOffset = internalConstraintRunner*3;
+				internalConstraintRunner += hashCell.m_numContacts;
+				empty = false;
+			}
+
+			if (hashCell.m_numConstraints)
+			{
+				hashCell.m_constraintListOffset = constraintRunner;
+				constraintRunner += hashCell.m_numConstraints;
+
+				bodyOffsetRunner += hashCell.m_numConstraints*2;
+
+				empty = false;
+			}
+			
+
+			emptyCellMask[i >> 5] |= (empty ? (1 << (i&31)) : 0);
+			// Align the bodyOffsetRunner to a whole number of 4 for right alignment in the list
+			bodyOffsetRunner = (bodyOffsetRunner+3)&~0x3;
+		}
+
+		numBodyOffsets = bodyOffsetRunner;
+	}
+
+	// Setup rigid bodies
+	// Allocate temporary data
+	solverBodyPool_persist.resize(numBodies + numManifolds + numConstraints);
+	SpuSolverBody* solverBodyPool = &solverBodyPool_persist[0];
+
+	solverBodyOffsetList_persist.resize(numBodyOffsets);
+	uint32_t* solverBodyOffsetList = &solverBodyOffsetList_persist[0];
+
+	solverInternalConstraintPool_persist.resize(m_numberOfContacts*3);
+	SpuSolverInternalConstraint* solverInternalConstraintPool = &solverInternalConstraintPool_persist[0];
+	
+	solverConstraintPool_persist.resize(numConstraints);
+	SpuSolverConstraint* solverConstraintPool = &solverConstraintPool_persist[0];
+
+	// Setup all the moving rigid bodies
+	{
+		BT_PROFILE("setup moving rigidbodies");
+
+		int bodiesPerTask = PARALLEL_SOLVER_BODIES_PER_TASK;
+		int bodiesToSchedule = numBodies;
+		int startBody = 0;
+
+		while (bodiesToSchedule > 0)
+		{
+			// Schedule a bunch of hash cells
+			int numBodiesInTask = bodiesToSchedule > bodiesPerTask ? bodiesPerTask : bodiesToSchedule;
+
+			SpuSolverTaskDesc* desc = m_taskScheduler.getTask();
+
+			desc->m_solverCommand = CMD_SOLVER_SETUP_BODIES;
+			desc->m_solverData.m_solverHash = m_solverHash;
+			desc->m_solverData.m_solverBodyList = solverBodyPool;
+
+			desc->m_commandData.m_bodySetup.m_startBody = startBody;
+			desc->m_commandData.m_bodySetup.m_numBodies = numBodiesInTask;
+			desc->m_commandData.m_bodySetup.m_rbList = &m_allObjects[0];
+
+			m_taskScheduler.issueTask();
+			bodiesToSchedule -= numBodiesInTask;
+			startBody += numBodiesInTask;
+		}
+		
+		m_taskScheduler.flushTasks();
+	}
+
+	// Manifold setup
+	{
+		int cellsPerTask = PARALLEL_SOLVER_CELLS_PER_TASK;
+		int cellsToSchedule = SPU_HASH_NUMCELLS;
+		int startCell = 0;
+
+		while (cellsToSchedule > 0)
+		{
+			int numCellsInTask = cellsToSchedule > cellsPerTask ? cellsPerTask : cellsToSchedule;
+			
+			SpuSolverTaskDesc* desc = m_taskScheduler.getTask();
+
+			desc->m_solverCommand = CMD_SOLVER_MANIFOLD_SETUP;
+			desc->m_solverData.m_solverHash = m_solverHash;
+			desc->m_solverData.m_solverBodyList = solverBodyPool;
+			desc->m_solverData.m_solverBodyOffsetList = solverBodyOffsetList;
+			desc->m_solverData.m_solverInternalConstraintList = solverInternalConstraintPool;
+			desc->m_solverData.m_solverConstraintList = solverConstraintPool;
+
+			desc->m_commandData.m_manifoldSetup.m_startCell = startCell;
+			desc->m_commandData.m_manifoldSetup.m_numCells = numCellsInTask;
+			desc->m_commandData.m_manifoldSetup.m_numBodies = numBodies;
+			desc->m_commandData.m_manifoldSetup.m_numManifolds = numManifolds;
+			desc->m_commandData.m_manifoldSetup.m_manifoldHolders = &m_sortedManifolds[0];
+			desc->m_commandData.m_manifoldSetup.m_constraintHolders = &m_sortedConstraints[0];
+			desc->m_commandData.m_manifoldSetup.m_solverInfo = info;
+
+			m_taskScheduler.issueTask();
+			cellsToSchedule -= numCellsInTask;
+			startCell += numCellsInTask;
+		}
+		m_taskScheduler.flushTasks();
+	}
+
+	{
+		BT_PROFILE("parallel_solve_iterations");
+
+		btSpinlock::SpinVariable* spinVar = (btSpinlock::SpinVariable*)btAlignedAlloc(sizeof(btSpinlock::SpinVariable), 128);
+		for (int iter = 0; iter < info.m_numIterations; ++iter)
+		{
+			btSpinlock lock (spinVar);
+			lock.Init();
+
+			// Clear the "processed cells" part of the hash
+			memcpy(m_solverHash->m_currentMask[0], emptyCellMask, sizeof(uint32_t)*SPU_HASH_NUMCELLDWORDS);
+
+			for (int task = 0; task < m_taskScheduler.getMaxOutstandingTasks(); ++task)
+			{
+				SpuSolverTaskDesc* desc = m_taskScheduler.getTask();
+				desc->m_solverCommand = CMD_SOLVER_SOLVE_ITERATE;
+
+				desc->m_solverData.m_solverHash = m_solverHash;
+				desc->m_solverData.m_solverBodyList = solverBodyPool;
+				desc->m_solverData.m_solverBodyOffsetList = solverBodyOffsetList;
+				desc->m_solverData.m_solverInternalConstraintList = solverInternalConstraintPool;
+				desc->m_solverData.m_solverConstraintList = solverConstraintPool;
+
+				desc->m_commandData.m_iterate.m_spinLockVar = spinVar;
+
+				m_taskScheduler.issueTask();
+			} 
+			m_taskScheduler.flushTasks();		
+		}
+		btAlignedFree((void*)spinVar);
+	}
+	
+	// Write back velocity
+	{
+		int bodiesPerTask = PARALLEL_SOLVER_BODIES_PER_TASK;
+		int bodiesToSchedule = numBodies;
+		int startBody = 0;
+
+		while (bodiesToSchedule > 0)
+		{
+			// Schedule a bunch of hash cells
+			int numBodiesInTask = bodiesToSchedule > bodiesPerTask ? bodiesPerTask : bodiesToSchedule;
+
+			SpuSolverTaskDesc* desc = m_taskScheduler.getTask();
+
+			desc->m_solverCommand = CMD_SOLVER_COPYBACK_BODIES;
+			desc->m_solverData.m_solverHash = m_solverHash;
+			desc->m_solverData.m_solverBodyList = solverBodyPool;
+
+			desc->m_commandData.m_bodyCopyback.m_startBody = startBody;
+			desc->m_commandData.m_bodyCopyback.m_numBodies = numBodiesInTask;
+			desc->m_commandData.m_bodyCopyback.m_rbList = &m_allObjects[0];
+
+			m_taskScheduler.issueTask();
+			bodiesToSchedule -= numBodiesInTask;
+			startBody += numBodiesInTask;
+		}
+
+		m_taskScheduler.flushTasks();
+	}
+
+
+	// Clean up
+	m_sortedManifolds.resize(0);
+	m_sortedConstraints.resize(0);
+	m_allObjects.resize(0);
+	clearHash(m_solverHash);
+
+
+	m_numberOfContacts = 0;
+}
+
+void btParallelSequentialImpulseSolver::reset()
+{
+	m_sortedManifolds.clear();
+	m_allObjects.clear();
+	m_numberOfContacts = 0;
+	clearHash(m_solverHash);
+
+	solverBodyPool_persist.clear();
+	solverBodyOffsetList_persist.clear();
+	solverConstraintPool_persist.clear();
+	solverInternalConstraintPool_persist.clear();
+}
+
+
+SolverTaskScheduler::SolverTaskScheduler(btThreadSupportInterface* threadIf, int maxOutstandingTasks)
+: m_threadInterface (threadIf), m_maxNumOutstandingTasks (maxOutstandingTasks > SPU_MAX_SPUS ? SPU_MAX_SPUS : maxOutstandingTasks), 
+m_currentTask (0), m_numBusyTasks (0)
+{
+	m_taskDescriptors.resize(m_maxNumOutstandingTasks);
+	m_taskBusy.resize(m_maxNumOutstandingTasks);
+
+	m_threadInterface->startSPU();
+}
+
+
+SolverTaskScheduler::~SolverTaskScheduler()
+{
+	m_threadInterface->stopSPU();
+}
+
+SpuSolverTaskDesc* SolverTaskScheduler::getTask()
+{
+	int taskIdx = -1;
+
+	if (m_taskBusy[m_currentTask])
+	{
+		//try to find a new one
+		for (int i = 0; i < m_maxNumOutstandingTasks; ++i)
+		{
+			if (!m_taskBusy[i])
+			{
+				taskIdx = i;
+				break;
+			}
+		}
+
+		if (taskIdx < 0)
+		{
+			// Have to wait
+			unsigned int taskId;
+			unsigned int outputSize;
+
+			for (int i=0;i<m_maxNumOutstandingTasks;i++)
+			  {
+				  if (m_taskBusy[i])
+				  {
+					  taskId = i;
+					  break;
+				  }
+			  }
+
+			m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+			m_taskBusy[taskId] = false;
+			m_numBusyTasks--;
+
+			taskIdx = taskId;
+		}
+
+		m_currentTask = taskIdx;
+	}
+
+
+	SpuSolverTaskDesc* result = &m_taskDescriptors[m_currentTask];
+	memset(result, 0, sizeof(SpuSolverTaskDesc));
+	result->m_taskId = m_currentTask;
+
+	return result;
+}
+
+void SolverTaskScheduler::issueTask()
+{
+	m_taskBusy[m_currentTask] = true;
+	m_numBusyTasks++;
+
+	SpuSolverTaskDesc& desc = m_taskDescriptors[m_currentTask];
+	
+	m_threadInterface->sendRequest(1, (uint32_t)&desc, m_currentTask);
+}
+
+void SolverTaskScheduler::flushTasks()
+{
+	while (m_numBusyTasks > 0)
+	{
+		unsigned int taskId;
+		unsigned int outputSize;
+		for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+
+		m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+		m_taskBusy[taskId] = false;
+		m_numBusyTasks--;
+	}
+}

src/BulletMultiThreaded/SpuParallelSolver.h

@@ -0,0 +1,75 @@
+/*
+Bullet Continuous Collision Detection and Physics Library - Parallel solver
+Copyright (c) 2007 Starbreeze Studios
+
+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.
+
+Written by: Marten Svanfeldt
+*/
+
+#ifndef SPU_PARALLELSOLVER_H
+#define SPU_PARALLELSOLVER_H
+
+#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
+#include "btThreadSupportInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class SolverTaskScheduler
+{
+protected:
+	class	btThreadSupportInterface*	m_threadInterface;
+	int						m_maxNumOutstandingTasks;
+
+	unsigned int						m_currentTask;
+	unsigned int						m_numBusyTasks;
+
+	btAlignedObjectArray<struct SpuSolverTaskDesc>	m_taskDescriptors;
+	btAlignedObjectArray<bool>						m_taskBusy;
+
+public:
+	SolverTaskScheduler (btThreadSupportInterface* threadIf, int maxOutstandingTasks);
+	~SolverTaskScheduler ();
+
+	struct SpuSolverTaskDesc* getTask ();
+
+	void issueTask();
+	void flushTasks();
+
+	int getMaxOutstandingTasks()
+	{
+		return m_maxNumOutstandingTasks;
+	}
+};
+
+class btParallelSequentialImpulseSolver : public btConstraintSolver
+{
+protected:
+
+	struct SpuSolverHash*						m_solverHash;
+	btAlignedObjectArray<struct ManifoldCellHolder>				m_sortedManifolds;
+	btAlignedObjectArray<struct ConstraintCellHolder>				m_sortedConstraints;
+	btAlignedObjectArray<class btRigidBody*>	m_allObjects;
+
+	int											m_numberOfContacts;	
+
+	SolverTaskScheduler							m_taskScheduler;
+
+public:
+	btParallelSequentialImpulseSolver (btThreadSupportInterface* threadIf, int maxOutstandingTasks);
+	virtual ~btParallelSequentialImpulseSolver();
+
+	virtual void prepareSolve (int numBodies, int numManifolds);
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
+	virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
+	virtual void reset ();
+};
+
+#endif

src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp

@@ -0,0 +1,786 @@
+
+
+#include "../PlatformDefinitions.h"
+#include "SpuRaycastTask.h"
+#include "../SpuCollisionObjectWrapper.h"
+#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h"
+#include "SpuSubSimplexConvexCast.h"
+#include "LinearMath/btAabbUtil2.h"
+
+
+/* Future optimization strategies: 
+1. BBOX prune before loading shape data
+2. Could reduce number of dmas for ray output data to a single read and write.
+   By sharing the temporary work unit output structures across objects.
+3. The reason SpuRaycastNodeCallback1 is slower is because the triangle data isn't
+   being cached across calls. Fix that by doing the final ray pruning inside the callback.
+*/
+
+/* Future work:
+1. support first hit, closest hit, etc rather than just closest hit.
+2. support compound objects
+*/
+
+#define CALLBACK_ALL
+
+struct RaycastTask_LocalStoreMemory
+{
+	ATTRIBUTE_ALIGNED16(char gColObj [sizeof(btCollisionObject)+16]);
+	btCollisionObject* getColObj()
+	{
+		return (btCollisionObject*) gColObj;
+	}
+
+	ATTRIBUTE_ALIGNED16(SpuCollisionObjectWrapper gCollisionObjectWrapper);
+	SpuCollisionObjectWrapper* getCollisionObjectWrapper ()
+	{
+		return &gCollisionObjectWrapper;
+	}
+
+	CollisionShape_LocalStoreMemory gCollisionShape;
+	ATTRIBUTE_ALIGNED16(int	spuIndices[16]);
+
+	bvhMeshShape_LocalStoreMemory bvhShapeData;
+	SpuConvexPolyhedronVertexData convexVertexData;
+	CompoundShape_LocalStoreMemory compoundShapeData;
+};
+
+#ifdef WIN32
+void* createRaycastLocalStoreMemory()
+{
+	return new RaycastTask_LocalStoreMemory;
+};
+#elif defined(__CELLOS_LV2__)
+ATTRIBUTE_ALIGNED16(RaycastTask_LocalStoreMemory gLocalStoreMemory);
+void* createRaycastLocalStoreMemory()
+{
+	return &gLocalStoreMemory;
+}
+#endif
+
+void GatherCollisionObjectAndShapeData (RaycastGatheredObjectData* gatheredObjectData, RaycastTask_LocalStoreMemory* lsMemPtr, ppu_address_t objectWrapper)
+{
+	register int dmaSize;
+	register ppu_address_t	dmaPpuAddress2;
+	/* DMA Collision object wrapper into local store */
+	dmaSize = sizeof(SpuCollisionObjectWrapper);
+	dmaPpuAddress2 = objectWrapper;
+	cellDmaGet(&lsMemPtr->gCollisionObjectWrapper, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+	/* DMA Collision object into local store */
+	dmaSize = sizeof(btCollisionObject);
+	dmaPpuAddress2 = lsMemPtr->getCollisionObjectWrapper()->getCollisionObjectPtr();
+	cellDmaGet(&lsMemPtr->gColObj, dmaPpuAddress2  , dmaSize, DMA_TAG(2), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(2));
+	
+	/* Gather information about collision object and shape */
+	gatheredObjectData->m_worldTransform = lsMemPtr->getColObj()->getWorldTransform();
+	gatheredObjectData->m_collisionMargin = lsMemPtr->getCollisionObjectWrapper()->getCollisionMargin ();
+	gatheredObjectData->m_shapeType = lsMemPtr->getCollisionObjectWrapper()->getShapeType ();
+	gatheredObjectData->m_collisionShape = (ppu_address_t)lsMemPtr->getColObj()->getCollisionShape();
+	gatheredObjectData->m_spuCollisionShape = (void*)&lsMemPtr->gCollisionShape.collisionShape;
+
+	/* DMA shape data */
+	dmaCollisionShape (gatheredObjectData->m_spuCollisionShape, gatheredObjectData->m_collisionShape, 1, gatheredObjectData->m_shapeType);
+	cellDmaWaitTagStatusAll(DMA_MASK(1));
+	if (btBroadphaseProxy::isConvex (gatheredObjectData->m_shapeType))
+	{
+		btConvexInternalShape* spuConvexShape = (btConvexInternalShape*)gatheredObjectData->m_spuCollisionShape;
+		gatheredObjectData->m_primitiveDimensions = spuConvexShape->getImplicitShapeDimensions ();
+	} else {
+		gatheredObjectData->m_primitiveDimensions = btVector3(1.0, 1.0, 1.0);
+	}
+
+}
+
+void dmaLoadRayOutput (ppu_address_t rayOutputAddr, SpuRaycastTaskWorkUnitOut* rayOutput, uint32_t dmaTag)
+{
+	cellDmaGet(rayOutput, rayOutputAddr, sizeof(*rayOutput), DMA_TAG(dmaTag), 0, 0);
+}
+
+void dmaStoreRayOutput (ppu_address_t rayOutputAddr, const SpuRaycastTaskWorkUnitOut* rayOutput, uint32_t dmaTag)
+{
+	cellDmaLargePut (rayOutput, rayOutputAddr, sizeof(*rayOutput), DMA_TAG(dmaTag), 0, 0);
+}
+
+#if 0
+SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_t ea, size_t size)
+{
+#if USE_SOFTWARE_CACHE
+	// Check for alignment requirements. We need to make sure the entire request fits within one cache line,
+	// so the first and last bytes should fall on the same cache line
+	btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK));
+
+	void* ls = spe_cache_read(ea);
+	memcpy(buffer, ls, size);
+#else
+	stallingUnalignedDmaSmallGet(buffer,ea,size);
+#endif
+}
+#endif
+
+void small_cache_read_triple(	void* ls0, ppu_address_t ea0,
+												void* ls1, ppu_address_t ea1,
+												void* ls2, ppu_address_t ea2,
+												size_t size)
+{
+		btAssert(size<16);
+		ATTRIBUTE_ALIGNED16(char	tmpBuffer0[32]);
+		ATTRIBUTE_ALIGNED16(char	tmpBuffer1[32]);
+		ATTRIBUTE_ALIGNED16(char	tmpBuffer2[32]);
+
+		uint32_t i;
+		
+
+		///make sure last 4 bits are the same, for cellDmaSmallGet
+		char* localStore0 = (char*)ls0;
+		uint32_t last4BitsOffset = ea0 & 0x0f;
+		char* tmpTarget0 = tmpBuffer0 + last4BitsOffset;
+		tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0);
+
+
+		char* localStore1 = (char*)ls1;
+		last4BitsOffset = ea1 & 0x0f;
+		char* tmpTarget1 = tmpBuffer1 + last4BitsOffset;
+		tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0);
+		
+		char* localStore2 = (char*)ls2;
+		last4BitsOffset = ea2 & 0x0f;
+		char* tmpTarget2 = tmpBuffer2 + last4BitsOffset;
+		tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0);
+		
+		
+		cellDmaWaitTagStatusAll( DMA_MASK(1) );
+
+		//this is slowish, perhaps memcpy on SPU is smarter?
+		for (i=0; btLikely( i<size );i++)
+		{
+			localStore0[i] = tmpTarget0[i];
+			localStore1[i] = tmpTarget1[i];
+			localStore2[i] = tmpTarget2[i];
+		}
+}
+
+void performRaycastAgainstConvex (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr);
+
+class spuRaycastNodeCallback1 : public btNodeOverlapCallback
+{
+	RaycastGatheredObjectData* m_gatheredObjectData;
+	const SpuRaycastTaskWorkUnit* m_workUnits;
+	SpuRaycastTaskWorkUnitOut* m_workUnitsOut;
+	int m_workUnit;
+	RaycastTask_LocalStoreMemory* m_lsMemPtr;
+
+	ATTRIBUTE_ALIGNED16(btVector3	spuTriangleVertices[3]);
+	ATTRIBUTE_ALIGNED16(btScalar	spuUnscaledVertex[4]);
+	//ATTRIBUTE_ALIGNED16(int	spuIndices[16]);
+public:
+	spuRaycastNodeCallback1(RaycastGatheredObjectData* gatheredObjectData,const SpuRaycastTaskWorkUnit* workUnits, SpuRaycastTaskWorkUnitOut* workUnitsOut, RaycastTask_LocalStoreMemory* lsMemPtr)
+		: m_gatheredObjectData(gatheredObjectData),
+		  m_workUnits(workUnits),
+		  m_workUnitsOut(workUnitsOut),
+		  m_workUnit(0),
+		  m_lsMemPtr (lsMemPtr)
+	{
+	}
+
+	void setWorkUnit (int workUnit) { m_workUnit = workUnit; }
+	virtual void processNode(int subPart, int triangleIndex)
+	{
+		///Create a triangle on the stack, call process collision, with GJK
+		///DMA the vertices, can benefit from software caching
+
+		//		spu_printf("processNode with triangleIndex %d\n",triangleIndex);
+
+			// ugly solution to support both 16bit and 32bit indices
+		if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT)
+		{
+			short int* indexBasePtr = (short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
+			ATTRIBUTE_ALIGNED16(short int tmpIndices[3]);
+
+			small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0],
+									&tmpIndices[1],(ppu_address_t)&indexBasePtr[1],
+									&tmpIndices[2],(ppu_address_t)&indexBasePtr[2],
+									sizeof(short int));
+
+			m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]);
+			m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]);
+			m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]);
+		} else
+		{
+			int* indexBasePtr = (int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
+
+			small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0],
+								&m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1],
+								&m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2],
+								sizeof(int));
+		}
+
+		//printf("%d %d %d\n", m_lsMemPtr->spuIndices[0], m_lsMemPtr->spuIndices[1], m_lsMemPtr->spuIndices[2]);
+		//		spu_printf("SPU index0=%d ,",spuIndices[0]);
+		//		spu_printf("SPU index1=%d ,",spuIndices[1]);
+		//		spu_printf("SPU index2=%d ,",spuIndices[2]);
+		//		spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr);
+
+		const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling();
+	
+		for (int j=2;btLikely( j>=0 );j--)
+		{
+			int graphicsindex = m_lsMemPtr->spuIndices[j];
+
+						//spu_printf("SPU index=%d ,",graphicsindex);
+			btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride);
+			
+			//			spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr);
+
+
+			///handle un-aligned vertices...
+
+			//another DMA for each vertex
+			small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0],
+									&spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1],
+									&spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2],
+									sizeof(btScalar));
+			
+			//printf("%f %f %f\n", spuUnscaledVertex[0],spuUnscaledVertex[1],spuUnscaledVertex[2]);
+			spuTriangleVertices[j] = btVector3(
+				spuUnscaledVertex[0]*meshScaling.getX(),
+				spuUnscaledVertex[1]*meshScaling.getY(),
+				spuUnscaledVertex[2]*meshScaling.getZ());
+
+				//spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z());
+		}
+		
+		RaycastGatheredObjectData triangleGatheredObjectData (*m_gatheredObjectData);
+		triangleGatheredObjectData.m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
+		triangleGatheredObjectData.m_spuCollisionShape = &spuTriangleVertices[0];
+
+		//printf("%f %f %f\n", spuTriangleVertices[0][0],spuTriangleVertices[0][1],spuTriangleVertices[0][2]);
+		//printf("%f %f %f\n", spuTriangleVertices[1][0],spuTriangleVertices[1][1],spuTriangleVertices[1][2]);
+		//printf("%f %f %f\n", spuTriangleVertices[2][0],spuTriangleVertices[2][1],spuTriangleVertices[2][2]);
+		SpuRaycastTaskWorkUnitOut out;
+		out.hitFraction = 1.0;
+		performRaycastAgainstConvex (&triangleGatheredObjectData, m_workUnits[m_workUnit], &out, m_lsMemPtr);
+		/* XXX: For now only take the closest hit */
+		if (out.hitFraction < m_workUnitsOut[m_workUnit].hitFraction)
+		{
+			m_workUnitsOut[m_workUnit].hitFraction = out.hitFraction;
+			m_workUnitsOut[m_workUnit].hitNormal = out.hitNormal;
+		}
+	}
+
+};
+
+class spuRaycastNodeCallback : public btNodeOverlapCallback
+{
+	RaycastGatheredObjectData* m_gatheredObjectData;
+	const SpuRaycastTaskWorkUnit* m_workUnits;
+	SpuRaycastTaskWorkUnitOut* m_workUnitsOut;
+	int m_numWorkUnits;
+	RaycastTask_LocalStoreMemory* m_lsMemPtr;
+
+	ATTRIBUTE_ALIGNED16(btVector3	spuTriangleVertices[3]);
+	ATTRIBUTE_ALIGNED16(btScalar	spuUnscaledVertex[4]);
+	//ATTRIBUTE_ALIGNED16(int	spuIndices[16]);
+public:
+	spuRaycastNodeCallback(RaycastGatheredObjectData* gatheredObjectData,const SpuRaycastTaskWorkUnit* workUnits, SpuRaycastTaskWorkUnitOut* workUnitsOut, int numWorkUnits, RaycastTask_LocalStoreMemory* lsMemPtr)
+		: m_gatheredObjectData(gatheredObjectData),
+		  m_workUnits(workUnits),
+		  m_workUnitsOut(workUnitsOut),
+		  m_numWorkUnits(numWorkUnits),
+		  m_lsMemPtr (lsMemPtr)
+	{
+	}
+
+	virtual void processNode(int subPart, int triangleIndex)
+	{
+		///Create a triangle on the stack, call process collision, with GJK
+		///DMA the vertices, can benefit from software caching
+
+		//		spu_printf("processNode with triangleIndex %d\n",triangleIndex);
+
+			// ugly solution to support both 16bit and 32bit indices
+		if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT)
+		{
+			short int* indexBasePtr = (short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
+			ATTRIBUTE_ALIGNED16(short int tmpIndices[3]);
+
+			small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0],
+									&tmpIndices[1],(ppu_address_t)&indexBasePtr[1],
+									&tmpIndices[2],(ppu_address_t)&indexBasePtr[2],
+									sizeof(short int));
+
+			m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]);
+			m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]);
+			m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]);
+		} else
+		{
+			int* indexBasePtr = (int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);
+
+			small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0],
+								&m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1],
+								&m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2],
+								sizeof(int));
+		}
+
+		//printf("%d %d %d\n", m_lsMemPtr->spuIndices[0], m_lsMemPtr->spuIndices[1], m_lsMemPtr->spuIndices[2]);
+		//		spu_printf("SPU index0=%d ,",spuIndices[0]);
+		//		spu_printf("SPU index1=%d ,",spuIndices[1]);
+		//		spu_printf("SPU index2=%d ,",spuIndices[2]);
+		//		spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr);
+
+		const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling();
+	
+		for (int j=2;btLikely( j>=0 );j--)
+		{
+			int graphicsindex = m_lsMemPtr->spuIndices[j];
+
+						//spu_printf("SPU index=%d ,",graphicsindex);
+			btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride);
+			
+			//			spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr);
+
+
+			///handle un-aligned vertices...
+
+			//another DMA for each vertex
+			small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0],
+									&spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1],
+									&spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2],
+									sizeof(btScalar));
+			
+			//printf("%f %f %f\n", spuUnscaledVertex[0],spuUnscaledVertex[1],spuUnscaledVertex[2]);
+			spuTriangleVertices[j] = btVector3(
+				spuUnscaledVertex[0]*meshScaling.getX(),
+				spuUnscaledVertex[1]*meshScaling.getY(),
+				spuUnscaledVertex[2]*meshScaling.getZ());
+
+				//spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z());
+		}
+		
+		RaycastGatheredObjectData triangleGatheredObjectData (*m_gatheredObjectData);
+		triangleGatheredObjectData.m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
+		triangleGatheredObjectData.m_spuCollisionShape = &spuTriangleVertices[0];
+
+		//printf("%f %f %f\n", spuTriangleVertices[0][0],spuTriangleVertices[0][1],spuTriangleVertices[0][2]);
+		//printf("%f %f %f\n", spuTriangleVertices[1][0],spuTriangleVertices[1][1],spuTriangleVertices[1][2]);
+		//printf("%f %f %f\n", spuTriangleVertices[2][0],spuTriangleVertices[2][1],spuTriangleVertices[2][2]);
+		for (int i = 0; i < m_numWorkUnits; i++)
+		{
+			SpuRaycastTaskWorkUnitOut out;
+			out.hitFraction = 1.0;
+			performRaycastAgainstConvex (&triangleGatheredObjectData, m_workUnits[i], &out, m_lsMemPtr);
+			/* XXX: For now only take the closest hit */
+			if (out.hitFraction < m_workUnitsOut[i].hitFraction)
+			{
+				m_workUnitsOut[i].hitFraction = out.hitFraction;
+				m_workUnitsOut[i].hitNormal = out.hitNormal;
+			}
+		}
+	}
+
+};
+
+
+void	spuWalkStacklessQuantizedTreeAgainstRays(RaycastTask_LocalStoreMemory* lsMemPtr, 
+						 btNodeOverlapCallback* nodeCallback,
+						 const btVector3* rayFrom,
+						 const btVector3* rayTo,
+						 int numWorkUnits,
+						 unsigned short int* quantizedQueryAabbMin,
+						 unsigned short int* quantizedQueryAabbMax,
+						 const btQuantizedBvhNode* rootNode,
+						 int startNodeIndex,int endNodeIndex)
+{
+	int curIndex = startNodeIndex;
+	int walkIterations = 0;
+	int subTreeSize = endNodeIndex - startNodeIndex;
+
+	int escapeIndex;
+
+	unsigned int boxBoxOverlap, rayBoxOverlap, anyRayBoxOverlap;
+	unsigned int isLeafNode;
+
+#define RAYAABB2
+#ifdef RAYAABB2
+	unsigned int sign[SPU_RAYCAST_WORK_UNITS_PER_TASK][3];
+	btVector3 rayInvDirection[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+	btScalar lambda_max[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+	for (int i = 0; i < numWorkUnits; i++)
+	{
+		btVector3 rayDirection = (rayTo[i]-rayFrom[i]);
+		rayDirection.normalize ();
+		lambda_max[i] = rayDirection.dot(rayTo[i]-rayFrom[i]);
+		rayInvDirection[i][0] = btScalar(1.0) / rayDirection[0];
+		rayInvDirection[i][1] = btScalar(1.0) / rayDirection[1];
+		rayInvDirection[i][2] = btScalar(1.0) / rayDirection[2];
+		sign[i][0] = rayDirection[0] < 0.0;
+		sign[i][1] = rayDirection[1] < 0.0;
+		sign[i][2] = rayDirection[2] < 0.0;
+	}
+#endif
+
+	while (curIndex < endNodeIndex)
+	{
+		//catch bugs in tree data
+		assert (walkIterations < subTreeSize);
+
+		walkIterations++;
+
+		isLeafNode = rootNode->isLeafNode();
+
+		anyRayBoxOverlap = 0;
+
+		for (int i = 0; i < numWorkUnits; i++)
+		{
+			unsigned short int* quamin = (quantizedQueryAabbMin + 3 * i);
+			unsigned short int* quamax = (quantizedQueryAabbMax + 3 * i);
+			boxBoxOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quamin,quamax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+			if (!boxBoxOverlap)
+				continue;
+
+			rayBoxOverlap = 0;
+			btScalar param = 1.0;
+			btVector3 normal;
+			btVector3 bounds[2];
+			bounds[0] = lsMemPtr->bvhShapeData.getOptimizedBvh()->unQuantize(rootNode->m_quantizedAabbMin);
+			bounds[1] = lsMemPtr->bvhShapeData.getOptimizedBvh()->unQuantize(rootNode->m_quantizedAabbMax);
+#ifdef RAYAABB2
+			rayBoxOverlap = btRayAabb2 (rayFrom[i], rayInvDirection[i], sign[i], bounds, param, 0.0, lambda_max[i]);
+#else
+			rayBoxOverlap = btRayAabb(rayFrom[i], rayTo[i], bounds[0], bounds[1], param, normal);
+#endif
+
+#ifndef CALLBACK_ALL
+			anyRayBoxOverlap = rayBoxOverlap || anyRayBoxOverlap;
+			/* If we have any ray vs. box overlap and this isn't a leaf node
+			   we know that we need to dig deeper
+			*/
+			if (!isLeafNode && anyRayBoxOverlap)
+				break;
+
+			if (isLeafNode && rayBoxOverlap)
+			{
+				spuRaycastNodeCallback1* callback = (spuRaycastNodeCallback1*)nodeCallback;
+				callback->setWorkUnit (i);
+				nodeCallback->processNode (0, rootNode->getTriangleIndex());
+			}
+#else
+			/* If we have any ray vs. box overlap and this isn't a leaf node
+			   we know that we need to dig deeper
+			*/
+			if (rayBoxOverlap)
+			{
+				anyRayBoxOverlap = 1;
+				break;
+			}
+#endif
+		}
+
+#ifdef CALLBACK_ALL
+		if (isLeafNode && anyRayBoxOverlap)
+		{
+			nodeCallback->processNode (0, rootNode->getTriangleIndex());
+		}
+#endif
+
+		if (anyRayBoxOverlap || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->getEscapeIndex();
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+
+}
+
+
+void performRaycastAgainstConcave (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit* workUnits, SpuRaycastTaskWorkUnitOut* workUnitsOut, int numWorkUnits, RaycastTask_LocalStoreMemory* lsMemPtr)
+{
+	//order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite
+	register int dmaSize;
+	register ppu_address_t	dmaPpuAddress2;
+
+	
+	btBvhTriangleMeshShape*	trimeshShape = (btBvhTriangleMeshShape*)gatheredObjectData->m_spuCollisionShape;
+
+	//need the mesh interface, for access to triangle vertices
+	dmaBvhShapeData (&(lsMemPtr->bvhShapeData), trimeshShape);
+
+	unsigned short int quantizedQueryAabbMin[SPU_RAYCAST_WORK_UNITS_PER_TASK][3];
+	unsigned short int quantizedQueryAabbMax[SPU_RAYCAST_WORK_UNITS_PER_TASK][3];
+	btVector3 rayFromInTriangleSpace[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+	btVector3 rayToInTriangleSpace[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+
+	/* Calculate the AABB for the ray in the triangle mesh shape */
+	btTransform rayInTriangleSpace;
+	rayInTriangleSpace = gatheredObjectData->m_worldTransform.inverse();
+
+	for (int i = 0; i < numWorkUnits; i++)
+	{
+		btVector3 aabbMin;
+		btVector3 aabbMax;
+
+		rayFromInTriangleSpace[i] = rayInTriangleSpace(workUnits[i].rayFrom);
+		rayToInTriangleSpace[i] = rayInTriangleSpace(workUnits[i].rayTo);
+
+		aabbMin = rayFromInTriangleSpace[i];
+		aabbMin.setMin (rayToInTriangleSpace[i]);
+		aabbMax = rayFromInTriangleSpace[i];
+		aabbMax.setMax (rayToInTriangleSpace[i]);
+
+		lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin[i],aabbMin,0);
+		lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax[i],aabbMax,1);
+	}
+
+	QuantizedNodeArray&	nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray();
+	//spu_printf("SPU: numNodes = %d\n",nodeArray.size());
+
+	BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray();	
+
+#ifdef CALLBACK_ALL
+	spuRaycastNodeCallback nodeCallback (gatheredObjectData, workUnits, workUnitsOut, numWorkUnits, lsMemPtr);
+#else
+	spuRaycastNodeCallback1 nodeCallback (gatheredObjectData, workUnits, workUnitsOut, lsMemPtr);
+#endif
+	
+	IndexedMeshArray&	indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray();
+
+	//spu_printf("SPU:indexArray.size() = %d\n",indexArray.size());
+	//	spu_printf("SPU: numSubTrees = %d\n",subTrees.size());
+	//not likely to happen
+	if (subTrees.size() && indexArray.size() == 1)
+	{
+		///DMA in the index info
+		dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+		
+		//display the headers
+		int numBatch = subTrees.size();
+		for (int i=0;i<numBatch;)
+		{
+// BEN: TODO - can reorder DMA transfers for less stall
+			int remaining = subTrees.size() - i;
+			int nextBatch = remaining < MAX_SPU_SUBTREE_HEADERS ? remaining : MAX_SPU_SUBTREE_HEADERS;
+			
+			dmaBvhSubTreeHeaders (&lsMemPtr->bvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1);
+			cellDmaWaitTagStatusAll(DMA_MASK(1));
+			
+
+			//			spu_printf("nextBatch = %d\n",nextBatch);
+
+			
+			for (int j=0;j<nextBatch;j++)
+			{
+				const btBvhSubtreeInfo& subtree = lsMemPtr->bvhShapeData.gSubtreeHeaders[j];
+				
+				unsigned int overlap = 1;
+				for (int boxId = 0; boxId < numWorkUnits; boxId++)
+				{
+					overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin[boxId],quantizedQueryAabbMax[boxId],subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+					if (overlap)
+						break;
+				}
+
+				if (overlap)
+				{
+					btAssert(subtree.m_subtreeSize);
+
+					//dma the actual nodes of this subtree
+					dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2);
+
+					cellDmaWaitTagStatusAll(DMA_MASK(2));
+
+					/* Walk this subtree */
+					
+					{
+
+						spuWalkStacklessQuantizedTreeAgainstRays(lsMemPtr,
+										        &nodeCallback,
+										        &rayFromInTriangleSpace[0],
+											&rayToInTriangleSpace[0],
+											numWorkUnits,
+											&quantizedQueryAabbMin[0][0],&quantizedQueryAabbMax[0][0],
+											&lsMemPtr->bvhShapeData.gSubtreeNodes[0], 0, subtree.m_subtreeSize);
+					}
+				}
+				//				spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize);
+			}
+
+			//	unsigned short int	m_quantizedAabbMin[3];
+			//	unsigned short int	m_quantizedAabbMax[3];
+			//	int			m_rootNodeIndex;
+			//	int			m_subtreeSize;
+			i+=nextBatch;
+		}
+
+		//pre-fetch first tree, then loop and double buffer
+	}
+	
+}
+
+void performRaycastAgainstCompound (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr)
+{
+	//XXX spu_printf ("Currently no support for ray. vs compound objects. Support coming soon.\n");
+}
+
+void
+performRaycastAgainstConvex (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr)
+{
+	SpuVoronoiSimplexSolver simplexSolver;
+
+	btTransform rayFromTrans, rayToTrans;
+	rayFromTrans.setIdentity ();
+	rayFromTrans.setOrigin (workUnit.rayFrom);
+	rayToTrans.setIdentity ();
+	rayToTrans.setOrigin (workUnit.rayTo);
+
+	SpuCastResult result;
+
+	/* Load the vertex data if the shape is a convex hull */
+	/* XXX: We might be loading the shape twice */
+	ATTRIBUTE_ALIGNED16(char convexHullShape[sizeof(btConvexHullShape)]);
+	if (gatheredObjectData->m_shapeType == CONVEX_HULL_SHAPE_PROXYTYPE)
+	{
+		register int dmaSize;
+		register ppu_address_t	dmaPpuAddress2;
+		dmaSize = sizeof(btConvexHullShape);
+		dmaPpuAddress2 = gatheredObjectData->m_collisionShape;
+		cellDmaGet(&convexHullShape, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0);
+		cellDmaWaitTagStatusAll(DMA_MASK(1));
+		dmaConvexVertexData (&lsMemPtr->convexVertexData, (btConvexHullShape*)&convexHullShape);
+		cellDmaWaitTagStatusAll(DMA_MASK(2)); // dmaConvexVertexData uses dma channel 2!
+		lsMemPtr->convexVertexData.gSpuConvexShapePtr = gatheredObjectData->m_spuCollisionShape;
+		lsMemPtr->convexVertexData.gConvexPoints = &lsMemPtr->convexVertexData.g_convexPointBuffer[0];
+	}
+
+	/* performRaycast */
+	SpuSubsimplexRayCast caster (gatheredObjectData->m_spuCollisionShape, &lsMemPtr->convexVertexData, gatheredObjectData->m_shapeType, gatheredObjectData->m_collisionMargin, &simplexSolver);
+	bool r = caster.calcTimeOfImpact (rayFromTrans, rayToTrans, gatheredObjectData->m_worldTransform, gatheredObjectData->m_worldTransform,result);
+
+	if (r)
+	{
+		workUnitOut->hitFraction = result.m_fraction;
+		workUnitOut->hitNormal = result.m_normal;
+	}
+}
+
+void	processRaycastTask(void* userPtr, void* lsMemory)
+{
+	RaycastTask_LocalStoreMemory* localMemory = (RaycastTask_LocalStoreMemory*)lsMemory;
+
+	SpuRaycastTaskDesc* taskDescPtr = (SpuRaycastTaskDesc*)userPtr;
+	SpuRaycastTaskDesc& taskDesc = *taskDescPtr;
+
+	SpuCollisionObjectWrapper* cows = (SpuCollisionObjectWrapper*)taskDesc.spuCollisionObjectsWrappers;
+
+	//spu_printf("in processRaycastTask %d\n", taskDesc.numSpuCollisionObjectWrappers);
+	/* for each object */
+	RaycastGatheredObjectData gatheredObjectData;
+	for (int objectId = 0; objectId < taskDesc.numSpuCollisionObjectWrappers; objectId++)
+	{
+		//spu_printf("%d / %d\n", objectId, taskDesc.numSpuCollisionObjectWrappers);
+		
+		/* load initial collision shape */
+		GatherCollisionObjectAndShapeData (&gatheredObjectData, localMemory, (ppu_address_t)&cows[objectId]);
+
+		if (btBroadphaseProxy::isConcave (gatheredObjectData.m_shapeType))
+		{
+			SpuRaycastTaskWorkUnitOut tWorkUnitsOut[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+			for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
+			{
+				tWorkUnitsOut[rayId].hitFraction = 1.0;
+			}
+
+			performRaycastAgainstConcave (&gatheredObjectData, &taskDesc.workUnits[0], &tWorkUnitsOut[0], taskDesc.numWorkUnits, localMemory);
+
+			for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
+			{
+				const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
+				if (tWorkUnitsOut[rayId].hitFraction == 1.0)
+					continue;
+
+				ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
+				dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+				
+				
+				/* XXX Only support taking the closest hit for now */
+				if (tWorkUnitsOut[rayId].hitFraction < workUnitOut.hitFraction)
+				{
+					workUnitOut.hitFraction = tWorkUnitsOut[rayId].hitFraction;
+					workUnitOut.hitNormal = tWorkUnitsOut[rayId].hitNormal;
+				}
+
+				/* write ray cast data back */
+				dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			}
+		} else if (btBroadphaseProxy::isConvex (gatheredObjectData.m_shapeType)) {
+
+			btVector3 objectBoxMin, objectBoxMax;
+			computeAabb (objectBoxMin, objectBoxMax, (btConvexInternalShape*)gatheredObjectData.m_spuCollisionShape, gatheredObjectData.m_collisionShape, gatheredObjectData.m_shapeType, gatheredObjectData.m_worldTransform);
+			for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
+			{
+				const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
+			
+				btScalar ignored_param = 1.0;
+				btVector3 ignored_normal;
+				if (btRayAabb(workUnit.rayFrom, workUnit.rayTo, objectBoxMin, objectBoxMax, ignored_param, ignored_normal))
+				{
+					ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
+					SpuRaycastTaskWorkUnitOut tWorkUnitOut;
+					tWorkUnitOut.hitFraction = 1.0;
+
+					performRaycastAgainstConvex (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory);
+					if (tWorkUnitOut.hitFraction == 1.0)
+						continue;
+	
+					dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+					cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+					/* XXX Only support taking the closest hit for now */
+					if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction)
+					{
+						workUnitOut.hitFraction = tWorkUnitOut.hitFraction;
+						workUnitOut.hitNormal = tWorkUnitOut.hitNormal;
+						/* write ray cast data back */
+						dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+					}
+				}
+			}
+
+		} else if (btBroadphaseProxy::isCompound (gatheredObjectData.m_shapeType)) {
+			for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
+			{
+				const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
+				ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
+				SpuRaycastTaskWorkUnitOut tWorkUnitOut;
+				tWorkUnitOut.hitFraction = 1.0;
+
+				performRaycastAgainstCompound (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory);
+				if (tWorkUnitOut.hitFraction == 1.0)
+					continue;
+
+				dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+				/* XXX Only support taking the closest hit for now */
+				if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction)
+				{
+					workUnitOut.hitFraction = tWorkUnitOut.hitFraction;
+					workUnitOut.hitNormal = tWorkUnitOut.hitNormal;
+				}
+
+				/* write ray cast data back */
+				dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			}
+		}
+	}
+}

src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.h

@@ -0,0 +1,50 @@
+#ifndef __SPU_RAYCAST_TASK_H
+#define __SPU_RAYCAST_TASK_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "LinearMath/btVector3.h"
+#include "../PlatformDefinitions.h"
+
+ATTRIBUTE_ALIGNED16(struct) RaycastGatheredObjectData
+{
+	ppu_address_t m_collisionShape;
+	void* m_spuCollisionShape;
+	btVector3	m_primitiveDimensions;
+	int		m_shapeType;
+	float	m_collisionMargin;
+	btTransform	m_worldTransform;
+};
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnitOut
+{
+	btVector3 hitNormal; /* out */
+	btScalar hitFraction; /* out */
+	btCollisionWorld::LocalShapeInfo shapeInfo; /* out */
+};
+
+/* Perform a raycast on collision object */
+ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnit
+{
+	btVector3 rayFrom; /* in */
+	btVector3 rayTo; /* in */
+	SpuRaycastTaskWorkUnitOut* output; /* out */
+};
+
+#define SPU_RAYCAST_WORK_UNITS_PER_TASK 16
+
+ATTRIBUTE_ALIGNED128(struct) SpuRaycastTaskDesc
+{
+	SpuRaycastTaskWorkUnit workUnits[SPU_RAYCAST_WORK_UNITS_PER_TASK];
+	unsigned int numWorkUnits;
+	void* spuCollisionObjectsWrappers;
+	unsigned int numSpuCollisionObjectWrappers;
+	int taskId;
+};
+
+
+void	processRaycastTask (void* userPtr, void* lsMemory);
+void*	createRaycastLocalStoreMemory ();
+
+#endif

src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp

@@ -0,0 +1,152 @@
+/*
+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 "SpuSubSimplexConvexCast.h"
+
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+
+
+SpuSubsimplexRayCast::SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB,
+										    SpuVoronoiSimplexSolver* simplexSolver)
+	:m_simplexSolver(simplexSolver), m_shapeB(shapeB), m_convexDataB(convexDataB), m_shapeTypeB(shapeTypeB), m_marginB(marginB)
+{
+}
+
+///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#else
+#define MAX_ITERATIONS 32
+#endif
+
+/* Returns the support point of the minkowski sum:
+ * MSUM(Pellet, ConvexShape)
+ *
+ */
+void supportPoints (const btTransform xformRay,
+		    const btTransform xformB,
+		    const int shapeType,
+		    const void* shape,
+		    SpuConvexPolyhedronVertexData* convexVertexData,
+		    const btScalar marginB,
+		    const btVector3& seperatingAxis,
+		    btVector3& w,
+		    btVector3& supVertexRay,
+		    btVector3& supVertexB)
+{
+	btVector3 saUnit = seperatingAxis;
+	saUnit.normalize();
+	btVector3 SupportPellet = xformRay(0.0001 * -saUnit);
+	btVector3 rotatedSeperatingAxis = seperatingAxis * xformB.getBasis();
+	btVector3 SupportShape = xformB(localGetSupportingVertexWithoutMargin(shapeType, (void*)shape, rotatedSeperatingAxis, convexVertexData));
+	SupportShape += saUnit * marginB;
+	w = SupportPellet - SupportShape;
+	supVertexRay = SupportPellet;
+	supVertexB = SupportShape;
+}
+
+bool	SpuSubsimplexRayCast::calcTimeOfImpact(const btTransform& fromRay,
+											   const btTransform& toRay,
+											   const btTransform& fromB,
+											   const btTransform& toB,
+											   SpuCastResult& result)
+{
+	m_simplexSolver->reset();
+
+	btVector3 linVelRay, linVelB;
+	linVelRay = toRay.getOrigin() - fromRay.getOrigin();
+	linVelB = toB.getOrigin() - fromB.getOrigin ();
+
+	btScalar lambda = btScalar(0.);
+	
+	btTransform interpolatedTransRay = fromRay;
+	btTransform interpolatedTransB = fromB;
+
+	btVector3 r = (linVelRay-linVelB);
+	btVector3 supVertexRay;
+	btVector3 supVertexB;
+	btVector3 v;
+	supportPoints (fromRay, fromB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, r, v, supVertexRay, supVertexB);
+
+	btVector3 n;
+	n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+	bool hasResult = false;
+	btVector3 c;
+	int maxIter = MAX_ITERATIONS;
+
+	btScalar lastLambda = lambda;
+
+	btScalar dist2 = v.length2();
+
+#ifdef BT_USE_DOUBLE_PRECISION
+	btScalar epsilon = btScalar(0.0001);
+#else
+	btScalar epsilon = btScalar(0.0001);
+#endif //BT_USE_DOUBLE_PRECISION
+	btVector3 w,p;
+	btScalar VdotR;
+	
+	while ( (dist2 > epsilon) && maxIter--)
+	{
+		supportPoints (interpolatedTransRay, interpolatedTransB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, v, w, supVertexRay, supVertexB);
+
+		btScalar VdotW = v.dot(w);
+
+		if (lambda > btScalar(1.0))
+		{
+			return false;
+		}
+
+		if ( VdotW > btScalar(0.))
+		{
+			VdotR = v.dot(r);
+
+			if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
+				return false;
+			else
+			{
+				lambda = lambda - VdotW / VdotR;
+				interpolatedTransRay.getOrigin().setInterpolate3(fromRay.getOrigin(), toRay.getOrigin(), lambda);
+				interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda);
+				lastLambda = lambda;
+				n = v;
+				hasResult = true;
+			}
+		} 
+		m_simplexSolver->addVertex(w, supVertexRay, supVertexB);
+		if (m_simplexSolver->closest(v))
+		{
+			dist2 = v.length2();
+			hasResult = true;
+			//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
+			//printf("DIST2=%f\n",dist2);
+			//printf("numverts = %i\n",m_simplexSolver->numVertices());
+		} else
+		{
+			dist2 = btScalar(0.);
+		} 
+	}
+
+	result.m_fraction = lambda;
+	result.m_normal = n;
+	btVector3 hitRay, hitB;
+	m_simplexSolver->compute_points (hitRay, hitB);
+	/* TODO: We could output hit point here (hitB) */
+	return true;
+}

src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.h

@@ -0,0 +1,60 @@
+/*
+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 SPU_SUBSIMPLEX_RAY_CAST_H
+#define SPU_SUBSIMPLEX_RAY_CAST_H
+
+#include "../SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h"
+#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h"
+#include "SpuRaycastTask.h"
+
+class btConvexShape;
+
+struct SpuCastResult
+{
+	float m_fraction;
+	btVector3 m_normal;
+};
+
+/// btSubsimplexConvexCast implements Gino van den Bergens' paper
+///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
+/// GJK based Ray Cast, optimized version
+/// Objects should not start in overlap, otherwise results are not defined.
+class SpuSubsimplexRayCast
+{
+	SpuVoronoiSimplexSolver* m_simplexSolver;
+	void* m_shapeB;
+	SpuConvexPolyhedronVertexData* m_convexDataB;
+	int m_shapeTypeB;
+	float m_marginB;
+
+public:
+	SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB,
+						  SpuVoronoiSimplexSolver* simplexSolver);
+
+	//virtual ~btSubsimplexConvexCast();
+
+	///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
+	///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
+	bool calcTimeOfImpact(const btTransform& fromRay,
+						  const btTransform& toRay,
+						  const btTransform& fromB,
+						  const btTransform& toB,
+						  SpuCastResult& result);
+
+};
+
+#endif //SUBSIMPLEX_RAY_CAST_H

src/BulletMultiThreaded/SpuRaycastTaskProcess.cpp

@@ -0,0 +1,189 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 "SpuRaycastTaskProcess.h"
+
+
+SpuRaycastTaskProcess::SpuRaycastTaskProcess(class	btThreadSupportInterface*	threadInterface,  int	maxNumOutstandingTasks)
+:m_threadInterface(threadInterface),
+m_maxNumOutstandingTasks(maxNumOutstandingTasks)
+{
+	m_workUnitTaskBuffers = (unsigned char *)0;
+	m_taskBusy.resize(m_maxNumOutstandingTasks);
+	m_spuRaycastTaskDesc.resize(m_maxNumOutstandingTasks);
+
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+	m_currentWorkUnitInTask = 0;
+
+	m_threadInterface->startSPU();
+
+	//printf("sizeof vec_float4: %d\n", sizeof(vec_float4));
+	//printf("sizeof SpuGatherAndProcessWorkUnitInput: %d\n", sizeof(SpuGatherAndProcessWorkUnitInput));
+
+}
+
+SpuRaycastTaskProcess::~SpuRaycastTaskProcess()
+{
+	
+	if (m_workUnitTaskBuffers != 0)
+	{
+		btAlignedFree(m_workUnitTaskBuffers);
+		m_workUnitTaskBuffers = 0;
+	}
+	
+	m_threadInterface->stopSPU();	
+}
+
+
+
+void SpuRaycastTaskProcess::initialize2(void* spuCollisionObjectsWrappers, int numSpuCollisionObjectWrappers)
+{
+	m_spuCollisionObjectWrappers = spuCollisionObjectsWrappers;
+	m_numSpuCollisionObjectWrappers = numSpuCollisionObjectWrappers;
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+	m_currentWorkUnitInTask = 0;
+
+#ifdef DEBUG_SpuRaycastTaskProcess
+	m_initialized = true;
+#endif
+}
+
+
+void SpuRaycastTaskProcess::issueTask2()
+{
+	m_taskBusy[m_currentTask] = true;
+	m_numBusyTasks++;
+
+	SpuRaycastTaskDesc& taskDesc = m_spuRaycastTaskDesc[m_currentTask];
+
+	taskDesc.taskId = m_currentTask;
+	m_threadInterface->sendRequest(1, (uint32_t) &taskDesc,m_currentTask);
+	//printf("send thread requested for task %d\n", m_currentTask);
+	// if all tasks busy, wait for spu event to clear the task.
+	if (m_numBusyTasks >= m_maxNumOutstandingTasks)
+	{
+		unsigned int taskId;
+		unsigned int outputSize;
+
+		for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+		m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+		//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	} else {
+		//printf("Sent request, not enough busy tasks\n");
+	}
+}
+
+void SpuRaycastTaskProcess::addWorkToTask(SpuRaycastTaskWorkUnit& workunit)
+{
+	m_spuRaycastTaskDesc[m_currentTask].workUnits[m_currentWorkUnitInTask] = workunit;
+	m_currentWorkUnitInTask++;
+	if (m_currentWorkUnitInTask == SPU_RAYCAST_WORK_UNITS_PER_TASK)
+	{
+		m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask;
+		m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers;
+		m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers;
+		//printf("Task buffer full, issuing\n");
+		issueTask2 ();
+		//printf("Returned from issueTask2()\n");
+		m_currentWorkUnitInTask = 0;
+
+		// find new task buffer
+		for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+		{
+			if (!m_taskBusy[i])
+			{
+				m_currentTask = i;
+				//init the task data
+				break;
+			}
+		}
+		//printf("next task = %d\n", m_currentTask);
+	}
+}
+
+
+void 
+SpuRaycastTaskProcess::flush2()
+{
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("\nSpuRaycastTaskProcess::flush()\n");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	
+	// if there's a partially filled task buffer, submit that task
+	//printf("Flushing... %d remaining\n", m_currentWorkUnitInTask);
+	if (m_currentWorkUnitInTask > 0)
+	{
+		m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask;
+		m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers;
+		m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers;
+		issueTask2();
+		m_currentWorkUnitInTask = 0;
+	}
+
+
+	// all tasks are issued, wait for all tasks to be complete
+	while(m_numBusyTasks > 0)
+	{
+	  // Consolidating SPU code
+	  unsigned int taskId;
+	  unsigned int outputSize;
+	  
+	  for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+
+	  //printf("Busy tasks... %d\n", m_numBusyTasks);
+
+	  {
+			// SPURS support.
+			m_threadInterface->waitForResponse(&taskId, &outputSize);
+		}
+
+		//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
+
+		//postProcess(taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	}
+}

src/BulletMultiThreaded/SpuRaycastTaskProcess.h

@@ -0,0 +1,72 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 SPU_RAY_TASK_PROCESS_H
+#define SPU_RAY_TASK_PROCESS_H
+
+#include <assert.h>
+#include <string.h>
+
+#include <LinearMath/btScalar.h>
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include <LinearMath/btAlignedAllocator.h>
+
+#include "PlatformDefinitions.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "SpuRaycastTask/SpuRaycastTask.h"
+
+#include "btThreadSupportInterface.h"
+
+/// SpuRaycastTaskProcess handles SPU processing of raycast requests
+class SpuRaycastTaskProcess
+{
+	unsigned char  *m_workUnitTaskBuffers;
+
+	// track task buffers that are being used, and total busy tasks
+	btAlignedObjectArray<bool>	m_taskBusy;
+	btAlignedObjectArray<SpuRaycastTaskDesc>	m_spuRaycastTaskDesc;
+
+	btThreadSupportInterface*	m_threadInterface;
+
+	int	m_maxNumOutstandingTasks;
+
+	int	m_numBusyTasks;
+
+	// the current task and the current entry to insert a new work unit
+	int m_currentTask;
+	int m_currentWorkUnitInTask;
+	int m_numSpuCollisionObjectWrappers;
+	void* m_spuCollisionObjectWrappers;
+	void issueTask2();
+	//void postProcess(unsigned int taskId, int outputSize);
+
+public:
+	SpuRaycastTaskProcess(btThreadSupportInterface*	threadInterface, int maxNumOutstandingTasks);
+	
+	~SpuRaycastTaskProcess();
+	
+	/// call initialize in the beginning of the frame, before addCollisionPairToTask
+	void initialize2(void* spuCollisionObjectsWrappers, int numSpuCollisionObjectWrappers);
+
+	/// batch up additional work to a current task for SPU processing. When batch is full, it issues the task.
+	void addWorkToTask(struct SpuRaycastTaskWorkUnit&);
+
+	/// call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished
+	void flush2();
+};
+
+
+#endif // SPU_COLLISION_TASK_PROCESS_H
+

src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.cpp

@@ -0,0 +1,214 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans
+
+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 "SpuSampleTask.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "../PlatformDefinitions.h"
+#include "../SpuFakeDma.h"
+#include "LinearMath/btMinMax.h"
+
+#ifdef __SPU__
+#include <spu_printf.h>
+#else
+#include <stdio.h>
+#define spu_printf printf
+#endif
+
+#define MAX_NUM_BODIES 8192
+
+struct SampleTask_LocalStoreMemory
+{
+	ATTRIBUTE_ALIGNED16(char gLocalRigidBody [sizeof(btRigidBody)+16]);
+	ATTRIBUTE_ALIGNED16(void* gPointerArray[MAX_NUM_BODIES]);
+
+};
+
+
+
+
+//-- MAIN METHOD
+void processSampleTask(void* userPtr, void* lsMemory)
+{
+	//	BT_PROFILE("processSampleTask");
+
+	SampleTask_LocalStoreMemory* localMemory = (SampleTask_LocalStoreMemory*)lsMemory;
+
+	SpuSampleTaskDesc* taskDescPtr = (SpuSampleTaskDesc*)userPtr;
+	SpuSampleTaskDesc& taskDesc = *taskDescPtr;
+
+	switch (taskDesc.m_sampleCommand)
+	{
+	case CMD_SAMPLE_INTEGRATE_BODIES:
+		{
+			btTransform predictedTrans;
+			btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr;
+
+			int batchSize = taskDesc.m_sampleValue;
+			if (batchSize>MAX_NUM_BODIES)
+			{
+				spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n");
+				break;
+			}
+			int dmaArraySize = batchSize*sizeof(void*);
+
+			uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr);
+
+			//			spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize);
+
+			if (dmaArraySize>=16)
+			{
+				cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress  , dmaArraySize, DMA_TAG(1), 0, 0);	
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			} else
+			{
+				stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress  , dmaArraySize);
+			}
+
+
+			for ( int i=0;i<batchSize;i++)
+			{
+				///DMA rigid body
+
+				void* localPtr = &localMemory->gLocalRigidBody[0];
+				void* shortAdd = localMemory->gPointerArray[i];
+				uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd);
+
+				//	spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr);
+
+				int dmaBodySize = sizeof(btRigidBody);
+
+				cellDmaGet((void*)localPtr, ppuRigidBodyAddress  , dmaBodySize, DMA_TAG(1), 0, 0);	
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+
+				float timeStep = 1.f/60.f;
+
+				btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj);
+				if (body)
+				{
+					if (body->isActive() && (!body->isStaticOrKinematicObject()))
+					{
+						body->predictIntegratedTransform(timeStep, predictedTrans);
+						body->proceedToTransform( predictedTrans);
+						void* ptr = (void*)localPtr;
+						//	spu_printf("cellDmaLargePut from %llx to LS %llx\n",ptr,ppuRigidBodyAddress);
+
+						cellDmaLargePut(ptr, ppuRigidBodyAddress  , dmaBodySize, DMA_TAG(1), 0, 0);
+						cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+					}
+				}
+
+			}
+			break;
+		}
+
+
+	case CMD_SAMPLE_PREDICT_MOTION_BODIES:
+		{
+			btTransform predictedTrans;
+			btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr;
+
+			int batchSize = taskDesc.m_sampleValue;
+			int dmaArraySize = batchSize*sizeof(void*);
+
+			if (batchSize>MAX_NUM_BODIES)
+			{
+				spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n");
+				break;
+			}
+
+			uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr);
+
+			//			spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize);
+
+			if (dmaArraySize>=16)
+			{
+				cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress  , dmaArraySize, DMA_TAG(1), 0, 0);	
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+			} else
+			{
+				stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress  , dmaArraySize);
+			}
+
+
+			for ( int i=0;i<batchSize;i++)
+			{
+				///DMA rigid body
+
+				void* localPtr = &localMemory->gLocalRigidBody[0];
+				void* shortAdd = localMemory->gPointerArray[i];
+				uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd);
+
+				//	spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr);
+
+				int dmaBodySize = sizeof(btRigidBody);
+
+				cellDmaGet((void*)localPtr, ppuRigidBodyAddress  , dmaBodySize, DMA_TAG(1), 0, 0);	
+				cellDmaWaitTagStatusAll(DMA_MASK(1));
+
+
+				float timeStep = 1.f/60.f;
+
+				btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj);
+				if (body)
+				{
+					if (!body->isStaticOrKinematicObject())
+					{
+						if (body->isActive())
+						{
+							body->integrateVelocities( timeStep);
+							//damping
+							body->applyDamping(timeStep);
+
+							body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+
+							void* ptr = (void*)localPtr;
+							cellDmaLargePut(ptr, ppuRigidBodyAddress  , dmaBodySize, DMA_TAG(1), 0, 0);
+							cellDmaWaitTagStatusAll(DMA_MASK(1));
+						}
+					}
+				}
+
+			}
+			break;
+		}
+	
+
+
+	default:
+		{
+
+		}
+	};
+}
+
+
+#if defined(__CELLOS_LV2__) || defined (LIBSPE2)
+
+ATTRIBUTE_ALIGNED16(SampleTask_LocalStoreMemory	gLocalStoreMemory);
+
+void* createSampleLocalStoreMemory()
+{
+	return &gLocalStoreMemory;
+}
+#else
+void* createSampleLocalStoreMemory()
+{
+	return new SampleTask_LocalStoreMemory;
+};
+
+#endif

src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.h

@@ -0,0 +1,54 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans
+
+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 SPU_SAMPLE_TASK_H
+#define SPU_SAMPLE_TASK_H
+
+#include "../PlatformDefinitions.h"
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+
+#include "LinearMath/btAlignedAllocator.h"
+
+
+enum
+{
+	CMD_SAMPLE_INTEGRATE_BODIES = 1,
+	CMD_SAMPLE_PREDICT_MOTION_BODIES
+};
+
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuSampleTaskDesc
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint32_t						m_sampleCommand;
+	uint32_t						m_taskId;
+
+	uint64_t 	m_mainMemoryPtr;
+	int			m_sampleValue;
+	
+
+};
+
+
+void	processSampleTask(void* userPtr, void* lsMemory);
+void*	createSampleLocalStoreMemory();
+
+
+#endif //SPU_SAMPLE_TASK_H
+

src/BulletMultiThreaded/SpuSampleTask/readme.txt

@@ -0,0 +1 @@
+Empty placeholder for future Libspe2 SPU task

src/BulletMultiThreaded/SpuSampleTaskProcess.cpp

@@ -0,0 +1,222 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+//#define __CELLOS_LV2__ 1
+
+#define USE_SAMPLE_PROCESS 1
+#ifdef USE_SAMPLE_PROCESS
+
+
+#include "SpuSampleTaskProcess.h"
+#include <stdio.h>
+
+#ifdef __SPU__
+
+
+
+void	SampleThreadFunc(void* userPtr,void* lsMemory)
+{
+	//do nothing
+	printf("hello world\n");
+}
+
+
+void*	SamplelsMemoryFunc()
+{
+	//don't create local store memory, just return 0
+	return 0;
+}
+
+
+#else
+
+
+#include "btThreadSupportInterface.h"
+
+//#	include "SPUAssert.h"
+#include <string.h>
+
+
+
+extern "C" {
+	extern char SPU_SAMPLE_ELF_SYMBOL[];
+};
+
+
+
+
+
+SpuSampleTaskProcess::SpuSampleTaskProcess(btThreadSupportInterface*	threadInterface, unsigned int maxNumOutstandingTasks)
+:m_threadInterface(threadInterface),
+m_maxNumOutstandingTasks(maxNumOutstandingTasks)
+{
+
+	m_taskBusy.resize(m_maxNumOutstandingTasks);
+	m_spuSampleTaskDesc.resize(m_maxNumOutstandingTasks);
+
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+
+	m_initialized = false;
+
+	m_threadInterface->startSPU();
+
+
+}
+
+SpuSampleTaskProcess::~SpuSampleTaskProcess()
+{
+	m_threadInterface->stopSPU();
+	
+}
+
+
+
+void	SpuSampleTaskProcess::initialize()
+{
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("SpuSampleTaskProcess::initialize()\n");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	
+	for (int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		m_taskBusy[i] = false;
+	}
+	m_numBusyTasks = 0;
+	m_currentTask = 0;
+	m_initialized = true;
+
+}
+
+
+void SpuSampleTaskProcess::issueTask(void* sampleMainMemPtr,int sampleValue,int sampleCommand)
+{
+
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("SpuSampleTaskProcess::issueTask (m_currentTask= %d\)n", m_currentTask);
+#endif //DEBUG_SPU_TASK_SCHEDULING
+
+	m_taskBusy[m_currentTask] = true;
+	m_numBusyTasks++;
+
+	SpuSampleTaskDesc& taskDesc = m_spuSampleTaskDesc[m_currentTask];
+	{
+		// send task description in event message
+		// no error checking here...
+		// but, currently, event queue can be no larger than NUM_WORKUNIT_TASKS.
+	
+		taskDesc.m_mainMemoryPtr = reinterpret_cast<uint64_t>(sampleMainMemPtr);
+		taskDesc.m_sampleValue = sampleValue;
+		taskDesc.m_sampleCommand = sampleCommand;
+
+		//some bookkeeping to recognize finished tasks
+		taskDesc.m_taskId = m_currentTask;
+	}
+
+
+	m_threadInterface->sendRequest(1, (uint32_t) &taskDesc, m_currentTask);
+
+	// if all tasks busy, wait for spu event to clear the task.
+	
+	if (m_numBusyTasks >= m_maxNumOutstandingTasks)
+	{
+		unsigned int taskId;
+		unsigned int outputSize;
+
+		for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+		m_threadInterface->waitForResponse(&taskId, &outputSize);
+
+		//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
+
+		postProcess(taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	}
+
+	// find new task buffer
+	for (unsigned int i = 0; i < m_maxNumOutstandingTasks; i++)
+	{
+		if (!m_taskBusy[i])
+		{
+			m_currentTask = i;
+			break;
+		}
+	}
+}
+
+
+///Optional PPU-size post processing for each task
+void SpuSampleTaskProcess::postProcess(int taskId, int outputSize)
+{
+
+}
+
+
+void SpuSampleTaskProcess::flush()
+{
+#ifdef DEBUG_SPU_TASK_SCHEDULING
+	printf("\nSpuCollisionTaskProcess::flush()\n");
+#endif //DEBUG_SPU_TASK_SCHEDULING
+	
+
+	// all tasks are issued, wait for all tasks to be complete
+	while(m_numBusyTasks > 0)
+	{
+// Consolidating SPU code
+	  unsigned int taskId;
+	  unsigned int outputSize;
+	  
+	  for (int i=0;i<m_maxNumOutstandingTasks;i++)
+	  {
+		  if (m_taskBusy[i])
+		  {
+			  taskId = i;
+			  break;
+		  }
+	  }
+	  {
+			
+		  m_threadInterface->waitForResponse(&taskId, &outputSize);
+	  }
+
+		//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
+
+		postProcess(taskId, outputSize);
+
+		m_taskBusy[taskId] = false;
+
+		m_numBusyTasks--;
+	}
+
+
+}
+
+#endif
+
+
+#endif //USE_SAMPLE_PROCESS

src/BulletMultiThreaded/SpuSampleTaskProcess.h

@@ -0,0 +1,153 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 SPU_SAMPLE_TASK_PROCESS_H
+#define SPU_SAMPLE_TASK_PROCESS_H
+
+#include <assert.h>
+
+
+#include "PlatformDefinitions.h"
+
+#include <stdlib.h>
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+#include "SpuSampleTask/SpuSampleTask.h"
+
+
+//just add your commands here, try to keep them globally unique for debugging purposes
+#define CMD_SAMPLE_TASK_COMMAND 10
+
+
+
+/// SpuSampleTaskProcess handles SPU processing of collision pairs.
+/// When PPU issues a task, it will look for completed task buffers
+/// PPU will do postprocessing, dependent on workunit output (not likely)
+class SpuSampleTaskProcess
+{
+	// track task buffers that are being used, and total busy tasks
+	btAlignedObjectArray<bool>	m_taskBusy;
+	btAlignedObjectArray<SpuSampleTaskDesc>m_spuSampleTaskDesc;
+	
+	unsigned int   m_numBusyTasks;
+
+	// the current task and the current entry to insert a new work unit
+	unsigned int   m_currentTask;
+
+	bool m_initialized;
+
+	void postProcess(int taskId, int outputSize);
+	
+	class	btThreadSupportInterface*	m_threadInterface;
+
+	unsigned int	m_maxNumOutstandingTasks;
+
+
+
+public:
+	SpuSampleTaskProcess(btThreadSupportInterface*	threadInterface, unsigned int maxNumOutstandingTasks);
+	
+	~SpuSampleTaskProcess();
+	
+	///call initialize in the beginning of the frame, before addCollisionPairToTask
+	void initialize();
+
+	void issueTask(void* sampleMainMemPtr,int sampleValue,int sampleCommand);
+
+	///call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished
+	void flush();
+};
+
+
+#if defined(USE_LIBSPE2) && defined(__SPU__)
+////////////////////MAIN/////////////////////////////
+#include "../SpuLibspe2Support.h"
+#include <spu_intrinsics.h>
+#include <spu_mfcio.h>
+#include <SpuFakeDma.h>
+
+void * SamplelsMemoryFunc();
+void SampleThreadFunc(void* userPtr,void* lsMemory);
+
+//#define DEBUG_LIBSPE2_MAINLOOP
+
+int main(unsigned long long speid, addr64 argp, addr64 envp)
+{
+	printf("SPU is up \n");
+	
+	ATTRIBUTE_ALIGNED128(btSpuStatus status);
+	ATTRIBUTE_ALIGNED16( SpuSampleTaskDesc taskDesc ) ;
+	unsigned int received_message = Spu_Mailbox_Event_Nothing;
+        bool shutdown = false;
+
+	cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(3));
+
+	status.m_status = Spu_Status_Free;
+	status.m_lsMemory.p = SamplelsMemoryFunc();
+
+	cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+	cellDmaWaitTagStatusAll(DMA_MASK(3));
+	
+	
+	while (!shutdown)
+	{
+		received_message = spu_read_in_mbox();
+		
+
+		
+		switch(received_message)
+		{
+		case Spu_Mailbox_Event_Shutdown:
+			shutdown = true;
+			break; 
+		case Spu_Mailbox_Event_Task:
+			// refresh the status
+#ifdef DEBUG_LIBSPE2_MAINLOOP
+			printf("SPU recieved Task \n");
+#endif //DEBUG_LIBSPE2_MAINLOOP
+			cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+			cellDmaWaitTagStatusAll(DMA_MASK(3));
+		
+			btAssert(status.m_status==Spu_Status_Occupied);
+			
+			cellDmaGet(&taskDesc, status.m_taskDesc.p, sizeof(SpuSampleTaskDesc), DMA_TAG(3), 0, 0);
+			cellDmaWaitTagStatusAll(DMA_MASK(3));
+			
+			SampleThreadFunc((void*)&taskDesc, reinterpret_cast<void*> (taskDesc.m_mainMemoryPtr) );
+			break;
+		case Spu_Mailbox_Event_Nothing:
+		default:
+			break;
+		}
+
+		// set to status free and wait for next task
+		status.m_status = Spu_Status_Free;
+		cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
+		cellDmaWaitTagStatusAll(DMA_MASK(3));		
+				
+		
+  	}
+  	return 0;
+}
+//////////////////////////////////////////////////////
+#endif
+
+
+
+#endif // SPU_SAMPLE_TASK_PROCESS_H
+

src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h

@@ -0,0 +1,279 @@
+/*
+Bullet Continuous Collision Detection and Physics Library - Parallel solver
+Copyright (c) 2007 Starbreeze Studios
+
+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.
+
+Written by: Marten Svanfeldt
+*/
+
+#ifndef SPU_PARALLELSOLVERTASK_H
+#define SPU_PARALLELSOLVERTASK_H
+
+#include "../PlatformDefinitions.h"
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "../SpuSync.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+
+ATTRIBUTE_ALIGNED16(struct) ManifoldCellHolder
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint32_t					m_hashCellIndex;		
+	class btPersistentManifold*	m_manifold;
+};
+
+ATTRIBUTE_ALIGNED16(struct) ConstraintCellHolder
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint32_t					m_hashCellIndex;		
+	uint32_t					m_constraintType;
+	class btTypedConstraint*	m_constraint;
+};
+
+enum
+{
+	SPU_HASH_NUMCELLS = 128,
+	SPU_HASH_WORDWIDTH = sizeof(uint32_t)*8,
+	SPU_HASH_NUMCELLDWORDS = ((SPU_HASH_NUMCELLS + SPU_HASH_WORDWIDTH - 1) / SPU_HASH_WORDWIDTH),
+	SPU_HASH_NUMUNUSEDBITS = (SPU_HASH_NUMCELLDWORDS * SPU_HASH_WORDWIDTH) - SPU_HASH_NUMCELLS, 
+	SPU_HASH_PHYSSIZE = 4, //TODO: MAKE CONFIGURABLE
+
+	SPU_MAX_BODIES_PER_CELL = 1024,
+
+	SPU_MAX_SPUS = 6
+};
+
+enum
+{
+	CMD_SOLVER_SETUP_BODIES = 1,
+	CMD_SOLVER_MANIFOLD_SETUP = 2,
+	CMD_SOLVER_CONSTRAINT_SETUP = 3,
+	CMD_SOLVER_SOLVE_ITERATE = 4,
+	CMD_SOLVER_COPYBACK_BODIES = 5
+};
+
+struct SpuSolverHashCell
+{
+	uint16_t						m_numLocalBodies;
+	uint16_t						m_solverBodyOffsetListOffset;
+
+	uint16_t						m_numManifolds;
+	uint16_t						m_manifoldListOffset;
+
+	uint16_t						m_numContacts;
+	uint16_t						m_internalConstraintListOffset;
+
+	uint16_t						m_numConstraints;
+	uint16_t						m_constraintListOffset;
+};
+
+// Shared data structures
+struct SpuSolverHash
+{
+	// Dependency matrix
+	ATTRIBUTE_ALIGNED16(uint32_t m_dependencyMatrix[SPU_HASH_NUMCELLS][SPU_HASH_NUMCELLDWORDS]);
+	ATTRIBUTE_ALIGNED16(uint32_t m_currentMask[SPU_MAX_SPUS+1][SPU_HASH_NUMCELLDWORDS]);
+
+	// The hash itself
+	ATTRIBUTE_ALIGNED16(SpuSolverHashCell m_Hash[SPU_HASH_NUMCELLS]);
+
+	// Hash meta-data	
+};
+
+inline unsigned int spuHash(unsigned int k)  { return k*2654435769u; };
+inline unsigned int spuGetHashCellIndex(int x, int y, int z)
+{
+	//int n = 0x8da6b343 * x + 0xd8163841 * y + 0xcb1ab31f * z;
+
+	int n = x ^ spuHash(y ^ spuHash (z));
+
+	return ((unsigned int)n) & (SPU_HASH_NUMCELLS-1);
+}
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuSolverBody
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btVector3			m_linearVelocity;
+	btVector3			m_angularVelocity;
+
+	btMatrix3x3			m_worldInvInertiaTensor;
+	btScalar            m_angularFactor;
+	btScalar				m_invertedMass;
+};
+
+ATTRIBUTE_ALIGNED16(struct) SpuSolverInternalConstraint
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint32_t			m_localOffsetBodyA;
+	uint32_t			m_localOffsetBodyB;
+
+	btScalar				m_appliedImpulse;
+
+	btScalar				m_friction;
+	btScalar				m_restitution;
+	btScalar				m_jacDiagABInv;
+	btScalar				m_penetration;
+
+	btVector3			m_normal;
+
+	btVector3			m_relpos1CrossNormal;
+	btVector3			m_relpos2CrossNormal;
+	btVector3			m_angularComponentA;
+	btVector3			m_angularComponentB;
+};
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint16_t			m_localOffsetBodyA;
+	uint16_t			m_localOffsetBodyB;
+
+	uint16_t			m_constraintType;
+	struct 
+	{
+		uint16_t		m_useLinear : 1;
+		
+		uint16_t		m_limit1	: 1;
+		uint16_t		m_limit2	: 1;
+		uint16_t		m_limit3	: 1;
+		uint16_t		m_limit4	: 1;
+		uint16_t		m_limit5	: 1;
+		uint16_t		m_limit6	: 1;
+
+		uint16_t		m_motor1	: 1;
+		uint16_t		m_motor2	: 1;
+		uint16_t		m_motor3	: 1;
+		uint16_t		m_motor4	: 1;
+		uint16_t		m_motor5	: 1;
+		uint16_t		m_motor6	: 1;
+	}					m_flags;
+
+	// Linear parts, used by all constraints
+	btQuadWordStorage	m_relPos1;
+	btQuadWordStorage	m_relPos2;
+	btQuadWordStorage	m_jacdiagABInv;		//Jacobian inverse multiplied by gamma (damping) for each axis
+	btQuadWordStorage	m_linearBias;		//depth*tau/(dt*gamma) along each axis
+
+	// Joint-specific parts
+	union
+	{
+		struct 
+		{
+			btQuadWordStorage	m_frameAinW[3];
+			btQuadWordStorage	m_frameBinW[3];
+
+			// For angular
+			btQuadWordStorage	m_angJacdiagABInv;		//1/j 
+			btQuadWordStorage	m_angularBias;			//error/dt, in x/y.		limit error*bias factor / (dt * relaxation factor) in z
+			
+			// For limit
+			float				m_limitAccumulatedImpulse;
+			float				m_limitJacFactor;		//limitSign*relaxation factor
+
+			// For motor
+			float				m_motorVelocity;
+			float				m_motorImpulse;
+		} hinge;
+		
+		struct  
+		{
+			btQuadWordStorage	m_swingAxis;
+			btQuadWordStorage	m_twistAxis;
+
+			float				m_swingError;
+			float				m_swingJacInv;
+			float				m_swingLimitImpulse;
+
+			float				m_twistError;
+			float				m_twistJacInv;
+			float				m_twistLimitImpulse;
+		} conetwist;
+	};
+};
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuSolverDataDesc
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	SpuSolverHash*					m_solverHash;
+	SpuSolverBody*					m_solverBodyList;
+	SpuSolverInternalConstraint*	m_solverInternalConstraintList;
+	SpuSolverConstraint*			m_solverConstraintList;
+	uint32_t*						m_solverBodyOffsetList;
+};
+
+
+ATTRIBUTE_ALIGNED16(struct) SpuSolverTaskDesc
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	uint32_t						m_solverCommand;
+	uint32_t						m_taskId;
+	SpuSolverDataDesc				m_solverData;
+
+	// command specific data
+	union
+	{
+		// Body setup
+		struct 
+		{
+			uint32_t				m_startBody;
+			uint32_t				m_numBodies;
+
+			class btRigidBody**		m_rbList;
+		} m_bodySetup, m_bodyCopyback;
+
+		struct 
+		{
+			uint32_t				m_startCell;
+			uint32_t				m_numCells;
+
+			uint32_t				m_numBodies;
+			uint32_t				m_numManifolds;
+
+			ManifoldCellHolder*		m_manifoldHolders;
+			ConstraintCellHolder*	m_constraintHolders;
+			btContactSolverInfoData	m_solverInfo;
+		} m_manifoldSetup;
+
+		struct  
+		{
+			btSpinlock::SpinVariable*	m_spinLockVar;
+		} m_iterate;
+	}								m_commandData;
+};
+
+void	processSolverTask(void* userPtr, void* lsMemory);
+void*	createSolverLocalStoreMemory();
+
+// Helper
+inline bool constraintTypeSupported(btTypedConstraintType type)
+{
+	return type == POINT2POINT_CONSTRAINT_TYPE ||
+		type == HINGE_CONSTRAINT_TYPE ||
+		type == CONETWIST_CONSTRAINT_TYPE ||
+		type == D6_CONSTRAINT_TYPE;
+}
+
+#endif

src/BulletMultiThreaded/SpuSync.h

@@ -0,0 +1,146 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2007 Starbreeze Studios
+
+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.
+
+Written by: Marten Svanfeldt
+*/
+
+#ifndef SPU_SYNC_H
+#define	SPU_SYNC_H
+
+
+#include "PlatformDefinitions.h"
+
+
+#if defined(WIN32)
+
+#define WIN32_LEAN_AND_MEAN
+#ifdef _XBOX
+#include <Xtl.h>
+#else
+#include <Windows.h>
+#endif
+
+class btSpinlock
+{
+public:
+	//typedef volatile LONG SpinVariable;
+	typedef CRITICAL_SECTION SpinVariable;
+
+	btSpinlock (SpinVariable* var)
+		: spinVariable (var)
+	{}
+
+	void Init ()
+	{
+		//*spinVariable = 0;
+		InitializeCriticalSection(spinVariable);
+	}
+
+	void Lock ()
+	{
+		EnterCriticalSection(spinVariable);
+	}
+
+	void Unlock ()
+	{
+		LeaveCriticalSection(spinVariable);
+	}
+
+private:
+	SpinVariable* spinVariable;
+};
+
+
+#elif defined (__CELLOS_LV2__)
+
+//#include <cell/atomic.h>
+#include <cell/sync/mutex.h>
+
+class btSpinlock
+{
+public:
+	typedef CellSyncMutex SpinVariable;
+
+	btSpinlock (SpinVariable* var)
+		: spinVariable (var)
+	{}
+
+	void Init ()
+	{
+#ifndef __SPU__
+		//*spinVariable = 1;
+		cellSyncMutexInitialize(spinVariable);
+#endif
+	}
+
+
+
+	void Lock ()
+	{
+#ifdef __SPU__
+		// lock semaphore
+		/*while (cellAtomicTestAndDecr32(atomic_buf, (uint64_t)spinVariable) == 0) 
+		{
+
+		};*/
+		cellSyncMutexLock((uint64_t)spinVariable);
+#endif
+	}
+
+	void Unlock ()
+	{
+#ifdef __SPU__
+		//cellAtomicIncr32(atomic_buf, (uint64_t)spinVariable);
+		cellSyncMutexUnlock((uint64_t)spinVariable);
+#endif 
+	}
+
+
+private:
+	SpinVariable*	spinVariable;
+	ATTRIBUTE_ALIGNED128(uint32_t		atomic_buf[32]);
+};
+
+#else
+//create a dummy implementation (without any locking) useful for serial processing
+class btSpinlock
+{
+public:
+	typedef int  SpinVariable;
+
+	btSpinlock (SpinVariable* var)
+		: spinVariable (var)
+	{}
+
+	void Init ()
+	{
+	}
+
+	void Lock ()
+	{
+	}
+
+	void Unlock ()
+	{
+	}
+
+private:
+	SpinVariable* spinVariable;
+};
+
+
+#endif
+
+
+#endif

src/BulletMultiThreaded/Win32ThreadSupport.cpp

@@ -0,0 +1,259 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+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 "Win32ThreadSupport.h"
+
+#ifdef USE_WIN32_THREADING
+
+#include <windows.h>
+
+#include "SpuCollisionTaskProcess.h"
+
+#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+
+
+
+///The number of threads should be equal to the number of available cores
+///Todo: each worker should be linked to a single core, using SetThreadIdealProcessor.
+
+///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+///Setup and initialize SPU/CELL/Libspe2
+Win32ThreadSupport::Win32ThreadSupport(const Win32ThreadConstructionInfo & threadConstructionInfo)
+{
+	startThreads(threadConstructionInfo);
+}
+
+///cleanup/shutdown Libspe2
+Win32ThreadSupport::~Win32ThreadSupport()
+{
+	stopSPU();
+}
+
+
+
+
+#include <stdio.h>
+
+DWORD WINAPI Thread_no_1( LPVOID lpParam ) 
+{
+
+	Win32ThreadSupport::btSpuStatus* status = (Win32ThreadSupport::btSpuStatus*)lpParam;
+
+	
+	while (1)
+	{
+		WaitForSingleObject(status->m_eventStartHandle,INFINITE);
+		
+		void* userPtr = status->m_userPtr;
+
+		if (userPtr)
+		{
+			btAssert(status->m_status);
+			status->m_userThreadFunc(userPtr,status->m_lsMemory);
+			status->m_status = 2;
+			SetEvent(status->m_eventCompletetHandle);
+		} else
+		{
+			//exit Thread
+			status->m_status = 3;
+			SetEvent(status->m_eventCompletetHandle);
+			printf("Thread with taskId %i with handle %i exiting\n",status->m_taskId, status->m_threadHandle);
+			break;
+		}
+		
+	}
+
+	printf("Thread TERMINATED\n");
+	return 0;
+
+}
+
+///send messages to SPUs
+void Win32ThreadSupport::sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t taskId)
+{
+	///	gMidphaseSPU.sendRequest(CMD_GATHER_AND_PROCESS_PAIRLIST, (uint32_t) &taskDesc);
+	
+	///we should spawn an SPU task here, and in 'waitForResponse' it should wait for response of the (one of) the first tasks that finished
+	
+
+
+	switch (uiCommand)
+	{
+	case 	CMD_GATHER_AND_PROCESS_PAIRLIST:
+		{
+
+
+//#define SINGLE_THREADED 1
+#ifdef SINGLE_THREADED
+
+			btSpuStatus&	spuStatus = m_activeSpuStatus[0];
+			spuStatus.m_userPtr=(void*)uiArgument0;
+			spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory);
+			HANDLE handle =0;
+#else
+
+
+			btSpuStatus&	spuStatus = m_activeSpuStatus[taskId];
+			btAssert(taskId>=0);
+			btAssert(taskId<m_activeSpuStatus.size());
+
+			spuStatus.m_commandId = uiCommand;
+			spuStatus.m_status = 1;
+			spuStatus.m_userPtr = (void*)uiArgument0;
+
+			///fire event to start new task
+			SetEvent(spuStatus.m_eventStartHandle);
+
+#endif //CollisionTask_LocalStoreMemory
+
+			
+
+			break;
+		}
+	default:
+		{
+			///not implemented
+			btAssert(0);
+		}
+
+	};
+
+
+}
+
+
+///check for messages from SPUs
+void Win32ThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
+{
+	///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response
+	
+	///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback'
+
+
+	btAssert(m_activeSpuStatus.size());
+
+	int last = -1;
+#ifndef SINGLE_THREADED
+	DWORD res = WaitForMultipleObjects(m_completeHandles.size(), &m_completeHandles[0], FALSE, INFINITE);
+	btAssert(res != WAIT_FAILED);
+	last = res - WAIT_OBJECT_0;
+
+	btSpuStatus& spuStatus = m_activeSpuStatus[last];
+	btAssert(spuStatus.m_threadHandle);
+	btAssert(spuStatus.m_eventCompletetHandle);
+
+	//WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE);
+	btAssert(spuStatus.m_status > 1);
+	spuStatus.m_status = 0;
+
+	///need to find an active spu
+	btAssert(last>=0);
+
+#else
+	last=0;
+	btSpuStatus& spuStatus = m_activeSpuStatus[last];
+#endif //SINGLE_THREADED
+
+	
+
+	*puiArgument0 = spuStatus.m_taskId;
+	*puiArgument1 = spuStatus.m_status;
+
+
+}
+
+
+
+void Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadConstructionInfo)
+{
+
+	m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads);
+	m_completeHandles.resize(threadConstructionInfo.m_numThreads);
+
+	for (int i=0;i<threadConstructionInfo.m_numThreads;i++)
+	{
+		printf("starting thread %d\n",i);
+
+		btSpuStatus&	spuStatus = m_activeSpuStatus[i];
+
+		LPSECURITY_ATTRIBUTES lpThreadAttributes=NULL;
+		SIZE_T dwStackSize=threadConstructionInfo.m_threadStackSize;
+		LPTHREAD_START_ROUTINE lpStartAddress=&Thread_no_1;
+		LPVOID lpParameter=&spuStatus;
+		DWORD dwCreationFlags=0;
+		LPDWORD lpThreadId=0;
+
+		spuStatus.m_userPtr=0;
+
+		sprintf(spuStatus.m_eventStartHandleName,"eventStart%s%d",threadConstructionInfo.m_uniqueName,i);
+		spuStatus.m_eventStartHandle = CreateEvent(0,false,false,spuStatus.m_eventStartHandleName);
+
+		sprintf(spuStatus.m_eventCompletetHandleName,"eventComplete%s%d",threadConstructionInfo.m_uniqueName,i);
+		spuStatus.m_eventCompletetHandle = CreateEvent(0,false,false,spuStatus.m_eventCompletetHandleName);
+
+		m_completeHandles[i] = spuStatus.m_eventCompletetHandle;
+
+		HANDLE handle = CreateThread(lpThreadAttributes,dwStackSize,lpStartAddress,lpParameter,	dwCreationFlags,lpThreadId);
+		SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST);
+		//SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL);
+
+		SetThreadAffinityMask(handle, 1<<i);
+
+		spuStatus.m_taskId = i;
+		spuStatus.m_commandId = 0;
+		spuStatus.m_status = 0;
+		spuStatus.m_threadHandle = handle;
+		spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
+		spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
+
+		printf("started thread %d with threadHandle %d\n",i,handle);
+		
+	}
+
+}
+
+void Win32ThreadSupport::startSPU()
+{
+}
+
+
+///tell the task scheduler we are done with the SPU tasks
+void Win32ThreadSupport::stopSPU()
+{
+	int i;
+	for (i=0;i<m_activeSpuStatus.size();i++)
+	{
+		btSpuStatus& spuStatus = m_activeSpuStatus[i];
+		if (spuStatus.m_status>0)
+		{
+			WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE);
+		}
+		
+
+		spuStatus.m_userPtr = 0;
+		SetEvent(spuStatus.m_eventStartHandle);
+		WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE);
+
+		CloseHandle(spuStatus.m_eventCompletetHandle);
+		CloseHandle(spuStatus.m_eventStartHandle);
+		CloseHandle(spuStatus.m_threadHandle);
+	}
+
+	m_activeSpuStatus.clear();
+	m_completeHandles.clear();
+
+}
+
+#endif //USE_WIN32_THREADING