add bullet

libs/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp

@@ -0,0 +1,236 @@
+/*
+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 "btContinuousConvexCollision.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "btGjkPairDetector.h"
+#include "btPointCollector.h"
+
+
+
+btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape*	convexA,const btConvexShape*	convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
+:m_simplexSolver(simplexSolver),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
+/// You don't want your game ever to lock-up.
+#define MAX_ITERATIONS 64
+
+bool	btContinuousConvexCollision::calcTimeOfImpact(
+				const btTransform& fromA,
+				const btTransform& toA,
+				const btTransform& fromB,
+				const btTransform& toB,
+				CastResult& result)
+{
+
+	m_simplexSolver->reset();
+
+	/// compute linear and angular velocity for this interval, to interpolate
+	btVector3 linVelA,angVelA,linVelB,angVelB;
+	btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
+	btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
+
+
+	btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
+	btScalar boundingRadiusB = m_convexB->getAngularMotionDisc();
+
+	btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
+	btVector3 relLinVel = (linVelB-linVelA);
+
+	btScalar relLinVelocLength = (linVelB-linVelA).length();
+	
+	if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
+		return false;
+
+
+	btScalar radius = btScalar(0.001);
+
+	btScalar lambda = btScalar(0.);
+	btVector3 v(1,0,0);
+
+	int maxIter = MAX_ITERATIONS;
+
+	btVector3 n;
+	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	bool hasResult = false;
+	btVector3 c;
+
+	btScalar lastLambda = lambda;
+	//btScalar epsilon = btScalar(0.001);
+
+	int numIter = 0;
+	//first solution, using GJK
+
+
+	btTransform identityTrans;
+	identityTrans.setIdentity();
+
+	btSphereShape	raySphere(btScalar(0.0));
+	raySphere.setMargin(btScalar(0.));
+
+
+//	result.drawCoordSystem(sphereTr);
+
+	btPointCollector	pointCollector1;
+
+	{
+		
+		btGjkPairDetector gjk(m_convexA,m_convexB,m_convexA->getShapeType(),m_convexB->getShapeType(),m_convexA->getMargin(),m_convexB->getMargin(),m_simplexSolver,m_penetrationDepthSolver);		
+		btGjkPairDetector::ClosestPointInput input;
+	
+		//we don't use margins during CCD
+	//	gjk.setIgnoreMargin(true);
+
+		input.m_transformA = fromA;
+		input.m_transformB = fromB;
+		gjk.getClosestPoints(input,pointCollector1,0);
+
+		hasResult = pointCollector1.m_hasResult;
+		c = pointCollector1.m_pointInWorld;
+	}
+
+	if (hasResult)
+	{
+		btScalar dist;
+		dist = pointCollector1.m_distance;
+		n = pointCollector1.m_normalOnBInWorld;
+
+		btScalar projectedLinearVelocity = relLinVel.dot(n);
+		
+		//not close enough
+		while (dist > radius)
+		{
+			if (result.m_debugDrawer)
+			{
+				result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
+			}
+			numIter++;
+			if (numIter > maxIter)
+			{
+				return false; //todo: report a failure
+			}
+			btScalar dLambda = btScalar(0.);
+
+			projectedLinearVelocity = relLinVel.dot(n);
+
+			//calculate safe moving fraction from distance / (linear+rotational velocity)
+			
+			//btScalar clippedDist  = GEN_min(angularConservativeRadius,dist);
+			//btScalar clippedDist  = dist;
+			
+			//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+			if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
+				return false;
+			
+			dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
+
+			
+			
+			lambda = lambda + dLambda;
+
+			if (lambda > btScalar(1.))
+				return false;
+
+			if (lambda < btScalar(0.))
+				return false;
+
+
+			//todo: next check with relative epsilon
+			if (lambda <= lastLambda)
+			{
+				return false;
+				//n.setValue(0,0,0);
+				break;
+			}
+			lastLambda = lambda;
+
+			
+
+			//interpolate to next lambda
+			btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
+
+			btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
+			btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
+			relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
+
+			if (result.m_debugDrawer)
+			{
+				result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0));
+			}
+
+			result.DebugDraw( lambda );
+
+			btPointCollector	pointCollector;
+			btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
+			btGjkPairDetector::ClosestPointInput input;
+			input.m_transformA = interpolatedTransA;
+			input.m_transformB = interpolatedTransB;
+			gjk.getClosestPoints(input,pointCollector,0);
+			if (pointCollector.m_hasResult)
+			{
+				if (pointCollector.m_distance < btScalar(0.))
+				{
+					//degenerate ?!
+					result.m_fraction = lastLambda;
+					n = pointCollector.m_normalOnBInWorld;
+					result.m_normal=n;//.setValue(1,1,1);// = n;
+					result.m_hitPoint = pointCollector.m_pointInWorld;
+					return true;
+				}
+				c = pointCollector.m_pointInWorld;		
+				n = pointCollector.m_normalOnBInWorld;
+				dist = pointCollector.m_distance;
+			} else
+			{
+				//??
+				return false;
+			}
+			
+
+		}
+	
+		if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
+			return false;
+			
+		result.m_fraction = lambda;
+		result.m_normal = n;
+		result.m_hitPoint = c;
+		return true;
+	}
+
+	return false;
+
+/*
+//todo:
+	//if movement away from normal, discard result
+	btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
+	if (result.m_fraction < btScalar(1.))
+	{
+		if (move.dot(result.m_normal) <= btScalar(0.))
+		{
+		}
+	}
+*/
+
+}

libs/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h

@@ -0,0 +1,52 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef CONTINUOUS_COLLISION_CONVEX_CAST_H
+#define CONTINUOUS_COLLISION_CONVEX_CAST_H
+
+#include "btConvexCast.h"
+#include "btSimplexSolverInterface.h"
+class btConvexPenetrationDepthSolver;
+class btConvexShape;
+
+/// btContinuousConvexCollision implements angular and linear time of impact for convex objects.
+/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
+/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
+/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
+class btContinuousConvexCollision : public btConvexCast
+{
+	btSimplexSolverInterface* m_simplexSolver;
+	btConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	const btConvexShape*	m_convexA;
+	const btConvexShape*	m_convexB;
+
+
+public:
+
+	btContinuousConvexCollision (const btConvexShape*	shapeA,const btConvexShape*	shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
+
+	virtual bool	calcTimeOfImpact(
+				const btTransform& fromA,
+				const btTransform& toA,
+				const btTransform& fromB,
+				const btTransform& toB,
+				CastResult& result);
+
+
+};
+
+#endif //CONTINUOUS_COLLISION_CONVEX_CAST_H
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp

@@ -0,0 +1,20 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btConvexCast.h"
+
+btConvexCast::~btConvexCast()
+{
+}

libs/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.h

@@ -0,0 +1,73 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef CONVEX_CAST_H
+#define CONVEX_CAST_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btScalar.h"
+class btMinkowskiSumShape;
+#include "LinearMath/btIDebugDraw.h"
+
+/// btConvexCast is an interface for Casting
+class btConvexCast
+{
+public:
+
+
+	virtual ~btConvexCast();
+
+	///RayResult stores the closest result
+	/// alternatively, add a callback method to decide about closest/all results
+	struct	CastResult
+	{
+		//virtual bool	addRayResult(const btVector3& normal,btScalar	fraction) = 0;
+				
+		virtual void	DebugDraw(btScalar	fraction) {(void)fraction;}
+		virtual void	drawCoordSystem(const btTransform& trans) {(void)trans;}
+
+		CastResult()
+			:m_fraction(btScalar(BT_LARGE_FLOAT)),
+			m_debugDrawer(0),
+			m_allowedPenetration(btScalar(0))
+		{
+		}
+
+
+		virtual ~CastResult() {};
+
+		btTransform	m_hitTransformA;
+		btTransform	m_hitTransformB;
+		btVector3	m_normal;
+		btVector3   m_hitPoint;
+		btScalar	m_fraction; //input and output
+		btIDebugDraw* m_debugDrawer;
+		btScalar	m_allowedPenetration;
+
+	};
+
+
+	/// cast a convex against another convex object
+	virtual bool	calcTimeOfImpact(
+					const btTransform& fromA,
+					const btTransform& toA,
+					const btTransform& fromB,
+					const btTransform& toB,
+					CastResult& result) = 0;
+};
+
+#endif //CONVEX_CAST_H

libs/bullet/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h

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

libs/bullet/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h

@@ -0,0 +1,89 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
+#define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+class btStackAlloc;
+
+/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
+/// This interface allows to query for closest points and penetration depth between two (convex) objects
+/// the closest point is on the second object (B), and the normal points from the surface on B towards A.
+/// distance is between closest points on B and closest point on A. So you can calculate closest point on A
+/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
+struct btDiscreteCollisionDetectorInterface
+{
+	
+	struct Result
+	{
+	
+		virtual ~Result(){}	
+
+		///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner
+		virtual void setShapeIdentifiersA(int partId0,int index0)=0;
+		virtual void setShapeIdentifiersB(int partId1,int index1)=0;
+		virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0;
+	};
+
+	struct ClosestPointInput
+	{
+		ClosestPointInput()
+			:m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)),
+			m_stackAlloc(0)
+		{
+		}
+
+		btTransform m_transformA;
+		btTransform m_transformB;
+		btScalar	m_maximumDistanceSquared;
+		btStackAlloc* m_stackAlloc;
+	};
+
+	virtual ~btDiscreteCollisionDetectorInterface() {};
+
+	//
+	// give either closest points (distance > 0) or penetration (distance)
+	// the normal always points from B towards A
+	//
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false) = 0;
+
+};
+
+struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
+{
+		btVector3	m_normalOnSurfaceB;
+		btVector3	m_closestPointInB;
+		btScalar	m_distance; //negative means penetration !
+
+		btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT))
+		{
+
+		}
+		virtual ~btStorageResult() {};
+
+		virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+		{
+			if (depth < m_distance)
+			{
+				m_normalOnSurfaceB = normalOnBInWorld;
+				m_closestPointInB = pointInWorld;
+				m_distance = depth;
+			}
+		}
+};
+
+#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE1_H

libs/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp

@@ -0,0 +1,176 @@
+/*
+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 "btGjkConvexCast.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "btGjkPairDetector.h"
+#include "btPointCollector.h"
+#include "LinearMath/btTransformUtil.h"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#else
+#define MAX_ITERATIONS 32
+#endif
+
+btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),
+m_convexB(convexB)
+{
+}
+
+bool	btGjkConvexCast::calcTimeOfImpact(
+					const btTransform& fromA,
+					const btTransform& toA,
+					const btTransform& fromB,
+					const btTransform& toB,
+					CastResult& result)
+{
+
+
+	m_simplexSolver->reset();
+
+	/// compute linear velocity for this interval, to interpolate
+	//assume no rotation/angular velocity, assert here?
+	btVector3 linVelA,linVelB;
+	linVelA = toA.getOrigin()-fromA.getOrigin();
+	linVelB = toB.getOrigin()-fromB.getOrigin();
+
+	btScalar radius = btScalar(0.001);
+	btScalar lambda = btScalar(0.);
+	btVector3 v(1,0,0);
+
+	int maxIter = MAX_ITERATIONS;
+
+	btVector3 n;
+	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	bool hasResult = false;
+	btVector3 c;
+	btVector3 r = (linVelA-linVelB);
+
+	btScalar lastLambda = lambda;
+	//btScalar epsilon = btScalar(0.001);
+
+	int numIter = 0;
+	//first solution, using GJK
+
+
+	btTransform identityTrans;
+	identityTrans.setIdentity();
+
+
+//	result.drawCoordSystem(sphereTr);
+
+	btPointCollector	pointCollector;
+
+		
+	btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);		
+	btGjkPairDetector::ClosestPointInput input;
+
+	//we don't use margins during CCD
+	//	gjk.setIgnoreMargin(true);
+
+	input.m_transformA = fromA;
+	input.m_transformB = fromB;
+	gjk.getClosestPoints(input,pointCollector,0);
+
+	hasResult = pointCollector.m_hasResult;
+	c = pointCollector.m_pointInWorld;
+
+	if (hasResult)
+	{
+		btScalar dist;
+		dist = pointCollector.m_distance;
+		n = pointCollector.m_normalOnBInWorld;
+
+	
+
+		//not close enough
+		while (dist > radius)
+		{
+			numIter++;
+			if (numIter > maxIter)
+			{
+				return false; //todo: report a failure
+			}
+			btScalar dLambda = btScalar(0.);
+
+			btScalar projectedLinearVelocity = r.dot(n);
+			
+			dLambda = dist / (projectedLinearVelocity);
+
+			lambda = lambda - dLambda;
+
+			if (lambda > btScalar(1.))
+				return false;
+
+			if (lambda < btScalar(0.))
+				return false;
+
+			//todo: next check with relative epsilon
+			if (lambda <= lastLambda)
+			{
+				return false;
+				//n.setValue(0,0,0);
+				break;
+			}
+			lastLambda = lambda;
+
+			//interpolate to next lambda
+			result.DebugDraw( lambda );
+			input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
+			input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
+			
+			gjk.getClosestPoints(input,pointCollector,0);
+			if (pointCollector.m_hasResult)
+			{
+				if (pointCollector.m_distance < btScalar(0.))
+				{
+					result.m_fraction = lastLambda;
+					n = pointCollector.m_normalOnBInWorld;
+					result.m_normal=n;
+					result.m_hitPoint = pointCollector.m_pointInWorld;
+					return true;
+				}
+				c = pointCollector.m_pointInWorld;		
+				n = pointCollector.m_normalOnBInWorld;
+				dist = pointCollector.m_distance;
+			} else
+			{
+				//??
+				return false;
+			}
+
+		}
+
+		//is n normalized?
+		//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+		if (n.dot(r)>=-result.m_allowedPenetration)
+			return false;
+
+		result.m_fraction = lambda;
+		result.m_normal = n;
+		result.m_hitPoint = c;
+		return true;
+	}
+
+	return false;
+
+
+}
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h

@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GJK_CONVEX_CAST_H
+#define GJK_CONVEX_CAST_H
+
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+#include "LinearMath/btVector3.h"
+#include "btConvexCast.h"
+class btConvexShape;
+class btMinkowskiSumShape;
+#include "btSimplexSolverInterface.h"
+
+///GjkConvexCast performs a raycast on a convex object using support mapping.
+class btGjkConvexCast : public btConvexCast
+{
+	btSimplexSolverInterface*	m_simplexSolver;
+	const btConvexShape*	m_convexA;
+	const btConvexShape*	m_convexB;
+
+public:
+
+	btGjkConvexCast(const btConvexShape*	convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);
+
+	/// cast a convex against another convex object
+	virtual bool	calcTimeOfImpact(
+					const btTransform& fromA,
+					const btTransform& toA,
+					const btTransform& fromB,
+					const btTransform& toB,
+					CastResult& result);
+
+};
+
+#endif //GJK_CONVEX_CAST_H

libs/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp

@@ -0,0 +1,989 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 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.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson, 2008
+*/
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "btGjkEpa2.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_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;
+
+	// MinkowskiDiff
+	struct	MinkowskiDiff
+	{
+		const btConvexShape*	m_shapes[2];
+		btMatrix3x3				m_toshape1;
+		btTransform				m_toshape0;
+#ifdef __SPU__
+		bool					m_enableMargin;
+#else
+		btVector3				(btConvexShape::*Ls)(const btVector3&) const;
+#endif//__SPU__
+		
+
+		MinkowskiDiff()
+		{
+
+		}
+#ifdef __SPU__
+			void					EnableMargin(bool enable)
+		{
+			m_enableMargin = enable;
+		}	
+		inline btVector3		Support0(const btVector3& d) const
+		{
+			if (m_enableMargin)
+			{
+				return m_shapes[0]->localGetSupportVertexNonVirtual(d);
+			} else
+			{
+				return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
+			}
+		}
+		inline btVector3		Support1(const btVector3& d) const
+		{
+			if (m_enableMargin)
+			{
+				return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
+			} else
+			{
+				return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
+			}
+		}
+#else
+		void					EnableMargin(bool enable)
+		{
+			if(enable)
+				Ls=&btConvexShape::localGetSupportVertexNonVirtual;
+			else
+				Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
+		}	
+		inline btVector3		Support0(const btVector3& d) const
+		{
+			return(((m_shapes[0])->*(Ls))(d));
+		}
+		inline btVector3		Support1(const btVector3& d) const
+		{
+			return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
+		}
+#endif //__SPU__
+
+		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=btDot(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;
+				default:
+					{
+					}
+				}	
+				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;
+							const btVector3	p=btCross(d,axis);
+							if(p.length2()>0)
+							{
+								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=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
+							m_simplex->c[2]->w-m_simplex->c[0]->w);
+						if(n.length2()>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?-btDot(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=btCross(dl[0],dl[1]);
+				const btScalar		l=n.length2();
+				if(l>GJK_SIMPLEX3_EPS)
+				{
+					btScalar	mindist=-1;
+					btScalar	subw[2]={0.f,0.f};
+					U			subm(0);
+					for(U i=0;i<3;++i)
+					{
+						if(btDot(*vt[i],btCross(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			=	static_cast<U>(((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=btDot(a,n);	
+						const btScalar	s=btSqrt(l);
+						const btVector3	p=n*(d/l);
+						mindist	=	p.length2();
+						m		=	7;
+						w[0]	=	(btCross(dl[1],b-p)).length()/s;
+						w[1]	=	(btCross(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*btDot(a,btCross(b-c,a-b)))<=0;
+				if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
+				{
+					btScalar	mindist=-1;
+					btScalar	subw[3]={0.f,0.f,0.f};
+					U			subm(0);
+					for(U i=0;i<3;++i)
+					{
+						const U			j=imd3[i];
+						const btScalar	s=vl*btDot(d,btCross(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			=	static_cast<U>((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();	
+			}
+
+
+			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;
+			}
+
+
+			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=btDot(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]	=	btCross(	outer.c[1]->w-projection,
+							outer.c[2]->w-projection).length();
+						m_result.p[1]	=	btCross(	outer.c[2]->w-projection,
+							outer.c[0]->w-projection).length();
+						m_result.p[2]	=	btCross(	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		=	btCross(b->w-a->w,c->w-a->w);
+					const btScalar	l=face->n.length();
+					const bool		v=l>EPA_ACCURACY;
+					face->p		=	btMin(btMin(
+						btDot(a->w,btCross(face->n,a->w-b->w)),
+						btDot(b->w,btCross(face->n,b->w-c->w))),
+						btDot(c->w,btCross(face->n,c->w-a->w)))	/
+						(v?l:1);
+					face->p		=	face->p>=-EPA_INSIDE_EPS?0:face->p;
+					if(v)
+					{
+						face->d		=	btDot(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((btDot(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 void	Initialize(	const btConvexShape* shape0,const btTransform& wtrs0,
+		const btConvexShape* shape1,const btTransform& wtrs1,
+		btGjkEpaSolver2::sResults& results,
+		tShape& shape,
+		bool withmargins)
+	{
+		/* Results		*/ 
+		results.witnesses[0]	=
+			results.witnesses[1]	=	btVector3(0,0,0);
+		results.status			=	btGjkEpaSolver2::sResults::Separated;
+		/* Shape		*/ 
+		shape.m_shapes[0]		=	shape0;
+		shape.m_shapes[1]		=	shape1;
+		shape.m_toshape1		=	wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
+		shape.m_toshape0		=	wtrs0.inverseTimes(wtrs1);
+		shape.EnableMargin(withmargins);
+	}
+
+}
+
+//
+// Api
+//
+
+using namespace	gjkepa2_impl;
+
+//
+int			btGjkEpaSolver2::StackSizeRequirement()
+{
+	return(sizeof(GJK)+sizeof(EPA));
+}
+
+//
+bool		btGjkEpaSolver2::Distance(	const btConvexShape*	shape0,
+									  const btTransform&		wtrs0,
+									  const btConvexShape*	shape1,
+									  const btTransform&		wtrs1,
+									  const btVector3&		guess,
+									  sResults&				results)
+{
+	tShape			shape;
+	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
+	GJK				gjk;
+	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,guess);
+	if(gjk_status==GJK::eStatus::Valid)
+	{
+		btVector3	w0=btVector3(0,0,0);
+		btVector3	w1=btVector3(0,0,0);
+		for(U i=0;i<gjk.m_simplex->rank;++i)
+		{
+			const btScalar	p=gjk.m_simplex->p[i];
+			w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
+			w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
+		}
+		results.witnesses[0]	=	wtrs0*w0;
+		results.witnesses[1]	=	wtrs0*w1;
+		results.normal			=	w0-w1;
+		results.distance		=	results.normal.length();
+		results.normal			/=	results.distance>GJK_MIN_DISTANCE?results.distance:1;
+		return(true);
+	}
+	else
+	{
+		results.status	=	gjk_status==GJK::eStatus::Inside?
+			sResults::Penetrating	:
+		sResults::GJK_Failed	;
+		return(false);
+	}
+}
+
+//
+bool	btGjkEpaSolver2::Penetration(	const btConvexShape*	shape0,
+									 const btTransform&		wtrs0,
+									 const btConvexShape*	shape1,
+									 const btTransform&		wtrs1,
+									 const btVector3&		guess,
+									 sResults&				results,
+									 bool					usemargins)
+{
+	tShape			shape;
+	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
+	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);
+				results.normal			=	-epa.m_normal;
+				results.distance		=	-epa.m_depth;
+				return(true);
+			} else results.status=sResults::EPA_Failed;
+		}
+		break;
+	case	GJK::eStatus::Failed:
+		results.status=sResults::GJK_Failed;
+		break;
+		default:
+					{
+					}
+	}
+	return(false);
+}
+
+#ifndef __SPU__
+//
+btScalar	btGjkEpaSolver2::SignedDistance(const btVector3& position,
+											btScalar margin,
+											const btConvexShape* shape0,
+											const btTransform& wtrs0,
+											sResults& results)
+{
+	tShape			shape;
+	btSphereShape	shape1(margin);
+	btTransform		wtrs1(btQuaternion(0,0,0,1),position);
+	Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
+	GJK				gjk;	
+	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
+	if(gjk_status==GJK::eStatus::Valid)
+	{
+		btVector3	w0=btVector3(0,0,0);
+		btVector3	w1=btVector3(0,0,0);
+		for(U i=0;i<gjk.m_simplex->rank;++i)
+		{
+			const btScalar	p=gjk.m_simplex->p[i];
+			w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
+			w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
+		}
+		results.witnesses[0]	=	wtrs0*w0;
+		results.witnesses[1]	=	wtrs0*w1;
+		const btVector3	delta=	results.witnesses[1]-
+			results.witnesses[0];
+		const btScalar	margin=	shape0->getMarginNonVirtual()+
+			shape1.getMarginNonVirtual();
+		const btScalar	length=	delta.length();	
+		results.normal			=	delta/length;
+		results.witnesses[0]	+=	results.normal*margin;
+		return(length-margin);
+	}
+	else
+	{
+		if(gjk_status==GJK::eStatus::Inside)
+		{
+			if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
+			{
+				const btVector3	delta=	results.witnesses[0]-
+					results.witnesses[1];
+				const btScalar	length=	delta.length();
+				if (length >= SIMD_EPSILON)
+					results.normal	=	delta/length;			
+				return(-length);
+			}
+		}	
+	}
+	return(SIMD_INFINITY);
+}
+
+//
+bool	btGjkEpaSolver2::SignedDistance(const btConvexShape*	shape0,
+										const btTransform&		wtrs0,
+										const btConvexShape*	shape1,
+										const btTransform&		wtrs1,
+										const btVector3&		guess,
+										sResults&				results)
+{
+	if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
+		return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
+	else
+		return(true);
+}
+#endif //__SPU__
+
+/* 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

libs/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h

@@ -0,0 +1,73 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 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.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson, 2008
+*/
+#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	btGjkEpaSolver2
+{
+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;
+	btScalar	distance;
+	};
+
+static int		StackSizeRequirement();
+
+static bool		Distance(	const btConvexShape* shape0,const btTransform& wtrs0,
+							const btConvexShape* shape1,const btTransform& wtrs1,
+							const btVector3& guess,
+							sResults& results);
+
+static bool		Penetration(const btConvexShape* shape0,const btTransform& wtrs0,
+							const btConvexShape* shape1,const btTransform& wtrs1,
+							const btVector3& guess,
+							sResults& results,
+							bool usemargins=true);
+#ifndef __SPU__
+static btScalar	SignedDistance(	const btVector3& position,
+								btScalar margin,
+								const btConvexShape* shape,
+								const btTransform& wtrs,
+								sResults& results);
+							
+static bool		SignedDistance(	const btConvexShape* shape0,const btTransform& wtrs0,
+								const btConvexShape* shape1,const btTransform& wtrs1,
+								const btVector3& guess,
+								sResults& results);
+#endif //__SPU__
+
+};
+
+#endif

libs/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp

@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "btGjkEpaPenetrationDepthSolver.h"
+
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+
+bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
+											  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
+											  const btTransform& transformA, const btTransform& transformB,
+											  btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
+											  class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc )
+{
+
+	(void)debugDraw;
+	(void)v;
+	(void)simplexSolver;
+
+//	const btScalar				radialmargin(btScalar(0.));
+	
+	btVector3	guessVector(transformA.getOrigin()-transformB.getOrigin());
+	btGjkEpaSolver2::sResults	results;
+	
+
+	if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
+								pConvexB,transformB,
+								guessVector,results))
+	
+		{
+	//	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+		//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+		wWitnessOnA = results.witnesses[0];
+		wWitnessOnB = results.witnesses[1];
+		v = results.normal;
+		return true;		
+		} else
+	{
+		if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results))
+		{
+			wWitnessOnA = results.witnesses[0];
+			wWitnessOnB = results.witnesses[1];
+			v = results.normal;
+			return false;
+		}
+	}
+
+	return false;
+}
+
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h

@@ -0,0 +1,43 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H
+#define BT_GJP_EPA_PENETRATION_DEPTH_H
+
+#include "btConvexPenetrationDepthSolver.h"
+
+///EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
+///calculate the penetration depth between two convex shapes.
+class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
+{
+	public :
+
+		btGjkEpaPenetrationDepthSolver()
+		{
+		}
+
+		bool			calcPenDepth( btSimplexSolverInterface& simplexSolver,
+									  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
+									  const btTransform& transformA, const btTransform& transformB,
+									  btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
+									  class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc );
+
+	private :
+
+};
+
+#endif	// BT_GJP_EPA_PENETRATION_DEPTH_H
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -0,0 +1,456 @@
+/*
+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 "btGjkPairDetector.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+
+
+
+#if defined(DEBUG) || defined (_DEBUG)
+//#define TEST_NON_VIRTUAL 1
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+//#define DEBUG_SPU_COLLISION_DETECTION 1
+#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 gNumDeepPenetrationChecks = 0;
+int gNumGjkChecks = 0;
+
+
+btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
+:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_shapeTypeA(objectA->getShapeType()),
+m_shapeTypeB(objectB->getShapeType()),
+m_marginA(objectA->getMargin()),
+m_marginB(objectB->getMargin()),
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(1)
+{
+}
+btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
+:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
+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	btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+{
+	(void)swapResults;
+
+	getClosestPointsNonVirtual(input,output,debugDraw);
+}
+
+#ifdef __SPU__
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+#else
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+#endif
+{
+	m_cachedSeparatingDistance = 0.f;
+
+	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;
+
+	bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
+
+	btScalar marginA = m_marginA;
+	btScalar marginB = m_marginB;
+
+	gNumGjkChecks++;
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+	spu_printf("inside gjk\n");
+#endif
+	//for CCD we don't use margins
+	if (m_ignoreMargin)
+	{
+		marginA = btScalar(0.);
+		marginB = btScalar(0.);
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("ignoring margin\n");
+#endif
+	}
+
+	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 = BT_LARGE_FLOAT;
+		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();
+
+#if 1
+
+			btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+			btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(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);
+
+#else
+#ifdef __SPU__
+			btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+			btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+#else
+			btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
+			btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
+#ifdef TEST_NON_VIRTUAL
+			btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
+			btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
+			btAssert((pInAv-pInA).length() < 0.0001);
+			btAssert((qInBv-qInB).length() < 0.0001);
+#endif //
+#endif //__SPU__
+#endif
+
+
+			btVector3  pWorld = localTransA(pInA);	
+			btVector3  qWorld = localTransB(qInB);
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("got local supporting vertices\n");
+#endif
+
+			if (check2d)
+			{
+				pWorld[2] = 0.f;
+				qWorld[2] = 0.f;
+			}
+
+			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)) 
+			{
+				m_degenerateSimplex = 10;
+				checkSimplex=true;
+				//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;
+				} else
+				{
+					m_degenerateSimplex = 11;
+				}
+				checkSimplex = true;
+				break;
+			}
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("addVertex 1\n");
+#endif
+			//add current vertex to simplex
+			m_simplexSolver->addVertex(w, pWorld, qWorld);
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("addVertex 2\n");
+#endif
+			btVector3 newCachedSeparatingAxis;
+
+			//calculate the closest point to the origin (update vector v)
+			if (!m_simplexSolver->closest(newCachedSeparatingAxis))
+			{
+				m_degenerateSimplex = 3;
+				checkSimplex = true;
+				break;
+			}
+
+			if(newCachedSeparatingAxis.length2()<REL_ERROR2)
+            {
+				m_cachedSeparatingAxis = newCachedSeparatingAxis;
+                m_degenerateSimplex = 6;
+                checkSimplex = true;
+                break;
+            }
+
+			btScalar previousSquaredDistance = squaredDistance;
+			squaredDistance = newCachedSeparatingAxis.length2();
+#if 0
+///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
+			if (squaredDistance>previousSquaredDistance)
+			{
+				m_degenerateSimplex = 7;
+				squaredDistance = previousSquaredDistance;
+                checkSimplex = false;
+                break;
+			}
+#endif //
+			
+			m_cachedSeparatingAxis = newCachedSeparatingAxis;
+
+			//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;
+				m_degenerateSimplex = 12;
+				
+				break;
+			}
+
+			  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
+              if (m_curIter++ > gGjkMaxIter)   
+              {   
+                      #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
+
+                              printf("btGjkPairDetector 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_minkowskiA->getShapeType(),   
+                              m_minkowskiB->getShapeType());   
+
+                      #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);
+				m_degenerateSimplex = 13;
+				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;
+				
+				gNumDeepPenetrationChecks++;
+				m_cachedSeparatingAxis.setZero();
+
+				bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
+					*m_simplexSolver, 
+					m_minkowskiA,m_minkowskiB,
+					localTransA,localTransB,
+					m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
+					debugDraw,input.m_stackAlloc
+					);
+
+
+				if (isValid2)
+				{
+					btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
+					btScalar lenSqr = tmpNormalInB.length2();
+					if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+					{
+						tmpNormalInB = m_cachedSeparatingAxis;
+						lenSqr = m_cachedSeparatingAxis.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
+						{
+							m_lastUsedMethod = 8;
+						}
+					} else
+					{
+						m_lastUsedMethod = 9;
+					}
+				} else
+
+				{
+					///this is another degenerate case, where the initial GJK calculation reports a degenerate case
+					///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
+					///reports a valid positive distance. Use the results of the second GJK instead of failing.
+					///thanks to Jacob.Langford for the reproduction case
+					///http://code.google.com/p/bullet/issues/detail?id=250
+
+				
+					if (m_cachedSeparatingAxis.length2() > btScalar(0.))
+					{
+						btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
+						//only replace valid distances when the distance is less
+						if (!isValid || (distance2 < distance))
+						{
+							distance = distance2;
+							pointOnA = tmpPointOnA;
+							pointOnB = tmpPointOnB;
+							pointOnA -= m_cachedSeparatingAxis * marginA ;
+							pointOnB += m_cachedSeparatingAxis * marginB ;
+							normalInB = m_cachedSeparatingAxis;
+							normalInB.normalize();
+							isValid = true;
+							m_lastUsedMethod = 6;
+						} else
+						{
+							m_lastUsedMethod = 5;
+						}
+					}
+				}
+				
+			}
+
+		}
+	}
+
+	
+
+	if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
+	{
+#if 0
+///some debugging
+//		if (check2d)
+		{
+			printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
+			printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
+		}
+#endif 
+
+		m_cachedSeparatingAxis = normalInB;
+		m_cachedSeparatingDistance = distance;
+
+		output.addContactPoint(
+			normalInB,
+			pointOnB+positionOffset,
+			distance);
+
+	}
+
+
+}
+
+
+
+
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h

@@ -0,0 +1,103 @@
+/*
+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 GJK_PAIR_DETECTOR_H
+#define GJK_PAIR_DETECTOR_H
+
+#include "btDiscreteCollisionDetectorInterface.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+class btConvexShape;
+#include "btSimplexSolverInterface.h"
+class btConvexPenetrationDepthSolver;
+
+/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
+class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
+{
+	
+
+	btVector3	m_cachedSeparatingAxis;
+	btConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	btSimplexSolverInterface* m_simplexSolver;
+	const btConvexShape* m_minkowskiA;
+	const btConvexShape* m_minkowskiB;
+	int	m_shapeTypeA;
+	int m_shapeTypeB;
+	btScalar	m_marginA;
+	btScalar	m_marginB;
+
+	bool		m_ignoreMargin;
+	btScalar	m_cachedSeparatingDistance;
+	
+
+public:
+
+	//some debugging to fix degeneracy problems
+	int			m_lastUsedMethod;
+	int			m_curIter;
+	int			m_degenerateSimplex;
+	int			m_catchDegeneracies;
+
+
+	btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
+	btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
+	virtual ~btGjkPairDetector() {};
+
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+
+	void	getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);
+	
+
+	void setMinkowskiA(btConvexShape* minkA)
+	{
+		m_minkowskiA = minkA;
+	}
+
+	void setMinkowskiB(btConvexShape* minkB)
+	{
+		m_minkowskiB = minkB;
+	}
+	void setCachedSeperatingAxis(const btVector3& seperatingAxis)
+	{
+		m_cachedSeparatingAxis = seperatingAxis;
+	}
+
+	const btVector3& getCachedSeparatingAxis() const
+	{
+		return m_cachedSeparatingAxis;
+	}
+	btScalar	getCachedSeparatingDistance() const
+	{
+		return m_cachedSeparatingDistance;
+	}
+
+	void	setPenetrationDepthSolver(btConvexPenetrationDepthSolver*	penetrationDepthSolver)
+	{
+		m_penetrationDepthSolver = penetrationDepthSolver;
+	}
+
+	///don't use setIgnoreMargin, it's for Bullet's internal use
+	void	setIgnoreMargin(bool ignoreMargin)
+	{
+		m_ignoreMargin = ignoreMargin;
+	}
+
+
+};
+
+#endif //GJK_PAIR_DETECTOR_H

libs/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -0,0 +1,153 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MANIFOLD_CONTACT_POINT_H
+#define MANIFOLD_CONTACT_POINT_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransformUtil.h"
+
+// Don't change following order of parameters
+ATTRIBUTE_ALIGNED16(struct) PfxConstraintRow {
+	btScalar mNormal[3];
+	btScalar mRhs;
+	btScalar mJacDiagInv;
+	btScalar mLowerLimit;
+	btScalar mUpperLimit;
+	btScalar mAccumImpulse;
+};
+
+
+
+
+/// ManifoldContactPoint collects and maintains persistent contactpoints.
+/// used to improve stability and performance of rigidbody dynamics response.
+class btManifoldPoint
+	{
+		public:
+			btManifoldPoint()
+				:m_userPersistentData(0),
+				m_appliedImpulse(0.f),
+				m_lateralFrictionInitialized(false),
+				m_appliedImpulseLateral1(0.f),
+				m_appliedImpulseLateral2(0.f),
+				m_contactMotion1(0.f),
+				m_contactMotion2(0.f),
+				m_contactCFM1(0.f),
+				m_contactCFM2(0.f),
+				m_lifeTime(0)
+			{
+			}
+
+			btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB, 
+					const btVector3 &normal, 
+					btScalar distance ) :
+					m_localPointA( pointA ), 
+					m_localPointB( pointB ), 
+					m_normalWorldOnB( normal ), 
+					m_distance1( distance ),
+					m_combinedFriction(btScalar(0.)),
+					m_combinedRestitution(btScalar(0.)),
+					m_userPersistentData(0),
+					m_appliedImpulse(0.f),
+					m_lateralFrictionInitialized(false),
+					m_appliedImpulseLateral1(0.f),
+					m_appliedImpulseLateral2(0.f),
+					m_contactMotion1(0.f),
+					m_contactMotion2(0.f),
+					m_contactCFM1(0.f),
+					m_contactCFM2(0.f),
+					m_lifeTime(0)
+			{
+				mConstraintRow[0].mAccumImpulse = 0.f;
+				mConstraintRow[1].mAccumImpulse = 0.f;
+				mConstraintRow[2].mAccumImpulse = 0.f;
+			}
+
+			
+
+			btVector3 m_localPointA;			
+			btVector3 m_localPointB;			
+			btVector3	m_positionWorldOnB;
+			///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
+			btVector3	m_positionWorldOnA;
+			btVector3 m_normalWorldOnB;
+		
+			btScalar	m_distance1;
+			btScalar	m_combinedFriction;
+			btScalar	m_combinedRestitution;
+
+         //BP mod, store contact triangles.
+         int	   m_partId0;
+         int      m_partId1;
+         int      m_index0;
+         int      m_index1;
+				
+			mutable void*	m_userPersistentData;
+			btScalar		m_appliedImpulse;
+
+			bool			m_lateralFrictionInitialized;
+			btScalar		m_appliedImpulseLateral1;
+			btScalar		m_appliedImpulseLateral2;
+			btScalar		m_contactMotion1;
+			btScalar		m_contactMotion2;
+			btScalar		m_contactCFM1;
+			btScalar		m_contactCFM2;
+
+			int				m_lifeTime;//lifetime of the contactpoint in frames
+			
+			btVector3		m_lateralFrictionDir1;
+			btVector3		m_lateralFrictionDir2;
+
+
+
+			PfxConstraintRow mConstraintRow[3];
+
+
+			btScalar getDistance() const
+			{
+				return m_distance1;
+			}
+			int	getLifeTime() const
+			{
+				return m_lifeTime;
+			}
+
+			const btVector3& getPositionWorldOnA() const {
+				return m_positionWorldOnA;
+//				return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
+			}
+
+			const btVector3& getPositionWorldOnB() const
+			{
+				return m_positionWorldOnB;
+			}
+
+			void	setDistance(btScalar dist)
+			{
+				m_distance1 = dist;
+			}
+			
+			///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver
+			btScalar	getAppliedImpulse() const
+			{
+				return m_appliedImpulse;
+			}
+
+			
+
+	};
+
+#endif //MANIFOLD_CONTACT_POINT_H

libs/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp

@@ -0,0 +1,362 @@
+/*
+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 "btMinkowskiPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+#define NUM_UNITSPHERE_POINTS 42
+
+
+bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
+												   const btConvexShape* convexA,const btConvexShape* convexB,
+												   const btTransform& transA,const btTransform& transB,
+												   btVector3& v, btVector3& pa, btVector3& pb,
+												   class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
+												   )
+{
+
+	(void)stackAlloc;
+	(void)v;
+	
+	bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
+
+	struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
+	{
+
+		btIntermediateResult():m_hasResult(false)
+		{
+		}
+		
+		btVector3 m_normalOnBInWorld;
+		btVector3 m_pointInWorld;
+		btScalar m_depth;
+		bool	m_hasResult;
+
+		virtual void setShapeIdentifiersA(int partId0,int index0)
+		{
+			(void)partId0;
+			(void)index0;
+		}
+		virtual void setShapeIdentifiersB(int partId1,int index1)
+		{
+			(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(BT_LARGE_FLOAT);
+	btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
+	btVector3 minA,minB;
+	btVector3 seperatingAxisInA,seperatingAxisInB;
+	btVector3 pInA,qInB,pWorld,qWorld,w;
+
+#ifndef __SPU__
+#define USE_BATCHED_SUPPORT 1
+#endif
+#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++)
+	{
+		btVector3 norm = getPenetrationDirections()[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;
+				getPenetrationDirections()[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;
+				getPenetrationDirections()[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++)
+	{
+		btVector3 norm = getPenetrationDirections()[i];
+		if (check2d)
+		{
+			norm[2] = 0.f;
+		}
+		if (norm.length2()>0.01)
+		{
+
+			seperatingAxisInA = seperatingAxisInABatch[i];
+			seperatingAxisInB = seperatingAxisInBBatch[i];
+
+			pInA = supportVerticesABatch[i];
+			qInB = supportVerticesBBatch[i];
+
+			pWorld = transA(pInA);	
+			qWorld = transB(qInB);
+			if (check2d)
+			{
+				pWorld[2] = 0.f;
+				qWorld[2] = 0.f;
+			}
+
+			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;
+
+#ifndef __SPU__
+	{
+		int numPDA = convexA->getNumPreferredPenetrationDirections();
+		if (numPDA)
+		{
+			for (int i=0;i<numPDA;i++)
+			{
+				btVector3 norm;
+				convexA->getPreferredPenetrationDirection(i,norm);
+				norm  = transA.getBasis() * norm;
+				getPenetrationDirections()[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+
+	{
+		int numPDB = convexB->getNumPreferredPenetrationDirections();
+		if (numPDB)
+		{
+			for (int i=0;i<numPDB;i++)
+			{
+				btVector3 norm;
+				convexB->getPreferredPenetrationDirection(i,norm);
+				norm  = transB.getBasis() * norm;
+				getPenetrationDirections()[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+#endif // __SPU__
+
+	for (int i=0;i<numSampleDirections;i++)
+	{
+		const btVector3& norm = getPenetrationDirections()[i];
+		seperatingAxisInA = (-norm)* transA.getBasis();
+		seperatingAxisInB = norm* transB.getBasis();
+		pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+		qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(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*convexA->getMarginNonVirtual();
+	minB -= minNorm*convexB->getMarginNonVirtual();
+	//no penetration
+	if (minProj < btScalar(0.))
+		return false;
+
+	btScalar extraSeparation = 0.5f;///scale dependent
+	minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
+
+
+
+
+
+//#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
+
+	
+
+	btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
+
+	btScalar offsetDist = minProj;
+	btVector3 offset = minNorm * offsetDist;
+	
+
+
+	btGjkPairDetector::ClosestPointInput input;
+		
+	btVector3 newOrg = transA.getOrigin() + offset;
+
+	btTransform displacedTrans = transA;
+	displacedTrans.setOrigin(newOrg);
+
+	input.m_transformA = displacedTrans;
+	input.m_transformB = transB;
+	input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
+	
+	btIntermediateResult res;
+	gjkdet.setCachedSeperatingAxis(-minNorm);
+	gjkdet.getClosestPoints(input,res,debugDraw);
+
+	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;
+		v = minNorm;
+		
+#ifdef DEBUG_DRAW
+		if (debugDraw)
+		{
+			btVector3 color(1,0,0);
+			debugDraw->drawLine(pa,pb,color);
+		}
+#endif//DEBUG_DRAW
+
+
+	}
+	return res.m_hasResult;
+}
+
+btVector3*	btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
+{
+	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))
+	};
+
+	return sPenetrationDirections;
+}
+
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h

@@ -0,0 +1,40 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
+#include "btConvexPenetrationDepthSolver.h"
+
+///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 btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
+{
+protected:
+
+	static btVector3*	getPenetrationDirections();
+
+public:
+
+	virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
+	const btConvexShape* convexA,const btConvexShape* convexB,
+				const btTransform& transA,const btTransform& transB,
+			btVector3& v, btVector3& pa, btVector3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
+			);
+};
+
+#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

@@ -0,0 +1,260 @@
+/*
+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 "btPersistentManifold.h"
+#include "LinearMath/btTransform.h"
+
+
+btScalar					gContactBreakingThreshold = btScalar(0.02);
+ContactDestroyedCallback	gContactDestroyedCallback = 0;
+ContactProcessedCallback	gContactProcessedCallback = 0;
+
+
+
+btPersistentManifold::btPersistentManifold()
+:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
+m_body0(0),
+m_body1(0),
+m_cachedPoints (0),
+m_index1a(0)
+{
+}
+
+
+
+
+#ifdef DEBUG_PERSISTENCY
+#include <stdio.h>
+void	btPersistentManifold::DebugPersistency()
+{
+	int i;
+	printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
+	for (i=0;i<m_cachedPoints;i++)
+	{
+		printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
+	}
+}
+#endif //DEBUG_PERSISTENCY
+
+void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
+{
+
+	void* oldPtr = pt.m_userPersistentData;
+	if (oldPtr)
+	{
+#ifdef DEBUG_PERSISTENCY
+		int i;
+		int occurance = 0;
+		for (i=0;i<m_cachedPoints;i++)
+		{
+			if (m_pointCache[i].m_userPersistentData == oldPtr)
+			{
+				occurance++;
+				if (occurance>1)
+					printf("error in clearUserCache\n");
+			}
+		}
+		btAssert(occurance<=0);
+#endif //DEBUG_PERSISTENCY
+
+		if (pt.m_userPersistentData && gContactDestroyedCallback)
+		{
+			(*gContactDestroyedCallback)(pt.m_userPersistentData);
+			pt.m_userPersistentData = 0;
+		}
+		
+#ifdef DEBUG_PERSISTENCY
+		DebugPersistency();
+#endif
+	}
+
+	
+}
+
+
+int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) 
+{
+
+		//calculate 4 possible cases areas, and take biggest area
+		//also need to keep 'deepest'
+		
+		int maxPenetrationIndex = -1;
+#define KEEP_DEEPEST_POINT 1
+#ifdef KEEP_DEEPEST_POINT
+		btScalar maxPenetration = pt.getDistance();
+		for (int i=0;i<4;i++)
+		{
+			if (m_pointCache[i].getDistance() < maxPenetration)
+			{
+				maxPenetrationIndex = i;
+				maxPenetration = m_pointCache[i].getDistance();
+			}
+		}
+#endif //KEEP_DEEPEST_POINT
+		
+		btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
+		if (maxPenetrationIndex != 0)
+		{
+			btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
+			btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+			btVector3 cross = a0.cross(b0);
+			res0 = cross.length2();
+		}
+		if (maxPenetrationIndex != 1)
+		{
+			btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+			btVector3 cross = a1.cross(b1);
+			res1 = cross.length2();
+		}
+
+		if (maxPenetrationIndex != 2)
+		{
+			btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
+			btVector3 cross = a2.cross(b2);
+			res2 = cross.length2();
+		}
+
+		if (maxPenetrationIndex != 3)
+		{
+			btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
+			btVector3 cross = a3.cross(b3);
+			res3 = cross.length2();
+		}
+
+		btVector4 maxvec(res0,res1,res2,res3);
+		int biggestarea = maxvec.closestAxis4();
+		return biggestarea;
+}
+
+
+int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
+{
+	btScalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
+	int size = getNumContacts();
+	int nearestPoint = -1;
+	for( int i = 0; i < size; i++ )
+	{
+		const btManifoldPoint &mp = m_pointCache[i];
+
+		btVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
+		const btScalar distToManiPoint = diffA.dot(diffA);
+		if( distToManiPoint < shortestDist )
+		{
+			shortestDist = distToManiPoint;
+			nearestPoint = i;
+		}
+	}
+	return nearestPoint;
+}
+
+int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint)
+{
+	btAssert(validContactDistance(newPoint));
+
+	int insertIndex = getNumContacts();
+	if (insertIndex == MANIFOLD_CACHE_SIZE)
+	{
+#if MANIFOLD_CACHE_SIZE >= 4
+		//sort cache so best points come first, based on area
+		insertIndex = sortCachedPoints(newPoint);
+#else
+		insertIndex = 0;
+#endif
+		clearUserCache(m_pointCache[insertIndex]);
+		
+	} else
+	{
+		m_cachedPoints++;
+
+		
+	}
+	if (insertIndex<0)
+		insertIndex=0;
+
+	btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
+	m_pointCache[insertIndex] = newPoint;
+	return insertIndex;
+}
+
+btScalar	btPersistentManifold::getContactBreakingThreshold() const
+{
+	return m_contactBreakingThreshold;
+}
+
+
+
+void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
+{
+	int i;
+#ifdef DEBUG_PERSISTENCY
+	printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
+		trA.getOrigin().getX(),
+		trA.getOrigin().getY(),
+		trA.getOrigin().getZ(),
+		trB.getOrigin().getX(),
+		trB.getOrigin().getY(),
+		trB.getOrigin().getZ());
+#endif //DEBUG_PERSISTENCY
+	/// first refresh worldspace positions and distance
+	for (i=getNumContacts()-1;i>=0;i--)
+	{
+		btManifoldPoint &manifoldPoint = m_pointCache[i];
+		manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
+		manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
+		manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
+		manifoldPoint.m_lifeTime++;
+	}
+
+	/// then 
+	btScalar distance2d;
+	btVector3 projectedDifference,projectedPoint;
+	for (i=getNumContacts()-1;i>=0;i--)
+	{
+		
+		btManifoldPoint &manifoldPoint = m_pointCache[i];
+		//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
+		if (!validContactDistance(manifoldPoint))
+		{
+			removeContactPoint(i);
+		} else
+		{
+			//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
+			projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
+			projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
+			distance2d = projectedDifference.dot(projectedDifference);
+			if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
+			{
+				removeContactPoint(i);
+			} else
+			{
+				//contact point processed callback
+				if (gContactProcessedCallback)
+					(*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
+			}
+		}
+	}
+#ifdef DEBUG_PERSISTENCY
+	DebugPersistency();
+#endif //
+}
+
+
+
+
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -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.
+*/
+
+#ifndef PERSISTENT_MANIFOLD_H
+#define PERSISTENT_MANIFOLD_H
+
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "btManifoldPoint.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+struct btCollisionResult;
+
+///maximum contact breaking and merging threshold
+extern btScalar gContactBreakingThreshold;
+
+typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
+typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1);
+extern ContactDestroyedCallback	gContactDestroyedCallback;
+extern ContactProcessedCallback gContactProcessedCallback;
+
+
+enum btContactManifoldTypes
+{
+	BT_PERSISTENT_MANIFOLD_TYPE = 1,
+	MAX_CONTACT_MANIFOLD_TYPE
+};
+
+#define MANIFOLD_CACHE_SIZE 4
+
+///btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase.
+///Those contact points are created by the collision narrow phase.
+///The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time.
+///updates/refreshes old contact points, and throw them away if necessary (distance becomes too large)
+///reduces the cache to 4 points, when more then 4 points are added, using following rules:
+///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points
+///note that some pairs of objects might have more then one contact manifold.
+
+
+ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject
+//ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject
+{
+
+	btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
+
+	/// this two body pointers can point to the physics rigidbody class.
+	/// void* will allow any rigidbody class
+	void* m_body0;
+	void* m_body1;
+
+	int	m_cachedPoints;
+
+	btScalar	m_contactBreakingThreshold;
+	btScalar	m_contactProcessingThreshold;
+
+	
+	/// sort cached points so most isolated points come first
+	int	sortCachedPoints(const btManifoldPoint& pt);
+
+	int		findContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt);
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	int	m_companionIdA;
+	int	m_companionIdB;
+
+	int m_index1a;
+
+	btPersistentManifold();
+
+	btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold)
+		: btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
+	m_body0(body0),m_body1(body1),m_cachedPoints(0),
+		m_contactBreakingThreshold(contactBreakingThreshold),
+		m_contactProcessingThreshold(contactProcessingThreshold)
+	{
+	}
+
+	SIMD_FORCE_INLINE void* getBody0() { return m_body0;}
+	SIMD_FORCE_INLINE void* getBody1() { return m_body1;}
+
+	SIMD_FORCE_INLINE const void* getBody0() const { return m_body0;}
+	SIMD_FORCE_INLINE const void* getBody1() const { return m_body1;}
+
+	void	setBodies(void* body0,void* body1)
+	{
+		m_body0 = body0;
+		m_body1 = body1;
+	}
+
+	void clearUserCache(btManifoldPoint& pt);
+
+#ifdef DEBUG_PERSISTENCY
+	void	DebugPersistency();
+#endif //
+	
+	SIMD_FORCE_INLINE int	getNumContacts() const { return m_cachedPoints;}
+
+	SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const
+	{
+		btAssert(index < m_cachedPoints);
+		return m_pointCache[index];
+	}
+
+	SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index)
+	{
+		btAssert(index < m_cachedPoints);
+		return m_pointCache[index];
+	}
+
+	///@todo: get this margin from the current physics / collision environment
+	btScalar	getContactBreakingThreshold() const;
+
+	btScalar	getContactProcessingThreshold() const
+	{
+		return m_contactProcessingThreshold;
+	}
+	
+	int getCacheEntry(const btManifoldPoint& newPoint) const;
+
+	int addManifoldPoint( const btManifoldPoint& newPoint);
+
+	void removeContactPoint (int index)
+	{
+		clearUserCache(m_pointCache[index]);
+
+		int lastUsedIndex = getNumContacts() - 1;
+//		m_pointCache[index] = m_pointCache[lastUsedIndex];
+		if(index != lastUsedIndex) 
+		{
+			m_pointCache[index] = m_pointCache[lastUsedIndex]; 
+			//get rid of duplicated userPersistentData pointer
+			m_pointCache[lastUsedIndex].m_userPersistentData = 0;
+			m_pointCache[lastUsedIndex].mConstraintRow[0].mAccumImpulse = 0.f;
+			m_pointCache[lastUsedIndex].mConstraintRow[1].mAccumImpulse = 0.f;
+			m_pointCache[lastUsedIndex].mConstraintRow[2].mAccumImpulse = 0.f;
+
+			m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
+			m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
+			m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
+			m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
+			m_pointCache[lastUsedIndex].m_lifeTime = 0;
+		}
+
+		btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0);
+		m_cachedPoints--;
+	}
+	void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
+	{
+		btAssert(validContactDistance(newPoint));
+
+#define MAINTAIN_PERSISTENCY 1
+#ifdef MAINTAIN_PERSISTENCY
+		int	lifeTime = m_pointCache[insertIndex].getLifeTime();
+		btScalar	appliedImpulse = m_pointCache[insertIndex].mConstraintRow[0].mAccumImpulse;
+		btScalar	appliedLateralImpulse1 = m_pointCache[insertIndex].mConstraintRow[1].mAccumImpulse;
+		btScalar	appliedLateralImpulse2 = m_pointCache[insertIndex].mConstraintRow[2].mAccumImpulse;
+//		bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized;
+		
+		
+			
+		btAssert(lifeTime>=0);
+		void* cache = m_pointCache[insertIndex].m_userPersistentData;
+		
+		m_pointCache[insertIndex] = newPoint;
+
+		m_pointCache[insertIndex].m_userPersistentData = cache;
+		m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
+		m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
+		m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
+		
+		m_pointCache[insertIndex].mConstraintRow[0].mAccumImpulse =  appliedImpulse;
+		m_pointCache[insertIndex].mConstraintRow[1].mAccumImpulse = appliedLateralImpulse1;
+		m_pointCache[insertIndex].mConstraintRow[2].mAccumImpulse = appliedLateralImpulse2;
+
+
+		m_pointCache[insertIndex].m_lifeTime = lifeTime;
+#else
+		clearUserCache(m_pointCache[insertIndex]);
+		m_pointCache[insertIndex] = newPoint;
+	
+#endif
+	}
+
+	bool validContactDistance(const btManifoldPoint& pt) const
+	{
+		return pt.m_distance1 <= getContactBreakingThreshold();
+	}
+	/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
+	void	refreshContactPoints(  const btTransform& trA,const btTransform& trB);
+
+	
+	SIMD_FORCE_INLINE	void	clearManifold()
+	{
+		int i;
+		for (i=0;i<m_cachedPoints;i++)
+		{
+			clearUserCache(m_pointCache[i]);
+		}
+		m_cachedPoints = 0;
+	}
+
+
+
+}
+;
+
+
+
+
+
+#endif //PERSISTENT_MANIFOLD_H

libs/bullet/BulletCollision/NarrowPhaseCollision/btPointCollector.h

@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef POINT_COLLECTOR_H
+#define POINT_COLLECTOR_H
+
+#include "btDiscreteCollisionDetectorInterface.h"
+
+
+
+struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
+{
+	
+	
+	btVector3 m_normalOnBInWorld;
+	btVector3 m_pointInWorld;
+	btScalar	m_distance;//negative means penetration
+
+	bool	m_hasResult;
+
+	btPointCollector () 
+		: m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false)
+	{
+	}
+
+	virtual void setShapeIdentifiersA(int partId0,int index0)
+	{
+		(void)partId0;
+		(void)index0;
+			
+	}
+	virtual void setShapeIdentifiersB(int partId1,int index1)
+	{
+		(void)partId1;
+		(void)index1;
+	}
+
+	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+	{
+		if (depth< m_distance)
+		{
+			m_hasResult = true;
+			m_normalOnBInWorld = normalOnBInWorld;
+			m_pointInWorld = pointInWorld;
+			//negative means penetration
+			m_distance = depth;
+		}
+	}
+};
+
+#endif //POINT_COLLECTOR_H
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp

@@ -0,0 +1,175 @@
+/*
+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 <stdio.h>
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "btRaycastCallback.h"
+
+btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags)
+	:
+	m_from(from),
+	m_to(to),
+   //@BP Mod
+   m_flags(flags),
+	m_hitFraction(btScalar(1.))
+{
+
+}
+
+
+
+void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+{
+	const btVector3 &vert0=triangle[0];
+	const btVector3 &vert1=triangle[1];
+	const btVector3 &vert2=triangle[2];
+
+	btVector3 v10; v10 = vert1 - vert0 ;
+	btVector3 v20; v20 = vert2 - vert0 ;
+
+	btVector3 triangleNormal; triangleNormal = v10.cross( v20 );
+	
+	const btScalar dist = vert0.dot(triangleNormal);
+	btScalar dist_a = triangleNormal.dot(m_from) ;
+	dist_a-= dist;
+	btScalar dist_b = triangleNormal.dot(m_to);
+	dist_b -= dist;
+
+	if ( dist_a * dist_b >= btScalar(0.0) )
+	{
+		return ; // same sign
+	}
+   //@BP Mod - Backface filtering
+   if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a > btScalar(0.0)))
+   {
+      // Backface, skip check
+      return;
+   }
+	
+	const btScalar proj_length=dist_a-dist_b;
+	const btScalar distance = (dist_a)/(proj_length);
+	// Now we have the intersection point on the plane, we'll see if it's inside the triangle
+	// Add an epsilon as a tolerance for the raycast,
+	// in case the ray hits exacly on the edge of the triangle.
+	// It must be scaled for the triangle size.
+	
+	if(distance < m_hitFraction)
+	{
+		
+
+		btScalar edge_tolerance =triangleNormal.length2();		
+		edge_tolerance *= btScalar(-0.0001);
+		btVector3 point; point.setInterpolate3( m_from, m_to, distance);
+		{
+			btVector3 v0p; v0p = vert0 - point;
+			btVector3 v1p; v1p = vert1 - point;
+			btVector3 cp0; cp0 = v0p.cross( v1p );
+
+			if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) 
+			{
+						
+
+				btVector3 v2p; v2p = vert2 -  point;
+				btVector3 cp1;
+				cp1 = v1p.cross( v2p);
+				if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) 
+				{
+					btVector3 cp2;
+					cp2 = v2p.cross(v0p);
+					
+					if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) 
+					{
+                  //@BP Mod
+                  // Triangle normal isn't normalized
+				      triangleNormal.normalize();
+
+                  //@BP Mod - Allow for unflipped normal when raycasting against backfaces
+                  if (((m_flags & kF_KeepUnflippedNormal) != 0) || (dist_a <= btScalar(0.0)))
+						{
+							m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
+						}
+						else
+						{
+                     m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+
+btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin)
+{
+	m_convexShape = convexShape;
+	m_convexShapeFrom = convexShapeFrom;
+	m_convexShapeTo = convexShapeTo;
+	m_triangleToWorld = triangleToWorld;
+	m_hitFraction = 1.0;
+    m_triangleCollisionMargin = triangleCollisionMargin;
+}
+
+void
+btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex)
+{
+	btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]);
+    triangleShape.setMargin(m_triangleCollisionMargin);
+
+	btVoronoiSimplexSolver	simplexSolver;
+	btGjkEpaPenetrationDepthSolver	gjkEpaPenetrationSolver;
+
+//#define  USE_SUBSIMPLEX_CONVEX_CAST 1
+//if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+	btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver);
+#else
+	//btGjkConvexCast	convexCaster(m_convexShape,&triangleShape,&simplexSolver);
+	btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver);
+#endif //#USE_SUBSIMPLEX_CONVEX_CAST
+	
+	btConvexCast::CastResult castResult;
+	castResult.m_fraction = btScalar(1.);
+	if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult))
+	{
+		//add hit
+		if (castResult.m_normal.length2() > btScalar(0.0001))
+		{					
+			if (castResult.m_fraction < m_hitFraction)
+			{
+/* btContinuousConvexCast's normal is already in world space */
+/*
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+				//rotate normal into worldspace
+				castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal;
+#endif //USE_SUBSIMPLEX_CONVEX_CAST
+*/
+				castResult.m_normal.normalize();
+
+				reportHit (castResult.m_normal,
+							castResult.m_hitPoint,
+							castResult.m_fraction,
+							partId,
+							triangleIndex);
+			}
+		}
+	}
+}

libs/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h

@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef RAYCAST_TRI_CALLBACK_H
+#define RAYCAST_TRI_CALLBACK_H
+
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "LinearMath/btTransform.h"
+struct btBroadphaseProxy;
+class btConvexShape;
+
+class  btTriangleRaycastCallback: public btTriangleCallback
+{
+public:
+
+	//input
+	btVector3 m_from;
+	btVector3 m_to;
+
+   //@BP Mod - allow backface filtering and unflipped normals
+   enum EFlags
+   {
+      kF_None                 = 0,
+      kF_FilterBackfaces      = 1 << 0,
+      kF_KeepUnflippedNormal  = 1 << 1,   // Prevents returned face normal getting flipped when a ray hits a back-facing triangle
+
+      kF_Terminator        = 0xFFFFFFFF
+   };
+   unsigned int m_flags;
+
+	btScalar	m_hitFraction;
+
+	btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0);
+	
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+
+	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0;
+	
+};
+
+class btTriangleConvexcastCallback : public btTriangleCallback
+{
+public:
+	const btConvexShape* m_convexShape;
+	btTransform m_convexShapeFrom;
+	btTransform m_convexShapeTo;
+	btTransform m_triangleToWorld;
+	btScalar m_hitFraction;
+    btScalar m_triangleCollisionMargin;
+
+	btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin);
+
+	virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex);
+
+	virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
+};
+
+#endif //RAYCAST_TRI_CALLBACK_H
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h

@@ -0,0 +1,63 @@
+/*
+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 SIMPLEX_SOLVER_INTERFACE_H
+#define SIMPLEX_SOLVER_INTERFACE_H
+
+#include "LinearMath/btVector3.h"
+
+#define NO_VIRTUAL_INTERFACE 1
+#ifdef NO_VIRTUAL_INTERFACE
+#include "btVoronoiSimplexSolver.h"
+#define btSimplexSolverInterface btVoronoiSimplexSolver
+#else
+
+/// btSimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
+/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
+/// voronoi regions or barycentric coordinates
+class btSimplexSolverInterface
+{
+	public:
+		virtual ~btSimplexSolverInterface() {};
+
+	virtual void reset() = 0;
+
+	virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0;
+	
+	virtual bool closest(btVector3& v) = 0;
+
+	virtual btScalar maxVertex() = 0;
+
+	virtual bool fullSimplex() const = 0;
+
+	virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0;
+
+	virtual bool inSimplex(const btVector3& w) = 0;
+	
+	virtual void backup_closest(btVector3& v) = 0;
+
+	virtual bool emptySimplex() const = 0;
+
+	virtual void compute_points(btVector3& p1, btVector3& p2) = 0;
+
+	virtual int numVertices() const =0;
+
+
+};
+#endif
+#endif //SIMPLEX_SOLVER_INTERFACE_H
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp

@@ -0,0 +1,160 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btSubSimplexConvexCast.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "btPointCollector.h"
+#include "LinearMath/btTransformUtil.h"
+
+btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+///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
+bool	btSubsimplexConvexCast::calcTimeOfImpact(
+		const btTransform& fromA,
+		const btTransform& toA,
+		const btTransform& fromB,
+		const btTransform& toB,
+		CastResult& result)
+{
+
+	m_simplexSolver->reset();
+
+	btVector3 linVelA,linVelB;
+	linVelA = toA.getOrigin()-fromA.getOrigin();
+	linVelB = toB.getOrigin()-fromB.getOrigin();
+
+	btScalar lambda = btScalar(0.);
+
+	btTransform interpolatedTransA = fromA;
+	btTransform interpolatedTransB = fromB;
+
+	///take relative motion
+	btVector3 r = (linVelA-linVelB);
+	btVector3 v;
+	
+	btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
+	btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
+	v = supVertexA-supVertexB;
+	int maxIter = MAX_ITERATIONS;
+
+	btVector3 n;
+	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	bool hasResult = false;
+	btVector3 c;
+
+	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--)
+	{
+		supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
+		supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
+		w = supVertexA-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;
+				//interpolate to next lambda
+				//	x = s + lambda * r;
+				interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
+				interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
+				//m_simplexSolver->reset();
+				//check next line
+				 w = supVertexA-supVertexB;
+				lastLambda = lambda;
+				n = v;
+				hasResult = true;
+			}
+		} 
+		///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
+		if (!m_simplexSolver->inSimplex(w))
+			m_simplexSolver->addVertex( w, supVertexA , supVertexB);
+
+		if (m_simplexSolver->closest(v))
+		{
+			dist2 = v.length2();
+			hasResult = true;
+			//todo: check this normal for validity
+			//n=v;
+			//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.);
+		} 
+	}
+
+	//int numiter = MAX_ITERATIONS - maxIter;
+//	printf("number of iterations: %d", numiter);
+	
+	//don't report a time of impact when moving 'away' from the hitnormal
+	
+
+	result.m_fraction = lambda;
+	if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON))
+		result.m_normal = n.normalized();
+	else
+		result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+
+	//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+	if (result.m_normal.dot(r)>=-result.m_allowedPenetration)
+		return false;
+
+	btVector3 hitA,hitB;
+	m_simplexSolver->compute_points(hitA,hitB);
+	result.m_hitPoint=hitB;
+	return true;
+}
+
+
+
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h

@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef SUBSIMPLEX_CONVEX_CAST_H
+#define SUBSIMPLEX_CONVEX_CAST_H
+
+#include "btConvexCast.h"
+#include "btSimplexSolverInterface.h"
+class btConvexShape;
+
+/// 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 btSubsimplexConvexCast : public btConvexCast
+{
+	btSimplexSolverInterface* m_simplexSolver;
+	const btConvexShape*	m_convexA;
+	const btConvexShape*	m_convexB;
+
+public:
+
+	btSubsimplexConvexCast (const btConvexShape*	shapeA,const btConvexShape*	shapeB,btSimplexSolverInterface* 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.
+	virtual bool	calcTimeOfImpact(
+			const btTransform& fromA,
+			const btTransform& toA,
+			const btTransform& fromB,
+			const btTransform& toB,
+			CastResult& result);
+
+};
+
+#endif //SUBSIMPLEX_CONVEX_CAST_H

libs/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp

@@ -0,0 +1,609 @@
+
+/*
+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 "btVoronoiSimplexSolver.h"
+
+#define VERTA  0
+#define VERTB  1
+#define VERTC  2
+#define VERTD  3
+
+#define CATCH_DEGENERATE_TETRAHEDRON 1
+void	btVoronoiSimplexSolver::removeVertex(int index)
+{
+	
+	btAssert(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	btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& 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 btVoronoiSimplexSolver::reset()
+{
+	m_cachedValidClosest = false;
+	m_numVertices = 0;
+	m_needsUpdate = true;
+	m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+	m_cachedBC.reset();
+}
+
+
+
+	//add a vertex
+void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& 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	btVoronoiSimplexSolver::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_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_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 btVoronoiSimplexSolver::closest(btVector3& v)
+{
+	bool succes = updateClosestVectorAndPoints();
+	v = m_cachedV;
+	return succes;
+}
+
+
+
+btScalar btVoronoiSimplexSolver::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 btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *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 btVoronoiSimplexSolver::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++)
+	{
+#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
+		if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
+#else
+		if (m_simplexVectorW[i] == w)
+#endif
+			found = true;
+	}
+
+	//check in case lastW is already removed
+	if (w == m_lastW)
+		return true;
+    	
+	return found;
+}
+
+void btVoronoiSimplexSolver::backup_closest(btVector3& v) 
+{
+	v = m_cachedV;
+}
+
+
+bool btVoronoiSimplexSolver::emptySimplex() const 
+{
+	return (numVertices() == 0);
+
+}
+
+void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) 
+{
+	updateClosestVectorAndPoints();
+	p1 = m_cachedP1;
+	p2 = m_cachedP2;
+
+}
+
+
+
+
+bool	btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& 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 btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& 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	btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
+{
+	btSubSimplexClosestResult 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);
+		btVector3 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);
+		btVector3 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);
+		btVector3 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.usedVertexB = tempResult.m_usedVertices.usedVertexC;
+			
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			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);
+		btVector3 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.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+
+			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;
+}
+

libs/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h

@@ -0,0 +1,178 @@
+/*
+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 btVoronoiSimplexSolver_H
+#define btVoronoiSimplexSolver_H
+
+#include "btSimplexSolverInterface.h"
+
+
+
+#define VORONOI_SIMPLEX_MAX_VERTS 5
+
+///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure
+#define BT_USE_EQUAL_VERTEX_THRESHOLD
+#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f
+
+
+struct btUsageBitfield{
+	btUsageBitfield()
+	{
+		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	btSubSimplexClosestResult
+{
+	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
+	btUsageBitfield	m_usedVertices;
+	btScalar	m_barycentricCoords[4];
+	bool m_degenerate;
+
+	void	reset()
+	{
+		m_degenerate = false;
+		setBarycentricCoordinates();
+		m_usedVertices.reset();
+	}
+	bool	isValid()
+	{
+		bool valid = (m_barycentricCoords[0] >= btScalar(0.)) &&
+			(m_barycentricCoords[1] >= btScalar(0.)) &&
+			(m_barycentricCoords[2] >= btScalar(0.)) &&
+			(m_barycentricCoords[3] >= btScalar(0.));
+
+
+		return valid;
+	}
+	void	setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.))
+	{
+		m_barycentricCoords[0] = a;
+		m_barycentricCoords[1] = b;
+		m_barycentricCoords[2] = c;
+		m_barycentricCoords[3] = d;
+	}
+
+};
+
+/// btVoronoiSimplexSolver 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.
+#ifdef NO_VIRTUAL_INTERFACE
+class btVoronoiSimplexSolver
+#else
+class btVoronoiSimplexSolver : public btSimplexSolverInterface
+#endif
+{
+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];
+
+	
+
+	btVector3	m_cachedP1;
+	btVector3	m_cachedP2;
+	btVector3	m_cachedV;
+	btVector3	m_lastW;
+	
+	btScalar	m_equalVertexThreshold;
+	bool		m_cachedValidClosest;
+
+
+	btSubSimplexClosestResult m_cachedBC;
+
+	bool	m_needsUpdate;
+	
+	void	removeVertex(int index);
+	void	reduceVertices (const btUsageBitfield& usedVerts);
+	bool	updateClosestVectorAndPoints();
+
+	bool	closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& 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,btSubSimplexClosestResult& result);
+
+public:
+
+	btVoronoiSimplexSolver()
+		:  m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD)
+	{
+	}
+	 void reset();
+
+	 void addVertex(const btVector3& w, const btVector3& p, const btVector3& q);
+
+	 void	setEqualVertexThreshold(btScalar threshold)
+	 {
+		 m_equalVertexThreshold = threshold;
+	 }
+
+	 btScalar	getEqualVertexThreshold() const
+	 {
+		 return m_equalVertexThreshold;
+	 }
+
+	 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 //VoronoiSimplexSolver