First stage in refactoring Bullet: moved Bullet Collision and Dynamics and LinearMath into src folder, and all files in Collision Detection and Dynamics have bt prefix.

Made all buildsystems to work again (jam, msvc, cmake)

src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp

@@ -0,0 +1,200 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btContinuousConvexCollision.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "LinearMath/SimdTransformUtil.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "btGjkPairDetector.h"
+#include "btPointCollector.h"
+
+
+
+ContinuousConvexCollision::ContinuousConvexCollision ( ConvexShape*	convexA,ConvexShape*	convexB,SimplexSolverInterface* simplexSolver, ConvexPenetrationDepthSolver* penetrationDepthSolver)
+:m_simplexSolver(simplexSolver),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
+/// You don't want your game ever to lock-up.
+#define MAX_ITERATIONS 1000
+
+bool	ContinuousConvexCollision::calcTimeOfImpact(
+				const SimdTransform& fromA,
+				const SimdTransform& toA,
+				const SimdTransform& fromB,
+				const SimdTransform& toB,
+				CastResult& result)
+{
+
+	m_simplexSolver->reset();
+
+	/// compute linear and angular velocity for this interval, to interpolate
+	SimdVector3 linVelA,angVelA,linVelB,angVelB;
+	SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linVelA,angVelA);
+	SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linVelB,angVelB);
+
+	SimdScalar boundingRadiusA = m_convexA->GetAngularMotionDisc();
+	SimdScalar boundingRadiusB = m_convexB->GetAngularMotionDisc();
+
+	SimdScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
+
+	float radius = 0.001f;
+
+	SimdScalar lambda = 0.f;
+	SimdVector3 v(1,0,0);
+
+	int maxIter = MAX_ITERATIONS;
+
+	SimdVector3 n;
+	n.setValue(0.f,0.f,0.f);
+	bool hasResult = false;
+	SimdVector3 c;
+
+	float lastLambda = lambda;
+	//float epsilon = 0.001f;
+
+	int numIter = 0;
+	//first solution, using GJK
+
+
+	SimdTransform identityTrans;
+	identityTrans.setIdentity();
+
+	SphereShape	raySphere(0.0f);
+	raySphere.SetMargin(0.f);
+
+
+//	result.DrawCoordSystem(sphereTr);
+
+	PointCollector	pointCollector1;
+
+	{
+		
+		GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);		
+		GjkPairDetector::ClosestPointInput input;
+	
+		//we don't use margins during CCD
+		gjk.SetIgnoreMargin(true);
+
+		input.m_transformA = fromA;
+		input.m_transformB = fromB;
+		gjk.GetClosestPoints(input,pointCollector1,0);
+
+		hasResult = pointCollector1.m_hasResult;
+		c = pointCollector1.m_pointInWorld;
+	}
+
+	if (hasResult)
+	{
+		SimdScalar dist;
+		dist = pointCollector1.m_distance;
+		n = pointCollector1.m_normalOnBInWorld;
+		
+		//not close enough
+		while (dist > radius)
+		{
+			numIter++;
+			if (numIter > maxIter)
+				return false; //todo: report a failure
+
+			float dLambda = 0.f;
+
+			//calculate safe moving fraction from distance / (linear+rotational velocity)
+			
+			//float clippedDist  = GEN_min(angularConservativeRadius,dist);
+			//float clippedDist  = dist;
+			
+			float projectedLinearVelocity = (linVelB-linVelA).dot(n);
+			
+			dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
+
+			lambda = lambda + dLambda;
+
+			if (lambda > 1.f)
+				return false;
+
+			if (lambda < 0.f)
+				return false;
+
+			//todo: next check with relative epsilon
+			if (lambda <= lastLambda)
+				break;
+			lastLambda = lambda;
+
+			
+
+			//interpolate to next lambda
+			SimdTransform interpolatedTransA,interpolatedTransB,relativeTrans;
+
+			SimdTransformUtil::IntegrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
+			SimdTransformUtil::IntegrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
+			relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
+
+			result.DebugDraw( lambda );
+
+			PointCollector	pointCollector;
+			GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
+			GjkPairDetector::ClosestPointInput input;
+			input.m_transformA = interpolatedTransA;
+			input.m_transformB = interpolatedTransB;
+			gjk.GetClosestPoints(input,pointCollector,0);
+			if (pointCollector.m_hasResult)
+			{
+				if (pointCollector.m_distance < 0.f)
+				{
+					//degenerate ?!
+					result.m_fraction = lastLambda;
+					result.m_normal = n;
+					return true;
+				}
+				c = pointCollector.m_pointInWorld;		
+				
+				dist = pointCollector.m_distance;
+			} else
+			{
+				//??
+				return false;
+			}
+
+		}
+
+		result.m_fraction = lambda;
+		result.m_normal = n;
+		return true;
+	}
+
+	return false;
+
+/*
+//todo:
+	//if movement away from normal, discard result
+	SimdVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
+	if (result.m_fraction < 1.f)
+	{
+		if (move.dot(result.m_normal) <= 0.f)
+		{
+		}
+	}
+*/
+
+}
+

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

src/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"
+
+ConvexCast::~ConvexCast()
+{
+}

src/BulletCollision/NarrowPhaseCollision/btConvexCast.h

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

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

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

src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp

@@ -0,0 +1,174 @@
+/*
+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 "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "btGjkPairDetector.h"
+#include "btPointCollector.h"
+
+
+GjkConvexCast::GjkConvexCast(ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),
+m_convexB(convexB)
+{
+}
+
+bool	GjkConvexCast::calcTimeOfImpact(
+					const SimdTransform& fromA,
+					const SimdTransform& toA,
+					const SimdTransform& fromB,
+					const SimdTransform& toB,
+					CastResult& result)
+{
+
+
+	MinkowskiSumShape combi(m_convexA,m_convexB);
+	MinkowskiSumShape* convex = &combi;
+
+	SimdTransform	rayFromLocalA;
+	SimdTransform	rayToLocalA;
+
+	rayFromLocalA = fromA.inverse()* fromB;
+	rayToLocalA = toA.inverse()* toB;
+
+
+	SimdTransform trA,trB;
+	trA = SimdTransform(fromA);
+	trB = SimdTransform(fromB);
+	trA.setOrigin(SimdPoint3(0,0,0));
+	trB.setOrigin(SimdPoint3(0,0,0));
+
+	convex->SetTransformA(trA);
+	convex->SetTransformB(trB);
+
+
+
+
+	float radius = 0.01f;
+
+	SimdScalar lambda = 0.f;
+	SimdVector3 s = rayFromLocalA.getOrigin();
+	SimdVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin();
+	SimdVector3 x = s;
+	SimdVector3 n;
+	n.setValue(0,0,0);
+	bool hasResult = false;
+	SimdVector3 c;
+
+	float lastLambda = lambda;
+
+	//first solution, using GJK
+
+	//no penetration support for now, perhaps pass a pointer when we really want it
+	ConvexPenetrationDepthSolver* penSolverPtr = 0;
+
+	SimdTransform identityTrans;
+	identityTrans.setIdentity();
+
+	SphereShape	raySphere(0.0f);
+	raySphere.SetMargin(0.f);
+
+	SimdTransform sphereTr;
+	sphereTr.setIdentity();
+	sphereTr.setOrigin( rayFromLocalA.getOrigin());
+
+	result.DrawCoordSystem(sphereTr);
+	{
+		PointCollector	pointCollector1;
+		GjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);		
+
+		GjkPairDetector::ClosestPointInput input;
+		input.m_transformA = sphereTr;
+		input.m_transformB = identityTrans;
+		gjk.GetClosestPoints(input,pointCollector1,0);
+
+		hasResult = pointCollector1.m_hasResult;
+		c = pointCollector1.m_pointInWorld;
+		n = pointCollector1.m_normalOnBInWorld;
+	}
+
+	
+
+	if (hasResult)
+	{
+		SimdScalar dist;
+		dist = (c-x).length();
+		if (dist < radius)
+		{
+			//penetration
+			lastLambda = 1.f;
+		}
+
+		//not close enough
+		while (dist > radius)
+		{
+			
+			n = x - c;
+			SimdScalar nDotr = n.dot(r);
+
+			if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON))
+				return false;
+			
+			lambda = lambda - n.dot(n) / nDotr;
+			if (lambda <= lastLambda)
+				break;
+
+			lastLambda = lambda;
+
+			x = s + lambda * r;
+
+			sphereTr.setOrigin( x );
+			result.DrawCoordSystem(sphereTr);
+			PointCollector	pointCollector;
+			GjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);
+			GjkPairDetector::ClosestPointInput input;
+			input.m_transformA = sphereTr;
+			input.m_transformB = identityTrans;
+			gjk.GetClosestPoints(input,pointCollector,0);
+			if (pointCollector.m_hasResult)
+			{
+				if (pointCollector.m_distance < 0.f)
+				{
+					//degeneracy, report a hit
+					result.m_fraction = lastLambda;
+					result.m_normal = n;
+					return true;
+				}
+				c = pointCollector.m_pointInWorld;			
+				dist = (c-x).length();
+			} else
+			{
+				//??
+				return false;
+			}
+
+		}
+
+		if (lastLambda < 1.f)
+		{
+		
+			result.m_fraction = lastLambda;
+			result.m_normal = n;
+			return true;
+		}
+	}
+
+	return false;
+}
+

src/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/SimdVector3.h"
+#include "btConvexCast.h"
+class ConvexShape;
+class MinkowskiSumShape;
+#include "btSimplexSolverInterface.h"
+
+///GjkConvexCast performs a raycast on a convex object using support mapping.
+class GjkConvexCast : public ConvexCast
+{
+	SimplexSolverInterface*	m_simplexSolver;
+	ConvexShape*	m_convexA;
+	ConvexShape*	m_convexB;
+
+public:
+
+	GjkConvexCast(ConvexShape*	convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver);
+
+	/// cast a convex against another convex object
+	virtual bool	calcTimeOfImpact(
+					const SimdTransform& fromA,
+					const SimdTransform& toA,
+					const SimdTransform& fromB,
+					const SimdTransform& toB,
+					CastResult& result);
+
+};
+
+#endif //GJK_CONVEX_CAST_H

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -0,0 +1,229 @@
+/*
+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)
+#include <stdio.h> //for debug printf
+#endif
+
+static const SimdScalar rel_error = SimdScalar(1.0e-5);
+SimdScalar rel_error2 = rel_error * rel_error;
+float maxdist2 = 1.e30f;
+
+
+int gGjkMaxIter=1000;
+
+GjkPairDetector::GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver*	penetrationDepthSolver)
+:m_cachedSeparatingAxis(0.f,0.f,1.f),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_ignoreMargin(false),
+m_partId0(-1),
+m_index0(-1),
+m_partId1(-1),
+m_index1(-1)
+{
+}
+
+void GjkPairDetector::GetClosestPoints(const ClosestPointInput& input,Result& output,class IDebugDraw* debugDraw)
+{
+	SimdScalar distance=0.f;
+	SimdVector3	normalInB(0.f,0.f,0.f);
+	SimdVector3 pointOnA,pointOnB;
+
+	float marginA = m_minkowskiA->GetMargin();
+	float marginB = m_minkowskiB->GetMargin();
+
+	//for CCD we don't use margins
+	if (m_ignoreMargin)
+	{
+		marginA = 0.f;
+		marginB = 0.f;
+	}
+
+int curIter = 0;
+
+	bool isValid = false;
+	bool checkSimplex = false;
+	bool checkPenetration = true;
+
+	{
+		SimdScalar squaredDistance = SIMD_INFINITY;
+		SimdScalar delta = 0.f;
+		
+		SimdScalar margin = marginA + marginB;
+		
+		
+
+		m_simplexSolver->reset();
+		
+		while (true)
+		{
+			//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a CollisionObject
+			if (curIter++ > gGjkMaxIter)
+			{
+				#if defined(DEBUG) || defined (_DEBUG)
+					printf("GjkPairDetector maxIter exceeded:%i\n",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;
+
+			}
+
+			SimdVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
+			SimdVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+
+			SimdVector3 pInA = m_minkowskiA->LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
+			SimdVector3 qInB = m_minkowskiB->LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
+			SimdPoint3  pWorld = input.m_transformA(pInA);	
+			SimdPoint3  qWorld = input.m_transformB(qInB);
+			
+			SimdVector3 w	= pWorld - qWorld;
+			delta = m_cachedSeparatingAxis.dot(w);
+
+			// potential exit, they don't overlap
+			if ((delta > SimdScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
+			{
+				checkPenetration = false;
+				break;
+			}
+
+			//exit 0: the new point is already in the simplex, or we didn't come any closer
+			if (m_simplexSolver->inSimplex(w))
+			{
+				checkSimplex = true;
+				break;
+			}
+			// are we getting any closer ?
+			if (squaredDistance - delta <= squaredDistance * rel_error2)
+			{
+				checkSimplex = true;
+				break;
+			}
+			//add current vertex to simplex
+			m_simplexSolver->addVertex(w, pWorld, qWorld);
+
+			//calculate the closest point to the origin (update vector v)
+			if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
+			{
+				checkSimplex = true;
+				break;
+			}
+
+			SimdScalar previousSquaredDistance = squaredDistance;
+			squaredDistance = m_cachedSeparatingAxis.length2();
+			
+			//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+
+			//are we getting any closer ?
+			if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
+			{ 
+				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+				checkSimplex = true;
+				break;
+			}
+			bool check = (!m_simplexSolver->fullSimplex());
+			//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+
+			if (!check)
+			{
+				//do we need this backup_closest here ?
+				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+				break;
+			}
+		}
+
+		if (checkSimplex)
+		{
+			m_simplexSolver->compute_points(pointOnA, pointOnB);
+			normalInB = pointOnA-pointOnB;
+			float lenSqr = m_cachedSeparatingAxis.length2();
+			//valid normal
+			if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+			{
+				float rlen = 1.f / SimdSqrt(lenSqr );
+				normalInB *= rlen; //normalize
+				SimdScalar s = SimdSqrt(squaredDistance);
+				ASSERT(s > SimdScalar(0.0));
+				pointOnA -= m_cachedSeparatingAxis * (marginA / s);
+				pointOnB += m_cachedSeparatingAxis * (marginB / s);
+				distance = ((1.f/rlen) - margin);
+				isValid = true;
+			}
+		}
+
+		if (checkPenetration && !isValid)
+		{
+			//penetration case
+		
+			//if there is no way to handle penetrations, bail out
+			if (m_penetrationDepthSolver)
+			{
+				// Penetration depth case.
+				isValid = m_penetrationDepthSolver->CalcPenDepth( 
+					*m_simplexSolver, 
+					m_minkowskiA,m_minkowskiB,
+					input.m_transformA,input.m_transformB,
+					m_cachedSeparatingAxis, pointOnA, pointOnB,
+					debugDraw
+					);
+
+				if (isValid)
+				{
+					normalInB = pointOnB-pointOnA;
+					float lenSqr = normalInB.length2();
+					if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+					{
+						normalInB /= SimdSqrt(lenSqr);
+						distance = -(pointOnA-pointOnB).length();
+					} else
+					{
+						isValid = false;
+					}
+				}
+			}
+		}
+	}
+
+	if (isValid)
+	{
+		output.SetShapeIdentifiers(m_partId0,m_index0,m_partId1,m_index1);
+
+		output.AddContactPoint(
+			normalInB,
+			pointOnB,
+			distance);
+		//printf("gjk add:%f",distance);
+	} 
+
+
+}
+
+
+
+
+

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h

@@ -0,0 +1,84 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+
+#ifndef GJK_PAIR_DETECTOR_H
+#define GJK_PAIR_DETECTOR_H
+
+#include "btDiscreteCollisionDetectorInterface.h"
+#include "LinearMath/SimdPoint3.h"
+
+#include <BulletCollision/CollisionShapes/btCollisionMargin.h>
+
+class ConvexShape;
+#include "btSimplexSolverInterface.h"
+class ConvexPenetrationDepthSolver;
+
+/// GjkPairDetector uses GJK to implement the DiscreteCollisionDetectorInterface
+class GjkPairDetector : public DiscreteCollisionDetectorInterface
+{
+	
+
+	SimdVector3	m_cachedSeparatingAxis;
+	ConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	SimplexSolverInterface* m_simplexSolver;
+	ConvexShape* m_minkowskiA;
+	ConvexShape* m_minkowskiB;
+	bool		m_ignoreMargin;
+	
+
+public:
+
+	//experimental feature information, per triangle, per convex etc.
+	//'material combiner' / contact added callback
+	int	m_partId0;
+	int	m_index0;
+	int	m_partId1;
+	int	m_index1;
+
+	GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver*	penetrationDepthSolver);
+	virtual ~GjkPairDetector() {};
+
+	virtual void	GetClosestPoints(const ClosestPointInput& input,Result& output,class IDebugDraw* debugDraw);
+
+	void SetMinkowskiA(ConvexShape* minkA)
+	{
+		m_minkowskiA = minkA;
+	}
+
+	void SetMinkowskiB(ConvexShape* minkB)
+	{
+		m_minkowskiB = minkB;
+	}
+	void SetCachedSeperatingAxis(const SimdVector3& seperatingAxis)
+	{
+		m_cachedSeparatingAxis = seperatingAxis;
+	}
+
+	void	SetPenetrationDepthSolver(ConvexPenetrationDepthSolver*	penetrationDepthSolver)
+	{
+		m_penetrationDepthSolver = penetrationDepthSolver;
+	}
+
+	void	SetIgnoreMargin(bool ignoreMargin)
+	{
+		m_ignoreMargin = ignoreMargin;
+	}
+
+};
+
+#endif //GJK_PAIR_DETECTOR_H

src/BulletCollision/NarrowPhaseCollision/btManifoldContactAddResult.cpp

@@ -0,0 +1,48 @@
+/*
+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 "btManifoldContactAddResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+ManifoldContactAddResult::ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr)
+		:m_manifoldPtr(manifoldPtr)
+{
+	m_transAInv = transA.inverse();
+	m_transBInv = transB.inverse();
+
+}
+
+
+void ManifoldContactAddResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+{
+	if (depth > m_manifoldPtr->GetContactBreakingTreshold())
+		return;
+
+
+	SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+	SimdVector3 localA = m_transAInv(pointA );
+	SimdVector3 localB = m_transBInv(pointInWorld);
+	ManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+
+	int insertIndex = m_manifoldPtr->GetCacheEntry(newPt);
+	if (insertIndex >= 0)
+	{
+		m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex);
+	} else
+	{
+		m_manifoldPtr->AddManifoldPoint(newPt);
+	}
+}
+

src/BulletCollision/NarrowPhaseCollision/btManifoldContactAddResult.h

@@ -0,0 +1,37 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef MANIFOLD_CONTACT_ADD_RESULT_H
+#define MANIFOLD_CONTACT_ADD_RESULT_H
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+class PersistentManifold;
+
+class ManifoldContactAddResult : public DiscreteCollisionDetectorInterface::Result
+{
+	PersistentManifold* m_manifoldPtr;
+	SimdTransform	m_transAInv;
+	SimdTransform	m_transBInv;
+
+public:
+
+	ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr);
+
+	virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth);
+
+};
+
+#endif //MANIFOLD_CONTACT_ADD_RESULT_H

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -0,0 +1,98 @@
+/*
+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/SimdVector3.h"
+#include "LinearMath/SimdTransformUtil.h"
+
+
+
+
+
+/// ManifoldContactPoint collects and maintains persistent contactpoints.
+/// used to improve stability and performance of rigidbody dynamics response.
+class ManifoldPoint
+	{
+		public:
+			ManifoldPoint()
+				:m_userPersistentData(0)
+			{
+			}
+
+			ManifoldPoint( const SimdVector3 &pointA, const SimdVector3 &pointB, 
+					const SimdVector3 &normal, 
+					SimdScalar distance ) :
+					m_localPointA( pointA ), 
+					m_localPointB( pointB ), 
+					m_normalWorldOnB( normal ), 
+					m_distance1( distance ),
+					m_combinedFriction(0.f),
+					m_combinedRestitution(0.f),
+					m_userPersistentData(0),					
+					m_lifeTime(0)
+			{
+				
+					
+			}
+
+			
+
+			SimdVector3 m_localPointA;			
+			SimdVector3 m_localPointB;			
+			SimdVector3	m_positionWorldOnB;
+			///m_positionWorldOnA is redundant information, see GetPositionWorldOnA(), but for clarity
+			SimdVector3	m_positionWorldOnA;
+			SimdVector3 m_normalWorldOnB;
+		
+			float	m_distance1;
+			float	m_combinedFriction;
+			float	m_combinedRestitution;
+
+				
+			void*	m_userPersistentData;
+
+			int		m_lifeTime;//lifetime of the contactpoint in frames
+			
+			float GetDistance() const
+			{
+				return m_distance1;
+			}
+			int	GetLifeTime() const
+			{
+				return m_lifeTime;
+			}
+
+			SimdVector3 GetPositionWorldOnA() {
+				return m_positionWorldOnA;
+//				return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
+			}
+
+			const SimdVector3& GetPositionWorldOnB()
+			{
+				return m_positionWorldOnB;
+			}
+
+			void	SetDistance(float dist)
+			{
+				m_distance1 = dist;
+			}
+			
+			
+
+	};
+
+#endif //MANIFOLD_CONTACT_POINT_H

src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp

@@ -0,0 +1,246 @@
+/*
+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/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+
+
+struct MyResult : public DiscreteCollisionDetectorInterface::Result
+{
+
+	MyResult():m_hasResult(false)
+	{
+	}
+	
+	SimdVector3 m_normalOnBInWorld;
+	SimdVector3 m_pointInWorld;
+	float m_depth;
+	bool	m_hasResult;
+
+	virtual void SetShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+	{
+	}
+	void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+	{
+		m_normalOnBInWorld = normalOnBInWorld;
+		m_pointInWorld = pointInWorld;
+		m_depth = depth;
+		m_hasResult = true;
+	}
+};
+
+#define NUM_UNITSPHERE_POINTS 42
+static SimdVector3	sPenetrationDirections[NUM_UNITSPHERE_POINTS] = 
+{
+SimdVector3(0.000000f , -0.000000f,-1.000000f),
+SimdVector3(0.723608f , -0.525725f,-0.447219f),
+SimdVector3(-0.276388f , -0.850649f,-0.447219f),
+SimdVector3(-0.894426f , -0.000000f,-0.447216f),
+SimdVector3(-0.276388f , 0.850649f,-0.447220f),
+SimdVector3(0.723608f , 0.525725f,-0.447219f),
+SimdVector3(0.276388f , -0.850649f,0.447220f),
+SimdVector3(-0.723608f , -0.525725f,0.447219f),
+SimdVector3(-0.723608f , 0.525725f,0.447219f),
+SimdVector3(0.276388f , 0.850649f,0.447219f),
+SimdVector3(0.894426f , 0.000000f,0.447216f),
+SimdVector3(-0.000000f , 0.000000f,1.000000f),
+SimdVector3(0.425323f , -0.309011f,-0.850654f),
+SimdVector3(-0.162456f , -0.499995f,-0.850654f),
+SimdVector3(0.262869f , -0.809012f,-0.525738f),
+SimdVector3(0.425323f , 0.309011f,-0.850654f),
+SimdVector3(0.850648f , -0.000000f,-0.525736f),
+SimdVector3(-0.525730f , -0.000000f,-0.850652f),
+SimdVector3(-0.688190f , -0.499997f,-0.525736f),
+SimdVector3(-0.162456f , 0.499995f,-0.850654f),
+SimdVector3(-0.688190f , 0.499997f,-0.525736f),
+SimdVector3(0.262869f , 0.809012f,-0.525738f),
+SimdVector3(0.951058f , 0.309013f,0.000000f),
+SimdVector3(0.951058f , -0.309013f,0.000000f),
+SimdVector3(0.587786f , -0.809017f,0.000000f),
+SimdVector3(0.000000f , -1.000000f,0.000000f),
+SimdVector3(-0.587786f , -0.809017f,0.000000f),
+SimdVector3(-0.951058f , -0.309013f,-0.000000f),
+SimdVector3(-0.951058f , 0.309013f,-0.000000f),
+SimdVector3(-0.587786f , 0.809017f,-0.000000f),
+SimdVector3(-0.000000f , 1.000000f,-0.000000f),
+SimdVector3(0.587786f , 0.809017f,-0.000000f),
+SimdVector3(0.688190f , -0.499997f,0.525736f),
+SimdVector3(-0.262869f , -0.809012f,0.525738f),
+SimdVector3(-0.850648f , 0.000000f,0.525736f),
+SimdVector3(-0.262869f , 0.809012f,0.525738f),
+SimdVector3(0.688190f , 0.499997f,0.525736f),
+SimdVector3(0.525730f , 0.000000f,0.850652f),
+SimdVector3(0.162456f , -0.499995f,0.850654f),
+SimdVector3(-0.425323f , -0.309011f,0.850654f),
+SimdVector3(-0.425323f , 0.309011f,0.850654f),
+SimdVector3(0.162456f , 0.499995f,0.850654f)
+};
+
+
+bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simplexSolver,
+												   ConvexShape* convexA,ConvexShape* convexB,
+												   const SimdTransform& transA,const SimdTransform& transB,
+												   SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb,
+												   class IDebugDraw* debugDraw
+												   )
+{
+
+	//just take fixed number of orientation, and sample the penetration depth in that direction
+	float minProj = 1e30f;
+	SimdVector3 minNorm;
+	SimdVector3 minVertex;
+	SimdVector3 minA,minB;
+	SimdVector3 seperatingAxisInA,seperatingAxisInB;
+	SimdVector3 pInA,qInB,pWorld,qWorld,w;
+
+#define USE_BATCHED_SUPPORT 1
+#ifdef USE_BATCHED_SUPPORT
+	SimdVector3	supportVerticesABatch[NUM_UNITSPHERE_POINTS];
+	SimdVector3	supportVerticesBBatch[NUM_UNITSPHERE_POINTS];
+	SimdVector3	seperatingAxisInABatch[NUM_UNITSPHERE_POINTS];
+	SimdVector3	seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS];
+	int i;
+
+	for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
+	{
+		const SimdVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInABatch[i] = (-norm)* transA.getBasis();
+		seperatingAxisInBBatch[i] = norm * transB.getBasis();
+	}
+
+	convexA->BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,NUM_UNITSPHERE_POINTS);
+	convexB->BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS);
+	for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
+	{
+		const SimdVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInA = seperatingAxisInABatch[i];
+		seperatingAxisInB = seperatingAxisInBBatch[i];
+
+		pInA = supportVerticesABatch[i];
+		qInB = supportVerticesBBatch[i];
+
+		pWorld = transA(pInA);	
+		qWorld = transB(qInB);
+		w	= qWorld - pWorld;
+		float delta = norm.dot(w);
+		//find smallest delta
+		if (delta < minProj)
+		{
+			minProj = delta;
+			minNorm = norm;
+			minA = pWorld;
+			minB = qWorld;
+		}
+	}	
+#else
+	for (int i=0;i<NUM_UNITSPHERE_POINTS;i++)
+	{
+		const SimdVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInA = (-norm)* transA.getBasis();
+		seperatingAxisInB = norm* transB.getBasis();
+		pInA = convexA->LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
+		qInB = convexB->LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
+		pWorld = transA(pInA);	
+		qWorld = transB(qInB);
+		w	= qWorld - pWorld;
+		float 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->GetMargin();
+	minB -= minNorm*convexB->GetMargin();
+	minProj += (convexA->GetMargin() + convexB->GetMargin());
+
+
+	
+
+//#define DEBUG_DRAW 1
+#ifdef DEBUG_DRAW
+	if (debugDraw)
+	{
+		SimdVector3 color(0,1,0);
+		debugDraw->DrawLine(minA,minB,color);
+		color = SimdVector3 (1,1,1);
+		SimdVector3 vec = minB-minA;
+		float prj2 = minNorm.dot(vec);
+		debugDraw->DrawLine(minA,minA+(minNorm*minProj),color);
+
+	}
+#endif //DEBUG_DRAW
+
+	
+
+	GjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
+
+	SimdScalar offsetDist = minProj;
+	SimdVector3 offset = minNorm * offsetDist;
+	
+
+
+	GjkPairDetector::ClosestPointInput input;
+		
+	SimdVector3 newOrg = transA.getOrigin() + offset;
+
+	SimdTransform displacedTrans = transA;
+	displacedTrans.setOrigin(newOrg);
+
+	input.m_transformA = displacedTrans;
+	input.m_transformB = transB;
+	input.m_maximumDistanceSquared = 1e30f;//minProj;
+	
+	MyResult res;
+	gjkdet.GetClosestPoints(input,res,debugDraw);
+
+	float correctedMinNorm = minProj - res.m_depth;
+
+
+	//the penetration depth is over-estimated, relax it
+	float penetration_relaxation= 1.f;
+	minNorm*=penetration_relaxation;
+
+	if (res.m_hasResult)
+	{
+
+		pa = res.m_pointInWorld - minNorm * correctedMinNorm;
+		pb = res.m_pointInWorld;
+		
+#ifdef DEBUG_DRAW
+		if (debugDraw)
+		{
+			SimdVector3 color(1,0,0);
+			debugDraw->DrawLine(pa,pb,color);
+		}
+#endif//DEBUG_DRAW
+
+
+	}
+	return res.m_hasResult;
+}
+
+
+

src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h

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

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

@@ -0,0 +1,246 @@
+/*
+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/SimdTransform.h"
+#include <assert.h>
+
+float						gContactBreakingTreshold = 0.02f;
+ContactDestroyedCallback	gContactDestroyedCallback = 0;
+
+
+
+PersistentManifold::PersistentManifold()
+:m_body0(0),
+m_body1(0),
+m_cachedPoints (0),
+m_index1(0)
+{
+}
+
+
+void	PersistentManifold::ClearManifold()
+{
+	int i;
+	for (i=0;i<m_cachedPoints;i++)
+	{
+		ClearUserCache(m_pointCache[i]);
+	}
+	m_cachedPoints = 0;
+}
+
+#ifdef DEBUG_PERSISTENCY
+#include <stdio.h>
+void	PersistentManifold::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 PersistentManifold::ClearUserCache(ManifoldPoint& 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");
+			}
+		}
+		assert(occurance<=0);
+#endif //DEBUG_PERSISTENCY
+
+		if (pt.m_userPersistentData && gContactDestroyedCallback)
+		{
+			(*gContactDestroyedCallback)(pt.m_userPersistentData);
+			pt.m_userPersistentData = 0;
+		}
+		
+#ifdef DEBUG_PERSISTENCY
+		DebugPersistency();
+#endif
+	}
+
+	
+}
+
+
+int PersistentManifold::SortCachedPoints(const ManifoldPoint& 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
+		float 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
+		
+		SimdScalar res0(0.f),res1(0.f),res2(0.f),res3(0.f);
+		if (maxPenetrationIndex != 0)
+		{
+			SimdVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
+			SimdVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+			SimdVector3 cross = a0.cross(b0);
+			res0 = cross.length2();
+		}
+		if (maxPenetrationIndex != 1)
+		{
+			SimdVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			SimdVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+			SimdVector3 cross = a1.cross(b1);
+			res1 = cross.length2();
+		}
+
+		if (maxPenetrationIndex != 2)
+		{
+			SimdVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			SimdVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
+			SimdVector3 cross = a2.cross(b2);
+			res2 = cross.length2();
+		}
+
+		if (maxPenetrationIndex != 3)
+		{
+			SimdVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			SimdVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
+			SimdVector3 cross = a3.cross(b3);
+			res3 = cross.length2();
+		}
+
+		SimdVector4 maxvec(res0,res1,res2,res3);
+		int biggestarea = maxvec.closestAxis4();
+		return biggestarea;
+}
+
+
+int PersistentManifold::GetCacheEntry(const ManifoldPoint& newPoint) const
+{
+	SimdScalar shortestDist =  GetContactBreakingTreshold() * GetContactBreakingTreshold();
+	int size = GetNumContacts();
+	int nearestPoint = -1;
+	for( int i = 0; i < size; i++ )
+	{
+		const ManifoldPoint &mp = m_pointCache[i];
+
+		SimdVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
+		const SimdScalar distToManiPoint = diffA.dot(diffA);
+		if( distToManiPoint < shortestDist )
+		{
+			shortestDist = distToManiPoint;
+			nearestPoint = i;
+		}
+	}
+	return nearestPoint;
+}
+
+void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint)
+{
+	assert(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
+
+		
+	} else
+	{
+		m_cachedPoints++;
+
+		
+	}
+	ReplaceContactPoint(newPoint,insertIndex);
+}
+
+float	PersistentManifold::GetContactBreakingTreshold() const
+{
+	return gContactBreakingTreshold;
+}
+
+void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const SimdTransform& trB)
+{
+	int i;
+
+	/// first refresh worldspace positions and distance
+	for (i=GetNumContacts()-1;i>=0;i--)
+	{
+		ManifoldPoint &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 
+	SimdScalar distance2d;
+	SimdVector3 projectedDifference,projectedPoint;
+	for (i=GetNumContacts()-1;i>=0;i--)
+	{
+		
+		ManifoldPoint &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  > GetContactBreakingTreshold()*GetContactBreakingTreshold() )
+			{
+				RemoveContactPoint(i);
+			}
+		}
+	}
+#ifdef DEBUG_PERSISTENCY
+	DebugPersistency();
+#endif //
+}
+
+
+
+
+

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -0,0 +1,140 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef PERSISTENT_MANIFOLD_H
+#define PERSISTENT_MANIFOLD_H
+
+
+#include "LinearMath/SimdVector3.h"
+#include "LinearMath/SimdTransform.h"
+#include "btManifoldPoint.h"
+
+struct CollisionResult;
+
+///contact breaking and merging treshold
+extern float gContactBreakingTreshold;
+
+typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
+extern ContactDestroyedCallback	gContactDestroyedCallback;
+
+
+
+
+#define MANIFOLD_CACHE_SIZE 4
+
+///PersistentManifold maintains contact points, and reduces them to 4.
+///It does contact filtering/contact reduction.
+class PersistentManifold 
+{
+
+	ManifoldPoint 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;
+
+	
+	/// sort cached points so most isolated points come first
+	int	SortCachedPoints(const ManifoldPoint& pt);
+
+	int		FindContactPoint(const ManifoldPoint* unUsed, int numUnused,const ManifoldPoint& pt);
+
+public:
+
+	int m_index1;
+
+	PersistentManifold();
+
+	PersistentManifold(void* body0,void* body1)
+		: m_body0(body0),m_body1(body1),m_cachedPoints(0)
+	{
+	}
+
+	inline void* GetBody0() { return m_body0;}
+	inline void* GetBody1() { return m_body1;}
+
+	inline const void* GetBody0() const { return m_body0;}
+	inline const void* GetBody1() const { return m_body1;}
+
+	void	SetBodies(void* body0,void* body1)
+	{
+		m_body0 = body0;
+		m_body1 = body1;
+	}
+
+	void ClearUserCache(ManifoldPoint& pt);
+
+#ifdef DEBUG_PERSISTENCY
+	void	DebugPersistency();
+#endif //
+	
+	inline int	GetNumContacts() const { return m_cachedPoints;}
+
+	inline const ManifoldPoint& GetContactPoint(int index) const
+	{
+		ASSERT(index < m_cachedPoints);
+		return m_pointCache[index];
+	}
+
+	inline ManifoldPoint& GetContactPoint(int index)
+	{
+		ASSERT(index < m_cachedPoints);
+		return m_pointCache[index];
+	}
+
+	/// todo: get this margin from the current physics / collision environment
+	float	GetContactBreakingTreshold() const;
+	
+	int GetCacheEntry(const ManifoldPoint& newPoint) const;
+
+	void AddManifoldPoint( const ManifoldPoint& newPoint);
+
+	void RemoveContactPoint (int index)
+	{
+		ClearUserCache(m_pointCache[index]);
+
+		int lastUsedIndex = GetNumContacts() - 1;
+		m_pointCache[index] = m_pointCache[lastUsedIndex];
+		//get rid of duplicated userPersistentData pointer
+		m_pointCache[lastUsedIndex].m_userPersistentData = 0;
+		m_cachedPoints--;
+	}
+	void ReplaceContactPoint(const ManifoldPoint& newPoint,int insertIndex)
+	{
+		assert(ValidContactDistance(newPoint));
+
+		ClearUserCache(m_pointCache[insertIndex]);
+		
+		m_pointCache[insertIndex] = newPoint;
+	}
+
+	bool ValidContactDistance(const ManifoldPoint& pt) const
+	{
+		return pt.m_distance1 <= GetContactBreakingTreshold();
+	}
+	/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
+	void	RefreshContactPoints(  const SimdTransform& trA,const SimdTransform& trB);
+
+	void	ClearManifold();
+
+
+
+};
+
+
+
+#endif //PERSISTENT_MANIFOLD_H

src/BulletCollision/NarrowPhaseCollision/btPointCollector.h

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

src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp

@@ -0,0 +1,101 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btRaycastCallback.h"
+
+TriangleRaycastCallback::TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to)
+	:
+	m_from(from),
+	m_to(to),
+	m_hitFraction(1.f)
+{
+
+}
+
+
+
+void TriangleRaycastCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex)
+{
+	
+
+	const SimdVector3 &vert0=triangle[0];
+	const SimdVector3 &vert1=triangle[1];
+	const SimdVector3 &vert2=triangle[2];
+
+	SimdVector3 v10; v10 = vert1 - vert0 ;
+	SimdVector3 v20; v20 = vert2 - vert0 ;
+
+	SimdVector3 triangleNormal; triangleNormal = v10.cross( v20 );
+	
+	const float dist = vert0.dot(triangleNormal);
+	float dist_a = triangleNormal.dot(m_from) ;
+	dist_a-= dist;
+	float dist_b = triangleNormal.dot(m_to);
+	dist_b -= dist;
+
+	if ( dist_a * dist_b >= 0.0f)
+	{
+		return ; // same sign
+	}
+	
+	const float proj_length=dist_a-dist_b;
+	const float 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)
+	{
+		
+
+		float edge_tolerance =triangleNormal.length2();		
+		edge_tolerance *= -0.0001f;
+		SimdVector3 point; point.setInterpolate3( m_from, m_to, distance);
+		{
+			SimdVector3 v0p; v0p = vert0 - point;
+			SimdVector3 v1p; v1p = vert1 - point;
+			SimdVector3 cp0; cp0 = v0p.cross( v1p );
+
+			if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance) 
+			{
+						
+
+				SimdVector3 v2p; v2p = vert2 -  point;
+				SimdVector3 cp1;
+				cp1 = v1p.cross( v2p);
+				if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance) 
+				{
+					SimdVector3 cp2;
+					cp2 = v2p.cross(v0p);
+					
+					if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance) 
+					{
+
+						if ( dist_a > 0 )
+						{
+							m_hitFraction = ReportHit(triangleNormal,distance,partId,triangleIndex);
+						}
+						else
+						{
+							m_hitFraction = ReportHit(-triangleNormal,distance,partId,triangleIndex);
+						}
+					}
+				}
+			}
+		}
+	}
+}

src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.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 RAYCAST_TRI_CALLBACK_H
+#define RAYCAST_TRI_CALLBACK_H
+
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+struct BroadphaseProxy;
+
+
+class  TriangleRaycastCallback: public TriangleCallback
+{
+public:
+
+		//input
+	SimdVector3 m_from;
+	SimdVector3 m_to;
+
+	float	m_hitFraction;
+
+	TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to);
+	
+	virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex);
+
+	virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0;
+	
+};
+
+#endif //RAYCAST_TRI_CALLBACK_H
+

src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.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 SIMPLEX_SOLVER_INTERFACE_H
+#define SIMPLEX_SOLVER_INTERFACE_H
+
+#include "LinearMath/SimdVector3.h"
+#include "LinearMath/SimdPoint3.h"
+
+#define NO_VIRTUAL_INTERFACE 1
+#ifdef NO_VIRTUAL_INTERFACE
+#include "btVoronoiSimplexSolver.h"
+#define SimplexSolverInterface VoronoiSimplexSolver
+#else
+
+/// SimplexSolverInterface 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 SimplexSolverInterface
+{
+	public:
+		virtual ~SimplexSolverInterface() {};
+
+	virtual void reset() = 0;
+
+	virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q) = 0;
+	
+	virtual bool closest(SimdVector3& v) = 0;
+
+	virtual SimdScalar maxVertex() = 0;
+
+	virtual bool fullSimplex() const = 0;
+
+	virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const = 0;
+
+	virtual bool inSimplex(const SimdVector3& w) = 0;
+	
+	virtual void backup_closest(SimdVector3& v) = 0;
+
+	virtual bool emptySimplex() const = 0;
+
+	virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2) = 0;
+
+	virtual int numVertices() const =0;
+
+
+};
+#endif
+#endif //SIMPLEX_SOLVER_INTERFACE_H
+

src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp

@@ -0,0 +1,132 @@
+/*
+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"
+
+
+SubsimplexConvexCast::SubsimplexConvexCast (ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+
+#define MAX_ITERATIONS 1000
+
+bool	SubsimplexConvexCast::calcTimeOfImpact(
+		const SimdTransform& fromA,
+		const SimdTransform& toA,
+		const SimdTransform& fromB,
+		const SimdTransform& toB,
+		CastResult& result)
+{
+
+	MinkowskiSumShape combi(m_convexA,m_convexB);
+	MinkowskiSumShape* convex = &combi;
+
+	SimdTransform	rayFromLocalA;
+	SimdTransform	rayToLocalA;
+
+	rayFromLocalA = fromA.inverse()* fromB;
+	rayToLocalA = toA.inverse()* toB;
+
+
+	m_simplexSolver->reset();
+
+	convex->SetTransformB(SimdTransform(rayFromLocalA.getBasis()));
+
+	//float radius = 0.01f;
+
+	SimdScalar lambda = 0.f;
+	//todo: need to verify this:
+	//because of minkowski difference, we need the inverse direction
+	
+	SimdVector3 s = -rayFromLocalA.getOrigin();
+	SimdVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin());
+	SimdVector3 x = s;
+	SimdVector3 v;
+	SimdVector3 arbitraryPoint = convex->LocalGetSupportingVertex(r);
+	
+	v = x - arbitraryPoint;
+
+	int maxIter = MAX_ITERATIONS;
+
+	SimdVector3 n;
+	n.setValue(0.f,0.f,0.f);
+	bool hasResult = false;
+	SimdVector3 c;
+
+	float lastLambda = lambda;
+
+
+	float dist2 = v.length2();
+	float epsilon = 0.0001f;
+
+	SimdVector3	w,p;
+	float VdotR;
+	
+	while ( (dist2 > epsilon) && maxIter--)
+	{
+		p = convex->LocalGetSupportingVertex( v);
+		 w = x - p;
+
+		float VdotW = v.dot(w);
+
+		if ( VdotW > 0.f)
+		{
+			VdotR = v.dot(r);
+
+			if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
+				return false;
+			else
+			{
+				lambda = lambda - VdotW / VdotR;
+				x = s + lambda * r;
+				m_simplexSolver->reset();
+				//check next line
+				w = x-p;
+				lastLambda = lambda;
+				n = v;
+				hasResult = true;
+			}
+		} 
+		m_simplexSolver->addVertex( w, x , p);
+		if (m_simplexSolver->closest(v))
+		{
+			dist2 = v.length2();
+			hasResult = true;
+			//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
+			//printf("DIST2=%f\n",dist2);
+			//printf("numverts = %i\n",m_simplexSolver->numVertices());
+		} else
+		{
+			dist2 = 0.f;
+		} 
+	}
+
+	//int numiter = MAX_ITERATIONS - maxIter;
+//	printf("number of iterations: %d", numiter);
+	result.m_fraction = lambda;
+	result.m_normal = n;
+
+	return true;
+}
+
+
+

src/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 ConvexShape;
+
+/// SubsimplexConvexCast implements Gino van den Bergens' paper
+///"Ray Casting against General 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 SubsimplexConvexCast : public ConvexCast
+{
+	SimplexSolverInterface* m_simplexSolver;
+	ConvexShape*	m_convexA;
+	ConvexShape*	m_convexB;
+
+public:
+
+	SubsimplexConvexCast (ConvexShape*	shapeA,ConvexShape*	shapeB,SimplexSolverInterface* simplexSolver);
+
+	//virtual ~SubsimplexConvexCast();
+	///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 GjkPairDetector.
+	virtual bool	calcTimeOfImpact(
+			const SimdTransform& fromA,
+			const SimdTransform& toA,
+			const SimdTransform& fromB,
+			const SimdTransform& toB,
+			CastResult& result);
+
+};
+
+#endif //SUBSIMPLEX_CONVEX_CAST_H

src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp

@@ -0,0 +1,598 @@
+
+/*
+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"
+#include <assert.h>
+#include <stdio.h>
+
+#define VERTA  0
+#define VERTB  1
+#define VERTC  2
+#define VERTD  3
+
+#define CATCH_DEGENERATE_TETRAHEDRON 1
+void	VoronoiSimplexSolver::removeVertex(int index)
+{
+	
+	assert(m_numVertices>0);
+	m_numVertices--;
+	m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
+	m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
+	m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
+}
+
+void	VoronoiSimplexSolver::ReduceVertices (const UsageBitfield& 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 VoronoiSimplexSolver::reset()
+{
+	m_cachedValidClosest = false;
+	m_numVertices = 0;
+	m_needsUpdate = true;
+	m_lastW = SimdVector3(1e30f,1e30f,1e30f);
+	m_cachedBC.Reset();
+}
+
+
+
+	//add a vertex
+void VoronoiSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& 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	VoronoiSimplexSolver::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(1.f,0.f,0.f,0.f);
+				m_cachedValidClosest = m_cachedBC.IsValid();
+				break;
+			};
+		case 2:
+			{
+			//closest point origin from line segment
+					const SimdVector3& from = m_simplexVectorW[0];
+					const SimdVector3& to = m_simplexVectorW[1];
+					SimdVector3 nearest;
+
+					SimdVector3 p (0.f,0.f,0.f);
+					SimdVector3 diff = p - from;
+					SimdVector3 v = to - from;
+					float t = v.dot(diff);
+					
+					if (t > 0) {
+						float 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
+				SimdVector3 p (0.f,0.f,0.f);
+				
+				const SimdVector3& a = m_simplexVectorW[0];
+				const SimdVector3& b = m_simplexVectorW[1];
+				const SimdVector3& c = m_simplexVectorW[2];
+
+				ClosestPtPointTriangle(p,a,b,c,m_cachedBC);
+				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+								m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+								m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+								m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+					m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+					m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+					m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+				m_cachedV = m_cachedP1-m_cachedP2;
+
+				ReduceVertices (m_cachedBC.m_usedVertices);
+				m_cachedValidClosest =  m_cachedBC.IsValid();
+
+				break;
+			}
+		case 4:
+			{
+
+				
+				SimdVector3 p (0.f,0.f,0.f);
+				
+				const SimdVector3& a = m_simplexVectorW[0];
+				const SimdVector3& b = m_simplexVectorW[1];
+				const SimdVector3& c = m_simplexVectorW[2];
+				const SimdVector3& 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(0.f,0.f,0.f);
+					}
+					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 VoronoiSimplexSolver::closest(SimdVector3& v)
+{
+	bool succes = UpdateClosestVectorAndPoints();
+	v = m_cachedV;
+	return succes;
+}
+
+
+
+SimdScalar VoronoiSimplexSolver::maxVertex()
+{
+	int i, numverts = numVertices();
+	SimdScalar maxV = 0.f;
+	for (i=0;i<numverts;i++)
+	{
+		SimdScalar curLen2 = m_simplexVectorW[i].length2();
+		if (maxV < curLen2)
+			maxV = curLen2;
+	}
+	return maxV;
+}
+
+
+
+	//return the current simplex
+int VoronoiSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *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 VoronoiSimplexSolver::inSimplex(const SimdVector3& w)
+{
+	bool found = false;
+	int i, numverts = numVertices();
+	//SimdScalar maxV = 0.f;
+	
+	//w is in the current (reduced) simplex
+	for (i=0;i<numverts;i++)
+	{
+		if (m_simplexVectorW[i] == w)
+			found = true;
+	}
+
+	//check in case lastW is already removed
+	if (w == m_lastW)
+		return true;
+    	
+	return found;
+}
+
+void VoronoiSimplexSolver::backup_closest(SimdVector3& v) 
+{
+	v = m_cachedV;
+}
+
+
+bool VoronoiSimplexSolver::emptySimplex() const 
+{
+	return (numVertices() == 0);
+
+}
+
+void VoronoiSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2) 
+{
+	UpdateClosestVectorAndPoints();
+	p1 = m_cachedP1;
+	p2 = m_cachedP2;
+
+}
+
+
+
+
+bool	VoronoiSimplexSolver::ClosestPtPointTriangle(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c,SubSimplexClosestResult& result)
+{
+	result.m_usedVertices.reset();
+
+    // Check if P in vertex region outside A
+    SimdVector3 ab = b - a;
+    SimdVector3 ac = c - a;
+    SimdVector3 ap = p - a;
+    float d1 = ab.dot(ap);
+    float d2 = ac.dot(ap);
+    if (d1 <= 0.0f && d2 <= 0.0f) 
+	{
+		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
+    SimdVector3 bp = p - b;
+    float d3 = ab.dot(bp);
+    float d4 = ac.dot(bp);
+    if (d3 >= 0.0f && 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
+    float vc = d1*d4 - d3*d2;
+    if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
+        float 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
+    SimdVector3 cp = p - c;
+    float d5 = ab.dot(cp);
+    float d6 = ac.dot(cp);
+    if (d6 >= 0.0f && 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
+    float vb = d5*d2 - d1*d6;
+    if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
+        float 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
+    float va = d3*d6 - d5*d4;
+    if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
+        float 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)
+    float denom = 1.0f / (va + vb + vc);
+    float v = vb * denom;
+    float 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 = 1.0f - v - w
+
+}
+
+
+
+
+
+/// Test if point p and d lie on opposite sides of plane through abc
+int VoronoiSimplexSolver::PointOutsideOfPlane(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d)
+{
+	SimdVector3 normal = (b-a).cross(c-a);
+
+    float signp = (p - a).dot(normal); // [AP AB AC]
+    float signd = (d - a).dot( normal); // [AD AB AC]
+
+#ifdef CATCH_DEGENERATE_TETRAHEDRON
+	if (signd * signd < (1e-4f * 1e-4f))
+	{
+//		printf("affine dependent/degenerate\n");//
+		return -1;
+	}
+#endif
+	// Points on opposite sides if expression signs are opposite
+    return signp * signd < 0.f;
+}
+
+
+bool	VoronoiSimplexSolver::ClosestPtPointTetrahedron(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d, SubSimplexClosestResult& finalResult)
+{
+	SubSimplexClosestResult 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;
+	 }
+
+
+    float bestSqDist = FLT_MAX;
+    // If point outside face abc then compute closest point on abc
+	if (pointOutsideABC) 
+	{
+        ClosestPtPointTriangle(p, a, b, c,tempResult);
+		SimdPoint3 q = tempResult.m_closestPointOnSimplex;
+		
+        float 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);
+		SimdPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        float 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);
+		SimdPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        float sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
+			finalResult.SetBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTC],
+					0,
+					tempResult.m_barycentricCoords[VERTB]
+			);
+
+		}
+    }
+    // Repeat test for face bdc
+    
+
+	if (pointOutsideBDC)
+	{
+        ClosestPtPointTriangle(p, b, d, c,tempResult);
+		SimdPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+        float sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+
+			finalResult.SetBarycentricCoordinates(
+					0,
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTC],
+					tempResult.m_barycentricCoords[VERTB]
+			);
+
+		}
+    }
+
+	//help! we ended up full !
+	
+	if (finalResult.m_usedVertices.usedVertexA &&
+		finalResult.m_usedVertices.usedVertexB &&
+		finalResult.m_usedVertices.usedVertexC &&
+		finalResult.m_usedVertices.usedVertexD) 
+	{
+		return true;
+	}
+
+    return true;
+}
+

src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h

@@ -0,0 +1,157 @@
+/*
+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 VoronoiSimplexSolver_H
+#define VoronoiSimplexSolver_H
+
+#include "btSimplexSolverInterface.h"
+
+
+
+#define VORONOI_SIMPLEX_MAX_VERTS 5
+
+struct UsageBitfield{
+	UsageBitfield()
+	{
+		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	SubSimplexClosestResult
+{
+	SimdPoint3	m_closestPointOnSimplex;
+	//MASK for m_usedVertices
+	//stores the simplex vertex-usage, using the MASK, 
+	// if m_usedVertices & MASK then the related vertex is used
+	UsageBitfield	m_usedVertices;
+	float	m_barycentricCoords[4];
+	bool m_degenerate;
+
+	void	Reset()
+	{
+		m_degenerate = false;
+		SetBarycentricCoordinates();
+		m_usedVertices.reset();
+	}
+	bool	IsValid()
+	{
+		bool valid = (m_barycentricCoords[0] >= 0.f) &&
+			(m_barycentricCoords[1] >= 0.f) &&
+			(m_barycentricCoords[2] >= 0.f) &&
+			(m_barycentricCoords[3] >= 0.f);
+
+
+		return valid;
+	}
+	void	SetBarycentricCoordinates(float a=0.f,float b=0.f,float c=0.f,float d=0.f)
+	{
+		m_barycentricCoords[0] = a;
+		m_barycentricCoords[1] = b;
+		m_barycentricCoords[2] = c;
+		m_barycentricCoords[3] = d;
+	}
+
+};
+
+/// VoronoiSimplexSolver 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 VoronoiSimplexSolver
+#else
+class VoronoiSimplexSolver : public SimplexSolverInterface
+#endif
+{
+public:
+
+	int	m_numVertices;
+
+	SimdVector3	m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
+	SimdPoint3	m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
+	SimdPoint3	m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
+
+	
+
+	SimdPoint3	m_cachedP1;
+	SimdPoint3	m_cachedP2;
+	SimdVector3	m_cachedV;
+	SimdVector3	m_lastW;
+	bool		m_cachedValidClosest;
+
+	SubSimplexClosestResult m_cachedBC;
+
+	bool	m_needsUpdate;
+	
+	void	removeVertex(int index);
+	void	ReduceVertices (const UsageBitfield& usedVerts);
+	bool	UpdateClosestVectorAndPoints();
+
+	bool	ClosestPtPointTetrahedron(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d, SubSimplexClosestResult& finalResult);
+	int		PointOutsideOfPlane(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d);
+	bool	ClosestPtPointTriangle(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c,SubSimplexClosestResult& result);
+
+public:
+
+	 void reset();
+
+	 void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q);
+
+
+	 bool closest(SimdVector3& v);
+
+	 SimdScalar maxVertex();
+
+	 bool fullSimplex() const
+	 {
+		 return (m_numVertices == 4);
+	 }
+
+	 int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const;
+
+	 bool inSimplex(const SimdVector3& w);
+	
+	 void backup_closest(SimdVector3& v) ;
+
+	 bool emptySimplex() const ;
+
+	 void compute_points(SimdPoint3& p1, SimdPoint3& p2) ;
+
+	 int numVertices() const 
+	 {
+		 return m_numVertices;
+	 }
+
+
+};
+
+#endif //VoronoiSimplexSolver