Merge pull request #21836 from AndreaCatania/bulletUpdate

Update bullet to Master 12409f1118a7c7a266f9071350c70789dfe73bb9

modules/bullet/SCsub

@@ -68,11 +68,13 @@ if env['builtin_bullet']:
         , "BulletCollision/CollisionShapes/btEmptyShape.cpp"
         , "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp"
         , "BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp"
+	, "BulletCollision/CollisionShapes/btMiniSDF.cpp"
         , "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp"
         , "BulletCollision/CollisionShapes/btMultiSphereShape.cpp"
         , "BulletCollision/CollisionShapes/btOptimizedBvh.cpp"
         , "BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp"
         , "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp"
+        , "BulletCollision/CollisionShapes/btSdfCollisionShape.cpp"
         , "BulletCollision/CollisionShapes/btShapeHull.cpp"
         , "BulletCollision/CollisionShapes/btSphereShape.cpp"
         , "BulletCollision/CollisionShapes/btStaticPlaneShape.cpp"

thirdparty/README.md

@@ -19,7 +19,7 @@ comments.
 ## bullet
 
 - Upstream: https://github.com/bulletphysics/bullet3
-- Version: git (d05ad4b, 2017)
+- Version: git (12409f1118a7c7a266f9071350c70789dfe73bb9, Commits on Sep 6, 2018 )
 - License: zlib
 
 Files extracted from upstream source:

thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h

@@ -528,6 +528,14 @@ protected:
 		otherArray.copy(0, otherSize, m_data);
 	}
 
+	void removeAtIndex(int index)
+    {
+        if (index<size())
+        {
+            swap( index,size()-1);
+            pop_back();
+        }
+    }
 };
 
 #endif //B3_OBJECT_ARRAY__

thirdparty/bullet/Bullet3Common/b3FileUtils.h

@@ -36,7 +36,7 @@ struct b3FileUtils
 
 			for (int i=0;!f && i<numPrefixes;i++)
 			{
-#ifdef _WIN32
+#ifdef _MSC_VER
 				sprintf_s(relativeFileName,maxRelativeFileNameMaxLen,"%s%s",prefix[i],orgFileName);
 #else
 				sprintf(relativeFileName,"%s%s",prefix[i],orgFileName);

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp

@@ -599,8 +599,8 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
 													tag_vel, 
 													info->fps * limot->m_stopERP);
 				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
-                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
-                info->m_upperLimit[srow] = limot->m_maxMotorForce;
+                info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+                info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
             }
         }
         if(limit)

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h

@@ -69,7 +69,7 @@ public:
     {
     	m_accumulatedImpulse = 0.f;
         m_targetVelocity = 0;
-        m_maxMotorForce = 0.1f;
+        m_maxMotorForce = 6.0f;
         m_maxLimitForce = 300.0f;
         m_loLimit = 1.0f;
         m_hiLimit = -1.0f;

thirdparty/bullet/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp

@@ -619,7 +619,7 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
 		strippedName = strip2(clFileNameForCaching,"\\");
 		strippedName = strip2(strippedName,"/");
 	
-#ifdef _MSVC_VER
+#ifdef _MSC_VER
 		sprintf_s(binaryFileName,B3_MAX_STRING_LENGTH,"%s/%s.%s.%s.bin",sCachedBinaryPath,strippedName, deviceName,driverVersion );
 #else
 		sprintf(binaryFileName,"%s/%s.%s.%s.bin",sCachedBinaryPath,strippedName, deviceName,driverVersion );

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

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,

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;

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

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

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--;
 		}
 	}
 }

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];

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--;
-				} 
+				}
 
 			}
 

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;
 					}

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);

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;
 

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;
 }
 

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;
 };
 
 

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();
 }

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

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);

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);
 				}
 				

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();
 	
+			}
 		}
 	
 	}

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;
 			}
+
 			
 		}
 

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) 

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);
 	}

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;
 

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)

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

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();
 

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
 	{

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;

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;

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__

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;
+}
+

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

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;

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;

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;
+}

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

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;
 }
 

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;

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];

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
 {

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;
-
 }
 

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
 {

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;
 }
 
-

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];
+	}
 
+}

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
+
 
 
 

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;
 	

thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp

@@ -0,0 +1,1128 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btBatchedConstraints.h"
+
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btStackAlloc.h"
+#include "LinearMath/btQuickprof.h"
+
+#include <string.h> //for memset
+
+const int kNoMerge = -1;
+
+bool btBatchedConstraints::s_debugDrawBatches = false;
+
+
+struct btBatchedConstraintInfo
+{
+    int constraintIndex;
+    int numConstraintRows;
+    int bodyIds[2];
+};
+
+
+struct btBatchInfo
+{
+    int numConstraints;
+    int mergeIndex;
+
+    btBatchInfo() : numConstraints(0), mergeIndex(kNoMerge) {}
+};
+
+
+bool btBatchedConstraints::validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const
+{
+    //
+    // validate: for debugging only. Verify coloring of bodies, that no body is touched by more than one batch in any given phase
+    //
+    int errors = 0;
+    const int kUnassignedBatch = -1;
+
+    btAlignedObjectArray<int> bodyBatchId;
+    for (int iPhase = 0; iPhase < m_phases.size(); ++iPhase)
+    {
+        bodyBatchId.resizeNoInitialize(0);
+        bodyBatchId.resize( bodies.size(), kUnassignedBatch );
+        const Range& phase = m_phases[iPhase];
+        for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
+        {
+            const Range& batch = m_batches[iBatch];
+            for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
+            {
+                int iCons = m_constraintIndices[iiCons];
+                const btSolverConstraint& cons = constraints->at(iCons);
+                const btSolverBody& bodyA = bodies[cons.m_solverBodyIdA];
+                const btSolverBody& bodyB = bodies[cons.m_solverBodyIdB];
+                if (! bodyA.internalGetInvMass().isZero())
+                {
+                    int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdA];
+                    if (thisBodyBatchId == kUnassignedBatch)
+                    {
+                        bodyBatchId[cons.m_solverBodyIdA] = iBatch;
+                    }
+                    else if (thisBodyBatchId != iBatch)
+                    {
+                        btAssert( !"dynamic body is used in 2 different batches in the same phase" );
+                        errors++;
+                    }
+                }
+                if (! bodyB.internalGetInvMass().isZero())
+                {
+                    int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdB];
+                    if (thisBodyBatchId == kUnassignedBatch)
+                    {
+                        bodyBatchId[cons.m_solverBodyIdB] = iBatch;
+                    }
+                    else if (thisBodyBatchId != iBatch)
+                    {
+                        btAssert( !"dynamic body is used in 2 different batches in the same phase" );
+                        errors++;
+                    }
+                }
+            }
+        }
+    }
+    return errors == 0;
+}
+
+
+static void debugDrawSingleBatch( const btBatchedConstraints* bc,
+    btConstraintArray* constraints,
+    const btAlignedObjectArray<btSolverBody>& bodies,
+    int iBatch,
+    const btVector3& color,
+    const btVector3& offset
+    )
+{
+    if (bc && bc->m_debugDrawer && iBatch < bc->m_batches.size())
+    {
+        const btBatchedConstraints::Range& b = bc->m_batches[iBatch];
+        for (int iiCon = b.begin; iiCon < b.end; ++iiCon)
+        {
+            int iCon = bc->m_constraintIndices[iiCon];
+            const btSolverConstraint& con = constraints->at(iCon);
+            int iBody0 = con.m_solverBodyIdA;
+            int iBody1 = con.m_solverBodyIdB;
+            btVector3 pos0 = bodies[iBody0].getWorldTransform().getOrigin() + offset;
+            btVector3 pos1 = bodies[iBody1].getWorldTransform().getOrigin() + offset;
+            bc->m_debugDrawer->drawLine(pos0, pos1, color);
+        }
+    }
+}
+
+
+static void debugDrawPhase( const btBatchedConstraints* bc,
+    btConstraintArray* constraints,
+    const btAlignedObjectArray<btSolverBody>& bodies,
+    int iPhase,
+    const btVector3& color0,
+    const btVector3& color1,
+    const btVector3& offset
+    )
+{
+    BT_PROFILE( "debugDrawPhase" );
+    if ( bc && bc->m_debugDrawer && iPhase < bc->m_phases.size() )
+    {
+        const btBatchedConstraints::Range& phase = bc->m_phases[iPhase];
+        for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
+        {
+            float tt = float(iBatch - phase.begin) / float(btMax(1, phase.end - phase.begin - 1));
+            btVector3 col = lerp(color0, color1, tt);
+            debugDrawSingleBatch(bc, constraints, bodies, iBatch, col, offset);
+        }
+    }
+}
+
+
+static void debugDrawAllBatches( const btBatchedConstraints* bc,
+    btConstraintArray* constraints,
+    const btAlignedObjectArray<btSolverBody>& bodies
+    )
+{
+    BT_PROFILE( "debugDrawAllBatches" );
+    if ( bc && bc->m_debugDrawer && bc->m_phases.size() > 0 )
+    {
+        btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
+        btVector3 bboxMax = -bboxMin;
+        for (int iBody = 0; iBody < bodies.size(); ++iBody)
+        {
+            const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin();
+            bboxMin.setMin(pos);
+            bboxMax.setMax(pos);
+        }
+        btVector3 bboxExtent = bboxMax - bboxMin;
+        btVector3 offsetBase = btVector3( 0, bboxExtent.y()*1.1f, 0 );
+        btVector3 offsetStep = btVector3( 0, 0, bboxExtent.z()*1.1f );
+        int numPhases = bc->m_phases.size();
+        for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+        {
+            float b = float(iPhase)/float(numPhases-1);
+            btVector3 color0 = btVector3(1,0,b);
+            btVector3 color1 = btVector3(0,1,b);
+            btVector3 offset = offsetBase + offsetStep*(float(iPhase) - float(numPhases-1)*0.5);
+            debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset);
+        }
+    }
+}
+
+
+static void initBatchedBodyDynamicFlags(btAlignedObjectArray<bool>* outBodyDynamicFlags, const btAlignedObjectArray<btSolverBody>& bodies)
+{
+    BT_PROFILE("initBatchedBodyDynamicFlags");
+    btAlignedObjectArray<bool>& bodyDynamicFlags = *outBodyDynamicFlags;
+    bodyDynamicFlags.resizeNoInitialize(bodies.size());
+    for (int i = 0; i < bodies.size(); ++i)
+    {
+        const btSolverBody& body = bodies[ i ];
+        bodyDynamicFlags[i] = ( body.internalGetInvMass().x() > btScalar( 0 ) );
+    }
+}
+
+
+static int runLengthEncodeConstraintInfo(btBatchedConstraintInfo* outConInfos, int numConstraints)
+{
+    BT_PROFILE("runLengthEncodeConstraintInfo");
+    // detect and run-length encode constraint rows that repeat the same bodies
+    int iDest = 0;
+    int iSrc = 0;
+    while (iSrc < numConstraints)
+    {
+        const btBatchedConstraintInfo& srcConInfo = outConInfos[iSrc];
+        btBatchedConstraintInfo& conInfo = outConInfos[iDest];
+        conInfo.constraintIndex = iSrc;
+        conInfo.bodyIds[0] = srcConInfo.bodyIds[0];
+        conInfo.bodyIds[1] = srcConInfo.bodyIds[1];
+        while (iSrc < numConstraints && outConInfos[iSrc].bodyIds[0] == srcConInfo.bodyIds[0] && outConInfos[iSrc].bodyIds[1] == srcConInfo.bodyIds[1])
+        {
+            ++iSrc;
+        }
+        conInfo.numConstraintRows = iSrc - conInfo.constraintIndex;
+        ++iDest;
+    }
+    return iDest;
+}
+
+
+struct ReadSolverConstraintsLoop : public btIParallelForBody
+{
+    btBatchedConstraintInfo* m_outConInfos;
+    btConstraintArray* m_constraints;
+
+    ReadSolverConstraintsLoop( btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints )
+    {
+        m_outConInfos = outConInfos;
+        m_constraints = constraints;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        for (int i = iBegin; i < iEnd; ++i)
+        {
+            btBatchedConstraintInfo& conInfo = m_outConInfos[i];
+            const btSolverConstraint& con = m_constraints->at( i );
+            conInfo.bodyIds[0] = con.m_solverBodyIdA;
+            conInfo.bodyIds[1] = con.m_solverBodyIdB;
+            conInfo.constraintIndex = i;
+            conInfo.numConstraintRows = 1;
+        }
+    }
+};
+
+
+static int initBatchedConstraintInfo(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints)
+{
+    BT_PROFILE("initBatchedConstraintInfo");
+    int numConstraints = constraints->size();
+    bool inParallel = true;
+    if (inParallel)
+    {
+        ReadSolverConstraintsLoop loop(outConInfos, constraints);
+        int grainSize = 1200;
+        btParallelFor(0, numConstraints, grainSize, loop);
+    }
+    else
+    {
+        for (int i = 0; i < numConstraints; ++i)
+        {
+            btBatchedConstraintInfo& conInfo = outConInfos[i];
+            const btSolverConstraint& con = constraints->at( i );
+            conInfo.bodyIds[0] = con.m_solverBodyIdA;
+            conInfo.bodyIds[1] = con.m_solverBodyIdB;
+            conInfo.constraintIndex = i;
+            conInfo.numConstraintRows = 1;
+        }
+    }
+    bool useRunLengthEncoding = true;
+    if (useRunLengthEncoding)
+    {
+        numConstraints = runLengthEncodeConstraintInfo(outConInfos, numConstraints);
+    }
+    return numConstraints;
+}
+
+
+static void expandConstraintRowsInPlace(int* constraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
+{
+    BT_PROFILE("expandConstraintRowsInPlace");
+    if (numConstraintRows > numConstraints)
+    {
+        // we walk the array in reverse to avoid overwriteing
+        for (int iCon = numConstraints - 1; iCon >= 0; --iCon)
+        {
+            const btBatchedConstraintInfo& conInfo = conInfos[iCon];
+            int iBatch = constraintBatchIds[iCon];
+            for (int i = conInfo.numConstraintRows - 1; i >= 0; --i)
+            {
+                int iDest = conInfo.constraintIndex + i;
+                btAssert(iDest >= iCon);
+                btAssert(iDest >= 0 && iDest < numConstraintRows);
+                constraintBatchIds[iDest] = iBatch;
+            }
+        }
+    }
+}
+
+
+static void expandConstraintRows(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
+{
+    BT_PROFILE("expandConstraintRows");
+    for ( int iCon = 0; iCon < numConstraints; ++iCon )
+    {
+        const btBatchedConstraintInfo& conInfo = conInfos[ iCon ];
+        int iBatch = srcConstraintBatchIds[ iCon ];
+        for ( int i = 0; i < conInfo.numConstraintRows; ++i )
+        {
+            int iDest = conInfo.constraintIndex + i;
+            btAssert( iDest >= iCon );
+            btAssert( iDest >= 0 && iDest < numConstraintRows );
+            destConstraintBatchIds[ iDest ] = iBatch;
+        }
+    }
+}
+
+
+struct ExpandConstraintRowsLoop : public btIParallelForBody
+{
+    int* m_destConstraintBatchIds;
+    const int* m_srcConstraintBatchIds;
+    const btBatchedConstraintInfo* m_conInfos;
+    int m_numConstraintRows;
+
+    ExpandConstraintRowsLoop( int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraintRows)
+    {
+        m_destConstraintBatchIds = destConstraintBatchIds;
+        m_srcConstraintBatchIds = srcConstraintBatchIds;
+        m_conInfos = conInfos;
+        m_numConstraintRows = numConstraintRows;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        expandConstraintRows(m_destConstraintBatchIds, m_srcConstraintBatchIds + iBegin, m_conInfos + iBegin, iEnd - iBegin, m_numConstraintRows);
+    }
+};
+
+
+static void expandConstraintRowsMt(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
+{
+    BT_PROFILE("expandConstraintRowsMt");
+    ExpandConstraintRowsLoop loop(destConstraintBatchIds, srcConstraintBatchIds, conInfos, numConstraintRows);
+    int grainSize = 600;
+    btParallelFor(0, numConstraints, grainSize, loop);
+}
+
+
+static void initBatchedConstraintInfoArray(btAlignedObjectArray<btBatchedConstraintInfo>* outConInfos, btConstraintArray* constraints)
+{
+    BT_PROFILE("initBatchedConstraintInfoArray");
+    btAlignedObjectArray<btBatchedConstraintInfo>& conInfos = *outConInfos;
+    int numConstraints = constraints->size();
+    conInfos.resizeNoInitialize(numConstraints);
+
+    int newSize = initBatchedConstraintInfo(&outConInfos->at(0), constraints);
+    conInfos.resizeNoInitialize(newSize);
+}
+
+
+static void mergeSmallBatches(btBatchInfo* batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize)
+{
+    BT_PROFILE("mergeSmallBatches");
+    for ( int iBatch = iEndBatch - 1; iBatch >= iBeginBatch; --iBatch )
+    {
+        btBatchInfo& batch = batches[ iBatch ];
+        if ( batch.mergeIndex == kNoMerge && batch.numConstraints > 0 && batch.numConstraints < minBatchSize )
+        {
+            for ( int iDestBatch = iBatch - 1; iDestBatch >= iBeginBatch; --iDestBatch )
+            {
+                btBatchInfo& destBatch = batches[ iDestBatch ];
+                if ( destBatch.mergeIndex == kNoMerge && ( destBatch.numConstraints + batch.numConstraints ) < maxBatchSize )
+                {
+                    destBatch.numConstraints += batch.numConstraints;
+                    batch.numConstraints = 0;
+                    batch.mergeIndex = iDestBatch;
+                    break;
+                }
+            }
+        }
+    }
+    // flatten mergeIndexes
+    // e.g. in case where A was merged into B and then B was merged into C, we need A to point to C instead of B
+    // Note: loop goes forward through batches because batches always merge from higher indexes to lower,
+    //     so by going from low to high it reduces the amount of trail-following
+    for ( int iBatch = iBeginBatch; iBatch < iEndBatch; ++iBatch )
+    {
+        btBatchInfo& batch = batches[ iBatch ];
+        if ( batch.mergeIndex != kNoMerge )
+        {
+            int iMergeDest = batches[ batch.mergeIndex ].mergeIndex;
+            // follow trail of merges to the end
+            while ( iMergeDest != kNoMerge )
+            {
+                int iNext = batches[ iMergeDest ].mergeIndex;
+                if ( iNext == kNoMerge )
+                {
+                    batch.mergeIndex = iMergeDest;
+                    break;
+                }
+                iMergeDest = iNext;
+            }
+        }
+    }
+}
+
+
+static void updateConstraintBatchIdsForMerges(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
+{
+    BT_PROFILE("updateConstraintBatchIdsForMerges");
+    // update batchIds to account for merges
+    for (int i = 0; i < numConstraints; ++i)
+    {
+        int iBatch = constraintBatchIds[i];
+        btAssert(iBatch < numBatches);
+        // if this constraint references a batch that was merged into another batch
+        if (batches[iBatch].mergeIndex != kNoMerge)
+        {
+            // update batchId
+            constraintBatchIds[i] = batches[iBatch].mergeIndex;
+        }
+    }
+}
+
+
+struct UpdateConstraintBatchIdsForMergesLoop : public btIParallelForBody
+{
+    int* m_constraintBatchIds;
+    const btBatchInfo* m_batches;
+    int m_numBatches;
+
+    UpdateConstraintBatchIdsForMergesLoop( int* constraintBatchIds, const btBatchInfo* batches, int numBatches )
+    {
+        m_constraintBatchIds = constraintBatchIds;
+        m_batches = batches;
+        m_numBatches = numBatches;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        BT_PROFILE( "UpdateConstraintBatchIdsForMergesLoop" );
+        updateConstraintBatchIdsForMerges( m_constraintBatchIds + iBegin, iEnd - iBegin, m_batches, m_numBatches );
+    }
+};
+
+
+static void updateConstraintBatchIdsForMergesMt(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
+{
+    BT_PROFILE( "updateConstraintBatchIdsForMergesMt" );
+    UpdateConstraintBatchIdsForMergesLoop loop(constraintBatchIds, batches, numBatches);
+    int grainSize = 800;
+    btParallelFor(0, numConstraints, grainSize, loop);
+}
+
+
+inline bool BatchCompare(const btBatchedConstraints::Range& a, const btBatchedConstraints::Range& b)
+{
+    int lenA = a.end - a.begin;
+    int lenB = b.end - b.begin;
+    return lenA > lenB;
+}
+
+
+static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints* bc,
+    const int* constraintBatchIds,
+    int numConstraints,
+    int* constraintIdPerBatch,
+    int batchBegin,
+    int batchEnd
+    )
+{
+    BT_PROFILE("writeOutConstraintIndicesForRangeOfBatches");
+    for ( int iCon = 0; iCon < numConstraints; ++iCon )
+    {
+        int iBatch = constraintBatchIds[ iCon ];
+        if (iBatch >= batchBegin && iBatch < batchEnd)
+        {
+            int iDestCon = constraintIdPerBatch[ iBatch ];
+            constraintIdPerBatch[ iBatch ] = iDestCon + 1;
+            bc->m_constraintIndices[ iDestCon ] = iCon;
+        }
+    }
+}
+
+
+struct WriteOutConstraintIndicesLoop : public btIParallelForBody
+{
+    btBatchedConstraints* m_batchedConstraints;
+    const int* m_constraintBatchIds;
+    int m_numConstraints;
+    int* m_constraintIdPerBatch;
+    int m_maxNumBatchesPerPhase;
+
+    WriteOutConstraintIndicesLoop( btBatchedConstraints* bc, const int* constraintBatchIds, int numConstraints, int* constraintIdPerBatch, int maxNumBatchesPerPhase )
+    {
+        m_batchedConstraints = bc;
+        m_constraintBatchIds = constraintBatchIds;
+        m_numConstraints = numConstraints;
+        m_constraintIdPerBatch = constraintIdPerBatch;
+        m_maxNumBatchesPerPhase = maxNumBatchesPerPhase;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        BT_PROFILE( "WriteOutConstraintIndicesLoop" );
+        int batchBegin = iBegin * m_maxNumBatchesPerPhase;
+        int batchEnd = iEnd * m_maxNumBatchesPerPhase;
+        writeOutConstraintIndicesForRangeOfBatches(m_batchedConstraints,
+            m_constraintBatchIds,
+            m_numConstraints,
+            m_constraintIdPerBatch,
+            batchBegin,
+            batchEnd
+        );
+    }
+};
+
+
+static void writeOutConstraintIndicesMt(btBatchedConstraints* bc,
+    const int* constraintBatchIds,
+    int numConstraints,
+    int* constraintIdPerBatch,
+    int maxNumBatchesPerPhase,
+    int numPhases
+    )
+{
+    BT_PROFILE("writeOutConstraintIndicesMt");
+    bool inParallel = true;
+    if (inParallel)
+    {
+        WriteOutConstraintIndicesLoop loop( bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase );
+        btParallelFor( 0, numPhases, 1, loop );
+    }
+    else
+    {
+        for ( int iCon = 0; iCon < numConstraints; ++iCon )
+        {
+            int iBatch = constraintBatchIds[ iCon ];
+            int iDestCon = constraintIdPerBatch[ iBatch ];
+            constraintIdPerBatch[ iBatch ] = iDestCon + 1;
+            bc->m_constraintIndices[ iDestCon ] = iCon;
+        }
+    }
+}
+
+
+static void writeGrainSizes(btBatchedConstraints* bc)
+{
+    typedef btBatchedConstraints::Range Range;
+    int numPhases = bc->m_phases.size();
+    bc->m_phaseGrainSize.resizeNoInitialize(numPhases);
+    int numThreads = btGetTaskScheduler()->getNumThreads();
+    for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+    {
+        const Range& phase = bc->m_phases[ iPhase ];
+        int numBatches = phase.end - phase.begin;
+        float grainSize = floor((0.25f*numBatches / float(numThreads)) + 0.0f);
+        bc->m_phaseGrainSize[ iPhase ] = btMax(1, int(grainSize));
+    }
+}
+
+
+static void writeOutBatches(btBatchedConstraints* bc,
+    const int* constraintBatchIds,
+    int numConstraints,
+    const btBatchInfo* batches,
+    int* batchWork,
+    int maxNumBatchesPerPhase,
+    int numPhases
+)
+{
+    BT_PROFILE("writeOutBatches");
+    typedef btBatchedConstraints::Range Range;
+    bc->m_constraintIndices.reserve( numConstraints );
+    bc->m_batches.resizeNoInitialize( 0 );
+    bc->m_phases.resizeNoInitialize( 0 );
+
+    //int maxNumBatches = numPhases * maxNumBatchesPerPhase;
+    {
+        int* constraintIdPerBatch = batchWork;  // for each batch, keep an index into the next available slot in the m_constraintIndices array
+        int iConstraint = 0;
+        for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+        {
+            int curPhaseBegin = bc->m_batches.size();
+            int iBegin = iPhase * maxNumBatchesPerPhase;
+            int iEnd = iBegin + maxNumBatchesPerPhase;
+            for ( int i = iBegin; i < iEnd; ++i )
+            {
+                const btBatchInfo& batch = batches[ i ];
+                int curBatchBegin = iConstraint;
+                constraintIdPerBatch[ i ] = curBatchBegin;  // record the start of each batch in m_constraintIndices array
+                int numConstraints = batch.numConstraints;
+                iConstraint += numConstraints;
+                if ( numConstraints > 0 )
+                {
+                    bc->m_batches.push_back( Range( curBatchBegin, iConstraint ) );
+                }
+            }
+            // if any batches were emitted this phase,
+            if ( bc->m_batches.size() > curPhaseBegin )
+            {
+                // output phase
+                bc->m_phases.push_back( Range( curPhaseBegin, bc->m_batches.size() ) );
+            }
+        }
+
+        btAssert(iConstraint == numConstraints);
+        bc->m_constraintIndices.resizeNoInitialize( numConstraints );
+        writeOutConstraintIndicesMt( bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase, numPhases );
+    }
+    // for each phase
+    for (int iPhase = 0; iPhase < bc->m_phases.size(); ++iPhase)
+    {
+        // sort the batches from largest to smallest (can be helpful to some task schedulers)
+        const Range& curBatches = bc->m_phases[iPhase];
+        bc->m_batches.quickSortInternal(BatchCompare, curBatches.begin, curBatches.end-1);
+    }
+    bc->m_phaseOrder.resize(bc->m_phases.size());
+    for (int i = 0; i < bc->m_phases.size(); ++i)
+    {
+        bc->m_phaseOrder[i] = i;
+    }
+    writeGrainSizes(bc);
+}
+
+
+//
+// PreallocatedMemoryHelper -- helper object for allocating a number of chunks of memory in a single contiguous block.
+//                             It is generally more efficient to do a single larger allocation than many smaller allocations.
+//
+// Example Usage:
+//
+//  btVector3* bodyPositions = NULL;
+//  btBatchedConstraintInfo* conInfos = NULL;
+//  {
+//    PreallocatedMemoryHelper<8> memHelper;
+//    memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() );
+//    memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints );
+//    void* memPtr = malloc( memHelper.getSizeToAllocate() );  // allocate the memory
+//    memHelper.setChunkPointers( memPtr );  // update pointers to chunks
+//  }
+template <int N>
+class PreallocatedMemoryHelper
+{
+    struct Chunk
+    {
+        void** ptr;
+        size_t size;
+    };
+    Chunk m_chunks[N];
+    int m_numChunks;
+public:
+    PreallocatedMemoryHelper() {m_numChunks=0;}
+    void addChunk( void** ptr, size_t sz )
+    {
+        btAssert( m_numChunks < N );
+        if ( m_numChunks < N )
+        {
+            Chunk& chunk = m_chunks[ m_numChunks ];
+            chunk.ptr = ptr;
+            chunk.size = sz;
+            m_numChunks++;
+        }
+    }
+    size_t getSizeToAllocate() const
+    {
+        size_t totalSize = 0;
+        for (int i = 0; i < m_numChunks; ++i)
+        {
+            totalSize += m_chunks[i].size;
+        }
+        return totalSize;
+    }
+    void setChunkPointers(void* mem) const
+    {
+        size_t totalSize = 0;
+        for (int i = 0; i < m_numChunks; ++i)
+        {
+            const Chunk& chunk = m_chunks[ i ];
+            char* chunkPtr = static_cast<char*>(mem) + totalSize;
+            *chunk.ptr = chunkPtr;
+            totalSize += chunk.size;
+        }
+    }
+};
+
+
+
+static btVector3 findMaxDynamicConstraintExtent(
+    btVector3* bodyPositions,
+    bool* bodyDynamicFlags,
+    btBatchedConstraintInfo* conInfos,
+    int numConstraints,
+    int numBodies
+    )
+{
+    BT_PROFILE("findMaxDynamicConstraintExtent");
+    btVector3 consExtent = btVector3(1,1,1) * 0.001;
+    for (int iCon = 0; iCon < numConstraints; ++iCon)
+    {
+        const btBatchedConstraintInfo& con = conInfos[ iCon ];
+        int iBody0 = con.bodyIds[0];
+        int iBody1 = con.bodyIds[1];
+        btAssert(iBody0 >= 0 && iBody0 < numBodies);
+        btAssert(iBody1 >= 0 && iBody1 < numBodies);
+        // is it a dynamic constraint?
+        if (bodyDynamicFlags[iBody0] && bodyDynamicFlags[iBody1])
+        {
+            btVector3 delta = bodyPositions[iBody1] - bodyPositions[iBody0];
+            consExtent.setMax(delta.absolute());
+        }
+    }
+    return consExtent;
+}
+
+
+struct btIntVec3
+{
+    int m_ints[ 3 ];
+
+    SIMD_FORCE_INLINE const int& operator[](int i) const {return m_ints[i];}
+    SIMD_FORCE_INLINE int&       operator[](int i)       {return m_ints[i];}
+};
+
+
+struct AssignConstraintsToGridBatchesParams
+{
+    bool* bodyDynamicFlags;
+    btIntVec3* bodyGridCoords;
+    int numBodies;
+    btBatchedConstraintInfo* conInfos;
+    int* constraintBatchIds;
+    btIntVec3 gridChunkDim;
+    int maxNumBatchesPerPhase;
+    int numPhases;
+    int phaseMask;
+
+    AssignConstraintsToGridBatchesParams()
+    {
+        memset(this, 0, sizeof(*this));
+    }
+};
+
+
+static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams& params, int iConBegin, int iConEnd)
+{
+    BT_PROFILE("assignConstraintsToGridBatches");
+    // (can be done in parallel)
+    for ( int iCon = iConBegin; iCon < iConEnd; ++iCon )
+    {
+        const btBatchedConstraintInfo& con = params.conInfos[ iCon ];
+        int iBody0 = con.bodyIds[ 0 ];
+        int iBody1 = con.bodyIds[ 1 ];
+        int iPhase = iCon; //iBody0; // pseudorandom choice to distribute evenly amongst phases
+        iPhase &= params.phaseMask;
+        int gridCoord[ 3 ];
+        // is it a dynamic constraint?
+        if ( params.bodyDynamicFlags[ iBody0 ] && params.bodyDynamicFlags[ iBody1 ] )
+        {
+            const btIntVec3& body0Coords = params.bodyGridCoords[iBody0];
+            const btIntVec3& body1Coords = params.bodyGridCoords[iBody1];
+            // for each dimension x,y,z,
+            for (int i = 0; i < 3; ++i)
+            {
+                int coordMin = btMin(body0Coords.m_ints[i], body1Coords.m_ints[i]);
+                int coordMax = btMax(body0Coords.m_ints[i], body1Coords.m_ints[i]);
+                if (coordMin != coordMax)
+                {
+                    btAssert( coordMax == coordMin + 1 );
+                    if ((coordMin&1) == 0)
+                    {
+                        iPhase &= ~(1 << i); // force bit off
+                    }
+                    else
+                    {
+                        iPhase |= (1 << i); // force bit on
+                        iPhase &= params.phaseMask;
+                    }
+                }
+                gridCoord[ i ] = coordMin;
+            }
+        }
+        else
+        {
+            if ( !params.bodyDynamicFlags[ iBody0 ] )
+            {
+                iBody0 = con.bodyIds[ 1 ];
+            }
+            btAssert(params.bodyDynamicFlags[ iBody0 ]);
+            const btIntVec3& body0Coords = params.bodyGridCoords[iBody0];
+            // for each dimension x,y,z,
+            for ( int i = 0; i < 3; ++i )
+            {
+                gridCoord[ i ] = body0Coords.m_ints[ i ];
+            }
+        }
+        // calculate chunk coordinates
+        int chunkCoord[ 3 ];
+        btIntVec3 gridChunkDim = params.gridChunkDim;
+        // for each dimension x,y,z,
+        for ( int i = 0; i < 3; ++i )
+        {
+            int coordOffset = ( iPhase >> i ) & 1;
+            chunkCoord[ i ] = (gridCoord[ i ] - coordOffset)/2;
+            btClamp( chunkCoord[ i ], 0, gridChunkDim[ i ] - 1);
+            btAssert( chunkCoord[ i ] < gridChunkDim[ i ] );
+        }
+        int iBatch = iPhase * params.maxNumBatchesPerPhase + chunkCoord[ 0 ] + chunkCoord[ 1 ] * gridChunkDim[ 0 ] + chunkCoord[ 2 ] * gridChunkDim[ 0 ] * gridChunkDim[ 1 ];
+        btAssert(iBatch >= 0 && iBatch < params.maxNumBatchesPerPhase*params.numPhases);
+        params.constraintBatchIds[ iCon ] = iBatch;
+    }
+}
+
+
+struct AssignConstraintsToGridBatchesLoop : public btIParallelForBody
+{
+    const AssignConstraintsToGridBatchesParams* m_params;
+
+    AssignConstraintsToGridBatchesLoop( const AssignConstraintsToGridBatchesParams& params )
+    {
+        m_params = &params;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        assignConstraintsToGridBatches(*m_params, iBegin, iEnd);
+    }
+};
+
+
+//
+// setupSpatialGridBatchesMt -- generate batches using a uniform 3D grid
+//
+/*
+
+Bodies are treated as 3D points at their center of mass. We only consider dynamic bodies at this stage,
+because only dynamic bodies are mutated when a constraint is solved, thus subject to race conditions.
+
+1. Compute a bounding box around all dynamic bodies
+2. Compute the maximum extent of all dynamic constraints. Each dynamic constraint is treated as a line segment, and we need the size of
+   box that will fully enclose any single dynamic constraint
+
+3. Establish the cell size of our grid, the cell size in each dimension must be at least as large as the dynamic constraints max-extent,
+   so that no dynamic constraint can span more than 2 cells of our grid on any axis of the grid. The cell size should be adjusted
+   larger in order to keep the total number of cells from being excessively high
+
+Key idea: Given that each constraint spans 1 or 2 grid cells in each dimension, we can handle all constraints by processing
+          in chunks of 2x2x2 cells with 8 different 1-cell offsets ((0,0,0),(0,0,1),(0,1,0),(0,1,1),(1,0,0)...).
+          For each of the 8 offsets, we create a phase, and for each 2x2x2 chunk with dynamic constraints becomes a batch in that phase.
+
+4. Once the grid is established, we can calculate for each constraint which phase and batch it belongs in.
+
+5. Do a merge small batches on the batches of each phase separately, to try to even out the sizes of batches
+
+Optionally, we can "collapse" one dimension of our 3D grid to turn it into a 2D grid, which reduces the number of phases
+to 4. With fewer phases, there are more constraints per phase and this makes it easier to create batches of a useful size.
+*/
+//
+static void setupSpatialGridBatchesMt(
+    btBatchedConstraints* batchedConstraints,
+    btAlignedObjectArray<char>* scratchMemory,
+    btConstraintArray* constraints,
+    const btAlignedObjectArray<btSolverBody>& bodies,
+    int minBatchSize,
+    int maxBatchSize,
+    bool use2DGrid
+)
+{
+    BT_PROFILE("setupSpatialGridBatchesMt");
+    const int numPhases = 8;
+    int numConstraints = constraints->size();
+    int numConstraintRows = constraints->size();
+
+    const int maxGridChunkCount = 128;
+    int allocNumBatchesPerPhase = maxGridChunkCount;
+    int minNumBatchesPerPhase = 16;
+    int allocNumBatches = allocNumBatchesPerPhase * numPhases;
+
+    btVector3* bodyPositions = NULL;
+    bool* bodyDynamicFlags = NULL;
+    btIntVec3* bodyGridCoords = NULL;
+    btBatchInfo* batches = NULL;
+    int* batchWork = NULL;
+    btBatchedConstraintInfo* conInfos = NULL;
+    int* constraintBatchIds = NULL;
+    int* constraintRowBatchIds = NULL;
+    {
+        PreallocatedMemoryHelper<10> memHelper;
+        memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() );
+        memHelper.addChunk( (void**) &bodyDynamicFlags, sizeof( bool ) * bodies.size() );
+        memHelper.addChunk( (void**) &bodyGridCoords, sizeof( btIntVec3 ) * bodies.size() );
+        memHelper.addChunk( (void**) &batches, sizeof( btBatchInfo )* allocNumBatches );
+        memHelper.addChunk( (void**) &batchWork, sizeof( int )* allocNumBatches );
+        memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints );
+        memHelper.addChunk( (void**) &constraintBatchIds, sizeof( int ) * numConstraints );
+        memHelper.addChunk( (void**) &constraintRowBatchIds, sizeof( int ) * numConstraintRows );
+        size_t scratchSize = memHelper.getSizeToAllocate();
+        // if we need to reallocate
+        if (scratchMemory->capacity() < scratchSize)
+        {
+            // allocate 6.25% extra to avoid repeated reallocs
+            scratchMemory->reserve( scratchSize + scratchSize/16 );
+        }
+        scratchMemory->resizeNoInitialize( scratchSize );
+        char* memPtr = &scratchMemory->at(0);
+        memHelper.setChunkPointers( memPtr );
+    }
+
+    numConstraints = initBatchedConstraintInfo(conInfos, constraints);
+
+    // compute bounding box around all dynamic bodies
+    // (could be done in parallel)
+    btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
+    btVector3 bboxMax = -bboxMin;
+    //int dynamicBodyCount = 0;
+    for (int i = 0; i < bodies.size(); ++i)
+    {
+        const btSolverBody& body = bodies[i];
+        btVector3 bodyPos = body.getWorldTransform().getOrigin();
+        bool isDynamic = ( body.internalGetInvMass().x() > btScalar( 0 ) );
+        bodyPositions[i] = bodyPos;
+        bodyDynamicFlags[i] = isDynamic;
+        if (isDynamic)
+        {
+            //dynamicBodyCount++;
+            bboxMin.setMin(bodyPos);
+            bboxMax.setMax(bodyPos);
+        }
+    }
+
+    // find max extent of all dynamic constraints
+    // (could be done in parallel)
+    btVector3 consExtent = findMaxDynamicConstraintExtent(bodyPositions, bodyDynamicFlags, conInfos, numConstraints, bodies.size());
+
+    btVector3 gridExtent = bboxMax - bboxMin;
+
+    btVector3 gridCellSize = consExtent;
+    int gridDim[3];
+    gridDim[ 0 ] = int( 1.0 + gridExtent.x() / gridCellSize.x() );
+    gridDim[ 1 ] = int( 1.0 + gridExtent.y() / gridCellSize.y() );
+    gridDim[ 2 ] = int( 1.0 + gridExtent.z() / gridCellSize.z() );
+
+    // if we can collapse an axis, it will cut our number of phases in half which could be more efficient
+    int phaseMask = 7;
+    bool collapseAxis = use2DGrid;
+    if ( collapseAxis )
+    {
+        // pick the smallest axis to collapse, leaving us with the greatest number of cells in our grid
+        int iAxisToCollapse = 0;
+        int axisDim = gridDim[iAxisToCollapse];
+        //for each dimension
+        for ( int i = 0; i < 3; ++i )
+        {
+            if (gridDim[i] < axisDim)
+            {
+                iAxisToCollapse = i;
+                axisDim = gridDim[i];
+            }
+        }
+        // collapse it
+        gridCellSize[iAxisToCollapse] = gridExtent[iAxisToCollapse] * 2.0f;
+        phaseMask &= ~(1 << iAxisToCollapse);
+    }
+
+    int numGridChunks = 0;
+    btIntVec3 gridChunkDim;  // each chunk is 2x2x2 group of cells
+    while (true)
+    {
+        gridDim[0] = int( 1.0 + gridExtent.x() / gridCellSize.x() );
+        gridDim[1] = int( 1.0 + gridExtent.y() / gridCellSize.y() );
+        gridDim[2] = int( 1.0 + gridExtent.z() / gridCellSize.z() );
+        gridChunkDim[ 0 ] = btMax( 1, ( gridDim[ 0 ] + 0 ) / 2 );
+        gridChunkDim[ 1 ] = btMax( 1, ( gridDim[ 1 ] + 0 ) / 2 );
+        gridChunkDim[ 2 ] = btMax( 1, ( gridDim[ 2 ] + 0 ) / 2 );
+        numGridChunks = gridChunkDim[ 0 ] * gridChunkDim[ 1 ] * gridChunkDim[ 2 ];
+        float nChunks = float(gridChunkDim[0]) * float(gridChunkDim[1]) * float(gridChunkDim[2]);  // suceptible to integer overflow
+        if ( numGridChunks <= maxGridChunkCount && nChunks <= maxGridChunkCount )
+        {
+            break;
+        }
+        gridCellSize *= 1.25; // should roughly cut numCells in half
+    }
+    btAssert(numGridChunks <= maxGridChunkCount );
+    int maxNumBatchesPerPhase = numGridChunks;
+
+    // for each dynamic body, compute grid coords
+    btVector3 invGridCellSize = btVector3(1,1,1)/gridCellSize;
+    // (can be done in parallel)
+    for (int iBody = 0; iBody < bodies.size(); ++iBody)
+    {
+        btIntVec3& coords = bodyGridCoords[iBody];
+        if (bodyDynamicFlags[iBody])
+        {
+            btVector3 v = ( bodyPositions[ iBody ] - bboxMin )*invGridCellSize;
+            coords.m_ints[0] = int(v.x());
+            coords.m_ints[1] = int(v.y());
+            coords.m_ints[2] = int(v.z());
+            btAssert(coords.m_ints[0] >= 0 && coords.m_ints[0] < gridDim[0]);
+            btAssert(coords.m_ints[1] >= 0 && coords.m_ints[1] < gridDim[1]);
+            btAssert(coords.m_ints[2] >= 0 && coords.m_ints[2] < gridDim[2]);
+        }
+        else
+        {
+            coords.m_ints[0] = -1;
+            coords.m_ints[1] = -1;
+            coords.m_ints[2] = -1;
+        }
+    }
+
+    for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+    {
+        int batchBegin = iPhase * maxNumBatchesPerPhase;
+        int batchEnd = batchBegin + maxNumBatchesPerPhase;
+        for ( int iBatch = batchBegin; iBatch < batchEnd; ++iBatch )
+        {
+            btBatchInfo& batch = batches[ iBatch ];
+            batch = btBatchInfo();
+        }
+    }
+
+    {
+        AssignConstraintsToGridBatchesParams params;
+        params.bodyDynamicFlags = bodyDynamicFlags;
+        params.bodyGridCoords = bodyGridCoords;
+        params.numBodies = bodies.size();
+        params.conInfos = conInfos;
+        params.constraintBatchIds = constraintBatchIds;
+        params.gridChunkDim = gridChunkDim;
+        params.maxNumBatchesPerPhase = maxNumBatchesPerPhase;
+        params.numPhases = numPhases;
+        params.phaseMask = phaseMask;
+        bool inParallel = true;
+        if (inParallel)
+        {
+            AssignConstraintsToGridBatchesLoop loop(params);
+            int grainSize = 250;
+            btParallelFor(0, numConstraints, grainSize, loop);
+        }
+        else
+        {
+            assignConstraintsToGridBatches( params, 0, numConstraints );
+        }
+    }
+    for ( int iCon = 0; iCon < numConstraints; ++iCon )
+    {
+        const btBatchedConstraintInfo& con = conInfos[ iCon ];
+        int iBatch = constraintBatchIds[ iCon ];
+        btBatchInfo& batch = batches[iBatch];
+        batch.numConstraints += con.numConstraintRows;
+    }
+
+    for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+    {
+        // if phase is legit,
+        if (iPhase == (iPhase&phaseMask))
+        {
+            int iBeginBatch = iPhase * maxNumBatchesPerPhase;
+            int iEndBatch = iBeginBatch + maxNumBatchesPerPhase;
+            mergeSmallBatches( batches, iBeginBatch, iEndBatch, minBatchSize, maxBatchSize );
+        }
+    }
+    // all constraints have been assigned a batchId
+    updateConstraintBatchIdsForMergesMt(constraintBatchIds, numConstraints, batches, maxNumBatchesPerPhase*numPhases);
+
+    if (numConstraintRows > numConstraints)
+    {
+        expandConstraintRowsMt(&constraintRowBatchIds[0], &constraintBatchIds[0], &conInfos[0], numConstraints, numConstraintRows);
+    }
+    else
+    {
+        constraintRowBatchIds = constraintBatchIds;
+    }
+
+    writeOutBatches(batchedConstraints, constraintRowBatchIds, numConstraintRows, batches, batchWork, maxNumBatchesPerPhase, numPhases);
+    btAssert(batchedConstraints->validate(constraints, bodies));
+}
+
+
+static void setupSingleBatch(
+    btBatchedConstraints* bc,
+    int numConstraints
+)
+{
+    BT_PROFILE("setupSingleBatch");
+    typedef btBatchedConstraints::Range Range;
+
+    bc->m_constraintIndices.resize( numConstraints );
+    for ( int i = 0; i < numConstraints; ++i )
+    {
+        bc->m_constraintIndices[ i ] = i;
+    }
+
+    bc->m_batches.resizeNoInitialize( 0 );
+    bc->m_phases.resizeNoInitialize( 0 );
+    bc->m_phaseOrder.resizeNoInitialize( 0 );
+    bc->m_phaseGrainSize.resizeNoInitialize( 0 );
+
+    if (numConstraints > 0)
+    {
+        bc->m_batches.push_back( Range( 0, numConstraints ) );
+        bc->m_phases.push_back( Range( 0, 1 ) );
+        bc->m_phaseOrder.push_back(0);
+        bc->m_phaseGrainSize.push_back(1);
+    }
+}
+
+
+void btBatchedConstraints::setup(
+    btConstraintArray* constraints,
+    const btAlignedObjectArray<btSolverBody>& bodies,
+    BatchingMethod batchingMethod,
+    int minBatchSize,
+    int maxBatchSize,
+    btAlignedObjectArray<char>* scratchMemory
+    )
+{
+    if (constraints->size() >= minBatchSize*4)
+    {
+        bool use2DGrid = batchingMethod == BATCHING_METHOD_SPATIAL_GRID_2D;
+        setupSpatialGridBatchesMt( this, scratchMemory, constraints, bodies, minBatchSize, maxBatchSize, use2DGrid );
+        if (s_debugDrawBatches)
+        {
+            debugDrawAllBatches( this, constraints, bodies );
+        }
+    }
+    else
+    {
+        setupSingleBatch( this, constraints->size() );
+    }
+}
+
+

thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.h

@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_BATCHED_CONSTRAINTS_H
+#define BT_BATCHED_CONSTRAINTS_H
+
+#include "LinearMath/btThreads.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
+#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
+
+
+class btIDebugDraw;
+
+struct btBatchedConstraints
+{
+    enum BatchingMethod
+    {
+        BATCHING_METHOD_SPATIAL_GRID_2D,
+        BATCHING_METHOD_SPATIAL_GRID_3D,
+        BATCHING_METHOD_COUNT
+    };
+    struct Range
+    {
+        int begin;
+        int end;
+
+        Range() : begin( 0 ), end( 0 ) {}
+        Range( int _beg, int _end ) : begin( _beg ), end( _end ) {}
+    };
+
+    btAlignedObjectArray<int> m_constraintIndices;
+    btAlignedObjectArray<Range> m_batches;  // each batch is a range of indices in the m_constraintIndices array
+    btAlignedObjectArray<Range> m_phases;  // each phase is range of indices in the m_batches array
+    btAlignedObjectArray<char> m_phaseGrainSize;  // max grain size for each phase
+    btAlignedObjectArray<int> m_phaseOrder;  // phases can be done in any order, so we can randomize the order here
+    btIDebugDraw* m_debugDrawer;
+
+    static bool s_debugDrawBatches;
+
+    btBatchedConstraints() {m_debugDrawer=NULL;}
+    void setup( btConstraintArray* constraints,
+        const btAlignedObjectArray<btSolverBody>& bodies,
+        BatchingMethod batchingMethod,
+        int minBatchSize,
+        int maxBatchSize,
+        btAlignedObjectArray<char>* scratchMemory
+    );
+    bool validate( btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies ) const;
+};
+
+
+#endif // BT_BATCHED_CONSTRAINTS_H
+

thirdparty/bullet/BulletDynamics/ConstraintSolver/btConstraintSolver.h

@@ -34,7 +34,8 @@ enum btConstraintSolverType
 {
 	BT_SEQUENTIAL_IMPULSE_SOLVER=1,
 	BT_MLCP_SOLVER=2,
-	BT_NNCG_SOLVER=4
+	BT_NNCG_SOLVER=4,
+    BT_MULTIBODY_SOLVER=8,
 };
 
 class btConstraintSolver

thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -29,7 +29,8 @@ enum	btSolverMode
 	SOLVER_CACHE_FRIENDLY = 128,
 	SOLVER_SIMD = 256,
 	SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
-	SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
+	SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024,
+	SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048
 };
 
 struct btContactSolverInfoData

thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -855,8 +855,8 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
 													tag_vel, 
 													info->fps * limot->m_stopERP);
 				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
-                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
-                info->m_upperLimit[srow] = limot->m_maxMotorForce;
+                info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+                info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
             }
         }
         if(limit)

thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

@@ -77,7 +77,7 @@ public:
     {
     	m_accumulatedImpulse = 0.f;
         m_targetVelocity = 0;
-        m_maxMotorForce = 0.1f;
+        m_maxMotorForce = 6.0f;
         m_maxLimitForce = 300.0f;
         m_loLimit = 1.0f;
         m_hiLimit = -1.0f;

thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp

@@ -719,8 +719,8 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 			tag_vel,
 			info->fps * limot->m_motorERP);
 		info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
-		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
-		info->m_upperLimit[srow] = limot->m_maxMotorForce;
+		info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 		info->cfm[srow] = limot->m_motorCFM;
 		srow += info->rowskip;
 		++count;
@@ -769,8 +769,8 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 			mot_fact = 0;
 		}
 		info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
-		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
-		info->m_upperLimit[srow] = limot->m_maxMotorForce;
+		info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 		info->cfm[srow] = limot->m_motorCFM;
 		srow += info->rowskip;
 		++count;
@@ -797,6 +797,12 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		btScalar cfm = BT_ZERO;
 		btScalar mA = BT_ONE / m_rbA.getInvMass();
 		btScalar mB = BT_ONE / m_rbB.getInvMass();
+		if (rotational) {
+			btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
+			btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
+			if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
+			if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
+		}
 		btScalar m = mA > mB ? mB : mA;
 		btScalar angularfreq = sqrt(ks / m);
 
@@ -815,7 +821,18 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
 		btScalar f = (fs+fd);
 
-		info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ;
+		// after the spring force affecting the body(es) the new velocity will be
+		// vel + f / m * (rotational ? -1 : 1)
+		// so in theory this should be set here for m_constraintError
+		// (with m_constraintError we set a desired velocity for the affected body(es))
+		// however in practice any value is fine as long as it is greater then the "proper" velocity,
+		// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
+		// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
+		// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
+		// will we not request a velocity with the wrong direction ?
+		// and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
+		// so the sign of the force that is really matters
+		info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
 
 		btScalar minf = f < fd ? f : fd;
 		btScalar maxf = f < fd ? fd : f;

thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

@@ -107,7 +107,7 @@ public:
 		m_motorCFM               = 0.f;
 		m_enableMotor            = false;
 		m_targetVelocity         = 0;
-		m_maxMotorForce          = 0.1f;
+		m_maxMotorForce          = 6.0f;
 		m_servoMotor             = false;
 		m_servoTarget            = 0;
 		m_enableSpring           = false;

thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp

@@ -131,7 +131,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
 			btScalar force = delta * m_springStiffness[i];
 			btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
 			m_linearLimits.m_targetVelocity[i] =  velFactor * force;
-			m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
+			m_linearLimits.m_maxMotorForce[i] =  btFabs(force);
 		}
 	}
 	for(i = 0; i < 3; i++)
@@ -146,7 +146,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
 			btScalar force = -delta * m_springStiffness[i+3];
 			btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
 			m_angularLimits[i].m_targetVelocity = velFactor * force;
-			m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
+			m_angularLimits[i].m_maxMotorForce = btFabs(force);
 		}
 	}
 }

thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -27,7 +27,7 @@ class btCollisionObject;
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
 
-typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
+typedef btScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
 
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
@@ -64,44 +64,48 @@ protected:
 
 	void setupFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
 									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
-									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
+									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
 									const btContactSolverInfo& infoGlobal,
 									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 
 	void setupTorsionalFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
 									btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
-									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
+									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
 									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 
 	btSolverConstraint&	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 	btSolverConstraint&	addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
 
-	
-	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, 
+
+	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
 								const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
 
 	static void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
 
-	void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, 
+	void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
 										 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
 
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
 	unsigned long	m_btSeed2;
 
-	
+
 	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
 
 	virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
 
 	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
 
+    virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
+    void convertJoint(btSolverConstraint* currentConstraintRow, btTypedConstraint* constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
+
+    virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
-	btSimdScalar	resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
+	btScalar	resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
     {
         return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
     }
 
-	btSimdScalar	resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
+	btScalar	resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
     {
         return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
     }
@@ -110,18 +114,20 @@ protected:
 	int		getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
 	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
 
-	btSimdScalar	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	btSimdScalar	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	btSimdScalar	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	btSimdScalar	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	btSimdScalar	resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
+	btScalar	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btScalar	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btScalar	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btScalar	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btScalar	resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
     {
         return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
     }
-		
+
 protected:
-	
-	
+
+    void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+    void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+    void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
 	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
 	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
@@ -133,15 +139,15 @@ protected:
 public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	btSequentialImpulseConstraintSolver();
 	virtual ~btSequentialImpulseConstraintSolver();
 
 	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-		
+
 	///clear internal cached data and reset random seed
 	virtual	void	reset();
-	
+
 	unsigned long btRand2();
 
 	int btRandInt2 (int n);
@@ -155,7 +161,7 @@ public:
 		return m_btSeed2;
 	}
 
-	
+
 	virtual btConstraintSolverType	getSolverType() const
 	{
 		return BT_SEQUENTIAL_IMPULSE_SOLVER;

thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp

@@ -0,0 +1,1621 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btSequentialImpulseConstraintSolverMt.h"
+
+#include "LinearMath/btQuickprof.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+
+
+bool btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops = false;  // some task schedulers don't like nested loops
+int btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching = 250;
+int btSequentialImpulseConstraintSolverMt::s_minBatchSize = 50;
+int btSequentialImpulseConstraintSolverMt::s_maxBatchSize = 100;
+btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
+btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_jointBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
+
+
+btSequentialImpulseConstraintSolverMt::btSequentialImpulseConstraintSolverMt()
+{
+    m_numFrictionDirections = 1;
+    m_useBatching = false;
+    m_useObsoleteJointConstraints = false;
+}
+
+
+btSequentialImpulseConstraintSolverMt::~btSequentialImpulseConstraintSolverMt()
+{
+}
+
+
+void btSequentialImpulseConstraintSolverMt::setupBatchedContactConstraints()
+{
+    BT_PROFILE("setupBatchedContactConstraints");
+    m_batchedContactConstraints.setup( &m_tmpSolverContactConstraintPool,
+        m_tmpSolverBodyPool,
+        s_contactBatchingMethod,
+        s_minBatchSize,
+        s_maxBatchSize,
+        &m_scratchMemory
+    );
+}
+
+
+void btSequentialImpulseConstraintSolverMt::setupBatchedJointConstraints()
+{
+    BT_PROFILE("setupBatchedJointConstraints");
+    m_batchedJointConstraints.setup( &m_tmpSolverNonContactConstraintPool,
+        m_tmpSolverBodyPool,
+        s_jointBatchingMethod,
+        s_minBatchSize,
+        s_maxBatchSize,
+        &m_scratchMemory
+    );
+}
+
+
+void btSequentialImpulseConstraintSolverMt::internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal)
+{
+    btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint];
+
+    btVector3 rel_pos1;
+    btVector3 rel_pos2;
+    btScalar relaxation;
+
+    int solverBodyIdA = contactConstraint.m_solverBodyIdA;
+    int solverBodyIdB = contactConstraint.m_solverBodyIdB;
+
+    btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ];
+    btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ];
+
+    btRigidBody* colObj0 = solverBodyA->m_originalBody;
+    btRigidBody* colObj1 = solverBodyB->m_originalBody;
+
+    btManifoldPoint& cp = *static_cast<btManifoldPoint*>( contactConstraint.m_originalContactPoint );
+
+    const btVector3& pos1 = cp.getPositionWorldOnA();
+    const btVector3& pos2 = cp.getPositionWorldOnB();
+
+    rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin();
+    rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin();
+
+    btVector3 vel1;
+    btVector3 vel2;
+
+    solverBodyA->getVelocityInLocalPointNoDelta( rel_pos1, vel1 );
+    solverBodyB->getVelocityInLocalPointNoDelta( rel_pos2, vel2 );
+
+    btVector3 vel = vel1 - vel2;
+    btScalar rel_vel = cp.m_normalWorldOnB.dot( vel );
+
+    setupContactConstraint( contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2 );
+
+    // setup rolling friction constraints
+    int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint];
+    if (rollingFrictionIndex >= 0)
+    {
+        btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ rollingFrictionIndex ];
+        btAssert( spinningFrictionConstraint.m_frictionIndex == iContactConstraint );
+        setupTorsionalFrictionConstraint( spinningFrictionConstraint,
+            cp.m_normalWorldOnB,
+            solverBodyIdA,
+            solverBodyIdB,
+            cp,
+            cp.m_combinedSpinningFriction,
+            rel_pos1,
+            rel_pos2,
+            colObj0,
+            colObj1,
+            relaxation,
+            0.0f,
+            0.0f
+        );
+        btVector3 axis[2];
+        btPlaneSpace1( cp.m_normalWorldOnB, axis[0], axis[1] );
+        axis[0].normalize();
+        axis[1].normalize();
+
+        applyAnisotropicFriction( colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION );
+        applyAnisotropicFriction( colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION );
+        applyAnisotropicFriction( colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION );
+        applyAnisotropicFriction( colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION );
+        // put the largest axis first
+        if (axis[1].length2() > axis[0].length2())
+        {
+            btSwap(axis[0], axis[1]);
+        }
+        const btScalar kRollingFrictionThreshold = 0.001f;
+        for (int i = 0; i < 2; ++i)
+        {
+            int iRollingFric = rollingFrictionIndex + 1 + i;
+            btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ];
+            btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint);
+            btVector3 dir = axis[i];
+            if ( dir.length() > kRollingFrictionThreshold )
+            {
+                setupTorsionalFrictionConstraint( rollingFrictionConstraint,
+                    dir,
+                    solverBodyIdA,
+                    solverBodyIdB,
+                    cp,
+                    cp.m_combinedRollingFriction,
+                    rel_pos1,
+                    rel_pos2,
+                    colObj0,
+                    colObj1,
+                    relaxation,
+                    0.0f,
+                    0.0f
+                );
+            }
+            else
+            {
+                rollingFrictionConstraint.m_frictionIndex = -1;  // disable constraint
+            }
+        }
+    }
+
+    // setup friction constraints
+    //	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
+    {
+        ///Bullet has several options to set the friction directions
+        ///By default, each contact has only a single friction direction that is recomputed automatically very frame
+        ///based on the relative linear velocity.
+        ///If the relative velocity it zero, it will automatically compute a friction direction.
+
+        ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
+        ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
+        ///
+        ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
+        ///
+        ///The user can manually override the friction directions for certain contacts using a contact callback,
+        ///and set the cp.m_lateralFrictionInitialized to true
+        ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
+        ///this will give a conveyor belt effect
+        ///
+	    btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex];
+        btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint);
+
+        btSolverConstraint* frictionConstraint2 = NULL;
+        if ( infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS )
+        {
+            frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1];
+            btAssert( frictionConstraint2->m_frictionIndex == iContactConstraint );
+        }
+
+        if ( !( infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING ) || !( cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ) )
+        {
+            cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+            btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+            if ( !( infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) && lat_rel_vel > SIMD_EPSILON )
+            {
+                cp.m_lateralFrictionDir1 *= 1.f / btSqrt( lat_rel_vel );
+                applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION );
+                applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION );
+                setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal );
+
+                if ( frictionConstraint2 )
+                {
+                    cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross( cp.m_normalWorldOnB );
+                    cp.m_lateralFrictionDir2.normalize();//??
+                    applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION );
+                    applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION );
+                    setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal );
+                }
+            }
+            else
+            {
+                btPlaneSpace1( cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2 );
+
+                applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION );
+                applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION );
+                setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal );
+
+                if ( frictionConstraint2 )
+                {
+                    applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION );
+                    applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION );
+                    setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal );
+                }
+
+                if ( ( infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) && ( infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) )
+                {
+                    cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
+                }
+            }
+        }
+        else
+        {
+            setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM );
+            if ( frictionConstraint2 )
+            {
+                setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM );
+            }
+        }
+    }
+
+    setFrictionConstraintImpulse( contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal );
+}
+
+
+struct SetupContactConstraintsLoop : public btIParallelForBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btBatchedConstraints* m_bc;
+    const btContactSolverInfo* m_infoGlobal;
+
+    SetupContactConstraintsLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal )
+    {
+        m_solver = solver;
+        m_bc = bc;
+        m_infoGlobal = &infoGlobal;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        BT_PROFILE( "SetupContactConstraintsLoop" );
+        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
+        {
+            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
+            for (int i = batch.begin; i < batch.end; ++i)
+            {
+                int iContact = m_bc->m_constraintIndices[i];
+                m_solver->internalSetupContactConstraints( iContact, *m_infoGlobal );
+            }
+        }
+    }
+};
+
+
+void btSequentialImpulseConstraintSolverMt::setupAllContactConstraints(const btContactSolverInfo& infoGlobal)
+{
+    BT_PROFILE( "setupAllContactConstraints" );
+    if ( m_useBatching )
+    {
+        const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+        SetupContactConstraintsLoop loop( this, &batchedCons, infoGlobal );
+        for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
+        {
+            int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
+            const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
+            int grainSize = 1;
+            btParallelFor( phase.begin, phase.end, grainSize, loop );
+        }
+    }
+    else
+    {
+        for ( int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i )
+        {
+            internalSetupContactConstraints( i, infoGlobal );
+        }
+    }
+}
+
+
+int	btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body,btScalar timeStep)
+{
+    //
+    // getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers)
+    //
+    // getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism)
+    //
+    int solverBodyId = -1;
+    bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL;
+    if ( isRigidBodyType && !body.isStaticOrKinematicObject() )
+    {
+        // dynamic body
+        // Dynamic bodies can only be in one island, so it's safe to write to the companionId
+        solverBodyId = body.getCompanionId();
+        if ( solverBodyId < 0 )
+        {
+            m_bodySolverArrayMutex.lock();
+            // now that we have the lock, check again
+            solverBodyId = body.getCompanionId();
+            if ( solverBodyId < 0 )
+            {
+                solverBodyId = m_tmpSolverBodyPool.size();
+                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+                initSolverBody( &solverBody, &body, timeStep );
+                body.setCompanionId( solverBodyId );
+            }
+            m_bodySolverArrayMutex.unlock();
+        }
+    }
+    else if (isRigidBodyType && body.isKinematicObject())
+    {
+        //
+        // NOTE: must test for kinematic before static because some kinematic objects also
+        //   identify as "static"
+        //
+        // Kinematic bodies can be in multiple islands at once, so it is a
+        // race condition to write to them, so we use an alternate method
+        // to record the solverBodyId
+        int uniqueId = body.getWorldArrayIndex();
+        const int INVALID_SOLVER_BODY_ID = -1;
+        if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId )
+        {
+            m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
+            // now that we have the lock, check again
+            if ( m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId )
+            {
+                m_kinematicBodyUniqueIdToSolverBodyTable.resize( uniqueId + 1, INVALID_SOLVER_BODY_ID );
+            }
+            m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
+        }
+        solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ];
+        // if no table entry yet,
+        if ( INVALID_SOLVER_BODY_ID == solverBodyId )
+        {
+            // need to acquire both locks
+            m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
+            m_bodySolverArrayMutex.lock();
+            // now that we have the lock, check again
+            solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ];
+            if ( INVALID_SOLVER_BODY_ID == solverBodyId )
+            {
+                // create a table entry for this body
+                solverBodyId = m_tmpSolverBodyPool.size();
+                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+                initSolverBody( &solverBody, &body, timeStep );
+                m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId;
+            }
+            m_bodySolverArrayMutex.unlock();
+            m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
+        }
+    }
+    else
+    {
+        // all fixed bodies (inf mass) get mapped to a single solver id
+        if ( m_fixedBodyId < 0 )
+        {
+            m_bodySolverArrayMutex.lock();
+            // now that we have the lock, check again
+            if ( m_fixedBodyId < 0 )
+            {
+                m_fixedBodyId = m_tmpSolverBodyPool.size();
+                btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+                initSolverBody( &fixedBody, 0, timeStep );
+            }
+            m_bodySolverArrayMutex.unlock();
+        }
+        solverBodyId = m_fixedBodyId;
+    }
+    btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() );
+	return solverBodyId;
+}
+
+
+void btSequentialImpulseConstraintSolverMt::internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+    BT_PROFILE("internalCollectContactManifoldCachedInfo");
+    for (int i = 0; i < numManifolds; ++i)
+    {
+        btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i];
+        btPersistentManifold* manifold = manifoldPtr[i];
+        btCollisionObject* colObj0 = (btCollisionObject*) manifold->getBody0();
+        btCollisionObject* colObj1 = (btCollisionObject*) manifold->getBody1();
+
+        int solverBodyIdA = getOrInitSolverBodyThreadsafe( *colObj0, infoGlobal.m_timeStep );
+        int solverBodyIdB = getOrInitSolverBodyThreadsafe( *colObj1, infoGlobal.m_timeStep );
+
+        cachedInfo->solverBodyIds[ 0 ] = solverBodyIdA;
+        cachedInfo->solverBodyIds[ 1 ] = solverBodyIdB;
+        cachedInfo->numTouchingContacts = 0;
+
+        btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ];
+        btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ];
+
+        // A contact manifold between 2 static object should not exist!
+        // check the collision flags of your objects if this assert fires.
+        // Incorrectly set collision object flags can degrade performance in various ways.
+        btAssert( !m_tmpSolverBodyPool[ solverBodyIdA ].m_invMass.isZero() || !m_tmpSolverBodyPool[ solverBodyIdB ].m_invMass.isZero() );
+
+        int iContact = 0;
+        for ( int j = 0; j < manifold->getNumContacts(); j++ )
+        {
+            btManifoldPoint& cp = manifold->getContactPoint( j );
+
+            if ( cp.getDistance() <= manifold->getContactProcessingThreshold() )
+            {
+                cachedInfo->contactPoints[ iContact ] = &cp;
+                cachedInfo->contactHasRollingFriction[ iContact ] = ( cp.m_combinedRollingFriction > 0.f );
+                iContact++;
+            }
+        }
+        cachedInfo->numTouchingContacts = iContact;
+    }
+}
+
+
+struct CollectContactManifoldCachedInfoLoop : public btIParallelForBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
+    btPersistentManifold** m_manifoldPtr;
+    const btContactSolverInfo* m_infoGlobal;
+
+    CollectContactManifoldCachedInfoLoop( btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal )
+    {
+        m_solver = solver;
+        m_cachedInfoArray = cachedInfoArray;
+        m_manifoldPtr = manifoldPtr;
+        m_infoGlobal = &infoGlobal;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        m_solver->internalCollectContactManifoldCachedInfo( m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal );
+    }
+};
+
+
+void btSequentialImpulseConstraintSolverMt::internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds)
+{
+    BT_PROFILE("internalAllocContactConstraints");
+    // possibly parallel part
+    for ( int iManifold = 0; iManifold < numManifolds; ++iManifold )
+    {
+        const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[ iManifold ];
+        int contactIndex = cachedInfo.contactIndex;
+        int frictionIndex = contactIndex * m_numFrictionDirections;
+        int rollingFrictionIndex = cachedInfo.rollingFrictionIndex;
+        for ( int i = 0; i < cachedInfo.numTouchingContacts; i++ )
+        {
+            btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex];
+            contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[ 0 ];
+            contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[ 1 ];
+            contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[ i ];
+
+            // allocate the friction constraints
+            contactConstraint.m_frictionIndex = frictionIndex;
+            for ( int iDir = 0; iDir < m_numFrictionDirections; ++iDir )
+            {
+                btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex];
+                frictionConstraint.m_frictionIndex = contactIndex;
+                frictionIndex++;
+            }
+
+            // allocate rolling friction constraints
+            if ( cachedInfo.contactHasRollingFriction[ i ] )
+            {
+                m_rollingFrictionIndexTable[ contactIndex ] = rollingFrictionIndex;
+                // allocate 3 (although we may use only 2 sometimes)
+                for ( int i = 0; i < 3; i++ )
+                {
+                    m_tmpSolverContactRollingFrictionConstraintPool[ rollingFrictionIndex ].m_frictionIndex = contactIndex;
+                    rollingFrictionIndex++;
+                }
+            }
+            else
+            {
+                // indicate there is no rolling friction for this contact point
+                m_rollingFrictionIndexTable[ contactIndex ] = -1;
+            }
+            contactIndex++;
+        }
+    }
+}
+
+
+struct AllocContactConstraintsLoop : public btIParallelForBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
+
+    AllocContactConstraintsLoop( btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray )
+    {
+        m_solver = solver;
+        m_cachedInfoArray = cachedInfoArray;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        m_solver->internalAllocContactConstraints( m_cachedInfoArray + iBegin, iEnd - iBegin );
+    }
+};
+
+
+void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+    BT_PROFILE( "allocAllContactConstraints" );
+    btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray; // = m_manifoldCachedInfoArray;
+    cachedInfoArray.resizeNoInitialize( numManifolds );
+    if (/* DISABLES CODE */ (false))
+    {
+        // sequential
+        internalCollectContactManifoldCachedInfo(&cachedInfoArray[ 0 ], manifoldPtr, numManifolds, infoGlobal);
+    }
+    else
+    {
+        // may alter ordering of bodies which affects determinism
+        CollectContactManifoldCachedInfoLoop loop( this, &cachedInfoArray[ 0 ], manifoldPtr, infoGlobal );
+        int grainSize = 200;
+        btParallelFor( 0, numManifolds, grainSize, loop );
+    }
+
+    {
+        // serial part
+        int numContacts = 0;
+        int numRollingFrictionConstraints = 0;
+        for ( int iManifold = 0; iManifold < numManifolds; ++iManifold )
+        {
+            btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[ iManifold ];
+            cachedInfo.contactIndex = numContacts;
+            cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints;
+            numContacts += cachedInfo.numTouchingContacts;
+            for (int i = 0; i < cachedInfo.numTouchingContacts; ++i)
+            {
+                if (cachedInfo.contactHasRollingFriction[i])
+                {
+                    numRollingFrictionConstraints += 3;
+                }
+            }
+        }
+        {
+            BT_PROFILE( "allocPools" );
+            if ( m_tmpSolverContactConstraintPool.capacity() < numContacts )
+            {
+                // if we need to reallocate, reserve some extra so we don't have to reallocate again next frame
+                int extraReserve = numContacts / 16;
+                m_tmpSolverContactConstraintPool.reserve( numContacts + extraReserve );
+                m_rollingFrictionIndexTable.reserve( numContacts + extraReserve );
+                m_tmpSolverContactFrictionConstraintPool.reserve( ( numContacts + extraReserve )*m_numFrictionDirections );
+                m_tmpSolverContactRollingFrictionConstraintPool.reserve( numRollingFrictionConstraints + extraReserve );
+            }
+            m_tmpSolverContactConstraintPool.resizeNoInitialize( numContacts );
+            m_rollingFrictionIndexTable.resizeNoInitialize( numContacts );
+            m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize( numContacts*m_numFrictionDirections );
+            m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize( numRollingFrictionConstraints );
+        }
+    }
+    {
+        AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]);
+        int grainSize = 200;
+        btParallelFor( 0, numManifolds, grainSize, loop );
+    }
+}
+
+
+void btSequentialImpulseConstraintSolverMt::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+    if (!m_useBatching)
+    {
+        btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal);
+        return;
+    }
+    BT_PROFILE( "convertContacts" );
+    if (numManifolds > 0)
+    {
+        if ( m_fixedBodyId < 0 )
+        {
+            m_fixedBodyId = m_tmpSolverBodyPool.size();
+            btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+            initSolverBody( &fixedBody, 0, infoGlobal.m_timeStep );
+        }
+        allocAllContactConstraints( manifoldPtr, numManifolds, infoGlobal );
+        if ( m_useBatching )
+        {
+            setupBatchedContactConstraints();
+        }
+        setupAllContactConstraints( infoGlobal );
+    }
+}
+
+
+void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints( btTypedConstraint** constraints, int iBegin, int iEnd )
+{
+    BT_PROFILE("internalInitMultipleJoints");
+    for ( int i = iBegin; i < iEnd; i++ )
+	{
+		btTypedConstraint* constraint = constraints[i];
+		btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+		if (constraint->isEnabled())
+        {
+            constraint->buildJacobian();
+            constraint->internalSetAppliedImpulse( 0.0f );
+            btJointFeedback* fb = constraint->getJointFeedback();
+            if ( fb )
+            {
+                fb->m_appliedForceBodyA.setZero();
+                fb->m_appliedTorqueBodyA.setZero();
+                fb->m_appliedForceBodyB.setZero();
+                fb->m_appliedTorqueBodyB.setZero();
+            }
+            constraint->getInfo1( &info1 );
+        }
+        else
+		{
+			info1.m_numConstraintRows = 0;
+			info1.nub = 0;
+		}
+	}
+}
+
+
+struct InitJointsLoop : public btIParallelForBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    btTypedConstraint** m_constraints;
+
+    InitJointsLoop( btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints )
+    {
+        m_solver = solver;
+        m_constraints = constraints;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        m_solver->internalInitMultipleJoints( m_constraints, iBegin, iEnd );
+    }
+};
+
+
+void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints( const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal )
+{
+    BT_PROFILE("internalConvertMultipleJoints");
+    for ( int i = iBegin; i < iEnd; ++i )
+    {
+        const JointParams& jointParams = jointParamsArray[ i ];
+        int currentRow = jointParams.m_solverConstraint;
+        if ( currentRow != -1 )
+        {
+            const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[ i ];
+            btAssert( currentRow < m_tmpSolverNonContactConstraintPool.size() );
+            btAssert( info1.m_numConstraintRows > 0 );
+
+            btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[ currentRow ];
+            btTypedConstraint* constraint = constraints[ i ];
+
+            convertJoint( currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal );
+        }
+    }
+}
+
+
+struct ConvertJointsLoop : public btIParallelForBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& m_jointParamsArray;
+    btTypedConstraint** m_srcConstraints;
+    const btContactSolverInfo& m_infoGlobal;
+
+    ConvertJointsLoop( btSequentialImpulseConstraintSolverMt* solver,
+        const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& jointParamsArray,
+        btTypedConstraint** srcConstraints,
+        const btContactSolverInfo& infoGlobal
+    ) :
+        m_jointParamsArray(jointParamsArray),
+        m_infoGlobal(infoGlobal)
+    {
+        m_solver = solver;
+        m_srcConstraints = srcConstraints;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        m_solver->internalConvertMultipleJoints( m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal );
+    }
+};
+
+
+void btSequentialImpulseConstraintSolverMt::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
+{
+    if ( !m_useBatching )
+    {
+        btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal);
+        return;
+    }
+    BT_PROFILE("convertJoints");
+    bool parallelJointSetup = true;
+	m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
+    if (parallelJointSetup)
+    {
+        InitJointsLoop loop(this, constraints);
+        int grainSize = 40;
+        btParallelFor(0, numConstraints, grainSize, loop);
+    }
+    else
+    {
+        internalInitMultipleJoints( constraints, 0, numConstraints );
+    }
+
+	int totalNumRows = 0;
+    btAlignedObjectArray<JointParams> jointParamsArray;
+    jointParamsArray.resizeNoInitialize(numConstraints);
+
+	//calculate the total number of contraint rows
+	for (int i=0;i<numConstraints;i++)
+	{
+        btTypedConstraint* constraint = constraints[ i ];
+
+        JointParams& params = jointParamsArray[ i ];
+		const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+
+		if (info1.m_numConstraintRows)
+		{
+            params.m_solverConstraint = totalNumRows;
+            params.m_solverBodyA = getOrInitSolverBody( constraint->getRigidBodyA(), infoGlobal.m_timeStep );
+            params.m_solverBodyB = getOrInitSolverBody( constraint->getRigidBodyB(), infoGlobal.m_timeStep );
+		}
+        else
+		{
+            params.m_solverConstraint = -1;
+		}
+		totalNumRows += info1.m_numConstraintRows;
+	}
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
+
+	///setup the btSolverConstraints
+    if ( parallelJointSetup )
+    {
+        ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal);
+        int grainSize = 20;
+        btParallelFor(0, numConstraints, grainSize, loop);
+    }
+    else
+    {
+        internalConvertMultipleJoints( jointParamsArray, constraints, 0, numConstraints, infoGlobal );
+    }
+    setupBatchedJointConstraints();
+}
+
+
+void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+    BT_PROFILE("internalConvertBodies");
+    for (int i=iBegin; i < iEnd; i++)
+	{
+        btCollisionObject* obj = bodies[i];
+		obj->setCompanionId(i);
+		btSolverBody& solverBody = m_tmpSolverBodyPool[i];
+        initSolverBody(&solverBody, obj, infoGlobal.m_timeStep);
+
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getInvMass())
+		{
+			btVector3 gyroForce (0,0,0);
+			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
+			{
+				gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
+				solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
+			}
+			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
+			{
+				gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
+				solverBody.m_externalTorqueImpulse += gyroForce;
+			}
+			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
+			{
+				gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
+				solverBody.m_externalTorqueImpulse += gyroForce;
+			}
+		}
+	}
+}
+
+
+struct ConvertBodiesLoop : public btIParallelForBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    btCollisionObject** m_bodies;
+    int m_numBodies;
+    const btContactSolverInfo& m_infoGlobal;
+
+    ConvertBodiesLoop( btSequentialImpulseConstraintSolverMt* solver,
+        btCollisionObject** bodies,
+        int numBodies,
+        const btContactSolverInfo& infoGlobal
+    ) :
+        m_infoGlobal(infoGlobal)
+    {
+        m_solver = solver;
+        m_bodies = bodies;
+        m_numBodies = numBodies;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        m_solver->internalConvertBodies( m_bodies, iBegin, iEnd, m_infoGlobal );
+    }
+};
+
+
+void btSequentialImpulseConstraintSolverMt::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+    BT_PROFILE("convertBodies");
+    m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 );
+
+	m_tmpSolverBodyPool.resizeNoInitialize(numBodies+1);
+
+    m_fixedBodyId = numBodies;
+    {
+        btSolverBody& fixedBody = m_tmpSolverBodyPool[ m_fixedBodyId ];
+        initSolverBody( &fixedBody, NULL, infoGlobal.m_timeStep );
+    }
+
+    bool parallelBodySetup = true;
+    if (parallelBodySetup)
+    {
+        ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal);
+        int grainSize = 40;
+        btParallelFor(0, numBodies, grainSize, loop);
+    }
+    else
+    {
+        internalConvertBodies( bodies, 0, numBodies, infoGlobal );
+    }
+}
+
+
+btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySetup(
+     btCollisionObject** bodies,
+     int numBodies,
+     btPersistentManifold** manifoldPtr,
+     int numManifolds,
+     btTypedConstraint** constraints,
+     int numConstraints,
+     const btContactSolverInfo& infoGlobal,
+     btIDebugDraw* debugDrawer
+     )
+{
+    m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
+    m_useBatching = false;
+    if ( numManifolds >= s_minimumContactManifoldsForBatching &&
+        (s_allowNestedParallelForLoops || !btThreadsAreRunning())
+        )
+    {
+        m_useBatching = true;
+        m_batchedContactConstraints.m_debugDrawer = debugDrawer;
+        m_batchedJointConstraints.m_debugDrawer = debugDrawer;
+    }
+    btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,
+                                                                       numBodies,
+                                                                       manifoldPtr,
+                                                                       numManifolds,
+                                                                       constraints,
+                                                                       numConstraints,
+                                                                       infoGlobal,
+                                                                       debugDrawer
+                                                                       );
+    return 0.0f;
+}
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd )
+{
+    btScalar leastSquaresResidual = 0.f;
+    for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons )
+    {
+        int iCons = consIndices[ iiCons ];
+        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iCons ];
+        btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ];
+        btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ];
+        btScalar residual = resolveSplitPenetrationImpulse( bodyA, bodyB, solveManifold );
+        leastSquaresResidual += residual*residual;
+    }
+    return leastSquaresResidual;
+}
+
+
+struct ContactSplitPenetrationImpulseSolverLoop : public btIParallelSumBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btBatchedConstraints* m_bc;
+
+    ContactSplitPenetrationImpulseSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc )
+    {
+        m_solver = solver;
+        m_bc = bc;
+    }
+    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        BT_PROFILE( "ContactSplitPenetrationImpulseSolverLoop" );
+        btScalar sum = 0;
+        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
+        {
+            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
+            sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints( m_bc->m_constraintIndices, batch.begin, batch.end );
+        }
+        return sum;
+    }
+};
+
+
+void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
+	if (infoGlobal.m_splitImpulse)
+	{
+        for ( int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ )
+        {
+            btScalar leastSquaresResidual = 0.f;
+            if (m_useBatching)
+            {
+                const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+                ContactSplitPenetrationImpulseSolverLoop loop( this, &batchedCons );
+                btScalar leastSquaresResidual = 0.f;
+                for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
+                {
+                    int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
+                    const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
+                    int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+                    leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
+                }
+            }
+            else
+            {
+                // non-batched
+                leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size());
+            }
+            if ( leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= ( infoGlobal.m_numIterations - 1 ) )
+            {
+#ifdef VERBOSE_RESIDUAL_PRINTF
+                printf( "residual = %f at iteration #%d\n", leastSquaresResidual, iteration );
+#endif
+                break;
+            }
+        }
+	}
+}
+
+
+btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+    if ( !m_useBatching )
+    {
+        return btSequentialImpulseConstraintSolver::solveSingleIteration( iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer );
+    }
+    BT_PROFILE( "solveSingleIterationMt" );
+    btScalar leastSquaresResidual = 0.f;
+
+	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+	{
+		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
+		{
+            randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations);
+		}
+	}
+
+	{
+		///solve all joint constraints
+        leastSquaresResidual += resolveAllJointConstraints(iteration);
+
+		if (iteration< infoGlobal.m_numIterations)
+		{
+            // this loop is only used for cone-twist constraints,
+            // it would be nice to skip this loop if none of the constraints need it
+            if ( m_useObsoleteJointConstraints )
+            {
+                for ( int j = 0; j<numConstraints; j++ )
+                {
+                    if ( constraints[ j ]->isEnabled() )
+                    {
+                        int bodyAid = getOrInitSolverBody( constraints[ j ]->getRigidBodyA(), infoGlobal.m_timeStep );
+                        int bodyBid = getOrInitSolverBody( constraints[ j ]->getRigidBodyB(), infoGlobal.m_timeStep );
+                        btSolverBody& bodyA = m_tmpSolverBodyPool[ bodyAid ];
+                        btSolverBody& bodyB = m_tmpSolverBodyPool[ bodyBid ];
+                        constraints[ j ]->solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep );
+                    }
+                }
+            }
+
+			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+			{
+                // solve all contact, contact-friction, and rolling friction constraints interleaved
+                leastSquaresResidual += resolveAllContactConstraintsInterleaved();
+			}
+			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			{
+                // don't interleave them
+				// solve all contact constraints
+                leastSquaresResidual += resolveAllContactConstraints();
+
+				// solve all contact friction constraints
+                leastSquaresResidual += resolveAllContactFrictionConstraints();
+
+                // solve all rolling friction constraints
+                leastSquaresResidual += resolveAllRollingFrictionConstraints();
+			}
+		}
+	}
+    return leastSquaresResidual;
+}
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration )
+{
+    btScalar leastSquaresResidual = 0.f;
+    for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons )
+    {
+        int iCons = consIndices[ iiCons ];
+        const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[ iCons ];
+        if ( iteration < constraint.m_overrideNumSolverIterations )
+        {
+            btSolverBody& bodyA = m_tmpSolverBodyPool[ constraint.m_solverBodyIdA ];
+            btSolverBody& bodyB = m_tmpSolverBodyPool[ constraint.m_solverBodyIdB ];
+            btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, constraint );
+            leastSquaresResidual += residual*residual;
+        }
+    }
+    return leastSquaresResidual;
+}
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd )
+{
+    btScalar leastSquaresResidual = 0.f;
+    for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons )
+    {
+        int iCons = consIndices[ iiCons ];
+        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iCons ];
+        btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ];
+        btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ];
+        btScalar residual = resolveSingleConstraintRowLowerLimit( bodyA, bodyB, solveManifold );
+        leastSquaresResidual += residual*residual;
+    }
+    return leastSquaresResidual;
+}
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd )
+{
+    btScalar leastSquaresResidual = 0.f;
+    for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons )
+    {
+        int iContact = consIndices[ iiCons ];
+        btScalar totalImpulse = m_tmpSolverContactConstraintPool[ iContact ].m_appliedImpulse;
+
+        // apply sliding friction
+        if ( totalImpulse > 0.0f )
+        {
+            int iBegin = iContact * m_numFrictionDirections;
+            int iEnd = iBegin + m_numFrictionDirections;
+            for ( int iFriction = iBegin; iFriction < iEnd; ++iFriction )
+            {
+                btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[ iFriction++ ];
+                btAssert( solveManifold.m_frictionIndex == iContact );
+
+                solveManifold.m_lowerLimit = -( solveManifold.m_friction*totalImpulse );
+                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+                btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ];
+                btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ];
+                btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, solveManifold );
+                leastSquaresResidual += residual*residual;
+            }
+        }
+    }
+    return leastSquaresResidual;
+}
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd )
+{
+    btScalar leastSquaresResidual = 0.f;
+    for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons )
+    {
+        int iContact = consIndices[ iiCons ];
+        int iFirstRollingFriction = m_rollingFrictionIndexTable[ iContact ];
+        if ( iFirstRollingFriction >= 0 )
+        {
+            btScalar totalImpulse = m_tmpSolverContactConstraintPool[ iContact ].m_appliedImpulse;
+            // apply rolling friction
+            if ( totalImpulse > 0.0f )
+            {
+                int iBegin = iFirstRollingFriction;
+                int iEnd = iBegin + 3;
+                for ( int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric )
+                {
+                    btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ];
+                    if ( rollingFrictionConstraint.m_frictionIndex != iContact )
+                    {
+                        break;
+                    }
+                    btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+                    if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
+                    {
+                        rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+                    }
+
+                    rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+                    rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+                    btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint );
+                    leastSquaresResidual += residual*residual;
+                }
+            }
+        }
+    }
+    return leastSquaresResidual;
+}
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& contactIndices,
+                                                                                          int batchBegin,
+                                                                                          int batchEnd
+                                                                                          )
+{
+    btScalar leastSquaresResidual = 0.f;
+    int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+
+    for ( int iiCons = batchBegin; iiCons < batchEnd; iiCons++ )
+    {
+        btScalar totalImpulse = 0;
+        int iContact = contactIndices[ iiCons ];
+        // apply penetration constraint
+        {
+            const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iContact ];
+            btScalar residual = resolveSingleConstraintRowLowerLimit( m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ], m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ], solveManifold );
+            leastSquaresResidual += residual*residual;
+            totalImpulse = solveManifold.m_appliedImpulse;
+        }
+
+        // apply sliding friction
+        if ( totalImpulse > 0.0f )
+        {
+            int iBegin = iContact * m_numFrictionDirections;
+            int iEnd = iBegin + m_numFrictionDirections;
+            for ( int iFriction = iBegin; iFriction < iEnd; ++iFriction )
+            {
+                btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[ iFriction ];
+                btAssert( solveManifold.m_frictionIndex == iContact );
+
+                solveManifold.m_lowerLimit = -( solveManifold.m_friction*totalImpulse );
+                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+                btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ];
+                btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ];
+                btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, solveManifold );
+                leastSquaresResidual += residual*residual;
+            }
+        }
+
+        // apply rolling friction
+        int iFirstRollingFriction = m_rollingFrictionIndexTable[ iContact ];
+        if ( totalImpulse > 0.0f && iFirstRollingFriction >= 0)
+        {
+            int iBegin = iFirstRollingFriction;
+            int iEnd = iBegin + 3;
+            for ( int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric )
+            {
+                btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ];
+                if ( rollingFrictionConstraint.m_frictionIndex != iContact )
+                {
+                    break;
+                }
+                btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+                if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
+                {
+                    rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+                }
+
+                rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+                rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+                btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint );
+                leastSquaresResidual += residual*residual;
+            }
+        }
+    }
+    return leastSquaresResidual;
+}
+
+
+void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints )
+{
+    btBatchedConstraints& bc = *batchedConstraints;
+    // randomize ordering of phases
+    for ( int ii = 1; ii < bc.m_phaseOrder.size(); ++ii )
+    {
+        int iSwap = btRandInt2( ii + 1 );
+        bc.m_phaseOrder.swap( ii, iSwap );
+    }
+
+    // for each batch,
+    for ( int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch )
+    {
+        // randomize ordering of constraints within the batch
+        const btBatchedConstraints::Range& batch = bc.m_batches[ iBatch ];
+        for ( int iiCons = batch.begin; iiCons < batch.end; ++iiCons )
+        {
+            int iSwap = batch.begin + btRandInt2( iiCons - batch.begin + 1 );
+            btAssert(iSwap >= batch.begin && iSwap < batch.end);
+            bc.m_constraintIndices.swap( iiCons, iSwap );
+        }
+    }
+}
+
+
+void btSequentialImpulseConstraintSolverMt::randomizeConstraintOrdering(int iteration, int numIterations)
+{
+    // randomize ordering of joint constraints
+    randomizeBatchedConstraintOrdering( &m_batchedJointConstraints );
+
+    //contact/friction constraints are not solved more than numIterations
+    if ( iteration < numIterations )
+    {
+        randomizeBatchedConstraintOrdering( &m_batchedContactConstraints );
+    }
+}
+
+
+struct JointSolverLoop : public btIParallelSumBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btBatchedConstraints* m_bc;
+    int m_iteration;
+
+    JointSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration )
+    {
+        m_solver = solver;
+        m_bc = bc;
+        m_iteration = iteration;
+    }
+    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        BT_PROFILE( "JointSolverLoop" );
+        btScalar sum = 0;
+        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
+        {
+            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
+            sum += m_solver->resolveMultipleJointConstraints( m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration );
+        }
+        return sum;
+    }
+};
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllJointConstraints(int iteration)
+{
+    BT_PROFILE( "resolveAllJointConstraints" );
+    const btBatchedConstraints& batchedCons = m_batchedJointConstraints;
+    JointSolverLoop loop( this, &batchedCons, iteration );
+    btScalar leastSquaresResidual = 0.f;
+    for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
+    {
+        int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
+        const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
+        int grainSize = 1;
+        leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
+    }
+    return leastSquaresResidual;
+}
+
+
+struct ContactSolverLoop : public btIParallelSumBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btBatchedConstraints* m_bc;
+
+    ContactSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc )
+    {
+        m_solver = solver;
+        m_bc = bc;
+    }
+    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        BT_PROFILE( "ContactSolverLoop" );
+        btScalar sum = 0;
+        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
+        {
+            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
+            sum += m_solver->resolveMultipleContactConstraints( m_bc->m_constraintIndices, batch.begin, batch.end );
+        }
+        return sum;
+    }
+};
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraints()
+{
+    BT_PROFILE( "resolveAllContactConstraints" );
+    const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+    ContactSolverLoop loop( this, &batchedCons );
+    btScalar leastSquaresResidual = 0.f;
+    for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
+    {
+        int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
+        const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
+        int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+        leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
+    }
+    return leastSquaresResidual;
+}
+
+
+struct ContactFrictionSolverLoop : public btIParallelSumBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btBatchedConstraints* m_bc;
+
+    ContactFrictionSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc )
+    {
+        m_solver = solver;
+        m_bc = bc;
+    }
+    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        BT_PROFILE( "ContactFrictionSolverLoop" );
+        btScalar sum = 0;
+        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
+        {
+            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
+            sum += m_solver->resolveMultipleContactFrictionConstraints( m_bc->m_constraintIndices, batch.begin, batch.end );
+        }
+        return sum;
+    }
+};
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactFrictionConstraints()
+{
+    BT_PROFILE( "resolveAllContactFrictionConstraints" );
+    const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+    ContactFrictionSolverLoop loop( this, &batchedCons );
+    btScalar leastSquaresResidual = 0.f;
+    for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
+    {
+        int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
+        const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
+        int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+        leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
+    }
+    return leastSquaresResidual;
+}
+
+
+struct InterleavedContactSolverLoop : public btIParallelSumBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btBatchedConstraints* m_bc;
+
+    InterleavedContactSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc )
+    {
+        m_solver = solver;
+        m_bc = bc;
+    }
+    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        BT_PROFILE( "InterleavedContactSolverLoop" );
+        btScalar sum = 0;
+        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
+        {
+            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
+            sum += m_solver->resolveMultipleContactConstraintsInterleaved( m_bc->m_constraintIndices, batch.begin, batch.end );
+        }
+        return sum;
+    }
+};
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraintsInterleaved()
+{
+    BT_PROFILE( "resolveAllContactConstraintsInterleaved" );
+    const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+    InterleavedContactSolverLoop loop( this, &batchedCons );
+    btScalar leastSquaresResidual = 0.f;
+    for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
+    {
+        int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
+        const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
+        int grainSize = 1;
+        leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
+    }
+    return leastSquaresResidual;
+}
+
+
+struct ContactRollingFrictionSolverLoop : public btIParallelSumBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btBatchedConstraints* m_bc;
+
+    ContactRollingFrictionSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc )
+    {
+        m_solver = solver;
+        m_bc = bc;
+    }
+    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        BT_PROFILE( "ContactFrictionSolverLoop" );
+        btScalar sum = 0;
+        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
+        {
+            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
+            sum += m_solver->resolveMultipleContactRollingFrictionConstraints( m_bc->m_constraintIndices, batch.begin, batch.end );
+        }
+        return sum;
+    }
+};
+
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllRollingFrictionConstraints()
+{
+    BT_PROFILE( "resolveAllRollingFrictionConstraints" );
+    btScalar leastSquaresResidual = 0.f;
+    //
+    // We do not generate batches for rolling friction constraints. We assume that
+    // one of two cases is true:
+    //
+    //  1. either most bodies in the simulation have rolling friction, in which case we can use the
+    //     batches for contacts and use a lookup table to translate contact indices to rolling friction
+    //     (ignoring any contact indices that don't map to a rolling friction constraint). As long as
+    //     most contacts have a corresponding rolling friction constraint, this should parallelize well.
+    //
+    //  -OR-
+    //
+    //  2. few bodies in the simulation have rolling friction, so it is not worth trying to use the
+    //     batches from contacts as most of the contacts won't have corresponding rolling friction
+    //     constraints and most threads would end up doing very little work. Most of the time would
+    //     go to threading overhead, so we don't bother with threading.
+    //
+    int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+    if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size())
+    {
+        // use batching if there are many rolling friction constraints
+        const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+        ContactRollingFrictionSolverLoop loop( this, &batchedCons );
+        btScalar leastSquaresResidual = 0.f;
+        for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
+        {
+            int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
+            const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
+            int grainSize = 1;
+            leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
+        }
+    }
+    else
+    {
+        // no batching, also ignores SOLVER_RANDMIZE_ORDER
+        for ( int j = 0; j < numRollingFrictionPoolConstraints; j++ )
+        {
+            btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ j ];
+            if ( rollingFrictionConstraint.m_frictionIndex >= 0 )
+            {
+                btScalar totalImpulse = m_tmpSolverContactConstraintPool[ rollingFrictionConstraint.m_frictionIndex ].m_appliedImpulse;
+                if ( totalImpulse > 0.0f )
+                {
+                    btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+                    if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
+                        rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+                    rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+                    rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+                    btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint );
+                    leastSquaresResidual += residual*residual;
+                }
+            }
+        }
+    }
+    return leastSquaresResidual;
+}
+
+
+void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal )
+{
+    BT_PROFILE("internalWriteBackContacts");
+    writeBackContacts(iBegin, iEnd, infoGlobal);
+    //for ( int iContact = iBegin; iContact < iEnd; ++iContact)
+    //{
+    //    const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ];
+    //    btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint;
+    //    btAssert( pt );
+    //    pt->m_appliedImpulse = contactConstraint.m_appliedImpulse;
+    //    pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse;
+    //    if ( m_numFrictionDirections == 2 )
+    //    {
+    //        pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse;
+    //    }
+    //}
+}
+
+
+void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal )
+{
+	BT_PROFILE("internalWriteBackJoints");
+    writeBackJoints(iBegin, iEnd, infoGlobal);
+}
+
+
+void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal )
+{
+	BT_PROFILE("internalWriteBackBodies");
+    writeBackBodies( iBegin, iEnd, infoGlobal );
+}
+
+
+struct WriteContactPointsLoop : public btIParallelForBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btContactSolverInfo* m_infoGlobal;
+
+    WriteContactPointsLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal )
+    {
+        m_solver = solver;
+        m_infoGlobal = &infoGlobal;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        m_solver->internalWriteBackContacts( iBegin, iEnd, *m_infoGlobal );
+    }
+};
+
+
+struct WriteJointsLoop : public btIParallelForBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btContactSolverInfo* m_infoGlobal;
+
+    WriteJointsLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal )
+    {
+        m_solver = solver;
+        m_infoGlobal = &infoGlobal;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        m_solver->internalWriteBackJoints( iBegin, iEnd, *m_infoGlobal );
+    }
+};
+
+
+struct WriteBodiesLoop : public btIParallelForBody
+{
+    btSequentialImpulseConstraintSolverMt* m_solver;
+    const btContactSolverInfo* m_infoGlobal;
+
+    WriteBodiesLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal )
+    {
+        m_solver = solver;
+        m_infoGlobal = &infoGlobal;
+    }
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        m_solver->internalWriteBackBodies( iBegin, iEnd, *m_infoGlobal );
+    }
+};
+
+
+btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("solveGroupCacheFriendlyFinish");
+
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+    {
+        WriteContactPointsLoop loop( this, infoGlobal );
+        int grainSize = 500;
+        btParallelFor( 0, m_tmpSolverContactConstraintPool.size(), grainSize, loop );
+    }
+
+    {
+        WriteJointsLoop loop( this, infoGlobal );
+        int grainSize = 400;
+        btParallelFor( 0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop );
+    }
+    {
+        WriteBodiesLoop loop( this, infoGlobal );
+        int grainSize = 100;
+        btParallelFor( 0, m_tmpSolverBodyPool.size(), grainSize, loop );
+    }
+
+	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
+
+	m_tmpSolverBodyPool.resizeNoInitialize(0);
+	return 0.f;
+}
+

thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h

@@ -0,0 +1,154 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
+#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
+
+#include "btSequentialImpulseConstraintSolver.h"
+#include "btBatchedConstraints.h"
+#include "LinearMath/btThreads.h"
+
+///
+/// btSequentialImpulseConstraintSolverMt
+///
+///  A multithreaded variant of the sequential impulse constraint solver. The constraints to be solved are grouped into
+///  batches and phases where each batch of constraints within a given phase can be solved in parallel with the rest.
+///  Ideally we want as few phases as possible, and each phase should have many batches, and all of the batches should
+///  have about the same number of constraints.
+///  This method works best on a large island of many constraints.
+///
+///  Supports all of the features of the normal sequential impulse solver such as:
+///    - split penetration impulse
+///    - rolling friction
+///    - interleaving constraints
+///    - warmstarting
+///    - 2 friction directions
+///    - randomized constraint ordering
+///    - early termination when leastSquaresResidualThreshold is satisfied
+///
+///  When the SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS flag is enabled, unlike the normal SequentialImpulse solver,
+///  the rolling friction is interleaved as well.
+///  Interleaving the contact penetration constraints with friction reduces the number of parallel loops that need to be done,
+///  which reduces threading overhead so it can be a performance win, however, it does seem to produce a less stable simulation,
+///  at least on stacks of blocks.
+///
+///  When the SOLVER_RANDMIZE_ORDER flag is enabled, the ordering of phases, and the ordering of constraints within each batch
+///  is randomized, however it does not swap constraints between batches.
+///  This is to avoid regenerating the batches for each solver iteration which would be quite costly in performance.
+///
+///  Note that a non-zero leastSquaresResidualThreshold could possibly affect the determinism of the simulation
+///  if the task scheduler's parallelSum operation is non-deterministic. The parallelSum operation can be non-deterministic
+///  because floating point addition is not associative due to rounding errors.
+///  The task scheduler can and should ensure that the result of any parallelSum operation is deterministic.
+///
+ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver
+{
+public:
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE;
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+
+    // temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads
+    struct btContactManifoldCachedInfo
+    {
+        static const int MAX_NUM_CONTACT_POINTS = 4;
+
+        int numTouchingContacts;
+        int solverBodyIds[ 2 ];
+        int contactIndex;
+        int rollingFrictionIndex;
+        bool contactHasRollingFriction[ MAX_NUM_CONTACT_POINTS ];
+        btManifoldPoint* contactPoints[ MAX_NUM_CONTACT_POINTS ];
+    };
+    // temp struct used for setting up joint constraints in parallel
+    struct JointParams
+    {
+        int m_solverConstraint;
+        int m_solverBodyA;
+        int m_solverBodyB;
+    };
+    void internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd);
+    void internalConvertMultipleJoints( const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal );
+
+    // parameters to control batching
+    static bool s_allowNestedParallelForLoops;        // whether to allow nested parallel operations
+    static int s_minimumContactManifoldsForBatching;  // don't even try to batch if fewer manifolds than this
+    static btBatchedConstraints::BatchingMethod s_contactBatchingMethod;
+    static btBatchedConstraints::BatchingMethod s_jointBatchingMethod;
+    static int s_minBatchSize;  // desired number of constraints per batch
+    static int s_maxBatchSize;
+
+protected:
+    static const int CACHE_LINE_SIZE = 64;
+
+    btBatchedConstraints m_batchedContactConstraints;
+    btBatchedConstraints m_batchedJointConstraints;
+    int m_numFrictionDirections;
+    bool m_useBatching;
+    bool m_useObsoleteJointConstraints;
+    btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray;
+    btAlignedObjectArray<int> m_rollingFrictionIndexTable;  // lookup table mapping contact index to rolling friction index
+    btSpinMutex m_bodySolverArrayMutex;
+    char m_antiFalseSharingPadding[CACHE_LINE_SIZE]; // padding to keep mutexes in separate cachelines
+    btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex;
+    btAlignedObjectArray<char> m_scratchMemory;
+
+    virtual void randomizeConstraintOrdering( int iteration, int numIterations );
+    virtual btScalar resolveAllJointConstraints( int iteration );
+    virtual btScalar resolveAllContactConstraints();
+    virtual btScalar resolveAllContactFrictionConstraints();
+    virtual btScalar resolveAllContactConstraintsInterleaved();
+    virtual btScalar resolveAllRollingFrictionConstraints();
+
+    virtual void setupBatchedContactConstraints();
+    virtual void setupBatchedJointConstraints();
+    virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+	virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+    virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+
+	int getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep);
+    void allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+    void setupAllContactConstraints(const btContactSolverInfo& infoGlobal);
+    void randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints );
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btSequentialImpulseConstraintSolverMt();
+	virtual ~btSequentialImpulseConstraintSolverMt();
+
+    btScalar resolveMultipleJointConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration );
+    btScalar resolveMultipleContactConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
+    btScalar resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
+    btScalar resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
+    btScalar resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
+    btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd );
+
+    void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+    void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
+    void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal);
+    void internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+    void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+    void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+    void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+};
+
+
+
+
+#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
+

thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

@@ -842,6 +842,9 @@ public:
 
 		btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
 
+		if(!m_dispatcher->needsCollision(m_me, otherObj))
+			return false;
+
 		//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
 		if (m_dispatcher->needsResponse(m_me,otherObj))
 		{
@@ -1342,9 +1345,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 					btVector3 axis = tr.getBasis().getColumn(0);
 					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
 					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
-					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
-					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
-					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+					if (minTh <= maxTh)
+					{
+						btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+						btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+						getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+					}
 					axis = tr.getBasis().getColumn(1);
 					btScalar ay = p6DOF->getAngle(1);
 					btScalar az = p6DOF->getAngle(2);
@@ -1533,6 +1539,8 @@ void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeRigidBodies(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 

thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp

@@ -50,63 +50,6 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 
 
-struct InplaceSolverIslandCallbackMt : public btSimulationIslandManagerMt::IslandCallback
-{
-	btContactSolverInfo*	m_solverInfo;
-	btConstraintSolver*		m_solver;
-	btIDebugDraw*			m_debugDrawer;
-	btDispatcher*			m_dispatcher;
-
-	InplaceSolverIslandCallbackMt(
-		btConstraintSolver*	solver,
-		btStackAlloc* stackAlloc,
-		btDispatcher* dispatcher)
-		:m_solverInfo(NULL),
-		m_solver(solver),
-		m_debugDrawer(NULL),
-		m_dispatcher(dispatcher)
-	{
-
-	}
-
-	InplaceSolverIslandCallbackMt& operator=(InplaceSolverIslandCallbackMt& other)
-	{
-		btAssert(0);
-		(void)other;
-		return *this;
-	}
-
-	SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btIDebugDraw* debugDrawer)
-	{
-		btAssert(solverInfo);
-		m_solverInfo = solverInfo;
-		m_debugDrawer = debugDrawer;
-	}
-
-
-	virtual	void	processIsland( btCollisionObject** bodies,
-                                   int numBodies,
-                                   btPersistentManifold** manifolds,
-                                   int numManifolds,
-                                   btTypedConstraint** constraints,
-                                   int numConstraints,
-                                   int islandId
-                                   )
-	{
-        m_solver->solveGroup( bodies,
-                              numBodies,
-                              manifolds,
-                              numManifolds,
-                              constraints,
-                              numConstraints,
-                              *m_solverInfo,
-                              m_debugDrawer,
-                              m_dispatcher
-                              );
-    }
-
-};
-
 
 ///
 /// btConstraintSolverPoolMt
@@ -209,7 +152,12 @@ void btConstraintSolverPoolMt::reset()
 /// btDiscreteDynamicsWorldMt
 ///
 
-btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolverPoolMt* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
+    btBroadphaseInterface* pairCache,
+    btConstraintSolverPoolMt* constraintSolver,
+    btConstraintSolver* constraintSolverMt,
+    btCollisionConfiguration* collisionConfiguration
+)
 : btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
 {
 	if (m_ownsIslandManager)
@@ -217,31 +165,18 @@ btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, b
 		m_islandManager->~btSimulationIslandManager();
 		btAlignedFree( m_islandManager);
 	}
-    {
-		void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallbackMt),16);
-		m_solverIslandCallbackMt = new (mem) InplaceSolverIslandCallbackMt (m_constraintSolver, 0, dispatcher);
-    }
 	{
 		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
 		btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
         im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
         m_islandManager = im;
 	}
+    m_constraintSolverMt = constraintSolverMt;
 }
 
 
 btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
 {
-	if (m_solverIslandCallbackMt)
-	{
-		m_solverIslandCallbackMt->~InplaceSolverIslandCallbackMt();
-		btAlignedFree(m_solverIslandCallbackMt);
-	}
-	if (m_ownsConstraintSolver)
-	{
-		m_constraintSolver->~btConstraintSolver();
-		btAlignedFree(m_constraintSolver);
-	}
 }
 
 
@@ -249,12 +184,17 @@ void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
 {
 	BT_PROFILE("solveConstraints");
 
-	m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 
 	/// solve all the constraints for this island
     btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
-    im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt );
+    btSimulationIslandManagerMt::SolverParams solverParams;
+    solverParams.m_solverPool = m_constraintSolver;
+    solverParams.m_solverMt = m_constraintSolverMt;
+    solverParams.m_solverInfo = &solverInfo;
+    solverParams.m_debugDrawer = m_debugDrawer;
+    solverParams.m_dispatcher = getCollisionWorld()->getDispatcher();
+    im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams );
 
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
 }
@@ -325,3 +265,14 @@ void btDiscreteDynamicsWorldMt::integrateTransforms( btScalar timeStep )
     }
 }
 
+
+int	btDiscreteDynamicsWorldMt::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep )
+{
+    int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
+    if (btITaskScheduler* scheduler = btGetTaskScheduler())
+    {
+        // tell Bullet's threads to sleep, so other threads can run
+        scheduler->sleepWorkerThreadsHint();
+    }
+    return numSubSteps;
+}

thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h

@@ -21,7 +21,6 @@ subject to the following restrictions:
 #include "btSimulationIslandManagerMt.h"
 #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
 
-struct InplaceSolverIslandCallbackMt;
 
 ///
 /// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them.
@@ -88,7 +87,7 @@ private:
 ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
 {
 protected:
-    InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt;
+    btConstraintSolver* m_constraintSolverMt;
 
     virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE;
 
@@ -126,9 +125,12 @@ public:
 	btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
         btBroadphaseInterface* pairCache,
         btConstraintSolverPoolMt* constraintSolver,   // Note this should be a solver-pool for multi-threading
+        btConstraintSolver* constraintSolverMt,    // single multi-threaded solver for large islands (or NULL)
         btCollisionConfiguration* collisionConfiguration
     );
 	virtual ~btDiscreteDynamicsWorldMt();
+
+    virtual int	stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep ) BT_OVERRIDE;
 };
 
 #endif //BT_DISCRETE_DYNAMICS_WORLD_H

thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp

@@ -22,6 +22,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h"  // for s_minimumContactManifoldsForBatching
 
 //#include <stdio.h>
 #include "LinearMath/btQuickprof.h"
@@ -275,7 +276,7 @@ btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland
 void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld )
 {
 
-	BT_PROFILE("islandUnionFindAndQuickSort");
+	BT_PROFILE("buildIslands");
 	
 	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 
@@ -314,13 +315,11 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl
 			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;
 				}
 			}
 		}
@@ -546,59 +545,103 @@ void btSimulationIslandManagerMt::mergeIslands()
 }
 
 
-void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
+void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams)
+{
+    btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[ 0 ] : NULL;
+    btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[ 0 ] : NULL;
+    solver->solveGroup( &island.bodyArray[ 0 ],
+        island.bodyArray.size(),
+        manifolds,
+        island.manifoldArray.size(),
+        constraintsPtr,
+        island.constraintArray.size(),
+        *solverParams.m_solverInfo,
+        solverParams.m_debugDrawer,
+        solverParams.m_dispatcher
+    );
+}
+
+
+void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams )
 {
     BT_PROFILE( "serialIslandDispatch" );
     // serial dispatch
     btAlignedObjectArray<Island*>& islands = *islandsPtr;
+    btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
     for ( int i = 0; i < islands.size(); ++i )
     {
-        Island* island = islands[ i ];
-        btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
-        btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
-        callback->processIsland( &island->bodyArray[ 0 ],
-                                 island->bodyArray.size(),
-                                 manifolds,
-                                 island->manifoldArray.size(),
-                                 constraintsPtr,
-                                 island->constraintArray.size(),
-                                 island->id
-                                 );
+        solveIsland(solver, *islands[ i ], solverParams);
     }
 }
 
+
 struct UpdateIslandDispatcher : public btIParallelForBody
 {
-    btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr;
-    btSimulationIslandManagerMt::IslandCallback* callback;
+    btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr;
+    const btSimulationIslandManagerMt::SolverParams& m_solverParams;
+
+    UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams)
+        : m_islandsPtr(islandsPtr), m_solverParams(solverParams)
+    {}
 
     void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
     {
+        btConstraintSolver* solver = m_solverParams.m_solverPool;
         for ( int i = iBegin; i < iEnd; ++i )
         {
-            btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ i ];
-            btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
-            btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
-            callback->processIsland( &island->bodyArray[ 0 ],
-                island->bodyArray.size(),
-                manifolds,
-                island->manifoldArray.size(),
-                constraintsPtr,
-                island->constraintArray.size(),
-                island->id
-            );
+            btSimulationIslandManagerMt::Island* island = m_islandsPtr[ i ];
+            btSimulationIslandManagerMt::solveIsland( solver, *island, m_solverParams );
         }
     }
 };
 
-void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
+
+void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams )
 {
     BT_PROFILE( "parallelIslandDispatch" );
-    int grainSize = 1;  // iterations per task
-    UpdateIslandDispatcher dispatcher;
-    dispatcher.islandsPtr = islandsPtr;
-    dispatcher.callback = callback;
-    btParallelFor( 0, islandsPtr->size(), grainSize, dispatcher );
+    //
+    // if there are islands with many contacts, it may be faster to submit these
+    // large islands *serially* to a single parallel constraint solver, and then later
+    // submit the remaining smaller islands in parallel to multiple sequential solvers.
+    //
+    // Some task schedulers do not deal well with nested parallelFor loops. One implementation
+    // of OpenMP was actually slower than doing everything single-threaded. Intel TBB
+    // on the other hand, seems to do a pretty respectable job with it.
+    //
+    // When solving islands in parallel, the worst case performance happens when there
+    // is one very large island and then perhaps a smattering of very small
+    // islands -- one worker thread takes the large island and the remaining workers
+    // tear through the smaller islands and then sit idle waiting for the first worker
+    // to finish. Solving islands in parallel works best when there are numerous small
+    // islands, roughly equal in size.
+    //
+    // By contrast, the other approach -- the parallel constraint solver -- is only
+    // able to deliver a worthwhile speedup when the island is large. For smaller islands,
+    // it is difficult to extract a useful amount of parallelism -- the overhead of grouping
+    // the constraints into batches and sending the batches to worker threads can nullify
+    // any gains from parallelism.
+    //
+
+    UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams);
+    // We take advantage of the fact the islands are sorted in order of decreasing size
+    int iBegin = 0;
+    if (solverParams.m_solverMt)
+    {
+        while ( iBegin < islandsPtr->size() )
+        {
+            btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ iBegin ];
+            if ( island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching )
+            {
+                // OK to submit the rest of the array in parallel
+                break;
+            }
+            // serial dispatch to parallel solver for large islands (if any)
+            solveIsland(solverParams.m_solverMt, *island, solverParams);
+            ++iBegin;
+        }
+    }
+    // parallel dispatch to sequential solvers for rest
+    btParallelFor( iBegin, islandsPtr->size(), 1, dispatcher );
 }
 
 
@@ -606,15 +649,14 @@ void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<I
 void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher,
                                                         btCollisionWorld* collisionWorld,
                                                         btAlignedObjectArray<btTypedConstraint*>& constraints,
-                                                        IslandCallback* callback
+                                                        const SolverParams& solverParams
                                                         )
 {
+	BT_PROFILE("buildAndProcessIslands");
 	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 
 	buildIslands(dispatcher,collisionWorld);
 
-	BT_PROFILE("processIslands");
-
 	if(!getSplitIslands())
 	{
         btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
@@ -646,14 +688,17 @@ void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatch
             }
         }
         btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
-		callback->processIsland(&collisionObjects[0],
-                                 collisionObjects.size(),
-                                 manifolds,
-                                 maxNumManifolds,
-                                 constraintsPtr,
-                                 constraints.size(),
-                                 -1
-                                 );
+        btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
+        solver->solveGroup(&collisionObjects[0],
+                           collisionObjects.size(),
+                           manifolds,
+                           maxNumManifolds,
+                           constraintsPtr,
+                           constraints.size(),
+                           *solverParams.m_solverInfo,
+                           solverParams.m_debugDrawer,
+                           solverParams.m_dispatcher
+                           );
 	}
 	else
 	{
@@ -673,6 +718,6 @@ void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatch
             mergeIslands();
         }
         // dispatch islands to solver
-        m_islandDispatch( &m_activeIslands, callback );
+        m_islandDispatch( &m_activeIslands, solverParams );
 	}
 }

thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h

@@ -19,7 +19,9 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 
 class btTypedConstraint;
-
+class btConstraintSolver;
+struct btContactSolverInfo;
+class btIDebugDraw;
 
 ///
 /// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager
@@ -45,22 +47,19 @@ public:
 
         void append( const Island& other );  // add bodies, manifolds, constraints to my own
     };
-    struct	IslandCallback
+    struct SolverParams
     {
-        virtual ~IslandCallback() {};
-
-        virtual	void processIsland( btCollisionObject** bodies,
-                                    int numBodies,
-                                    btPersistentManifold** manifolds,
-                                    int numManifolds,
-                                    btTypedConstraint** constraints,
-                                    int numConstraints,
-                                    int islandId
-                                    ) = 0;
+        btConstraintSolver*		m_solverPool;
+        btConstraintSolver*		m_solverMt;
+        btContactSolverInfo*	m_solverInfo;
+        btIDebugDraw*			m_debugDrawer;
+        btDispatcher*			m_dispatcher;
     };
-    typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
-    static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
-    static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
+    static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams);
+
+    typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams );
+    static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams );
+    static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams );
 protected:
     btAlignedObjectArray<Island*> m_allocatedIslands;  // owner of all Islands
     btAlignedObjectArray<Island*> m_activeIslands;  // islands actively in use
@@ -83,7 +82,11 @@ public:
 	btSimulationIslandManagerMt();
 	virtual ~btSimulationIslandManagerMt();
 
-    virtual void buildAndProcessIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld, btAlignedObjectArray<btTypedConstraint*>& constraints, IslandCallback* callback );
+    virtual void buildAndProcessIslands( btDispatcher* dispatcher,
+        btCollisionWorld* collisionWorld,
+        btAlignedObjectArray<btTypedConstraint*>& constraints,
+        const SolverParams& solverParams
+    );
 
 	virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
 
@@ -106,5 +109,6 @@ public:
     }
 };
 
+
 #endif //BT_SIMULATION_ISLAND_MANAGER_H
 

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -112,14 +112,15 @@ btMultiBody::btMultiBody(int n_links,
 		m_userObjectPointer(0),
 		m_userIndex2(-1),
 		m_userIndex(-1),
+		m_companionId(-1),
 		m_linearDamping(0.04f),
 		m_angularDamping(0.04f),
 		m_useGyroTerm(true),
-			m_maxAppliedImpulse(1000.f),
+		m_maxAppliedImpulse(1000.f),
 		m_maxCoordinateVelocity(100.f),
-			m_hasSelfCollision(true),		
+		m_hasSelfCollision(true),		
 		__posUpdated(false),
-			m_dofCount(0),
+		m_dofCount(0),
 		m_posVarCnt(0),
 		m_useRK4(false), 	
 		m_useGlobalVelocities(false),
@@ -136,6 +137,9 @@ btMultiBody::btMultiBody(int n_links,
 
     m_baseForce.setValue(0, 0, 0);
     m_baseTorque.setValue(0, 0, 0);
+
+	clearConstraintForces();
+	clearForcesAndTorques();
 }
 
 btMultiBody::~btMultiBody()
@@ -740,13 +744,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
 	const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
 
-    btVector3 base_vel = getBaseVel();
-    btVector3 base_omega = getBaseOmega();
+	const btVector3 base_vel = getBaseVel();
+	const btVector3 base_omega = getBaseOmega();
 
     // Temporary matrices/vectors -- use scratch space from caller
     // so that we don't have to keep reallocating every frame
 
-    scratch_r.resize(2*m_dofCount + 6);				//multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
+    scratch_r.resize(2*m_dofCount + 7);				//multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
     scratch_v.resize(8*num_links + 6);
     scratch_m.resize(4*num_links + 4);
 
@@ -777,7 +781,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
     // hhat is NOT stored for the base (but ahat is)    
 	btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
 	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
-	v_ptr += num_links * 2 + 2;
+	v_ptr += num_links * 2 + 2; 
 	//
     // Y_i, invD_i
     btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
@@ -815,13 +819,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
     }
 	else 
 	{
-		btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
-		btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
+		const btVector3& baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
+		const btVector3& baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
 		//external forces		
 		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));	
 
 		//adding damping terms (only)
-		btScalar linDampMult = 1., angDampMult = 1.;
+		const btScalar linDampMult = 1., angDampMult = 1.;
 		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
 								   linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
 
@@ -963,16 +967,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			//
 			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
 			- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
-			- spatCoriolisAcc[i].dot(hDof)
-			;
-		}
+			- spatCoriolisAcc[i].dot(hDof);
 
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{
-			btScalar *D_row = &D[dof * m_links[i].m_dofCount];			
+		}
+ 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{	
+			btScalar *D_row = &D[dof * m_links[i].m_dofCount];
 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
 			{
-				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
 				D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
 			}
 		}
@@ -983,14 +986,20 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			case btMultibodyLink::ePrismatic:
 			case btMultibodyLink::eRevolute:
 			{
-				invDi[0] = 1.0f / D[0];
+				if (D[0]>=SIMD_EPSILON)
+				{
+					invDi[0] = 1.0f / D[0];
+				} else
+				{
+					invDi[0] = 0;
+				}
 				break;
 			}
 			case btMultibodyLink::eSpherical:
 			case btMultibodyLink::ePlanar:
 			{
-				btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
-				btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
+				const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+				const btMatrix3x3 invD3x3(D3x3.inverse());
 
 				//unroll the loop?
 				for(int row = 0; row < 3; ++row)
@@ -1016,7 +1025,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
 			{				
-				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
 				//				
 				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
 			}
@@ -1027,7 +1036,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		//determine (h*D^{-1}) * h^{T}
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{			
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//
 			dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
 		}
@@ -1048,7 +1057,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{				
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//
 			spatForceVecTemps[0] += hDof * invD_times_Y[dof];			
 		}
@@ -1099,7 +1108,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//			
 			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);			
 		}
@@ -1159,12 +1168,12 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
     }
 
     // transform base accelerations back to the world frame.
-    btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
+	const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
 	output[0] = omegadot_out[0];
 	output[1] = omegadot_out[1];
 	output[2] = omegadot_out[2];
 
-    btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
+	const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
 	output[3] = vdot_out[0];
 	output[4] = vdot_out[1];
 	output[5] = vdot_out[2];
@@ -1266,12 +1275,29 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo
     if (num_links == 0) 
 	{
 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
-        result[0] = rhs_bot[0] / m_baseInertia[0];
-        result[1] = rhs_bot[1] / m_baseInertia[1];
+   
+	if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+	{
+		result[0] = rhs_bot[0] / m_baseInertia[0];
+		result[1] = rhs_bot[1] / m_baseInertia[1];
         result[2] = rhs_bot[2] / m_baseInertia[2];
-        result[3] = rhs_top[0] / m_baseMass;
-        result[4] = rhs_top[1] / m_baseMass;
-        result[5] = rhs_top[2] / m_baseMass;
+	} else
+	{
+		result[0] = 0;
+		result[1] = 0;
+		result[2] = 0;
+	}
+	if (m_baseMass>=SIMD_EPSILON)
+	{
+        	result[3] = rhs_top[0] / m_baseMass;
+        	result[4] = rhs_top[1] / m_baseMass;
+        	result[5] = rhs_top[2] / m_baseMass;
+	} else
+	{
+		result[3] = 0;
+		result[4] = 0;
+		result[5] = 0;
+	}
     } else 
 	{
 		if (!m_cachedInertiaValid)
@@ -1322,9 +1348,21 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV
     if (num_links == 0) 
 	{
 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
-		result.setAngular(rhs.getAngular() / m_baseInertia);
-		result.setLinear(rhs.getLinear() / m_baseMass);		
-    } else 
+		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+        	{
+			result.setAngular(rhs.getAngular() / m_baseInertia);       
+        	} else  
+        	{       
+                	result.setAngular(btVector3(0,0,0));
+        	}
+        	if (m_baseMass>=SIMD_EPSILON)
+        	{       
+			result.setLinear(rhs.getLinear() / m_baseMass);               	 	
+        	} else  
+        	{       
+                	result.setLinear(btVector3(0,0,0));
+        	}
+    	} else 
 	{
 		/// Special routine for calculating the inverse of a spatial inertia matrix
 		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
@@ -1808,6 +1846,7 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link,
 
 void btMultiBody::wakeUp()
 {
+	m_sleepTimer = 0;
     m_awake = true;
 }
 
@@ -1956,7 +1995,11 @@ int	btMultiBody::calculateSerializeBufferSize()	const
 const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
 {
 		btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
-		getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
+		getBasePos().serialize(mbd->m_baseWorldPosition);
+		getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
+		getBaseVel().serialize(mbd->m_baseLinearVelocity);
+		getBaseOmega().serialize(mbd->m_baseAngularVelocity);
+
 		mbd->m_baseMass = this->getBaseMass();
 		getBaseInertia().serialize(mbd->m_baseInertia);
 		{
@@ -1982,6 +2025,12 @@ const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
 				memPtr->m_posVarCount = getLink(i).m_posVarCount;
 				
 				getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
+
+				getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);
+				getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);
+				getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);
+				getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);
+
 				memPtr->m_linkMass = getLink(i).m_mass;
 				memPtr->m_parentIndex = getLink(i).m_parent;
 				memPtr->m_jointDamping = getLink(i).m_jointDamping;
@@ -1991,7 +2040,7 @@ const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
 				memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
 				memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
 
-				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
+				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
 				getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
 				getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
 				btAssert(memPtr->m_dofCount<=3);

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h

@@ -702,15 +702,18 @@ private:
 	int	m_companionId;
 	btScalar	m_linearDamping;
 	btScalar	m_angularDamping;
-	bool	m_useGyroTerm;
+	bool		m_useGyroTerm;
 	btScalar	m_maxAppliedImpulse;
 	btScalar	m_maxCoordinateVelocity;
 	bool		m_hasSelfCollision;
 	
-		bool __posUpdated;
-		int m_dofCount, m_posVarCnt;
+	bool __posUpdated;
+	int m_dofCount, m_posVarCnt;
+
 	bool m_useRK4, m_useGlobalVelocities;
-	
+	//for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis
+	//https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing
+
 	///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
 	bool m_internalNeedsJointFeedback;
 };
@@ -718,12 +721,17 @@ private:
 struct btMultiBodyLinkDoubleData
 {
 	btQuaternionDoubleData	m_zeroRotParentToThis;
-	btVector3DoubleData		m_parentComToThisComOffset;
+	btVector3DoubleData		m_parentComToThisPivotOffset;
 	btVector3DoubleData		m_thisPivotToThisComOffset;
 	btVector3DoubleData		m_jointAxisTop[6];
 	btVector3DoubleData		m_jointAxisBottom[6];
 
 	btVector3DoubleData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
+	btVector3DoubleData		m_absFrameTotVelocityTop;
+	btVector3DoubleData		m_absFrameTotVelocityBottom;
+	btVector3DoubleData		m_absFrameLocVelocityTop;
+	btVector3DoubleData		m_absFrameLocVelocityBottom;
+
 	double					m_linkMass;
 	int						m_parentIndex;
 	int						m_jointType;
@@ -751,11 +759,16 @@ struct btMultiBodyLinkDoubleData
 struct btMultiBodyLinkFloatData
 {
 	btQuaternionFloatData	m_zeroRotParentToThis;
-	btVector3FloatData		m_parentComToThisComOffset;
+	btVector3FloatData		m_parentComToThisPivotOffset;
 	btVector3FloatData		m_thisPivotToThisComOffset;
 	btVector3FloatData		m_jointAxisTop[6];
 	btVector3FloatData		m_jointAxisBottom[6];
-	btVector3FloatData	m_linkInertia;   // inertia of the base (in local frame; diagonal)
+	btVector3FloatData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
+	btVector3FloatData		m_absFrameTotVelocityTop;
+	btVector3FloatData		m_absFrameTotVelocityBottom;
+	btVector3FloatData		m_absFrameLocVelocityTop;
+	btVector3FloatData		m_absFrameLocVelocityBottom;
+
 	int						m_dofCount;
 	float				m_linkMass;
 	int					m_parentIndex;
@@ -784,29 +797,38 @@ struct btMultiBodyLinkFloatData
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btMultiBodyDoubleData
 {
-	btTransformDoubleData m_baseWorldTransform;
+	btVector3DoubleData m_baseWorldPosition;
+	btQuaternionDoubleData m_baseWorldOrientation;
+	btVector3DoubleData m_baseLinearVelocity;
+	btVector3DoubleData m_baseAngularVelocity;
 	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
 	double	m_baseMass;
+	int		m_numLinks;
+	char	m_padding[4];
 
 	char	*m_baseName;
 	btMultiBodyLinkDoubleData	*m_links;
 	btCollisionObjectDoubleData	*m_baseCollider;
-	char	*m_paddingPtr;
-	int		m_numLinks;
-	char	m_padding[4];
+	
+	
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btMultiBodyFloatData
 {
-	char	*m_baseName;
-	btMultiBodyLinkFloatData	*m_links;
-	btCollisionObjectFloatData	*m_baseCollider;
-	btTransformFloatData m_baseWorldTransform;
+	btVector3FloatData m_baseWorldPosition;
+	btQuaternionFloatData m_baseWorldOrientation;
+	btVector3FloatData m_baseLinearVelocity;
+	btVector3FloatData m_baseAngularVelocity;
+
 	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)
-	
 	float	m_baseMass;
 	int		m_numLinks;
+
+	char	*m_baseName;
+	btMultiBodyLinkFloatData	*m_links;
+	btCollisionObjectFloatData	*m_baseCollider;
+
 };
 
 

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -253,7 +253,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
         {
             vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
             if (angConstraint) {
-                denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec);
+				denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
             }
             else {
                 denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
@@ -277,7 +277,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
         {
             vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
             if (angConstraint) {
-                denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec);
+				denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
             }
             else {
                 denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
@@ -315,7 +315,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
         }
         else if(rb0)
         {
-            rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+			rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
+			rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
         }
         if (multiBodyB)
         {
@@ -327,7 +328,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
         }
         else if(rb1)
         {
-            rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+			rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
+			rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
         }
         
         solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -119,6 +119,14 @@ public:
 		return m_bodyB;
 	}
 
+	int getLinkA() const
+	{
+		return m_linkA;
+	}
+	int getLinkB() const
+	{
+		return m_linkB;
+	}
 	void	internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
 	{
 		btAssert(dof>=0);

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -39,7 +39,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
 		
 		btScalar residual = resolveSingleConstraintRowGeneric(constraint);
-		leastSquaredResidual += residual*residual;
+		leastSquaredResidual = btMax(leastSquaredResidual,residual*residual);
 
 		if(constraint.m_multiBodyA) 
 			constraint.m_multiBodyA->setPosUpdated(false);
@@ -60,36 +60,101 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 			residual = resolveSingleConstraintRowGeneric(constraint);
 		}
 
-		leastSquaredResidual += residual*residual;
+		leastSquaredResidual = btMax(leastSquaredResidual,residual*residual);
  
 		if(constraint.m_multiBodyA) 
 			constraint.m_multiBodyA->setPosUpdated(false);
 		if(constraint.m_multiBodyB) 
 			constraint.m_multiBodyB->setPosUpdated(false);
 	}
-	
-	//solve featherstone frictional contact
 
-	for (int j1=0;j1<this->m_multiBodyFrictionContactConstraints.size();j1++)
+	//solve featherstone frictional contact
+	if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
 	{
-		if (iteration < infoGlobal.m_numIterations)
+		for (int j1 = 0; j1<this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;//iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				//adjust friction limits here
+				if (totalImpulse>btScalar(0))
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
+					btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+					leastSquaredResidual = btMax(leastSquaredResidual , residual*residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
+
+		for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
 		{
-			int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
+
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				j1++;
+				int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+				btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2];
+				btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
+
+				if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
+					frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse);
+					frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse;
+					btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual*residual);
+					
+					if (frictionConstraintB.m_multiBodyA)
+						frictionConstraintB.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraintB.m_multiBodyB)
+						frictionConstraintB.m_multiBodyB->setPosUpdated(false);
+				
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
 
-			btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
-			btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
-			//adjust friction limits here
-			if (totalImpulse>btScalar(0))
+		
+	}
+	else
+	{
+		for (int j1 = 0; j1<this->m_multiBodyFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
 			{
-				frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
-				frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
-				btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
-				leastSquaredResidual += residual*residual;
-
-				if(frictionConstraint.m_multiBodyA) 
-					frictionConstraint.m_multiBodyA->setPosUpdated(false);
-				if(frictionConstraint.m_multiBodyB) 
-					frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				//adjust friction limits here
+				if (totalImpulse>btScalar(0))
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
+					btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual*residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
 			}
 		}
 	}
@@ -101,6 +166,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
 	m_multiBodyNonContactConstraints.resize(0);
 	m_multiBodyNormalContactConstraints.resize(0);
 	m_multiBodyFrictionContactConstraints.resize(0);
+	m_multiBodyTorsionalFrictionContactConstraints.resize(0);
+	
 	m_data.m_jacobians.resize(0);
 	m_data.m_deltaVelocitiesUnitImpulse.resize(0);
 	m_data.m_deltaVelocities.resize(0);
@@ -128,82 +195,267 @@ void	btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar im
 btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
 {
 
-	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	btScalar deltaVelADotn=0;
-	btScalar deltaVelBDotn=0;
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
+	btScalar deltaVelADotn = 0;
+	btScalar deltaVelBDotn = 0;
 	btSolverBody* bodyA = 0;
 	btSolverBody* bodyB = 0;
-	int ndofA=0;
-	int ndofB=0;
+	int ndofA = 0;
+	int ndofB = 0;
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = c.m_multiBodyA->getNumDofs() + 6;
-		for (int i = 0; i < ndofA; ++i) 
-			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
-	} else if(c.m_solverBodyIdA >= 0)
+		ndofA = c.m_multiBodyA->getNumDofs() + 6;
+		for (int i = 0; i < ndofA; ++i)
+			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
+	}
+	else if (c.m_solverBodyIdA >= 0)
 	{
 		bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
-		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
 	}
 
 	if (c.m_multiBodyB)
 	{
-		ndofB  = c.m_multiBodyB->getNumDofs() + 6;
-		for (int i = 0; i < ndofB; ++i) 
-			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
-	} else if(c.m_solverBodyIdB >= 0)
+		ndofB = c.m_multiBodyB->getNumDofs() + 6;
+		for (int i = 0; i < ndofB; ++i)
+			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
+	}
+	else if (c.m_solverBodyIdB >= 0)
 	{
 		bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
-		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 	}
 
-	
-	deltaImpulse	-=	deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
-	deltaImpulse	-=	deltaVelBDotn*c.m_jacDiagABInv;
+
+	deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
+	deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
-	
+
 	if (sum < c.m_lowerLimit)
 	{
-		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_lowerLimit;
 	}
-	else if (sum > c.m_upperLimit) 
+	else if (sum > c.m_upperLimit)
 	{
-		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_upperLimit;
 	}
 	else
 	{
 		c.m_appliedImpulse = sum;
 	}
-
+	
 	if (c.m_multiBodyA)
 	{
-		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
-		c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+		c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	} else if(c.m_solverBodyIdA >= 0)
+	}
+	else if (c.m_solverBodyIdA >= 0)
 	{
-		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
 
 	}
 	if (c.m_multiBodyB)
 	{
-		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
-		c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+		c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	} else if(c.m_solverBodyIdB >= 0)
+	}
+	else if (c.m_solverBodyIdB >= 0)
 	{
-		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 	}
-	return deltaImpulse;
+    btScalar deltaVel =deltaImpulse/c.m_jacDiagABInv;
+	return deltaVel;
+}
+
+
+btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1,const btMultiBodySolverConstraint& cB)
+{
+	int ndofA=0;
+	int ndofB=0;
+	btSolverBody* bodyA = 0;
+	btSolverBody* bodyB = 0;
+	btScalar deltaImpulseB = 0.f;
+	btScalar sumB = 0.f;
+	{
+		deltaImpulseB = cB.m_rhs-btScalar(cB.m_appliedImpulse)*cB.m_cfm;
+		btScalar deltaVelADotn=0;
+		btScalar deltaVelBDotn=0;
+		if (cB.m_multiBodyA)
+		{
+			ndofA  = cB.m_multiBodyA->getNumDofs() + 6;
+			for (int i = 0; i < ndofA; ++i) 
+				deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex+i];
+		} else if(cB.m_solverBodyIdA >= 0)
+		{
+			bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
+			deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+		}
+
+		if (cB.m_multiBodyB)
+		{
+			ndofB  = cB.m_multiBodyB->getNumDofs() + 6;
+			for (int i = 0; i < ndofB; ++i) 
+				deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex+i];
+		} else if(cB.m_solverBodyIdB >= 0)
+		{
+			bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
+			deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+		}
+
+	
+		deltaImpulseB	-=	deltaVelADotn*cB.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
+		deltaImpulseB	-=	deltaVelBDotn*cB.m_jacDiagABInv;
+		sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
+	}
+
+	btScalar deltaImpulseA = 0.f;
+	btScalar sumA = 0.f;
+	const btMultiBodySolverConstraint& cA = cA1;
+	{
+		{
+			deltaImpulseA = cA.m_rhs-btScalar(cA.m_appliedImpulse)*cA.m_cfm;
+			btScalar deltaVelADotn=0;
+			btScalar deltaVelBDotn=0;
+			if (cA.m_multiBodyA)
+			{
+				ndofA  = cA.m_multiBodyA->getNumDofs() + 6;
+				for (int i = 0; i < ndofA; ++i) 
+					deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex+i];
+			} else if(cA.m_solverBodyIdA >= 0)
+			{
+				bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
+				deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+			}
+
+			if (cA.m_multiBodyB)
+			{
+				ndofB  = cA.m_multiBodyB->getNumDofs() + 6;
+				for (int i = 0; i < ndofB; ++i) 
+					deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex+i];
+			} else if(cA.m_solverBodyIdB >= 0)
+			{
+				bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
+				deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+			}
+
+	
+			deltaImpulseA	-=	deltaVelADotn*cA.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
+			deltaImpulseA	-=	deltaVelBDotn*cA.m_jacDiagABInv;
+			sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
+		}
+	}
+
+	if (sumA*sumA+sumB*sumB>=cA.m_lowerLimit*cB.m_lowerLimit)
+	{
+		btScalar angle = btAtan2(sumA,sumB);
+		btScalar sumAclipped = btFabs(cA.m_lowerLimit*btSin(angle));
+		btScalar sumBclipped = btFabs(cB.m_lowerLimit*btCos(angle));
+
+		
+		if (sumA < -sumAclipped)
+		{
+			deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
+			cA.m_appliedImpulse = -sumAclipped;
+		}
+		else if (sumA > sumAclipped)
+		{
+			deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
+			cA.m_appliedImpulse = sumAclipped;
+		}
+		else
+		{
+			cA.m_appliedImpulse = sumA;
+		}
+
+		if (sumB < -sumBclipped)
+		{
+			deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
+			cB.m_appliedImpulse = -sumBclipped;
+		}
+		else if (sumB > sumBclipped)
+		{
+			deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
+			cB.m_appliedImpulse = sumBclipped;
+		}
+		else
+		{
+			cB.m_appliedImpulse = sumB;
+		}
+		//deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
+		//cA.m_appliedImpulse = sumAclipped;
+		//deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
+		//cB.m_appliedImpulse = sumBclipped;
+	} 
+	else
+	{
+		cA.m_appliedImpulse = sumA;
+		cB.m_appliedImpulse = sumB;
+	}
+	
+	if (cA.m_multiBodyA)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA,cA.m_deltaVelAindex,ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA);
+#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	} else if(cA.m_solverBodyIdA >= 0)
+	{
+		bodyA->internalApplyImpulse(cA.m_contactNormal1*bodyA->internalGetInvMass(),cA.m_angularComponentA,deltaImpulseA);
+
+	}
+	if (cA.m_multiBodyB)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA,cA.m_deltaVelBindex,ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA);
+#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	} else if(cA.m_solverBodyIdB >= 0)
+	{
+		bodyB->internalApplyImpulse(cA.m_contactNormal2*bodyB->internalGetInvMass(),cA.m_angularComponentB,deltaImpulseA);
+	}
+
+	if (cB.m_multiBodyA)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB,cB.m_deltaVelAindex,ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB);
+#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	} else if(cB.m_solverBodyIdA >= 0)
+	{
+		bodyA->internalApplyImpulse(cB.m_contactNormal1*bodyA->internalGetInvMass(),cB.m_angularComponentA,deltaImpulseB);
+	}
+	if (cB.m_multiBodyB)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB,cB.m_deltaVelBindex,ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB);
+#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	} else if(cB.m_solverBodyIdB >= 0)
+	{
+		bodyB->internalApplyImpulse(cB.m_contactNormal2*bodyB->internalGetInvMass(),cB.m_angularComponentB,deltaImpulseB);
+	}
+
+    btScalar deltaVel =deltaImpulseA/cA.m_jacDiagABInv+deltaImpulseB/cB.m_jacDiagABInv;
+    return deltaVel;
 }
 
 
@@ -908,7 +1160,10 @@ btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyTorsionalF
                                                                                                      btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
     BT_PROFILE("addMultiBodyRollingFrictionConstraint");
-    btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
+
+	bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0));
+
+    btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing();
     solverConstraint.m_orgConstraint = 0;
     solverConstraint.m_orgDofIndex = -1;
     
@@ -1151,6 +1406,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 
 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {
+	//printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
 	return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
 }
 
@@ -1234,27 +1490,12 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 
 	if (c.m_multiBodyA)
 	{
-		
-		if(c.m_multiBodyA->isMultiDof())
-		{
-			c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
-		else
-		{
-			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
+		c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
 	}
 	
 	if (c.m_multiBodyB)
 	{
-		if(c.m_multiBodyB->isMultiDof())
-		{
-			c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
-		else
-		{
-			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
+		c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
 	}
 #endif
 
@@ -1416,6 +1657,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 
 void  btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {
+	//printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
+
 	//printf("solveMultiBodyGroup start\n");
 	m_tmpMultiBodyConstraints = multiBodyConstraints;
 	m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -36,6 +36,7 @@ protected:
 
 	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
 	btMultiBodyConstraintArray			m_multiBodyFrictionContactConstraints;
+	btMultiBodyConstraintArray			m_multiBodyTorsionalFrictionContactConstraints;
 
 	btMultiBodyJacobianData				m_data;
 	
@@ -45,6 +46,9 @@ protected:
 
 	btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
 	
+	//solve 2 friction directions and clamp against the implicit friction cone
+	btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB);
+	
 
 	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
     

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -277,7 +277,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		m_multiBodyConstraints.resize(0);
 	}
 
-	
+    void    setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
+    {
+        m_solver = solver;
+    }
+    
 	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
 	{
 		if (islandId<0)
@@ -348,7 +352,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 				for (i=0;i<numCurMultiBodyConstraints;i++)
 					m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
 				
-				if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
+				if ((m_multiBodyConstraints.size()+m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
 				{
 					processConstraints();
 				} else
@@ -394,6 +398,22 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
 	delete m_solverMultiBodyIslandCallback;
 }
 
+void    btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
+{
+    m_multiBodyConstraintSolver = solver;
+    m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver);
+    btDiscreteDynamicsWorld::setConstraintSolver(solver);
+}
+
+void    btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+{
+    if (solver->getSolverType()==BT_MULTIBODY_SOLVER)
+    {
+        m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver;
+    }
+    btDiscreteDynamicsWorld::setConstraintSolver(solver);
+}
+
 void	btMultiBodyDynamicsWorld::forwardKinematics()
 {
 
@@ -411,6 +431,8 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 
 	BT_PROFILE("solveConstraints");
 	
+	clearMultiBodyConstraintForces();
+
 	m_sortedConstraints.resize( m_constraints.size());
 	int i; 
 	for (i=0;i<getNumConstraints();i++)
@@ -433,8 +455,6 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 	m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 	
-	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
 
 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 	{
@@ -669,7 +689,9 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		}
 	}
 
-	clearMultiBodyConstraintForces();
+	/// solve all the constraints for this island
+	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+
 
 	m_solverMultiBodyIslandCallback->processConstraints();
 	
@@ -824,21 +846,24 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 			{
 				btMultiBody* bod = m_multiBodies[b];
 				bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1);
-				
-				getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
-
+		
+				if (mode  & btIDebugDraw::DBG_DrawFrames)
+				{
+					getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
+				}
 
 				for (int m = 0; m<bod->getNumLinks(); m++)
 				{
 					
 					const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
-
-					getDebugDrawer()->drawTransform(tr, 0.1);
-
+					if (mode  & btIDebugDraw::DBG_DrawFrames)
+					{
+						getDebugDrawer()->drawTransform(tr, 0.1);
+					}
 						//draw the joint axis
 					if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec)*0.1;
 					
 						btVector4 color(0,0,0,1);//1,1,1);
 						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@@ -847,7 +872,7 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 					}
 					if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec)*0.1;
 					
 						btVector4 color(0,0,0,1);//1,1,1);
 						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@@ -856,7 +881,7 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 					}
 					if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec)*0.1;
 					
 						btVector4 color(0,0,0,1);//1,1,1);
 						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@@ -970,6 +995,8 @@ void	btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeCollisionObjects(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 
@@ -988,4 +1015,17 @@ void	btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
 		}
 	}
 
-}
+	//serialize all multibody links (collision objects)
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+		{
+			int len = colObj->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len,1);
+			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk,structType,BT_MB_LINKCOLLIDER_CODE,colObj);
+		}
+	}
+
+}

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -109,6 +109,8 @@ public:
 	virtual void applyGravity();
 	
 	virtual	void	serialize(btSerializer* serializer);
+	virtual void	setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
+    virtual void    setConstraintSolver(btConstraintSolver* solver);
 
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp

@@ -65,13 +65,16 @@ int btMultiBodyFixedConstraint::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -83,16 +86,17 @@ int btMultiBodyFixedConstraint::getIslandIdB() const
 		return m_rigidBodyB->getIslandTag();
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp

@@ -45,16 +45,18 @@ btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
 
 int btMultiBodyGearConstraint::getIslandIdA() const
 {
-
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -64,16 +66,17 @@ int btMultiBodyGearConstraint::getIslandIdB() const
 {
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
@@ -134,6 +137,10 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
 		if (m_erp!=0)
 		{
 			btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+			if (m_gearAuxLink >= 0)
+			{
+				currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
+			}
 			btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
 			btScalar diff = currentPositionB+currentPositionA;
 			btScalar desiredPositionDiff = this->m_relativePositionTarget;

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -53,17 +53,22 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 {
 }
 
+
 int btMultiBodyJointLimitConstraint::getIslandIdA() const
 {
-	if(m_bodyA)
+
+	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -71,18 +76,19 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const
 
 int btMultiBodyJointLimitConstraint::getIslandIdB() const
 {
-	if(m_bodyB)
+	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -74,29 +74,37 @@ btMultiBodyJointMotor::~btMultiBodyJointMotor()
 
 int btMultiBodyJointMotor::getIslandIdA() const
 {
-	btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-	for (int i=0;i<m_bodyA->getNumLinks();i++)
+	if (this->m_linkA < 0)
 	{
-		if (m_bodyA->getLink(i).m_collider)
-			return m_bodyA->getLink(i).m_collider->getIslandTag();
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
 
 int btMultiBodyJointMotor::getIslandIdB() const
 {
-	btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-
-	for (int i=0;i<m_bodyB->getNumLinks();i++)
+	if (m_linkB < 0)
 	{
-		col = m_bodyB->getLink(i).m_collider;
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 		if (col)
 			return col->getIslandTag();
 	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
 	return -1;
 }
 

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -182,6 +182,8 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 		m_cachedRVector.setValue(0, 0, 0);
 		m_appliedForce.setValue( 0, 0, 0);
 		m_appliedTorque.setValue(0, 0, 0);
+		m_appliedConstraintForce.setValue(0,0,0);
+		m_appliedConstraintTorque.setValue(0,0,0);
 		//		
 		m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
 		m_jointPos[3] = 1.f;			//"quat.w"

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h

@@ -19,6 +19,16 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 #include "btMultiBody.h"
+#include "LinearMath/btSerializer.h"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData"
+#else
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData"
+#endif
+
 
 class btMultiBodyLinkCollider : public btCollisionObject
 {
@@ -119,7 +129,49 @@ public:
 		}
 		return true;
 	}
+
+	virtual	int	calculateSerializeBufferSize()	const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
+
+};
+
+
+struct	btMultiBodyLinkColliderFloatData
+{
+	btCollisionObjectFloatData m_colObjData;
+	btMultiBodyFloatData	*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
 };
 
+struct	btMultiBodyLinkColliderDoubleData
+{
+	btCollisionObjectDoubleData m_colObjData;
+	btMultiBodyDoubleData		*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
+};
+
+SIMD_FORCE_INLINE	int	btMultiBodyLinkCollider::calculateSerializeBufferSize() const
+{
+	return sizeof(btMultiBodyLinkColliderData);
+}
+
+SIMD_FORCE_INLINE	const char*	btMultiBodyLinkCollider::serialize(void* dataBuffer,  class btSerializer* serializer) const
+{
+	btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer;
+	btCollisionObject::serialize(&dataOut->m_colObjData,serializer);
+	
+	dataOut->m_link = this->m_link;
+	dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody);
+
+	// Fill padding with zeros to appease msan.
+	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
+
+	return btMultiBodyLinkColliderDataName;
+}
+
 #endif //BT_FEATHERSTONE_LINK_COLLIDER_H
 

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp

@@ -0,0 +1,966 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
+
+#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+
+static bool interleaveContactAndFriction = false;
+
+struct btJointNode
+{
+	int jointIndex;          // pointer to enclosing dxJoint object
+	int otherBodyIndex;      // *other* body this joint is connected to
+	int nextJointNodeIndex;  //-1 for null
+	int constraintRowIndex;
+};
+
+// Helper function to compute a delta velocity in the constraint space.
+static btScalar computeDeltaVelocityInConstraintSpace(
+	const btVector3& angularDeltaVelocity,
+	const btVector3& contactNormal,
+	btScalar invMass,
+	const btVector3& angularJacobian,
+	const btVector3& linearJacobian)
+{
+	return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass;
+}
+
+// Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are
+// identical.
+static btScalar computeDeltaVelocityInConstraintSpace(
+	const btVector3& angularDeltaVelocity,
+	btScalar invMass,
+	const btVector3& angularJacobian)
+{
+	return angularDeltaVelocity.dot(angularJacobian) + invMass;
+}
+
+// Helper function to compute a delta velocity in the constraint space.
+static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size)
+{
+	btScalar result = 0;
+	for (int i = 0; i < size; ++i)
+		result += deltaVelocity[i] * jacobian[i];
+
+	return result;
+}
+
+static btScalar computeConstraintMatrixDiagElementMultiBody(
+	const btAlignedObjectArray<btSolverBody>& solverBodyPool,
+	const btMultiBodyJacobianData& data,
+	const btMultiBodySolverConstraint& constraint)
+{
+	btScalar ret = 0;
+
+	const btMultiBody* multiBodyA = constraint.m_multiBodyA;
+	const btMultiBody* multiBodyB = constraint.m_multiBodyB;
+
+	if (multiBodyA)
+	{
+		const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex];
+		const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+		const int ndofA = multiBodyA->getNumDofs() + 6;
+		ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA);
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		btAssert(solverBodyIdA != -1);
+		const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+		const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+		ret += computeDeltaVelocityInConstraintSpace(
+			constraint.m_relpos1CrossNormal,
+			invMassA,
+			constraint.m_angularComponentA);
+	}
+
+	if (multiBodyB)
+	{
+		const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex];
+		const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+		const int ndofB = multiBodyB->getNumDofs() + 6;
+		ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB);
+	}
+	else
+	{
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+		btAssert(solverBodyIdB != -1);
+		const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+		const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+		ret += computeDeltaVelocityInConstraintSpace(
+			constraint.m_relpos2CrossNormal,
+			invMassB,
+			constraint.m_angularComponentB);
+	}
+
+	return ret;
+}
+
+static btScalar computeConstraintMatrixOffDiagElementMultiBody(
+	const btAlignedObjectArray<btSolverBody>& solverBodyPool,
+	const btMultiBodyJacobianData& data,
+	const btMultiBodySolverConstraint& constraint,
+	const btMultiBodySolverConstraint& offDiagConstraint)
+{
+	btScalar offDiagA = btScalar(0);
+
+	const btMultiBody* multiBodyA = constraint.m_multiBodyA;
+	const btMultiBody* multiBodyB = constraint.m_multiBodyB;
+	const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA;
+	const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB;
+
+	// Assumed at least one system is multibody
+	btAssert(multiBodyA || multiBodyB);
+	btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
+
+	if (offDiagMultiBodyA)
+	{
+		const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex];
+
+		if (offDiagMultiBodyA == multiBodyA)
+		{
+			const int ndofA = multiBodyA->getNumDofs() + 6;
+			const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA);
+		}
+		else if (offDiagMultiBodyA == multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB);
+		}
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+
+		const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA;
+		btAssert(offDiagSolverBodyIdA != -1);
+
+		if (offDiagSolverBodyIdA == solverBodyIdA)
+		{
+			btAssert(solverBodyIdA != -1);
+			const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+			const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos1CrossNormal,
+				offDiagConstraint.m_contactNormal1,
+				invMassA, constraint.m_angularComponentA,
+				constraint.m_contactNormal1);
+		}
+		else if (offDiagSolverBodyIdA == solverBodyIdB)
+		{
+			btAssert(solverBodyIdB != -1);
+			const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+			const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos1CrossNormal,
+				offDiagConstraint.m_contactNormal1,
+				invMassB,
+				constraint.m_angularComponentB,
+				constraint.m_contactNormal2);
+		}
+	}
+
+	if (offDiagMultiBodyB)
+	{
+		const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex];
+
+		if (offDiagMultiBodyB == multiBodyA)
+		{
+			const int ndofA = multiBodyA->getNumDofs() + 6;
+			const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA);
+		}
+		else if (offDiagMultiBodyB == multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB);
+		}
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+
+		const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB;
+		btAssert(offDiagSolverBodyIdB != -1);
+
+		if (offDiagSolverBodyIdB == solverBodyIdA)
+		{
+			btAssert(solverBodyIdA != -1);
+			const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+			const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos2CrossNormal,
+				offDiagConstraint.m_contactNormal2,
+				invMassA, constraint.m_angularComponentA,
+				constraint.m_contactNormal1);
+		}
+		else if (offDiagSolverBodyIdB == solverBodyIdB)
+		{
+			btAssert(solverBodyIdB != -1);
+			const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+			const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos2CrossNormal,
+				offDiagConstraint.m_contactNormal2,
+				invMassB, constraint.m_angularComponentB,
+				constraint.m_contactNormal2);
+		}
+	}
+
+	return offDiagA;
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
+{
+	createMLCPFastRigidBody(infoGlobal);
+	createMLCPFastMultiBody(infoGlobal);
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
+{
+	int numContactRows = interleaveContactAndFriction ? 3 : 1;
+
+	int numConstraintRows = m_allConstraintPtrArray.size();
+
+	if (numConstraintRows == 0)
+		return;
+
+	int n = numConstraintRows;
+	{
+		BT_PROFILE("init b (rhs)");
+		m_b.resize(numConstraintRows);
+		m_bSplit.resize(numConstraintRows);
+		m_b.setZero();
+		m_bSplit.setZero();
+		for (int i = 0; i < numConstraintRows; i++)
+		{
+			btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			if (!btFuzzyZero(jacDiag))
+			{
+				btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
+				btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
+				m_b[i] = rhs / jacDiag;
+				m_bSplit[i] = rhsPenetration / jacDiag;
+			}
+		}
+	}
+
+	//	btScalar* w = 0;
+	//	int nub = 0;
+
+	m_lo.resize(numConstraintRows);
+	m_hi.resize(numConstraintRows);
+
+	{
+		BT_PROFILE("init lo/ho");
+
+		for (int i = 0; i < numConstraintRows; i++)
+		{
+			if (0)  //m_limitDependencies[i]>=0)
+			{
+				m_lo[i] = -BT_INFINITY;
+				m_hi[i] = BT_INFINITY;
+			}
+			else
+			{
+				m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+				m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
+			}
+		}
+	}
+
+	//
+	int m = m_allConstraintPtrArray.size();
+
+	int numBodies = m_tmpSolverBodyPool.size();
+	btAlignedObjectArray<int> bodyJointNodeArray;
+	{
+		BT_PROFILE("bodyJointNodeArray.resize");
+		bodyJointNodeArray.resize(numBodies, -1);
+	}
+	btAlignedObjectArray<btJointNode> jointNodeArray;
+	{
+		BT_PROFILE("jointNodeArray.reserve");
+		jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
+	}
+
+	btMatrixXu& J3 = m_scratchJ3;
+	{
+		BT_PROFILE("J3.resize");
+		J3.resize(2 * m, 8);
+	}
+	btMatrixXu& JinvM3 = m_scratchJInvM3;
+	{
+		BT_PROFILE("JinvM3.resize/setZero");
+
+		JinvM3.resize(2 * m, 8);
+		JinvM3.setZero();
+		J3.setZero();
+	}
+	int cur = 0;
+	int rowOffset = 0;
+	btAlignedObjectArray<int>& ofs = m_scratchOfs;
+	{
+		BT_PROFILE("ofs resize");
+		ofs.resize(0);
+		ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
+	}
+	{
+		BT_PROFILE("Compute J and JinvM");
+		int c = 0;
+
+		int numRows = 0;
+
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
+		{
+			ofs[c] = rowOffset;
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
+			if (orgBodyA)
+			{
+				{
+					int slotA = -1;
+					//find free jointNode slot for sbA
+					slotA = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbA];
+					bodyJointNodeArray[sbA] = slotA;
+					jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotA].jointIndex = c;
+					jointNodeArray[slotA].constraintRowIndex = i;
+					jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
+				}
+				for (int row = 0; row < numRows; row++, cur++)
+				{
+					btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
+					btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
+
+					for (int r = 0; r < 3; r++)
+					{
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMass[r]);
+						JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
+					}
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			if (orgBodyB)
+			{
+				{
+					int slotB = -1;
+					//find free jointNode slot for sbA
+					slotB = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbB];
+					bodyJointNodeArray[sbB] = slotB;
+					jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotB].jointIndex = c;
+					jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
+					jointNodeArray[slotB].constraintRowIndex = i;
+				}
+
+				for (int row = 0; row < numRows; row++, cur++)
+				{
+					btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
+					btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
+
+					for (int r = 0; r < 3; r++)
+					{
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMassB[r]);
+						JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
+					}
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			rowOffset += numRows;
+		}
+	}
+
+	//compute JinvM = J*invM.
+	const btScalar* JinvM = JinvM3.getBufferPointer();
+
+	const btScalar* Jptr = J3.getBufferPointer();
+	{
+		BT_PROFILE("m_A.resize");
+		m_A.resize(n, n);
+	}
+
+	{
+		BT_PROFILE("m_A.setZero");
+		m_A.setZero();
+	}
+	int c = 0;
+	{
+		int numRows = 0;
+		BT_PROFILE("Compute A");
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
+		{
+			int row__ = ofs[c];
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+			//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
+
+			const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+
+			{
+				int startJointNodeA = bodyJointNodeArray[sbA];
+				while (startJointNodeA >= 0)
+				{
+					int j0 = jointNodeArray[startJointNodeA].jointIndex;
+					int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
+					if (j0 < c)
+					{
+						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
+						//printf("%d joint i %d and j0: %d: ",count++,i,j0);
+						m_A.multiplyAdd2_p8r(JinvMrow,
+											 Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
+					}
+					startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
+				}
+			}
+
+			{
+				int startJointNodeB = bodyJointNodeArray[sbB];
+				while (startJointNodeB >= 0)
+				{
+					int j1 = jointNodeArray[startJointNodeB].jointIndex;
+					int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
+
+					if (j1 < c)
+					{
+						int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
+						m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
+											 Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
+					}
+					startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
+				}
+			}
+		}
+
+		{
+			BT_PROFILE("compute diagonal");
+			// compute diagonal blocks of m_A
+
+			int row__ = 0;
+			int numJointRows = m_allConstraintPtrArray.size();
+
+			int jj = 0;
+			for (; row__ < numJointRows;)
+			{
+				//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
+				int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
+				//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+				const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
+
+				const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+				const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
+				m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
+				if (orgBodyB)
+				{
+					m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
+				}
+				row__ += infom;
+				jj++;
+			}
+		}
+	}
+
+	if (1)
+	{
+		// add cfm to the diagonal of m_A
+		for (int i = 0; i < m_A.rows(); ++i)
+		{
+			m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+		}
+	}
+
+	///fill the upper triangle of the matrix, to make it symmetric
+	{
+		BT_PROFILE("fill the upper triangle ");
+		m_A.copyLowerToUpperTriangle();
+	}
+
+	{
+		BT_PROFILE("resize/init x");
+		m_x.resize(numConstraintRows);
+		m_xSplit.resize(numConstraintRows);
+
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		{
+			for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
+			{
+				const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+				m_x[i] = c.m_appliedImpulse;
+				m_xSplit[i] = c.m_appliedPushImpulse;
+			}
+		}
+		else
+		{
+			m_x.setZero();
+			m_xSplit.setZero();
+		}
+	}
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal)
+{
+	const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size();
+
+	if (multiBodyNumConstraints == 0)
+		return;
+
+	// 1. Compute b
+	{
+		BT_PROFILE("init b (rhs)");
+
+		m_multiBodyB.resize(multiBodyNumConstraints);
+		m_multiBodyB.setZero();
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			const btScalar jacDiag = constraint.m_jacDiagABInv;
+
+			if (!btFuzzyZero(jacDiag))
+			{
+				// Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet.
+				const btScalar rhs = constraint.m_rhs;
+				m_multiBodyB[i] = rhs / jacDiag;
+			}
+		}
+	}
+
+	// 2. Compute lo and hi
+	{
+		BT_PROFILE("init lo/ho");
+
+		m_multiBodyLo.resize(multiBodyNumConstraints);
+		m_multiBodyHi.resize(multiBodyNumConstraints);
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			m_multiBodyLo[i] = constraint.m_lowerLimit;
+			m_multiBodyHi[i] = constraint.m_upperLimit;
+		}
+	}
+
+	// 3. Construct A matrix by using the impulse testing
+	{
+		BT_PROFILE("Compute A");
+
+		{
+			BT_PROFILE("m_A.resize");
+			m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
+		}
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			// Compute the diagonal of A, which is A(i, i)
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint);
+			m_multiBodyA.setElem(i, i, diagA);
+
+			// Computes the off-diagonals of A:
+			//   a. The rest of i-th row of A, from A(i, i+1) to A(i, n)
+			//   b. The rest of i-th column of A, from A(i+1, i) to A(n, i)
+			for (int j = i + 1; j < multiBodyNumConstraints; ++j)
+			{
+				const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j];
+				const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint);
+
+				// Set the off-diagonal values of A. Note that A is symmetric.
+				m_multiBodyA.setElem(i, j, offDiagA);
+				m_multiBodyA.setElem(j, i, offDiagA);
+			}
+		}
+	}
+
+	// Add CFM to the diagonal of m_A
+	for (int i = 0; i < m_multiBodyA.rows(); ++i)
+	{
+		m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+	}
+
+	// 4. Initialize x
+	{
+		BT_PROFILE("resize/init x");
+
+		m_multiBodyX.resize(multiBodyNumConstraints);
+
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		{
+			for (int i = 0; i < multiBodyNumConstraints; ++i)
+			{
+				const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+				m_multiBodyX[i] = constraint.m_appliedImpulse;
+			}
+		}
+		else
+		{
+			m_multiBodyX.setZero();
+		}
+	}
+}
+
+bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
+{
+	bool result = true;
+
+	if (m_A.rows() != 0)
+	{
+		// If using split impulse, we solve 2 separate (M)LCPs
+		if (infoGlobal.m_splitImpulse)
+		{
+			const btMatrixXu Acopy = m_A;
+			const btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
+			// TODO(JS): Do we really need these copies when solveMLCP takes them as const?
+
+			result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
+			if (result)
+				result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
+		}
+		else
+		{
+			result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
+		}
+	}
+
+	if (!result)
+		return false;
+
+	if (m_multiBodyA.rows() != 0)
+	{
+		result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations);
+	}
+
+	return result;
+}
+
+btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
+	btCollisionObject** bodies,
+	int numBodies,
+	btPersistentManifold** manifoldPtr,
+	int numManifolds,
+	btTypedConstraint** constraints,
+	int numConstraints,
+	const btContactSolverInfo& infoGlobal,
+	btIDebugDraw* debugDrawer)
+{
+	// 1. Setup for rigid-bodies
+	btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(
+		bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+
+	// 2. Setup for multi-bodies
+	//   a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray
+	//   b. Set the index array for frictional contact constraints, m_limitDependencies
+	{
+		BT_PROFILE("gather constraint data");
+
+		int dindex = 0;
+
+		const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size();
+		const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
+
+		m_allConstraintPtrArray.resize(0);
+		m_multiBodyAllConstraintPtrArray.resize(0);
+
+		// i. Setup for rigid bodies
+
+		m_limitDependencies.resize(numRigidBodyConstraints);
+
+		for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i)
+		{
+			m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
+			m_limitDependencies[dindex++] = -1;
+		}
+
+		int firstContactConstraintOffset = dindex;
+
+		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		if (interleaveContactAndFriction)
+		{
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
+			{
+				const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
+
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
+				int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
+				m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				if (numFrictionPerContact == 2)
+				{
+					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
+					m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				}
+			}
+		}
+		else
+		{
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+			}
+			for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
+			}
+		}
+
+		if (!m_allConstraintPtrArray.size())
+		{
+			m_A.resize(0, 0);
+			m_b.resize(0);
+			m_x.resize(0);
+			m_lo.resize(0);
+			m_hi.resize(0);
+		}
+
+		// ii. Setup for multibodies
+
+		dindex = 0;
+
+		m_multiBodyLimitDependencies.resize(numMultiBodyConstraints);
+
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
+		{
+			m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]);
+			m_multiBodyLimitDependencies[dindex++] = -1;
+		}
+
+		firstContactConstraintOffset = dindex;
+
+		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		if (interleaveContactAndFriction)
+		{
+			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
+			{
+				const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2;
+
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = -1;
+
+				btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact];
+				m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1);
+
+				const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
+
+				m_multiBodyLimitDependencies[dindex++] = findex;
+
+				if (numtiBodyNumFrictionPerContact == 2)
+				{
+					btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1];
+					m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2);
+
+					m_multiBodyLimitDependencies[dindex++] = findex;
+				}
+			}
+		}
+		else
+		{
+			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
+			{
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = -1;
+			}
+			for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
+			{
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset;
+			}
+		}
+
+		if (!m_multiBodyAllConstraintPtrArray.size())
+		{
+			m_multiBodyA.resize(0, 0);
+			m_multiBodyB.resize(0);
+			m_multiBodyX.resize(0);
+			m_multiBodyLo.resize(0);
+			m_multiBodyHi.resize(0);
+		}
+	}
+
+	// Construct MLCP terms
+	{
+		BT_PROFILE("createMLCPFast");
+		createMLCPFast(infoGlobal);
+	}
+
+	return btScalar(0);
+}
+
+btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	bool result = true;
+	{
+		BT_PROFILE("solveMLCP");
+		result = solveMLCP(infoGlobal);
+	}
+
+	// Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid.
+	if (!result)
+	{
+		m_fallback++;
+		return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	}
+
+	{
+		BT_PROFILE("process MLCP results");
+
+		for (int i = 0; i < m_allConstraintPtrArray.size(); ++i)
+		{
+			const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+
+			const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
+			c.m_appliedImpulse = m_x[i];
+
+			int sbA = c.m_solverBodyIdA;
+			int sbB = c.m_solverBodyIdB;
+
+			btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+			btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+
+			solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+			solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+
+			if (infoGlobal.m_splitImpulse)
+			{
+				const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
+				solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse);
+				solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse);
+				c.m_appliedPushImpulse = m_xSplit[i];
+			}
+		}
+
+		for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i)
+		{
+			btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i];
+
+			const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse;
+			c.m_appliedImpulse = m_multiBodyX[i];
+
+			btMultiBody* multiBodyA = c.m_multiBodyA;
+			if (multiBodyA)
+			{
+				const int ndofA = multiBodyA->getNumDofs() + 6;
+				applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+				//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+				//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+				multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
+#endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+			}
+			else
+			{
+				const int sbA = c.m_solverBodyIdA;
+				btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+				solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+			}
+
+			btMultiBody* multiBodyB = c.m_multiBodyB;
+			if (multiBodyB)
+			{
+				const int ndofB = multiBodyB->getNumDofs() + 6;
+				applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+				//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+				//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+				multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
+#endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+			}
+			else
+			{
+				const int sbB = c.m_solverBodyIdB;
+				btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+				solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+			}
+		}
+	}
+
+	return btScalar(0);
+}
+
+btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver)
+	: m_solver(solver), m_fallback(0)
+{
+	// Do nothing
+}
+
+btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver()
+{
+	// Do nothing
+}
+
+void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver)
+{
+	m_solver = solver;
+}
+
+int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const
+{
+	return m_fallback;
+}
+
+void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num)
+{
+	m_fallback = num;
+}
+
+btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const
+{
+	return BT_MLCP_SOLVER;
+}

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h

@@ -0,0 +1,187 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
+#define BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
+
+#include "LinearMath/btMatrixX.h"
+#include "LinearMath/btThreads.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+class btMLCPSolverInterface;
+class btMultiBody;
+
+class btMultiBodyMLCPConstraintSolver : public btMultiBodyConstraintSolver
+{
+protected:
+	/// \name MLCP Formulation for Rigid Bodies
+	/// \{
+
+	/// A matrix in the MLCP formulation
+	btMatrixXu m_A;
+
+	/// b vector in the MLCP formulation.
+	btVectorXu m_b;
+
+	/// Constraint impulse, which is an output of MLCP solving.
+	btVectorXu m_x;
+
+	/// Lower bound of constraint impulse, \c m_x.
+	btVectorXu m_lo;
+
+	/// Upper bound of constraint impulse, \c m_x.
+	btVectorXu m_hi;
+
+	/// \}
+
+	/// \name Cache Variables for Split Impulse for Rigid Bodies
+	/// When using 'split impulse' we solve two separate (M)LCPs
+	/// \{
+
+	/// Split impulse Cache vector corresponding to \c m_b.
+	btVectorXu m_bSplit;
+
+	/// Split impulse cache vector corresponding to \c m_x.
+	btVectorXu m_xSplit;
+
+	/// \}
+
+	/// \name MLCP Formulation for Multibodies
+	/// \{
+
+	/// A matrix in the MLCP formulation
+	btMatrixXu m_multiBodyA;
+
+	/// b vector in the MLCP formulation.
+	btVectorXu m_multiBodyB;
+
+	/// Constraint impulse, which is an output of MLCP solving.
+	btVectorXu m_multiBodyX;
+
+	/// Lower bound of constraint impulse, \c m_x.
+	btVectorXu m_multiBodyLo;
+
+	/// Upper bound of constraint impulse, \c m_x.
+	btVectorXu m_multiBodyHi;
+
+	/// \}
+
+	/// Indices of normal contact constraint associated with frictional contact constraint for rigid bodies.
+	///
+	/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
+	/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
+	/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
+	/// Otherwise, -1.
+	btAlignedObjectArray<int> m_limitDependencies;
+
+	/// Indices of normal contact constraint associated with frictional contact constraint for multibodies.
+	///
+	/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
+	/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
+	/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
+	/// Otherwise, -1.
+	btAlignedObjectArray<int> m_multiBodyLimitDependencies;
+
+	/// Array of all the rigid body constraints
+	btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
+
+	/// Array of all the multibody constraints
+	btAlignedObjectArray<btMultiBodySolverConstraint*> m_multiBodyAllConstraintPtrArray;
+
+	/// MLCP solver
+	btMLCPSolverInterface* m_solver;
+
+	/// Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver fails.
+	int m_fallback;
+
+	/// \name MLCP Scratch Variables
+	/// The following scratch variables are not stateful -- contents are cleared prior to each use.
+	/// They are only cached here to avoid extra memory allocations and deallocations and to ensure
+	/// that multiple instances of the solver can be run in parallel.
+	///
+	/// \{
+
+	/// Cache variable for constraint Jacobian matrix.
+	btMatrixXu m_scratchJ3;
+
+	/// Cache variable for constraint Jacobian times inverse mass matrix.
+	btMatrixXu m_scratchJInvM3;
+
+	/// Cache variable for offsets.
+	btAlignedObjectArray<int> m_scratchOfs;
+
+	/// \}
+
+	/// Constructs MLCP terms, which are \c m_A, \c m_b, \c m_lo, and \c m_hi.
+	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
+
+	/// Constructs MLCP terms for constraints of two rigid bodies
+	void createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal);
+
+	/// Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody
+	void createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal);
+
+	/// Solves MLCP and returns the success
+	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
+
+	// Documentation inherited
+	btScalar solveGroupCacheFriendlySetup(
+		btCollisionObject** bodies,
+		int numBodies,
+		btPersistentManifold** manifoldPtr,
+		int numManifolds,
+		btTypedConstraint** constraints,
+		int numConstraints,
+		const btContactSolverInfo& infoGlobal,
+		btIDebugDraw* debugDrawer) BT_OVERRIDE;
+
+	// Documentation inherited
+	btScalar solveGroupCacheFriendlyIterations(
+		btCollisionObject** bodies,
+		int numBodies,
+		btPersistentManifold** manifoldPtr,
+		int numManifolds,
+		btTypedConstraint** constraints,
+		int numConstraints,
+		const btContactSolverInfo& infoGlobal,
+		btIDebugDraw* debugDrawer) BT_OVERRIDE;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR()
+
+	/// Constructor
+	///
+	/// \param[in] solver MLCP solver. Assumed it's not null.
+	/// \param[in] maxLCPSize Maximum size of LCP to solve using MLCP solver. If the MLCP size exceeds this number, sequaltial impulse method will be used.
+	explicit btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver);
+
+	/// Destructor
+	virtual ~btMultiBodyMLCPConstraintSolver();
+
+	/// Sets MLCP solver. Assumed it's not null.
+	void setMLCPSolver(btMLCPSolverInterface* solver);
+
+	/// Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP
+	/// solver fails.
+	int getNumFallbacks() const;
+
+	/// Sets the number of fallbacks. This function may be used to reset the number to zero.
+	void setNumFallbacks(int num);
+
+	/// Returns the constraint solver type.
+	virtual btConstraintSolverType getSolverType() const;
+};
+
+#endif  // BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -64,13 +64,16 @@ int btMultiBodyPoint2Point::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();			
 		}
 	}
 	return -1;
@@ -82,16 +85,17 @@ int btMultiBodyPoint2Point::getIslandIdB() const
 		return m_rigidBodyB->getIslandTag();
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp

@@ -68,13 +68,16 @@ int btMultiBodySliderConstraint::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -86,20 +89,20 @@ int btMultiBodySliderConstraint::getIslandIdB() const
 		return m_rigidBodyB->getIslandTag();
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
-
 void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
 {
     // Convert local points back to world

thirdparty/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp

@@ -121,12 +121,19 @@ void	btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT
 	btQuaternion rotatingOrn(right,-wheel.m_rotation);
 	btMatrix3x3 rotatingMat(rotatingOrn);
 
-	btMatrix3x3 basis2(
-		right[0],fwd[0],up[0],
-		right[1],fwd[1],up[1],
-		right[2],fwd[2],up[2]
-	);
-	
+    btMatrix3x3 basis2;
+    basis2[0][m_indexRightAxis] = -right[0];
+    basis2[1][m_indexRightAxis] = -right[1];
+    basis2[2][m_indexRightAxis] = -right[2];
+
+    basis2[0][m_indexUpAxis] = up[0];
+    basis2[1][m_indexUpAxis] = up[1];
+    basis2[2][m_indexUpAxis] = up[2];
+
+    basis2[0][m_indexForwardAxis] = fwd[0];
+    basis2[1][m_indexForwardAxis] = fwd[1];
+    basis2[2][m_indexForwardAxis] = fwd[2];
+
 	wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
 	wheel.m_worldTransform.setOrigin(
 		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
@@ -493,8 +500,8 @@ struct btWheelContactPoint
 
 };
 
-btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
-btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround);
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround)
 {
 
 	btScalar j1=0.f;
@@ -513,7 +520,7 @@ btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
 	btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
 
 	// calculate j that moves us to zero relative velocity
-	j1 = -vrel * contactPoint.m_jacDiagABInv;
+	j1 = -vrel * contactPoint.m_jacDiagABInv/btScalar(numWheelsOnGround);
 	btSetMin(j1, maxImpulse);
 	btSetMax(j1, -maxImpulse);
 
@@ -567,7 +574,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 					const btTransform& wheelTrans = getWheelTransformWS( i );
 
 					btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
-					m_axle[i] = btVector3(	
+					m_axle[i] = -btVector3(	
 						wheelBasis0[0][m_indexRightAxis],
 						wheelBasis0[1][m_indexRightAxis],
 						wheelBasis0[2][m_indexRightAxis]);
@@ -615,7 +622,8 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 					btScalar defaultRollingFrictionImpulse = 0.f;
 					btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
 					btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
-					rollingFriction = calcRollingFriction(contactPt);
+					btAssert(numWheelsOnGround > 0);
+					rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround);
 				}
 			}
 

thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp

@@ -7,19 +7,19 @@
 
 #if !defined(BT_ID_WO_BULLET) && !defined(BT_USE_INVERSE_DYNAMICS_WITH_BULLET2)
 #include "Bullet3Common/b3Logging.h"
-#define error_message(...) b3Error(__VA_ARGS__)
-#define warning_message(...) b3Warning(__VA_ARGS__)
+#define bt_id_error_message(...) b3Error(__VA_ARGS__)
+#define bt_id_warning_message(...) b3Warning(__VA_ARGS__)
 #define id_printf(...) b3Printf(__VA_ARGS__)
 #else  // BT_ID_WO_BULLET
 #include <cstdio>
 /// print error message with file/line information
-#define error_message(...)																		 \
+#define bt_id_error_message(...)																		 \
 	do {																						   \
 		fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__);					   \
 		fprintf(stderr, __VA_ARGS__);															  \
 	} while (0)
 /// print warning message with file/line information
-#define warning_message(...)																	   \
+#define bt_id_warning_message(...)																	   \
 	do {																						   \
 		fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__);					 \
 		fprintf(stderr, __VA_ARGS__);															  \

thirdparty/bullet/BulletInverseDynamics/IDMath.cpp

@@ -81,7 +81,7 @@ idScalar maxAbsMat3x(const mat3x &m) {
 
 void mul(const mat33 &a, const mat3x &b, mat3x *result) {
     if (b.cols() != result->cols()) {
-        error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
+        bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
                       static_cast<int>(b.cols()), static_cast<int>(result->cols()));
         abort();
     }
@@ -97,7 +97,7 @@ void mul(const mat33 &a, const mat3x &b, mat3x *result) {
 }
 void add(const mat3x &a, const mat3x &b, mat3x *result) {
     if (a.cols() != b.cols()) {
-        error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
+        bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
                       static_cast<int>(a.cols()), static_cast<int>(b.cols()));
         abort();
     }
@@ -109,7 +109,7 @@ void add(const mat3x &a, const mat3x &b, mat3x *result) {
 }
 void sub(const mat3x &a, const mat3x &b, mat3x *result) {
     if (a.cols() != b.cols()) {
-        error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
+        bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
                       static_cast<int>(a.cols()), static_cast<int>(b.cols()));
         abort();
     }
@@ -305,10 +305,10 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
 	//			  the determinant of the inertia tensor about the joint axis is almost
 	//			  zero and can have a very small negative value.
 	if (!isPositiveSemiDefiniteFuzzy(I)) {
-		error_message("invalid inertia matrix for body %d, not positive definite "
+		bt_id_error_message("invalid inertia matrix for body %d, not positive definite "
 					  "(fixed joint)\n",
 					  index);
-		error_message("matrix is:\n"
+		bt_id_error_message("matrix is:\n"
 					  "[%.20e %.20e %.20e;\n"
 					  "%.20e %.20e %.20e;\n"
 					  "%.20e %.20e %.20e]\n",
@@ -321,8 +321,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
 	// check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
 	if (!has_fixed_joint) {
 		if (I(0, 0) + I(1, 1) < I(2, 2)) {
-			error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
-			error_message("matrix is:\n"
+			bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
+			bt_id_error_message("matrix is:\n"
 						  "[%.20e %.20e %.20e;\n"
 						  "%.20e %.20e %.20e;\n"
 						  "%.20e %.20e %.20e]\n",
@@ -331,8 +331,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
 			return false;
 		}
 		if (I(0, 0) + I(1, 1) < I(2, 2)) {
-			error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
-			error_message("matrix is:\n"
+			bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
+			bt_id_error_message("matrix is:\n"
 						  "[%.20e %.20e %.20e;\n"
 						  "%.20e %.20e %.20e;\n"
 						  "%.20e %.20e %.20e]\n",
@@ -341,8 +341,8 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
 			return false;
 		}
 		if (I(1, 1) + I(2, 2) < I(0, 0)) {
-			error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
-			error_message("matrix is:\n"
+			bt_id_error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
+			bt_id_error_message("matrix is:\n"
 						  "[%.20e %.20e %.20e;\n"
 						  "%.20e %.20e %.20e;\n"
 						  "%.20e %.20e %.20e]\n",
@@ -354,25 +354,25 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
 	// check positive/zero diagonal elements
 	for (int i = 0; i < 3; i++) {
 		if (I(i, i) < 0) {  // accept zero
-			error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
+			bt_id_error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
 			return false;
 		}
 	}
 	// check symmetry
 	if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) {
-		error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
+		bt_id_error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
 					  "%e\n",
 					  index, I(1, 0) - I(0, 1));
 		return false;
 	}
 	if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) {
-		error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
+		bt_id_error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
 					  "%e\n",
 					  index, I(2, 0) - I(0, 2));
 		return false;
 	}
 	if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) {
-		error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
+		bt_id_error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
 					  I(1, 2) - I(2, 1));
 		return false;
 	}
@@ -381,7 +381,7 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
 
 bool isValidTransformMatrix(const mat33 &m) {
 #define print_mat(x)																			   \
-	error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2),   \
+	bt_id_error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2),   \
 				  x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
 
 	// check for unit length column vectors
@@ -389,7 +389,7 @@ bool isValidTransformMatrix(const mat33 &m) {
 		const idScalar length_minus_1 =
 			BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
 		if (length_minus_1 > kAxisLengthEpsilon) {
-			error_message("Not a valid rotation matrix (column %d not unit length)\n"
+			bt_id_error_message("Not a valid rotation matrix (column %d not unit length)\n"
 						  "column = [%.18e %.18e %.18e]\n"
 						  "length-1.0= %.18e\n",
 						  i, m(0, i), m(1, i), m(2, i), length_minus_1);
@@ -399,23 +399,23 @@ bool isValidTransformMatrix(const mat33 &m) {
 	}
 	// check for orthogonal column vectors
 	if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
-		error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
+		bt_id_error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
 		print_mat(m);
 		return false;
 	}
 	if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
-		error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
+		bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
 		print_mat(m);
 		return false;
 	}
 	if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
-		error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
+		bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
 		print_mat(m);
 		return false;
 	}
 	// check determinant (rotation not reflection)
 	if (determinant(m) <= 0) {
-		error_message("Not a valid rotation matrix (determinant <=0)\n");
+		bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n");
 		print_mat(m);
 		return false;
 	}

thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp

@@ -83,11 +83,11 @@ int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
 int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
 											vecx *joint_forces) {
 	if (false == m_is_finalized) {
-		error_message("system has not been initialized\n");
+		bt_id_error_message("system has not been initialized\n");
 		return -1;
 	}
 	if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) {
-		error_message("error in inverse dynamics calculation\n");
+		bt_id_error_message("error in inverse dynamics calculation\n");
 		return -1;
 	}
 	return 0;
@@ -97,13 +97,13 @@ int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinemati
 									   const bool initialize_matrix,
 									   const bool set_lower_triangular_matrix, matxx *mass_matrix) {
 	if (false == m_is_finalized) {
-		error_message("system has not been initialized\n");
+		bt_id_error_message("system has not been initialized\n");
 		return -1;
 	}
 	if (-1 ==
 		m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
 									set_lower_triangular_matrix, mass_matrix)) {
-		error_message("error in mass matrix calculation\n");
+		bt_id_error_message("error in mass matrix calculation\n");
 		return -1;
 	}
 	return 0;
@@ -121,12 +121,12 @@ int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx&
     setZero(m_impl->m_world_gravity);
 
     if (false == m_is_finalized) {
-        error_message("system has not been initialized\n");
+        bt_id_error_message("system has not been initialized\n");
         return -1;
     }
     if (-1 == m_impl->calculateKinematics(q, u, dot_u,
                                           MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) {
-        error_message("error in kinematics calculation\n");
+        bt_id_error_message("error in kinematics calculation\n");
         return -1;
     }
 
@@ -137,12 +137,12 @@ int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx&
 
 int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
 	if (false == m_is_finalized) {
-		error_message("system has not been initialized\n");
+		bt_id_error_message("system has not been initialized\n");
 		return -1;
 	}
 	if (-1 == m_impl->calculateKinematics(q, q, q,
                                               MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
-		error_message("error in kinematics calculation\n");
+		bt_id_error_message("error in kinematics calculation\n");
 		return -1;
 	}
 	return 0;
@@ -150,12 +150,12 @@ int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
 
 int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) {
 	if (false == m_is_finalized) {
-		error_message("system has not been initialized\n");
+		bt_id_error_message("system has not been initialized\n");
 		return -1;
 	}
 	if (-1 == m_impl->calculateKinematics(q, u, u,
                                               MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
-		error_message("error in kinematics calculation\n");
+		bt_id_error_message("error in kinematics calculation\n");
 		return -1;
 	}
 	return 0;
@@ -165,12 +165,12 @@ int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const v
 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
 int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
     if (false == m_is_finalized) {
-        error_message("system has not been initialized\n");
+        bt_id_error_message("system has not been initialized\n");
         return -1;
     }
     if (-1 == m_impl->calculateJacobians(q, u,
                                          MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
-        error_message("error in jacobian calculation\n");
+        bt_id_error_message("error in jacobian calculation\n");
         return -1;
     }
     return 0;
@@ -178,12 +178,12 @@ int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
 
 int MultiBodyTree::calculateJacobians(const vecx& q){
     if (false == m_is_finalized) {
-        error_message("system has not been initialized\n");
+        bt_id_error_message("system has not been initialized\n");
         return -1;
     }
     if (-1 == m_impl->calculateJacobians(q, q,
                                          MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) {
-        error_message("error in jacobian calculation\n");
+        bt_id_error_message("error in jacobian calculation\n");
         return -1;
     }
     return 0;
@@ -214,7 +214,7 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
 						   const vec3 &body_r_body_com, const mat33 &body_I_body,
 						   const int user_int, void *user_ptr) {
 	if (body_index < 0) {
-		error_message("body index must be positive (got %d)\n", body_index);
+		bt_id_error_message("body index must be positive (got %d)\n", body_index);
 		return -1;
 	}
 	vec3 body_axis_of_motion(body_axis_of_motion_);
@@ -223,14 +223,14 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
 		case PRISMATIC:
 			// check if axis is unit vector
 			if (!isUnitVector(body_axis_of_motion)) {
-				warning_message(
+				bt_id_warning_message(
 					"axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
 					body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2));
 				idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) +
 									   BT_ID_POW(body_axis_of_motion(1), 2) +
 									   BT_ID_POW(body_axis_of_motion(2), 2));
 				if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min())) {
-					error_message("axis of motion vector too short (%e)\n", length);
+					bt_id_error_message("axis of motion vector too short (%e)\n", length);
 					return -1;
 				}
 				body_axis_of_motion = (1.0 / length) * body_axis_of_motion;
@@ -241,14 +241,14 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
 		case FLOATING:
 			break;
 		default:
-			error_message("unknown joint type %d\n", joint_type);
+			bt_id_error_message("unknown joint type %d\n", joint_type);
 			return -1;
 	}
 
 	// sanity check for mass properties. Zero mass is OK.
 	if (mass < 0) {
 		m_mass_parameters_are_valid = false;
-		error_message("Body %d has invalid mass %e\n", body_index, mass);
+		bt_id_error_message("Body %d has invalid mass %e\n", body_index, mass);
 		if (!m_accept_invalid_mass_parameters) {
 			return -1;
 		}
@@ -296,7 +296,7 @@ int MultiBodyTree::finalize() {
 	const int &num_dofs = m_init_cache->numDoFs();
 
         if(num_dofs<=0) {
-            error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
+            bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
             //return -1;
         }
 
@@ -331,6 +331,22 @@ int MultiBodyTree::finalize() {
 		rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
 		rigid_body.m_joint_type = joint.m_type;
 
+		int user_int;
+		if (-1 == m_init_cache->getUserInt(index, &user_int)) {
+			return -1;
+		}
+		if (-1 == m_impl->setUserInt(index, user_int)) {
+			return -1;
+		}
+
+		void* user_ptr;
+		if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) {
+			return -1;
+		}
+		if (-1 == m_impl->setUserPtr(index, user_ptr)) {
+			return -1;
+		}
+
 		// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
 		// matrices.
 		switch (rigid_body.m_joint_type) {
@@ -370,14 +386,14 @@ int MultiBodyTree::finalize() {
 				rigid_body.m_Jac_JT(2) = 0.0;
 				break;
 			default:
-				error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
+				bt_id_error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
 				return -1;
 		}
 	}
 
 	// 4 assign degree of freedom indices & build per-joint-type index arrays
 	if (-1 == m_impl->generateIndexSets()) {
-		error_message("generating index sets\n");
+		bt_id_error_message("generating index sets\n");
 		return -1;
 	}
 

thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp

@@ -49,9 +49,9 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
 
 class vecx : public btVectorX<idScalar> {
 public:
-	vecx(int size) : btVectorX(size) {}
+	vecx(int size) : btVectorX<idScalar>(size) {}
 	const vecx& operator=(const btVectorX<idScalar>& rhs) {
-		*static_cast<btVectorX*>(this) = rhs;
+		*static_cast<btVectorX<idScalar>*>(this) = rhs;
 		return *this;
 	}
 
@@ -78,7 +78,7 @@ inline vecx operator+(const vecx& a, const vecx& b) {
 	vecx result(a.size());
 	// TODO: error handling for a.size() != b.size()??
 	if (a.size() != b.size()) {
-		error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
 		abort();
 	}
 	for (int i = 0; i < a.size(); i++) {
@@ -92,7 +92,7 @@ inline vecx operator-(const vecx& a, const vecx& b) {
 	vecx result(a.size());
 	// TODO: error handling for a.size() != b.size()??
 	if (a.size() != b.size()) {
-		error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
 		abort();
 	}
 	for (int i = 0; i < a.size(); i++) {
@@ -121,7 +121,7 @@ public:
     }
     void operator=(const mat3x& rhs) {
 	if (m_cols != rhs.m_cols) {
-            error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
+            bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
             abort();
 	}
         for(int i=0;i<rows();i++) {
@@ -139,7 +139,7 @@ public:
 inline vec3 operator*(const mat3x& a, const vecx& b) {
     vec3 result;
     if (a.cols() != b.size()) {
-        error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
+        bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
         abort();
     }
     result(0)=0.0;

thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp

@@ -123,7 +123,7 @@ public:
     };
     void operator=(const mat3x& rhs) {
 	if (m_cols != rhs.m_cols) {
-            error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
+            bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
             abort();
 	}
         for(int i=0;i<3*m_cols;i++) {
@@ -336,7 +336,7 @@ inline vec3 operator/(const vec3& a, const idScalar& s) {
 
 inline const vecx& vecx::operator=(const vecx& rhs) {
 	if (size() != rhs.size()) {
-		error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
+		bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
 		abort();
 	}
 	if (&rhs != this) {
@@ -356,7 +356,7 @@ inline vecx operator+(const vecx& a, const vecx& b) {
 	vecx result(a.size());
 	// TODO: error handling for a.size() != b.size()??
 	if (a.size() != b.size()) {
-		error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
 		abort();
 	}
 	for (int i = 0; i < a.size(); i++) {
@@ -369,7 +369,7 @@ inline vecx operator-(const vecx& a, const vecx& b) {
 	vecx result(a.size());
 	// TODO: error handling for a.size() != b.size()??
 	if (a.size() != b.size()) {
-		error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
+		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
 		abort();
 	}
 	for (int i = 0; i < a.size(); i++) {
@@ -389,7 +389,7 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
 inline vec3 operator*(const mat3x& a, const vecx& b) {
     vec3 result;
     if (a.cols() != b.size()) {
-        error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
+        bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
         abort();
     }
     result(0)=0.0;

thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp

@@ -80,7 +80,7 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const {
 		case FLOATING:
 			return 6;
 	}
-	error_message("unknown joint type %d\n", type);
+	bt_id_error_message("unknown joint type %d\n", type);
 	return 0;
 }
 
@@ -136,13 +136,13 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
 				q_index += 6;
 				break;
 			default:
-				error_message("unsupported joint type %d\n", body.m_joint_type);
+				bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
 				return -1;
 		}
 	}
 	// sanity check
 	if (q_index != m_num_dofs) {
-		error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
+		bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
 		return -1;
 	}
 
@@ -155,10 +155,10 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
 		} else {
 			if (-1 == parent) {
 				// multiple bodies are directly linked to the environment, ie, not a single root
-				error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
+				bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
 			} else {
 				// should never happen
-				error_message(
+				bt_id_error_message(
 					"building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n",
 					child, parent, static_cast<int>(m_parent_index.size()));
 			}
@@ -234,7 +234,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
 														   const vecx &dot_u, vecx *joint_forces) {
 	if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ||
 		joint_forces->size() != m_num_dofs) {
-		error_message("wrong vector dimension. system has %d DOFs,\n"
+		bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
 					  "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n",
 					  m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
 					  static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
@@ -242,7 +242,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
 	}
 	// 1. relative kinematics
         if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) {
-            error_message("error in calculateKinematics\n");
+            bt_id_error_message("error in calculateKinematics\n");
             return -1;
         }
         // 2. update contributions to equations of motion for every body.
@@ -322,14 +322,14 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
 int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u,
                                                       const KinUpdateType type) {
     	if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) {
-		error_message("wrong vector dimension. system has %d DOFs,\n"
+		bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
 					  "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
 					  m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
 					  static_cast<int>(dot_u.size()));
 		return -1;
 	}
         if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) {
-            error_message("invalid type %d\n", type);
+            bt_id_error_message("invalid type %d\n", type);
             return -1;
         }
 
@@ -516,13 +516,13 @@ void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body)
 
 int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) {
     if (q.size() != m_num_dofs || u.size() != m_num_dofs) {
-        error_message("wrong vector dimension. system has %d DOFs,\n"
+        bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
                       "but dim(q)= %d, dim(u)= %d\n",
                       m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
         return -1;
     }
     if(type != POSITION_ONLY && type != POSITION_VELOCITY) {
-        error_message("invalid type %d\n", type);
+        bt_id_error_message("invalid type %d\n", type);
         return -1;
     }
 
@@ -606,7 +606,7 @@ static inline int jointNumDoFs(const JointType &type) {
 			return 6;
 	}
 	// this should never happen
-	error_message("invalid joint type\n");
+	bt_id_error_message("invalid joint type\n");
 	// TODO add configurable abort/crash function
 	abort();
 	return 0;
@@ -626,7 +626,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 
 	if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
 		mass_matrix->cols() != m_num_dofs) {
-		error_message("Dimension error. System has %d DOFs,\n"
+		bt_id_error_message("Dimension error. System has %d DOFs,\n"
 					  "but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
 					  m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
 					  static_cast<int>(mass_matrix->cols()));
@@ -734,7 +734,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 				// 1. for multi-dof joints, rest of the dofs of this body
 				for (int row = col - 1; row >= q_index_min; row--) {
 					if (FLOATING != body.m_joint_type) {
-						error_message("??\n");
+						bt_id_error_message("??\n");
 						return -1;
 					}
 					setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
@@ -788,7 +788,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 #define CHECK_IF_BODY_INDEX_IS_VALID(index)														\
 	do {																						   \
 		if (index < 0 || index >= m_num_bodies) {												  \
-			error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies);			 \
+			bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies);			 \
 			return -1;																			 \
 		}																						  \
 	} while (0)

thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp

@@ -29,13 +29,13 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
 			m_num_dofs += 6;
 			break;
 		default:
-			error_message("unknown joint type %d\n", joint_type);
+			bt_id_error_message("unknown joint type %d\n", joint_type);
 			return -1;
 	}
 
 	if(-1 == parent_index) {
 		if(m_root_index>=0) {
-			error_message("trying to add body %d as root, but already added %d as root body\n",
+			bt_id_error_message("trying to add body %d as root, but already added %d as root body\n",
 						  body_index, m_root_index);
 			return -1;
 		}
@@ -63,7 +63,7 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
 }
 int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const {
 	if (index < 0 || index > static_cast<int>(m_inertias.size())) {
-		error_message("index out of range\n");
+		bt_id_error_message("index out of range\n");
 		return -1;
 	}
 
@@ -73,7 +73,7 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert
 
 int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
 	if (index < 0 || index > static_cast<int>(m_user_int.size())) {
-		error_message("index out of range\n");
+		bt_id_error_message("index out of range\n");
 		return -1;
 	}
 	*user_int = m_user_int[index];
@@ -82,7 +82,7 @@ int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
 
 int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const {
 	if (index < 0 || index > static_cast<int>(m_user_ptr.size())) {
-		error_message("index out of range\n");
+		bt_id_error_message("index out of range\n");
 		return -1;
 	}
 	*user_ptr = m_user_ptr[index];
@@ -91,7 +91,7 @@ int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const
 
 int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const {
 	if (index < 0 || index > static_cast<int>(m_joints.size())) {
-		error_message("index out of range\n");
+		bt_id_error_message("index out of range\n");
 		return -1;
 	}
 	*joint = m_joints[index];

thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp

@@ -157,7 +157,7 @@ void	btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* coll
 
 void	btSoftMultiBodyDynamicsWorld::debugDrawWorld()
 {
-	btDiscreteDynamicsWorld::debugDrawWorld();
+	btMultiBodyDynamicsWorld::debugDrawWorld();
 
 	if (getDebugDrawer())
 	{
@@ -357,10 +357,14 @@ void	btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeSoftBodies(serializer);
 
+	serializeMultiBodies(serializer);
+
 	serializeRigidBodies(serializer);
 
 	serializeCollisionObjects(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 

thirdparty/bullet/LinearMath/TaskScheduler/btTaskScheduler.cpp

@@ -0,0 +1,802 @@
+
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btThreads.h"
+#include "LinearMath/btQuickprof.h"
+#include <stdio.h>
+#include <algorithm>
+
+
+
+#if BT_THREADSAFE
+
+#include "btThreadSupportInterface.h"
+
+#if defined( _WIN32 )
+
+#define WIN32_LEAN_AND_MEAN
+
+#include <windows.h>
+
+#endif
+
+
+typedef unsigned long long btU64;
+static const int kCacheLineSize = 64;
+
+void btSpinPause()
+{
+#if defined( _WIN32 )
+    YieldProcessor();
+#endif
+}
+
+
+struct WorkerThreadStatus
+{
+    enum Type
+    {
+        kInvalid,
+        kWaitingForWork,
+        kWorking,
+        kSleeping,
+    };
+};
+
+
+ATTRIBUTE_ALIGNED64(class) WorkerThreadDirectives
+{
+    static const int kMaxThreadCount = BT_MAX_THREAD_COUNT;
+    // directives for all worker threads packed into a single cacheline
+    char m_threadDirs[kMaxThreadCount];
+
+public:
+    enum Type
+    {
+        kInvalid,
+        kGoToSleep,         // go to sleep
+        kStayAwakeButIdle,  // wait for not checking job queue
+        kScanForJobs,       // actively scan job queue for jobs
+    };
+    WorkerThreadDirectives()
+    {
+        for ( int i = 0; i < kMaxThreadCount; ++i )
+        {
+            m_threadDirs[ i ] = 0;
+        }
+    }
+
+    Type getDirective(int threadId)
+    {
+        btAssert(threadId < kMaxThreadCount);
+        return static_cast<Type>(m_threadDirs[threadId]);
+    }
+
+    void setDirectiveByRange(int threadBegin, int threadEnd, Type dir)
+    {
+        btAssert( threadBegin < threadEnd );
+        btAssert( threadEnd <= kMaxThreadCount );
+        char dirChar = static_cast<char>(dir);
+        for ( int i = threadBegin; i < threadEnd; ++i )
+        {
+            m_threadDirs[ i ] = dirChar;
+        }
+    }
+};
+
+class JobQueue;
+
+ATTRIBUTE_ALIGNED64(struct) ThreadLocalStorage
+{
+    int m_threadId;
+    WorkerThreadStatus::Type m_status;
+    int m_numJobsFinished;
+    btSpinMutex m_mutex;
+    btScalar m_sumResult;
+    WorkerThreadDirectives * m_directive;
+    JobQueue* m_queue;
+    btClock* m_clock;
+    unsigned int m_cooldownTime;
+};
+
+
+struct IJob
+{
+    virtual void executeJob(int threadId) = 0;
+};
+
+class ParallelForJob : public IJob
+{
+    const btIParallelForBody* m_body;
+    int m_begin;
+    int m_end;
+
+public:
+    ParallelForJob( int iBegin, int iEnd, const btIParallelForBody& body )
+    {
+        m_body = &body;
+        m_begin = iBegin;
+        m_end = iEnd;
+    }
+    virtual void executeJob(int threadId) BT_OVERRIDE
+    {
+        BT_PROFILE( "executeJob" );
+
+        // call the functor body to do the work
+        m_body->forLoop( m_begin, m_end );
+    }
+};
+
+
+class ParallelSumJob : public IJob
+{
+    const btIParallelSumBody* m_body;
+    ThreadLocalStorage* m_threadLocalStoreArray;
+    int m_begin;
+    int m_end;
+
+public:
+    ParallelSumJob( int iBegin, int iEnd, const btIParallelSumBody& body, ThreadLocalStorage* tls )
+    {
+        m_body = &body;
+        m_threadLocalStoreArray = tls;
+        m_begin = iBegin;
+        m_end = iEnd;
+    }
+    virtual void executeJob( int threadId ) BT_OVERRIDE
+    {
+        BT_PROFILE( "executeJob" );
+
+        // call the functor body to do the work
+        btScalar val = m_body->sumLoop( m_begin, m_end );
+#if BT_PARALLEL_SUM_DETERMINISTISM
+        // by truncating bits of the result, we can make the parallelSum deterministic (at the expense of precision)
+        const float TRUNC_SCALE = float(1<<19);
+        val = floor(val*TRUNC_SCALE+0.5f)/TRUNC_SCALE;  // truncate some bits
+#endif
+        m_threadLocalStoreArray[threadId].m_sumResult += val;
+    }
+};
+
+
+ATTRIBUTE_ALIGNED64(class) JobQueue
+{
+    btThreadSupportInterface* m_threadSupport;
+    btCriticalSection* m_queueLock;
+    btSpinMutex m_mutex;
+
+    btAlignedObjectArray<IJob*> m_jobQueue;
+    char* m_jobMem;
+    int m_jobMemSize;
+    bool m_queueIsEmpty;
+    int m_tailIndex;
+    int m_headIndex;
+    int m_allocSize;
+    bool m_useSpinMutex;
+    btAlignedObjectArray<JobQueue*> m_neighborContexts;
+    char m_cachePadding[kCacheLineSize];  // prevent false sharing
+
+    void freeJobMem()
+    {
+        if ( m_jobMem )
+        {
+            // free old
+            btAlignedFree(m_jobMem);
+            m_jobMem = NULL;
+        }
+    }
+    void resizeJobMem(int newSize)
+    {
+        if (newSize > m_jobMemSize)
+        {
+            freeJobMem();
+            m_jobMem = static_cast<char*>(btAlignedAlloc(newSize, kCacheLineSize));
+            m_jobMemSize = newSize;
+        }
+    }
+
+public:
+
+    JobQueue()
+    {
+        m_jobMem = NULL;
+        m_jobMemSize = 0;
+        m_threadSupport = NULL;
+        m_queueLock = NULL;
+        m_headIndex = 0;
+        m_tailIndex = 0;
+        m_useSpinMutex = false;
+    }
+    ~JobQueue()
+    {
+		exit();
+    }
+	void exit()
+    {
+		freeJobMem();
+        if (m_queueLock && m_threadSupport)
+        {
+            m_threadSupport->deleteCriticalSection(m_queueLock);
+            m_queueLock = NULL;
+			m_threadSupport = 0;
+        }
+	}
+
+    void init(btThreadSupportInterface* threadSup, btAlignedObjectArray<JobQueue>* contextArray)
+    {
+        m_threadSupport = threadSup;
+        if (threadSup)
+        {
+            m_queueLock = m_threadSupport->createCriticalSection();
+        }
+        setupJobStealing(contextArray, contextArray->size());
+    }
+    void setupJobStealing(btAlignedObjectArray<JobQueue>* contextArray, int numActiveContexts)
+    {
+        btAlignedObjectArray<JobQueue>& contexts = *contextArray;
+        int selfIndex = 0;
+        for (int i = 0; i < contexts.size(); ++i)
+        {
+            if ( this == &contexts[ i ] )
+            {
+                selfIndex = i;
+                break;
+            }
+        }
+        int numNeighbors = btMin(2, contexts.size() - 1);
+        int neighborOffsets[ ] = {-1, 1, -2, 2, -3, 3};
+        int numOffsets = sizeof(neighborOffsets)/sizeof(neighborOffsets[0]);
+        m_neighborContexts.reserve( numNeighbors );
+        m_neighborContexts.resizeNoInitialize(0);
+        for (int i = 0; i < numOffsets && m_neighborContexts.size() < numNeighbors; i++)
+        {
+            int neighborIndex = selfIndex + neighborOffsets[i];
+            if ( neighborIndex >= 0 && neighborIndex < numActiveContexts)
+            {
+                m_neighborContexts.push_back( &contexts[ neighborIndex ] );
+            }
+        }
+    }
+
+    bool isQueueEmpty() const {return m_queueIsEmpty;}
+    void lockQueue()
+    {
+        if ( m_useSpinMutex )
+        {
+            m_mutex.lock();
+        }
+        else
+        {
+            m_queueLock->lock();
+        }
+    }
+    void unlockQueue()
+    {
+        if ( m_useSpinMutex )
+        {
+            m_mutex.unlock();
+        }
+        else
+        {
+            m_queueLock->unlock();
+        }
+    }
+    void clearQueue(int jobCount, int jobSize)
+    {
+        lockQueue();
+        m_headIndex = 0;
+        m_tailIndex = 0;
+        m_allocSize = 0;
+        m_queueIsEmpty = true;
+        int jobBufSize = jobSize * jobCount;
+        // make sure we have enough memory allocated to store jobs
+        if ( jobBufSize > m_jobMemSize )
+        {
+            resizeJobMem( jobBufSize );
+        }
+        // make sure job queue is big enough
+        if ( jobCount > m_jobQueue.capacity() )
+        {
+            m_jobQueue.reserve( jobCount );
+        }
+        unlockQueue();
+        m_jobQueue.resizeNoInitialize( 0 );
+    }
+    void* allocJobMem(int jobSize)
+    {
+        btAssert(m_jobMemSize >= (m_allocSize + jobSize));
+        void* jobMem = &m_jobMem[m_allocSize];
+        m_allocSize += jobSize;
+        return jobMem;
+    }
+    void submitJob( IJob* job )
+    {
+        btAssert( reinterpret_cast<char*>( job ) >= &m_jobMem[ 0 ] && reinterpret_cast<char*>( job ) < &m_jobMem[ 0 ] + m_allocSize );
+        m_jobQueue.push_back( job );
+        lockQueue();
+        m_tailIndex++;
+        m_queueIsEmpty = false;
+        unlockQueue();
+    }
+    IJob* consumeJobFromOwnQueue()
+    {
+        if ( m_queueIsEmpty )
+        {
+            // lock free path. even if this is taken erroneously it isn't harmful
+            return NULL;
+        }
+        IJob* job = NULL;
+        lockQueue();
+        if ( !m_queueIsEmpty )
+        {
+            job = m_jobQueue[ m_headIndex++ ];
+            btAssert( reinterpret_cast<char*>( job ) >= &m_jobMem[ 0 ] && reinterpret_cast<char*>( job ) < &m_jobMem[ 0 ] + m_allocSize );
+            if ( m_headIndex == m_tailIndex )
+            {
+                m_queueIsEmpty = true;
+            }
+        }
+        unlockQueue();
+        return job;
+    }
+    IJob* consumeJob()
+    {
+        if (IJob* job = consumeJobFromOwnQueue())
+        {
+            return job;
+        }
+        // own queue is empty, try to steal from neighbor
+        for (int i = 0; i < m_neighborContexts.size(); ++i)
+        {
+            JobQueue* otherContext = m_neighborContexts[ i ];
+            if ( IJob* job = otherContext->consumeJobFromOwnQueue() )
+            {
+                return job;
+            }
+        }
+        return NULL;
+    }
+};
+
+
+static void WorkerThreadFunc( void* userPtr )
+{
+    BT_PROFILE( "WorkerThreadFunc" );
+    ThreadLocalStorage* localStorage = (ThreadLocalStorage*) userPtr;
+    JobQueue* jobQueue = localStorage->m_queue;
+
+    bool shouldSleep = false;
+    int threadId = localStorage->m_threadId;
+    while (! shouldSleep)
+    {
+        // do work
+        localStorage->m_mutex.lock();
+        while ( IJob* job = jobQueue->consumeJob() )
+        {
+            localStorage->m_status = WorkerThreadStatus::kWorking;
+            job->executeJob( threadId );
+            localStorage->m_numJobsFinished++;
+        }
+        localStorage->m_status = WorkerThreadStatus::kWaitingForWork;
+        localStorage->m_mutex.unlock();
+        btU64 clockStart = localStorage->m_clock->getTimeMicroseconds();
+        // while queue is empty,
+        while (jobQueue->isQueueEmpty())
+        {
+            // todo: spin wait a bit to avoid hammering the empty queue
+            btSpinPause();
+            if ( localStorage->m_directive->getDirective(threadId) == WorkerThreadDirectives::kGoToSleep )
+            {
+                shouldSleep = true;
+                break;
+            }
+            // if jobs are incoming,
+            if ( localStorage->m_directive->getDirective( threadId ) == WorkerThreadDirectives::kScanForJobs )
+            {
+                clockStart = localStorage->m_clock->getTimeMicroseconds(); // reset clock
+            }
+            else
+            {
+                for ( int i = 0; i < 50; ++i )
+                {
+                    btSpinPause();
+                    btSpinPause();
+                    btSpinPause();
+                    btSpinPause();
+                    if (localStorage->m_directive->getDirective( threadId ) == WorkerThreadDirectives::kScanForJobs || !jobQueue->isQueueEmpty())
+                    {
+                        break;
+                    }
+                }
+                // if no jobs incoming and queue has been empty for the cooldown time, sleep
+                btU64 timeElapsed = localStorage->m_clock->getTimeMicroseconds() - clockStart;
+                if (timeElapsed > localStorage->m_cooldownTime)
+                {
+                    shouldSleep = true;
+                    break;
+                }
+            }
+        }
+    }
+	{
+		BT_PROFILE("sleep");
+		// go sleep
+		localStorage->m_mutex.lock();
+		localStorage->m_status = WorkerThreadStatus::kSleeping;
+		localStorage->m_mutex.unlock();
+	}
+}
+
+
+class btTaskSchedulerDefault : public btITaskScheduler
+{
+    btThreadSupportInterface* m_threadSupport;
+    WorkerThreadDirectives* m_workerDirective;
+    btAlignedObjectArray<JobQueue> m_jobQueues;
+    btAlignedObjectArray<JobQueue*> m_perThreadJobQueues;
+    btAlignedObjectArray<ThreadLocalStorage> m_threadLocalStorage;
+    btSpinMutex m_antiNestingLock;  // prevent nested parallel-for
+    btClock m_clock;
+    int m_numThreads;
+    int m_numWorkerThreads;
+    int m_numActiveJobQueues;
+    int m_maxNumThreads;
+    int m_numJobs;
+    static const int kFirstWorkerThreadId = 1;
+public:
+
+    btTaskSchedulerDefault() : btITaskScheduler("ThreadSupport")
+    {
+        m_threadSupport = NULL;
+        m_workerDirective = NULL;
+    }
+
+    virtual ~btTaskSchedulerDefault()
+    {
+        waitForWorkersToSleep();
+
+		for ( int i = 0; i < m_jobQueues.size(); ++i )
+        {
+            m_jobQueues[i].exit();
+        }
+
+        if (m_threadSupport)
+        {
+            delete m_threadSupport;
+            m_threadSupport = NULL;
+        }
+        if (m_workerDirective)
+        {
+            btAlignedFree(m_workerDirective);
+            m_workerDirective = NULL;
+        }
+    }
+
+    void init()
+    {
+        btThreadSupportInterface::ConstructionInfo constructionInfo( "TaskScheduler", WorkerThreadFunc );
+        m_threadSupport = btThreadSupportInterface::create( constructionInfo );
+        m_workerDirective = static_cast<WorkerThreadDirectives*>(btAlignedAlloc(sizeof(*m_workerDirective), 64));
+
+        m_numWorkerThreads = m_threadSupport->getNumWorkerThreads();
+        m_maxNumThreads = m_threadSupport->getNumWorkerThreads() + 1;
+        m_numThreads = m_maxNumThreads;
+        // ideal to have one job queue for each physical processor (except for the main thread which needs no queue)
+        int numThreadsPerQueue = m_threadSupport->getLogicalToPhysicalCoreRatio();
+        int numJobQueues = (numThreadsPerQueue == 1) ? (m_maxNumThreads-1) : (m_maxNumThreads / numThreadsPerQueue);
+        m_jobQueues.resize(numJobQueues);
+        m_numActiveJobQueues = numJobQueues;
+        for ( int i = 0; i < m_jobQueues.size(); ++i )
+        {
+            m_jobQueues[i].init( m_threadSupport, &m_jobQueues );
+        }
+        m_perThreadJobQueues.resize(m_numThreads);
+        for ( int i = 0; i < m_numThreads; i++ )
+        {
+            JobQueue* jq = NULL;
+            // only worker threads get a job queue
+            if (i > 0)
+            {
+                if (numThreadsPerQueue == 1)
+                {
+                    // one queue per worker thread
+                    jq = &m_jobQueues[ i - kFirstWorkerThreadId ];
+                }
+                else
+                {
+                    // 2 threads share each queue
+                    jq = &m_jobQueues[ i / numThreadsPerQueue ];
+                }
+            }
+            m_perThreadJobQueues[i] = jq;
+        }
+        m_threadLocalStorage.resize(m_numThreads);
+        for ( int i = 0; i < m_numThreads; i++ )
+        {
+            ThreadLocalStorage& storage = m_threadLocalStorage[i];
+            storage.m_threadId = i;
+            storage.m_directive = m_workerDirective;
+            storage.m_status = WorkerThreadStatus::kSleeping;
+            storage.m_cooldownTime = 100; // 100 microseconds, threads go to sleep after this long if they have nothing to do
+            storage.m_clock = &m_clock;
+            storage.m_queue = m_perThreadJobQueues[i];
+        }
+        setWorkerDirectives( WorkerThreadDirectives::kGoToSleep ); // no work for them yet
+        setNumThreads( m_threadSupport->getCacheFriendlyNumThreads() );
+    }
+
+    void setWorkerDirectives(WorkerThreadDirectives::Type dir)
+    {
+        m_workerDirective->setDirectiveByRange(kFirstWorkerThreadId, m_numThreads, dir);
+    }
+
+    virtual int getMaxNumThreads() const BT_OVERRIDE
+    {
+        return m_maxNumThreads;
+    }
+
+    virtual int getNumThreads() const BT_OVERRIDE
+    {
+        return m_numThreads;
+    }
+
+    virtual void setNumThreads( int numThreads ) BT_OVERRIDE
+    {
+        m_numThreads = btMax( btMin(numThreads, int(m_maxNumThreads)), 1 );
+        m_numWorkerThreads = m_numThreads - 1;
+        m_numActiveJobQueues = 0;
+        // if there is at least 1 worker,
+        if ( m_numWorkerThreads > 0 )
+        {
+            // re-setup job stealing between queues to avoid attempting to steal from an inactive job queue
+            JobQueue* lastActiveContext = m_perThreadJobQueues[ m_numThreads - 1 ];
+            int iLastActiveContext = lastActiveContext - &m_jobQueues[0];
+            m_numActiveJobQueues = iLastActiveContext + 1;
+            for ( int i = 0; i < m_jobQueues.size(); ++i )
+            {
+                m_jobQueues[ i ].setupJobStealing( &m_jobQueues, m_numActiveJobQueues );
+            }
+        }
+        m_workerDirective->setDirectiveByRange(m_numThreads, BT_MAX_THREAD_COUNT, WorkerThreadDirectives::kGoToSleep);
+    }
+
+    void waitJobs()
+    {
+        BT_PROFILE( "waitJobs" );
+        // have the main thread work until the job queues are empty
+        int numMainThreadJobsFinished = 0;
+        for ( int i = 0; i < m_numActiveJobQueues; ++i )
+        {
+            while ( IJob* job = m_jobQueues[i].consumeJob() )
+            {
+                job->executeJob( 0 );
+                numMainThreadJobsFinished++;
+            }
+        }
+
+        // done with jobs for now, tell workers to rest (but not sleep)
+        setWorkerDirectives( WorkerThreadDirectives::kStayAwakeButIdle );
+
+        btU64 clockStart = m_clock.getTimeMicroseconds();
+        // wait for workers to finish any jobs in progress
+        while ( true )
+        {
+            int numWorkerJobsFinished = 0;
+            for ( int iThread = kFirstWorkerThreadId; iThread < m_numThreads; ++iThread )
+            {
+                ThreadLocalStorage* storage = &m_threadLocalStorage[iThread];
+                storage->m_mutex.lock();
+                numWorkerJobsFinished += storage->m_numJobsFinished;
+                storage->m_mutex.unlock();
+            }
+            if (numWorkerJobsFinished + numMainThreadJobsFinished == m_numJobs)
+            {
+                break;
+            }
+            btU64 timeElapsed = m_clock.getTimeMicroseconds() - clockStart;
+            btAssert(timeElapsed < 1000);
+            if (timeElapsed > 100000)
+            {
+                break;
+            }
+            btSpinPause();
+        }
+    }
+
+    void wakeWorkers(int numWorkersToWake)
+    {
+        BT_PROFILE( "wakeWorkers" );
+        btAssert( m_workerDirective->getDirective(1) == WorkerThreadDirectives::kScanForJobs );
+        int numDesiredWorkers = btMin(numWorkersToWake, m_numWorkerThreads);
+        int numActiveWorkers = 0;
+        for ( int iWorker = 0; iWorker < m_numWorkerThreads; ++iWorker )
+        {
+            // note this count of active workers is not necessarily totally reliable, because a worker thread could be
+            // just about to put itself to sleep. So we may on occasion fail to wake up all the workers. It should be rare.
+            ThreadLocalStorage& storage = m_threadLocalStorage[ kFirstWorkerThreadId + iWorker ];
+            if (storage.m_status != WorkerThreadStatus::kSleeping)
+            {
+                numActiveWorkers++;
+            }
+        }
+        for ( int iWorker = 0; iWorker < m_numWorkerThreads && numActiveWorkers < numDesiredWorkers; ++iWorker )
+        {
+            ThreadLocalStorage& storage = m_threadLocalStorage[ kFirstWorkerThreadId + iWorker ];
+            if (storage.m_status == WorkerThreadStatus::kSleeping)
+            {
+                m_threadSupport->runTask( iWorker, &storage );
+                numActiveWorkers++;
+            }
+        }
+    }
+
+    void waitForWorkersToSleep()
+    {
+        BT_PROFILE( "waitForWorkersToSleep" );
+        setWorkerDirectives( WorkerThreadDirectives::kGoToSleep );
+        m_threadSupport->waitForAllTasks();
+        for ( int i = kFirstWorkerThreadId; i < m_numThreads; i++ )
+        {
+            ThreadLocalStorage& storage = m_threadLocalStorage[i];
+            btAssert( storage.m_status == WorkerThreadStatus::kSleeping );
+        }
+    }
+
+    virtual void sleepWorkerThreadsHint() BT_OVERRIDE
+    {
+        BT_PROFILE( "sleepWorkerThreadsHint" );
+        // hint the task scheduler that we may not be using these threads for a little while
+        setWorkerDirectives( WorkerThreadDirectives::kGoToSleep );
+    }
+
+    void prepareWorkerThreads()
+    {
+        for ( int i = kFirstWorkerThreadId; i < m_numThreads; ++i )
+        {
+            ThreadLocalStorage& storage = m_threadLocalStorage[i];
+            storage.m_mutex.lock();
+            storage.m_numJobsFinished = 0;
+            storage.m_mutex.unlock();
+        }
+        setWorkerDirectives( WorkerThreadDirectives::kScanForJobs );
+    }
+
+    virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) BT_OVERRIDE
+    {
+        BT_PROFILE( "parallelFor_ThreadSupport" );
+        btAssert( iEnd >= iBegin );
+        btAssert( grainSize >= 1 );
+        int iterationCount = iEnd - iBegin;
+        if ( iterationCount > grainSize && m_numWorkerThreads > 0 && m_antiNestingLock.tryLock() )
+        {
+            typedef ParallelForJob JobType;
+            int jobCount = ( iterationCount + grainSize - 1 ) / grainSize;
+            m_numJobs = jobCount;
+            btAssert( jobCount >= 2 );  // need more than one job for multithreading
+            int jobSize = sizeof( JobType );
+
+            for (int i = 0; i < m_numActiveJobQueues; ++i)
+            {
+                m_jobQueues[i].clearQueue( jobCount, jobSize );
+            }
+            // prepare worker threads for incoming work
+            prepareWorkerThreads();
+            // submit all of the jobs
+            int iJob = 0;
+            int iThread = kFirstWorkerThreadId;  // first worker thread
+            for ( int i = iBegin; i < iEnd; i += grainSize )
+            {
+                btAssert( iJob < jobCount );
+                int iE = btMin( i + grainSize, iEnd );
+                JobQueue* jq = m_perThreadJobQueues[ iThread ];
+                btAssert(jq);
+                btAssert((jq - &m_jobQueues[0]) < m_numActiveJobQueues);
+                void* jobMem = jq->allocJobMem(jobSize);
+                JobType* job = new ( jobMem ) ParallelForJob( i, iE, body );  // placement new
+                jq->submitJob( job );
+                iJob++;
+                iThread++;
+                if ( iThread >= m_numThreads )
+                {
+                    iThread = kFirstWorkerThreadId;  // first worker thread
+                }
+            }
+            wakeWorkers( jobCount - 1 );
+
+            // put the main thread to work on emptying the job queue and then wait for all workers to finish
+            waitJobs();
+            m_antiNestingLock.unlock();
+        }
+        else
+        {
+            BT_PROFILE( "parallelFor_mainThread" );
+            // just run on main thread
+            body.forLoop( iBegin, iEnd );
+        }
+    }
+    virtual btScalar parallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body ) BT_OVERRIDE
+    {
+        BT_PROFILE( "parallelSum_ThreadSupport" );
+        btAssert( iEnd >= iBegin );
+        btAssert( grainSize >= 1 );
+        int iterationCount = iEnd - iBegin;
+        if ( iterationCount > grainSize && m_numWorkerThreads > 0 && m_antiNestingLock.tryLock() )
+        {
+            typedef ParallelSumJob JobType;
+            int jobCount = ( iterationCount + grainSize - 1 ) / grainSize;
+            m_numJobs = jobCount;
+            btAssert( jobCount >= 2 );  // need more than one job for multithreading
+            int jobSize = sizeof( JobType );
+            for (int i = 0; i < m_numActiveJobQueues; ++i)
+            {
+                m_jobQueues[i].clearQueue( jobCount, jobSize );
+            }
+
+            // initialize summation
+            for ( int iThread = 0; iThread < m_numThreads; ++iThread )
+            {
+                m_threadLocalStorage[iThread].m_sumResult = btScalar(0);
+            }
+
+            // prepare worker threads for incoming work
+            prepareWorkerThreads();
+            // submit all of the jobs
+            int iJob = 0;
+            int iThread = kFirstWorkerThreadId;  // first worker thread
+            for ( int i = iBegin; i < iEnd; i += grainSize )
+            {
+                btAssert( iJob < jobCount );
+                int iE = btMin( i + grainSize, iEnd );
+                JobQueue* jq = m_perThreadJobQueues[ iThread ];
+                btAssert(jq);
+                btAssert((jq - &m_jobQueues[0]) < m_numActiveJobQueues);
+                void* jobMem = jq->allocJobMem(jobSize);
+                JobType* job = new ( jobMem ) ParallelSumJob( i, iE, body, &m_threadLocalStorage[0] );  // placement new
+                jq->submitJob( job );
+                iJob++;
+                iThread++;
+                if ( iThread >= m_numThreads )
+                {
+                    iThread = kFirstWorkerThreadId;  // first worker thread
+                }
+            }
+            wakeWorkers( jobCount - 1 );
+
+            // put the main thread to work on emptying the job queue and then wait for all workers to finish
+            waitJobs();
+
+            // add up all the thread sums
+            btScalar sum = btScalar(0);
+            for ( int iThread = 0; iThread < m_numThreads; ++iThread )
+            {
+                sum += m_threadLocalStorage[ iThread ].m_sumResult;
+            }
+            m_antiNestingLock.unlock();
+            return sum;
+        }
+        else
+        {
+            BT_PROFILE( "parallelSum_mainThread" );
+            // just run on main thread
+            return body.sumLoop( iBegin, iEnd );
+        }
+    }
+};
+
+
+
+btITaskScheduler* btCreateDefaultTaskScheduler()
+{
+    btTaskSchedulerDefault* ts = new btTaskSchedulerDefault();
+    ts->init();
+    return ts;
+}
+
+#else // #if BT_THREADSAFE
+
+btITaskScheduler* btCreateDefaultTaskScheduler()
+{
+    return NULL;
+}
+
+#endif // #else // #if BT_THREADSAFE

thirdparty/bullet/LinearMath/TaskScheduler/btThreadSupportInterface.h

@@ -0,0 +1,70 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2018 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_THREAD_SUPPORT_INTERFACE_H
+#define BT_THREAD_SUPPORT_INTERFACE_H
+
+
+
+class btCriticalSection
+{
+public:
+    btCriticalSection() {}
+    virtual ~btCriticalSection() {}
+
+    virtual void lock() = 0;
+    virtual void unlock() = 0;
+};
+
+
+class btThreadSupportInterface
+{
+public:
+
+    virtual ~btThreadSupportInterface() {}
+
+    virtual int getNumWorkerThreads() const = 0;  // number of worker threads (total number of logical processors - 1)
+    virtual int getCacheFriendlyNumThreads() const = 0;  // the number of logical processors sharing a single L3 cache
+    virtual int getLogicalToPhysicalCoreRatio() const = 0;  // the number of logical processors per physical processor (usually 1 or 2)
+    virtual void runTask( int threadIndex, void* userData ) = 0;
+    virtual void waitForAllTasks() = 0;
+
+    virtual btCriticalSection* createCriticalSection() = 0;
+    virtual void deleteCriticalSection( btCriticalSection* criticalSection ) = 0;
+
+    typedef void( *ThreadFunc )( void* userPtr );
+
+    struct ConstructionInfo
+    {
+        ConstructionInfo( const char* uniqueName,
+            ThreadFunc userThreadFunc,
+            int threadStackSize = 65535
+        )
+            :m_uniqueName( uniqueName ),
+            m_userThreadFunc( userThreadFunc ),
+            m_threadStackSize( threadStackSize )
+        {
+        }
+
+        const char*     m_uniqueName;
+        ThreadFunc      m_userThreadFunc;
+        int             m_threadStackSize;
+    };
+
+    static btThreadSupportInterface* create( const ConstructionInfo& info );
+};
+
+#endif //BT_THREAD_SUPPORT_INTERFACE_H
+