Update bullet to Master 12409f1118a7c7a266f9071350c70789dfe73bb9

46 files changed, 2877 insertions, 397 deletions

diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h
index 2c4d41bc04..323aa96dca 100644
--- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h
+++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h
@@ -628,7 +628,6 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* /*dispatcher*
 }       
 
 
-extern int gOverlappingPairs;
 //#include <stdio.h>
 
 template <typename BP_FP_INT_TYPE>
@@ -695,10 +694,9 @@ void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatche
 				pair.m_pProxy0 = 0;
 				pair.m_pProxy1 = 0;
 				m_invalidPair++;
-				gOverlappingPairs--;
-			} 
-			
-		}
+      }
+
+    }
 
 	///if you don't like to skip the invalid pairs in the array, execute following code:
 	#define CLEAN_INVALID_PAIRS 1
diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
index adaf083a21..f6e1202a69 100644
--- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
+++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
@@ -66,6 +66,7 @@ CONCAVE_SHAPES_START_HERE,
 	EMPTY_SHAPE_PROXYTYPE,
 	STATIC_PLANE_PROXYTYPE,
 	CUSTOM_CONCAVE_SHAPE_TYPE,
+	SDF_SHAPE_PROXYTYPE=CUSTOM_CONCAVE_SHAPE_TYPE,
 CONCAVE_SHAPES_END_HERE,
 
 	COMPOUND_SHAPE_PROXYTYPE,
diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
index 4d12b1c9c7..14cd1a31ea 100644
--- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
+++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
@@ -17,7 +17,7 @@ subject to the following restrictions:
 
 #include "btDbvtBroadphase.h"
 #include "LinearMath/btThreads.h"
-
+btScalar gDbvtMargin = btScalar(0.05);
 //
 // Profiling
 //
@@ -332,12 +332,9 @@ void							btDbvtBroadphase::setAabb(		btBroadphaseProxy* absproxy,
 				if(delta[0]<0) velocity[0]=-velocity[0];
 				if(delta[1]<0) velocity[1]=-velocity[1];
 				if(delta[2]<0) velocity[2]=-velocity[2];
-				if	(
-#ifdef DBVT_BP_MARGIN				
-					m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
-#else
-					m_sets[0].update(proxy->leaf,aabb,velocity)
-#endif
+				if (
+					m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin)
+
 					)
 				{
 					++m_updates_done;
diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
index 8feb95d51f..90b333d846 100644
--- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
+++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
@@ -29,7 +29,8 @@ subject to the following restrictions:
 #define DBVT_BP_PREVENTFALSEUPDATE		0
 #define DBVT_BP_ACCURATESLEEPING		0
 #define DBVT_BP_ENABLE_BENCHMARK		0
-#define DBVT_BP_MARGIN					(btScalar)0.05
+//#define DBVT_BP_MARGIN					(btScalar)0.05
+extern btScalar gDbvtMargin;
 
 #if DBVT_BP_PROFILE
 #define	DBVT_BP_PROFILING_RATE	256
diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h
index 7b0f9489af..a0e4c18927 100644
--- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h
+++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h
@@ -46,7 +46,8 @@ struct btDispatcherInfo
 		m_useEpa(true),
 		m_allowedCcdPenetration(btScalar(0.04)),
 		m_useConvexConservativeDistanceUtil(false),
-		m_convexConservativeDistanceThreshold(0.0f)
+		m_convexConservativeDistanceThreshold(0.0f),
+		m_deterministicOverlappingPairs(false)
 	{
 
 	}
@@ -62,6 +63,7 @@ struct btDispatcherInfo
 	btScalar	m_allowedCcdPenetration;
 	bool		m_useConvexConservativeDistanceUtil;
 	btScalar	m_convexConservativeDistanceThreshold;
+	bool		m_deterministicOverlappingPairs;
 };
 
 enum ebtDispatcherQueryType
diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
index 55ebf06f1e..9e3337c5f6 100644
--- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
+++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
@@ -23,11 +23,6 @@ subject to the following restrictions:
 
 #include <stdio.h>
 
-int	gOverlappingPairs = 0;
-
-int gRemovePairs =0;
-int gAddedPairs =0;
-int gFindPairs =0;
 
 
 
@@ -134,13 +129,12 @@ void	btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroad
 
 btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 {
-	gFindPairs++;
-	if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
+	if(proxy0->m_uniqueId>proxy1->m_uniqueId)
 		btSwap(proxy0,proxy1);
 	int proxyId1 = proxy0->getUid();
 	int proxyId2 = proxy1->getUid();
 
-	/*if (proxyId1 > proxyId2) 
+	/*if (proxyId1 > proxyId2)
 		btSwap(proxyId1, proxyId2);*/
 
 	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
@@ -271,13 +265,12 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx
 
 void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
 {
-	gRemovePairs++;
-	if(proxy0->m_uniqueId>proxy1->m_uniqueId) 
+	if(proxy0->m_uniqueId>proxy1->m_uniqueId)
 		btSwap(proxy0,proxy1);
 	int proxyId1 = proxy0->getUid();
 	int proxyId2 = proxy1->getUid();
 
-	/*if (proxyId1 > proxyId2) 
+	/*if (proxyId1 > proxyId2)
 		btSwap(proxyId1, proxyId2);*/
 
 	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
@@ -386,8 +379,6 @@ void	btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
 		if (callback->processOverlap(*pair))
 		{
 			removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
-
-			gOverlappingPairs--;
 		} else
 		{
 			i++;
@@ -395,6 +386,70 @@ void	btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
 	}
 }
 
+struct MyPairIndex
+{
+    int m_orgIndex;
+    int m_uidA0;
+    int m_uidA1;
+};
+
+class MyPairIndeSortPredicate
+{
+public:
+    
+    bool operator() ( const MyPairIndex& a, const MyPairIndex& b ) const
+    {
+        const int uidA0 = a.m_uidA0;
+        const int uidB0 = b.m_uidA0;
+        const int uidA1 = a.m_uidA1;
+        const int uidB1 = b.m_uidA1;
+        return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
+    }
+};
+
+void    btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
+{
+    if (dispatchInfo.m_deterministicOverlappingPairs)
+    {
+        btBroadphasePairArray& pa = getOverlappingPairArray();
+        btAlignedObjectArray<MyPairIndex> indices;
+        {
+            BT_PROFILE("sortOverlappingPairs");
+            indices.resize(pa.size());
+            for (int i=0;i<indices.size();i++)
+            {
+                const btBroadphasePair& p = pa[i];
+                const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1;
+                const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1;
+                
+                indices[i].m_uidA0 = uidA0;
+                indices[i].m_uidA1 = uidA1;
+                indices[i].m_orgIndex = i;
+            }
+            indices.quickSort(MyPairIndeSortPredicate());
+        }
+        {
+            BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
+            int i;
+            for (i=0;i<indices.size();)
+            {
+                btBroadphasePair* pair = &pa[indices[i].m_orgIndex];
+                if (callback->processOverlap(*pair))
+                {
+                    removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
+                } else
+                {
+                    i++;
+                }
+            }
+        }
+    } else
+    {
+        processAllOverlappingPairs(callback, dispatcher);
+    }
+}
+
+
 void	btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
 {
 	///need to keep hashmap in sync with pair address, so rebuild all
@@ -435,7 +490,6 @@ void*	btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
 		int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
 		if (findIndex < m_overlappingPairArray.size())
 		{
-			gOverlappingPairs--;
 			btBroadphasePair& pair = m_overlappingPairArray[findIndex];
 			void* userData = pair.m_internalInfo1;
 			cleanOverlappingPair(pair,dispatcher);
@@ -468,11 +522,8 @@ btBroadphasePair*	btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP
 	
 	void* mem = &m_overlappingPairArray.expandNonInitializing();
 	btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
-	
-	gOverlappingPairs++;
-	gAddedPairs++;
-	
-	if (m_ghostPairCallback)
+
+  if (m_ghostPairCallback)
 		m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
 	return pair;
 	
@@ -526,7 +577,6 @@ void	btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
 			pair->m_pProxy1 = 0;
 			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 			m_overlappingPairArray.pop_back();
-			gOverlappingPairs--;
 		} else
 		{
 			i++;
@@ -559,7 +609,6 @@ void	btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,b
 			pair.m_algorithm->~btCollisionAlgorithm();
 			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
 			pair.m_algorithm=0;
-			gRemovePairs--;
 		}
 	}
 }
diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
index f7be7d45b3..7a38d34f05 100644
--- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
+++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
@@ -49,10 +49,6 @@ struct btOverlapFilterCallback
 
 
 
-extern int gRemovePairs;
-extern int gAddedPairs;
-extern int gFindPairs;
-
 const int BT_NULL_PAIR=0xffffffff;
 
 ///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
@@ -78,6 +74,10 @@ public:
 
 	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0;
 
+	virtual void    processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
+	{
+		processAllOverlappingPairs(callback, dispatcher);
+	}
 	virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0;
 
 	virtual bool	hasDeferredRemoval() = 0;
@@ -129,8 +129,6 @@ public:
 	// no new pair is created and the old one is returned.
 	virtual btBroadphasePair* 	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 	{
-		gAddedPairs++;
-
 		if (!needsBroadphaseCollision(proxy0,proxy1))
 			return 0;
 
@@ -144,6 +142,8 @@ public:
 	
 	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
 
+    virtual void    processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo);
+
 	virtual btBroadphasePair*	getOverlappingPairArrayPtr()
 	{
 		return &m_overlappingPairArray[0];
diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
index f1d5f5476e..5f89f960e8 100644
--- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -24,8 +24,6 @@ subject to the following restrictions:
 
 #include <new>
 
-extern int gOverlappingPairs;
-
 void	btSimpleBroadphase::validate()
 {
 	for (int i=0;i<m_numHandles;i++)
@@ -315,8 +313,7 @@ void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 					pair.m_pProxy0 = 0;
 					pair.m_pProxy1 = 0;
 					m_invalidPair++;
-					gOverlappingPairs--;
-				} 
+				}
 
 			}
 
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
index c81af95672..e5bac8438e 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
@@ -132,6 +132,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
 			else {
 				// Could be inside one of the contact capsules
 				btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
+				btScalar minDistSqr = contactCapsuleRadiusSqr;
 				btVector3 nearestOnEdge;
 				for (int i = 0; i < m_triangle->getNumEdges(); i++) {
 
@@ -141,8 +142,9 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
 					m_triangle->getEdge(i, pa, pb);
 
 					btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
-					if (distanceSqr < contactCapsuleRadiusSqr) {
-						// Yep, we're inside a capsule
+					if (distanceSqr < minDistSqr) {
+						// Yep, we're inside a capsule, and record the capsule with smallest distance
+						minDistSqr = distanceSqr;
 						hasContact = true;
 						contactPoint = nearestOnEdge;
 					}
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
index 2c36277821..cabbb0bf6a 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
@@ -151,8 +151,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1
 	int index = 0;
 	btScalar minDot = BT_LARGE_FLOAT;
 
-    if( count2 > 0 )
-        index = (int) normal1.minDot( vertices2, count2, minDot);
+	if( count2 > 0 )
+		index = (int) normal1.minDot( vertices2, count2, minDot);
 
 	btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
 	btVector3 v2 = b2Mul(xf2, vertices2[index]);
@@ -175,8 +175,8 @@ static btScalar FindMaxSeparation(int* edgeIndex,
 	// Find edge normal on poly1 that has the largest projection onto d.
 	int edge = 0;
     btScalar maxDot;
-    if( count1 > 0 )
-        edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
+	if( count1 > 0 )
+		edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
 
 	// Get the separation for the edge normal.
 	btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
index 5739a1ef01..f8794dec47 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
@@ -27,8 +27,6 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
-int gNumManifold = 0;
-
 #ifdef BT_DEBUG
 #include <stdio.h>
 #endif
@@ -77,8 +75,6 @@ btCollisionDispatcher::~btCollisionDispatcher()
 
 btPersistentManifold*	btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) 
 { 
-	gNumManifold++;
-	
 	//btAssert(gNumManifold < 65535);
 	
 
@@ -121,7 +117,6 @@ void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
 void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 {
 	
-	gNumManifold--;
 
 	//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
 	clearManifold(manifold);
@@ -246,13 +241,17 @@ public:
 
 
 
+
 void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) 
 {
 	//m_blockedForChanges = true;
 
 	btCollisionPairCallback	collisionCallback(dispatchInfo,this);
 
-	pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
+    {
+		BT_PROFILE("processAllOverlappingPairs");
+		pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher, dispatchInfo);
+	}
 
 	//m_blockedForChanges = false;
 
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp
index b595c56bc5..05f96a14bc 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp
@@ -16,6 +16,7 @@ subject to the following restrictions:
 
 #include "btCollisionObject.h"
 #include "LinearMath/btSerializer.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 
 btCollisionObject::btCollisionObject()
 	:	m_interpolationLinearVelocity(0.f, 0.f, 0.f),
@@ -38,7 +39,7 @@ btCollisionObject::btCollisionObject()
 		m_rollingFriction(0.0f),
         m_spinningFriction(0.f),
 		m_contactDamping(.1),
-		m_contactStiffness(1e4),
+		m_contactStiffness(BT_LARGE_FLOAT),
 		m_internalType(CO_COLLISION_OBJECT),
 		m_userObjectPointer(0),
 		m_userIndex2(-1),
@@ -114,10 +115,18 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
 	dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius;
 	dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
 	dataOut->m_checkCollideWith = m_checkCollideWith;
-
-	// Fill padding with zeros to appease msan.
-	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
-
+	if (m_broadphaseHandle)
+	{
+		dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup;
+		dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask;
+		dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId;
+	}
+	else
+	{
+		dataOut->m_collisionFilterGroup = 0;
+		dataOut->m_collisionFilterMask = 0;
+		dataOut->m_uniqueId = -1;
+	}
 	return btCollisionObjectDataName;
 }
 
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h
index fec831bffc..135f8a033c 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -621,7 +621,6 @@ struct	btCollisionObjectDoubleData
 	double					m_hitFraction; 
 	double					m_ccdSweptSphereRadius;
 	double					m_ccdMotionThreshold;
-
 	int						m_hasAnisotropicFriction;
 	int						m_collisionFlags;
 	int						m_islandTag1;
@@ -629,8 +628,9 @@ struct	btCollisionObjectDoubleData
 	int						m_activationState1;
 	int						m_internalType;
 	int						m_checkCollideWith;
-
-	char	m_padding[4];
+	int						m_collisionFilterGroup;
+	int						m_collisionFilterMask;
+	int						m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
@@ -650,13 +650,12 @@ struct	btCollisionObjectFloatData
 	float					m_deactivationTime;
 	float					m_friction;
 	float					m_rollingFriction;
-    float                   m_contactDamping;
+	float                   m_contactDamping;
     float                   m_contactStiffness;
 	float					m_restitution;
 	float					m_hitFraction; 
 	float					m_ccdSweptSphereRadius;
 	float					m_ccdMotionThreshold;
-
 	int						m_hasAnisotropicFriction;
 	int						m_collisionFlags;
 	int						m_islandTag1;
@@ -664,7 +663,9 @@ struct	btCollisionObjectFloatData
 	int						m_activationState1;
 	int						m_internalType;
 	int						m_checkCollideWith;
-	char					m_padding[4];
+	int						m_collisionFilterGroup;
+	int						m_collisionFilterMask;
+	int						m_uniqueId;
 };
 
 
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index c3e912fdca..3de8d6995e 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -1646,7 +1646,7 @@ void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 	for (i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
-		if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK))
+		if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
 		{
 			colObj->serializeSingleObject(serializer);
 		}
@@ -1654,12 +1654,36 @@ void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 }
 
 
+
+void btCollisionWorld::serializeContactManifolds(btSerializer* serializer)
+{
+	if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS)
+	{
+		int numManifolds = getDispatcher()->getNumManifolds();
+		for (int i = 0; i < numManifolds; i++)
+		{
+			const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i];
+			//don't serialize empty manifolds, they just take space 
+			//(may have to do it anyway if it destroys determinism)
+			if (manifold->getNumContacts() == 0)
+				continue;
+
+			btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1);
+			const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold);
+		}
+	}
+}
+
+
 void	btCollisionWorld::serialize(btSerializer* serializer)
 {
 
 	serializer->startSerialization();
 	
 	serializeCollisionObjects(serializer);
+
+	serializeContactManifolds(serializer);
 	
 	serializer->finishSerialization();
 }
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h
index eede2b28ca..886476e8ad 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h
@@ -107,6 +107,9 @@ protected:
 
 	void	serializeCollisionObjects(btSerializer* serializer);
 
+	void serializeContactManifolds(btSerializer* serializer);
+
+
 public:
 
 	//this constructor doesn't own the dispatcher and paircache/broadphase
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
index 7f4dea1c6d..91b7809c17 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
@@ -156,10 +156,12 @@ public:
 			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
 			
 			btCollisionAlgorithm* algo = 0;
+			bool allocatedAlgorithm = false;
 
 			if (m_resultOut->m_closestPointDistanceThreshold > 0)
 			{
 				algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
+				allocatedAlgorithm = true;
 			}
 			else
 			{
@@ -204,7 +206,11 @@ public:
 			{
 				m_resultOut->setBody1Wrap(tmpWrap);
 			}
-			
+			if(allocatedAlgorithm)
+			{
+				algo->~btCollisionAlgorithm();
+				m_dispatcher->freeCollisionAlgorithm(algo);
+ 			}
 		}
 	}
 	void		Process(const btDbvtNode* leaf)
@@ -253,9 +259,9 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 		m_compoundShapeRevision = compoundShape->getUpdateRevision();
 	}
 
-    if (m_childCollisionAlgorithms.size()==0)
-        return;
-    
+	if (m_childCollisionAlgorithms.size()==0)
+		return;
+
 	const btDbvt* tree = compoundShape->getDynamicAabbTree();
 	//use a dynamic aabb tree to cull potential child-overlaps
 	btCompoundLeafCallback  callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
index d4a1aa78e4..20b542f670 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
@@ -181,11 +181,12 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 			
 
 			btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
-
+			bool removePair = false;
 			btCollisionAlgorithm* colAlgo = 0;
 			if (m_resultOut->m_closestPointDistanceThreshold > 0)
 			{
 				colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
+				removePair = true;
 			}
 			else
 			{
@@ -223,7 +224,11 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 			m_resultOut->setBody0Wrap(tmpWrap0);
 			m_resultOut->setBody1Wrap(tmpWrap1);
 			
-
+			if (removePair)
+			{
+				colAlgo->~btCollisionAlgorithm();
+				m_dispatcher->freeCollisionAlgorithm(colAlgo);
+			}
 
 		}
 	}
@@ -396,32 +401,24 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 				btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
 
 				{
-					btTransform	orgTrans0;
 					const btCollisionShape* childShape0 = 0;
 					
 					btTransform	newChildWorldTrans0;
-					btTransform	orgInterpolationTrans0;
 					childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
-					orgTrans0 = col0ObjWrap->getWorldTransform();
-					orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
 					const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
-					newChildWorldTrans0 = orgTrans0*childTrans0 ;
+					newChildWorldTrans0 = col0ObjWrap->getWorldTransform()*childTrans0 ;
 					childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
 				}
 				btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
 				aabbMin0 -= thresholdVec;
 				aabbMax0 += thresholdVec;
 				{
-					btTransform	orgInterpolationTrans1;
 					const btCollisionShape* childShape1 = 0;
-					btTransform	orgTrans1;
 					btTransform	newChildWorldTrans1;
 
 					childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
-					orgTrans1 = col1ObjWrap->getWorldTransform();
-					orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
 					const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
-					newChildWorldTrans1 = orgTrans1*childTrans1 ;
+					newChildWorldTrans1 = col1ObjWrap->getWorldTransform()*childTrans1 ;
 					childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
 				}
 				
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
index 39ff7934d9..d8cbe96142 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
@@ -27,6 +27,7 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
 
 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
@@ -152,7 +153,7 @@ partId, int triangleIndex)
 		{
 			m_resultOut->setBody1Wrap(tmpWrap);
 		}
-		
+
 
 
 		colAlgo->~btCollisionAlgorithm();
@@ -163,7 +164,7 @@ partId, int triangleIndex)
 
 
 
-void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
+void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
 {
 	m_convexBodyWrap = convexBodyWrap;
 	m_triBodyWrap = triBodyWrap;
@@ -177,14 +178,14 @@ void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
 	convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
 	const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
 	//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
-	convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
-	btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold;
-	
-	btVector3 extra(extraMargin,extraMargin,extraMargin);
+	convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax);
+	btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold;
+
+	btVector3 extra(extraMargin, extraMargin, extraMargin);
 
 	m_aabbMax += extra;
 	m_aabbMin -= extra;
-	
+
 }
 
 void btConvexConcaveCollisionAlgorithm::clearCache()
@@ -193,35 +194,99 @@ void btConvexConcaveCollisionAlgorithm::clearCache()
 
 }
 
-void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
-	
+
 	const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
 	const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
 
 	if (triBodyWrap->getCollisionShape()->isConcave())
 	{
+		if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE)
+		{
+			btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape();
+			if (convexBodyWrap->getCollisionShape()->isConvex())
+			{
 
+				btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape();
+				btAlignedObjectArray<btVector3> queryVertices;
+
+				if (convex->isPolyhedral())
+				{
+					btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
+					for (int v = 0; v < poly->getNumVertices(); v++)
+					{
+						btVector3 vtx;
+						poly->getVertex(v, vtx);
+						queryVertices.push_back(vtx);
+					}
+				}
+				btScalar maxDist = SIMD_EPSILON;
+
+				if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
+				{
+					queryVertices.push_back(btVector3(0, 0, 0));
+					btSphereShape* sphere = (btSphereShape*)convex;
+					maxDist = sphere->getRadius() + SIMD_EPSILON;
+
+				}
+				if (queryVertices.size())
+				{
+					resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+					//m_btConvexTriangleCallback.m_manifoldPtr->clearManifold();
+
+					btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
+					for (int v = 0; v < queryVertices.size(); v++)
+					{
+						const btVector3& vtx = queryVertices[v];
+						btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform()*vtx;
+						btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace);
+
+						btVector3 normalLocal;
+						btScalar dist;
+						if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal))
+						{
+							if (dist <= maxDist)
+							{
+								normalLocal.safeNormalize();
+								btVector3 normal = triBodyWrap->getWorldTransform().getBasis()*normalLocal;
+
+								if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
+								{
+									btSphereShape* sphere = (btSphereShape*)convex;
+									dist -= sphere->getRadius();
+									vtxWorldSpace -= sphere->getRadius()*normal;
+
+								}
+								resultOut->addContactPoint(normal,vtxWorldSpace-normal*dist, dist);
+							}
+						}
+					}
+					resultOut->refreshContactPoints();
+				}
 
-		
-		const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
-		
-		if (convexBodyWrap->getCollisionShape()->isConvex())
+			}
+		} else
 		{
-			btScalar collisionMarginTriangle = concaveShape->getMargin();
+			const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
+		
+			if (convexBodyWrap->getCollisionShape()->isConvex())
+			{
+				btScalar collisionMarginTriangle = concaveShape->getMargin();
 					
-			resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
-			m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
+				resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+				m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
 
-			m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
+				m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
 
-			concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
+				concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
 			
-			resultOut->refreshContactPoints();
+				resultOut->refreshContactPoints();
 
-			m_btConvexTriangleCallback.clearWrapperData();
+				m_btConvexTriangleCallback.clearWrapperData();
 	
+			}
 		}
 	
 	}
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
index b54bd48932..3e8bc6e426 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
@@ -28,6 +28,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
 
 
 
@@ -259,7 +260,7 @@ struct btPerturbedContactResult : public btManifoldResult
 			btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
 			endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
 			newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
-			startPt = endPt+normalOnBInWorld*newDepth;
+			startPt = endPt - normalOnBInWorld*newDepth;
 		} else
 		{
 			endPt = pointInWorld + normalOnBInWorld*orgDepth;
@@ -442,10 +443,26 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 
 		struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
 		{
+			btVector3 m_normalOnBInWorld;
+			btVector3 m_pointInWorld;
+			btScalar m_depth;
+			bool m_hasContact;
+			
+
+			btDummyResult()
+				: m_hasContact(false)
+			{
+			}
+			
+			
 			virtual void setShapeIdentifiersA(int partId0,int index0){}
 			virtual void setShapeIdentifiersB(int partId1,int index1){}
 			virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) 
 			{
+				m_hasContact = true;
+				m_normalOnBInWorld = normalOnBInWorld;
+				m_pointInWorld = pointInWorld;
+				m_depth = depth;
 			}
 		};
 
@@ -560,15 +577,18 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 
 		} else
 		{
+
+
 			//we can also deal with convex versus triangle (without connectivity data)
-			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
+			if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
 			{
 
-				btVertexArray vertices;
+
+				btVertexArray worldSpaceVertices;
 				btTriangleShape* tri = (btTriangleShape*)polyhedronB;
-				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
-				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
-				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
+				worldSpaceVertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
+				worldSpaceVertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
+				worldSpaceVertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
 				
 				//tri->initializePolyhedralFeatures();
 
@@ -579,17 +599,99 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 				btScalar maxDist = threshold;
 				
 				bool foundSepAxis = false;
-				if (0)
+				bool useSatSepNormal = true;
+
+				if (useSatSepNormal)
 				{
-					polyhedronB->initializePolyhedralFeatures();
+#if 0
+					if (0)
+					{
+						//initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle
+						polyhedronB->initializePolyhedralFeatures();
+					} else
+#endif
+					{
+
+						btVector3 uniqueEdges[3] = {tri->m_vertices1[1]-tri->m_vertices1[0],
+							tri->m_vertices1[2]-tri->m_vertices1[1],
+							tri->m_vertices1[0]-tri->m_vertices1[2]};
+
+						uniqueEdges[0].normalize();
+						uniqueEdges[1].normalize();
+						uniqueEdges[2].normalize();
+
+						btConvexPolyhedron polyhedron;
+						polyhedron.m_vertices.push_back(tri->m_vertices1[2]);
+						polyhedron.m_vertices.push_back(tri->m_vertices1[0]);
+						polyhedron.m_vertices.push_back(tri->m_vertices1[1]);
+						
+						
+						{
+							btFace combinedFaceA;
+							combinedFaceA.m_indices.push_back(0);
+							combinedFaceA.m_indices.push_back(1);
+							combinedFaceA.m_indices.push_back(2);
+							btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]);
+							faceNormal.normalize();
+							btScalar planeEq=1e30f;
+							for (int v=0;v<combinedFaceA.m_indices.size();v++)
+							{
+								btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal);
+								if (planeEq>eq)
+								{
+									planeEq=eq;
+								}
+							}
+							combinedFaceA.m_plane[0] = faceNormal[0];
+							combinedFaceA.m_plane[1] = faceNormal[1];
+							combinedFaceA.m_plane[2] = faceNormal[2];
+							combinedFaceA.m_plane[3] = -planeEq;
+							polyhedron.m_faces.push_back(combinedFaceA);
+						}
+						{
+							btFace combinedFaceB;
+							combinedFaceB.m_indices.push_back(0);
+							combinedFaceB.m_indices.push_back(2);
+							combinedFaceB.m_indices.push_back(1);
+							btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]);
+							faceNormal.normalize();
+							btScalar planeEq=1e30f;
+							for (int v=0;v<combinedFaceB.m_indices.size();v++)
+							{
+								btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal);
+								if (planeEq>eq)
+								{
+									planeEq=eq;
+								}
+							}
+
+							combinedFaceB.m_plane[0] = faceNormal[0];
+							combinedFaceB.m_plane[1] = faceNormal[1];
+							combinedFaceB.m_plane[2] = faceNormal[2];
+							combinedFaceB.m_plane[3] = -planeEq;
+							polyhedron.m_faces.push_back(combinedFaceB);
+						}
+
+						
+						polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]);
+						polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]);
+						polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]);
+						polyhedron.initialize2();
+
+						polyhedronB->setPolyhedralFeatures(polyhedron);
+					}
+
+					
+
 					 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
-					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
-					body0Wrap->getWorldTransform(), 
-					body1Wrap->getWorldTransform(),
-					sepNormalWorldSpace,*resultOut);
+						*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
+						body0Wrap->getWorldTransform(), 
+						body1Wrap->getWorldTransform(),
+						sepNormalWorldSpace,*resultOut);
 				//	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
 
-				} else
+				} 
+				else
 				{
 #ifdef ZERO_MARGIN
 					gjkPairDetector.setIgnoreMargin(true);
@@ -598,6 +700,24 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 					gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
 #endif//ZERO_MARGIN
 					
+					if (dummy.m_hasContact && dummy.m_depth<0)
+					{
+						
+						if (foundSepAxis)
+						{
+							if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace)<0.99)
+							{
+								printf("?\n");
+							}
+						} else
+						{
+							printf("!\n");
+						}
+						sepNormalWorldSpace.setValue(0,0,1);// = dummy.m_normalOnBInWorld;
+						//minDist = dummy.m_depth;
+						foundSepAxis = true;
+					}
+#if 0
 					btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
 					if (l2>SIMD_EPSILON)
 					{
@@ -607,6 +727,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 						minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
 						foundSepAxis = true;
 					}
+#endif
 				}
 
 				
@@ -614,7 +735,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 			{
 				worldVertsB2.resize(0);
 				btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
-					body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
+					body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
 			}
 				
 				
@@ -625,6 +746,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 				
 				return;
 			}
+
 			
 		}
 
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
index 8c8a7c3c1e..8271981b29 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
@@ -20,11 +20,12 @@ subject to the following restrictions:
 
 #include <stdio.h>
 
+#ifdef BT_DEBUG_COLLISION_PAIRS
 int	gOverlappingSimplePairs = 0;
 int gRemoveSimplePairs =0;
 int gAddedSimplePairs =0;
 int gFindSimplePairs =0;
-
+#endif //BT_DEBUG_COLLISION_PAIRS
 
 
 
@@ -61,7 +62,9 @@ void btHashedSimplePairCache::removeAllPairs()
 
 btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB)
 {
+#ifdef BT_DEBUG_COLLISION_PAIRS
 	gFindSimplePairs++;
+#endif
 	
 	
 	/*if (indexA > indexB) 
@@ -172,7 +175,9 @@ btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB)
 
 void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
 {
+#ifdef BT_DEBUG_COLLISION_PAIRS
 	gRemoveSimplePairs++;
+#endif
 	
 
 	/*if (indexA > indexB) 
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
index 2aaf6201f3..318981cda1 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
@@ -43,12 +43,12 @@ struct btSimplePair
 typedef btAlignedObjectArray<btSimplePair>	btSimplePairArray;
 
 
-
+#ifdef BT_DEBUG_COLLISION_PAIRS
 extern int gOverlappingSimplePairs;
 extern int gRemoveSimplePairs;
 extern int gAddedSimplePairs;
 extern int gFindSimplePairs;
-
+#endif //BT_DEBUG_COLLISION_PAIRS
 
 
 
@@ -75,7 +75,9 @@ public:
 	// no new pair is created and the old one is returned.
 	virtual btSimplePair* 	addOverlappingPair(int indexA,int indexB)
 	{
+#ifdef BT_DEBUG_COLLISION_PAIRS
 		gAddedSimplePairs++;
+#endif
 
 		return internalAddPair(indexA,indexB);
 	}
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
index 6cba442ca5..898320ee1a 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
@@ -455,11 +455,20 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr
 	btBvhTriangleMeshShape* trimesh = 0;
 	
 	if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
-	   trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
-   else	   
-	   trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
-	   
-   	btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
+	{
+		trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
+	}
+	else
+	{
+		if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+		{
+			trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
+		}
+	}
+	if (trimesh==0)
+		return;
+
+	btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
 	if (!triangleInfoMapPtr)
 		return;
 
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
index 1344782257..91c76a8dac 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
@@ -199,6 +199,22 @@ class btPersistentManifoldSortPredicate
 		}
 };
 
+class btPersistentManifoldSortPredicateDeterministic
+{
+public:
+
+	SIMD_FORCE_INLINE bool operator() (const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
+	{
+		return (
+			(getIslandId(lhs) < getIslandId(rhs))
+							|| ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) 
+						||((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) &&
+					(lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId))
+			);
+
+	}
+};
+
 
 void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
 {
@@ -245,13 +261,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
 			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 			if (colObj0->getIslandTag() == islandId)
 			{
-				if (colObj0->getActivationState()== ACTIVE_TAG)
-				{
-					allSleeping = false;
-				}
-				if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
+				if (colObj0->getActivationState()== ACTIVE_TAG ||
+				   colObj0->getActivationState()== DISABLE_DEACTIVATION)
 				{
 					allSleeping = false;
+					break;
 				}
 			}
 		}
@@ -318,7 +332,12 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
 	for (i=0;i<maxNumManifolds ;i++)
 	{
 		 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
-		 
+		 if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
+		 {
+			if (manifold->getNumContacts() == 0)
+				continue;
+		 }
+
 		 const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
 		 const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
 		
@@ -379,7 +398,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 		//tried a radix sort, but quicksort/heapsort seems still faster
 		//@todo rewrite island management
-		m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+		//btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid,
+		//but also based on object0 unique id and object1 unique id
+		if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
+		{
+			m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic());
+		} else
+		{
+			m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+		}
+
 		//m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
 
 		//now process all active islands (sets of manifolds for now)
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h
index 7d64b46abf..5a3362834a 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h
@@ -49,6 +49,7 @@ public:
 	virtual void setMargin(btScalar collisionMargin)
 	{
 		//don't override the margin for capsules, their entire radius == margin
+		(void)collisionMargin;
 	}
 
 	virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp
index e8c8c336cd..85572da307 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp
@@ -213,7 +213,7 @@ void	btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co
 
 
 
-void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
+void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const
 {
 	int n = m_children.size();
 
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.h
index 4eef8dba30..2cbcd1bfca 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.h
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.h
@@ -160,7 +160,7 @@ public:
 	///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
 	///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
 	///of the collision object by the principal transform.
-	void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
+	void calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const;
 
 	int	getUpdateRevision() const
 	{
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
index 4f45319a83..0fea00df5c 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
@@ -104,9 +104,9 @@ void	btConvexPolyhedron::initialize()
 
 	btHashMap<btInternalVertexPair,btInternalEdge> edges;
 
-	btScalar TotalArea = 0.0f;
 	
-	m_localCenter.setValue(0, 0, 0);
+	
+	
 	for(int i=0;i<m_faces.size();i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
@@ -172,6 +172,13 @@ void	btConvexPolyhedron::initialize()
 	}
 #endif//USE_CONNECTED_FACES
 
+	initialize2();
+}
+
+void	btConvexPolyhedron::initialize2()
+{
+	m_localCenter.setValue(0, 0, 0);
+	btScalar TotalArea = 0.0f;
 	for(int i=0;i<m_faces.size();i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
@@ -274,7 +281,6 @@ void	btConvexPolyhedron::initialize()
 	}
 #endif
 }
-
 void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
 {
 	minProj = FLT_MAX;
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.h
index d3cd066ac8..c5aa20f453 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.h
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.h
@@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
 	btVector3		mE;
 
 	void	initialize();
+	void	initialize2();
 	bool testContainment() const;
 
 	void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp
index 8d7fb054d6..2f84858598 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp
@@ -118,9 +118,13 @@ static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector
 	return supVec;
 #else
 
-    btScalar maxDot;
-    long ptIndex = vec.maxDot( points, numPoints, maxDot);
+	btScalar maxDot;
+	long ptIndex = vec.maxDot( points, numPoints, maxDot);
 	btAssert(ptIndex >= 0);
+	if (ptIndex<0)
+	{
+		ptIndex = 0;
+	}
 	btVector3 supVec = points[ptIndex] * localScaling;
 	return supVec;
 #endif //__SPU__
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp
new file mode 100644
index 0000000000..afe45e1d2d
--- /dev/null
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp
@@ -0,0 +1,532 @@
+#include "btMiniSDF.h"
+
+//
+//Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid
+//example:
+//GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj
+//The MIT License (MIT)
+//
+//Copyright (c) 2017 Dan Koschier
+//
+
+#include <limits.h>
+#include <string.h> //memcpy
+
+struct btSdfDataStream
+{
+	const char* m_data;
+	int m_size;
+	
+	int m_currentOffset;
+	
+	btSdfDataStream(const char* data, int size)
+		:m_data(data),
+		m_size(size),
+		m_currentOffset(0)
+	{
+		
+	}
+
+	template<class T> bool read(T& val)
+	{
+		int bytes = sizeof(T);
+		if (m_currentOffset+bytes<=m_size)
+		{
+			char* dest = (char*)&val;
+			memcpy(dest,&m_data[m_currentOffset],bytes);
+			m_currentOffset+=bytes;
+			return true;
+		}
+		btAssert(0);
+		return false;
+	}
+};
+
+
+bool btMiniSDF::load(const char* data, int size)
+{
+		int fileSize = -1;
+
+		btSdfDataStream ds(data,size);
+		{
+			double buf[6];
+			ds.read(buf);
+			m_domain.m_min[0] = buf[0];
+			m_domain.m_min[1] = buf[1];
+			m_domain.m_min[2] = buf[2];
+			m_domain.m_min[3] = 0;
+			m_domain.m_max[0] = buf[3];
+			m_domain.m_max[1] = buf[4];
+			m_domain.m_max[2] = buf[5];
+			m_domain.m_max[3] = 0;
+		}
+		{
+			unsigned int buf2[3];
+			ds.read(buf2);
+			m_resolution[0] = buf2[0];
+			m_resolution[1] = buf2[1];
+			m_resolution[2] = buf2[2];
+		}
+		{
+			double buf[3];
+			ds.read(buf);
+			m_cell_size[0] = buf[0];
+			m_cell_size[1] = buf[1];
+			m_cell_size[2] = buf[2];
+		}
+		{
+			double buf[3];
+			ds.read(buf);
+			m_inv_cell_size[0] = buf[0];
+			m_inv_cell_size[1] = buf[1];
+			m_inv_cell_size[2] = buf[2];
+		}
+		{
+			unsigned long long int cells;
+			ds.read(cells);
+			m_n_cells = cells;
+		}
+		{
+			unsigned long long int fields;
+			ds.read(fields);
+			m_n_fields = fields;
+		}
+
+		
+		unsigned long long int nodes0;
+		std::size_t n_nodes0;
+		ds.read(nodes0);
+		n_nodes0 = nodes0;
+		if (n_nodes0 > 1024 * 1024 * 1024)
+		{
+			return m_isValid;
+		}
+		m_nodes.resize(n_nodes0);
+		for (unsigned int i=0;i<n_nodes0;i++)
+		{
+			unsigned long long int n_nodes1;
+			ds.read(n_nodes1);
+			btAlignedObjectArray<double>& nodes = m_nodes[i];
+			nodes.resize(n_nodes1);
+			for ( int j=0;j<nodes.size();j++)
+			{
+				double& node = nodes[j];
+				ds.read(node);
+			}
+		}
+
+		unsigned long long int n_cells0;
+		ds.read(n_cells0);
+		m_cells.resize(n_cells0);
+		for (int i=0;i<n_cells0;i++)
+		{
+			unsigned long long int n_cells1;
+			btAlignedObjectArray<btCell32 >& cells = m_cells[i];
+			ds.read(n_cells1);
+			cells.resize(n_cells1);
+			for (int j=0;j<n_cells1;j++)
+			{
+				btCell32& cell = cells[j];
+				ds.read(cell);
+			}
+		}
+
+		{
+			unsigned long long int n_cell_maps0;
+			ds.read(n_cell_maps0);
+		
+			m_cell_map.resize(n_cell_maps0);
+			for (int i=0;i<n_cell_maps0;i++)
+			{
+				unsigned long long int n_cell_maps1;
+				btAlignedObjectArray<unsigned int>& cell_maps = m_cell_map[i];
+				ds.read(n_cell_maps1);
+				cell_maps.resize(n_cell_maps1);
+				for (int j=0;j<n_cell_maps1;j++)
+				{
+					unsigned int& cell_map = cell_maps[j];
+					ds.read(cell_map);
+				}
+			}
+		}
+
+		m_isValid = (ds.m_currentOffset == ds.m_size);
+		return m_isValid;
+}
+
+
+unsigned int btMiniSDF::multiToSingleIndex(btMultiIndex const & ijk) const
+{
+	return m_resolution[1] * m_resolution[0] * ijk.ijk[2] + m_resolution[0] * ijk.ijk[1] + ijk.ijk[0];
+}
+	
+btAlignedBox3d
+btMiniSDF::subdomain(btMultiIndex const& ijk) const
+{
+	btAssert(m_isValid);
+	btVector3 tmp;
+	tmp.m_floats[0] = m_cell_size[0]*(double)ijk.ijk[0];
+	tmp.m_floats[1] = m_cell_size[1]*(double)ijk.ijk[1];
+	tmp.m_floats[2] = m_cell_size[2]*(double)ijk.ijk[2];
+	
+
+	btVector3 origin = m_domain.min() + tmp;
+
+	btAlignedBox3d box = btAlignedBox3d (origin, origin + m_cell_size);
+	return box;
+}
+
+btMultiIndex
+btMiniSDF::singleToMultiIndex(unsigned int l) const
+{
+	btAssert(m_isValid);
+	unsigned int n01 = m_resolution[0] * m_resolution[1];
+	unsigned int  k = l / n01;
+	unsigned int  temp = l % n01;
+	unsigned int j = temp / m_resolution[0];
+	unsigned int i = temp % m_resolution[0];
+	btMultiIndex mi;
+	mi.ijk[0] = i;
+	mi.ijk[1] = j;
+	mi.ijk[2] = k;
+	return mi;
+}
+
+btAlignedBox3d
+btMiniSDF::subdomain(unsigned int l) const
+{
+	btAssert(m_isValid);
+	return subdomain(singleToMultiIndex(l));
+}
+
+
+btShapeMatrix
+btMiniSDF::shape_function_(btVector3 const& xi, btShapeGradients* gradient) const
+{
+	btAssert(m_isValid);
+	btShapeMatrix res;
+	
+	btScalar x = xi[0];
+	btScalar y = xi[1];
+	btScalar z = xi[2];
+
+	btScalar  x2 = x*x;
+	btScalar  y2 = y*y;
+	btScalar  z2 = z*z;
+
+	btScalar  _1mx = 1.0 - x;
+	btScalar  _1my = 1.0 - y;
+	btScalar  _1mz = 1.0 - z;
+
+	btScalar  _1px = 1.0 + x;
+	btScalar  _1py = 1.0 + y;
+	btScalar  _1pz = 1.0 + z;
+
+	btScalar  _1m3x = 1.0 - 3.0 * x;
+	btScalar  _1m3y = 1.0 - 3.0 * y;
+	btScalar  _1m3z = 1.0 - 3.0 * z;
+
+	btScalar  _1p3x = 1.0 + 3.0 * x;
+	btScalar  _1p3y = 1.0 + 3.0 * y;
+	btScalar  _1p3z = 1.0 + 3.0 * z;
+
+	btScalar  _1mxt1my = _1mx * _1my;
+	btScalar  _1mxt1py = _1mx * _1py;
+	btScalar  _1pxt1my = _1px * _1my;
+	btScalar  _1pxt1py = _1px * _1py;
+
+	btScalar  _1mxt1mz = _1mx * _1mz;
+	btScalar  _1mxt1pz = _1mx * _1pz;
+	btScalar  _1pxt1mz = _1px * _1mz;
+	btScalar  _1pxt1pz = _1px * _1pz;
+
+	btScalar  _1myt1mz = _1my * _1mz;
+	btScalar  _1myt1pz = _1my * _1pz;
+	btScalar  _1pyt1mz = _1py * _1mz;
+	btScalar  _1pyt1pz = _1py * _1pz;
+
+	btScalar  _1mx2 = 1.0 - x2;
+	btScalar  _1my2 = 1.0 - y2;
+	btScalar  _1mz2 = 1.0 - z2;
+
+
+	// Corner nodes.
+	btScalar  fac = 1.0 / 64.0 * (9.0 * (x2 + y2 + z2) - 19.0);
+	res[0] = fac * _1mxt1my * _1mz;
+	res[1] = fac * _1pxt1my * _1mz;
+	res[2] = fac * _1mxt1py * _1mz;
+	res[3] = fac * _1pxt1py * _1mz;
+	res[4] = fac * _1mxt1my * _1pz;
+	res[5] = fac * _1pxt1my * _1pz;
+	res[6] = fac * _1mxt1py * _1pz;
+	res[7] = fac * _1pxt1py * _1pz;
+
+	// Edge nodes.
+
+	fac = 9.0 / 64.0 * _1mx2;
+	btScalar  fact1m3x = fac * _1m3x;
+	btScalar  fact1p3x = fac * _1p3x;
+	res[ 8] = fact1m3x * _1myt1mz;
+	res[ 9] = fact1p3x * _1myt1mz;
+	res[10] = fact1m3x * _1myt1pz;
+	res[11] = fact1p3x * _1myt1pz;
+	res[12] = fact1m3x * _1pyt1mz;
+	res[13] = fact1p3x * _1pyt1mz;
+	res[14] = fact1m3x * _1pyt1pz;
+	res[15] = fact1p3x * _1pyt1pz;
+
+	fac = 9.0 / 64.0 * _1my2;
+	btScalar  fact1m3y = fac * _1m3y;
+	btScalar  fact1p3y = fac * _1p3y;
+	res[16] = fact1m3y * _1mxt1mz;
+	res[17] = fact1p3y * _1mxt1mz;
+	res[18] = fact1m3y * _1pxt1mz;
+	res[19] = fact1p3y * _1pxt1mz;
+	res[20] = fact1m3y * _1mxt1pz;
+	res[21] = fact1p3y * _1mxt1pz;
+	res[22] = fact1m3y * _1pxt1pz;
+	res[23] = fact1p3y * _1pxt1pz;
+
+	fac = 9.0 / 64.0 * _1mz2;
+	btScalar  fact1m3z = fac * _1m3z;
+	btScalar  fact1p3z = fac * _1p3z;
+	res[24] = fact1m3z * _1mxt1my;
+	res[25] = fact1p3z * _1mxt1my;
+	res[26] = fact1m3z * _1mxt1py;
+	res[27] = fact1p3z * _1mxt1py;
+	res[28] = fact1m3z * _1pxt1my;
+	res[29] = fact1p3z * _1pxt1my;
+	res[30] = fact1m3z * _1pxt1py;
+	res[31] = fact1p3z * _1pxt1py;
+
+	if (gradient)
+	{
+		btShapeGradients& dN = *gradient;
+
+		btScalar _9t3x2py2pz2m19 = 9.0 * (3.0 * x2 + y2 + z2) - 19.0;
+		btScalar _9tx2p3y2pz2m19 = 9.0 * (x2 + 3.0 * y2 + z2) - 19.0;
+		btScalar _9tx2py2p3z2m19 = 9.0 * (x2 + y2 + 3.0 * z2) - 19.0;
+		btScalar _18x = 18.0 * x;
+		btScalar _18y = 18.0 * y;
+		btScalar _18z = 18.0 * z;
+		
+		btScalar _3m9x2 = 3.0 - 9.0 * x2;
+		btScalar _3m9y2 = 3.0 - 9.0 * y2;
+		btScalar _3m9z2 = 3.0 - 9.0 * z2;
+
+		btScalar _2x = 2.0 * x;
+		btScalar _2y = 2.0 * y;
+		btScalar _2z = 2.0 * z;
+
+		btScalar _18xm9t3x2py2pz2m19 = _18x - _9t3x2py2pz2m19;
+		btScalar _18xp9t3x2py2pz2m19 = _18x + _9t3x2py2pz2m19;
+		btScalar _18ym9tx2p3y2pz2m19 = _18y - _9tx2p3y2pz2m19;
+		btScalar _18yp9tx2p3y2pz2m19 = _18y + _9tx2p3y2pz2m19;
+		btScalar _18zm9tx2py2p3z2m19 = _18z - _9tx2py2p3z2m19;
+		btScalar _18zp9tx2py2p3z2m19 = _18z + _9tx2py2p3z2m19;
+
+		dN(0,0) =_18xm9t3x2py2pz2m19 * _1myt1mz;
+		dN(0,1) =_1mxt1mz * _18ym9tx2p3y2pz2m19;
+		dN(0,2) =_1mxt1my * _18zm9tx2py2p3z2m19;
+		dN(1,0) =_18xp9t3x2py2pz2m19 * _1myt1mz;
+		dN(1,1) =_1pxt1mz * _18ym9tx2p3y2pz2m19;
+		dN(1,2) =_1pxt1my * _18zm9tx2py2p3z2m19;
+		dN(2,0) =_18xm9t3x2py2pz2m19 * _1pyt1mz;
+		dN(2,1) =_1mxt1mz * _18yp9tx2p3y2pz2m19;
+		dN(2,2) =_1mxt1py * _18zm9tx2py2p3z2m19;
+		dN(3,0) =_18xp9t3x2py2pz2m19 * _1pyt1mz;
+		dN(3,1) =_1pxt1mz * _18yp9tx2p3y2pz2m19;
+		dN(3,2) =_1pxt1py * _18zm9tx2py2p3z2m19;
+		dN(4,0) =_18xm9t3x2py2pz2m19 * _1myt1pz;
+		dN(4,1) =_1mxt1pz * _18ym9tx2p3y2pz2m19;
+		dN(4,2) =_1mxt1my * _18zp9tx2py2p3z2m19;
+		dN(5,0) =_18xp9t3x2py2pz2m19 * _1myt1pz;
+		dN(5,1) =_1pxt1pz * _18ym9tx2p3y2pz2m19;
+		dN(5,2) =_1pxt1my * _18zp9tx2py2p3z2m19;
+		dN(6,0) =_18xm9t3x2py2pz2m19 * _1pyt1pz;
+		dN(6,1) =_1mxt1pz * _18yp9tx2p3y2pz2m19;
+		dN(6,2) =_1mxt1py * _18zp9tx2py2p3z2m19;
+		dN(7,0) =_18xp9t3x2py2pz2m19 * _1pyt1pz;
+		dN(7,1) =_1pxt1pz * _18yp9tx2p3y2pz2m19;
+		dN(7,2) =_1pxt1py * _18zp9tx2py2p3z2m19;
+
+		dN.topRowsDivide(8, 64.0);
+
+		btScalar _m3m9x2m2x = -_3m9x2 - _2x;
+		btScalar _p3m9x2m2x =  _3m9x2 - _2x;
+		btScalar _1mx2t1m3x = _1mx2 * _1m3x;
+		btScalar _1mx2t1p3x = _1mx2 * _1p3x;
+		dN( 8,0) =    _m3m9x2m2x * _1myt1mz,
+		dN( 8,1) =   -_1mx2t1m3x * _1mz,
+		dN( 8,2) =   -_1mx2t1m3x * _1my;
+		dN( 9,0) =    _p3m9x2m2x * _1myt1mz,
+		dN( 9,1) =   -_1mx2t1p3x * _1mz,
+		dN( 9,2) =   -_1mx2t1p3x * _1my;
+		dN(10,0) =    _m3m9x2m2x * _1myt1pz,
+		dN(10,1) =   -_1mx2t1m3x * _1pz,
+		dN(10,2) =   _1mx2t1m3x * _1my;
+		dN(11,0) =    _p3m9x2m2x * _1myt1pz,
+		dN(11,1) =   -_1mx2t1p3x * _1pz,
+		dN(11,2) =   _1mx2t1p3x * _1my;
+		dN(12,0) =    _m3m9x2m2x * _1pyt1mz,
+		dN(12,1) =   _1mx2t1m3x * _1mz,
+		dN(12,2) =   -_1mx2t1m3x * _1py;
+		dN(13,0) =    _p3m9x2m2x * _1pyt1mz,
+		dN(13,1) =   _1mx2t1p3x * _1mz,
+		dN(13,2) =   -_1mx2t1p3x * _1py;
+		dN(14,0) =    _m3m9x2m2x * _1pyt1pz,
+		dN(14,1) =   _1mx2t1m3x * _1pz,
+		dN(14,2) =   _1mx2t1m3x * _1py;
+		dN(15,0) =    _p3m9x2m2x * _1pyt1pz,
+		dN(15,1) =   _1mx2t1p3x * _1pz,
+		dN(15,2) =   _1mx2t1p3x * _1py;
+
+		btScalar _m3m9y2m2y = -_3m9y2 - _2y;
+		btScalar _p3m9y2m2y =  _3m9y2 - _2y;
+		btScalar _1my2t1m3y = _1my2 * _1m3y;
+		btScalar _1my2t1p3y = _1my2 * _1p3y;
+		dN(16,0) =    -_1my2t1m3y * _1mz,
+		dN(16,1) =   _m3m9y2m2y * _1mxt1mz,
+		dN(16,2) =   -_1my2t1m3y * _1mx;
+		dN(17,0) =    -_1my2t1p3y * _1mz,
+		dN(17,1) =   _p3m9y2m2y * _1mxt1mz,
+		dN(17,2) =   -_1my2t1p3y * _1mx;
+		dN(18,0) =    _1my2t1m3y * _1mz,
+		dN(18,1) =   _m3m9y2m2y * _1pxt1mz,
+		dN(18,2) =   -_1my2t1m3y * _1px;
+		dN(19,0) =    _1my2t1p3y * _1mz,
+		dN(19,1) =   _p3m9y2m2y * _1pxt1mz,
+		dN(19,2) =   -_1my2t1p3y * _1px;
+		dN(20,0) =    -_1my2t1m3y * _1pz,
+		dN(20,1) =   _m3m9y2m2y * _1mxt1pz,
+		dN(20,2) =   _1my2t1m3y * _1mx;
+		dN(21,0) =    -_1my2t1p3y * _1pz,
+		dN(21,1) =   _p3m9y2m2y * _1mxt1pz,
+		dN(21,2) =   _1my2t1p3y * _1mx;
+		dN(22,0) =    _1my2t1m3y * _1pz,
+		dN(22,1) =   _m3m9y2m2y * _1pxt1pz,
+		dN(22,2) =   _1my2t1m3y * _1px;
+		dN(23,0) =    _1my2t1p3y * _1pz,
+		dN(23,1) =   _p3m9y2m2y * _1pxt1pz,
+		dN(23,2) =   _1my2t1p3y * _1px;
+
+
+		btScalar _m3m9z2m2z = -_3m9z2 - _2z;
+		btScalar _p3m9z2m2z =  _3m9z2 - _2z;
+		btScalar _1mz2t1m3z = _1mz2 * _1m3z;
+		btScalar _1mz2t1p3z = _1mz2 * _1p3z;
+		dN(24,0) =    -_1mz2t1m3z * _1my,
+		dN(24,1) =  -_1mz2t1m3z * _1mx,
+		dN(24,2) =   _m3m9z2m2z * _1mxt1my;
+		dN(25,0) =    -_1mz2t1p3z * _1my,
+		dN(25,1) =   -_1mz2t1p3z * _1mx,
+		dN(25,2) =   _p3m9z2m2z * _1mxt1my;
+		dN(26,0) =    -_1mz2t1m3z * _1py,
+		dN(26,1) =   _1mz2t1m3z * _1mx,
+		dN(26,2) =   _m3m9z2m2z * _1mxt1py;
+		dN(27,0) =    -_1mz2t1p3z * _1py,
+		dN(27,1) =   _1mz2t1p3z * _1mx,
+		dN(27,2) =   _p3m9z2m2z * _1mxt1py;
+		dN(28,0) =    _1mz2t1m3z * _1my,
+		dN(28,1) =   -_1mz2t1m3z * _1px,
+		dN(28,2) =   _m3m9z2m2z * _1pxt1my;
+		dN(29,0) =    _1mz2t1p3z * _1my,
+		dN(29,1) =   -_1mz2t1p3z * _1px,
+		dN(29,2) =   _p3m9z2m2z * _1pxt1my;
+		dN(30,0) =    _1mz2t1m3z * _1py,
+		dN(30,1) =   _1mz2t1m3z * _1px,
+		dN(30,2) =   _m3m9z2m2z * _1pxt1py;
+		dN(31,0) =    _1mz2t1p3z * _1py,
+		dN(31,1) =   _1mz2t1p3z * _1px,
+		dN(31,2) =   _p3m9z2m2z * _1pxt1py;
+
+		dN.bottomRowsMul(32u - 8u, 9.0 / 64.0);
+
+	}
+
+	return res;
+}
+
+
+
+bool btMiniSDF::interpolate(unsigned int field_id, double& dist, btVector3 const& x,
+	btVector3* gradient) const
+{
+	btAssert(m_isValid);
+	if (!m_isValid)
+		return false;
+
+	if (!m_domain.contains(x))
+		return false;
+
+	btVector3 tmpmi = ((x - m_domain.min())*(m_inv_cell_size));//.cast<unsigned int>().eval();
+	unsigned int mi[3] = {(unsigned int )tmpmi[0],(unsigned int )tmpmi[1],(unsigned int )tmpmi[2]};
+	if (mi[0] >= m_resolution[0])
+		mi[0] = m_resolution[0]-1;
+	if (mi[1] >= m_resolution[1])
+		mi[1] = m_resolution[1]-1;
+	if (mi[2] >= m_resolution[2])
+		mi[2] = m_resolution[2]-1;
+	btMultiIndex mui; 
+	mui.ijk[0] = mi[0];
+	mui.ijk[1] = mi[1];
+	mui.ijk[2] = mi[2];
+	int i = multiToSingleIndex(mui);
+	unsigned int i_ = m_cell_map[field_id][i];
+	if (i_ == UINT_MAX)
+		return false;
+
+	btAlignedBox3d sd = subdomain(i);
+	i = i_;
+	btVector3 d = sd.m_max-sd.m_min;//.diagonal().eval();
+
+	btVector3 denom = (sd.max() - sd.min());
+	btVector3 c0 = btVector3(2.0,2.0,2.0)/denom;
+	btVector3 c1 = (sd.max() + sd.min())/denom;
+	btVector3 xi = (c0*x - c1);
+
+	btCell32 const& cell = m_cells[field_id][i];
+	if (!gradient)
+	{
+		//auto phi = m_coefficients[field_id][i].dot(shape_function_(xi, 0));
+		double phi = 0.0;
+		btShapeMatrix N = shape_function_(xi, 0);
+		for (unsigned int j = 0u; j < 32u; ++j)
+		{
+			unsigned int v = cell.m_cells[j];
+			double c = m_nodes[field_id][v];
+			if (c == DBL_MAX)
+			{
+				return false;;
+			}
+			phi += c * N[j];
+		}
+
+		dist = phi;
+		return true;
+	}
+
+	btShapeGradients dN;
+	btShapeMatrix N = shape_function_(xi, &dN);
+
+	double phi = 0.0;
+	gradient->setZero();
+	for (unsigned int j = 0u; j < 32u; ++j)
+	{
+		unsigned int v = cell.m_cells[j];
+		double c = m_nodes[field_id][v];
+		if (c == DBL_MAX)
+		{
+			gradient->setZero();
+			return false;
+		}
+		phi += c * N[j];
+		(*gradient)[0] += c * dN(j, 0);
+		(*gradient)[1] += c * dN(j, 1);
+		(*gradient)[2] += c * dN(j, 2);
+	}
+	(*gradient) *= c0;
+	dist = phi;
+	return true;
+}
+
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.h
new file mode 100644
index 0000000000..3de90e4f8a
--- /dev/null
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.h
@@ -0,0 +1,134 @@
+#ifndef MINISDF_H
+#define MINISDF_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+struct btMultiIndex
+{
+	unsigned int ijk[3];
+};
+
+struct btAlignedBox3d
+{
+	btVector3 m_min;
+	btVector3 m_max;
+
+	const btVector3& min() const
+	{
+		return m_min;
+	}
+
+	const btVector3& max() const
+	{
+		return m_max;
+	}
+	
+
+	bool contains(const btVector3& x) const
+	{
+		return TestPointAgainstAabb2(m_min, m_max, x);
+	}
+
+	btAlignedBox3d(const btVector3& mn, const btVector3& mx)
+		:m_min(mn),
+		m_max(mx)
+	{
+	}
+
+	btAlignedBox3d()
+	{
+	}
+};
+
+struct btShapeMatrix
+{
+	double m_vec[32];
+
+	inline double&       operator[](int i)
+	{
+		return m_vec[i];
+	}
+
+	inline const double&       operator[](int i) const
+	{
+		return m_vec[i];
+	}
+
+};
+
+struct btShapeGradients
+{
+	btVector3	m_vec[32];
+
+	void topRowsDivide(int row, double denom)
+	{
+		for (int i=0;i<row;i++)
+		{
+			m_vec[i] /= denom;
+		}
+	}
+	
+	void bottomRowsMul(int row, double val)
+	{
+		for (int i=32-row;i<32;i++)
+		{
+			m_vec[i] *= val;
+		}
+	}
+	
+	inline btScalar&       operator()(int i, int j)
+	{
+		return m_vec[i][j];
+	}
+};
+
+struct btCell32
+{
+	unsigned int m_cells[32];
+};
+
+struct btMiniSDF
+{
+
+	btAlignedBox3d m_domain;
+	unsigned int m_resolution[3];
+	btVector3 m_cell_size;
+	btVector3 m_inv_cell_size;
+	std::size_t m_n_cells;
+	std::size_t m_n_fields;
+	bool m_isValid;
+
+
+	btAlignedObjectArray<btAlignedObjectArray<double> > m_nodes;
+	btAlignedObjectArray<btAlignedObjectArray<btCell32 > > m_cells;
+	btAlignedObjectArray<btAlignedObjectArray<unsigned int> > m_cell_map;
+
+	btMiniSDF()
+	:m_isValid(false)
+	{
+	}
+	bool load(const char* data, int size);
+	bool isValid() const
+	{
+		return m_isValid;
+	}
+	unsigned int multiToSingleIndex(btMultiIndex const & ijk) const;
+	
+	btAlignedBox3d subdomain(btMultiIndex const& ijk) const;
+
+	btMultiIndex singleToMultiIndex(unsigned int l) const;
+
+	btAlignedBox3d subdomain(unsigned int l) const;
+
+
+	btShapeMatrix
+	shape_function_(btVector3 const& xi, btShapeGradients* gradient = 0) const;
+
+	bool interpolate(unsigned int field_id, double& dist, btVector3 const& x, btVector3* gradient) const;
+};
+
+
+#endif //MINISDF_H
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
index 4854f370f7..d51b6760fc 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
@@ -39,6 +39,17 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
 	}
 }
 
+void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron)
+{
+	if (m_polyhedron)
+	{
+		*m_polyhedron = polyhedron;
+	} else
+	{
+		void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
+		m_polyhedron = new (mem) btConvexPolyhedron(polyhedron);
+	}
+}
 
 bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
 {
@@ -87,8 +98,72 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
 		conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
 	}
 
+#ifndef BT_RECONSTRUCT_FACES
+	
+	int numVertices = conv.vertices.size();
+	m_polyhedron->m_vertices.resize(numVertices);
+	for (int p=0;p<numVertices;p++)
+	{
+		m_polyhedron->m_vertices[p] = conv.vertices[p];
+	}
+
+	int v0, v1;
+	for (int j = 0; j < conv.faces.size(); j++)
+	{
+		btVector3 edges[3];
+		int numEdges = 0;
+		btFace combinedFace;
+		const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]];
+		v0 = edge->getSourceVertex();
+		int prevVertex=v0;
+		combinedFace.m_indices.push_back(v0);
+		v1 = edge->getTargetVertex();
+		while (v1 != v0)
+		{
+		
+			btVector3 wa = conv.vertices[prevVertex];
+			btVector3 wb = conv.vertices[v1];
+			btVector3 newEdge = wb-wa;
+			newEdge.normalize();
+			if (numEdges<2)
+				edges[numEdges++] = newEdge;
 
 
+			//face->addIndex(v1);
+			combinedFace.m_indices.push_back(v1);
+			edge = edge->getNextEdgeOfFace();
+			prevVertex = v1;
+			int v01 = edge->getSourceVertex();
+			v1 = edge->getTargetVertex();
+
+		}
+		
+		btAssert(combinedFace.m_indices.size() > 2);
+
+		btVector3 faceNormal = edges[0].cross(edges[1]);
+		faceNormal.normalize();
+
+		btScalar planeEq=1e30f;
+
+		for (int v=0;v<combinedFace.m_indices.size();v++)
+		{
+			btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal);
+			if (planeEq>eq)
+			{
+				planeEq=eq;
+			}
+		}
+		combinedFace.m_plane[0] = faceNormal.getX();
+		combinedFace.m_plane[1] = faceNormal.getY();
+		combinedFace.m_plane[2] = faceNormal.getZ();
+		combinedFace.m_plane[3] = -planeEq;
+		
+		m_polyhedron->m_faces.push_back(combinedFace);
+	}
+
+
+#else//BT_RECONSTRUCT_FACES
+
 	btAlignedObjectArray<btVector3> faceNormals;
 	int numFaces = conv.faces.size();
 	faceNormals.resize(numFaces);
@@ -311,7 +386,9 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
 
 
 	}
-	
+
+#endif //BT_RECONSTRUCT_FACES
+
 	m_polyhedron->initialize();
 
 	return true;
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
index 7bf8e01c1f..b7ddb6e060 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
@@ -43,6 +43,9 @@ public:
 	///experimental/work-in-progress
 	virtual bool	initializePolyhedralFeatures(int shiftVerticesByMargin=0);
 
+	virtual void setPolyhedralFeatures(btConvexPolyhedron& polyhedron);
+	
+
 	const btConvexPolyhedron*	getConvexPolyhedron() const
 	{
 		return m_polyhedron;
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp
new file mode 100644
index 0000000000..828acda470
--- /dev/null
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp
@@ -0,0 +1,99 @@
+#include "btSdfCollisionShape.h"
+#include "btMiniSDF.h"
+#include "LinearMath/btAabbUtil2.h"
+
+struct btSdfCollisionShapeInternalData
+{
+	btVector3 m_localScaling;
+	btScalar m_margin;
+	btMiniSDF m_sdf;
+
+	btSdfCollisionShapeInternalData()
+		:m_localScaling(1,1,1),
+		m_margin(0)
+	{
+
+	}
+};
+
+bool btSdfCollisionShape::initializeSDF(const char* sdfData, int sizeInBytes)
+{
+	bool valid = m_data->m_sdf.load(sdfData, sizeInBytes);
+	return valid;
+}
+btSdfCollisionShape::btSdfCollisionShape()
+{
+	m_shapeType = SDF_SHAPE_PROXYTYPE;
+	m_data = new btSdfCollisionShapeInternalData();
+
+
+	
+	//"E:/develop/bullet3/data/toys/ground_hole64_64_8.cdf");//ground_cube.cdf");
+	/*unsigned int field_id=0;
+	Eigen::Vector3d x (1,10,1);
+	Eigen::Vector3d gradient;
+	double dist = m_data->m_sdf.interpolate(field_id, x, &gradient);
+	printf("dist=%g\n", dist);
+	*/
+
+}
+btSdfCollisionShape::~btSdfCollisionShape()
+{
+	delete m_data;
+}
+
+void btSdfCollisionShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	btAssert(m_data->m_sdf.isValid());
+	btVector3 localAabbMin = m_data->m_sdf.m_domain.m_min;
+	btVector3 localAabbMax = m_data->m_sdf.m_domain.m_max;
+	btScalar margin(0);
+	btTransformAabb(localAabbMin,localAabbMax,margin,t,aabbMin,aabbMax);
+
+}
+
+	
+void	btSdfCollisionShape::setLocalScaling(const btVector3& scaling)
+{
+	m_data->m_localScaling = scaling;
+}
+const btVector3& btSdfCollisionShape::getLocalScaling() const
+{
+	return m_data->m_localScaling;
+}
+void	btSdfCollisionShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	inertia.setValue(0,0,0);
+}
+const char*	btSdfCollisionShape::getName()const
+{
+	return "btSdfCollisionShape";
+}
+void	btSdfCollisionShape::setMargin(btScalar margin)
+{
+	m_data->m_margin = margin;
+}
+btScalar	btSdfCollisionShape::getMargin() const
+{
+	return m_data->m_margin;
+}
+
+void	btSdfCollisionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	//not yet
+}
+
+
+bool btSdfCollisionShape::queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal)
+{
+	int field = 0;
+	btVector3 grad;
+	double dist;
+	bool hasResult = m_data->m_sdf.interpolate(field,dist, ptInSDF,&grad);
+	if (hasResult)
+	{
+		normal.setValue(grad[0],grad[1],grad[2]);
+		distOut= dist;
+	}
+	return hasResult;
+}
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.h
new file mode 100644
index 0000000000..6e32db9cd8
--- /dev/null
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.h
@@ -0,0 +1,30 @@
+#ifndef BT_SDF_COLLISION_SHAPE_H
+#define BT_SDF_COLLISION_SHAPE_H
+
+#include "btConcaveShape.h"
+
+class btSdfCollisionShape : public btConcaveShape
+{
+	struct btSdfCollisionShapeInternalData* m_data;
+
+public:
+		
+	btSdfCollisionShape();
+	virtual ~btSdfCollisionShape();
+	
+	bool initializeSDF(const char* sdfData, int sizeInBytes);
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual const btVector3& getLocalScaling() const;
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	virtual const char*	getName()const;
+	virtual void	setMargin(btScalar margin);
+	virtual btScalar	getMargin() const;
+
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+	bool queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal);
+};
+
+#endif //BT_SDF_COLLISION_SHAPE_H
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp
index 3beaf86580..9f712fe555 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp
@@ -20,6 +20,8 @@ subject to the following restrictions:
 #include "LinearMath/btConvexHull.h"
 
 #define NUM_UNITSPHERE_POINTS 42
+#define NUM_UNITSPHERE_POINTS_HIGHRES 256
+
 
 btShapeHull::btShapeHull (const btConvexShape* shape)
 {
@@ -36,9 +38,9 @@ btShapeHull::~btShapeHull ()
 }
 
 bool
-btShapeHull::buildHull (btScalar /*margin*/)
+btShapeHull::buildHull (btScalar /*margin*/, int highres)
 {
-	int numSampleDirections = NUM_UNITSPHERE_POINTS;
+	int numSampleDirections = highres? NUM_UNITSPHERE_POINTS_HIGHRES:NUM_UNITSPHERE_POINTS;
 	{
 		int numPDA = m_shape->getNumPreferredPenetrationDirections();
 		if (numPDA)
@@ -47,17 +49,17 @@ btShapeHull::buildHull (btScalar /*margin*/)
 			{
 				btVector3 norm;
 				m_shape->getPreferredPenetrationDirection(i,norm);
-				getUnitSpherePoints()[numSampleDirections] = norm;
+				getUnitSpherePoints(highres)[numSampleDirections] = norm;
 				numSampleDirections++;
 			}
 		}
 	}
 
-	btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
 	int i;
 	for (i = 0; i < numSampleDirections; i++)
 	{
-		supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]);
+		supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]);
 	}
 
 	HullDesc hd;
@@ -118,9 +120,268 @@ btShapeHull::numIndices () const
 }
 
 
-btVector3* btShapeHull::getUnitSpherePoints()
+btVector3* btShapeHull::getUnitSpherePoints(int highres)
 {
-	static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
+	static btVector3 sUnitSpherePointsHighres[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] =
+	{
+		btVector3(btScalar(0.997604), btScalar(0.067004), btScalar(0.017144)),
+		btVector3(btScalar(0.984139), btScalar(-0.086784), btScalar(-0.154427)),
+		btVector3(btScalar(0.971065), btScalar(0.124164), btScalar(-0.203224)),
+		btVector3(btScalar(0.955844), btScalar(0.291173), btScalar(-0.037704)),
+		btVector3(btScalar(0.957405), btScalar(0.212238), btScalar(0.195157)),
+		btVector3(btScalar(0.971650), btScalar(-0.012709), btScalar(0.235561)),
+		btVector3(btScalar(0.984920), btScalar(-0.161831), btScalar(0.059695)),
+		btVector3(btScalar(0.946673), btScalar(-0.299288), btScalar(-0.117536)),
+		btVector3(btScalar(0.922670), btScalar(-0.219186), btScalar(-0.317019)),
+		btVector3(btScalar(0.928134), btScalar(-0.007265), btScalar(-0.371867)),
+		btVector3(btScalar(0.875642), btScalar(0.198434), btScalar(-0.439988)),
+		btVector3(btScalar(0.908035), btScalar(0.325975), btScalar(-0.262562)),
+		btVector3(btScalar(0.864519), btScalar(0.488706), btScalar(-0.116755)),
+		btVector3(btScalar(0.893009), btScalar(0.428046), btScalar(0.137185)),
+		btVector3(btScalar(0.857494), btScalar(0.362137), btScalar(0.364776)),
+		btVector3(btScalar(0.900815), btScalar(0.132524), btScalar(0.412987)),
+		btVector3(btScalar(0.934964), btScalar(-0.241739), btScalar(0.259179)),
+		btVector3(btScalar(0.894570), btScalar(-0.103504), btScalar(0.434263)),
+		btVector3(btScalar(0.922085), btScalar(-0.376668), btScalar(0.086241)),
+		btVector3(btScalar(0.862177), btScalar(-0.499154), btScalar(-0.085330)),
+		btVector3(btScalar(0.861982), btScalar(-0.420218), btScalar(-0.282861)),
+		btVector3(btScalar(0.818076), btScalar(-0.328256), btScalar(-0.471804)),
+		btVector3(btScalar(0.762657), btScalar(-0.179329), btScalar(-0.621124)),
+		btVector3(btScalar(0.826857), btScalar(0.019760), btScalar(-0.561786)),
+		btVector3(btScalar(0.731434), btScalar(0.206599), btScalar(-0.649817)),
+		btVector3(btScalar(0.769486), btScalar(0.379052), btScalar(-0.513770)),
+		btVector3(btScalar(0.796806), btScalar(0.507176), btScalar(-0.328145)),
+		btVector3(btScalar(0.679722), btScalar(0.684101), btScalar(-0.264123)),
+		btVector3(btScalar(0.786854), btScalar(0.614886), btScalar(0.050912)),
+		btVector3(btScalar(0.769486), btScalar(0.571141), btScalar(0.285139)),
+		btVector3(btScalar(0.707432), btScalar(0.492789), btScalar(0.506288)),
+		btVector3(btScalar(0.774560), btScalar(0.268037), btScalar(0.572652)),
+		btVector3(btScalar(0.796220), btScalar(0.031230), btScalar(0.604077)),
+		btVector3(btScalar(0.837395), btScalar(-0.320285), btScalar(0.442461)),
+		btVector3(btScalar(0.848127), btScalar(-0.450548), btScalar(0.278307)),
+		btVector3(btScalar(0.775536), btScalar(-0.206354), btScalar(0.596465)),
+		btVector3(btScalar(0.816320), btScalar(-0.567007), btScalar(0.109469)),
+		btVector3(btScalar(0.741191), btScalar(-0.668690), btScalar(-0.056832)),
+		btVector3(btScalar(0.755632), btScalar(-0.602975), btScalar(-0.254949)),
+		btVector3(btScalar(0.720311), btScalar(-0.521318), btScalar(-0.457165)),
+		btVector3(btScalar(0.670746), btScalar(-0.386583), btScalar(-0.632835)),
+		btVector3(btScalar(0.587031), btScalar(-0.219769), btScalar(-0.778836)),
+		btVector3(btScalar(0.676015), btScalar(-0.003182), btScalar(-0.736676)),
+		btVector3(btScalar(0.566932), btScalar(0.186963), btScalar(-0.802064)),
+		btVector3(btScalar(0.618254), btScalar(0.398105), btScalar(-0.677533)),
+		btVector3(btScalar(0.653964), btScalar(0.575224), btScalar(-0.490933)),
+		btVector3(btScalar(0.525367), btScalar(0.743205), btScalar(-0.414028)),
+		btVector3(btScalar(0.506439), btScalar(0.836528), btScalar(-0.208885)),
+		btVector3(btScalar(0.651427), btScalar(0.756426), btScalar(-0.056247)),
+		btVector3(btScalar(0.641670), btScalar(0.745149), btScalar(0.180908)),
+		btVector3(btScalar(0.602643), btScalar(0.687211), btScalar(0.405180)),
+		btVector3(btScalar(0.516586), btScalar(0.596999), btScalar(0.613447)),
+		btVector3(btScalar(0.602252), btScalar(0.387801), btScalar(0.697573)),
+		btVector3(btScalar(0.646549), btScalar(0.153911), btScalar(0.746956)),
+		btVector3(btScalar(0.650842), btScalar(-0.087756), btScalar(0.753983)),
+		btVector3(btScalar(0.740411), btScalar(-0.497404), btScalar(0.451830)),
+		btVector3(btScalar(0.726946), btScalar(-0.619890), btScalar(0.295093)),
+		btVector3(btScalar(0.637768), btScalar(-0.313092), btScalar(0.703624)),
+		btVector3(btScalar(0.678942), btScalar(-0.722934), btScalar(0.126645)),
+		btVector3(btScalar(0.489072), btScalar(-0.867195), btScalar(-0.092942)),
+		btVector3(btScalar(0.622742), btScalar(-0.757541), btScalar(-0.194636)),
+		btVector3(btScalar(0.596788), btScalar(-0.693576), btScalar(-0.403098)),
+		btVector3(btScalar(0.550150), btScalar(-0.582172), btScalar(-0.598287)),
+		btVector3(btScalar(0.474436), btScalar(-0.429745), btScalar(-0.768101)),
+		btVector3(btScalar(0.372574), btScalar(-0.246016), btScalar(-0.894583)),
+		btVector3(btScalar(0.480095), btScalar(-0.026513), btScalar(-0.876626)),
+		btVector3(btScalar(0.352474), btScalar(0.177242), btScalar(-0.918787)),
+		btVector3(btScalar(0.441848), btScalar(0.374386), btScalar(-0.814946)),
+		btVector3(btScalar(0.492389), btScalar(0.582223), btScalar(-0.646693)),
+		btVector3(btScalar(0.343498), btScalar(0.866080), btScalar(-0.362693)),
+		btVector3(btScalar(0.362036), btScalar(0.745149), btScalar(-0.559639)),
+		btVector3(btScalar(0.334131), btScalar(0.937044), btScalar(-0.099774)),
+		btVector3(btScalar(0.486925), btScalar(0.871718), btScalar(0.052473)),
+		btVector3(btScalar(0.452776), btScalar(0.845665), btScalar(0.281820)),
+		btVector3(btScalar(0.399503), btScalar(0.771785), btScalar(0.494576)),
+		btVector3(btScalar(0.296469), btScalar(0.673018), btScalar(0.677469)),
+		btVector3(btScalar(0.392088), btScalar(0.479179), btScalar(0.785213)),
+		btVector3(btScalar(0.452190), btScalar(0.252094), btScalar(0.855286)),
+		btVector3(btScalar(0.478339), btScalar(0.013149), btScalar(0.877928)),
+		btVector3(btScalar(0.481656), btScalar(-0.219380), btScalar(0.848259)),
+		btVector3(btScalar(0.615327), btScalar(-0.494293), btScalar(0.613837)),
+		btVector3(btScalar(0.594642), btScalar(-0.650414), btScalar(0.472325)),
+		btVector3(btScalar(0.562249), btScalar(-0.771345), btScalar(0.297631)),
+		btVector3(btScalar(0.467411), btScalar(-0.437133), btScalar(0.768231)),
+		btVector3(btScalar(0.519513), btScalar(-0.847947), btScalar(0.103808)),
+		btVector3(btScalar(0.297640), btScalar(-0.938159), btScalar(-0.176288)),
+		btVector3(btScalar(0.446727), btScalar(-0.838615), btScalar(-0.311359)),
+		btVector3(btScalar(0.331790), btScalar(-0.942437), btScalar(0.040762)),
+		btVector3(btScalar(0.413358), btScalar(-0.748403), btScalar(-0.518259)),
+		btVector3(btScalar(0.347596), btScalar(-0.621640), btScalar(-0.701737)),
+		btVector3(btScalar(0.249831), btScalar(-0.456186), btScalar(-0.853984)),
+		btVector3(btScalar(0.131772), btScalar(-0.262931), btScalar(-0.955678)),
+		btVector3(btScalar(0.247099), btScalar(-0.042261), btScalar(-0.967975)),
+		btVector3(btScalar(0.113624), btScalar(0.165965), btScalar(-0.979491)),
+		btVector3(btScalar(0.217438), btScalar(0.374580), btScalar(-0.901220)),
+		btVector3(btScalar(0.307983), btScalar(0.554615), btScalar(-0.772786)),
+		btVector3(btScalar(0.166702), btScalar(0.953181), btScalar(-0.252021)),
+		btVector3(btScalar(0.172751), btScalar(0.844499), btScalar(-0.506743)),
+		btVector3(btScalar(0.177630), btScalar(0.711125), btScalar(-0.679876)),
+		btVector3(btScalar(0.120064), btScalar(0.992260), btScalar(-0.030482)),
+		btVector3(btScalar(0.289640), btScalar(0.949098), btScalar(0.122546)),
+		btVector3(btScalar(0.239879), btScalar(0.909047), btScalar(0.340377)),
+		btVector3(btScalar(0.181142), btScalar(0.821363), btScalar(0.540641)),
+		btVector3(btScalar(0.066986), btScalar(0.719097), btScalar(0.691327)),
+		btVector3(btScalar(0.156750), btScalar(0.545478), btScalar(0.823079)),
+		btVector3(btScalar(0.236172), btScalar(0.342306), btScalar(0.909353)),
+		btVector3(btScalar(0.277541), btScalar(0.112693), btScalar(0.953856)),
+		btVector3(btScalar(0.295299), btScalar(-0.121974), btScalar(0.947415)),
+		btVector3(btScalar(0.287883), btScalar(-0.349254), btScalar(0.891591)),
+		btVector3(btScalar(0.437165), btScalar(-0.634666), btScalar(0.636869)),
+		btVector3(btScalar(0.407113), btScalar(-0.784954), btScalar(0.466664)),
+		btVector3(btScalar(0.375111), btScalar(-0.888193), btScalar(0.264839)),
+		btVector3(btScalar(0.275394), btScalar(-0.560591), btScalar(0.780723)),
+		btVector3(btScalar(0.122015), btScalar(-0.992209), btScalar(-0.024821)),
+		btVector3(btScalar(0.087866), btScalar(-0.966156), btScalar(-0.241676)),
+		btVector3(btScalar(0.239489), btScalar(-0.885665), btScalar(-0.397437)),
+		btVector3(btScalar(0.167287), btScalar(-0.965184), btScalar(0.200817)),
+		btVector3(btScalar(0.201632), btScalar(-0.776789), btScalar(-0.596335)),
+		btVector3(btScalar(0.122015), btScalar(-0.637971), btScalar(-0.760098)),
+		btVector3(btScalar(0.008054), btScalar(-0.464741), btScalar(-0.885214)),
+		btVector3(btScalar(-0.116054), btScalar(-0.271096), btScalar(-0.955482)),
+		btVector3(btScalar(-0.000727), btScalar(-0.056065), btScalar(-0.998424)),
+		btVector3(btScalar(-0.134007), btScalar(0.152939), btScalar(-0.978905)),
+		btVector3(btScalar(-0.025900), btScalar(0.366026), btScalar(-0.930108)),
+		btVector3(btScalar(0.081231), btScalar(0.557337), btScalar(-0.826072)),
+		btVector3(btScalar(-0.002874), btScalar(0.917213), btScalar(-0.398023)),
+		btVector3(btScalar(-0.050683), btScalar(0.981761), btScalar(-0.182534)),
+		btVector3(btScalar(-0.040536), btScalar(0.710153), btScalar(-0.702713)),
+		btVector3(btScalar(-0.139081), btScalar(0.827973), btScalar(-0.543048)),
+		btVector3(btScalar(-0.101029), btScalar(0.994010), btScalar(0.041152)),
+		btVector3(btScalar(0.069328), btScalar(0.978067), btScalar(0.196133)),
+		btVector3(btScalar(0.023860), btScalar(0.911380), btScalar(0.410645)),
+		btVector3(btScalar(-0.153521), btScalar(0.736789), btScalar(0.658145)),
+		btVector3(btScalar(-0.070002), btScalar(0.591750), btScalar(0.802780)),
+		btVector3(btScalar(0.002590), btScalar(0.312948), btScalar(0.949562)),
+		btVector3(btScalar(0.090988), btScalar(-0.020680), btScalar(0.995627)),
+		btVector3(btScalar(0.088842), btScalar(-0.250099), btScalar(0.964006)),
+		btVector3(btScalar(0.083378), btScalar(-0.470185), btScalar(0.878318)),
+		btVector3(btScalar(0.240074), btScalar(-0.749764), btScalar(0.616374)),
+		btVector3(btScalar(0.210803), btScalar(-0.885860), btScalar(0.412987)),
+		btVector3(btScalar(0.077524), btScalar(-0.660524), btScalar(0.746565)),
+		btVector3(btScalar(-0.096736), btScalar(-0.990070), btScalar(-0.100945)),
+		btVector3(btScalar(-0.052634), btScalar(-0.990264), btScalar(0.127426)),
+		btVector3(btScalar(-0.106102), btScalar(-0.938354), btScalar(-0.328340)),
+		btVector3(btScalar(0.013323), btScalar(-0.863112), btScalar(-0.504596)),
+		btVector3(btScalar(-0.002093), btScalar(-0.936993), btScalar(0.349161)),
+		btVector3(btScalar(-0.106297), btScalar(-0.636610), btScalar(-0.763612)),
+		btVector3(btScalar(-0.229430), btScalar(-0.463769), btScalar(-0.855546)),
+		btVector3(btScalar(-0.245236), btScalar(-0.066175), btScalar(-0.966999)),
+		btVector3(btScalar(-0.351587), btScalar(-0.270513), btScalar(-0.896145)),
+		btVector3(btScalar(-0.370906), btScalar(0.133108), btScalar(-0.918982)),
+		btVector3(btScalar(-0.264360), btScalar(0.346000), btScalar(-0.900049)),
+		btVector3(btScalar(-0.151375), btScalar(0.543728), btScalar(-0.825291)),
+		btVector3(btScalar(-0.218697), btScalar(0.912741), btScalar(-0.344346)),
+		btVector3(btScalar(-0.274507), btScalar(0.953764), btScalar(-0.121635)),
+		btVector3(btScalar(-0.259677), btScalar(0.692266), btScalar(-0.673044)),
+		btVector3(btScalar(-0.350416), btScalar(0.798810), btScalar(-0.488786)),
+		btVector3(btScalar(-0.320170), btScalar(0.941127), btScalar(0.108297)),
+		btVector3(btScalar(-0.147667), btScalar(0.952792), btScalar(0.265034)),
+		btVector3(btScalar(-0.188061), btScalar(0.860636), btScalar(0.472910)),
+		btVector3(btScalar(-0.370906), btScalar(0.739900), btScalar(0.560941)),
+		btVector3(btScalar(-0.297143), btScalar(0.585334), btScalar(0.754178)),
+		btVector3(btScalar(-0.189622), btScalar(0.428241), btScalar(0.883393)),
+		btVector3(btScalar(-0.091272), btScalar(0.098695), btScalar(0.990747)),
+		btVector3(btScalar(-0.256945), btScalar(0.228375), btScalar(0.938827)),
+		btVector3(btScalar(-0.111761), btScalar(-0.133251), btScalar(0.984696)),
+		btVector3(btScalar(-0.118006), btScalar(-0.356253), btScalar(0.926725)),
+		btVector3(btScalar(-0.119372), btScalar(-0.563896), btScalar(0.817029)),
+		btVector3(btScalar(0.041228), btScalar(-0.833949), btScalar(0.550010)),
+		btVector3(btScalar(-0.121909), btScalar(-0.736543), btScalar(0.665172)),
+		btVector3(btScalar(-0.307681), btScalar(-0.931160), btScalar(-0.195026)),
+		btVector3(btScalar(-0.283679), btScalar(-0.957990), btScalar(0.041348)),
+		btVector3(btScalar(-0.227284), btScalar(-0.935243), btScalar(0.270890)),
+		btVector3(btScalar(-0.293436), btScalar(-0.858252), btScalar(-0.420860)),
+		btVector3(btScalar(-0.175767), btScalar(-0.780677), btScalar(-0.599262)),
+		btVector3(btScalar(-0.170108), btScalar(-0.858835), btScalar(0.482865)),
+		btVector3(btScalar(-0.332854), btScalar(-0.635055), btScalar(-0.696857)),
+		btVector3(btScalar(-0.447791), btScalar(-0.445299), btScalar(-0.775128)),
+		btVector3(btScalar(-0.470622), btScalar(-0.074146), btScalar(-0.879164)),
+		btVector3(btScalar(-0.639417), btScalar(-0.340505), btScalar(-0.689049)),
+		btVector3(btScalar(-0.598438), btScalar(0.104722), btScalar(-0.794256)),
+		btVector3(btScalar(-0.488575), btScalar(0.307699), btScalar(-0.816313)),
+		btVector3(btScalar(-0.379882), btScalar(0.513592), btScalar(-0.769077)),
+		btVector3(btScalar(-0.425740), btScalar(0.862775), btScalar(-0.272516)),
+		btVector3(btScalar(-0.480769), btScalar(0.875412), btScalar(-0.048439)),
+		btVector3(btScalar(-0.467890), btScalar(0.648716), btScalar(-0.600043)),
+		btVector3(btScalar(-0.543799), btScalar(0.730956), btScalar(-0.411881)),
+		btVector3(btScalar(-0.516284), btScalar(0.838277), btScalar(0.174076)),
+		btVector3(btScalar(-0.353343), btScalar(0.876384), btScalar(0.326519)),
+		btVector3(btScalar(-0.572875), btScalar(0.614497), btScalar(0.542007)),
+		btVector3(btScalar(-0.503600), btScalar(0.497261), btScalar(0.706161)),
+		btVector3(btScalar(-0.530920), btScalar(0.754870), btScalar(0.384685)),
+		btVector3(btScalar(-0.395884), btScalar(0.366414), btScalar(0.841818)),
+		btVector3(btScalar(-0.300656), btScalar(0.001678), btScalar(0.953661)),
+		btVector3(btScalar(-0.461060), btScalar(0.146912), btScalar(0.875000)),
+		btVector3(btScalar(-0.315486), btScalar(-0.232212), btScalar(0.919893)),
+		btVector3(btScalar(-0.323682), btScalar(-0.449187), btScalar(0.832644)),
+		btVector3(btScalar(-0.318999), btScalar(-0.639527), btScalar(0.699134)),
+		btVector3(btScalar(-0.496771), btScalar(-0.866029), btScalar(-0.055271)),
+		btVector3(btScalar(-0.496771), btScalar(-0.816257), btScalar(-0.294377)),
+		btVector3(btScalar(-0.456377), btScalar(-0.869528), btScalar(0.188130)),
+		btVector3(btScalar(-0.380858), btScalar(-0.827144), btScalar(0.412792)),
+		btVector3(btScalar(-0.449352), btScalar(-0.727405), btScalar(-0.518259)),
+		btVector3(btScalar(-0.570533), btScalar(-0.551064), btScalar(-0.608632)),
+		btVector3(btScalar(-0.656394), btScalar(-0.118280), btScalar(-0.744874)),
+		btVector3(btScalar(-0.756696), btScalar(-0.438105), btScalar(-0.484882)),
+		btVector3(btScalar(-0.801773), btScalar(-0.204798), btScalar(-0.561005)),
+		btVector3(btScalar(-0.785186), btScalar(0.038618), btScalar(-0.617805)),
+		btVector3(btScalar(-0.709082), btScalar(0.262399), btScalar(-0.654306)),
+		btVector3(btScalar(-0.583412), btScalar(0.462265), btScalar(-0.667383)),
+		btVector3(btScalar(-0.616001), btScalar(0.761286), btScalar(-0.201272)),
+		btVector3(btScalar(-0.660687), btScalar(0.750204), btScalar(0.020072)),
+		btVector3(btScalar(-0.744987), btScalar(0.435823), btScalar(-0.504791)),
+		btVector3(btScalar(-0.713765), btScalar(0.605554), btScalar(-0.351373)),
+		btVector3(btScalar(-0.686251), btScalar(0.687600), btScalar(0.236927)),
+		btVector3(btScalar(-0.680201), btScalar(0.429407), btScalar(0.593732)),
+		btVector3(btScalar(-0.733474), btScalar(0.546450), btScalar(0.403814)),
+		btVector3(btScalar(-0.591023), btScalar(0.292923), btScalar(0.751445)),
+		btVector3(btScalar(-0.500283), btScalar(-0.080757), btScalar(0.861922)),
+		btVector3(btScalar(-0.643710), btScalar(0.070115), btScalar(0.761985)),
+		btVector3(btScalar(-0.506332), btScalar(-0.308425), btScalar(0.805122)),
+		btVector3(btScalar(-0.503015), btScalar(-0.509847), btScalar(0.697573)),
+		btVector3(btScalar(-0.482525), btScalar(-0.682105), btScalar(0.549229)),
+		btVector3(btScalar(-0.680396), btScalar(-0.716323), btScalar(-0.153451)),
+		btVector3(btScalar(-0.658346), btScalar(-0.746264), btScalar(0.097562)),
+		btVector3(btScalar(-0.653272), btScalar(-0.646915), btScalar(-0.392948)),
+		btVector3(btScalar(-0.590828), btScalar(-0.732655), btScalar(0.337645)),
+		btVector3(btScalar(-0.819140), btScalar(-0.518013), btScalar(-0.246166)),
+		btVector3(btScalar(-0.900513), btScalar(-0.282178), btScalar(-0.330487)),
+		btVector3(btScalar(-0.914953), btScalar(-0.028652), btScalar(-0.402122)),
+		btVector3(btScalar(-0.859924), btScalar(0.220209), btScalar(-0.459898)),
+		btVector3(btScalar(-0.777185), btScalar(0.613720), btScalar(-0.137836)),
+		btVector3(btScalar(-0.805285), btScalar(0.586889), btScalar(0.082728)),
+		btVector3(btScalar(-0.872413), btScalar(0.406077), btScalar(-0.271735)),
+		btVector3(btScalar(-0.859339), btScalar(0.448072), btScalar(0.246101)),
+		btVector3(btScalar(-0.757671), btScalar(0.216320), btScalar(0.615594)),
+		btVector3(btScalar(-0.826165), btScalar(0.348139), btScalar(0.442851)),
+		btVector3(btScalar(-0.671810), btScalar(-0.162803), btScalar(0.722557)),
+		btVector3(btScalar(-0.796504), btScalar(-0.004543), btScalar(0.604468)),
+		btVector3(btScalar(-0.676298), btScalar(-0.378223), btScalar(0.631794)),
+		btVector3(btScalar(-0.668883), btScalar(-0.558258), btScalar(0.490673)),
+		btVector3(btScalar(-0.821287), btScalar(-0.570118), btScalar(0.006994)),
+		btVector3(btScalar(-0.767428), btScalar(-0.587810), btScalar(0.255470)),
+		btVector3(btScalar(-0.933296), btScalar(-0.349837), btScalar(-0.079865)),
+		btVector3(btScalar(-0.982667), btScalar(-0.100393), btScalar(-0.155208)),
+		btVector3(btScalar(-0.961396), btScalar(0.160910), btScalar(-0.222938)),
+		btVector3(btScalar(-0.934858), btScalar(0.354555), btScalar(-0.006864)),
+		btVector3(btScalar(-0.941687), btScalar(0.229736), btScalar(0.245711)),
+		btVector3(btScalar(-0.884317), btScalar(0.131552), btScalar(0.447536)),
+		btVector3(btScalar(-0.810359), btScalar(-0.219769), btScalar(0.542788)),
+		btVector3(btScalar(-0.915929), btScalar(-0.210048), btScalar(0.341743)),
+		btVector3(btScalar(-0.816799), btScalar(-0.407192), btScalar(0.408303)),
+		btVector3(btScalar(-0.903050), btScalar(-0.392416), btScalar(0.174076)),
+		btVector3(btScalar(-0.980325), btScalar(-0.170969), btScalar(0.096586)),
+		btVector3(btScalar(-0.995936), btScalar(0.084891), btScalar(0.029441)),
+		btVector3(btScalar(-0.960031), btScalar(0.002650), btScalar(0.279283)),
+	};
+	static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] =
 	{
 		btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
 		btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
@@ -165,6 +426,8 @@ btVector3* btShapeHull::getUnitSpherePoints()
 		btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
 		btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
 	};
+	if (highres)
+		return sUnitSpherePointsHighres;
 	return sUnitSpherePoints;
 }
 
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.h
index e959f198b6..78ea4b6501 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.h
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.h
@@ -34,7 +34,7 @@ protected:
 	unsigned int m_numIndices;
 	const btConvexShape* m_shape;
 
-	static btVector3* getUnitSpherePoints();
+	static btVector3* getUnitSpherePoints(int highres=0);
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -42,7 +42,7 @@ public:
 	btShapeHull (const btConvexShape* shape);
 	~btShapeHull ();
 
-	bool buildHull (btScalar margin);
+	bool buildHull (btScalar margin, int highres=0);
 
 	int numTriangles () const;
 	int numVertices () const;
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp
index d17141e3f2..b5e0e716d4 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp
@@ -69,8 +69,6 @@ void	btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const
 	//tangentDir0/tangentDir1 can be precalculated
 	btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1);
 
-	btVector3 supVertex0,supVertex1;
-
 	btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal;
 	
 	btVector3 triangle[3];
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
index 9e1544e87a..b7a6f74361 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
@@ -63,7 +63,7 @@ typedef btAlignedObjectArray<btIndexedMesh>	IndexedMeshArray;
 
 ///The btTriangleIndexVertexArray allows to access multiple triangle meshes, by indexing into existing triangle/index arrays.
 ///Additional meshes can be added using addIndexedMesh
-///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays.
+///No duplicate is made of the vertex/index data, it only indexes into external vertex/index arrays.
 ///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray.
 ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface
 {
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
index 940282f576..3481fec850 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
@@ -113,12 +113,7 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 	if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
 		return false;
 
-
-
 	btScalar lambda = btScalar(0.);
-	btVector3 v(1,0,0);
-
-	int maxIter = MAX_ITERATIONS;
 
 	btVector3 n;
 	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
@@ -137,8 +132,7 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 
 	btPointCollector	pointCollector1;
 
-	{
-	
+	{	
 		computeClosestPoints(fromA,fromB,pointCollector1);
 
 		hasResult = pointCollector1.m_hasResult;
@@ -172,28 +166,20 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 			
 			dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
 
-			
-			
-			lambda = lambda + dLambda;
+			lambda += dLambda;
 
-			if (lambda > btScalar(1.))
+			if (lambda > btScalar(1.) || lambda < btScalar(0.))
 				return false;
 
-			if (lambda < btScalar(0.))
-				return false;
-
-
 			//todo: next check with relative epsilon
 			if (lambda <= lastLambda)
 			{
 				return false;
 				//n.setValue(0,0,0);
-				break;
+				//break;
 			}
 			lastLambda = lambda;
 
-			
-
 			//interpolate to next lambda
 			btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
 
@@ -223,7 +209,7 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 			}
 
 			numIter++;
-			if (numIter > maxIter)
+			if (numIter > MAX_ITERATIONS)
 			{
 				result.reportFailure(-2, numIter);
 				return false;
@@ -237,6 +223,5 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 	}
 
 	return false;
-
 }
 
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
index bdc0572f75..528b5e0101 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
@@ -25,7 +25,7 @@ class btStaticPlaneShape;
 
 /// btContinuousConvexCollision implements angular and linear time of impact for convex objects.
 /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
-/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
+/// Algorithm operates in worldspace, in order to keep in between motion globally consistent.
 /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
 class btContinuousConvexCollision : public btConvexCast
 {
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
index 572ec36f56..b79f49d611 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
@@ -21,46 +21,64 @@ subject to the following restrictions:
 
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 
-bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
-											  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
-											  const btTransform& transformA, const btTransform& transformB,
-											  btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
-											  class btIDebugDraw* debugDraw)
+bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
+	const btConvexShape* pConvexA, const btConvexShape* pConvexB,
+	const btTransform& transformA, const btTransform& transformB,
+	btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
+	class btIDebugDraw* debugDraw)
 {
 
 	(void)debugDraw;
 	(void)v;
 	(void)simplexSolver;
 
-//	const btScalar				radialmargin(btScalar(0.));
-	
-	btVector3	guessVector(transformB.getOrigin()-transformA.getOrigin());
-	btGjkEpaSolver2::sResults	results;
-	
+	btVector3 guessVectors[] = {
+		btVector3(transformB.getOrigin() - transformA.getOrigin()).normalized(),
+		btVector3(transformA.getOrigin() - transformB.getOrigin()).normalized(),
+		btVector3(0, 0, 1),
+		btVector3(0, 1, 0),
+		btVector3(1, 0, 0),
+		btVector3(1, 1, 0),
+		btVector3(1, 1, 1),
+		btVector3(0, 1, 1),
+		btVector3(1, 0, 1),
+	};
 
-	if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
-								pConvexB,transformB,
-								guessVector,results))
-	
-		{
-	//	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
-		//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
-		wWitnessOnA = results.witnesses[0];
-		wWitnessOnB = results.witnesses[1];
-		v = results.normal;
-		return true;		
-		} else
+	int numVectors = sizeof(guessVectors) / sizeof(btVector3);
+
+	for (int i = 0; i < numVectors; i++)
 	{
-		if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results))
+		simplexSolver.reset();
+		btVector3	guessVector = guessVectors[i];
+
+		btGjkEpaSolver2::sResults	results;
+
+		if (btGjkEpaSolver2::Penetration(pConvexA, transformA,
+			pConvexB, transformB,
+			guessVector, results))
+
 		{
 			wWitnessOnA = results.witnesses[0];
 			wWitnessOnB = results.witnesses[1];
 			v = results.normal;
-			return false;
+			return true;
+		}
+		else
+		{
+			if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results))
+			{
+				wWitnessOnA = results.witnesses[0];
+				wWitnessOnB = results.witnesses[1];
+				v = results.normal;
+				return false;
+			}
 		}
 	}
 
+	//failed to find a distance/penetration
+	wWitnessOnA.setValue(0, 0, 0);
+	wWitnessOnB.setValue(0, 0, 0);
+	v.setValue(0, 0, 0);
 	return false;
 }
 
-
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index 257b026d9b..a0b825f0e8 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -1,4 +1,4 @@
-/*
+/*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
@@ -32,7 +32,7 @@ subject to the following restrictions:
 //must be above the machine epsilon
 #ifdef  BT_USE_DOUBLE_PRECISION
 	#define REL_ERROR2 btScalar(1.0e-12)
-	btScalar gGjkEpaPenetrationTolerance = 1e-7;
+	btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
 #else
 	#define REL_ERROR2 btScalar(1.0e-6)
 	btScalar gGjkEpaPenetrationTolerance = 0.001;
@@ -83,6 +83,593 @@ void	btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 	getClosestPointsNonVirtual(input,output,debugDraw);
 }
 
+static void btComputeSupport(const btConvexShape* convexA, const btTransform& localTransA, const btConvexShape* convexB, const btTransform& localTransB, const btVector3& dir, bool check2d, btVector3& supAworld, btVector3& supBworld, btVector3& aMinb)
+{
+	btVector3 seperatingAxisInA = (dir)* localTransA.getBasis();
+	btVector3 seperatingAxisInB = (-dir)* localTransB.getBasis();
+
+	btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+	btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+	btVector3 pInA = pInANoMargin;
+	btVector3 qInB = qInBNoMargin;
+
+	supAworld = localTransA(pInA);
+	supBworld = localTransB(qInB);
+
+	if (check2d)
+	{
+		supAworld[2] = 0.f;
+		supBworld[2] = 0.f;
+	}
+
+	aMinb = supAworld - supBworld;
+}
+
+struct btSupportVector 
+{
+	btVector3 v;  //!< Support point in minkowski sum
+	btVector3 v1; //!< Support point in obj1
+	btVector3 v2; //!< Support point in obj2
+};
+
+struct btSimplex 
+{
+	btSupportVector ps[4];
+	int last; //!< index of last added point
+};
+
+static btVector3 ccd_vec3_origin(0, 0, 0);
+
+
+inline void btSimplexInit(btSimplex *s)
+{
+	s->last = -1;
+}
+
+inline int btSimplexSize(const btSimplex *s)
+{
+	return s->last + 1;
+}
+
+inline const btSupportVector *btSimplexPoint(const btSimplex *s, int idx)
+{
+	// here is no check on boundaries
+	return &s->ps[idx];
+}
+inline void btSupportCopy(btSupportVector *d, const btSupportVector *s)
+{
+	*d = *s;
+}
+
+inline  void btVec3Copy(btVector3 *v, const btVector3* w)
+{
+	*v = *w;
+}
+
+inline void ccdVec3Add(btVector3*v, const btVector3*w)
+{
+	v->m_floats[0] += w->m_floats[0];
+	v->m_floats[1] += w->m_floats[1];
+	v->m_floats[2] += w->m_floats[2];
+}
+
+
+inline void ccdVec3Sub(btVector3 *v, const btVector3 *w)
+{
+	*v -= *w;
+}
+inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
+{
+	*d = (*v) - (*w);
+	
+}
+inline  btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
+{
+	btScalar dot;
+	dot = a->dot(*b);
+	
+	return dot;
+}
+
+inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3*b)
+{
+	btVector3 ab;
+	btVec3Sub2(&ab, a, b);
+	return btVec3Dot(&ab, &ab);
+}
+
+
+inline void btVec3Scale(btVector3 *d, btScalar k)
+{
+	d->m_floats[0] *= k;
+	d->m_floats[1] *= k;
+	d->m_floats[2] *= k;
+}
+
+inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
+{
+	d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]);
+	d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]);
+	d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]);
+}
+
+inline void btTripleCross(const btVector3 *a, const btVector3 *b,
+	const btVector3 *c, btVector3 *d)
+{
+	btVector3 e;
+	btVec3Cross(&e, a, b);
+	btVec3Cross(d, &e, c);
+}
+
+inline int ccdEq(btScalar _a, btScalar _b)
+{
+	btScalar ab;
+	btScalar a, b;
+
+	ab = btFabs(_a - _b);
+	if (btFabs(ab) < SIMD_EPSILON)
+		return 1;
+
+	a = btFabs(_a);
+	b = btFabs(_b);
+	if (b > a) {
+		return ab < SIMD_EPSILON * b;
+	}
+	else {
+		return ab < SIMD_EPSILON * a;
+	}
+}
+
+btScalar ccdVec3X(const btVector3* v)
+{
+	return v->x();
+}
+
+btScalar ccdVec3Y(const btVector3* v)
+{
+	return v->y();
+}
+
+btScalar ccdVec3Z(const btVector3* v)
+{
+	return v->z();
+}
+inline int btVec3Eq(const btVector3 *a, const btVector3 *b)
+{
+	return ccdEq(ccdVec3X(a), ccdVec3X(b))
+		&& ccdEq(ccdVec3Y(a), ccdVec3Y(b))
+		&& ccdEq(ccdVec3Z(a), ccdVec3Z(b));
+}
+
+
+inline void btSimplexAdd(btSimplex *s, const btSupportVector *v)
+{
+	// here is no check on boundaries in sake of speed
+	++s->last;
+	btSupportCopy(s->ps + s->last, v);
+}
+
+
+inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a)
+{
+	btSupportCopy(s->ps + pos, a);
+}
+
+inline void btSimplexSetSize(btSimplex *s, int size)
+{
+	s->last = size - 1;
+}
+
+inline const btSupportVector *ccdSimplexLast(const btSimplex *s)
+{
+	return btSimplexPoint(s, s->last);
+}
+
+inline int ccdSign(btScalar val)
+{
+	if (btFuzzyZero(val)) {
+		return 0;
+	}
+	else if (val < btScalar(0)) {
+		return -1;
+	}
+	return 1;
+}
+
+
+inline btScalar btVec3PointSegmentDist2(const btVector3 *P,
+	const btVector3 *x0,
+	const btVector3 *b,
+	btVector3 *witness)
+{
+	// The computation comes from solving equation of segment:
+	//      S(t) = x0 + t.d
+	//          where - x0 is initial point of segment
+	//                - d is direction of segment from x0 (|d| > 0)
+	//                - t belongs to <0, 1> interval
+	// 
+	// Than, distance from a segment to some point P can be expressed:
+	//      D(t) = |x0 + t.d - P|^2
+	//          which is distance from any point on segment. Minimization
+	//          of this function brings distance from P to segment.
+	// Minimization of D(t) leads to simple quadratic equation that's
+	// solving is straightforward.
+	//
+	// Bonus of this method is witness point for free.
+
+	btScalar dist, t;
+	btVector3 d, a;
+
+	// direction of segment
+	btVec3Sub2(&d, b, x0);
+
+	// precompute vector from P to x0
+	btVec3Sub2(&a, x0, P);
+
+	t = -btScalar(1.) * btVec3Dot(&a, &d);
+	t /= btVec3Dot(&d, &d);
+
+	if (t < btScalar(0) || btFuzzyZero(t)) {
+		dist = ccdVec3Dist2(x0, P);
+		if (witness)
+			btVec3Copy(witness, x0);
+	}
+	else if (t > btScalar(1) || ccdEq(t, btScalar(1))) {
+		dist = ccdVec3Dist2(b, P);
+		if (witness)
+			btVec3Copy(witness, b);
+	}
+	else {
+		if (witness) {
+			btVec3Copy(witness, &d);
+			btVec3Scale(witness, t);
+			ccdVec3Add(witness, x0);
+			dist = ccdVec3Dist2(witness, P);
+		}
+		else {
+			// recycling variables
+			btVec3Scale(&d, t);
+			ccdVec3Add(&d, &a);
+			dist = btVec3Dot(&d, &d);
+		}
+	}
+
+	return dist;
+}
+
+
+btScalar btVec3PointTriDist2(const btVector3 *P,
+	const btVector3 *x0, const btVector3 *B,
+	const btVector3 *C,
+	btVector3 *witness)
+{
+	// Computation comes from analytic expression for triangle (x0, B, C)
+	//      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
+	// Then equation for distance is:
+	//      D(s, t) = | T(s, t) - P |^2
+	// This leads to minimization of quadratic function of two variables.
+	// The solution from is taken only if s is between 0 and 1, t is
+	// between 0 and 1 and t + s < 1, otherwise distance from segment is
+	// computed.
+
+	btVector3 d1, d2, a;
+	double u, v, w, p, q, r;
+	double s, t, dist, dist2;
+	btVector3 witness2;
+
+	btVec3Sub2(&d1, B, x0);
+	btVec3Sub2(&d2, C, x0);
+	btVec3Sub2(&a, x0, P);
+
+	u = btVec3Dot(&a, &a);
+	v = btVec3Dot(&d1, &d1);
+	w = btVec3Dot(&d2, &d2);
+	p = btVec3Dot(&a, &d1);
+	q = btVec3Dot(&a, &d2);
+	r = btVec3Dot(&d1, &d2);
+
+	s = (q * r - w * p) / (w * v - r * r);
+	t = (-s * r - q) / w;
+
+	if ((btFuzzyZero(s) || s > btScalar(0))
+		&& (ccdEq(s, btScalar(1)) || s < btScalar(1))
+		&& (btFuzzyZero(t) || t > btScalar(0))
+		&& (ccdEq(t, btScalar(1)) || t < btScalar(1))
+		&& (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1))) {
+
+		if (witness) 
+		{
+			btVec3Scale(&d1, s);
+			btVec3Scale(&d2, t);
+			btVec3Copy(witness, x0);
+			ccdVec3Add(witness, &d1);
+			ccdVec3Add(witness, &d2);
+
+			dist = ccdVec3Dist2(witness, P);
+		}
+		else 
+		{
+			dist = s * s * v;
+			dist += t * t * w;
+			dist += btScalar(2.) * s * t * r;
+			dist += btScalar(2.) * s * p;
+			dist += btScalar(2.) * t * q;
+			dist += u;
+		}
+	}
+	else {
+		dist = btVec3PointSegmentDist2(P, x0, B, witness);
+
+		dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2);
+		if (dist2 < dist) {
+			dist = dist2;
+			if (witness)
+				btVec3Copy(witness, &witness2);
+		}
+
+		dist2 = btVec3PointSegmentDist2(P, B, C, &witness2);
+		if (dist2 < dist) {
+			dist = dist2;
+			if (witness)
+				btVec3Copy(witness, &witness2);
+		}
+	}
+
+	return dist;
+}
+
+
+static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
+{
+	const btSupportVector *A, *B;
+	btVector3 AB, AO, tmp;
+	btScalar dot;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other point
+	B = btSimplexPoint(simplex, 0);
+	// compute AB oriented segment
+	btVec3Sub2(&AB, &B->v, &A->v);
+	// compute AO vector
+	btVec3Copy(&AO, &A->v);
+	btVec3Scale(&AO, -btScalar(1));
+
+	// dot product AB . AO
+	dot = btVec3Dot(&AB, &AO);
+
+	// check if origin doesn't lie on AB segment
+	btVec3Cross(&tmp, &AB, &AO);
+	if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0)) {
+		return 1;
+	}
+
+	// check if origin is in area where AB segment is
+	if (btFuzzyZero(dot) || dot < btScalar(0)) {
+		// origin is in outside are of A
+		btSimplexSet(simplex, 0, A);
+		btSimplexSetSize(simplex, 1);
+		btVec3Copy(dir, &AO);
+	}
+	else {
+		// origin is in area where AB segment is
+
+		// keep simplex untouched and set direction to
+		// AB x AO x AB
+		btTripleCross(&AB, &AO, &AB, dir);
+	}
+
+	return 0;
+}
+
+
+
+static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
+{
+	const btSupportVector *A, *B, *C;
+	btVector3 AO, AB, AC, ABC, tmp;
+	btScalar dot, dist;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other points
+	B = btSimplexPoint(simplex, 1);
+	C = btSimplexPoint(simplex, 0);
+
+	// check touching contact
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
+	if (btFuzzyZero(dist)) {
+		return 1;
+	}
+
+	// check if triangle is really triangle (has area > 0)
+	// if not simplex can't be expanded and thus no itersection is found
+	if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v)) {
+		return -1;
+	}
+
+	// compute AO vector
+	btVec3Copy(&AO, &A->v);
+	btVec3Scale(&AO, -btScalar(1));
+
+	// compute AB and AC segments and ABC vector (perpendircular to triangle)
+	btVec3Sub2(&AB, &B->v, &A->v);
+	btVec3Sub2(&AC, &C->v, &A->v);
+	btVec3Cross(&ABC, &AB, &AC);
+
+	btVec3Cross(&tmp, &ABC, &AC);
+	dot = btVec3Dot(&tmp, &AO);
+	if (btFuzzyZero(dot) || dot > btScalar(0)) {
+		dot = btVec3Dot(&AC, &AO);
+		if (btFuzzyZero(dot) || dot > btScalar(0)) {
+			// C is already in place
+			btSimplexSet(simplex, 1, A);
+			btSimplexSetSize(simplex, 2);
+			btTripleCross(&AC, &AO, &AC, dir);
+		}
+		else {
+		
+			dot = btVec3Dot(&AB, &AO);
+			if (btFuzzyZero(dot) || dot > btScalar(0)) {
+				btSimplexSet(simplex, 0, B);
+				btSimplexSet(simplex, 1, A);
+				btSimplexSetSize(simplex, 2);
+				btTripleCross(&AB, &AO, &AB, dir);
+			}
+			else {
+				btSimplexSet(simplex, 0, A);
+				btSimplexSetSize(simplex, 1);
+				btVec3Copy(dir, &AO);
+			}
+		}
+	}
+	else {
+		btVec3Cross(&tmp, &AB, &ABC);
+		dot = btVec3Dot(&tmp, &AO);
+		if (btFuzzyZero(dot) || dot > btScalar(0)) 
+		{
+			dot = btVec3Dot(&AB, &AO);
+			if (btFuzzyZero(dot) || dot > btScalar(0)) {
+				btSimplexSet(simplex, 0, B);
+				btSimplexSet(simplex, 1, A);
+				btSimplexSetSize(simplex, 2);
+				btTripleCross(&AB, &AO, &AB, dir);
+			}
+			else {
+				btSimplexSet(simplex, 0, A);
+				btSimplexSetSize(simplex, 1);
+				btVec3Copy(dir, &AO);
+			}
+		}
+		else {
+			dot = btVec3Dot(&ABC, &AO);
+			if (btFuzzyZero(dot) || dot > btScalar(0)) {
+				btVec3Copy(dir, &ABC);
+			}
+			else {
+				btSupportVector tmp;
+				btSupportCopy(&tmp, C);
+				btSimplexSet(simplex, 0, B);
+				btSimplexSet(simplex, 1, &tmp);
+
+				btVec3Copy(dir, &ABC);
+				btVec3Scale(dir, -btScalar(1));
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
+{
+	const btSupportVector *A, *B, *C, *D;
+	btVector3 AO, AB, AC, AD, ABC, ACD, ADB;
+	int B_on_ACD, C_on_ADB, D_on_ABC;
+	int AB_O, AC_O, AD_O;
+	btScalar dist;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other points
+	B = btSimplexPoint(simplex, 2);
+	C = btSimplexPoint(simplex, 1);
+	D = btSimplexPoint(simplex, 0);
+
+	// check if tetrahedron is really tetrahedron (has volume > 0)
+	// if it is not simplex can't be expanded and thus no intersection is
+	// found
+	dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0);
+	if (btFuzzyZero(dist)) {
+		return -1;
+	}
+
+	// check if origin lies on some of tetrahedron's face - if so objects
+	// intersect
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+
+	// compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
+	btVec3Copy(&AO, &A->v);
+	btVec3Scale(&AO, -btScalar(1));
+	btVec3Sub2(&AB, &B->v, &A->v);
+	btVec3Sub2(&AC, &C->v, &A->v);
+	btVec3Sub2(&AD, &D->v, &A->v);
+	btVec3Cross(&ABC, &AB, &AC);
+	btVec3Cross(&ACD, &AC, &AD);
+	btVec3Cross(&ADB, &AD, &AB);
+
+	// side (positive or negative) of B, C, D relative to planes ACD, ADB
+	// and ABC respectively
+	B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB));
+	C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC));
+	D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD));
+
+	// whether origin is on same side of ACD, ADB, ABC as B, C, D
+	// respectively
+	AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD;
+	AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB;
+	AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC;
+
+	if (AB_O && AC_O && AD_O) {
+		// origin is in tetrahedron
+		return 1;
+		// rearrange simplex to triangle and call btDoSimplex3()
+	}
+	else if (!AB_O) {
+		// B is farthest from the origin among all of the tetrahedron's
+		// points, so remove it from the list and go on with the triangle
+		// case
+
+		// D and C are in place
+		btSimplexSet(simplex, 2, A);
+		btSimplexSetSize(simplex, 3);
+	}
+	else if (!AC_O) {
+		// C is farthest
+		btSimplexSet(simplex, 1, D);
+		btSimplexSet(simplex, 0, B);
+		btSimplexSet(simplex, 2, A);
+		btSimplexSetSize(simplex, 3);
+	}
+	else { // (!AD_O)
+		btSimplexSet(simplex, 0, C);
+		btSimplexSet(simplex, 1, B);
+		btSimplexSet(simplex, 2, A);
+		btSimplexSetSize(simplex, 3);
+	}
+
+	return btDoSimplex3(simplex, dir);
+}
+
+static int btDoSimplex(btSimplex *simplex, btVector3 *dir)
+{
+	if (btSimplexSize(simplex) == 2) {
+		// simplex contains segment only one segment
+		return btDoSimplex2(simplex, dir);
+	}
+	else if (btSimplexSize(simplex) == 3) {
+		// simplex contains triangle
+		return btDoSimplex3(simplex, dir);
+	}
+	else { // btSimplexSize(simplex) == 4
+			// tetrahedron - this is the only shape which can encapsule origin
+			// so btDoSimplex4() also contains test on it
+		return btDoSimplex4(simplex, dir);
+	}
+}
+
 #ifdef __SPU__
 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
 #else
@@ -123,193 +710,308 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 	bool checkSimplex = false;
 	bool checkPenetration = true;
 	m_degenerateSimplex = 0;
-
+	
 	m_lastUsedMethod = -1;
-
+	int status = -2;
+	btVector3 orgNormalInB(0, 0, 0);
+	btScalar margin = marginA + marginB;
+	
+	//we add a separate implementation to check if the convex shapes intersect
+	//See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang
+	//Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver
+	//and remove this temporary code from libCCD
+	//this fixes issue https://github.com/bulletphysics/bullet3/issues/1703
+	//note, for large differences in shapes, use double precision build!
 	{
 		btScalar squaredDistance = BT_LARGE_FLOAT;
 		btScalar delta = btScalar(0.);
-		
-		btScalar margin = marginA + marginB;
-		
-		
 
-		m_simplexSolver->reset();
+
+
 		
-		for ( ; ; )
-		//while (true)
-		{
+		btSimplex simplex1;
+		btSimplex* simplex = &simplex1;
+		btSimplexInit(simplex);
 
-			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
-			btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+		btVector3 dir(1, 0, 0);
 
+		{
 
-			btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
-			btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+			btVector3 lastSupV;
+			btVector3 supAworld;
+			btVector3 supBworld;
+			btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
+			
+			btSupportVector last;
+			last.v = lastSupV;
+			last.v1 = supAworld;
+			last.v2 = supBworld;
 
-			btVector3  pWorld = localTransA(pInA);	
-			btVector3  qWorld = localTransB(qInB);
+			btSimplexAdd(simplex, &last);
 
+			dir = -lastSupV;
 
-			if (check2d)
+			
+			
+			// start iterations
+			for (int iterations = 0; iterations <gGjkMaxIter; iterations++)
 			{
-				pWorld[2] = 0.f;
-				qWorld[2] = 0.f;
-			}
+				// obtain support point
+				btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
+
+				// check if farthest point in Minkowski difference in direction dir
+				// isn't somewhere before origin (the test on negative dot product)
+				// - because if it is, objects are not intersecting at all.
+				btScalar delta = lastSupV.dot(dir);
+				if (delta < 0)
+				{
+					//no intersection, besides margin
+					status = -1;
+					break;
+				}
+				
+				// add last support vector to simplex
+				last.v = lastSupV;
+				last.v1 = supAworld;
+				last.v2 = supBworld;
 
-			btVector3 w	= pWorld - qWorld;
-			delta = m_cachedSeparatingAxis.dot(w);
+				btSimplexAdd(simplex, &last);
 
-			// potential exit, they don't overlap
-			if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
-			{
-				m_degenerateSimplex = 10;
-				checkSimplex=true;
-				//checkPenetration = false;
-				break;
-			}
+				// if btDoSimplex returns 1 if objects intersect, -1 if objects don't
+				// intersect and 0 if algorithm should continue
 
-			//exit 0: the new point is already in the simplex, or we didn't come any closer
-			if (m_simplexSolver->inSimplex(w))
-			{
-				m_degenerateSimplex = 1;
-				checkSimplex = true;
-				break;
-			}
-			// are we getting any closer ?
-			btScalar f0 = squaredDistance - delta;
-			btScalar f1 = squaredDistance * REL_ERROR2;
+				btVector3 newDir;
+				int do_simplex_res = btDoSimplex(simplex, &dir); 
 
-			if (f0 <= f1)
-			{
-				if (f0 <= btScalar(0.))
+				if (do_simplex_res == 1) 
 				{
-					m_degenerateSimplex = 2;
-				} else
+					status = 0; // intersection found
+					break;
+				}
+				else if (do_simplex_res == -1) 
+				{
+					// intersection not found
+					status = -1;
+					break;
+				}
+				
+				if (btFuzzyZero(btVec3Dot(&dir, &dir))) 
+				{
+					// intersection not found
+					status = -1;
+				}
+
+				if (dir.length2() < SIMD_EPSILON)
 				{
-					m_degenerateSimplex = 11;
+					//no intersection, besides margin
+					status = -1;
+					break;
+				}
+
+				if (dir.fuzzyZero())
+				{
+					// intersection not found
+					status = -1;
+					break;
 				}
-				checkSimplex = true;
-				break;
 			}
 
-			//add current vertex to simplex
-			m_simplexSolver->addVertex(w, pWorld, qWorld);
-			btVector3 newCachedSeparatingAxis;
+		}
+
+		m_simplexSolver->reset();
+		if (status == 0)
+		{
+			//status = 0;
+			//printf("Intersect!\n");
+		}
 
-			//calculate the closest point to the origin (update vector v)
-			if (!m_simplexSolver->closest(newCachedSeparatingAxis))
+		if (status==-1)
+		{
+			//printf("not intersect\n");
+		}
+		//printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]);
+		if (1)
+		{
+			for (; ; )
+				//while (true)
 			{
-				m_degenerateSimplex = 3;
-				checkSimplex = true;
-				break;
-			}
 
-			if(newCachedSeparatingAxis.length2()<REL_ERROR2)
-            {
-				m_cachedSeparatingAxis = newCachedSeparatingAxis;
-                m_degenerateSimplex = 6;
-                checkSimplex = true;
-                break;
-            }
+				btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
+				btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();
+
+
+				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+				btVector3  pWorld = localTransA(pInA);
+				btVector3  qWorld = localTransB(qInB);
 
-			btScalar previousSquaredDistance = squaredDistance;
-			squaredDistance = newCachedSeparatingAxis.length2();
+
+				if (check2d)
+				{
+					pWorld[2] = 0.f;
+					qWorld[2] = 0.f;
+				}
+
+				btVector3 w = pWorld - qWorld;
+				delta = m_cachedSeparatingAxis.dot(w);
+
+				// potential exit, they don't overlap
+				if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
+				{
+					m_degenerateSimplex = 10;
+					checkSimplex = true;
+					//checkPenetration = false;
+					break;
+				}
+
+				//exit 0: the new point is already in the simplex, or we didn't come any closer
+				if (m_simplexSolver->inSimplex(w))
+				{
+					m_degenerateSimplex = 1;
+					checkSimplex = true;
+					break;
+				}
+				// are we getting any closer ?
+				btScalar f0 = squaredDistance - delta;
+				btScalar f1 = squaredDistance * REL_ERROR2;
+
+				if (f0 <= f1)
+				{
+					if (f0 <= btScalar(0.))
+					{
+						m_degenerateSimplex = 2;
+					}
+					else
+					{
+						m_degenerateSimplex = 11;
+					}
+					checkSimplex = true;
+					break;
+				}
+
+				//add current vertex to simplex
+				m_simplexSolver->addVertex(w, pWorld, qWorld);
+				btVector3 newCachedSeparatingAxis;
+
+				//calculate the closest point to the origin (update vector v)
+				if (!m_simplexSolver->closest(newCachedSeparatingAxis))
+				{
+					m_degenerateSimplex = 3;
+					checkSimplex = true;
+					break;
+				}
+
+				if (newCachedSeparatingAxis.length2() < REL_ERROR2)
+				{
+					m_cachedSeparatingAxis = newCachedSeparatingAxis;
+					m_degenerateSimplex = 6;
+					checkSimplex = true;
+					break;
+				}
+
+				btScalar previousSquaredDistance = squaredDistance;
+				squaredDistance = newCachedSeparatingAxis.length2();
 #if 0
-///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
-			if (squaredDistance>previousSquaredDistance)
-			{
-				m_degenerateSimplex = 7;
-				squaredDistance = previousSquaredDistance;
-                checkSimplex = false;
-                break;
-			}
+				///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
+				if (squaredDistance > previousSquaredDistance)
+				{
+					m_degenerateSimplex = 7;
+					squaredDistance = previousSquaredDistance;
+					checkSimplex = false;
+					break;
+				}
 #endif //
-			
 
-			//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
 
-			//are we getting any closer ?
-			if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
-			{ 
-//				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
-				checkSimplex = true;
-				m_degenerateSimplex = 12;
-				
-				break;
-			}
+				//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
 
-			m_cachedSeparatingAxis = newCachedSeparatingAxis;
+				//are we getting any closer ?
+				if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
+				{
+					//				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+					checkSimplex = true;
+					m_degenerateSimplex = 12;
+
+					break;
+				}
 
-			  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
-              if (m_curIter++ > gGjkMaxIter)   
-              {   
-                      #if defined(DEBUG) || defined (_DEBUG)
+				m_cachedSeparatingAxis = newCachedSeparatingAxis;
 
-                              printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
-                              printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
-                              m_cachedSeparatingAxis.getX(),   
-                              m_cachedSeparatingAxis.getY(),   
-                              m_cachedSeparatingAxis.getZ(),   
-                              squaredDistance,   
-                              m_minkowskiA->getShapeType(),   
-                              m_minkowskiB->getShapeType());   
+				//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
+				if (m_curIter++ > gGjkMaxIter)
+				{
+#if defined(DEBUG) || defined (_DEBUG)
 
-                      #endif   
-                      break;   
+					printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
+					printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
+						m_cachedSeparatingAxis.getX(),
+						m_cachedSeparatingAxis.getY(),
+						m_cachedSeparatingAxis.getZ(),
+						squaredDistance,
+						m_minkowskiA->getShapeType(),
+						m_minkowskiB->getShapeType());
 
-              } 
+#endif   
+					break;
 
+				}
 
-			bool check = (!m_simplexSolver->fullSimplex());
-			//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
 
-			if (!check)
-			{
-				//do we need this backup_closest here ?
-//				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
-				m_degenerateSimplex = 13;
-				break;
+				bool check = (!m_simplexSolver->fullSimplex());
+				//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+
+				if (!check)
+				{
+					//do we need this backup_closest here ?
+	//				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+					m_degenerateSimplex = 13;
+					break;
+				}
 			}
-		}
 
-		if (checkSimplex)
-		{
+			if (checkSimplex)
+			{
 			m_simplexSolver->compute_points(pointOnA, pointOnB);
 			normalInB = m_cachedSeparatingAxis;
 
 			btScalar lenSqr =m_cachedSeparatingAxis.length2();
 			
-			//valid normal
-			if (lenSqr < REL_ERROR2)
-			{
-				m_degenerateSimplex = 5;
-			} 
-			if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
-			{
-				btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
-				normalInB *= rlen; //normalize
+				//valid normal
+				if (lenSqr < REL_ERROR2)
+				{
+					m_degenerateSimplex = 5;
+				}
+				if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
+				{
+					btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
+					normalInB *= rlen; //normalize
 
-				btScalar s = btSqrt(squaredDistance);
-			
-				btAssert(s > btScalar(0.0));
-				pointOnA -= m_cachedSeparatingAxis * (marginA / s);
-				pointOnB += m_cachedSeparatingAxis * (marginB / s);
-				distance = ((btScalar(1.)/rlen) - margin);
-				isValid = true;
-				
-				m_lastUsedMethod = 1;
-			} else
-			{
-				m_lastUsedMethod = 2;
+					btScalar s = btSqrt(squaredDistance);
+
+					btAssert(s > btScalar(0.0));
+					pointOnA -= m_cachedSeparatingAxis * (marginA / s);
+					pointOnB += m_cachedSeparatingAxis * (marginB / s);
+					distance = ((btScalar(1.) / rlen) - margin);
+					isValid = true;
+					orgNormalInB = normalInB;
+
+					m_lastUsedMethod = 1;
+				}
+				else
+				{
+					m_lastUsedMethod = 2;
+				}
 			}
 		}
 
+
+		
 		bool catchDegeneratePenetrationCase = 
 			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
 
 		//if (checkPenetration && !isValid)
-		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
+		if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase )) || (status == 0))
 		{
 			//penetration case
 
@@ -331,70 +1033,79 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					);
 
 
-				if (isValid2)
+				if (m_cachedSeparatingAxis.length2())
 				{
-					btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
-					btScalar lenSqr = tmpNormalInB.length2();
-					if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+					if (isValid2)
 					{
-						tmpNormalInB = m_cachedSeparatingAxis;
-						lenSqr = m_cachedSeparatingAxis.length2();
-					}
+						btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
+						btScalar lenSqr = tmpNormalInB.length2();
+						if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+						{
+							tmpNormalInB = m_cachedSeparatingAxis;
+							lenSqr = m_cachedSeparatingAxis.length2();
+						}
 
-					if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
-					{
-						tmpNormalInB /= btSqrt(lenSqr);
-						btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
-						m_lastUsedMethod = 3;
-						//only replace valid penetrations when the result is deeper (check)
-						if (!isValid || (distance2 < distance))
+						if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
 						{
-							distance = distance2;
-							pointOnA = tmpPointOnA;
-							pointOnB = tmpPointOnB;
-							normalInB = tmpNormalInB;
-							
-							isValid = true;
-							
-						} else
+							tmpNormalInB /= btSqrt(lenSqr);
+							btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
+							m_lastUsedMethod = 3;
+							//only replace valid penetrations when the result is deeper (check)
+							if (!isValid || (distance2 < distance))
+							{
+								distance = distance2;
+								pointOnA = tmpPointOnA;
+								pointOnB = tmpPointOnB;
+								normalInB = tmpNormalInB;
+								isValid = true;
+
+							}
+							else
+							{
+								m_lastUsedMethod = 8;
+							}
+						}
+						else
 						{
-							m_lastUsedMethod = 8;
+							m_lastUsedMethod = 9;
 						}
-					} else
-					{
-						m_lastUsedMethod = 9;
 					}
-				} else
-
-				{
-					///this is another degenerate case, where the initial GJK calculation reports a degenerate case
-					///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
-					///reports a valid positive distance. Use the results of the second GJK instead of failing.
-					///thanks to Jacob.Langford for the reproduction case
-					///http://code.google.com/p/bullet/issues/detail?id=250
+					else
 
-				
-					if (m_cachedSeparatingAxis.length2() > btScalar(0.))
 					{
-						btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
-						//only replace valid distances when the distance is less
-						if (!isValid || (distance2 < distance))
-						{
-							distance = distance2;
-							pointOnA = tmpPointOnA;
-							pointOnB = tmpPointOnB;
-							pointOnA -= m_cachedSeparatingAxis * marginA ;
-							pointOnB += m_cachedSeparatingAxis * marginB ;
-							normalInB = m_cachedSeparatingAxis;
-							normalInB.normalize();
-
-							isValid = true;
-							m_lastUsedMethod = 6;
-						} else
+						///this is another degenerate case, where the initial GJK calculation reports a degenerate case
+						///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
+						///reports a valid positive distance. Use the results of the second GJK instead of failing.
+						///thanks to Jacob.Langford for the reproduction case
+						///http://code.google.com/p/bullet/issues/detail?id=250
+
+
+						if (m_cachedSeparatingAxis.length2() > btScalar(0.))
 						{
-							m_lastUsedMethod = 5;
+							btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
+							//only replace valid distances when the distance is less
+							if (!isValid || (distance2 < distance))
+							{
+								distance = distance2;
+								pointOnA = tmpPointOnA;
+								pointOnB = tmpPointOnB;
+								pointOnA -= m_cachedSeparatingAxis * marginA;
+								pointOnB += m_cachedSeparatingAxis * marginB;
+								normalInB = m_cachedSeparatingAxis;
+								normalInB.normalize();
+
+								isValid = true;
+								m_lastUsedMethod = 6;
+							}
+							else
+							{
+								m_lastUsedMethod = 5;
+							}
 						}
 					}
+				} else
+				{
+					//printf("EPA didn't return a valid value\n");
 				}
 				
 			}
@@ -409,17 +1120,33 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 		m_cachedSeparatingAxis = normalInB;
 		m_cachedSeparatingDistance = distance;
-
+		if (1)
 		{
 		///todo: need to track down this EPA penetration solver degeneracy
 		///the penetration solver reports penetration but the contact normal
 		///connecting the contact points is pointing in the opposite direction
 		///until then, detect the issue and revert the normal
 
+			btScalar d2 = 0.f;
+			{
+				btVector3 seperatingAxisInA = (-orgNormalInB)* localTransA.getBasis();
+				btVector3 seperatingAxisInB = orgNormalInB* localTransB.getBasis();
+
+
+				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+				btVector3  pWorld = localTransA(pInA);
+				btVector3  qWorld = localTransB(qInB);
+				btVector3 w = pWorld - qWorld;
+				d2 = orgNormalInB.dot(w)- margin;
+			}
+			
 			btScalar d1=0;
 			{
-				btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
-				btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
+				
+				btVector3 seperatingAxisInA = (normalInB)* localTransA.getBasis();
+				btVector3 seperatingAxisInB = -normalInB* localTransB.getBasis();
 			
 
 				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
@@ -428,7 +1155,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 				btVector3  pWorld = localTransA(pInA);	
 				btVector3  qWorld = localTransB(qInB);
 				btVector3 w	= pWorld - qWorld;
-				d1 = (-normalInB).dot(w);
+				d1 = (-normalInB).dot(w)- margin;
+
 			}
 			btScalar d0 = 0.f;
 			{
@@ -442,21 +1170,37 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 				btVector3  pWorld = localTransA(pInA);	
 				btVector3  qWorld = localTransB(qInB);
 				btVector3 w	= pWorld - qWorld;
-				d0 = normalInB.dot(w);
+				d0 = normalInB.dot(w)-margin;
 			}
+			
 			if (d1>d0)
 			{
 				m_lastUsedMethod = 10;
 				normalInB*=-1;
 			} 
 
+			if (orgNormalInB.length2())
+			{
+				if (d2 > d0 && d2 > d1 && d2 > distance)
+				{
+
+					normalInB = orgNormalInB;
+					distance = d2;
+				}
+			}
 		}
+
+
 		output.addContactPoint(
 			normalInB,
 			pointOnB+positionOffset,
 			distance);
 
 	}
+	else
+	{
+		//printf("invalid gjk query\n");
+	}
 
 
 }
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
index 23aaece22b..9603a8bbdc 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
@@ -16,7 +16,13 @@ subject to the following restrictions:
 
 #include "btPersistentManifold.h"
 #include "LinearMath/btTransform.h"
+#include "LinearMath/btSerializer.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btCollisionObjectData btCollisionObjectDoubleData
+#else
+#define btCollisionObjectData btCollisionObjectFloatData
+#endif
 
 btScalar					gContactBreakingThreshold = btScalar(0.02);
 ContactDestroyedCallback	gContactDestroyedCallback = 0;
@@ -33,6 +39,8 @@ btPersistentManifold::btPersistentManifold()
 m_body0(0),
 m_body1(0),
 m_cachedPoints (0),
+m_companionIdA(0),
+m_companionIdB(0),
 m_index1a(0)
 {
 }
@@ -303,6 +311,149 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT
 }
 
 
+int	btPersistentManifold::calculateSerializeBufferSize()	const
+{
+	return sizeof(btPersistentManifoldData);
+}
+
+const char*	btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
+{
+	btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
+	memset(dataOut, 0, sizeof(btPersistentManifoldData));
+
+	dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0());
+	dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1());
+	dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
+	dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
+	dataOut->m_numCachedPoints = manifold->getNumContacts();
+	dataOut->m_companionIdA = manifold->m_companionIdA;
+	dataOut->m_companionIdB = manifold->m_companionIdB;
+	dataOut->m_index1a = manifold->m_index1a;
+	dataOut->m_objectType = manifold->m_objectType;
+
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		const btManifoldPoint& pt = manifold->getContactPoint(i);
+		dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
+		dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
+		dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
+		pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]);
+		dataOut->m_pointCacheDistance[i] = pt.m_distance1;
+		dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1;
+		dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1;
+		dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime;
+		dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
+		dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
+		dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
+		dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
+		dataOut->m_pointCacheIndex0[i] = pt.m_index0;
+		dataOut->m_pointCacheIndex1[i] = pt.m_index1;
+		dataOut->m_pointCachePartId0[i] = pt.m_partId0;
+		dataOut->m_pointCachePartId1[i] = pt.m_partId1;
+		pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]);
+		dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction;
+		pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]);
+		dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction;
+		dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction;
+		dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution;
+		dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1;
+		dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2;
+	}
+	return btPersistentManifoldDataName;
+}
+
+void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr)
+{
+	m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
+	m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
+	m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
+	m_companionIdA = manifoldDataPtr->m_companionIdA;
+	m_companionIdB = manifoldDataPtr->m_companionIdB;
+	//m_index1a = manifoldDataPtr->m_index1a;
+	m_objectType = manifoldDataPtr->m_objectType;
+
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		btManifoldPoint& pt = m_pointCache[i];
+		
+		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
+		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
+		pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
+		pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
+		pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
+		pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
+		pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
+		pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
+		pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
+		pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
+		pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
+		pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
+		pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
+		pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
+		pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
+		pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
+		pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
+		pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
+		pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
+		pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
+		pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
+		pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
+		pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
+	}
 
+}
 
+void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr)
+{
+	m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
+	m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
+	m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
+	m_companionIdA = manifoldDataPtr->m_companionIdA;
+	m_companionIdB = manifoldDataPtr->m_companionIdB;
+	//m_index1a = manifoldDataPtr->m_index1a;
+	m_objectType = manifoldDataPtr->m_objectType;
+
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		btManifoldPoint& pt = m_pointCache[i];
+
+		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
+		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
+		pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
+		pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
+		pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
+		pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
+		pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
+		pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
+		pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
+		pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
+		pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
+		pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
+		pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
+		pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
+		pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
+		pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
+		pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
+		pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
+		pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
+		pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
+		pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
+		pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
+		pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
+	}
 
+}
\ No newline at end of file
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
index f872c8e1c9..67be0c48eb 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
@@ -24,6 +24,8 @@ class btCollisionObject;
 #include "LinearMath/btAlignedAllocator.h"
 
 struct btCollisionResult;
+struct btCollisionObjectDoubleData;
+struct btCollisionObjectFloatData;
 
 ///maximum contact breaking and merging threshold
 extern btScalar gContactBreakingThreshold;
@@ -95,7 +97,10 @@ public:
 		: btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
 	m_body0(body0),m_body1(body1),m_cachedPoints(0),
 		m_contactBreakingThreshold(contactBreakingThreshold),
-		m_contactProcessingThreshold(contactProcessingThreshold)
+		m_contactProcessingThreshold(contactProcessingThreshold),
+		m_companionIdA(0),
+		m_companionIdB(0),
+		m_index1a(0)
 	{
 	}
 
@@ -256,10 +261,115 @@ public:
 		m_cachedPoints = 0;
 	}
 
+	int	calculateSerializeBufferSize()	const;
+	const char*	serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
+	void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
+	void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr);
 
 
-}
-;
+};
+
+
+
+struct btPersistentManifoldDoubleData
+{
+	btVector3DoubleData m_pointCacheLocalPointA[4];
+	btVector3DoubleData m_pointCacheLocalPointB[4];
+	btVector3DoubleData m_pointCachePositionWorldOnA[4];
+	btVector3DoubleData m_pointCachePositionWorldOnB[4];
+	btVector3DoubleData m_pointCacheNormalWorldOnB[4];
+	btVector3DoubleData	m_pointCacheLateralFrictionDir1[4];
+	btVector3DoubleData	m_pointCacheLateralFrictionDir2[4];
+	double m_pointCacheDistance[4];
+	double m_pointCacheAppliedImpulse[4];
+	double m_pointCacheCombinedFriction[4];
+	double m_pointCacheCombinedRollingFriction[4];
+	double m_pointCacheCombinedSpinningFriction[4];
+	double m_pointCacheCombinedRestitution[4];
+	int	m_pointCachePartId0[4];
+	int	m_pointCachePartId1[4];
+	int	m_pointCacheIndex0[4];
+	int	m_pointCacheIndex1[4];
+	int m_pointCacheContactPointFlags[4];
+	double m_pointCacheAppliedImpulseLateral1[4];
+	double m_pointCacheAppliedImpulseLateral2[4];
+	double m_pointCacheContactMotion1[4];
+	double m_pointCacheContactMotion2[4];
+	double m_pointCacheContactCFM[4];
+	double m_pointCacheCombinedContactStiffness1[4];
+	double m_pointCacheContactERP[4];
+	double m_pointCacheCombinedContactDamping1[4];
+	double m_pointCacheFrictionCFM[4];
+	int m_pointCacheLifeTime[4];
+
+	int m_numCachedPoints;
+	int m_companionIdA;
+	int m_companionIdB;
+	int m_index1a;
+
+	int m_objectType;
+	double	m_contactBreakingThreshold;
+	double	m_contactProcessingThreshold;
+	int m_padding;
+
+	btCollisionObjectDoubleData *m_body0;
+	btCollisionObjectDoubleData *m_body1;
+};
+
+
+struct btPersistentManifoldFloatData
+{
+	btVector3FloatData m_pointCacheLocalPointA[4];
+	btVector3FloatData m_pointCacheLocalPointB[4];
+	btVector3FloatData m_pointCachePositionWorldOnA[4];
+	btVector3FloatData m_pointCachePositionWorldOnB[4];
+	btVector3FloatData m_pointCacheNormalWorldOnB[4];
+	btVector3FloatData	m_pointCacheLateralFrictionDir1[4];
+	btVector3FloatData	m_pointCacheLateralFrictionDir2[4];
+	float m_pointCacheDistance[4];
+	float m_pointCacheAppliedImpulse[4];
+	float m_pointCacheCombinedFriction[4];
+	float m_pointCacheCombinedRollingFriction[4];
+	float m_pointCacheCombinedSpinningFriction[4];
+	float m_pointCacheCombinedRestitution[4];
+	int	m_pointCachePartId0[4];
+	int	m_pointCachePartId1[4];
+	int	m_pointCacheIndex0[4];
+	int	m_pointCacheIndex1[4];
+	int m_pointCacheContactPointFlags[4];
+	float m_pointCacheAppliedImpulseLateral1[4];
+	float m_pointCacheAppliedImpulseLateral2[4];
+	float m_pointCacheContactMotion1[4];
+	float m_pointCacheContactMotion2[4];
+	float m_pointCacheContactCFM[4];
+	float m_pointCacheCombinedContactStiffness1[4];
+	float m_pointCacheContactERP[4];
+	float m_pointCacheCombinedContactDamping1[4];
+	float m_pointCacheFrictionCFM[4];
+	int m_pointCacheLifeTime[4];
+
+	int m_numCachedPoints;
+	int m_companionIdA;
+	int m_companionIdB;
+	int m_index1a;
+
+	int m_objectType;
+	float	m_contactBreakingThreshold;
+	float	m_contactProcessingThreshold;
+	int m_padding;
+
+	btCollisionObjectFloatData *m_body0;
+	btCollisionObjectFloatData *m_body1;
+};
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btPersistentManifoldData	btPersistentManifoldDoubleData
+#define btPersistentManifoldDataName	"btPersistentManifoldDoubleData"
+#else
+#define btPersistentManifoldData	btPersistentManifoldFloatData
+#define btPersistentManifoldDataName	"btPersistentManifoldFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
 
 
 
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
index ec638f60ba..08d6e6de86 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
@@ -72,11 +72,18 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 
 
 	btScalar dist2 = v.length2();
+
 #ifdef BT_USE_DOUBLE_PRECISION
-	btScalar epsilon = btScalar(0.0001);
+	btScalar epsilon = SIMD_EPSILON * 10;
 #else
+//todo: epsilon kept for backward compatibility of unit tests.
+//will need to digg deeper to make the algorithm more robust
+//since, a large epsilon can cause an early termination with false
+//positive results (ray intersections that shouldn't be there)
 	btScalar epsilon = btScalar(0.0001);
 #endif //BT_USE_DOUBLE_PRECISION
+
+
 	btVector3	w,p;
 	btScalar VdotR;