catch up with latest Bullet 2.x (not in use yet)

24 files changed, 1155 insertions, 83 deletions

diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h
index bbc5bd3f5d0..415c398a101 100644
--- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h
+++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h
@@ -38,7 +38,9 @@ struct btDispatcherInfo
 		DISPATCH_CONTINUOUS
 	};
 	btDispatcherInfo()
-		:m_dispatchFunc(DISPATCH_DISCRETE),
+		:m_timeStep(0.f),
+		m_stepCount(0),
+		m_dispatchFunc(DISPATCH_DISCRETE),
 		m_timeOfImpact(1.f),
 		m_useContinuous(false),
 		m_debugDraw(0),
diff --git a/extern/bullet2/src/BulletCollision/CMakeLists.txt b/extern/bullet2/src/BulletCollision/CMakeLists.txt
index 3169f15199d..e565bf7edea 100644
--- a/extern/bullet2/src/BulletCollision/CMakeLists.txt
+++ b/extern/bullet2/src/BulletCollision/CMakeLists.txt
@@ -47,6 +47,8 @@ ADD_LIBRARY(LibBulletCollision
 				CollisionShapes/btTriangleMesh.cpp
 				CollisionShapes/btTriangleMeshShape.cpp
 				NarrowPhaseCollision/btContinuousConvexCollision.cpp
+				NarrowPhaseCollision/btGjkEpa.cpp
+				NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
 				NarrowPhaseCollision/btConvexCast.cpp
 				NarrowPhaseCollision/btGjkConvexCast.cpp
 				NarrowPhaseCollision/btGjkPairDetector.cpp
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
index 22262340222..8fedba19922 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
@@ -45,4 +45,5 @@ private:
 
 	
 };
-#endif //SPHERE_TRIANGLE_DETECTOR_H
\ No newline at end of file
+#endif //SPHERE_TRIANGLE_DETECTOR_H
+
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index 7edd3e8abfe..775ed3e13d8 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -121,13 +121,13 @@ void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
 
 }
 
-void	btCollisionWorld::performDiscreteCollisionDetection()
+
+
+
+void	btCollisionWorld::performDiscreteCollisionDetection(btDispatcherInfo& dispatchInfo)
 {
 	BEGIN_PROFILE("performDiscreteCollisionDetection");
 
-	btDispatcherInfo	dispatchInfo;
-	dispatchInfo.m_timeStep = 0.f;
-	dispatchInfo.m_stepCount = 0;
 
 	//update aabb (of all moved objects)
 
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
index e2b8eb11238..e212e520bda 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
@@ -236,8 +236,8 @@ public:
 
 	void	removeCollisionObject(btCollisionObject* collisionObject);
 
-	virtual void	performDiscreteCollisionDetection();
-
+	virtual void	performDiscreteCollisionDetection(	btDispatcherInfo&	dispatchInfo);
+	
 };
 
 
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
index 36b0d15eac5..e896508bdb9 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
@@ -39,7 +39,9 @@ subject to the following restrictions:
 
 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
 
-//#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+
 
 #ifdef WIN32
 #if _MSC_VER >= 1310
@@ -59,7 +61,7 @@ subject to the following restrictions:
 
 #endif //USE_HULL
 
-bool gUseEpa = false;
+bool gUseEpa = true;
 
 
 #ifdef WIN32
@@ -113,13 +115,10 @@ void	btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
 
 
 
+static btGjkEpaPenetrationDepthSolver	gEpaPenetrationDepthSolver;
 static btMinkowskiPenetrationDepthSolver	gPenetrationDepthSolver;
 
-//static EpaPenetrationDepthSolver	gEpaPenetrationDepthSolver;
 
-#ifdef USE_EPA
-Solid3EpaPenetrationDepth	gSolidEpaPenetrationSolver;
-#endif //USE_EPA
 
 void	btConvexConvexAlgorithm::checkPenetrationDepthSolver()
 {
@@ -128,10 +127,7 @@ void	btConvexConvexAlgorithm::checkPenetrationDepthSolver()
 		m_useEpa  = gUseEpa;
 		if (m_useEpa)
 		{
-			
-		//	m_gjkPairDetector.setPenetrationDepthSolver(&gEpaPenetrationDepthSolver);
-						
-			
+			m_gjkPairDetector.setPenetrationDepthSolver(&gEpaPenetrationDepthSolver);
 		} else
 		{
 			m_gjkPairDetector.setPenetrationDepthSolver(&gPenetrationDepthSolver);
@@ -153,8 +149,21 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
 		m_ownManifold = true;
 	}
+	resultOut->setPersistentManifold(m_manifoldPtr);
 
-
+#ifdef USE_BT_GJKEPA
+	btConvexShape*				shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
+	btConvexShape*				shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
+	const btScalar				radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/);
+	btGjkEpaSolver::sResults	results;
+	if(btGjkEpaSolver::Collide(	shape0,body0->getWorldTransform(),
+								shape1,body1->getWorldTransform(),
+								radialmargin,results))
+		{
+		dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+		resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+		}
+#else
 	checkPenetrationDepthSolver();
 
 	btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
@@ -173,8 +182,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	input.m_transformA = body0->getWorldTransform();
 	input.m_transformB = body1->getWorldTransform();
 	
-	resultOut->setPersistentManifold(m_manifoldPtr);
 	m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+#endif
 
 }
 
@@ -274,3 +283,4 @@ float	btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC
 	return resultFraction;
 
 }
+
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp
index 13875fc5fe6..4cf11f1a977 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp
@@ -16,50 +16,71 @@ subject to the following restrictions:
 #include "btConeShape.h"
 #include "LinearMath/btPoint3.h"
 
-#ifdef WIN32
-static int coneindices[3] = {1,2,0};
-#else
-static int coneindices[3] = {2,1,0};
-#endif
+
 
 btConeShape::btConeShape (btScalar radius,btScalar height):
 m_radius (radius),
 m_height(height)
 {
+	setConeUpIndex(1);
 	btVector3 halfExtents;
 	m_sinAngle = (m_radius / sqrt(m_radius * m_radius + m_height * m_height));
 }
 
+///choose upAxis index
+void	btConeShape::setConeUpIndex(int upIndex)
+{
+	switch (upIndex)
+	{
+	case 0:
+			m_coneIndices[0] = 1;
+			m_coneIndices[1] = 0;
+			m_coneIndices[2] = 2;
+		break;
+	case 1:
+			m_coneIndices[0] = 0;
+			m_coneIndices[1] = 1;
+			m_coneIndices[2] = 2;
+		break;
+	case 2:
+			m_coneIndices[0] = 0;
+			m_coneIndices[1] = 2;
+			m_coneIndices[2] = 1;
+		break;
+	default:
+		assert(0);
+	};
+}
 
 btVector3 btConeShape::coneLocalSupport(const btVector3& v) const
 {
 	
 	float halfHeight = m_height * 0.5f;
 
- if (v[coneindices[1]] > v.length() * m_sinAngle)
+ if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
  {
 	btVector3 tmp;
 
-	tmp[coneindices[0]] = 0.f;
-	tmp[coneindices[1]] = halfHeight;
-	tmp[coneindices[2]] = 0.f;
+	tmp[m_coneIndices[0]] = 0.f;
+	tmp[m_coneIndices[1]] = halfHeight;
+	tmp[m_coneIndices[2]] = 0.f;
 	return tmp;
  }
   else {
-    btScalar s = btSqrt(v[coneindices[0]] * v[coneindices[0]] + v[coneindices[2]] * v[coneindices[2]]);
+    btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
     if (s > SIMD_EPSILON) {
       btScalar d = m_radius / s;
 	  btVector3 tmp;
-	  tmp[coneindices[0]] = v[coneindices[0]] * d;
-	  tmp[coneindices[1]] = -halfHeight;
-	  tmp[coneindices[2]] = v[coneindices[2]] * d;
+	  tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
+	  tmp[m_coneIndices[1]] = -halfHeight;
+	  tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
 	  return tmp;
     }
     else  {
 		btVector3 tmp;
-		tmp[coneindices[0]] = 0.f;
-		tmp[coneindices[1]] = -halfHeight;
-		tmp[coneindices[2]] = 0.f;
+		tmp[m_coneIndices[0]] = 0.f;
+		tmp[m_coneIndices[1]] = -halfHeight;
+		tmp[m_coneIndices[2]] = 0.f;
 		return tmp;
 	}
   }
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h
index 0fd3ce177fe..52d925fe714 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h
@@ -19,7 +19,7 @@ subject to the following restrictions:
 #include "btConvexShape.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
 
-/// implements cone shape interface
+///btConeShape implements a Cone shape, around the Y axis
 class btConeShape : public btConvexShape
 
 {
@@ -27,7 +27,7 @@ class btConeShape : public btConvexShape
 	float m_sinAngle;
 	float m_radius;
 	float m_height;
-	
+	int		m_coneIndices[3];
 	btVector3 coneLocalSupport(const btVector3& v) const;
 
 
@@ -76,8 +76,28 @@ public:
 		{
 			return "Cone";
 		}
+		
+		///choose upAxis index
+		void	setConeUpIndex(int upIndex);
+		
+		int	getConeUpIndex() const
+		{
+			return m_coneIndices[1];
+		}
 };
 
+///btConeShape implements a Cone shape, around the X axis
+class btConeShapeX : public btConeShape
+{
+	public:
+		btConeShapeX(btScalar radius,btScalar height);
+};
 
+///btConeShapeZ implements a Cone shape, around the Z axis
+class btConeShapeZ : public btConeShape
+{
+	public:
+		btConeShapeZ(btScalar radius,btScalar height);
+};
 #endif //CONE_MINKOWSKI_H
 
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
index 078ba38fdf4..54864c32f3a 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
@@ -39,3 +39,4 @@ void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int  trian
 			
 		m_triangleBuffer.push_back(tri);
 }
+
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
index 4f421a616c8..d04341aa809 100644
--- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
@@ -57,4 +57,5 @@ public:
 };
 
 
-#endif //BT_TRIANGLE_BUFFER_H
\ No newline at end of file
+#endif //BT_TRIANGLE_BUFFER_H
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp
new file mode 100644
index 00000000000..55123d5adb6
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp
@@ -0,0 +1,709 @@
+/*
+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.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson
+Nov.2006
+*/
+
+
+#include "btGjkEpa.h"
+#include <string.h> //for memset
+
+namespace gjkepa_impl
+{
+
+//
+// Port. typedefs
+//
+
+typedef	btScalar		F;
+typedef bool			Z;
+typedef int				I;
+typedef unsigned int	U;
+typedef unsigned char	U1;
+typedef unsigned short	U2;
+
+typedef btVector3		Vector3;
+typedef btMatrix3x3		Rotation;
+
+//
+// Config
+//
+
+#if 0
+#define BTLOCALSUPPORT	localGetSupportingVertexWithoutMargin
+#else
+#define BTLOCALSUPPORT	localGetSupportingVertex
+#endif
+
+//
+// Const
+//
+
+static const U			chkPrecision		=1/U(sizeof(F)==4);
+
+static const F			cstInf				=F(1/sin(0.));
+static const F			cstPi				=F(acos(-1.));
+static const F			cst2Pi				=cstPi*2;
+
+static const U			GJK_maxiterations	=128;
+static const U			GJK_hashsize		=1<<6;
+static const U			GJK_hashmask		=GJK_hashsize-1;
+static const F			GJK_insimplex_eps	=F(0.0001);
+static const F			GJK_sqinsimplex_eps	=GJK_insimplex_eps*GJK_insimplex_eps;
+
+static const U			EPA_maxiterations	=256;
+static const F			EPA_inface_eps		=F(0.01);
+static const F			EPA_accuracy		=F(0.001);
+
+//
+// Utils
+//
+
+static inline F								Abs(F v)					{ return(v<0?-v:v); }
+static inline F								Sign(F v)					{ return(F(v<0?-1:1)); }
+template <typename T> static inline void	Swap(T& a,T& b)				{ T 
+t(a);a=b;b=t; }
+template <typename T> static inline T		Min(const T& a,const T& b)	{ 
+return(a<b?a:b); }
+template <typename T> static inline T		Max(const T& a,const T& b)	{ 
+return(a>b?a:b); }
+static inline void							ClearMemory(void* p,U sz)	{ memset(p,0,(size_t)sz); 
+}
+#if 0
+template <typename T> static inline void	Raise(const T& object)		{ 
+throw(object); }
+#else
+template <typename T> static inline void	Raise(const T&)				{}
+#endif
+static inline F								Det(const Vector3& a,const Vector3& b,const Vector3& 
+c,const Vector3& d)
+		{
+		return(	-(a.z()*b.y()*c.x()) + a.y()*b.z()*c.x() + a.z()*b.x()*c.y() - 
+a.x()*b.z()*c.y() - a.y()*b.x()*c.z() + a.x()*b.y()*c.z() +
+				a.z()*b.y()*d.x() - a.y()*b.z()*d.x() - a.z()*c.y()*d.x() + 
+b.z()*c.y()*d.x() + a.y()*c.z()*d.x() - b.y()*c.z()*d.x() -
+				a.z()*b.x()*d.y() + a.x()*b.z()*d.y() + a.z()*c.x()*d.y() - 
+b.z()*c.x()*d.y() - a.x()*c.z()*d.y() + b.x()*c.z()*d.y() +
+				a.y()*b.x()*d.z() - a.x()*b.y()*d.z() - a.y()*c.x()*d.z() + 
+b.y()*c.x()*d.z() + a.x()*c.y()*d.z() - b.x()*c.y()*d.z());
+		}
+
+//
+// StackAlloc
+//
+struct StackAlloc
+	{
+	struct Block
+		{
+		Block*	previous;
+		U1*		address;
+		};
+				StackAlloc()		{ ctor(); }
+				StackAlloc(U size)	{ ctor();Create(size); }
+				~StackAlloc()		{ Free(); }
+	void		Create(U size)
+		{
+		Free();
+		data		=	new U1[size];
+		totalsize	=	size;
+		}
+	StackAlloc*	CreateChild(U size)
+		{
+		StackAlloc*	sa(Allocate<StackAlloc>());
+		sa->ischild		=	true;
+		sa->data		=	Allocate(size);
+		sa->totalsize	=	size;
+		sa->usedsize	=	0;
+		sa->current		=	0;
+		return(sa);
+		}
+	void		Free()
+		{
+		if(usedsize==0)
+			{
+			if(!ischild)		delete[] data;
+			data				=	0;
+			usedsize			=	0;
+			} else Raise(L"StackAlloc is still in use");
+		}
+	Block*		BeginBlock()
+		{
+		Block*	pb(Allocate<Block>());
+		pb->previous	=	current;
+		pb->address		=	data+usedsize;
+		current			=	pb;
+		return(pb);
+		}
+	void		EndBlock(Block* block)
+		{
+		if(block==current)
+			{
+			current		=	block->previous;
+			usedsize	=	(U)((block->address-data)-sizeof(Block));
+			} else Raise(L"Unmatched blocks");
+		}
+	U1*			Allocate(U size)
+		{
+		const U	nus(usedsize+size);
+		if(nus<totalsize)
+			{
+			usedsize=nus;
+			return(data+(usedsize-size));
+			}
+		Raise(L"Not enough memory");
+		return(0);
+		}
+	template <typename T> T*	Allocate()				{
+return((T*)Allocate((U)sizeof(T))); }
+	template <typename T> T*	AllocateArray(U count)	{
+return((T*)Allocate((U)sizeof(T)*count)); }
+	private:
+	void		ctor()
+		{
+		data		=	0;
+		totalsize	=	0;
+		usedsize	=	0;
+		current		=	0;
+		ischild		=	false;
+		}
+	U1*		data;
+	U		totalsize;
+	U		usedsize;
+	Block*	current;
+	Z		ischild;
+	};
+
+//
+// GJK
+//
+struct	GJK
+	{
+	struct Mkv
+		{
+		Vector3	w;		/* Minkowski vertice	*/
+		Vector3	r;		/* Ray					*/
+		};
+	struct He
+		{
+		Vector3	v;
+		He*		n;
+		};
+	static const U			hashsize=64;
+	StackAlloc*				sa;
+	StackAlloc::Block*		sablock;
+	He*						table[hashsize];
+	Rotation				wrotations[2];
+	Vector3					positions[2];
+	const btConvexShape*	shapes[2];
+	Mkv						simplex[5];
+	Vector3					ray;
+	U						order;
+	U						iterations;
+	F						margin;
+	Z						failed;
+	//
+					GJK(StackAlloc* psa,
+						const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0,
+						const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1,
+						F pmargin=0)
+		{
+		wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0;
+		wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1;
+		sa		=psa;
+		sablock	=sa->BeginBlock();
+		margin	=pmargin;
+		failed	=false;
+		}
+	//
+					~GJK()
+		{
+		sa->EndBlock(sablock);
+		}
+	// vdh : very dumm hash
+	static inline U	Hash(const Vector3& v)
+		{
+                const U h((U)(v[0]*15461)^(U)(v[1]*83003)^(U)(v[2]*15473));
+		return(((*((const U*)&h))*169639)&GJK_hashmask);
+		}
+	//
+	inline Vector3	LocalSupport(const Vector3& d,U i) const
+		{
+		return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]);
+		}
+	//
+	inline void		Support(const Vector3& d,Mkv& v) const
+		{
+		v.r	=	d;
+		v.w	=	LocalSupport(d,0)-LocalSupport(-d,1)+d*margin;
+		}
+	#define SPX(_i_)	simplex[_i_]
+	#define SPXW(_i_)	simplex[_i_].w
+	//
+	inline Z		FetchSupport()
+		{
+		const U			h(Hash(ray));
+		He*				e(table[h]);
+		while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; }
+		e=sa->Allocate<He>();e->v=ray;e->n=table[h];table[h]=e;
+		Support(ray,simplex[++order]);
+		return(ray.dot(SPXW(order))>0);
+		}
+	//
+	inline Z		SolveSimplex2(const Vector3& ao,const Vector3& ab)
+		{
+		if(ab.dot(ao)>=0)
+			{
+			const Vector3	cabo(cross(ab,ao));
+			if(cabo.length2()>GJK_sqinsimplex_eps)
+				{ ray=cross(cabo,ab); }
+				else
+				{ return(true); }
+			}
+			else
+			{ order=0;SPX(0)=SPX(1);ray=ao;	}
+		return(false);
+		}
+	//
+	inline Z		SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& 
+ac)
+		{
+		return(SolveSimplex3a(ao,ab,ac,cross(ab,ac)));
+		}
+	//
+	inline Z		SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& 
+ac,const Vector3& cabc)
+		{
+				if((cross(cabc,ab)).dot(ao)<-GJK_insimplex_eps)
+			{ order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab));	}
+		else	if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps)
+			{ order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); }
+		else
+			{
+			const F			d(cabc.dot(ao));
+			if(Abs(d)>GJK_insimplex_eps)
+				{
+				if(d>0)
+					{ ray=cabc; }
+					else
+					{ ray=-cabc;Swap(SPX(0),SPX(1)); }
+				return(false);
+				} else return(true);
+			}
+		}
+	//
+	inline Z		SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& 
+ac,const Vector3& ad)
+		{
+		Vector3			crs;
+				if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps)
+			{ 
+order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); 
+}
+		else	if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps)
+			{ order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); }
+		else	if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps)
+			{ 
+order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); 
+}
+		else	return(true);
+		}
+	//
+	inline Z		SearchOrigin(const Vector3& initray=Vector3(1,0,0))
+		{
+		iterations	=	0;
+		order		=	0;
+		failed		=	false;
+		Support(initray,simplex[0]);ray=-SPXW(0);
+		ClearMemory(table,sizeof(void*)*hashsize);
+		for(;iterations<GJK_maxiterations;++iterations)
+			{
+			const F		rl(ray.length());
+			ray/=rl>0?rl:1;
+			if(FetchSupport())
+				{
+				Z	found(false);
+				switch(order)
+					{
+					case	1:	found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break;
+					case	2:	found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break;
+					case	3:	found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break;
+					}
+				if(found) return(true);
+				} else return(false);
+			}
+		failed=true;
+		return(false);
+		}
+	//
+	inline Z		EncloseOrigin()
+		{
+		switch(order)
+			{
+			/* Point		*/
+			case	0:	break;
+			/* Line			*/
+			case	1:
+				{
+				const Vector3	ab(SPXW(1)-SPXW(0));
+				const Vector3	b[]={	cross(ab,Vector3(1,0,0)),
+										cross(ab,Vector3(0,1,0)),
+										cross(ab,Vector3(0,0,1))};
+				const F			m[]={b[0].length2(),b[1].length2(),b[2].length2()};
+				const Rotation	r(btQuaternion(ab.normalized(),cst2Pi/3));
+				Vector3			w(b[m[0]>m[1]?m[0]>m[2]?0:2:m[1]>m[2]?1:2]);
+				Support(w.normalized(),simplex[4]);w=r*w;
+				Support(w.normalized(),simplex[2]);w=r*w;
+				Support(w.normalized(),simplex[3]);w=r*w;
+				order=4;
+				return(true);
+				}
+			break;
+			/* Triangle		*/
+			case	2:
+				{
+				const 
+Vector3	n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized());
+				Support( n,simplex[3]);
+				Support(-n,simplex[4]);
+				order=4;
+				return(true);
+				}
+			break;
+			/* Tetrahedron	*/
+			case	3:	return(true);
+			/* Hexahedron	*/
+			case	4:	return(true);
+			}
+		return(false);
+		}
+	#undef SPX
+	#undef SPXW
+	};
+
+//
+// EPA
+//
+struct	EPA
+	{
+	//
+	struct Face
+		{
+		const GJK::Mkv*	v[3];
+		Face*			f[3];
+		U				e[3];
+		Vector3			n;
+		F				d;
+		U				mark;
+		Face*			prev;
+		Face*			next;
+		Face()			{}
+		};
+	//
+	GJK*			gjk;
+	StackAlloc*		sa;
+	Face*			root;
+	U				nfaces;
+	U				iterations;
+	Vector3			features[2][3];
+	Vector3			nearest[2];
+	Vector3			normal;
+	F				depth;
+	Z				failed;
+	//
+					EPA(GJK* pgjk)
+		{
+		gjk		=	pgjk;
+		sa		=	pgjk->sa;
+		}
+	//
+					~EPA()
+		{
+		}
+	//
+	inline Vector3	GetCoordinates(const Face* face) const
+		{
+		const Vector3	o(face->n*-face->d);
+		const F			a[]={	cross(face->v[0]->w-o,face->v[1]->w-o).length(),
+								cross(face->v[1]->w-o,face->v[2]->w-o).length(),
+								cross(face->v[2]->w-o,face->v[0]->w-o).length()};
+		const F			sm(a[0]+a[1]+a[2]);
+		return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1));
+		}
+	//
+	inline Face*	FindBest() const
+		{
+		Face*	bf(0);
+		if(root)
+			{
+			Face*	cf(root);
+			F		bd(cstInf);
+			do	{
+				if(cf->d<bd) { bd=cf->d;bf=cf; }
+				} while(0!=(cf=cf->next));
+			}
+		return(bf);
+		}
+	//
+	inline Z		Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* 
+c) const
+		{
+		const Vector3	nrm(cross(b->w-a->w,c->w-a->w));
+		const F			len(nrm.length());
+		const Z			valid(	(cross(a->w,b->w).dot(nrm)>=-EPA_inface_eps)&&
+								(cross(b->w,c->w).dot(nrm)>=-EPA_inface_eps)&&
+								(cross(c->w,a->w).dot(nrm)>=-EPA_inface_eps));
+		f->v[0]	=	a;
+		f->v[1]	=	b;
+		f->v[2]	=	c;
+		f->mark	=	0;
+		f->n	=	nrm/(len>0?len:cstInf);
+		f->d	=	Max<F>(0,-f->n.dot(a->w));
+		return(valid);
+		}
+	//
+	inline Face*	NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c)
+		{
+		Face*	pf(sa->Allocate<Face>());
+		if(Set(pf,a,b,c))
+			{
+			if(root) root->prev=pf;
+			pf->prev=0;
+			pf->next=root;
+			root	=pf;
+			++nfaces;
+			}
+			else
+			{
+			pf->prev=pf->next=0;
+			}
+		return(pf);
+		}
+	//
+	inline void		Detach(Face* face)
+		{
+		if(face->prev||face->next)
+			{
+			--nfaces;
+			if(face==root)
+				{ root=face->next;root->prev=0; }
+				else
+				{
+				if(face->next==0)
+					{ face->prev->next=0; }
+					else
+					{ face->prev->next=face->next;face->next->prev=face->prev; }
+				}
+			face->prev=face->next=0;
+			}
+		}
+	//
+	inline void		Link(Face* f0,U e0,Face* f1,U e1) const
+		{
+		f0->f[e0]=f1;f1->e[e1]=e0;
+		f1->f[e1]=f0;f0->e[e0]=e1;
+		}
+	//
+	GJK::Mkv*		Support(const Vector3& w) const
+		{
+		GJK::Mkv*		v(sa->Allocate<GJK::Mkv>());
+		gjk->Support(w,*v);
+		return(v);
+		}
+	//
+	U				BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& 
+ff)
+		{
+		static const U	mod3[]={0,1,2,0,1};
+		U				ne(0);
+		if(f.mark!=markid)
+			{
+			const U	e1(mod3[e+1]);
+			if((f.n.dot(w->w)+f.d)>0)
+				{
+				Face*	nf(NewFace(f.v[e1],f.v[e],w));
+				Link(nf,0,&f,e);
+				if(cf) Link(cf,1,nf,2); else ff=nf;
+				cf=nf;ne=1;
+				}
+				else
+				{
+				const U	e2(mod3[e+2]);
+				Detach(&f);
+				f.mark	=	markid;
+				ne		+=	BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff);
+				ne		+=	BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff);
+				}
+			}
+		return(ne);
+		}
+	//
+	inline F		EvaluatePD(F accuracy=EPA_accuracy)
+		{
+		StackAlloc::Block*	sablock(sa->BeginBlock());
+		Face*				bestface(0);
+		U					markid(1);
+		depth		=	-cstInf;
+		normal		=	Vector3(0,0,0);
+		root		=	0;
+		nfaces		=	0;
+		iterations	=	0;
+		failed		=	false;
+		/* Prepare hull		*/
+		if(gjk->EncloseOrigin())
+			{
+			const U*		pfidx(0);
+			U				nfidx(0);
+			const U*		peidx(0);
+			U				neidx(0);
+			GJK::Mkv*		basemkv[5];
+			Face*			basefaces[6];
+			switch(gjk->order)
+				{
+				/* Tetrahedron		*/
+				case	3:	{
+							static const U	fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}};
+							static const 
+U	eidx[6][4]={{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}};
+							pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6;
+							}	break;
+				/* Hexahedron		*/
+				case	4:	{
+							static const 
+U	fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}};
+							static const 
+U	eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}};
+							pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9;
+							}	break;
+				}
+			for(U i=0;i<=gjk->order;++i)		{ 
+basemkv[i]=sa->Allocate<GJK::Mkv>();*basemkv[i]=gjk->simplex[i]; }
+			for(U i=0;i<nfidx;++i,pfidx+=3)		{ 
+basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); 
+}
+			for(U i=0;i<neidx;++i,peidx+=4)		{ 
+Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); }
+			}
+		if(0==nfaces)
+			{
+			sa->EndBlock(sablock);
+			return(depth);
+			}
+		/* Expand hull		*/
+		for(;iterations<EPA_maxiterations;++iterations)
+			{
+			Face*		bf(FindBest());
+			if(bf)
+				{
+				GJK::Mkv*	w(Support(-bf->n));
+				const F		d(bf->n.dot(w->w)+bf->d);
+				bestface	=	bf;
+				if(d<-accuracy)
+					{
+					Face*	cf(0);
+					Face*	ff(0);
+					U		nf(0);
+					Detach(bf);
+					bf->mark=++markid;
+					for(U i=0;i<3;++i)	{ 
+nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); }
+					if(nf<=2)			{ break; }
+					Link(cf,1,ff,2);
+					} else break;
+				} else break;
+			}
+		/* Extract contact	*/
+		if(bestface)
+			{
+			const Vector3	b(GetCoordinates(bestface));
+			normal			=	bestface->n;
+			depth			=	Max<F>(0,bestface->d);
+			for(U i=0;i<2;++i)
+				{
+				const F	s(F(i?-1:1));
+				for(U j=0;j<3;++j)
+					{
+					features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i);
+					}
+				}
+			nearest[0]		=	features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z();
+			nearest[1]		=	features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z();
+			} else failed=true;
+		sa->EndBlock(sablock);
+		return(depth);
+		}
+	};
+}
+
+//
+// Api
+//
+
+using namespace	gjkepa_impl;
+
+/* Need some kind of stackalloc , create a static one till bullet provide
+one.	*/
+static const U		g_sasize((1024<<10)*2);
+static StackAlloc	g_sa(g_sasize);
+
+//
+bool	btGjkEpaSolver::Collide(btConvexShape *shape0,const btTransform &wtrs0,
+								btConvexShape *shape1,const btTransform &wtrs1,
+								btScalar	radialmargin,
+								sResults&	results)
+{
+/* Initialize					*/
+results.witnesses[0]	=
+results.witnesses[1]	=
+results.normal			=	Vector3(0,0,0);
+results.depth			=	0;
+results.status			=	sResults::Separated;
+results.epa_iterations	=	0;
+results.gjk_iterations	=	0;
+/* Use GJK to locate origin		*/
+GJK			gjk(&g_sa,
+				wtrs0.getBasis(),wtrs0.getOrigin(),shape0,
+				wtrs1.getBasis(),wtrs1.getOrigin(),shape1,
+				radialmargin+EPA_accuracy);
+const Z		collide(gjk.SearchOrigin());
+results.gjk_iterations	=	gjk.iterations+1;
+if(collide)
+	{
+	/* Then EPA for penetration depth	*/
+	EPA			epa(&gjk);
+	const F		pd(epa.EvaluatePD());
+	results.epa_iterations	=	epa.iterations+1;
+	if(pd>0)
+		{
+		results.status			=	sResults::Penetrating;
+		results.normal			=	epa.normal;
+		results.depth			=	pd;
+		results.witnesses[0]	=	epa.nearest[0];
+		results.witnesses[1]	=	epa.nearest[1];
+		return(true);
+		} else { if(epa.failed) results.status=sResults::EPA_Failed; }
+	} else { if(gjk.failed) results.status=sResults::GJK_Failed; }
+return(false);
+}
+
+
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h
new file mode 100644
index 00000000000..d26ac9cdfe5
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.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.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson
+Nov.2006
+*/
+
+
+#ifndef _05E48D53_04E0_49ad_BB0A_D74FE62E7366_
+#define _05E48D53_04E0_49ad_BB0A_D74FE62E7366_
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+///btGjkEpaSolver contributed under zlib by Nathanael Presson
+struct	btGjkEpaSolver
+{
+struct	sResults
+	{
+	enum eStatus
+		{
+		Separated,		/* Shapes doesnt penetrate												*/ 
+		Penetrating,	/* Shapes are penetrating												*/ 
+		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
+		EPA_Failed,		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+		}		status;
+	btVector3	witnesses[2];
+	btVector3	normal;
+	btScalar	depth;
+	int	epa_iterations;
+	int	gjk_iterations;
+	};
+static bool	Collide(btConvexShape* shape0,const btTransform& wtrs0,
+					btConvexShape* shape1,const btTransform& wtrs1,
+					btScalar	radialmargin,
+					sResults&	results);
+};
+
+#endif
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
new file mode 100644
index 00000000000..c2958b22b6a
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
@@ -0,0 +1,47 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
+
+bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
+											  btConvexShape* pConvexA, btConvexShape* pConvexB,
+											  const btTransform& transformA, const btTransform& transformB,
+											  btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
+											  class btIDebugDraw* debugDraw )
+{
+
+
+	const btScalar				radialmargin(0.f);
+	
+	btGjkEpaSolver::sResults	results;
+	if(btGjkEpaSolver::Collide(	pConvexA,transformA,
+								pConvexB,transformB,
+								radialmargin,results))
+		{
+	//	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+		//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+		wWitnessOnA = results.witnesses[0];
+		wWitnessOnB = results.witnesses[1];
+		return true;		
+		}
+
+	return false;
+}
+
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
new file mode 100644
index 00000000000..a7332a7a51a
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
@@ -0,0 +1,39 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H
+#define BT_GJP_EPA_PENETRATION_DEPTH_H
+
+#include "btConvexPenetrationDepthSolver.h"
+
+///EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
+///calculate the penetration depth between two convex shapes.
+class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
+{
+	public :
+
+		bool			calcPenDepth( btSimplexSolverInterface& simplexSolver,
+									  btConvexShape* pConvexA, btConvexShape* pConvexB,
+									  const btTransform& transformA, const btTransform& transformB,
+									  btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
+									  class btIDebugDraw* debugDraw );
+
+	private :
+
+};
+
+#endif	// BT_GJP_EPA_PENETRATION_DEPTH_H
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index 507fcaf21f7..646c07a5d39 100644
--- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -25,6 +25,9 @@ subject to the following restrictions:
 //must be above the machine epsilon
 #define REL_ERROR2 1.0e-6f
 
+//temp globals, to improve GJK/EPA/penetration calculations
+int gNumDeepPenetrationChecks = 0;
+int gNumGjkChecks = 0;
 
 #ifdef __SPU__
 #include <spu_printf.h>
@@ -39,7 +42,7 @@ m_minkowskiA(objectA),
 m_minkowskiB(objectB),
 m_ignoreMargin(false),
 m_lastUsedMethod(-1),
-m_catchDegeneracies(0)
+m_catchDegeneracies(1)
 {
 }
 
@@ -57,6 +60,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 	float marginA = m_minkowskiA->getMargin();
 	float marginB = m_minkowskiB->getMargin();
 
+	gNumGjkChecks++;
+
 	//for CCD we don't use margins
 	if (m_ignoreMargin)
 	{
@@ -71,7 +76,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 	bool isValid = false;
 	bool checkSimplex = false;
 	bool checkPenetration = true;
-	m_degenerateSimplex = false;
+	m_degenerateSimplex = 0;
 
 	m_lastUsedMethod = -1;
 
@@ -109,6 +114,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			//exit 0: the new point is already in the simplex, or we didn't come any closer
 			if (m_simplexSolver->inSimplex(w))
 			{
+				m_degenerateSimplex = 1;
 				checkSimplex = true;
 				break;
 			}
@@ -116,14 +122,12 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			float f0 = squaredDistance - delta;
 			float f1 = squaredDistance * REL_ERROR2;
 
-			if (f0 <= 0.f)
-			{
-				m_degenerateSimplex = 2;
-			}
-
-			if (f0 >= 0.f && (f0 <= f1))
-			//if (f0 <= f1)
+			if (f0 <= f1)
 			{
+				if (f0 <= 0.f)
+				{
+					m_degenerateSimplex = 2;
+				}
 				checkSimplex = true;
 				break;
 			}
@@ -133,7 +137,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			//calculate the closest point to the origin (update vector v)
 			if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
 			{
-				m_degenerateSimplex = 1;
+				m_degenerateSimplex = 3;
 				checkSimplex = true;
 				break;
 			}
@@ -188,7 +192,10 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			normalInB = pointOnA-pointOnB;
 			float lenSqr = m_cachedSeparatingAxis.length2();
 			//valid normal
-			//if (lenSqr > (0.1f*margin)) //SIMD_EPSILON*SIMD_EPSILON))
+			if (lenSqr < 0.0001)
+			{
+				m_degenerateSimplex = 5;
+			} 
 			if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
 			{
 				float rlen = 1.f / btSqrt(lenSqr );
@@ -200,6 +207,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 				pointOnB += m_cachedSeparatingAxis * (marginB / s);
 				distance = ((1.f/rlen) - margin);
 				isValid = true;
+				
 				m_lastUsedMethod = 1;
 			} else
 			{
@@ -207,8 +215,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			}
 		}
 
+		bool catchDegeneratePenetrationCase = 
+			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
+
 		//if (checkPenetration && !isValid)
-		if (checkPenetration && (!isValid || (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex) ))
+		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
 		{
 			//penetration case
 		
@@ -217,6 +228,9 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 			{
 				// Penetration depth case.
 				btVector3 tmpPointOnA,tmpPointOnB;
+				
+				gNumDeepPenetrationChecks++;
+
 				bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
 					*m_simplexSolver, 
 					m_minkowskiA,m_minkowskiB,
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
index 8d22689ec53..1b85a0eea42 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
@@ -104,7 +104,7 @@ float resolveSingleCollision(
 	
 	btScalar Kfps = 1.f / solverInfo.m_timeStep ;
 
-	float damping = solverInfo.m_damping ;
+	// float damping = solverInfo.m_damping ;
 	float Kerp = solverInfo.m_erp;
 	float Kcor = Kerp *Kfps;
 
@@ -136,7 +136,7 @@ float resolveSingleCollision(
 	{
 		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
 	}
-#else USE_INTERNAL_APPLY_IMPULSE
+#else //USE_INTERNAL_APPLY_IMPULSE
 	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
 	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
 #endif //USE_INTERNAL_APPLY_IMPULSE
@@ -215,7 +215,7 @@ float resolveSingleFriction(
 		body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
 		body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);	
 	}
-#else USE_INTERNAL_APPLY_IMPULSE
+#else //USE_INTERNAL_APPLY_IMPULSE
 	body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
 	body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
 #endif //USE_INTERNAL_APPLY_IMPULSE
@@ -317,7 +317,7 @@ float resolveSingleCollisionCombined(
 	
 	btScalar Kfps = 1.f / solverInfo.m_timeStep ;
 
-	float damping = solverInfo.m_damping ;
+	//float damping = solverInfo.m_damping ;
 	float Kerp = solverInfo.m_erp;
 	float Kcor = Kerp *Kfps;
 
@@ -350,7 +350,7 @@ float resolveSingleCollisionCombined(
 	{
 		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
 	}
-#else USE_INTERNAL_APPLY_IMPULSE
+#else //USE_INTERNAL_APPLY_IMPULSE
 	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
 	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
 #endif //USE_INTERNAL_APPLY_IMPULSE
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 568bcc40939..cf33b1d674e 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -63,8 +63,14 @@ bool  MyContactDestroyedCallback(void* userPersistentData)
 	return true;
 }
 
+btSequentialImpulseConstraintSolver2::btSequentialImpulseConstraintSolver2()
+{
+	setSolverMode(SOLVER_USE_WARMSTARTING);
+}
+
 
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+:m_solverMode(SOLVER_RANDMIZE_ORDER)
 {
 	gContactDestroyedCallback = &MyContactDestroyedCallback;
 
@@ -92,57 +98,127 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 
 	int totalPoints = 0;
 
+
 	{
 		int j;
 		for (j=0;j<numManifolds;j++)
 		{
-			prepareConstraints(manifoldPtr[j],info,debugDrawer);
+			btPersistentManifold* manifold = manifoldPtr[j];
+			prepareConstraints(manifold,info,debugDrawer);
+			for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
+			{
+				gOrder[totalPoints].m_manifoldIndex = j;
+				gOrder[totalPoints].m_pointIndex = p;
+				totalPoints++;
+			}
 		}
 	}
+	
+	//should traverse the contacts random order...
+	int iteration;
+
+	{
+		for ( iteration = 0;iteration<numiter-1;iteration++)
+		{
+			int j;
+			if (m_solverMode & SOLVER_RANDMIZE_ORDER)
+			{
+				if ((iteration & 7) == 0) {
+					for (j=0; j<totalPoints; ++j) {
+						btOrderIndex tmp = gOrder[j];
+						int swapi = btRandInt2(j+1);
+						gOrder[j] = gOrder[swapi];
+						gOrder[swapi] = tmp;
+					}
+				}
+			}
+
+			for (j=0;j<totalPoints;j++)
+			{
+				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
+				solve( (btRigidBody*)manifold->getBody0(),
+									(btRigidBody*)manifold->getBody1()
+				,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+			}
+		
+			for (j=0;j<totalPoints;j++)
+			{
+				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
+				solveFriction((btRigidBody*)manifold->getBody0(),
+					(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+			}
+		}
+	}
+		
+#ifdef USE_PROFILE
+	btProfiler::endBlock("solve");
+#endif //USE_PROFILE
+
+	return 0.f;
+}
+
+
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+float btSequentialImpulseConstraintSolver2::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	
+	btContactSolverInfo info = infoGlobal;
+
+	int numiter = infoGlobal.m_numIterations;
+#ifdef USE_PROFILE
+	btProfiler::beginBlock("solve");
+#endif //USE_PROFILE
 
 	{
 		int j;
 		for (j=0;j<numManifolds;j++)
 		{
+			btPersistentManifold* manifold = manifoldPtr[j];
+			prepareConstraints(manifold,info,debugDrawer);
 			for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
 			{
-				gOrder[totalPoints].m_manifoldIndex = j;
-				gOrder[totalPoints].m_pointIndex = p;
-				totalPoints++;
+				//interleaving here gives better results
+				solve( (btRigidBody*)manifold->getBody0(),
+									(btRigidBody*)manifold->getBody1()
+				,manifoldPtr[j]->getContactPoint(p),info,0,debugDrawer);
 			}
 		}
 	}
 	
-	
 	//should traverse the contacts random order...
 	int iteration;
+
 	for ( iteration = 0;iteration<numiter-1;iteration++)
 	{
 		int j;
-		if ((iteration & 7) == 0) {
-			for (j=0; j<totalPoints; ++j) {
-				btOrderIndex tmp = gOrder[j];
-				int swapi = btRandInt2(j+1);
-				gOrder[j] = gOrder[swapi];
-				gOrder[swapi] = tmp;
-			}
-		}
 
-		for (j=0;j<totalPoints;j++)
+		for (j=0;j<numManifolds;j++)
 		{
-			btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-			solve( (btRigidBody*)manifold->getBody0(),
-								(btRigidBody*)manifold->getBody1()
-			,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+			btPersistentManifold* manifold = manifoldPtr[j];
+			for (int p=0;p<manifold->getNumContacts();p++)
+			{
+				solve( (btRigidBody*)manifold->getBody0(),
+									(btRigidBody*)manifold->getBody1()
+				,manifold->getContactPoint(p),info,iteration,debugDrawer);
+			}
 		}
 	
-		for (j=0;j<totalPoints;j++)
+	}
+
+	for ( iteration = 0;iteration<numiter-1;iteration++)
+	{
+		int j;
+		for (j=0;j<numManifolds;j++)
 		{
-			btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-			solveFriction((btRigidBody*)manifold->getBody0(),
-				(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+			btPersistentManifold* manifold = manifoldPtr[j];
+			for (int p=0;p<manifold->getNumContacts();p++)
+			{
+				solveFriction((btRigidBody*)manifold->getBody0(),
+					(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(p),info,iteration,debugDrawer);
+			}
 		}
 	}
+
 		
 #ifdef USE_PROFILE
 	btProfiler::endBlock("solve");
@@ -264,7 +340,14 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 				
 
 				float relaxation = info.m_damping;
-				cpd->m_appliedImpulse =0.f;//*= relaxation;
+				if (m_solverMode & SOLVER_USE_WARMSTARTING)
+				{
+					cpd->m_appliedImpulse *= relaxation;
+				} else
+				{
+					cpd->m_appliedImpulse =0.f;
+				}
+	
 				//for friction
 				cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
 				
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index 8264f5071dd..bcc3806b923 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -30,6 +30,7 @@ class btIDebugDraw;
 class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
 
+protected:
 	float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
 	float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
 	void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
@@ -37,8 +38,18 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
 	ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
 	ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
 
+	//choose between several modes, different friction model etc.
+	int	m_solverMode;
+
 public:
 
+	enum	eSolverMode
+	{
+		SOLVER_RANDMIZE_ORDER = 1,
+		SOLVER_FRICTION_SEPARATE = 2,
+		SOLVER_USE_WARMSTARTING = 4
+	};
+
 	btSequentialImpulseConstraintSolver();
 
 	///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
@@ -59,7 +70,29 @@ public:
 	
 	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
 
+	void	setSolverMode(int mode)
+	{
+		m_solverMode = mode;
+	}
+
+	int	getSolverMode() const
+	{
+		return m_solverMode;
+	}
+};
+
+/// Small variation on btSequentialImpulseConstraintSolver: warmstarting, separate friction, non-randomized ordering
+class btSequentialImpulseConstraintSolver2 : public btSequentialImpulseConstraintSolver
+{
+public:
+
+	btSequentialImpulseConstraintSolver2();
+
+	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
+
+
 };
 
+
 #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index 125e933ad90..bdde013aadb 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -267,8 +267,14 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
 
+	btDispatcherInfo	dispatchInfo;
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = getDebugDrawer();
+
+
 	///perform collision detection
-	performDiscreteCollisionDetection();
+	performDiscreteCollisionDetection(dispatchInfo);
 
 	calculateSimulationIslands();
 
@@ -828,6 +834,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 	}
 }
 
+
 void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
 	if (m_ownsConstraintSolver)
@@ -838,3 +845,15 @@ void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 	m_constraintSolver = solver;
 }
 
+int		btDiscreteDynamicsWorld::getNumConstraints() const
+{
+	return int(m_constraints.size());
+}
+btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
+{
+	return m_constraints[index];
+}
+const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
+{
+	return m_constraints[index];
+}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 12939753442..4aa83683cd5 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -137,6 +137,13 @@ public:
 	void	debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
 
 	virtual void	setConstraintSolver(btConstraintSolver* solver);
+	
+	virtual	int		getNumConstraints() const;
+
+	virtual btTypedConstraint* getConstraint(int index)	;
+
+	virtual const btTypedConstraint* getConstraint(int index) const;
+
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
index 6023f29b4fb..a991fdca79d 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -68,6 +68,12 @@ class btDynamicsWorld : public btCollisionWorld
 		virtual void	removeRigidBody(btRigidBody* body) = 0;
 
 		virtual void	setConstraintSolver(btConstraintSolver* solver) = 0;
+		
+		virtual	int		getNumConstraints() const {	return 0;		}
+		
+		virtual btTypedConstraint* getConstraint(int index)		{			return 0;		}
+		
+		virtual const btTypedConstraint* getConstraint(int index) const	{		return 0;	}
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
index dabf291669d..fe6a73939a7 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -52,8 +52,13 @@ int		btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
 
+	btDispatcherInfo	dispatchInfo;
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = getDebugDrawer();
+
 	///perform collision detection
-	performDiscreteCollisionDetection();
+	performDiscreteCollisionDetection(dispatchInfo );
 
 	///solve contact constraints
 	int numManifolds = m_dispatcher1->getNumManifolds();
diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp
index 69d061d27d0..5176d317417 100644
--- a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp
+++ b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp
@@ -165,4 +165,5 @@ void	btGeometryUtil::getVerticesFromPlaneEquations(const std::vector<btVector3>&
 			}
 		}
 	}
-}
\ No newline at end of file
+}
+
diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.h b/extern/bullet2/src/LinearMath/btGeometryUtil.h
index 3170c215008..525291e6051 100644
--- a/extern/bullet2/src/LinearMath/btGeometryUtil.h
+++ b/extern/bullet2/src/LinearMath/btGeometryUtil.h
@@ -36,4 +36,5 @@ class btGeometryUtil
 };
 
 
-#endif //BT_GEOMETRY_UTIL_H
\ No newline at end of file
+#endif //BT_GEOMETRY_UTIL_H
+