Merge pull request #48300 from akien-mga/3.x-bullet-3.09

modules/bullet/SCsub

@@ -10,7 +10,7 @@ env_bullet = env_modules.Clone()
 thirdparty_obj = []
 
 if env["builtin_bullet"]:
-    # Build only version 2 for now (as of 2.89)
+    # Build only "Bullet2" API (not "Bullet3" folders).
     # Sync file list with relevant upstream CMakeLists.txt for each folder.
     thirdparty_dir = "#thirdparty/bullet/"
 
@@ -177,6 +177,7 @@ if env["builtin_bullet"]:
         "BulletSoftBody/btDeformableContactProjection.cpp",
         "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp",
         "BulletSoftBody/btDeformableContactConstraint.cpp",
+        "BulletSoftBody/poly34.cpp",
         # clew
         "clew/clew.c",
         # LinearMath
@@ -186,6 +187,7 @@ if env["builtin_bullet"]:
         "LinearMath/btGeometryUtil.cpp",
         "LinearMath/btPolarDecomposition.cpp",
         "LinearMath/btQuickprof.cpp",
+        "LinearMath/btReducedVector.cpp",
         "LinearMath/btSerializer.cpp",
         "LinearMath/btSerializer64.cpp",
         "LinearMath/btThreads.cpp",
@@ -197,13 +199,7 @@ if env["builtin_bullet"]:
 
     thirdparty_sources = [thirdparty_dir + file for file in bullet2_src]
 
-    # Treat Bullet headers as system headers to avoid raising warnings. Not supported on MSVC.
-    if not env.msvc:
-        env_bullet.Append(CPPFLAGS=["-isystem", Dir(thirdparty_dir).path])
-    else:
     env_bullet.Prepend(CPPPATH=[thirdparty_dir])
-    # if env['target'] == "debug" or env['target'] == "release_debug":
-    #     env_bullet.Append(CPPDEFINES=['BT_DEBUG'])
 
     env_bullet.Append(CPPDEFINES=["BT_USE_OLD_DAMPING_METHOD"])
 

platform/server/detect.py

@@ -149,15 +149,17 @@ def configure(env):
         env.ParseConfig("pkg-config libpng16 --cflags --libs")
 
     if not env["builtin_bullet"]:
-        # We need at least version 2.89
+        # We need at least version 2.90
+        min_bullet_version = "2.90"
+
         import subprocess
 
         bullet_version = subprocess.check_output(["pkg-config", "bullet", "--modversion"]).strip()
-        if str(bullet_version) < "2.89":
+        if str(bullet_version) < min_bullet_version:
             # Abort as system bullet was requested but too old
             print(
                 "Bullet: System version {0} does not match minimal requirements ({1}). Aborting.".format(
-                    bullet_version, "2.89"
+                    bullet_version, min_bullet_version
                 )
             )
             sys.exit(255)

platform/x11/detect.py

@@ -239,15 +239,17 @@ def configure(env):
         env.ParseConfig("pkg-config libpng16 --cflags --libs")
 
     if not env["builtin_bullet"]:
-        # We need at least version 2.89
+        # We need at least version 2.90
+        min_bullet_version = "2.90"
+
         import subprocess
 
         bullet_version = subprocess.check_output(["pkg-config", "bullet", "--modversion"]).strip()
-        if str(bullet_version) < "2.89":
+        if str(bullet_version) < min_bullet_version:
             # Abort as system bullet was requested but too old
             print(
                 "Bullet: System version {0} does not match minimal requirements ({1}). Aborting.".format(
-                    bullet_version, "2.89"
+                    bullet_version, min_bullet_version
                 )
             )
             sys.exit(255)

thirdparty/README.md

@@ -8,13 +8,15 @@ readability.
 ## bullet
 
 - Upstream: https://github.com/bulletphysics/bullet3
-- Version: 2.89 (830f0a9565b1829a07e21e2f16be2aa9966bd28c, 2019)
+- Version: 3.17 (ebe1916b90acae8b13cd8c6b637d8327cdc64e94, 2021)
 - License: zlib
 
 Files extracted from upstream source:
 
-- src/* apart from CMakeLists.txt and premake4.lua files
-- LICENSE.txt
+- `src/*` apart from CMakeLists.txt and premake4.lua files
+- `LICENSE.txt`, and `VERSION` as `VERSION.txt`
+
+Includes a warning fix which should be upstreamed soon (see patch in `patches`).
 
 
 ## certs

thirdparty/bullet/0001-old-damping-def.patch

@@ -1,34 +0,0 @@
-diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
-index 9e8705b001..f1b50b39c8 100644
---- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
-+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
-@@ -136,8 +136,13 @@ void btRigidBody::setGravity(const btVector3& acceleration)
- 
- void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
- {
--	m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
--	m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-+#ifdef BT_USE_OLD_DAMPING_METHOD
-+	m_linearDamping = btMax(lin_damping, btScalar(0.0));
-+	m_angularDamping = btMax(ang_damping, btScalar(0.0));
-+#else
-+	m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
-+	m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
-+#endif
- }
- 
- ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
-@@ -146,10 +151,9 @@ void btRigidBody::applyDamping(btScalar timeStep)
- 	//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
- 	//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
- 
--//#define USE_OLD_DAMPING_METHOD 1
--#ifdef USE_OLD_DAMPING_METHOD
--	m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
--	m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-+#ifdef BT_USE_OLD_DAMPING_METHOD
-+	m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
-+	m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
- #else
- 	m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
- 	m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);

thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp

@@ -285,7 +285,6 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface, int
 				meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart);
 
 				curNodeSubPart = nodeSubPart;
-				b3Assert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
 			}
 			//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
 
@@ -293,7 +292,13 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface, int
 
 			for (int j = 2; j >= 0; j--)
 			{
-				int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
+				int graphicsindex;
+                                switch (indicestype) {
+                                        case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
+                                        case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
+                                        case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
+                                        default: b3Assert(0);
+                                }
 				if (type == PHY_FLOAT)
 				{
 					float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);

thirdparty/bullet/Bullet3Serialize/Bullet2FileLoader/b3File.cpp

@@ -851,12 +851,12 @@ void bFile::swapData(char *data, short type, int arraySize, bool ignoreEndianFla
 
 void bFile::safeSwapPtr(char *dst, const char *src)
 {
+	if (!src || !dst)
+		return;
+
 	int ptrFile = mFileDNA->getPointerSize();
 	int ptrMem = mMemoryDNA->getPointerSize();
 
-	if (!src && !dst)
-		return;
-
 	if (ptrFile == ptrMem)
 	{
 		memcpy(dst, src, ptrMem);

thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.h

@@ -203,8 +203,8 @@ struct btDbvntNode
 
     btDbvntNode(const btDbvtNode* n)
     : volume(n->volume)
-    , angle(0)
     , normal(0,0,0)
+    , angle(0)
     , data(n->data)
     {
         childs[0] = 0;

thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

@@ -61,7 +61,8 @@ public:
 	virtual void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) = 0;
 
 	virtual int getNumOverlappingPairs() const = 0;
-
+	virtual bool needsBroadphaseCollision(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) const = 0;
+	virtual btOverlapFilterCallback* getOverlapFilterCallback() = 0;
 	virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0;
 
 	virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0;
@@ -380,6 +381,14 @@ public:
 	{
 	}
 
+	bool needsBroadphaseCollision(btBroadphaseProxy*, btBroadphaseProxy*) const
+	{
+		return true;
+	}
+	btOverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return 0;
+	}
 	virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/)
 	{
 	}

thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

@@ -346,8 +346,6 @@ void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb
 	}
 }
 
-int maxIterations = 0;
-
 void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
 	btAssert(!m_useQuantization);
@@ -387,8 +385,6 @@ void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, cons
 			curIndex += escapeIndex;
 		}
 	}
-	if (maxIterations < walkIterations)
-		maxIterations = walkIterations;
 }
 
 /*
@@ -468,7 +464,7 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall
 
 #ifdef RAYAABB2
 	btVector3 rayDir = (rayTarget - raySource);
-	rayDir.normalize();
+	rayDir.safeNormalize();// stephengold changed normalize to safeNormalize 2020-02-17
 	lambda_max = rayDir.dot(rayTarget - raySource);
 	///what about division by zero? --> just set rayDirection[i] to 1.0
 	btVector3 rayDirectionInverse;
@@ -529,8 +525,6 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall
 			curIndex += escapeIndex;
 		}
 	}
-	if (maxIterations < walkIterations)
-		maxIterations = walkIterations;
 }
 
 void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const
@@ -554,7 +548,7 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
 
 #ifdef RAYAABB2
 	btVector3 rayDirection = (rayTarget - raySource);
-	rayDirection.normalize();
+	rayDirection.safeNormalize();// stephengold changed normalize to safeNormalize 2020-02-17
 	lambda_max = rayDirection.dot(rayTarget - raySource);
 	///what about division by zero? --> just set rayDirection[i] to 1.0
 	rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0];
@@ -654,8 +648,6 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
 			curIndex += escapeIndex;
 		}
 	}
-	if (maxIterations < walkIterations)
-		maxIterations = walkIterations;
 }
 
 void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const
@@ -718,8 +710,6 @@ void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb
 			curIndex += escapeIndex;
 		}
 	}
-	if (maxIterations < walkIterations)
-		maxIterations = walkIterations;
 }
 
 //This traversal can be called from Playstation 3 SPU

thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

@@ -46,8 +46,6 @@ protected:
 
 	btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr;
 
-	btManifoldResult m_defaultManifoldResult;
-
 	btNearCallback m_nearCallback;
 
 	btPoolAllocator* m_collisionAlgorithmPoolAllocator;
@@ -95,11 +93,15 @@ public:
 
 	btPersistentManifold* getManifoldByIndexInternal(int index)
 	{
+		btAssert(index>=0);
+		btAssert(index<m_manifoldsPtr.size());
 		return m_manifoldsPtr[index];
 	}
 
 	const btPersistentManifold* getManifoldByIndexInternal(int index) const
 	{
+		btAssert(index>=0);
+		btAssert(index<m_manifoldsPtr.size());
 		return m_manifoldsPtr[index];
 	}
 

thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp

@@ -28,6 +28,7 @@ subject to the following restrictions:
 btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize)
 	: btCollisionDispatcher(config)
 {
+	m_batchManifoldsPtr.resize(btGetTaskScheduler()->getNumThreads());
 	m_batchUpdating = false;
 	m_grainSize = grainSize;  // iterations per task
 }
@@ -65,6 +66,10 @@ btPersistentManifold* btCollisionDispatcherMt::getNewManifold(const btCollisionO
 		manifold->m_index1a = m_manifoldsPtr.size();
 		m_manifoldsPtr.push_back(manifold);
 	}
+	else
+	{
+		m_batchManifoldsPtr[btGetCurrentThreadIndex()].push_back(manifold);
+	}
 
 	return manifold;
 }
@@ -121,7 +126,7 @@ struct CollisionDispatcherUpdater : public btIParallelForBody
 
 void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher)
 {
-	int pairCount = pairCache->getNumOverlappingPairs();
+	const int pairCount = pairCache->getNumOverlappingPairs();
 	if (pairCount == 0)
 	{
 		return;
@@ -136,16 +141,17 @@ void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache*
 	btParallelFor(0, pairCount, m_grainSize, updater);
 	m_batchUpdating = false;
 
-	// reconstruct the manifolds array to ensure determinism
-	m_manifoldsPtr.resizeNoInitialize(0);
+	// merge new manifolds, if any
+	for (int i = 0; i < m_batchManifoldsPtr.size(); ++i)
+	{
+		btAlignedObjectArray<btPersistentManifold*>& batchManifoldsPtr = m_batchManifoldsPtr[i];
 
-	btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr();
-	for (int i = 0; i < pairCount; ++i)
+		for (int j = 0; j < batchManifoldsPtr.size(); ++j)
 		{
-		if (btCollisionAlgorithm* algo = pairs[i].m_algorithm)
-		{
-			algo->getAllContactManifolds(m_manifoldsPtr);
+			m_manifoldsPtr.push_back(batchManifoldsPtr[j]);
 		}
+
+		batchManifoldsPtr.resizeNoInitialize(0);
 	}
 
 	// update the indices (used when releasing manifolds)

thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h

@@ -30,6 +30,7 @@ public:
 	virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE;
 
 protected:
+	btAlignedObjectArray<btAlignedObjectArray<btPersistentManifold*> > m_batchManifoldsPtr;
 	bool m_batchUpdating;
 	int m_grainSize;
 };

thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -24,6 +24,7 @@ subject to the following restrictions:
 #define WANTS_DEACTIVATION 3
 #define DISABLE_DEACTIVATION 4
 #define DISABLE_SIMULATION 5
+#define FIXED_BASE_MULTI_BODY 6
 
 struct btBroadphaseProxy;
 class btCollisionShape;
@@ -127,6 +128,7 @@ public:
 
 	enum CollisionFlags
 	{
+		CF_DYNAMIC_OBJECT = 0,
 		CF_STATIC_OBJECT = 1,
 		CF_KINEMATIC_OBJECT = 2,
 		CF_NO_CONTACT_RESPONSE = 4,
@@ -251,6 +253,16 @@ public:
 		m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0;
 	}
 
+        int getNumObjectsWithoutCollision() const
+	{
+		return m_objectsWithoutCollisionCheck.size();
+	}
+
+	const btCollisionObject* getObjectWithoutCollision(int index)
+	{
+		return m_objectsWithoutCollisionCheck[index];
+	}
+
 	virtual bool checkCollideWithOverride(const btCollisionObject* co) const
 	{
 		int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
@@ -293,7 +305,7 @@ public:
 
 	SIMD_FORCE_INLINE bool isActive() const
 	{
-		return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
+		return ((getActivationState() != FIXED_BASE_MULTI_BODY) && (getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
 	}
 
 	void setRestitution(btScalar rest)

thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -1037,7 +1037,7 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback
 		  m_castShape(castShape)
 	{
 		btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin() - m_convexFromTrans.getOrigin());
-		btVector3 rayDir = unnormalizedRayDir.normalized();
+		btVector3 rayDir = unnormalizedRayDir.fuzzyZero() ? btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0)) : unnormalizedRayDir.normalized();
 		///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
 		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
 		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
@@ -1294,9 +1294,7 @@ public:
 			btVector3 normalColor(1, 1, 0);
 			m_debugDrawer->drawLine(center, center + normal, normalColor);
 		}
-		m_debugDrawer->drawLine(wv0, wv1, m_color);
-		m_debugDrawer->drawLine(wv1, wv2, m_color);
-		m_debugDrawer->drawLine(wv2, wv0, m_color);
+		m_debugDrawer->drawTriangle(wv0, wv1, wv2, m_color, 1.0);
 	}
 };
 

thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -139,7 +139,12 @@ public:
 
 		if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 		{
-			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, childTrans, -1, index);
+			btTransform preTransform = childTrans;
+			if (this->m_compoundColObjWrap->m_preTransform)
+			{
+				preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform));
+			}
+			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index);
 
 			btCollisionAlgorithm* algo = 0;
 			bool allocatedAlgorithm = false;

thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp

@@ -361,7 +361,13 @@ void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangle
 
 			for (int j = 2; j >= 0; j--)
 			{
-				int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
+				int graphicsindex;
+                                switch (indicestype) {
+                                        case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
+                                        case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
+                                        case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
+                                        default: btAssert(0);
+                                }
 				if (type == PHY_FLOAT)
 				{
 					float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);

thirdparty/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp

@@ -124,12 +124,17 @@ void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const
 				nodeSubPart);
 
 			unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
-			btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
 
 			const btVector3& meshScaling = m_meshInterface->getScaling();
 			for (int j = 2; j >= 0; j--)
 			{
-				int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
+				int graphicsindex;
+                                switch (indicestype) {
+                                        case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
+                                        case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
+                                        case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
+                                        default: btAssert(0);
+                                }
 
 				if (type == PHY_FLOAT)
 				{
@@ -193,12 +198,17 @@ void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, con
 				nodeSubPart);
 
 			unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
-			btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
 
 			const btVector3& meshScaling = m_meshInterface->getScaling();
 			for (int j = 2; j >= 0; j--)
 			{
-				int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
+				int graphicsindex;
+                                switch (indicestype) {
+                                        case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
+                                        case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
+                                        case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
+                                        default: btAssert(0);
+                                }
 
 				if (type == PHY_FLOAT)
 				{

thirdparty/bullet/BulletCollision/CollisionShapes/btCollisionShape.h

@@ -30,11 +30,12 @@ protected:
 	int m_shapeType;
 	void* m_userPointer;
 	int m_userIndex;
+	int m_userIndex2;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1)
+	btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1), m_userIndex2(-1)
 	{
 	}
 
@@ -137,6 +138,16 @@ public:
 		return m_userIndex;
 	}
 
+	void setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
+
+	int getUserIndex2() const
+	{
+		return m_userIndex2;
+	}
+
 	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)

thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

@@ -17,27 +17,73 @@ subject to the following restrictions:
 
 #include "LinearMath/btTransformUtil.h"
 
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(
+	int heightStickWidth, int heightStickLength,
+	const float* heightfieldData, btScalar minHeight, btScalar maxHeight,
+	int upAxis, bool flipQuadEdges)
+	: m_userValue3(0), m_triangleInfoMap(0)
+{
+	initialize(heightStickWidth, heightStickLength, heightfieldData,
+			   /*heightScale=*/1, minHeight, maxHeight, upAxis, PHY_FLOAT,
+			   flipQuadEdges);
+}
+
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(
+	int heightStickWidth, int heightStickLength, const double* heightfieldData,
+	btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges)
+	: m_userValue3(0), m_triangleInfoMap(0)
+{
+	initialize(heightStickWidth, heightStickLength, heightfieldData,
+			   /*heightScale=*/1, minHeight, maxHeight, upAxis, PHY_DOUBLE,
+			   flipQuadEdges);
+}
+
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(
+	int heightStickWidth, int heightStickLength, const short* heightfieldData, btScalar heightScale,
+	btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges)
+	: m_userValue3(0), m_triangleInfoMap(0)
+{
+	initialize(heightStickWidth, heightStickLength, heightfieldData,
+			   heightScale, minHeight, maxHeight, upAxis, PHY_SHORT,
+			   flipQuadEdges);
+}
+
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(
+	int heightStickWidth, int heightStickLength, const unsigned char* heightfieldData, btScalar heightScale,
+	btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges)
+	: m_userValue3(0), m_triangleInfoMap(0)
+{
+	initialize(heightStickWidth, heightStickLength, heightfieldData,
+			   heightScale, minHeight, maxHeight, upAxis, PHY_UCHAR,
+			   flipQuadEdges);
+}
+
 btHeightfieldTerrainShape::btHeightfieldTerrainShape(
 	int heightStickWidth, int heightStickLength, const void* heightfieldData,
 	btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
 	PHY_ScalarType hdt, bool flipQuadEdges)
-	:m_userIndex2(-1),
-	m_userValue3(0),
+	:m_userValue3(0),
 	m_triangleInfoMap(0)
 {
+	// legacy constructor: Assumes PHY_FLOAT means btScalar.
+#ifdef BT_USE_DOUBLE_PRECISION
+	if (hdt == PHY_FLOAT) hdt = PHY_DOUBLE;
+#endif
 	initialize(heightStickWidth, heightStickLength, heightfieldData,
 			   heightScale, minHeight, maxHeight, upAxis, hdt,
 			   flipQuadEdges);
 }
 
 btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges)
-	:m_userIndex2(-1),
-	m_userValue3(0),
+	:	m_userValue3(0),
 	m_triangleInfoMap(0)
 {
-	// legacy constructor: support only float or unsigned char,
-	// 	and min height is zero
+	// legacy constructor: support only btScalar or unsigned char data,
+	// and min height is zero.
 	PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR;
+#ifdef BT_USE_DOUBLE_PRECISION
+	if (hdt == PHY_FLOAT) hdt = PHY_DOUBLE;
+#endif
 	btScalar minHeight = 0.0f;
 
 	// previously, height = uchar * maxHeight / 65535.
@@ -61,7 +107,7 @@ void btHeightfieldTerrainShape::initialize(
 	// btAssert(heightScale) -- do we care?  Trust caller here
 	btAssert(minHeight <= maxHeight);                                    // && "bad min/max height");
 	btAssert(upAxis >= 0 && upAxis < 3);                                 // && "bad upAxis--should be in range [0,2]");
-	btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);  // && "Bad height data type enum");
+	btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_DOUBLE || hdt != PHY_SHORT);  // && "Bad height data type enum");
 
 	// initialize member variables
 	m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
@@ -154,6 +200,12 @@ btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const
 			break;
 		}
 
+		case PHY_DOUBLE:
+		{
+			val = m_heightfieldDataDouble[(y * m_heightStickWidth) + x];
+			break;
+		}
+
 		case PHY_UCHAR:
 		{
 			unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x];
@@ -234,6 +286,30 @@ getQuantized(
 	return (int)(x + 0.5);
 }
 
+// Equivalent to std::minmax({a, b, c}).
+// Performs at most 3 comparisons.
+static btHeightfieldTerrainShape::Range minmaxRange(btScalar a, btScalar b, btScalar c)
+{
+	if (a > b)
+	{
+		if (b > c)
+			return btHeightfieldTerrainShape::Range(c, a);
+		else if (a > c)
+			return btHeightfieldTerrainShape::Range(b, a);
+		else
+			return btHeightfieldTerrainShape::Range(b, c);
+	}
+	else
+	{
+		if (a > c)
+			return btHeightfieldTerrainShape::Range(c, b);
+		else if (b > c)
+			return btHeightfieldTerrainShape::Range(a, b);
+		else
+			return btHeightfieldTerrainShape::Range(a, c);
+	}
+}
+
 /// given input vector, return quantized version
 /**
   This routine is basically determining the gridpoint indices for a given
@@ -337,6 +413,7 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 
 	// TODO If m_vboundsGrid is available, use it to determine if we really need to process this area
 	
+	const Range aabbUpRange(aabbMin[m_upAxis], aabbMax[m_upAxis]);
 	for (int j = startJ; j < endJ; j++)
 	{
 		for (int x = startX; x < endX; x++)
@@ -351,28 +428,50 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 
 			if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1)))
 			{
-				//first triangle
 				getVertex(x, j, vertices[indices[0]]);
 				getVertex(x, j + 1, vertices[indices[1]]);
 				getVertex(x + 1, j + 1, vertices[indices[2]]);
+
+				// Skip triangle processing if the triangle is out-of-AABB.
+				Range upRange = minmaxRange(vertices[0][m_upAxis], vertices[1][m_upAxis], vertices[2][m_upAxis]);
+
+				if (upRange.overlaps(aabbUpRange))
 					callback->processTriangle(vertices, 2 * x, j);
-				//second triangle
-				//  getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
-				getVertex(x + 1, j + 1, vertices[indices[1]]);
+			
+				// already set: getVertex(x, j, vertices[indices[0]])
+
+				// equivalent to: getVertex(x + 1, j + 1, vertices[indices[1]]);
+				vertices[indices[1]] = vertices[indices[2]];
+
 				getVertex(x + 1, j, vertices[indices[2]]);
+				upRange.min = btMin(upRange.min, vertices[indices[2]][m_upAxis]);
+				upRange.max = btMax(upRange.max, vertices[indices[2]][m_upAxis]);
+
+				if (upRange.overlaps(aabbUpRange))
 					callback->processTriangle(vertices, 2 * x + 1, j);
 			}
 			else
 			{
-				//first triangle
 				getVertex(x, j, vertices[indices[0]]);
 				getVertex(x, j + 1, vertices[indices[1]]);
 				getVertex(x + 1, j, vertices[indices[2]]);
+
+				// Skip triangle processing if the triangle is out-of-AABB.
+				Range upRange = minmaxRange(vertices[0][m_upAxis], vertices[1][m_upAxis], vertices[2][m_upAxis]);
+
+				if (upRange.overlaps(aabbUpRange))
 					callback->processTriangle(vertices, 2 * x, j);
-				//second triangle
-				getVertex(x + 1, j, vertices[indices[0]]);
-				//getVertex(x,j+1,vertices[1]);
+
+				// already set: getVertex(x, j + 1, vertices[indices[1]]);
+
+				// equivalent to: getVertex(x + 1, j, vertices[indices[0]]);
+				vertices[indices[0]] = vertices[indices[2]];
+
 				getVertex(x + 1, j + 1, vertices[indices[2]]);
+				upRange.min = btMin(upRange.min, vertices[indices[2]][m_upAxis]);
+				upRange.max = btMax(upRange.max, vertices[indices[2]][m_upAxis]);
+
+				if (upRange.overlaps(aabbUpRange))
 					callback->processTriangle(vertices, 2 * x + 1, j);
 			}
 		}

thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h

@@ -50,17 +50,15 @@ subject to the following restrictions:
   The heightfield heights are determined from the data type used for the
   heightfieldData array.  
 
-   - PHY_UCHAR: height at a point is the uchar value at the
+   - unsigned char: height at a point is the uchar value at the
        grid point, multipled by heightScale.  uchar isn't recommended
        because of its inability to deal with negative values, and
        low resolution (8-bit).
 
-   - PHY_SHORT: height at a point is the short int value at that grid
+   - short: height at a point is the short int value at that grid
        point, multipled by heightScale.
 
-   - PHY_FLOAT: height at a point is the float value at that grid
-       point.  heightScale is ignored when using the float heightfield
-       data type.
+   - float or dobule: height at a point is the value at that grid point.
 
   Whatever the caller specifies as minHeight and maxHeight will be honored.
   The class will not inspect the heightfield to discover the actual minimum
@@ -75,6 +73,14 @@ btHeightfieldTerrainShape : public btConcaveShape
 public:
 	struct Range
 	{
+		Range() {}
+		Range(btScalar min, btScalar max) : min(min), max(max) {}
+
+		bool overlaps(const Range& other) const
+		{
+			return !(min > other.max || max < other.min);
+		}
+
 		btScalar min;
 		btScalar max;
 	};
@@ -95,7 +101,8 @@ protected:
 	union {
 		const unsigned char* m_heightfieldDataUnsignedChar;
 		const short* m_heightfieldDataShort;
-		const btScalar* m_heightfieldDataFloat;
+		const float* m_heightfieldDataFloat;
+		const double* m_heightfieldDataDouble;
 		const void* m_heightfieldDataUnknown;
 	};
 
@@ -114,7 +121,7 @@ protected:
 	int m_vboundsGridLength;
 	int m_vboundsChunkSize;
 
-	int m_userIndex2;
+	
 	btScalar m_userValue3;
 
 	struct btTriangleInfoMap* m_triangleInfoMap;
@@ -135,11 +142,33 @@ protected:
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	/// preferred constructor
+	/// preferred constructors
+	btHeightfieldTerrainShape(
+		int heightStickWidth, int heightStickLength,
+		const float* heightfieldData, btScalar minHeight, btScalar maxHeight,
+		int upAxis, bool flipQuadEdges);
+	btHeightfieldTerrainShape(
+		int heightStickWidth, int heightStickLength,
+		const double* heightfieldData, btScalar minHeight, btScalar maxHeight,
+		int upAxis, bool flipQuadEdges);
+	btHeightfieldTerrainShape(
+		int heightStickWidth, int heightStickLength,
+		const short* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight,
+		int upAxis, bool flipQuadEdges);
+	btHeightfieldTerrainShape(
+		int heightStickWidth, int heightStickLength,
+		const unsigned char* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight,
+		int upAxis, bool flipQuadEdges);
+
+	/// legacy constructor
 	/**
 	  This constructor supports a range of heightfield
 	  data types, and allows for a non-zero minimum height value.
 	  heightScale is needed for any integer-based heightfield data types.
+
+	  This legacy constructor considers `PHY_FLOAT` to mean `btScalar`.
+	  With `BT_USE_DOUBLE_PRECISION`, it will expect `heightfieldData`
+	  to be double-precision.
 	 */
 	btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,
 							  const void* heightfieldData, btScalar heightScale,
@@ -150,7 +179,7 @@ public:
 	/// legacy constructor
 	/**
 	  The legacy constructor assumes the heightfield has a minimum height
-	  of zero.  Only unsigned char or floats are supported.  For legacy
+	  of zero.  Only unsigned char or btScalar data are supported.  For legacy
 	  compatibility reasons, heightScale is calculated as maxHeight / 65535 
 	  (and is only used when useFloatData = false).
  	 */
@@ -192,14 +221,6 @@ public:
 	virtual const char* getName() const { return "HEIGHTFIELD"; }
 
 	
-	void setUserIndex2(int index)
-	{
-		m_userIndex2 = index;
-	}
-	int getUserIndex2() const
-	{
-		return m_userIndex2;
-	}
 	void setUserValue3(btScalar value)
 	{
 		m_userValue3 = value;

thirdparty/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp

@@ -286,7 +286,6 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int
 				meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart);
 
 				curNodeSubPart = nodeSubPart;
-				btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
 			}
 			//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
 
@@ -294,7 +293,13 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int
 
 			for (int j = 2; j >= 0; j--)
 			{
-				int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
+				int graphicsindex;
+                                switch (indicestype) {
+                                        case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
+                                        case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
+                                        case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
+                                        default: btAssert(0);
+                                }
 				if (type == PHY_FLOAT)
 				{
 					float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);

thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp

@@ -2,8 +2,11 @@
 #include "btMiniSDF.h"
 #include "LinearMath/btAabbUtil2.h"
 
-struct btSdfCollisionShapeInternalData
+ATTRIBUTE_ALIGNED16(struct)
+btSdfCollisionShapeInternalData
 {
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btVector3 m_localScaling;
 	btScalar m_margin;
 	btMiniSDF m_sdf;

thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h

@@ -623,13 +623,21 @@ public:
 				i1 = s_indices[1];
 				i2 = s_indices[2];
 			}
-			else
+			else if (indicestype == PHY_INTEGER)
 			{
 				unsigned int* i_indices = (unsigned int*)(indexbase + face_index * indexstride);
 				i0 = i_indices[0];
 				i1 = i_indices[1];
 				i2 = i_indices[2];
 			}
+			else
+			{
+				btAssert(indicestype == PHY_UCHAR);
+				unsigned char* i_indices = (unsigned char*)(indexbase + face_index * indexstride);
+				i0 = i_indices[0];
+				i1 = i_indices[1];
+				i2 = i_indices[2];
+			}
 		}
 
 		SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3& vertex) const

thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp

@@ -1049,7 +1049,8 @@ btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
 		const btScalar length = delta.length();
 		results.normal = delta / length;
 		results.witnesses[0] += results.normal * margin;
-		return (length - margin);
+		results.distance = length - margin;
+		return results.distance;
 	}
 	else
 	{

thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp

@@ -852,7 +852,7 @@ static void setupSpatialGridBatchesMt(
 		memHelper.addChunk((void**)&constraintRowBatchIds, sizeof(int) * numConstraintRows);
 		size_t scratchSize = memHelper.getSizeToAllocate();
 		// if we need to reallocate
-		if (scratchMemory->capacity() < scratchSize)
+		if (static_cast<size_t>(scratchMemory->capacity()) < scratchSize)
 		{
 			// allocate 6.25% extra to avoid repeated reallocs
 			scratchMemory->reserve(scratchSize + scratchSize / 16);

thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -47,6 +47,8 @@ struct btContactSolverInfoData
 	btScalar m_erp;          //error reduction for non-contact constraints
 	btScalar m_erp2;         //error reduction for contact constraints
 	btScalar m_deformable_erp;          //error reduction for deformable constraints
+	btScalar m_deformable_cfm;          //constraint force mixing for deformable constraints
+	btScalar m_deformable_maxErrorReduction; // maxErrorReduction for deformable contact
 	btScalar m_globalCfm;    //constraint force mixing for contacts and non-contacts
 	btScalar m_frictionERP;  //error reduction for friction constraints
 	btScalar m_frictionCFM;  //constraint force mixing for friction constraints
@@ -67,6 +69,7 @@ struct btContactSolverInfoData
 	bool m_jointFeedbackInWorldSpace;
 	bool m_jointFeedbackInJointFrame;
 	int m_reportSolverAnalytics;
+	int m_numNonContactInnerIterations;
 };
 
 struct btContactSolverInfo : public btContactSolverInfoData
@@ -82,7 +85,9 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_numIterations = 10;
 		m_erp = btScalar(0.2);
 		m_erp2 = btScalar(0.2);
-        m_deformable_erp = btScalar(0.);
+		m_deformable_erp = btScalar(0.06);
+		m_deformable_cfm = btScalar(0.01);
+		m_deformable_maxErrorReduction = btScalar(0.1);
 		m_globalCfm = btScalar(0.);
 		m_frictionERP = btScalar(0.2);  //positional friction 'anchors' are disabled by default
 		m_frictionCFM = btScalar(0.);
@@ -104,6 +109,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_jointFeedbackInWorldSpace = false;
 		m_jointFeedbackInJointFrame = false;
 		m_reportSolverAnalytics = 0;
+		m_numNonContactInnerIterations = 1;   // the number of inner iterations for solving motor constraint in a single iteration of the constraint solve
 	}
 };
 

thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp

@@ -876,7 +876,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		// will we not request a velocity with the wrong direction ?
 		// 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
+		if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR)
 			info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
+		else
+			info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
 
 		btScalar minf = f < fd ? f : fd;
 		btScalar maxf = f < fd ? fd : f;

thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

@@ -265,6 +265,7 @@ enum bt6DofFlags2
 	BT_6DOF_FLAGS_ERP_STOP2 = 2,
 	BT_6DOF_FLAGS_CFM_MOTO2 = 4,
 	BT_6DOF_FLAGS_ERP_MOTO2 = 8,
+	BT_6DOF_FLAGS_USE_INFINITE_ERROR = (1<<16)
 };
 #define BT_6DOF_FLAGS_AXIS_SHIFT2 4  // bits per axis
 

thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -14,7 +14,9 @@ subject to the following restrictions:
 */
 
 //#define COMPUTE_IMPULSE_DENOM 1
-//#define BT_ADDITIONAL_DEBUG
+#ifdef BT_DEBUG
+#	define BT_ADDITIONAL_DEBUG
+#endif
 
 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
 
@@ -690,8 +692,10 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 {
 #if BT_THREADSAFE
 	int solverBodyId = -1;
-	bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
-	if (isRigidBodyType && !body.isStaticOrKinematicObject())
+	const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
+	const bool isStaticOrKinematic = body.isStaticOrKinematicObject();
+	const bool isKinematic = body.isKinematicObject();
+	if (isRigidBodyType && !isStaticOrKinematic)
 	{
 		// dynamic body
 		// Dynamic bodies can only be in one island, so it's safe to write to the companionId
@@ -704,7 +708,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 			body.setCompanionId(solverBodyId);
 		}
 	}
-	else if (isRigidBodyType && body.isKinematicObject())
+	else if (isRigidBodyType && isKinematic)
 	{
 		//
 		// NOTE: must test for kinematic before static because some kinematic objects also

thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -800,6 +800,14 @@ public:
 		///don't do CCD when the collision filters are not matching
 		if (!ClosestConvexResultCallback::needsCollision(proxy0))
 			return false;
+		if (m_pairCache->getOverlapFilterCallback()) {
+			btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle();
+			bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1);
+			if (!collides)
+			{
+				return false;
+			}
+		}
 
 		btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
 

thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -384,6 +384,9 @@ void btRigidBody::integrateVelocities(btScalar step)
 	{
 		m_angularVelocity *= (MAX_ANGVEL / step) / angvel;
 	}
+	#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+	clampVelocity(m_angularVelocity);
+	#endif
 }
 
 btQuaternion btRigidBody::getOrientation() const

thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h

@@ -305,6 +305,9 @@ public:
 	void applyTorque(const btVector3& torque)
 	{
 		m_totalTorque += torque * m_angularFactor;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_totalTorque);
+		#endif
 	}
 
 	void applyForce(const btVector3& force, const btVector3& rel_pos)
@@ -316,11 +319,17 @@ public:
 	void applyCentralImpulse(const btVector3& impulse)
 	{
 		m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_linearVelocity);
+		#endif
 	}
 
 	void applyTorqueImpulse(const btVector3& torque)
 	{
 		m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_angularVelocity);
+		#endif
 	}
 
 	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
@@ -347,12 +356,12 @@ public:
         }
     }
     
-    btVector3 getPushVelocity()
+    btVector3 getPushVelocity() const
     {
         return m_pushVelocity;
     }
     
-    btVector3 getTurnVelocity()
+    btVector3 getTurnVelocity() const
     {
         return m_turnVelocity;
     }
@@ -362,19 +371,45 @@ public:
         m_pushVelocity = v;
     }
 
+    #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+    void clampVelocity(btVector3& v) const {
+        v.setX(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
+        );
+        v.setY(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
+        );
+        v.setZ(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
+        );
+    }
+    #endif
+
     void setTurnVelocity(const btVector3& v)
     {
         m_turnVelocity = v;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_turnVelocity);
+        #endif
     }
     
     void applyCentralPushImpulse(const btVector3& impulse)
     {
         m_pushVelocity += impulse * m_linearFactor * m_inverseMass;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_pushVelocity);
+        #endif
     }
     
     void applyTorqueTurnImpulse(const btVector3& torque)
     {
         m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_turnVelocity);
+        #endif
     }
 
 	void clearForces()
@@ -408,12 +443,18 @@ public:
 	{
 		m_updateRevision++;
 		m_linearVelocity = lin_vel;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_linearVelocity);
+		#endif
 	}
 
 	inline void setAngularVelocity(const btVector3& ang_vel)
 	{
 		m_updateRevision++;
 		m_angularVelocity = ang_vel;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_angularVelocity);
+		#endif
 	}
 
 	btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
@@ -425,6 +466,12 @@ public:
 		//		return 	(m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
 	}
     
+    btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const
+    {
+        //we also calculate lin/ang velocity for kinematic objects
+        return m_pushVelocity + m_turnVelocity.cross(rel_pos);
+    }
+
 	void translate(const btVector3& v)
 	{
 		m_worldTransform.getOrigin() += v;

thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp

@@ -171,6 +171,8 @@ void btSimulationIslandManagerMt::initIslandPools()
 
 btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id)
 {
+	btAssert(id >= 0);
+	btAssert(id < m_lookupIslandFromId.size());
 	Island* island = m_lookupIslandFromId[id];
 	if (island == NULL)
 	{

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -33,8 +33,8 @@
 
 namespace
 {
-const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
-const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
+const btScalar INITIAL_SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
+const btScalar INITIAL_SLEEP_TIMEOUT = btScalar(2);     // in seconds
 }  // namespace
 
 void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
@@ -110,6 +110,9 @@ btMultiBody::btMultiBody(int n_links,
 	  m_canSleep(canSleep),
 	  m_canWakeup(true),
 	  m_sleepTimer(0),
+      m_sleepEpsilon(INITIAL_SLEEP_EPSILON),
+	  m_sleepTimeout(INITIAL_SLEEP_TIMEOUT),
+
 	  m_userObjectPointer(0),
 	  m_userIndex2(-1),
 	  m_userIndex(-1),
@@ -125,7 +128,8 @@ btMultiBody::btMultiBody(int n_links,
 	  m_posVarCnt(0),
 	  m_useRK4(false),
 	  m_useGlobalVelocities(false),
-	  m_internalNeedsJointFeedback(false)
+	  m_internalNeedsJointFeedback(false),
+		m_kinematic_calculate_velocity(false)
 {
 	m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
 	m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
@@ -344,6 +348,8 @@ void btMultiBody::finalizeMultiDof()
 {
 	m_deltaV.resize(0);
 	m_deltaV.resize(6 + m_dofCount);
+    m_splitV.resize(0);
+    m_splitV.resize(6 + m_dofCount);
 	m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount);  //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
 	m_vectorBuf.resize(2 * m_dofCount);                                           //two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
 	m_matrixBuf.resize(m_links.size() + 1);
@@ -583,52 +589,6 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
 	}
 }
 
-btScalar btMultiBody::getKineticEnergy() const
-{
-	int num_links = getNumLinks();
-	// TODO: would be better not to allocate memory here
-	btAlignedObjectArray<btVector3> omega;
-	omega.resize(num_links + 1);
-	btAlignedObjectArray<btVector3> vel;
-	vel.resize(num_links + 1);
-	compTreeLinkVelocities(&omega[0], &vel[0]);
-
-	// we will do the factor of 0.5 at the end
-	btScalar result = m_baseMass * vel[0].dot(vel[0]);
-	result += omega[0].dot(m_baseInertia * omega[0]);
-
-	for (int i = 0; i < num_links; ++i)
-	{
-		result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
-		result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]);
-	}
-
-	return 0.5f * result;
-}
-
-btVector3 btMultiBody::getAngularMomentum() const
-{
-	int num_links = getNumLinks();
-	// TODO: would be better not to allocate memory here
-	btAlignedObjectArray<btVector3> omega;
-	omega.resize(num_links + 1);
-	btAlignedObjectArray<btVector3> vel;
-	vel.resize(num_links + 1);
-	btAlignedObjectArray<btQuaternion> rot_from_world;
-	rot_from_world.resize(num_links + 1);
-	compTreeLinkVelocities(&omega[0], &vel[0]);
-
-	rot_from_world[0] = m_baseQuat;
-	btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0]));
-
-	for (int i = 0; i < num_links; ++i)
-	{
-		rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1];
-		result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1])));
-	}
-
-	return result;
-}
 
 void btMultiBody::clearConstraintForces()
 {
@@ -717,6 +677,30 @@ btScalar *btMultiBody::getJointTorqueMultiDof(int i)
 	return &m_links[i].m_jointTorque[0];
 }
 
+bool btMultiBody::hasFixedBase() const
+{
+	return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticObject());
+}
+
+bool btMultiBody::isBaseStaticOrKinematic() const
+{
+	return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticOrKinematicObject());
+}
+
+bool btMultiBody::isBaseKinematic() const
+{
+	return getBaseCollider() && getBaseCollider()->isKinematicObject();
+}
+
+void btMultiBody::setBaseDynamicType(int dynamicType)
+{
+	if(getBaseCollider()) {
+		int oldFlags = getBaseCollider()->getCollisionFlags();
+		oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
+		getBaseCollider()->setCollisionFlags(oldFlags | dynamicType);
+	}
+}
+
 inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)  //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
 {
 	btVector3 row0 = btVector3(
@@ -842,7 +826,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
 	spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
 
-	if (m_fixedBase)
+	if (isBaseStaticOrKinematic())
 	{
 		zeroAccSpatFrc[0].setZero();
 	}
@@ -918,6 +902,11 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 		// calculate zhat_i^A
 		//
+		if (isLinkAndAllAncestorsKinematic(i))
+		{
+			zeroAccSpatFrc[i].setZero();
+		}
+		else{
 			//external forces
 			btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
 			btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
@@ -943,6 +932,23 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			btScalar linDampMult = 1., angDampMult = 1.;
 			zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
 											linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
+			//p += vhat x Ihat vhat - done in a simpler way
+			if (m_useGyroTerm)
+				zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
+			//
+			zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
+			//
+			//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
+			////clamp parent's omega
+			//btScalar parOmegaMod = temp.length();
+			//btScalar parOmegaModMax = 1000;
+			//if(parOmegaMod > parOmegaModMax)
+			//	temp *= parOmegaModMax / parOmegaMod;
+			//zeroAccSpatFrc[i+1].addLinear(temp);
+			//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
+			//temp = spatCoriolisAcc[i].getLinear();
+			//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
+		}
 
 		// calculate Ihat_i^A
 		//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
@@ -955,22 +961,6 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 									 btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
 												 0, m_links[i].m_inertiaLocal[1], 0,
 												 0, 0, m_links[i].m_inertiaLocal[2]));
-		//
-		//p += vhat x Ihat vhat - done in a simpler way
-		if (m_useGyroTerm)
-			zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
-		//
-		zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
-		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
-		////clamp parent's omega
-		//btScalar parOmegaMod = temp.length();
-		//btScalar parOmegaModMax = 1000;
-		//if(parOmegaMod > parOmegaModMax)
-		//	temp *= parOmegaModMax / parOmegaMod;
-		//zeroAccSpatFrc[i+1].addLinear(temp);
-		//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
-		//temp = spatCoriolisAcc[i].getLinear();
-		//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
 
 		//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
 		//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
@@ -981,6 +971,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	// (part of TreeForwardDynamics in Mirtich.)
 	for (int i = num_links - 1; i >= 0; --i)
 	{
+		if(isLinkAndAllAncestorsKinematic(i))
+			continue;
 		const int parent = m_links[i].m_parent;
 		fromParent.m_rotMat = rot_from_parent[i + 1];
 		fromParent.m_trnVec = m_links[i].m_cachedRVector;
@@ -1093,7 +1085,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	// Second 'upward' loop
 	// (part of TreeForwardDynamics in Mirtich)
 
-	if (m_fixedBase)
+	if (isBaseStaticOrKinematic())
 	{
 		spatAcc[0].setZero();
 	}
@@ -1127,13 +1119,14 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
 
+		if(!isLinkAndAllAncestorsKinematic(i))
+		{
 			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 			{
 				const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 				//
 				Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
 			}
-
 			btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
 			//D^{-1} * (Y - h^{T}*apar)
 			mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
@@ -1142,6 +1135,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 				spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
+		}
 
 		if (m_links[i].m_jointFeedback)
 		{
@@ -1420,7 +1414,7 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV
 	}
 }
 
-void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
+void btMultiBody::mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
 {
 	for (int row = 0; row < rowsA; row++)
 	{
@@ -1478,7 +1472,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 
 	// Fill in zero_acc
 	// -- set to force/torque on the base, zero otherwise
-	if (m_fixedBase)
+	if (isBaseStaticOrKinematic())
 	{
 		zeroAccSpatFrc[0].setZero();
 	}
@@ -1497,6 +1491,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	// (part of TreeForwardDynamics in Mirtich.)
 	for (int i = num_links - 1; i >= 0; --i)
 	{
+		if(isLinkAndAllAncestorsKinematic(i))
+			continue;
 		const int parent = m_links[i].m_parent;
 		fromParent.m_rotMat = rot_from_parent[i + 1];
 		fromParent.m_trnVec = m_links[i].m_cachedRVector;
@@ -1540,7 +1536,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	// Second 'upward' loop
 	// (part of TreeForwardDynamics in Mirtich)
 
-	if (m_fixedBase)
+	if (isBaseStaticOrKinematic())
 	{
 		spatAcc[0].setZero();
 	}
@@ -1553,6 +1549,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	// now do the loop over the m_links
 	for (int i = 0; i < num_links; ++i)
 	{
+		if(isLinkAndAllAncestorsKinematic(i))
+			continue;
 		const int parent = m_links[i].m_parent;
 		fromParent.m_rotMat = rot_from_parent[i + 1];
 		fromParent.m_trnVec = m_links[i].m_cachedRVector;
@@ -1596,6 +1594,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 void btMultiBody::predictPositionsMultiDof(btScalar dt)
 {
     int num_links = getNumLinks();
+		if(!isBaseKinematic())
+		{
       // step position by adding dt * velocity
       //btVector3 v = getBaseVel();
       //m_basePos += dt * v;
@@ -1613,6 +1613,7 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
     	pBasePos[0] += dt * pBaseVel[0];
     	pBasePos[1] += dt * pBaseVel[1];
     	pBasePos[2] += dt * pBaseVel[2];
+		}
     
     ///////////////////////////////
     //local functor for quaternion integration (to avoid error prone redundancy)
@@ -1663,6 +1664,8 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
     
     //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
     //
+		if(!isBaseKinematic())
+		{
         btScalar *pBaseQuat;
 
         // reset to current orientation
@@ -1683,6 +1686,7 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
         pBaseQuat[1] = baseQuat.y();
         pBaseQuat[2] = baseQuat.z();
         pBaseQuat[3] = baseQuat.w();
+		}
 
     // Finally we can update m_jointPos for each of the m_links
     for (int i = 0; i < num_links; ++i)
@@ -1690,6 +1694,38 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
         btScalar *pJointPos;
         pJointPos = &m_links[i].m_jointPos_interpolate[0];
         
+        if (m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic()) 
+		{
+            switch (m_links[i].m_jointType) 
+						{
+                case btMultibodyLink::ePrismatic:
+                case btMultibodyLink::eRevolute:
+                {
+                    pJointPos[0] = m_links[i].m_jointPos[0];
+                    break;
+                }
+                case btMultibodyLink::eSpherical:
+                {
+                    for (int j = 0; j < 4; ++j)
+                    {
+                        pJointPos[j] = m_links[i].m_jointPos[j];
+                    }
+                    break;
+                }
+                case btMultibodyLink::ePlanar:
+                {
+                    for (int j = 0; j < 3; ++j)
+                    {
+                        pJointPos[j] = m_links[i].m_jointPos[j];
+                    }
+                    break;
+                }
+                default:
+                   break;
+            }
+        }
+        else
+        {
             btScalar *pJointVel = getJointVelMultiDof(i); 
 
             switch (m_links[i].m_jointType)
@@ -1741,6 +1777,7 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
                 {
                 }
             }
+        }
         
         m_links[i].updateInterpolationCacheMultiDof();
     }
@@ -1749,6 +1786,8 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
 {
 	int num_links = getNumLinks();
+	if(!isBaseKinematic())
+	{
 		// step position by adding dt * velocity
 		//btVector3 v = getBaseVel();
 		//m_basePos += dt * v;
@@ -1759,6 +1798,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 		pBasePos[0] += dt * pBaseVel[0];
 		pBasePos[1] += dt * pBaseVel[1];
 		pBasePos[2] += dt * pBaseVel[2];
+	}
 
 	///////////////////////////////
 	//local functor for quaternion integration (to avoid error prone redundancy)
@@ -1809,6 +1849,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 
 	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
 	//
+	if(!isBaseKinematic())
+	{
 		btScalar *pBaseQuat = pq ? pq : m_baseQuat;
 		btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
 		//
@@ -1825,6 +1867,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 		//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
 		//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
 		//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
+	}
 
 	if (pq)
 		pq += 7;
@@ -1833,6 +1876,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 
 	// Finally we can update m_jointPos for each of the m_links
 	for (int i = 0; i < num_links; ++i)
+	{
+		if (!(m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic()))
 		{
 			btScalar *pJointPos;
 			pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
@@ -1878,6 +1923,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 				{
 				}
 			}
+		}
 
 		m_links[i].updateCacheMultiDof(pq);
 
@@ -2061,10 +2107,10 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 			motion += m_realBuf[i] * m_realBuf[i];
 	}
 
-	if (motion < SLEEP_EPSILON)
+	if (motion < m_sleepEpsilon)
 	{
 		m_sleepTimer += timestep;
-		if (m_sleepTimer > SLEEP_TIMEOUT)
+		if (m_sleepTimer > m_sleepTimeout)
 		{
 			goToSleep();
 		}
@@ -2181,8 +2227,15 @@ void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObj
     world_to_local.resize(getNumLinks() + 1);
     local_origin.resize(getNumLinks() + 1);
     
+		if(isBaseKinematic()){
+        world_to_local[0] = getWorldToBaseRot();
+        local_origin[0] = getBasePos();
+		}
+		else
+		{
         world_to_local[0] = getInterpolateWorldToBaseRot();
         local_origin[0] = getInterpolateBasePos();
+		}
     
     if (getBaseCollider())
     {
@@ -2328,3 +2381,81 @@ const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *seriali
 
 	return btMultiBodyDataName;
 }
+
+void btMultiBody::saveKinematicState(btScalar timeStep)
+{
+	//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
+	if (m_kinematic_calculate_velocity && timeStep != btScalar(0.))
+	{
+		btVector3 linearVelocity, angularVelocity;
+		btTransformUtil::calculateVelocity(getInterpolateBaseWorldTransform(), getBaseWorldTransform(), timeStep, linearVelocity, angularVelocity);
+		setBaseVel(linearVelocity);
+		setBaseOmega(angularVelocity);
+		setInterpolateBaseWorldTransform(getBaseWorldTransform());
+	}
+}
+
+void btMultiBody::setLinkDynamicType(const int i, int type)
+{
+	if (i == -1)
+	{
+		setBaseDynamicType(type);
+	}
+	else if (i >= 0 && i < getNumLinks())
+	{
+		if (m_links[i].m_collider)
+		{
+			m_links[i].m_collider->setDynamicType(type);
+		}
+	}
+}
+
+bool btMultiBody::isLinkStaticOrKinematic(const int i) const
+{
+	if (i == -1)
+	{
+		return isBaseStaticOrKinematic();
+	}
+	else
+	{
+		if (m_links[i].m_collider)
+			return m_links[i].m_collider->isStaticOrKinematic();
+	}
+	return false;
+}
+
+bool btMultiBody::isLinkKinematic(const int i) const
+{
+	if (i == -1)
+	{
+		return isBaseKinematic();
+	}
+	else
+	{
+		if (m_links[i].m_collider)
+			return m_links[i].m_collider->isKinematic();
+	}
+	return false;
+}
+
+bool btMultiBody::isLinkAndAllAncestorsStaticOrKinematic(const int i) const
+{
+	int link = i;
+	while (link != -1) {
+		if (!isLinkStaticOrKinematic(link))
+			return false;
+		link = m_links[link].m_parent;
+	}
+	return isBaseStaticOrKinematic();
+}
+
+bool btMultiBody::isLinkAndAllAncestorsKinematic(const int i) const
+{
+	int link = i;
+	while (link != -1) {
+		if (!isLinkKinematic(link))
+			return false;
+		link = m_links[link].m_parent;
+	}
+	return isBaseKinematic();
+}

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h

@@ -210,6 +210,12 @@ public:
 	void setBasePos(const btVector3 &pos)
 	{
 		m_basePos = pos;
+		if(!isBaseKinematic())
+			m_basePos_interpolate = pos;
+	}
+
+	void setInterpolateBasePos(const btVector3 &pos)
+	{
 		m_basePos_interpolate = pos;
 	}
 
@@ -227,17 +233,39 @@ public:
 		return tr;
 	}
 
+	void setInterpolateBaseWorldTransform(const btTransform &tr)
+	{
+		setInterpolateBasePos(tr.getOrigin());
+		setInterpolateWorldToBaseRot(tr.getRotation().inverse());
+	}
+
+	btTransform getInterpolateBaseWorldTransform() const
+	{
+		btTransform tr;
+		tr.setOrigin(getInterpolateBasePos());
+		tr.setRotation(getInterpolateWorldToBaseRot().inverse());
+		return tr;
+	}
+
 	void setBaseVel(const btVector3 &vel)
 	{
 		m_realBuf[3] = vel[0];
 		m_realBuf[4] = vel[1];
 		m_realBuf[5] = vel[2];
 	}
+
 	void setWorldToBaseRot(const btQuaternion &rot)
 	{
 		m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?
+		if(!isBaseKinematic())
 			m_baseQuat_interpolate = rot;
 	}
+
+	void setInterpolateWorldToBaseRot(const btQuaternion &rot)
+	{
+		m_baseQuat_interpolate = rot;
+	}
+
 	void setBaseOmega(const btVector3 &omega)
 	{
 		m_realBuf[0] = omega[0];
@@ -245,6 +273,8 @@ public:
 		m_realBuf[2] = omega[2];
 	}
 
+	void saveKinematicState(btScalar timeStep);
+
 	//
 	// get/set pos/vel for child m_links (i = 0 to num_links-1)
 	//
@@ -278,6 +308,11 @@ public:
     {
         return &m_deltaV[0];
     }
+    
+    const btScalar *getSplitVelocityVector() const
+    {
+        return &m_splitV[0];
+    }
 	/*    btScalar * getVelocityVector() 
 	{ 
 		return &real_buf[0]; 
@@ -307,13 +342,6 @@ public:
 	//
 	btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
 
-	//
-	// calculate kinetic energy and angular momentum
-	// useful for debugging.
-	//
-
-	btScalar getKineticEnergy() const;
-	btVector3 getAngularMomentum() const;
 
 	//
 	// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
@@ -404,6 +432,26 @@ public:
 			m_deltaV[dof] += delta_vee[dof] * multiplier;
 		}
 	}
+    void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
+    {
+        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+        {
+            m_splitV[dof] += delta_vee[dof] * multiplier;
+        }
+    }
+    void addSplitV()
+    {
+        applyDeltaVeeMultiDof(&m_splitV[0], 1);
+    }
+    void substractSplitV()
+    {
+        applyDeltaVeeMultiDof(&m_splitV[0], -1);
+        
+        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+        {
+            m_splitV[dof] = 0.f;
+        }
+    }
 	void processDeltaVeeMultiDof2()
 	{
 		applyDeltaVeeMultiDof(&m_deltaV[0], 1);
@@ -497,19 +545,30 @@ public:
 	{
 		m_canWakeup = canWakeup;
 	}
-	bool isAwake() const { return m_awake; }
+	bool isAwake() const 
+	{ 
+		return m_awake; 
+	}
 	void wakeUp();
 	void goToSleep();
 	void checkMotionAndSleepIfRequired(btScalar timestep);
 
-	bool hasFixedBase() const
-	{
-		return m_fixedBase;
-	}
+	bool hasFixedBase() const;
+
+	bool isBaseKinematic() const;
+
+	bool isBaseStaticOrKinematic() const;
+
+	// set the dynamic type in the base's collision flags.
+	void setBaseDynamicType(int dynamicType);
 
 	void setFixedBase(bool fixedBase)
 	{
 		m_fixedBase = fixedBase;
+		if(m_fixedBase)
+			setBaseDynamicType(btCollisionObject::CF_STATIC_OBJECT);
+		else
+			setBaseDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
 	}
 
 	int getCompanionId() const
@@ -660,6 +719,25 @@ public:
 		btVector3 &top_out,         // top part of output vector
 		btVector3 &bottom_out);      // bottom part of output vector
 
+	void setLinkDynamicType(const int i, int type);
+
+	bool isLinkStaticOrKinematic(const int i) const;
+
+	bool isLinkKinematic(const int i) const;
+
+	bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const;
+
+	bool isLinkAndAllAncestorsKinematic(const int i) const;
+
+	void setSleepThreshold(btScalar sleepThreshold)
+	{
+		m_sleepEpsilon = sleepThreshold;
+	}
+
+	void setSleepTimeout(btScalar sleepTimeout)
+	{
+		this->m_sleepTimeout = sleepTimeout;
+	}
 
 
 private:
@@ -681,7 +759,7 @@ private:
 		}
 	}
 
-	void mulMatrix(btScalar * pA, btScalar * pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
+	void mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
 
 private:
 	btMultiBodyLinkCollider *m_baseCollider;  //can be NULL
@@ -718,6 +796,7 @@ private:
 	//  offset         size         array
 	//   0              num_links+1  rot_from_parent
 	//
+    btAlignedObjectArray<btScalar> m_splitV;
 	btAlignedObjectArray<btScalar> m_deltaV;
 	btAlignedObjectArray<btScalar> m_realBuf;
 	btAlignedObjectArray<btVector3> m_vectorBuf;
@@ -736,6 +815,8 @@ private:
 	bool m_canSleep;
 	bool m_canWakeup;
 	btScalar m_sleepTimer;
+	btScalar m_sleepEpsilon;
+	btScalar m_sleepTimeout;
 
 	void *m_userObjectPointer;
 	int m_userIndex2;
@@ -758,6 +839,9 @@ private:
 
 	///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
 	bool m_internalNeedsJointFeedback;
+
+  //If enabled, calculate the velocity based on kinematic transform changes. Currently only implemented for the base.
+	bool m_kinematic_calculate_velocity;
 };
 
 struct btMultiBodyLinkDoubleData

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -2,11 +2,12 @@
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "btMultiBodyPoint2Point.h"  //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
 
-btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
+btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type)
 	: m_bodyA(bodyA),
 	  m_bodyB(bodyB),
 	  m_linkA(linkA),
 	  m_linkB(linkB),
+	  m_type(type),
 	  m_numRows(numRows),
 	  m_jacSizeA(0),
 	  m_jacSizeBoth(0),
@@ -60,7 +61,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra
 														btScalar lowerLimit, btScalar upperLimit,
 														bool angConstraint,
 														btScalar relaxation,
-														bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+														bool isFriction, btScalar desiredVelocity, btScalar cfmSlip,
+														btScalar damping)
 {
 	solverConstraint.m_multiBodyA = m_bodyA;
 	solverConstraint.m_multiBodyB = m_bodyB;
@@ -347,7 +349,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra
 
 	{
 		btScalar positionalError = 0.f;
-		btScalar velocityError = desiredVelocity - rel_vel;  // * damping;
+		btScalar velocityError = (desiredVelocity - rel_vel) * damping;
 
 		btScalar erp = infoGlobal.m_erp2;
 

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -20,6 +20,21 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 #include "btMultiBody.h"
 
+
+//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
+enum btTypedMultiBodyConstraintType
+{
+	MULTIBODY_CONSTRAINT_LIMIT=3,
+	MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR,
+	MULTIBODY_CONSTRAINT_GEAR,
+	MULTIBODY_CONSTRAINT_POINT_TO_POINT,
+	MULTIBODY_CONSTRAINT_SLIDER,
+	MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
+	MULTIBODY_CONSTRAINT_FIXED,
+	
+	MAX_MULTIBODY_CONSTRAINT_TYPE,
+};
+
 class btMultiBody;
 struct btSolverInfo;
 
@@ -46,6 +61,8 @@ protected:
 	int m_linkA;
 	int m_linkB;
 
+	int m_type; //btTypedMultiBodyConstraintType
+
 	int m_numRows;
 	int m_jacSizeA;
 	int m_jacSizeBoth;
@@ -77,17 +94,21 @@ protected:
 									 bool angConstraint = false,
 
 									 btScalar relaxation = 1.f,
-									 bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+									 bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0, btScalar damping = 1.0);
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral);
+	btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type);
 	virtual ~btMultiBodyConstraint();
 
 	void updateJacobianSizes();
 	void allocateJacobiansMultiDof();
 
+	int getConstraintType() const
+	{
+		return m_type;
+	}
 	//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
 	virtual void setFrameInB(const btMatrix3x3& frameInB) {}
 	virtual void setPivotInB(const btVector3& pivotInB) {}

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -30,9 +30,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 	btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	//solve featherstone non-contact constraints
-
+	btScalar nonContactResidual = 0;
 	//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
-
+	for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i)
+	{
+		// reset the nonContactResdual to 0 at start of each inner iteration
+		nonContactResidual = 0;
 		for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
 		{
 			int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
@@ -40,13 +43,15 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 			btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
 
 			btScalar residual = resolveSingleConstraintRowGeneric(constraint);
-		leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+			nonContactResidual = btMax(nonContactResidual, residual * residual);
 
 			if (constraint.m_multiBodyA)
 				constraint.m_multiBodyA->setPosUpdated(false);
 			if (constraint.m_multiBodyB)
 				constraint.m_multiBodyB->setPosUpdated(false);
 		}
+	}
+	leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual);
 
 	//solve featherstone normal contact
 	for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
@@ -1270,7 +1275,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 	//	return;
 
 	//only a single rollingFriction per manifold
-	int rollingFriction = 1;
+	int rollingFriction = 4;
 
 	for (int j = 0; j < manifold->getNumContacts(); j++)
 	{

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -137,7 +137,14 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 				btMultiBodyLinkCollider* col = body->getBaseCollider();
 				if (col && col->getActivationState() == ACTIVE_TAG)
 				{
+                    if (body->hasFixedBase())
+					{
+                        col->setActivationState(FIXED_BASE_MULTI_BODY);
+                    } else
+					{
                         col->setActivationState(WANTS_DEACTIVATION);
+                    }
+					
 					col->setDeactivationTime(0.f);
 				}
 				for (int b = 0; b < body->getNumLinks(); b++)
@@ -592,6 +599,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
 
 			if (!isSleeping)
 			{
+				bod->addSplitV();
 				int nLinks = bod->getNumLinks();
 
 				///base + num m_links
@@ -610,6 +618,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
 				m_scratch_world_to_local.resize(nLinks + 1);
 				m_scratch_local_origin.resize(nLinks + 1);
                 bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+				bod->substractSplitV();
 			}
 			else
 			{
@@ -867,6 +876,18 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
 		}
 	}
 }
+
+void btMultiBodyDynamicsWorld::saveKinematicState(btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::saveKinematicState(timeStep);
+	for(int i = 0; i < m_multiBodies.size(); i++)
+	{
+		btMultiBody* body = m_multiBodies[i];
+		if(body->isBaseKinematic())
+			body->saveKinematicState(timeStep);
+	}
+}
+
 //
 //void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
 //{

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -120,5 +120,7 @@ public:
     virtual void solveExternalForces(btContactSolverInfo& solverInfo);
     virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
     void buildIslands();
+
+	virtual void saveKinematicState(btScalar timeStep);
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp

@@ -24,7 +24,7 @@ subject to the following restrictions:
 #define BTMBFIXEDCONSTRAINT_DIM 6
 
 btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
-	: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false),
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
 	  m_rigidBodyA(0),
 	  m_rigidBodyB(bodyB),
 	  m_pivotInA(pivotInA),
@@ -36,7 +36,7 @@ btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int li
 }
 
 btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
-	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false),
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
 	  m_rigidBodyA(0),
 	  m_rigidBodyB(0),
 	  m_pivotInA(pivotInA),

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp

@@ -21,7 +21,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
-	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false),
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR),
 	  m_gearRatio(1),
 	  m_gearAuxLink(-1),
 	  m_erp(0),

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -22,7 +22,7 @@ subject to the following restrictions:
 
 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
 	//:btMultiBodyConstraint(body,0,link,-1,2,true),
-	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true),
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT),
 	  m_lowerBound(lower),
 	  m_upperBound(upper)
 {

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -42,6 +42,22 @@ public:
 	{
 		//todo(erwincoumans)
 	}
+	btScalar getLowerBound() const
+	{
+		return m_lowerBound;
+	}
+	btScalar getUpperBound() const
+	{
+		return m_upperBound;
+	}
+	void setLowerBound(btScalar lower)
+	{
+		m_lowerBound = lower;
+	}
+	void setUpperBound(btScalar upper)
+	{
+		m_upperBound = upper;
+	}
 };
 
 #endif  //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -21,7 +21,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
-	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
 	  m_desiredVelocity(desiredVelocity),
 	  m_desiredPosition(0),
 	  m_kd(1.),
@@ -51,7 +51,7 @@ void btMultiBodyJointMotor::finalizeMultiDof()
 
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
 	//:btMultiBodyConstraint(body,0,link,-1,1,true),
-	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
 	  m_desiredVelocity(desiredVelocity),
 	  m_desiredPosition(0),
 	  m_kd(1.),

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -295,6 +295,9 @@ struct btMultibodyLink
             }
         }
     }
+
+ 
+
 };
 
 #endif  //BT_MULTIBODY_LINK_H

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h

@@ -130,6 +130,23 @@ public:
 		return true;
 	}
 
+	bool isStaticOrKinematic() const
+	{
+		return isStaticOrKinematicObject();
+	}
+
+	bool isKinematic() const
+	{
+		return isKinematicObject();
+	}
+
+	void setDynamicType(int dynamicType)
+	{
+		int oldFlags = getCollisionFlags();
+		oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
+		setCollisionFlags(oldFlags | dynamicType);
+	}
+
 	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -27,7 +27,7 @@ subject to the following restrictions:
 #endif
 
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
-	: btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false),
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
 	  m_rigidBodyA(0),
 	  m_rigidBodyB(bodyB),
 	  m_pivotInA(pivotInA),
@@ -37,7 +37,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi
 }
 
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
-	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false),
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
 	  m_rigidBodyA(0),
 	  m_rigidBodyB(0),
 	  m_pivotInA(pivotInA),

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp

@@ -25,7 +25,7 @@ subject to the following restrictions:
 #define EPSILON 0.000001
 
 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
-	: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false),
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
 	  m_rigidBodyA(0),
 	  m_rigidBodyB(bodyB),
 	  m_pivotInA(pivotInA),
@@ -38,7 +38,7 @@ btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int
 }
 
 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
-	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false),
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
 	  m_rigidBodyA(0),
 	  m_rigidBodyB(0),
 	  m_pivotInA(pivotInA),

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp

@@ -23,13 +23,16 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
 
 btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
-	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true),
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR),
 	m_desiredVelocity(0, 0, 0),
 	m_desiredPosition(0,0,0,1),
-	m_kd(1.),
-	m_kp(0.2),
+	m_use_multi_dof_params(false),
+	m_kd(1., 1., 1.),
+	m_kp(0.2, 0.2, 0.2),
 	m_erp(1),
-	m_rhsClamp(SIMD_INFINITY)
+	m_rhsClamp(SIMD_INFINITY),
+	m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse),
+	m_damping(1.0, 1.0, 1.0)
 {
 
 	m_maxAppliedImpulse = maxMotorImpulse;
@@ -139,7 +142,8 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat;
 		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
 		btScalar desiredVelocity = this->m_desiredVelocity[row];
 		
-		btScalar velocityError = desiredVelocity - currentVelocity;
+		double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
+		btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
 
 		btMatrix3x3 frameAworld;
 		frameAworld.setIdentity();
@@ -152,12 +156,16 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat;
 				case btMultibodyLink::eSpherical:
 				{
 					btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
-					posError = m_kp*angleDiff[row % 3];
+					double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
+					posError = kp*angleDiff[row % 3];
+					double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse;
 					fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
 						btVector3(0,0,0), dummy, dummy,
 						posError,
 						infoGlobal,
-						-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+						-max_applied_impulse, max_applied_impulse, true,
+						1.0, false, 0, 0,
+						m_damping[row % 3]);
 					constraintRow.m_orgConstraint = this;
 					constraintRow.m_orgDofIndex = row;
 					break;

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h

@@ -26,10 +26,13 @@ class btMultiBodySphericalJointMotor : public btMultiBodyConstraint
 protected:
 	btVector3 m_desiredVelocity;
 	btQuaternion m_desiredPosition;
-	btScalar m_kd;
-	btScalar m_kp;
+	bool m_use_multi_dof_params;
+	btVector3 m_kd;
+	btVector3 m_kp;
 	btScalar m_erp;
 	btScalar m_rhsClamp;  //maximum error
+	btVector3 m_maxAppliedImpulseMultiDof;
+	btVector3 m_damping;
 
 public:
 	btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse);
@@ -44,16 +47,32 @@ public:
 									  btMultiBodyJacobianData& data,
 									  const btContactSolverInfo& infoGlobal);
 
-	virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f)
+	virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.0)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = btVector3(kd, kd, kd);
+		m_use_multi_dof_params = false;
+	}
+
+	virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0))
 	{
 		m_desiredVelocity = velTarget;
 		m_kd = kd;
+		m_use_multi_dof_params = true;
 	}
 
 	virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = btVector3(kp, kp, kp);
+		m_use_multi_dof_params = false;
+	}
+
+	virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f))
 	{
 		m_desiredPosition = posTarget;
 		m_kp = kp;
+		m_use_multi_dof_params = true;
 	}
 
 	virtual void setErp(btScalar erp)
@@ -68,6 +87,28 @@ public:
 	{
 		m_rhsClamp = rhsClamp;
 	}
+
+	btScalar getMaxAppliedImpulseMultiDof(int i) const
+	{
+		return m_maxAppliedImpulseMultiDof[i];
+	}
+
+	void setMaxAppliedImpulseMultiDof(const btVector3& maxImp)
+	{
+		m_maxAppliedImpulseMultiDof = maxImp;
+		m_use_multi_dof_params = true;
+	}
+
+	btScalar getDamping(int i) const
+	{
+		return m_damping[i];
+	}
+
+	void setDamping(const btVector3& damping)
+	{
+		m_damping = damping;
+	}
+
 	virtual void debugDraw(class btIDebugDraw* drawer)
 	{
 		//todo(erwincoumans)

thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp

@@ -532,7 +532,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 	J_transpose = J.transpose();
 
 	btMatrixXu& tmp = m_scratchTmp;
-
+	//Minv.printMatrix("Minv=");
 	{
 		{
 			BT_PROFILE("J*Minv");
@@ -543,7 +543,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 			m_A = tmp * J_transpose;
 		}
 	}
-
+	//J.printMatrix("J");
 	if (1)
 	{
 		// add cfm to the diagonal of m_A

thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h

@@ -18,7 +18,6 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver
 	{
 	}
 
-
 	virtual void processConstraints(int islandId = -1)
 	{
 		btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;

thirdparty/bullet/BulletSoftBody/btCGProjection.h

@@ -75,8 +75,7 @@ public:
 	const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
 
 	btCGProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const btScalar& dt)
-    : m_softBodies(softBodies)
-    , m_dt(dt)
+		: m_softBodies(softBodies), m_dt(dt)
 	{
 	}
 
@@ -102,5 +101,4 @@ public:
 	}
 };
 
-
 #endif /* btCGProjection_h */

thirdparty/bullet/BulletSoftBody/btConjugateGradient.h

@@ -15,24 +15,18 @@
 
 #ifndef BT_CONJUGATE_GRADIENT_H
 #define BT_CONJUGATE_GRADIENT_H
-#include <iostream>
-#include <cmath>
-#include <limits>
-#include <LinearMath/btAlignedObjectArray.h>
-#include <LinearMath/btVector3.h>
-#include "LinearMath/btQuickprof.h"
+#include "btKrylovSolver.h"
 template <class MatrixX>
-class btConjugateGradient
+class btConjugateGradient : public btKrylovSolver<MatrixX>
 {
 	typedef btAlignedObjectArray<btVector3> TVStack;
+	typedef btKrylovSolver<MatrixX> Base;
 	TVStack r, p, z, temp;
-    int max_iterations;
-    btScalar tolerance_squared;
+
 public:
 	btConjugateGradient(const int max_it_in)
-    : max_iterations(max_it_in)
+		: btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON)
 	{
-       tolerance_squared = 1e-5;
 	}
 
 	virtual ~btConjugateGradient() {}
@@ -43,15 +37,22 @@ public:
 		BT_PROFILE("CGSolve");
 		btAssert(x.size() == b.size());
 		reinitialize(b);
+		temp = b;
+		A.project(temp);
+		p = temp;
+		A.precondition(p, z);
+		btScalar d0 = this->dot(z, temp);
+		d0 = btMin(btScalar(1), d0);
 		// r = b - A * x --with assigned dof zeroed out
 		A.multiply(x, temp);
-        r = sub(b, temp);
+		r = this->sub(b, temp);
 		A.project(r);
 		// z = M^(-1) * r
 		A.precondition(r, z);
 		A.project(z);
-        btScalar r_dot_z = dot(z,r);
-        if (r_dot_z <= tolerance_squared) {
+		btScalar r_dot_z = this->dot(z, r);
+		if (r_dot_z <= Base::m_tolerance * d0)
+		{
 			if (verbose)
 			{
 				std::cout << "Iteration = 0" << std::endl;
@@ -61,11 +62,12 @@ public:
 		}
 		p = z;
 		btScalar r_dot_z_new = r_dot_z;
-        for (int k = 1; k <= max_iterations; k++) {
+		for (int k = 1; k <= Base::m_maxIterations; k++)
+		{
 			// temp = A*p
 			A.multiply(p, temp);
 			A.project(temp);
-            if (dot(p,temp) < SIMD_EPSILON)
+			if (this->dot(p, temp) < 0)
 			{
 				if (verbose)
 					std::cout << "Encountered negative direction in CG!" << std::endl;
@@ -76,31 +78,32 @@ public:
 				return k;
 			}
 			// alpha = r^T * z / (p^T * A * p)
-            btScalar alpha = r_dot_z_new / dot(p, temp);
+			btScalar alpha = r_dot_z_new / this->dot(p, temp);
 			//  x += alpha * p;
-            multAndAddTo(alpha, p, x);
+			this->multAndAddTo(alpha, p, x);
 			//  r -= alpha * temp;
-            multAndAddTo(-alpha, temp, r);
+			this->multAndAddTo(-alpha, temp, r);
 			// z = M^(-1) * r
 			A.precondition(r, z);
 			r_dot_z = r_dot_z_new;
-            r_dot_z_new = dot(r,z);
-            if (r_dot_z_new < tolerance_squared) {
+			r_dot_z_new = this->dot(r, z);
+			if (r_dot_z_new < Base::m_tolerance * d0)
+			{
 				if (verbose)
 				{
-                    std::cout << "ConjugateGradient iterations " << k << std::endl;
+					std::cout << "ConjugateGradient iterations " << k << " residual = " << r_dot_z_new << std::endl;
 				}
 				return k;
 			}
 
 			btScalar beta = r_dot_z_new / r_dot_z;
-            p = multAndAdd(beta, p, z);
+			p = this->multAndAdd(beta, p, z);
 		}
 		if (verbose)
 		{
-            std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
+			std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << " error = " << r_dot_z_new << std::endl;
 		}
-        return max_iterations;
+		return Base::m_maxIterations;
 	}
 
 	void reinitialize(const TVStack& b)
@@ -110,49 +113,5 @@ public:
 		z.resize(b.size());
 		temp.resize(b.size());
 	}
-    
-    TVStack sub(const TVStack& a, const TVStack& b)
-    {
-        // c = a-b
-        btAssert(a.size() == b.size());
-        TVStack c;
-        c.resize(a.size());
-        for (int i = 0; i < a.size(); ++i)
-        {
-            c[i] = a[i] - b[i];
-        }
-        return c;
-    }
-    
-    btScalar squaredNorm(const TVStack& a)
-    {
-        return dot(a,a);
-    }
-    
-    btScalar dot(const TVStack& a, const TVStack& b)
-    {
-        btScalar ans(0);
-        for (int i = 0; i < a.size(); ++i)
-            ans += a[i].dot(b[i]);
-        return ans;
-    }
-    
-    void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
-    {
-//        result += s*a
-        btAssert(a.size() == result.size());
-        for (int i = 0; i < a.size(); ++i)
-            result[i] += s * a[i];
-    }
-    
-    TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
-    {
-        // result = a*s + b
-        TVStack result;
-        result.resize(a.size());
-        for (int i = 0; i < a.size(); ++i)
-            result[i] = s * a[i] + b[i];
-        return result;
-    }
 };
 #endif /* btConjugateGradient_h */

thirdparty/bullet/BulletSoftBody/btConjugateResidual.h

@@ -0,0 +1,112 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_CONJUGATE_RESIDUAL_H
+#define BT_CONJUGATE_RESIDUAL_H
+#include "btKrylovSolver.h"
+
+template <class MatrixX>
+class btConjugateResidual : public btKrylovSolver<MatrixX>
+{
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	typedef btKrylovSolver<MatrixX> Base;
+	TVStack r, p, z, temp_p, temp_r, best_x;
+	// temp_r = A*r
+	// temp_p = A*p
+	// z = M^(-1) * temp_p = M^(-1) * A * p
+	btScalar best_r;
+
+public:
+	btConjugateResidual(const int max_it_in)
+		: Base(max_it_in, 1e-8)
+	{
+	}
+
+	virtual ~btConjugateResidual() {}
+
+	// return the number of iterations taken
+	int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
+	{
+		BT_PROFILE("CRSolve");
+		btAssert(x.size() == b.size());
+		reinitialize(b);
+		// r = b - A * x --with assigned dof zeroed out
+		A.multiply(x, temp_r);  // borrow temp_r here to store A*x
+		r = this->sub(b, temp_r);
+		// z = M^(-1) * r
+		A.precondition(r, z);  // borrow z to store preconditioned r
+		r = z;
+		btScalar residual_norm = this->norm(r);
+		if (residual_norm <= Base::m_tolerance)
+		{
+			return 0;
+		}
+		p = r;
+		btScalar r_dot_Ar, r_dot_Ar_new;
+		// temp_p = A*p
+		A.multiply(p, temp_p);
+		// temp_r = A*r
+		temp_r = temp_p;
+		r_dot_Ar = this->dot(r, temp_r);
+		for (int k = 1; k <= Base::m_maxIterations; k++)
+		{
+			// z = M^(-1) * Ap
+			A.precondition(temp_p, z);
+			// alpha = r^T * A * r / (Ap)^T * M^-1 * Ap)
+			btScalar alpha = r_dot_Ar / this->dot(temp_p, z);
+			//  x += alpha * p;
+			this->multAndAddTo(alpha, p, x);
+			//  r -= alpha * z;
+			this->multAndAddTo(-alpha, z, r);
+			btScalar norm_r = this->norm(r);
+			if (norm_r < best_r)
+			{
+				best_x = x;
+				best_r = norm_r;
+				if (norm_r < Base::m_tolerance)
+				{
+					return k;
+				}
+			}
+			// temp_r = A * r;
+			A.multiply(r, temp_r);
+			r_dot_Ar_new = this->dot(r, temp_r);
+			btScalar beta = r_dot_Ar_new / r_dot_Ar;
+			r_dot_Ar = r_dot_Ar_new;
+			// p = beta*p + r;
+			p = this->multAndAdd(beta, p, r);
+			// temp_p = beta*temp_p + temp_r;
+			temp_p = this->multAndAdd(beta, temp_p, temp_r);
+		}
+		if (verbose)
+		{
+			std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl;
+		}
+		x = best_x;
+		return Base::m_maxIterations;
+	}
+
+	void reinitialize(const TVStack& b)
+	{
+		r.resize(b.size());
+		p.resize(b.size());
+		z.resize(b.size());
+		temp_p.resize(b.size());
+		temp_r.resize(b.size());
+		best_x.resize(b.size());
+		best_r = SIMD_INFINITY;
+	}
+};
+#endif /* btConjugateResidual_h */

thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -18,17 +18,17 @@
 #include "LinearMath/btQuickprof.h"
 
 btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v)
-: m_softBodies(softBodies)
-, m_projection(softBodies)
-, m_backupVelocity(backup_v)
-, m_implicit(false)
+	: m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false)
 {
-    m_preconditioner = new MassPreconditioner(m_softBodies);
+	m_massPreconditioner = new MassPreconditioner(m_softBodies);
+	m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit);
+	m_preconditioner = m_KKTPreconditioner;
 }
 
 btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
 {
-    delete m_preconditioner;
+	delete m_KKTPreconditioner;
+	delete m_massPreconditioner;
 }
 
 void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
@@ -46,8 +46,19 @@ void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar
 	{
 		m_lf[i]->reinitialize(nodeUpdated);
 	}
+	btMatrix3x3 I;
+	I.setIdentity();
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			if (psb->m_nodes[j].m_im > 0)
+				psb->m_nodes[j].m_effectiveMass = I * (1.0 / psb->m_nodes[j].m_im);
+		}
+	}
 	m_projection.reinitialize(nodeUpdated);
-    m_preconditioner->reinitialize(nodeUpdated);
+	//    m_preconditioner->reinitialize(nodeUpdated);
 }
 
 void btDeformableBackwardEulerObjective::setDt(btScalar dt)
@@ -75,11 +86,39 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
 	{
 		// add damping matrix
 		m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
-        if (m_implicit)
+        // Always integrate picking force implicitly for stability.
+        if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE)
 		{
 			m_lf[i]->addScaledElasticForceDifferential(-m_dt * m_dt, x, b);
 		}
 	}
+	int offset = m_nodes.size();
+	for (int i = offset; i < b.size(); ++i)
+	{
+		b[i].setZero();
+	}
+	// add in the lagrange multiplier terms
+
+	for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c)
+	{
+		// C^T * lambda
+		const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c];
+		for (int i = 0; i < lm.m_num_nodes; ++i)
+		{
+			for (int j = 0; j < lm.m_num_constraints; ++j)
+			{
+				b[lm.m_indices[i]] += x[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
+			}
+		}
+		// C * x
+		for (int d = 0; d < lm.m_num_constraints; ++d)
+		{
+			for (int i = 0; i < lm.m_num_nodes; ++i)
+			{
+				b[offset + c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]);
+			}
+		}
+	}
 }
 
 void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
@@ -106,12 +145,25 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
 			counter += psb->m_nodes.size();
 			continue;
 		}
+		if (m_implicit)
+		{
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				if (psb->m_nodes[j].m_im != 0)
+				{
+					psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++];
+				}
+			}
+		}
+		else
+		{
 			for (int j = 0; j < psb->m_nodes.size(); ++j)
 			{
 				btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
 				psb->m_nodes[j].m_v += one_over_mass * force[counter++];
 			}
 		}
+	}
 	if (setZero)
 	{
 		for (int i = 0; i < force.size(); ++i)
@@ -125,7 +177,8 @@ void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &r
 	// add implicit force
 	for (int i = 0; i < m_lf.size(); ++i)
 	{
-        if (m_implicit)
+        // Always integrate picking force implicitly for stability.
+		if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE)
 		{
 			m_lf[i]->addScaledForces(dt, residual);
 		}
@@ -134,7 +187,7 @@ void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &r
 			m_lf[i]->addScaledDampingForce(dt, residual);
 		}
 	}
-    m_projection.project(residual);
+	//    m_projection.project(residual);
 }
 
 btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
@@ -163,11 +216,60 @@ void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
 	{
 		m_softBodies[i]->advanceDeformation();
 	}
-    
+	if (m_implicit)
+	{
+		// apply forces except gravity force
+		btVector3 gravity;
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			if (m_lf[i]->getForceType() == BT_GRAVITY_FORCE)
+			{
+				gravity = static_cast<btDeformableGravityForce*>(m_lf[i])->m_gravity;
+			}
+			else
+			{
+				m_lf[i]->addScaledForces(m_dt, force);
+			}
+		}
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			m_lf[i]->addScaledHessian(m_dt);
+		}
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					// add gravity explicitly
+					psb->m_nodes[j].m_v += m_dt * psb->m_gravityFactor * gravity;
+				}
+			}
+		}
+	}
+	else
+	{
 		for (int i = 0; i < m_lf.size(); ++i)
 		{
 			m_lf[i]->addScaledExplicitForce(m_dt, force);
 		}
+	}
+	// calculate inverse mass matrix for all nodes
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				if (psb->m_nodes[j].m_im > 0)
+				{
+					psb->m_nodes[j].m_effectiveMass_inv = psb->m_nodes[j].m_effectiveMass.inverse();
+				}
+			}
+		}
+	}
 	applyForce(force, true);
 }
 
@@ -186,9 +288,9 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack
 }
 
 //set constraints as projections
-void btDeformableBackwardEulerObjective::setConstraints()
+void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal)
 {
-    m_projection.setConstraints();
+	m_projection.setConstraints(infoGlobal);
 }
 
 void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)

thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -15,11 +15,12 @@
 
 #ifndef BT_BACKWARD_EULER_OBJECTIVE_H
 #define BT_BACKWARD_EULER_OBJECTIVE_H
-#include "btConjugateGradient.h"
+//#include "btConjugateGradient.h"
 #include "btDeformableLagrangianForce.h"
 #include "btDeformableMassSpringForce.h"
 #include "btDeformableGravityForce.h"
 #include "btDeformableCorotatedForce.h"
+#include "btDeformableMousePickingForce.h"
 #include "btDeformableLinearElasticityForce.h"
 #include "btDeformableNeoHookeanForce.h"
 #include "btDeformableContactProjection.h"
@@ -39,6 +40,8 @@ public:
 	const TVStack& m_backupVelocity;
 	btAlignedObjectArray<btSoftBody::Node*> m_nodes;
 	bool m_implicit;
+	MassPreconditioner* m_massPreconditioner;
+	KKTPreconditioner* m_KKTPreconditioner;
 
 	btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v);
 
@@ -79,7 +82,7 @@ public:
 	void updateVelocity(const TVStack& dv);
 
 	//set constraints as projections
-    void setConstraints();
+	void setConstraints(const btContactSolverInfo& infoGlobal);
 
 	// update the projections and project the residual
 	void project(TVStack& r)
@@ -129,6 +132,67 @@ public:
 
 	// Calculate the total potential energy in the system
 	btScalar totalEnergy(btScalar dt);
+
+	void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec)
+	{
+		extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size());
+		for (int i = 0; i < vec.size(); ++i)
+		{
+			extended_vec[i] = vec[i];
+		}
+		int offset = vec.size();
+		for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
+		{
+			extended_vec[offset + i].setZero();
+		}
+	}
+
+	void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual)
+	{
+		extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size());
+		for (int i = 0; i < residual.size(); ++i)
+		{
+			extended_residual[i] = residual[i];
+		}
+		int offset = residual.size();
+		for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i)
+		{
+			const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i];
+			extended_residual[offset + i].setZero();
+			for (int d = 0; d < lm.m_num_constraints; ++d)
+			{
+				for (int n = 0; n < lm.m_num_nodes; ++n)
+				{
+					extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]);
+				}
+			}
+		}
+	}
+
+	void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f)
+	{
+		size_t counter = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im;
+				++counter;
+			}
+		}
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			// add damping matrix
+			m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f);
+		}
+		counter = 0;
+		for (; counter < f.size(); ++counter)
+		{
+			f[counter] = rhs[counter] - f[counter];
+		}
+	}
 };
 
 #endif /* btBackwardEulerObjective_h */

thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp

@@ -18,13 +18,9 @@
 #include "btDeformableBodySolver.h"
 #include "btSoftBodyInternals.h"
 #include "LinearMath/btQuickprof.h"
-static const int kMaxConjugateGradientIterations  = 50;
+static const int kMaxConjugateGradientIterations = 300;
 btDeformableBodySolver::btDeformableBodySolver()
-: m_numNodes(0)
-, m_cg(kMaxConjugateGradientIterations)
-, m_maxNewtonIterations(5)
-, m_newtonTolerance(1e-4)
-, m_lineSearch(false)
+	: m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
 {
 	m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
 }
@@ -41,7 +37,22 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
 	{
 		m_objective->computeResidual(solverdt, m_residual);
 		m_objective->applyDynamicFriction(m_residual);
+		if (m_useProjection)
+		{
 			computeStep(m_dv, m_residual);
+		}
+		else
+		{
+			TVStack rhs, x;
+			m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
+			m_objective->addLagrangeMultiplier(m_dv, x);
+			m_objective->m_preconditioner->reinitialize(true);
+			computeStep(x, rhs);
+			for (int i = 0; i < m_dv.size(); ++i)
+			{
+				m_dv[i] = x[i];
+			}
+		}
 		updateVelocity();
 	}
 	else
@@ -78,9 +89,11 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
 				btScalar scale = 2;
 				btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2;
 				backupDv();
-                do {
+				do
+				{
 					scale *= beta;
-                    if (scale < 1e-8) {
+					if (scale < 1e-8)
+					{
 						return;
 					}
 					updateEnergy(scale);
@@ -149,7 +162,6 @@ void btDeformableBodySolver::updateEnergy(btScalar scale)
 	updateState();
 }
 
-
 btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose)
 {
 	m_cg.solve(*m_objective, ddv, residual, false);
@@ -200,7 +212,10 @@ void btDeformableBodySolver::updateDv(btScalar scale)
 
 void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
 {
-    m_cg.solve(*m_objective, ddv, residual);
+	if (m_useProjection)
+		m_cg.solve(*m_objective, ddv, residual, false);
+	else
+		m_cr.solve(*m_objective, ddv, residual, false);
 }
 
 void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt)
@@ -224,34 +239,27 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
 		m_residual[i].setZero();
 	}
 
+	if (dt > 0)
+	{
 		m_dt = dt;
+	}
 	m_objective->reinitialize(nodeUpdated, dt);
+	updateSoftBodies();
 }
 
-void btDeformableBodySolver::setConstraints()
+void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("setConstraint");
-    m_objective->setConstraints();
+	m_objective->setConstraints(infoGlobal);
 }
 
-btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies)
+btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("solveContactConstraints");
-    btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies);
+	btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal);
 	return maxSquaredResidual;
 }
 
-btScalar btDeformableBodySolver::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
-{
-    BT_PROFILE("solveSplitImpulse");
-    return m_objective->m_projection.solveSplitImpulse(infoGlobal);
-}
-
-void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
-{
-     m_objective->m_projection.splitImpulseSetup(infoGlobal);
-}
-
 void btDeformableBodySolver::updateVelocity()
 {
 	int counter = 0;
@@ -271,7 +279,14 @@ void btDeformableBodySolver::updateVelocity()
 			{
 				m_dv[counter].setZero();
 			}
+			if (m_implicit)
+			{
 				psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
+			}
+			else
+			{
+				psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
+			}
 			psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
 			++counter;
 		}
@@ -291,7 +306,7 @@ void btDeformableBodySolver::updateTempPosition()
 		}
 		for (int j = 0; j < psb->m_nodes.size(); ++j)
 		{
-            psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
+			psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv);
 			++counter;
 		}
 		psb->updateDeformation();
@@ -326,15 +341,18 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
 		{
 			if (implicit)
 			{
-                if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON)
-                    m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
+				// setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint.
+				if (psb->m_nodes[j].m_v == m_backupVelocity[counter])
+					m_dv[counter].setZero();
 				else
 					m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
 				m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
 			}
 			else
-                m_dv[counter] =  psb->m_nodes[j].m_v - m_backupVelocity[counter];
-            psb->m_nodes[j].m_v = m_backupVelocity[counter] + psb->m_nodes[j].m_vsplit;
+			{
+				m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
+			}
+			psb->m_nodes[j].m_v = m_backupVelocity[counter];
 			++counter;
 		}
 	}
@@ -366,14 +384,31 @@ bool btDeformableBodySolver::updateNodes()
 	return false;
 }
 
-
 void btDeformableBodySolver::predictMotion(btScalar solverdt)
 {
 	// apply explicit forces to velocity
+	if (m_implicit)
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt;
+				}
+			}
+		}
+	}
 	m_objective->applyExplicitForce(m_residual);
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		btSoftBody* psb = m_softBodies[i];
+		/* Clear contacts */
+		psb->m_nodeRigidContacts.resize(0);
+		psb->m_faceRigidContacts.resize(0);
+		psb->m_faceNodeContacts.resize(0);
 
 		if (psb->isActive())
 		{
@@ -385,6 +420,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt)
 
 void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
 {
+	BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
 	int i, ni;
 
 	/* Update                */
@@ -417,6 +453,12 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
 		// apply drag
 		n.m_v *= (1 - psb->m_cfg.drag);
 		// scale velocity back
+		if (m_implicit)
+		{
+			n.m_q = n.m_x;
+		}
+		else
+		{
 			if (n.m_v.norm() > max_v)
 			{
 				n.m_v.safeNormalize();
@@ -424,42 +466,21 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
 			}
 			n.m_q = n.m_x + n.m_v * dt;
 		}
+		n.m_splitv.setZero();
+		n.m_constrained = false;
+	}
 
 	/* Nodes                */
-    ATTRIBUTE_ALIGNED16(btDbvtVolume)
-    vol;
-    for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
-    {
-        btSoftBody::Node& n = psb->m_nodes[i];
-        btVector3 points[2] = {n.m_x, n.m_q};
-        vol = btDbvtVolume::FromPoints(points, 2);
-        vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg));
-        psb->m_ndbvt.update(n.m_leaf, vol);
-    }
-
+	psb->updateNodeTree(true, true);
 	if (!psb->m_fdbvt.empty())
 	{
-        for (int i = 0; i < psb->m_faces.size(); ++i)
-        {
-            btSoftBody::Face& f = psb->m_faces[i];
-            btVector3 points[6] = {f.m_n[0]->m_x, f.m_n[0]->m_q,
-                                   f.m_n[1]->m_x, f.m_n[1]->m_q,
-                                   f.m_n[2]->m_x, f.m_n[2]->m_q};
-            vol = btDbvtVolume::FromPoints(points, 6);
-            vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg));
-            psb->m_fdbvt.update(f.m_leaf, vol);
+		psb->updateFaceTree(true, true);
 	}
-    }
-    /* Clear contacts        */
-    psb->m_nodeRigidContacts.resize(0);
-    psb->m_faceRigidContacts.resize(0);
-    psb->m_faceNodeContacts.resize(0);
 	/* Optimize dbvt's        */
-    psb->m_ndbvt.optimizeIncremental(1);
-    psb->m_fdbvt.optimizeIncremental(1);
+	//    psb->m_ndbvt.optimizeIncremental(1);
+	//    psb->m_fdbvt.optimizeIncremental(1);
 }
 
-
 void btDeformableBodySolver::updateSoftBodies()
 {
 	BT_PROFILE("updateSoftBodies");

thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h

@@ -16,13 +16,13 @@
 #ifndef BT_DEFORMABLE_BODY_SOLVERS_H
 #define BT_DEFORMABLE_BODY_SOLVERS_H
 
-
 #include "btSoftBodySolvers.h"
 #include "btDeformableBackwardEulerObjective.h"
 #include "btDeformableMultiBodyDynamicsWorld.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
-
+#include "btConjugateResidual.h"
+#include "btConjugateGradient.h"
 struct btCollisionObjectWrapper;
 class btDeformableBackwardEulerObjective;
 class btDeformableMultiBodyDynamicsWorld;
@@ -30,6 +30,7 @@ class btDeformableMultiBodyDynamicsWorld;
 class btDeformableBodySolver : public btSoftBodySolver
 {
 	typedef btAlignedObjectArray<btVector3> TVStack;
+
 protected:
 	int m_numNodes;                                                // total number of deformable body nodes
 	TVStack m_dv;                                                  // v_{n+1} - v_n
@@ -40,14 +41,15 @@ protected:
 	TVStack m_backupVelocity;                                      // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
 	btScalar m_dt;                                                 // dt
 	btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;  // CG solver
+	btConjugateResidual<btDeformableBackwardEulerObjective> m_cr;  // CR solver
 	bool m_implicit;                                               // use implicit scheme if true, explicit scheme if false
 	int m_maxNewtonIterations;                                     // max number of newton iterations
 	btScalar m_newtonTolerance;                                    // stop newton iterations if f(x) < m_newtonTolerance
 	bool m_lineSearch;                                             // If true, use newton's method with line search under implicit scheme
-    
 public:
 	// handles data related to objective function
 	btDeformableBackwardEulerObjective* m_objective;
+	bool m_useProjection;
 
 	btDeformableBodySolver();
 
@@ -61,23 +63,16 @@ public:
 	// update soft body normals
 	virtual void updateSoftBodies();
 
+	virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
 	// solve the momentum equation
 	virtual void solveDeformableConstraints(btScalar solverdt);
 
-    // solve the contact between deformable and rigid as well as among deformables
-    btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies);
-    
-    // solve the position error  between deformable and rigid as well as among deformables;
-    btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
-    
-    // set up the position error in split impulse
-    void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
-
 	// resize/clear data structures
 	void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
 
 	// set up contact constraints
-    void setConstraints();
+	void setConstraints(const btContactSolverInfo& infoGlobal);
 
 	// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
 	virtual void predictMotion(btScalar solverdt);
@@ -116,7 +111,8 @@ public:
 	}
 
 	// process collision between deformable and deformable
-    virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
+	virtual void processCollision(btSoftBody* softBody, btSoftBody* otherSoftBody)
+	{
 		softBody->defaultCollisionHandler(otherSoftBody);
 	}
 

thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -15,15 +15,13 @@
 
 #include "btDeformableContactConstraint.h"
 /* ================   Deformable Node Anchor   =================== */
-btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
-: m_anchor(&a)
-, btDeformableContactConstraint(a.m_cti.m_normal)
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal)
+	: m_anchor(&a), btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal)
 {
 }
 
 btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
-: m_anchor(other.m_anchor)
-, btDeformableContactConstraint(other)
+	: m_anchor(other.m_anchor), btDeformableContactConstraint(other)
 {
 }
 
@@ -79,14 +77,14 @@ btVector3 btDeformableNodeAnchorConstraint::getVa() const
 	return va;
 }
 
-btScalar btDeformableNodeAnchorConstraint::solveConstraint()
+btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
 	const btSoftBody::sCti& cti = m_anchor->m_cti;
 	btVector3 va = getVa();
 	btVector3 vb = getVb();
 	btVector3 vr = (vb - va);
 	// + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
-    const btScalar dn = btDot(vr, cti.m_normal);
+	const btScalar dn = btDot(vr, vr);
 	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
 	btScalar residualSquare = dn * dn;
 	btVector3 impulse = m_anchor->m_c0 * vr;
@@ -134,26 +132,24 @@ void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
 }
 
 /* ================   Deformable vs. Rigid   =================== */
-btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
-: m_contact(&c)
-, btDeformableContactConstraint(c.m_cti.m_normal)
+btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal)
+	: m_contact(&c), btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
 {
 	m_total_normal_dv.setZero();
 	m_total_tangent_dv.setZero();
-    // penetration is non-positive. The magnitude of penetration is the depth of penetration.
-    m_penetration = btMin(btScalar(0), c.m_cti.m_offset);
+	// The magnitude of penetration is the depth of penetration.
+	m_penetration = c.m_cti.m_offset;
+	m_total_split_impulse = 0;
+	m_binding = false;
 }
 
 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
-: m_contact(other.m_contact)
-, btDeformableContactConstraint(other)
-, m_penetration(other.m_penetration)
+	: m_contact(other.m_contact), btDeformableContactConstraint(other), m_penetration(other.m_penetration), m_total_split_impulse(other.m_total_split_impulse), m_binding(other.m_binding)
 {
 	m_total_normal_dv = other.m_total_normal_dv;
 	m_total_tangent_dv = other.m_total_tangent_dv;
 }
 
-
 btVector3 btDeformableRigidContactConstraint::getVa() const
 {
 	const btSoftBody::sCti& cti = m_contact->m_cti;
@@ -206,28 +202,96 @@ btVector3 btDeformableRigidContactConstraint::getVa() const
 	return va;
 }
 
-btScalar btDeformableRigidContactConstraint::solveConstraint()
+btVector3 btDeformableRigidContactConstraint::getSplitVa() const
+{
+	const btSoftBody::sCti& cti = m_contact->m_cti;
+	btVector3 va(0, 0, 0);
+	if (cti.m_colObj->hasContactResponse())
+	{
+		btRigidBody* rigidCol = 0;
+		btMultiBodyLinkCollider* multibodyLinkCol = 0;
+
+		// grab the velocity of the rigid body
+		if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+		{
+			rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+			va = rigidCol ? (rigidCol->getPushVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
+		}
+		else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+		{
+			multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+			if (multibodyLinkCol)
+			{
+				const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+				const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
+				const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
+				const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
+				const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector();
+				// add in the normal component of the va
+				btScalar vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_split_v[k] * J_n[k];
+				}
+				va = cti.m_normal * vel;
+				// add in the tangential components of the va
+				vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_split_v[k] * J_t1[k];
+				}
+				va += m_contact->t1 * vel;
+				vel = 0.0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_split_v[k] * J_t2[k];
+				}
+				va += m_contact->t2 * vel;
+			}
+		}
+	}
+	return va;
+}
+
+btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
 	const btSoftBody::sCti& cti = m_contact->m_cti;
 	btVector3 va = getVa();
 	btVector3 vb = getVb();
 	btVector3 vr = vb - va;
-    const btScalar dn = btDot(vr, cti.m_normal);
+	btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm;
+	if (m_penetration > 0)
+	{
+		dn += m_penetration / infoGlobal.m_timeStep;
+	}
+	if (!infoGlobal.m_splitImpulse)
+	{
+		dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+	}
 	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
-    btScalar residualSquare = dn*dn;
-    btVector3 impulse = m_contact->m_c0 * vr;
-    const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
+	btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
+	if (!infoGlobal.m_splitImpulse)
+	{
+		impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal);
+	}
+	btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
 	btVector3 impulse_tangent = impulse - impulse_normal;
+	if (dn > 0)
+	{
+		return 0;
+	}
+	m_binding = true;
+	btScalar residualSquare = dn * dn;
 	btVector3 old_total_tangent_dv = m_total_tangent_dv;
-    // m_c2 is the inverse mass of the deformable node/face
-    m_total_normal_dv -= impulse_normal * m_contact->m_c2;
-    m_total_tangent_dv -= impulse_tangent * m_contact->m_c2;
+	// m_c5 is the inverse mass of the deformable node/face
+	m_total_normal_dv -= m_contact->m_c5 * impulse_normal;
+	m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent;
 
 	if (m_total_normal_dv.dot(cti.m_normal) < 0)
 	{
 		// separating in the normal direction
+		m_binding = false;
 		m_static = false;
-        m_total_tangent_dv = btVector3(0,0,0);
 		impulse_tangent.setZero();
 	}
 	else
@@ -245,7 +309,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
 			{
 				m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
 			}
-            impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
+			//            impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
+			impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv);
 		}
 		else
 		{
@@ -290,16 +355,36 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
 
 btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
 {
+	btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction;
 	const btSoftBody::sCti& cti = m_contact->m_cti;
-    const btScalar dn = m_penetration;
-    if (dn != 0)
+	btVector3 vb = getSplitVb();
+	btVector3 va = getSplitVa();
+	btScalar p = m_penetration;
+	if (p > 0)
 	{
-        const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep));
-        // one iteration of the position impulse corrects all the position error at this timestep
-        m_penetration -= dn;
-        // apply impulse to deformable nodes involved and change their position
+		return 0;
+	}
+	btVector3 vr = vb - va;
+	btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+	if (dn > 0)
+	{
+		return 0;
+	}
+	if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION)
+	{
+		dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse;
+	}
+	if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION)
+	{
+		dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse;
+	}
+	m_total_split_impulse += dn;
+
+	btScalar residualSquare = dn * dn;
+	const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn);
 	applySplitImpulse(impulse);
-        // apply impulse to the rigid/multibodies involved and change their position
+
+	// apply split impulse to the rigid/multibodies involved and change their velocities
 	if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 	{
 		btRigidBody* rigidCol = 0;
@@ -311,23 +396,25 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo
 	}
 	else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
 	{
-            // todo xuchenhan@
+		btMultiBodyLinkCollider* multibodyLinkCol = 0;
+		multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+		if (multibodyLinkCol)
+		{
+			const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+			// apply normal component of the impulse
+			multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
 		}
-        return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep);
 	}
-    return 0;
+	return residualSquare;
 }
-
 /* ================   Node vs. Rigid   =================== */
-btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
-    : m_node(contact.m_node)
-    , btDeformableRigidContactConstraint(contact)
+btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
+	: m_node(contact.m_node), btDeformableRigidContactConstraint(contact, infoGlobal)
 {
 }
 
 btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other)
-: m_node(other.m_node)
-, btDeformableRigidContactConstraint(other)
+	: m_node(other.m_node), btDeformableRigidContactConstraint(other)
 {
 }
 
@@ -336,6 +423,10 @@ btVector3 btDeformableNodeRigidContactConstraint::getVb() const
 	return m_node->m_v;
 }
 
+btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const
+{
+	return m_node->m_splitv;
+}
 
 btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
 {
@@ -345,27 +436,25 @@ btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node*
 void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
 {
 	const btSoftBody::DeformableNodeRigidContact* contact = getContact();
-    btVector3 dv = impulse * contact->m_c2;
+	btVector3 dv = contact->m_c5 * impulse;
 	contact->m_node->m_v -= dv;
 }
 
 void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
 {
 	const btSoftBody::DeformableNodeRigidContact* contact = getContact();
-    btVector3 dv = impulse * contact->m_c2;
-    contact->m_node->m_vsplit -= dv;
-};
+	btVector3 dv = contact->m_c5 * impulse;
+	contact->m_node->m_splitv -= dv;
+}
 
 /* ================   Face vs. Rigid   =================== */
-btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
-: m_face(contact.m_face)
-, btDeformableRigidContactConstraint(contact)
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting)
+	: m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btDeformableRigidContactConstraint(contact, infoGlobal)
 {
 }
 
 btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
-: m_face(other.m_face)
-, btDeformableRigidContactConstraint(other)
+	: m_face(other.m_face), m_useStrainLimiting(other.m_useStrainLimiting), btDeformableRigidContactConstraint(other)
 {
 }
 
@@ -376,7 +465,6 @@ btVector3 btDeformableFaceRigidContactConstraint::getVb() const
 	return vb;
 }
 
-
 btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
 {
 	btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
@@ -411,47 +499,99 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
 		v1 -= dv * contact->m_weights[1];
 	if (im2 > 0)
 		v2 -= dv * contact->m_weights[2];
-    
+	if (m_useStrainLimiting)
+	{
+		btScalar relaxation = 1. / btScalar(m_infoGlobal->m_numIterations);
+		btScalar m01 = (relaxation / (im0 + im1));
+		btScalar m02 = (relaxation / (im0 + im2));
+		btScalar m12 = (relaxation / (im1 + im2));
+#ifdef USE_STRAIN_RATE_LIMITING
+		// apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%.
+		btScalar p = 0.01;
+		btVector3& x0 = face->m_n[0]->m_x;
+		btVector3& x1 = face->m_n[1]->m_x;
+		btVector3& x2 = face->m_n[2]->m_x;
+		const btVector3 x_diff[3] = {x1 - x0, x2 - x0, x2 - x1};
+		const btVector3 v_diff[3] = {v1 - v0, v2 - v0, v2 - v1};
+		btVector3 u[3];
+		btScalar x_diff_dot_u, dn[3];
+		btScalar dt = m_infoGlobal->m_timeStep;
+		for (int i = 0; i < 3; ++i)
+		{
+			btScalar x_diff_norm = x_diff[i].safeNorm();
+			btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm();
+			btScalar strainRate = x_diff_norm_new / x_diff_norm;
+			u[i] = v_diff[i];
+			u[i].safeNormalize();
+			if (x_diff_norm == 0 || (1 - p <= strainRate && strainRate <= 1 + p))
+			{
+				dn[i] = 0;
+				continue;
+			}
+			x_diff_dot_u = btDot(x_diff[i], u[i]);
+			btScalar s;
+			if (1 - p > strainRate)
+			{
+				s = 1 / dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p - 2 * p) * x_diff_norm * x_diff_norm));
+			}
+			else
+			{
+				s = 1 / dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p + 2 * p) * x_diff_norm * x_diff_norm));
+			}
+			//		x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm();
+			//		strainRate = x_diff_norm_new/x_diff_norm;
+			dn[i] = s - v_diff[i].safeNorm();
+		}
+		btVector3 dv0 = im0 * (m01 * u[0] * (-dn[0]) + m02 * u[1] * -(dn[1]));
+		btVector3 dv1 = im1 * (m01 * u[0] * (dn[0]) + m12 * u[2] * (-dn[2]));
+		btVector3 dv2 = im2 * (m12 * u[2] * (dn[2]) + m02 * u[1] * (dn[1]));
+#else
 		// apply strain limiting to prevent undamped modes
-    btScalar m01 = (btScalar(1)/(im0 + im1));
-    btScalar m02 = (btScalar(1)/(im0 + im2));
-    btScalar m12 = (btScalar(1)/(im1 + im2));
-    
 		btVector3 dv0 = im0 * (m01 * (v1 - v0) + m02 * (v2 - v0));
 		btVector3 dv1 = im1 * (m01 * (v0 - v1) + m12 * (v2 - v1));
 		btVector3 dv2 = im2 * (m12 * (v1 - v2) + m02 * (v0 - v2));
-    
+#endif
 		v0 += dv0;
 		v1 += dv1;
 		v2 += dv2;
 	}
+}
+
+btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2];
+	return vb;
+}
 
 void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
 {
 	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
 	btVector3 dv = impulse * contact->m_c2;
 	btSoftBody::Face* face = contact->m_face;
-
-    btVector3& v0 = face->m_n[0]->m_vsplit;
-    btVector3& v1 = face->m_n[1]->m_vsplit;
-    btVector3& v2 = face->m_n[2]->m_vsplit;
+	btVector3& v0 = face->m_n[0]->m_splitv;
+	btVector3& v1 = face->m_n[1]->m_splitv;
+	btVector3& v2 = face->m_n[2]->m_splitv;
 	const btScalar& im0 = face->m_n[0]->m_im;
 	const btScalar& im1 = face->m_n[1]->m_im;
 	const btScalar& im2 = face->m_n[2]->m_im;
 	if (im0 > 0)
+	{
 		v0 -= dv * contact->m_weights[0];
+	}
 	if (im1 > 0)
+	{
 		v1 -= dv * contact->m_weights[1];
+	}
 	if (im2 > 0)
+	{
 		v2 -= dv * contact->m_weights[2];
 	}
+}
 
 /* ================   Face vs. Node   =================== */
-btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
-: m_node(contact.m_node)
-, m_face(contact.m_face)
-, m_contact(&contact)
-, btDeformableContactConstraint(contact.m_normal)
+btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal)
+	: m_node(contact.m_node), m_face(contact.m_face), m_contact(&contact), btDeformableContactConstraint(contact.m_normal, infoGlobal)
 {
 	m_total_normal_dv.setZero();
 	m_total_tangent_dv.setZero();
@@ -487,7 +627,7 @@ btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n
 	return dv * contact->m_weights[2];
 }
 
-btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
+btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
 	btVector3 va = getVa();
 	btVector3 vb = getVb();
@@ -577,15 +717,4 @@ void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impuls
 	{
 		v2 -= dvb * contact->m_weights[2];
 	}
-    // todo: Face node constraints needs more work
-//    btScalar m01 = (btScalar(1)/(im0 + im1));
-//    btScalar m02 = (btScalar(1)/(im0 + im2));
-//    btScalar m12 = (btScalar(1)/(im1 + im2));
-//
-//    btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
-//    btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
-//    btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
-//    v0 += dv0;
-//    v1 += dv1;
-//    v2 += dv2;
 }

thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h

@@ -24,34 +24,31 @@ public:
 	// True if the friction is static
 	// False if the friction is dynamic
 	bool m_static;
+	const btContactSolverInfo* m_infoGlobal;
 
 	// normal of the contact
 	btVector3 m_normal;
 
-    btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal)
+	btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal)
 	{
 	}
 
-    btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal)
+	btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
 	{
 	}
 
+	btDeformableContactConstraint() {}
+
 	btDeformableContactConstraint(const btDeformableContactConstraint& other)
-    : m_static(other.m_static)
-    , m_normal(other.m_normal)
+		: m_static(other.m_static), m_normal(other.m_normal), m_infoGlobal(other.m_infoGlobal)
 	{
-        
 	}
-    btDeformableContactConstraint(){}
 
 	virtual ~btDeformableContactConstraint() {}
 
 	// solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence
 	// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
-    virtual btScalar solveConstraint() = 0;
-    
-    // solve the position error by applying an inelastic impulse that changes only the position (not velocity)
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0;
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
 
 	// get the velocity of the object A in the contact
 	virtual btVector3 getVa() const = 0;
@@ -65,9 +62,6 @@ public:
 	// apply impulse to the soft body node and/or face involved
 	virtual void applyImpulse(const btVector3& impulse) = 0;
 
-    // apply position based impulse to the soft body node and/or face involved
-    virtual void applySplitImpulse(const btVector3& impulse) = 0;
-    
 	// scale the penetration depth by erp
 	virtual void setPenetrationScale(btScalar scale) = 0;
 };
@@ -77,29 +71,20 @@ public:
 class btDeformableStaticConstraint : public btDeformableContactConstraint
 {
 public:
-    const btSoftBody::Node* m_node;
+	btSoftBody::Node* m_node;
 
-    btDeformableStaticConstraint(){}
-    
-    btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0))
+	btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal) : m_node(node), btDeformableContactConstraint(false, btVector3(0, 0, 0), infoGlobal)
 	{
 	}
-    
+	btDeformableStaticConstraint() {}
 	btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
-    : m_node(other.m_node)
-    , btDeformableContactConstraint(other)
+		: m_node(other.m_node), btDeformableContactConstraint(other)
 	{
-        
 	}
 
 	virtual ~btDeformableStaticConstraint() {}
 
-    virtual btScalar solveConstraint()
-    {
-        return 0;
-    }
-    
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
 	{
 		return 0;
 	}
@@ -120,7 +105,6 @@ public:
 	}
 
 	virtual void applyImpulse(const btVector3& impulse) {}
-    virtual void applySplitImpulse(const btVector3& impulse){}
 	virtual void setPenetrationScale(btScalar scale) {}
 };
 
@@ -131,18 +115,14 @@ class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
 public:
 	const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
 
-    btDeformableNodeAnchorConstraint(){}
-    btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c);
+	btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
 	btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
+	btDeformableNodeAnchorConstraint() {}
 	virtual ~btDeformableNodeAnchorConstraint()
 	{
 	}
-    virtual btScalar solveConstraint();
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
-    {
-        // todo xuchenhan@
-        return 0;
-    }
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+
 	// object A is the rigid/multi body, and object B is the deformable node/face
 	virtual btVector3 getVa() const;
 	// get the velocity of the deformable node in contact
@@ -152,14 +132,10 @@ public:
 		return btVector3(0, 0, 0);
 	}
 	virtual void applyImpulse(const btVector3& impulse);
-    virtual void applySplitImpulse(const btVector3& impulse)
-    {
-        // todo xuchenhan@
-    };
+
 	virtual void setPenetrationScale(btScalar scale) {}
 };
 
-
 //
 // Constraint between rigid/multi body and deformable objects
 class btDeformableRigidContactConstraint : public btDeformableContactConstraint
@@ -168,11 +144,13 @@ public:
 	btVector3 m_total_normal_dv;
 	btVector3 m_total_tangent_dv;
 	btScalar m_penetration;
+	btScalar m_total_split_impulse;
+	bool m_binding;
 	const btSoftBody::DeformableRigidContact* m_contact;
 
-    btDeformableRigidContactConstraint(){}
-    btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c);
+	btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
 	btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
+	btDeformableRigidContactConstraint() {}
 	virtual ~btDeformableRigidContactConstraint()
 	{
 	}
@@ -180,14 +158,22 @@ public:
 	// object A is the rigid/multi body, and object B is the deformable node/face
 	virtual btVector3 getVa() const;
 
-    virtual btScalar solveConstraint();
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const = 0;
 
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
+	// get the split impulse velocity of the rigid/multibdoy at the contaft
+	virtual btVector3 getSplitVa() const;
+
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
 
 	virtual void setPenetrationScale(btScalar scale)
 	{
 		m_penetration *= scale;
 	}
+
+	btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
+
+	virtual void applySplitImpulse(const btVector3& impulse) = 0;
 };
 
 //
@@ -196,12 +182,11 @@ class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactCo
 {
 public:
 	// the deformable node in contact
-    const btSoftBody::Node* m_node;
+	btSoftBody::Node* m_node;
 
-    btDeformableNodeRigidContactConstraint(){}
-    btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact);
+	btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
 	btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
-    
+	btDeformableNodeRigidContactConstraint() {}
 	virtual ~btDeformableNodeRigidContactConstraint()
 	{
 	}
@@ -209,6 +194,9 @@ public:
 	// get the velocity of the deformable node in contact
 	virtual btVector3 getVb() const;
 
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const;
+
 	// get the velocity change of the input soft body node in the constraint
 	virtual btVector3 getDv(const btSoftBody::Node*) const;
 
@@ -219,6 +207,7 @@ public:
 	}
 
 	virtual void applyImpulse(const btVector3& impulse);
+
 	virtual void applySplitImpulse(const btVector3& impulse);
 };
 
@@ -227,11 +216,11 @@ public:
 class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
 {
 public:
-    const btSoftBody::Face* m_face;
-    btDeformableFaceRigidContactConstraint(){}
-    btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact);
+	btSoftBody::Face* m_face;
+	bool m_useStrainLimiting;
+	btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting);
 	btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
-    
+	btDeformableFaceRigidContactConstraint() : m_useStrainLimiting(false) {}
 	virtual ~btDeformableFaceRigidContactConstraint()
 	{
 	}
@@ -239,6 +228,9 @@ public:
 	// get the velocity of the deformable face at the contact point
 	virtual btVector3 getVb() const;
 
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const;
+
 	// get the velocity change of the input soft body node in the constraint
 	virtual btVector3 getDv(const btSoftBody::Node*) const;
 
@@ -249,6 +241,7 @@ public:
 	}
 
 	virtual void applyImpulse(const btVector3& impulse);
+
 	virtual void applySplitImpulse(const btVector3& impulse);
 };
 
@@ -263,19 +256,11 @@ public:
 	btVector3 m_total_normal_dv;
 	btVector3 m_total_tangent_dv;
 
+	btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
 	btDeformableFaceNodeContactConstraint() {}
-    
-    btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact);
-    
 	virtual ~btDeformableFaceNodeContactConstraint() {}
 
-    virtual btScalar solveConstraint();
-    
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
-    {
-        // todo: xuchenhan@
-        return 0;
-    }
+	virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
 
 	// get the velocity of the object A in the contact
 	virtual btVector3 getVa() const;
@@ -293,10 +278,7 @@ public:
 	}
 
 	virtual void applyImpulse(const btVector3& impulse);
-    virtual void applySplitImpulse(const btVector3& impulse)
-    {
-        // todo xuchenhan@
-    }
+
 	virtual void setPenetrationScale(btScalar scale) {}
 };
 #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp

@@ -17,7 +17,7 @@
 #include "btDeformableMultiBodyDynamicsWorld.h"
 #include <algorithm>
 #include <cmath>
-btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
+btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
 	btScalar residualSquare = 0;
 	for (int i = 0; i < numDeformableBodies; ++i)
@@ -32,25 +32,25 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
 			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
 			{
 				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
 			{
 				btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
 			{
 				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
 			{
 				btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 		}
@@ -58,57 +58,36 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
 	return residualSquare;
 }
 
-void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
-{
-	for (int i = 0; i < m_softBodies.size(); ++i)
-	{
-		// node constraints
-		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
-		{
-			btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
-			constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
-		}
-		// face constraints
-		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-		{
-			btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
-			constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
-		}
-	}
-}
-
-btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
 	btScalar residualSquare = 0;
-	for (int i = 0; i < m_softBodies.size(); ++i)
+	for (int i = 0; i < numDeformableBodies; ++i)
 	{
-		// node constraints
-		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		for (int j = 0; j < m_softBodies.size(); ++j)
 		{
-			btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
+			btCollisionObject* psb = m_softBodies[j];
+			if (psb != deformableBodies[i])
+			{
+				continue;
+			}
+			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
+			{
+				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
 				btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
-		// anchor constraints
-		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
 			{
-			btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j];
+				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
 				btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
-		// face constraints
-		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
-		{
-			btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
-			btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
-			residualSquare = btMax(residualSquare, localResidualSquare);
 		}
-
 	}
 	return residualSquare;
 }
 
-void btDeformableContactProjection::setConstraints()
+void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("setConstraints");
 	for (int i = 0; i < m_softBodies.size(); ++i)
@@ -124,7 +103,7 @@ void btDeformableContactProjection::setConstraints()
 		{
 			if (psb->m_nodes[j].m_im == 0)
 			{
-				btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
+				btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal);
 				m_staticConstraints[i].push_back(static_constraint);
 			}
 		}
@@ -139,7 +118,7 @@ void btDeformableContactProjection::setConstraints()
 				continue;
 			}
 			anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
-			btDeformableNodeAnchorConstraint constraint(anchor);
+			btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
 			m_nodeAnchorConstraints[i].push_back(constraint);
 		}
 
@@ -152,17 +131,9 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-			btDeformableNodeRigidContactConstraint constraint(contact);
-			btVector3 va = constraint.getVa();
-			btVector3 vb = constraint.getVb();
-			const btVector3 vr = vb - va;
-			const btSoftBody::sCti& cti = contact.m_cti;
-			const btScalar dn = btDot(vr, cti.m_normal);
-			if (dn < SIMD_EPSILON)
-			{
+			btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
 			m_nodeRigidConstraints[i].push_back(constraint);
 		}
-		}
 
 		// set Deformable Face vs. Rigid constraint
 		for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
@@ -173,38 +144,15 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-			btDeformableFaceRigidContactConstraint constraint(contact);
-			btVector3 va = constraint.getVa();
-			btVector3 vb = constraint.getVb();
-			const btVector3 vr = vb - va;
-			const btSoftBody::sCti& cti = contact.m_cti;
-			const btScalar dn = btDot(vr, cti.m_normal);
-			if (dn < SIMD_EPSILON)
-			{
+			btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
 			m_faceRigidConstraints[i].push_back(constraint);
 		}
 	}
-		
-		// set Deformable Face vs. Deformable Node constraint
-		for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
-		{
-			const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
-
-			btDeformableFaceNodeContactConstraint constraint(contact);
-			btVector3 va = constraint.getVa();
-			btVector3 vb = constraint.getVb();
-			const btVector3 vr = vb - va;
-			const btScalar dn = btDot(vr, contact.m_normal);
-			if (dn > -SIMD_EPSILON)
-			{
-				m_deformableConstraints[i].push_back(constraint);
-			}
-		}
-	}
 }
 
 void btDeformableContactProjection::project(TVStack& x)
 {
+#ifndef USE_MGS
 	const int dim = 3;
 	for (int index = 0; index < m_projectionsDict.size(); ++index)
 	{
@@ -224,7 +172,6 @@ void btDeformableContactProjection::project(TVStack& x)
 			if (free_dir.safeNorm() < SIMD_EPSILON)
 			{
 				x[i] -= x[i].dot(dir0) * dir0;
-				x[i] -= x[i].dot(dir1) * dir1;
 			}
 			else
 			{
@@ -239,10 +186,23 @@ void btDeformableContactProjection::project(TVStack& x)
 			x[i] -= x[i].dot(dir0) * dir0;
 		}
 	}
+#else
+	btReducedVector p(x.size());
+	for (int i = 0; i < m_projections.size(); ++i)
+	{
+		p += (m_projections[i].dot(x) * m_projections[i]);
+	}
+	for (int i = 0; i < p.m_indices.size(); ++i)
+	{
+		x[p.m_indices[i]] -= p.m_vecs[i];
+	}
+#endif
 }
 
 void btDeformableContactProjection::setProjection()
 {
+#ifndef USE_MGS
+	BT_PROFILE("btDeformableContactProjection::setProjection");
 	btAlignedObjectArray<btVector3> units;
 	units.push_back(btVector3(1, 0, 0));
 	units.push_back(btVector3(0, 1, 0));
@@ -257,6 +217,7 @@ void btDeformableContactProjection::setProjection()
 		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
 		{
 			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_constrained = true;
 			if (m_projectionsDict.find(index) == NULL)
 			{
 				m_projectionsDict.insert(index, units);
@@ -273,6 +234,7 @@ void btDeformableContactProjection::setProjection()
 		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
 		{
 			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
 			if (m_projectionsDict.find(index) == NULL)
 			{
 				m_projectionsDict.insert(index, units);
@@ -289,6 +251,9 @@ void btDeformableContactProjection::setProjection()
 		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
 		{
 			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
+			if (m_nodeRigidConstraints[i][j].m_binding)
+			{
 				if (m_nodeRigidConstraints[i][j].m_static)
 				{
 					if (m_projectionsDict.find(index) == NULL)
@@ -319,12 +284,20 @@ void btDeformableContactProjection::setProjection()
 					}
 				}
 			}
+		}
 		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
 		{
 			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+			if (m_faceRigidConstraints[i][j].m_binding)
+			{
 				for (int k = 0; k < 3; ++k)
 				{
-				const btSoftBody::Node* node = face->m_n[k];
+					face->m_n[k]->m_constrained = true;
+				}
+			}
+			for (int k = 0; k < 3; ++k)
+			{
+				btSoftBody::Node* node = face->m_n[k];
 				int index = node->index;
 				if (m_faceRigidConstraints[i][j].m_static)
 				{
@@ -335,9 +308,9 @@ void btDeformableContactProjection::setProjection()
 					else
 					{
 						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-						for (int k = 0; k < 3; ++k)
+						for (int l = 0; l < 3; ++l)
 						{
-							projections.push_back(units[k]);
+							projections.push_back(units[l]);
 						}
 					}
 				}
@@ -357,80 +330,236 @@ void btDeformableContactProjection::setProjection()
 				}
 			}
 		}
-		for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
+	}
+#else
+	int dof = 0;
+	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
-			const btSoftBody::Face* face = m_deformableConstraints[i][j].m_face;
+		dof += m_softBodies[i]->m_nodes.size();
+	}
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
+			btAlignedObjectArray<int> indices;
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			indices.push_back(index);
+			vecs1.push_back(btVector3(1, 0, 0));
+			vecs2.push_back(btVector3(0, 1, 0));
+			vecs3.push_back(btVector3(0, 0, 1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs2));
+			m_projections.push_back(btReducedVector(dof, indices, vecs3));
+		}
+
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
+			btAlignedObjectArray<int> indices;
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			indices.push_back(index);
+			vecs1.push_back(btVector3(1, 0, 0));
+			vecs2.push_back(btVector3(0, 1, 0));
+			vecs3.push_back(btVector3(0, 0, 1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			m_projections.push_back(btReducedVector(dof, indices, vecs2));
+			m_projections.push_back(btReducedVector(dof, indices, vecs3));
+		}
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			btAlignedObjectArray<int> indices;
+			indices.push_back(index);
+			btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
+			if (m_nodeRigidConstraints[i][j].m_static)
+			{
+				vecs1.push_back(btVector3(1, 0, 0));
+				vecs2.push_back(btVector3(0, 1, 0));
+				vecs3.push_back(btVector3(0, 0, 1));
+				m_projections.push_back(btReducedVector(dof, indices, vecs1));
+				m_projections.push_back(btReducedVector(dof, indices, vecs2));
+				m_projections.push_back(btReducedVector(dof, indices, vecs3));
+			}
+			else
+			{
+				vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
+				m_projections.push_back(btReducedVector(dof, indices, vecs1));
+			}
+		}
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+			btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
+			btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
 			for (int k = 0; k < 3; ++k)
 			{
-				const btSoftBody::Node* node = face->m_n[k];
-				int index = node->index;
-				if (m_deformableConstraints[i][j].m_static)
-				{
-					if (m_projectionsDict.find(index) == NULL)
-					{
-						m_projectionsDict.insert(index, units);
+				face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
 			}
-					else
+			if (m_faceRigidConstraints[i][j].m_static)
 			{
-						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
+				for (int l = 0; l < 3; ++l)
+				{
+					btReducedVector rv(dof);
 					for (int k = 0; k < 3; ++k)
 					{
-							projections.push_back(units[k]);
+						rv.m_indices.push_back(face->m_n[k]->index);
+						btVector3 v(0, 0, 0);
+						v[l] = bary[k];
+						rv.m_vecs.push_back(v);
+						rv.sort();
 					}
+					m_projections.push_back(rv);
 				}
 			}
 			else
 			{
-					if (m_projectionsDict.find(index) == NULL)
+				btReducedVector rv(dof);
+				for (int k = 0; k < 3; ++k)
 				{
-						btAlignedObjectArray<btVector3> projections;
-						projections.push_back(m_deformableConstraints[i][j].m_normal);
-						m_projectionsDict.insert(index, projections);
+					rv.m_indices.push_back(face->m_n[k]->index);
+					rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
+					rv.sort();
+				}
+				m_projections.push_back(rv);
+			}
+		}
+	}
+	btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
+	mgs.solve();
+	m_projections = mgs.m_out;
+#endif
+}
+
+void btDeformableContactProjection::checkConstraints(const TVStack& x)
+{
+	for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
+	{
+		btVector3 d(0, 0, 0);
+		const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
+		for (int j = 0; j < lm.m_num_constraints; ++j)
+		{
+			for (int k = 0; k < lm.m_num_nodes; ++k)
+			{
+				d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
+			}
+		}
+		//		printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
+		//        printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]);
+	}
+}
+
+void btDeformableContactProjection::setLagrangeMultiplier()
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (!psb->isActive())
+		{
+			continue;
+		}
+		for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+		{
+			int index = m_staticConstraints[i][j].m_node->index;
+			m_staticConstraints[i][j].m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			lm.m_num_constraints = 3;
+			lm.m_dirs[0] = btVector3(1, 0, 0);
+			lm.m_dirs[1] = btVector3(0, 1, 0);
+			lm.m_dirs[2] = btVector3(0, 0, 1);
+			m_lagrangeMultipliers.push_back(lm);
+		}
+		for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
+		{
+			int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			lm.m_num_constraints = 3;
+			lm.m_dirs[0] = btVector3(1, 0, 0);
+			lm.m_dirs[1] = btVector3(0, 1, 0);
+			lm.m_dirs[2] = btVector3(0, 0, 1);
+			m_lagrangeMultipliers.push_back(lm);
+		}
+
+		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+		{
+			if (!m_nodeRigidConstraints[i][j].m_binding)
+			{
+				continue;
+			}
+			int index = m_nodeRigidConstraints[i][j].m_node->index;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 1;
+			lm.m_indices[0] = index;
+			lm.m_weights[0] = 1.0;
+			if (m_nodeRigidConstraints[i][j].m_static)
+			{
+				lm.m_num_constraints = 3;
+				lm.m_dirs[0] = btVector3(1, 0, 0);
+				lm.m_dirs[1] = btVector3(0, 1, 0);
+				lm.m_dirs[2] = btVector3(0, 0, 1);
 			}
 			else
 			{
-						btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-						projections.push_back(m_deformableConstraints[i][j].m_normal);
+				lm.m_num_constraints = 1;
+				lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
+			}
+			m_lagrangeMultipliers.push_back(lm);
+		}
+
+		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+		{
+			if (!m_faceRigidConstraints[i][j].m_binding)
+			{
+				continue;
+			}
+			btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+
+			btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
+			LagrangeMultiplier lm;
+			lm.m_num_nodes = 3;
+
+			for (int k = 0; k < 3; ++k)
+			{
+				face->m_n[k]->m_constrained = true;
+				lm.m_indices[k] = face->m_n[k]->index;
+				lm.m_weights[k] = bary[k];
+			}
+			if (m_faceRigidConstraints[i][j].m_static)
+			{
+				face->m_pcontact[3] = 1;
+				lm.m_num_constraints = 3;
+				lm.m_dirs[0] = btVector3(1, 0, 0);
+				lm.m_dirs[1] = btVector3(0, 1, 0);
+				lm.m_dirs[2] = btVector3(0, 0, 1);
+			}
+			else
+			{
+				face->m_pcontact[3] = 0;
+				lm.m_num_constraints = 1;
+				lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
+			}
+			m_lagrangeMultipliers.push_back(lm);
 		}
 	}
 }
 
-			const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node;
-			int index = node->index;
-			if (m_deformableConstraints[i][j].m_static)
-			{
-				if (m_projectionsDict.find(index) == NULL)
-				{
-					m_projectionsDict.insert(index, units);
-				}
-				else
-				{
-					btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-					for (int k = 0; k < 3; ++k)
-					{
-						projections.push_back(units[k]);
-					}
-				}
-			}
-			else
-			{
-				if (m_projectionsDict.find(index) == NULL)
-				{
-					btAlignedObjectArray<btVector3> projections;
-					projections.push_back(m_deformableConstraints[i][j].m_normal);
-					m_projectionsDict.insert(index, projections);
-				}
-				else
-				{
-					btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
-					projections.push_back(m_deformableConstraints[i][j].m_normal);
-				}
-			}
-		}
-	}
-}
-
-
+//
 void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 {
 	for (int i = 0; i < m_softBodies.size(); ++i)
@@ -492,7 +621,6 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 		m_nodeRigidConstraints.resize(N);
 		m_faceRigidConstraints.resize(N);
 		m_deformableConstraints.resize(N);
-		
 	}
 	for (int i = 0; i < N; ++i)
 	{
@@ -502,8 +630,10 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 		m_faceRigidConstraints[i].clear();
 		m_deformableConstraints[i].clear();
 	}
+#ifndef USE_MGS
 	m_projectionsDict.clear();
+#else
+	m_projections.clear();
+#endif
+	m_lagrangeMultipliers.clear();
 }
-
-
-

thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h

@@ -21,29 +21,35 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include "btDeformableContactConstraint.h"
 #include "LinearMath/btHashMap.h"
+#include "LinearMath/btReducedVector.h"
+#include "LinearMath/btModifiedGramSchmidt.h"
 #include <vector>
+
+struct LagrangeMultiplier
+{
+	int m_num_constraints;  // Number of constraints
+	int m_num_nodes;        // Number of nodes in these constraints
+	btScalar m_weights[3];  // weights of the nodes involved, same size as m_num_nodes
+	btVector3 m_dirs[3];    // Constraint directions, same size of m_num_constraints;
+	int m_indices[3];       // indices of the nodes involved, same size as m_num_nodes;
+};
+
 class btDeformableContactProjection
 {
 public:
 	typedef btAlignedObjectArray<btVector3> TVStack;
 	btAlignedObjectArray<btSoftBody*>& m_softBodies;
 
-//    // map from node index to static constraint
-//    btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
-//    // map from node index to node rigid constraint
-//    btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
-//    // map from node index to face rigid constraint
-//    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
-//    // map from node index to deformable constraint
-//    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
-//    // map from node index to node anchor constraint
-//    btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
-	
 	// all constraints involving face
 	btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
-    
+#ifndef USE_MGS
 	// map from node index to projection directions
 	btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
+#else
+	btAlignedObjectArray<btReducedVector> m_projections;
+#endif
+
+	btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
 
 	// map from node index to static constraint
 	btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
@@ -56,6 +62,8 @@ public:
 	// map from node index to node anchor constraint
 	btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
 
+	bool m_useStrainLimiting;
+
 	btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies)
 		: m_softBodies(softBodies)
 	{
@@ -72,19 +80,20 @@ public:
 	virtual void applyDynamicFriction(TVStack& f);
 
 	// update and solve the constraints
-    virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies);
-    
-    // solve the position error using split impulse
-    virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
+	virtual btScalar update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
 
 	// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
-    virtual void setConstraints();
+	virtual void setConstraints(const btContactSolverInfo& infoGlobal);
 
 	// Set up projections for each vertex by adding the projection direction to
 	virtual void setProjection();
 
 	virtual void reinitialize(bool nodeUpdated);
 
-    virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
+	btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
+	virtual void setLagrangeMultiplier();
+
+	void checkConstraints(const TVStack& x);
 };
 #endif /* btDeformableContactProjection_h */

thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h

@@ -32,7 +32,6 @@ public:
 	btScalar m_mu, m_lambda;
 	btDeformableCorotatedForce() : m_mu(1), m_lambda(1)
 	{
-        
 	}
 
 	btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda)
@@ -114,12 +113,12 @@ public:
 	{
 	}
 
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
 	virtual btDeformableLagrangianForceType getForceType()
 	{
 		return BT_COROTATED_FORCE;
 	}
-    
 };
 
-
 #endif /* btCorotated_h */

thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h

@@ -50,6 +50,8 @@ public:
 	{
 	}
 
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
 	virtual void addScaledGravityForce(btScalar scale, TVStack& force)
 	{
 		int numNodes = getNumNodes();
@@ -66,7 +68,7 @@ public:
 				btSoftBody::Node& n = psb->m_nodes[j];
 				size_t id = n.index;
 				btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
-                btVector3 scaled_force = scale * m_gravity * mass;
+				btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor;
 				force[id] += scaled_force;
 			}
 		}
@@ -99,7 +101,5 @@ public:
 		}
 		return e;
 	}
-    
-    
 };
 #endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */

thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h

@@ -26,7 +26,8 @@ enum btDeformableLagrangianForceType
 	BT_MASSSPRING_FORCE = 2,
 	BT_COROTATED_FORCE = 3,
 	BT_NEOHOOKEAN_FORCE = 4,
-    BT_LINEAR_ELASTICITY_FORCE = 5
+	BT_LINEAR_ELASTICITY_FORCE = 5,
+	BT_MOUSE_PICKING_FORCE = 6
 };
 
 static inline double randomDouble(double low, double high)
@@ -53,6 +54,9 @@ public:
 	// add damping df
 	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
 
+	// build diagonal of A matrix
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
+
 	// add elastic df
 	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
 
@@ -62,6 +66,8 @@ public:
 	// add all damping forces
 	virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
 
+	virtual void addScaledHessian(btScalar scale) {}
+
 	virtual btDeformableLagrangianForceType getForceType() = 0;
 
 	virtual void reinitialize(bool nodeUpdated)
@@ -85,6 +91,11 @@ public:
 		m_softBodies.push_back(psb);
 	}
 
+	virtual void removeSoftBody(btSoftBody* psb)
+	{
+		m_softBodies.remove(psb);
+	}
+
 	virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
 	{
 		m_nodes = nodes;
@@ -169,7 +180,6 @@ public:
 				dphi += dphi_dx[i].dot(dx[i]);
 			}
 
-
 			for (int i = 0; i < m_softBodies.size(); ++i)
 			{
 				btSoftBody* psb = m_softBodies[i];
@@ -232,7 +242,6 @@ public:
 			psb->updateDeformation();
 		}
 
-        
 		TVStack dx;
 		dx.resize(getNumNodes());
 		TVStack df;
@@ -242,7 +251,6 @@ public:
 		TVStack f2;
 		f2.resize(dx.size());
 
-        
 		// write down the current position
 		TVStack x;
 		x.resize(dx.size());

thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h

@@ -18,23 +18,64 @@
 
 #include "btDeformableLagrangianForce.h"
 #include "LinearMath/btQuickprof.h"
+#include "btSoftBodyInternals.h"
+#define TETRA_FLAT_THRESHOLD 0.01
 class btDeformableLinearElasticityForce : public btDeformableLagrangianForce
 {
 public:
 	typedef btAlignedObjectArray<btVector3> TVStack;
 	btScalar m_mu, m_lambda;
-    btScalar m_mu_damp, m_lambda_damp;
-    btDeformableLinearElasticityForce(): m_mu(1), m_lambda(1)
+	btScalar m_E, m_nu;  // Young's modulus and Poisson ratio
+	btScalar m_damping_alpha, m_damping_beta;
+	btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1), m_damping_alpha(0.01), m_damping_beta(0.01)
 	{
-        btScalar damping = 0.05;
-        m_mu_damp = damping * m_mu;
-        m_lambda_damp = damping * m_lambda;
+		updateYoungsModulusAndPoissonRatio();
 	}
 
-    btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
+	btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping_alpha = 0.01, btScalar damping_beta = 0.01) : m_mu(mu), m_lambda(lambda), m_damping_alpha(damping_alpha), m_damping_beta(damping_beta)
 	{
-        m_mu_damp = damping * m_mu;
-        m_lambda_damp = damping * m_lambda;
+		updateYoungsModulusAndPoissonRatio();
+	}
+
+	void updateYoungsModulusAndPoissonRatio()
+	{
+		// conversion from Lame Parameters to Young's modulus and Poisson ratio
+		// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
+		m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
+		m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
+	}
+
+	void updateLameParameters()
+	{
+		// conversion from Young's modulus and Poisson ratio to Lame Parameters
+		// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
+		m_mu = m_E * 0.5 / (1 + m_nu);
+		m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
+	}
+
+	void setYoungsModulus(btScalar E)
+	{
+		m_E = E;
+		updateLameParameters();
+	}
+
+	void setPoissonRatio(btScalar nu)
+	{
+		m_nu = nu;
+		updateLameParameters();
+	}
+
+	void setDamping(btScalar damping_alpha, btScalar damping_beta)
+	{
+		m_damping_alpha = damping_alpha;
+		m_damping_beta = damping_beta;
+	}
+
+	void setLameParameters(btScalar mu, btScalar lambda)
+	{
+		m_mu = mu;
+		m_lambda = lambda;
+		updateYoungsModulusAndPoissonRatio();
 	}
 
 	virtual void addScaledForces(btScalar scale, TVStack& force)
@@ -51,8 +92,10 @@ public:
 	// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
 	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
 	{
-        if (m_mu_damp == 0 && m_lambda_damp == 0)
+		if (m_damping_alpha == 0 && m_damping_beta == 0)
 			return;
+		btScalar mu_damp = m_damping_beta * m_mu;
+		btScalar lambda_damp = m_damping_beta * m_lambda;
 		int numNodes = getNumNodes();
 		btAssert(numNodes <= force.size());
 		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
@@ -65,6 +108,7 @@ public:
 			}
 			for (int j = 0; j < psb->m_tetras.size(); ++j)
 			{
+				bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD);
 				btSoftBody::Tetra& tetra = psb->m_tetras[j];
 				btSoftBody::Node* node0 = tetra.m_n[0];
 				btSoftBody::Node* node1 = tetra.m_n[1];
@@ -75,13 +119,19 @@ public:
 				size_t id2 = node2->index;
 				size_t id3 = node3->index;
 				btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
+				if (!close_to_flat)
+				{
+					dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF;
+				}
 				btMatrix3x3 I;
 				I.setIdentity();
-                btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
-                //                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
-                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+				btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp);
 				btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
-                
+				if (!close_to_flat)
+				{
+					df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123;
+				}
+				btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
 				// damping force differential
 				btScalar scale1 = scale * tetra.m_element_measure;
 				force[id0] -= scale1 * df_on_node0;
@@ -89,6 +139,15 @@ public:
 				force[id2] -= scale1 * df_on_node123.getColumn(1);
 				force[id3] -= scale1 * df_on_node123.getColumn(2);
 			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				size_t id = node.index;
+				if (node.m_im > 0)
+				{
+					force[id] -= scale * node.m_v / node.m_im * m_damping_alpha;
+				}
+			}
 		}
 	}
 
@@ -201,7 +260,7 @@ public:
 				}
 #endif
 				//                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
-                btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
+				btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose();
 				btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
 
 				btSoftBody::Node* node0 = tetra.m_n[0];
@@ -223,11 +282,15 @@ public:
 		}
 	}
 
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
 	// The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
 	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
 	{
-        if (m_mu_damp == 0 && m_lambda_damp == 0)
+		if (m_damping_alpha == 0 && m_damping_beta == 0)
 			return;
+		btScalar mu_damp = m_damping_beta * m_mu;
+		btScalar lambda_damp = m_damping_beta * m_lambda;
 		int numNodes = getNumNodes();
 		btAssert(numNodes <= df.size());
 		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
@@ -240,6 +303,7 @@ public:
 			}
 			for (int j = 0; j < psb->m_tetras.size(); ++j)
 			{
+				bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD);
 				btSoftBody::Tetra& tetra = psb->m_tetras[j];
 				btSoftBody::Node* node0 = tetra.m_n[0];
 				btSoftBody::Node* node1 = tetra.m_n[1];
@@ -250,12 +314,18 @@ public:
 				size_t id2 = node2->index;
 				size_t id3 = node3->index;
 				btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
+				if (!close_to_flat)
+				{
+					dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF;
+				}
 				btMatrix3x3 I;
 				I.setIdentity();
-                btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
-                //                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
-                //                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+				btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp);
 				btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+				if (!close_to_flat)
+				{
+					df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123;
+				}
 				btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
 
 				// damping force differential
@@ -265,6 +335,15 @@ public:
 				df[id2] -= scale1 * df_on_node123.getColumn(1);
 				df[id3] -= scale1 * df_on_node123.getColumn(2);
 			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				size_t id = node.index;
+				if (node.m_im > 0)
+				{
+					df[id] -= scale * dv[id] / node.m_im * m_damping_alpha;
+				}
+			}
 		}
 	}
 
@@ -291,11 +370,11 @@ public:
 				size_t id1 = node1->index;
 				size_t id2 = node2->index;
 				size_t id3 = node3->index;
-                btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
+				btMatrix3x3 dF = psb->m_tetraScratches[j].m_corotation.transpose() * Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
 				btMatrix3x3 dP;
 				firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
 				//                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
-                btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+				btMatrix3x3 df_on_node123 = psb->m_tetraScratches[j].m_corotation * dP * tetra.m_Dm_inverse.transpose();
 				btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
 
 				// elastic force differential
@@ -310,7 +389,9 @@ public:
 
 	void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
 	{
-        btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
+		btMatrix3x3 corotated_F = s.m_corotation.transpose() * s.m_F;
+
+		btMatrix3x3 epsilon = (corotated_F + corotated_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
 		btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
 		P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
 	}
@@ -327,14 +408,55 @@ public:
 	// This function calculates the dP = dQ/dF * dF
 	void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
 	{
+		btScalar mu_damp = m_damping_beta * m_mu;
+		btScalar lambda_damp = m_damping_beta * m_lambda;
 		btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
-        dP = (dF + dF.transpose()) * m_mu_damp +  btMatrix3x3::getIdentity()  * m_lambda_damp * trace;
+		dP = (dF + dF.transpose()) * mu_damp + btMatrix3x3::getIdentity() * lambda_damp * trace;
+	}
+
+	virtual void addScaledHessian(btScalar scale)
+	{
+		btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			for (int j = 0; j < psb->m_tetras.size(); ++j)
+			{
+				btSoftBody::Tetra& tetra = psb->m_tetras[j];
+				btMatrix3x3 P;
+				firstPiola(psb->m_tetraScratches[j], P);  // make sure scratch is evaluated at x_n + dt * vn
+				btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose();
+				btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
+				btSoftBody::Node* node0 = tetra.m_n[0];
+				btSoftBody::Node* node1 = tetra.m_n[1];
+				btSoftBody::Node* node2 = tetra.m_n[2];
+				btSoftBody::Node* node3 = tetra.m_n[3];
+				btScalar scale1 = scale * (scale + m_damping_beta) * tetra.m_element_measure;  // stiff and stiffness-damping terms;
+				node0->m_effectiveMass += OuterProduct(force_on_node0, force_on_node0) * scale1;
+				node1->m_effectiveMass += OuterProduct(force_on_node123.getColumn(0), force_on_node123.getColumn(0)) * scale1;
+				node2->m_effectiveMass += OuterProduct(force_on_node123.getColumn(1), force_on_node123.getColumn(1)) * scale1;
+				node3->m_effectiveMass += OuterProduct(force_on_node123.getColumn(2), force_on_node123.getColumn(2)) * scale1;
+			}
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				btSoftBody::Node& node = psb->m_nodes[j];
+				if (node.m_im > 0)
+				{
+					btMatrix3x3 I;
+					I.setIdentity();
+					node.m_effectiveMass += I * (scale * (1.0 / node.m_im) * m_damping_alpha);
+				}
+			}
+		}
 	}
 
 	virtual btDeformableLagrangianForceType getForceType()
 	{
 		return BT_LINEAR_ELASTICITY_FORCE;
 	}
-    
 };
 #endif /* BT_LINEAR_ELASTICITY_H */

thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h

@@ -24,6 +24,7 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce
 	// If false, the damping force will be in the direction of the velocity
 	bool m_momentum_conserving;
 	btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness;
+
 public:
 	typedef btAlignedObjectArray<btVector3> TVStack;
 	btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
@@ -149,6 +150,52 @@ public:
 		}
 	}
 
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
+	{
+		// implicit damping force differential
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (!psb->isActive())
+			{
+				continue;
+			}
+			btScalar scaled_k_damp = m_dampingStiffness * scale;
+			for (int j = 0; j < psb->m_links.size(); ++j)
+			{
+				const btSoftBody::Link& link = psb->m_links[j];
+				btSoftBody::Node* node1 = link.m_n[0];
+				btSoftBody::Node* node2 = link.m_n[1];
+				size_t id1 = node1->index;
+				size_t id2 = node2->index;
+				if (m_momentum_conserving)
+				{
+					if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
+					{
+						btVector3 dir = (node2->m_x - node1->m_x).normalized();
+						for (int d = 0; d < 3; ++d)
+						{
+							if (node1->m_im > 0)
+								diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
+							if (node2->m_im > 0)
+								diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
+						}
+					}
+				}
+				else
+				{
+					for (int d = 0; d < 3; ++d)
+					{
+						if (node1->m_im > 0)
+							diagA[id1][d] -= scaled_k_damp;
+						if (node2->m_im > 0)
+							diagA[id2][d] -= scaled_k_damp;
+					}
+				}
+			}
+		}
+	}
+
 	virtual double totalElasticEnergy(btScalar dt)
 	{
 		double energy = 0;
@@ -249,7 +296,6 @@ public:
 	{
 		return BT_MASSSPRING_FORCE;
 	}
-    
 };
 
 #endif /* btMassSpring_h */

thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h

@@ -0,0 +1,162 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_MOUSE_PICKING_FORCE_H
+#define BT_MOUSE_PICKING_FORCE_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableMousePickingForce : public btDeformableLagrangianForce
+{
+	// If true, the damping force will be in the direction of the spring
+	// If false, the damping force will be in the direction of the velocity
+	btScalar m_elasticStiffness, m_dampingStiffness;
+	const btSoftBody::Face& m_face;
+	btVector3 m_mouse_pos;
+	btScalar m_maxForce;
+
+public:
+	typedef btAlignedObjectArray<btVector3> TVStack;
+	btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, const btVector3& mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce)
+	{
+	}
+
+	virtual void addScaledForces(btScalar scale, TVStack& force)
+	{
+		addScaledDampingForce(scale, force);
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+	{
+		addScaledElasticForce(scale, force);
+	}
+
+	virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+	{
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 v_diff = m_face.m_n[i]->m_v;
+			btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
+			if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+			{
+				btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+				scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
+			}
+			force[m_face.m_n[i]->index] -= scaled_force;
+		}
+	}
+
+	virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+	{
+		btScalar scaled_stiffness = scale * m_elasticStiffness;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+			btVector3 scaled_force = scaled_stiffness * dir;
+			if (scaled_force.safeNorm() > m_maxForce)
+			{
+				scaled_force.safeNormalize();
+				scaled_force *= m_maxForce;
+			}
+			force[m_face.m_n[i]->index] -= scaled_force;
+		}
+	}
+
+	virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+	{
+		btScalar scaled_k_damp = m_dampingStiffness * scale;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index];
+			if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+			{
+				btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+				local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir;
+			}
+			df[m_face.m_n[i]->index] -= local_scaled_df;
+		}
+	}
+
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
+	virtual double totalElasticEnergy(btScalar dt)
+	{
+		double energy = 0;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+			btVector3 scaled_force = m_elasticStiffness * dir;
+			if (scaled_force.safeNorm() > m_maxForce)
+			{
+				scaled_force.safeNormalize();
+				scaled_force *= m_maxForce;
+			}
+			energy += 0.5 * scaled_force.dot(dir);
+		}
+		return energy;
+	}
+
+	virtual double totalDampingEnergy(btScalar dt)
+	{
+		double energy = 0;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 v_diff = m_face.m_n[i]->m_v;
+			btVector3 scaled_force = m_dampingStiffness * v_diff;
+			if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+			{
+				btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+				scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir;
+			}
+			energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt;
+		}
+		return energy;
+	}
+
+	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+	{
+		btScalar scaled_stiffness = scale * m_elasticStiffness;
+		for (int i = 0; i < 3; ++i)
+		{
+			btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+			btScalar dir_norm = dir.norm();
+			btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
+			int id = m_face.m_n[i]->index;
+			btVector3 dx_diff = dx[id];
+			btScalar r = 0;  // rest length is 0 for picking spring
+			btVector3 scaled_df = btVector3(0, 0, 0);
+			if (dir_norm > SIMD_EPSILON)
+			{
+				scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized;
+				scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
+				scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff;
+			}
+			df[id] += scaled_df;
+		}
+	}
+
+	void setMousePos(const btVector3& p)
+	{
+		m_mouse_pos = p;
+	}
+
+	virtual btDeformableLagrangianForceType getForceType()
+	{
+		return BT_MOUSE_PICKING_FORCE;
+	}
+};
+
+#endif /* btMassSpring_h */

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -13,7 +13,6 @@
  3. This notice may not be removed or altered from any source distribution.
  */
 
-
 #include "btDeformableMultiBodyConstraintSolver.h"
 #include <iostream>
 // override the iterations method to include deformable/multibody contact
@@ -21,7 +20,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
 {
 	{
 		///this is a special step to resolve penetrations (just for contacts)
-        solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+		solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
 		for (int iteration = 0; iteration < maxIterations; iteration++)
@@ -32,7 +31,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
 			m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 			// solver body velocity -> rigid body velocity
 			solverBodyWriteBack(infoGlobal);
-            btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies);
+			btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal);
 			// update rigid body velocity in rigid/deformable contact
 			m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
 			// solver body velocity <- rigid body velocity
@@ -41,6 +40,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
 			if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
 			{
 #ifdef VERBOSE_RESIDUAL_PRINTF
+				if (iteration >= (maxIterations - 1))
 					printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
 #endif
 				m_analyticsData.m_numSolverCalls++;
@@ -105,14 +105,13 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
 	}
 }
 
-void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
 	int iteration;
 	if (infoGlobal.m_splitImpulse)
 	{
 		{
-            m_deformableSolver->splitImpulseSetup(infoGlobal);
 			for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
 			{
 				btScalar leastSquaresResidual = 0.f;
@@ -127,13 +126,15 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI
 						leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 					}
 					// solve the position correction between deformable and rigid/multibody
-                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
+					//                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
+					btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
 					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 				}
 				if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
 				{
 #ifdef VERBOSE_RESIDUAL_PRINTF
-                    printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
+					if (iteration >= (infoGlobal.m_numIterations - 1))
+						printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
 #endif
 					break;
 				}

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h

@@ -13,7 +13,6 @@
  3. This notice may not be removed or altered from any source distribution.
  */
 
-
 #ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
 #define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
 
@@ -44,9 +43,10 @@ protected:
 	// write the velocity of the underlying rigid body to the the the solver body
 	void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
-    virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
 	virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -22,7 +22,6 @@ Call internalStepSimulation multiple times, to achieve 240Hz (4 steps of 60Hz).
 2. Detect discrete collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
 
 3a. Solve all constraints, including LCP. Contact, position correction due to numerical drift, friction, and anchors for deformable.
-    TODO: add option for positional drift correction (using vel_target += erp * pos_error/dt
 
 3b. 5 Newton steps (multiple step). Conjugent Gradient solves linear system. Deformable Damping: Then velocities of deformable bodies v_{n+1} are solved in
         M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass,
@@ -42,7 +41,8 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
 #include "btSoftBodyInternals.h"
 btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver)
 	: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
-m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
+	  m_deformableBodySolver(deformableBodySolver),
+	  m_solverCallback(0)
 {
 	m_drawFlags = fDrawFlags::Std;
 	m_drawNodeTree = true;
@@ -58,14 +58,20 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
 	m_sbi.water_density = 0;
 	m_sbi.water_offset = 0;
 	m_sbi.water_normal = btVector3(0, 0, 0);
-	m_sbi.m_gravity.setValue(0, -10, 0);
+	m_sbi.m_gravity.setValue(0, -9.8, 0);
 	m_internalTime = 0.0;
 	m_implicit = false;
 	m_lineSearch = false;
-	m_selfCollision = true;
+	m_useProjection = false;
+	m_ccdIterations = 5;
 	m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
 }
 
+btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld()
+{
+	delete m_solverDeformableBodyIslandCallback;
+}
+
 void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
 	BT_PROFILE("internalSingleStepSimulation");
@@ -74,20 +80,16 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
 		(*m_internalPreTickCallback)(this, timeStep);
 	}
 	reinitialize(timeStep);
+
 	// add gravity to velocity of rigid and multi bodys
 	applyRigidBodyGravity(timeStep);
 
 	///apply gravity and explicit force to velocity, predict motion
 	predictUnconstraintMotion(timeStep);
 
-    ///perform collision detection
+	///perform collision detection that involves rigid/multi bodies
 	btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
 
-    if (m_selfCollision)
-    {
-        softBodySelfCollision();
-    }
-    
 	btMultiBodyDynamicsWorld::calculateSimulationIslands();
 
 	beforeSolverCallbacks(timeStep);
@@ -97,6 +99,12 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
 
 	afterSolverCallbacks(timeStep);
 
+	performDeformableCollisionDetection();
+
+	applyRepulsionForce(timeStep);
+
+	performGeometricCollisions(timeStep);
+
 	integrateTransforms(timeStep);
 
 	///update vehicle simulation
@@ -107,6 +115,27 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
 	// ///////////////////////////////
 }
 
+void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		m_softBodies[i]->m_softSoftCollision = true;
+	}
+
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		for (int j = i; j < m_softBodies.size(); ++j)
+		{
+			m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]);
+		}
+	}
+
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		m_softBodies[i]->m_softSoftCollision = false;
+	}
+}
+
 void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 {
 	for (int i = 0; i < m_softBodies.size(); i++)
@@ -131,10 +160,106 @@ void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep
 	btMultiBodyDynamicsWorld::updateActivationState(timeStep);
 }
 
+void btDeformableMultiBodyDynamicsWorld::applyRepulsionForce(btScalar timeStep)
+{
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce");
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
+			psb->applyRepulsionForce(timeStep, true);
+		}
+	}
+}
+
+void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar timeStep)
+{
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions");
+	// refit the BVH tree for CCD
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		if (psb->isActive())
+		{
+			m_softBodies[i]->updateFaceTree(true, false);
+			m_softBodies[i]->updateNodeTree(true, false);
+			for (int j = 0; j < m_softBodies[i]->m_faces.size(); ++j)
+			{
+				btSoftBody::Face& f = m_softBodies[i]->m_faces[j];
+				f.m_n0 = (f.m_n[1]->m_x - f.m_n[0]->m_x).cross(f.m_n[2]->m_x - f.m_n[0]->m_x);
+			}
+		}
+	}
+
+	// clear contact points & update DBVT
+	for (int r = 0; r < m_ccdIterations; ++r)
+	{
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				// clear contact points in the previous iteration
+				psb->m_faceNodeContacts.clear();
+
+				// update m_q and normals for CCD calculation
+				for (int j = 0; j < psb->m_nodes.size(); ++j)
+				{
+					psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + timeStep * psb->m_nodes[j].m_v;
+				}
+				for (int j = 0; j < psb->m_faces.size(); ++j)
+				{
+					btSoftBody::Face& f = psb->m_faces[j];
+					f.m_n1 = (f.m_n[1]->m_q - f.m_n[0]->m_q).cross(f.m_n[2]->m_q - f.m_n[0]->m_q);
+					f.m_vn = (f.m_n[1]->m_v - f.m_n[0]->m_v).cross(f.m_n[2]->m_v - f.m_n[0]->m_v) * timeStep * timeStep;
+				}
+			}
+		}
+
+		// apply CCD to register new contact points
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			for (int j = i; j < m_softBodies.size(); ++j)
+			{
+				btSoftBody* psb1 = m_softBodies[i];
+				btSoftBody* psb2 = m_softBodies[j];
+				if (psb1->isActive() && psb2->isActive())
+				{
+					m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]);
+				}
+			}
+		}
+
+		int penetration_count = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				penetration_count += psb->m_faceNodeContacts.size();
+			}
+		}
+		if (penetration_count == 0)
+		{
+			break;
+		}
+
+		// apply inelastic impulse
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			if (psb->isActive())
+			{
+				psb->applyRepulsionForce(timeStep, false);
+			}
+		}
+	}
+}
 
 void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
 {
-    m_deformableBodySolver->updateSoftBodies();
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision");
 	for (int i = 0; i < m_softBodies.size(); i++)
 	{
 		btSoftBody* psb = m_softBodies[i];
@@ -191,9 +316,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 					node.m_v[c] = -clampDeltaV;
 				}
 			}
-            node.m_x  =  node.m_x + timeStep * node.m_v;
-            node.m_v -= node.m_vsplit;
-            node.m_vsplit.setZero();
+			node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
 			node.m_q = node.m_x;
 			node.m_vn = node.m_v;
 		}
@@ -255,6 +378,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 
 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 {
+	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
 	// save v_{n+1}^* velocity after explicit forces
 	m_deformableBodySolver->backupVelocity();
 
@@ -265,7 +389,10 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 	solveContactConstraints();
 
 	// set up the directions in which the velocity does not change in the momentum solve
+	if (m_useProjection)
 		m_deformableBodySolver->m_objective->m_projection.setProjection();
+	else
+		m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier();
 
 	// for explicit scheme, m_backupVelocity = v_{n+1}^*
 	// for implicit scheme, m_backupVelocity = v_n
@@ -280,7 +407,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 void btDeformableMultiBodyDynamicsWorld::setupConstraints()
 {
 	// set up constraints between multibody and deformable bodies
-    m_deformableBodySolver->setConstraints();
+	m_deformableBodySolver->setConstraints(m_solverInfo);
 
 	// set up constraints among multibodies
 	{
@@ -313,7 +440,6 @@ void btDeformableMultiBodyDynamicsWorld::sortConstraints()
 	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
 }
 
-    
 void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
 {
 	// process constraints on each island
@@ -403,12 +529,22 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 	dispatchInfo.m_stepCount = 0;
 	dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
 	btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
+	if (m_useProjection)
+	{
+		m_deformableBodySolver->m_useProjection = true;
+		m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
+		m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner;
+	}
+	else
+	{
+		m_deformableBodySolver->m_useProjection = false;
+		m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
+		m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
+	}
 }
-
 
 void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
 {
-
 	btMultiBodyDynamicsWorld::debugDrawWorld();
 
 	for (int i = 0; i < getSoftBodyArray().size(); i++)
@@ -419,8 +555,6 @@ void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
 			btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags());
 		}
 	}
-
-	
 }
 
 void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
@@ -566,8 +700,36 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL
 	}
 }
 
+void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force)
+{
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	int removed_index = -1;
+	for (int i = 0; i < forces.size(); ++i)
+	{
+		if (forces[i]->getForceType() == force->getForceType())
+		{
+			forces[i]->removeSoftBody(psb);
+			if (forces[i]->m_softBodies.size() == 0)
+				removed_index = i;
+			break;
+		}
+	}
+	if (removed_index >= 0)
+		forces.removeAtIndex(removed_index);
+}
+
+void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb)
+{
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	for (int i = 0; i < forces.size(); ++i)
+	{
+		forces[i]->removeSoftBody(psb);
+	}
+}
+
 void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
 {
+	removeSoftBodyForce(body);
 	m_softBodies.remove(body);
 	btCollisionWorld::removeCollisionObject(body);
 	// force a reinitialize so that node indices get updated.
@@ -583,7 +745,6 @@ void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject
 		btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
 }
 
-
 int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
 {
 	startProfiling(timeStep);

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -46,10 +46,10 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
 	bool m_drawClusterTree;
 	btSoftBodyWorldInfo m_sbi;
 	btScalar m_internalTime;
-    int m_contact_iterations;
+	int m_ccdIterations;
 	bool m_implicit;
 	bool m_lineSearch;
-    bool m_selfCollision;
+	bool m_useProjection;
 	DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback;
 
 	typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
@@ -80,9 +80,7 @@ public:
 		m_solverCallback = cb;
 	}
 
-    virtual ~btDeformableMultiBodyDynamicsWorld()
-    {
-    }
+	virtual ~btDeformableMultiBodyDynamicsWorld();
 
 	virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
 	{
@@ -133,6 +131,10 @@ public:
 
 	void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
 
+	void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
+
+	void removeSoftBodyForce(btSoftBody* psb);
+
 	void removeSoftBody(btSoftBody* body);
 
 	void removeCollisionObject(btCollisionObject* collisionObject);
@@ -142,6 +144,8 @@ public:
 
 	void setupConstraints();
 
+	void performDeformableCollisionDetection();
+
 	void solveMultiBodyConstraints();
 
 	void solveContactConstraints();
@@ -160,6 +164,153 @@ public:
 		m_lineSearch = lineSearch;
 	}
 
+	void setUseProjection(bool useProjection)
+	{
+		m_useProjection = useProjection;
+	}
+
+	void applyRepulsionForce(btScalar timeStep);
+
+	void performGeometricCollisions(btScalar timeStep);
+
+	struct btDeformableSingleRayCallback : public btBroadphaseRayCallback
+	{
+		btVector3 m_rayFromWorld;
+		btVector3 m_rayToWorld;
+		btTransform m_rayFromTrans;
+		btTransform m_rayToTrans;
+		btVector3 m_hitNormal;
+
+		const btDeformableMultiBodyDynamicsWorld* m_world;
+		btCollisionWorld::RayResultCallback& m_resultCallback;
+
+		btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
+			: m_rayFromWorld(rayFromWorld),
+			  m_rayToWorld(rayToWorld),
+			  m_world(world),
+			  m_resultCallback(resultCallback)
+		{
+			m_rayFromTrans.setIdentity();
+			m_rayFromTrans.setOrigin(m_rayFromWorld);
+			m_rayToTrans.setIdentity();
+			m_rayToTrans.setOrigin(m_rayToWorld);
+
+			btVector3 rayDir = (rayToWorld - rayFromWorld);
+
+			rayDir.normalize();
+			///what about division by zero? --> just set rayDirection[i] to INF/1e30
+			m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
+			m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
+			m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+			m_signs[0] = m_rayDirectionInverse[0] < 0.0;
+			m_signs[1] = m_rayDirectionInverse[1] < 0.0;
+			m_signs[2] = m_rayDirectionInverse[2] < 0.0;
+
+			m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
+		}
+
+		virtual bool process(const btBroadphaseProxy* proxy)
+		{
+			///terminate further ray tests, once the closestHitFraction reached zero
+			if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
+				return false;
+
+			btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject;
+
+			//only perform raycast if filterMask matches
+			if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
+			{
+				//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+				//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+#if 0
+#ifdef RECALCULATE_AABB
+                btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+                collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#else
+                //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
+                const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
+                const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
+#endif
+				//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
+				//culling already done by broadphase
+				//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
+				{
+					m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans,
+										   collisionObject,
+										   collisionObject->getCollisionShape(),
+										   collisionObject->getWorldTransform(),
+										   m_resultCallback);
+				}
+			}
+			return true;
+		}
+	};
+
+	void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+	{
+		BT_PROFILE("rayTest");
+		/// use the broadphase to accelerate the search for objects, based on their aabb
+		/// and for each object with ray-aabb overlap, perform an exact ray test
+		btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback);
+
+#ifndef USE_BRUTEFORCE_RAYBROADPHASE
+		m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
+#else
+		for (int i = 0; i < this->getNumCollisionObjects(); i++)
+		{
+			rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+		}
+#endif  //USE_BRUTEFORCE_RAYBROADPHASE
+	}
+
+	void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans,
+					   btCollisionObject* collisionObject,
+					   const btCollisionShape* collisionShape,
+					   const btTransform& colObjWorldTransform,
+					   RayResultCallback& resultCallback) const
+	{
+		if (collisionShape->isSoftBody())
+		{
+			btSoftBody* softBody = btSoftBody::upcast(collisionObject);
+			if (softBody)
+			{
+				btSoftBody::sRayCast softResult;
+				if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
+				{
+					if (softResult.fraction <= resultCallback.m_closestHitFraction)
+					{
+						btCollisionWorld::LocalShapeInfo shapeInfo;
+						shapeInfo.m_shapePart = 0;
+						shapeInfo.m_triangleIndex = softResult.index;
+						// get the normal
+						btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
+						btVector3 normal = -rayDir;
+						normal.normalize();
+						{
+							normal = softBody->m_faces[softResult.index].m_normal;
+							if (normal.dot(rayDir) > 0)
+							{
+								// normal always point toward origin of the ray
+								normal = -normal;
+							}
+						}
+
+						btCollisionWorld::LocalRayResult rayResult(collisionObject,
+																   &shapeInfo,
+																   normal,
+																   softResult.fraction);
+						bool normalInWorldSpace = true;
+						resultCallback.addSingleResult(rayResult, normalInWorldSpace);
+					}
+				}
+			}
+		}
+		else
+		{
+			btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback);
+		}
+	}
 };
 
 #endif  //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H

thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h

@@ -24,19 +24,63 @@ class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
 {
 public:
 	typedef btAlignedObjectArray<btVector3> TVStack;
-    btScalar m_mu, m_lambda;
+	btScalar m_mu, m_lambda;  // Lame Parameters
+	btScalar m_E, m_nu;       // Young's modulus and Poisson ratio
 	btScalar m_mu_damp, m_lambda_damp;
 	btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1)
 	{
 		btScalar damping = 0.05;
 		m_mu_damp = damping * m_mu;
 		m_lambda_damp = damping * m_lambda;
+		updateYoungsModulusAndPoissonRatio();
 	}
 
 	btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda)
 	{
 		m_mu_damp = damping * m_mu;
 		m_lambda_damp = damping * m_lambda;
+		updateYoungsModulusAndPoissonRatio();
+	}
+
+	void updateYoungsModulusAndPoissonRatio()
+	{
+		// conversion from Lame Parameters to Young's modulus and Poisson ratio
+		// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
+		m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
+		m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
+	}
+
+	void updateLameParameters()
+	{
+		// conversion from Young's modulus and Poisson ratio to Lame Parameters
+		// https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
+		m_mu = m_E * 0.5 / (1 + m_nu);
+		m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
+	}
+
+	void setYoungsModulus(btScalar E)
+	{
+		m_E = E;
+		updateLameParameters();
+	}
+
+	void setPoissonRatio(btScalar nu)
+	{
+		m_nu = nu;
+		updateLameParameters();
+	}
+
+	void setDamping(btScalar damping)
+	{
+		m_mu_damp = damping * m_mu;
+		m_lambda_damp = damping * m_lambda;
+	}
+
+	void setLameParameters(btScalar mu, btScalar lambda)
+	{
+		m_mu = mu;
+		m_lambda = lambda;
+		updateYoungsModulusAndPoissonRatio();
 	}
 
 	virtual void addScaledForces(btScalar scale, TVStack& force)
@@ -269,6 +313,8 @@ public:
 		}
 	}
 
+	virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
+
 	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
 	{
 		int numNodes = getNumNodes();
@@ -370,6 +416,5 @@ public:
 	{
 		return BT_NEOHOOKEAN_FORCE;
 	}
-    
 };
 #endif /* BT_NEOHOOKEAN_H */

thirdparty/bullet/BulletSoftBody/btKrylovSolver.h

@@ -0,0 +1,107 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_KRYLOV_SOLVER_H
+#define BT_KRYLOV_SOLVER_H
+#include <iostream>
+#include <cmath>
+#include <limits>
+#include <LinearMath/btAlignedObjectArray.h>
+#include <LinearMath/btVector3.h>
+#include <LinearMath/btScalar.h>
+#include "LinearMath/btQuickprof.h"
+
+template <class MatrixX>
+class btKrylovSolver
+{
+	typedef btAlignedObjectArray<btVector3> TVStack;
+
+public:
+	int m_maxIterations;
+	btScalar m_tolerance;
+	btKrylovSolver(int maxIterations, btScalar tolerance)
+		: m_maxIterations(maxIterations), m_tolerance(tolerance)
+	{
+	}
+
+	virtual ~btKrylovSolver() {}
+
+	virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0;
+
+	virtual void reinitialize(const TVStack& b) = 0;
+
+	virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b)
+	{
+		// c = a-b
+		btAssert(a.size() == b.size());
+		TVStack c;
+		c.resize(a.size());
+		for (int i = 0; i < a.size(); ++i)
+		{
+			c[i] = a[i] - b[i];
+		}
+		return c;
+	}
+
+	virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a)
+	{
+		return dot(a, a);
+	}
+
+	virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a)
+	{
+		btScalar ret = 0;
+		for (int i = 0; i < a.size(); ++i)
+		{
+			for (int d = 0; d < 3; ++d)
+			{
+				ret = btMax(ret, btFabs(a[i][d]));
+			}
+		}
+		return ret;
+	}
+
+	virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b)
+	{
+		btScalar ans(0);
+		for (int i = 0; i < a.size(); ++i)
+			ans += a[i].dot(b[i]);
+		return ans;
+	}
+
+	virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
+	{
+		//        result += s*a
+		btAssert(a.size() == result.size());
+		for (int i = 0; i < a.size(); ++i)
+			result[i] += s * a[i];
+	}
+
+	virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
+	{
+		// result = a*s + b
+		TVStack result;
+		result.resize(a.size());
+		for (int i = 0; i < a.size(); ++i)
+			result[i] = s * a[i] + b[i];
+		return result;
+	}
+
+	virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance)
+	{
+		m_tolerance = tolerance;
+	}
+};
+#endif /* BT_KRYLOV_SOLVER_H */

thirdparty/bullet/BulletSoftBody/btPreconditioner.h

@@ -45,6 +45,7 @@ class MassPreconditioner : public Preconditioner
 {
 	btAlignedObjectArray<btScalar> m_inv_mass;
 	const btAlignedObjectArray<btSoftBody*>& m_softBodies;
+
 public:
 	MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies)
 		: m_softBodies(softBodies)
@@ -68,12 +69,217 @@ public:
 	virtual void operator()(const TVStack& x, TVStack& b)
 	{
 		btAssert(b.size() == x.size());
-        btAssert(m_inv_mass.size() == x.size());
-        for (int i = 0; i < b.size(); ++i)
+		btAssert(m_inv_mass.size() <= x.size());
+		for (int i = 0; i < m_inv_mass.size(); ++i)
 		{
 			b[i] = x[i] * m_inv_mass[i];
 		}
+		for (int i = m_inv_mass.size(); i < b.size(); ++i)
+		{
+			b[i] = x[i];
+		}
 	}
 };
 
+class KKTPreconditioner : public Preconditioner
+{
+	const btAlignedObjectArray<btSoftBody*>& m_softBodies;
+	const btDeformableContactProjection& m_projections;
+	const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf;
+	TVStack m_inv_A, m_inv_S;
+	const btScalar& m_dt;
+	const bool& m_implicit;
+
+public:
+	KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit)
+		: m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit)
+	{
+	}
+
+	virtual void reinitialize(bool nodeUpdated)
+	{
+		if (nodeUpdated)
+		{
+			int num_nodes = 0;
+			for (int i = 0; i < m_softBodies.size(); ++i)
+			{
+				btSoftBody* psb = m_softBodies[i];
+				num_nodes += psb->m_nodes.size();
+			}
+			m_inv_A.resize(num_nodes);
+		}
+		buildDiagonalA(m_inv_A);
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			//            printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]);
+			for (int d = 0; d < 3; ++d)
+			{
+				m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d];
+			}
+		}
+		m_inv_S.resize(m_projections.m_lagrangeMultipliers.size());
+		//        printf("S.size() = %d \n", m_inv_S.size());
+		buildDiagonalS(m_inv_A, m_inv_S);
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			//            printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]);
+			for (int d = 0; d < 3; ++d)
+			{
+				m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0 / m_inv_S[i][d];
+			}
+		}
+	}
+
+	void buildDiagonalA(TVStack& diagA) const
+	{
+		size_t counter = 0;
+		for (int i = 0; i < m_softBodies.size(); ++i)
+		{
+			btSoftBody* psb = m_softBodies[i];
+			for (int j = 0; j < psb->m_nodes.size(); ++j)
+			{
+				const btSoftBody::Node& node = psb->m_nodes[j];
+				diagA[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : btVector3(1.0 / node.m_im, 1.0 / node.m_im, 1.0 / node.m_im);
+				++counter;
+			}
+		}
+		if (m_implicit)
+		{
+			printf("implicit not implemented\n");
+			btAssert(false);
+		}
+		for (int i = 0; i < m_lf.size(); ++i)
+		{
+			// add damping matrix
+			m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA);
+		}
+	}
+
+	void buildDiagonalS(const TVStack& inv_A, TVStack& diagS)
+	{
+		for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
+		{
+			// S[k,k] = e_k^T * C A_d^-1 C^T * e_k
+			const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
+			btVector3& t = diagS[c];
+			t.setZero();
+			for (int j = 0; j < lm.m_num_constraints; ++j)
+			{
+				for (int i = 0; i < lm.m_num_nodes; ++i)
+				{
+					for (int d = 0; d < 3; ++d)
+					{
+						t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i];
+					}
+				}
+			}
+		}
+	}
+//#define USE_FULL_PRECONDITIONER
+#ifndef USE_FULL_PRECONDITIONER
+	virtual void operator()(const TVStack& x, TVStack& b)
+	{
+		btAssert(b.size() == x.size());
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			b[i] = x[i] * m_inv_A[i];
+		}
+		int offset = m_inv_A.size();
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			b[i + offset] = x[i + offset] * m_inv_S[i];
+		}
+	}
+#else
+	virtual void operator()(const TVStack& x, TVStack& b)
+	{
+		btAssert(b.size() == x.size());
+		int offset = m_inv_A.size();
+
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			b[i] = x[i] * m_inv_A[i];
+		}
+
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			b[i + offset].setZero();
+		}
+
+		for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
+		{
+			const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
+			// C * x
+			for (int d = 0; d < lm.m_num_constraints; ++d)
+			{
+				for (int i = 0; i < lm.m_num_nodes; ++i)
+				{
+					b[offset + c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]);
+				}
+			}
+		}
+
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			b[i + offset] = b[i + offset] * m_inv_S[i];
+		}
+
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			b[i].setZero();
+		}
+
+		for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
+		{
+			// C^T * lambda
+			const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
+			for (int i = 0; i < lm.m_num_nodes; ++i)
+			{
+				for (int j = 0; j < lm.m_num_constraints; ++j)
+				{
+					b[lm.m_indices[i]] += b[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
+				}
+			}
+		}
+
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			b[i] = (x[i] - b[i]) * m_inv_A[i];
+		}
+
+		TVStack t;
+		t.resize(b.size());
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			t[i + offset] = x[i + offset] * m_inv_S[i];
+		}
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			t[i].setZero();
+		}
+		for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
+		{
+			// C^T * lambda
+			const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
+			for (int i = 0; i < lm.m_num_nodes; ++i)
+			{
+				for (int j = 0; j < lm.m_num_constraints; ++j)
+				{
+					t[lm.m_indices[i]] += t[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
+				}
+			}
+		}
+		for (int i = 0; i < m_inv_A.size(); ++i)
+		{
+			b[i] += t[i] * m_inv_A[i];
+		}
+
+		for (int i = 0; i < m_inv_S.size(); ++i)
+		{
+			b[i + offset] -= x[i + offset] * m_inv_S[i];
+		}
+	}
+#endif
+};
+
 #endif /* BT_PRECONDITIONER_H */

thirdparty/bullet/BulletSoftBody/btSoftBody.cpp

@@ -18,12 +18,114 @@ subject to the following restrictions:
 #include "BulletSoftBody/btSoftBodySolvers.h"
 #include "btSoftBodyData.h"
 #include "LinearMath/btSerializer.h"
+#include "LinearMath/btImplicitQRSVD.h"
 #include "LinearMath/btAlignedAllocator.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
 #include <iostream>
+//
+static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& leafNodes, btAlignedObjectArray<btAlignedObjectArray<int> >& adj)
+{
+	int N = leafNodes.size();
+	if (N == 0)
+	{
+		return NULL;
+	}
+	while (N > 1)
+	{
+		btAlignedObjectArray<bool> marked;
+		btAlignedObjectArray<btDbvtNode*> newLeafNodes;
+		btAlignedObjectArray<std::pair<int, int> > childIds;
+		btAlignedObjectArray<btAlignedObjectArray<int> > newAdj;
+		marked.resize(N);
+		for (int i = 0; i < N; ++i)
+			marked[i] = false;
+
+		// pair adjacent nodes into new(parent) node
+		for (int i = 0; i < N; ++i)
+		{
+			if (marked[i])
+				continue;
+			bool merged = false;
+			for (int j = 0; j < adj[i].size(); ++j)
+			{
+				int n = adj[i][j];
+				if (!marked[adj[i][j]])
+				{
+					btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+					node->parent = NULL;
+					node->childs[0] = leafNodes[i];
+					node->childs[1] = leafNodes[n];
+					leafNodes[i]->parent = node;
+					leafNodes[n]->parent = node;
+					newLeafNodes.push_back(node);
+					childIds.push_back(std::make_pair(i, n));
+					merged = true;
+					marked[n] = true;
+					break;
+				}
+			}
+			if (!merged)
+			{
+				newLeafNodes.push_back(leafNodes[i]);
+				childIds.push_back(std::make_pair(i, -1));
+			}
+			marked[i] = true;
+		}
+		// update adjacency matrix
+		newAdj.resize(newLeafNodes.size());
+		for (int i = 0; i < newLeafNodes.size(); ++i)
+		{
+			for (int j = i + 1; j < newLeafNodes.size(); ++j)
+			{
+				bool neighbor = false;
+				const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first];
+				for (int k = 0; k < leftChildNeighbors.size(); ++k)
+				{
+					if (leftChildNeighbors[k] == childIds[j].first || leftChildNeighbors[k] == childIds[j].second)
+					{
+						neighbor = true;
+						break;
+					}
+				}
+				if (!neighbor && childIds[i].second != -1)
+				{
+					const btAlignedObjectArray<int>& rightChildNeighbors = adj[childIds[i].second];
+					for (int k = 0; k < rightChildNeighbors.size(); ++k)
+					{
+						if (rightChildNeighbors[k] == childIds[j].first || rightChildNeighbors[k] == childIds[j].second)
+						{
+							neighbor = true;
+							break;
+						}
+					}
+				}
+				if (neighbor)
+				{
+					newAdj[i].push_back(j);
+					newAdj[j].push_back(i);
+				}
+			}
+		}
+		leafNodes = newLeafNodes;
+		//this assignment leaks memory, the assignment doesn't do a deep copy, for now a manual copy
+		//adj = newAdj;
+		adj.clear();
+		adj.resize(newAdj.size());
+		for (int i = 0; i < newAdj.size(); i++)
+		{
+			for (int j = 0; j < newAdj[i].size(); j++)
+			{
+				adj[i].push_back(newAdj[i][j]);
+			}
+		}
+		N = leafNodes.size();
+	}
+	return leafNodes[0];
+}
+
 //
 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
 	: m_softBodySolver(0), m_worldInfo(worldInfo)
@@ -41,6 +143,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
 	/* Nodes			*/
 	const btScalar margin = getCollisionShape()->getMargin();
 	m_nodes.resize(node_count);
+	m_X.resize(node_count);
 	for (int i = 0, ni = node_count; i < ni; ++i)
 	{
 		Node& n = m_nodes[i];
@@ -51,8 +154,11 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
 		n.m_im = n.m_im > 0 ? 1 / n.m_im : 0;
 		n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
 		n.m_material = pm;
+		m_X[i] = n.m_x;
 	}
 	updateBounds();
+	setCollisionQuadrature(3);
+	m_fdbvnt = 0;
 }
 
 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
@@ -111,15 +217,20 @@ void btSoftBody::initDefaults()
 	m_collisionShape = new btSoftBodyCollisionShape(this);
 	m_collisionShape->setMargin(0.25f);
 
-	m_initialWorldTransform.setIdentity();
+	m_worldTransform.setIdentity();
 
 	m_windVelocity = btVector3(0, 0, 0);
 	m_restLengthScale = btScalar(1.0);
-    m_dampingCoefficient = 1;
-    m_sleepingThreshold = 0.1;
-    m_useFaceContact = true;
+	m_dampingCoefficient = 1.0;
+	m_sleepingThreshold = .04;
 	m_useSelfCollision = false;
 	m_collisionFlags = 0;
+	m_softSoftCollision = false;
+	m_maxSpeedSquared = 0;
+	m_repulsionStiffness = 0.5;
+	m_gravityFactor = 1;
+	m_cacheBarycenter = false;
+	m_fdbvnt = 0;
 }
 
 //
@@ -134,6 +245,8 @@ btSoftBody::~btSoftBody()
 		btAlignedFree(m_materials[i]);
 	for (i = 0; i < m_joints.size(); ++i)
 		btAlignedFree(m_joints[i]);
+	if (m_fdbvnt)
+		delete m_fdbvnt;
 }
 
 //
@@ -447,6 +560,23 @@ void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
 	m_deformableAnchors.push_back(c);
 }
 
+void btSoftBody::removeAnchor(int node)
+{
+	const btSoftBody::Node& n = m_nodes[node];
+	for (int i = 0; i < m_deformableAnchors.size();)
+	{
+		const DeformableNodeRigidAnchor& c = m_deformableAnchors[i];
+		if (c.m_node == &n)
+		{
+			m_deformableAnchors.removeAtIndex(i);
+		}
+		else
+		{
+			i++;
+		}
+	}
+}
+
 //
 void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
 {
@@ -620,7 +750,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex
 					fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
 
 					// Check angle of attack
-					// cos(10º) = 0.98480
+					// cos(10º) = 0.98480
 					if (0 < n_dot_v && n_dot_v < 0.98480f)
 						fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
 
@@ -706,7 +836,7 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex
 				fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
 
 				// Check angle of attack
-				// cos(10º) = 0.98480
+				// cos(10º) = 0.98480
 				if (0 < n_dot_v && n_dot_v < 0.98480f)
 					fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
 
@@ -771,6 +901,7 @@ void btSoftBody::setVelocity(const btVector3& velocity)
 		if (n.m_im > 0)
 		{
 			n.m_v = velocity;
+			n.m_vn = velocity;
 		}
 	}
 }
@@ -896,6 +1027,75 @@ void btSoftBody::setVolumeDensity(btScalar density)
 	setVolumeMass(volume * density / 6);
 }
 
+//
+btVector3 btSoftBody::getLinearVelocity()
+{
+	btVector3 total_momentum = btVector3(0, 0, 0);
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0 / m_nodes[i].m_im;
+		total_momentum += mass * m_nodes[i].m_v;
+	}
+	btScalar total_mass = getTotalMass();
+	return total_mass == 0 ? total_momentum : total_momentum / total_mass;
+}
+
+//
+void btSoftBody::setLinearVelocity(const btVector3& linVel)
+{
+	btVector3 old_vel = getLinearVelocity();
+	btVector3 diff = linVel - old_vel;
+	for (int i = 0; i < m_nodes.size(); ++i)
+		m_nodes[i].m_v += diff;
+}
+
+//
+void btSoftBody::setAngularVelocity(const btVector3& angVel)
+{
+	btVector3 old_vel = getLinearVelocity();
+	btVector3 com = getCenterOfMass();
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel;
+	}
+}
+
+//
+btTransform btSoftBody::getRigidTransform()
+{
+	btVector3 t = getCenterOfMass();
+	btMatrix3x3 S;
+	S.setZero();
+	// Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
+	// It's important to make sure that S has the correct signs.
+	// SVD is only unique up to the ordering of singular values.
+	// SVD will manipulate U and V to ensure the ordering of singular values. If all three singular
+	// vaues are negative, SVD will permute colums of U to make two of them positive.
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		S -= OuterProduct(m_X[i], t - m_nodes[i].m_x);
+	}
+	btVector3 sigma;
+	btMatrix3x3 U, V;
+	singularValueDecomposition(S, U, sigma, V);
+	btMatrix3x3 R = V * U.transpose();
+	btTransform trs;
+	trs.setIdentity();
+	trs.setOrigin(t);
+	trs.setBasis(R);
+	return trs;
+}
+
+//
+void btSoftBody::transformTo(const btTransform& trs)
+{
+	// get the current best rigid fit
+	btTransform current_transform = getRigidTransform();
+	// apply transform in material space
+	btTransform new_transform = trs * current_transform.inverse();
+	transform(new_transform);
+}
+
 //
 void btSoftBody::transform(const btTransform& trs)
 {
@@ -916,7 +1116,6 @@ void btSoftBody::transform(const btTransform& trs)
 	updateNormals();
 	updateBounds();
 	updateConstants();
-	m_initialWorldTransform = trs;
 }
 
 //
@@ -1834,6 +2033,25 @@ bool btSoftBody::rayTest(const btVector3& rayFrom,
 	return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0);
 }
 
+bool btSoftBody::rayFaceTest(const btVector3& rayFrom,
+							 const btVector3& rayTo,
+							 sRayCast& results)
+{
+	if (m_faces.size() == 0)
+		return false;
+	else
+	{
+		if (m_fdbvt.empty())
+			initializeFaceTree();
+	}
+
+	results.body = this;
+	results.fraction = 1.f;
+	results.index = -1;
+
+	return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0);
+}
+
 //
 void btSoftBody::setSolver(eSolverPresets::_ preset)
 {
@@ -1968,7 +2186,6 @@ void btSoftBody::predictMotion(btScalar dt)
 	m_cdbvt.optimizeIncremental(1);
 }
 
-
 //
 void btSoftBody::solveConstraints()
 {
@@ -2339,15 +2556,161 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
 	return (cnt);
 }
 
+int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
+							btScalar& mint, int& index) const
+{
+	int cnt = 0;
+	{ /* Use dbvt	*/
+		RayFromToCaster collider(rayFrom, rayTo, mint);
+
+		btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
+		if (collider.m_face)
+		{
+			mint = collider.m_mint;
+			index = (int)(collider.m_face - &m_faces[0]);
+			cnt = 1;
+		}
+	}
+	return (cnt);
+}
+
 //
+static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
+{
+	if (n == 0)
+		return 0;
+	btDbvntNode* root = new btDbvntNode(n);
+	if (n->isinternal())
+	{
+		btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
+		root->childs[0] = c0;
+		btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
+		root->childs[1] = c1;
+	}
+	return root;
+}
+
+static inline void calculateNormalCone(btDbvntNode* root)
+{
+	if (!root)
+		return;
+	if (root->isleaf())
+	{
+		const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
+		root->normal = face->m_normal;
+		root->angle = 0;
+	}
+	else
+	{
+		btVector3 n0(0, 0, 0), n1(0, 0, 0);
+		btScalar a0 = 0, a1 = 0;
+		if (root->childs[0])
+		{
+			calculateNormalCone(root->childs[0]);
+			n0 = root->childs[0]->normal;
+			a0 = root->childs[0]->angle;
+		}
+		if (root->childs[1])
+		{
+			calculateNormalCone(root->childs[1]);
+			n1 = root->childs[1]->normal;
+			a1 = root->childs[1]->angle;
+		}
+		root->normal = (n0 + n1).safeNormalize();
+		root->angle = btMax(a0, a1) + btAngle(n0, n1) * 0.5;
+	}
+}
+
 void btSoftBody::initializeFaceTree()
 {
+	BT_PROFILE("btSoftBody::initializeFaceTree");
 	m_fdbvt.clear();
+	// create leaf nodes;
+	btAlignedObjectArray<btDbvtNode*> leafNodes;
+	leafNodes.resize(m_faces.size());
 	for (int i = 0; i < m_faces.size(); ++i)
 	{
 		Face& f = m_faces[i];
-		f.m_leaf = m_fdbvt.insert(VolumeOf(f, 0), &f);
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		vol = VolumeOf(f, 0);
+		btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+		node->parent = NULL;
+		node->data = &f;
+		node->childs[1] = 0;
+		node->volume = vol;
+		leafNodes[i] = node;
+		f.m_leaf = node;
 	}
+	btAlignedObjectArray<btAlignedObjectArray<int> > adj;
+	adj.resize(m_faces.size());
+	// construct the adjacency list for triangles
+	for (int i = 0; i < adj.size(); ++i)
+	{
+		for (int j = i + 1; j < adj.size(); ++j)
+		{
+			int dup = 0;
+			for (int k = 0; k < 3; ++k)
+			{
+				for (int l = 0; l < 3; ++l)
+				{
+					if (m_faces[i].m_n[k] == m_faces[j].m_n[l])
+					{
+						++dup;
+						break;
+					}
+				}
+				if (dup == 2)
+				{
+					adj[i].push_back(j);
+					adj[j].push_back(i);
+				}
+			}
+		}
+	}
+	m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj);
+	if (m_fdbvnt)
+		delete m_fdbvnt;
+	m_fdbvnt = copyToDbvnt(m_fdbvt.m_root);
+	updateFaceTree(false, false);
+	rebuildNodeTree();
+}
+
+//
+void btSoftBody::rebuildNodeTree()
+{
+	m_ndbvt.clear();
+	btAlignedObjectArray<btDbvtNode*> leafNodes;
+	leafNodes.resize(m_nodes.size());
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		Node& n = m_nodes[i];
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		vol = btDbvtVolume::FromCR(n.m_x, 0);
+		btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+		node->parent = NULL;
+		node->data = &n;
+		node->childs[1] = 0;
+		node->volume = vol;
+		leafNodes[i] = node;
+		n.m_leaf = node;
+	}
+	btAlignedObjectArray<btAlignedObjectArray<int> > adj;
+	adj.resize(m_nodes.size());
+	btAlignedObjectArray<int> old_id;
+	old_id.resize(m_nodes.size());
+	for (int i = 0; i < m_nodes.size(); ++i)
+		old_id[i] = m_nodes[i].index;
+	for (int i = 0; i < m_nodes.size(); ++i)
+		m_nodes[i].index = i;
+	for (int i = 0; i < m_links.size(); ++i)
+	{
+		Link& l = m_links[i];
+		adj[l.m_n[0]->index].push_back(l.m_n[1]->index);
+		adj[l.m_n[1]->index].push_back(l.m_n[0]->index);
+	}
+	m_ndbvt.m_root = buildTreeBottomUp(leafNodes, adj);
+	for (int i = 0; i < m_nodes.size(); ++i)
+		m_nodes[i].index = old_id[i];
 }
 
 //
@@ -2403,16 +2766,15 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
 	const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
 	// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
 	// but resolve contact at x_n
-//    btTransform wtr = (predict) ?
-//    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
-//                 : colObjWrap->getWorldTransform();
-    const btTransform& wtr = colObjWrap->getWorldTransform();
+	btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+								: colObjWrap->getWorldTransform();
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
 			wtr.invXform(x),
 			shp,
 			nrm,
 			margin);
+
 	if (!predict)
 	{
 		cti.m_colObj = colObjWrap->getCollisionObject();
@@ -2454,49 +2816,12 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 	const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
 	// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
 	// but resolve contact at x_n
-    btTransform wtr = (predict) ?
-    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+	btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
 								: colObjWrap->getWorldTransform();
-//    const btTransform& wtr = colObjWrap->getWorldTransform();
 	btScalar dst;
+	btGjkEpaSolver2::sResults results;
 
 //	#define USE_QUADRATURE 1
-//#define CACHE_PREV_COLLISION
-    
-    // use the contact position of the previous collision
-#ifdef CACHE_PREV_COLLISION
-    if (f.m_pcontact[3] != 0)
-    {
-        for (int i = 0; i < 3; ++i)
-            bary[i] = f.m_pcontact[i];
-        contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-        dst = m_worldInfo->m_sparsesdf.Evaluate(
-                                          wtr.invXform(contact_point),
-                                          shp,
-                                          nrm,
-                                          margin);
-        nrm = wtr.getBasis() * nrm;
-        // use cached contact point
-    }
-    else
-    {
-        btGjkEpaSolver2::sResults results;
-        btTransform triangle_transform;
-        triangle_transform.setIdentity();
-        triangle_transform.setOrigin(f.m_n[0]->m_x);
-        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
-        btVector3 guess(0,0,0);
-        const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
-        btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
-        dst = results.distance - margin;
-        contact_point = results.witnesses[0];
-        getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-        nrm = results.normal;
-        for (int i = 0; i < 3; ++i)
-            f.m_pcontact[i] = bary[i];
-    }
-
-#endif
 
 	// use collision quadrature point
 #ifdef USE_QUADRATURE
@@ -2505,7 +2830,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 		btVector3 local_nrm;
 		for (int q = 0; q < m_quads.size(); ++q)
 		{
-            btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
+			btVector3 p;
+			if (predict)
+				p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
+			else
+				p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
 			btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
 				wtr.invXform(p),
 				shp,
@@ -2513,46 +2842,80 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 				margin);
 			if (local_dst < dst)
 			{
+				if (local_dst < 0 && predict)
+					return true;
 				dst = local_dst;
 				contact_point = p;
 				bary = m_quads[q];
-                nrm = wtr.getBasis() * local_nrm;
+				nrm = local_nrm;
 			}
-        }
-    }
-#endif
-    
-    // regular face contact
-    {
-        btGjkEpaSolver2::sResults results;
-        btTransform triangle_transform;
-        triangle_transform.setIdentity();
-        triangle_transform.setOrigin(f.m_n[0]->m_x);
-        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
-        btVector3 guess(0,0,0);
-        const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
-        btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
-        dst = results.distance - margin;
-        contact_point = results.witnesses[0];
-        getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-        nrm = results.normal;
-        for (int i = 0; i < 3; ++i)
-            f.m_pcontact[i] = bary[i];
-    }
-    
 			if (!predict)
 			{
 				cti.m_colObj = colObjWrap->getCollisionObject();
-        cti.m_normal = nrm;
+				cti.m_normal = wtr.getBasis() * nrm;
 				cti.m_offset = dst;
 			}
+		}
+		return (dst < 0);
+	}
+#endif
 
-    if (dst < 0)
+	// collision detection using x*
+	btTransform triangle_transform;
+	triangle_transform.setIdentity();
+	triangle_transform.setOrigin(f.m_n[0]->m_q);
+	btTriangleShape triangle(btVector3(0, 0, 0), f.m_n[1]->m_q - f.m_n[0]->m_q, f.m_n[2]->m_q - f.m_n[0]->m_q);
+	btVector3 guess(0, 0, 0);
+	const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+	btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
+	dst = results.distance - 2.0 * csh->getMargin() - margin;  // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable
+	if (dst >= 0)
+		return false;
+
+	// Use consistent barycenter to recalculate distance.
+	if (this->m_cacheBarycenter)
+	{
+		if (f.m_pcontact[3] != 0)
+		{
+			for (int i = 0; i < 3; ++i)
+				bary[i] = f.m_pcontact[i];
+			contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+			const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+			btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results);
+			cti.m_colObj = colObjWrap->getCollisionObject();
+			dst = results.distance;
+			cti.m_normal = results.normal;
+			cti.m_offset = dst;
+
+			//point-convex CD
+			wtr = colObjWrap->getWorldTransform();
+			btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
+			triangle_transform.setOrigin(f.m_n[0]->m_x);
+			btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
+
+			dst = results.distance - csh->getMargin() - margin;
+			return true;
+		}
+	}
+
+	// Use triangle-convex CD.
+	wtr = colObjWrap->getWorldTransform();
+	btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
+	triangle_transform.setOrigin(f.m_n[0]->m_x);
+	btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
+	contact_point = results.witnesses[0];
+	getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+
+	for (int i = 0; i < 3; ++i)
+		f.m_pcontact[i] = bary[i];
+
+	dst = results.distance - csh->getMargin() - margin;
+	cti.m_colObj = colObjWrap->getCollisionObject();
+	cti.m_normal = results.normal;
+	cti.m_offset = dst;
 	return true;
-    return (false);
 }
 
-//
 void btSoftBody::updateNormals()
 {
 	const btVector3 zv(0, 0, 0);
@@ -3075,6 +3438,17 @@ void btSoftBody::setSpringStiffness(btScalar k)
 	{
 		m_links[i].Feature::m_material->m_kLST = k;
 	}
+	m_repulsionStiffness = k;
+}
+
+void btSoftBody::setGravityFactor(btScalar gravFactor)
+{
+	m_gravityFactor = gravFactor;
+}
+
+void btSoftBody::setCacheBarycenter(bool cacheBarycenter)
+{
+	m_cacheBarycenter = cacheBarycenter;
 }
 
 void btSoftBody::initializeDmInverse()
@@ -3092,11 +3466,46 @@ void btSoftBody::initializeDmInverse()
 					   c1.getZ(), c2.getZ(), c3.getZ());
 		t.m_element_measure = Dm.determinant() * unit_simplex_measure;
 		t.m_Dm_inverse = Dm.inverse();
+
+		// calculate the first three columns of P^{-1}
+		btVector3 a = t.m_n[0]->m_x;
+		btVector3 b = t.m_n[1]->m_x;
+		btVector3 c = t.m_n[2]->m_x;
+		btVector3 d = t.m_n[3]->m_x;
+
+		btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]);
+
+		btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2];
+		btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2];
+		btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1];
+		btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2];
+		btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2];
+		btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1];
+		btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2];
+		btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2];
+		btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1];
+		btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2];
+		btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2];
+		btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1];
+
+		btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det);
+		btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det);
+		btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det);
+
+		t.m_P_inv[0] = p1;
+		t.m_P_inv[1] = p2;
+		t.m_P_inv[2] = p3;
 	}
 }
 
+static btScalar Dot4(const btVector4& a, const btVector4& b)
+{
+	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
+}
+
 void btSoftBody::updateDeformation()
 {
+	btQuaternion q;
 	for (int i = 0; i < m_tetras.size(); ++i)
 	{
 		btSoftBody::Tetra& t = m_tetras[i];
@@ -3114,6 +3523,21 @@ void btSoftBody::updateDeformation()
 		btMatrix3x3 C = t.m_F.transpose() * t.m_F;
 		s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
 		s.m_cofF = t.m_F.adjoint().transpose();
+
+		btVector3 a = t.m_n[0]->m_q;
+		btVector3 b = t.m_n[1]->m_q;
+		btVector3 c = t.m_n[2]->m_q;
+		btVector3 d = t.m_n[3]->m_q;
+		btVector4 q1(a[0], b[0], c[0], d[0]);
+		btVector4 q2(a[1], b[1], c[1], d[1]);
+		btVector4 q3(a[2], b[2], c[2], d[2]);
+		btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]),
+					  Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]),
+					  Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2]));
+		q.setRotation(btVector3(0, 0, 1), 0);
+		B.extractRotation(q, 0.01);  // precision of the rotation is not very important for visual correctness.
+		btMatrix3x3 Q(q);
+		s.m_corotation = Q;
 	}
 }
 
@@ -3371,10 +3795,30 @@ void btSoftBody::setMaxStress(btScalar maxStress)
 
 //
 void btSoftBody::interpolateRenderMesh()
+{
+	if (m_z.size() > 0)
 	{
 		for (int i = 0; i < m_renderNodes.size(); ++i)
 		{
-        Node& n = m_renderNodes[i];
+			const Node* p0 = m_renderNodesParents[i][0];
+			const Node* p1 = m_renderNodesParents[i][1];
+			const Node* p2 = m_renderNodesParents[i][2];
+			btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x);
+			btVector3 unit_normal = normal.normalized();
+			RenderNode& n = m_renderNodes[i];
+			n.m_x.setZero();
+			for (int j = 0; j < 3; ++j)
+			{
+				n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
+			}
+			n.m_x += m_z[i] * unit_normal;
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_renderNodes.size(); ++i)
+		{
+			RenderNode& n = m_renderNodes[i];
 			n.m_x.setZero();
 			for (int j = 0; j < 4; ++j)
 			{
@@ -3385,6 +3829,7 @@ void btSoftBody::interpolateRenderMesh()
 			}
 		}
 	}
+}
 
 void btSoftBody::setCollisionQuadrature(int N)
 {
@@ -3649,13 +4094,10 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 		break;
 		case fCollision::SDF_RD:
 		{
-            
 			btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
 			if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
 			{
 				const btTransform wtr = pcoWrap->getWorldTransform();
-//                const btTransform ctr = pcoWrap->getWorldTransform();
-//                const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
 				const btScalar timemargin = 0;
 				const btScalar basemargin = getCollisionShape()->getMargin();
 				btVector3 mins;
@@ -3667,6 +4109,8 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 													  maxs);
 				volume = btDbvtVolume::FromMM(mins, maxs);
 				volume.Expand(btVector3(basemargin, basemargin, basemargin));
+				if (m_cfg.collisions & fCollision::SDF_RDN)
+				{
 					btSoftColliders::CollideSDF_RD docollideNode;
 					docollideNode.psb = this;
 					docollideNode.m_colObj1Wrap = pcoWrap;
@@ -3674,8 +4118,9 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 					docollideNode.dynmargin = basemargin + timemargin;
 					docollideNode.stamargin = basemargin;
 					m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
+				}
 
-                if (this->m_useFaceContact)
+				if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF)))
 				{
 					btSoftColliders::CollideSDF_RDF docollideFace;
 					docollideFace.psb = this;
@@ -3691,51 +4136,6 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 	}
 }
 
-static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
-{
-    if (n == 0)
-        return 0;
-    btDbvntNode* root = new btDbvntNode(n);
-    if (n->isinternal())
-    {
-        btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
-        root->childs[0] = c0;
-        btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
-        root->childs[1] = c1;
-    }
-    return root;
-}
-
-static inline void calculateNormalCone(btDbvntNode* root)
-{
-    if (!root)
-        return;
-    if (root->isleaf())
-    {
-        const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
-        root->normal = face->m_normal;
-        root->angle = 0;
-    }
-    else
-    {
-        btVector3 n0(0,0,0), n1(0,0,0);
-        btScalar a0 = 0, a1 = 0;
-        if (root->childs[0])
-        {
-            calculateNormalCone(root->childs[0]);
-            n0 = root->childs[0]->normal;
-            a0 = root->childs[0]->angle;
-        }
-        if (root->childs[1])
-        {
-            calculateNormalCone(root->childs[1]);
-            n1 = root->childs[1]->normal;
-            a1 = root->childs[1]->angle;
-        }
-        root->normal = (n0+n1).safeNormalize();
-        root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
-    }
-}
 //
 void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 {
@@ -3779,6 +4179,8 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 		break;
 		case fCollision::VF_DD:
 		{
+			if (!psb->m_softSoftCollision)
+				return;
 			if (psb->isActive() || this->isActive())
 			{
 				if (this != psb)
@@ -3794,6 +4196,64 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 						docollide.useFaceNormal = false;
 					docollide.psb[0] = this;
 					docollide.psb[1] = psb;
+					docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+														docollide.psb[1]->m_fdbvt.m_root,
+														docollide);
+
+					/* psb1 nodes vs psb0 faces    */
+					if (this->m_tetras.size() > 0)
+						docollide.useFaceNormal = true;
+					else
+						docollide.useFaceNormal = false;
+					docollide.psb[0] = psb;
+					docollide.psb[1] = this;
+					docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+														docollide.psb[1]->m_fdbvt.m_root,
+														docollide);
+				}
+				else
+				{
+					if (psb->useSelfCollision())
+					{
+						btSoftColliders::CollideFF_DD docollide;
+						docollide.mrg = 2 * getCollisionShape()->getMargin();
+						docollide.psb[0] = this;
+						docollide.psb[1] = psb;
+						if (this->m_tetras.size() > 0)
+							docollide.useFaceNormal = true;
+						else
+							docollide.useFaceNormal = false;
+						/* psb0 faces vs psb0 faces    */
+						calculateNormalCone(this->m_fdbvnt);
+						this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
+					}
+				}
+			}
+		}
+		break;
+		default:
+		{
+		}
+	}
+}
+
+void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
+{
+	if (psb->isActive() || this->isActive())
+	{
+		if (this != psb)
+		{
+			btSoftColliders::CollideCCD docollide;
+			/* common                    */
+			docollide.mrg = SAFE_EPSILON;  // for rounding error instead of actual margin
+			docollide.dt = psb->m_sst.sdt;
+			/* psb0 nodes vs psb1 faces    */
+			if (psb->m_tetras.size() > 0)
+				docollide.useFaceNormal = true;
+			else
+				docollide.useFaceNormal = false;
+			docollide.psb[0] = this;
+			docollide.psb[1] = psb;
 			docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
 												docollide.psb[1]->m_fdbvt.m_root,
 												docollide);
@@ -3812,30 +4272,22 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 		{
 			if (psb->useSelfCollision())
 			{
-                      btSoftColliders::CollideFF_DD docollide;
-                      docollide.mrg = getCollisionShape()->getMargin() +
-                      psb->getCollisionShape()->getMargin();
+				btSoftColliders::CollideCCD docollide;
+				docollide.mrg = SAFE_EPSILON;
 				docollide.psb[0] = this;
 				docollide.psb[1] = psb;
+				docollide.dt = psb->m_sst.sdt;
 				if (this->m_tetras.size() > 0)
 					docollide.useFaceNormal = true;
 				else
 					docollide.useFaceNormal = false;
 				/* psb0 faces vs psb0 faces    */
-                      btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
-                      calculateNormalCone(root);
-                      this->m_fdbvt.selfCollideT(root,docollide);
-                      delete root;
+				calculateNormalCone(this->m_fdbvnt);  // should compute this outside of this scope
+				this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
 			}
 		}
 	}
 }
-        break;
-		default:
-		{
-		}
-	}
-}
 
 void btSoftBody::setWindVelocity(const btVector3& velocity)
 {
@@ -4250,7 +4702,6 @@ void btSoftBody::updateDeactivation(btScalar timeStep)
 	}
 }
 
-
 void btSoftBody::setZeroVelocity()
 {
 	for (int i = 0; i < m_nodes.size(); ++i)

thirdparty/bullet/BulletSoftBody/btSoftBody.h

@@ -35,6 +35,8 @@ subject to the following restrictions:
 //#else
 #define btSoftBodyData btSoftBodyFloatData
 #define btSoftBodyDataName "btSoftBodyFloatData"
+static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1;
+static unsigned long seed = 243703;
 //#endif //BT_USE_DOUBLE_PRECISION
 
 class btBroadphaseInterface;
@@ -161,14 +163,18 @@ public:
 			RVSmask = 0x000f,  ///Rigid versus soft mask
 			SDF_RS = 0x0001,   ///SDF based rigid vs soft
 			CL_RS = 0x0002,    ///Cluster vs convex rigid vs soft
-            SDF_RD = 0x0003,   ///DF based rigid vs deformable
-            SDF_RDF = 0x0004,   ///DF based rigid vs deformable faces
+			SDF_RD = 0x0004,   ///rigid vs deformable
 
-			SVSmask = 0x00F0,  ///Rigid versus soft mask
+			SVSmask = 0x00f0,  ///Rigid versus soft mask
 			VF_SS = 0x0010,    ///Vertex vs face soft vs soft handling
 			CL_SS = 0x0020,    ///Cluster vs cluster soft vs soft handling
 			CL_SELF = 0x0040,  ///Cluster soft body self collision
-            VF_DD = 0x0050,    ///Vertex vs face soft vs soft handling
+			VF_DD = 0x0080,    ///Vertex vs face soft vs soft handling
+
+			RVDFmask = 0x0f00,  /// Rigid versus deformable face mask
+			SDF_RDF = 0x0100,   /// GJK based Rigid vs. deformable face
+			SDF_MDF = 0x0200,   /// GJK based Multibody vs. deformable face
+			SDF_RDN = 0x0400,   /// SDF based Rigid vs. deformable node
 			/* presets	*/
 			Default = SDF_RS,
 			END
@@ -252,20 +258,29 @@ public:
 		Material* m_material;  // Material
 	};
 	/* Node			*/
+	struct RenderNode
+	{
+		btVector3 m_x;
+		btVector3 m_uv1;
+		btVector3 m_normal;
+	};
 	struct Node : Feature
 	{
 		btVector3 m_x;       // Position
 		btVector3 m_q;       // Previous step position/Test position
 		btVector3 m_v;       // Velocity
-        btVector3 m_vsplit;  // Temporary Velocity in addintion to velocity used in split impulse
 		btVector3 m_vn;      // Previous step velocity
 		btVector3 m_f;       // Force accumulator
 		btVector3 m_n;       // Normal
 		btScalar m_im;       // 1/mass
 		btScalar m_area;     // Area
 		btDbvtNode* m_leaf;  // Leaf data
+		int m_constrained;   // depth of penetration
 		int m_battach : 1;   // Attached
 		int index;
+		btVector3 m_splitv;               // velocity associated with split impulse
+		btMatrix3x3 m_effectiveMass;      // effective mass in contact
+		btMatrix3x3 m_effectiveMass_inv;  // inverse of effective mass
 	};
 	/* Link			*/
 	ATTRIBUTE_ALIGNED16(struct)
@@ -281,6 +296,11 @@ public:
 
 		BT_DECLARE_ALIGNED_ALLOCATOR();
 	};
+	struct RenderFace
+	{
+		RenderNode* m_n[3];          // Node pointers
+	};
+
 	/* Face			*/
 	struct Face : Feature
 	{
@@ -289,6 +309,7 @@ public:
 		btScalar m_ra;         // Rest area
 		btDbvtNode* m_leaf;    // Leaf data
 		btVector4 m_pcontact;  // barycentric weights of the persistent contact
+		btVector3 m_n0, m_n1, m_vn;
 		int m_index;
 	};
 	/* Tetra		*/
@@ -303,6 +324,7 @@ public:
 		btMatrix3x3 m_Dm_inverse;  // rest Dm^-1
 		btMatrix3x3 m_F;
 		btScalar m_element_measure;
+		btVector4 m_P_inv[3];  // first three columns of P_inv matrix
 	};
 
 	/*  TetraScratch  */
@@ -312,6 +334,7 @@ public:
 		btScalar m_trace;          // trace of F^T * F
 		btScalar m_J;              // det(F)
 		btMatrix3x3 m_cofF;        // cofactor of F
+		btMatrix3x3 m_corotation;  // corotatio of the tetra
 	};
 
 	/* RContact		*/
@@ -342,6 +365,7 @@ public:
 		btScalar m_c2;     // inverse mass of node/face
 		btScalar m_c3;     // Friction
 		btScalar m_c4;     // Hardness
+		btMatrix3x3 m_c5;  // inverse effective mass
 
 		// jacobians and unit impulse responses for multibody
 		btMultiBodyJacobianData jacobianData_normal;
@@ -717,6 +741,15 @@ public:
 	/* SolverState	*/
 	struct SolverState
 	{
+		//if you add new variables, always initialize them!
+		SolverState()
+			: sdt(0),
+			  isdt(0),
+			  velmrg(0),
+			  radmrg(0),
+			  updmrg(0)
+		{
+		}
 		btScalar sdt;     // dt*timescale
 		btScalar isdt;    // 1/sdt
 		btScalar velmrg;  // velocity margin
@@ -753,9 +786,11 @@ public:
 	typedef btAlignedObjectArray<Cluster*> tClusterArray;
 	typedef btAlignedObjectArray<Note> tNoteArray;
 	typedef btAlignedObjectArray<Node> tNodeArray;
+	typedef btAlignedObjectArray< RenderNode> tRenderNodeArray;
 	typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
 	typedef btAlignedObjectArray<Link> tLinkArray;
 	typedef btAlignedObjectArray<Face> tFaceArray;
+	typedef btAlignedObjectArray<RenderFace> tRenderFaceArray;
 	typedef btAlignedObjectArray<Tetra> tTetraArray;
 	typedef btAlignedObjectArray<Anchor> tAnchorArray;
 	typedef btAlignedObjectArray<RContact> tRContactArray;
@@ -775,10 +810,10 @@ public:
 	btSoftBodyWorldInfo* m_worldInfo;  // World info
 	tNoteArray m_notes;                // Notes
 	tNodeArray m_nodes;                // Nodes
-    tNodeArray m_renderNodes;                // Nodes
+	tRenderNodeArray m_renderNodes;    // Render Nodes
 	tLinkArray m_links;                // Links
 	tFaceArray m_faces;                // Faces
-    tFaceArray m_renderFaces;                // Faces
+	tRenderFaceArray m_renderFaces;          // Faces
 	tTetraArray m_tetras;              // Tetras
 	btAlignedObjectArray<TetraScratch> m_tetraScratches;
 	btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
@@ -796,22 +831,26 @@ public:
 	bool m_bUpdateRtCst;            // Update runtime constants
 	btDbvt m_ndbvt;                 // Nodes tree
 	btDbvt m_fdbvt;                 // Faces tree
+	btDbvntNode* m_fdbvnt;          // Faces tree with normals
 	btDbvt m_cdbvt;                 // Clusters tree
 	tClusterArray m_clusters;       // Clusters
 	btScalar m_dampingCoefficient;  // Damping Coefficient
 	btScalar m_sleepingThreshold;
 	btScalar m_maxSpeedSquared;
-    bool m_useFaceContact;
 	btAlignedObjectArray<btVector3> m_quads;  // quadrature points for collision detection
+	btScalar m_repulsionStiffness;
+	btScalar m_gravityFactor;
+	bool m_cacheBarycenter;
+	btAlignedObjectArray<btVector3> m_X;  // initial positions
 
 	btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
 	btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
+	btAlignedObjectArray<btScalar> m_z;  // vertical distance used in extrapolation
 	bool m_useSelfCollision;
+	bool m_softSoftCollision;
 
 	btAlignedObjectArray<bool> m_clusterConnectivity;  //cluster connectivity, for self-collision
 
-	btTransform m_initialWorldTransform;
-
 	btVector3 m_windVelocity;
 
 	btScalar m_restLengthScale;
@@ -844,11 +883,6 @@ public:
 		m_dampingCoefficient = damping_coeff;
 	}
 
-    void setUseFaceContact(bool useFaceContact)
-    {
-        m_useFaceContact = false;
-    }
-
 	///@todo: avoid internal softbody shape hack and move collision code to collision library
 	virtual void setCollisionShape(btCollisionShape* collisionShape)
 	{
@@ -913,6 +947,7 @@ public:
 	void appendAnchor(int node,
 					  btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
 	void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
+	void removeAnchor(int node);
 	/* Append linear joint													*/
 	void appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1);
 	void appendLinearJoint(const LJoint::Specs& specs, Body body = Body());
@@ -957,6 +992,16 @@ public:
 	void setVolumeMass(btScalar mass);
 	/* Set volume density (using tetrahedrons)								*/
 	void setVolumeDensity(btScalar density);
+	/* Get the linear velocity of the center of mass                        */
+	btVector3 getLinearVelocity();
+	/* Set the linear velocity of the center of mass                        */
+	void setLinearVelocity(const btVector3& linVel);
+	/* Set the angular velocity of the center of mass                       */
+	void setAngularVelocity(const btVector3& angVel);
+	/* Get best fit rigid transform                                         */
+	btTransform getRigidTransform();
+	/* Transform to given pose                                              */
+	void transformTo(const btTransform& trs);
 	/* Transform															*/
 	void transform(const btTransform& trs);
 	/* Translate															*/
@@ -1023,6 +1068,11 @@ public:
 	bool rayTest(const btVector3& rayFrom,
 				 const btVector3& rayTo,
 				 sRayCast& results);
+	bool rayFaceTest(const btVector3& rayFrom,
+					 const btVector3& rayTo,
+					 sRayCast& results);
+	int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
+					btScalar& mint, int& index) const;
 	/* Solver presets														*/
 	void setSolver(eSolverPresets::_ preset);
 	/* predictMotion														*/
@@ -1120,6 +1170,7 @@ public:
 	int rayTest(const btVector3& rayFrom, const btVector3& rayTo,
 				btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
 	void initializeFaceTree();
+	void rebuildNodeTree();
 	btVector3 evaluateCom() const;
 	bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
 	bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
@@ -1138,6 +1189,8 @@ public:
 	void applyClusters(bool drift);
 	void dampClusters();
 	void setSpringStiffness(btScalar k);
+	void setGravityFactor(btScalar gravFactor);
+	void setCacheBarycenter(bool cacheBarycenter);
 	void initializeDmInverse();
 	void updateDeformation();
 	void advanceDeformation();
@@ -1152,7 +1205,186 @@ public:
 	static void VSolve_Links(btSoftBody* psb, btScalar kst);
 	static psolver_t getSolver(ePSolver::_ solver);
 	static vsolver_t getSolver(eVSolver::_ solver);
+	void geometricCollisionHandler(btSoftBody* psb);
+#define SAFE_EPSILON SIMD_EPSILON * 100.0
+	void updateNode(btDbvtNode* node, bool use_velocity, bool margin)
+	{
+		if (node->isleaf())
+		{
+			btSoftBody::Node* n = (btSoftBody::Node*)(node->data);
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
+			btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON;  // use user defined margin or margin for floating point precision
+			if (use_velocity)
+			{
+				btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v};
+				vol = btDbvtVolume::FromPoints(points, 2);
+				vol.Expand(btVector3(pad, pad, pad));
+			}
+			else
+			{
+				vol = btDbvtVolume::FromCR(n->m_x, pad);
+			}
+			node->volume = vol;
+			return;
+		}
+		else
+		{
+			updateNode(node->childs[0], use_velocity, margin);
+			updateNode(node->childs[1], use_velocity, margin);
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
+			Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
+			node->volume = vol;
+		}
+	}
 
+	void updateNodeTree(bool use_velocity, bool margin)
+	{
+		if (m_ndbvt.m_root)
+			updateNode(m_ndbvt.m_root, use_velocity, margin);
+	}
+
+	template <class DBVTNODE>  // btDbvtNode or btDbvntNode
+	void updateFace(DBVTNODE* node, bool use_velocity, bool margin)
+	{
+		if (node->isleaf())
+		{
+			btSoftBody::Face* f = (btSoftBody::Face*)(node->data);
+			btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON;  // use user defined margin or margin for floating point precision
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
+			if (use_velocity)
+			{
+				btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v,
+									   f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v,
+									   f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v};
+				vol = btDbvtVolume::FromPoints(points, 6);
+			}
+			else
+			{
+				btVector3 points[3] = {f->m_n[0]->m_x,
+									   f->m_n[1]->m_x,
+									   f->m_n[2]->m_x};
+				vol = btDbvtVolume::FromPoints(points, 3);
+			}
+			vol.Expand(btVector3(pad, pad, pad));
+			node->volume = vol;
+			return;
+		}
+		else
+		{
+			updateFace(node->childs[0], use_velocity, margin);
+			updateFace(node->childs[1], use_velocity, margin);
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
+			Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
+			node->volume = vol;
+		}
+	}
+	void updateFaceTree(bool use_velocity, bool margin)
+	{
+		if (m_fdbvt.m_root)
+			updateFace(m_fdbvt.m_root, use_velocity, margin);
+		if (m_fdbvnt)
+			updateFace(m_fdbvnt, use_velocity, margin);
+	}
+
+	template <typename T>
+	static inline T BaryEval(const T& a,
+							 const T& b,
+							 const T& c,
+							 const btVector3& coord)
+	{
+		return (a * coord.x() + b * coord.y() + c * coord.z());
+	}
+
+	void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
+	{
+		btAlignedObjectArray<int> indices;
+		{
+			// randomize the order of repulsive force
+			indices.resize(m_faceNodeContacts.size());
+			for (int i = 0; i < m_faceNodeContacts.size(); ++i)
+				indices[i] = i;
+#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
+			int i, ni;
+
+			for (i = 0, ni = indices.size(); i < ni; ++i)
+			{
+				btSwap(indices[i], indices[NEXTRAND % ni]);
+			}
+		}
+		for (int k = 0; k < m_faceNodeContacts.size(); ++k)
+		{
+			int idx = indices[k];
+			btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[idx];
+			btSoftBody::Node* node = c.m_node;
+			btSoftBody::Face* face = c.m_face;
+			const btVector3& w = c.m_bary;
+			const btVector3& n = c.m_normal;
+			btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w);
+			btScalar d = c.m_margin - n.dot(l);
+			d = btMax(btScalar(0), d);
+
+			const btVector3& va = node->m_v;
+			btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w);
+			btVector3 vr = va - vb;
+			const btScalar vn = btDot(vr, n);  // dn < 0 <==> opposing
+			if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep)
+				continue;
+			btVector3 vt = vr - vn * n;
+			btScalar I = 0;
+			btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im;
+			if (applySpringForce)
+				I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
+			if (vn < 0)
+				I += 0.5 * mass * vn;
+			int face_penetration = 0, node_penetration = node->m_constrained;
+			for (int i = 0; i < 3; ++i)
+				face_penetration |= face->m_n[i]->m_constrained;
+			btScalar I_tilde = 2.0 * I / (1.0 + w.length2());
+
+			//             double the impulse if node or face is constrained.
+			if (face_penetration > 0 || node_penetration > 0)
+			{
+				I_tilde *= 2.0;
+			}
+			if (face_penetration <= 0)
+			{
+				for (int j = 0; j < 3; ++j)
+					face->m_n[j]->m_v += w[j] * n * I_tilde * node->m_im;
+			}
+			if (node_penetration <= 0)
+			{
+				node->m_v -= I_tilde * node->m_im * n;
+			}
+
+			// apply frictional impulse
+			btScalar vt_norm = vt.safeNorm();
+			if (vt_norm > SIMD_EPSILON)
+			{
+				btScalar delta_vn = -2 * I * node->m_im;
+				btScalar mu = c.m_friction;
+				btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm;
+				I = 0.5 * mass * (vt_norm - vt_new);
+				vt.safeNormalize();
+				I_tilde = 2.0 * I / (1.0 + w.length2());
+				//                 double the impulse if node or face is constrained.
+				if (face_penetration > 0 || node_penetration > 0)
+					I_tilde *= 2.0;
+				if (face_penetration <= 0)
+				{
+					for (int j = 0; j < 3; ++j)
+						face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im;
+				}
+				if (node_penetration <= 0)
+				{
+					node->m_v -= I_tilde * node->m_im * vt;
+				}
+			}
+		}
+	}
 	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)

thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -1296,17 +1296,28 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
 			position.setY(p);
 			ss >> p;
 			position.setZ(p);
+			//printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ());
 			X[x_count++] = position;
 		}
 		else if (reading_tets)
 		{
+			int d;
+			ss >> d;
+			if (d != 4)
+			{
+				printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
+				fs.close();
+				return 0;
+			}
 			ss.ignore(128, ' ');  // ignore "4"
 			Index tet;
 			tet.resize(4);
 			for (size_t i = 0; i < 4; i++)
 			{
 				ss >> tet[i];
+				//printf("%d ", tet[i]);
 			}
+			//printf("\n");
 			indices[indices_count++] = tet;
 		}
 	}
@@ -1326,7 +1337,6 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
 		}
 	}
 
-    
 	generateBoundaryFaces(psb);
 	psb->initializeDmInverse();
 	psb->m_tetraScratches.resize(psb->m_tetras.size());
@@ -1407,14 +1417,53 @@ void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb)
 	{
 		std::vector<int> f = it->second;
 		psb->appendFace(f[0], f[1], f[2]);
+		//printf("f %d %d %d\n", f[0] + 1, f[1] + 1, f[2] + 1);
 	}
 }
 
+//Write the surface mesh to an obj file.
 void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
 {
 	std::ofstream fs;
 	fs.open(filename);
 	btAssert(fs);
+
+	if (psb->m_tetras.size() > 0)
+	{
+		// For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/
+		std::map<int, int> dict;
+		for (int i = 0; i < psb->m_faces.size(); i++)
+		{
+			for (int d = 0; d < 3; d++)
+			{
+				int index = psb->m_faces[i].m_n[d]->index;
+				if (dict.find(index) == dict.end())
+				{
+					int dict_size = dict.size();
+					dict[index] = dict_size;
+					fs << "v";
+					for (int k = 0; k < 3; k++)
+					{
+						fs << " " << psb->m_nodes[index].m_x[k];
+					}
+					fs << "\n";
+				}
+			}
+		}
+		// Write surface mesh.
+		for (int i = 0; i < psb->m_faces.size(); ++i)
+		{
+			fs << "f";
+			for (int n = 0; n < 3; n++)
+			{
+				fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1;
+			}
+			fs << "\n";
+		}
+	}
+	else
+	{
+		// For trimesh, directly write out all the nodes and faces.xs
 		for (int i = 0; i < psb->m_nodes.size(); ++i)
 		{
 			fs << "v";
@@ -1434,6 +1483,7 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
 			}
 			fs << "\n";
 		}
+	}
 	fs.close();
 }
 
@@ -1500,10 +1550,27 @@ void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector
 	bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6);
 }
 
+// Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0.
+void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary)
+{
+	btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
+	btScalar d00 = btDot(v0, v0);
+	btScalar d01 = btDot(v0, v1);
+	btScalar d11 = btDot(v1, v1);
+	btScalar d20 = btDot(v2, v0);
+	btScalar d21 = btDot(v2, v1);
+	btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01);
+	bary[1] = (d11 * d20 - d01 * d21) * invDenom;
+	bary[2] = (d00 * d21 - d01 * d20) * invDenom;
+	bary[0] = 1.0 - bary[1] - bary[2];
+	bary[3] = 0;
+}
+
 // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
 // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
 void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
 {
+	psb->m_z.resize(0);
 	psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
 	psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
 	for (int i = 0; i < psb->m_renderNodes.size(); ++i)
@@ -1513,7 +1580,6 @@ void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
 		btVector4 optimal_bary;
 		btScalar min_bary_weight = -1e3;
 		btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
-        bool found = false;
 		for (int j = 0; j < psb->m_tetras.size(); ++j)
 		{
 			const btSoftBody::Tetra& t = psb->m_tetras[j];
@@ -1544,3 +1610,54 @@ void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
 		psb->m_renderNodesParents[i] = optimal_parents;
 	}
 }
+
+// Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense.
+void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
+{
+	psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
+	psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
+	psb->m_z.resize(psb->m_renderNodes.size());
+	for (int i = 0; i < psb->m_renderNodes.size(); ++i)
+	{
+		const btVector3& p = psb->m_renderNodes[i].m_x;
+		btVector4 bary;
+		btVector4 optimal_bary;
+		btScalar min_bary_weight = -SIMD_INFINITY;
+		btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
+		btScalar dist = 0, optimal_dist = 0;
+		for (int j = 0; j < psb->m_faces.size(); ++j)
+		{
+			const btSoftBody::Face& f = psb->m_faces[j];
+			btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
+			btVector3 unit_n = n.normalized();
+			dist = (p - f.m_n[0]->m_x).dot(unit_n);
+			btVector3 proj_p = p - dist * unit_n;
+			getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary);
+			btScalar new_min_bary_weight = bary[0];
+			for (int k = 1; k < 3; ++k)
+			{
+				new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
+			}
+
+			// p is out of the current best triangle, we found a traingle that's better
+			bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.);
+			// p is inside of the current best triangle, we found a triangle that's better
+			bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist));
+
+			if (better_than_closest_outisde || better_than_best_inside)
+			{
+				btAlignedObjectArray<const btSoftBody::Node*> parents;
+				parents.push_back(f.m_n[0]);
+				parents.push_back(f.m_n[1]);
+				parents.push_back(f.m_n[2]);
+				optimal_parents = parents;
+				optimal_bary = bary;
+				optimal_dist = dist;
+				min_bary_weight = new_min_bary_weight;
+			}
+		}
+		psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
+		psb->m_renderNodesParents[i] = optimal_parents;
+		psb->m_z[i] = optimal_dist;
+	}
+}

thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h

@@ -148,8 +148,12 @@ struct btSoftBodyHelpers
 
 	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
 
+	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary);
+
 	static void interpolateBarycentricWeights(btSoftBody* psb);
 
+	static void extrapolateBarycentricWeights(btSoftBody* psb);
+
 	static void generateBoundaryFaces(btSoftBody* psb);
 
 	static void duplicateFaces(const char* filename, const btSoftBody* psb);

thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h

@@ -18,7 +18,6 @@ subject to the following restrictions:
 #define _BT_SOFT_BODY_INTERNALS_H
 
 #include "btSoftBody.h"
-
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btPolarDecomposition.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
@@ -29,9 +28,10 @@ subject to the following restrictions:
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include <string.h>  //for memset
 #include <cmath>
+#include "poly34.h"
 
 // Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction
-static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
+static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
 										   btMultiBodyJacobianData& jacobianData,
 										   const btVector3& contact_point,
 										   const btVector3& dir)
@@ -44,7 +44,7 @@ static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
 	multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
 	multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
 }
-static btVector3 generateUnitOrthogonalVector(const btVector3& u)
+static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3& u)
 {
 	btScalar ux = u.getX();
 	btScalar uy = u.getY();
@@ -62,6 +62,575 @@ static btVector3 generateUnitOrthogonalVector(const btVector3& u)
 	v.normalize();
 	return v;
 }
+
+static SIMD_FORCE_INLINE bool proximityTest(const btVector3& x1, const btVector3& x2, const btVector3& x3, const btVector3& x4, const btVector3& normal, const btScalar& mrg, btVector3& bary)
+{
+	btVector3 x43 = x4 - x3;
+	if (std::abs(x43.dot(normal)) > mrg)
+		return false;
+	btVector3 x13 = x1 - x3;
+	btVector3 x23 = x2 - x3;
+	btScalar a11 = x13.length2();
+	btScalar a22 = x23.length2();
+	btScalar a12 = x13.dot(x23);
+	btScalar b1 = x13.dot(x43);
+	btScalar b2 = x23.dot(x43);
+	btScalar det = a11 * a22 - a12 * a12;
+	if (det < SIMD_EPSILON)
+		return false;
+	btScalar w1 = (b1 * a22 - b2 * a12) / det;
+	btScalar w2 = (b2 * a11 - b1 * a12) / det;
+	btScalar w3 = 1 - w1 - w2;
+	btScalar delta = mrg / std::sqrt(0.5 * std::abs(x13.cross(x23).safeNorm()));
+	bary = btVector3(w1, w2, w3);
+	for (int i = 0; i < 3; ++i)
+	{
+		if (bary[i] < -delta || bary[i] > 1 + delta)
+			return false;
+	}
+	return true;
+}
+static const int KDOP_COUNT = 13;
+static btVector3 dop[KDOP_COUNT] = {btVector3(1, 0, 0),
+									btVector3(0, 1, 0),
+									btVector3(0, 0, 1),
+									btVector3(1, 1, 0),
+									btVector3(1, 0, 1),
+									btVector3(0, 1, 1),
+									btVector3(1, -1, 0),
+									btVector3(1, 0, -1),
+									btVector3(0, 1, -1),
+									btVector3(1, 1, 1),
+									btVector3(1, -1, 1),
+									btVector3(1, 1, -1),
+									btVector3(1, -1, -1)};
+
+static inline int getSign(const btVector3& n, const btVector3& x)
+{
+	btScalar d = n.dot(x);
+	if (d > SIMD_EPSILON)
+		return 1;
+	if (d < -SIMD_EPSILON)
+		return -1;
+	return 0;
+}
+
+static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
+{
+	btVector3 hex[6] = {face->m_n[0]->m_x - node->m_x,
+						face->m_n[1]->m_x - node->m_x,
+						face->m_n[2]->m_x - node->m_x,
+						face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x,
+						face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x,
+						face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x};
+	btVector3 segment = dt * node->m_v;
+	for (int i = 0; i < KDOP_COUNT; ++i)
+	{
+		int s = getSign(dop[i], segment);
+		int j = 0;
+		for (; j < 6; ++j)
+		{
+			if (getSign(dop[i], hex[j]) == s)
+				break;
+		}
+		if (j == 6)
+			return true;
+	}
+	return false;
+}
+
+static SIMD_FORCE_INLINE bool nearZero(const btScalar& a)
+{
+	return (a > -SAFE_EPSILON && a < SAFE_EPSILON);
+}
+static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b)
+{
+	return (nearZero(a) || nearZero(b) || (a > SAFE_EPSILON && b > SAFE_EPSILON) || (a < -SAFE_EPSILON && b < -SAFE_EPSILON));
+}
+static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b)
+{
+	return !sameSign(a, b);
+}
+inline btScalar evaluateBezier2(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& t, const btScalar& s)
+{
+	btScalar s2 = s * s;
+	btScalar t2 = t * t;
+
+	return p0 * s2 + p1 * btScalar(2.0) * s * t + p2 * t2;
+}
+inline btScalar evaluateBezier(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& p3, const btScalar& t, const btScalar& s)
+{
+	btScalar s2 = s * s;
+	btScalar s3 = s2 * s;
+	btScalar t2 = t * t;
+	btScalar t3 = t2 * t;
+
+	return p0 * s3 + p1 * btScalar(3.0) * s2 * t + p2 * btScalar(3.0) * s * t2 + p3 * t3;
+}
+static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar& lt0, btScalar& lt1)
+{
+	if (sameSign(t0, t1))
+	{
+		lt0 = t0;
+		lt1 = t0;
+		return true;
+	}
+
+	if (type_c || diffSign(k0, k3))
+	{
+		btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
+		if (t0 < -0)
+			ft = -ft;
+
+		if (sameSign(ft, k0))
+		{
+			lt0 = t1;
+			lt1 = t1;
+		}
+		else
+		{
+			lt0 = t0;
+			lt1 = t0;
+		}
+		return true;
+	}
+
+	if (!type_c)
+	{
+		btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
+		if (t0 < -0)
+			ft = -ft;
+
+		if (diffSign(ft, k0))
+		{
+			lt0 = t0;
+			lt1 = t1;
+			return true;
+		}
+
+		btScalar fk = evaluateBezier2(k1 - k0, k2 - k1, k3 - k2, t0, -t1);
+
+		if (sameSign(fk, k1 - k0))
+			lt0 = lt1 = t1;
+		else
+			lt0 = lt1 = t0;
+
+		return true;
+	}
+	return false;
+}
+
+static SIMD_FORCE_INLINE void getBernsteinCoeff(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, btScalar& k0, btScalar& k1, btScalar& k2, btScalar& k3)
+{
+	const btVector3& n0 = face->m_n0;
+	const btVector3& n1 = face->m_n1;
+	btVector3 n_hat = n0 + n1 - face->m_vn;
+	btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x;
+	btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q;
+	k0 = (p0ma0).dot(n0) * 3.0;
+	k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0);
+	k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1);
+	k3 = (p1ma1).dot(n1) * 3.0;
+}
+
+static SIMD_FORCE_INLINE void polyDecomposition(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, btScalar& u0, btScalar& u1, btScalar& v0, btScalar& v1)
+{
+	btScalar denom = 4.0 * (j1 - j2) * (j1 - j0) + (j2 - j0) * (j2 - j0);
+	u0 = (2.0 * (j1 - j2) * (3.0 * k1 - 2.0 * k0 - k3) - (j0 - j2) * (3.0 * k2 - 2.0 * k3 - k0)) / denom;
+	u1 = (2.0 * (j1 - j0) * (3.0 * k2 - 2.0 * k3 - k0) - (j2 - j0) * (3.0 * k1 - 2.0 * k0 - k3)) / denom;
+	v0 = k0 - u0 * j0;
+	v1 = k3 - u1 * j2;
+}
+
+static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3)
+{
+	btScalar u0, u1, v0, v1;
+	btScalar j0 = 3.0 * (k1 - k0);
+	btScalar j1 = 3.0 * (k2 - k1);
+	btScalar j2 = 3.0 * (k3 - k2);
+	polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+	if (sameSign(v0, v1))
+	{
+		btScalar Ypa = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * v0 * (1.0 - v0) + j2 * v0 * v0;  // Y'(v0)
+		if (sameSign(Ypa, j0))
+		{
+			return (diffSign(k0, v1));
+		}
+	}
+	return diffSign(k0, v0);
+}
+
+static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2)
+{
+	const btVector3& a0 = a->m_x;
+	const btVector3& b0 = b->m_x;
+	const btVector3& c0 = c->m_x;
+	const btVector3& va = a->m_v;
+	const btVector3& vb = b->m_v;
+	const btVector3& vc = c->m_v;
+	const btVector3 a1 = a0 + dt * va;
+	const btVector3 b1 = b0 + dt * vb;
+	const btVector3 c1 = c0 + dt * vc;
+	btVector3 n0 = (b0 - a0).cross(c0 - a0);
+	btVector3 n1 = (b1 - a1).cross(c1 - a1);
+	btVector3 n_hat = n0 + n1 - dt * dt * (vb - va).cross(vc - va);
+	const btVector3& p0 = p->m_x;
+	const btVector3& vp = p->m_v;
+	btVector3 p1 = p0 + dt * vp;
+	btVector3 m0 = (b0 - p0).cross(c0 - p0);
+	btVector3 m1 = (b1 - p1).cross(c1 - p1);
+	btVector3 m_hat = m0 + m1 - dt * dt * (vb - vp).cross(vc - vp);
+	btScalar l0 = m0.dot(n0);
+	btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0));
+	btScalar l2 = btScalar(1) / btScalar(6) * (m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0));
+	btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat));
+	btScalar l4 = m1.dot(n1);
+
+	btScalar k1p = 0.25 * k0 + 0.75 * k1;
+	btScalar k2p = 0.5 * k1 + 0.5 * k2;
+	btScalar k3p = 0.75 * k2 + 0.25 * k3;
+
+	btScalar s0 = (l1 * k0 - l0 * k1p) * 4.0;
+	btScalar s1 = (l2 * k0 - l0 * k2p) * 2.0;
+	btScalar s2 = (l3 * k0 - l0 * k3p) * btScalar(4) / btScalar(3);
+	btScalar s3 = l4 * k0 - l0 * k3;
+
+	j0 = (s1 * k0 - s0 * k1) * 3.0;
+	j1 = (s2 * k0 - s0 * k2) * 1.5;
+	j2 = (s3 * k0 - s0 * k3);
+}
+
+static SIMD_FORCE_INLINE bool signDetermination1Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1)
+{
+	btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0;  // Y(u0)
+	btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0;  // Y(v0)
+
+	btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0;
+	btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0;
+	sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
+	btScalar K = (sameSign(sign_Ytp, k0)) ? v1 : v0;
+	return diffSign(L, K);
+}
+
+static SIMD_FORCE_INLINE bool signDetermination2Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1)
+{
+	btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0;  // Y(u0)
+	btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2;
+	if (diffSign(sign_Ytp, k0))
+	{
+		L1 = u0;
+		L2 = u1;
+	}
+	else
+	{
+		btScalar Yp_u0 = j0 * (1.0 - u0) * (1.0 - u0) + 2.0 * j1 * (1.0 - u0) * u0 + j2 * u0 * u0;
+		if (sameSign(Yp_u0, j0))
+		{
+			L1 = u1;
+			L2 = u1;
+		}
+		else
+		{
+			L1 = u0;
+			L2 = u0;
+		}
+	}
+	btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0;  // Y(uv0)
+	sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
+	btScalar K1, K2;
+	if (diffSign(sign_Ytp, k0))
+	{
+		K1 = v0;
+		K2 = v1;
+	}
+	else
+	{
+		btScalar Yp_v0 = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * (1.0 - v0) * v0 + j2 * v0 * v0;
+		if (sameSign(Yp_v0, j0))
+		{
+			K1 = v1;
+			K2 = v1;
+		}
+		else
+		{
+			K1 = v0;
+			K2 = v0;
+		}
+	}
+	return (diffSign(K1, L1) || diffSign(L2, K2));
+}
+
+static SIMD_FORCE_INLINE bool signDetermination1(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
+{
+	btScalar j0, j1, j2, u0, u1, v0, v1;
+	// p1
+	getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
+			return false;
+	}
+	// p2
+	getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
+			return false;
+	}
+	// p3
+	getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
+			return false;
+	}
+	return true;
+}
+
+static SIMD_FORCE_INLINE bool signDetermination2(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
+{
+	btScalar j0, j1, j2, u0, u1, v0, v1;
+	// p1
+	getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		bool bt0 = true, bt1 = true;
+		getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			bt0 = false;
+		if (lt1 < -SAFE_EPSILON)
+			bt1 = false;
+		if (!bt0 && !bt1)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
+			return false;
+	}
+	// p2
+	getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		bool bt0 = true, bt1 = true;
+		getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			bt0 = false;
+		if (lt1 < -SAFE_EPSILON)
+			bt1 = false;
+		if (!bt0 && !bt1)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
+			return false;
+	}
+	// p3
+	getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
+	if (nearZero(j0 + j2 - j1 * 2.0))
+	{
+		btScalar lt0, lt1;
+		bool bt0 = true, bt1 = true;
+		getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
+		if (lt0 < -SAFE_EPSILON)
+			bt0 = false;
+		if (lt1 < -SAFE_EPSILON)
+			bt1 = false;
+		if (!bt0 && !bt1)
+			return false;
+	}
+	else
+	{
+		polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+		if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
+			return false;
+	}
+	return true;
+}
+
+static SIMD_FORCE_INLINE bool coplanarAndInsideTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
+{
+	// Coplanar test
+	if (diffSign(k1 - k0, k3 - k2))
+	{
+		// Case b:
+		if (sameSign(k0, k3) && !rootFindingLemma(k0, k1, k2, k3))
+			return false;
+		// inside test
+		return signDetermination2(k0, k1, k2, k3, face, node, dt);
+	}
+	else
+	{
+		// Case c:
+		if (sameSign(k0, k3))
+			return false;
+		// inside test
+		return signDetermination1(k0, k1, k2, k3, face, node, dt);
+	}
+	return false;
+}
+static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg)
+{
+	if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg)
+		return true;
+	if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg)
+		return true;
+	return false;
+}
+
+static SIMD_FORCE_INLINE bool bernsteinVFTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
+{
+	if (conservativeCulling(k0, k1, k2, k3, mrg))
+		return false;
+	return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
+}
+
+static SIMD_FORCE_INLINE void deCasteljau(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, btScalar& k10, btScalar& k20, btScalar& k30, btScalar& k21, btScalar& k12)
+{
+	k10 = k0 * (1.0 - t0) + k1 * t0;
+	btScalar k11 = k1 * (1.0 - t0) + k2 * t0;
+	k12 = k2 * (1.0 - t0) + k3 * t0;
+	k20 = k10 * (1.0 - t0) + k11 * t0;
+	k21 = k11 * (1.0 - t0) + k12 * t0;
+	k30 = k20 * (1.0 - t0) + k21 * t0;
+}
+static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg)
+{
+	btScalar k0, k1, k2, k3;
+	getBernsteinCoeff(face, node, dt, k0, k1, k2, k3);
+	if (conservativeCulling(k0, k1, k2, k3, mrg))
+		return false;
+	return true;
+	if (diffSign(k2 - 2.0 * k1 + k0, k3 - 2.0 * k2 + k1))
+	{
+		btScalar k10, k20, k30, k21, k12;
+		btScalar t0 = (k2 - 2.0 * k1 + k0) / (k0 - 3.0 * k1 + 3.0 * k2 - k3);
+		deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12);
+		return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt);
+	}
+	return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
+}
+
+static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary)
+{
+	if (hasSeparatingPlane(face, node, dt))
+		return false;
+	btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x;
+	btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x;
+	btVector3 x41 = node->m_x - face->m_n[0]->m_x;
+	btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v;
+	btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v;
+	btVector3 v41 = node->m_v - face->m_n[0]->m_v;
+	btVector3 a = x21.cross(x31);
+	btVector3 b = x21.cross(v31) + v21.cross(x31);
+	btVector3 c = v21.cross(v31);
+	btVector3 d = x41;
+	btVector3 e = v41;
+	btScalar a0 = a.dot(d);
+	btScalar a1 = a.dot(e) + b.dot(d);
+	btScalar a2 = c.dot(d) + b.dot(e);
+	btScalar a3 = c.dot(e);
+	btScalar eps = SAFE_EPSILON;
+	int num_roots = 0;
+	btScalar roots[3];
+	if (std::abs(a3) < eps)
+	{
+		// cubic term is zero
+		if (std::abs(a2) < eps)
+		{
+			if (std::abs(a1) < eps)
+			{
+				if (std::abs(a0) < eps)
+				{
+					num_roots = 2;
+					roots[0] = 0;
+					roots[1] = dt;
+				}
+			}
+			else
+			{
+				num_roots = 1;
+				roots[0] = -a0 / a1;
+			}
+		}
+		else
+		{
+			num_roots = SolveP2(roots, a1 / a2, a0 / a2);
+		}
+	}
+	else
+	{
+		num_roots = SolveP3(roots, a2 / a3, a1 / a3, a0 / a3);
+	}
+	//    std::sort(roots, roots+num_roots);
+	if (num_roots > 1)
+	{
+		if (roots[0] > roots[1])
+			btSwap(roots[0], roots[1]);
+	}
+	if (num_roots > 2)
+	{
+		if (roots[0] > roots[2])
+			btSwap(roots[0], roots[2]);
+		if (roots[1] > roots[2])
+			btSwap(roots[1], roots[2]);
+	}
+	for (int r = 0; r < num_roots; ++r)
+	{
+		double root = roots[r];
+		if (root <= 0)
+			continue;
+		if (root > dt + SIMD_EPSILON)
+			return false;
+		btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v;
+		btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v;
+		btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v;
+		btVector3 x4 = node->m_x + root * node->m_v;
+		btVector3 normal = (x2 - x1).cross(x3 - x1);
+		normal.safeNormalize();
+		if (proximityTest(x1, x2, x3, x4, normal, mrg, bary))
+			return true;
+	}
+	return false;
+}
+static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary)
+{
+	if (!bernsteinVFTest(face, node, dt, mrg))
+		return false;
+	if (!continuousCollisionDetection(face, node, dt, 1e-6, bary))
+		return false;
+	return true;
+}
+
 //
 // btSymMatrix
 //
@@ -373,6 +942,25 @@ static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,con
 	return (m);
 }
 
+static inline btMatrix3x3 OuterProduct(const btVector3& v1, const btVector3& v2)
+{
+	btMatrix3x3 m;
+	btScalar a11 = v1[0] * v2[0];
+	btScalar a12 = v1[0] * v2[1];
+	btScalar a13 = v1[0] * v2[2];
+
+	btScalar a21 = v1[1] * v2[0];
+	btScalar a22 = v1[1] * v2[1];
+	btScalar a23 = v1[1] * v2[2];
+
+	btScalar a31 = v1[2] * v2[0];
+	btScalar a32 = v1[2] * v2[1];
+	btScalar a33 = v1[2] * v2[2];
+	m[0] = btVector3(a11, a12, a13);
+	m[1] = btVector3(a21, a22, a23);
+	m[2] = btVector3(a31, a32, a33);
+	return (m);
+}
 
 //
 static inline btMatrix3x3 Add(const btMatrix3x3& a,
@@ -422,6 +1010,20 @@ static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
 	return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse());
 }
 
+//
+static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
+										const btMatrix3x3& effective_mass_inv,
+										btScalar imb,
+										const btMatrix3x3& iwi,
+										const btVector3& r)
+{
+	return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse());
+	//    btMatrix3x3 iimb = MassMatrix(imb, iwi, r);
+	//    if (iimb.determinant() == 0)
+	//        return effective_mass_inv.inverse();
+	//    return effective_mass_inv.inverse() *  Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse();
+}
+
 //
 static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
 										btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
@@ -544,9 +1146,7 @@ static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVec
 	if (dir_norm < SIMD_EPSILON)
 		return false;
 	dir.normalize();
-
 	btScalar t;
-    
 	bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
 
 	if (ret)
@@ -573,7 +1173,6 @@ static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVec
 	return ret;
 }
 
-
 //
 template <typename T>
 static inline T BaryEval(const T& a,
@@ -1070,8 +1669,8 @@ struct btSoftColliders
 
 			if (!n.m_battach)
 			{
-                // check for collision at x_{n+1}^* as well at x_n
-                if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
+				// check for collision at x_{n+1}^*
+				if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
 				{
 					const btScalar ima = n.m_im;
 					// todo: collision between multibody and fixed deformable node will be missed.
@@ -1087,6 +1686,7 @@ struct btSoftColliders
 						c.m_c2 = ima;
 						c.m_c3 = fc;
 						c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+						c.m_c5 = n.m_effectiveMass_inv;
 
 						if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 						{
@@ -1095,7 +1695,8 @@ struct btSoftColliders
 							const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
 							const btVector3 ra = n.m_x - wtr.getOrigin();
 
-                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+							c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
+							//                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
 							c.m_c1 = ra;
 						}
 						else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
@@ -1123,7 +1724,7 @@ struct btSoftColliders
 												t1.getX(), t1.getY(), t1.getZ(),
 												t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
 								const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                                btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+								btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
 								c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
 								c.jacobianData_normal = jacobianData_normal;
 								c.jacobianData_t1 = jacobianData_t1;
@@ -1159,14 +1760,12 @@ struct btSoftColliders
 			btSoftBody::Node* n0 = f.m_n[0];
 			btSoftBody::Node* n1 = f.m_n[1];
 			btSoftBody::Node* n2 = f.m_n[2];
-            
 			const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0) ? dynmargin : stamargin;
 			btSoftBody::DeformableFaceRigidContact c;
 			btVector3 contact_point;
 			btVector3 bary;
 			if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
 			{
-                f.m_pcontact[3] = 1;
 				btScalar ima = n0->m_im + n1->m_im + n2->m_im;
 				const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
 				// todo: collision between multibody and fixed deformable face will be missed.
@@ -1174,21 +1773,23 @@ struct btSoftColliders
 				if (ms > 0)
 				{
 					// resolve contact at x_n
-                    psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
+					//                    psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
 					btSoftBody::sCti& cti = c.m_cti;
 					c.m_contactPoint = contact_point;
 					c.m_bary = bary;
 					// todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
 					c.m_weights = btScalar(2) / (btScalar(1) + bary.length2()) * bary;
 					c.m_face = &f;
+					// friction is handled by the nodes to prevent sticking
+					//                    const btScalar fc = 0;
 					const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
 
 					// the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
 					ima = bary.getX() * c.m_weights.getX() * n0->m_im + bary.getY() * c.m_weights.getY() * n1->m_im + bary.getZ() * c.m_weights.getZ() * n2->m_im;
-                    
 					c.m_c2 = ima;
 					c.m_c3 = fc;
 					c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+					c.m_c5 = Diagonal(ima);
 					if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 					{
 						const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
@@ -1237,11 +1838,10 @@ struct btSoftColliders
 					psb->m_faceRigidContacts.push_back(c);
 				}
 			}
-            else
-            {
+			// Set caching barycenters to be false after collision detection.
+			// Only turn on when contact is static.
 			f.m_pcontact[3] = 0;
 		}
-        }
 		btSoftBody* psb;
 		const btCollisionObjectWrapper* m_colObj1Wrap;
 		btRigidBody* m_rigidBody;
@@ -1305,7 +1905,6 @@ struct btSoftColliders
 		btScalar mrg;
 	};
 
-    
 	//
 	// CollideVF_DD
 	//
@@ -1316,19 +1915,11 @@ struct btSoftColliders
 		{
 			btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
 			btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
-            
-            btVector3 o = node->m_x;
-            btVector3 p;
-            btScalar d = SIMD_INFINITY;
-            ProjectOrigin(face->m_n[0]->m_x - o,
-                          face->m_n[1]->m_x - o,
-                          face->m_n[2]->m_x - o,
-                          p, d);
-            const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
-            if (d < (m * m))
+			btVector3 bary;
+			if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary))
 			{
 				const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
-                const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
+				const btVector3 w = bary;
 				const btScalar ma = node->m_im;
 				btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
 				if ((n[0]->m_im <= 0) ||
@@ -1341,20 +1932,14 @@ struct btSoftColliders
 				if (ms > 0)
 				{
 					btSoftBody::DeformableFaceNodeContact c;
-                    if (useFaceNormal)
 					c.m_normal = face->m_normal;
-                    else
-                        c.m_normal = p / -btSqrt(d);
+					if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+						c.m_normal = -face->m_normal;
 					c.m_margin = mrg;
 					c.m_node = node;
 					c.m_face = face;
 					c.m_bary = w;
-                    // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
-                    c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
 					c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
-                    // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
-                    c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
-                    c.m_c0 = btScalar(1)/(ma + c.m_imf);
 					psb[0]->m_faceNodeContacts.push_back(c);
 				}
 			}
@@ -1372,69 +1957,152 @@ struct btSoftColliders
 		void Process(const btDbvntNode* lface1,
 					 const btDbvntNode* lface2)
 		{
-            btSoftBody::Face* f = (btSoftBody::Face*)lface1->data;
-            btSoftBody::Face* face = (btSoftBody::Face*)lface2->data;
+			btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
+			btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
+			if (f1 != f2)
+			{
+				Repel(f1, f2);
+				Repel(f2, f1);
+			}
+		}
+		void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
+		{
+			//#define REPEL_NEIGHBOR 1
+#ifndef REPEL_NEIGHBOR
 			for (int node_id = 0; node_id < 3; ++node_id)
 			{
-                btSoftBody::Node* node = f->m_n[node_id];
-                bool skip = false;
+				btSoftBody::Node* node = f1->m_n[node_id];
 				for (int i = 0; i < 3; ++i)
 				{
-                    if (face->m_n[i] == node)
+					if (f2->m_n[i] == node)
+						return;
+				}
+			}
+#endif
+			bool skip = false;
+			for (int node_id = 0; node_id < 3; ++node_id)
+			{
+				btSoftBody::Node* node = f1->m_n[node_id];
+#ifdef REPEL_NEIGHBOR
+				for (int i = 0; i < 3; ++i)
+				{
+					if (f2->m_n[i] == node)
 					{
 						skip = true;
 						break;
 					}
 				}
 				if (skip)
+				{
+					skip = false;
 					continue;
-                btVector3 o = node->m_x;
-                btVector3 p;
-                btScalar d = SIMD_INFINITY;
-                ProjectOrigin(face->m_n[0]->m_x - o,
-                              face->m_n[1]->m_x - o,
-                              face->m_n[2]->m_x - o,
-                              p, d);
-                const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
-                if (d < (m * m))
-                {
-                    const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
-                    const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
-                    const btScalar ma = node->m_im;
-                    btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
-                    if ((n[0]->m_im <= 0) ||
-                        (n[1]->m_im <= 0) ||
-                        (n[2]->m_im <= 0))
-                    {
-                        mb = 0;
 				}
-                    const btScalar ms = ma + mb;
-                    if (ms > 0)
-                    {
+#endif
+				btSoftBody::Face* face = f2;
+				btVector3 bary;
+				if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary))
+					continue;
 				btSoftBody::DeformableFaceNodeContact c;
-                        if (useFaceNormal)
 				c.m_normal = face->m_normal;
-                        else
-                            c.m_normal = p / -btSqrt(d);
+				if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+					c.m_normal = -face->m_normal;
 				c.m_margin = mrg;
 				c.m_node = node;
 				c.m_face = face;
-                        c.m_bary = w;
-                        // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
-                        c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
+				c.m_bary = bary;
 				c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
-                        // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
-                        c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
-                        c.m_c0 = btScalar(1)/(ma + c.m_imf);
 				psb[0]->m_faceNodeContacts.push_back(c);
 			}
 		}
-            }
-        }
 		btSoftBody* psb[2];
 		btScalar mrg;
 		bool useFaceNormal;
 	};
-};
 
+	struct CollideCCD : btDbvt::ICollide
+	{
+		void Process(const btDbvtNode* lnode,
+					 const btDbvtNode* lface)
+		{
+			btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
+			btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
+			btVector3 bary;
+			if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
+			{
+				btSoftBody::DeformableFaceNodeContact c;
+				c.m_normal = face->m_normal;
+				if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+					c.m_normal = -face->m_normal;
+				c.m_node = node;
+				c.m_face = face;
+				c.m_bary = bary;
+				c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
+				psb[0]->m_faceNodeContacts.push_back(c);
+			}
+		}
+		void Process(const btDbvntNode* lface1,
+					 const btDbvntNode* lface2)
+		{
+			btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
+			btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
+			if (f1 != f2)
+			{
+				Repel(f1, f2);
+				Repel(f2, f1);
+			}
+		}
+		void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
+		{
+			//#define REPEL_NEIGHBOR 1
+#ifndef REPEL_NEIGHBOR
+			for (int node_id = 0; node_id < 3; ++node_id)
+			{
+				btSoftBody::Node* node = f1->m_n[node_id];
+				for (int i = 0; i < 3; ++i)
+				{
+					if (f2->m_n[i] == node)
+						return;
+				}
+			}
+#endif
+			bool skip = false;
+			for (int node_id = 0; node_id < 3; ++node_id)
+			{
+				btSoftBody::Node* node = f1->m_n[node_id];
+#ifdef REPEL_NEIGHBOR
+				for (int i = 0; i < 3; ++i)
+				{
+					if (f2->m_n[i] == node)
+					{
+						skip = true;
+						break;
+					}
+				}
+				if (skip)
+				{
+					skip = false;
+					continue;
+				}
+#endif
+				btSoftBody::Face* face = f2;
+				btVector3 bary;
+				if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
+				{
+					btSoftBody::DeformableFaceNodeContact c;
+					c.m_normal = face->m_normal;
+					if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+						c.m_normal = -face->m_normal;
+					c.m_node = node;
+					c.m_face = face;
+					c.m_bary = bary;
+					c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
+					psb[0]->m_faceNodeContacts.push_back(c);
+				}
+			}
+		}
+		btSoftBody* psb[2];
+		btScalar dt, mrg;
+		bool useFaceNormal;
+	};
+};
 #endif  //_BT_SOFT_BODY_INTERNALS_H

thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp

@@ -100,6 +100,11 @@ void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeSte
 	///update soft bodies
 	m_softBodySolver->updateSoftBodies();
 
+	for (int i = 0; i < m_softBodies.size(); i++)
+	{
+		btSoftBody* psb = (btSoftBody*)m_softBodies[i];
+		psb->interpolateRenderMesh();
+	}
 	// End solver-wise simulation step
 	// ///////////////////////////////
 }

thirdparty/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp

@@ -48,9 +48,10 @@ btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
 }
 
 #include <stdio.h>
-
+#include "LinearMath/btQuickprof.h"
 void btSoftRigidCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
+	BT_PROFILE("btSoftRigidCollisionAlgorithm::processCollision");
 	(void)dispatchInfo;
 	(void)resultOut;
 	//printf("btSoftRigidCollisionAlgorithm\n");

thirdparty/bullet/BulletSoftBody/btSparseSDF.h

@@ -23,18 +23,19 @@ subject to the following restrictions:
 // Fast Hash
 
 #if !defined(get16bits)
-#define get16bits(d) ((((unsigned int)(((const unsigned char *)(d))[1])) << 8)\
-+(unsigned int)(((const unsigned char *)(d))[0]) )
+#define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0]))
 #endif
 //
 // super hash function by Paul Hsieh
 //
-inline unsigned int HsiehHash (const char * data, int len) {
+inline unsigned int HsiehHash(const char* data, int len)
+{
 	unsigned int hash = len, tmp;
 	len >>= 2;
 
 	/* Main loop */
-    for (;len > 0; len--) {
+	for (; len > 0; len--)
+	{
 		hash += get16bits(data);
 		tmp = (get16bits(data + 2) << 11) ^ hash;
 		hash = (hash << 16) ^ tmp;
@@ -42,7 +43,6 @@ inline unsigned int HsiehHash (const char * data, int len) {
 		hash += hash >> 11;
 	}
 
-
 	/* Force "avalanching" of final 127 bits */
 	hash ^= hash << 3;
 	hash += hash >> 5;

thirdparty/bullet/BulletSoftBody/poly34.cpp

@@ -0,0 +1,447 @@
+// poly34.cpp : solution of cubic and quartic equation
+// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html
+// khash2 (at) gmail.com
+// Thanks to Alexandr Rakhmanin <rakhmanin (at) gmail.com>
+// public domain
+//
+#include <math.h>
+
+#include "poly34.h"  // solution of cubic and quartic equation
+#define TwoPi 6.28318530717958648
+const btScalar eps = SIMD_EPSILON;
+
+//=============================================================================
+// _root3, root3 from http://prografix.narod.ru
+//=============================================================================
+static SIMD_FORCE_INLINE btScalar _root3(btScalar x)
+{
+	btScalar s = 1.;
+	while (x < 1.)
+	{
+		x *= 8.;
+		s *= 0.5;
+	}
+	while (x > 8.)
+	{
+		x *= 0.125;
+		s *= 2.;
+	}
+	btScalar r = 1.5;
+	r -= 1. / 3. * (r - x / (r * r));
+	r -= 1. / 3. * (r - x / (r * r));
+	r -= 1. / 3. * (r - x / (r * r));
+	r -= 1. / 3. * (r - x / (r * r));
+	r -= 1. / 3. * (r - x / (r * r));
+	r -= 1. / 3. * (r - x / (r * r));
+	return r * s;
+}
+
+btScalar SIMD_FORCE_INLINE root3(btScalar x)
+{
+	if (x > 0)
+		return _root3(x);
+	else if (x < 0)
+		return -_root3(-x);
+	else
+		return 0.;
+}
+
+// x - array of size 2
+// return 2: 2 real roots x[0], x[1]
+// return 0: pair of complex roots: x[0]i*x[1]
+int SolveP2(btScalar* x, btScalar a, btScalar b)
+{  // solve equation x^2 + a*x + b = 0
+	btScalar D = 0.25 * a * a - b;
+	if (D >= 0)
+	{
+		D = sqrt(D);
+		x[0] = -0.5 * a + D;
+		x[1] = -0.5 * a - D;
+		return 2;
+	}
+	x[0] = -0.5 * a;
+	x[1] = sqrt(-D);
+	return 0;
+}
+//---------------------------------------------------------------------------
+// x - array of size 3
+// In case 3 real roots: => x[0], x[1], x[2], return 3
+//         2 real roots: x[0], x[1],          return 2
+//         1 real root : x[0], x[1]  i*x[2], return 1
+int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
+{  // solve cubic equation x^3 + a*x^2 + b*x + c = 0
+	btScalar a2 = a * a;
+	btScalar q = (a2 - 3 * b) / 9;
+	if (q < 0)
+		q = eps;
+	btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54;
+	// equation x^3 + q*x + r = 0
+	btScalar r2 = r * r;
+	btScalar q3 = q * q * q;
+	btScalar A, B;
+	if (r2 <= (q3 + eps))
+	{  //<<-- FIXED!
+		btScalar t = r / sqrt(q3);
+		if (t < -1)
+			t = -1;
+		if (t > 1)
+			t = 1;
+		t = acos(t);
+		a /= 3;
+		q = -2 * sqrt(q);
+		x[0] = q * cos(t / 3) - a;
+		x[1] = q * cos((t + TwoPi) / 3) - a;
+		x[2] = q * cos((t - TwoPi) / 3) - a;
+		return (3);
+	}
+	else
+	{
+		//A =-pow(fabs(r)+sqrt(r2-q3),1./3);
+		A = -root3(fabs(r) + sqrt(r2 - q3));
+		if (r < 0)
+			A = -A;
+		B = (A == 0 ? 0 : q / A);
+
+		a /= 3;
+		x[0] = (A + B) - a;
+		x[1] = -0.5 * (A + B) - a;
+		x[2] = 0.5 * sqrt(3.) * (A - B);
+		if (fabs(x[2]) < eps)
+		{
+			x[2] = x[1];
+			return (2);
+		}
+		return (1);
+	}
+}  // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) {
+//---------------------------------------------------------------------------
+// a>=0!
+void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b)  // returns:  a+i*s = sqrt(x+i*y)
+{
+	btScalar r = sqrt(x * x + y * y);
+	if (y == 0)
+	{
+		r = sqrt(r);
+		if (x >= 0)
+		{
+			a = r;
+			b = 0;
+		}
+		else
+		{
+			a = 0;
+			b = r;
+		}
+	}
+	else
+	{  // y != 0
+		a = sqrt(0.5 * (x + r));
+		b = 0.5 * y / a;
+	}
+}
+//---------------------------------------------------------------------------
+int SolveP4Bi(btScalar* x, btScalar b, btScalar d)  // solve equation x^4 + b*x^2 + d = 0
+{
+	btScalar D = b * b - 4 * d;
+	if (D >= 0)
+	{
+		btScalar sD = sqrt(D);
+		btScalar x1 = (-b + sD) / 2;
+		btScalar x2 = (-b - sD) / 2;  // x2 <= x1
+		if (x2 >= 0)                  // 0 <= x2 <= x1, 4 real roots
+		{
+			btScalar sx1 = sqrt(x1);
+			btScalar sx2 = sqrt(x2);
+			x[0] = -sx1;
+			x[1] = sx1;
+			x[2] = -sx2;
+			x[3] = sx2;
+			return 4;
+		}
+		if (x1 < 0)  // x2 <= x1 < 0, two pair of imaginary roots
+		{
+			btScalar sx1 = sqrt(-x1);
+			btScalar sx2 = sqrt(-x2);
+			x[0] = 0;
+			x[1] = sx1;
+			x[2] = 0;
+			x[3] = sx2;
+			return 0;
+		}
+		// now x2 < 0 <= x1 , two real roots and one pair of imginary root
+		btScalar sx1 = sqrt(x1);
+		btScalar sx2 = sqrt(-x2);
+		x[0] = -sx1;
+		x[1] = sx1;
+		x[2] = 0;
+		x[3] = sx2;
+		return 2;
+	}
+	else
+	{  // if( D < 0 ), two pair of compex roots
+		btScalar sD2 = 0.5 * sqrt(-D);
+		CSqrt(-0.5 * b, sD2, x[0], x[1]);
+		CSqrt(-0.5 * b, -sD2, x[2], x[3]);
+		return 0;
+	}  // if( D>=0 )
+}  // SolveP4Bi(btScalar *x, btScalar b, btScalar d)    // solve equation x^4 + b*x^2 d
+//---------------------------------------------------------------------------
+#define SWAP(a, b) \
+	{              \
+		t = b;     \
+		b = a;     \
+		a = t;     \
+	}
+static void dblSort3(btScalar& a, btScalar& b, btScalar& c)  // make: a <= b <= c
+{
+	btScalar t;
+	if (a > b)
+		SWAP(a, b);  // now a<=b
+	if (c < b)
+	{
+		SWAP(b, c);  // now a<=b, b<=c
+		if (a > b)
+			SWAP(a, b);  // now a<=b
+	}
+}
+//---------------------------------------------------------------------------
+int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d)  // solve equation x^4 + b*x^2 + c*x + d
+{
+	//if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
+	if (fabs(c) < 1e-14 * (fabs(b) + fabs(d)))
+		return SolveP4Bi(x, b, d);  // After that, c!=0
+
+	int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c);  // solve resolvent
+	// by Viet theorem:  x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
+	if (res3 > 1)  // 3 real roots,
+	{
+		dblSort3(x[0], x[1], x[2]);  // sort roots to x[0] <= x[1] <= x[2]
+		// Note: x[0]*x[1]*x[2]= c*c > 0
+		if (x[0] > 0)  // all roots are positive
+		{
+			btScalar sz1 = sqrt(x[0]);
+			btScalar sz2 = sqrt(x[1]);
+			btScalar sz3 = sqrt(x[2]);
+			// Note: sz1*sz2*sz3= -c (and not equal to 0)
+			if (c > 0)
+			{
+				x[0] = (-sz1 - sz2 - sz3) / 2;
+				x[1] = (-sz1 + sz2 + sz3) / 2;
+				x[2] = (+sz1 - sz2 + sz3) / 2;
+				x[3] = (+sz1 + sz2 - sz3) / 2;
+				return 4;
+			}
+			// now: c<0
+			x[0] = (-sz1 - sz2 + sz3) / 2;
+			x[1] = (-sz1 + sz2 - sz3) / 2;
+			x[2] = (+sz1 - sz2 - sz3) / 2;
+			x[3] = (+sz1 + sz2 + sz3) / 2;
+			return 4;
+		}  // if( x[0] > 0) // all roots are positive
+		// now x[0] <= x[1] < 0, x[2] > 0
+		// two pair of comlex roots
+		btScalar sz1 = sqrt(-x[0]);
+		btScalar sz2 = sqrt(-x[1]);
+		btScalar sz3 = sqrt(x[2]);
+
+		if (c > 0)  // sign = -1
+		{
+			x[0] = -sz3 / 2;
+			x[1] = (sz1 - sz2) / 2;  // x[0]i*x[1]
+			x[2] = sz3 / 2;
+			x[3] = (-sz1 - sz2) / 2;  // x[2]i*x[3]
+			return 0;
+		}
+		// now: c<0 , sign = +1
+		x[0] = sz3 / 2;
+		x[1] = (-sz1 + sz2) / 2;
+		x[2] = -sz3 / 2;
+		x[3] = (sz1 + sz2) / 2;
+		return 0;
+	}  // if( res3>1 )    // 3 real roots,
+	// now resoventa have 1 real and pair of compex roots
+	// x[0] - real root, and x[0]>0,
+	// x[1]i*x[2] - complex roots,
+	// x[0] must be >=0. But one times x[0]=~ 1e-17, so:
+	if (x[0] < 0)
+		x[0] = 0;
+	btScalar sz1 = sqrt(x[0]);
+	btScalar szr, szi;
+	CSqrt(x[1], x[2], szr, szi);  // (szr+i*szi)^2 = x[1]+i*x[2]
+	if (c > 0)                    // sign = -1
+	{
+		x[0] = -sz1 / 2 - szr;  // 1st real root
+		x[1] = -sz1 / 2 + szr;  // 2nd real root
+		x[2] = sz1 / 2;
+		x[3] = szi;
+		return 2;
+	}
+	// now: c<0 , sign = +1
+	x[0] = sz1 / 2 - szr;  // 1st real root
+	x[1] = sz1 / 2 + szr;  // 2nd real root
+	x[2] = -sz1 / 2;
+	x[3] = szi;
+	return 2;
+}  // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d)    // solve equation x^4 + b*x^2 + c*x + d
+//-----------------------------------------------------------------------------
+btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d)  // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
+{
+	btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c;  // f'(x)
+	if (fxs == 0)
+		return x;                                       //return 1e99; <<-- FIXED!
+	btScalar fx = (((x + a) * x + b) * x + c) * x + d;  // f(x)
+	return x - fx / fxs;
+}
+//-----------------------------------------------------------------------------
+// x - array of size 4
+// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
+// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
+// return 0: two pair of complex roots: x[0]i*x[1],  x[2]i*x[3],
+int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d)
+{  // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
+	// move to a=0:
+	btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c);
+	btScalar c1 = c + 0.5 * a * (0.25 * a * a - b);
+	btScalar b1 = b - 0.375 * a * a;
+	int res = SolveP4De(x, b1, c1, d1);
+	if (res == 4)
+	{
+		x[0] -= a / 4;
+		x[1] -= a / 4;
+		x[2] -= a / 4;
+		x[3] -= a / 4;
+	}
+	else if (res == 2)
+	{
+		x[0] -= a / 4;
+		x[1] -= a / 4;
+		x[2] -= a / 4;
+	}
+	else
+	{
+		x[0] -= a / 4;
+		x[2] -= a / 4;
+	}
+	// one Newton step for each real root:
+	if (res > 0)
+	{
+		x[0] = N4Step(x[0], a, b, c, d);
+		x[1] = N4Step(x[1], a, b, c, d);
+	}
+	if (res > 2)
+	{
+		x[2] = N4Step(x[2], a, b, c, d);
+		x[3] = N4Step(x[3], a, b, c, d);
+	}
+	return res;
+}
+//-----------------------------------------------------------------------------
+#define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e)
+//-----------------------------------------------------------------------------
+btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e)  // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+{
+	int cnt;
+	if (fabs(e) < eps)
+		return 0;
+
+	btScalar brd = fabs(a);  // brd - border of real roots
+	if (fabs(b) > brd)
+		brd = fabs(b);
+	if (fabs(c) > brd)
+		brd = fabs(c);
+	if (fabs(d) > brd)
+		brd = fabs(d);
+	if (fabs(e) > brd)
+		brd = fabs(e);
+	brd++;  // brd - border of real roots
+
+	btScalar x0, f0;       // less than root
+	btScalar x1, f1;       // greater than root
+	btScalar x2, f2, f2s;  // next values, f(x2), f'(x2)
+	btScalar dx = 0;
+
+	if (e < 0)
+	{
+		x0 = 0;
+		x1 = brd;
+		f0 = e;
+		f1 = F5(x1);
+		x2 = 0.01 * brd;
+	}  // positive root
+	else
+	{
+		x0 = -brd;
+		x1 = 0;
+		f0 = F5(x0);
+		f1 = e;
+		x2 = -0.01 * brd;
+	}  // negative root
+
+	if (fabs(f0) < eps)
+		return x0;
+	if (fabs(f1) < eps)
+		return x1;
+
+	// now x0<x1, f(x0)<0, f(x1)>0
+	// Firstly 10 bisections
+	for (cnt = 0; cnt < 10; cnt++)
+	{
+		x2 = (x0 + x1) / 2;  // next point
+		//x2 = x0 - f0*(x1 - x0) / (f1 - f0);        // next point
+		f2 = F5(x2);  // f(x2)
+		if (fabs(f2) < eps)
+			return x2;
+		if (f2 > 0)
+		{
+			x1 = x2;
+			f1 = f2;
+		}
+		else
+		{
+			x0 = x2;
+			f0 = f2;
+		}
+	}
+
+	// At each step:
+	// x0<x1, f(x0)<0, f(x1)>0.
+	// x2 - next value
+	// we hope that x0 < x2 < x1, but not necessarily
+	do
+	{
+		if (cnt++ > 50)
+			break;
+		if (x2 <= x0 || x2 >= x1)
+			x2 = (x0 + x1) / 2;  // now  x0 < x2 < x1
+		f2 = F5(x2);             // f(x2)
+		if (fabs(f2) < eps)
+			return x2;
+		if (f2 > 0)
+		{
+			x1 = x2;
+			f1 = f2;
+		}
+		else
+		{
+			x0 = x2;
+			f0 = f2;
+		}
+		f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d;  // f'(x2)
+		if (fabs(f2s) < eps)
+		{
+			x2 = 1e99;
+			continue;
+		}
+		dx = f2 / f2s;
+		x2 -= dx;
+	} while (fabs(dx) > eps);
+	return x2;
+}  // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e)    // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+//-----------------------------------------------------------------------------
+int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e)  // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+{
+	btScalar r = x[0] = SolveP5_1(a, b, c, d, e);
+	btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1;
+	return 1 + SolveP4(x + 1, a1, b1, c1, d1);
+}  // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e)    // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+//-----------------------------------------------------------------------------

thirdparty/bullet/BulletSoftBody/poly34.h

@@ -0,0 +1,38 @@
+// poly34.h : solution of cubic and quartic equation
+// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html
+// khash2 (at) gmail.com
+
+#ifndef POLY_34
+#define POLY_34
+#include "LinearMath/btScalar.h"
+// x - array of size 2
+// return 2: 2 real roots x[0], x[1]
+// return 0: pair of complex roots: x[0]i*x[1]
+int SolveP2(btScalar* x, btScalar a, btScalar b);  // solve equation x^2 + a*x + b = 0
+
+// x - array of size 3
+// return 3: 3 real roots x[0], x[1], x[2]
+// return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2]
+int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c);  // solve cubic equation x^3 + a*x^2 + b*x + c = 0
+
+// x - array of size 4
+// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
+// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
+// return 0: two pair of complex roots: x[0]i*x[1],  x[2]i*x[3],
+int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d);  // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method
+
+// x - array of size 5
+// return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots
+// return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4],
+// return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2],  x[3]i*x[4],
+int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e);  // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+
+//-----------------------------------------------------------------------------
+// And some additional functions for internal use.
+// Your may remove this definitions from here
+int SolveP4Bi(btScalar* x, btScalar b, btScalar d);                              // solve equation x^4 + b*x^2 + d = 0
+int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d);                  // solve equation x^4 + b*x^2 + c*x + d = 0
+void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b);                    // returns as a+i*s,  sqrt(x+i*y)
+btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d);     // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
+btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e);  // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
+#endif

thirdparty/bullet/LinearMath/btAlignedAllocator.cpp

@@ -138,7 +138,7 @@ struct btDebugPtrMagic
 	};
 };
 
-void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filename)
+void *btAlignedAllocInternal(size_t size, int alignment, int line, const char *filename)
 {
 	if (size == 0)
 	{
@@ -195,7 +195,7 @@ void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filenam
 	return (ret);
 }
 
-void btAlignedFreeInternal(void *ptr, int line, char *filename)
+void btAlignedFreeInternal(void *ptr, int line, const char *filename)
 {
 	void *real;
 

thirdparty/bullet/LinearMath/btAlignedAllocator.h

@@ -35,9 +35,9 @@ int btDumpMemoryLeaks();
 #define btAlignedFree(ptr) \
 	btAlignedFreeInternal(ptr, __LINE__, __FILE__)
 
-void* btAlignedAllocInternal(size_t size, int alignment, int line, char* filename);
+void* btAlignedAllocInternal(size_t size, int alignment, int line, const char* filename);
 
-void btAlignedFreeInternal(void* ptr, int line, char* filename);
+void btAlignedFreeInternal(void* ptr, int line, const char* filename);
 
 #else
 void* btAlignedAllocInternal(size_t size, int alignment);

thirdparty/bullet/LinearMath/btConvexHullComputer.cpp

@@ -105,7 +105,7 @@ public:
 
 		Point64 cross(const Point32& b) const
 		{
-			return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
+			return Point64(((int64_t)y) * b.z - ((int64_t)z) * b.y, ((int64_t)z) * b.x - ((int64_t)x) * b.z, ((int64_t)x) * b.y - ((int64_t)y) * b.x);
 		}
 
 		Point64 cross(const Point64& b) const
@@ -115,7 +115,7 @@ public:
 
 		int64_t dot(const Point32& b) const
 		{
-			return x * b.x + y * b.y + z * b.z;
+			return ((int64_t)x) * b.x + ((int64_t)y) * b.y + ((int64_t)z) * b.z;
 		}
 
 		int64_t dot(const Point64& b) const
@@ -2673,6 +2673,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
 	}
 
 	vertices.resize(0);
+	original_vertex_index.resize(0);
 	edges.resize(0);
 	faces.resize(0);
 
@@ -2683,6 +2684,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in
 	{
 		btConvexHullInternal::Vertex* v = oldVertices[copied];
 		vertices.push_back(hull.getCoordinates(v));
+		original_vertex_index.push_back(v->point.index);
 		btConvexHullInternal::Edge* firstEdge = v->edges;
 		if (firstEdge)
 		{

thirdparty/bullet/LinearMath/btConvexHullComputer.h

@@ -66,6 +66,9 @@ public:
 	// Vertices of the output hull
 	btAlignedObjectArray<btVector3> vertices;
 
+	// The original vertex index in the input coords array
+	btAlignedObjectArray<int> original_vertex_index;
+
 	// Edges of the output hull
 	btAlignedObjectArray<Edge> edges;
 

thirdparty/bullet/LinearMath/btIDebugDraw.h

@@ -21,7 +21,7 @@ subject to the following restrictions:
 
 ///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
 ///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld.
-///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum.
+///A class that implements the btIDebugDraw interface will need to provide non-empty implementations of the the drawLine and getDebugMode methods at a minimum.
 ///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1]
 class btIDebugDraw
 {

thirdparty/bullet/LinearMath/btImplicitQRSVD.h

@@ -41,7 +41,7 @@
 
 #ifndef btImplicitQRSVD_h
 #define btImplicitQRSVD_h
-
+#include <limits>
 #include "btMatrix3x3.h"
 class btMatrix2x2
 {
@@ -753,7 +753,7 @@ inline int singularValueDecomposition(const btMatrix3x3& A,
                                      btMatrix3x3& V,
                                      btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
 {
-    using std::fabs;
+//    using std::fabs;
     btMatrix3x3 B = A;
     U.setIdentity();
     V.setIdentity();

thirdparty/bullet/LinearMath/btMatrix3x3.h

@@ -26,10 +26,12 @@ subject to the following restrictions:
 #endif
 
 #if defined(BT_USE_SSE)
+#define v0000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f))
 #define v1000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 1.0f))
 #define v0100 (_mm_set_ps(0.0f, 0.0f, 1.0f, 0.0f))
 #define v0010 (_mm_set_ps(0.0f, 1.0f, 0.0f, 0.0f))
 #elif defined(BT_USE_NEON)
+const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0000) = {0.0f, 0.0f, 0.0f, 0.0f};
 const btSimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
 const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
 const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
@@ -331,6 +333,20 @@ public:
 #endif
 	}
     
+    /**@brief Set the matrix to the identity */
+    void setZero()
+    {
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+        m_el[0] = v0000;
+        m_el[1] = v0000;
+        m_el[2] = v0000;
+#else
+        setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0),
+                 btScalar(0.0), btScalar(0.0), btScalar(0.0),
+                 btScalar(0.0), btScalar(0.0), btScalar(0.0));
+#endif
+    }
+
 	static const btMatrix3x3& getIdentity()
 	{
 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)

thirdparty/bullet/LinearMath/btMatrixX.h

@@ -346,10 +346,9 @@ struct btMatrixX
 					T dotProd = 0;
 					{
 						{
-							int r = rows();
 							int c = cols();
 
-							for (int k = 0; k < cols(); k++)
+							for (int k = 0; k < c; k++)
 							{
 								T w = (*this)(i, k);
 								if (other(k, j) != 0.f)

thirdparty/bullet/LinearMath/btModifiedGramSchmidt.h

@@ -0,0 +1,83 @@
+//
+//  btModifiedGramSchmidt.h
+//  LinearMath
+//
+//  Created by Xuchen Han on 4/4/20.
+//
+
+#ifndef btModifiedGramSchmidt_h
+#define btModifiedGramSchmidt_h
+
+#include "btReducedVector.h"
+#include "btAlignedObjectArray.h"
+#include <iostream>
+#include <cmath>
+template<class TV>
+class btModifiedGramSchmidt
+{
+public:
+    btAlignedObjectArray<TV> m_in;
+    btAlignedObjectArray<TV> m_out;
+    
+    btModifiedGramSchmidt(const btAlignedObjectArray<TV>& vecs): m_in(vecs)
+    {
+        m_out.resize(0);
+    }
+    
+    void solve()
+    {
+        m_out.resize(m_in.size());
+        for (int i = 0; i < m_in.size(); ++i)
+        {
+//            printf("========= starting %d ==========\n", i);
+            TV v(m_in[i]);
+//            v.print();
+            for (int j = 0; j < i; ++j)
+            {
+                v = v - v.proj(m_out[j]);
+//                v.print();
+            }
+            v.normalize();
+            m_out[i] = v;
+//            v.print();
+        }
+    }
+    
+    void test()
+    {
+        std::cout << SIMD_EPSILON << std::endl;
+        printf("=======inputs=========\n");
+        for (int i = 0; i < m_out.size(); ++i)
+        {
+            m_in[i].print();
+        }
+        printf("=======output=========\n");
+        for (int i = 0; i < m_out.size(); ++i)
+        {
+            m_out[i].print();
+        }
+        btScalar eps = SIMD_EPSILON;
+        for (int i = 0; i < m_out.size(); ++i)
+        {
+            for (int j = 0; j < m_out.size(); ++j)
+            {
+                if (i == j)
+                {
+                    if (std::abs(1.0-m_out[i].dot(m_out[j])) > eps)// && std::abs(m_out[i].dot(m_out[j])) > eps)
+                    {
+                        printf("vec[%d] is not unit, norm squared = %f\n", i,m_out[i].dot(m_out[j]));
+                    }
+                }
+                else
+                {
+                    if (std::abs(m_out[i].dot(m_out[j])) > eps)
+                    {
+                        printf("vec[%d] and vec[%d] is not orthogonal, dot product = %f\n", i, j, m_out[i].dot(m_out[j]));
+                    }
+                }
+            }
+        }
+    }
+};
+template class btModifiedGramSchmidt<btReducedVector>;
+#endif /* btModifiedGramSchmidt_h */

thirdparty/bullet/LinearMath/btQuickprof.cpp

@@ -720,6 +720,9 @@ void btLeaveProfileZoneDefault()
   #define BT_HAVE_TLS 1
 #elif __linux__
   #define BT_HAVE_TLS 1
+#elif defined(__FreeBSD__) || defined(__NetBSD__)
+  // TODO: At the moment disabling purposely OpenBSD, albeit tls support exists but not fully functioning
+  #define BT_HAVE_TLS 1
 #endif
 
 // __thread is broken on Andorid clang until r12b. See

thirdparty/bullet/LinearMath/btReducedVector.cpp

@@ -0,0 +1,170 @@
+//
+//  btReducedVector.cpp
+//  LinearMath
+//
+//  Created by Xuchen Han on 4/4/20.
+//
+#include <stdio.h>
+#include "btReducedVector.h"
+#include <cmath>
+
+// returns the projection of this onto other
+btReducedVector btReducedVector::proj(const btReducedVector& other) const
+{
+    btReducedVector ret(m_sz);
+    btScalar other_length2 = other.length2();
+    if (other_length2 < SIMD_EPSILON)
+    {
+        return ret;
+    }
+    return other*(this->dot(other))/other_length2;
+}
+
+void btReducedVector::normalize()
+{
+    if (this->length2() < SIMD_EPSILON)
+    {
+        m_indices.clear();
+        m_vecs.clear();
+        return;
+    }
+    *this /= std::sqrt(this->length2());
+}
+
+bool btReducedVector::testAdd() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btAlignedObjectArray<int> id2;
+    id2.push_back(2);
+    id2.push_back(3);
+    id2.push_back(5);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,3,1));
+    v2.push_back(btVector3(3,4,9));
+    v2.push_back(btVector3(0,4,0));
+    btAlignedObjectArray<int> id3;
+    id3.push_back(1);
+    id3.push_back(2);
+    id3.push_back(3);
+    id3.push_back(5);
+    btAlignedObjectArray<btVector3> v3;
+    v3.push_back(btVector3(1,0,1));
+    v3.push_back(btVector3(2,3,1));
+    v3.push_back(btVector3(6,5,14));
+    v3.push_back(btVector3(0,4,0));
+    btReducedVector rv1(sz, id1, v1);
+    btReducedVector rv2(sz, id2, v2);
+    btReducedVector ans(sz, id3, v3);
+    bool ret = ((ans == rv1+rv2) && (ans == rv2+rv1));
+    if (!ret)
+        printf("btReducedVector testAdd failed\n");
+    return ret;
+}
+
+bool btReducedVector::testMinus() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btAlignedObjectArray<int> id2;
+    id2.push_back(2);
+    id2.push_back(3);
+    id2.push_back(5);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,3,1));
+    v2.push_back(btVector3(3,4,9));
+    v2.push_back(btVector3(0,4,0));
+    btAlignedObjectArray<int> id3;
+    id3.push_back(1);
+    id3.push_back(2);
+    id3.push_back(3);
+    id3.push_back(5);
+    btAlignedObjectArray<btVector3> v3;
+    v3.push_back(btVector3(-1,-0,-1));
+    v3.push_back(btVector3(2,3,1));
+    v3.push_back(btVector3(0,3,4));
+    v3.push_back(btVector3(0,4,0));
+    btReducedVector rv1(sz, id1, v1);
+    btReducedVector rv2(sz, id2, v2);
+    btReducedVector ans(sz, id3, v3);
+    bool ret = (ans == rv2-rv1);
+    if (!ret)
+        printf("btReducedVector testMinus failed\n");
+    return ret;
+}
+
+bool btReducedVector::testDot() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btAlignedObjectArray<int> id2;
+    id2.push_back(2);
+    id2.push_back(3);
+    id2.push_back(5);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,3,1));
+    v2.push_back(btVector3(3,4,9));
+    v2.push_back(btVector3(0,4,0));
+    btReducedVector rv1(sz, id1, v1);
+    btReducedVector rv2(sz, id2, v2);
+    btScalar ans = 58;
+    bool ret = (ans == rv2.dot(rv1) && ans == rv1.dot(rv2));
+    ans = 14+16+9+16+81;
+    ret &= (ans==rv2.dot(rv2));
+    
+    if (!ret)
+        printf("btReducedVector testDot failed\n");
+    return ret;
+}
+
+bool btReducedVector::testMultiply() const
+{
+    int sz = 5;
+    btAlignedObjectArray<int> id1;
+    id1.push_back(1);
+    id1.push_back(3);
+    btAlignedObjectArray<btVector3> v1;
+    v1.push_back(btVector3(1,0,1));
+    v1.push_back(btVector3(3,1,5));
+    btScalar s = 2;
+    btReducedVector rv1(sz, id1, v1);
+    btAlignedObjectArray<int> id2;
+    id2.push_back(1);
+    id2.push_back(3);
+    btAlignedObjectArray<btVector3> v2;
+    v2.push_back(btVector3(2,0,2));
+    v2.push_back(btVector3(6,2,10));
+    btReducedVector ans(sz, id2, v2);
+    bool ret = (ans == rv1*s);
+    if (!ret)
+        printf("btReducedVector testMultiply failed\n");
+    return ret;
+}
+
+void btReducedVector::test() const
+{
+    bool ans = testAdd() && testMinus() && testDot() && testMultiply();
+    if (ans)
+    {
+        printf("All tests passed\n");
+    }
+    else
+    {
+        printf("Tests failed\n");
+    }
+}

thirdparty/bullet/LinearMath/btReducedVector.h

@@ -0,0 +1,320 @@
+//
+//  btReducedVectors.h
+//  BulletLinearMath
+//
+//  Created by Xuchen Han on 4/4/20.
+//
+#ifndef btReducedVectors_h
+#define btReducedVectors_h
+#include "btVector3.h"
+#include "btMatrix3x3.h"
+#include "btAlignedObjectArray.h"
+#include <stdio.h>
+#include <vector>
+#include <algorithm>
+struct TwoInts
+{
+    int a,b;
+};
+inline bool operator<(const TwoInts& A, const TwoInts& B)
+{
+    return A.b < B.b;
+}
+
+
+// A helper vector type used for CG projections
+class btReducedVector
+{
+public:
+    btAlignedObjectArray<int> m_indices;
+    btAlignedObjectArray<btVector3> m_vecs;
+    int m_sz; // all m_indices value < m_sz
+public:
+	btReducedVector():m_sz(0)
+	{
+		m_indices.resize(0);
+		m_vecs.resize(0);
+        m_indices.clear();
+        m_vecs.clear();
+	}
+	
+    btReducedVector(int sz): m_sz(sz)
+    {
+        m_indices.resize(0);
+        m_vecs.resize(0);
+        m_indices.clear();
+        m_vecs.clear();
+    }
+    
+    btReducedVector(int sz, const btAlignedObjectArray<int>& indices, const btAlignedObjectArray<btVector3>& vecs): m_sz(sz), m_indices(indices), m_vecs(vecs)
+    {
+    }
+    
+    void simplify()
+    {
+        btAlignedObjectArray<int> old_indices(m_indices);
+        btAlignedObjectArray<btVector3> old_vecs(m_vecs);
+        m_indices.resize(0);
+        m_vecs.resize(0);
+        m_indices.clear();
+        m_vecs.clear();
+        for (int i = 0; i < old_indices.size(); ++i)
+        {
+            if (old_vecs[i].length2() > SIMD_EPSILON)
+            {
+                m_indices.push_back(old_indices[i]);
+                m_vecs.push_back(old_vecs[i]);
+            }
+        }
+    }
+    
+    btReducedVector operator+(const btReducedVector& other)
+    {
+		btReducedVector ret(m_sz);
+		int i=0, j=0;
+		while (i < m_indices.size() && j < other.m_indices.size())
+		{
+			if (m_indices[i] < other.m_indices[j])
+			{
+				ret.m_indices.push_back(m_indices[i]);
+				ret.m_vecs.push_back(m_vecs[i]);
+				++i;
+			}
+			else if (m_indices[i] > other.m_indices[j])
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(other.m_vecs[j]);
+				++j;
+			}
+			else
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(m_vecs[i] + other.m_vecs[j]);
+				++i; ++j;
+			}
+		}
+		while (i < m_indices.size())
+		{
+			ret.m_indices.push_back(m_indices[i]);
+			ret.m_vecs.push_back(m_vecs[i]);
+			++i;
+		}
+		while (j < other.m_indices.size())
+		{
+			ret.m_indices.push_back(other.m_indices[j]);
+			ret.m_vecs.push_back(other.m_vecs[j]);
+			++j;
+		}
+        ret.simplify();
+        return ret;
+    }
+
+    btReducedVector operator-()
+    {
+        btReducedVector ret(m_sz);
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            ret.m_indices.push_back(m_indices[i]);
+            ret.m_vecs.push_back(-m_vecs[i]);
+        }
+        ret.simplify();
+        return ret;
+    }
+    
+    btReducedVector operator-(const btReducedVector& other)
+    {
+		btReducedVector ret(m_sz);
+		int i=0, j=0;
+		while (i < m_indices.size() && j < other.m_indices.size())
+		{
+			if (m_indices[i] < other.m_indices[j])
+			{
+				ret.m_indices.push_back(m_indices[i]);
+				ret.m_vecs.push_back(m_vecs[i]);
+				++i;
+			}
+			else if (m_indices[i] > other.m_indices[j])
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(-other.m_vecs[j]);
+				++j;
+			}
+			else
+			{
+				ret.m_indices.push_back(other.m_indices[j]);
+				ret.m_vecs.push_back(m_vecs[i] - other.m_vecs[j]);
+				++i; ++j;
+			}
+		}
+		while (i < m_indices.size())
+		{
+			ret.m_indices.push_back(m_indices[i]);
+			ret.m_vecs.push_back(m_vecs[i]);
+			++i;
+		}
+		while (j < other.m_indices.size())
+		{
+			ret.m_indices.push_back(other.m_indices[j]);
+			ret.m_vecs.push_back(-other.m_vecs[j]);
+			++j;
+		}
+        ret.simplify();
+		return ret;
+    }
+    
+    bool operator==(const btReducedVector& other) const
+    {
+        if (m_sz != other.m_sz)
+            return false;
+        if (m_indices.size() != other.m_indices.size())
+            return false;
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            if (m_indices[i] != other.m_indices[i] || m_vecs[i] != other.m_vecs[i])
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+    
+    bool operator!=(const btReducedVector& other) const
+    {
+        return !(*this == other);
+    }
+	
+	btReducedVector& operator=(const btReducedVector& other)
+	{
+		if (this == &other)
+		{
+			return *this;
+		}
+        m_sz = other.m_sz;
+		m_indices.copyFromArray(other.m_indices);
+		m_vecs.copyFromArray(other.m_vecs);
+		return *this;
+	}
+    
+    btScalar dot(const btReducedVector& other) const
+    {
+        btScalar ret = 0;
+        int j = 0;
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            while (j < other.m_indices.size() && other.m_indices[j] < m_indices[i])
+            {
+                ++j;
+            }
+            if (j < other.m_indices.size() && other.m_indices[j] == m_indices[i])
+            {
+                ret += m_vecs[i].dot(other.m_vecs[j]);
+//                ++j;
+            }
+        }
+        return ret;
+    }
+    
+    btScalar dot(const btAlignedObjectArray<btVector3>& other) const
+    {
+        btScalar ret = 0;
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            ret += m_vecs[i].dot(other[m_indices[i]]);
+        }
+        return ret;
+    }
+    
+    btScalar length2() const
+    {
+        return this->dot(*this);
+    }
+	
+	void normalize();
+    
+    // returns the projection of this onto other
+    btReducedVector proj(const btReducedVector& other) const;
+    
+    bool testAdd() const;
+    
+    bool testMinus() const;
+    
+    bool testDot() const;
+    
+    bool testMultiply() const;
+    
+    void test() const;
+    
+    void print() const
+    {
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            printf("%d: (%f, %f, %f)/", m_indices[i], m_vecs[i][0],m_vecs[i][1],m_vecs[i][2]);
+        }
+        printf("\n");
+    }
+    
+    
+    void sort()
+    {
+        std::vector<TwoInts> tuples;
+        for (int i = 0; i < m_indices.size(); ++i)
+        {
+            TwoInts ti;
+            ti.a = i;
+            ti.b = m_indices[i];
+            tuples.push_back(ti);
+        }
+        std::sort(tuples.begin(), tuples.end());
+        btAlignedObjectArray<int> new_indices;
+        btAlignedObjectArray<btVector3> new_vecs;
+        for (size_t i = 0; i < tuples.size(); ++i)
+        {
+            new_indices.push_back(tuples[i].b);
+            new_vecs.push_back(m_vecs[tuples[i].a]);
+        }
+        m_indices = new_indices;
+        m_vecs = new_vecs;
+    }
+};
+
+SIMD_FORCE_INLINE btReducedVector operator*(const btReducedVector& v, btScalar s)
+{
+    btReducedVector ret(v.m_sz);
+    for (int i = 0; i < v.m_indices.size(); ++i)
+    {
+        ret.m_indices.push_back(v.m_indices[i]);
+        ret.m_vecs.push_back(s*v.m_vecs[i]);
+    }
+    ret.simplify();
+    return ret;
+}
+
+SIMD_FORCE_INLINE btReducedVector operator*(btScalar s, const btReducedVector& v)
+{
+    return v*s;
+}
+
+SIMD_FORCE_INLINE btReducedVector operator/(const btReducedVector& v, btScalar s)
+{
+	return v * (1.0/s);
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator/=(btReducedVector& v, btScalar s)
+{
+	v = v/s;
+	return v;
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator+=(btReducedVector& v1, const btReducedVector& v2)
+{
+	v1 = v1+v2;
+	return v1;
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator-=(btReducedVector& v1, const btReducedVector& v2)
+{
+	v1 = v1-v2;
+	return v1;
+}
+
+#endif /* btReducedVectors_h */