3rdparty: update bullet3

3rdparty/bullet3/src/Bullet3Common/b3AlignedAllocator.cpp

@@ -15,9 +15,11 @@ subject to the following restrictions:
 
 #include "b3AlignedAllocator.h"
 
+#ifdef B3_ALLOCATOR_STATISTICS
 int b3g_numAlignedAllocs = 0;
 int b3g_numAlignedFree = 0;
 int b3g_totalBytesAlignedAllocs = 0;  //detect memory leaks
+#endif
 
 static void *b3AllocDefault(size_t size)
 {
@@ -109,10 +111,10 @@ void *b3AlignedAllocInternal(size_t size, int alignment, int line, char *filenam
 {
 	void *ret;
 	char *real;
-
+#ifdef B3_ALLOCATOR_STATISTICS
 	b3g_totalBytesAlignedAllocs += size;
 	b3g_numAlignedAllocs++;
-
+#endif
 	real = (char *)b3s_allocFunc(size + 2 * sizeof(void *) + (alignment - 1));
 	if (real)
 	{
@@ -135,14 +137,16 @@ void *b3AlignedAllocInternal(size_t size, int alignment, int line, char *filenam
 void b3AlignedFreeInternal(void *ptr, int line, char *filename)
 {
 	void *real;
+#ifdef B3_ALLOCATOR_STATISTICS
 	b3g_numAlignedFree++;
-
+#endif
 	if (ptr)
 	{
 		real = *((void **)(ptr)-1);
 		int size = *((int *)(ptr)-2);
+#ifdef B3_ALLOCATOR_STATISTICS
 		b3g_totalBytesAlignedAllocs -= size;
-
+#endif
 		b3Printf("free #%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedFree, real, filename, line, size);
 
 		b3s_freeFunc(real);
@@ -157,7 +161,9 @@ void b3AlignedFreeInternal(void *ptr, int line, char *filename)
 
 void *b3AlignedAllocInternal(size_t size, int alignment)
 {
+#ifdef B3_ALLOCATOR_STATISTICS
 	b3g_numAlignedAllocs++;
+#endif
 	void *ptr;
 	ptr = b3s_alignedAllocFunc(size, alignment);
 	//	b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
@@ -170,8 +176,9 @@ void b3AlignedFreeInternal(void *ptr)
 	{
 		return;
 	}
-
+#ifdef B3_ALLOCATOR_STATISTICS
 	b3g_numAlignedFree++;
+#endif
 	//	b3Printf("b3AlignedFreeInternal %x\n",ptr);
 	b3s_alignedFreeFunc(ptr);
 }

3rdparty/bullet3/src/Bullet3Common/b3Quaternion.h

@@ -92,13 +92,23 @@ public:
 	/**@brief Set the rotation using axis angle notation 
    * @param axis The axis around which to rotate
    * @param angle The magnitude of the rotation in Radians */
-	void setRotation(const b3Vector3& axis, const b3Scalar& _angle)
+	void setRotation(const b3Vector3& axis1, const b3Scalar& _angle)
 	{
+		b3Vector3 axis = axis1;
+		axis.safeNormalize();
+		
 		b3Scalar d = axis.length();
 		b3Assert(d != b3Scalar(0.0));
-		b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
-		setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
-				 b3Cos(_angle * b3Scalar(0.5)));
+		if (d < B3_EPSILON)
+		{
+			setValue(0, 0, 0, 1);
+		}
+		else
+		{
+			b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
+			setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
+				b3Cos(_angle * b3Scalar(0.5)));
+		}
 	}
 	/**@brief Set the quaternion using Euler angles
    * @param yaw Angle around Y

3rdparty/bullet3/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp

@@ -1227,7 +1227,7 @@ void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs)
 		m_overlappingPairs.resize(numPairs);
 
 	}  //B3_PROFILE("GPU_RADIX SORT");
-	//init3dSap();
+	   //init3dSap();
 }
 
 void b3GpuSapBroadphase::writeAabbsToGpu()

3rdparty/bullet3/src/BulletCollision/BroadphaseCollision/btDbvt.cpp

@@ -37,7 +37,7 @@ static DBVT_INLINE int indexof(const btDbvtNode* node)
 static DBVT_INLINE btDbvtVolume merge(const btDbvtVolume& a,
 									  const btDbvtVolume& b)
 {
-#if (DBVT_MERGE_IMPL == DBVT_IMPL_SSE)
+#ifdef BT_USE_SSE
 	ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]);
 	btDbvtVolume* ptr = (btDbvtVolume*)locals;
 	btDbvtVolume& res = *ptr;
@@ -298,7 +298,7 @@ static int split(btDbvtNode** leaves,
 static btDbvtVolume bounds(btDbvtNode** leaves,
 						   int count)
 {
-#if DBVT_MERGE_IMPL == DBVT_IMPL_SSE
+#ifdef BT_USE_SSE
 	ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]);
 	btDbvtVolume* ptr = (btDbvtVolume*)locals;
 	btDbvtVolume& volume = *ptr;

3rdparty/bullet3/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -247,7 +247,9 @@ void btDbvtBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
 	// instead of just a local.
 	int threadIndex = btGetCurrentThreadIndex();
 	btAlignedObjectArray<const btDbvtNode*> localStack;
-	if (threadIndex < m_rayTestStacks.size())
+	//todo(erwincoumans, "why do we get tsan issue here?")
+	if (0)//threadIndex < m_rayTestStacks.size())
+	//if (threadIndex < m_rayTestStacks.size())
 	{
 		// use per-thread preallocated stack if possible to avoid dynamic allocations
 		stack = &m_rayTestStacks[threadIndex];

3rdparty/bullet3/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -43,6 +43,7 @@ btCollisionObject::btCollisionObject()
 	  m_userObjectPointer(0),
 	  m_userIndex2(-1),
 	  m_userIndex(-1),
+	  m_userIndex3(-1),
 	  m_hitFraction(btScalar(1.)),
 	  m_ccdSweptSphereRadius(btScalar(0.)),
 	  m_ccdMotionThreshold(btScalar(0.)),

3rdparty/bullet3/src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -101,6 +101,8 @@ protected:
 
 	int m_userIndex;
 
+	int m_userIndex3;
+
 	///time of impact calculation
 	btScalar m_hitFraction;
 
@@ -526,6 +528,11 @@ public:
 		return m_userIndex2;
 	}
 
+	int getUserIndex3() const
+	{
+		return m_userIndex3;
+	}
+
 	///users can point to their objects, userPointer is not used by Bullet
 	void setUserPointer(void* userPointer)
 	{
@@ -543,6 +550,11 @@ public:
 		m_userIndex2 = index;
 	}
 
+	void setUserIndex3(int index)
+	{
+		m_userIndex3 = index;
+	}
+
 	int getUpdateRevisionInternal() const
 	{
 		return m_updateRevision;

3rdparty/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -113,6 +113,12 @@ public:
 		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
 		btAssert(index < compoundShape->getNumChildShapes());
 
+		if (gCompoundChildShapePairCallback)
+		{
+			if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape))
+				return;
+		}
+
 		//backup
 		btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
 
@@ -130,11 +136,6 @@ public:
 		btVector3 aabbMin1, aabbMax1;
 		m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
 
-		if (gCompoundChildShapePairCallback)
-		{
-			if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape))
-				return;
-		}
 
 		if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 		{

3rdparty/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h

@@ -34,6 +34,8 @@ class btCollisionObject;
 
 class btCollisionShape;
 
+extern btShapePairCallback gCompoundCompoundChildShapePairCallback;
+
 /// btCompoundCompoundCollisionAlgorithm  supports collision between two btCompoundCollisionShape shapes
 class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm
 {

3rdparty/bullet3/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -296,7 +296,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
 		btCapsuleShape* capsuleA = (btCapsuleShape*)min0;
 		btCapsuleShape* capsuleB = (btCapsuleShape*)min1;
 
-		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
 		btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
 											   capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(),
@@ -318,7 +318,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
 		btCapsuleShape* capsuleA = (btCapsuleShape*)min0;
 		btSphereShape* capsuleB = (btSphereShape*)min1;
 
-		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
 		btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
 											   0., capsuleB->getRadius(), capsuleA->getUpAxis(), 1,
@@ -340,7 +340,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
 		btSphereShape* capsuleA = (btSphereShape*)min0;
 		btCapsuleShape* capsuleB = (btCapsuleShape*)min1;
 
-		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
 		btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, 0., capsuleA->getRadius(),
 											   capsuleB->getHalfHeight(), capsuleB->getRadius(), 1, capsuleB->getUpAxis(),
@@ -480,7 +480,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
 			btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*)min1;
 			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
 			{
-				btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+				btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
 				btScalar minDist = -1e30f;
 				btVector3 sepNormalWorldSpace;
@@ -547,7 +547,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
 
 					//tri->initializePolyhedralFeatures();
 
-					btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+					btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 
 					btVector3 sepNormalWorldSpace;
 					btScalar minDist = -1e30f;

3rdparty/bullet3/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp

@@ -116,7 +116,7 @@ void btConvexPlaneCollisionAlgorithm::processCollision(const btCollisionObjectWr
 	btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
 	btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
 
-	hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
+	hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
 	resultOut->setPersistentManifold(m_manifoldPtr);
 	if (hasCollision)
 	{

3rdparty/bullet3/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp

@@ -27,7 +27,7 @@ btConvexPolyhedron::~btConvexPolyhedron()
 {
 }
 
-inline bool IsAlmostZero(const btVector3& v)
+inline bool IsAlmostZero1(const btVector3& v)
 {
 	if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false;
 	return true;
@@ -122,8 +122,8 @@ void btConvexPolyhedron::initialize()
 
 			for (int p = 0; p < m_uniqueEdges.size(); p++)
 			{
-				if (IsAlmostZero(m_uniqueEdges[p] - edge) ||
-					IsAlmostZero(m_uniqueEdges[p] + edge))
+				if (IsAlmostZero1(m_uniqueEdges[p] - edge) ||
+					IsAlmostZero1(m_uniqueEdges[p] + edge))
 				{
 					found = true;
 					break;

3rdparty/bullet3/src/BulletCollision/CollisionShapes/btShapeHull.cpp

@@ -37,21 +37,8 @@ btShapeHull::~btShapeHull()
 
 bool btShapeHull::buildHull(btScalar /*margin*/, int highres)
 {
+	
 	int numSampleDirections = highres ? NUM_UNITSPHERE_POINTS_HIGHRES : NUM_UNITSPHERE_POINTS;
-	{
-		int numPDA = m_shape->getNumPreferredPenetrationDirections();
-		if (numPDA)
-		{
-			for (int i = 0; i < numPDA; i++)
-			{
-				btVector3 norm;
-				m_shape->getPreferredPenetrationDirection(i, norm);
-				getUnitSpherePoints(highres)[numSampleDirections] = norm;
-				numSampleDirections++;
-			}
-		}
-	}
-
 	btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
 	int i;
 	for (i = 0; i < numSampleDirections; i++)
@@ -59,6 +46,17 @@ bool btShapeHull::buildHull(btScalar /*margin*/, int highres)
 		supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]);
 	}
 
+	int numPDA = m_shape->getNumPreferredPenetrationDirections();
+	if (numPDA)
+	{
+		for (int s = 0; s < numPDA; s++)
+		{
+			btVector3 norm;
+			m_shape->getPreferredPenetrationDirection(s, norm);
+			supportPoints[i++] = m_shape->localGetSupportingVertex(norm);
+			numSampleDirections++;
+		}
+	}
 	HullDesc hd;
 	hd.mFlags = QF_TRIANGLES;
 	hd.mVcount = static_cast<unsigned int>(numSampleDirections);

3rdparty/bullet3/src/BulletCollision/Gimpact/btGImpactBvh.h

@@ -1,5 +1,5 @@
-#ifndef GIM_BOX_SET_H_INCLUDED
-#define GIM_BOX_SET_H_INCLUDED
+#ifndef BT_GIMPACT_BVH_H_INCLUDED
+#define BT_GIMPACT_BVH_H_INCLUDED
 
 /*! \file gim_box_set.h
 \author Francisco Leon Najera
@@ -306,4 +306,4 @@ public:
 							   btPairSet& collision_pairs);
 };
 
-#endif  // GIM_BOXPRUNING_H_INCLUDED
+#endif  // BT_GIMPACT_BVH_H_INCLUDED

3rdparty/bullet3/src/BulletCollision/Gimpact/btGImpactBvhStructs.h

@@ -28,28 +28,7 @@ subject to the following restrictions:
 
 #include "btBoxCollision.h"
 #include "btTriangleShapeEx.h"
-
-//! Overlapping pair
-struct GIM_PAIR
-{
-	int m_index1;
-	int m_index2;
-	GIM_PAIR()
-	{
-	}
-
-	GIM_PAIR(const GIM_PAIR& p)
-	{
-		m_index1 = p.m_index1;
-		m_index2 = p.m_index2;
-	}
-
-	GIM_PAIR(int index1, int index2)
-	{
-		m_index1 = index1;
-		m_index2 = index2;
-	}
-};
+#include "gim_pair.h" //for GIM_PAIR
 
 ///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box
 struct GIM_BVH_DATA

3rdparty/bullet3/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp

@@ -18,7 +18,7 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 /*
-Author: Francisco Len Nßjera
+Author: Francisco Leon Najera
 Concave-Concave Collision
 
 */
@@ -50,10 +50,11 @@ public:
 
 	void get_plane_equation_transformed(const btTransform& trans, btVector4& equation) const
 	{
-		equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal);
-		equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal);
-		equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal);
-		equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant;
+		const btVector3 normal = trans.getBasis() * m_planeNormal;
+		equation[0] = normal[0];
+		equation[1] = normal[1];
+		equation[2] = normal[2];
+		equation[3] = normal.dot(trans * (m_planeConstant * m_planeNormal));
 	}
 };
 
@@ -589,14 +590,16 @@ void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrappe
 		}
 
 		btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0);
-		const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap();
+		const btCollisionObjectWrapper* prevObj;
 
 		if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject())
 		{
+			prevObj = m_resultOut->getBody0Wrap();
 			m_resultOut->setBody0Wrap(&ob0);
 		}
 		else
 		{
+			prevObj = m_resultOut->getBody1Wrap();
 			m_resultOut->setBody1Wrap(&ob0);
 		}
 
@@ -609,7 +612,15 @@ void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrappe
 		{
 			shape_vs_shape_collision(&ob0, body1Wrap, colshape0, shape1);
 		}
-		m_resultOut->setBody0Wrap(prevObj0);
+
+		if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject())
+		{
+			m_resultOut->setBody0Wrap(prevObj);
+		}
+		else
+		{
+			m_resultOut->setBody1Wrap(prevObj);
+		}
 	}
 
 	shape0->unlockChildShapes();
@@ -821,6 +832,12 @@ void btGImpactCollisionAlgorithm::processCollision(const btCollisionObjectWrappe
 
 		gimpact_vs_shape(body1Wrap, body0Wrap, gimpactshape1, body0Wrap->getCollisionShape(), true);
 	}
+
+	// Ensure that gContactProcessedCallback is called for concave shapes.
+	if (getLastManifold())
+	{
+		m_resultOut->refreshContactPoints();
+	}
 }
 
 btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)

3rdparty/bullet3/src/BulletCollision/Gimpact/gim_box_set.h

@@ -37,28 +37,7 @@ email: [email protected]
 #include "gim_radixsort.h"
 #include "gim_box_collision.h"
 #include "gim_tri_collision.h"
-
-//! Overlapping pair
-struct GIM_PAIR
-{
-	GUINT m_index1;
-	GUINT m_index2;
-	GIM_PAIR()
-	{
-	}
-
-	GIM_PAIR(const GIM_PAIR& p)
-	{
-		m_index1 = p.m_index1;
-		m_index2 = p.m_index2;
-	}
-
-	GIM_PAIR(GUINT index1, GUINT index2)
-	{
-		m_index1 = index1;
-		m_index2 = index2;
-	}
-};
+#include "gim_pair.h"
 
 //! A pairset array
 class gim_pair_set : public gim_array<GIM_PAIR>

3rdparty/bullet3/src/BulletCollision/Gimpact/gim_linear_math.h

@@ -967,7 +967,7 @@ Last column is added as the position
 		GREAL len;                                                        \
                                                                           \
 		/* do nothing if off-diagonals are zero and diagonals are 	\
-    * equal */     \
+    * equal */      \
 		if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) \
 		{                                                                 \
 			p[0] = m[1][1] * v[0] - m[1][0] * v[1];                       \

3rdparty/bullet3/src/BulletCollision/Gimpact/gim_pair.h

@@ -0,0 +1,28 @@
+#ifndef GIM_PAIR_H
+#define GIM_PAIR_H
+
+
+//! Overlapping pair
+struct GIM_PAIR
+{
+        int m_index1;
+        int m_index2;
+        GIM_PAIR()
+        {
+        }
+
+        GIM_PAIR(const GIM_PAIR& p)
+        {
+                m_index1 = p.m_index1;
+                m_index2 = p.m_index2;
+        }
+
+        GIM_PAIR(int index1, int index2)
+        {
+                m_index1 = index1;
+                m_index2 = index2;
+        }
+};
+
+#endif //GIM_PAIR_H
+

3rdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h

@@ -22,6 +22,19 @@ subject to the following restrictions:
 class btMinkowskiSumShape;
 #include "LinearMath/btIDebugDraw.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_CONVEX_CAST_ITERATIONS 64
+#define MAX_CONVEX_CAST_EPSILON (SIMD_EPSILON * 10)
+#else
+#define MAX_CONVEX_CAST_ITERATIONS 32
+#define MAX_CONVEX_CAST_EPSILON btScalar(0.0001)
+#endif
+///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+//will need to digg deeper to make the algorithm more robust
+//since, a large epsilon can cause an early termination with false
+//positive results (ray intersections that shouldn't be there)
+
 /// btConvexCast is an interface for Casting
 class btConvexCast
 {
@@ -44,7 +57,9 @@ public:
 		CastResult()
 			: m_fraction(btScalar(BT_LARGE_FLOAT)),
 			  m_debugDrawer(0),
-			  m_allowedPenetration(btScalar(0))
+			  m_allowedPenetration(btScalar(0)),
+			  m_subSimplexCastMaxIterations(MAX_CONVEX_CAST_ITERATIONS),
+			  m_subSimplexCastEpsilon(MAX_CONVEX_CAST_EPSILON)
 		{
 		}
 
@@ -57,6 +72,10 @@ public:
 		btScalar m_fraction;  //input and output
 		btIDebugDraw* m_debugDrawer;
 		btScalar m_allowedPenetration;
+		
+		int m_subSimplexCastMaxIterations;
+		btScalar m_subSimplexCastEpsilon;
+
 	};
 
 	/// cast a convex against another convex object

3rdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp

@@ -31,8 +31,8 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simp
 	(void)simplexSolver;
 
 	btVector3 guessVectors[] = {
-		btVector3(transformB.getOrigin() - transformA.getOrigin()).normalized(),
-		btVector3(transformA.getOrigin() - transformB.getOrigin()).normalized(),
+		btVector3(transformB.getOrigin() - transformA.getOrigin()).safeNormalize(),
+		btVector3(transformA.getOrigin() - transformB.getOrigin()).safeNormalize(),
 		btVector3(0, 0, 1),
 		btVector3(0, 1, 0),
 		btVector3(1, 0, 0),

3rdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -36,9 +36,6 @@ btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
 btScalar gGjkEpaPenetrationTolerance = 0.001;
 #endif
 
-//temp globals, to improve GJK/EPA/penetration calculations
-int gNumDeepPenetrationChecks = 0;
-int gNumGjkChecks = 0;
 
 btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
 	: m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
@@ -708,7 +705,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &inpu
 	btScalar marginA = m_marginA;
 	btScalar marginB = m_marginB;
 
-	gNumGjkChecks++;
 
 	//for CCD we don't use margins
 	if (m_ignoreMargin)
@@ -1021,7 +1017,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &inpu
 				// Penetration depth case.
 				btVector3 tmpPointOnA, tmpPointOnB;
 
-				gNumDeepPenetrationChecks++;
 				m_cachedSeparatingAxis.setZero();
 
 				bool isValid2 = m_penetrationDepthSolver->calcPenDepth(

3rdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp

@@ -28,13 +28,7 @@ btSubsimplexConvexCast::btSubsimplexConvexCast(const btConvexShape* convexA, con
 {
 }
 
-///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
-///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
-#ifdef BT_USE_DOUBLE_PRECISION
-#define MAX_ITERATIONS 64
-#else
-#define MAX_ITERATIONS 32
-#endif
+
 bool btSubsimplexConvexCast::calcTimeOfImpact(
 	const btTransform& fromA,
 	const btTransform& toA,
@@ -60,7 +54,7 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
 	btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r * fromA.getBasis()));
 	btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r * fromB.getBasis()));
 	v = supVertexA - supVertexB;
-	int maxIter = MAX_ITERATIONS;
+	int maxIter = result.m_subSimplexCastMaxIterations;
 
 	btVector3 n;
 	n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
@@ -69,20 +63,12 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
 
 	btScalar dist2 = v.length2();
 
-#ifdef BT_USE_DOUBLE_PRECISION
-	btScalar epsilon = SIMD_EPSILON * 10;
-#else
-	//todo: epsilon kept for backward compatibility of unit tests.
-	//will need to digg deeper to make the algorithm more robust
-	//since, a large epsilon can cause an early termination with false
-	//positive results (ray intersections that shouldn't be there)
-	btScalar epsilon = btScalar(0.0001);
-#endif  //BT_USE_DOUBLE_PRECISION
+
 
 	btVector3 w, p;
 	btScalar VdotR;
 
-	while ((dist2 > epsilon) && maxIter--)
+	while ((dist2 > result.m_subSimplexCastEpsilon) && maxIter--)
 	{
 		supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v * interpolatedTransA.getBasis()));
 		supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v * interpolatedTransB.getBasis()));

3rdparty/bullet3/src/BulletDynamics/Character/btKinematicCharacterController.cpp

@@ -737,7 +737,7 @@ void btKinematicCharacterController::playerStep(btCollisionWorld* collisionWorld
 	}
 
 	// quick check...
-	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0))
+	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) 
 	{
 		//		printf("\n");
 		return;  // no motion

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h

@@ -35,6 +35,7 @@ enum btConstraintSolverType
 	BT_MLCP_SOLVER = 2,
 	BT_NNCG_SOLVER = 4,
 	BT_MULTIBODY_SOLVER = 8,
+	BT_BLOCK_SOLVER = 16,
 };
 
 class btConstraintSolver

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -62,6 +62,8 @@ struct btContactSolverInfoData
 	btScalar m_singleAxisRollingFrictionThreshold;
 	btScalar m_leastSquaresResidualThreshold;
 	btScalar m_restitutionVelocityThreshold;
+	bool m_jointFeedbackInWorldSpace;
+	bool m_jointFeedbackInJointFrame;
 };
 
 struct btContactSolverInfo : public btContactSolverInfoData
@@ -94,6 +96,8 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_singleAxisRollingFrictionThreshold = 1e30f;          ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
 		m_leastSquaresResidualThreshold = 0.f;
 		m_restitutionVelocityThreshold = 0.2f;  //if the relative velocity is below this threshold, there is zero restitution
+		m_jointFeedbackInWorldSpace = false;
+		m_jointFeedbackInJointFrame = false;
 	}
 };
 

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp

@@ -820,8 +820,17 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		btScalar dt = BT_ONE / info->fps;
 		btScalar kd = limot->m_springDamping;
 		btScalar ks = limot->m_springStiffness;
-		btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
-		//		btScalar erp = 0.1;
+		btScalar vel;
+		if (rotational)
+		{
+			vel = angVelA.dot(ax1) - angVelB.dot(ax1);
+		}
+		else
+		{
+			btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());
+			btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());
+			vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);
+		}
 		btScalar cfm = BT_ZERO;
 		btScalar mA = BT_ONE / m_rbA.getInvMass();
 		btScalar mB = BT_ONE / m_rbB.getInvMass();
@@ -832,7 +841,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 			if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
 			if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
 		}
-		btScalar m = mA > mB ? mB : mA;
+		btScalar m;
+		if (m_rbA.getInvMass() == 0) m = mB; else
+		if (m_rbB.getInvMass() == 0) m = mA; else
+			m = mA*mB / (mA + mB);
 		btScalar angularfreq = sqrt(ks / m);
 
 		//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
@@ -856,9 +868,12 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		// however in practice any value is fine as long as it is greater then the "proper" velocity,
 		// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
 		// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
+		// (Even with our best intent the "new" velocity is only an estimation. If we underestimate
+		// the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't
+		// matter, because the solver will limit it according the force limit)
 		// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
 		// will we not request a velocity with the wrong direction ?
-		// and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
+		// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
 		// so the sign of the force that is really matters
 		info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
 

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

@@ -264,7 +264,7 @@ enum bt6DofFlags2
 	BT_6DOF_FLAGS_CFM_STOP2 = 1,
 	BT_6DOF_FLAGS_ERP_STOP2 = 2,
 	BT_6DOF_FLAGS_CFM_MOTO2 = 4,
-	BT_6DOF_FLAGS_ERP_MOTO2 = 8
+	BT_6DOF_FLAGS_ERP_MOTO2 = 8,
 };
 #define BT_6DOF_FLAGS_AXIS_SHIFT2 4  // bits per axis
 

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -764,6 +764,12 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	btVector3 ax1A = trA.getBasis().getColumn(2);
 	btVector3 ax1B = trB.getBasis().getColumn(2);
 	btVector3 ax1 = ax1A * factA + ax1B * factB;
+	if (ax1.length2()<SIMD_EPSILON)
+	{
+		factA=0.f;
+		factB=1.f;
+		ax1 = ax1A * factA + ax1B * factB;
+	}
 	ax1.normalize();
 	// fill first 3 rows
 	// we want: velA + wA x relA == velB + wB x relB

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -29,6 +29,68 @@ class btCollisionObject;
 
 typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
 
+struct btSISolverSingleIterationData
+{
+	btAlignedObjectArray<btSolverBody>& m_tmpSolverBodyPool;
+	btConstraintArray& m_tmpSolverContactConstraintPool;
+	btConstraintArray& m_tmpSolverNonContactConstraintPool;
+	btConstraintArray& m_tmpSolverContactFrictionConstraintPool;
+	btConstraintArray& m_tmpSolverContactRollingFrictionConstraintPool;
+
+	btAlignedObjectArray<int>& m_orderTmpConstraintPool;
+	btAlignedObjectArray<int>& m_orderNonContactConstraintPool;
+	btAlignedObjectArray<int>& m_orderFrictionConstraintPool;
+	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& m_tmpConstraintSizesPool;
+	unsigned long& m_seed;
+
+	btSingleConstraintRowSolver& m_resolveSingleConstraintRowGeneric;
+	btSingleConstraintRowSolver& m_resolveSingleConstraintRowLowerLimit;
+	btSingleConstraintRowSolver& m_resolveSplitPenetrationImpulse;
+	btAlignedObjectArray<int>& m_kinematicBodyUniqueIdToSolverBodyTable;
+	int& m_fixedBodyId;
+	int& m_maxOverrideNumSolverIterations;
+	int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
+	static void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep);
+	int getSolverBody(btCollisionObject& body) const;
+
+
+	btSISolverSingleIterationData(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
+		btConstraintArray& tmpSolverContactConstraintPool,
+		btConstraintArray& tmpSolverNonContactConstraintPool,
+		btConstraintArray& tmpSolverContactFrictionConstraintPool,
+		btConstraintArray& tmpSolverContactRollingFrictionConstraintPool,
+		btAlignedObjectArray<int>& orderTmpConstraintPool,
+		btAlignedObjectArray<int>& orderNonContactConstraintPool,
+		btAlignedObjectArray<int>& orderFrictionConstraintPool,
+		btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& tmpConstraintSizesPool,
+		btSingleConstraintRowSolver& resolveSingleConstraintRowGeneric,
+		btSingleConstraintRowSolver& resolveSingleConstraintRowLowerLimit,
+		btSingleConstraintRowSolver& resolveSplitPenetrationImpulse,
+		btAlignedObjectArray<int>& kinematicBodyUniqueIdToSolverBodyTable,
+		unsigned long& seed,
+		int& fixedBodyId,
+		int& maxOverrideNumSolverIterations
+	)
+		:m_tmpSolverBodyPool(tmpSolverBodyPool),
+		m_tmpSolverContactConstraintPool(tmpSolverContactConstraintPool),
+		m_tmpSolverNonContactConstraintPool(tmpSolverNonContactConstraintPool),
+		m_tmpSolverContactFrictionConstraintPool(tmpSolverContactFrictionConstraintPool),
+		m_tmpSolverContactRollingFrictionConstraintPool(tmpSolverContactRollingFrictionConstraintPool),
+		m_orderTmpConstraintPool(orderTmpConstraintPool),
+		m_orderNonContactConstraintPool(orderNonContactConstraintPool),
+		m_orderFrictionConstraintPool(orderFrictionConstraintPool),
+		m_tmpConstraintSizesPool(tmpConstraintSizesPool),
+		m_resolveSingleConstraintRowGeneric(resolveSingleConstraintRowGeneric),
+		m_resolveSingleConstraintRowLowerLimit(resolveSingleConstraintRowLowerLimit),
+		m_resolveSplitPenetrationImpulse(resolveSplitPenetrationImpulse),
+		m_kinematicBodyUniqueIdToSolverBodyTable(kinematicBodyUniqueIdToSolverBodyTable),
+		m_seed(seed),
+		m_fixedBodyId(fixedBodyId),
+		m_maxOverrideNumSolverIterations(maxOverrideNumSolverIterations)
+	{
+	}
+};
+
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 ATTRIBUTE_ALIGNED16(class)
 btSequentialImpulseConstraintSolver : public btConstraintSolver
@@ -64,26 +126,26 @@ protected:
 	btScalar m_leastSquaresResidual;
 
 	void setupFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
-								 btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2,
-								 btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
-								 const btContactSolverInfo& infoGlobal,
-								 btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
+		btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+		const btContactSolverInfo& infoGlobal,
+		btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
 
 	void setupTorsionalFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
-										  btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
-										  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
-										  btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
+		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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
 	btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar torsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.f);
 
 	void setupContactConstraint(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
-								const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
+		const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
 
 	static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode);
 
 	void setFrictionConstraintImpulse(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB,
-									  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
+		btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
 
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
 	unsigned long m_btSeed2;
@@ -97,6 +159,7 @@ protected:
 	virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
 	void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
 
+
 	virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
 	btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
@@ -122,7 +185,8 @@ protected:
 		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
 	}
 
-protected:
+public:
+
 	void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
 	void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
 	void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
@@ -130,6 +194,7 @@ protected:
 	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
+
 	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
@@ -141,13 +206,52 @@ public:
 
 	virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
 
+	static btScalar solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
+	static void convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+	static void convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
+	static void convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
+	static void setupContactConstraintInternal(btSISolverSingleIterationData& siData, btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation,
+		const btVector3& rel_pos1, const btVector3& rel_pos2);
+	static btScalar restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
+	static btSolverConstraint& addTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, 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 = 0, btScalar cfmSlip = 0.);
+	static void setupTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, 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);
+	static void setupFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip);
+	static btSolverConstraint& addFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
+	static void setFrictionConstraintImpulseInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool,
+
+		btSolverConstraint& solverConstraint,
+		int solverBodyIdA, int solverBodyIdB,
+		btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
+	static void convertJointInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
+		int& maxOverrideNumSolverIterations,
+		btSolverConstraint* currentConstraintRow,
+		btTypedConstraint* constraint,
+		const btTypedConstraint::btConstraintInfo1& info1,
+		int solverBodyIdA,
+		int solverBodyIdB,
+		const btContactSolverInfo& infoGlobal);
+
+	static btScalar solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+
+	static void writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+
+	static void writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	static void writeBackBodiesInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	static void solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+
+
 	///clear internal cached data and reset random seed
 	virtual void reset();
 
 	unsigned long btRand2();
-
 	int btRandInt2(int n);
 
+	static unsigned long btRand2a(unsigned long& seed);
+	static int btRandInt2a(int n, unsigned long& seed);
+
 	void setRandSeed(unsigned long seed)
 	{
 		m_btSeed2 = seed;
@@ -180,14 +284,18 @@ public:
 	}
 
 	///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
-	btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
-	btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
-	btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
+	static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
+	static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
+	static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
 
 	///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
-	btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
-	btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
-	btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
+	static btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
+	static btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
+	static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
+
+	static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
+	static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
+
 };
 
-#endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+#endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

3rdparty/bullet3/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp

@@ -147,10 +147,10 @@ void btConstraintSolverPoolMt::reset()
 
 btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
 													 btBroadphaseInterface* pairCache,
-													 btConstraintSolverPoolMt* constraintSolver,
+													 btConstraintSolverPoolMt* solverPool,
 													 btConstraintSolver* constraintSolverMt,
 													 btCollisionConfiguration* collisionConfiguration)
-	: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration)
+	: btDiscreteDynamicsWorld(dispatcher, pairCache, solverPool, collisionConfiguration)
 {
 	if (m_ownsIslandManager)
 	{

3rdparty/bullet3/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h

@@ -120,7 +120,7 @@ public:
 
 	btDiscreteDynamicsWorldMt(btDispatcher * dispatcher,
 							  btBroadphaseInterface * pairCache,
-							  btConstraintSolverPoolMt * constraintSolver,  // Note this should be a solver-pool for multi-threading
+							  btConstraintSolverPoolMt * solverPool,        // Note this should be a solver-pool for multi-threading
 							  btConstraintSolver * constraintSolverMt,      // single multi-threaded solver for large islands (or NULL)
 							  btCollisionConfiguration * collisionConfiguration);
 	virtual ~btDiscreteDynamicsWorldMt();

3rdparty/bullet3/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp

@@ -65,7 +65,7 @@ inline int getIslandId(const btPersistentManifold* lhs)
 	return islandId;
 }
 
-SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
+SIMD_FORCE_INLINE int btGetConstraintIslandId1(const btTypedConstraint* lhs)
 {
 	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
 	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
@@ -452,7 +452,7 @@ void btSimulationIslandManagerMt::addConstraintsToIslands(btAlignedObjectArray<b
 		btTypedConstraint* constraint = constraints[i];
 		if (constraint->isEnabled())
 		{
-			int islandId = btGetConstraintIslandId(constraint);
+			int islandId = btGetConstraintIslandId1(constraint);
 			// if island is not sleeping,
 			if (Island* island = getIsland(islandId))
 			{

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -30,9 +30,6 @@
 //#include "Bullet3Common/b3Logging.h"
 // #define INCLUDE_GYRO_TERM
 
-///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
-bool gJointFeedbackInWorldSpace = false;
-bool gJointFeedbackInJointFrame = false;
 
 namespace
 {
@@ -40,19 +37,21 @@ const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m
 const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
 }  // namespace
 
-namespace
-{
-void SpatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
-					  const btVector3 &displacement,       // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
-					  const btVector3 &top_in,             // top part of input vector
-					  const btVector3 &bottom_in,          // bottom part of input vector
-					  btVector3 &top_out,                  // top part of output vector
-					  btVector3 &bottom_out)               // bottom part of output vector
+void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
+	const btVector3 &displacement,       // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
+	const btVector3 &top_in,             // top part of input vector
+	const btVector3 &bottom_in,          // bottom part of input vector
+	btVector3 &top_out,                  // top part of output vector
+	btVector3 &bottom_out)               // bottom part of output vector
 {
 	top_out = rotation_matrix * top_in;
 	bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
 }
 
+namespace
+{
+
+
 #if 0
     void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
                                  const btVector3 &displacement,
@@ -107,6 +106,7 @@ btMultiBody::btMultiBody(int n_links,
 	  m_fixedBase(fixedBase),
 	  m_awake(true),
 	  m_canSleep(canSleep),
+	  m_canWakeup(true),
 	  m_sleepTimer(0),
 	  m_userObjectPointer(0),
 	  m_userIndex2(-1),
@@ -333,6 +333,9 @@ void btMultiBody::setupPlanar(int i,
 	m_links[i].updateCacheMultiDof();
 	//
 	updateLinksDofOffsets();
+
+	m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());
+	m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());
 }
 
 void btMultiBody::finalizeMultiDof()
@@ -399,23 +402,40 @@ void btMultiBody::setJointPos(int i, btScalar q)
 	m_links[i].updateCacheMultiDof();
 }
 
-void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
+
+void btMultiBody::setJointPosMultiDof(int i, const double *q)
 {
 	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
-		m_links[i].m_jointPos[pos] = q[pos];
+		m_links[i].m_jointPos[pos] = (btScalar)q[pos];
 
 	m_links[i].updateCacheMultiDof();
 }
 
+void btMultiBody::setJointPosMultiDof(int i, const float *q)
+{
+	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
+		m_links[i].m_jointPos[pos] = (btScalar)q[pos];
+
+	m_links[i].updateCacheMultiDof();
+}
+
+
+
 void btMultiBody::setJointVel(int i, btScalar qdot)
 {
 	m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
 }
 
-void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
+void btMultiBody::setJointVelMultiDof(int i, const double *qdot)
+{
+	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
+}
+
+void btMultiBody::setJointVelMultiDof(int i, const float* qdot)
 {
 	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
+		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
 }
 
 const btVector3 &btMultiBody::getRVector(int i) const
@@ -526,35 +546,26 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
 {
 	int num_links = getNumLinks();
 	// Calculates the velocities of each link (and the base) in its local frame
-	omega[0] = quatRotate(m_baseQuat, getBaseOmega());
-	vel[0] = quatRotate(m_baseQuat, getBaseVel());
+	const btQuaternion& base_rot = getWorldToBaseRot();
+	omega[0] = quatRotate(base_rot, getBaseOmega());
+	vel[0] = quatRotate(base_rot, getBaseVel());
 
 	for (int i = 0; i < num_links; ++i)
 	{
-		const int parent = m_links[i].m_parent;
+		const btMultibodyLink& link = getLink(i);
+		const int parent = link.m_parent;
 
 		// transform parent vel into this frame, store in omega[i+1], vel[i+1]
-		SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
-						 omega[parent + 1], vel[parent + 1],
-						 omega[i + 1], vel[i + 1]);
+		spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector,
+			omega[parent + 1], vel[parent + 1],
+			omega[i + 1], vel[i + 1]);
 
 		// now add qidot * shat_i
-		//only supported for revolute/prismatic joints, todo: spherical and planar joints
-		switch (m_links[i].m_jointType)
+		const btScalar* jointVel = getJointVelMultiDof(i);
+		for (int dof = 0; dof < link.m_dofCount; ++dof)
 		{
-			case btMultibodyLink::ePrismatic:
-			case btMultibodyLink::eRevolute:
-			{
-				btVector3 axisTop = m_links[i].getAxisTop(0);
-				btVector3 axisBottom = m_links[i].getAxisBottom(0);
-				btScalar jointVel = getJointVel(i);
-				omega[i + 1] += jointVel * axisTop;
-				vel[i + 1] += jointVel * axisBottom;
-				break;
-			}
-			default:
-			{
-			}
+			omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);
+			vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);
 		}
 	}
 }
@@ -718,10 +729,12 @@ inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)  //ren
 //
 
 void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
-																	   btAlignedObjectArray<btScalar> &scratch_r,
-																	   btAlignedObjectArray<btVector3> &scratch_v,
-																	   btAlignedObjectArray<btMatrix3x3> &scratch_m,
-																	   bool isConstraintPass)
+    btAlignedObjectArray<btScalar> &scratch_r,
+    btAlignedObjectArray<btVector3> &scratch_v,
+    btAlignedObjectArray<btMatrix3x3> &scratch_m,
+	bool isConstraintPass,
+	bool jointFeedbackInWorldSpace,
+	bool jointFeedbackInJointFrame)
 {
 	// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
 	// and the base linear & angular accelerations.
@@ -1124,7 +1137,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
 			btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
 
-			if (gJointFeedbackInJointFrame)
+			if (jointFeedbackInJointFrame)
 			{
 				//shift the reaction forces to the joint frame
 				//linear (force) component is the same
@@ -1132,7 +1145,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 				angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
 			}
 
-			if (gJointFeedbackInWorldSpace)
+			if (jointFeedbackInWorldSpace)
 			{
 				if (isConstraintPass)
 				{
@@ -1711,7 +1724,7 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link,
 												 const btVector3 &normal_ang,
 												 const btVector3 &normal_lin,
 												 btScalar *jac,
-												 btAlignedObjectArray<btScalar> &scratch_r,
+												 btAlignedObjectArray<btScalar> &scratch_r1,
 												 btAlignedObjectArray<btVector3> &scratch_v,
 												 btAlignedObjectArray<btMatrix3x3> &scratch_m) const
 {
@@ -1730,9 +1743,20 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link,
 	v_ptr += num_links + 1;
 	btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
 
-	scratch_r.resize(m_dofCount);
-	btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0;
+	//scratch_r.resize(m_dofCount);
+	//btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0;
 
+    scratch_r1.resize(m_dofCount+num_links);
+    btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0;
+    btScalar* links = num_links? &scratch_r1[m_dofCount] : 0;
+    int numLinksChildToRoot=0;
+    int l = link;
+    while (l != -1)
+    {
+        links[numLinksChildToRoot++]=l;
+        l = m_links[l].m_parent;
+    }
+    
 	btMatrix3x3 *rot_from_world = &scratch_m[0];
 
 	const btVector3 p_minus_com_world = contact_point - m_basePos;
@@ -1766,14 +1790,14 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link,
 	// Qdot coefficients, if necessary.
 	if (num_links > 0 && link > -1)
 	{
-		// TODO: speed this up -- don't calculate for m_links we don't need.
-		// (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
+        // TODO: (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
 		// which is resulting in repeated work being done...)
 
 		// calculate required normals & positions in the local frames.
-		for (int i = 0; i < num_links; ++i)
-		{
-			// transform to local frame
+        for (int a = 0; a < numLinksChildToRoot; a++)
+        {
+            int i = links[numLinksChildToRoot-1-a];
+        	// transform to local frame
 			const int parent = m_links[i].m_parent;
 			const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
 			rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
@@ -1859,6 +1883,8 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 		return;
 	}
 
+	
+
 	// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
 	btScalar motion = 0;
 	{
@@ -1877,8 +1903,11 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 	else
 	{
 		m_sleepTimer = 0;
-		if (!m_awake)
-			wakeUp();
+		if (m_canWakeup)
+		{
+			if (!m_awake)
+				wakeUp();
+		}
 	}
 }
 

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBody.h

@@ -134,6 +134,15 @@ public:
 		return m_baseCollider;
 	}
 
+	const btMultiBodyLinkCollider *getLinkCollider(int index) const
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
 	btMultiBodyLinkCollider *getLinkCollider(int index)
 	{
 		if (index >= 0 && index < getNumLinks())
@@ -173,7 +182,10 @@ public:
 	// get/set pos/vel/rot/omega for the base link
 	//
 
-	const btVector3 &getBasePos() const { return m_basePos; }  // in world frame
+	const btVector3 &getBasePos() const 
+	{ 
+		return m_basePos; 
+	}  // in world frame
 	const btVector3 getBaseVel() const
 	{
 		return btVector3(m_realBuf[3], m_realBuf[4], m_realBuf[5]);
@@ -235,8 +247,10 @@ public:
 
 	void setJointPos(int i, btScalar q);
 	void setJointVel(int i, btScalar qdot);
-	void setJointPosMultiDof(int i, btScalar *q);
-	void setJointVelMultiDof(int i, btScalar *qdot);
+	void setJointPosMultiDof(int i, const double *q);
+	void setJointVelMultiDof(int i, const double *qdot);
+	void setJointPosMultiDof(int i, const float *q);
+	void setJointVelMultiDof(int i, const float *qdot);
 
 	//
 	// direct access to velocities as a vector of 6 + num_links elements.
@@ -338,17 +352,20 @@ public:
 															  btAlignedObjectArray<btScalar> & scratch_r,
 															  btAlignedObjectArray<btVector3> & scratch_v,
 															  btAlignedObjectArray<btMatrix3x3> & scratch_m,
-															  bool isConstraintPass = false);
+															  bool isConstraintPass,
+                                                              bool jointFeedbackInWorldSpace,
+                                                              bool jointFeedbackInJointFrame
+                                                              );
 
 	///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)
-	{
-		computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass);
-	}
+	//void stepVelocitiesMultiDof(btScalar dt,
+	//							btAlignedObjectArray<btScalar> & scratch_r,
+	//							btAlignedObjectArray<btVector3> & scratch_v,
+	//							btAlignedObjectArray<btMatrix3x3> & scratch_m,
+	//							bool isConstraintPass = false)
+	//{
+	//	computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
+	//}
 
 	// calcAccelerationDeltasMultiDof
 	// input: force vector (in same format as jacobian, i.e.:
@@ -437,7 +454,10 @@ public:
 	//
 	void setCanSleep(bool canSleep)
 	{
-		m_canSleep = canSleep;
+		if (m_canWakeup)
+		{
+			m_canSleep = canSleep;
+		}
 	}
 
 	bool getCanSleep() const
@@ -445,6 +465,15 @@ public:
 		return m_canSleep;
 	}
 
+	bool getCanWakeup() const
+	{
+		return m_canWakeup;
+	}
+	
+	void setCanWakeup(bool canWakeup) 
+	{
+		m_canWakeup = canWakeup;
+	}
 	bool isAwake() const { return m_awake; }
 	void wakeUp();
 	void goToSleep();
@@ -455,6 +484,11 @@ public:
 		return m_fixedBase;
 	}
 
+	void setFixedBase(bool fixedBase)
+	{
+		m_fixedBase = fixedBase;
+	}
+
 	int getCompanionId() const
 	{
 		return m_companionId;
@@ -595,6 +629,15 @@ public:
 		m_userIndex2 = index;
 	}
 
+	static void spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
+		const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
+		const btVector3 &top_in,       // top part of input vector
+		const btVector3 &bottom_in,    // bottom part of input vector
+		btVector3 &top_out,         // top part of output vector
+		btVector3 &bottom_out);      // bottom part of output vector
+
+
+
 private:
 	btMultiBody(const btMultiBody &);     // not implemented
 	void operator=(const btMultiBody &);  // not implemented
@@ -665,6 +708,7 @@ private:
 	// Sleep parameters.
 	bool m_awake;
 	bool m_canSleep;
+	bool m_canWakeup;
 	btScalar m_sleepTimer;
 
 	void *m_userObjectPointer;

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -70,6 +70,30 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 	//solve featherstone frictional contact
 	if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
 	{
+		for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodySpinningFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				//adjust friction limits here
+				if (totalImpulse > btScalar(0))
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
+					btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
+
 		for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
 		{
 			if (iteration < infoGlobal.m_numIterations)
@@ -78,18 +102,29 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 
 				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index];
 				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				j1++;
+				int index2 = j1;
+				btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints[index2];
 				//adjust friction limits here
-				if (totalImpulse > btScalar(0))
+				if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
 				{
 					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
 					frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
-					btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+					frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
+					frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
+
+					btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
 					leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
 
 					if (frictionConstraint.m_multiBodyA)
 						frictionConstraint.m_multiBodyA->setPosUpdated(false);
 					if (frictionConstraint.m_multiBodyB)
 						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+
+					if (frictionConstraintB.m_multiBodyA)
+						frictionConstraintB.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraintB.m_multiBodyB)
+						frictionConstraintB.m_multiBodyB->setPosUpdated(false);
 				}
 			}
 		}
@@ -164,6 +199,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
 	m_multiBodyNormalContactConstraints.resize(0);
 	m_multiBodyFrictionContactConstraints.resize(0);
 	m_multiBodyTorsionalFrictionContactConstraints.resize(0);
+	m_multiBodySpinningFrictionContactConstraints.resize(0);
 
 	m_data.m_jacobians.resize(0);
 	m_data.m_deltaVelocitiesUnitImpulse.resize(0);
@@ -1169,6 +1205,43 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalF
 	return solverConstraint;
 }
 
+btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+	btScalar combinedTorsionalFriction,
+	btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	BT_PROFILE("addMultiBodyRollingFrictionConstraint");
+
+	btMultiBodySolverConstraint& solverConstraint = m_multiBodySpinningFrictionContactConstraints.expandNonInitializing();
+	solverConstraint.m_orgConstraint = 0;
+	solverConstraint.m_orgDofIndex = -1;
+
+	solverConstraint.m_frictionIndex = frictionIndex;
+	bool isFriction = true;
+
+	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
+	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
+
+	btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
+	btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
+
+	int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+	solverConstraint.m_multiBodyA = mbA;
+	if (mbA)
+		solverConstraint.m_linkA = fcA->m_link;
+
+	solverConstraint.m_multiBodyB = mbB;
+	if (mbB)
+		solverConstraint.m_linkB = fcB->m_link;
+
+	solverConstraint.m_originalContactPoint = &cp;
+
+	setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
+	return solverConstraint;
+}
 void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
 {
 	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
@@ -1232,7 +1305,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			/////setup the friction constraints
 #define ENABLE_FRICTION
 #ifdef ENABLE_FRICTION
-			solverConstraint.m_frictionIndex = frictionIndex;
+			solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size();
 
 			///Bullet has several options to set the friction directions
 			///By default, each contact has only a single friction direction that is recomputed automatically every frame
@@ -1258,7 +1331,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			{
 				if (cp.m_combinedSpinningFriction > 0)
 				{
-					addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
+					addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
 				}
 				if (cp.m_combinedRollingFriction > 0)
 				{
@@ -1267,11 +1340,8 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 
-					if (cp.m_lateralFrictionDir1.length() > 0.001)
-						addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
-
-					if (cp.m_lateralFrictionDir2.length() > 0.001)
-						addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
+					addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
+					addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
 				}
 				rollingFriction--;
 			}
@@ -1501,9 +1571,13 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 			{
 				pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse;
+			} else
+			{
+				pt->m_appliedImpulseLateral2 = 0;
 			}
-			//do a callback here?
 		}
+		
+			//do a callback here?
 	}
 #if 0
 	//multibody joint feedback

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -34,6 +34,7 @@ protected:
 	btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
 	btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
 	btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints;
+	btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints;
 
 	btMultiBodyJacobianData m_data;
 
@@ -54,6 +55,10 @@ protected:
 																		 btScalar combinedTorsionalFriction,
 																		 btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
 
+	btMultiBodySolverConstraint& addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+		btScalar combinedTorsionalFriction,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
 	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow,
 											btScalar * jacA, btScalar * jacB,
 											btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -491,11 +491,14 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 				m_scratch_v.resize(bod->getNumLinks() + 1);
 				m_scratch_m.resize(bod->getNumLinks() + 1);
 				bool doNotUpdatePos = false;
-
+                bool isConstraintPass = false;
 				{
 					if (!bod->isUsingRK4Integration())
 					{
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m);
+						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
+						m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
+						getSolverInfo().m_jointFeedbackInWorldSpace,
+						getSolverInfo().m_jointFeedbackInJointFrame);
 					}
 					else
 					{
@@ -593,7 +596,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 						btScalar h = solverInfo.m_timeStep;
 #define output &m_scratch_r[bod->getNumDofs()]
 						//calc qdd0 from: q0 & qd0
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
+						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+						getSolverInfo().m_jointFeedbackInJointFrame);
 						pCopy(output, scratch_qdd0, 0, numDofs);
 						//calc q1 = q0 + h/2 * qd0
 						pResetQx();
@@ -603,7 +608,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 						//
 						//calc qdd1 from: q1 & qd1
 						pCopyToVelocityVector(bod, scratch_qd1);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
+						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+						getSolverInfo().m_jointFeedbackInJointFrame);
 						pCopy(output, scratch_qdd1, 0, numDofs);
 						//calc q2 = q0 + h/2 * qd1
 						pResetQx();
@@ -613,7 +620,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 						//
 						//calc qdd2 from: q2 & qd2
 						pCopyToVelocityVector(bod, scratch_qd2);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
+						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+						getSolverInfo().m_jointFeedbackInJointFrame);
 						pCopy(output, scratch_qdd2, 0, numDofs);
 						//calc q3 = q0 + h * qd2
 						pResetQx();
@@ -623,7 +632,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 						//
 						//calc qdd3 from: q3 & qd3
 						pCopyToVelocityVector(bod, scratch_qd3);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
+						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+						getSolverInfo().m_jointFeedbackInJointFrame);
 						pCopy(output, scratch_qdd3, 0, numDofs);
 
 						//
@@ -660,7 +671,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 						{
 							for (int link = 0; link < bod->getNumLinks(); ++link)
 								bod->getLink(link).updateCacheMultiDof();
-							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m);
+							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
+							isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+							getSolverInfo().m_jointFeedbackInJointFrame);
 						}
 					}
 				}
@@ -708,7 +721,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 					if (!bod->isUsingRK4Integration())
 					{
 						bool isConstraintPass = true;
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
+						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+						getSolverInfo().m_jointFeedbackInWorldSpace,
+						getSolverInfo().m_jointFeedbackInJointFrame);
 					}
 				}
 			}

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h

@@ -36,6 +36,10 @@ public:
 	btMultiBody* m_multiBody;
 	int m_link;
 
+	virtual ~btMultiBodyLinkCollider()
+	{
+
+	}
 	btMultiBodyLinkCollider(btMultiBody* multiBody, int link)
 		: m_multiBody(multiBody),
 		  m_link(link)

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp

@@ -22,9 +22,9 @@ subject to the following restrictions:
 
 #define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 
-static bool interleaveContactAndFriction = false;
+static bool interleaveContactAndFriction1 = false;
 
-struct btJointNode
+struct btJointNode1
 {
 	int jointIndex;          // pointer to enclosing dxJoint object
 	int otherBodyIndex;      // *other* body this joint is connected to
@@ -241,7 +241,7 @@ void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo&
 
 void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
 {
-	int numContactRows = interleaveContactAndFriction ? 3 : 1;
+	int numContactRows = interleaveContactAndFriction1 ? 3 : 1;
 
 	int numConstraintRows = m_allConstraintPtrArray.size();
 
@@ -301,7 +301,7 @@ void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSol
 		BT_PROFILE("bodyJointNodeArray.resize");
 		bodyJointNodeArray.resize(numBodies, -1);
 	}
-	btAlignedObjectArray<btJointNode> jointNodeArray;
+	btAlignedObjectArray<btJointNode1> jointNodeArray;
 	{
 		BT_PROFILE("jointNodeArray.reserve");
 		jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
@@ -729,7 +729,7 @@ btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
 		int firstContactConstraintOffset = dindex;
 
 		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
-		if (interleaveContactAndFriction)
+		if (interleaveContactAndFriction1)
 		{
 			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
 			{
@@ -785,7 +785,7 @@ btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
 		firstContactConstraintOffset = dindex;
 
 		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
-		if (interleaveContactAndFriction)
+		if (interleaveContactAndFriction1)
 		{
 			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
 			{

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h

@@ -156,7 +156,7 @@ protected:
 		btTypedConstraint** constraints,
 		int numConstraints,
 		const btContactSolverInfo& infoGlobal,
-		btIDebugDraw* debugDrawer) BT_OVERRIDE;
+		btIDebugDraw* debugDrawer) ;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR()

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp

@@ -0,0 +1,172 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 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.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySphericalJointMotor.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+
+btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true),
+	m_desiredVelocity(0, 0, 0),
+	m_desiredPosition(0,0,0,1),
+	m_kd(1.),
+	m_kp(0.2),
+	m_erp(1),
+	m_rhsClamp(SIMD_INFINITY)
+{
+
+	m_maxAppliedImpulse = maxMotorImpulse;
+}
+
+
+void btMultiBodySphericalJointMotor::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+
+btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor()
+{
+}
+
+int btMultiBodySphericalJointMotor::getIslandIdA() const
+{
+	if (this->m_linkA < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySphericalJointMotor::getIslandIdB() const
+{
+	if (m_linkB < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												 btMultiBodyJacobianData& data,
+												 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+	
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	
+	btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
+	
+	btQuaternion desiredQuat = m_desiredPosition;
+	btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
+		m_bodyA->getJointPosMultiDof(m_linkA)[1],
+		m_bodyA->getJointPosMultiDof(m_linkA)[2],
+		m_bodyA->getJointPosMultiDof(m_linkA)[3]);
+
+btQuaternion relRot = currentQuat.inverse() * desiredQuat;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
+
+
+
+	for (int row = 0; row < getNumRows(); row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		int dof = row;
+		
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar desiredVelocity = this->m_desiredVelocity[row];
+		
+		btScalar velocityError = desiredVelocity - currentVelocity;
+
+		btMatrix3x3 frameAworld;
+		frameAworld.setIdentity();
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+		btScalar posError = 0;
+		{
+			btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eSpherical:
+				{
+					btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
+					posError = m_kp*angleDiff[row % 3];
+					fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+						btVector3(0,0,0), dummy, dummy,
+						posError,
+						infoGlobal,
+						-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+					constraintRow.m_orgConstraint = this;
+					constraintRow.m_orgDofIndex = row;
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h

@@ -0,0 +1,77 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 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.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
+#define BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodySphericalJointMotor : public btMultiBodyConstraint
+{
+protected:
+	btVector3 m_desiredVelocity;
+	btQuaternion m_desiredPosition;
+	btScalar m_kd;
+	btScalar m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
+
+public:
+	btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse);
+	
+	virtual ~btMultiBodySphericalJointMotor();
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+	}
+
+	virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+	}
+
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+};
+
+#endif  //BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H

3rdparty/bullet3/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

@@ -56,7 +56,7 @@ public:
 				{
 					for (int h = 0; h < A.m_rowNonZeroElements1[i].size(); h++)
 					{
-						int j = A.m_rowNonZeroElements1[i][h];
+						j = A.m_rowNonZeroElements1[i][h];
 						if (j != i)  //skip main diagonal
 						{
 							delta += A(i, j) * x[j];

3rdparty/bullet3/src/BulletInverseDynamics/MultiBodyTree.cpp

@@ -281,6 +281,8 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
 			break;
 		case FLOATING:
 			break;
+		case SPHERICAL:
+			break;
 		default:
 			bt_id_error_message("unknown joint type %d\n", joint_type);
 			return -1;
@@ -347,7 +349,7 @@ int MultiBodyTree::finalize()
 	const int &num_bodies = m_init_cache->numBodies();
 	const int &num_dofs = m_init_cache->numDoFs();
 
-	if (num_dofs <= 0)
+	if (num_dofs < 0)
 	{
 		bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
 		//return -1;
@@ -437,6 +439,16 @@ int MultiBodyTree::finalize()
 				rigid_body.m_Jac_JT(1) = 0.0;
 				rigid_body.m_Jac_JT(2) = 0.0;
 				break;
+			case SPHERICAL:
+				// NOTE/TODO: this is not really correct.
+				// the Jacobians should be 3x3 matrices here !
+				rigid_body.m_Jac_JR(0) = 0.0;
+				rigid_body.m_Jac_JR(1) = 0.0;
+				rigid_body.m_Jac_JR(2) = 0.0;
+				rigid_body.m_Jac_JT(0) = 0.0;
+				rigid_body.m_Jac_JT(1) = 0.0;
+				rigid_body.m_Jac_JT(2) = 0.0;
+				break;
 			case FLOATING:
 				// NOTE/TODO: this is not really correct.
 				// the Jacobians should be 3x3 matrices here !

3rdparty/bullet3/src/BulletInverseDynamics/MultiBodyTree.hpp

@@ -16,7 +16,9 @@ enum JointType
 	/// one translational degree of freedom relative to parent
 	PRISMATIC,
 	/// six degrees of freedom relative to parent
-	FLOATING
+	FLOATING,
+	/// three degrees of freedom, relative to parent
+	SPHERICAL
 };
 
 /// Interface class for calculating inverse dynamics for tree structured
@@ -31,12 +33,14 @@ enum JointType
 ///	- PRISMATIC: displacement [m]
 ///	- FLOATING:  Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m]
 ///				 (in that order)
+/// - SPHERICAL: Euler x-y-z angles [rad]
 /// The u vector contains the generalized speeds, which are
 ///	- FIXED:	 none
 ///	- REVOLUTE:  time derivative of angle of rotation [rad/s]
 ///	- PRISMATIC: time derivative of displacement [m/s]
 ///	- FLOATING:  angular velocity [rad/s] (*not* time derivative of rpy angles)
 ///				 and time derivative of displacement in parent frame [m/s]
+//	- SPHERICAL:  angular velocity [rad/s]
 ///
 /// The q and u vectors are obtained by stacking contributions of all bodies in one
 /// vector in the order of body indices.
@@ -47,7 +51,7 @@ enum JointType
 ///	 - PRISMATIC: force  [N], along joint axis
 ///	 - FLOATING:  moment vector [Nm] and force vector [N], both in body-fixed frame
 ///				  (in that order)
-///
+///  - SPHERICAL: moment vector [Nm] 
 /// TODO - force element interface (friction, springs, dampers, etc)
 ///	  - gears and motor inertia
 class MultiBodyTree

3rdparty/bullet3/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp

@@ -35,6 +35,8 @@ const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &typ
 			return "prismatic";
 		case FLOATING:
 			return "floating";
+		case SPHERICAL:
+			return "spherical";
 	}
 	return "error: invalid";
 }
@@ -88,6 +90,8 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const
 			return 1;
 		case FLOATING:
 			return 6;
+		case SPHERICAL:
+			return 3;
 	}
 	bt_id_error_message("unknown joint type %d\n", type);
 	return 0;
@@ -150,6 +154,11 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets()
 				body.m_q_index = q_index;
 				q_index += 6;
 				break;
+			case SPHERICAL:
+				m_body_spherical_list.push_back(i);
+				body.m_q_index = q_index;
+				q_index += 3;
+				break;
 			default:
 				bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
 				return -1;
@@ -238,6 +247,16 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData()
 			case FLOATING:
 				// no static data
 				break;
+			case SPHERICAL:
+				//todo: review
+				body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
+				body.m_parent_vel_rel(0) = 0;
+				body.m_parent_vel_rel(1) = 0;
+				body.m_parent_vel_rel(2) = 0;
+				body.m_parent_acc_rel(0) = 0;
+				body.m_parent_acc_rel(1) = 0;
+				body.m_parent_acc_rel(2) = 0;
+				break;
 		}
 
 			// resize & initialize jacobians to zero.
@@ -352,6 +371,15 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
 		(*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
 	}
 
+	// 4.4 spherical bodies (3-DoF joints)
+	for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
+	{
+		//todo: review
+		RigidBody &body = m_body_list[m_body_spherical_list[i]];
+		(*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
+		(*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
+		(*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
+	}
 	return 0;
 }
 
@@ -413,7 +441,8 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
 		RigidBody &body = m_body_list[m_body_floating_list[i]];
 
 		body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
-							   transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index));
+							   transformY(q(body.m_q_index + 1)) *
+							   transformX(q(body.m_q_index));
 		body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
 		body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
 		body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
@@ -444,6 +473,40 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
 			body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
 		}
 	}
+	
+	for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
+	{
+		//todo: review
+		RigidBody &body = m_body_list[m_body_spherical_list[i]];
+
+		mat33 T;
+
+		T = transformX(q(body.m_q_index)) *
+				transformY(q(body.m_q_index + 1)) *
+				transformZ(q(body.m_q_index + 2));
+		body.m_body_T_parent = T * body.m_body_T_parent_ref;
+			
+		body.m_parent_pos_parent_body(0)=0;
+		body.m_parent_pos_parent_body(1)=0;
+		body.m_parent_pos_parent_body(2)=0;
+		
+		body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
+
+		if (type >= POSITION_VELOCITY)
+		{
+			body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
+			body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
+			body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
+			body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
+		}
+		if (type >= POSITION_VELOCITY_ACCELERATION)
+		{
+			body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
+			body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
+			body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
+			body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
+		}
+	}
 
 	// 2. absolute kinematic quantities (vector valued)
 	// NOTE: this should be optimized by specializing for different body types
@@ -560,6 +623,12 @@ void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody &body)
 			setMat3xElem(2, idx + 4, body.m_body_T_parent(1, 2), &body.m_body_Jac_T);
 			setMat3xElem(2, idx + 5, body.m_body_T_parent(2, 2), &body.m_body_Jac_T);
 
+			break;
+		case SPHERICAL:
+			//todo: review
+			setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
+			setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
+			setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
 			break;
 	}
 }
@@ -608,6 +677,32 @@ int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &
 }
 #endif
 
+static inline void setThreeDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
+{
+	switch (dof)
+	{
+		// rotational part
+		case 0:
+			Jac_JR(0) = 1;
+			Jac_JR(1) = 0;
+			Jac_JR(2) = 0;
+			setZero(Jac_JT);
+			break;
+		case 1:
+			Jac_JR(0) = 0;
+			Jac_JR(1) = 1;
+			Jac_JR(2) = 0;
+			setZero(Jac_JT);
+			break;
+		case 2:
+			Jac_JR(0) = 0;
+			Jac_JR(1) = 0;
+			Jac_JR(2) = 1;
+			setZero(Jac_JT);
+			break;
+	}
+}
+
 static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
 {
 	switch (dof)
@@ -664,6 +759,8 @@ static inline int jointNumDoFs(const JointType &type)
 			return 1;
 		case FLOATING:
 			return 6;
+		case SPHERICAL:
+			return 3;
 	}
 	// this should never happen
 	bt_id_error_message("invalid joint type\n");
@@ -743,6 +840,25 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 
 			body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
 		}
+
+		for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
+		{
+			//todo: review
+			RigidBody &body = m_body_list[m_body_spherical_list[i]];
+
+			mat33 T;
+
+			T = transformX(q(body.m_q_index)) *
+				transformY(q(body.m_q_index + 1)) *
+				transformZ(q(body.m_q_index + 2));
+			body.m_body_T_parent = T * body.m_body_T_parent_ref;
+
+			body.m_parent_pos_parent_body(0)=0;
+			body.m_parent_pos_parent_body(1)=0;
+			body.m_parent_pos_parent_body(2)=0;
+			
+			body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
+		}
 	}
 	for (int i = m_body_list.size() - 1; i >= 0; i--)
 	{
@@ -798,6 +914,11 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 			{
 				setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
 			}
+			if (SPHERICAL == body.m_joint_type)
+			{
+				//todo: review
+				setThreeDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
+			}
 
 			vec3 body_eom_rot =
 				body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
@@ -810,14 +931,19 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 				// 1. for multi-dof joints, rest of the dofs of this body
 				for (int row = col - 1; row >= q_index_min; row--)
 				{
-					if (FLOATING != body.m_joint_type)
+					if (SPHERICAL == body.m_joint_type)
+					{
+						//todo: review
+						setThreeDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
+						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
+						setMatxxElem(col, row, Mrc, mass_matrix);
+					}
+					if (FLOATING == body.m_joint_type)
 					{
-						bt_id_error_message("??\n");
-						return -1;
+						setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
+						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
+						setMatxxElem(col, row, Mrc, mass_matrix);
 					}
-					setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
-					const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
-					setMatxxElem(col, row, Mrc, mass_matrix);
 				}
 				// 2. ancestor dofs
 				int child_idx = i;
@@ -839,6 +965,11 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 					vec3 Jac_JT = parent_body.m_Jac_JT;
 					for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--)
 					{
+						if (SPHERICAL == parent_body.m_joint_type)
+						{
+							//todo: review
+							setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
+						}
 						// set jacobians for 6-DoF joints
 						if (FLOATING == parent_body.m_joint_type)
 						{

3rdparty/bullet3/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp

@@ -274,6 +274,8 @@ private:
 	idArray<int>::type m_body_prismatic_list;
 	// Indices of floating joints
 	idArray<int>::type m_body_floating_list;
+	// Indices of spherical joints
+	idArray<int>::type m_body_spherical_list;
 	// a user-provided integer
 	idArray<int>::type m_user_int;
 	// a user-provided pointer

3rdparty/bullet3/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp

@@ -28,6 +28,9 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
 			// does not add a degree of freedom
 			// m_num_dofs+=0;
 			break;
+		case SPHERICAL:
+			m_num_dofs += 3;
+			break;
 		case FLOATING:
 			m_num_dofs += 6;
 			break;

3rdparty/bullet3/src/BulletSoftBody/btSparseSDF.h

@@ -139,7 +139,7 @@ struct btSparseSdf
 		nqueries = 1;
 		nprobes = 1;
 		++puid;  ///@todo: Reset puid's when int range limit is reached	*/
-		/* else setup a priority list...						*/
+				 /* else setup a priority list...						*/
 	}
 	//
 	int RemoveReferences(btCollisionShape* pcs)

3rdparty/bullet3/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp

@@ -1,3 +1,4 @@
+
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2018 Erwin Coumans  http://bulletphysics.com
@@ -72,7 +73,7 @@ public:
 		pthread_t thread;
 		//each tread will wait until this signal to start its work
 		sem_t* startSemaphore;
-
+		btCriticalSection* m_cs;
 		// this is a copy of m_mainSemaphore,
 		//each tread will signal once it is finished with its work
 		sem_t* m_mainSemaphore;
@@ -90,7 +91,7 @@ private:
 	void startThreads(const ConstructionInfo& threadInfo);
 	void stopThreads();
 	int waitForResponse();
-
+	btCriticalSection* m_cs;
 public:
 	btThreadSupportPosix(const ConstructionInfo& threadConstructionInfo);
 	virtual ~btThreadSupportPosix();
@@ -119,6 +120,7 @@ public:
 
 btThreadSupportPosix::btThreadSupportPosix(const ConstructionInfo& threadConstructionInfo)
 {
+	m_cs = createCriticalSection();
 	startThreads(threadConstructionInfo);
 }
 
@@ -126,6 +128,8 @@ btThreadSupportPosix::btThreadSupportPosix(const ConstructionInfo& threadConstru
 btThreadSupportPosix::~btThreadSupportPosix()
 {
 	stopThreads();
+	deleteCriticalSection(m_cs);
+	m_cs=0;
 }
 
 #if (defined(__APPLE__))
@@ -181,21 +185,23 @@ static void* threadFunction(void* argument)
 		{
 			btAssert(status->m_status);
 			status->m_userThreadFunc(userPtr);
+			status->m_cs->lock();
 			status->m_status = 2;
+			status->m_cs->unlock();
 			checkPThreadFunction(sem_post(status->m_mainSemaphore));
 			status->threadUsed++;
 		}
 		else
 		{
 			//exit Thread
+			status->m_cs->lock();
 			status->m_status = 3;
+			status->m_cs->unlock();
 			checkPThreadFunction(sem_post(status->m_mainSemaphore));
-			printf("Thread with taskId %i exiting\n", status->m_taskId);
 			break;
 		}
 	}
 
-	printf("Thread TERMINATED\n");
 	return 0;
 }
 
@@ -206,7 +212,7 @@ void btThreadSupportPosix::runTask(int threadIndex, void* userData)
 	btThreadStatus& threadStatus = m_activeThreadStatus[threadIndex];
 	btAssert(threadIndex >= 0);
 	btAssert(threadIndex < m_activeThreadStatus.size());
-
+	threadStatus.m_cs = m_cs;
 	threadStatus.m_commandId = 1;
 	threadStatus.m_status = 1;
 	threadStatus.m_userPtr = userData;
@@ -231,7 +237,10 @@ int btThreadSupportPosix::waitForResponse()
 
 	for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t)
 	{
-		if (2 == m_activeThreadStatus[t].m_status)
+		m_cs->lock();
+		bool hasFinished = (2 == m_activeThreadStatus[t].m_status);
+		m_cs->unlock(); 
+		if (hasFinished)
 		{
 			last = t;
 			break;
@@ -261,7 +270,6 @@ void btThreadSupportPosix::waitForAllTasks()
 void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructionInfo)
 {
 	m_numThreads = btGetNumHardwareThreads() - 1;  // main thread exists already
-	printf("%s creating %i threads.\n", __FUNCTION__, m_numThreads);
 	m_activeThreadStatus.resize(m_numThreads);
 	m_startedThreadsMask = 0;
 
@@ -270,20 +278,18 @@ void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructi
 
 	for (int i = 0; i < m_numThreads; i++)
 	{
-		printf("starting thread %d\n", i);
 		btThreadStatus& threadStatus = m_activeThreadStatus[i];
 		threadStatus.startSemaphore = createSem("threadLocal");
-		checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus));
-
 		threadStatus.m_userPtr = 0;
+		threadStatus.m_cs = m_cs;
 		threadStatus.m_taskId = i;
 		threadStatus.m_commandId = 0;
 		threadStatus.m_status = 0;
 		threadStatus.m_mainSemaphore = m_mainSemaphore;
 		threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
 		threadStatus.threadUsed = 0;
+		checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus));
 
-		printf("started thread %d \n", i);
 	}
 }
 
@@ -293,20 +299,15 @@ void btThreadSupportPosix::stopThreads()
 	for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t)
 	{
 		btThreadStatus& threadStatus = m_activeThreadStatus[t];
-		printf("%s: Thread %i used: %ld\n", __FUNCTION__, int(t), threadStatus.threadUsed);
 
 		threadStatus.m_userPtr = 0;
 		checkPThreadFunction(sem_post(threadStatus.startSemaphore));
 		checkPThreadFunction(sem_wait(m_mainSemaphore));
 
-		printf("destroy semaphore\n");
 		destroySem(threadStatus.startSemaphore);
-		printf("semaphore destroyed\n");
 		checkPThreadFunction(pthread_join(threadStatus.thread, 0));
 	}
-	printf("destroy main semaphore\n");
 	destroySem(m_mainSemaphore);
-	printf("main semaphore destroyed\n");
 	m_activeThreadStatus.clear();
 }
 

3rdparty/bullet3/src/LinearMath/btAlignedObjectArray.h

@@ -38,13 +38,6 @@ subject to the following restrictions:
 #include <new>  //for placement new
 #endif          //BT_USE_PLACEMENT_NEW
 
-// The register keyword is deprecated in C++11 so don't use it.
-#if __cplusplus > 199711L
-#define BT_REGISTER
-#else
-#define BT_REGISTER register
-#endif
-
 ///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods
 ///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
 template <typename T>
@@ -209,7 +202,7 @@ public:
 
 	SIMD_FORCE_INLINE void resize(int newsize, const T& fillData = T())
 	{
-		const BT_REGISTER int curSize = size();
+		const int curSize = size();
 
 		if (newsize < curSize)
 		{
@@ -236,7 +229,7 @@ public:
 	}
 	SIMD_FORCE_INLINE T& expandNonInitializing()
 	{
-		const BT_REGISTER int sz = size();
+		const int sz = size();
 		if (sz == capacity())
 		{
 			reserve(allocSize(size()));
@@ -248,7 +241,7 @@ public:
 
 	SIMD_FORCE_INLINE T& expand(const T& fillValue = T())
 	{
-		const BT_REGISTER int sz = size();
+		const int sz = size();
 		if (sz == capacity())
 		{
 			reserve(allocSize(size()));
@@ -263,7 +256,7 @@ public:
 
 	SIMD_FORCE_INLINE void push_back(const T& _Val)
 	{
-		const BT_REGISTER int sz = size();
+		const int sz = size();
 		if (sz == capacity())
 		{
 			reserve(allocSize(size()));

3rdparty/bullet3/src/LinearMath/btConvexHullComputer.cpp

@@ -926,7 +926,7 @@ int btConvexHullInternal::Rational64::compare(const Rational64& b) const
 		"decb %%bh\n\t"        // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
 		"shll $16, %%ebx\n\t"  // ebx has same sign as difference
 		: "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
-		: "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
+		: "a"(m_denominator), [bn] "g"(b.m_numerator), [tn] "g"(m_numerator), [bd] "g"(b.m_denominator)
 		: "%rdx", "cc");
 	return result ? result ^ sign  // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
 								   // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)

3rdparty/bullet3/src/LinearMath/btMatrixX.h

@@ -263,7 +263,10 @@ struct btMatrixX
 	{
 		{
 			BT_PROFILE("storage=0");
-			btSetZero(&m_storage[0], m_storage.size());
+			if (m_storage.size())
+			{
+				btSetZero(&m_storage[0], m_storage.size());
+			}
 			//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
 			//for (int i=0;i<m_storage.size();i++)
 			//			m_storage[i]=0;

3rdparty/bullet3/src/LinearMath/btQuickprof.cpp

@@ -694,6 +694,24 @@ void CProfileManager::dumpAll()
 	CProfileManager::Release_Iterator(profileIterator);
 }
 
+
+void btEnterProfileZoneDefault(const char* name)
+{
+}
+void btLeaveProfileZoneDefault()
+{
+}
+
+#else
+void btEnterProfileZoneDefault(const char* name)
+{
+}
+void btLeaveProfileZoneDefault()
+{
+}
+#endif  //BT_NO_PROFILE
+
+
 // clang-format off
 #if defined(_WIN32) && (defined(__MINGW32__) || defined(__MINGW64__))
   #define BT_HAVE_TLS 1
@@ -743,22 +761,6 @@ unsigned int btQuickprofGetCurrentThreadIndex2()
 #endif  //BT_THREADSAFE
 }
 
-void btEnterProfileZoneDefault(const char* name)
-{
-}
-void btLeaveProfileZoneDefault()
-{
-}
-
-#else
-void btEnterProfileZoneDefault(const char* name)
-{
-}
-void btLeaveProfileZoneDefault()
-{
-}
-#endif  //BT_NO_PROFILE
-
 static btEnterProfileZoneFunc* bts_enterFunc = btEnterProfileZoneDefault;
 static btLeaveProfileZoneFunc* bts_leaveFunc = btLeaveProfileZoneDefault;
 

3rdparty/bullet3/src/LinearMath/btQuickprof.h

@@ -61,18 +61,19 @@ btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc();
 void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc);
 void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc);
 
-#ifndef BT_NO_PROFILE  // FIX redefinition
-//To disable built-in profiling, please comment out next line
-//#define BT_NO_PROFILE 1
+#ifndef BT_ENABLE_PROFILE
+#define BT_NO_PROFILE 1
 #endif  //BT_NO_PROFILE
 
 const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64;
 
-#ifndef BT_NO_PROFILE
 //btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined,
 //otherwise returns thread index in range [0..maxThreads]
 unsigned int btQuickprofGetCurrentThreadIndex2();
 
+#ifndef BT_NO_PROFILE
+
+
 #include <stdio.h>  //@todo remove this, backwards compatibility
 
 #include "btAlignedAllocator.h"

3rdparty/bullet3/src/LinearMath/btScalar.h

@@ -124,7 +124,7 @@ inline int btGetVersion()
 	#ifdef BT_DEBUG
 		#ifdef _MSC_VER
 			#include <stdio.h>
-			#define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak();	}}
+			#define btAssert(x) { if(!(x)){printf("Assert " __FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak();	}}
 		#else//_MSC_VER
 			#include <assert.h>
 			#define btAssert assert
@@ -152,7 +152,7 @@ inline int btGetVersion()
 			#ifdef __SPU__
 				#include <spu_printf.h>
 				#define printf spu_printf
-				#define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
+				#define btAssert(x) {if(!(x)){printf("Assert " __FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
 			#else
 				#define btAssert assert
 			#endif

3rdparty/bullet3/src/LinearMath/btThreads.cpp

@@ -241,7 +241,7 @@ struct ThreadsafeCounter
 	}
 };
 
-static btITaskScheduler* gBtTaskScheduler;
+static btITaskScheduler* gBtTaskScheduler=0;
 static int gThreadsRunningCounter = 0;  // useful for detecting if we are trying to do nested parallel-for calls
 static btSpinMutex gThreadsRunningCounterMutex;
 static ThreadsafeCounter gThreadCounter;

3rdparty/bullet3/src/btBulletCollisionAll.cpp

@@ -0,0 +1,96 @@
+#include "BulletCollision/BroadphaseCollision/btAxisSweep3.cpp"
+#include "BulletCollision/BroadphaseCollision/btDbvt.cpp"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp"
+#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp"
+#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.cpp"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp"
+#include "BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp"
+#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp"
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp"
+#include "BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.cpp"
+#include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp"
+#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp"
+#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp"
+#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.cpp"
+#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp"
+#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.cpp"
+#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp"
+#include "BulletCollision/CollisionDispatch/btUnionFind.cpp"
+#include "BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp"
+#include "BulletCollision/CollisionDispatch/btGhostObject.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btConvexCast.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp"
+#include "BulletCollision/CollisionShapes/btBox2dShape.cpp"
+#include "BulletCollision/CollisionShapes/btConvexPolyhedron.cpp"
+#include "BulletCollision/CollisionShapes/btShapeHull.cpp"
+#include "BulletCollision/CollisionShapes/btBoxShape.cpp"
+#include "BulletCollision/CollisionShapes/btConvexShape.cpp"
+#include "BulletCollision/CollisionShapes/btSphereShape.cpp"
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.cpp"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.cpp"
+#include "BulletCollision/CollisionShapes/btCylinderShape.cpp"
+#include "BulletCollision/CollisionShapes/btStridingMeshInterface.cpp"
+#include "BulletCollision/CollisionShapes/btCollisionShape.cpp"
+#include "BulletCollision/CollisionShapes/btEmptyShape.cpp"
+#include "BulletCollision/CollisionShapes/btTetrahedronShape.cpp"
+#include "BulletCollision/CollisionShapes/btCompoundShape.cpp"
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp"
+#include "BulletCollision/CollisionShapes/btTriangleBuffer.cpp"
+#include "BulletCollision/CollisionShapes/btConcaveShape.cpp"
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.cpp"
+#include "BulletCollision/CollisionShapes/btConeShape.cpp"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.cpp"
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp"
+#include "BulletCollision/CollisionShapes/btConvex2dShape.cpp"
+#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp"
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.cpp"
+#include "BulletCollision/CollisionShapes/btOptimizedBvh.cpp"
+#include "BulletCollision/CollisionShapes/btTriangleMesh.cpp"
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.cpp"
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.cpp"
+#include "BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp"
+#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp"
+#include "BulletCollision/CollisionShapes/btSdfCollisionShape.cpp"
+#include "BulletCollision/CollisionShapes/btMiniSDF.cpp"
+#include "BulletCollision/CollisionShapes/btUniformScalingShape.cpp"
+#include "BulletCollision/Gimpact/btContactProcessing.cpp"
+#include "BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp"
+#include "BulletCollision/Gimpact/btTriangleShapeEx.cpp"
+#include "BulletCollision/Gimpact/gim_memory.cpp"
+#include "BulletCollision/Gimpact/btGImpactBvh.cpp"
+#include "BulletCollision/Gimpact/btGImpactShape.cpp"
+#include "BulletCollision/Gimpact/gim_box_set.cpp"
+#include "BulletCollision/Gimpact/gim_tri_collision.cpp"
+#include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp"
+#include "BulletCollision/Gimpact/btGenericPoolAllocator.cpp"
+#include "BulletCollision/Gimpact/gim_contact.cpp"

3rdparty/bullet3/src/btBulletDynamicsAll.cpp

@@ -0,0 +1,42 @@
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp"
+#include "BulletDynamics/Dynamics/btRigidBody.cpp"
+#include "BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp"
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp"
+#include "BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp"
+#include "BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btSliderConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btFixedConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btHingeConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btGearConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp"
+#include "BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp"
+#include "BulletDynamics/MLCPSolvers/btDantzigLCP.cpp"
+#include "BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolver.cpp"
+#include "BulletDynamics/Featherstone/btMultiBody.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp"
+#include "BulletDynamics/Vehicle/btRaycastVehicle.cpp"
+#include "BulletDynamics/Vehicle/btWheelInfo.cpp"
+#include "BulletDynamics/Character/btKinematicCharacterController.cpp"
+

3rdparty/bullet3/src/btLinearMathAll.cpp

@@ -0,0 +1,14 @@
+#include "LinearMath/btAlignedAllocator.cpp"
+#include "LinearMath/btGeometryUtil.cpp"
+#include "LinearMath/btSerializer.cpp"
+#include "LinearMath/btVector3.cpp"
+#include "LinearMath/btConvexHull.cpp"
+#include "LinearMath/btPolarDecomposition.cpp"
+#include "LinearMath/btSerializer64.cpp"
+#include "LinearMath/btConvexHullComputer.cpp"
+#include "LinearMath/btQuickprof.cpp"
+#include "LinearMath/btThreads.cpp"
+#include "LinearMath/TaskScheduler/btTaskScheduler.cpp"
+#include "LinearMath/TaskScheduler/btThreadSupportPosix.cpp"
+#include "LinearMath/TaskScheduler/btThreadSupportWin32.cpp"
+