update to bullet 2.87

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

@@ -41,6 +41,10 @@ struct	btBroadphaseRayCallback : public btBroadphaseAabbCallback
 	btScalar		m_lambda_max;
 
 	virtual ~btBroadphaseRayCallback() {}
+	
+protected:
+    
+    btBroadphaseRayCallback() {}
 };
 
 #include "LinearMath/btVector3.h"
@@ -53,7 +57,7 @@ class btBroadphaseInterface
 public:
 	virtual ~btBroadphaseInterface() {}
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) =0;
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr,  int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher) =0;
 	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
 	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0;

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp

@@ -15,3 +15,4 @@ subject to the following restrictions:
 
 #include "btBroadphaseProxy.h"
 
+BT_NOT_EMPTY_FILE // fix warning LNK4221: This object file does not define any previously undefined public symbols, so it will not be used by any link operation that consumes this library

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

@@ -101,10 +101,10 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	//Usually the client btCollisionObject or Rigidbody class
 	void*	m_clientObject;
-	short int m_collisionFilterGroup;
-	short int m_collisionFilterMask;
-	void*	m_multiSapParentProxy;		
-	int			m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+	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.
 
 	btVector3	m_aabbMin;
 	btVector3	m_aabbMax;
@@ -115,18 +115,17 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
 	}
 
 	//used for memory pools
-	btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0)
+	btBroadphaseProxy() :m_clientObject(0)
 	{
 	}
 
-	btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
+	btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr, int collisionFilterGroup,  int collisionFilterMask)
 		:m_clientObject(userPtr),
 		m_collisionFilterGroup(collisionFilterGroup),
 		m_collisionFilterMask(collisionFilterMask),
 		m_aabbMin(aabbMin),
 		m_aabbMax(aabbMax)
 	{
-		m_multiSapParentProxy = multiSapParentProxy;
 	}
 
 	

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.cpp

@@ -229,25 +229,60 @@ static void						fetchleaves(btDbvt* pdbvt,
 }
 
 //
-static void						split(	const tNodeArray& leaves,
-									  tNodeArray& left,
-									  tNodeArray& right,
+static bool						leftOfAxis(	const btDbvtNode* node,
+										   const btVector3& org,
+										   const btVector3& axis)
+{
+	return btDot(axis, node->volume.Center() - org) <= 0;
+}
+
+
+// Partitions leaves such that leaves[0, n) are on the
+// left of axis, and leaves[n, count) are on the right
+// of axis. returns N.
+static int						split(	btDbvtNode** leaves,
+									  int count,
 									  const btVector3& org,
 									  const btVector3& axis)
 {
-	left.resize(0);
-	right.resize(0);
-	for(int i=0,ni=leaves.size();i<ni;++i)
+	int begin=0;
+	int end=count;
+	for(;;)
 	{
-		if(btDot(axis,leaves[i]->volume.Center()-org)<0)
-			left.push_back(leaves[i]);
-		else
-			right.push_back(leaves[i]);
+		while(begin!=end && leftOfAxis(leaves[begin],org,axis))
+		{
+			++begin;
+		}
+
+		if(begin==end)
+		{
+			break;
+		}
+
+		while(begin!=end && !leftOfAxis(leaves[end-1],org,axis))
+		{
+			--end;
+		}
+
+		if(begin==end)
+		{
+			break;
+		}
+
+		// swap out of place nodes
+		--end;
+		btDbvtNode* temp=leaves[begin];
+		leaves[begin]=leaves[end];
+		leaves[end]=temp;
+		++begin;
 	}
+
+	return begin;
 }
 
 //
-static btDbvtVolume				bounds(	const tNodeArray& leaves)
+static btDbvtVolume				bounds(	btDbvtNode** leaves,
+									   int count)
 {
 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
 	ATTRIBUTE_ALIGNED16(char	locals[sizeof(btDbvtVolume)]);
@@ -257,7 +292,7 @@ static btDbvtVolume				bounds(	const tNodeArray& leaves)
 #else
 	btDbvtVolume volume=leaves[0]->volume;
 #endif
-	for(int i=1,ni=leaves.size();i<ni;++i)
+	for(int i=1,ni=count;i<ni;++i)
 	{
 		Merge(volume,leaves[i]->volume,volume);
 	}
@@ -266,15 +301,16 @@ static btDbvtVolume				bounds(	const tNodeArray& leaves)
 
 //
 static void						bottomup(	btDbvt* pdbvt,
-										 tNodeArray& leaves)
+										 btDbvtNode** leaves,
+										 int count)
 {
-	while(leaves.size()>1)
+	while(count>1)
 	{
 		btScalar	minsize=SIMD_INFINITY;
 		int			minidx[2]={-1,-1};
-		for(int i=0;i<leaves.size();++i)
+		for(int i=0;i<count;++i)
 		{
-			for(int j=i+1;j<leaves.size();++j)
+			for(int j=i+1;j<count;++j)
 			{
 				const btScalar	sz=size(merge(leaves[i]->volume,leaves[j]->volume));
 				if(sz<minsize)
@@ -292,31 +328,33 @@ static void						bottomup(	btDbvt* pdbvt,
 		n[0]->parent		=	p;
 		n[1]->parent		=	p;
 		leaves[minidx[0]]	=	p;
-		leaves.swap(minidx[1],leaves.size()-1);
-		leaves.pop_back();
+		leaves[minidx[1]]	=	leaves[count-1];
+		--count;
 	}
 }
 
 //
 static btDbvtNode*			topdown(btDbvt* pdbvt,
-									tNodeArray& leaves,
+									btDbvtNode** leaves,
+									int count,
 									int bu_treshold)
 {
 	static const btVector3	axis[]={btVector3(1,0,0),
 		btVector3(0,1,0),
 		btVector3(0,0,1)};
-	if(leaves.size()>1)
+	btAssert(bu_treshold>2);
+	if(count>1)
 	{
-		if(leaves.size()>bu_treshold)
+		if(count>bu_treshold)
 		{
-			const btDbvtVolume	vol=bounds(leaves);
+			const btDbvtVolume	vol=bounds(leaves,count);
 			const btVector3			org=vol.Center();
-			tNodeArray				sets[2];
+			int						partition;
 			int						bestaxis=-1;
-			int						bestmidp=leaves.size();
+			int						bestmidp=count;
 			int						splitcount[3][2]={{0,0},{0,0},{0,0}};
 			int i;
-			for( i=0;i<leaves.size();++i)
+			for( i=0;i<count;++i)
 			{
 				const btVector3	x=leaves[i]->volume.Center()-org;
 				for(int j=0;j<3;++j)
@@ -338,29 +376,23 @@ static btDbvtNode*			topdown(btDbvt* pdbvt,
 			}
 			if(bestaxis>=0)
 			{
-				sets[0].reserve(splitcount[bestaxis][0]);
-				sets[1].reserve(splitcount[bestaxis][1]);
-				split(leaves,sets[0],sets[1],org,axis[bestaxis]);
+				partition=split(leaves,count,org,axis[bestaxis]);
+				btAssert(partition!=0 && partition!=count);
 			}
 			else
 			{
-				sets[0].reserve(leaves.size()/2+1);
-				sets[1].reserve(leaves.size()/2);
-				for(int i=0,ni=leaves.size();i<ni;++i)
-				{
-					sets[i&1].push_back(leaves[i]);
-				}
+				partition=count/2+1;
 			}
 			btDbvtNode*	node=createnode(pdbvt,0,vol,0);
-			node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
-			node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
+			node->childs[0]=topdown(pdbvt,&leaves[0],partition,bu_treshold);
+			node->childs[1]=topdown(pdbvt,&leaves[partition],count-partition,bu_treshold);
 			node->childs[0]->parent=node;
 			node->childs[1]->parent=node;
 			return(node);
 		}
 		else
 		{
-			bottomup(pdbvt,leaves);
+			bottomup(pdbvt,leaves,count);
 			return(leaves[0]);
 		}
 	}
@@ -444,7 +476,7 @@ void			btDbvt::optimizeBottomUp()
 		tNodeArray leaves;
 		leaves.reserve(m_leaves);
 		fetchleaves(this,m_root,leaves);
-		bottomup(this,leaves);
+		bottomup(this,&leaves[0],leaves.size());
 		m_root=leaves[0];
 	}
 }
@@ -457,7 +489,7 @@ void			btDbvt::optimizeTopDown(int bu_treshold)
 		tNodeArray	leaves;
 		leaves.reserve(m_leaves);
 		fetchleaves(this,m_root,leaves);
-		m_root=topdown(this,leaves,bu_treshold);
+		m_root=topdown(this,&leaves[0],leaves.size(),bu_treshold);
 	}
 }
 

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.h

@@ -942,7 +942,13 @@ inline void		btDbvt::collideTV(	const btDbvtNode* root,
 		ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume(vol);
 		btAlignedObjectArray<const btDbvtNode*>	stack;
 		stack.resize(0);
+#ifndef BT_DISABLE_STACK_TEMP_MEMORY
+		char tempmemory[SIMPLE_STACKSIZE*sizeof(const btDbvtNode*)];
+		stack.initializeFromBuffer(tempmemory, 0, SIMPLE_STACKSIZE);
+#else
 		stack.reserve(SIMPLE_STACKSIZE);
+#endif //BT_DISABLE_STACK_TEMP_MEMORY
+
 		stack.push_back(root);
 		do	{
 			const btDbvtNode*	n=stack[stack.size()-1];
@@ -1078,7 +1084,12 @@ inline void		btDbvt::rayTest(	const btDbvtNode* root,
 			int								depth=1;
 			int								treshold=DOUBLE_STACKSIZE-2;
 
+			char tempmemory[DOUBLE_STACKSIZE * sizeof(const btDbvtNode*)];
+#ifndef BT_DISABLE_STACK_TEMP_MEMORY
+			stack.initializeFromBuffer(tempmemory, DOUBLE_STACKSIZE, DOUBLE_STACKSIZE);
+#else//BT_DISABLE_STACK_TEMP_MEMORY
 			stack.resize(DOUBLE_STACKSIZE);
+#endif //BT_DISABLE_STACK_TEMP_MEMORY
 			stack[0]=root;
 			btVector3 bounds[2];
 			do	{

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -168,10 +168,9 @@ btBroadphaseProxy*				btDbvtBroadphase::createProxy(	const btVector3& aabbMin,
 															  const btVector3& aabbMax,
 															  int /*shapeType*/,
 															  void* userPtr,
-															  short int collisionFilterGroup,
-															  short int collisionFilterMask,
-															  btDispatcher* /*dispatcher*/,
-															  void* /*multiSapProxy*/)
+															  int collisionFilterGroup,
+															  int collisionFilterMask,
+															  btDispatcher* /*dispatcher*/)
 {
 	btDbvtProxy*		proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(	aabbMin,aabbMax,userPtr,
 		collisionFilterGroup,
@@ -545,7 +544,9 @@ void							btDbvtBroadphase::collide(btDispatcher* dispatcher)
 	btDbvtProxy*	current=m_stageRoots[m_stageCurrent];
 	if(current)
 	{
+#if DBVT_BP_ACCURATESLEEPING
 		btDbvtTreeCollider	collider(this);
+#endif
 		do	{
 			btDbvtProxy*	next=current->links[1];
 			listremove(current,m_stageRoots[current->stage]);

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

@@ -47,7 +47,7 @@ struct btDbvtProxy : btBroadphaseProxy
 	btDbvtProxy*	links[2];
 	int				stage;
 	/* ctor			*/ 
-	btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
+	btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr, int collisionFilterGroup,  int collisionFilterMask) :
 	btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
 	{
 		links[0]=links[1]=0;
@@ -105,7 +105,7 @@ struct	btDbvtBroadphase : btBroadphaseInterface
 	void							optimize();
 	
 	/* btBroadphaseInterface Implementation	*/
-	btBroadphaseProxy*				createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+	btBroadphaseProxy*				createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher);
 	virtual void					destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 	virtual void					setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
 	virtual void					rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

@@ -90,7 +90,8 @@ public:
 };
 
 /// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
-class btHashedOverlappingPairCache : public btOverlappingPairCache
+
+ATTRIBUTE_ALIGNED16(class) btHashedOverlappingPairCache : public btOverlappingPairCache
 {
 	btBroadphasePairArray	m_overlappingPairArray;
 	btOverlapFilterCallback* m_overlapFilterCallback;
@@ -103,6 +104,8 @@ protected:
 
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
 	btHashedOverlappingPairCache();
 	virtual ~btHashedOverlappingPairCache();
 
@@ -212,10 +215,9 @@ private:
 	*/
 
 
-	
-	SIMD_FORCE_INLINE	unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
+	SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
 	{
-		int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16));
+		unsigned int key = proxyId1 | (proxyId2 << 16);
 		// Thomas Wang's hash
 
 		key += ~(key << 15);
@@ -224,13 +226,11 @@ private:
 		key ^=  (key >> 6);
 		key += ~(key << 11);
 		key ^=  (key >> 16);
-		return static_cast<unsigned int>(key);
+		return key;
 	}
 	
 
 
-
-
 	SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash)
 	{
 		int proxyId1 = proxy0->getUid();

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h

@@ -23,6 +23,9 @@ struct  btBroadphasePair;
 ///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
 class btOverlappingPairCallback
 {
+protected:
+    btOverlappingPairCallback() {}
+    
 public:
 	virtual ~btOverlappingPairCallback()
 	{

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

@@ -1336,6 +1336,8 @@ const char*	btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer
 			memPtr->m_escapeIndex = m_contiguousNodes[i].m_escapeIndex;
 			memPtr->m_subPart = m_contiguousNodes[i].m_subPart;
 			memPtr->m_triangleIndex = m_contiguousNodes[i].m_triangleIndex;
+			// Fill padding with zeros to appease msan.
+			memset(memPtr->m_pad, 0, sizeof(memPtr->m_pad));
 		}
 		serializer->finalizeChunk(chunk,"btOptimizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_contiguousNodes[0]);
 	}

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h

@@ -169,7 +169,7 @@ typedef btAlignedObjectArray<btBvhSubtreeInfo>		BvhSubtreeInfoArray;
 
 
 ///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU.
-///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase.
+///It is used by the btBvhTriangleMeshShape as midphase.
 ///It is recommended to use quantization for better performance and lower memory requirements.
 ATTRIBUTE_ALIGNED16(class) btQuantizedBvh
 {

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

@@ -84,7 +84,7 @@ btSimpleBroadphase::~btSimpleBroadphase()
 }
 
 
-btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy)
+btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr , int collisionFilterGroup, int collisionFilterMask, btDispatcher* /*dispatcher*/)
 {
 	if (m_numHandles >= m_maxHandles)
 	{
@@ -94,7 +94,7 @@ btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,
 	btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
 
 	int newHandleIndex = allocHandle();
-	btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
+	btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
 
 	return proxy;
 }

Engine/lib/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

@@ -29,8 +29,8 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 	
 	btSimpleBroadphaseProxy() {};
 
-	btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
-	:btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy)
+	btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask)
+	:btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask)
 	{
 		(void)shapeType;
 	}
@@ -127,7 +127,7 @@ public:
 		static bool	aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1);
 
 
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr , int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
 
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
 

Engine/lib/bullet/src/BulletCollision/CMakeLists.txt

@@ -7,7 +7,6 @@ SET(BulletCollision_SRCS
 	BroadphaseCollision/btDbvt.cpp
 	BroadphaseCollision/btDbvtBroadphase.cpp
 	BroadphaseCollision/btDispatcher.cpp
-	BroadphaseCollision/btMultiSapBroadphase.cpp
 	BroadphaseCollision/btOverlappingPairCache.cpp
 	BroadphaseCollision/btQuantizedBvh.cpp
 	BroadphaseCollision/btSimpleBroadphase.cpp
@@ -16,6 +15,7 @@ SET(BulletCollision_SRCS
 	CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
 	CollisionDispatch/btBoxBoxDetector.cpp
 	CollisionDispatch/btCollisionDispatcher.cpp
+	CollisionDispatch/btCollisionDispatcherMt.cpp
 	CollisionDispatch/btCollisionObject.cpp
 	CollisionDispatch/btCollisionWorld.cpp
 	CollisionDispatch/btCollisionWorldImporter.cpp
@@ -103,6 +103,7 @@ SET(Root_HDRS
 	../btBulletCollisionCommon.h
 )
 SET(BroadphaseCollision_HDRS
+    BroadphaseCollision/btAxisSweep3Internal.h
 	BroadphaseCollision/btAxisSweep3.h
 	BroadphaseCollision/btBroadphaseInterface.h
 	BroadphaseCollision/btBroadphaseProxy.h
@@ -110,7 +111,6 @@ SET(BroadphaseCollision_HDRS
 	BroadphaseCollision/btDbvt.h
 	BroadphaseCollision/btDbvtBroadphase.h
 	BroadphaseCollision/btDispatcher.h
-	BroadphaseCollision/btMultiSapBroadphase.h
 	BroadphaseCollision/btOverlappingPairCache.h
 	BroadphaseCollision/btOverlappingPairCallback.h
 	BroadphaseCollision/btQuantizedBvh.h
@@ -124,6 +124,7 @@ SET(CollisionDispatch_HDRS
 	CollisionDispatch/btCollisionConfiguration.h
 	CollisionDispatch/btCollisionCreateFunc.h
 	CollisionDispatch/btCollisionDispatcher.h
+	CollisionDispatch/btCollisionDispatcherMt.h
 	CollisionDispatch/btCollisionObject.h
 	CollisionDispatch/btCollisionObjectWrapper.h
 	CollisionDispatch/btCollisionWorld.h
@@ -191,12 +192,15 @@ SET(CollisionShapes_HDRS
 SET(Gimpact_HDRS
 	Gimpact/btBoxCollision.h
 	Gimpact/btClipPolygon.h
+	Gimpact/btContactProcessingStructs.h
 	Gimpact/btContactProcessing.h
 	Gimpact/btGenericPoolAllocator.h
 	Gimpact/btGeometryOperations.h
+	Gimpact/btGImpactBvhStructs.h
 	Gimpact/btGImpactBvh.h
 	Gimpact/btGImpactCollisionAlgorithm.h
 	Gimpact/btGImpactMassUtil.h
+	Gimpact/btGImpactQuantizedBvhStructs.h
 	Gimpact/btGImpactQuantizedBvh.h
 	Gimpact/btGImpactShape.h
 	Gimpact/btQuantization.h

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

@@ -100,45 +100,54 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
 	btScalar radiusWithThreshold = radius + contactBreakingThreshold;
 
 	btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
-	normal.safeNormalize();
-	btVector3 p1ToCentre = sphereCenter - vertices[0];
-	btScalar distanceFromPlane = p1ToCentre.dot(normal);
 
-	if (distanceFromPlane < btScalar(0.))
-	{
-		//triangle facing the other way
-		distanceFromPlane *= btScalar(-1.);
-		normal *= btScalar(-1.);
-	}
-
-	bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
-	
-	// Check for contact / intersection
+	btScalar l2 = normal.length2();
 	bool hasContact = false;
 	btVector3 contactPoint;
-	if (isInsideContactPlane) {
-		if (facecontains(sphereCenter,vertices,normal)) {
-			// Inside the contact wedge - touches a point on the shell plane
-			hasContact = true;
-			contactPoint = sphereCenter - normal*distanceFromPlane;
-		} else {
-			// Could be inside one of the contact capsules
-			btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
-			btVector3 nearestOnEdge;
-			for (int i = 0; i < m_triangle->getNumEdges(); i++) {
-				
-				btVector3 pa;
-				btVector3 pb;
-				
-				m_triangle->getEdge(i,pa,pb);
 
-				btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
-				if (distanceSqr < contactCapsuleRadiusSqr) {
-					// Yep, we're inside a capsule
-					hasContact = true;
-					contactPoint = nearestOnEdge;
+	if (l2 >= SIMD_EPSILON*SIMD_EPSILON)
+	{
+		normal /= btSqrt(l2);
+
+		btVector3 p1ToCentre = sphereCenter - vertices[0];
+		btScalar distanceFromPlane = p1ToCentre.dot(normal);
+
+		if (distanceFromPlane < btScalar(0.))
+		{
+			//triangle facing the other way
+			distanceFromPlane *= btScalar(-1.);
+			normal *= btScalar(-1.);
+		}
+
+		bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
+	
+		// Check for contact / intersection
+	
+		if (isInsideContactPlane) {
+			if (facecontains(sphereCenter, vertices, normal)) {
+				// Inside the contact wedge - touches a point on the shell plane
+				hasContact = true;
+				contactPoint = sphereCenter - normal*distanceFromPlane;
+			}
+			else {
+				// Could be inside one of the contact capsules
+				btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
+				btVector3 nearestOnEdge;
+				for (int i = 0; i < m_triangle->getNumEdges(); i++) {
+
+					btVector3 pa;
+					btVector3 pb;
+
+					m_triangle->getEdge(i, pa, pb);
+
+					btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
+					if (distanceSqr < contactCapsuleRadiusSqr) {
+						// Yep, we're inside a capsule
+						hasContact = true;
+						contactPoint = nearestOnEdge;
+					}
+
 				}
-				
 			}
 		}
 	}

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h

@@ -24,12 +24,13 @@ class btActivatingCollisionAlgorithm : public btCollisionAlgorithm
 //	btCollisionObject* m_colObj0;
 //	btCollisionObject* m_colObj1;
 
-public:
+protected:
 
 	btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci);
 
 	btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
 
+public:
 	virtual ~btActivatingCollisionAlgorithm();
 
 };

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -239,10 +239,7 @@ public:
 
 	virtual bool	processOverlap(btBroadphasePair& pair)
 	{
-		BT_PROFILE("btCollisionDispatcher::processOverlap");
-
 		(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
-
 		return false;
 	}
 };

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp

@@ -0,0 +1,164 @@
+/*
+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 "btCollisionDispatcherMt.h"
+#include "LinearMath/btQuickprof.h"
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btPoolAllocator.h"
+#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+
+btCollisionDispatcherMt::btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize )
+    : btCollisionDispatcher( config )
+{
+    m_batchUpdating = false;
+    m_grainSize = grainSize;  // iterations per task
+}
+
+
+btPersistentManifold* btCollisionDispatcherMt::getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 )
+{
+    //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
+
+    btScalar contactBreakingThreshold = ( m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD ) ?
+        btMin( body0->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ), body1->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ) )
+        : gContactBreakingThreshold;
+
+    btScalar contactProcessingThreshold = btMin( body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold() );
+
+    void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) );
+    if ( NULL == mem )
+    {
+        //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
+        if ( ( m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION ) == 0 )
+        {
+            mem = btAlignedAlloc( sizeof( btPersistentManifold ), 16 );
+        }
+        else
+        {
+            btAssert( 0 );
+            //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
+            return 0;
+        }
+    }
+    btPersistentManifold* manifold = new( mem ) btPersistentManifold( body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold );
+    if ( !m_batchUpdating )
+    {
+        // batch updater will update manifold pointers array after finishing, so
+        // only need to update array when not batch-updating
+        //btAssert( !btThreadsAreRunning() );
+        manifold->m_index1a = m_manifoldsPtr.size();
+        m_manifoldsPtr.push_back( manifold );
+    }
+
+    return manifold;
+}
+
+void btCollisionDispatcherMt::releaseManifold( btPersistentManifold* manifold )
+{
+    clearManifold( manifold );
+    //btAssert( !btThreadsAreRunning() );
+    if ( !m_batchUpdating )
+    {
+        // batch updater will update manifold pointers array after finishing, so
+        // only need to update array when not batch-updating
+        int findIndex = manifold->m_index1a;
+        btAssert( findIndex < m_manifoldsPtr.size() );
+        m_manifoldsPtr.swap( findIndex, m_manifoldsPtr.size() - 1 );
+        m_manifoldsPtr[ findIndex ]->m_index1a = findIndex;
+        m_manifoldsPtr.pop_back();
+    }
+
+    manifold->~btPersistentManifold();
+    if ( m_persistentManifoldPoolAllocator->validPtr( manifold ) )
+    {
+        m_persistentManifoldPoolAllocator->freeMemory( manifold );
+    }
+    else
+    {
+        btAlignedFree( manifold );
+    }
+}
+
+struct CollisionDispatcherUpdater : public btIParallelForBody
+{
+    btBroadphasePair* mPairArray;
+    btNearCallback mCallback;
+    btCollisionDispatcher* mDispatcher;
+    const btDispatcherInfo* mInfo;
+
+    CollisionDispatcherUpdater()
+    {
+        mPairArray = NULL;
+        mCallback = NULL;
+        mDispatcher = NULL;
+        mInfo = NULL;
+    }
+    void forLoop( int iBegin, int iEnd ) const
+    {
+        for ( int i = iBegin; i < iEnd; ++i )
+        {
+            btBroadphasePair* pair = &mPairArray[ i ];
+            mCallback( *pair, *mDispatcher, *mInfo );
+        }
+    }
+};
+
+
+void btCollisionDispatcherMt::dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher )
+{
+    int pairCount = pairCache->getNumOverlappingPairs();
+    if ( pairCount == 0 )
+    {
+        return;
+    }
+    CollisionDispatcherUpdater updater;
+    updater.mCallback = getNearCallback();
+    updater.mPairArray = pairCache->getOverlappingPairArrayPtr();
+    updater.mDispatcher = this;
+    updater.mInfo = &info;
+
+    m_batchUpdating = true;
+    btParallelFor( 0, pairCount, m_grainSize, updater );
+    m_batchUpdating = false;
+
+    // reconstruct the manifolds array to ensure determinism
+    m_manifoldsPtr.resizeNoInitialize( 0 );
+
+    btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr();
+    for ( int i = 0; i < pairCount; ++i )
+    {
+        if (btCollisionAlgorithm* algo = pairs[ i ].m_algorithm)
+        {
+            algo->getAllContactManifolds( m_manifoldsPtr );
+        }
+    }
+
+    // update the indices (used when releasing manifolds)
+    for ( int i = 0; i < m_manifoldsPtr.size(); ++i )
+    {
+        m_manifoldsPtr[ i ]->m_index1a = i;
+    }
+}
+
+

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h

@@ -0,0 +1,39 @@
+/*
+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_COLLISION_DISPATCHER_MT_H
+#define BT_COLLISION_DISPATCHER_MT_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "LinearMath/btThreads.h"
+
+
+class btCollisionDispatcherMt : public btCollisionDispatcher
+{
+public:
+    btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize = 40 );
+
+    virtual btPersistentManifold* getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) BT_OVERRIDE;
+    virtual void releaseManifold( btPersistentManifold* manifold ) BT_OVERRIDE;
+
+    virtual void dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher ) BT_OVERRIDE;
+
+protected:
+    bool m_batchUpdating;
+    int m_grainSize;
+};
+
+#endif //BT_COLLISION_DISPATCHER_MT_H
+

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -18,9 +18,11 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 
 btCollisionObject::btCollisionObject()
-	:	m_anisotropicFriction(1.f,1.f,1.f),
-	m_hasAnisotropicFriction(false),
-	m_contactProcessingThreshold(BT_LARGE_FLOAT),
+	:	m_interpolationLinearVelocity(0.f, 0.f, 0.f),
+		m_interpolationAngularVelocity(0.f, 0.f, 0.f),
+		m_anisotropicFriction(1.f,1.f,1.f),
+		m_hasAnisotropicFriction(false),
+		m_contactProcessingThreshold(BT_LARGE_FLOAT),
 		m_broadphaseHandle(0),
 		m_collisionShape(0),
 		m_extensionPointer(0),
@@ -48,6 +50,7 @@ btCollisionObject::btCollisionObject()
 		m_updateRevision(0)
 {
 	m_worldTransform.setIdentity();
+	m_interpolationWorldTransform.setIdentity();
 }
 
 btCollisionObject::~btCollisionObject()
@@ -112,6 +115,9 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
 	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));
+
 	return btCollisionObjectDataName;
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -139,6 +139,8 @@ public:
 		CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
 		CF_HAS_CONTACT_STIFFNESS_DAMPING = 128,
 		CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
+		CF_HAS_FRICTION_ANCHOR = 512,
+		CF_HAS_COLLISION_SOUND_TRIGGER = 1024
 	};
 
 	enum	CollisionObjectTypes

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -108,7 +108,7 @@ btCollisionWorld::~btCollisionWorld()
 
 
 
-void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask)
 {
 
 	btAssert(collisionObject);
@@ -135,8 +135,7 @@ void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
 		collisionObject,
 		collisionFilterGroup,
 		collisionFilterMask,
-		m_dispatcher1,0
-		))	;
+		m_dispatcher1))	;
 
 
 
@@ -257,7 +256,7 @@ void	btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
 
 
     int iObj = collisionObject->getWorldArrayIndex();
-    btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously?
+//    btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously?
     if (iObj >= 0 && iObj < m_collisionObjects.size())
     {
         btAssert(collisionObject == m_collisionObjects[iObj]);
@@ -1334,7 +1333,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
 	// Draw a small simplex at the center of the object
 	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
 	{
-		getDebugDrawer()->drawTransform(worldTransform,1);
+		getDebugDrawer()->drawTransform(worldTransform,.1);
 	}
 
 	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
@@ -1515,6 +1514,8 @@ void	btCollisionWorld::debugDrawWorld()
 {
 	if (getDebugDrawer())
 	{
+		getDebugDrawer()->clearLines();
+
 		btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors();
 
 		if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -205,8 +205,8 @@ public:
 	{
 		btScalar	m_closestHitFraction;
 		const btCollisionObject*		m_collisionObject;
-		short int	m_collisionFilterGroup;
-		short int	m_collisionFilterMask;
+		int	m_collisionFilterGroup;
+		int	m_collisionFilterMask;
 		//@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback.h. Apply any of the EFlags defined there on m_flags here to invoke.
 		unsigned int m_flags;
 
@@ -340,8 +340,8 @@ public:
 	struct	ConvexResultCallback
 	{
 		btScalar	m_closestHitFraction;
-		short int	m_collisionFilterGroup;
-		short int	m_collisionFilterMask;
+		int	m_collisionFilterGroup;
+		int	m_collisionFilterMask;
 		
 		ConvexResultCallback()
 			:m_closestHitFraction(btScalar(1.)),
@@ -410,8 +410,8 @@ public:
 	///ContactResultCallback is used to report contact points
 	struct	ContactResultCallback
 	{
-		short int	m_collisionFilterGroup;
-		short int	m_collisionFilterMask;
+		int	m_collisionFilterGroup;
+		int	m_collisionFilterMask;
 		btScalar	m_closestDistanceThreshold;
 
 		ContactResultCallback()
@@ -483,7 +483,7 @@ public:
 											const btCollisionObjectWrapper* colObjWrap,
 											ConvexResultCallback& resultCallback, btScalar allowedPenetration);
 
-	virtual void	addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+	virtual void	addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter);
 
 	btCollisionObjectArray& getCollisionObjectArray()
 	{

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp

@@ -579,13 +579,13 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionS
 				btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData;
 				btCompoundShape* compoundShape = createCompoundShape();
 
-				btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0];
+				//btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0];
 
 
 				btAlignedObjectArray<btCollisionShape*> childShapes;
 				for (int i=0;i<compoundData->m_numChildShapes;i++)
 				{
-					btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i];
+					//btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i];
 
 					btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape;
 

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h

@@ -124,7 +124,6 @@ public:
 	btCollisionShape* getCollisionShapeByIndex(int index);
 	int getNumRigidBodies() const;
 	btCollisionObject* getRigidBodyByIndex(int index) const;
-	int getNumConstraints() const;
 
 	int getNumBvhs() const;
 	btOptimizedBvh*  getBvhByIndex(int index) const;

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp

@@ -24,6 +24,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
+//USE_LOCAL_STACK will avoid most (often all) dynamic memory allocations due to resizing in processCollision and MycollideTT
+#define USE_LOCAL_STACK 1
 
 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
 
@@ -251,7 +253,12 @@ static inline void		MycollideTT(	const btDbvtNode* root0,
 			int								depth=1;
 			int								treshold=btDbvt::DOUBLE_STACKSIZE-4;
 			btAlignedObjectArray<btDbvt::sStkNN>	stkStack;
+#ifdef USE_LOCAL_STACK
+			ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]);
+			stkStack.initializeFromBuffer(&localStack,btDbvt::DOUBLE_STACKSIZE,btDbvt::DOUBLE_STACKSIZE);
+#else
 			stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
+#endif
 			stkStack[0]=btDbvt::sStkNN(root0,root1);
 			do	{
 				btDbvt::sStkNN	p=stkStack[--depth];
@@ -329,6 +336,10 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 	{
 		int i;
 		btManifoldArray manifoldArray;
+#ifdef USE_LOCAL_STACK 
+		btPersistentManifold localManifolds[4];
+		manifoldArray.initializeFromBuffer(&localManifolds,0,4);
+#endif
 		btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
 		for (i=0;i<pairs.size();i++)
 		{

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h

@@ -33,8 +33,6 @@ class btDispatcher;
 class btCollisionObject;
 
 class btCollisionShape;
-typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1);
-extern btShapePairCallback gCompoundCompoundChildShapePairCallback;
 
 /// btCompoundCompoundCollisionAlgorithm  supports collision between two btCompoundCollisionShape shapes
 class btCompoundCompoundCollisionAlgorithm  : public btCompoundCollisionAlgorithm

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -30,8 +30,8 @@ subject to the following restrictions:
 
 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_isSwapped(isSwapped),
-m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
+m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped),
+m_isSwapped(isSwapped)
 {
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -16,7 +16,7 @@ subject to the following restrictions:
 ///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
 ///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
 ///with reproduction case
-//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
+//#define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
 //#define ZERO_MARGIN
 
 #include "btConvexConvexAlgorithm.h"
@@ -310,10 +310,10 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
 	if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
 	{
+		//m_manifoldPtr->clearManifold();
+
 		btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
 		btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
-	//	btVector3 localScalingA = capsuleA->getLocalScaling();
-	//	btVector3 localScalingB = capsuleB->getLocalScaling();
 		
 		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
 
@@ -329,6 +329,50 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 		resultOut->refreshContactPoints();
 		return;
 	}
+
+	if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == SPHERE_SHAPE_PROXYTYPE))
+	{
+		//m_manifoldPtr->clearManifold();
+
+		btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
+		btSphereShape* capsuleB = (btSphereShape*) min1;
+		
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+
+		btScalar dist = capsuleCapsuleDistance(normalOnB,	pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
+			0.,capsuleB->getRadius(),capsuleA->getUpAxis(),1,
+			body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
+
+		if (dist<threshold)
+		{
+			btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
+			resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);	
+		}
+		resultOut->refreshContactPoints();
+		return;
+	}
+
+	if ((min0->getShapeType() == SPHERE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
+	{
+		//m_manifoldPtr->clearManifold();
+
+		btSphereShape* capsuleA = (btSphereShape*) min0;
+		btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
+		
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+
+		btScalar dist = capsuleCapsuleDistance(normalOnB,	pointOnBWorld,0.,capsuleA->getRadius(),
+			capsuleB->getHalfHeight(),capsuleB->getRadius(),1,capsuleB->getUpAxis(),
+			body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
+
+		if (dist<threshold)
+		{
+			btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
+			resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);	
+		}
+		resultOut->refreshContactPoints();
+		return;
+	}
 #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
 
 

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h

@@ -123,9 +123,9 @@ private:
 
 	
 	
-	SIMD_FORCE_INLINE	unsigned int getHash(unsigned int indexA, unsigned int indexB)
+	SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB)
 	{
-		int key = static_cast<int>(((unsigned int)indexA) | (((unsigned int)indexB) <<16));
+		unsigned int key = indexA | (indexB << 16);
 		// Thomas Wang's hash
 
 		key += ~(key << 15);
@@ -134,7 +134,7 @@ private:
 		key ^=  (key >> 6);
 		key += ~(key << 11);
 		key ^=  (key >> 16);
-		return static_cast<unsigned int>(key);
+		return key;
 	}
 	
 

Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -111,6 +111,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 		return;
 
 	bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
+	bool isNewCollision = m_manifoldPtr->getNumContacts() == 0;
 
 	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
 
@@ -145,7 +146,13 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
         newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
         newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
     }
-	
+
+	if (    (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR) ||
+            (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR))
+    {
+        newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR;
+    }
+
 	btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
 	
 
@@ -187,5 +194,9 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 		(*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1);
 	}
 
+	if (gContactStartedCallback && isNewCollision)
+	{
+		gContactStartedCallback(m_manifoldPtr);
+	}
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.h

@@ -103,11 +103,12 @@ public:
 		btScalar minDimension = boxHalfExtents.getX();
 		if (minDimension>boxHalfExtents.getY())
 			minDimension = boxHalfExtents.getY();
-		setSafeMargin(minDimension);
 
 		m_shapeType = BOX_2D_SHAPE_PROXYTYPE;
 		btVector3 margin(getMargin(),getMargin(),getMargin());
 		m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
+
+		setSafeMargin(minDimension);
 	};
 
 	virtual void setMargin(btScalar collisionMargin)

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBoxShape.cpp

@@ -19,10 +19,10 @@ btBoxShape::btBoxShape( const btVector3& boxHalfExtents)
 {
 	m_shapeType = BOX_SHAPE_PROXYTYPE;
 
-	setSafeMargin(boxHalfExtents);
-
 	btVector3 margin(getMargin(),getMargin(),getMargin());
 	m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
+
+	setSafeMargin(boxHalfExtents);
 };
 
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp

@@ -437,6 +437,9 @@ const char*	btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* se
 		trimeshData->m_triangleInfoMap = 0;
 	}
 
+	// Fill padding with zeros to appease msan.
+	memset(trimeshData->m_pad3, 0, sizeof(trimeshData->m_pad3));
+
 	return "btTriangleMeshShapeData";
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp

@@ -16,11 +16,11 @@ subject to the following restrictions:
 
 #include "btCapsuleShape.h"
 
-#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 #include "LinearMath/btQuaternion.h"
 
 btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape ()
 {
+	m_collisionMargin = radius;
 	m_shapeType = CAPSULE_SHAPE_PROXYTYPE;
 	m_upAxis = 1;
 	m_implicitShapeDimensions.setValue(radius,0.5f*height,radius);
@@ -48,14 +48,13 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 	btVector3 vtx;
 	btScalar newDot;
 	
-	btScalar radius = getRadius();
-
+	
 
 	{
 		btVector3 pos(0,0,0);
 		pos[getUpAxis()] = getHalfHeight();
 
-		vtx = pos +vec*(radius) - vec * getMargin();
+		vtx = pos;
 		newDot = vec.dot(vtx);
 		if (newDot > maxDot)
 		{
@@ -67,7 +66,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 		btVector3 pos(0,0,0);
 		pos[getUpAxis()] = -getHalfHeight();
 
-		vtx = pos +vec*(radius) - vec * getMargin();
+		vtx = pos;
 		newDot = vec.dot(vtx);
 		if (newDot > maxDot)
 		{
@@ -84,8 +83,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 {
 
 	
-	btScalar radius = getRadius();
-
+	
 	for (int j=0;j<numVectors;j++)
 	{
 		btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
@@ -96,7 +94,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 		{
 			btVector3 pos(0,0,0);
 			pos[getUpAxis()] = getHalfHeight();
-			vtx = pos +vec*(radius) - vec * getMargin();
+			vtx = pos;
 			newDot = vec.dot(vtx);
 			if (newDot > maxDot)
 			{
@@ -107,7 +105,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 		{
 			btVector3 pos(0,0,0);
 			pos[getUpAxis()] = -getHalfHeight();
-			vtx = pos +vec*(radius) - vec * getMargin();
+			vtx = pos;
 			newDot = vec.dot(vtx);
 			if (newDot > maxDot)
 			{
@@ -133,11 +131,9 @@ void	btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) con
 	btVector3 halfExtents(radius,radius,radius);
 	halfExtents[getUpAxis()]+=getHalfHeight();
 
-	btScalar margin = CONVEX_DISTANCE_MARGIN;
-
-	btScalar lx=btScalar(2.)*(halfExtents[0]+margin);
-	btScalar ly=btScalar(2.)*(halfExtents[1]+margin);
-	btScalar lz=btScalar(2.)*(halfExtents[2]+margin);
+	btScalar lx=btScalar(2.)*(halfExtents[0]);
+	btScalar ly=btScalar(2.)*(halfExtents[1]);
+	btScalar lz=btScalar(2.)*(halfExtents[2]);
 	const btScalar x2 = lx*lx;
 	const btScalar y2 = ly*ly;
 	const btScalar z2 = lz*lz;
@@ -151,6 +147,7 @@ void	btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) con
 
 btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height)
 {
+	m_collisionMargin = radius;
 	m_upAxis = 0;
 	m_implicitShapeDimensions.setValue(0.5f*height, radius,radius);
 }
@@ -162,6 +159,7 @@ btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height)
 
 btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height)
 {
+	m_collisionMargin = radius;
 	m_upAxis = 2;
 	m_implicitShapeDimensions.setValue(radius,radius,0.5f*height);
 }

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.h

@@ -48,21 +48,13 @@ public:
 	
 	virtual void setMargin(btScalar collisionMargin)
 	{
-		//correct the m_implicitShapeDimensions for the margin
-		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
-		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
-		
-		btConvexInternalShape::setMargin(collisionMargin);
-		btVector3 newMargin(getMargin(),getMargin(),getMargin());
-		m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
-
+		//don't override the margin for capsules, their entire radius == margin
 	}
 
 	virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 	{
 			btVector3 halfExtents(getRadius(),getRadius(),getRadius());
 			halfExtents[m_upAxis] = getRadius() + getHalfHeight();
-			halfExtents += btVector3(getMargin(),getMargin(),getMargin());
 			btMatrix3x3 abs_b = t.getBasis().absolute();  
 			btVector3 center = t.getOrigin();
             btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
@@ -94,14 +86,12 @@ public:
 
 	virtual void	setLocalScaling(const btVector3& scaling)
 	{
-		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
-		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
-		btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
-
-		btConvexInternalShape::setLocalScaling(scaling);
-
-		m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
-
+		btVector3 unScaledImplicitShapeDimensions = m_implicitShapeDimensions / m_localScaling;
+                btConvexInternalShape::setLocalScaling(scaling);
+		m_implicitShapeDimensions = (unScaledImplicitShapeDimensions * scaling);
+		//update m_collisionMargin, since entire radius==margin
+		int radiusAxis = (m_upAxis+2)%3;
+		m_collisionMargin = m_implicitShapeDimensions[radiusAxis];
 	}
 
 	virtual btVector3	getAnisotropicRollingFrictionDirection() const
@@ -174,11 +164,17 @@ SIMD_FORCE_INLINE	int	btCapsuleShape::calculateSerializeBufferSize() const
 SIMD_FORCE_INLINE	const char*	btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer;
-	
+
 	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
 
 	shapeData->m_upAxis = m_upAxis;
-	
+
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding[0] = 0;
+	shapeData->m_padding[1] = 0;
+	shapeData->m_padding[2] = 0;
+	shapeData->m_padding[3] = 0;
+
 	return "btCapsuleShapeData";
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.cpp

@@ -106,7 +106,10 @@ const char*	btCollisionShape::serialize(void* dataBuffer, btSerializer* serializ
 		serializer->serializeName(name);
 	}
 	shapeData->m_shapeType = m_shapeType;
-	//shapeData->m_padding//??
+
+	// Fill padding with zeros to appease msan.
+	memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding));
+
 	return "btCollisionShapeData";
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConeShape.h

@@ -168,11 +168,17 @@ SIMD_FORCE_INLINE	int	btConeShape::calculateSerializeBufferSize() const
 SIMD_FORCE_INLINE	const char*	btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btConeShapeData* shapeData = (btConeShapeData*) dataBuffer;
-	
+
 	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
-	
+
 	shapeData->m_upIndex = m_coneIndices[1];
-	
+
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding[0] = 0;
+	shapeData->m_padding[1] = 0;
+	shapeData->m_padding[2] = 0;
+	shapeData->m_padding[3] = 0;
+
 	return "btConeShapeData";
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp

@@ -211,7 +211,10 @@ const char*	btConvexHullShape::serialize(void* dataBuffer, btSerializer* seriali
 		}
 		serializer->finalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]);
 	}
-	
+
+	// Fill padding with zeros to appease msan.
+	memset(shapeData->m_padding3, 0, sizeof(shapeData->m_padding3));
+
 	return "btConvexHullShapeData";
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.h

@@ -172,6 +172,9 @@ SIMD_FORCE_INLINE	const char*	btConvexInternalShape::serialize(void* dataBuffer,
 	m_localScaling.serializeFloat(shapeData->m_localScaling);
 	shapeData->m_collisionMargin = float(m_collisionMargin);
 
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding = 0;
+
 	return "btConvexInternalShapeData";
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp

@@ -230,14 +230,13 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV
 		btScalar halfHeight = capsuleShape->getHalfHeight();
 		int capsuleUpAxis = capsuleShape->getUpAxis();
 
-		btScalar radius = capsuleShape->getRadius();
 		btVector3 supVec(0,0,0);
 
 		btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
 
 		btVector3 vec = vec0;
 		btScalar lenSqr = vec.length2();
-		if (lenSqr < btScalar(0.0001))
+		if (lenSqr < SIMD_EPSILON*SIMD_EPSILON)
 		{
 			vec.setValue(1,0,0);
 		} else
@@ -251,8 +250,7 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV
 			btVector3 pos(0,0,0);
 			pos[capsuleUpAxis] = halfHeight;
 
-			//vtx = pos +vec*(radius);
-			vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
+			vtx = pos;
 			newDot = vec.dot(vtx);
 			
 
@@ -266,8 +264,7 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV
 			btVector3 pos(0,0,0);
 			pos[capsuleUpAxis] = -halfHeight;
 
-			//vtx = pos +vec*(radius);
-			vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
+			vtx = pos;
 			newDot = vec.dot(vtx);
 			if (newDot > maxDot)
 			{
@@ -427,7 +424,6 @@ void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin,
 		btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius());
 		int m_upAxis = capsuleShape->getUpAxis();
 		halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
-		halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual());
 		btMatrix3x3 abs_b = t.getBasis().absolute();  
 		btVector3 center = t.getOrigin();
         btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);    

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.cpp

@@ -19,10 +19,11 @@ btCylinderShape::btCylinderShape (const btVector3& halfExtents)
 :btConvexInternalShape(),
 m_upAxis(1)
 {
-	setSafeMargin(halfExtents);
-
 	btVector3 margin(getMargin(),getMargin(),getMargin());
 	m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin;
+
+	setSafeMargin(halfExtents);
+
 	m_shapeType = CYLINDER_SHAPE_PROXYTYPE;
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.h

@@ -199,11 +199,17 @@ SIMD_FORCE_INLINE	int	btCylinderShape::calculateSerializeBufferSize() const
 SIMD_FORCE_INLINE	const char*	btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btCylinderShapeData* shapeData = (btCylinderShapeData*) dataBuffer;
-	
+
 	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
 
 	shapeData->m_upAxis = m_upAxis;
-	
+
+	// Fill padding with zeros to appease msan.
+	shapeData->m_padding[0] = 0;
+	shapeData->m_padding[1] = 0;
+	shapeData->m_padding[2] = 0;
+	shapeData->m_padding[3] = 0;
+
 	return "btCylinderShapeData";
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp

@@ -55,6 +55,23 @@ btScalar	btMinkowskiSumShape::getMargin() const
 void	btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
 {
 	(void)mass;
-	btAssert(0);
-	inertia.setValue(0,0,0);
+	//inertia of the AABB of the Minkowski sum
+	btTransform identity;
+	identity.setIdentity();
+	btVector3 aabbMin,aabbMax;
+	getAabb(identity,aabbMin,aabbMax);
+
+	btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
+
+	btScalar margin = getMargin();
+
+	btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
+	btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
+	btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
+	const btScalar x2 = lx*lx;
+	const btScalar y2 = ly*ly;
+	const btScalar z2 = lz*lz;
+	const btScalar scaledmass = mass * btScalar(0.08333333);
+
+	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
 }

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp

@@ -175,7 +175,10 @@ const char*	btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serial
 		}
 		serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]);
 	}
-	
+
+	// Fill padding with zeros to appease msan.
+	memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding));
+
 	return "btMultiSphereShapeData";
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h

@@ -93,10 +93,12 @@ protected:
 		aabbMax = m_localAabbMax;
 	}
 
-public:
+protected:
 
 	btPolyhedralConvexAabbCachingShape();
 	
+public:
+	
 	inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const
 	{
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btSphereShape.h

@@ -29,8 +29,11 @@ public:
 	btSphereShape (btScalar radius) : btConvexInternalShape ()
 	{
 		m_shapeType = SPHERE_SHAPE_PROXYTYPE;
+		m_localScaling.setValue(1.0, 1.0, 1.0);
+		m_implicitShapeDimensions.setZero();
 		m_implicitShapeDimensions.setX(radius);
 		m_collisionMargin = radius;
+		m_padding = 0;
 	}
 	
 	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h

@@ -94,7 +94,13 @@ SIMD_FORCE_INLINE	const char*	btStaticPlaneShape::serialize(void* dataBuffer, bt
 	m_localScaling.serializeFloat(planeData->m_localScaling);
 	m_planeNormal.serializeFloat(planeData->m_planeNormal);
 	planeData->m_planeConstant = float(m_planeConstant);
-		
+
+	// Fill padding with zeros to appease msan.
+	planeData->m_pad[0] = 0;
+	planeData->m_pad[1] = 0;
+	planeData->m_pad[2] = 0;
+	planeData->m_pad[3] = 0;
+
 	return "btStaticPlaneShapeData";
 }
 

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp

@@ -293,6 +293,9 @@ const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s
 							tmpIndices[gfxindex].m_values[0] = tri_indices[0];
 							tmpIndices[gfxindex].m_values[1] = tri_indices[1];
 							tmpIndices[gfxindex].m_values[2] = tri_indices[2];
+							// Fill padding with zeros to appease msan.
+							tmpIndices[gfxindex].m_pad[0] = 0;
+							tmpIndices[gfxindex].m_pad[1] = 0;
 						}
 						serializer->finalizeChunk(chunk,"btShortIntIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
 					}
@@ -311,6 +314,8 @@ const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s
 							tmpIndices[gfxindex].m_values[0] = tri_indices[0];
 							tmpIndices[gfxindex].m_values[1] = tri_indices[1];
 							tmpIndices[gfxindex].m_values[2] = tri_indices[2];
+							// Fill padding with zeros to appease msan.
+							tmpIndices[gfxindex].m_pad = 0;
 						}
 						serializer->finalizeChunk(chunk,"btCharIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr);
 					}
@@ -375,6 +380,8 @@ const char*	btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s
 		serializer->finalizeChunk(chunk,"btMeshPartData",BT_ARRAY_CODE,chunk->m_oldPtr);
 	}
 
+	// Fill padding with zeros to appease msan.
+	memset(trimeshData->m_padding, 0, sizeof(trimeshData->m_padding));
 
 	m_scaling.serializeFloat(trimeshData->m_scaling);
 	return "btStridingMeshInterfaceData";

Engine/lib/bullet/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h

@@ -195,6 +195,13 @@ SIMD_FORCE_INLINE	const char*	btTriangleInfoMap::serialize(void* dataBuffer, btS
 		serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*) &m_keyArray[0]);
 
 	}
+
+	// Fill padding with zeros to appease msan.
+	tmapData->m_padding[0] = 0;
+	tmapData->m_padding[1] = 0;
+	tmapData->m_padding[2] = 0;
+	tmapData->m_padding[3] = 0;
+
 	return "btTriangleInfoMapData";
 }
 

Engine/lib/bullet/src/BulletCollision/Gimpact/btCompoundFromGimpact.h

@@ -5,6 +5,22 @@
 #include "btGImpactShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
 
+ATTRIBUTE_ALIGNED16(class) btCompoundFromGimpactShape	: public btCompoundShape
+{
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	virtual ~btCompoundFromGimpactShape()
+	{
+		/*delete all the btBU_Simplex1to4 ChildShapes*/
+		for (int i = 0; i < m_children.size(); i++)
+		{
+			delete m_children[i].m_childShape;
+		}
+	}
+
+};
+
 struct MyCallback : public btTriangleRaycastCallback
 		{
 			int	m_ignorePart;
@@ -77,7 +93,7 @@ struct MyCallback : public btTriangleRaycastCallback
 		
 btCompoundShape*	btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth)
 {
-	btCompoundShape* colShape = new btCompoundShape();
+	btCompoundShape* colShape = new btCompoundFromGimpactShape();
 		
 		btTransform tr;
 		tr.setIdentity();
@@ -90,4 +106,4 @@ btCompoundShape*	btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimp
 	return colShape;	
 }	
 
-#endif //BT_COMPOUND_FROM_GIMPACT
+#endif //BT_COMPOUND_FROM_GIMPACT

Engine/lib/bullet/src/BulletCollision/Gimpact/btContactProcessing.h

@@ -27,86 +27,7 @@ subject to the following restrictions:
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "btTriangleShapeEx.h"
-
-
-
-/**
-Configuration var for applying interpolation of  contact normals
-*/
-#define NORMAL_CONTACT_AVERAGE 1
-
-#define CONTACT_DIFF_EPSILON 0.00001f
-
-///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint.
-///@todo: remove and replace GIM_CONTACT by btManifoldPoint.
-class GIM_CONTACT
-{
-public:
-    btVector3 m_point;
-    btVector3 m_normal;
-    btScalar m_depth;//Positive value indicates interpenetration
-    btScalar m_distance;//Padding not for use
-    int m_feature1;//Face number
-    int m_feature2;//Face number
-public:
-    GIM_CONTACT()
-    {
-    }
-
-    GIM_CONTACT(const GIM_CONTACT & contact):
-				m_point(contact.m_point),
-				m_normal(contact.m_normal),
-				m_depth(contact.m_depth),
-				m_feature1(contact.m_feature1),
-				m_feature2(contact.m_feature2)
-    {
-    }
-
-    GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
-    	 			btScalar depth, int feature1, int feature2):
-				m_point(point),
-				m_normal(normal),
-				m_depth(depth),
-				m_feature1(feature1),
-				m_feature2(feature2)
-    {
-    }
-
-	//! Calcs key for coord classification
-    SIMD_FORCE_INLINE unsigned int calc_key_contact() const
-    {
-    	int _coords[] = {
-    		(int)(m_point[0]*1000.0f+1.0f),
-    		(int)(m_point[1]*1333.0f),
-    		(int)(m_point[2]*2133.0f+3.0f)};
-		unsigned int _hash=0;
-		unsigned int *_uitmp = (unsigned int *)(&_coords[0]);
-		_hash = *_uitmp;
-		_uitmp++;
-		_hash += (*_uitmp)<<4;
-		_uitmp++;
-		_hash += (*_uitmp)<<8;
-		return _hash;
-    }
-
-    SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count)
-    {
-    	btVector3 vec_sum(m_normal);
-		for(int i=0;i<normal_count;i++)
-		{
-			vec_sum += normals[i];
-		}
-
-		btScalar vec_sum_len = vec_sum.length2();
-		if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
-
-		//GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
-
-		m_normal = vec_sum/btSqrt(vec_sum_len);
-    }
-
-};
-
+#include "btContactProcessingStructs.h"
 
 class btContactArray:public btAlignedObjectArray<GIM_CONTACT>
 {
@@ -141,5 +62,4 @@ public:
 	void merge_contacts_unique(const btContactArray & contacts);
 };
 
-
 #endif // GIM_CONTACT_H_INCLUDED

Engine/lib/bullet/src/BulletCollision/Gimpact/btContactProcessingStructs.h

@@ -0,0 +1,109 @@
+#ifndef BT_CONTACT_H_STRUCTS_INCLUDED
+#define BT_CONTACT_H_STRUCTS_INCLUDED
+
+/*! \file gim_contact.h
+\author Francisco Leon Najera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.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.
+*/
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btTriangleShapeEx.h"
+
+
+/**
+Configuration var for applying interpolation of  contact normals
+*/
+#define NORMAL_CONTACT_AVERAGE 1
+
+#define CONTACT_DIFF_EPSILON 0.00001f
+
+///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint.
+///@todo: remove and replace GIM_CONTACT by btManifoldPoint.
+class GIM_CONTACT
+{
+public:
+    btVector3 m_point;
+    btVector3 m_normal;
+    btScalar m_depth;//Positive value indicates interpenetration
+    btScalar m_distance;//Padding not for use
+    int m_feature1;//Face number
+    int m_feature2;//Face number
+public:
+    GIM_CONTACT()
+    {
+    }
+
+    GIM_CONTACT(const GIM_CONTACT & contact):
+				m_point(contact.m_point),
+				m_normal(contact.m_normal),
+				m_depth(contact.m_depth),
+				m_feature1(contact.m_feature1),
+				m_feature2(contact.m_feature2)
+    {
+    }
+
+    GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
+    	 			btScalar depth, int feature1, int feature2):
+				m_point(point),
+				m_normal(normal),
+				m_depth(depth),
+				m_feature1(feature1),
+				m_feature2(feature2)
+    {
+    }
+
+	//! Calcs key for coord classification
+    SIMD_FORCE_INLINE unsigned int calc_key_contact() const
+    {
+    	int _coords[] = {
+    		(int)(m_point[0]*1000.0f+1.0f),
+    		(int)(m_point[1]*1333.0f),
+    		(int)(m_point[2]*2133.0f+3.0f)};
+		unsigned int _hash=0;
+		unsigned int *_uitmp = (unsigned int *)(&_coords[0]);
+		_hash = *_uitmp;
+		_uitmp++;
+		_hash += (*_uitmp)<<4;
+		_uitmp++;
+		_hash += (*_uitmp)<<8;
+		return _hash;
+    }
+
+    SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count)
+    {
+    	btVector3 vec_sum(m_normal);
+		for(int i=0;i<normal_count;i++)
+		{
+			vec_sum += normals[i];
+		}
+
+		btScalar vec_sum_len = vec_sum.length2();
+		if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
+
+		//GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
+
+		m_normal = vec_sum/btSqrt(vec_sum_len);
+    }
+
+};
+
+#endif // BT_CONTACT_H_STRUCTS_INCLUDED

Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactBvh.h

@@ -29,31 +29,7 @@ subject to the following restrictions:
 
 #include "btBoxCollision.h"
 #include "btTriangleShapeEx.h"
-
-
-
-
-
-//! Overlapping pair
-struct GIM_PAIR
-{
-    int m_index1;
-    int m_index2;
-    GIM_PAIR()
-    {}
-
-    GIM_PAIR(const GIM_PAIR & p)
-    {
-    	m_index1 = p.m_index1;
-    	m_index2 = p.m_index2;
-	}
-
-	GIM_PAIR(int index1, int index2)
-    {
-    	m_index1 = index1;
-    	m_index2 = index2;
-	}
-};
+#include "btGImpactBvhStructs.h"
 
 //! A pairset array
 class btPairSet: public btAlignedObjectArray<GIM_PAIR>
@@ -74,59 +50,6 @@ public:
 	}
 };
 
-
-///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box
-struct GIM_BVH_DATA
-{
-	btAABB m_bound;
-	int m_data;
-};
-
-//! Node Structure for trees
-class GIM_BVH_TREE_NODE
-{
-public:
-	btAABB m_bound;
-protected:
-	int	m_escapeIndexOrDataIndex;
-public:
-	GIM_BVH_TREE_NODE()
-	{
-		m_escapeIndexOrDataIndex = 0;
-	}
-
-	SIMD_FORCE_INLINE bool isLeafNode() const
-	{
-		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
-		return (m_escapeIndexOrDataIndex>=0);
-	}
-
-	SIMD_FORCE_INLINE int getEscapeIndex() const
-	{
-		//btAssert(m_escapeIndexOrDataIndex < 0);
-		return -m_escapeIndexOrDataIndex;
-	}
-
-	SIMD_FORCE_INLINE void setEscapeIndex(int index)
-	{
-		m_escapeIndexOrDataIndex = -index;
-	}
-
-	SIMD_FORCE_INLINE int getDataIndex() const
-	{
-		//btAssert(m_escapeIndexOrDataIndex >= 0);
-
-		return m_escapeIndexOrDataIndex;
-	}
-
-	SIMD_FORCE_INLINE void setDataIndex(int index)
-	{
-		m_escapeIndexOrDataIndex = index;
-	}
-
-};
-
-
 class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray<GIM_BVH_DATA>
 {
 };
@@ -392,5 +315,4 @@ public:
 		btPairSet & collision_pairs);
 };
 
-
 #endif // GIM_BOXPRUNING_H_INCLUDED

Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactBvhStructs.h

@@ -0,0 +1,105 @@
+#ifndef GIM_BOX_SET_STRUCT_H_INCLUDED
+#define GIM_BOX_SET_STRUCT_H_INCLUDED
+
+/*! \file gim_box_set.h
+\author Francisco Leon Najera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.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.
+*/
+
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btBoxCollision.h"
+#include "btTriangleShapeEx.h"
+
+//! Overlapping pair
+struct GIM_PAIR
+{
+    int m_index1;
+    int m_index2;
+    GIM_PAIR()
+    {}
+
+    GIM_PAIR(const GIM_PAIR & p)
+    {
+    	m_index1 = p.m_index1;
+    	m_index2 = p.m_index2;
+	}
+
+	GIM_PAIR(int index1, int index2)
+    {
+    	m_index1 = index1;
+    	m_index2 = index2;
+	}
+};
+
+///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box
+struct GIM_BVH_DATA
+{
+	btAABB m_bound;
+	int m_data;
+};
+
+//! Node Structure for trees
+class GIM_BVH_TREE_NODE
+{
+public:
+	btAABB m_bound;
+protected:
+	int	m_escapeIndexOrDataIndex;
+public:
+	GIM_BVH_TREE_NODE()
+	{
+		m_escapeIndexOrDataIndex = 0;
+	}
+
+	SIMD_FORCE_INLINE bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrDataIndex>=0);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex < 0);
+		return -m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setEscapeIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = -index;
+	}
+
+	SIMD_FORCE_INLINE int getDataIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex >= 0);
+
+		return m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setDataIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = index;
+	}
+
+};
+
+#endif // GIM_BOXPRUNING_H_INCLUDED

Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h

@@ -26,73 +26,7 @@ subject to the following restrictions:
 
 #include "btGImpactBvh.h"
 #include "btQuantization.h"
-
-
-
-
-
-///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
-///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
-ATTRIBUTE_ALIGNED16	(struct) BT_QUANTIZED_BVH_NODE
-{
-	//12 bytes
-	unsigned short int	m_quantizedAabbMin[3];
-	unsigned short int	m_quantizedAabbMax[3];
-	//4 bytes
-	int	m_escapeIndexOrDataIndex;
-
-	BT_QUANTIZED_BVH_NODE()
-	{
-		m_escapeIndexOrDataIndex = 0;
-	}
-
-	SIMD_FORCE_INLINE bool isLeafNode() const
-	{
-		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
-		return (m_escapeIndexOrDataIndex>=0);
-	}
-
-	SIMD_FORCE_INLINE int getEscapeIndex() const
-	{
-		//btAssert(m_escapeIndexOrDataIndex < 0);
-		return -m_escapeIndexOrDataIndex;
-	}
-
-	SIMD_FORCE_INLINE void setEscapeIndex(int index)
-	{
-		m_escapeIndexOrDataIndex = -index;
-	}
-
-	SIMD_FORCE_INLINE int getDataIndex() const
-	{
-		//btAssert(m_escapeIndexOrDataIndex >= 0);
-
-		return m_escapeIndexOrDataIndex;
-	}
-
-	SIMD_FORCE_INLINE void setDataIndex(int index)
-	{
-		m_escapeIndexOrDataIndex = index;
-	}
-
-	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
-		unsigned short * quantizedMin,unsigned short * quantizedMax) const
-	{
-		if(m_quantizedAabbMin[0] > quantizedMax[0] ||
-		   m_quantizedAabbMax[0] < quantizedMin[0] ||
-		   m_quantizedAabbMin[1] > quantizedMax[1] ||
-		   m_quantizedAabbMax[1] < quantizedMin[1] ||
-		   m_quantizedAabbMin[2] > quantizedMax[2] ||
-		   m_quantizedAabbMax[2] < quantizedMin[2])
-		{
-			return false;
-		}
-		return true;
-	}
-
-};
-
-
+#include "btGImpactQuantizedBvhStructs.h"
 
 class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE>
 {
@@ -368,5 +302,4 @@ public:
 		btPairSet & collision_pairs);
 };
 
-
 #endif // GIM_BOXPRUNING_H_INCLUDED

Engine/lib/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h

@@ -0,0 +1,91 @@
+#ifndef GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED
+#define GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED
+
+/*! \file btGImpactQuantizedBvh.h
+\author Francisco Leon Najera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.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.
+*/
+
+#include "btGImpactBvh.h"
+#include "btQuantization.h"
+
+///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+ATTRIBUTE_ALIGNED16	(struct) BT_QUANTIZED_BVH_NODE
+{
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes
+	int	m_escapeIndexOrDataIndex;
+
+	BT_QUANTIZED_BVH_NODE()
+	{
+		m_escapeIndexOrDataIndex = 0;
+	}
+
+	SIMD_FORCE_INLINE bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrDataIndex>=0);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex < 0);
+		return -m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setEscapeIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = -index;
+	}
+
+	SIMD_FORCE_INLINE int getDataIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex >= 0);
+
+		return m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setDataIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = index;
+	}
+
+	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
+		unsigned short * quantizedMin,unsigned short * quantizedMax) const
+	{
+		if(m_quantizedAabbMin[0] > quantizedMax[0] ||
+		   m_quantizedAabbMax[0] < quantizedMin[0] ||
+		   m_quantizedAabbMin[1] > quantizedMax[1] ||
+		   m_quantizedAabbMax[1] < quantizedMin[1] ||
+		   m_quantizedAabbMin[2] > quantizedMax[2] ||
+		   m_quantizedAabbMax[2] < quantizedMin[2])
+		{
+			return false;
+		}
+		return true;
+	}
+
+};
+
+#endif // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED

Engine/lib/bullet/src/BulletCollision/Gimpact/gim_array.h

@@ -228,7 +228,7 @@ public:
 	inline void push_back_memcpy(const T & obj)
 	{
 	    this->growingCheck();
-	    irr_simd_memcpy(&m_data[m_size],&obj,sizeof(T));
+	    gim_simd_memcpy(&m_data[m_size],&obj,sizeof(T));
 	    m_size++;
 	}
 

Engine/lib/bullet/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h

@@ -41,10 +41,13 @@ email: projectileman@yahoo.com
 
 
 
-
+#ifndef PLANEDIREPSILON
 #define PLANEDIREPSILON 0.0000001f
-#define PARALELENORMALS 0.000001f
+#endif
 
+#ifndef PARALELENORMALS
+#define PARALELENORMALS 0.000001f
+#endif
 
 #define TRIANGLE_NORMAL(v1,v2,v3,n)\
 {\

Engine/lib/bullet/src/BulletCollision/Gimpact/gim_box_collision.h

@@ -97,6 +97,8 @@ email: projectileman@yahoo.com
 //	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
 //}
 
+#ifndef TEST_CROSS_EDGE_BOX_MCR
+
 #define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
 {\
 	const btScalar dir0 = -edge[i_dir_0];\
@@ -113,6 +115,7 @@ email: projectileman@yahoo.com
 	if(pmin>rad || -rad>pmax) return false;\
 }\
 
+#endif
 
 #define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
 {\
@@ -190,8 +193,9 @@ public:
 	}
 };
 
-
+#ifndef BOX_PLANE_EPSILON
 #define BOX_PLANE_EPSILON 0.000001f
+#endif
 
 //! Axis aligned box
 class GIM_AABB
@@ -571,7 +575,7 @@ public:
 	}
 };
 
-
+#ifndef BT_BOX_COLLISION_H_INCLUDED
 //! Compairison of transformation objects
 SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
 {
@@ -582,6 +586,7 @@ SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btT
 	if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
 	return true;
 }
+#endif
 
 
 

Engine/lib/bullet/src/BulletCollision/Gimpact/gim_contact.h

@@ -40,8 +40,15 @@ email: projectileman@yahoo.com
 /**
 Configuration var for applying interpolation of  contact normals
 */
+#ifndef NORMAL_CONTACT_AVERAGE
 #define NORMAL_CONTACT_AVERAGE 1
+#endif
+
+#ifndef CONTACT_DIFF_EPSILON
 #define CONTACT_DIFF_EPSILON 0.00001f
+#endif
+
+#ifndef BT_CONTACT_H_STRUCTS_INCLUDED
 
 /// Structure for collision results
 ///Functions for managing and sorting contacts resulting from a collision query.
@@ -121,6 +128,7 @@ public:
 
 };
 
+#endif
 
 class gim_contact_array:public gim_array<GIM_CONTACT>
 {

Engine/lib/bullet/src/BulletCollision/Gimpact/gim_tri_collision.h

@@ -38,8 +38,9 @@ email: projectileman@yahoo.com
 
 
 
-
+#ifndef MAX_TRI_CLIPPING
 #define MAX_TRI_CLIPPING 16
+#endif
 
 //! Structure for collision
 struct GIM_TRIANGLE_CONTACT_DATA

Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h

@@ -67,10 +67,12 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
 		btVector3	m_closestPointInB;
 		btScalar	m_distance; //negative means penetration !
 
+    protected:
 		btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT))
 		{
-
 		}
+		
+	public:
 		virtual ~btStorageResult() {};
 
 		virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)

Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -41,6 +41,7 @@ enum btContactPointFlags
 	BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
 	BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
     BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
+	BT_CONTACT_FLAG_FRICTION_ANCHOR = 16,
 };
 
 /// ManifoldContactPoint collects and maintains persistent contactpoints.

Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

@@ -21,6 +21,8 @@ subject to the following restrictions:
 btScalar					gContactBreakingThreshold = btScalar(0.02);
 ContactDestroyedCallback	gContactDestroyedCallback = 0;
 ContactProcessedCallback	gContactProcessedCallback = 0;
+ContactStartedCallback		gContactStartedCallback = 0;
+ContactEndedCallback		gContactEndedCallback = 0;
 ///gContactCalcArea3Points will approximate the convex hull area using 3 points
 ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower
 bool						gContactCalcArea3Points = true;
@@ -279,6 +281,7 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT
 			removeContactPoint(i);
 		} else
 		{
+            //todo: friction anchor may require the contact to be around a bit longer
 			//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
 			projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
 			projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;

Engine/lib/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -28,10 +28,18 @@ struct btCollisionResult;
 ///maximum contact breaking and merging threshold
 extern btScalar gContactBreakingThreshold;
 
+#ifndef SWIG
+class btPersistentManifold;
+
 typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
 typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1);
+typedef void (*ContactStartedCallback)(btPersistentManifold* const &manifold);
+typedef void (*ContactEndedCallback)(btPersistentManifold* const &manifold);
 extern ContactDestroyedCallback	gContactDestroyedCallback;
 extern ContactProcessedCallback gContactProcessedCallback;
+extern ContactStartedCallback gContactStartedCallback;
+extern ContactEndedCallback gContactEndedCallback;
+#endif //SWIG
 
 //the enum starts at 1024 to avoid type conflicts with btTypedConstraint
 enum btContactManifoldTypes
@@ -51,8 +59,8 @@ enum btContactManifoldTypes
 ///note that some pairs of objects might have more then one contact manifold.
 
 
-ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject
-//ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject
+//ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject
+ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject
 {
 
 	btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
@@ -171,40 +179,60 @@ public:
 
 		btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0);
 		m_cachedPoints--;
+
+		if (gContactEndedCallback && m_cachedPoints == 0)
+		{
+			gContactEndedCallback(this);
+		}
 	}
-	void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
+	void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex)
 	{
 		btAssert(validContactDistance(newPoint));
 
 #define MAINTAIN_PERSISTENCY 1
 #ifdef MAINTAIN_PERSISTENCY
-		int	lifeTime = m_pointCache[insertIndex].getLifeTime();
-		btScalar	appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
-		btScalar	appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
-		btScalar	appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
-//		bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized;
-		
-		
-			
-		btAssert(lifeTime>=0);
-		void* cache = m_pointCache[insertIndex].m_userPersistentData;
-		
-		m_pointCache[insertIndex] = newPoint;
-		m_pointCache[insertIndex].m_userPersistentData = cache;
-		m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
-		m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
-		m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
-		
+		int lifeTime = m_pointCache[insertIndex].getLifeTime();
+		btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
+		btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
+		btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
+
+		bool replacePoint = true;
+		///we keep existing contact points for friction anchors
+		///if the friction force is within the Coulomb friction cone
+		if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
+		{
+			//   printf("appliedImpulse=%f\n", appliedImpulse);
+			//   printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1);
+			//   printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2);
+			//   printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction);
+			btScalar mu = m_pointCache[insertIndex].m_combinedFriction;
+			btScalar eps = 0;  //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7
+			btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2;
+			btScalar b = eps + mu * appliedImpulse;
+			b = b * b;
+			replacePoint = (a) > (b);
+		}
+
+		if (replacePoint)
+		{
+			btAssert(lifeTime >= 0);
+			void* cache = m_pointCache[insertIndex].m_userPersistentData;
+
+			m_pointCache[insertIndex] = newPoint;
+			m_pointCache[insertIndex].m_userPersistentData = cache;
+			m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
+			m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
+			m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
+		}
 
 		m_pointCache[insertIndex].m_lifeTime = lifeTime;
 #else
 		clearUserCache(m_pointCache[insertIndex]);
 		m_pointCache[insertIndex] = newPoint;
-	
+
 #endif
 	}
 
-	
 	bool validContactDistance(const btManifoldPoint& pt) const
 	{
 		return pt.m_distance1 <= getContactBreakingThreshold();
@@ -220,6 +248,11 @@ public:
 		{
 			clearUserCache(m_pointCache[i]);
 		}
+
+		if (gContactEndedCallback && m_cachedPoints)
+		{
+			gContactEndedCallback(this);
+		}
 		m_cachedPoints = 0;
 	}
 

Engine/lib/bullet/src/BulletDynamics/CMakeLists.txt

@@ -37,6 +37,7 @@ SET(BulletDynamics_SRCS
 	Featherstone/btMultiBodyFixedConstraint.cpp
 	Featherstone/btMultiBodySliderConstraint.cpp
 	Featherstone/btMultiBodyJointMotor.cpp
+	Featherstone/btMultiBodyGearConstraint.cpp
 	MLCPSolvers/btDantzigLCP.cpp
 	MLCPSolvers/btMLCPSolver.cpp
 	MLCPSolvers/btLemkeAlgorithm.cpp
@@ -98,6 +99,7 @@ SET(Featherstone_HDRS
 	Featherstone/btMultiBodyFixedConstraint.h
 	Featherstone/btMultiBodySliderConstraint.h
 	Featherstone/btMultiBodyJointMotor.h
+	Featherstone/btMultiBodyGearConstraint.h	
 )
 
 SET(MLCPSolvers_HDRS

Engine/lib/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp

@@ -137,8 +137,6 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
 	m_ghostObject = ghostObject;
 	m_up.setValue(0.0f, 0.0f, 1.0f);
 	m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
-	setUp(up);
-	setStepHeight(stepHeight);
 	m_addedMargin = 0.02;
 	m_walkDirection.setValue(0.0,0.0,0.0);
 	m_AngVel.setValue(0.0, 0.0, 0.0);
@@ -156,13 +154,16 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
 	m_wasOnGround = false;
 	m_wasJumping = false;
 	m_interpolateUp = true;
-	setMaxSlope(btRadians(45.0));
 	m_currentStepOffset = 0.0;
 	m_maxPenetrationDepth = 0.2;
 	full_drop = false;
 	bounce_fix = false;
 	m_linearDamping = btScalar(0.0);
 	m_angularDamping = btScalar(0.0);
+
+	setUp(up);
+	setStepHeight(stepHeight);
+	setMaxSlope(btRadians(45.0));
 }
 
 btKinematicCharacterController::~btKinematicCharacterController ()
@@ -559,7 +560,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		break;
 	}
 
-	if (m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) || runonce == true)
+	if ((m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))) || runonce == true)
 	{
 		// we dropped a fraction of the height -> hit floor
 		btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
@@ -657,7 +658,7 @@ void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity
 		if (c != 0)
 		{
 			//there is a component in walkdirection for vertical velocity
-			btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length());
+			btVector3 upComponent = m_up * (btSin(SIMD_HALF_PI - btAcos(c)) * m_walkDirection.length());
 			m_walkDirection -= upComponent;
 			m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
 			
@@ -751,7 +752,7 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	m_wasOnGround = onGround();
 
 	//btVector3 lvel = m_walkDirection;
-	btScalar c = 0.0f;
+	//btScalar c = 0.0f;
 	
 	if (m_walkDirection.length2() > 0)
 	{

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -642,7 +642,7 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 		btTransform trDeltaAB = trB * trPose * trA.inverse();
 		btQuaternion qDeltaAB = trDeltaAB.getRotation();
 		btVector3 swingAxis = 	btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
-		float swingAxisLen2 = swingAxis.length2();
+		btScalar swingAxisLen2 = swingAxis.length2();
 		if(btFuzzyZero(swingAxisLen2))
 		{
 		   return;
@@ -903,7 +903,7 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc
 	//  a^2   b^2
 	// Do the math and it should be clear.
 
-	float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
+	btScalar swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
 	if (fabs(xEllipse) > SIMD_EPSILON)
 	{
 		btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -260,7 +260,7 @@ public:
 
 	inline int getSolveSwingLimit()
 	{
-		return m_solveTwistLimit;
+		return m_solveSwingLimit;
 	}
 
 	inline btScalar getTwistLimitSign()

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h

@@ -28,11 +28,13 @@ protected:
 
 	btPersistentManifold m_contactManifold;
 
-public:
+protected:
 
 
 	btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
 
+public:
+
 	void	setContactManifold(btPersistentManifold* contactManifold);
 
 	btPersistentManifold* getContactManifold()

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -43,10 +43,13 @@ struct btContactSolverInfoData
 	btScalar	m_restitution;
 	int		m_numIterations;
 	btScalar	m_maxErrorReduction;
-	btScalar	m_sor;
-	btScalar	m_erp;//used as Baumgarte factor
-	btScalar	m_erp2;//used in Split Impulse
-	btScalar	m_globalCfm;//constraint force mixing
+	btScalar	m_sor;//successive over-relaxation term
+	btScalar	m_erp;//error reduction for non-contact constraints
+	btScalar	m_erp2;//error reduction for contact constraints
+	btScalar	m_globalCfm;//constraint force mixing for contacts and non-contacts
+	btScalar	m_frictionERP;//error reduction for friction constraints
+	btScalar	m_frictionCFM;//constraint force mixing for friction constraints
+
 	int			m_splitImpulse;
 	btScalar	m_splitImpulsePenetrationThreshold;
 	btScalar	m_splitImpulseTurnErp;
@@ -59,6 +62,7 @@ struct btContactSolverInfoData
 	btScalar	m_maxGyroscopicForce;
 	btScalar	m_singleAxisRollingFrictionThreshold;
 	btScalar	m_leastSquaresResidualThreshold;
+	btScalar	m_restitutionVelocityThreshold;
 
 };
 
@@ -79,6 +83,8 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_erp = btScalar(0.2);
 		m_erp2 = btScalar(0.2);
 		m_globalCfm = btScalar(0.);
+		m_frictionERP = btScalar(0.2);//positional friction 'anchors' are disabled by default
+		m_frictionCFM = btScalar(0.);
 		m_sor = btScalar(1.);
 		m_splitImpulse = true;
 		m_splitImpulsePenetrationThreshold = -.04f;
@@ -92,6 +98,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force
 		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
 		m_leastSquaresResidualThreshold = 0.f;
+		m_restitutionVelocityThreshold = 0.2f;//if the relative velocity is below this threshold, there is zero restitution
 	}
 };
 

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h

@@ -141,6 +141,14 @@ SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSe
 
 	gear->m_ratio = m_ratio;
 
+	// Fill padding with zeros to appease msan.
+#ifndef BT_USE_DOUBLE_PRECISION
+	gear->m_padding[0] = 0;
+	gear->m_padding[1] = 0;
+	gear->m_padding[2] = 0;
+	gear->m_padding[3] = 0;
+#endif
+
 	return btGearConstraintDataName;
 }
 

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -776,7 +776,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
 	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
 {
     int srow = row * info->rowskip;
-    int powered = limot->m_enableMotor;
+    bool powered = limot->m_enableMotor;
     int limit = limot->m_currentLimit;
     if (powered || limit)
     {   // if the joint is powered, or has joint limits, add in the extra row
@@ -840,7 +840,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
         }
         // if we're limited low and high simultaneously, the joint motor is
         // ineffective
-        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
+        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
         info->m_constraintError[srow] = btScalar(0.f);
         if (powered)
         {

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp

@@ -729,6 +729,21 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 	if (limot->m_enableMotor && limot->m_servoMotor)
 	{
 		btScalar error = limot->m_currentPosition - limot->m_servoTarget;
+		btScalar curServoTarget = limot->m_servoTarget;
+		if (rotational)
+		{
+			if (error > SIMD_PI)
+			{
+				error -= SIMD_2_PI;
+				curServoTarget +=SIMD_2_PI;
+			}
+			if (error < -SIMD_PI)
+			{
+				error += SIMD_2_PI;
+				curServoTarget -=SIMD_2_PI;
+			}
+		}
+
 		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
 		btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
 		btScalar tag_vel = -targetvelocity;
@@ -739,13 +754,13 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 			btScalar hiLimit;
 			if(limot->m_loLimit > limot->m_hiLimit)
 			{
-				lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
-				hiLimit  = error < 0 ? limot->m_servoTarget :  SIMD_INFINITY;
+				lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
+				hiLimit  = error < 0 ? curServoTarget :  SIMD_INFINITY;
 			}
 			else
 			{
-				lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit;
-				hiLimit  = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit;
+				lowLimit = error > 0 && curServoTarget>limot->m_loLimit ? curServoTarget : limot->m_loLimit;
+				hiLimit  = error < 0 && curServoTarget<limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
 			}
 			mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
 		} 
@@ -998,13 +1013,49 @@ void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar veloc
 		m_angularLimits[index - 3].m_targetVelocity = velocity;
 }
 
-void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target)
+
+
+void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg)
 {
 	btAssert((index >= 0) && (index < 6));
 	if (index<3)
-		m_linearLimits.m_servoTarget[index] = target;
+	{
+		m_linearLimits.m_servoTarget[index] = targetOrg;
+	}
 	else
+	{
+		//wrap between -PI and PI, see also
+		//https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
+
+		btScalar target = targetOrg+SIMD_PI;
+		if (1)
+		{
+			btScalar m = target - SIMD_2_PI * floor(target/SIMD_2_PI);
+			// handle boundary cases resulted from floating-point cut off:
+			{
+				if (m>=SIMD_2_PI)
+				{
+					target = 0;
+				} else
+				{
+					if (m<0 )
+					{
+						if (SIMD_2_PI+m == SIMD_2_PI)
+							target = 0;
+						else
+							target = SIMD_2_PI+m;
+					}
+					else
+					{
+						target = m;
+					}
+				}
+			}
+			target -= SIMD_PI;
+		}
+		
 		m_angularLimits[index - 3].m_servoTarget = target;
+	}
 }
 
 void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

@@ -664,6 +664,11 @@ SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* da
 
 	dof->m_rotateOrder = m_rotateOrder;
 
+	dof->m_padding1[0] = 0;
+	dof->m_padding1[1] = 0;
+	dof->m_padding1[2] = 0;
+	dof->m_padding1[3] = 0;
+
 	return btGeneric6DofSpring2ConstraintDataName;
 }
 

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -556,12 +556,8 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 
 	}
 	// if the hinge has joint limits or motor, add in the extra row
-	int powered = 0;
-	if(getEnableAngularMotor())
-	{
-		powered = 1;
-	}
-	if(limit || powered) 
+	bool powered = getEnableAngularMotor();
+	if(limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
@@ -577,7 +573,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 		btScalar histop = getUpperLimit();
 		if(limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
 		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
@@ -951,12 +947,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 
 	}
 	// if the hinge has joint limits or motor, add in the extra row
-	int powered = 0;
-	if(getEnableAngularMotor())
-	{
-		powered = 1;
-	}
-	if(limit || powered) 
+	bool powered = getEnableAngularMotor();
+	if(limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
@@ -972,7 +964,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 		btScalar histop = getUpperLimit();
 		if(limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
 		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -314,7 +314,7 @@ public:
 	{ 
 		return m_enableAngularMotor; 
 	}
-	inline btScalar getMotorTargetVelosity() 
+	inline btScalar getMotorTargetVelocity() 
 	{ 
 		return m_motorTargetVelocity; 
 	}
@@ -489,6 +489,14 @@ SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btS
 	hingeData->m_relaxationFactor = float(m_relaxationFactor);
 #endif
 
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	hingeData->m_padding1[0] = 0;
+	hingeData->m_padding1[1] = 0;
+	hingeData->m_padding1[2] = 0;
+	hingeData->m_padding1[3] = 0;
+#endif
+
 	return btHingeConstraintDataName;
 }
 

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp

@@ -78,20 +78,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 
 
 	btScalar deltaflengthsqr = 0;
-
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
-	{
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations) 
-			{
-				btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-				m_deltafNC[j] = deltaf;
-				deltaflengthsqr += deltaf * deltaf;
-			}
-		}
-	} else 
 	{
 		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
 		{
@@ -141,7 +127,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 
 
 
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
 	{
 
 		if (iteration< infoGlobal.m_numIterations)
@@ -158,7 +143,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 				}
 			}
 
-			///solve all contact constraints using SIMD, if available
+			///solve all contact constraints
 			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
 			{
 				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
@@ -170,7 +155,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
-						btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);						
+						btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 						m_deltafC[c] = deltaf;
 						deltaflengthsqr += deltaf*deltaf;
 						totalImpulse = solveManifold.m_appliedImpulse;
@@ -186,7 +171,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 							{
 								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
 								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 								m_deltafCF[c*multiplier] = deltaf;
 								deltaflengthsqr += deltaf*deltaf;
 							} else {
@@ -203,7 +188,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 							{
 								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
 								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 								m_deltafCF[c*multiplier+1] = deltaf;
 								deltaflengthsqr += deltaf*deltaf;
 							} else {
@@ -223,7 +208,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 				for (j=0;j<numPoolConstraints;j++)
 				{
 					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					m_deltafC[j] = deltaf;
 					deltaflengthsqr += deltaf*deltaf;
@@ -231,7 +215,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 
 
 
-				///solve all friction constraints, using SIMD, if available
+				///solve all friction constraints
 
 				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
 				for (j=0;j<numFrictionPoolConstraints;j++)
@@ -244,7 +228,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
 						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
 
-						//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 						m_deltafCF[j] = deltaf;
 						deltaflengthsqr += deltaf*deltaf;
@@ -252,10 +235,11 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 						m_deltafCF[j] = 0;
 					}
 				}
+			}
 
-
+            {
 				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				for (int j=0;j<numRollingFrictionPoolConstraints;j++)
 				{
 
 					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
@@ -269,87 +253,19 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
 						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-						btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
 						m_deltafCRF[j] = deltaf;
 						deltaflengthsqr += deltaf*deltaf;
 					} else {
 						m_deltafCRF[j] = 0;
 					}
 				}
+            }
 
-
-			}
 		}
 
 
 
-	} else
-	{
-
-		if (iteration< infoGlobal.m_numIterations)
-		{
-			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);
-				}
-			}
-			///solve all contact constraints
-			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-			for (int j=0;j<numPoolConstraints;j++)
-			{
-				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-				m_deltafC[j] = deltaf;
-				deltaflengthsqr += deltaf*deltaf;
-			}
-			///solve all friction constraints
-			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-			for (int j=0;j<numFrictionPoolConstraints;j++)
-			{
-				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-
-				if (totalImpulse>btScalar(0))
-				{
-					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					m_deltafCF[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
-				} else {
-					m_deltafCF[j] = 0;
-				}
-			}
-
-			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
-			{
-				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-				if (totalImpulse>btScalar(0))
-				{
-					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
-						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
-
-					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
-
-					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
-					m_deltafCRF[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
-				} else {
-					m_deltafCRF[j] = 0;
-				}
-			}
-		}
 	}
 
 
@@ -362,10 +278,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
 			for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
 			for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
-			if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) 
-			{
-				for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
-			}
+			for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
 		} else 
 		{
 			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
@@ -374,9 +287,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
 				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
 				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
-				if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
-					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
-				}
+				for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
 			} else {
 				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
 				{
@@ -420,7 +331,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
 					}
 				}
-				if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
+				{
 					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
 					{
 						btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -269,36 +269,28 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBo
 
 btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
-#ifdef USE_SIMD
 	return m_resolveSingleConstraintRowGeneric(body1, body2, c);
-#else
-	return resolveSingleConstraintRowGeneric(body1,body2,c);
-#endif
 }
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
 btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
-	return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c);
+	return m_resolveSingleConstraintRowGeneric(body1, body2, c);
 }
 
 btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
-#ifdef USE_SIMD
 	return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
-#else
-	return resolveSingleConstraintRowLowerLimit(body1,body2,c);
-#endif
 }
 
 
 btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
-	return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c);
+	return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
 }
 
 
-btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
+static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference(
         btSolverBody& body1,
         btSolverBody& body2,
         const btSolverConstraint& c)
@@ -330,7 +322,7 @@ btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCach
 		return deltaImpulse;
 }
 
-btSimdScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	if (!c.m_rhsPenetration)
@@ -362,32 +354,42 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(bt
 	body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
 	return deltaImpulse;
 #else
-	return resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+	return gResolveSplitPenetrationImpulse_scalar_reference(body1,body2,c);
 #endif
 }
 
 
- btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-	 : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
-	 m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference),
-	 m_btSeed2(0)
- {
+btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+{
+    m_btSeed2 = 0;
+    m_cachedSolverMode = 0;
+    setupSolverFunctions( false );
+}
 
+void btSequentialImpulseConstraintSolver::setupSolverFunctions( bool useSimd )
+{
+    m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference;
+    m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference;
+    m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference;
+
+    if ( useSimd )
+    {
 #ifdef USE_SIMD
-	 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
-	 m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2;
-#endif //USE_SIMD
+        m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
+        m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2;
+        m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2;
 
 #ifdef BT_ALLOW_SSE4
-	 int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
-	 if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1))
-	 {
-		m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
-		m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
-	 }
+        int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
+        if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1))
+        {
+            m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
+            m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
+        }
 #endif//BT_ALLOW_SSE4
-
- }
+#endif //USE_SIMD
+    }
+}
 
  btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
  {
@@ -506,8 +508,12 @@ void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
 
 
 
-btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
+btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
 {
+	//printf("rel_vel =%f\n", rel_vel);
+	if (btFabs(rel_vel)<velocityThreshold)
+		return 0.;
+
 	btScalar rest = restitution * -rel_vel;
 	return rest;
 }
@@ -534,7 +540,7 @@ void	btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb
 
 
 
-void btSequentialImpulseConstraintSolver::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, btScalar desiredVelocity, btScalar cfmSlip)
+void btSequentialImpulseConstraintSolver::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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
 
 
@@ -612,7 +618,17 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 
 		btScalar velocityError =  desiredVelocity - rel_vel;
 		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
-		solverConstraint.m_rhs = velocityImpulse;
+
+		btScalar penetrationImpulse = btScalar(0);
+
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
+		{
+			btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
+			btScalar positionalError = -distance * infoGlobal.m_frictionERP/infoGlobal.m_timeStep;
+			penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+		}
+
+		solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
 		solverConstraint.m_rhsPenetration = 0.f;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
@@ -621,12 +637,12 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 	}
 }
 
-btSolverConstraint&	btSequentialImpulseConstraintSolver::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, btScalar desiredVelocity, btScalar cfmSlip)
+btSolverConstraint&	btSequentialImpulseConstraintSolver::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, btScalar cfmSlip)
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
 	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
-							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+							colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
@@ -937,7 +953,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					solverConstraint.m_friction = cp.m_combinedFriction;
 
 
-					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
 					if (restitution <= btScalar(0.))
 					{
 						restitution = 0.f;
@@ -1100,8 +1116,6 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			int frictionIndex = m_tmpSolverContactConstraintPool.size();
 			btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
-			btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-			btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
@@ -1113,9 +1127,9 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 
-			btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
-			btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-
+			btVector3 vel1;
+			btVector3 vel2;
+			
 			solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
 			solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
 
@@ -1126,8 +1140,6 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 
 
-//			const btVector3& pos1 = cp.getPositionWorldOnA();
-//			const btVector3& pos2 = cp.getPositionWorldOnB();
 
 			/////setup the friction constraints
 
@@ -1172,6 +1184,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			///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
 			///
+		
 			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;
@@ -1181,7 +1194,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 					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);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,infoGlobal);
 
 					if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
@@ -1189,7 +1202,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 						cp.m_lateralFrictionDir2.normalize();//??
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
 					}
 
 				} else
@@ -1198,13 +1211,13 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
 					}
 
 
@@ -1216,10 +1229,10 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			} else
 			{
-				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM);
+				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
 
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
+					addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
 
 			}
 			setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
@@ -1251,6 +1264,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	BT_PROFILE("solveGroupCacheFriendlySetup");
 	(void)debugDrawer;
 
+    // if solver mode has changed,
+    if ( infoGlobal.m_solverMode != m_cachedSolverMode )
+    {
+        // update solver functions to use SIMD or non-SIMD
+        bool useSimd = !!( infoGlobal.m_solverMode & SOLVER_SIMD );
+        setupSolverFunctions( useSimd );
+        m_cachedSolverMode = infoGlobal.m_solverMode;
+    }
 	m_maxOverrideNumSolverIterations = 0;
 
 #ifdef BT_ADDITIONAL_DEBUG
@@ -1530,7 +1551,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 							sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
 							btScalar fsum = btFabs(sum);
 							btAssert(fsum > SIMD_EPSILON);
-							solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
+							btScalar sorRelaxation = 1.f;//todo: get from globalInfo?
+							solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f;
 						}
 
 
@@ -1646,145 +1668,6 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		}
 	}
 
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
-	{
-		///solve all joint constraints, using SIMD, if available
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations)
-			{
-				btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-				leastSquaresResidual += residual*residual;
-			}
-		}
-
-		if (iteration< infoGlobal.m_numIterations)
-		{
-			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);
-				}
-			}
-
-			///solve all contact constraints using SIMD, if available
-			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
-			{
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
-
-				for (int c=0;c<numPoolConstraints;c++)
-				{
-					btScalar totalImpulse =0;
-
-					{
-						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
-						btScalar residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						leastSquaresResidual += residual*residual;
-
-						totalImpulse = solveManifold.m_appliedImpulse;
-					}
-					bool applyFriction = true;
-					if (applyFriction)
-					{
-						{
-
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
-
-							if (totalImpulse>btScalar(0))
-							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-								btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-								leastSquaresResidual += residual*residual;
-							}
-						}
-
-						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
-						{
-
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
-
-							if (totalImpulse>btScalar(0))
-							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-								btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-								leastSquaresResidual += residual*residual;
-							}
-						}
-					}
-				}
-
-			}
-			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
-			{
-				//solve the friction constraints after all contact constraints, don't interleave them
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int j;
-
-				for (j=0;j<numPoolConstraints;j++)
-				{
-					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					btScalar residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					leastSquaresResidual += residual*residual;
-				}
-
-
-
-				///solve all friction constraints, using SIMD, if available
-
-				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
-				{
-					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-
-					if (totalImpulse>btScalar(0))
-					{
-						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-						btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						leastSquaresResidual += residual*residual;
-					}
-				}
-
-
-				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-				for (j=0;j<numRollingFrictionPoolConstraints;j++)
-				{
-
-					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-					if (totalImpulse>btScalar(0))
-					{
-						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 = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
-						leastSquaresResidual += residual*residual;
-					}
-				}
-
-
-			}
-		}
-	} else
-	{
-		//non-SIMD version
 		///solve all joint constraints
 		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
 		{
@@ -1809,51 +1692,116 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
 				}
 			}
+
 			///solve all contact constraints
-			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-			for (int j=0;j<numPoolConstraints;j++)
+			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
 			{
-				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-				leastSquaresResidual += residual*residual;
-			}
-			///solve all friction constraints
-			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-			for (int j=0;j<numFrictionPoolConstraints;j++)
-			{
-				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
 
-				if (totalImpulse>btScalar(0))
+				for (int c=0;c<numPoolConstraints;c++)
 				{
-					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+					btScalar totalImpulse =0;
 
-					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+						btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						leastSquaresResidual += residual*residual;
+
+						totalImpulse = solveManifold.m_appliedImpulse;
+					}
+					bool applyFriction = true;
+					if (applyFriction)
+					{
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+								btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								leastSquaresResidual += residual*residual;
+							}
+						}
+
+						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+								btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								leastSquaresResidual += residual*residual;
+							}
+						}
+					}
+				}
+
+			}
+			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			{
+				//solve the friction constraints after all contact constraints, don't interleave them
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int j;
+
+				for (j=0;j<numPoolConstraints;j++)
+				{
+					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					leastSquaresResidual += residual*residual;
 				}
+
+
+
+				///solve all friction constraints
+
+				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+				for (j=0;j<numFrictionPoolConstraints;j++)
+				{
+					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+					if (totalImpulse>btScalar(0))
+					{
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+						btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						leastSquaresResidual += residual*residual;
+					}
+				}
 			}
 
-			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
-			{
-				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-				if (totalImpulse>btScalar(0))
+
+				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+				for (int j=0;j<numRollingFrictionPoolConstraints;j++)
 				{
-					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
-						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
 
-					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+					if (totalImpulse>btScalar(0))
+					{
+						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
 
-					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
-					leastSquaresResidual += residual*residual;
+						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;
 }
 
@@ -1863,7 +1811,6 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 	int iteration;
 	if (infoGlobal.m_splitImpulse)
 	{
-		if (infoGlobal.m_solverMode & SOLVER_SIMD)
 		{
 			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
 			{
@@ -1875,7 +1822,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						btScalar residual = resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 						leastSquaresResidual += residual*residual;
 					}
 				}
@@ -1887,32 +1834,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 					break;
 				}
 			}
-		}
-		else
-		{
-			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
-			{
-				btScalar leastSquaresResidual = 0.f;
-				{
-					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-					int j;
-					for (j=0;j<numPoolConstraints;j++)
-					{
-						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-
-						btScalar residual = resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						leastSquaresResidual += residual*residual;
-					}
-					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;
-					}
-				}
-			}
-		}
+        }
 	}
 }
 

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -56,12 +56,16 @@ protected:
 
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
+    btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
+    int m_cachedSolverMode;  // used to check if SOLVER_SIMD flag has been changed
+    void setupSolverFunctions( bool useSimd );
 
 	btScalar	m_leastSquaresResidual;
 
 	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, 
+									const btContactSolverInfo& infoGlobal,
 									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 
 	void setupTorsionalFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
@@ -69,7 +73,7 @@ protected:
 									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, 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);
 
 	
@@ -85,20 +89,22 @@ protected:
 	unsigned long	m_btSeed2;
 
 	
-	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
+	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);
 
 
-	btSimdScalar	resolveSplitPenetrationSIMD(
-     btSolverBody& bodyA,btSolverBody& bodyB,
-        const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
+    {
+        return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
+    }
 
-	btScalar	resolveSplitPenetrationImpulseCacheFriendly(
-       btSolverBody& bodyA,btSolverBody& bodyB,
-        const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
+    {
+        return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
+    }
 
 	//internal method
 	int		getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
@@ -108,6 +114,10 @@ protected:
 	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)
+    {
+        return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
+    }
 		
 protected:
 	

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

@@ -364,7 +364,6 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	int srow;
 	btScalar limit_err;
 	int limit;
-	int powered;
 
 	// next two rows. 
 	// we want: velA + wA x relA == velB + wB x relB ... but this would
@@ -470,13 +469,9 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		limit_err = getLinDepth() *  signFact;
 		limit = (limit_err > btScalar(0.0)) ? 2 : 1;
 	}
-	powered = 0;
-	if(getPoweredLinMotor())
-	{
-		powered = 1;
-	}
+	bool powered = getPoweredLinMotor();
 	// if the slider has joint limits or motor, add in the extra row
-	if (limit || powered) 
+	if (limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
@@ -524,7 +519,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		btScalar histop = getUpperLinLimit();
 		if(limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		info->m_constraintError[srow] = 0.;
 		info->m_lowerLimit[srow] = 0.;
@@ -609,12 +604,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
 	}
 	// if the slider has joint limits, add in the extra row
-	powered = 0;
-	if(getPoweredAngMotor())
-	{
-		powered = 1;
-	}
-	if(limit || powered) 
+	powered = getPoweredAngMotor();
+	if(limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
@@ -630,7 +621,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		btScalar histop = getUpperAngLimit();
 		if(limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
 		if(powered)

Engine/lib/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

@@ -19,7 +19,7 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 
 
-#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
 
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
 :btTypedObject(type),

Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -374,7 +374,7 @@ void	btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
 
 void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 {
-	BT_PROFILE("synchronizeMotionStates");
+//	BT_PROFILE("synchronizeMotionStates");
 	if (m_synchronizeAllMotionStates)
 	{
 		//iterate  over all collision objects
@@ -402,7 +402,6 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 {
 	startProfiling(timeStep);
 
-	BT_PROFILE("stepSimulation");
 
 	int numSimulationSubSteps = 0;
 
@@ -539,7 +538,7 @@ btVector3 btDiscreteDynamicsWorld::getGravity () const
 	return m_gravity;
 }
 
-void	btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+void	btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask)
 {
 	btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);
 }
@@ -578,14 +577,14 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
 		}
 
 		bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
-		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
-		short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+		int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+		int collisionFilterMask = isDynamic? 	int(btBroadphaseProxy::AllFilter) : 	int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
 
 		addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
 	}
 }
 
-void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
 {
 	if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
 	{
@@ -1512,6 +1511,9 @@ void	btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
 
 		worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
 
+		// Fill padding with zeros to appease msan.
+		memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding));
+
 #ifdef BT_USE_DOUBLE_PRECISION
 		const char* structType = "btDynamicsWorldDoubleData";
 #else//BT_USE_DOUBLE_PRECISION

Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -144,11 +144,11 @@ public:
 
 	virtual btVector3 getGravity () const;
 
-	virtual void	addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+	virtual void	addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::StaticFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
 
 	virtual void	addRigidBody(btRigidBody* body);
 
-	virtual void	addRigidBody(btRigidBody* body, short group, short mask);
+	virtual void	addRigidBody(btRigidBody* body, int group, int mask);
 
 	virtual void	removeRigidBody(btRigidBody* body);
 

Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp

@@ -108,8 +108,108 @@ struct InplaceSolverIslandCallbackMt : public btSimulationIslandManagerMt::Islan
 };
 
 
+///
+/// btConstraintSolverPoolMt
+///
 
-btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver()
+{
+    int i = 0;
+#if BT_THREADSAFE
+    i = btGetCurrentThreadIndex() % m_solvers.size();
+#endif // #if BT_THREADSAFE
+    while ( true )
+    {
+        ThreadSolver& solver = m_solvers[ i ];
+        if ( solver.mutex.tryLock() )
+        {
+            return &solver;
+        }
+        // failed, try the next one
+        i = ( i + 1 ) % m_solvers.size();
+    }
+    return NULL;
+}
+
+void btConstraintSolverPoolMt::init( btConstraintSolver** solvers, int numSolvers )
+{
+    m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
+    m_solvers.resize( numSolvers );
+    for ( int i = 0; i < numSolvers; ++i )
+    {
+        m_solvers[ i ].solver = solvers[ i ];
+    }
+    if ( numSolvers > 0 )
+    {
+        m_solverType = solvers[ 0 ]->getSolverType();
+    }
+}
+
+// create the solvers for me
+btConstraintSolverPoolMt::btConstraintSolverPoolMt( int numSolvers )
+{
+    btAlignedObjectArray<btConstraintSolver*> solvers;
+    solvers.reserve( numSolvers );
+    for ( int i = 0; i < numSolvers; ++i )
+    {
+        btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
+        solvers.push_back( solver );
+    }
+    init( &solvers[ 0 ], numSolvers );
+}
+
+// pass in fully constructed solvers (destructor will delete them)
+btConstraintSolverPoolMt::btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers )
+{
+    init( solvers, numSolvers );
+}
+
+btConstraintSolverPoolMt::~btConstraintSolverPoolMt()
+{
+    // delete all solvers
+    for ( int i = 0; i < m_solvers.size(); ++i )
+    {
+        ThreadSolver& solver = m_solvers[ i ];
+        delete solver.solver;
+        solver.solver = NULL;
+    }
+}
+
+///solve a group of constraints
+btScalar btConstraintSolverPoolMt::solveGroup( btCollisionObject** bodies,
+    int numBodies,
+    btPersistentManifold** manifolds,
+    int numManifolds,
+    btTypedConstraint** constraints,
+    int numConstraints,
+    const btContactSolverInfo& info,
+    btIDebugDraw* debugDrawer,
+    btDispatcher* dispatcher
+)
+{
+    ThreadSolver* ts = getAndLockThreadSolver();
+    ts->solver->solveGroup( bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher );
+    ts->mutex.unlock();
+    return 0.0f;
+}
+
+void btConstraintSolverPoolMt::reset()
+{
+    for ( int i = 0; i < m_solvers.size(); ++i )
+    {
+        ThreadSolver& solver = m_solvers[ i ];
+        solver.mutex.lock();
+        solver.solver->reset();
+        solver.mutex.unlock();
+    }
+}
+
+
+///
+/// btDiscreteDynamicsWorldMt
+///
+
+btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolverPoolMt* constraintSolver, btCollisionConfiguration* collisionConfiguration)
 : btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
 {
 	if (m_ownsIslandManager)
@@ -124,8 +224,8 @@ btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,bt
 	{
 		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
 		btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
-        m_islandManager = im;
         im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
+        m_islandManager = im;
 	}
 }
 
@@ -145,7 +245,7 @@ btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
 }
 
 
-void	btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
+void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	BT_PROFILE("solveConstraints");
 
@@ -160,3 +260,68 @@ void	btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
 }
 
 
+struct UpdaterUnconstrainedMotion : public btIParallelForBody
+{
+    btScalar timeStep;
+    btRigidBody** rigidBodies;
+
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        for ( int i = iBegin; i < iEnd; ++i )
+        {
+            btRigidBody* body = rigidBodies[ i ];
+            if ( !body->isStaticOrKinematicObject() )
+            {
+                //don't integrate/update velocities here, it happens in the constraint solver
+                body->applyDamping( timeStep );
+                body->predictIntegratedTransform( timeStep, body->getInterpolationWorldTransform() );
+            }
+        }
+    }
+};
+
+
+void btDiscreteDynamicsWorldMt::predictUnconstraintMotion( btScalar timeStep )
+{
+    BT_PROFILE( "predictUnconstraintMotion" );
+    if ( m_nonStaticRigidBodies.size() > 0 )
+    {
+        UpdaterUnconstrainedMotion update;
+        update.timeStep = timeStep;
+        update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
+        int grainSize = 50;  // num of iterations per task for task scheduler
+        btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
+    }
+}
+
+
+void btDiscreteDynamicsWorldMt::createPredictiveContacts( btScalar timeStep )
+{
+    BT_PROFILE( "createPredictiveContacts" );
+    releasePredictiveContacts();
+    if ( m_nonStaticRigidBodies.size() > 0 )
+    {
+        UpdaterCreatePredictiveContacts update;
+        update.world = this;
+        update.timeStep = timeStep;
+        update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
+        int grainSize = 50;  // num of iterations per task for task scheduler
+        btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
+    }
+}
+
+
+void btDiscreteDynamicsWorldMt::integrateTransforms( btScalar timeStep )
+{
+    BT_PROFILE( "integrateTransforms" );
+    if ( m_nonStaticRigidBodies.size() > 0 )
+    {
+        UpdaterIntegrateTransforms update;
+        update.world = this;
+        update.timeStep = timeStep;
+        update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
+        int grainSize = 50;  // num of iterations per task for task scheduler
+        btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update );
+    }
+}
+

Engine/lib/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h

@@ -18,24 +18,116 @@ subject to the following restrictions:
 #define BT_DISCRETE_DYNAMICS_WORLD_MT_H
 
 #include "btDiscreteDynamicsWorld.h"
+#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.
+///
+///  Each solver in the pool is protected by a mutex.  When solveGroup is called from a thread,
+///  the pool looks for a solver that isn't being used by another thread, locks it, and dispatches the
+///  call to the solver.
+///  So long as there are at least as many solvers as there are hardware threads, it should never need to
+///  spin wait.
+///
+class btConstraintSolverPoolMt : public btConstraintSolver
+{
+public:
+    // create the solvers for me
+    explicit btConstraintSolverPoolMt( int numSolvers );
+
+    // pass in fully constructed solvers (destructor will delete them)
+    btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers );
+
+    virtual ~btConstraintSolverPoolMt();
+
+    ///solve a group of constraints
+    virtual btScalar solveGroup( btCollisionObject** bodies,
+        int numBodies,
+        btPersistentManifold** manifolds,
+        int numManifolds,
+        btTypedConstraint** constraints,
+        int numConstraints,
+        const btContactSolverInfo& info,
+        btIDebugDraw* debugDrawer,
+        btDispatcher* dispatcher
+    ) BT_OVERRIDE;
+
+    virtual	void reset() BT_OVERRIDE;
+    virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; }
+
+private:
+    const static size_t kCacheLineSize = 128;
+    struct ThreadSolver
+    {
+        btConstraintSolver* solver;
+        btSpinMutex mutex;
+        char _cachelinePadding[ kCacheLineSize - sizeof( btSpinMutex ) - sizeof( void* ) ];  // keep mutexes from sharing a cache line
+    };
+    btAlignedObjectArray<ThreadSolver> m_solvers;
+    btConstraintSolverType m_solverType;
+
+    ThreadSolver* getAndLockThreadSolver();
+    void init( btConstraintSolver** solvers, int numSolvers );
+};
+
+
+
 ///
 /// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support
 ///                              solving simulation islands on multiple threads.
 ///
+///  Should function exactly like btDiscreteDynamicsWorld.
+///  Also 3 methods that iterate over all of the rigidbodies can run in parallel:
+///     - predictUnconstraintMotion
+///     - integrateTransforms
+///     - createPredictiveContacts
+///
 ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
 {
 protected:
     InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt;
 
-    virtual void	solveConstraints(btContactSolverInfo& solverInfo);
+    virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE;
+
+    virtual void predictUnconstraintMotion( btScalar timeStep ) BT_OVERRIDE;
+
+    struct UpdaterCreatePredictiveContacts : public btIParallelForBody
+    {
+        btScalar timeStep;
+        btRigidBody** rigidBodies;
+        btDiscreteDynamicsWorldMt* world;
+
+        void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+        {
+            world->createPredictiveContactsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
+        }
+    };
+    virtual void createPredictiveContacts( btScalar timeStep ) BT_OVERRIDE;
+
+    struct UpdaterIntegrateTransforms : public btIParallelForBody
+    {
+        btScalar timeStep;
+        btRigidBody** rigidBodies;
+        btDiscreteDynamicsWorldMt* world;
+
+        void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+        {
+            world->integrateTransformsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
+        }
+    };
+    virtual void integrateTransforms( btScalar timeStep ) BT_OVERRIDE;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
+        btBroadphaseInterface* pairCache,
+        btConstraintSolverPoolMt* constraintSolver,   // Note this should be a solver-pool for multi-threading
+        btCollisionConfiguration* collisionConfiguration
+    );
 	virtual ~btDiscreteDynamicsWorldMt();
 };
 

Engine/lib/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h

@@ -89,7 +89,7 @@ public:
 
 		virtual void	addRigidBody(btRigidBody* body) = 0;
 
-		virtual void	addRigidBody(btRigidBody* body, short group, short mask) = 0;
+		virtual void	addRigidBody(btRigidBody* body, int group, int mask) = 0;
 
 		virtual void	removeRigidBody(btRigidBody* body) = 0;
 
@@ -135,6 +135,11 @@ public:
 			return m_solverInfo;
 		}
 
+		const btContactSolverInfo& getSolverInfo() const
+		{
+			return m_solverInfo;
+		}
+
 
 		///obsolete, use addAction instead.
 		virtual void	addVehicle(btActionInterface* vehicle) {(void)vehicle;}

Engine/lib/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -507,6 +507,11 @@ const char*	btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali
 	rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
 	rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
 
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	memset(rbd->m_padding, 0, sizeof(rbd->m_padding));
+#endif
+
 	return btRigidBodyDataName;
 }
 

Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

@@ -155,7 +155,7 @@ void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 	}
 }
 
-void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
 {
 	body->setGravity(m_gravity);
 

Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h

@@ -56,7 +56,7 @@ public:
 
 	virtual void	addRigidBody(btRigidBody* body);
 
-	virtual void	addRigidBody(btRigidBody* body, short group, short mask);
+	virtual void	addRigidBody(btRigidBody* body, int group, int mask);
 
 	virtual void	removeRigidBody(btRigidBody* body);
 

Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp

@@ -15,6 +15,7 @@ subject to the following restrictions:
 
 
 #include "LinearMath/btScalar.h"
+#include "LinearMath/btThreads.h"
 #include "btSimulationIslandManagerMt.h"
 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
@@ -44,7 +45,7 @@ btSimulationIslandManagerMt::btSimulationIslandManagerMt()
 {
     m_minimumSolverBatchSize = calcBatchCost(0, 128, 0);
     m_batchIslandMinBodyCount = 32;
-    m_islandDispatch = defaultIslandDispatch;
+    m_islandDispatch = parallelIslandDispatch;
     m_batchIsland = NULL;
 }
 
@@ -545,8 +546,9 @@ void btSimulationIslandManagerMt::mergeIslands()
 }
 
 
-void btSimulationIslandManagerMt::defaultIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
+void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
 {
+    BT_PROFILE( "serialIslandDispatch" );
     // serial dispatch
     btAlignedObjectArray<Island*>& islands = *islandsPtr;
     for ( int i = 0; i < islands.size(); ++i )
@@ -565,6 +567,41 @@ void btSimulationIslandManagerMt::defaultIslandDispatch( btAlignedObjectArray<Is
     }
 }
 
+struct UpdateIslandDispatcher : public btIParallelForBody
+{
+    btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr;
+    btSimulationIslandManagerMt::IslandCallback* callback;
+
+    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
+    {
+        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
+            );
+        }
+    }
+};
+
+void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
+{
+    BT_PROFILE( "parallelIslandDispatch" );
+    int grainSize = 1;  // iterations per task
+    UpdateIslandDispatcher dispatcher;
+    dispatcher.islandsPtr = islandsPtr;
+    dispatcher.callback = callback;
+    btParallelFor( 0, islandsPtr->size(), grainSize, dispatcher );
+}
+
+
 ///@todo: this is random access, it can be walked 'cache friendly'!
 void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher,
                                                         btCollisionWorld* collisionWorld,

Engine/lib/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h

@@ -59,7 +59,8 @@ public:
                                     ) = 0;
     };
     typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
-    static void defaultIslandDispatch( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
+    static void serialIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
+    static void parallelIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback );
 protected:
     btAlignedObjectArray<Island*> m_allocatedIslands;  // owner of all Islands
     btAlignedObjectArray<Island*> m_activeIslands;  // islands actively in use

Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -52,6 +52,7 @@ namespace {
         bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
     }
 
+#if 0
     void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
                                  const btVector3 &displacement,
                                  const btVector3 &top_in,
@@ -81,6 +82,8 @@ namespace {
 		top_out = a_top.cross(b_top);
 		bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
 	}
+#endif
+	
 }
 
 
@@ -151,6 +154,8 @@ void btMultiBody::setupFixed(int i,
 	m_links[i].m_mass = mass;
     m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
+	 m_links[i].setAxisTop(0, 0., 0., 0.);
+    m_links[i].setAxisBottom(0, btVector3(0,0,0));
     m_links[i].m_zeroRotParentToThis = rotParentToThis;
 	m_links[i].m_dVector = thisPivotToThisComOffset;
     m_links[i].m_eVector = parentComToThisPivotOffset;    
@@ -427,6 +432,13 @@ const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
 
 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
 {
+	btAssert(i>=-1);
+	btAssert(i<m_links.size());
+	if ((i<-1) || (i>=m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
+	}
+
     btVector3 result = local_pos;
     while (i != -1) {
         // 'result' is in frame i. transform it to frame parent(i)
@@ -444,6 +456,13 @@ btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
 
 btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
 {
+	btAssert(i>=-1);
+	btAssert(i<m_links.size());
+	if ((i<-1) || (i>=m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
+	}
+
     if (i == -1) {
         // world to base
         return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
@@ -455,6 +474,14 @@ btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
 
 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
 {
+	btAssert(i>=-1);
+	btAssert(i<m_links.size());
+	if ((i<-1) || (i>=m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
+	}
+
+
     btVector3 result = local_dir;
     while (i != -1) {
         result = quatRotate(getParentToLocalRot(i).inverse() , result);
@@ -466,6 +493,13 @@ btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
 
 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
 {
+	btAssert(i>=-1);
+	btAssert(i<m_links.size());
+	if ((i<-1) || (i>=m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
+	}
+
     if (i == -1) {
         return quatRotate(getWorldToBaseRot(), world_dir);
     } else {
@@ -490,7 +524,8 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
     omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
     vel[0] = quatRotate(m_baseQuat ,getBaseVel());
     
-    for (int i = 0; i < num_links; ++i) {
+    for (int i = 0; i < num_links; ++i) 
+	{
         const int parent = m_links[i].m_parent;
 
         // transform parent vel into this frame, store in omega[i+1], vel[i+1]
@@ -499,9 +534,24 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
                          omega[i+1], vel[i+1]);
 
         // now add qidot * shat_i
-        omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
-        vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
-    }
+		//only supported for revolute/prismatic joints, todo: spherical and planar joints
+		switch(m_links[i].m_jointType)
+		{
+			case btMultibodyLink::ePrismatic:
+			case btMultibodyLink::eRevolute:
+			{
+				btVector3 axisTop = m_links[i].getAxisTop(0);
+				btVector3 axisBottom = m_links[i].getAxisBottom(0);
+				btScalar jointVel = getJointVel(i);
+				omega[i+1] += jointVel * axisTop;
+				vel[i+1] += jointVel * axisBottom;
+				break;
+			}
+			default:
+			{
+			}
+		}
+	}
 }
 
 btScalar btMultiBody::getKineticEnergy() const
@@ -1206,7 +1256,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 
 
-void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
+void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const
 {
 	int num_links = getNumLinks();
 	///solve I * x = rhs, so the result = invI * rhs
@@ -1765,7 +1815,6 @@ void btMultiBody::goToSleep()
 
 void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 {
-	int num_links = getNumLinks();
 	extern bool gDisableDeactivation;
     if (!m_canSleep || gDisableDeactivation) 
 	{
@@ -1934,6 +1983,10 @@ const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
 				memPtr->m_parentIndex = getLink(i).m_parent;
 				memPtr->m_jointDamping = getLink(i).m_jointDamping;
 				memPtr->m_jointFriction = getLink(i).m_jointFriction;
+				memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit;
+				memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit;
+				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_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
@@ -1978,5 +2031,10 @@ const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
 		}
 		mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
 
+		// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+		memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
+#endif
+
 		return btMultiBodyDataName;
 }

Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBody.h

@@ -140,6 +140,15 @@ public:
 		return m_baseCollider;
 	}
 
+	btMultiBodyLinkCollider* getLinkCollider(int index)
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
     //
     // get parent
     // input: link num from 0 to num_links-1
@@ -560,6 +569,8 @@ void addJointTorque(int i, btScalar Q);
 	}
 	void	forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
 
+	void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
+
 	void	updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
 	
 	virtual	int	calculateSerializeBufferSize()	const;
@@ -613,9 +624,8 @@ private:
     btMultiBody(const btMultiBody &);  // not implemented
     void operator=(const btMultiBody &);  // not implemented
 
-    void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
 
-	void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
+	void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const;
 	void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
 	
 	void updateLinksDofOffsets()
@@ -649,7 +659,6 @@ private:
     btVector3 m_baseConstraintTorque;    // external torque applied to base. World frame.
  
     btAlignedObjectArray<btMultibodyLink> m_links;    // array of m_links, excluding the base. index from 0 to num_links-1.
-	btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
 
     
     //
@@ -727,7 +736,11 @@ struct btMultiBodyLinkDoubleData
 
 	double					m_jointDamping;
 	double					m_jointFriction;
-
+	double					m_jointLowerLimit;
+	double					m_jointUpperLimit;
+	double					m_jointMaxForce;
+	double					m_jointMaxVelocity;
+	
 	char					*m_linkName;
 	char					*m_jointName;
 	btCollisionObjectDoubleData	*m_linkCollider;
@@ -756,7 +769,11 @@ struct btMultiBodyLinkFloatData
 	int						m_posVarCount;
 	float					m_jointDamping;
 	float					m_jointFriction;
-
+	float					m_jointLowerLimit;
+	float					m_jointUpperLimit;
+	float					m_jointMaxForce;
+	float					m_jointMaxVelocity;
+	
 	char				*m_linkName;
 	char				*m_jointName;
 	btCollisionObjectFloatData	*m_linkCollider;

Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -39,7 +39,7 @@ struct btMultiBodyJacobianData
 };
 
 
-class btMultiBodyConstraint
+ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraint
 {
 protected:
 
@@ -84,12 +84,18 @@ protected:
 
 public:
 
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
 	virtual ~btMultiBodyConstraint();
 
 	void updateJacobianSizes();
 	void allocateJacobiansMultiDof();
 
+	//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
+	virtual void setFrameInB(const btMatrix3x3& frameInB) {}
+	virtual void setPivotInB(const btVector3& pivotInB){}
+
 	virtual void finalizeMultiDof()=0;
 
 	virtual int getIslandIdA() const =0;
@@ -177,6 +183,12 @@ public:
 
 	virtual void debugDraw(class btIDebugDraw* drawer)=0;
 
+	virtual void setGearRatio(btScalar ratio) {}
+	virtual void setGearAuxLink(int gearAuxLink) {}
+	virtual void setRelativePositionTarget(btScalar relPosTarget){}
+	virtual void setErp(btScalar erp){}
+	
+	
 };
 
 #endif //BT_MULTIBODY_CONSTRAINT_H

Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -48,9 +48,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 	}
 
 	//solve featherstone normal contact
-	for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
+	for (int j0=0;j0<m_multiBodyNormalContactConstraints.size();j0++)
 	{
-		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
+		int index = j0;//iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
+
+		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
 		btScalar residual = 0.f;
 
 		if (iteration < infoGlobal.m_numIterations)
@@ -68,11 +70,13 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 	
 	//solve featherstone frictional contact
 
-	for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
+	for (int j1=0;j1<this->m_multiBodyFrictionContactConstraints.size();j1++)
 	{
 		if (iteration < infoGlobal.m_numIterations)
 		{
-			btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
+			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))
@@ -240,32 +244,39 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	 //cfm = 1 /       ( dt * kp + kd )
     //erp = dt * kp / ( dt * kp + kd )
     
-    btScalar cfm = infoGlobal.m_globalCfm;
-    btScalar erp = infoGlobal.m_erp2;
-
-    if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
-    {
-        if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
-            cfm  = cp.m_contactCFM;
-        if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
-            erp = cp.m_contactERP;                
-    } else
-    {
-        if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
-        {
-            btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
-            if (denom < SIMD_EPSILON)
-            {
-                denom = SIMD_EPSILON;
-            }
-            cfm = btScalar(1) / denom; 
-            erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
-        }
-    }
-    
-    cfm *= invTimeStep;
+    btScalar cfm;
+	btScalar erp;
+	if (isFriction)
+	{
+		cfm = infoGlobal.m_frictionCFM;
+		erp = infoGlobal.m_frictionERP;
+	} else
+	{
+		cfm = infoGlobal.m_globalCfm;
+		erp = infoGlobal.m_erp2;
 
+		if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
+		{
+			if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
+				cfm  = cp.m_contactCFM;
+			if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
+				erp = cp.m_contactERP;                
+		} else
+		{
+			if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
+			{
+				btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
+				if (denom < SIMD_EPSILON)
+				{
+					denom = SIMD_EPSILON;
+				}
+				cfm = btScalar(1) / denom; 
+				erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
+			}
+		}
+	}
 
+	cfm *= invTimeStep;
 
 	if (multiBodyA)
 	{
@@ -425,8 +436,19 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	
 
 	btScalar restitution = 0.f;
-	btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
-
+    btScalar distance = 0;
+    if (!isFriction)
+    {
+        distance = cp.getDistance()+infoGlobal.m_linearSlop;
+    } else
+    {
+        if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
+        {
+          distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
+        }
+    }
+  
+    
 	btScalar rel_vel = 0.f;
 	int ndofA  = 0;
 	int ndofB  = 0;
@@ -469,7 +491,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 		if(!isFriction)
 		{
-			restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);	
+			restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);	
 			if (restitution <= btScalar(0.))
 			{
 				restitution = 0.f;
@@ -521,15 +543,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 		btScalar positionalError = 0.f;
 		btScalar velocityError = restitution - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
-
-		if (penetration>0)
+		if (isFriction)
 		{
-			positionalError = 0;
-			velocityError -= penetration / infoGlobal.m_timeStep;
-
+			positionalError = -distance * erp/infoGlobal.m_timeStep;
 		} else
 		{
-			positionalError = -penetration * erp/infoGlobal.m_timeStep;
+    			if (distance>0)
+			{
+				positionalError = 0;
+				velocityError -= distance / infoGlobal.m_timeStep;
+
+			} else
+			{
+				positionalError = -distance * erp/infoGlobal.m_timeStep;
+			}
 		}
 
 		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
@@ -556,7 +583,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 		else
 		{
-			solverConstraint.m_rhs = velocityImpulse;
+			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
 			solverConstraint.m_rhsPenetration = 0.f;
 			solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 			solverConstraint.m_upperLimit = solverConstraint.m_friction;
@@ -602,7 +629,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
     
     relaxation = infoGlobal.m_sor;
     
-    btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
+   // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
     
     
     if (multiBodyA)
@@ -638,12 +665,12 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
         multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
         
-        btVector3 torqueAxis0 = constraintNormal;
+        btVector3 torqueAxis0 = -constraintNormal;
         solverConstraint.m_relpos1CrossNormal = torqueAxis0;
         solverConstraint.m_contactNormal1 = btVector3(0,0,0);
     } else
     {
-        btVector3 torqueAxis0 = constraintNormal;
+        btVector3 torqueAxis0 = -constraintNormal;
         solverConstraint.m_relpos1CrossNormal = torqueAxis0;
         solverConstraint.m_contactNormal1 = btVector3(0,0,0);
         solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -681,21 +708,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
         
         btVector3 torqueAxis1 = constraintNormal;
-        solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+        solverConstraint.m_relpos2CrossNormal = torqueAxis1;
         solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
         
     } else
     {
         btVector3 torqueAxis1 = constraintNormal;
-        solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+        solverConstraint.m_relpos2CrossNormal = torqueAxis1;
         solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
         
-        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
     }
     
     {
         
-        btVector3 vec;
         btScalar denom0 = 0.f;
         btScalar denom1 = 0.f;
         btScalar* jacB = 0;
@@ -718,8 +744,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         {
             if (rb0)
             {
-                vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-                denom0 = rb0->getInvMass() + constraintNormal.dot(vec);
+				btVector3 iMJaA = rb0?rb0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
+				denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
             }
         }
         if (multiBodyB)
@@ -738,8 +764,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         {
             if (rb1)
             {
-                vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-                denom1 = rb1->getInvMass() + constraintNormal.dot(vec);
+				btVector3 iMJaB = rb1?rb1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
+				denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
             }
         }
         
@@ -781,7 +807,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         {
             if (rb0)
             {
-                rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+				btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
+				rel_vel += solverConstraint.m_contactNormal1.dot(rb0?solverBodyA->m_linearVelocity+solverBodyA->m_externalForceImpulse:btVector3(0,0,0))
+				+ solverConstraint.m_relpos1CrossNormal.dot(rb0?solverBodyA->m_angularVelocity:btVector3(0,0,0));
+	
             }
         }
         if (multiBodyB)
@@ -795,7 +824,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         {
             if (rb1)
             {
-                rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+				btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
+				rel_vel += solverConstraint.m_contactNormal2.dot(rb1?solverBodyB->m_linearVelocity+solverBodyB->m_externalForceImpulse:btVector3(0,0,0))
+			+ solverConstraint.m_relpos2CrossNormal.dot(rb1?solverBodyB->m_angularVelocity:btVector3(0,0,0));
+
             }
         }
 
@@ -803,7 +835,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         
         if(!isFriction)
         {
-            restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+            restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
             if (restitution <= btScalar(0.))
             {
                 restitution = 0.f;
@@ -817,13 +849,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
     
     {
         
-        btScalar positionalError = 0.f;
-        btScalar velocityError = restitution - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
+        btScalar velocityError = 0 - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
         
-        if (penetration>0)
-        {
-            velocityError -= penetration / infoGlobal.m_timeStep;
-        } 
+       
         
         btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
         
@@ -995,6 +1023,33 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			///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
 			///
+
+			btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+			cp.m_lateralFrictionDir1.normalize();
+			cp.m_lateralFrictionDir2.normalize();
+
+            if (rollingFriction > 0 )
+             {
+                if (cp.m_combinedSpinningFriction>0)
+                {
+                    addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
+                }
+                if (cp.m_combinedRollingFriction>0)
+                {
+
+					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					
+					if (cp.m_lateralFrictionDir1.length()>0.001)
+	                    addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
+
+					if (cp.m_lateralFrictionDir2.length()>0.001)
+                    addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
+                }
+              rollingFriction--;
+            }
 			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;
@@ -1019,20 +1074,12 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 				} 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);
 					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
                     
-                    if (rollingFriction > 0)
-                    {
-                        addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
-                        addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
-                        addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
-
-                        rollingFriction--;
-                    }
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{

Engine/lib/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -24,7 +24,7 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 
 
-void	btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
+void	btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask)
 {
 	m_multiBodies.push_back(body);
 
@@ -332,10 +332,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 				}
 			}
 
-			if (m_solverInfo->m_minimumSolverBatchSize<=1)
-			{
-				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-			} else
+			//if (m_solverInfo->m_minimumSolverBatchSize<=1)
+			//{
+			//	m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
+			//} else
 			{
 				
 				for (i=0;i<numBodies;i++)
@@ -798,6 +798,8 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 {
 	BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
 
+	btDiscreteDynamicsWorld::debugDrawWorld();
+
 	bool drawConstraints = false;
 	if (getDebugDrawer())
 	{
@@ -867,7 +869,7 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 		}
 	}
 
-	btDiscreteDynamicsWorld::debugDrawWorld();
+	
 }
 
 
@@ -917,7 +919,7 @@ void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
 void btMultiBodyDynamicsWorld::clearMultiBodyForces()
 {
               {
-                BT_PROFILE("clearMultiBodyForces");
+               // BT_PROFILE("clearMultiBodyForces");
                 for (int i=0;i<this->m_multiBodies.size();i++)
                 {
                         btMultiBody* bod = m_multiBodies[i];