Update Bullet to 2.86.1. Thanks to slapin for the initial work.

Docs/Urho3D.dox

@@ -205,7 +205,7 @@ Urho3D uses the following third-party libraries:
 - AngelScript 2.31.2 (http://www.angelcode.com/angelscript)
 - Boost 1.64.0 (http://www.boost.org) - only used for AngelScript generic bindings
 - Box2D 2.3.2 WIP (http://box2d.org)
-- Bullet 2.83.6 (http://www.bulletphysics.org)
+- Bullet 2.86.1 (http://www.bulletphysics.org)
 - Civetweb 1.7 (https://github.com/civetweb/civetweb)
 - FreeType 2.7.1 (https://www.freetype.org)
 - GLEW 1.13.0 (http://glew.sourceforge.net)

README.md

@@ -169,7 +169,7 @@ Urho3D uses the following third-party libraries:
 - AngelScript 2.31.2 (http://www.angelcode.com/angelscript)
 - Boost 1.64.0 (http://www.boost.org) - only used for AngelScript generic bindings
 - Box2D 2.3.2 WIP (http://box2d.org)
-- Bullet 2.83.6 (http://www.bulletphysics.org)
+- Bullet 2.86.1 (http://www.bulletphysics.org)
 - Civetweb 1.7 (https://github.com/civetweb/civetweb)
 - FreeType 2.7.1 (https://www.freetype.org)
 - GLEW 1.13.0 (http://glew.sourceforge.net)

Source/ThirdParty/Bullet/AUTHORS.txt

@@ -35,5 +35,6 @@ Marten Svanfeldt
 Pierre Terdiman
 Steven Thompson
 Tamas Umenhoffer
+Yunfei Bai
 
 If your name is missing, please send an email to [email protected] or file an issue at http://github.com/bulletphysics/bullet3

Source/ThirdParty/Bullet/README.md

@@ -2,18 +2,20 @@
 [![Travis Build Status](https://api.travis-ci.org/bulletphysics/bullet3.png?branch=master)](https://travis-ci.org/bulletphysics/bullet3)
 [![Appveyor Build status](https://ci.appveyor.com/api/projects/status/6sly9uxajr6xsstq)](https://ci.appveyor.com/project/erwincoumans/bullet3)
 
-# Bullet 2.x including experimental Bullet 3 OpenCL.
+# Bullet Physics SDK
 
-This is the official repository of Bullet 2.x, moved from http://bullet.googlecode.com
-It includes the optional work-in-progress Bullet 3 GPU pipeline.
+This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc.
 
-The Bullet 2 API will stay default and up-to-date while slowly moving to Bullet 3.
-The steps towards Bullet 3 are in a nutshell:
+New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR
+
+The Bullet 2 API will stay default and up-to-date while slowly moving to a new API.
+The steps towards a new API is in a nutshell:
 
 1. The old Bullet2 demos are being merged into the examples/ExampleBrowser
-2. A new Bullet 3 API is created
-3. All Bullet2 functionality is added to Bullet 3.
-4. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl
+2. A new physics-engine agnostic C-API is created, see examples/SharedMemory/PhysicsClientC_API.h
+3. Python bindings in pybullet are on top of this C-API, see examples/pybullet
+4. A Virtual Reality sandbox using openvr for HTC Vive and Oculus Rift is available
+5. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl
 
 You can still use svn or svn externals using the github git repository: use svn co https://github.com/bulletphysics/bullet3/trunk
 
@@ -27,14 +29,14 @@ Some optional demos require OpenGL 2 or OpenGL 3, there are some non-graphical d
 
 https://docs.google.com/document/d/1u9vyzPtrVoVhYqQOGNWUgjRbfwfCdIts_NzmvgiJ144/edit
 
-## Requirements for Bullet 3
+## Requirements for experimental OpenCL GPGPU support
 
-The entire collision detection and rigid body dynamics is executed on the GPU.
+The entire collision detection and rigid body dynamics can be executed on the GPU.
 
 A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better.
 We succesfully tested the software under Windows, Linux and Mac OSX.
 The software currently doesn't work on OpenCL CPU devices. It might run
-on a laptop GPU but performance is likely not very good. Note that
+on a laptop GPU but performance will not likely be very good. Note that
 often an OpenCL drivers fails to compile a kernel. Some unit tests exist to
 track down the issue, but more work is required to cover all OpenCL kernels.
 
@@ -47,7 +49,19 @@ All source code files are licensed under the permissive zlib license
 
 **Windows**
 
-Click on build3/vs2010.bat and open build3/vs2010/0MySolution.sln
+Click on build_visual_studio.bat and open build3/vs2010/0MySolution.sln
+
+**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift**
+
+Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln
+Edit this batch file to choose where Python include/lib directories are located.
+Build and run the App_SharedMemoryPhysics_VR project, preferably in Release/optimized build.
+You can connect from Python pybullet to the sandbox using:
+
+```
+import pybullet as p
+p.connect(p.SHARED_MEMORY)
+```
 
 **Linux and Mac OSX gnu make**
 
@@ -55,7 +69,7 @@ In a terminal type:
 
 	cd build3
 
-Dependend on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines
+Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines
 
 	./premake4_linux gmake
 	./premake4_linux64 gmake
@@ -80,15 +94,14 @@ You can just run it though a terminal/command prompt, or by clicking it.
 
 ```
 [--start_demo_name="Demo Name"]     Start with a selected demo  
-[--enable_experimental_opencl]      Enable some experimental OpenCL examples
 [--mp4=moviename.mp4]               Create a mp4 movie of the window, requires ffmpeg installed
 [--mouse_move_multiplier=0.400000]  Set the mouse move sensitivity
 [--mouse_wheel_multiplier=0.01]     Set the mouse wheel sensitivity
-[--background_color_red= 0.9]       Set the red component for background color. Same for green and blue.
+[--background_color_red= 0.9]       Set the red component for background color. Same for green and blue
 [--fixed_timestep= 0.0]             Use either a real-time delta time (0.0) or a fixed step size (0.016666)
 ```
 
-You can use mouse picking to grab objects. When holding the ALT of CONTROL key, you have Maya style camera mouse controls.
-Press F1 to create series of screenshot. Hit ESCAPE to exit the demo app.
+You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls.
+Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app.
 
-See docs folder for further information.
+Check out the docs folder and the Bullet physics forums for further information.

Source/ThirdParty/Bullet/VERSION

@@ -1 +1 @@
-2.84
+2.86

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -134,7 +134,7 @@ public:
 
 	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
 	
-	BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+	BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher);
 	void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
 	void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
 	SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
@@ -144,7 +144,7 @@ public:
 	void	processAllOverlappingPairs(btOverlapCallback* callback);
 
 	//Broadphase Interface
-	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	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
 	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
 	virtual void  getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
@@ -228,16 +228,16 @@ void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinalit
 #endif //DEBUG_BROADPHASE
 
 template <typename BP_FP_INT_TYPE>
-btBroadphaseProxy*	btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
+btBroadphaseProxy*	btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher)
 {
 		(void)shapeType;
-		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy);
+		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher);
 		
 		Handle* handle = getHandle(handleId);
 		
 		if (m_raycastAccelerator)
 		{
-			btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0);
+			btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher);
 			handle->m_dbvtProxy = rayProxy;
 		}
 		return handle;
@@ -502,7 +502,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
 
 
 template <typename BP_FP_INT_TYPE>
-BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher)
 {
 	// quantize the bounds
 	BP_FP_INT_TYPE min[3], max[3];
@@ -520,7 +520,6 @@ BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3&
 	pHandle->m_clientObject = pOwner;
 	pHandle->m_collisionFilterGroup = collisionFilterGroup;
 	pHandle->m_collisionFilterMask = collisionFilterMask;
-	pHandle->m_multiSapParentProxy = multiSapProxy;
 
 	// compute current limit of edge arrays
 	BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2);

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

@@ -53,7 +53,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;

Source/ThirdParty/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;
 	}
 
 	

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btDbvt.h

@@ -122,6 +122,7 @@ subject to the following restrictions:
 #error "DBVT_INT0_IMPL undefined"
 #endif
 
+
 //
 // Defaults volumes
 //
@@ -188,6 +189,9 @@ struct	btDbvtNode
 	};
 };
 
+typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack;
+
+
 ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
 ///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
 ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
@@ -263,7 +267,6 @@ struct	btDbvt
 
 	
 	btAlignedObjectArray<sStkNN>	m_stkStack;
-	mutable btAlignedObjectArray<const btDbvtNode*>	m_rayTestStack;
 
 
 	// Methods
@@ -325,6 +328,16 @@ struct	btDbvt
 		void		collideTV(	const btDbvtNode* root,
 		const btDbvtVolume& volume,
 		DBVT_IPOLICY) const;
+	
+	DBVT_PREFIX
+	void		collideTVNoStackAlloc(	const btDbvtNode* root,
+						  const btDbvtVolume& volume,
+						  btNodeStack& stack,
+						  DBVT_IPOLICY) const;
+	
+	
+	
+	
 	///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
 	///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
 	DBVT_PREFIX
@@ -343,6 +356,7 @@ struct	btDbvt
 								btScalar lambda_max,
 								const btVector3& aabbMin,
 								const btVector3& aabbMax,
+                                btAlignedObjectArray<const btDbvtNode*>& stack,
 								DBVT_IPOLICY) const;
 
 	DBVT_PREFIX
@@ -917,39 +931,72 @@ inline void		btDbvt::collideTT(	const btDbvtNode* root0,
 }
 #endif 
 
-//
 DBVT_PREFIX
 inline void		btDbvt::collideTV(	const btDbvtNode* root,
 								  const btDbvtVolume& vol,
 								  DBVT_IPOLICY) const
 {
 	DBVT_CHECKTYPE
-		if(root)
-		{
-			ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume(vol);
-			btAlignedObjectArray<const btDbvtNode*>	stack;
-			stack.resize(0);
-			stack.reserve(SIMPLE_STACKSIZE);
-			stack.push_back(root);
-			do	{
-				const btDbvtNode*	n=stack[stack.size()-1];
-				stack.pop_back();
-				if(Intersect(n->volume,volume))
+	if(root)
+	{
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume(vol);
+		btAlignedObjectArray<const btDbvtNode*>	stack;
+		stack.resize(0);
+		stack.reserve(SIMPLE_STACKSIZE);
+		stack.push_back(root);
+		do	{
+			const btDbvtNode*	n=stack[stack.size()-1];
+			stack.pop_back();
+			if(Intersect(n->volume,volume))
+			{
+				if(n->isinternal())
 				{
-					if(n->isinternal())
-					{
-						stack.push_back(n->childs[0]);
-						stack.push_back(n->childs[1]);
-					}
-					else
-					{
-						policy.Process(n);
-					}
+					stack.push_back(n->childs[0]);
+					stack.push_back(n->childs[1]);
 				}
-			} while(stack.size()>0);
-		}
+				else
+				{
+					policy.Process(n);
+				}
+			}
+		} while(stack.size()>0);
+	}
 }
 
+//
+DBVT_PREFIX
+inline void		btDbvt::collideTVNoStackAlloc(	const btDbvtNode* root,
+											 const btDbvtVolume& vol,
+											 btNodeStack& stack,
+											 DBVT_IPOLICY) const
+{
+	DBVT_CHECKTYPE
+	if(root)
+	{
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume(vol);
+		stack.resize(0);
+		stack.reserve(SIMPLE_STACKSIZE);
+		stack.push_back(root);
+		do	{
+			const btDbvtNode*	n=stack[stack.size()-1];
+			stack.pop_back();
+			if(Intersect(n->volume,volume))
+			{
+				if(n->isinternal())
+				{
+					stack.push_back(n->childs[0]);
+					stack.push_back(n->childs[1]);
+				}
+				else
+				{
+					policy.Process(n);
+				}
+			}
+		} while(stack.size()>0);
+	}
+}
+
+
 DBVT_PREFIX
 inline void		btDbvt::rayTestInternal(	const btDbvtNode* root,
 								const btVector3& rayFrom,
@@ -959,7 +1006,8 @@ inline void		btDbvt::rayTestInternal(	const btDbvtNode* root,
 								btScalar lambda_max,
 								const btVector3& aabbMin,
 								const btVector3& aabbMax,
-								DBVT_IPOLICY) const
+                                btAlignedObjectArray<const btDbvtNode*>& stack,
+                                DBVT_IPOLICY ) const
 {
         (void) rayTo;
 	DBVT_CHECKTYPE
@@ -969,7 +1017,6 @@ inline void		btDbvt::rayTestInternal(	const btDbvtNode* root,
 
 		int								depth=1;
 		int								treshold=DOUBLE_STACKSIZE-2;
-		btAlignedObjectArray<const btDbvtNode*>&	stack = m_rayTestStack;
 		stack.resize(DOUBLE_STACKSIZE);
 		stack[0]=root;
 		btVector3 bounds[2];
@@ -1196,23 +1243,32 @@ inline void		btDbvt::collideOCL(	const btDbvtNode* root,
 							
 							//void * memmove ( void * destination, const void * source, size_t num );
 							
-//#if DBVT_USE_MEMMOVE
-//							memmove(&stack[j],&stack[j-1],sizeof(int)*(stack.size()-j-1));
-//#else
-							for(int k=stack.size()-1;k>j;--k) 
-							{
+#if DBVT_USE_MEMMOVE
+                     {
+                     int num_items_to_move = stack.size()-1-j;
+                     if(num_items_to_move > 0)
+                        memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move);
+                     }
+#else
+                     for(int k=stack.size()-1;k>j;--k) {
 								stack[k]=stack[k-1];
-							}
-//#endif
+                     }
+#endif
 							stack[j]=allocate(ifree,stock,nes[q]);
 							/* Insert 1	*/ 
 							j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
 							stack.push_back(0);
-//#if DBVT_USE_MEMMOVE
-//							memmove(&stack[j],&stack[j-1],sizeof(int)*(stack.size()-j-1));
-//#else
-							for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
-//#endif
+#if DBVT_USE_MEMMOVE
+                     {
+                     int num_items_to_move = stack.size()-1-j;
+                     if(num_items_to_move > 0)
+                        memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move);
+                     }
+#else
+                     for(int k=stack.size()-1;k>j;--k) {
+                        stack[k]=stack[k-1];
+                     }
+#endif
 							stack[j]=allocate(ifree,stock,nes[1-q]);
 						}
 						else

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -16,6 +16,7 @@ subject to the following restrictions:
 ///btDbvtBroadphase implementation by Nathanael Presson
 
 #include "btDbvtBroadphase.h"
+#include "LinearMath/btThreads.h"
 
 //
 // Profiling
@@ -142,6 +143,11 @@ btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
 	{
 		m_stageRoots[i]=0;
 	}
+#if BT_THREADSAFE
+    m_rayTestStacks.resize(BT_MAX_THREAD_COUNT);
+#else
+    m_rayTestStacks.resize(1);
+#endif
 #if DBVT_BP_PROFILE
 	clear(m_profiling);
 #endif
@@ -162,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,
@@ -227,6 +232,23 @@ struct	BroadphaseRayTester : btDbvt::ICollide
 void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
 {
 	BroadphaseRayTester callback(rayCallback);
+    btAlignedObjectArray<const btDbvtNode*>* stack = &m_rayTestStacks[0];
+#if BT_THREADSAFE
+    // for this function to be threadsafe, each thread must have a separate copy
+    // of this stack.  This could be thread-local static to avoid dynamic allocations,
+    // instead of just a local.
+    int threadIndex = btGetCurrentThreadIndex();
+    btAlignedObjectArray<const btDbvtNode*> localStack;
+    if (threadIndex < m_rayTestStacks.size())
+    {
+        // use per-thread preallocated stack if possible to avoid dynamic allocations
+        stack = &m_rayTestStacks[threadIndex];
+    }
+    else
+    {
+        stack = &localStack;
+    }
+#endif
 
 	m_sets[0].rayTestInternal(	m_sets[0].m_root,
 		rayFrom,
@@ -236,6 +258,7 @@ void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
 		rayCallback.m_lambda_max,
 		aabbMin,
 		aabbMax,
+        *stack,
 		callback);
 
 	m_sets[1].rayTestInternal(	m_sets[1].m_root,
@@ -246,6 +269,7 @@ void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
 		rayCallback.m_lambda_max,
 		aabbMin,
 		aabbMax,
+        *stack,
 		callback);
 
 }

Source/ThirdParty/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;
@@ -87,6 +87,7 @@ struct	btDbvtBroadphase : btBroadphaseInterface
 	bool					m_releasepaircache;			// Release pair cache on delete
 	bool					m_deferedcollide;			// Defere dynamic/static collision to collide call
 	bool					m_needcleanup;				// Need to run cleanup?
+    btAlignedObjectArray< btAlignedObjectArray<const btDbvtNode*> > m_rayTestStacks;
 #if DBVT_BP_PROFILE
 	btClock					m_clock;
 	struct	{
@@ -104,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));

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.h

@@ -64,6 +64,12 @@ struct btDispatcherInfo
 	btScalar	m_convexConservativeDistanceThreshold;
 };
 
+enum ebtDispatcherQueryType
+{
+	BT_CONTACT_POINT_ALGORITHMS = 1,
+	BT_CLOSEST_POINT_ALGORITHMS = 2
+};
+
 ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.
 ///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic).
 class btDispatcher
@@ -73,7 +79,7 @@ class btDispatcher
 public:
 	virtual ~btDispatcher() ;
 
-	virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold=0) = 0;
+	virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0;
 
 	virtual btPersistentManifold*	getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0;
 

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

@@ -1,489 +0,0 @@
-/*
-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 "btMultiSapBroadphase.h"
-
-#include "btSimpleBroadphase.h"
-#include "LinearMath/btAabbUtil2.h"
-#include "btQuantizedBvh.h"
-
-///	btSapBroadphaseArray	m_sapBroadphases;
-
-///	btOverlappingPairCache*	m_overlappingPairs;
-extern int gOverlappingPairs;
-
-/*
-class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
-{
-public:
-
-	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
-	{
-		return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy);
-	}
-};
-
-*/
-
-btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache)
-:m_overlappingPairs(pairCache),
-m_optimizedAabbTree(0),
-m_ownsPairCache(false),
-m_invalidPair(0)
-{
-	if (!m_overlappingPairs)
-	{
-		m_ownsPairCache = true;
-		void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16);
-		m_overlappingPairs = new (mem)btSortedOverlappingPairCache();
-	}
-
-	struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
-	{
-		virtual ~btMultiSapOverlapFilterCallback()
-		{}
-		// return true when pairs need collision
-		virtual bool	needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
-		{
-			btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
-			btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
-			
-			bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
-			collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
-	
-			return collides;
-		}
-	};
-
-	void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16);
-	m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
-
-	m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
-//	mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
-//	m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
-}
-
-btMultiSapBroadphase::~btMultiSapBroadphase()
-{
-	if (m_ownsPairCache)
-	{
-		m_overlappingPairs->~btOverlappingPairCache();
-		btAlignedFree(m_overlappingPairs);
-	}
-}
-
-
-void	btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax)
-{
-	m_optimizedAabbTree = new btQuantizedBvh();
-	m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax);
-	QuantizedNodeArray&	nodes = m_optimizedAabbTree->getLeafNodeArray();
-	for (int i=0;i<m_sapBroadphases.size();i++)
-	{
-		btQuantizedBvhNode node;
-		btVector3 aabbMin,aabbMax;
-		m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax);
-		m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
-		m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
-		int partId = 0;
-		node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i;
-		nodes.push_back(node);
-	}
-	m_optimizedAabbTree->buildInternal();
-}
-
-btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/)
-{
-	//void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
-
-	void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
-	btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
-	m_multiSapProxies.push_back(proxy);
-
-	///this should deal with inserting/removal into child broadphases
-	setAabb(proxy,aabbMin,aabbMax,dispatcher);
-	return proxy;
-}
-
-void	btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
-{
-	///not yet
-	btAssert(0);
-
-}
-
-
-void	btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*	childBroadphase)
-{
-	void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16);
-	btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy;
-	bridgeProxyRef->m_childProxy = childProxy;
-	bridgeProxyRef->m_childBroadphase = childBroadphase;
-	parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef);
-}
-
-
-bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax);
-bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
-{
-return
-amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
-amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
-amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
-}
-
-
-
-
-
-
-void	btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
-{
-	btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
-	aabbMin = multiProxy->m_aabbMin;
-	aabbMax = multiProxy->m_aabbMax;
-}
-
-void	btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
-{
-	for (int i=0;i<m_multiSapProxies.size();i++)
-	{
-		rayCallback.process(m_multiSapProxies[i]);
-	}
-}
-
-
-//#include <stdio.h>
-
-void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
-{
-	btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
-	multiProxy->m_aabbMin = aabbMin;
-	multiProxy->m_aabbMax = aabbMax;
-	
-	
-//	bool fullyContained = false;
-//	bool alreadyInSimple = false;
-	
-
-
-	
-	struct MyNodeOverlapCallback : public btNodeOverlapCallback
-	{
-		btMultiSapBroadphase*	m_multiSap;
-		btMultiSapProxy*		m_multiProxy;
-		btDispatcher*			m_dispatcher;
-
-		MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
-			:m_multiSap(multiSap),
-			m_multiProxy(multiProxy),
-			m_dispatcher(dispatcher)
-		{
-
-		}
-
-		virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex)
-		{
-			btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
-
-			int containingBroadphaseIndex = -1;
-			//already found?
-			for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
-			{
-
-				if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
-				{
-					containingBroadphaseIndex = i;
-					break;
-				}
-			}
-			if (containingBroadphaseIndex<0)
-			{
-				//add it
-				btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy);
-				m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
-
-			}
-		}
-	};
-
-	MyNodeOverlapCallback	myNodeCallback(this,multiProxy,dispatcher);
-
-
-
-	
-	if (m_optimizedAabbTree)
-		m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
-
-	int i;
-
-	for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
-	{
-		btVector3 worldAabbMin,worldAabbMax;
-		multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
-		bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
-		if (!overlapsBroadphase)
-		{
-			//remove it now
-			btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
-
-			btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
-			bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
-			
-			multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
-			multiProxy->m_bridgeProxies.pop_back();
-
-		}
-	}
-
-
-	/*
-
-	if (1)
-	{
-
-		//find broadphase that contain this multiProxy
-		int numChildBroadphases = getBroadphaseArray().size();
-		for (int i=0;i<numChildBroadphases;i++)
-		{
-			btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
-			btVector3 worldAabbMin,worldAabbMax;
-			childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
-			bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
-			
-		//	fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
-			int containingBroadphaseIndex = -1;
-			
-			//if already contains this
-			
-			for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
-			{
-				if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
-				{
-					containingBroadphaseIndex = i;
-				}
-				alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
-			}
-
-			if (overlapsBroadphase)
-			{
-				if (containingBroadphaseIndex<0)
-				{
-					btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
-					childProxy->m_multiSapParentProxy = multiProxy;
-					addToChildBroadphase(multiProxy,childProxy,childBroadphase);
-				}
-			} else
-			{
-				if (containingBroadphaseIndex>=0)
-				{
-					//remove
-					btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
-
-					btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
-					bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
-					
-					multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
-					multiProxy->m_bridgeProxies.pop_back();
-				}
-			}
-		}
-
-
-		///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force)
-		///hopefully we don't end up with many entries here (can assert/provide feedback on stats)
-		if (0)//!multiProxy->m_bridgeProxies.size())
-		{
-			///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
-			///this is needed to be able to calculate the aabb overlap
-			btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
-			childProxy->m_multiSapParentProxy = multiProxy;
-			addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
-		}
-	}
-
-	if (!multiProxy->m_bridgeProxies.size())
-	{
-		///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
-		///this is needed to be able to calculate the aabb overlap
-		btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
-		childProxy->m_multiSapParentProxy = multiProxy;
-		addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
-	}
-*/
-
-
-	//update
-	for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
-	{
-		btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
-		bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
-	}
-
-}
-bool stopUpdating=false;
-
-
-
-class btMultiSapBroadphasePairSortPredicate
-{
-	public:
-
-		bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) const
-		{
-				btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0;
-				btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0;
-				btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0;
-				btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
-
-				 return aProxy0 > bProxy0 || 
-					(aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
-					(aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); 
-		}
-};
-
-
-        ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
-void    btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
-{
-
-//	m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
-
-	if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
-	{
-	
-		btBroadphasePairArray&	overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
-
-	//	quicksort(overlappingPairArray,0,overlappingPairArray.size());
-
-		overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
-
-		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
-	//	overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
-
-		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-		m_invalidPair = 0;
-
-		
-		int i;
-
-		btBroadphasePair previousPair;
-		previousPair.m_pProxy0 = 0;
-		previousPair.m_pProxy1 = 0;
-		previousPair.m_algorithm = 0;
-		
-		
-		for (i=0;i<overlappingPairArray.size();i++)
-		{
-		
-			btBroadphasePair& pair = overlappingPairArray[i];
-
-			btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
-			btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0;
-			btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
-			btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
-
-			bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
-			
-			previousPair = pair;
-
-			bool needsRemoval = false;
-
-			if (!isDuplicate)
-			{
-				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
-
-				if (hasOverlap)
-				{
-					needsRemoval = false;//callback->processOverlap(pair);
-				} else
-				{
-					needsRemoval = true;
-				}
-			} else
-			{
-				//remove duplicate
-				needsRemoval = true;
-				//should have no algorithm
-				btAssert(!pair.m_algorithm);
-			}
-			
-			if (needsRemoval)
-			{
-				getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
-
-		//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
-		//		m_overlappingPairArray.pop_back();
-				pair.m_pProxy0 = 0;
-				pair.m_pProxy1 = 0;
-				m_invalidPair++;
-				gOverlappingPairs--;
-			} 
-			
-		}
-
-	///if you don't like to skip the invalid pairs in the array, execute following code:
-	#define CLEAN_INVALID_PAIRS 1
-	#ifdef CLEAN_INVALID_PAIRS
-
-		//perform a sort, to sort 'invalid' pairs to the end
-		//overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
-		overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
-
-		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-		m_invalidPair = 0;
-	#endif//CLEAN_INVALID_PAIRS
-		
-		//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
-	}
-
-
-}
-
-
-bool	btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
-{
-	btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
-		btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
-
-		return	TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
-			multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
-		
-}
-
-
-void	btMultiSapBroadphase::printStats()
-{
-/*	printf("---------------------------------\n");
-	
-		printf("btMultiSapBroadphase.h\n");
-		printf("numHandles = %d\n",m_multiSapProxies.size());
-			//find broadphase that contain this multiProxy
-		int numChildBroadphases = getBroadphaseArray().size();
-		for (int i=0;i<numChildBroadphases;i++)
-		{
-
-			btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
-			childBroadphase->printStats();
-
-		}
-		*/
-
-}
-
-void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
-{
-	// not yet
-}

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

@@ -1,151 +0,0 @@
-/*
-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_MULTI_SAP_BROADPHASE
-#define BT_MULTI_SAP_BROADPHASE
-
-#include "btBroadphaseInterface.h"
-#include "LinearMath/btAlignedObjectArray.h"
-#include "btOverlappingPairCache.h"
-
-
-class btBroadphaseInterface;
-class btSimpleBroadphase;
-
-
-typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray;
-
-///The btMultiSapBroadphase is a research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead.
-///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases.
-///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time.
-///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy.
-///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328
-///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329
-class btMultiSapBroadphase :public btBroadphaseInterface
-{
-	btSapBroadphaseArray	m_sapBroadphases;
-	
-	btSimpleBroadphase*		m_simpleBroadphase;
-
-	btOverlappingPairCache*	m_overlappingPairs;
-
-	class btQuantizedBvh*			m_optimizedAabbTree;
-
-
-	bool					m_ownsPairCache;
-	
-	btOverlapFilterCallback*	m_filterCallback;
-
-	int			m_invalidPair;
-
-	struct	btBridgeProxy
-	{
-		btBroadphaseProxy*		m_childProxy;
-		btBroadphaseInterface*	m_childBroadphase;
-	};
-
-
-public:
-
-	struct	btMultiSapProxy	: public btBroadphaseProxy
-	{
-
-		///array with all the entries that this proxy belongs to
-		btAlignedObjectArray<btBridgeProxy*> m_bridgeProxies;
-		btVector3	m_aabbMin;
-		btVector3	m_aabbMax;
-
-		int	m_shapeType;
-
-/*		void*	m_userPtr;
-		short int	m_collisionFilterGroup;
-		short int	m_collisionFilterMask;
-*/
-		btMultiSapProxy(const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
-			:btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask),
-			m_aabbMin(aabbMin),
-			m_aabbMax(aabbMax),
-			m_shapeType(shapeType)
-		{
-			m_multiSapParentProxy =this;
-		}
-
-		
-	};
-
-protected:
-
-
-	btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies;
-
-public:
-
-	btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0);
-
-
-	btSapBroadphaseArray&	getBroadphaseArray()
-	{
-		return m_sapBroadphases;
-	}
-
-	const btSapBroadphaseArray&	getBroadphaseArray() const
-	{
-		return m_sapBroadphases;
-	}
-
-	virtual ~btMultiSapBroadphase();
-
-	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
-	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
-	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
-	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
-
-	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));
-
-	void	addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*	childBroadphase);
-
-	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
-	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
-
-	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
-
-	virtual	btOverlappingPairCache*	getOverlappingPairCache()
-	{
-		return m_overlappingPairs;
-	}
-	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const
-	{
-		return m_overlappingPairs;
-	}
-
-	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
-	///will add some transform later
-	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
-	{
-		aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
-		aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
-	}
-
-	void	buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax);
-
-	virtual void	printStats();
-
-	void quicksort (btBroadphasePairArray& a, int lo, int hi);
-
-	///reset broadphase internal structures, to ensure determinism/reproducability
-	virtual void resetPool(btDispatcher* dispatcher);
-
-};
-
-#endif //BT_MULTI_SAP_BROADPHASE

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

@@ -34,7 +34,6 @@ int gFindPairs =0;
 
 btHashedOverlappingPairCache::btHashedOverlappingPairCache():
 	m_overlapFilterCallback(0),
-	m_blockedForChanges(false),
 	m_ghostPairCallback(0)
 {
 	int initialAllocatedSize= 2;
@@ -373,10 +372,10 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
 	return userData;
 }
 //#include <stdio.h>
-
+#include "LinearMath/btQuickprof.h"
 void	btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
 {
-
+	BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
 	int i;
 
 //	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

@@ -90,11 +90,11 @@ 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;
-	bool		m_blockedForChanges;
 
 protected:
 	
@@ -104,6 +104,8 @@ protected:
 
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
 	btHashedOverlappingPairCache();
 	virtual ~btHashedOverlappingPairCache();
 

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

@@ -107,6 +107,8 @@ void	btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btV
 			v = unQuantize(vecIn);
 			m_bvhAabbMin.setMin(v-clampValue);
 		}
+        aabbSize = m_bvhAabbMax - m_bvhAabbMin;
+        m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize;
 		{
 			quantize(vecIn,m_bvhAabbMax,true);
 			v = unQuantize(vecIn);

Source/ThirdParty/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
 {

Source/ThirdParty/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;
 }

Source/ThirdParty/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);
 

Source/ThirdParty/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.normalize();
-	btVector3 p1ToCentre = sphereCenter - vertices[0];
-	btScalar distanceFromPlane = p1ToCentre.dot(normal);
 
-	if (distanceFromPlane < btScalar(0.))
+	btScalar l2 = normal.length2();
+	bool hasContact = false;
+	btVector3 contactPoint;
+
+	if (l2 >= SIMD_EPSILON*SIMD_EPSILON)
 	{
-		//triangle facing the other way
-		distanceFromPlane *= btScalar(-1.);
-		normal *= btScalar(-1.);
-	}
+		normal /= btSqrt(l2);
+
+		btVector3 p1ToCentre = sphereCenter - vertices[0];
+		btScalar distanceFromPlane = p1ToCentre.dot(normal);
 
-	bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
+		if (distanceFromPlane < btScalar(0.))
+		{
+			//triangle facing the other way
+			distanceFromPlane *= btScalar(-1.);
+			normal *= btScalar(-1.);
+		}
+
+		bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
 	
-	// Check for contact / intersection
-	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;
+		// 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;
+					}
+
 				}
-				
 			}
 		}
 	}

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h

@@ -40,6 +40,9 @@ public:
 
 	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
 
+	virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0;
+
+
 };
 
 #endif //BT_COLLISION_CONFIGURATION

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -16,7 +16,7 @@ subject to the following restrictions:
 
 
 #include "btCollisionDispatcher.h"
-
+#include "LinearMath/btQuickprof.h"
 
 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 
@@ -50,8 +50,10 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH
 	{
 		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
 		{
-			m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
-			btAssert(m_doubleDispatch[i][j]);
+			m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
+			btAssert(m_doubleDispatchContactPoints[i][j]);
+			m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j);
+
 		}
 	}
 	
@@ -61,7 +63,12 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH
 
 void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
 {
-	m_doubleDispatch[proxyType0][proxyType1] = createFunc;
+	m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc;
+}
+
+void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
+{
+	m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc;
 }
 
 btCollisionDispatcher::~btCollisionDispatcher()
@@ -84,14 +91,10 @@ btPersistentManifold*	btCollisionDispatcher::getNewManifold(const btCollisionObj
 
 	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
 		
- 	void* mem = 0;
-	
-	if (m_persistentManifoldPoolAllocator->getFreeCount())
-	{
-		mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
-	} else
+ 	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.
+        //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);
@@ -142,14 +145,23 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 
 	
 
-btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold)
+
+btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType)
 {
 	
 	btCollisionAlgorithmConstructionInfo ci;
 
 	ci.m_dispatcher1 = this;
 	ci.m_manifold = sharedManifold;
-	btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap);
+	btCollisionAlgorithm* algo = 0;
+	if (algoType == BT_CONTACT_POINT_ALGORITHMS)
+	{
+		algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
+	}
+	else
+	{
+		algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
+	}
 
 	return algo;
 }
@@ -228,7 +240,6 @@ public:
 	virtual bool	processOverlap(btBroadphasePair& pair)
 	{
 		(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
-
 		return false;
 	}
 };
@@ -249,7 +260,6 @@ void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pa
 
 
 
-
 //by default, Bullet will use this near callback
 void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
 {
@@ -265,7 +275,7 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair,
 			//dispatcher will keep algorithms persistent in the collision pair
 			if (!collisionPair.m_algorithm)
 			{
-				collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap);
+				collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap,0, BT_CONTACT_POINT_ALGORITHMS);
 			}
 
 			if (collisionPair.m_algorithm)
@@ -293,13 +303,13 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair,
 
 void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
 {
-	if (m_collisionAlgorithmPoolAllocator->getFreeCount())
-	{
-		return m_collisionAlgorithmPoolAllocator->allocate(size);
-	}
-	
-	//warn user for overflow?
-	return	btAlignedAlloc(static_cast<size_t>(size), 16);
+    void* mem = m_collisionAlgorithmPoolAllocator->allocate( size );
+    if (NULL == mem)
+    {
+	    //warn user for overflow?
+	    return btAlignedAlloc(static_cast<size_t>(size), 16);
+    }
+    return mem;
 }
 
 void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

@@ -57,7 +57,9 @@ protected:
 
 	btPoolAllocator*	m_persistentManifoldPoolAllocator;
 
-	btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
+	btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
+
+	btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
 
 	btCollisionConfiguration*	m_collisionConfiguration;
 
@@ -84,6 +86,8 @@ public:
 	///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
 	void	registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
 
+	void	registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc);
+
 	int	getNumManifolds() const
 	{ 
 		return int( m_manifoldsPtr.size());
@@ -115,7 +119,7 @@ public:
 
 	virtual void clearManifold(btPersistentManifold* manifold);
 
-	btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold = 0);
+	btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType);
 		
 	virtual bool	needsCollision(const btCollisionObject* body0,const btCollisionObject* body1);
 	

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -28,13 +28,18 @@ btCollisionObject::btCollisionObject()
 		m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
 		m_islandTag1(-1),
 		m_companionId(-1),
+        m_worldArrayIndex(-1),
 		m_activationState1(1),
 		m_deactivationTime(btScalar(0.)),
 		m_friction(btScalar(0.5)),
 		m_restitution(btScalar(0.)),
 		m_rollingFriction(0.0f),
+        m_spinningFriction(0.f),
+		m_contactDamping(.1),
+		m_contactStiffness(1e4),
 		m_internalType(CO_COLLISION_OBJECT),
 		m_userObjectPointer(0),
+		m_userIndex2(-1),
 		m_userIndex(-1),
 		m_hitFraction(btScalar(1.)),
 		m_ccdSweptSphereRadius(btScalar(0.)),
@@ -91,6 +96,8 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
 	dataOut->m_deactivationTime = m_deactivationTime;
 	dataOut->m_friction = m_friction;
 	dataOut->m_rollingFriction = m_rollingFriction;
+	dataOut->m_contactDamping = m_contactDamping;
+	dataOut->m_contactStiffness = m_contactStiffness;
 	dataOut->m_restitution = m_restitution;
 	dataOut->m_internalType = m_internalType;
 	

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -79,13 +79,19 @@ protected:
 
 	int				m_islandTag1;
 	int				m_companionId;
+    int             m_worldArrayIndex;  // index of object in world's collisionObjects array
 
 	mutable int				m_activationState1;
 	mutable btScalar			m_deactivationTime;
 
 	btScalar		m_friction;
 	btScalar		m_restitution;
-	btScalar		m_rollingFriction;
+	btScalar		m_rollingFriction;//torsional friction orthogonal to contact normal (useful to stop spheres rolling forever)
+    btScalar        m_spinningFriction; // torsional friction around the contact normal (useful for grasping)
+	btScalar		m_contactDamping;
+	btScalar		m_contactStiffness;
+	
+	
 
 	///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
 	///do not assign your own m_internalType unless you write a new dynamics object class.
@@ -93,8 +99,10 @@ protected:
 
 	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
 
-    void*			m_userObjectPointer;
-    
+	void*			m_userObjectPointer;
+
+	int				m_userIndex2;
+	
     int	m_userIndex;
 
 	///time of impact calculation
@@ -114,6 +122,7 @@ protected:
 	///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation.
 	int			m_updateRevision;
 
+	btVector3	m_customDebugColorRGB;
 
 public:
 
@@ -127,7 +136,9 @@ public:
 		CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
 		CF_CHARACTER_OBJECT = 16,
 		CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
-		CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing
+		CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
+		CF_HAS_CONTACT_STIFFNESS_DAMPING = 128,
+		CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
 	};
 
 	enum	CollisionObjectTypes
@@ -316,8 +327,40 @@ public:
 	{
 		return m_rollingFriction;
 	}
-
-
+    void	setSpinningFriction(btScalar frict)
+    {
+        m_updateRevision++;
+        m_spinningFriction = frict;
+    }
+    btScalar	getSpinningFriction() const
+    {
+        return m_spinningFriction;
+    }
+    void	setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
+	{
+		m_updateRevision++;
+		m_contactStiffness = stiffness;
+		m_contactDamping = damping;
+		
+		m_collisionFlags |=CF_HAS_CONTACT_STIFFNESS_DAMPING;
+		
+        //avoid divisions by zero...
+		if (m_contactStiffness< SIMD_EPSILON)
+        {
+            m_contactStiffness = SIMD_EPSILON;
+        }
+	}
+	
+	btScalar	getContactStiffness() const
+	{
+		return m_contactStiffness;
+	}
+	
+	btScalar	getContactDamping() const
+	{
+		return m_contactDamping;
+	}
+    
 	///reserved for Bullet internal usage
 	int	getInternalType() const
 	{
@@ -415,7 +458,18 @@ public:
 		m_companionId = id;
 	}
 
-	SIMD_FORCE_INLINE btScalar			getHitFraction() const
+    SIMD_FORCE_INLINE int getWorldArrayIndex() const
+    {
+        return	m_worldArrayIndex;
+    }
+
+    // only should be called by CollisionWorld
+    void setWorldArrayIndex(int ix)
+    {
+        m_worldArrayIndex = ix;
+    }
+
+    SIMD_FORCE_INLINE btScalar			getHitFraction() const
 	{
 		return m_hitFraction; 
 	}
@@ -476,6 +530,12 @@ public:
 	{
 		return m_userIndex;
 	}
+	
+	int	getUserIndex2() const
+	{
+		return m_userIndex2;
+	}
+	
 	///users can point to their objects, userPointer is not used by Bullet
 	void	setUserPointer(void* userPointer)
 	{
@@ -487,12 +547,37 @@ public:
 	{
 		m_userIndex = index;
 	}
+	
+	void	setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
 
 	int	getUpdateRevisionInternal() const
 	{
 		return m_updateRevision;
 	}
 
+	void	setCustomDebugColor(const btVector3& colorRGB)
+	{
+		m_customDebugColorRGB = colorRGB;
+		m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
+	}
+
+	void	removeCustomDebugColor()
+	{
+		m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
+	}
+
+	bool getCustomDebugColor(btVector3& colorRGB) const
+	{
+		bool hasCustomColor = (0!=(m_collisionFlags&CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR));
+		if (hasCustomColor)
+		{
+			colorRGB = m_customDebugColorRGB;
+		}
+		return hasCustomColor;
+	}
 
 	inline bool checkCollideWith(const btCollisionObject* co) const
 	{
@@ -528,6 +613,8 @@ struct	btCollisionObjectDoubleData
 	double					m_deactivationTime;
 	double					m_friction;
 	double					m_rollingFriction;
+	double                  m_contactDamping;
+	double                  m_contactStiffness;
 	double					m_restitution;
 	double					m_hitFraction; 
 	double					m_ccdSweptSphereRadius;
@@ -561,7 +648,8 @@ struct	btCollisionObjectFloatData
 	float					m_deactivationTime;
 	float					m_friction;
 	float					m_rollingFriction;
-
+    float                   m_contactDamping;
+    float                   m_contactStiffness;
 	float					m_restitution;
 	float					m_hitFraction; 
 	float					m_ccdSweptSphereRadius;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -110,14 +110,16 @@ btCollisionWorld::~btCollisionWorld()
 
 
 
-void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask)
 {
 
 	btAssert(collisionObject);
 
 	//check that the object isn't already added
 	btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
+    btAssert(collisionObject->getWorldArrayIndex() == -1);  // do not add the same object to more than one collision world
 
+    collisionObject->setWorldArrayIndex(m_collisionObjects.size());
 	m_collisionObjects.push_back(collisionObject);
 
 	//calculate new AABB
@@ -135,8 +137,7 @@ void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
 		collisionObject,
 		collisionFilterGroup,
 		collisionFilterMask,
-		m_dispatcher1,0
-		))	;
+		m_dispatcher1))	;
 
 
 
@@ -197,6 +198,7 @@ void	btCollisionWorld::updateAabbs()
 	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
+        btAssert(colObj->getWorldArrayIndex() == i);
 
 		//only update aabb of active objects
 		if (m_forceUpdateAllAabbs || colObj->isActive())
@@ -255,9 +257,25 @@ void	btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
 	}
 
 
-	//swapremove
-	m_collisionObjects.remove(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?
+    if (iObj >= 0 && iObj < m_collisionObjects.size())
+    {
+        btAssert(collisionObject == m_collisionObjects[iObj]);
+        m_collisionObjects.swap(iObj, m_collisionObjects.size()-1);
+        m_collisionObjects.pop_back();
+        if (iObj < m_collisionObjects.size())
+        {
+            m_collisionObjects[iObj]->setWorldArrayIndex(iObj);
+        }
+    }
+    else
+    {
+        // slow linear search
+        //swapremove
+        m_collisionObjects.remove(collisionObject);
+    }
+    collisionObject->setWorldArrayIndex(-1);
 }
 
 
@@ -792,23 +810,50 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 				}
 			}
 		} else {
-			///@todo : use AABB tree or other BVH acceleration structure!
 			if (collisionShape->isCompound())
 			{
-				BT_PROFILE("convexSweepCompound");
-				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
-				int i=0;
-				for (i=0;i<compoundShape->getNumChildShapes();i++)
+				struct	btCompoundLeafCallback : btDbvt::ICollide
 				{
-					btTransform childTrans = compoundShape->getChildTransform(i);
-					const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
-					btTransform childWorldTrans = colObjWorldTransform * childTrans;
-					
-                    struct	LocalInfoAdder : public ConvexResultCallback {
-                            ConvexResultCallback* m_userCallback;
+					btCompoundLeafCallback(
+										   const btCollisionObjectWrapper* colObjWrap,
+										   const btConvexShape* castShape,
+										   const btTransform& convexFromTrans,
+										   const btTransform& convexToTrans,
+										   btScalar allowedPenetration,
+										   const btCompoundShape* compoundShape,
+										   const btTransform& colObjWorldTransform,
+										   ConvexResultCallback& resultCallback)
+					: 
+					  m_colObjWrap(colObjWrap),
+						m_castShape(castShape),
+						m_convexFromTrans(convexFromTrans),
+						m_convexToTrans(convexToTrans),
+						m_allowedPenetration(allowedPenetration),
+						m_compoundShape(compoundShape),
+						m_colObjWorldTransform(colObjWorldTransform),
+						m_resultCallback(resultCallback) {
+					}
+
+				  const btCollisionObjectWrapper* m_colObjWrap;
+					const btConvexShape* m_castShape;
+					const btTransform& m_convexFromTrans;
+					const btTransform& m_convexToTrans;
+					btScalar m_allowedPenetration;
+					const btCompoundShape* m_compoundShape;
+					const btTransform& m_colObjWorldTransform;
+					ConvexResultCallback& m_resultCallback;
+
+				public:
+
+					void		ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape)
+					{
+						btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
+
+						struct	LocalInfoAdder : public ConvexResultCallback {
+							ConvexResultCallback* m_userCallback;
 							int m_i;
 
-                            LocalInfoAdder (int i, ConvexResultCallback *user)
+							LocalInfoAdder(int i, ConvexResultCallback *user)
 								: m_userCallback(user), m_i(i)
 							{
 								m_closestHitFraction = m_userCallback->m_closestHitFraction;
@@ -817,27 +862,66 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 							{
 								return m_userCallback->needsCollision(p);
 							}
-                            virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult&	r,	bool b)
-                            {
-                                    btCollisionWorld::LocalShapeInfo	shapeInfo;
-                                    shapeInfo.m_shapePart = -1;
-                                    shapeInfo.m_triangleIndex = m_i;
-                                    if (r.m_localShapeInfo == NULL)
-                                        r.m_localShapeInfo = &shapeInfo;
-									const btScalar result = m_userCallback->addSingleResult(r, b);
-									m_closestHitFraction = m_userCallback->m_closestHitFraction;
-									return result;
-                                    
-                            }
-                    };
+							virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult&	r, bool b)
+							{
+								btCollisionWorld::LocalShapeInfo	shapeInfo;
+								shapeInfo.m_shapePart = -1;
+								shapeInfo.m_triangleIndex = m_i;
+								if (r.m_localShapeInfo == NULL)
+									r.m_localShapeInfo = &shapeInfo;
+								const btScalar result = m_userCallback->addSingleResult(r, b);
+								m_closestHitFraction = m_userCallback->m_closestHitFraction;
+								return result;
 
-                    LocalInfoAdder my_cb(i, &resultCallback);
-					
-					btCollisionObjectWrapper tmpObj(colObjWrap,childCollisionShape,colObjWrap->getCollisionObject(),childWorldTrans,-1,i);
+							}
+						};
 
-					objectQuerySingleInternal(castShape, convexFromTrans,convexToTrans,
-						&tmpObj,my_cb, allowedPenetration);
-					
+						LocalInfoAdder my_cb(index, &m_resultCallback);
+
+						btCollisionObjectWrapper tmpObj(m_colObjWrap, childCollisionShape, m_colObjWrap->getCollisionObject(), childWorldTrans, -1, index);
+
+						objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration);
+					}
+
+					void		Process(const btDbvtNode* leaf)
+					{
+						// Processing leaf node
+						int index = leaf->dataAsInt;
+
+						btTransform childTrans = m_compoundShape->getChildTransform(index);
+						const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(index);
+
+						ProcessChild(index, childTrans, childCollisionShape);
+					}
+				};
+
+				BT_PROFILE("convexSweepCompound");
+				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+
+				btVector3 fromLocalAabbMin, fromLocalAabbMax;
+				btVector3 toLocalAabbMin, toLocalAabbMax;
+
+				castShape->getAabb(colObjWorldTransform.inverse() * convexFromTrans, fromLocalAabbMin, fromLocalAabbMax);
+				castShape->getAabb(colObjWorldTransform.inverse() * convexToTrans, toLocalAabbMin, toLocalAabbMax);
+
+				fromLocalAabbMin.setMin(toLocalAabbMin);
+				fromLocalAabbMax.setMax(toLocalAabbMax);
+
+				btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans,
+					  allowedPenetration, compoundShape, colObjWorldTransform, resultCallback);
+
+				const btDbvt* tree = compoundShape->getDynamicAabbTree();
+				if (tree) {
+					const ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax);
+					tree->collideTV(tree->m_root, bounds, callback);
+				} else {
+					int i;
+					for (i=0;i<compoundShape->getNumChildShapes();i++)
+					{
+						const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
+						btTransform childTrans = compoundShape->getChildTransform(i);
+						callback.ProcessChild(i, childTrans, childCollisionShape);
+					}
 				}
 			}
 		}
@@ -1148,7 +1232,7 @@ struct btSingleContactCallback : public btBroadphaseAabbCallback
 			btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1);
 			btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1);
 
-			btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1);
+			btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1,0, BT_CLOSEST_POINT_ALGORITHMS);
 			if (algorithm)
 			{
 				btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback);
@@ -1184,10 +1268,11 @@ void	btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionOb
 	btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1);
 	btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1);
 
-	btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB);
+	btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB, 0, BT_CLOSEST_POINT_ALGORITHMS);
 	if (algorithm)
 	{
 		btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
+		contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold;
 		//discrete collision detection query
 		algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);
 
@@ -1500,6 +1585,8 @@ void	btCollisionWorld::debugDrawWorld()
 							}
 						};
 
+						colObj->getCustomDebugColor(color);
+
 						debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
 					}
 					if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -27,7 +27,7 @@ subject to the following restrictions:
  * @section install_sec Installation
  *
  * @subsection step1 Step 1: Download
- * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list
+ * You can download the Bullet Physics Library from the github repository: https://github.com/bulletphysics/bullet3/releases 
  *
  * @subsection step2 Step 2: Building
  * Bullet has multiple build systems, including premake, cmake and autotools. Premake and cmake support all platforms.
@@ -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,12 +410,14 @@ 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()
 			:m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
-			m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+			m_collisionFilterMask(btBroadphaseProxy::AllFilter),
+			m_closestDistanceThreshold(0)
 		{
 		}
 
@@ -481,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()
 	{

Source/ThirdParty/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;
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -67,7 +67,13 @@ void	btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionO
 			const btCollisionShape* childShape = compoundShape->getChildShape(i);
 
 			btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully)
-			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold);
+			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
+
+
+			btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact;
+			btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints;
+
+
 		}
 	}
 }
@@ -130,8 +136,14 @@ public:
 		btTransform	newChildWorldTrans = orgTrans*childTrans ;
 
 		//perform an AABB check first
-		btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+		btVector3 aabbMin0,aabbMax0;
 		childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+
+		btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
+		aabbMin0 -= extendAabb;
+		aabbMax0 += extendAabb;
+
+		btVector3 aabbMin1, aabbMax1;
 		m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
 
 		if (gCompoundChildShapePairCallback)
@@ -144,12 +156,22 @@ public:
 		{
 
 			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
+			
+			btCollisionAlgorithm* algo = 0;
 
-
-			//the contactpoint is still projected back using the original inverted worldtrans
-			if (!m_childCollisionAlgorithms[index])
-				m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold);
-
+			if (m_resultOut->m_closestPointDistanceThreshold > 0)
+			{
+				algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
+			}
+			else
+			{
+				//the contactpoint is still projected back using the original inverted worldtrans
+				if (!m_childCollisionAlgorithms[index])
+				{
+					m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
+				}
+				algo = m_childCollisionAlgorithms[index];
+			}
 			
 			const btCollisionObjectWrapper* tmpWrap = 0;
 
@@ -166,8 +188,7 @@ public:
 				m_resultOut->setShapeIdentifiersB(-1,index);
 			}
 
-
-			m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
+			algo->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
 
 #if 0
 			if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
@@ -250,7 +271,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 	///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
 	{
 		int i;
-		btManifoldArray manifoldArray;
+		manifoldArray.resize(0);
 		for (i=0;i<m_childCollisionAlgorithms.size();i++)
 		{
 			if (m_childCollisionAlgorithms[i])
@@ -277,10 +298,13 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 		btTransform otherInCompoundSpace;
 		otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
 		otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
+		btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
+		localAabbMin -= extraExtends;
+		localAabbMax += extraExtends;
 
 		const ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
 		//process all children, that overlap with  the given AABB bounds
-		tree->collideTV(tree->m_root,bounds,callback);
+		tree->collideTVNoStackAlloc(tree->m_root,bounds,stack2,callback);
 
 	} else
 	{
@@ -297,7 +321,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 				//iterate over all children, perform an AABB check inside ProcessChildShape
 		int numChildren = m_childCollisionAlgorithms.size();
 		int i;
-		btManifoldArray	manifoldArray;
+		manifoldArray.resize(0);
         const btCollisionShape* childShape = 0;
         btTransform	orgTrans;
         

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h

@@ -26,6 +26,7 @@ class btDispatcher;
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 #include "btCollisionCreateFunc.h"
 #include "LinearMath/btAlignedObjectArray.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
 class btDispatcher;
 class btCollisionObject;
 
@@ -36,6 +37,9 @@ extern btShapePairCallback gCompoundChildShapePairCallback;
 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
 class btCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
 {
+	btNodeStack stack2;
+	btManifoldArray manifoldArray;
+
 protected:
 	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
 	bool m_isSwapped;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp

@@ -15,6 +15,7 @@ subject to the following restrictions:
 */
 
 #include "btCompoundCompoundCollisionAlgorithm.h"
+#include "LinearMath/btQuickprof.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
@@ -23,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;
 
@@ -124,6 +127,7 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 	
 	void		Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
 	{
+		BT_PROFILE("btCompoundCompoundLeafCallback::Process");
 		m_numOverlapPairs++;
 
 
@@ -159,6 +163,11 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 		childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
 		childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
 		
+		btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
+
+		aabbMin0 -= thresholdVec;
+		aabbMax0 += thresholdVec;
+
 		if (gCompoundCompoundChildShapePairCallback)
 		{
 			if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
@@ -174,17 +183,24 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 			btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
 
 			btCollisionAlgorithm* colAlgo = 0;
-
-			if (pair)
+			if (m_resultOut->m_closestPointDistanceThreshold > 0)
 			{
-				colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
-				
-			} else
+				colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
+			}
+			else
 			{
-				colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
-				pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
-				btAssert(pair);
-				pair->m_userPointer = colAlgo;
+				if (pair)
+				{
+					colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
+
+				}
+				else
+				{
+					colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
+					pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1);
+					btAssert(pair);
+					pair->m_userPointer = colAlgo;
+				}
 			}
 
 			btAssert(colAlgo);
@@ -215,10 +231,12 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 
 
 static DBVT_INLINE bool		MyIntersect(	const btDbvtAabbMm& a,
-								  const btDbvtAabbMm& b, const btTransform& xform)
+								  const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
 {
 	btVector3 newmin,newmax;
 	btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
+	newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
+	newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
 	btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
 	return Intersect(a,newb);
 }
@@ -227,7 +245,7 @@ static DBVT_INLINE bool		MyIntersect(	const btDbvtAabbMm& a,
 static inline void		MycollideTT(	const btDbvtNode* root0,
 								  const btDbvtNode* root1,
 								  const btTransform& xform,
-								  btCompoundCompoundLeafCallback* callback)
+								  btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
 {
 
 		if(root0&&root1)
@@ -235,11 +253,16 @@ 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];
-				if(MyIntersect(p.a->volume,p.b->volume,xform))
+				if(MyIntersect(p.a->volume,p.b->volume,xform, distanceThreshold))
 				{
 					if(depth>treshold)
 					{
@@ -313,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++)
 		{
@@ -341,7 +368,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 
 
 	const btTransform	xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
-	MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
+	MycollideTT(tree0->m_root,tree1->m_root,xform,&callback, resultOut->m_closestPointDistanceThreshold);
 
 	//printf("#compound-compound child/leaf overlap =%d                      \r",callback.m_numOverlapPairs);
 
@@ -381,7 +408,9 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 					newChildWorldTrans0 = orgTrans0*childTrans0 ;
 					childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
 				}
-
+				btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
+				aabbMin0 -= thresholdVec;
+				aabbMax0 += thresholdVec;
 				{
 					btTransform	orgInterpolationTrans1;
 					const btCollisionShape* childShape1 = 0;
@@ -396,7 +425,8 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 					childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
 				}
 				
-				
+				aabbMin1 -= thresholdVec;
+				aabbMax1 += thresholdVec;
 
 				if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
 				{

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp

@@ -47,8 +47,6 @@ subject to the following restrictions:
 
 btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
 {
-	m_numPerturbationIterations = 0;
-	m_minimumPointsPerturbationThreshold = 3;
 	m_simplexSolver = simplexSolver;
 	m_pdSolver = pdSolver;
 }
@@ -57,15 +55,13 @@ btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
 { 
 }
 
-btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */)
 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
 m_simplexSolver(simplexSolver),
 m_pdSolver(pdSolver),
 m_ownManifold (false),
 m_manifoldPtr(mf),
-m_lowLevelOfDetail(false),
- m_numPerturbationIterations(numPerturbationIterations),
-m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+m_lowLevelOfDetail(false)
 {
 	(void)body0Wrap;
 	(void)body1Wrap;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h

@@ -40,9 +40,6 @@ class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm
 	btPersistentManifold*	m_manifoldPtr;
 	bool			m_lowLevelOfDetail;
 	
-	int m_numPerturbationIterations;
-	int m_minimumPointsPerturbationThreshold;
-
 public:
 
 	btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -15,6 +15,7 @@ subject to the following restrictions:
 
 
 #include "btConvexConcaveCollisionAlgorithm.h"
+#include "LinearMath/btQuickprof.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
@@ -29,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)
 {
 }
 
@@ -79,6 +80,7 @@ void	btConvexTriangleCallback::clearCache()
 void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
 partId, int triangleIndex)
 {
+	BT_PROFILE("btConvexTriangleCallback::processTriangle");
 
 	if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
 	{
@@ -116,8 +118,16 @@ partId, int triangleIndex)
 		
 		
 		btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
-		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr);
-
+		btCollisionAlgorithm* colAlgo = 0;
+		
+		if (m_resultOut->m_closestPointDistanceThreshold > 0)
+		{
+			colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
+		}
+		else
+		{
+			colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS);
+		}
 		const btCollisionObjectWrapper* tmpWrap = 0;
 
 		if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
@@ -168,7 +178,8 @@ void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
 	const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
 	//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
 	convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
-	btScalar extraMargin = collisionMarginTriangle;
+	btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold;
+	
 	btVector3 extra(extraMargin,extraMargin,extraMargin);
 
 	m_aabbMax += extra;
@@ -184,7 +195,7 @@ void btConvexConcaveCollisionAlgorithm::clearCache()
 
 void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
-	
+	BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
 	
 	const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
 	const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
@@ -265,6 +276,7 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
 		
 		virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 		{
+			BT_PROFILE("processTriangle");
 			(void)partId;
 			(void)triangleIndex;
 			//do a swept sphere for now

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h

@@ -26,14 +26,16 @@ class btDispatcher;
 #include "btCollisionCreateFunc.h"
 
 ///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called.
-class btConvexTriangleCallback : public btTriangleCallback
+ATTRIBUTE_ALIGNED16(class)  btConvexTriangleCallback : public btTriangleCallback
 {
-	const btCollisionObjectWrapper* m_convexBodyWrap;
-	const btCollisionObjectWrapper* m_triBodyWrap;
 
 	btVector3	m_aabbMin;
 	btVector3	m_aabbMax ;
 
+	const btCollisionObjectWrapper* m_convexBodyWrap;
+	const btCollisionObjectWrapper* m_triBodyWrap;
+
+
 
 	btManifoldResult* m_resultOut;
 	btDispatcher*	m_dispatcher;
@@ -41,6 +43,8 @@ class btConvexTriangleCallback : public btTriangleCallback
 	btScalar m_collisionMarginTriangle;
 	
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
 int	m_triangleCount;
 	
 	btPersistentManifold*	m_manifoldPtr;
@@ -75,17 +79,19 @@ int	m_triangleCount;
 
 
 /// btConvexConcaveCollisionAlgorithm  supports collision between convex shapes and (concave) trianges meshes.
-class btConvexConcaveCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+ATTRIBUTE_ALIGNED16(class)  btConvexConcaveCollisionAlgorithm  : public btActivatingCollisionAlgorithm
 {
 
-	bool	m_isSwapped;
-
 	btConvexTriangleCallback m_btConvexTriangleCallback;
 
+	bool	m_isSwapped;
+
 
 
 public:
 
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
 	btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
 
 	virtual ~btConvexConcaveCollisionAlgorithm();

Source/ThirdParty/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"
@@ -179,11 +179,10 @@ static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
 
 
 
-btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* pdSolver)
 {
 	m_numPerturbationIterations = 0;
 	m_minimumPointsPerturbationThreshold = 3;
-	m_simplexSolver = simplexSolver;
 	m_pdSolver = pdSolver;
 }
 
@@ -191,9 +190,8 @@ btConvexConvexAlgorithm::CreateFunc::~CreateFunc()
 { 
 }
 
-btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_simplexSolver(simplexSolver),
 m_pdSolver(pdSolver),
 m_ownManifold (false),
 m_manifoldPtr(mf),
@@ -312,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();
 
@@ -331,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
 
 
@@ -349,8 +391,8 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 
 	
 	btGjkPairDetector::ClosestPointInput input;
-
-	btGjkPairDetector	gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
+    btVoronoiSimplexSolver simplexSolver;
+    btGjkPairDetector	gjkPairDetector( min0, min1, &simplexSolver, m_pdSolver );
 	//TODO: if (dispatchInfo.m_useContinuous)
 	gjkPairDetector.setMinkowskiA(min0);
 	gjkPairDetector.setMinkowskiB(min1);
@@ -367,7 +409,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 		//	input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
 		//} else
 		//{
-		input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+		input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold()+resultOut->m_closestPointDistanceThreshold;
 //		}
 
 		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
@@ -503,9 +545,11 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 				
 //				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
 
+				worldVertsB1.resize(0);
 				btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
 					body0Wrap->getWorldTransform(), 
-					body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut);
+																 body1Wrap->getWorldTransform(), minDist-threshold, threshold, worldVertsB1,worldVertsB2,
+																 *resultOut);
  				
 			}
 			if (m_ownManifold)
@@ -568,8 +612,9 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 				
 			if (foundSepAxis)
 			{
+				worldVertsB2.resize(0);
 				btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
-					body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
+					body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
 			}
 				
 				

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

@@ -24,6 +24,7 @@ subject to the following restrictions:
 #include "btCollisionCreateFunc.h"
 #include "btCollisionDispatcher.h"
 #include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
 
 class btConvexPenetrationDepthSolver;
 
@@ -42,9 +43,10 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
 #ifdef USE_SEPDISTANCE_UTIL2
 	btConvexSeparatingDistanceUtil	m_sepDistance;
 #endif
-	btSimplexSolverInterface*		m_simplexSolver;
 	btConvexPenetrationDepthSolver* m_pdSolver;
 
+	btVertexArray worldVertsB1;
+	btVertexArray worldVertsB2;
 	
 	bool	m_ownManifold;
 	btPersistentManifold*	m_manifoldPtr;
@@ -59,7 +61,7 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
 
 public:
 
-	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
 
 	virtual ~btConvexConvexAlgorithm();
 
@@ -87,18 +89,17 @@ public:
 	{
 
 		btConvexPenetrationDepthSolver*		m_pdSolver;
-		btSimplexSolverInterface*			m_simplexSolver;
 		int m_numPerturbationIterations;
 		int m_minimumPointsPerturbationThreshold;
 
-		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+		CreateFunc(btConvexPenetrationDepthSolver* pdSolver);
 		
 		virtual ~CreateFunc();
 
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
-			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
 		}
 	};
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

@@ -44,9 +44,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
 //btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc*	stackAlloc,btPoolAllocator*	persistentManifoldPool,btPoolAllocator*	collisionAlgorithmPool)
 {
 
-	void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16);
-	m_simplexSolver = new (mem)btVoronoiSimplexSolver();
-
+    void* mem = NULL;
 	if (constructionInfo.m_useEpaPenetrationAlgorithm)
 	{
 		mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
@@ -59,7 +57,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
 	
 	//default CreationFunctions, filling the m_doubleDispatch table
 	mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16);
-	m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver);
+	m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_pdSolver);
 	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
 	m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc;
 	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
@@ -123,6 +121,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
 		m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize);
 	}
 	
+	collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize+16)&0xffffffffffff0;
 	if (constructionInfo.m_collisionAlgorithmPool)
 	{
 		m_ownsCollisionAlgorithmPool = false;
@@ -192,9 +191,6 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 	m_planeConvexCF->~btCollisionAlgorithmCreateFunc();
 	btAlignedFree( m_planeConvexCF);
 
-	m_simplexSolver->~btVoronoiSimplexSolver();
-	btAlignedFree(m_simplexSolver);
-
 	m_pdSolver->~btConvexPenetrationDepthSolver();
 	
 	btAlignedFree(m_pdSolver);
@@ -202,6 +198,86 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 
 }
 
+btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1)
+{
+
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereSphereCF;
+	}
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereBoxCF;
+	}
+
+	if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_boxSphereCF;
+	}
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereTriangleCF;
+	}
+
+	if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_triangleSphereCF;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_convexPlaneCF;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_planeConvexCF;
+	}
+
+
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
+	{
+		return m_convexConvexCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
+	{
+		return m_convexConcaveCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
+	{
+		return m_swappedConvexConcaveCreateFunc;
+	}
+
+
+	if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1))
+	{
+		return m_compoundCompoundCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isCompound(proxyType0))
+	{
+		return m_compoundCreateFunc;
+	}
+	else
+	{
+		if (btBroadphaseProxy::isCompound(proxyType1))
+		{
+			return m_swappedCompoundCreateFunc;
+		}
+	}
+
+	//failed to find an algorithm
+	return m_emptyCreateFunc;
+
+}
 
 btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
 {

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

@@ -60,8 +60,7 @@ protected:
 	btPoolAllocator*	m_collisionAlgorithmPool;
 	bool	m_ownsCollisionAlgorithmPool;
 
-	//default simplex/penetration depth solvers
-	btVoronoiSimplexSolver*	m_simplexSolver;
+	//default penetration depth solver
 	btConvexPenetrationDepthSolver*	m_pdSolver;
 	
 	//default CreationFunctions, filling the m_doubleDispatch table
@@ -102,14 +101,10 @@ public:
 	}
 
 
-	virtual	btVoronoiSimplexSolver*	getSimplexSolver()
-	{
-		return m_simplexSolver;
-	}
-
-
 	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
 
+	virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1);
+
 	///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.
 	///By default, this feature is disabled for best performance.
 	///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature.

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp

@@ -28,9 +28,7 @@ int gFindSimplePairs =0;
 
 
 
-btHashedSimplePairCache::btHashedSimplePairCache():
-	m_blockedForChanges(false)
-{
+btHashedSimplePairCache::btHashedSimplePairCache() {
 	int initialAllocatedSize= 2;
 	m_overlappingPairArray.reserve(initialAllocatedSize);
 	growTables();

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h

@@ -55,9 +55,7 @@ extern int gFindSimplePairs;
 class btHashedSimplePairCache
 {
 	btSimplePairArray	m_overlappingPairArray;
-	
-	bool		m_blockedForChanges;
-	
+		
 
 protected:
 	

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp

@@ -193,7 +193,7 @@ struct btConnectivityProcessor : public btTriangleCallback
 				btScalar len2 = calculatedEdge.length2();
 
 				btScalar correctedAngle(0);
-				btVector3 calculatedNormalB = normalA;
+				//btVector3 calculatedNormalB = normalA;
 				bool isConvex = false;
 
 				if (len2<m_triangleInfoMap->m_planarEpsilon)
@@ -213,10 +213,6 @@ struct btConnectivityProcessor : public btTriangleCallback
 					isConvex = (dotA<0.);
 
 					correctedAngle = isConvex ? ang4 : -ang4;
-					btQuaternion orn2(calculatedEdge,-correctedAngle);
-					calculatedNormalB = btMatrix3x3(orn2)*normalA;
-
-
 				}
 
 				

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -24,10 +24,9 @@ ContactAddedCallback		gContactAddedCallback=0;
 
 
 
-///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
-inline btScalar	calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+btScalar	btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
 {
-	btScalar friction = body0->getRollingFriction() * body1->getRollingFriction();
+	btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction();
 
 	const btScalar MAX_FRICTION  = btScalar(10.);
 	if (friction < -MAX_FRICTION)
@@ -38,6 +37,17 @@ inline btScalar	calculateCombinedRollingFriction(const btCollisionObject* body0,
 
 }
 
+btScalar	btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+    btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction();
+    
+    const btScalar MAX_FRICTION  = btScalar(10.);
+    if (friction < -MAX_FRICTION)
+        friction = -MAX_FRICTION;
+    if (friction > MAX_FRICTION)
+        friction = MAX_FRICTION;
+    return friction;
+}
 
 ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
 btScalar	btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
@@ -58,6 +68,22 @@ btScalar	btManifoldResult::calculateCombinedRestitution(const btCollisionObject*
 	return body0->getRestitution() * body1->getRestitution();
 }
 
+btScalar	btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+    return body0->getContactDamping() + body1->getContactDamping();
+}
+
+btScalar	btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+    
+    btScalar s0 = body0->getContactStiffness();
+    btScalar s1 = body1->getContactStiffness();
+    
+    btScalar tmp0 = btScalar(1)/s0;
+    btScalar tmp1 = btScalar(1)/s1;
+    btScalar combinedStiffness = btScalar(1) / (tmp0+tmp1);
+    return combinedStiffness;
+}
 
 
 btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
@@ -70,6 +96,7 @@ btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,con
 	m_index0(-1),
 	m_index1(-1)
 #endif //DEBUG_PART_INDEX
+	, m_closestPointDistanceThreshold(0)
 {
 }
 
@@ -109,6 +136,16 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 	newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
 	newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
 	newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+    newPt.m_combinedSpinningFriction = calculateCombinedSpinningFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+	
+	if (    (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
+            (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
+    {
+        newPt.m_combinedContactDamping1 = calculateCombinedContactDamping(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+        newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+        newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
+    }
+	
 	btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
 	
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.h

@@ -49,17 +49,19 @@ protected:
 	int m_index0;
 	int m_index1;
 	
-
+	
 public:
 
 	btManifoldResult()
-#ifdef DEBUG_PART_INDEX
 		:
+#ifdef DEBUG_PART_INDEX
+		
 	m_partId0(-1),
 	m_partId1(-1),
 	m_index0(-1),
 	m_index1(-1)
 #endif //DEBUG_PART_INDEX
+		m_closestPointDistanceThreshold(0)
 	{
 	}
 
@@ -142,9 +144,15 @@ public:
 		return m_body1Wrap->getCollisionObject();
 	}
 
+	btScalar	m_closestPointDistanceThreshold;
+
 	/// in the future we can let the user override the methods to combine restitution and friction
 	static btScalar	calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
 	static btScalar	calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+	static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+    static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+    static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1);
+	static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1);
 };
 
 #endif //BT_MANIFOLD_RESULT_H

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp

@@ -12,6 +12,7 @@ subject to the following restrictions:
 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.
 */
+#define CLEAR_MANIFOLD 1
 
 #include "btSphereSphereCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
@@ -62,7 +63,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject
 #endif
 
 	///iff distance positive, don't generate a new contact
-	if ( len > (radius0+radius1))
+	if ( len > (radius0+radius1+resultOut->m_closestPointDistanceThreshold))
 	{
 #ifndef CLEAR_MANIFOLD
 		resultOut->refreshContactPoints();

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp

@@ -56,7 +56,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObje
 	
 	/// report a contact. internally this will be kept persistent, and contact reduction is done
 	resultOut->setPersistentManifold(m_manifoldPtr);
-	SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
+	SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold);
 	
 	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
 	input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp

@@ -245,16 +245,18 @@ void	btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
 		btStridingMeshInterface*	m_meshInterface;
 		btTriangleCallback*		m_callback;
 		btVector3				m_triangle[3];
-
+		int m_numOverlap;
 
 		MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
 			:m_meshInterface(meshInterface),
-			m_callback(callback)
+			m_callback(callback),
+			m_numOverlap(0)
 		{
 		}
 				
 		virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
 		{
+			m_numOverlap++;
 			const unsigned char *vertexbase;
 			int numverts;
 			PHY_ScalarType type;
@@ -321,8 +323,7 @@ void	btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
 	MyNodeOverlapCallback	myNodeCallback(callback,m_meshInterface);
 
 	m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
-
-
+	
 #endif//DISABLE_BVH
 
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h

@@ -39,7 +39,11 @@ ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape
 
 	bool m_useQuantizedAabbCompression;
 	bool m_ownsBvh;
+#ifdef __clang__
+	bool m_pad[11] __attribute__((unused));////need padding due to alignment
+#else
 	bool m_pad[11];////need padding due to alignment
+#endif
 
 public:
 

Source/ThirdParty/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);
 }

Source/ThirdParty/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

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btCollisionShape.h

@@ -48,7 +48,7 @@ public:
 
 	virtual void	getBoundingSphere(btVector3& center,btScalar& radius) const;
 
-	///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations.
+	///getAngularMotionDisc returns the maximum radius needed for Conservative Advancement to handle time-of-impact with rotations.
 	virtual btScalar	getAngularMotionDisc() const;
 
 	virtual btScalar	getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConeShape.h

@@ -43,6 +43,15 @@ public:
 	btScalar getRadius() const { return m_radius;}
 	btScalar getHeight() const { return m_height;}
 
+	void setRadius(const btScalar radius)
+	{
+		m_radius = radius;
+	}
+	void setHeight(const btScalar height)
+	{
+		m_height = height;
+	}
+
 
 	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
 	{

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp

@@ -22,6 +22,8 @@ subject to the following restrictions:
 
 #include "LinearMath/btQuaternion.h"
 #include "LinearMath/btSerializer.h"
+#include "btConvexPolyhedron.h"
+#include "LinearMath/btConvexHullComputer.h"
 
 btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape ()
 {
@@ -121,10 +123,17 @@ btVector3	btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const
 }
 
 
-
-
-
-
+void btConvexHullShape::optimizeConvexHull()
+{
+	btConvexHullComputer conv;
+	conv.compute(&m_unscaledPoints[0].getX(), sizeof(btVector3),m_unscaledPoints.size(),0.f,0.f);
+	int numVerts = conv.vertices.size();
+	m_unscaledPoints.resize(0);
+	for (int i=0;i<numVerts;i++)
+    {
+        m_unscaledPoints.push_back(conv.vertices[i]);
+    }
+}
 
 
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h

@@ -55,9 +55,8 @@ public:
 		return getUnscaledPoints();
 	}
 
-	
-
-
+    void optimizeConvexHull();
+    
 	SIMD_FORCE_INLINE	btVector3 getScaledPoint(int i) const
 	{
 		return m_unscaledPoints[i] * m_localScaling;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp

@@ -21,6 +21,7 @@ subject to the following restrictions:
 #include "btConvexPolyhedron.h"
 #include "LinearMath/btHashMap.h"
 
+
 btConvexPolyhedron::btConvexPolyhedron()
 {
 
@@ -33,7 +34,7 @@ btConvexPolyhedron::~btConvexPolyhedron()
 
 inline bool IsAlmostZero(const btVector3& v)
 {
-	if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6)	return false;
+	if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6)	return false;
 	return true;
 }
 

Source/ThirdParty/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]);    

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp

@@ -75,7 +75,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala
 		int inner_count = MIN( numSpheres - k, 128 );
         for( long i = 0; i < inner_count; i++ )
         {
-            temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
+            temp[i] = (*pos)*m_localScaling +vec*m_localScaling*(*rad) - vec * getMargin();
             pos++;
             rad++;
         }
@@ -113,7 +113,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala
             int inner_count = MIN( numSpheres - k, 128 );
             for( long i = 0; i < inner_count; i++ )
             {
-                temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
+                temp[i] = (*pos)*m_localScaling +vec*m_localScaling*(*rad) - vec * getMargin();
                 pos++;
                 rad++;
             }

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h

@@ -25,7 +25,6 @@ subject to the following restrictions:
 ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape
 {
     btAlignedObjectArray <btMaterial*> m_materialList;
-    int ** m_triangleMaterials;
 
 public:
 

Source/ThirdParty/Bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h

@@ -122,7 +122,7 @@ protected:
 		checkManifold(body0Wrap,body1Wrap);
 
 		btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(
-				body0Wrap,body1Wrap,getLastManifold());
+				body0Wrap,body1Wrap,getLastManifold(), BT_CONTACT_POINT_ALGORITHMS);
 		return convex_algorithm ;
 	}
 

Source/ThirdParty/Bullet/src/BulletCollision/Gimpact/btGImpactShape.cpp

@@ -23,6 +23,59 @@ subject to the following restrictions:
 #include "btGImpactMassUtil.h"
 
 
+btGImpactMeshShapePart::btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part )
+{
+    // moved from .h to .cpp because of conditional compilation
+    // (The setting of BT_THREADSAFE may differ between various cpp files, so it is best to
+    // avoid using it in h files)
+    m_primitive_manager.m_meshInterface = meshInterface;
+    m_primitive_manager.m_part = part;
+    m_box_set.setPrimitiveManager( &m_primitive_manager );
+#if BT_THREADSAFE
+    // If threadsafe is requested, this object uses a different lock/unlock
+    //  model with the btStridingMeshInterface -- lock once when the object is constructed
+    //  and unlock once in the destructor.
+    // The other way of locking and unlocking for each collision check in the narrowphase
+    // is not threadsafe.  Note these are not thread-locks, they are calls to the meshInterface's
+    // getLockedReadOnlyVertexIndexBase virtual function, which by default just returns a couple of
+    // pointers.  In theory a client could override the lock function to do all sorts of
+    // things like reading data from GPU memory, or decompressing data on the fly, but such things
+    // do not seem all that likely or useful, given the performance cost.
+    m_primitive_manager.lock();
+#endif
+}
+
+btGImpactMeshShapePart::~btGImpactMeshShapePart()
+{
+    // moved from .h to .cpp because of conditional compilation
+#if BT_THREADSAFE
+    m_primitive_manager.unlock();
+#endif
+}
+
+void btGImpactMeshShapePart::lockChildShapes() const
+{
+    // moved from .h to .cpp because of conditional compilation
+#if ! BT_THREADSAFE
+    // called in the narrowphase -- not threadsafe!
+    void * dummy = (void*) ( m_box_set.getPrimitiveManager() );
+    TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>( dummy );
+    dummymanager->lock();
+#endif
+}
+
+void btGImpactMeshShapePart::unlockChildShapes()  const
+{
+    // moved from .h to .cpp because of conditional compilation
+#if ! BT_THREADSAFE
+    // called in the narrowphase -- not threadsafe!
+    void * dummy = (void*) ( m_box_set.getPrimitiveManager() );
+    TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>( dummy );
+    dummymanager->unlock();
+#endif
+}
+
+
 #define CALC_EXACT_INERTIA 1
 
 

Source/ThirdParty/Bullet/src/BulletCollision/Gimpact/btGImpactShape.h

@@ -722,17 +722,8 @@ public:
 		m_box_set.setPrimitiveManager(&m_primitive_manager);
 	}
 
-
-	btGImpactMeshShapePart(btStridingMeshInterface * meshInterface,	int part)
-	{
-		m_primitive_manager.m_meshInterface = meshInterface;
-		m_primitive_manager.m_part = part;
-		m_box_set.setPrimitiveManager(&m_primitive_manager);
-	}
-
-	virtual ~btGImpactMeshShapePart()
-	{
-	}
+    btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part );
+    virtual ~btGImpactMeshShapePart();
 
 	//! if true, then its children must get transforms.
 	virtual bool childrenHasTransform() const
@@ -742,19 +733,8 @@ public:
 
 
 	//! call when reading child shapes
-	virtual void lockChildShapes() const
-	{
-		void * dummy = (void*)(m_box_set.getPrimitiveManager());
-		TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy);
-		dummymanager->lock();
-	}
-
-	virtual void unlockChildShapes()  const
-	{
-		void * dummy = (void*)(m_box_set.getPrimitiveManager());
-		TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy);
-		dummymanager->unlock();
-	}
+    virtual void lockChildShapes() const;
+    virtual void unlockChildShapes()  const;
 
 	//! Gets the number of children
 	virtual int	getNumChildShapes() const

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp

@@ -41,21 +41,38 @@ namespace gjkepa2_impl
 
 	/* GJK	*/ 
 #define GJK_MAX_ITERATIONS	128
-#define GJK_ACCURARY		((btScalar)0.0001)
-#define GJK_MIN_DISTANCE	((btScalar)0.0001)
-#define GJK_DUPLICATED_EPS	((btScalar)0.0001)
+
+#ifdef BT_USE_DOUBLE_PRECISION
+	#define GJK_ACCURACY		((btScalar)1e-12)
+	#define GJK_MIN_DISTANCE	((btScalar)1e-12)
+	#define GJK_DUPLICATED_EPS	((btScalar)1e-12)
+#else
+	#define GJK_ACCURACY		((btScalar)0.0001)
+	#define GJK_MIN_DISTANCE	((btScalar)0.0001)
+	#define GJK_DUPLICATED_EPS	((btScalar)0.0001)
+#endif //BT_USE_DOUBLE_PRECISION
+
+
 #define GJK_SIMPLEX2_EPS	((btScalar)0.0)
 #define GJK_SIMPLEX3_EPS	((btScalar)0.0)
 #define GJK_SIMPLEX4_EPS	((btScalar)0.0)
 
 	/* EPA	*/ 
-#define EPA_MAX_VERTICES	64
-#define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
+#define EPA_MAX_VERTICES	128
 #define EPA_MAX_ITERATIONS	255
-#define EPA_ACCURACY		((btScalar)0.0001)
-#define EPA_FALLBACK		(10*EPA_ACCURACY)
-#define EPA_PLANE_EPS		((btScalar)0.00001)
-#define EPA_INSIDE_EPS		((btScalar)0.01)
+
+#ifdef BT_USE_DOUBLE_PRECISION
+	#define EPA_ACCURACY		((btScalar)1e-12)
+	#define EPA_PLANE_EPS		((btScalar)1e-14)
+	#define EPA_INSIDE_EPS		((btScalar)1e-9)
+#else
+	#define EPA_ACCURACY		((btScalar)0.0001)
+	#define EPA_PLANE_EPS		((btScalar)0.00001)
+	#define EPA_INSIDE_EPS		((btScalar)0.01)
+#endif
+
+#define EPA_FALLBACK            (10*EPA_ACCURACY)
+#define EPA_MAX_FACES           (EPA_MAX_VERTICES*2)
 
 
 	// Shorthands
@@ -242,7 +259,7 @@ namespace gjkepa2_impl
 					/* Check for termination				*/ 
 					const btScalar	omega=btDot(m_ray,w)/rl;
 					alpha=btMax(omega,alpha);
-					if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+					if(((rl-alpha)-(GJK_ACCURACY*rl))<=0)
 					{/* Return old simplex				*/ 
 						removevertice(m_simplices[m_current]);
 						break;
@@ -1015,7 +1032,7 @@ bool	btGjkEpaSolver2::SignedDistance(const btConvexShape*	shape0,
 /* Symbols cleanup		*/ 
 
 #undef GJK_MAX_ITERATIONS
-#undef GJK_ACCURARY
+#undef GJK_ACCURACY
 #undef GJK_MIN_DISTANCE
 #undef GJK_DUPLICATED_EPS
 #undef GJK_SIMPLEX2_EPS

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -30,7 +30,13 @@ subject to the following restrictions:
 #endif
 
 //must be above the machine epsilon
-#define REL_ERROR2 btScalar(1.0e-6)
+#ifdef  BT_USE_DOUBLE_PRECISION
+	#define REL_ERROR2 btScalar(1.0e-12)
+	btScalar gGjkEpaPenetrationTolerance = 1e-7;
+#else
+	#define REL_ERROR2 btScalar(1.0e-6)
+	btScalar gGjkEpaPenetrationTolerance = 0.001;
+#endif
 
 //temp globals, to improve GJK/EPA/penetration calculations
 int gNumDeepPenetrationChecks = 0;
@@ -275,7 +281,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 			btScalar lenSqr =m_cachedSeparatingAxis.length2();
 			
 			//valid normal
-			if (lenSqr < 0.0001)
+			if (lenSqr < REL_ERROR2)
 			{
 				m_degenerateSimplex = 5;
 			} 
@@ -300,7 +306,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 		}
 
 		bool catchDegeneratePenetrationCase = 
-			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
+			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
 
 		//if (checkPenetration && !isValid)
 		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
@@ -339,6 +345,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					{
 						tmpNormalInB /= btSqrt(lenSqr);
 						btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
+						m_lastUsedMethod = 3;
 						//only replace valid penetrations when the result is deeper (check)
 						if (!isValid || (distance2 < distance))
 						{
@@ -346,9 +353,9 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 							pointOnA = tmpPointOnA;
 							pointOnB = tmpPointOnB;
 							normalInB = tmpNormalInB;
-
+							
 							isValid = true;
-							m_lastUsedMethod = 3;
+							
 						} else
 						{
 							m_lastUsedMethod = 8;
@@ -403,6 +410,47 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 		m_cachedSeparatingAxis = normalInB;
 		m_cachedSeparatingDistance = distance;
 
+		{
+		///todo: need to track down this EPA penetration solver degeneracy
+		///the penetration solver reports penetration but the contact normal
+		///connecting the contact points is pointing in the opposite direction
+		///until then, detect the issue and revert the normal
+
+			btScalar d1=0;
+			{
+				btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
+				btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
+			
+
+				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+				btVector3  pWorld = localTransA(pInA);	
+				btVector3  qWorld = localTransB(qInB);
+				btVector3 w	= pWorld - qWorld;
+				d1 = (-normalInB).dot(w);
+			}
+			btScalar d0 = 0.f;
+			{
+				btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
+				btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis();
+			
+
+				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+				btVector3  pWorld = localTransA(pInA);	
+				btVector3  qWorld = localTransB(qInB);
+				btVector3 w	= pWorld - qWorld;
+				d0 = normalInB.dot(w);
+			}
+			if (d1>d0)
+			{
+				m_lastUsedMethod = 10;
+				normalInB*=-1;
+			} 
+
+		}
 		output.addContactPoint(
 			normalInB,
 			pointOnB+positionOffset,

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -35,7 +35,13 @@ typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow;
 	typedef btConstraintRow PfxConstraintRow;
 #endif //PFX_USE_FREE_VECTORMATH
 
-
+enum btContactPointFlags
+{
+	BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1,
+	BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
+	BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
+    BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
+};
 
 /// ManifoldContactPoint collects and maintains persistent contactpoints.
 /// used to improve stability and performance of rigidbody dynamics response.
@@ -44,14 +50,15 @@ class btManifoldPoint
 		public:
 			btManifoldPoint()
 				:m_userPersistentData(0),
-				m_lateralFrictionInitialized(false),
-                m_appliedImpulse(0.f),
+				m_contactPointFlags(0),
+				m_appliedImpulse(0.f),
                 m_appliedImpulseLateral1(0.f),
 				m_appliedImpulseLateral2(0.f),
 				m_contactMotion1(0.f),
 				m_contactMotion2(0.f),
-				m_contactCFM1(0.f),
-				m_contactCFM2(0.f),
+				m_contactCFM(0.f),
+				m_contactERP(0.f),
+				m_frictionCFM(0.f),
 				m_lifeTime(0)
 			{
 			}
@@ -65,16 +72,18 @@ class btManifoldPoint
 					m_distance1( distance ),
 					m_combinedFriction(btScalar(0.)),
 					m_combinedRollingFriction(btScalar(0.)),
-					m_combinedRestitution(btScalar(0.)),
+                    m_combinedSpinningFriction(btScalar(0.)),
+                    m_combinedRestitution(btScalar(0.)),
 					m_userPersistentData(0),
-					m_lateralFrictionInitialized(false),
-                    m_appliedImpulse(0.f),
+					m_contactPointFlags(0),
+					m_appliedImpulse(0.f),
                     m_appliedImpulseLateral1(0.f),
 					m_appliedImpulseLateral2(0.f),
 					m_contactMotion1(0.f),
 					m_contactMotion2(0.f),
-					m_contactCFM1(0.f),
-					m_contactCFM2(0.f),
+					m_contactCFM(0.f),
+					m_contactERP(0.f),
+					m_frictionCFM(0.f),
 					m_lifeTime(0)
 			{
 				
@@ -91,8 +100,9 @@ class btManifoldPoint
 		
 			btScalar	m_distance1;
 			btScalar	m_combinedFriction;
-			btScalar	m_combinedRollingFriction;
-			btScalar	m_combinedRestitution;
+			btScalar	m_combinedRollingFriction;//torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever
+            btScalar	m_combinedSpinningFriction;//torsional friction around contact normal, useful for grasping objects
+            btScalar	m_combinedRestitution;
 
 			//BP mod, store contact triangles.
 			int			m_partId0;
@@ -101,15 +111,28 @@ class btManifoldPoint
 			int			m_index1;
 				
 			mutable void*	m_userPersistentData;
-			bool			m_lateralFrictionInitialized;
-
+			//bool			m_lateralFrictionInitialized;
+			int				m_contactPointFlags;
+			
 			btScalar		m_appliedImpulse;
 			btScalar		m_appliedImpulseLateral1;
 			btScalar		m_appliedImpulseLateral2;
 			btScalar		m_contactMotion1;
 			btScalar		m_contactMotion2;
-			btScalar		m_contactCFM1;
-			btScalar		m_contactCFM2;
+			
+			union
+			{
+                btScalar		m_contactCFM;
+                btScalar        m_combinedContactStiffness1;
+			};
+			
+			union
+			{
+                btScalar		m_contactERP;
+                btScalar        m_combinedContactDamping1;
+			};
+
+			btScalar		m_frictionCFM;
 
 			int				m_lifeTime;//lifetime of the contactpoint in frames
 			

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -51,8 +51,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];
@@ -163,7 +163,7 @@ public:
 			//get rid of duplicated userPersistentData pointer
 			m_pointCache[lastUsedIndex].m_userPersistentData = 0;
 			m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
-			m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
+			m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
 			m_pointCache[lastUsedIndex].m_lifeTime = 0;
@@ -190,16 +190,11 @@ public:
 		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_appliedImpulse =  appliedImpulse;
-		m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
-		m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
-
 
 		m_pointCache[insertIndex].m_lifeTime = lifeTime;
 #else

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp

@@ -116,7 +116,7 @@ static int gActualSATPairTests=0;
 
 inline bool IsAlmostZero(const btVector3& v)
 {
-	if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6)	return false;
+	if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6)	return false;
 	return true;
 }
 
@@ -411,9 +411,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 	return true;
 }
 
-void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
+void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
-	btVertexArray worldVertsB2;
+	worldVertsB2.resize(0);
 	btVertexArray* pVtxIn = &worldVertsB1;
 	btVertexArray* pVtxOut = &worldVertsB2;
 	pVtxOut->reserve(pVtxIn->size());
@@ -527,7 +527,7 @@ void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
 
 
 
-void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
+void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
 
 	btVector3 separatingNormal = separatingNormal1.normalized();
@@ -552,7 +552,7 @@ void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
 			}
 		}
 	}
-				btVertexArray worldVertsB1;
+	worldVertsB1.resize(0);
 				{
 					const btFace& polyB = hullB.m_faces[closestFaceB];
 					const int numVertices = polyB.m_indices.size();
@@ -565,6 +565,6 @@ void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
 
 	
 	if (closestFaceB>=0)
-		clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
+		clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, worldVertsB2,minDist, maxDist,resultOut);
 
 }

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h

@@ -32,8 +32,11 @@ typedef btAlignedObjectArray<btVector3> btVertexArray;
 // Clips a face to the back of a plane
 struct btPolyhedralContactClipping
 {
-	static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut);
-	static void	clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);
+
+	static void	clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut);
+
+	static void	clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);
+
 
 	static bool findSeparatingAxis(	const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut);
 

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp

@@ -294,7 +294,10 @@ bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
 #else
 		if (m_simplexVectorW[i] == w)
 #endif
+		{
 			found = true;
+			break;
+		}
 	}
 
 	//check in case lastW is already removed

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h

@@ -26,8 +26,12 @@ subject to the following restrictions:
 
 ///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure
 #define BT_USE_EQUAL_VERTEX_THRESHOLD
-#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 1e-12f
+#else
+#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f
+#endif//BT_USE_DOUBLE_PRECISION
 
 struct btUsageBitfield{
 	btUsageBitfield()

Source/ThirdParty/Bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h

@@ -37,7 +37,7 @@ public:
 	virtual void	preStep ( btCollisionWorld* collisionWorld) = 0;
 	virtual void	playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
 	virtual bool	canJump () const = 0;
-	virtual void	jump () = 0;
+	virtual void	jump(const btVector3& dir = btVector3()) = 0;
 
 	virtual bool	onGround () const = 0;
 	virtual void	setUpInterpolate (bool value) = 0;

Source/ThirdParty/Bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp

@@ -132,30 +132,37 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector
 	return direction - parallelComponent(direction, normal);
 }
 
-btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
+btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
 {
-	m_upAxis = upAxis;
-	m_addedMargin = 0.02;
-	m_walkDirection.setValue(0,0,0);
-	m_useGhostObjectSweepTest = true;
 	m_ghostObject = ghostObject;
-	m_stepHeight = stepHeight;
+	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);
+	m_useGhostObjectSweepTest = true;	
 	m_turnAngle = btScalar(0.0);
 	m_convexShape=convexShape;	
 	m_useWalkDirection = true;	// use walk direction by default, legacy behavior
 	m_velocityTimeInterval = 0.0;
 	m_verticalVelocity = 0.0;
 	m_verticalOffset = 0.0;
-	m_gravity = 9.8 * 3 ; // 3G acceleration.
+	m_gravity = 9.8 * 3.0 ; // 3G acceleration.
 	m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
 	m_jumpSpeed = 10.0; // ?
+	m_SetjumpSpeed = m_jumpSpeed;
 	m_wasOnGround = false;
 	m_wasJumping = false;
 	m_interpolateUp = true;
 	setMaxSlope(btRadians(45.0));
-	m_currentStepOffset = 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);
 }
 
 btKinematicCharacterController::~btKinematicCharacterController ()
@@ -190,7 +197,7 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 
 	m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
 	
-	btScalar maxPen = btScalar(0.0);
+//	btScalar maxPen = btScalar(0.0);
 	for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
 	{
 		m_manifoldArray.resize(0);
@@ -198,10 +205,13 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 		btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
 
 		btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
-                btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
+        btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
 
 		if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
 			continue;
+
+		if (!needsCollision(obj0, obj1))
+			continue;
 		
 		if (collisionPair->m_algorithm)
 			collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
@@ -217,14 +227,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 
 				btScalar dist = pt.getDistance();
 
-				if (dist < 0.0)
+				if (dist < -m_maxPenetrationDepth)
 				{
-					if (dist < maxPen)
-					{
-						maxPen = dist;
-						m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+					// TODO: cause problems on slopes, not sure if it is needed
+					//if (dist < maxPen)
+					//{
+					//	maxPen = dist;
+					//	m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
 
-					}
+					//}
 					m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
 					penetration = true;
 				} else {
@@ -244,18 +255,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 
 void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
 {
+	btScalar stepHeight = 0.0f;
+	if (m_verticalVelocity < 0.0)
+		stepHeight = m_stepHeight;
+
 	// phase 1: up
 	btTransform start, end;
-	m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
 
 	start.setIdentity ();
 	end.setIdentity ();
 
 	/* FIXME: Handle penetration properly */
-	start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
+	start.setOrigin(m_currentPosition);
+
+	m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
+	m_currentPosition = m_targetPosition;
+
 	end.setOrigin (m_targetPosition);
 
-	btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
+	start.setRotation(m_currentOrientation);
+	end.setRotation(m_targetOrientation);
+
+	btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
 	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
 	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 	
@@ -265,29 +286,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
 	}
 	else
 	{
-		world->convexSweepTest (m_convexShape, start, end, callback);
+		world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
 	}
-	
-	if (callback.hasHit())
+
+	if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
 	{
 		// Only modify the position if the hit was a slope and not a wall or ceiling.
-		if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
+		if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
 		{
 			// we moved up only a fraction of the step height
-			m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+			m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
 			if (m_interpolateUp == true)
 				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 			else
 				m_currentPosition = m_targetPosition;
 		}
-		m_verticalVelocity = 0.0;
-		m_verticalOffset = 0.0;
+
+		btTransform& xform = m_ghostObject->getWorldTransform();
+		xform.setOrigin(m_currentPosition);
+		m_ghostObject->setWorldTransform(xform);
+
+		// fix penetration if we hit a ceiling for example
+		int numPenetrationLoops = 0;
+		m_touchingContact = false;
+		while (recoverFromPenetration(world))
+		{
+			numPenetrationLoops++;
+			m_touchingContact = true;
+			if (numPenetrationLoops > 4)
+			{
+				//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+				break;
+			}
+		}
+		m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
+		m_currentPosition = m_targetPosition;
+
+		if (m_verticalOffset > 0)
+		{
+			m_verticalOffset = 0.0;
+			m_verticalVelocity = 0.0;
+			m_currentStepOffset = m_stepHeight;
+		}
 	} else {
-		m_currentStepOffset = m_stepHeight;
+		m_currentStepOffset = stepHeight;
 		m_currentPosition = m_targetPosition;
 	}
 }
 
+bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
+{
+	bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
+	collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
+	return collides;
+}
+
 void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
 {
 	btVector3 movementDirection = m_targetPosition - m_currentPosition;
@@ -330,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 	// 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
 	// phase 2: forward and strafe
 	btTransform start, end;
+
 	m_targetPosition = m_currentPosition + walkMove;
 
 	start.setIdentity ();
@@ -339,15 +393,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 	btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
 //	printf("distance2=%f\n",distance2);
 
-	if (m_touchingContact)
-	{
-		if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
-		{
-			//interferes with step movement
-			//updateTargetPositionBasedOnCollision (m_touchingNormal);
-		}
-	}
-
 	int maxIter = 10;
 
 	while (fraction > btScalar(0.01) && maxIter-- > 0)
@@ -356,6 +401,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 		end.setOrigin (m_targetPosition);
 		btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
 
+		start.setRotation(m_currentOrientation);
+		end.setRotation(m_targetOrientation);
+
 		btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
 		callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
 		callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -364,21 +412,23 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 		btScalar margin = m_convexShape->getMargin();
 		m_convexShape->setMargin(margin + m_addedMargin);
 
-
-		if (m_useGhostObjectSweepTest)
-		{
-			m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-		} else
+		if (!(start == end))
 		{
-			collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			if (m_useGhostObjectSweepTest)
+			{
+				m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			}
+			else
+			{
+				collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			}
 		}
-		
 		m_convexShape->setMargin(margin);
 
 		
 		fraction -= callback.m_closestHitFraction;
 
-		if (callback.hasHit())
+		if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
 		{	
 			// we moved only a fraction
 			//btScalar hitDistance;
@@ -403,14 +453,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 				break;
 			}
 
-		} else {
-			// we moved whole way
-			m_currentPosition = m_targetPosition;
 		}
-
-	//	if (callback.m_closestHitFraction == 0.f)
-	//		break;
-
+        else
+        {
+            m_currentPosition = m_targetPosition;
+		}
 	}
 }
 
@@ -421,27 +468,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 
 	// phase 3: down
 	/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
-	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
+	btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
 	btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
-	btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
+	btVector3 gravity_drop = m_up * downVelocity; 
 	m_targetPosition -= (step_drop + gravity_drop);*/
 
 	btVector3 orig_position = m_targetPosition;
 	
 	btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
 
+	if (m_verticalVelocity > 0.0)
+		return;
+
 	if(downVelocity > 0.0 && downVelocity > m_fallSpeed
 		&& (m_wasOnGround || !m_wasJumping))
 		downVelocity = m_fallSpeed;
 
-	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+	btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
 	m_targetPosition -= step_drop;
 
-	btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+	btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-        btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+	btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
         callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
         callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
@@ -455,6 +505,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		start.setOrigin (m_currentPosition);
 		end.setOrigin (m_targetPosition);
 
+		start.setRotation(m_currentOrientation);
+		end.setRotation(m_targetOrientation);
+
 		//set double test for 2x the step drop, to check for a large drop vs small drop
 		end_double.setOrigin (m_targetPosition - step_drop);
 
@@ -462,7 +515,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		{
 			m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-			if (!callback.hasHit())
+			if (!callback.hasHit() && m_ghostObject->hasContactResponse())
 			{
 				//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
 				m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
@@ -471,30 +524,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		{
 			collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-			if (!callback.hasHit())
-					{
-							//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
-							collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-					}
+			if (!callback.hasHit() && m_ghostObject->hasContactResponse())
+			{
+				//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
+				collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			}
 		}
 	
 		btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-		bool has_hit = false;
+		bool has_hit;
 		if (bounce_fix == true)
-			has_hit = callback.hasHit() || callback2.hasHit();
+			has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
 		else
-			has_hit = callback2.hasHit();
+			has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject);
+
+		btScalar stepHeight = 0.0f;
+		if (m_verticalVelocity < 0.0)
+			stepHeight = m_stepHeight;
 
-		if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
+		if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false
 					&& (m_wasOnGround || !m_wasJumping))
 		{
 			//redo the velocity calculation when falling a small amount, for fast stairs motion
 			//for larger falls, use the smoother/slower interpolated movement by not touching the target position
 
 			m_targetPosition = orig_position;
-					downVelocity = m_stepHeight;
+			downVelocity = stepHeight;
 
-				btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+			step_drop = m_up * (m_currentStepOffset + downVelocity);
 			m_targetPosition -= step_drop;
 			runonce = true;
 			continue; //re-run previous tests
@@ -502,10 +559,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		break;
 	}
 
-	if (callback.hasHit() || 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;
 
 		//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
@@ -513,10 +569,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		if (bounce_fix == true)
 		{
 			if (full_drop == true)
-                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-                        else
-                                //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
-                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
+				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+            else
+				//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
+				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
 		}
 		else
 			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
@@ -528,7 +584,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		m_wasJumping = false;
 	} else {
 		// we dropped the full height
-		
+
 		full_drop = true;
 
 		if (bounce_fix == true)
@@ -538,7 +594,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 			{
 				m_targetPosition += step_drop; //undo previous target change
 				downVelocity = m_fallSpeed;
-				step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+				step_drop = m_up * (m_currentStepOffset + downVelocity);
 				m_targetPosition -= step_drop;
 			}
 		}
@@ -579,21 +635,63 @@ btScalar timeInterval
 	m_velocityTimeInterval += timeInterval;
 }
 
-void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
+void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
 {
-        m_verticalVelocity = 0.0;
-        m_verticalOffset = 0.0;
-        m_wasOnGround = false;
-        m_wasJumping = false;
-        m_walkDirection.setValue(0,0,0);
-        m_velocityTimeInterval = 0.0;
+	m_AngVel = velocity;
+}
 
-        //clear pair cache
-        btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
-        while (cache->getOverlappingPairArray().size() > 0)
-        {
-                cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
-        }
+const btVector3& btKinematicCharacterController::getAngularVelocity() const
+{
+	return m_AngVel;
+}
+
+void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
+{
+	m_walkDirection = velocity;
+
+	// HACK: if we are moving in the direction of the up, treat it as a jump :(
+	if (m_walkDirection.length2() > 0)
+	{
+		btVector3 w = velocity.normalized();
+		btScalar c = w.dot(m_up);
+		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());
+			m_walkDirection -= upComponent;
+			m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
+			
+			if (c > 0.0f)
+			{
+				m_wasJumping = true;
+				m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+			}
+		}
+	}
+	else
+		m_verticalVelocity = 0.0f;
+}
+
+btVector3 btKinematicCharacterController::getLinearVelocity() const
+{
+	return m_walkDirection + (m_verticalVelocity * m_up);
+}
+
+void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
+{
+    m_verticalVelocity = 0.0;
+    m_verticalOffset = 0.0;
+    m_wasOnGround = false;
+    m_wasJumping = false;
+    m_walkDirection.setValue(0,0,0);
+    m_velocityTimeInterval = 0.0;
+
+    //clear pair cache
+    btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
+    while (cache->getOverlappingPairArray().size() > 0)
+    {
+            cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
+    }
 }
 
 void btKinematicCharacterController::warp (const btVector3& origin)
@@ -607,62 +705,99 @@ void btKinematicCharacterController::warp (const btVector3& origin)
 
 void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
 {
-	
-	int numPenetrationLoops = 0;
-	m_touchingContact = false;
-	while (recoverFromPenetration (collisionWorld))
-	{
-		numPenetrationLoops++;
-		m_touchingContact = true;
-		if (numPenetrationLoops > 4)
-		{
-			//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
-			break;
-		}
-	}
-
 	m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
 	m_targetPosition = m_currentPosition;
-//	printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
 
-	
+	m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+	m_targetOrientation = m_currentOrientation;
+//	printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
 }
 
-#include <stdio.h>
-
 void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
 {
 //	printf("playerStep(): ");
 //	printf("  dt = %f", dt);
 
+	if (m_AngVel.length2() > 0.0f)
+	{
+		m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
+	}
+
+	// integrate for angular velocity
+	if (m_AngVel.length2() > 0.0f)
+	{
+		btTransform xform;
+		xform = m_ghostObject->getWorldTransform();
+
+		btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
+
+		btQuaternion orn = rot * xform.getRotation();
+
+		xform.setRotation(orn);
+		m_ghostObject->setWorldTransform(xform);
+
+		m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+		m_targetPosition = m_currentPosition;
+		m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+		m_targetOrientation = m_currentOrientation;
+	}
+
 	// quick check...
-	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
+	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
 //		printf("\n");
 		return;		// no motion
 	}
 
 	m_wasOnGround = onGround();
 
+	//btVector3 lvel = m_walkDirection;
+	//btScalar c = 0.0f;
+	
+	if (m_walkDirection.length2() > 0)
+	{
+		// apply damping
+		m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
+	}
+
+	m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
+	
 	// Update fall velocity.
 	m_verticalVelocity -= m_gravity * dt;
-	if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+	if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
 	{
 		m_verticalVelocity = m_jumpSpeed;
 	}
-	if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+	if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
 	{
 		m_verticalVelocity = -btFabs(m_fallSpeed);
 	}
 	m_verticalOffset = m_verticalVelocity * dt;
 
-
 	btTransform xform;
-	xform = m_ghostObject->getWorldTransform ();
+	xform = m_ghostObject->getWorldTransform();
 
 //	printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
 //	printf("walkSpeed=%f\n",walkSpeed);
 
-	stepUp (collisionWorld);
+	stepUp(collisionWorld);
+	//todo: Experimenting with behavior of controller when it hits a ceiling..
+	//bool hitUp = stepUp (collisionWorld);	
+	//if (hitUp)
+	//{
+	//	m_verticalVelocity -= m_gravity * dt;
+	//	if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+	//	{
+	//		m_verticalVelocity = m_jumpSpeed;
+	//	}
+	//	if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+	//	{
+	//		m_verticalVelocity = -btFabs(m_fallSpeed);
+	//	}
+	//	m_verticalOffset = m_verticalVelocity * dt;
+
+	//	xform = m_ghostObject->getWorldTransform();
+	//}
+
 	if (m_useWalkDirection) {
 		stepForwardAndStrafe (collisionWorld, m_walkDirection);
 	} else {
@@ -682,10 +817,38 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 	}
 	stepDown (collisionWorld, dt);
 
+	//todo: Experimenting with max jump height
+	//if (m_wasJumping)
+	//{
+	//	btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
+	//	if (ds > m_maxJumpHeight)
+	//	{
+	//		// substract the overshoot
+	//		m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
+
+	//		// max height was reached, so potential energy is at max 
+	//		// and kinematic energy is 0, thus velocity is 0.
+	//		if (m_verticalVelocity > 0.0)
+	//			m_verticalVelocity = 0.0;
+	//	}
+	//}
 	// printf("\n");
 
 	xform.setOrigin (m_currentPosition);
 	m_ghostObject->setWorldTransform (xform);
+
+	int numPenetrationLoops = 0;
+	m_touchingContact = false;
+	while (recoverFromPenetration(collisionWorld))
+	{
+		numPenetrationLoops++;
+		m_touchingContact = true;
+		if (numPenetrationLoops > 4)
+		{
+			//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+			break;
+		}
+	}
 }
 
 void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
@@ -696,6 +859,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
 void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
 {
 	m_jumpSpeed = jumpSpeed;
+	m_SetjumpSpeed = m_jumpSpeed;
 }
 
 void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
@@ -708,14 +872,16 @@ bool btKinematicCharacterController::canJump () const
 	return onGround();
 }
 
-void btKinematicCharacterController::jump ()
+void btKinematicCharacterController::jump(const btVector3& v)
 {
-	if (!canJump())
-		return;
-
+	m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
 	m_verticalVelocity = m_jumpSpeed;
 	m_wasJumping = true;
 
+	m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
+
+	m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+
 #if 0
 	currently no jumping.
 	btTransform xform;
@@ -727,14 +893,16 @@ void btKinematicCharacterController::jump ()
 #endif
 }
 
-void btKinematicCharacterController::setGravity(btScalar gravity)
+void btKinematicCharacterController::setGravity(const btVector3& gravity)
 {
-	m_gravity = gravity;
+	if (gravity.length2() > 0) setUpVector(-gravity);
+
+	m_gravity = gravity.length();
 }
 
-btScalar btKinematicCharacterController::getGravity() const
+btVector3 btKinematicCharacterController::getGravity() const
 {
-	return m_gravity;
+	return -m_gravity * m_up;
 }
 
 void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
@@ -748,11 +916,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const
 	return m_maxSlopeRadians;
 }
 
+void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
+{
+	m_maxPenetrationDepth = d;
+}
+
+btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
+{
+	return m_maxPenetrationDepth;
+}
+
 bool btKinematicCharacterController::onGround () const
 {
-	return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
+	return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
 }
 
+void btKinematicCharacterController::setStepHeight(btScalar h) 
+{
+	m_stepHeight = h;
+}
 
 btVector3* btKinematicCharacterController::getUpAxisDirections()
 {
@@ -769,3 +951,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value)
 {
 	m_interpolateUp = value;
 }
+
+void btKinematicCharacterController::setUp(const btVector3& up)
+{
+	if (up.length2() > 0 && m_gravity > 0.0f)
+	{
+		setGravity(-m_gravity * up.normalized());
+		return;
+	}
+
+	setUpVector(up);
+}
+
+void btKinematicCharacterController::setUpVector(const btVector3& up)
+{
+	if (m_up == up)
+		return;
+
+	btVector3 u = m_up;
+
+	if (up.length2() > 0)
+		m_up = up.normalized();
+	else
+		m_up = btVector3(0.0, 0.0, 0.0);
+
+	if (!m_ghostObject) return;
+	btQuaternion rot = getRotation(m_up, u);
+
+	//set orientation with new up
+	btTransform xform;
+	xform = m_ghostObject->getWorldTransform();
+	btQuaternion orn = rot.inverse() * xform.getRotation();
+	xform.setRotation(orn);
+	m_ghostObject->setWorldTransform(xform);
+}
+
+btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
+{
+	if (v0.length2() == 0.0f || v1.length2() == 0.0f)
+	{
+		btQuaternion q;
+		return q;
+	}
+
+	return shortestArcQuatNormalize2(v0, v1);
+}
+

Source/ThirdParty/Bullet/src/BulletDynamics/Character/btKinematicCharacterController.h

@@ -43,10 +43,12 @@ protected:
 	btPairCachingGhostObject* m_ghostObject;
 	btConvexShape*	m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
 	
+	btScalar m_maxPenetrationDepth;
 	btScalar m_verticalVelocity;
 	btScalar m_verticalOffset;
 	btScalar m_fallSpeed;
 	btScalar m_jumpSpeed;
+	btScalar m_SetjumpSpeed;
 	btScalar m_maxJumpHeight;
 	btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
 	btScalar m_maxSlopeCosine;  // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
@@ -61,24 +63,34 @@ protected:
 	///this is the desired walk direction, set by the user
 	btVector3	m_walkDirection;
 	btVector3	m_normalizedDirection;
+	btVector3	m_AngVel;
+
+	btVector3	m_jumpPosition;
 
 	//some internal variables
 	btVector3 m_currentPosition;
 	btScalar  m_currentStepOffset;
 	btVector3 m_targetPosition;
 
+	btQuaternion m_currentOrientation;
+	btQuaternion m_targetOrientation;
+
 	///keep track of the contact manifolds
 	btManifoldArray	m_manifoldArray;
 
 	bool m_touchingContact;
 	btVector3 m_touchingNormal;
 
+	btScalar m_linearDamping;
+	btScalar m_angularDamping;
+
 	bool  m_wasOnGround;
 	bool  m_wasJumping;
 	bool	m_useGhostObjectSweepTest;
 	bool	m_useWalkDirection;
 	btScalar	m_velocityTimeInterval;
-	int m_upAxis;
+	btVector3 m_up;
+	btVector3 m_jumpAxis;
 
 	static btVector3* getUpAxisDirections();
 	bool  m_interpolateUp;
@@ -94,11 +106,18 @@ protected:
 	void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
 	void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
 	void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
+
+	virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
+
+	void setUpVector(const btVector3& up);
+
+	btQuaternion getRotation(btVector3& v0, btVector3& v1) const;
+
 public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
+	btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0));
 	~btKinematicCharacterController ();
 	
 
@@ -112,14 +131,9 @@ public:
 	///btActionInterface interface
 	void	debugDraw(btIDebugDraw* debugDrawer);
 
-	void setUpAxis (int axis)
-	{
-		if (axis < 0)
-			axis = 0;
-		if (axis > 2)
-			axis = 2;
-		m_upAxis = axis;
-	}
+	void setUp(const btVector3& up);
+
+	const btVector3& getUp() { return m_up; }
 
 	/// This should probably be called setPositionIncrementPerSimulatorStep.
 	/// This is neither a direction nor a velocity, but the amount to
@@ -136,27 +150,47 @@ public:
 	virtual void setVelocityForTimeInterval(const btVector3& velocity,
 				btScalar timeInterval);
 
+	virtual void setAngularVelocity(const btVector3& velocity);
+	virtual const btVector3& getAngularVelocity() const;
+
+	virtual void setLinearVelocity(const btVector3& velocity);
+	virtual btVector3 getLinearVelocity() const;
+
+	void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+	btScalar getLinearDamping() const { return  m_linearDamping; }
+	void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+	btScalar getAngularDamping() const { return  m_angularDamping; }
+
 	void reset ( btCollisionWorld* collisionWorld );
 	void warp (const btVector3& origin);
 
 	void preStep (  btCollisionWorld* collisionWorld);
 	void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
 
+	void setStepHeight(btScalar h);
+	btScalar getStepHeight() const { return m_stepHeight; }
 	void setFallSpeed (btScalar fallSpeed);
+	btScalar getFallSpeed() const { return m_fallSpeed; }
 	void setJumpSpeed (btScalar jumpSpeed);
+	btScalar getJumpSpeed() const { return m_jumpSpeed; }
 	void setMaxJumpHeight (btScalar maxJumpHeight);
 	bool canJump () const;
 
-	void jump ();
+	void jump(const btVector3& v = btVector3());
+
+	void applyImpulse(const btVector3& v) { jump(v); }
 
-	void setGravity(btScalar gravity);
-	btScalar getGravity() const;
+	void setGravity(const btVector3& gravity);
+	btVector3 getGravity() const;
 
 	/// The max slope determines the maximum angle that the controller can walk up.
 	/// The slope angle is measured in radians.
 	void setMaxSlope(btScalar slopeRadians);
 	btScalar getMaxSlope() const;
 
+	void setMaxPenetrationDepth(btScalar d);
+	btScalar getMaxPenetrationDepth() const;
+
 	btPairCachingGhostObject* getGhostObject();
 	void	setUseGhostSweepTest(bool useGhostObjectSweepTest)
 	{

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -214,7 +214,7 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
 			}
 			// m_swingCorrection is always positive or 0
 			info->m_lowerLimit[srow] = 0;
-			info->m_upperLimit[srow] = SIMD_INFINITY;
+			info->m_upperLimit[srow] = (m_bMotorEnabled && m_maxMotorImpulse >= 0.0f) ? m_maxMotorImpulse : SIMD_INFINITY;
 			srow += info->rowskip;
 		}
 	}

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -170,6 +170,11 @@ public:
 	{
 		m_angularOnly = angularOnly;
 	}
+	
+	bool    getAngularOnly() const
+	{
+	    return m_angularOnly;
+	}
 
 	void	setLimit(int limitIndex,btScalar limitValue)
 	{
@@ -196,6 +201,33 @@ public:
 		};
 	}
 
+    btScalar getLimit(int limitIndex) const
+	{
+		switch (limitIndex)
+		{
+		case 3:
+			{
+				return m_twistSpan;
+				break;
+			}
+		case 4:
+			{
+				return m_swingSpan2;
+				break;
+			}
+		case 5:
+			{
+				return m_swingSpan1;
+				break;
+			}
+		default:
+			{
+			    btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
+			    return 0.0;
+			}
+		};
+	}
+
 	// setLimit(), a few notes:
 	// _softness:
 	//		0->1, recommend ~0.8->1.
@@ -218,8 +250,8 @@ public:
 		m_relaxationFactor = _relaxationFactor;
 	}
 
-	const btTransform& getAFrame() { return m_rbAFrame; };	
-	const btTransform& getBFrame() { return m_rbBFrame; };
+	const btTransform& getAFrame() const { return m_rbAFrame; };	
+	const btTransform& getBFrame() const { return m_rbBFrame; };
 
 	inline int getSolveTwistLimit()
 	{
@@ -239,27 +271,43 @@ public:
 	void calcAngleInfo();
 	void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
 
-	inline btScalar getSwingSpan1()
+	inline btScalar getSwingSpan1() const
 	{
 		return m_swingSpan1;
 	}
-	inline btScalar getSwingSpan2()
+	inline btScalar getSwingSpan2() const
 	{
 		return m_swingSpan2;
 	}
-	inline btScalar getTwistSpan()
+	inline btScalar getTwistSpan() const
 	{
 		return m_twistSpan;
 	}
-	inline btScalar getTwistAngle()
+	inline btScalar getLimitSoftness() const
+	{
+		return m_limitSoftness;
+	}
+	inline btScalar getBiasFactor() const
+	{
+		return m_biasFactor;
+	}
+	inline btScalar getRelaxationFactor() const
+	{
+		return m_relaxationFactor;
+	}
+	inline btScalar getTwistAngle() const
 	{
 		return m_twistAngle;
 	}
 	bool isPastSwingLimit() { return m_solveSwingLimit; }
 
+	btScalar getDamping() const { return m_damping; }
 	void setDamping(btScalar damping) { m_damping = damping; }
 
 	void enableMotor(bool b) { m_bMotorEnabled = b; }
+	bool isMotorEnabled() const { return m_bMotorEnabled; }
+	btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; }
+	bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; }
 	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
 	void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
 
@@ -271,6 +319,7 @@ public:
 	// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
 	// note: don't forget to enableMotor()
 	void setMotorTarget(const btQuaternion &q);
+	const btQuaternion& getMotorTarget() const { return m_qTarget; }
 
 	// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
 	void setMotorTargetInConstraintSpace(const btQuaternion &q);
@@ -297,6 +346,11 @@ public:
 	///return the local value of parameter
 	virtual	btScalar getParam(int num, int axis = -1) const;
 
+	int getFlags() const
+	{
+		return m_flags;
+	}
+
 	virtual	int	calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -58,7 +58,7 @@ struct btContactSolverInfoData
 	int			m_minimumSolverBatchSize;
 	btScalar	m_maxGyroscopicForce;
 	btScalar	m_singleAxisRollingFrictionThreshold;
-
+	btScalar	m_leastSquaresResidualThreshold;
 
 };
 
@@ -77,7 +77,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_maxErrorReduction = btScalar(20.);
 		m_numIterations = 10;
 		m_erp = btScalar(0.2);
-		m_erp2 = btScalar(0.8);
+		m_erp2 = btScalar(0.2);
 		m_globalCfm = btScalar(0.);
 		m_sor = btScalar(1.);
 		m_splitImpulse = true;
@@ -91,6 +91,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
 		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;
 	}
 };
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

@@ -111,14 +111,14 @@ public:
 
 
 	//! Is limited
-    bool isLimited()
+    bool isLimited() const
     {
     	if(m_loLimit > m_hiLimit) return false;
     	return true;
     }
 
 	//! Need apply correction
-    bool needApplyTorques()
+    bool needApplyTorques() const
     {
     	if(m_currentLimit == 0 && m_enableMotor == false) return false;
     	return true;
@@ -207,11 +207,11 @@ public:
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    inline bool	isLimited(int limitIndex)
+    inline bool	isLimited(int limitIndex) const
     {
        return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
     }
-    inline bool needApplyForce(int limitIndex)
+    inline bool needApplyForce(int limitIndex) const
     {
     	if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
     	return true;
@@ -457,7 +457,7 @@ public:
     	m_linearLimits.m_lowerLimit = linearLower;
     }
 
-	void	getLinearLowerLimit(btVector3& linearLower)
+	void	getLinearLowerLimit(btVector3& linearLower) const
 	{
 		linearLower = m_linearLimits.m_lowerLimit;
 	}
@@ -467,7 +467,7 @@ public:
 		m_linearLimits.m_upperLimit = linearUpper;
 	}
 
-	void	getLinearUpperLimit(btVector3& linearUpper)
+	void	getLinearUpperLimit(btVector3& linearUpper) const
 	{
 		linearUpper = m_linearLimits.m_upperLimit;
 	}
@@ -478,7 +478,7 @@ public:
 			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
     }
 
-	void	getAngularLowerLimit(btVector3& angularLower)
+	void	getAngularLowerLimit(btVector3& angularLower) const
 	{
 		for(int i = 0; i < 3; i++) 
 			angularLower[i] = m_angularLimits[i].m_loLimit;
@@ -490,7 +490,7 @@ public:
 			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
     }
 
-	void	getAngularUpperLimit(btVector3& angularUpper)
+	void	getAngularUpperLimit(btVector3& angularUpper) const
 	{
 		for(int i = 0; i < 3; i++)
 			angularUpper[i] = m_angularLimits[i].m_hiLimit;
@@ -532,7 +532,7 @@ public:
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    bool	isLimited(int limitIndex)
+    bool	isLimited(int limitIndex) const
     {
     	if(limitIndex<3)
     	{
@@ -549,8 +549,11 @@ public:
 								btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
 
 	// access for UseFrameOffset
-	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+	bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; }
 	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+	
+	bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; }
+	void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; }
 
 	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
 	///If no axis is provided, it uses the default axis for this constraint.
@@ -560,6 +563,10 @@ public:
 
 	void setAxis( const btVector3& axis1, const btVector3& axis2);
 
+    	virtual	int getFlags() const
+    	{
+        	return m_flags;
+	}
 
 	virtual	int	calculateSerializeBufferSize() const;
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

@@ -319,14 +319,6 @@ protected:
 		const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
 		btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
 
-	static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
-	static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
-	static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
-	static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
-	static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
-	static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
-	static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
-
 public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -489,6 +481,14 @@ public:
 	//If no axis is provided, it uses the default axis for this constraint.
 	virtual void setParam(int num, btScalar value, int axis = -1);
 	virtual btScalar getParam(int num, int axis = -1) const;
+    
+    static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
+    static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
+    static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
+    static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
+    static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
+    static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
+    static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
 };
 
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h

@@ -63,6 +63,26 @@ public:
 	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
 	void setEquilibriumPoint(int index, btScalar val);
 
+	bool isSpringEnabled(int index) const
+	{
+	    return m_springEnabled[index];
+	}
+
+	btScalar getStiffness(int index) const
+	{
+	    return m_springStiffness[index];
+	}
+
+	btScalar getDamping(int index) const
+	{
+	    return m_springDamping[index];
+	}
+
+	btScalar getEquilibriumPoint(int index) const
+	{
+	    return m_equilibriumPoint[index];
+	}
+
 	virtual void setAxis( const btVector3& axis1, const btVector3& axis2);
 
 	virtual void getInfo2 (btConstraintInfo2* info);

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -177,6 +177,7 @@ public:
 	//       maintain a given angular target.
 	void enableMotor(bool enableMotor) 	{ m_enableAngularMotor = enableMotor; }
 	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
+	void setMotorTargetVelocity(btScalar motorTargetVelocity) { m_motorTargetVelocity = motorTargetVelocity; }
 	void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
 	void setMotorTarget(btScalar targetAngle, btScalar dt);
 
@@ -193,6 +194,33 @@ public:
 		m_relaxationFactor = _relaxationFactor;
 #endif
 	}
+	
+	btScalar getLimitSoftness() const
+	{
+#ifdef	_BT_USE_CENTER_LIMIT_
+		return m_limit.getSoftness();
+#else
+		return m_limitSoftness;
+#endif
+	}
+
+	btScalar getLimitBiasFactor() const
+	{
+#ifdef	_BT_USE_CENTER_LIMIT_
+		return m_limit.getBiasFactor();
+#else
+		return m_biasFactor;
+#endif
+	}
+
+	btScalar getLimitRelaxationFactor() const
+	{
+#ifdef	_BT_USE_CENTER_LIMIT_
+		return m_limit.getRelaxationFactor();
+#else
+		return m_relaxationFactor;
+#endif
+	}
 
 	void	setAxis(btVector3& axisInA)
 	{
@@ -297,13 +325,20 @@ public:
 	// access for UseFrameOffset
 	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
-
+	// access for UseReferenceFrameA
+	bool getUseReferenceFrameA() const { return m_useReferenceFrameA; }
+	void setUseReferenceFrameA(bool useReferenceFrameA) { m_useReferenceFrameA = useReferenceFrameA; }
 
 	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
 	///If no axis is provided, it uses the default axis for this constraint.
 	virtual	void	setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
 	virtual	btScalar getParam(int num, int axis = -1) const;
+	
+	virtual	int getFlags() const
+	{
+  	    return m_flags;
+	}
 
 	virtual	int	calculateSerializeBufferSize() const;
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

@@ -116,6 +116,11 @@ public:
 	virtual	void	setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
 	virtual	btScalar getParam(int num, int axis = -1) const;
+	
+	virtual	int getFlags() const
+	{
+        	return m_flags;
+    	}
 
 	virtual	int	calculateSerializeBufferSize() const;
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -41,7 +41,7 @@ int		gNumSplitImpulseRecoveries = 0;
 
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
-
+//#define VERBOSE_RESIDUAL_PRINTF 1
 ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
 ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
 static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
@@ -300,15 +300,17 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowe
 }
 
 
-void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
+btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
         btSolverBody& body1,
         btSolverBody& body2,
         const btSolverConstraint& c)
 {
+	btScalar deltaImpulse = 0.f;
+
 		if (c.m_rhsPenetration)
         {
 			gNumSplitImpulseRecoveries++;
-			btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
+			deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
 			const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetPushVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
 			const btScalar deltaVel2Dotn	=	c.m_contactNormal2.dot(body2.internalGetPushVelocity())		+ c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
 
@@ -327,13 +329,14 @@ void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
 			body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 			body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
         }
+		return deltaImpulse;
 }
 
- void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	if (!c.m_rhsPenetration)
-		return;
+		return 0.f;
 
 	gNumSplitImpulseRecoveries++;
 
@@ -359,8 +362,9 @@ void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
 	body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
 	body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
 	body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	return deltaImpulse;
 #else
-	resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+	return resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
 #endif
 }
 
@@ -630,8 +634,8 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 }
 
 
-void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
-									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
+void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
+									btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
 									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
 									btScalar desiredVelocity, btScalar cfmSlip)
 
@@ -650,8 +654,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
-	solverConstraint.m_friction = cp.m_combinedRollingFriction;
-	solverConstraint.m_originalContactPoint = 0;
+    solverConstraint.m_friction = combinedTorsionalFriction;
+    solverConstraint.m_originalContactPoint = 0;
 
 	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
@@ -708,11 +712,11 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 
 
 
-btSolverConstraint&	btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(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::addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
+	setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,rel_pos1, rel_pos2,
 							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
@@ -720,8 +724,67 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addRollingFrictionConst
 
 int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
 {
+#if BT_THREADSAFE
+    int solverBodyId = -1;
+    if ( !body.isStaticOrKinematicObject() )
+    {
+        // dynamic body
+        // Dynamic bodies can only be in one island, so it's safe to write to the companionId
+        solverBodyId = body.getCompanionId();
+        if ( solverBodyId < 0 )
+        {
+            if ( btRigidBody* rb = btRigidBody::upcast( &body ) )
+            {
+                solverBodyId = m_tmpSolverBodyPool.size();
+                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+                initSolverBody( &solverBody, &body, timeStep );
+                body.setCompanionId( solverBodyId );
+            }
+        }
+    }
+    else if (body.isKinematicObject())
+    {
+        //
+        // NOTE: must test for kinematic before static because some kinematic objects also
+        //   identify as "static"
+        //
+        // Kinematic bodies can be in multiple islands at once, so it is a
+        // race condition to write to them, so we use an alternate method
+        // to record the solverBodyId
+        int uniqueId = body.getWorldArrayIndex();
+        const int INVALID_SOLVER_BODY_ID = -1;
+        if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
+        {
+            m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
+        }
+        solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ];
+        // if no table entry yet,
+        if ( solverBodyId == INVALID_SOLVER_BODY_ID )
+        {
+            // create a table entry for this body
+            btRigidBody* rb = btRigidBody::upcast( &body );
+            solverBodyId = m_tmpSolverBodyPool.size();
+            btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+            initSolverBody( &solverBody, &body, timeStep );
+            m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId;
+        }
+    }
+    else
+    {
+        // all fixed bodies (inf mass) get mapped to a single solver id
+        if ( m_fixedBodyId < 0 )
+        {
+            m_fixedBodyId = m_tmpSolverBodyPool.size();
+            btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+            initSolverBody( &fixedBody, 0, timeStep );
+        }
+        solverBodyId = m_fixedBodyId;
+    }
+    btAssert( solverBodyId < m_tmpSolverBodyPool.size() );
+	return solverBodyId;
+#else // BT_THREADSAFE
 
-	int solverBodyIdA = -1;
+    int solverBodyIdA = -1;
 
 	if (body.getCompanionId() >= 0)
 	{
@@ -753,6 +816,7 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 	}
 
 	return solverBodyIdA;
+#endif // BT_THREADSAFE
 
 }
 #include <stdio.h>
@@ -779,7 +843,37 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 			//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
 			//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
-			relaxation = 1.f;
+			relaxation = infoGlobal.m_sor;
+			btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
+
+            //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;
+            
 
 			btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
 			solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -806,7 +900,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					}
 #endif //COMPUTE_IMPULSE_DENOM
 
-					btScalar denom = relaxation/(denom0+denom1);
+					btScalar denom = relaxation/(denom0+denom1+cfm);
 					solverConstraint.m_jacDiagABInv = denom;
 				}
 
@@ -888,20 +982,16 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					btScalar	velocityError = restitution - rel_vel;// * damping;
 
 
-					btScalar erp = infoGlobal.m_erp2;
-					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-					{
-						erp = infoGlobal.m_erp;
-					}
-
+				
 					if (penetration>0)
 					{
 						positionalError = 0;
 
-						velocityError -= penetration / infoGlobal.m_timeStep;
+						velocityError -= penetration *invTimeStep;
 					} else
 					{
-						positionalError = -penetration * erp/infoGlobal.m_timeStep;
+						positionalError = -penetration * erp*invTimeStep;
+
 					}
 
 					btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
@@ -919,7 +1009,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 						solverConstraint.m_rhs = velocityImpulse;
 						solverConstraint.m_rhsPenetration = penetrationImpulse;
 					}
-					solverConstraint.m_cfm = 0.f;
+					solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
 					solverConstraint.m_lowerLimit = 0;
 					solverConstraint.m_upperLimit = 1e10f;
 				}
@@ -1014,8 +1104,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;
 
@@ -1027,9 +1115,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 );
 
@@ -1040,45 +1128,31 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 
 
-//			const btVector3& pos1 = cp.getPositionWorldOnA();
-//			const btVector3& pos2 = cp.getPositionWorldOnB();
 
 			/////setup the friction constraints
 
 			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
 
-			btVector3 angVelA(0,0,0),angVelB(0,0,0);
-			if (rb0)
-				angVelA = rb0->getAngularVelocity();
-			if (rb1)
-				angVelB = rb1->getAngularVelocity();
-			btVector3 relAngVel = angVelB-angVelA;
-
 			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
 			{
-				//only a single rollingFriction per manifold
-				rollingFriction--;
-				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
+               
 				{
-					relAngVel.normalize();
-					applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (relAngVel.length()>0.001)
-						addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
-				} else
-				{
-					addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 					btVector3 axis0,axis1;
 					btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
+					axis0.normalize();
+					axis1.normalize();
+					
 					applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					if (axis0.length()>0.001)
-						addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
+                                                       cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 					if (axis1.length()>0.001)
-						addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
+                                                       cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 
 				}
 			}
@@ -1098,7 +1172,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_lateralFrictionInitialized)
+			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
 			{
 				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
@@ -1136,16 +1210,16 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
-						cp.m_lateralFrictionInitialized = true;
+						cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
 					}
 				}
 
 			} else
 			{
-				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
+				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,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_contactCFM2);
+					addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
 
 			}
 			setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
@@ -1253,7 +1327,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	{
 		bodies[i]->setCompanionId(-1);
 	}
-
+#if BT_THREADSAFE
+    m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 );
+#endif // BT_THREADSAFE
 
 	m_tmpSolverBodyPool.reserve(numBodies+1);
 	m_tmpSolverBodyPool.resize(0);
@@ -1532,6 +1608,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
 {
+	btScalar leastSquaresResidual = 0.f;
 
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
@@ -1576,7 +1653,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		{
 			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
 			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+			{
+				btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				leastSquaresResidual += residual*residual;
+			}
 		}
 
 		if (iteration< infoGlobal.m_numIterations)
@@ -1605,7 +1685,9 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
-						resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						leastSquaresResidual += residual*residual;
+
 						totalImpulse = solveManifold.m_appliedImpulse;
 					}
 					bool applyFriction = true;
@@ -1620,7 +1702,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 								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 residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								leastSquaresResidual += residual*residual;
 							}
 						}
 
@@ -1634,7 +1717,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 								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 residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								leastSquaresResidual += residual*residual;
 							}
 						}
 					}
@@ -1650,8 +1734,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 				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 residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					leastSquaresResidual += residual*residual;
 				}
 
 
@@ -1669,7 +1753,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						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 residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						leastSquaresResidual += residual*residual;
 					}
 				}
 
@@ -1689,7 +1774,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
 						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						leastSquaresResidual += residual*residual;
 					}
 				}
 
@@ -1704,7 +1790,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		{
 			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
 			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+			{
+				btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				leastSquaresResidual += residual*residual;
+			}
 		}
 
 		if (iteration< infoGlobal.m_numIterations)
@@ -1725,7 +1814,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 			for (int j=0;j<numPoolConstraints;j++)
 			{
 				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+				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();
@@ -1739,7 +1829,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
 					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
 
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					leastSquaresResidual += residual*residual;
 				}
 			}
 
@@ -1757,12 +1848,13 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
 					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					leastSquaresResidual += residual*residual;
 				}
 			}
 		}
 	}
-	return 0.f;
+	return leastSquaresResidual;
 }
 
 
@@ -1775,6 +1867,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 		{
 			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
 			{
+				btScalar leastSquaresResidual =0.f;
 				{
 					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 					int j;
@@ -1782,15 +1875,24 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar residual = resolveSplitPenetrationSIMD(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;
+				}
 			}
 		}
 		else
 		{
 			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
 			{
+				btScalar leastSquaresResidual = 0.f;
 				{
 					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 					int j;
@@ -1798,7 +1900,15 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						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;
 					}
 				}
 			}
@@ -1819,7 +1929,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 		for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
 		//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
 		{
-			solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+			m_leastSquaresResidual = solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+
+			if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration>= (maxIterations-1)))
+			{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+						printf("residual = %f at iteration #%d\n",m_leastSquaresResidual,iteration);
+#endif
+				break;
+			}
 		}
 
 	}

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -45,22 +45,32 @@ protected:
 	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
 	int							m_maxOverrideNumSolverIterations;
 	int m_fixedBodyId;
+    // When running solvers on multiple threads, a race condition exists for Kinematic objects that
+    // participate in more than one solver.
+    // The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body
+    // for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island
+    // (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once.
+    // To avoid this race condition, this solver does not write the companionId, instead it stores the solver body
+    // index in this solver-local table, indexed by the uniqueId of the body.
+    btAlignedObjectArray<int>	m_kinematicBodyUniqueIdToSolverBodyTable;  // only used for multithreading
 
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
 
+	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, 
 									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 
-	void setupRollingFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
-									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
+	void setupTorsionalFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
+									btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
 									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
 									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&	addRollingFrictionConstraint(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.f);
+	btSolverConstraint&	addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
 
 	
 	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, 
@@ -82,11 +92,11 @@ protected:
 	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
 
 
-	void	resolveSplitPenetrationSIMD(
+	btSimdScalar	resolveSplitPenetrationSIMD(
      btSolverBody& bodyA,btSolverBody& bodyB,
         const btSolverConstraint& contactConstraint);
 
-	void	resolveSplitPenetrationImpulseCacheFriendly(
+	btScalar	resolveSplitPenetrationImpulseCacheFriendly(
        btSolverBody& bodyA,btSolverBody& bodyB,
         const btSolverConstraint& contactConstraint);
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h

@@ -25,6 +25,8 @@ TODO:
 #ifndef BT_SLIDER_CONSTRAINT_H
 #define BT_SLIDER_CONSTRAINT_H
 
+#include "LinearMath/btScalar.h"//for BT_USE_DOUBLE_PRECISION
+
 #ifdef BT_USE_DOUBLE_PRECISION
 #define btSliderConstraintData2		btSliderConstraintDoubleData
 #define btSliderConstraintDataName  "btSliderConstraintDoubleData"
@@ -280,6 +282,11 @@ public:
 	virtual	void	setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
 	virtual	btScalar getParam(int num, int axis = -1) const;
+	
+	virtual	int getFlags() const
+    	{
+		return m_flags;
+	}
 
 	virtual	int	calculateSerializeBufferSize() const;
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -66,6 +66,7 @@ enum btConstraintParams
 
 ATTRIBUTE_ALIGNED16(struct)	btJointFeedback
 {
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 	btVector3	m_appliedForceBodyA;
 	btVector3	m_appliedTorqueBodyA;
 	btVector3	m_appliedForceBodyB;
@@ -144,11 +145,6 @@ public:
 		// lo and hi limits for variables (set to -/+ infinity on entry).
 		btScalar *m_lowerLimit,*m_upperLimit;
 
-		// findex vector for variables. see the LCP solver interface for a
-		// description of what this does. this is set to -1 on entry.
-		// note that the returned indexes are relative to the first index of
-		// the constraint.
-		int *findex;
 		// number of solver iterations
 		int m_numIterations;
 

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -462,6 +462,8 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 		synchronizeMotionStates();
 	}
 
+	clearForces();
+
 #ifndef BT_NO_PROFILE
 	CProfileManager::Increment_Frame_Counter();
 #endif //BT_NO_PROFILE
@@ -537,7 +539,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);
 }
@@ -576,14 +578,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))
 	{
@@ -876,25 +878,12 @@ public:
 int gNumClampedCcdMotions=0;
 
 
-void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
+void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep)
 {
-	BT_PROFILE("createPredictiveContacts");
-
-	{
-		BT_PROFILE("release predictive contact manifolds");
-
-		for (int i=0;i<m_predictiveManifolds.size();i++)
-		{
-			btPersistentManifold* manifold = m_predictiveManifolds[i];
-			this->m_dispatcher1->releaseManifold(manifold);
-		}
-		m_predictiveManifolds.clear();
-	}
-
 	btTransform predictedTrans;
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for ( int i=0;i<numBodies;i++)
 	{
-		btRigidBody* body = m_nonStaticRigidBodies[i];
+		btRigidBody* body = bodies[i];
 		body->setHitFraction(1.f);
 
 		if (body->isActive() && (!body->isStaticOrKinematicObject()))
@@ -951,7 +940,9 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 
 
 						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
+                        btMutexLock( &m_predictiveManifoldsMutex );
 						m_predictiveManifolds.push_back(manifold);
+                        btMutexUnlock( &m_predictiveManifoldsMutex );
 
 						btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
 						btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
@@ -972,13 +963,35 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 		}
 	}
 }
-void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
+
+void btDiscreteDynamicsWorld::releasePredictiveContacts()
+{
+    BT_PROFILE( "release predictive contact manifolds" );
+
+    for ( int i = 0; i < m_predictiveManifolds.size(); i++ )
+    {
+        btPersistentManifold* manifold = m_predictiveManifolds[ i ];
+        this->m_dispatcher1->releaseManifold( manifold );
+    }
+    m_predictiveManifolds.clear();
+}
+
+void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
+{
+	BT_PROFILE("createPredictiveContacts");
+    releasePredictiveContacts();
+    if (m_nonStaticRigidBodies.size() > 0)
+    {
+        createPredictiveContactsInternal( &m_nonStaticRigidBodies[ 0 ], m_nonStaticRigidBodies.size(), timeStep );
+    }
+}
+
+void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep )
 {
-	BT_PROFILE("integrateTransforms");
 	btTransform predictedTrans;
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i=0;i<numBodies;i++)
 	{
-		btRigidBody* body = m_nonStaticRigidBodies[i];
+		btRigidBody* body = bodies[i];
 		body->setHitFraction(1.f);
 
 		if (body->isActive() && (!body->isStaticOrKinematicObject()))
@@ -1078,7 +1091,17 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 
 	}
 
-	///this should probably be switched on by default, but it is not well tested yet
+}
+
+void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+	BT_PROFILE("integrateTransforms");
+    if (m_nonStaticRigidBodies.size() > 0)
+    {
+        integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
+    }
+
+    ///this should probably be switched on by default, but it is not well tested yet
 	if (m_applySpeculativeContactRestitution)
 	{
 		BT_PROFILE("apply speculative contact restitution");
@@ -1112,14 +1135,12 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			}
 		}
 	}
-
 }
 
 
 
 
 
-
 void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
 	BT_PROFILE("predictUnconstraintMotion");

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -30,6 +30,7 @@ class btIDebugDraw;
 struct InplaceSolverIslandCallback;
 
 #include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btThreads.h"
 
 
 ///btDiscreteDynamicsWorld provides discrete rigid body simulation
@@ -68,9 +69,11 @@ protected:
 	bool	m_latencyMotionStateInterpolation;
 
 	btAlignedObjectArray<btPersistentManifold*>	m_predictiveManifolds;
+    btSpinMutex m_predictiveManifoldsMutex;  // used to synchronize threads creating predictive contacts
 
 	virtual void	predictUnconstraintMotion(btScalar timeStep);
 	
+    void integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep );  // can be called in parallel
 	virtual void	integrateTransforms(btScalar timeStep);
 		
 	virtual void	calculateSimulationIslands();
@@ -85,7 +88,9 @@ protected:
 
 	virtual void	internalSingleStepSimulation( btScalar timeStep);
 
-	void	createPredictiveContacts(btScalar timeStep);
+    void releasePredictiveContacts();
+    void createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep );  // can be called in parallel
+	virtual void	createPredictiveContacts(btScalar timeStep);
 
 	virtual void	saveKinematicState(btScalar timeStep);
 
@@ -139,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);
 

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp

@@ -0,0 +1,162 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btDiscreteDynamicsWorldMt.h"
+
+//collision detection
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "btSimulationIslandManagerMt.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btQuickprof.h"
+
+//rigidbody & constraints
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+
+
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+
+#include "BulletDynamics/Dynamics/btActionInterface.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btMotionState.h"
+
+#include "LinearMath/btSerializer.h"
+
+
+struct InplaceSolverIslandCallbackMt : public btSimulationIslandManagerMt::IslandCallback
+{
+	btContactSolverInfo*	m_solverInfo;
+	btConstraintSolver*		m_solver;
+	btIDebugDraw*			m_debugDrawer;
+	btDispatcher*			m_dispatcher;
+
+	InplaceSolverIslandCallbackMt(
+		btConstraintSolver*	solver,
+		btStackAlloc* stackAlloc,
+		btDispatcher* dispatcher)
+		:m_solverInfo(NULL),
+		m_solver(solver),
+		m_debugDrawer(NULL),
+		m_dispatcher(dispatcher)
+	{
+
+	}
+
+	InplaceSolverIslandCallbackMt& operator=(InplaceSolverIslandCallbackMt& other)
+	{
+		btAssert(0);
+		(void)other;
+		return *this;
+	}
+
+	SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btIDebugDraw* debugDrawer)
+	{
+		btAssert(solverInfo);
+		m_solverInfo = solverInfo;
+		m_debugDrawer = debugDrawer;
+	}
+
+
+	virtual	void	processIsland( btCollisionObject** bodies,
+                                   int numBodies,
+                                   btPersistentManifold** manifolds,
+                                   int numManifolds,
+                                   btTypedConstraint** constraints,
+                                   int numConstraints,
+                                   int islandId
+                                   )
+	{
+        m_solver->solveGroup( bodies,
+                              numBodies,
+                              manifolds,
+                              numManifolds,
+                              constraints,
+                              numConstraints,
+                              *m_solverInfo,
+                              m_debugDrawer,
+                              m_dispatcher
+                              );
+    }
+
+};
+
+
+
+btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+{
+	if (m_ownsIslandManager)
+	{
+		m_islandManager->~btSimulationIslandManager();
+		btAlignedFree( m_islandManager);
+	}
+    {
+		void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallbackMt),16);
+		m_solverIslandCallbackMt = new (mem) InplaceSolverIslandCallbackMt (m_constraintSolver, 0, dispatcher);
+    }
+	{
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
+		btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
+        m_islandManager = im;
+        im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
+	}
+}
+
+
+btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
+{
+	if (m_solverIslandCallbackMt)
+	{
+		m_solverIslandCallbackMt->~InplaceSolverIslandCallbackMt();
+		btAlignedFree(m_solverIslandCallbackMt);
+	}
+	if (m_ownsConstraintSolver)
+	{
+		m_constraintSolver->~btConstraintSolver();
+		btAlignedFree(m_constraintSolver);
+	}
+}
+
+
+void	btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
+{
+	BT_PROFILE("solveConstraints");
+
+	m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
+	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+
+	/// solve all the constraints for this island
+    btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
+    im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt );
+
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+}
+
+

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h

@@ -0,0 +1,42 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef BT_DISCRETE_DYNAMICS_WORLD_MT_H
+#define BT_DISCRETE_DYNAMICS_WORLD_MT_H
+
+#include "btDiscreteDynamicsWorld.h"
+
+struct InplaceSolverIslandCallbackMt;
+
+///
+/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support
+///                              solving simulation islands on multiple threads.
+///
+ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
+{
+protected:
+    InplaceSolverIslandCallbackMt* m_solverIslandCallbackMt;
+
+    virtual void	solveConstraints(btContactSolverInfo& solverInfo);
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	virtual ~btDiscreteDynamicsWorldMt();
+};
+
+#endif //BT_DISCRETE_DYNAMICS_WORLD_H

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h

@@ -34,7 +34,8 @@ enum btDynamicsWorldType
 	BT_DISCRETE_DYNAMICS_WORLD=2,
 	BT_CONTINUOUS_DYNAMICS_WORLD=3,
 	BT_SOFT_RIGID_DYNAMICS_WORLD=4,
-	BT_GPU_DYNAMICS_WORLD=5
+	BT_GPU_DYNAMICS_WORLD=5,
+    BT_SOFT_MULTIBODY_DYNAMICS_WORLD=6
 };
 
 ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
@@ -88,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;
 

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -79,6 +79,8 @@ void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
 	//moved to btCollisionObject
 	m_friction = constructionInfo.m_friction;
 	m_rollingFriction = constructionInfo.m_rollingFriction;
+    m_spinningFriction = constructionInfo.m_spinningFriction;
+    
 	m_restitution = constructionInfo.m_restitution;
 
 	setCollisionShape( constructionInfo.m_collisionShape );
@@ -303,13 +305,6 @@ btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForc
 	return gf;
 }
 
-void btSetCrossMatrixMinus(btMatrix3x3& res, const btVector3& a)
-{
-	const btScalar a_0 = a[0], a_1 = a[1], a_2 = a[2];
-	res.setValue(0, +a_2, -a_1,
-		-a_2, 0, +a_0,
-		+a_1, -a_0, 0);
-}
 
 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
 {	

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btRigidBody.h

@@ -135,6 +135,8 @@ public:
 		///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever.
 		///See Bullet/Demos/RollingFrictionDemo for usage
 		btScalar			m_rollingFriction;
+        btScalar			m_spinningFriction;//torsional friction around contact normal
+        
 		///best simulation results using zero restitution.
 		btScalar			m_restitution;
 
@@ -158,6 +160,7 @@ public:
 			m_angularDamping(btScalar(0.)),
 			m_friction(btScalar(0.5)),
 			m_rollingFriction(btScalar(0)),
+            m_spinningFriction(btScalar(0)),
 			m_restitution(btScalar(0.)),
 			m_linearSleepingThreshold(btScalar(0.8)),
 			m_angularSleepingThreshold(btScalar(1.f)),

Source/ThirdParty/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);
 

Source/ThirdParty/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);
 

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp

@@ -0,0 +1,641 @@
+/*
+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 "LinearMath/btScalar.h"
+#include "btSimulationIslandManagerMt.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+//#include <stdio.h>
+#include "LinearMath/btQuickprof.h"
+
+
+SIMD_FORCE_INLINE int calcBatchCost( int bodies, int manifolds, int constraints )
+{
+    // rough estimate of the cost of a batch, used for merging
+    int batchCost = bodies + 8 * manifolds + 4 * constraints;
+    return batchCost;
+}
+
+
+SIMD_FORCE_INLINE int calcBatchCost( const btSimulationIslandManagerMt::Island* island )
+{
+    return calcBatchCost( island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size() );
+}
+
+
+btSimulationIslandManagerMt::btSimulationIslandManagerMt()
+{
+    m_minimumSolverBatchSize = calcBatchCost(0, 128, 0);
+    m_batchIslandMinBodyCount = 32;
+    m_islandDispatch = defaultIslandDispatch;
+    m_batchIsland = NULL;
+}
+
+
+btSimulationIslandManagerMt::~btSimulationIslandManagerMt()
+{
+    for ( int i = 0; i < m_allocatedIslands.size(); ++i )
+    {
+        delete m_allocatedIslands[ i ];
+    }
+    m_allocatedIslands.resize( 0 );
+    m_activeIslands.resize( 0 );
+    m_freeIslands.resize( 0 );
+}
+
+
+inline	int	getIslandId(const btPersistentManifold* lhs)
+{
+	const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
+	const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
+    int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
+	return islandId;
+}
+
+
+SIMD_FORCE_INLINE	int	btGetConstraintIslandId( const btTypedConstraint* lhs )
+{
+    const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+    const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+    int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
+    return islandId;
+}
+
+/// function object that routes calls to operator<
+class IslandBatchSizeSortPredicate
+{
+public:
+    bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const
+    {
+        int lCost = calcBatchCost( lhs );
+        int rCost = calcBatchCost( rhs );
+        return lCost > rCost;
+    }
+};
+
+
+class IslandBodyCapacitySortPredicate
+{
+public:
+    bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const
+    {
+        return lhs->bodyArray.capacity() > rhs->bodyArray.capacity();
+    }
+};
+
+
+void btSimulationIslandManagerMt::Island::append( const Island& other )
+{
+    // append bodies
+    for ( int i = 0; i < other.bodyArray.size(); ++i )
+    {
+        bodyArray.push_back( other.bodyArray[ i ] );
+    }
+    // append manifolds
+    for ( int i = 0; i < other.manifoldArray.size(); ++i )
+    {
+        manifoldArray.push_back( other.manifoldArray[ i ] );
+    }
+    // append constraints
+    for ( int i = 0; i < other.constraintArray.size(); ++i )
+    {
+        constraintArray.push_back( other.constraintArray[ i ] );
+    }
+}
+
+
+bool btIsBodyInIsland( const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj )
+{
+    for ( int i = 0; i < island.bodyArray.size(); ++i )
+    {
+        if ( island.bodyArray[ i ] == obj )
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+
+void btSimulationIslandManagerMt::initIslandPools()
+{
+    // reset island pools
+    int numElem = getUnionFind().getNumElements();
+    m_lookupIslandFromId.resize( numElem );
+    for ( int i = 0; i < m_lookupIslandFromId.size(); ++i )
+    {
+        m_lookupIslandFromId[ i ] = NULL;
+    }
+    m_activeIslands.resize( 0 );
+    m_freeIslands.resize( 0 );
+    // check whether allocated islands are sorted by body capacity (largest to smallest)
+    int lastCapacity = 0;
+    bool isSorted = true;
+    for ( int i = 0; i < m_allocatedIslands.size(); ++i )
+    {
+        Island* island = m_allocatedIslands[ i ];
+        int cap = island->bodyArray.capacity();
+        if ( cap > lastCapacity )
+        {
+            isSorted = false;
+            break;
+        }
+        lastCapacity = cap;
+    }
+    if ( !isSorted )
+    {
+        m_allocatedIslands.quickSort( IslandBodyCapacitySortPredicate() );
+    }
+
+    m_batchIsland = NULL;
+    // mark all islands free (but avoid deallocation)
+    for ( int i = 0; i < m_allocatedIslands.size(); ++i )
+    {
+        Island* island = m_allocatedIslands[ i ];
+        island->bodyArray.resize( 0 );
+        island->manifoldArray.resize( 0 );
+        island->constraintArray.resize( 0 );
+        island->id = -1;
+        island->isSleeping = true;
+        m_freeIslands.push_back( island );
+    }
+}
+
+
+btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland( int id )
+{
+    Island* island = m_lookupIslandFromId[ id ];
+    if ( island == NULL )
+    {
+        // search for existing island
+        for ( int i = 0; i < m_activeIslands.size(); ++i )
+        {
+            if ( m_activeIslands[ i ]->id == id )
+            {
+                island = m_activeIslands[ i ];
+                break;
+            }
+        }
+        m_lookupIslandFromId[ id ] = island;
+    }
+    return island;
+}
+
+
+btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland( int id, int numBodies )
+{
+    Island* island = NULL;
+    int allocSize = numBodies;
+    if ( numBodies < m_batchIslandMinBodyCount )
+    {
+        if ( m_batchIsland )
+        {
+            island = m_batchIsland;
+            m_lookupIslandFromId[ id ] = island;
+            // if we've made a large enough batch,
+            if ( island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount )
+            {
+                // next time start a new batch
+                m_batchIsland = NULL;
+            }
+            return island;
+        }
+        else
+        {
+            // need to allocate a batch island
+            allocSize = m_batchIslandMinBodyCount * 2;
+        }
+    }
+    btAlignedObjectArray<Island*>& freeIslands = m_freeIslands;
+
+    // search for free island
+    if ( freeIslands.size() > 0 )
+    {
+        // try to reuse a previously allocated island
+        int iFound = freeIslands.size();
+        // linear search for smallest island that can hold our bodies
+        for ( int i = freeIslands.size() - 1; i >= 0; --i )
+        {
+            if ( freeIslands[ i ]->bodyArray.capacity() >= allocSize )
+            {
+                iFound = i;
+                island = freeIslands[ i ];
+                island->id = id;
+                break;
+            }
+        }
+        // if found, shrink array while maintaining ordering
+        if ( island )
+        {
+            int iDest = iFound;
+            int iSrc = iDest + 1;
+            while ( iSrc < freeIslands.size() )
+            {
+                freeIslands[ iDest++ ] = freeIslands[ iSrc++ ];
+            }
+            freeIslands.pop_back();
+        }
+    }
+    if ( island == NULL )
+    {
+        // no free island found, allocate
+        island = new Island();  // TODO: change this to use the pool allocator
+        island->id = id;
+        island->bodyArray.reserve( allocSize );
+        m_allocatedIslands.push_back( island );
+    }
+    m_lookupIslandFromId[ id ] = island;
+    if ( numBodies < m_batchIslandMinBodyCount )
+    {
+        m_batchIsland = island;
+    }
+    m_activeIslands.push_back( island );
+    return island;
+}
+
+
+void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld )
+{
+
+	BT_PROFILE("islandUnionFindAndQuickSort");
+	
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	//we are going to sort the unionfind array, and store the element id in the size
+	//afterwards, we clean unionfind, to make sure no-one uses it anymore
+	
+	getUnionFind().sortIslands();
+	int numElem = getUnionFind().getNumElements();
+
+	int endIslandIndex=1;
+	int startIslandIndex;
+
+	//update the sleeping state for bodies, if all are sleeping
+	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+	{
+		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+		for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+		{
+		}
+
+		//int numSleeping = 0;
+
+		bool allSleeping = true;
+
+		int idx;
+		for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+		{
+			int i = getUnionFind().getElement(idx).m_sz;
+
+			btCollisionObject* colObj0 = collisionObjects[i];
+			if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+			{
+//				printf("error in island management\n");
+			}
+
+			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+			if (colObj0->getIslandTag() == islandId)
+			{
+				if (colObj0->getActivationState()== ACTIVE_TAG)
+				{
+					allSleeping = false;
+				}
+				if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
+				{
+					allSleeping = false;
+				}
+			}
+		}
+
+		if (allSleeping)
+		{
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+//					printf("error in island management\n");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					colObj0->setActivationState( ISLAND_SLEEPING );
+				}
+			}
+		} else
+		{
+
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+//					printf("error in island management\n");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+					{
+						colObj0->setActivationState( WANTS_DEACTIVATION);
+						colObj0->setDeactivationTime(0.f);
+					}
+				}
+			}
+		}
+	}
+}
+
+
+void btSimulationIslandManagerMt::addBodiesToIslands( btCollisionWorld* collisionWorld )
+{
+    btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+    int endIslandIndex = 1;
+    int startIslandIndex;
+    int numElem = getUnionFind().getNumElements();
+
+    // create explicit islands and add bodies to each
+    for ( startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex )
+    {
+        int islandId = getUnionFind().getElement( startIslandIndex ).m_id;
+
+        // find end index
+        for ( endIslandIndex = startIslandIndex; ( endIslandIndex < numElem ) && ( getUnionFind().getElement( endIslandIndex ).m_id == islandId ); endIslandIndex++ )
+        {
+        }
+        // check if island is sleeping
+        bool islandSleeping = true;
+        for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ )
+        {
+            int i = getUnionFind().getElement( iElem ).m_sz;
+            btCollisionObject* colObj = collisionObjects[ i ];
+            if ( colObj->isActive() )
+            {
+                islandSleeping = false;
+            }
+        }
+        if ( !islandSleeping )
+        {
+            // want to count the number of bodies before allocating the island to optimize memory usage of the Island structures
+            int numBodies = endIslandIndex - startIslandIndex;
+            Island* island = allocateIsland( islandId, numBodies );
+            island->isSleeping = false;
+
+            // add bodies to island
+            for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ )
+            {
+                int i = getUnionFind().getElement( iElem ).m_sz;
+                btCollisionObject* colObj = collisionObjects[ i ];
+                island->bodyArray.push_back( colObj );
+            }
+        }
+    }
+
+}
+
+
+void btSimulationIslandManagerMt::addManifoldsToIslands( btDispatcher* dispatcher )
+{
+    // walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island
+    int maxNumManifolds = dispatcher->getNumManifolds();
+    for ( int i = 0; i < maxNumManifolds; i++ )
+    {
+        btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal( i );
+
+        const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
+        const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );
+
+        ///@todo: check sleeping conditions!
+        if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
+             ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
+        {
+
+            //kinematic objects don't merge islands, but wake up all connected objects
+            if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
+            {
+                if ( colObj0->hasContactResponse() )
+                    colObj1->activate();
+            }
+            if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
+            {
+                if ( colObj1->hasContactResponse() )
+                    colObj0->activate();
+            }
+            //filtering for response
+            if ( dispatcher->needsResponse( colObj0, colObj1 ) )
+            {
+                // scatter manifolds into various islands
+                int islandId = getIslandId( manifold );
+                // if island not sleeping,
+                if ( Island* island = getIsland( islandId ) )
+                {
+                    island->manifoldArray.push_back( manifold );
+                }
+            }
+        }
+    }
+}
+
+
+void btSimulationIslandManagerMt::addConstraintsToIslands( btAlignedObjectArray<btTypedConstraint*>& constraints )
+{
+    // walk constraints
+    for ( int i = 0; i < constraints.size(); i++ )
+    {
+        // scatter constraints into various islands
+        btTypedConstraint* constraint = constraints[ i ];
+        if ( constraint->isEnabled() )
+        {
+            int islandId = btGetConstraintIslandId( constraint );
+            // if island is not sleeping,
+            if ( Island* island = getIsland( islandId ) )
+            {
+                island->constraintArray.push_back( constraint );
+            }
+        }
+    }
+}
+
+
+void btSimulationIslandManagerMt::mergeIslands()
+{
+    // sort islands in order of decreasing batch size
+    m_activeIslands.quickSort( IslandBatchSizeSortPredicate() );
+
+    // merge small islands to satisfy minimum batch size
+    // find first small batch island
+    int destIslandIndex = m_activeIslands.size();
+    for ( int i = 0; i < m_activeIslands.size(); ++i )
+    {
+        Island* island = m_activeIslands[ i ];
+        int batchSize = calcBatchCost( island );
+        if ( batchSize < m_minimumSolverBatchSize )
+        {
+            destIslandIndex = i;
+            break;
+        }
+    }
+    int lastIndex = m_activeIslands.size() - 1;
+    while ( destIslandIndex < lastIndex )
+    {
+        // merge islands from the back of the list
+        Island* island = m_activeIslands[ destIslandIndex ];
+        int numBodies = island->bodyArray.size();
+        int numManifolds = island->manifoldArray.size();
+        int numConstraints = island->constraintArray.size();
+        int firstIndex = lastIndex;
+        // figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have
+        while ( true )
+        {
+            Island* src = m_activeIslands[ firstIndex ];
+            numBodies += src->bodyArray.size();
+            numManifolds += src->manifoldArray.size();
+            numConstraints += src->constraintArray.size();
+            int batchCost = calcBatchCost( numBodies, numManifolds, numConstraints );
+            if ( batchCost >= m_minimumSolverBatchSize )
+            {
+                break;
+            }
+            if ( firstIndex - 1 == destIslandIndex )
+            {
+                break;
+            }
+            firstIndex--;
+        }
+        // reserve space for these pointers to minimize reallocation
+        island->bodyArray.reserve( numBodies );
+        island->manifoldArray.reserve( numManifolds );
+        island->constraintArray.reserve( numConstraints );
+        // merge islands
+        for ( int i = firstIndex; i <= lastIndex; ++i )
+        {
+            island->append( *m_activeIslands[ i ] );
+        }
+        // shrink array to exclude the islands that were merged from
+        m_activeIslands.resize( firstIndex );
+        lastIndex = firstIndex - 1;
+        destIslandIndex++;
+    }
+}
+
+
+void btSimulationIslandManagerMt::defaultIslandDispatch( btAlignedObjectArray<Island*>* islandsPtr, IslandCallback* callback )
+{
+    // serial dispatch
+    btAlignedObjectArray<Island*>& islands = *islandsPtr;
+    for ( int i = 0; i < islands.size(); ++i )
+    {
+        Island* island = islands[ i ];
+        btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
+        btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
+        callback->processIsland( &island->bodyArray[ 0 ],
+                                 island->bodyArray.size(),
+                                 manifolds,
+                                 island->manifoldArray.size(),
+                                 constraintsPtr,
+                                 island->constraintArray.size(),
+                                 island->id
+                                 );
+    }
+}
+
+///@todo: this is random access, it can be walked 'cache friendly'!
+void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher,
+                                                        btCollisionWorld* collisionWorld,
+                                                        btAlignedObjectArray<btTypedConstraint*>& constraints,
+                                                        IslandCallback* callback
+                                                        )
+{
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	buildIslands(dispatcher,collisionWorld);
+
+	BT_PROFILE("processIslands");
+
+	if(!getSplitIslands())
+	{
+        btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
+        int maxNumManifolds = dispatcher->getNumManifolds();
+
+        for ( int i = 0; i < maxNumManifolds; i++ )
+        {
+            btPersistentManifold* manifold = manifolds[ i ];
+
+            const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
+            const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );
+
+            ///@todo: check sleeping conditions!
+            if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
+                 ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
+            {
+
+                //kinematic objects don't merge islands, but wake up all connected objects
+                if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
+                {
+                    if ( colObj0->hasContactResponse() )
+                        colObj1->activate();
+                }
+                if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
+                {
+                    if ( colObj1->hasContactResponse() )
+                        colObj0->activate();
+                }
+            }
+        }
+        btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
+		callback->processIsland(&collisionObjects[0],
+                                 collisionObjects.size(),
+                                 manifolds,
+                                 maxNumManifolds,
+                                 constraintsPtr,
+                                 constraints.size(),
+                                 -1
+                                 );
+	}
+	else
+	{
+        initIslandPools();
+
+        //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
+        addBodiesToIslands( collisionWorld );
+        addManifoldsToIslands( dispatcher );
+        addConstraintsToIslands( constraints );
+
+        // m_activeIslands array should now contain all non-sleeping Islands, and each Island should
+        // have all the necessary bodies, manifolds and constraints.
+
+        // if we want to merge islands with small batch counts,
+        if ( m_minimumSolverBatchSize > 1 )
+        {
+            mergeIslands();
+        }
+        // dispatch islands to solver
+        m_islandDispatch( &m_activeIslands, callback );
+	}
+}

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h

@@ -0,0 +1,109 @@
+/*
+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_SIMULATION_ISLAND_MANAGER_MT_H
+#define BT_SIMULATION_ISLAND_MANAGER_MT_H
+
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+
+class btTypedConstraint;
+
+
+///
+/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager
+///                       Splits the world up into islands which can be solved in parallel.
+///                       In order to solve islands in parallel, an IslandDispatch function
+///                       must be provided which will dispatch calls to multiple threads.
+///                       The amount of parallelism that can be achieved depends on the number
+///                       of islands. If only a single island exists, then no parallelism is
+///                       possible.
+///
+class btSimulationIslandManagerMt : public btSimulationIslandManager
+{
+public:
+    struct Island
+    {
+        // a simulation island consisting of bodies, manifolds and constraints,
+        // to be passed into a constraint solver.
+        btAlignedObjectArray<btCollisionObject*> bodyArray;
+        btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+        btAlignedObjectArray<btTypedConstraint*> constraintArray;
+        int id;  // island id
+        bool isSleeping;
+
+        void append( const Island& other );  // add bodies, manifolds, constraints to my own
+    };
+    struct	IslandCallback
+    {
+        virtual ~IslandCallback() {};
+
+        virtual	void processIsland( btCollisionObject** bodies,
+                                    int numBodies,
+                                    btPersistentManifold** manifolds,
+                                    int numManifolds,
+                                    btTypedConstraint** constraints,
+                                    int numConstraints,
+                                    int islandId
+                                    ) = 0;
+    };
+    typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
+    static void defaultIslandDispatch( btAlignedObjectArray<Island*>* islands, IslandCallback* callback );
+protected:
+    btAlignedObjectArray<Island*> m_allocatedIslands;  // owner of all Islands
+    btAlignedObjectArray<Island*> m_activeIslands;  // islands actively in use
+    btAlignedObjectArray<Island*> m_freeIslands;  // islands ready to be reused
+    btAlignedObjectArray<Island*> m_lookupIslandFromId;  // big lookup table to map islandId to Island pointer
+    Island* m_batchIsland;
+    int m_minimumSolverBatchSize;
+    int m_batchIslandMinBodyCount;
+    IslandDispatchFunc m_islandDispatch;
+
+    Island* getIsland( int id );
+    virtual Island* allocateIsland( int id, int numBodies );
+    virtual void initIslandPools();
+    virtual void addBodiesToIslands( btCollisionWorld* collisionWorld );
+    virtual void addManifoldsToIslands( btDispatcher* dispatcher );
+    virtual void addConstraintsToIslands( btAlignedObjectArray<btTypedConstraint*>& constraints );
+    virtual void mergeIslands();
+	
+public:
+	btSimulationIslandManagerMt();
+	virtual ~btSimulationIslandManagerMt();
+
+    virtual void buildAndProcessIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld, btAlignedObjectArray<btTypedConstraint*>& constraints, IslandCallback* callback );
+
+	virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+
+    int getMinimumSolverBatchSize() const
+    {
+        return m_minimumSolverBatchSize;
+    }
+    void setMinimumSolverBatchSize( int sz )
+    {
+        m_minimumSolverBatchSize = sz;
+    }
+    IslandDispatchFunc getIslandDispatchFunction() const
+    {
+        return m_islandDispatch;
+    }
+    // allow users to set their own dispatch function for multithreaded dispatch
+    void setIslandDispatchFunction( IslandDispatchFunc func )
+    {
+        m_islandDispatch = func;
+    }
+};
+
+#endif //BT_SIMULATION_ISLAND_MANAGER_H
+

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBody.h

@@ -6,7 +6,8 @@
  *   
  * COPYRIGHT:
  *   Copyright (C) Stephen Thompson, <[email protected]>, 2011-2013
- *   Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
+ *   Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
+ *   Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
 
  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.
@@ -30,6 +31,8 @@
 #include "LinearMath/btMatrix3x3.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
+
+///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
 #ifdef BT_USE_DOUBLE_PRECISION
 	#define btMultiBodyData	btMultiBodyDoubleData
 	#define btMultiBodyDataName	"btMultiBodyDoubleData"
@@ -45,7 +48,7 @@
 #include "btMultiBodyLink.h"
 class btMultiBodyLinkCollider;
 
-class btMultiBody 
+ATTRIBUTE_ALIGNED16(class) btMultiBody 
 {
 public:
 
@@ -60,21 +63,19 @@ public:
 		btScalar mass,                // mass of base
 		const btVector3 &inertia,    // inertia of base, in base frame; assumed diagonal
 		bool fixedBase,           // whether the base is fixed (true) or can move (false)
-		bool canSleep,
-		bool multiDof = false
-			  );
+		bool canSleep, bool deprecatedMultiDof=true);
 
 
 	virtual ~btMultiBody();
     
+	//note: fixed link collision with parent is always disabled
 	void setupFixed(int linkIndex,
 						   btScalar mass,
 						   const btVector3 &inertia,
 						   int parent,
 						   const btQuaternion &rotParentToThis,
 						   const btVector3 &parentComToThisPivotOffset,
-                           const btVector3 &thisPivotToThisComOffset,
-						   bool disableParentCollision);
+                           const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true);
 
 						
 	void setupPrismatic(int i,
@@ -271,7 +272,11 @@ public:
     btVector3 localDirToWorld(int i, const btVector3 &vec) const;
     btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
     btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
-    
+
+    //
+    // transform a frame in local coordinate to a frame in world coordinate
+    //
+    btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;
 
     //
     // calculate kinetic energy and angular momentum
@@ -336,72 +341,36 @@ void addJointTorque(int i, btScalar Q);
     // improvement, at least on Windows (where dynamic memory
     // allocation appears to be fairly slow).
     //
-    void stepVelocities(btScalar dt,
+   
+ 
+	void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
                         btAlignedObjectArray<btScalar> &scratch_r,
                         btAlignedObjectArray<btVector3> &scratch_v,
-                        btAlignedObjectArray<btMatrix3x3> &scratch_m);
+                        btAlignedObjectArray<btMatrix3x3> &scratch_m,
+			bool isConstraintPass=false
+		);
 
-	void stepVelocitiesMultiDof(btScalar dt,
+///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
+        void stepVelocitiesMultiDof(btScalar dt,
                         btAlignedObjectArray<btScalar> &scratch_r,
                         btAlignedObjectArray<btVector3> &scratch_v,
                         btAlignedObjectArray<btMatrix3x3> &scratch_m,
-			bool isConstraintPass=false
-		);
+                        bool isConstraintPass=false)
+	{
+		computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass);
+        }
 
-    // calcAccelerationDeltas
+    // calcAccelerationDeltasMultiDof
     // input: force vector (in same format as jacobian, i.e.:
     //                      3 torque values, 3 force values, num_links joint torque values)
     // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
     // (existing contents of output array are replaced)
-    // stepVelocities must have been called first.
-    void calcAccelerationDeltas(const btScalar *force, btScalar *output,
-                                btAlignedObjectArray<btScalar> &scratch_r,
-                                btAlignedObjectArray<btVector3> &scratch_v) const;
-
+    // calcAccelerationDeltasMultiDof must have been called first.
 	void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
                                 btAlignedObjectArray<btScalar> &scratch_r,
                                 btAlignedObjectArray<btVector3> &scratch_v) const;
-
-    // apply a delta-vee directly. used in sequential impulses code.
-    void applyDeltaVee(const btScalar * delta_vee) 
-	{
-
-        for (int i = 0; i < 6 + getNumLinks(); ++i) 
-		{
-			m_realBuf[i] += delta_vee[i];
-		}
-		
-    }
-    void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) 
-	{
-		btScalar sum = 0;
-        for (int i = 0; i < 6 + getNumLinks(); ++i)
-		{
-			sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
-		}
-		btScalar l = btSqrt(sum);
-		/*
-		static btScalar maxl = -1e30f;
-		if (l>maxl)
-		{
-			maxl=l;
-	//		printf("maxl=%f\n",maxl);
-		}
-		*/
-		if (l>m_maxAppliedImpulse)
-		{
-//			printf("exceeds 100: l=%f\n",maxl);
-			multiplier *= m_maxAppliedImpulse/l;
-		}
-
-        for (int i = 0; i < 6 + getNumLinks(); ++i)
-		{
-			sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
-			m_realBuf[i] += delta_vee[i] * multiplier;
-			btClamp(m_realBuf[i],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
-		}
-    }
-
+	
+  
 	void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
 	{
 		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
@@ -447,7 +416,6 @@ void addJointTorque(int i, btScalar Q);
 	
 	
     // timestep the positions (given current velocities).
-    void stepPositions(btScalar dt);
 	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
 
 
@@ -458,26 +426,18 @@ void addJointTorque(int i, btScalar Q);
     // This routine fills out a contact constraint jacobian for this body.
     // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
     // 'normal' & 'contact_point' are both given in world coordinates.
-    void fillContactJacobian(int link,
-                             const btVector3 &contact_point,
-                             const btVector3 &normal,
-                             btScalar *jac,
-                             btAlignedObjectArray<btScalar> &scratch_r,
-                             btAlignedObjectArray<btVector3> &scratch_v,
-                             btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
-
-	//multidof version of fillContactJacobian
+	
 	void fillContactJacobianMultiDof(int link,
                              const btVector3 &contact_point,
                              const btVector3 &normal,
                              btScalar *jac,
                              btAlignedObjectArray<btScalar> &scratch_r,
                              btAlignedObjectArray<btVector3> &scratch_v,
-							 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
+							 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
 
 	//a more general version of fillContactJacobianMultiDof which does not assume..
 	//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
-	void filConstraintJacobianMultiDof(int link,
+	void fillConstraintJacobianMultiDof(int link,
                              const btVector3 &contact_point,
 							 const btVector3 &normal_ang,
                              const btVector3 &normal_lin,
@@ -576,7 +536,7 @@ void addJointTorque(int i, btScalar Q);
 		return m_hasSelfCollision;
 	}
 
-	bool isMultiDof() { return m_isMultiDof; }
+	
 	void finalizeMultiDof();
 
 	void useRK4Integration(bool use) { m_useRK4 = use; }
@@ -600,6 +560,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;
@@ -617,11 +579,42 @@ void addJointTorque(int i, btScalar Q);
 		m_baseName = name;
 	}
 
+	///users can point to their objects, userPointer is not used by Bullet
+	void*	getUserPointer() const
+	{
+		return m_userObjectPointer;
+	}
+
+	int	getUserIndex() const
+	{
+		return m_userIndex;
+	}
+
+	int	getUserIndex2() const
+	{
+		return m_userIndex2;
+	}
+	///users can point to their objects, userPointer is not used by Bullet
+	void	setUserPointer(void* userPointer)
+	{
+		m_userObjectPointer = userPointer;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void	setUserIndex(int index)
+	{
+		m_userIndex = index;
+	}
+
+	void	setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
+
 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 btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
@@ -685,6 +678,7 @@ private:
 	btMatrix3x3 m_cachedInertiaTopRight;
 	btMatrix3x3 m_cachedInertiaLowerLeft;
 	btMatrix3x3 m_cachedInertiaLowerRight;
+	bool m_cachedInertiaValid;
 
     bool m_fixedBase;
 
@@ -693,6 +687,10 @@ private:
     bool m_canSleep;
     btScalar m_sleepTimer;
 
+	void* m_userObjectPointer;
+	int m_userIndex2;
+	int m_userIndex;
+
 	int	m_companionId;
 	btScalar	m_linearDamping;
 	btScalar	m_angularDamping;
@@ -700,7 +698,7 @@ private:
 	btScalar	m_maxAppliedImpulse;
 	btScalar	m_maxCoordinateVelocity;
 	bool		m_hasSelfCollision;
-	bool		m_isMultiDof;
+	
 		bool __posUpdated;
 		int m_dofCount, m_posVarCnt;
 	bool m_useRK4, m_useGlobalVelocities;
@@ -717,27 +715,25 @@ struct btMultiBodyLinkDoubleData
 	btVector3DoubleData		m_jointAxisTop[6];
 	btVector3DoubleData		m_jointAxisBottom[6];
 
-
-	char					*m_linkName;
-	char					*m_jointName;
-	btCollisionObjectDoubleData	*m_linkCollider;
-	
 	btVector3DoubleData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
 	double					m_linkMass;
 	int						m_parentIndex;
 	int						m_jointType;
-	
-
-	
 
 	int						m_dofCount;
 	int						m_posVarCount;
 	double					m_jointPos[7];
 	double					m_jointVel[6];
 	double					m_jointTorque[6];
-	
-	
-	
+
+	double					m_jointDamping;
+	double					m_jointFriction;
+
+	char					*m_linkName;
+	char					*m_jointName;
+	btCollisionObjectDoubleData	*m_linkCollider;
+	char					*m_paddingPtr;
+
 };
 
 struct btMultiBodyLinkFloatData
@@ -747,12 +743,6 @@ struct btMultiBodyLinkFloatData
 	btVector3FloatData		m_thisPivotToThisComOffset;
 	btVector3FloatData		m_jointAxisTop[6];
 	btVector3FloatData		m_jointAxisBottom[6];
-	
-
-	char				*m_linkName;
-	char				*m_jointName;
-	btCollisionObjectFloatData	*m_linkCollider;
-	
 	btVector3FloatData	m_linkInertia;   // inertia of the base (in local frame; diagonal)
 	int						m_dofCount;
 	float				m_linkMass;
@@ -765,25 +755,29 @@ struct btMultiBodyLinkFloatData
 	float					m_jointVel[6];
 	float					m_jointTorque[6];
 	int						m_posVarCount;
-	
+	float					m_jointDamping;
+	float					m_jointFriction;
+
+	char				*m_linkName;
+	char				*m_jointName;
+	btCollisionObjectFloatData	*m_linkCollider;
+	char				*m_paddingPtr;
 
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btMultiBodyDoubleData
 {
-	char	*m_baseName;
-	btMultiBodyLinkDoubleData	*m_links;
-	btCollisionObjectDoubleData	*m_baseCollider;
-
 	btTransformDoubleData m_baseWorldTransform;
 	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
-	
-	int		m_numLinks;
 	double	m_baseMass;
 
-	char m_padding[4];
-	
+	char	*m_baseName;
+	btMultiBodyLinkDoubleData	*m_links;
+	btCollisionObjectDoubleData	*m_baseCollider;
+	char	*m_paddingPtr;
+	int		m_numLinks;
+	char	m_padding[4];
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -23,18 +23,12 @@ void btMultiBodyConstraint::updateJacobianSizes()
 {
     if(m_bodyA)
 	{
-		if(m_bodyA->isMultiDof())
-			m_jacSizeA = (6 + m_bodyA->getNumDofs());
-		else
-			m_jacSizeA = (6 + m_bodyA->getNumLinks());
+		m_jacSizeA = (6 + m_bodyA->getNumDofs());
 	}
 
 	if(m_bodyB)
 	{
-		if(m_bodyB->isMultiDof())
-			m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
-		else
-			m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumLinks();
+		m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
 	}
 	else
 		m_jacSizeBoth = m_jacSizeA;
@@ -59,335 +53,365 @@ void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScala
 }
 
 btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstraint& solverConstraint,
-															btMultiBodyJacobianData& data,
-															btScalar* jacOrgA, btScalar* jacOrgB,
-															const btVector3& contactNormalOnB,
-															const btVector3& posAworld, const btVector3& posBworld,
-															btScalar posError,
-															const btContactSolverInfo& infoGlobal,
-															btScalar lowerLimit, btScalar upperLimit,
-															btScalar relaxation,
-															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+                                                        btMultiBodyJacobianData& data,
+                                                        btScalar* jacOrgA, btScalar* jacOrgB,
+                                                        const btVector3& constraintNormalAng,
+                                                        const btVector3& constraintNormalLin,
+                                                        const btVector3& posAworld, const btVector3& posBworld,
+                                                        btScalar posError,
+                                                        const btContactSolverInfo& infoGlobal,
+                                                        btScalar lowerLimit, btScalar upperLimit,
+                                                        bool angConstraint,
+                                                        btScalar relaxation,
+                                                        bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
-
-
-	solverConstraint.m_multiBodyA = m_bodyA;
-	solverConstraint.m_multiBodyB = m_bodyB;
-	solverConstraint.m_linkA = m_linkA;
-	solverConstraint.m_linkB = m_linkB;
-
-	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
-	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
-
-	btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
-	btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
-
-	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
-	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
-
-	btVector3 rel_pos1, rel_pos2;				//these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
-	if (bodyA)
-		rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
-	if (bodyB)
-		rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
-
-	if (multiBodyA)
-	{
-		if (solverConstraint.m_linkA<0)
-		{
-			rel_pos1 = posAworld - multiBodyA->getBasePos();
-		} else
-		{
-			rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
-		}
-
-		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
-
-		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
-
-		if (solverConstraint.m_deltaVelAindex <0)
-		{
-			solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
-			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
-		} else
-		{
-			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
-		}
-
-		//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
-		//resize..
-		solverConstraint.m_jacAindex = data.m_jacobians.size();
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
-		//copy/determine
-		if(jacOrgA)
-		{
-			for (int i=0;i<ndofA;i++)
-				data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
-		}
-		else
-		{
-			btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
-			if(multiBodyA->isMultiDof())
-				multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
-			else
-				multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
-		}
-
-		//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
-		//resize..
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);		//=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
-		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-		//determine..
-		if(multiBodyA->isMultiDof())
-			multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-		else
-			multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-
-		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
-		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-		solverConstraint.m_contactNormal1 = contactNormalOnB;
-	}
-	else //if(rb0)
-	{
-		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
-		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
-		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-		solverConstraint.m_contactNormal1 = contactNormalOnB;
-	}
-
-	if (multiBodyB)
-	{
-		if (solverConstraint.m_linkB<0)
-		{
-			rel_pos2 = posBworld - multiBodyB->getBasePos();
-		} else
-		{
-			rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
-		}
-
-		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
-
-		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
-		if (solverConstraint.m_deltaVelBindex <0)
-		{
-			solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
-			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
-		}
-
-		//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
-		//resize..
-		solverConstraint.m_jacBindex = data.m_jacobians.size();
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
-		//copy/determine..
-		if(jacOrgB)
-		{
-			for (int i=0;i<ndofB;i++)
-				data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
-		}
-		else
-		{
-			if(multiBodyB->isMultiDof())
-				multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
-			else
-				multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
-		}
-
-		//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
-		//resize..
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
-		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-		//determine..
-		if(multiBodyB->isMultiDof())
-			multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
-		else
-			multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
-
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
-		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-		solverConstraint.m_contactNormal2 = -contactNormalOnB;
-
-	}
-	else //if(rb1)
-	{
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
-		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
-		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-		solverConstraint.m_contactNormal2 = -contactNormalOnB;
-	}
-	{
-
-		btVector3 vec;
-		btScalar denom0 = 0.f;
-		btScalar denom1 = 0.f;
-		btScalar* jacB = 0;
-		btScalar* jacA = 0;
-		btScalar* deltaVelA = 0;
-		btScalar* deltaVelB = 0;
-		int ndofA  = 0;
-		//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
-		if (multiBodyA)
-		{
-			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
-			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA; ++i)
-			{
-				btScalar j = jacA[i] ;
-				btScalar l = deltaVelA[i];
-				denom0 += j*l;
-			}
-		}
-		else if(rb0)
-		{
-			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-			denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
-		}
-		//
-		if (multiBodyB)
-		{
-			const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
-			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB; ++i)
-			{
-				btScalar j = jacB[i] ;
-				btScalar l = deltaVelB[i];
-				denom1 += j*l;
-			}
-
-		}
-		else if(rb1)
-		{
-			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-			denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
-		}
-
-		//
-		btScalar d = denom0+denom1;
-		if (d>SIMD_EPSILON)
-		{
-			solverConstraint.m_jacDiagABInv = relaxation/(d);
-		}
-		else
-		{
-		//disable the constraint row to handle singularity/redundant constraint
-			solverConstraint.m_jacDiagABInv  = 0.f;
-		}
-	}
-
-
-	//compute rhs and remaining solverConstraint fields
-	btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
-
-	btScalar rel_vel = 0.f;
-	int ndofA  = 0;
-	int ndofB  = 0;
-	{
-		btVector3 vel1,vel2;
-		if (multiBodyA)
-		{
-			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
-			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i)
-				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		}
-		else if(rb0)
-		{
-			rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
-		}
-		if (multiBodyB)
-		{
-			ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
-			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i)
-				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
-		}
-		else if(rb1)
-		{
-			rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
-		}
-
-		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
-	}
-
-
-	///warm starting (or zero if disabled)
-	/*
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-	{
-		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-
-		if (solverConstraint.m_appliedImpulse)
-		{
-			if (multiBodyA)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVee(deltaV,impulse);
-				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-			} else
-			{
-				if (rb0)
+    solverConstraint.m_multiBodyA = m_bodyA;
+    solverConstraint.m_multiBodyB = m_bodyB;
+    solverConstraint.m_linkA = m_linkA;
+    solverConstraint.m_linkB = m_linkB;
+    
+    btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+    btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+    
+    btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
+    btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
+    
+    btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
+    btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
+    
+    btVector3 rel_pos1, rel_pos2;				//these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
+    if (bodyA)
+        rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
+    if (bodyB)
+        rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
+    
+    if (multiBodyA)
+    {
+        if (solverConstraint.m_linkA<0)
+        {
+            rel_pos1 = posAworld - multiBodyA->getBasePos();
+        } else
+        {
+            rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
+        }
+        
+        const int ndofA  = multiBodyA->getNumDofs() + 6;
+        
+        solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
+        
+        if (solverConstraint.m_deltaVelAindex <0)
+        {
+            solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
+            multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
+            data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
+        } else
+        {
+            btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+        }
+        
+        //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
+        //resize..
+        solverConstraint.m_jacAindex = data.m_jacobians.size();
+        data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
+        //copy/determine
+        if(jacOrgA)
+        {
+            for (int i=0;i<ndofA;i++)
+                data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
+        }
+        else
+        {
+            btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
+            //multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+            multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+        }
+        
+        //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+        //resize..
+        data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);		//=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
+        btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+        btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+        //determine..
+        multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+        
+        btVector3 torqueAxis0;
+        if (angConstraint) {
+            torqueAxis0 = constraintNormalAng;
+        }
+        else {
+            torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+            
+        }
+        solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+        solverConstraint.m_contactNormal1 = constraintNormalLin;
+    }
+    else //if(rb0)
+    {
+        btVector3 torqueAxis0;
+        if (angConstraint) {
+            torqueAxis0 = constraintNormalAng;
+        }
+        else {
+            torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+        }
+        solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+        solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+        solverConstraint.m_contactNormal1 = constraintNormalLin;
+    }
+    
+    if (multiBodyB)
+    {
+        if (solverConstraint.m_linkB<0)
+        {
+            rel_pos2 = posBworld - multiBodyB->getBasePos();
+        } else
+        {
+            rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
+        }
+        
+        const int ndofB  = multiBodyB->getNumDofs() + 6;
+        
+        solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
+        if (solverConstraint.m_deltaVelBindex <0)
+        {
+            solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
+            multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
+            data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
+        }
+        
+        //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
+        //resize..
+        solverConstraint.m_jacBindex = data.m_jacobians.size();
+        data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+        //copy/determine..
+        if(jacOrgB)
+        {
+            for (int i=0;i<ndofB;i++)
+                data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
+        }
+        else
+        {
+            //multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+            multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+        }
+        
+        //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+        //resize..
+        data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+        btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+        btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+        //determine..
+        multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+        
+        btVector3 torqueAxis1;
+        if (angConstraint) {
+            torqueAxis1 = constraintNormalAng;
+        }
+        else {
+            torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+        }
+        solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+        solverConstraint.m_contactNormal2 = -constraintNormalLin;
+    }
+    else //if(rb1)
+    {
+        btVector3 torqueAxis1;
+        if (angConstraint) {
+            torqueAxis1 = constraintNormalAng;
+        }
+        else {
+            torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+        }
+        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+        solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+        solverConstraint.m_contactNormal2 = -constraintNormalLin;
+    }
+    {
+        
+        btVector3 vec;
+        btScalar denom0 = 0.f;
+        btScalar denom1 = 0.f;
+        btScalar* jacB = 0;
+        btScalar* jacA = 0;
+        btScalar* deltaVelA = 0;
+        btScalar* deltaVelB = 0;
+        int ndofA  = 0;
+        //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
+        if (multiBodyA)
+        {
+            ndofA = multiBodyA->getNumDofs() + 6;
+            jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+            deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+            for (int i = 0; i < ndofA; ++i)
+            {
+                btScalar j = jacA[i] ;
+                btScalar l = deltaVelA[i];
+                denom0 += j*l;
+            }
+        }
+        else if(rb0)
+        {
+            vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+            if (angConstraint) {
+                denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec);
+            }
+            else {
+                denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
+            }
+        }
+        //
+        if (multiBodyB)
+        {
+            const int ndofB = multiBodyB->getNumDofs() + 6;
+            jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+            deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+            for (int i = 0; i < ndofB; ++i)
+            {
+                btScalar j = jacB[i] ;
+                btScalar l = deltaVelB[i];
+                denom1 += j*l;
+            }
+            
+        }
+        else if(rb1)
+        {
+            vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+            if (angConstraint) {
+                denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec);
+            }
+            else {
+                denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
+            }
+        }
+        
+        //
+        btScalar d = denom0+denom1;
+        if (d>SIMD_EPSILON)
+        {
+            solverConstraint.m_jacDiagABInv = relaxation/(d);
+        }
+        else
+        {
+            //disable the constraint row to handle singularity/redundant constraint
+            solverConstraint.m_jacDiagABInv  = 0.f;
+        }
+    }
+    
+    
+    //compute rhs and remaining solverConstraint fields
+    btScalar penetration = isFriction? 0 : posError;
+    
+    btScalar rel_vel = 0.f;
+    int ndofA  = 0;
+    int ndofB  = 0;
+    {
+        btVector3 vel1,vel2;
+        if (multiBodyA)
+        {
+            ndofA = multiBodyA->getNumDofs() + 6;
+            btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+            for (int i = 0; i < ndofA ; ++i)
+                rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+        }
+        else if(rb0)
+        {
+            rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+        }
+        if (multiBodyB)
+        {
+            ndofB = multiBodyB->getNumDofs() + 6;
+            btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+            for (int i = 0; i < ndofB ; ++i)
+                rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+            
+        }
+        else if(rb1)
+        {
+            rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+        }
+        
+        solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
+    }
+    
+    
+    ///warm starting (or zero if disabled)
+    /*
+     if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+     {
+     solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+     
+     if (solverConstraint.m_appliedImpulse)
+     {
+     if (multiBodyA)
+     {
+     btScalar impulse = solverConstraint.m_appliedImpulse;
+     btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+     multiBodyA->applyDeltaVee(deltaV,impulse);
+     applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
+     } else
+     {
+     if (rb0)
 					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-			}
-			if (multiBodyB)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVee(deltaV,impulse);
-				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-			} else
-			{
-				if (rb1)
+     }
+     if (multiBodyB)
+     {
+     btScalar impulse = solverConstraint.m_appliedImpulse;
+     btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+     multiBodyB->applyDeltaVee(deltaV,impulse);
+     applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
+     } else
+     {
+     if (rb1)
 					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-			}
-		}
-	} else
-	*/
-
-	solverConstraint.m_appliedImpulse = 0.f;
-	solverConstraint.m_appliedPushImpulse = 0.f;
-
-	{
-
-		btScalar positionalError = 0.f;
-		btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
-
-
-		btScalar erp = infoGlobal.m_erp2;
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-		{
-			erp = infoGlobal.m_erp;
-		}
-
-		positionalError = -penetration * erp/infoGlobal.m_timeStep;
-
-		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-		{
-			//combine position and velocity into rhs
-			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-			solverConstraint.m_rhsPenetration = 0.f;
-
-		} else
-		{
-			//split position and velocity into rhs and m_rhsPenetration
-			solverConstraint.m_rhs = velocityImpulse;
-			solverConstraint.m_rhsPenetration = penetrationImpulse;
-		}
-
-		solverConstraint.m_cfm = 0.f;
-		solverConstraint.m_lowerLimit = lowerLimit;
-		solverConstraint.m_upperLimit = upperLimit;
-	}
-
-	return rel_vel;
-
+     }
+     }
+     } else
+     */
+    
+    solverConstraint.m_appliedImpulse = 0.f;
+    solverConstraint.m_appliedPushImpulse = 0.f;
+    
+    {
+        
+        btScalar positionalError = 0.f;
+        btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
+        
+        
+        btScalar erp = infoGlobal.m_erp2;
+		
+		//split impulse is not implemented yet for btMultiBody*
+		//if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+        {
+            erp = infoGlobal.m_erp;
+        }
+        
+        positionalError = -penetration * erp/infoGlobal.m_timeStep;
+        
+        btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+        btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+        
+		//split impulse is not implemented yet for btMultiBody*
+
+      //  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+        {
+            //combine position and velocity into rhs
+            solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+            solverConstraint.m_rhsPenetration = 0.f;
+            
+        } 
+		/*else
+        {
+            //split position and velocity into rhs and m_rhsPenetration
+            solverConstraint.m_rhs = velocityImpulse;
+            solverConstraint.m_rhsPenetration = penetrationImpulse;
+        }
+        */
+
+        solverConstraint.m_cfm = 0.f;
+        solverConstraint.m_lowerLimit = lowerLimit;
+        solverConstraint.m_upperLimit = upperLimit;
+    }
+    
+    return rel_vel;
+    
 }

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -39,7 +39,7 @@ struct btMultiBodyJacobianData
 };
 
 
-class btMultiBodyConstraint
+ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraint
 {
 protected:
 
@@ -66,26 +66,36 @@ protected:
     btAlignedObjectArray<btScalar> m_data;
 
 	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
-
+    
 	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
 																btMultiBodyJacobianData& data,
-																btScalar* jacOrgA, btScalar* jacOrgB,
-																const btVector3& contactNormalOnB,
+                                     btScalar* jacOrgA, btScalar* jacOrgB,
+                                     const btVector3& constraintNormalAng,
+                                     
+																const btVector3& constraintNormalLin,
 																const btVector3& posAworld, const btVector3& posBworld,
 																btScalar posError,
 																const btContactSolverInfo& infoGlobal,
-																btScalar lowerLimit, btScalar upperLimit,
+                                     btScalar lowerLimit, btScalar upperLimit,
+                                     bool angConstraint = false,
+                                     
 																btScalar relaxation = 1.f,
 																bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
 
 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;
@@ -114,6 +124,7 @@ public:
 		btAssert(dof>=0);
 		btAssert(dof < getNumRows());
 		m_data[dof] = appliedImpulse;
+        
 	}
 	
 	btScalar	getAppliedImpulse(int dof)