1)Added SCE Physics Effects boxBoxDistance

BulletMultiThreaded/NarrowPhaseCollision makes use of this boxBoxDistance.
Cache some values in src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp, to avoid DMA transfers

2) Added btConvexSeparatingDistanceUtil: this allows caching of separating distance/vector as early-out to avoid convex-convex collision detection.
btConvexSeparatingDistanceUtil is used in src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp and can be controlled by btDispatcherInfo.m_useConvexConservativeDistanceUtil/m_convexConservativeDistanceThreshold

3) Use BulletMultiThreaded/vectormath/scalar/cpp/vectormath/scalar/cpp/vectormath_aos.h as fallback for non-PlayStation 3 Cell SPU/PPU platforms (used by boxBoxDistance).
Note there are other implementations in Extras/vectormath folder, that are potentially faster for IBM Cell SDK 3.0 SPU (libspe2)

Demos/Benchmarks/BenchmarkDemo.cpp

@@ -26,6 +26,11 @@ subject to the following restrictions:
 #include "Taru.mdl"
 #include "landscape.mdl"
 #include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
+#include "BulletMultiThreaded/SpuGatheringCollisionDispatcher.h"
+#include "BulletMultiThreaded/SequentialThreadSupport.h"
+#include "BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+
+
 
 
 
@@ -268,6 +273,16 @@ void	BenchmarkDemo::initPhysics()
 	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
 	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
 
+#if 0
+	SequentialThreadSupport::SequentialThreadConstructionInfo sci("spuCD",
+									processCollisionTask,
+									createCollisionLocalStoreMemory);
+
+	SequentialThreadSupport* seq = new SequentialThreadSupport(sci);
+	m_dispatcher = new	SpuGatheringCollisionDispatcher(seq,1,m_collisionConfiguration);
+#endif
+
+
 	///the maximum size of the collision world. Make sure objects stay within these boundaries
 	///Don't make the world AABB size too large, it will harm simulation quality and performance
 	btVector3 worldAabbMin(-10000,-10000,-10000);
@@ -1132,9 +1147,9 @@ void BenchmarkDemo::castRays()
 
 void	BenchmarkDemo::createTest7()
 {
-	setCameraDistance(btScalar(150.));
+	
 	createTest6();
-
+	setCameraDistance(btScalar(150.));
 	initRays();
 }
 

Demos/Benchmarks/BenchmarkDemo.h

@@ -17,7 +17,7 @@ subject to the following restrictions:
 
 #include "DemoApplication.h"
 #include "LinearMath/btAlignedObjectArray.h"
-#define NUMRAYS 200
+#define NUMRAYS 500
 
 class btBroadphaseInterface;
 class btCollisionShape;

Demos/Benchmarks/main.cpp

@@ -27,12 +27,12 @@ int main(int argc,char** argv)
 	GLDebugDrawer	gDebugDrawer;
 
 //	BenchmarkDemo1 benchmarkDemo;
-//	BenchmarkDemo2 benchmarkDemo;
+	BenchmarkDemo2 benchmarkDemo;
 //	BenchmarkDemo3 benchmarkDemo;
 //	BenchmarkDemo4 benchmarkDemo;
 //	BenchmarkDemo5 benchmarkDemo;
 //	BenchmarkDemo6 benchmarkDemo;
-	BenchmarkDemo7 benchmarkDemo;
+//	BenchmarkDemo7 benchmarkDemo;
 
 
 	benchmarkDemo.initPhysics();

src/BulletCollision/BroadphaseCollision/btDispatcher.h

@@ -46,22 +46,25 @@ struct btDispatcherInfo
 		m_enableSPU(true),
 		m_useEpa(true),
 		m_allowedCcdPenetration(btScalar(0.04)),
+		m_useConvexConservativeDistanceUtil(true),
+		m_convexConservativeDistanceThreshold(0.01f),
 		m_stackAllocator(0)
 	{
 
 	}
 	btScalar	m_timeStep;
-	int		m_stepCount;
+	int			m_stepCount;
-	int		m_dispatchFunc;
+	int			m_dispatchFunc;
 	mutable btScalar	m_timeOfImpact;
-	bool	m_useContinuous;
+	bool		m_useContinuous;
 	class btIDebugDraw*	m_debugDraw;
-	bool	m_enableSatConvex;
+	bool		m_enableSatConvex;
-	bool	m_enableSPU;
+	bool		m_enableSPU;
-	bool	m_useEpa;
+	bool		m_useEpa;
 	btScalar	m_allowedCcdPenetration;
+	bool		m_useConvexConservativeDistanceUtil;
+	btScalar	m_convexConservativeDistanceThreshold;
 	btStackAlloc*	m_stackAllocator;
-	
 };
 
 ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -64,12 +64,12 @@ btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const
 m_gjkPairDetector(0,0,simplexSolver,pdSolver),
 m_ownManifold (false),
 m_manifoldPtr(mf),
-m_lowLevelOfDetail(false)
+m_lowLevelOfDetail(false),
+m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
+			  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc())
 {
 	(void)body0;
 	(void)body1;
-
-
 }
 
 
@@ -107,6 +107,16 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	}
 	resultOut->setPersistentManifold(m_manifoldPtr);
 
+	
+
+	btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
+	btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
+
+	m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
+
+	if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
+	{
+
 #ifdef USE_BT_GJKEPA
 	btConvexShape*				shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
 	btConvexShape*				shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
@@ -121,16 +131,15 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 		}
 #else
 
-	btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
+	
-	btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
 	
 	btGjkPairDetector::ClosestPointInput input;
 
 	//TODO: if (dispatchInfo.m_useContinuous)
 	m_gjkPairDetector.setMinkowskiA(min0);
 	m_gjkPairDetector.setMinkowskiB(min1);
-	input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+	input.m_maximumDistanceSquared = 1e30f;//min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
-	input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+	//input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
 	input.m_stackAlloc = dispatchInfo.m_stackAllocator;
 
 //	input.m_maximumDistanceSquared = btScalar(1e30);
@@ -141,6 +150,13 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 #endif
 
+	btScalar sepDist = m_gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
+
+	m_sepDistance.initSeparatingDistance(m_gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
+
+
+	}
+
 	if (m_ownManifold)
 	{
 		resultOut->refreshContactPoints();

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

@@ -23,6 +23,7 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "btCollisionCreateFunc.h"
 #include "btCollisionDispatcher.h"
+#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
 
 class btConvexPenetrationDepthSolver;
 
@@ -36,6 +37,9 @@ public:
 	btPersistentManifold*	m_manifoldPtr;
 	bool			m_lowLevelOfDetail;
 	
+	///cache separating vector to speedup collision detection
+	btConvexSeparatingDistanceUtil	m_sepDistance;
+
 
 public:
 

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -53,6 +53,8 @@ m_catchDegeneracies(1)
 
 void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
 {
+	m_cachedSeparatingDistance = 0.f;
+
 	btScalar distance=btScalar(0.);
 	btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
 	btVector3 pointOnA,pointOnB;
@@ -331,6 +333,9 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 #ifdef DEBUG_SPU_COLLISION_DETECTION
 		spu_printf("output 1\n");
 #endif
+		m_cachedSeparatingAxis = normalInB;
+		m_cachedSeparatingDistance = distance;
+
 		output.addContactPoint(
 			normalInB,
 			pointOnB+positionOffset,

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h

@@ -38,6 +38,7 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
 	const btConvexShape* m_minkowskiA;
 	const btConvexShape* m_minkowskiB;
 	bool		m_ignoreMargin;
+	btScalar	m_cachedSeparatingDistance;
 	
 
 public:
@@ -68,6 +69,15 @@ public:
 		m_cachedSeparatingAxis = seperatingAxis;
 	}
 
+	const btVector3& getCachedSeparatingAxis() const
+	{
+		return m_cachedSeparatingAxis;
+	}
+	btScalar	getCachedSeparatingDistance() const
+	{
+		return m_cachedSeparatingDistance;
+	}
+
 	void	setPenetrationDepthSolver(btConvexPenetrationDepthSolver*	penetrationDepthSolver)
 	{
 		m_penetrationDepthSolver = penetrationDepthSolver;

src/BulletMultiThreaded/CMakeLists.txt

@@ -1,5 +1,5 @@
 INCLUDE_DIRECTORIES(
-${BULLET_PHYSICS_SOURCE_DIR}/src
+${BULLET_PHYSICS_SOURCE_DIR}/src ${BULLET_PHYSICS_SOURCE_DIR}/src/BulletMultiThreaded/vectormath/scalar/cpp
 )
 
 ADD_LIBRARY(LibBulletMultiThreaded
@@ -30,6 +30,9 @@ ADD_LIBRARY(LibBulletMultiThreaded
 		SpuGatheringCollisionDispatcher.cpp
 		SpuContactManifoldCollisionAlgorithm.cpp
 		SpuContactManifoldCollisionAlgorithm.h
+		SpuNarrowPhaseCollisionTask/Box.h
+		SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp
+		SpuNarrowPhaseCollisionTask/boxBoxDistance.h
 		SpuNarrowPhaseCollisionTask/SpuContactResult.cpp
 		SpuNarrowPhaseCollisionTask/SpuContactResult.h
 		SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp

src/BulletMultiThreaded/Jamfile

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

src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp

@@ -17,7 +17,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
-
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
 
 SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm()
 :m_manifoldPtr(0)
@@ -46,7 +46,19 @@ SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const
 	m_shapeType1 = body1->getCollisionShape()->getShapeType();
 	m_collisionMargin0 = body0->getCollisionShape()->getMargin();
 	m_collisionMargin1 = body1->getCollisionShape()->getMargin();
+	m_collisionObject0 = body0;
+	m_collisionObject1 = body1;
 
+	if (body0->getCollisionShape()->isPolyhedral())
+	{
+		btPolyhedralConvexShape* convex0 = (btPolyhedralConvexShape*)body0->getCollisionShape();
+		m_shapeDimensions0 = convex0->getImplicitShapeDimensions();
+	}
+	if (body1->getCollisionShape()->isPolyhedral())
+	{
+		btPolyhedralConvexShape* convex1 = (btPolyhedralConvexShape*)body1->getCollisionShape();
+		m_shapeDimensions1 = convex1->getImplicitShapeDimensions();
+	}
 }
 #endif //__SPU__
 

src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h

@@ -26,13 +26,18 @@ class btPersistentManifold;
 /// SpuContactManifoldCollisionAlgorithm  provides contact manifold and should be processed on SPU.
 ATTRIBUTE_ALIGNED16(class) SpuContactManifoldCollisionAlgorithm : public btCollisionAlgorithm
 {
-
+	btVector3	m_shapeDimensions0;
+	btVector3	m_shapeDimensions1;
 	btPersistentManifold*	m_manifoldPtr;
 	int		m_shapeType0;
 	int		m_shapeType1;
 	float	m_collisionMargin0;
 	float	m_collisionMargin1;
 
+	btCollisionObject*	m_collisionObject0;
+	btCollisionObject*	m_collisionObject1;
+	
+
 	
 public:
 	
@@ -57,6 +62,16 @@ public:
 		return m_manifoldPtr;
 	}
 
+	btCollisionObject*	getCollisionObject0()
+	{
+		return m_collisionObject0;
+	}
+	
+	btCollisionObject*	getCollisionObject1()
+	{
+		return m_collisionObject1;
+	}
+
 	int		getShapeType0() const
 	{
 		return m_shapeType0;
@@ -75,6 +90,16 @@ public:
 		return m_collisionMargin1;
 	}
 
+	const btVector3&	getShapeDimensions0() const
+	{
+		return m_shapeDimensions0;
+	}
+
+	const btVector3&	getShapeDimensions1() const
+	{
+		return m_shapeDimensions1;
+	}
+
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)

src/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp

@@ -126,7 +126,7 @@ public:
 					if (m_dispatcher->supportsDispatchPairOnSpu(proxyType0,proxyType1))
 					{
 						int so = sizeof(SpuContactManifoldCollisionAlgorithm);
-						void* mem = m_dispatcher->allocateCollisionAlgorithm(so);
+						void* mem = btAlignedAlloc(sizeof(SpuContactManifoldCollisionAlgorithm),16);//m_dispatcher->allocateCollisionAlgorithm(so);
 						collisionPair.m_algorithm = new(mem) SpuContactManifoldCollisionAlgorithm(ci,colObj0,colObj1);
 						collisionPair.m_userInfo = (void*) 2;
 					} else

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/Box.h

@@ -0,0 +1,158 @@
+/*
+   Copyright (C) 2006, 2008 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+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 __BOX_H__
+#define __BOX_H__
+
+#include <math.h>
+#include <vectormath_aos.h>
+
+using namespace Vectormath::Aos;
+
+enum FeatureType { F, E, V };
+
+//----------------------------------------------------------------------------
+// Box
+//----------------------------------------------------------------------------
+
+class Box
+{
+public:
+	Vector3 half;
+
+	inline Box()
+	{}
+	inline Box(Vector3 half_);
+	inline Box(float hx, float hy, float hz);
+
+	inline void Set(Vector3 half_);
+	inline void Set(float hx, float hy, float hz);
+
+	inline Vector3 GetAABB(const Matrix3& rotation) const;
+};
+
+inline
+Box::Box(Vector3 half_)
+{
+	Set(half_);
+}
+
+inline
+Box::Box(float hx, float hy, float hz)
+{
+	Set(hx, hy, hz);
+}
+
+inline
+void
+Box::Set(Vector3 half_)
+{
+	half = half_;
+}
+
+inline
+void
+Box::Set(float hx, float hy, float hz)
+{
+	half = Vector3(hx, hy, hz);
+}
+
+inline
+Vector3
+Box::GetAABB(const Matrix3& rotation) const
+{
+	return absPerElem(rotation) * half;
+}
+
+//-------------------------------------------------------------------------------------------------
+// BoxPoint
+//-------------------------------------------------------------------------------------------------
+
+class BoxPoint
+{
+public:
+	BoxPoint() : localPoint(0.0f) {}
+
+	Point3      localPoint;
+	FeatureType featureType;
+	int         featureIdx;
+
+	inline void setVertexFeature(int plusX, int plusY, int plusZ);
+	inline void setEdgeFeature(int dim0, int plus0, int dim1, int plus1);
+	inline void setFaceFeature(int dim, int plus);
+
+	inline void getVertexFeature(int & plusX, int & plusY, int & plusZ) const;
+	inline void getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const;
+	inline void getFaceFeature(int & dim, int & plus) const;
+};
+
+inline
+void
+BoxPoint::setVertexFeature(int plusX, int plusY, int plusZ)
+{
+	featureType = V;
+	featureIdx = plusX << 2 | plusY << 1 | plusZ;
+}
+
+inline
+void
+BoxPoint::setEdgeFeature(int dim0, int plus0, int dim1, int plus1)
+{
+	featureType = E;
+
+	if (dim0 > dim1) {
+		featureIdx = plus1 << 5 | dim1 << 3 | plus0 << 2 | dim0;
+	} else {
+		featureIdx = plus0 << 5 | dim0 << 3 | plus1 << 2 | dim1;
+	}
+}
+
+inline
+void
+BoxPoint::setFaceFeature(int dim, int plus)
+{
+	featureType = F;
+	featureIdx = plus << 2 | dim;
+}
+
+inline
+void
+BoxPoint::getVertexFeature(int & plusX, int & plusY, int & plusZ) const
+{
+	plusX = featureIdx >> 2;
+	plusY = featureIdx >> 1 & 1;
+	plusZ = featureIdx & 1;
+}
+
+inline
+void
+BoxPoint::getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const
+{
+	plus0 = featureIdx >> 5;
+	dim0 = featureIdx >> 3 & 3;
+	plus1 = featureIdx >> 2 & 1;
+	dim1 = featureIdx & 3;
+}
+
+inline
+void
+BoxPoint::getFaceFeature(int & dim, int & plus) const
+{
+	plus = featureIdx >> 2;
+	dim = featureIdx & 3;
+}
+
+#endif /* __BOX_H__ */

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp

@@ -26,7 +26,8 @@
 #include "SpuEpaPenetrationDepthSolver.h"
 #include "SpuGjkPairDetector.h"
 #include "SpuVoronoiSimplexSolver.h"
-
+#include "boxBoxDistance.h"
+#include "Util/Vectormath2Bullet.h"
 #include "SpuCollisionShapes.h" //definition of SpuConvexPolyhedronVertexData
 
 #ifdef __SPU__
@@ -89,11 +90,14 @@ bool gUseEpa = false;
 #include <LibSN_SPU.h>
 #endif //USE_SN_TUNER
 
-#if defined (__CELLOS_LV2__) || defined (USE_LIBSPE2)
+#if defined (__SPU__) || defined (USE_LIBSPE2)
+#include <spu_printf.h>
 #else
 #define IGNORE_ALIGNMENT 1
-#define spu_printf printf
 #include <stdio.h>
+#include <stdlib.h>
+#define spu_printf printf
+
 #endif
 
 //int gNumConvexPoints0=0;
@@ -101,14 +105,7 @@ bool gUseEpa = false;
 ///Make sure no destructors are called on this memory
 struct	CollisionTask_LocalStoreMemory
 {
-	ATTRIBUTE_ALIGNED16(char	bufferProxy0[sizeof(btBroadphaseProxy)+16]);
+	
-	ATTRIBUTE_ALIGNED16(char	bufferProxy1[sizeof(btBroadphaseProxy)+16]);
-
-	ATTRIBUTE_ALIGNED16(btBroadphaseProxy*	gProxyPtr0);
-	ATTRIBUTE_ALIGNED16(btBroadphaseProxy*	gProxyPtr1);
-
-	//ATTRIBUTE_ALIGNED16(btCollisionObject	gColObj0);
-	//ATTRIBUTE_ALIGNED16(btCollisionObject	gColObj1);
 	ATTRIBUTE_ALIGNED16(char gColObj0 [sizeof(btCollisionObject)+16]);
 	ATTRIBUTE_ALIGNED16(char gColObj1 [sizeof(btCollisionObject)+16]);
 	
@@ -472,18 +469,19 @@ void	ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa
 		//try generic GJK
 
 		SpuVoronoiSimplexSolver vsSolver;
-		SpuEpaPenetrationDepthSolver epaPenetrationSolver;
 		SpuMinkowskiPenetrationDepthSolver	minkowskiPenetrationSolver;
 		SpuConvexPenetrationDepthSolver* penetrationSolver;
-
+#ifdef ENABLE_EPA
+		SpuEpaPenetrationDepthSolver epaPenetrationSolver;
 		if (gUseEpa)
 		{
 			penetrationSolver = &epaPenetrationSolver;
-		} else {
+		} else
+#endif
+		{
 			penetrationSolver = &minkowskiPenetrationSolver;
 		}
 
-
 		///DMA in the vertices for convex shapes
 		ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
 		ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]);
@@ -582,12 +580,12 @@ SIMD_FORCE_INLINE void	dmaAndSetupCollisionObjects(SpuCollisionPairInput& collis
 	register int dmaSize;
 	register ppu_address_t	dmaPpuAddress2;
 		
-	dmaSize = sizeof(btCollisionObject);
+	dmaSize = sizeof(btCollisionObject);//btTransform);
-	dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr1->m_clientObject :*/ (ppu_address_t)lsMem.gProxyPtr0->m_clientObject;
+	dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr1->m_clientObject :*/ (ppu_address_t)lsMem.gSpuContactManifoldAlgo.getCollisionObject0();
 	cellDmaGet(&lsMem.gColObj0, dmaPpuAddress2  , dmaSize, DMA_TAG(1), 0, 0);		
 
-	dmaSize = sizeof(btCollisionObject);
+	dmaSize = sizeof(btCollisionObject);//btTransform);
-	dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr0->m_clientObject :*/ (ppu_address_t)lsMem.gProxyPtr1->m_clientObject;
+	dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr0->m_clientObject :*/ (ppu_address_t)lsMem.gSpuContactManifoldAlgo.getCollisionObject1();
 	cellDmaGet(&lsMem.gColObj1, dmaPpuAddress2  , dmaSize, DMA_TAG(2), 0, 0);		
 	
 	cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
@@ -796,8 +794,9 @@ void	handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTas
 		}
 
 	}
-
+	
 	spuContacts.flush();
+
 }
 
 
@@ -912,6 +911,7 @@ void	processCollisionTask(void* userPtr, void* lsMemPtr)
 #endif //DEBUG_SPU_COLLISION_DETECTION
 
 						
+						/*
 						dmaSize = sizeof(btBroadphaseProxy);
 						dmaPpuAddress2 = (ppu_address_t)pair.m_pProxy0;
 						//stallingUnalignedDmaSmallGet(lsMem.gProxyPtr0, dmaPpuAddress2  , dmaSize);
@@ -923,6 +923,8 @@ void	processCollisionTask(void* userPtr, void* lsMemPtr)
 						tmpPtr  = cellDmaSmallGetReadOnly(&lsMem.bufferProxy1, dmaPpuAddress2  , dmaSize,DMA_TAG(1), 0, 0);
 
 						lsMem.gProxyPtr1 = (btBroadphaseProxy*)tmpPtr;
+						*/
+
 
 						cellDmaWaitTagStatusAll(DMA_MASK(1));
 
@@ -963,11 +965,80 @@ void	processCollisionTask(void* userPtr, void* lsMemPtr)
 								// Get the collision objects
 								dmaAndSetupCollisionObjects(collisionPairInput, lsMem);
 
-								if (lsMem.getColObj0()->isActive() || lsMem.getColObj1()->isActive())
+								//if (lsMem.getColObj0()->isActive() || lsMem.getColObj1()->isActive())
 								{
-									handleCollisionPair(collisionPairInput, lsMem, spuContacts, 
+									bool boxbox = ((lsMem.gSpuContactManifoldAlgo.getShapeType0()==BOX_SHAPE_PROXYTYPE)&&
-										(ppu_address_t)lsMem.getColObj0()->getCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape,
+										(lsMem.gSpuContactManifoldAlgo.getShapeType1()==BOX_SHAPE_PROXYTYPE));
-										(ppu_address_t)lsMem.getColObj1()->getCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape);
+									if (boxbox && !gUseEpa)//for now use gUseEpa for this toggle
+									{
+										//getVmVector3
+										//getBtVector3
+										//getVmQuat
+										//getBtQuat
+										//getVmMatrix3
+
+										//getCollisionMargin0
+										btScalar margin0 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin0();
+										btScalar margin1 = lsMem.gSpuContactManifoldAlgo.getCollisionMargin1();
+										btVector3 shapeDim0 = lsMem.gSpuContactManifoldAlgo.getShapeDimensions0()+btVector3(margin0,margin0,margin0);
+										btVector3 shapeDim1 = lsMem.gSpuContactManifoldAlgo.getShapeDimensions1()+btVector3(margin1,margin1,margin1);
+
+										Box boxA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ());
+										Vector3 vmPos0 = getVmVector3(collisionPairInput.m_worldTransform0.getOrigin());
+										Vector3 vmPos1 = getVmVector3(collisionPairInput.m_worldTransform1.getOrigin());
+										Matrix3 vmMatrix0 = getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis());
+										Matrix3 vmMatrix1 = getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis());
+
+										Transform3 transformA(vmMatrix0,vmPos0);
+										Box boxB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ());
+										Transform3 transformB(vmMatrix1,vmPos1);
+										BoxPoint resultClosestBoxPointA;
+										BoxPoint resultClosestBoxPointB;
+										Vector3 resultNormal;
+										float distanceThreshold = gContactBreakingThreshold;//0.0f;//FLT_MAX;//use epsilon?	
+										float distance = boxBoxDistance(resultNormal,resultClosestBoxPointA,resultClosestBoxPointB,
+														   boxA, transformA, boxB,transformB,distanceThreshold);
+
+										
+										if(distance < distanceThreshold) 
+										{
+											//spu_printf("boxbox dist = %f\n",distance);
+											btPersistentManifold* spuManifold=&lsMem.gPersistentManifold;
+											btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr;
+											ppu_address_t manifoldAddress = (ppu_address_t)manifold;
+
+											//spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped);
+											spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(),
+												lsMem.getColObj1()->getWorldTransform(),
+												lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(),
+												lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(),
+												collisionPairInput.m_isSwapped);
+
+											btVector3 normalInB = -getBtVector3(resultNormal);
+
+											btVector3 pointOnB = collisionPairInput.m_worldTransform1(getBtVector3(resultClosestBoxPointB.localPoint));
+
+											//transform pointOnB to worldspace?
+
+											spuContacts.addContactPoint(
+												normalInB,
+												pointOnB,
+												distance);
+											//normalInB,
+											//pointOnB+positionOffset,
+											//distance);
+											//SET_CONTACT_POINT(cp[0],distance,-testNormal,
+											//	boxPointA,relTransformA,primIndexA,
+											//	boxPointB,relTransformB,primIndexB);
+											spuContacts.flush();
+										}
+
+									} else
+									{
+										handleCollisionPair(collisionPairInput, lsMem, spuContacts, 
+											(ppu_address_t)lsMem.getColObj0()->getCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape,
+											(ppu_address_t)lsMem.getColObj1()->getCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape);
+									}
 								}
 
 							}
@@ -981,5 +1052,6 @@ void	processCollisionTask(void* userPtr, void* lsMemPtr)
 	}//	for 
 
 
+
 	return;
 }

src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.h

@@ -0,0 +1,66 @@
+/*
+   Copyright (C) 2006, 2008 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+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 __BOXBOXDISTANCE_H__
+#define __BOXBOXDISTANCE_H__
+
+
+#include "Box.h"
+
+using namespace Vectormath::Aos;
+
+//---------------------------------------------------------------------------
+// boxBoxDistance:
+//
+// description:
+//    this computes info that can be used for the collision response of two boxes.  when the boxes
+//    do not overlap, the points are set to the closest points of the boxes, and a positive
+//    distance between them is returned.  if the boxes do overlap, a negative distance is returned
+//    and the points are set to two points that would touch after the boxes are translated apart.
+//    the contact normal gives the direction to repel or separate the boxes when they touch or
+//    overlap (it's being approximated here as one of the 15 "separating axis" directions).
+//
+// returns:
+//    positive or negative distance between two boxes.
+//
+// args:
+//    Vector3& normal: set to a unit contact normal pointing from box A to box B.
+//
+//    BoxPoint& boxPointA, BoxPoint& boxPointB:
+//       set to a closest point or point of penetration on each box.
+//
+//    Box boxA, Box boxB:
+//       boxes, represented as 3 half-widths
+//
+//    const Transform3& transformA, const Transform3& transformB:
+//       box transformations, in world coordinates
+//
+//    float distanceThreshold:
+//       the algorithm will exit early if it finds that the boxes are more distant than this
+//       threshold, and not compute a contact normal or points.  if this distance returned
+//       exceeds the threshold, all the other output data may not have been computed.  by
+//       default, this is set to MAX_FLOAT so it will have no effect.
+//
+//---------------------------------------------------------------------------
+
+float
+boxBoxDistance(Vector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,
+			   Box boxA, const Transform3 & transformA, Box boxB,
+			   const Transform3 & transformB,
+			   float distanceThreshold = FLT_MAX );
+
+#endif /* __BOXBOXDISTANCE_H__ */

src/BulletMultiThreaded/vectormath/scalar/cpp/quat_aos.h

@@ -0,0 +1,432 @@
+/*
+   Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+   All rights reserved.
+
+   Redistribution and use in source and binary forms,
+   with or without modification, are permitted provided that the
+   following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Sony Computer Entertainment Inc nor the names
+      of its contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+   POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef _VECTORMATH_QUAT_AOS_CPP_H
+#define _VECTORMATH_QUAT_AOS_CPP_H
+//-----------------------------------------------------------------------------
+// Definitions
+
+#ifndef _VECTORMATH_INTERNAL_FUNCTIONS
+#define _VECTORMATH_INTERNAL_FUNCTIONS
+
+#endif
+
+namespace Vectormath {
+namespace Aos {
+
+inline Quat::Quat( const Quat & quat )
+{
+    mX = quat.mX;
+    mY = quat.mY;
+    mZ = quat.mZ;
+    mW = quat.mW;
+}
+
+inline Quat::Quat( float _x, float _y, float _z, float _w )
+{
+    mX = _x;
+    mY = _y;
+    mZ = _z;
+    mW = _w;
+}
+
+inline Quat::Quat( const Vector3 & xyz, float _w )
+{
+    this->setXYZ( xyz );
+    this->setW( _w );
+}
+
+inline Quat::Quat( const Vector4 & vec )
+{
+    mX = vec.getX();
+    mY = vec.getY();
+    mZ = vec.getZ();
+    mW = vec.getW();
+}
+
+inline Quat::Quat( float scalar )
+{
+    mX = scalar;
+    mY = scalar;
+    mZ = scalar;
+    mW = scalar;
+}
+
+inline const Quat Quat::identity( )
+{
+    return Quat( 0.0f, 0.0f, 0.0f, 1.0f );
+}
+
+inline const Quat lerp( float t, const Quat & quat0, const Quat & quat1 )
+{
+    return ( quat0 + ( ( quat1 - quat0 ) * t ) );
+}
+
+inline const Quat slerp( float t, const Quat & unitQuat0, const Quat & unitQuat1 )
+{
+    Quat start;
+    float recipSinAngle, scale0, scale1, cosAngle, angle;
+    cosAngle = dot( unitQuat0, unitQuat1 );
+    if ( cosAngle < 0.0f ) {
+        cosAngle = -cosAngle;
+        start = ( -unitQuat0 );
+    } else {
+        start = unitQuat0;
+    }
+    if ( cosAngle < _VECTORMATH_SLERP_TOL ) {
+        angle = acosf( cosAngle );
+        recipSinAngle = ( 1.0f / sinf( angle ) );
+        scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
+        scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
+    } else {
+        scale0 = ( 1.0f - t );
+        scale1 = t;
+    }
+    return ( ( start * scale0 ) + ( unitQuat1 * scale1 ) );
+}
+
+inline const Quat squad( float t, const Quat & unitQuat0, const Quat & unitQuat1, const Quat & unitQuat2, const Quat & unitQuat3 )
+{
+    Quat tmp0, tmp1;
+    tmp0 = slerp( t, unitQuat0, unitQuat3 );
+    tmp1 = slerp( t, unitQuat1, unitQuat2 );
+    return slerp( ( ( 2.0f * t ) * ( 1.0f - t ) ), tmp0, tmp1 );
+}
+
+inline Quat & Quat::operator =( const Quat & quat )
+{
+    mX = quat.mX;
+    mY = quat.mY;
+    mZ = quat.mZ;
+    mW = quat.mW;
+    return *this;
+}
+
+inline Quat & Quat::setXYZ( const Vector3 & vec )
+{
+    mX = vec.getX();
+    mY = vec.getY();
+    mZ = vec.getZ();
+    return *this;
+}
+
+inline const Vector3 Quat::getXYZ( ) const
+{
+    return Vector3( mX, mY, mZ );
+}
+
+inline Quat & Quat::setX( float _x )
+{
+    mX = _x;
+    return *this;
+}
+
+inline float Quat::getX( ) const
+{
+    return mX;
+}
+
+inline Quat & Quat::setY( float _y )
+{
+    mY = _y;
+    return *this;
+}
+
+inline float Quat::getY( ) const
+{
+    return mY;
+}
+
+inline Quat & Quat::setZ( float _z )
+{
+    mZ = _z;
+    return *this;
+}
+
+inline float Quat::getZ( ) const
+{
+    return mZ;
+}
+
+inline Quat & Quat::setW( float _w )
+{
+    mW = _w;
+    return *this;
+}
+
+inline float Quat::getW( ) const
+{
+    return mW;
+}
+
+inline Quat & Quat::setElem( int idx, float value )
+{
+    *(&mX + idx) = value;
+    return *this;
+}
+
+inline float Quat::getElem( int idx ) const
+{
+    return *(&mX + idx);
+}
+
+inline float & Quat::operator []( int idx )
+{
+    return *(&mX + idx);
+}
+
+inline float Quat::operator []( int idx ) const
+{
+    return *(&mX + idx);
+}
+
+inline const Quat Quat::operator +( const Quat & quat ) const
+{
+    return Quat(
+        ( mX + quat.mX ),
+        ( mY + quat.mY ),
+        ( mZ + quat.mZ ),
+        ( mW + quat.mW )
+    );
+}
+
+inline const Quat Quat::operator -( const Quat & quat ) const
+{
+    return Quat(
+        ( mX - quat.mX ),
+        ( mY - quat.mY ),
+        ( mZ - quat.mZ ),
+        ( mW - quat.mW )
+    );
+}
+
+inline const Quat Quat::operator *( float scalar ) const
+{
+    return Quat(
+        ( mX * scalar ),
+        ( mY * scalar ),
+        ( mZ * scalar ),
+        ( mW * scalar )
+    );
+}
+
+inline Quat & Quat::operator +=( const Quat & quat )
+{
+    *this = *this + quat;
+    return *this;
+}
+
+inline Quat & Quat::operator -=( const Quat & quat )
+{
+    *this = *this - quat;
+    return *this;
+}
+
+inline Quat & Quat::operator *=( float scalar )
+{
+    *this = *this * scalar;
+    return *this;
+}
+
+inline const Quat Quat::operator /( float scalar ) const
+{
+    return Quat(
+        ( mX / scalar ),
+        ( mY / scalar ),
+        ( mZ / scalar ),
+        ( mW / scalar )
+    );
+}
+
+inline Quat & Quat::operator /=( float scalar )
+{
+    *this = *this / scalar;
+    return *this;
+}
+
+inline const Quat Quat::operator -( ) const
+{
+    return Quat(
+        -mX,
+        -mY,
+        -mZ,
+        -mW
+    );
+}
+
+inline const Quat operator *( float scalar, const Quat & quat )
+{
+    return quat * scalar;
+}
+
+inline float dot( const Quat & quat0, const Quat & quat1 )
+{
+    float result;
+    result = ( quat0.getX() * quat1.getX() );
+    result = ( result + ( quat0.getY() * quat1.getY() ) );
+    result = ( result + ( quat0.getZ() * quat1.getZ() ) );
+    result = ( result + ( quat0.getW() * quat1.getW() ) );
+    return result;
+}
+
+inline float norm( const Quat & quat )
+{
+    float result;
+    result = ( quat.getX() * quat.getX() );
+    result = ( result + ( quat.getY() * quat.getY() ) );
+    result = ( result + ( quat.getZ() * quat.getZ() ) );
+    result = ( result + ( quat.getW() * quat.getW() ) );
+    return result;
+}
+
+inline float length( const Quat & quat )
+{
+    return sqrtf( norm( quat ) );
+}
+
+inline const Quat normalize( const Quat & quat )
+{
+    float lenSqr, lenInv;
+    lenSqr = norm( quat );
+    lenInv = ( 1.0f / sqrtf( lenSqr ) );
+    return Quat(
+        ( quat.getX() * lenInv ),
+        ( quat.getY() * lenInv ),
+        ( quat.getZ() * lenInv ),
+        ( quat.getW() * lenInv )
+    );
+}
+
+inline const Quat Quat::rotation( const Vector3 & unitVec0, const Vector3 & unitVec1 )
+{
+    float cosHalfAngleX2, recipCosHalfAngleX2;
+    cosHalfAngleX2 = sqrtf( ( 2.0f * ( 1.0f + dot( unitVec0, unitVec1 ) ) ) );
+    recipCosHalfAngleX2 = ( 1.0f / cosHalfAngleX2 );
+    return Quat( ( cross( unitVec0, unitVec1 ) * recipCosHalfAngleX2 ), ( cosHalfAngleX2 * 0.5f ) );
+}
+
+inline const Quat Quat::rotation( float radians, const Vector3 & unitVec )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( ( unitVec * s ), c );
+}
+
+inline const Quat Quat::rotationX( float radians )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( s, 0.0f, 0.0f, c );
+}
+
+inline const Quat Quat::rotationY( float radians )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( 0.0f, s, 0.0f, c );
+}
+
+inline const Quat Quat::rotationZ( float radians )
+{
+    float s, c, angle;
+    angle = ( radians * 0.5f );
+    s = sinf( angle );
+    c = cosf( angle );
+    return Quat( 0.0f, 0.0f, s, c );
+}
+
+inline const Quat Quat::operator *( const Quat & quat ) const
+{
+    return Quat(
+        ( ( ( ( mW * quat.mX ) + ( mX * quat.mW ) ) + ( mY * quat.mZ ) ) - ( mZ * quat.mY ) ),
+        ( ( ( ( mW * quat.mY ) + ( mY * quat.mW ) ) + ( mZ * quat.mX ) ) - ( mX * quat.mZ ) ),
+        ( ( ( ( mW * quat.mZ ) + ( mZ * quat.mW ) ) + ( mX * quat.mY ) ) - ( mY * quat.mX ) ),
+        ( ( ( ( mW * quat.mW ) - ( mX * quat.mX ) ) - ( mY * quat.mY ) ) - ( mZ * quat.mZ ) )
+    );
+}
+
+inline Quat & Quat::operator *=( const Quat & quat )
+{
+    *this = *this * quat;
+    return *this;
+}
+
+inline const Vector3 rotate( const Quat & quat, const Vector3 & vec )
+{
+    float tmpX, tmpY, tmpZ, tmpW;
+    tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
+    tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
+    tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
+    tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
+    return Vector3(
+        ( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
+        ( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
+        ( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
+    );
+}
+
+inline const Quat conj( const Quat & quat )
+{
+    return Quat( -quat.getX(), -quat.getY(), -quat.getZ(), quat.getW() );
+}
+
+inline const Quat select( const Quat & quat0, const Quat & quat1, bool select1 )
+{
+    return Quat(
+        ( select1 )? quat1.getX() : quat0.getX(),
+        ( select1 )? quat1.getY() : quat0.getY(),
+        ( select1 )? quat1.getZ() : quat0.getZ(),
+        ( select1 )? quat1.getW() : quat0.getW()
+    );
+}
+
+#ifdef _VECTORMATH_DEBUG
+
+inline void print( const Quat & quat )
+{
+    printf( "( %f %f %f %f )\n", quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
+}
+
+inline void print( const Quat & quat, const char * name )
+{
+    printf( "%s: ( %f %f %f %f )\n", name, quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
+}
+
+#endif
+
+} // namespace Aos
+} // namespace Vectormath
+
+#endif

src/LinearMath/btTransformUtil.h

@@ -140,5 +140,71 @@ public:
 
 };
 
+
+///The btConvexSeparatingDistanceUtil can help speed up convex collision detection 
+///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
+class	btConvexSeparatingDistanceUtil
+{
+	btTransform	m_cachedTransformA;
+	btTransform m_cachedTransformB;
+
+	btScalar	m_boundingRadiusA;
+	btScalar	m_boundingRadiusB;
+
+	btVector3	m_separatingNormal;
+	btScalar	m_separatingDistance;
+
+public:
+
+	btConvexSeparatingDistanceUtil(btScalar	boundingRadiusA,btScalar	boundingRadiusB)
+		:m_boundingRadiusA(boundingRadiusA),
+		m_boundingRadiusB(boundingRadiusB),
+		m_separatingDistance(0.f)
+	{
+	}
+
+	btScalar	getConservativeSeparatingDistance()
+	{
+		return m_separatingDistance;
+	}
+
+	void	updateSeparatingDistance(const btTransform& transA,const btTransform& transB)
+	{
+		if (m_separatingDistance>0.f)
+		{
+			const btTransform& fromA = m_cachedTransformA;
+			const btTransform& fromB = m_cachedTransformB;
+			const btTransform& toA = transA;
+			const btTransform& toB = transB;
+			btVector3 linVelA,angVelA,linVelB,angVelB;
+			btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
+			btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
+			btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
+			btVector3 relLinVel = (linVelB-linVelA);
+			btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal);
+			if (relLinVelocLength<0.f)
+			{
+				relLinVelocLength = 0.f;
+			}
+	
+			btScalar	projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
+			m_separatingDistance -= projectedMotion;
+		}
+	
+		m_cachedTransformA = transA;
+		m_cachedTransformB = transB;
+	}
+
+	void	initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
+	{
+		m_separatingNormal = separatingVector;
+		m_separatingDistance = separatingDistance;
+		m_cachedTransformA = transA;
+		m_cachedTransformB = transB;
+	}
+
+};
+
+
 #endif //SIMD_TRANSFORM_UTIL_H