Merge pull request #44455 from akien-mga/bullet-3.07

bullet: Sync with upstream 3.07

thirdparty/README.md

@@ -40,11 +40,9 @@ Files extracted from upstream source:
 ## bullet
 
 - Upstream: https://github.com/bulletphysics/bullet3
-- Version: git pre-2.90 (cd8cf7521cbb8b7808126a6adebd47bb83ea166a, 2020)
+- Version: 3.07 (e32fc59c88a3908876949c6f2665e8d091d987fa, 2020)
 - License: zlib
 
-Important: Synced with a pre-release version of bullet 2.90 from the master branch.
-
 Files extracted from upstream source:
 
 - src/* apart from CMakeLists.txt and premake4.lua files

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/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;
 }
 
 /*
@@ -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
@@ -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/btCollisionObject.h

@@ -127,6 +127,7 @@ public:
 
 	enum CollisionFlags
 	{
+		CF_DYNAMIC_OBJECT = 0,
 		CF_STATIC_OBJECT = 1,
 		CF_KINEMATIC_OBJECT = 2,
 		CF_NO_CONTACT_RESPONSE = 4,
@@ -251,6 +252,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);

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

@@ -21,8 +21,7 @@ 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)
 {
 	initialize(heightStickWidth, heightStickLength, heightfieldData,
@@ -31,8 +30,7 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(
 }
 
 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,

thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h

@@ -114,7 +114,7 @@ protected:
 	int m_vboundsGridLength;
 	int m_vboundsChunkSize;
 
-	int m_userIndex2;
+	
 	btScalar m_userValue3;
 
 	struct btTriangleInfoMap* m_triangleInfoMap;
@@ -192,14 +192,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
@@ -83,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.1);
+		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.);

thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h

@@ -356,12 +356,12 @@ public:
         }
     }
     
-    btVector3 getPushVelocity()
+    btVector3 getPushVelocity() const
     {
         return m_pushVelocity;
     }
     
-    btVector3 getTurnVelocity()
+    btVector3 getTurnVelocity() const
     {
         return m_turnVelocity;
     }
@@ -466,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/Featherstone/btMultiBody.cpp

@@ -344,6 +344,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);
@@ -671,6 +673,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(
@@ -796,7 +822,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();
 	}
@@ -872,6 +898,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;
@@ -897,6 +928,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)
@@ -909,22 +957,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());
@@ -935,6 +967,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;
@@ -1047,7 +1081,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	// Second 'upward' loop
 	// (part of TreeForwardDynamics in Mirtich)
 
-	if (m_fixedBase)
+	if (isBaseStaticOrKinematic())
 	{
 		spatAcc[0].setZero();
 	}
@@ -1081,13 +1115,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]);
@@ -1096,6 +1131,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)
 		{
@@ -1432,7 +1468,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();
 	}
@@ -1451,6 +1487,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;
@@ -1494,7 +1532,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	// Second 'upward' loop
 	// (part of TreeForwardDynamics in Mirtich)
 
-	if (m_fixedBase)
+	if (isBaseStaticOrKinematic())
 	{
 		spatAcc[0].setZero();
 	}
@@ -1507,6 +1545,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;
@@ -1550,6 +1590,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;
@@ -1567,6 +1609,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)
@@ -1617,6 +1660,8 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
     
     //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
     //
+		if(!isBaseKinematic())
+		{
         btScalar *pBaseQuat;
 
         // reset to current orientation
@@ -1637,6 +1682,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)
@@ -1644,6 +1690,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)
@@ -1695,6 +1773,7 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
                 {
                 }
             }
+        }
         
         m_links[i].updateInterpolationCacheMultiDof();
     }
@@ -1703,6 +1782,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;
@@ -1713,6 +1794,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)
@@ -1763,6 +1845,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)
 		//
@@ -1779,6 +1863,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;
@@ -1787,6 +1872,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]);
@@ -1832,6 +1919,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 				{
 				}
 			}
+		}
 
 		m_links[i].updateCacheMultiDof(pq);
 
@@ -2135,8 +2223,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())
     {
@@ -2282,3 +2377,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 (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]; 
@@ -397,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);
@@ -495,14 +550,22 @@ public:
 	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
@@ -653,7 +716,15 @@ 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;
 
 private:
 	btMultiBody(const btMultiBody &);     // not implemented
@@ -711,6 +782,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;

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

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;
@@ -82,12 +99,16 @@ protected:
 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/btMultiBodyDynamicsWorld.cpp

@@ -592,6 +592,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
 
 			if (!isSleeping)
 			{
+				bod->addSplitV();
 				int nLinks = bod->getNumLinks();
 
 				///base + num m_links
@@ -610,6 +611,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 +869,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,7 +23,7 @@ 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.),

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

@@ -15,28 +15,23 @@
 
 #ifndef BT_CONJUGATE_RESIDUAL_H
 #define BT_CONJUGATE_RESIDUAL_H
-#include <iostream>
-#include <cmath>
-#include <limits>
-#include <LinearMath/btAlignedObjectArray.h>
-#include <LinearMath/btVector3.h>
-#include <LinearMath/btScalar.h>
-#include "LinearMath/btQuickprof.h"
+#include "btKrylovSolver.h"
+
 template <class MatrixX>
-class btConjugateResidual
+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
-    int max_iterations;
-    btScalar tolerance_squared, best_r;
+	btScalar best_r;
+
 public:
 	btConjugateResidual(const int max_it_in)
-    : max_iterations(max_it_in)
+		: Base(max_it_in, 1e-8)
 	{
-        tolerance_squared = 1e-2;
 	}
 
 	virtual ~btConjugateResidual() {}
@@ -49,17 +44,13 @@ public:
 		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 = sub(b, temp_r);
+		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 = norm(r);
-        if (residual_norm <= tolerance_squared) {
-            if (verbose)
+		btScalar residual_norm = this->norm(r);
+		if (residual_norm <= Base::m_tolerance)
 		{
-                std::cout << "Iteration = 0" << std::endl;
-                std::cout << "Two norm of the residual = " << residual_norm << std::endl;
-            }
 			return 0;
 		}
 		p = r;
@@ -68,52 +59,43 @@ public:
 		A.multiply(p, temp_p);
 		// temp_r = A*r
 		temp_r = temp_p;
-        r_dot_Ar = dot(r, temp_r);
-        for (int k = 1; k <= max_iterations; k++) {
+		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 / dot(temp_p, z);
+			btScalar alpha = r_dot_Ar / this->dot(temp_p, z);
 			//  x += alpha * p;
-            multAndAddTo(alpha, p, x);
+			this->multAndAddTo(alpha, p, x);
 			//  r -= alpha * z;
-            multAndAddTo(-alpha, z, r);
-            btScalar norm_r = norm(r);
+			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 < tolerance_squared) {
-                    if (verbose)
+				if (norm_r < Base::m_tolerance)
 				{
-                        std::cout << "ConjugateResidual iterations " << k << std::endl;
-                    }
 					return k;
 				}
-                else
-                {
-                    if (verbose)
-                    {
-                        std::cout << "ConjugateResidual iterations " << k << " has residual "<< norm_r << std::endl;
-                    }
-                }
 			}
 			// temp_r = A * r;
 			A.multiply(r, temp_r);
-            r_dot_Ar_new = dot(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 = multAndAdd(beta, p, r);
+			p = this->multAndAdd(beta, p, r);
 			// temp_p = beta*temp_p + temp_r;
-            temp_p = multAndAdd(beta, temp_p, temp_r);
+			temp_p = this->multAndAdd(beta, temp_p, temp_r);
 		}
 		if (verbose)
 		{
-            std::cout << "ConjugateResidual max iterations reached " << max_iterations << std::endl;
+			std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl;
 		}
 		x = best_x;
-        return max_iterations;
+		return Base::m_maxIterations;
 	}
 
 	void reinitialize(const TVStack& b)
@@ -126,63 +108,5 @@ public:
 		best_x.resize(b.size());
 		best_r = SIMD_INFINITY;
 	}
-    
-    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 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;
-    }
-    
-    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 /* btConjugateResidual_h */
-

thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -18,10 +18,7 @@
 #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_massPreconditioner = new MassPreconditioner(m_softBodies);
 	m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit);
@@ -49,6 +46,17 @@ 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);
 }
@@ -78,7 +86,8 @@ 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);
 		}
@@ -136,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)
@@ -155,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);
 		}
@@ -193,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);
 }
 

thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -168,6 +168,31 @@ public:
 			}
 		}
 	}
+
+	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,15 +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_cr(kMaxConjugateGradientIterations)
-, m_maxNewtonIterations(5)
-, m_newtonTolerance(1e-4)
-, m_lineSearch(false)
-, m_useProjection(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);
 }
@@ -95,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);
@@ -166,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);
@@ -244,7 +239,10 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
 		m_residual[i].setZero();
 	}
 
+	if (dt > 0)
+	{
 		m_dt = dt;
+	}
 	m_objective->reinitialize(nodeUpdated, dt);
 	updateSoftBodies();
 }
@@ -262,11 +260,6 @@ btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** def
 	return maxSquaredResidual;
 }
 
-void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
-{
-     m_objective->m_projection.splitImpulseSetup(infoGlobal);
-}
-
 void btDeformableBodySolver::updateVelocity()
 {
 	int counter = 0;
@@ -286,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;
 		}
@@ -306,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();
@@ -341,15 +341,16 @@ 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];
+				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;
@@ -383,10 +384,23 @@ 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)
 	{
@@ -435,13 +449,21 @@ 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();
 				n.m_v *= max_v;
 			}
 			n.m_q = n.m_x + n.m_v * dt;
-        n.m_penetration = 0;
+		}
+		n.m_splitv.setZero();
+		n.m_constrained = false;
 	}
 
 	/* Nodes                */
@@ -459,7 +481,6 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
 	//    psb->m_fdbvt.optimizeIncremental(1);
 }
 
-
 void btDeformableBodySolver::updateSoftBodies()
 {
 	BT_PROFILE("updateSoftBodies");

thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h

@@ -16,7 +16,6 @@
 #ifndef BT_DEFORMABLE_BODY_SOLVERS_H
 #define BT_DEFORMABLE_BODY_SOLVERS_H
 
-
 #include "btSoftBodySolvers.h"
 #include "btDeformableBackwardEulerObjective.h"
 #include "btDeformableMultiBodyDynamicsWorld.h"
@@ -31,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
@@ -68,9 +68,6 @@ public:
 	// solve the momentum equation
 	virtual void solveDeformableConstraints(btScalar solverdt);
 
-    // 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);
 
@@ -114,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

@@ -16,14 +16,12 @@
 #include "btDeformableContactConstraint.h"
 /* ================   Deformable Node Anchor   =================== */
 btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal)
-: m_anchor(&a)
-, btDeformableContactConstraint(a.m_cti.m_normal, 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)
 {
 }
 
@@ -135,26 +133,23 @@ void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
 
 /* ================   Deformable vs. Rigid   =================== */
 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal)
-: m_contact(&c)
-, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
+	: m_contact(&c), btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
 {
 	m_total_normal_dv.setZero();
 	m_total_tangent_dv.setZero();
 	// The magnitude of penetration is the depth of penetration.
 	m_penetration = c.m_cti.m_offset;
-//	m_penetration = btMin(btScalar(0),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;
@@ -207,28 +202,96 @@ btVector3 btDeformableRigidContactConstraint::getVa() const
 	return va;
 }
 
+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;
-    btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+	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 + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ;
-    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
@@ -246,7 +309,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 			{
 				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
 		{
@@ -257,8 +321,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 	impulse = impulse_normal + impulse_tangent;
 	// apply impulse to deformable nodes involved and change their velocities
 	applyImpulse(impulse);
-	if (residualSquare < 1e-7)
-		return residualSquare;
 	// apply impulse to the rigid/multibodies involved and change their velocities
 	if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 	{
@@ -288,23 +350,71 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 			}
 		}
 	}
-//    va = getVa();
-//    vb = getVb();
-//    vr = vb - va;
-//    btScalar dn1 = btDot(vr, cti.m_normal) / 150;
-//    m_penetration += dn1;
+	return residualSquare;
+}
+
+btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+{
+	btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction;
+	const btSoftBody::sCti& cti = m_contact->m_cti;
+	btVector3 vb = getSplitVb();
+	btVector3 va = getSplitVa();
+	btScalar p = m_penetration;
+	if (p > 0)
+	{
+		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 split impulse to the rigid/multibodies involved and change their velocities
+	if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+	{
+		btRigidBody* rigidCol = 0;
+		rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+		if (rigidCol)
+		{
+			rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
+		}
+	}
+	else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+	{
+		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 residualSquare;
 }
 /* ================   Node vs. Rigid   =================== */
 btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
-    : m_node(contact.m_node)
-    , btDeformableRigidContactConstraint(contact, 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)
 {
 }
 
@@ -313,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
 {
@@ -322,22 +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 = contact->m_c5 * impulse;
+	contact->m_node->m_splitv -= dv;
+}
+
 /* ================   Face vs. Rigid   =================== */
 btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting)
-: m_face(contact.m_face)
-, m_useStrainLimiting(useStrainLimiting)
-, btDeformableRigidContactConstraint(contact, infoGlobal)
+	: m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btDeformableRigidContactConstraint(contact, infoGlobal)
 {
 }
 
 btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
-: m_face(other.m_face)
-, m_useStrainLimiting(other.m_useStrainLimiting)
-, btDeformableRigidContactConstraint(other)
+	: m_face(other.m_face), m_useStrainLimiting(other.m_useStrainLimiting), btDeformableRigidContactConstraint(other)
 {
 }
 
@@ -348,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;
@@ -441,12 +557,41 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
 	}
 }
 
+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_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, const btContactSolverInfo& infoGlobal)
-: m_node(contact.m_node)
-, m_face(contact.m_face)
-, m_contact(&contact)
-, btDeformableContactConstraint(contact.m_normal, 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();

thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h

@@ -40,9 +40,7 @@ public:
 	btDeformableContactConstraint() {}
 
 	btDeformableContactConstraint(const btDeformableContactConstraint& other)
-	: m_static(other.m_static)
-	, m_normal(other.m_normal)
-	, m_infoGlobal(other.m_infoGlobal)
+		: m_static(other.m_static), m_normal(other.m_normal), m_infoGlobal(other.m_infoGlobal)
 	{
 	}
 
@@ -80,8 +78,7 @@ public:
 	}
 	btDeformableStaticConstraint() {}
 	btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
-    : m_node(other.m_node)
-    , btDeformableContactConstraint(other)
+		: m_node(other.m_node), btDeformableContactConstraint(other)
 	{
 	}
 
@@ -139,7 +136,6 @@ public:
 	virtual void setPenetrationScale(btScalar scale) {}
 };
 
-
 //
 // Constraint between rigid/multi body and deformable objects
 class btDeformableRigidContactConstraint : public btDeformableContactConstraint
@@ -148,6 +144,8 @@ 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(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
@@ -160,12 +158,22 @@ public:
 	// object A is the rigid/multi body, and object B is the deformable node/face
 	virtual btVector3 getVa() const;
 
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const = 0;
+
+	// 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;
 };
 
 //
@@ -186,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;
 
@@ -196,6 +207,8 @@ public:
 	}
 
 	virtual void applyImpulse(const btVector3& impulse);
+
+	virtual void applySplitImpulse(const btVector3& impulse);
 };
 
 //
@@ -203,7 +216,7 @@ public:
 class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
 {
 public:
-    const btSoftBody::Face* m_face;
+	btSoftBody::Face* m_face;
 	bool m_useStrainLimiting;
 	btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting);
 	btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
@@ -215,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;
 
@@ -225,6 +241,8 @@ public:
 	}
 
 	virtual void applyImpulse(const btVector3& impulse);
+
+	virtual void applySplitImpulse(const btVector3& impulse);
 };
 
 //

thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp

@@ -58,24 +58,34 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
 	return residualSquare;
 }
 
-void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
+btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
-	for (int i = 0; i < m_softBodies.size(); ++i)
+	btScalar residualSquare = 0;
+	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];
-			constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
+			btCollisionObject* psb = m_softBodies[j];
+			if (psb != deformableBodies[i])
+			{
+				continue;
 			}
-		// face constraints
-		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
+			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
 			{
-			btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
-			constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
+				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
+			{
+				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 		}
 	}
+	return residualSquare;
+}
 
 void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
 {
@@ -122,16 +132,8 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 				continue;
 			}
 			btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
-			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)
-			{
 			m_nodeRigidConstraints[i].push_back(constraint);
 		}
-		}
 
 		// set Deformable Face vs. Rigid constraint
 		for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
@@ -143,18 +145,10 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 				continue;
 			}
 			btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
-			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)
-			{
 			m_faceRigidConstraints[i].push_back(constraint);
 		}
 	}
 }
-}
 
 void btDeformableContactProjection::project(TVStack& x)
 {
@@ -178,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
 			{
@@ -224,7 +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_penetration = SIMD_INFINITY;
+			m_staticConstraints[i][j].m_node->m_constrained = true;
 			if (m_projectionsDict.find(index) == NULL)
 			{
 				m_projectionsDict.insert(index, units);
@@ -241,7 +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_penetration = SIMD_INFINITY;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
 			if (m_projectionsDict.find(index) == NULL)
 			{
 				m_projectionsDict.insert(index, units);
@@ -258,7 +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_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			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)
@@ -289,18 +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;
-            btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			if (m_faceRigidConstraints[i][j].m_binding)
+			{
 				for (int k = 0; k < 3; ++k)
 				{
-                face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
+					face->m_n[k]->m_constrained = true;
+				}
 			}
 			for (int k = 0; k < 3; ++k)
 			{
 				btSoftBody::Node* node = face->m_n[k];
-                node->m_penetration = true;
 				int index = node->index;
 				if (m_faceRigidConstraints[i][j].m_static)
 				{
@@ -311,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]);
 						}
 					}
 				}
@@ -411,7 +408,6 @@ void btDeformableContactProjection::setProjection()
 			{
 				for (int l = 0; l < 3; ++l)
 				{
-					
 					btReducedVector rv(dof);
 					for (int k = 0; k < 3; ++k)
 					{
@@ -456,7 +452,8 @@ void btDeformableContactProjection::checkConstraints(const TVStack& x)
 				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("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]);
 	}
 }
 
@@ -472,7 +469,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
 		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;
+			m_staticConstraints[i][j].m_node->m_constrained = true;
 			LagrangeMultiplier lm;
 			lm.m_num_nodes = 1;
 			lm.m_indices[0] = index;
@@ -486,7 +483,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
 		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;
+			m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
 			LagrangeMultiplier lm;
 			lm.m_num_nodes = 1;
 			lm.m_indices[0] = index;
@@ -497,10 +494,15 @@ void btDeformableContactProjection::setLagrangeMultiplier()
 			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_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
 			LagrangeMultiplier lm;
 			lm.m_num_nodes = 1;
 			lm.m_indices[0] = index;
@@ -519,22 +521,28 @@ void btDeformableContactProjection::setLagrangeMultiplier()
 			}
 			m_lagrangeMultipliers.push_back(lm);
 		}
+
 		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)
+			{
+				continue;
+			}
+			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;
 			LagrangeMultiplier lm;
 			lm.m_num_nodes = 3;
+
 			for (int k = 0; k < 3; ++k)
 			{
-				face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
+				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);
@@ -542,6 +550,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
 			}
 			else
 			{
+				face->m_pcontact[3] = 0;
 				lm.m_num_constraints = 1;
 				lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
 			}
@@ -612,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)
 	{
@@ -629,6 +637,3 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 #endif
 	m_lagrangeMultipliers.clear();
 }
-
-
-

thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h

@@ -34,7 +34,6 @@ struct LagrangeMultiplier
 	int m_indices[3];       // indices of the nodes involved, same size as m_num_nodes;
 };
 
-
 class btDeformableContactProjection
 {
 public:
@@ -91,7 +90,7 @@ public:
 
 	virtual void reinitialize(bool nodeUpdated);
 
-    virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
+	btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
 
 	virtual void setLagrangeMultiplier();
 

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)
@@ -120,8 +119,6 @@ public:
 	{
 		return BT_COROTATED_FORCE;
 	}
-    
 };
 
-
 #endif /* btCorotated_h */

thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h

@@ -68,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;
 			}
 		}
@@ -101,7 +101,5 @@ public:
 		}
 		return e;
 	}
-    
-    
 };
 #endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */

thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h

@@ -66,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)
@@ -178,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];
@@ -241,7 +242,6 @@ public:
 			psb->updateDeformation();
 		}
 
-        
 		TVStack dx;
 		dx.resize(getNumNodes());
 		TVStack df;
@@ -251,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)
@@ -295,7 +296,6 @@ public:
 	{
 		return BT_MASSSPRING_FORCE;
 	}
-    
 };
 
 #endif /* btMassSpring_h */

thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h

@@ -26,6 +26,7 @@ class btDeformableMousePickingForce : public btDeformableLagrangianForce
 	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, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce)
@@ -127,7 +128,24 @@ public:
 
 	virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
 	{
-        //TODO
+		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)
@@ -139,7 +157,6 @@ public:
 	{
 		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++)
@@ -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;
@@ -128,12 +127,14 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI
 					}
 					// solve the position correction between deformable and rigid/multibody
 					//                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
-//                    leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+					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

@@ -41,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;
@@ -61,7 +62,7 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
 	m_internalTime = 0.0;
 	m_implicit = false;
 	m_lineSearch = false;
-    m_useProjection = true;
+	m_useProjection = false;
 	m_ccdIterations = 5;
 	m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
 }
@@ -315,7 +316,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 					node.m_v[c] = -clampDeltaV;
 				}
 			}
-            node.m_x  =  node.m_x + timeStep * node.m_v;
+			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;
 		}
@@ -439,7 +440,6 @@ void btDeformableMultiBodyDynamicsWorld::sortConstraints()
 	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
 }
 
-    
 void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
 {
 	// process constraints on each island
@@ -532,20 +532,19 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 	if (m_useProjection)
 	{
 		m_deformableBodySolver->m_useProjection = true;
-//        m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = 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++)
@@ -556,8 +555,6 @@ void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
 			btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags());
 		}
 	}
-
-	
 }
 
 void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
@@ -721,8 +718,18 @@ void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformab
 		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.
@@ -738,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

@@ -133,6 +133,8 @@ public:
 
 	void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
 
+	void removeSoftBodyForce(btSoftBody* psb);
+
 	void removeSoftBody(btSoftBody* body);
 
 	void removeCollisionObject(btCollisionObject* collisionObject);
@@ -162,6 +164,11 @@ public:
 		m_lineSearch = lineSearch;
 	}
 
+	void setUseProjection(bool useProjection)
+	{
+		m_useProjection = useProjection;
+	}
+
 	void applyRepulsionForce(btScalar timeStep);
 
 	void performGeometricCollisions(btScalar timeStep);
@@ -240,8 +247,6 @@ public:
 		}
 	};
 
-    
-    
 	void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
 	{
 		BT_PROFILE("rayTest");

thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h

@@ -416,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)
@@ -80,7 +81,6 @@ public:
 	}
 };
 
-
 class KKTPreconditioner : public Preconditioner
 {
 	const btAlignedObjectArray<btSoftBody*>& m_softBodies;
@@ -89,13 +89,10 @@ class KKTPreconditioner : public Preconditioner
 	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)
+		: m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit)
 	{
 	}
 
@@ -178,7 +175,7 @@ public:
 			}
 		}
 	}
-#define USE_FULL_PRECONDITIONER
+//#define USE_FULL_PRECONDITIONER
 #ifndef USE_FULL_PRECONDITIONER
 	virtual void operator()(const TVStack& x, TVStack& b)
 	{

thirdparty/bullet/BulletSoftBody/btSoftBody.cpp

@@ -222,12 +222,14 @@ void btSoftBody::initDefaults()
 	m_windVelocity = btVector3(0, 0, 0);
 	m_restLengthScale = btScalar(1.0);
 	m_dampingCoefficient = 1.0;
-	m_sleepingThreshold = .4;
+	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;
 }
 
@@ -558,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)
 {
@@ -731,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));
 
@@ -817,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));
 
@@ -882,6 +901,7 @@ void btSoftBody::setVelocity(const btVector3& velocity)
 		if (n.m_im > 0)
 		{
 			n.m_v = velocity;
+			n.m_vn = velocity;
 		}
 	}
 }
@@ -1046,10 +1066,14 @@ btTransform btSoftBody::getRigidTransform()
 	btVector3 t = getCenterOfMass();
 	btMatrix3x3 S;
 	S.setZero();
-    // get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
+	// 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);
+		S -= OuterProduct(m_X[i], t - m_nodes[i].m_x);
 	}
 	btVector3 sigma;
 	btMatrix3x3 U, V;
@@ -2162,7 +2186,6 @@ void btSoftBody::predictMotion(btScalar dt)
 	m_cdbvt.optimizeIncremental(1);
 }
 
-
 //
 void btSoftBody::solveConstraints()
 {
@@ -2551,7 +2574,6 @@ int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
 	return (cnt);
 }
 
-
 //
 static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
 {
@@ -2609,7 +2631,8 @@ void btSoftBody::initializeFaceTree()
 	for (int i = 0; i < m_faces.size(); ++i)
 	{
 		Face& f = m_faces[i];
-		ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0);
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		vol = VolumeOf(f, 0);
 		btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
 		node->parent = NULL;
 		node->data = &f;
@@ -2661,7 +2684,8 @@ void btSoftBody::rebuildNodeTree()
 	for (int i = 0; i < m_nodes.size(); ++i)
 	{
 		Node& n = m_nodes[i];
-		ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0);
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		vol = btDbvtVolume::FromCR(n.m_x, 0);
 		btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
 		node->parent = NULL;
 		node->data = &n;
@@ -2742,8 +2766,7 @@ 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())
+	btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
 								: colObjWrap->getWorldTransform();
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
@@ -2751,6 +2774,7 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
 			shp,
 			nrm,
 			margin);
+
 	if (!predict)
 	{
 		cti.m_colObj = colObjWrap->getCollisionObject();
@@ -2792,50 +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();
 	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;
-        cti.m_colObj = colObjWrap->getCollisionObject();
-        // 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;
-        cti.m_colObj = colObjWrap->getCollisionObject();
-        for (int i = 0; i < 3; ++i)
-            f.m_pcontact[i] = bary[i];
-    }
-    return (dst < 0);
-#endif
 
 	// use collision quadrature point
 #ifdef USE_QUADRATURE
@@ -2874,45 +2860,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 	}
 #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);
-//        if (predict)
-//        {
-//            triangle_transform.setOrigin(f.m_n[0]->m_q);
-//            triangle = btTriangleShape(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);
-//        }
-//        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];
-//        btGjkEpaSolver2::Penetration(&triangle, triangle_transform, csh, wtr, guess, results);
-//        if (results.status == btGjkEpaSolver2::sResults::Separated)
-//            return false;
-//        dst = results.distance - margin;
-//        contact_point = results.witnesses[1];
-//        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_offset = dst;
-//    }
-//
-    
-    // regular face contact
-    {
-        btGjkEpaSolver2::sResults results;
+	// collision detection using x*
 	btTransform triangle_transform;
 	triangle_transform.setIdentity();
 	triangle_transform.setOrigin(f.m_n[0]->m_q);
@@ -2920,22 +2868,54 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 	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-csh->getMargin();
-        dst -= margin;
+	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;
-        contact_point = results.witnesses[0];
-        getBarycentric(contact_point, f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, bary);
-        btVector3 curr = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-        nrm = results.normal;
+
+	// 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();
-        cti.m_normal = nrm;
-        cti.m_offset = dst + (curr - contact_point).dot(nrm);
+			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;
 		}
-    return (dst < 0);
 	}
 
-//
+	// 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;
+}
+
 void btSoftBody::updateNormals()
 {
 	const btVector3 zv(0, 0, 0);
@@ -3461,6 +3441,16 @@ void btSoftBody::setSpringStiffness(btScalar k)
 	m_repulsionStiffness = k;
 }
 
+void btSoftBody::setGravityFactor(btScalar gravFactor)
+{
+	m_gravityFactor = gravFactor;
+}
+
+void btSoftBody::setCacheBarycenter(bool cacheBarycenter)
+{
+	m_cacheBarycenter = cacheBarycenter;
+}
+
 void btSoftBody::initializeDmInverse()
 {
 	btScalar unit_simplex_measure = 1. / 6.;
@@ -3476,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];
@@ -3498,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;
 	}
 }
 
@@ -3765,7 +3805,7 @@ void btSoftBody::interpolateRenderMesh()
 			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();
-			Node& n = m_renderNodes[i];
+			RenderNode& n = m_renderNodes[i];
 			n.m_x.setZero();
 			for (int j = 0; j < 3; ++j)
 			{
@@ -3778,7 +3818,7 @@ void btSoftBody::interpolateRenderMesh()
 	{
 		for (int i = 0; i < m_renderNodes.size(); ++i)
 		{
-			Node& n = m_renderNodes[i];
+			RenderNode& n = m_renderNodes[i];
 			n.m_x.setZero();
 			for (int j = 0; j < 4; ++j)
 			{
@@ -4662,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

@@ -258,6 +258,12 @@ public:
 		Material* m_material;  // Material
 	};
 	/* Node			*/
+	struct RenderNode
+	{
+		btVector3 m_x;
+		btVector3 m_uv1;
+		btVector3 m_normal;
+	};
 	struct Node : Feature
 	{
 		btVector3 m_x;       // Position
@@ -269,9 +275,12 @@ public:
 		btScalar m_im;       // 1/mass
 		btScalar m_area;     // Area
 		btDbvtNode* m_leaf;  // Leaf data
-		btScalar m_penetration;   // depth of penetration
+		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)
@@ -287,6 +296,11 @@ public:
 
 		BT_DECLARE_ALIGNED_ALLOCATOR();
 	};
+	struct RenderFace
+	{
+		RenderNode* m_n[3];          // Node pointers
+	};
+
 	/* Face			*/
 	struct Face : Feature
 	{
@@ -310,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  */
@@ -319,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		*/
@@ -349,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;
@@ -769,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;
@@ -791,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;
@@ -820,6 +839,8 @@ public:
 	btScalar m_maxSpeedSquared;
 	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;
@@ -926,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());
@@ -1167,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();
@@ -1188,7 +1212,8 @@ public:
 		if (node->isleaf())
 		{
 			btSoftBody::Node* n = (btSoftBody::Node*)(node->data);
-			ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+			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)
 			{
@@ -1207,7 +1232,8 @@ public:
 		{
 			updateNode(node->childs[0], use_velocity, margin);
 			updateNode(node->childs[1], use_velocity, margin);
-			ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
 			Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
 			node->volume = vol;
 		}
@@ -1226,7 +1252,8 @@ public:
 		{
 			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;
+			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,
@@ -1249,7 +1276,8 @@ public:
 		{
 			updateFace(node->childs[0], use_velocity, margin);
 			updateFace(node->childs[1], use_velocity, margin);
-			ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)
+			vol;
 			Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
 			node->volume = vol;
 		}
@@ -1312,20 +1340,22 @@ public:
 				I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
 			if (vn < 0)
 				I += 0.5 * mass * vn;
-			btScalar face_penetration = 0, node_penetration = node->m_penetration;
+			int face_penetration = 0, node_penetration = node->m_constrained;
 			for (int i = 0; i < 3; ++i)
-				face_penetration =  btMax(face_penetration, face->m_n[i]->m_penetration);
-			btScalar I_tilde = .5 *I /(1.0+w.length2());
+				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 <= node_penetration)
+			}
+			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 (face_penetration >= node_penetration)
+			if (node_penetration <= 0)
 			{
 				node->m_v -= I_tilde * node->m_im * n;
 			}
@@ -1339,16 +1369,16 @@ public:
 				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 = .5 *I /(1.0+w.length2());
+				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 <= node_penetration)
+				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 (face_penetration >= node_penetration)
+				if (node_penetration <= 0)
 				{
 					node->m_v -= I_tilde * node->m_im * vt;
 				}

thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -1296,6 +1296,7 @@ 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)
@@ -1314,9 +1315,9 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
 			for (size_t i = 0; i < 4; i++)
 			{
 				ss >> tet[i];
-                printf("%d ", tet[i]);
+				//printf("%d ", tet[i]);
 			}
-            printf("\n");
+			//printf("\n");
 			indices[indices_count++] = tet;
 		}
 	}
@@ -1336,7 +1337,6 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
 		}
 	}
 
-    
 	generateBoundaryFaces(psb);
 	psb->initializeDmInverse();
 	psb->m_tetraScratches.resize(psb->m_tetras.size());
@@ -1417,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";
@@ -1444,6 +1483,7 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
 			}
 			fs << "\n";
 		}
+	}
 	fs.close();
 }
 
@@ -1571,7 +1611,6 @@ void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
 	}
 }
 
-
 // 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)
 {

thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h

@@ -103,8 +103,7 @@ static btVector3 dop[KDOP_COUNT]={btVector3(1,0,0),
 									btVector3(1, 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)
 {
@@ -123,8 +122,7 @@ static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, c
 						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
-	};
+						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)
 	{
@@ -171,34 +169,40 @@ inline btScalar evaluateBezier(const btScalar &p0, const btScalar &p1, const btS
 }
 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)) {
+	if (sameSign(t0, t1))
+	{
 		lt0 = t0;
 		lt1 = t0;
 		return true;
 	}
 
-    if (type_c || diffSign(k0, k3)) {
+	if (type_c || diffSign(k0, k3))
+	{
 		btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
 		if (t0 < -0)
 			ft = -ft;
 
-        if (sameSign(ft, k0)) {
+		if (sameSign(ft, k0))
+		{
 			lt0 = t1;
 			lt1 = t1;
 		}
-        else {
+		else
+		{
 			lt0 = t0;
 			lt1 = t0;
 		}
 		return true;
 	}
 
-    if (!type_c) {
+	if (!type_c)
+	{
 		btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
 		if (t0 < -0)
 			ft = -ft;
 
-        if (diffSign(ft, k0)) {
+		if (diffSign(ft, k0))
+		{
 			lt0 = t0;
 			lt1 = t1;
 			return true;
@@ -958,7 +962,6 @@ static inline btMatrix3x3 OuterProduct(const btVector3& v1,const btVector3& v2)
 	return (m);
 }
 
-
 //
 static inline btMatrix3x3 Add(const btMatrix3x3& a,
 							  const btMatrix3x3& b)
@@ -1007,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)
@@ -1129,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)
@@ -1158,7 +1173,6 @@ static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVec
 	return ret;
 }
 
-
 //
 template <typename T>
 static inline T BaryEval(const T& a,
@@ -1672,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)
 						{
@@ -1680,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)
@@ -1708,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;
@@ -1750,7 +1766,6 @@ struct btSoftColliders
 			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.
@@ -1774,6 +1789,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 = Diagonal(ima);
 					if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 					{
 						const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
@@ -1822,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;
@@ -1890,7 +1905,6 @@ struct btSoftColliders
 		btScalar mrg;
 	};
 
-    
 	//
 	// CollideVF_DD
 	//

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

@@ -16,11 +16,13 @@ const btScalar eps = SIMD_EPSILON;
 static SIMD_FORCE_INLINE btScalar _root3(btScalar x)
 {
 	btScalar s = 1.;
-    while (x < 1.) {
+	while (x < 1.)
+	{
 		x *= 8.;
 		s *= 0.5;
 	}
-    while (x > 8.) {
+	while (x > 8.)
+	{
 		x *= 0.125;
 		s *= 2.;
 	}
@@ -50,7 +52,8 @@ btScalar SIMD_FORCE_INLINE root3(btScalar x)
 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) {
+	if (D >= 0)
+	{
 		D = sqrt(D);
 		x[0] = -0.5 * a + D;
 		x[1] = -0.5 * a - D;
@@ -76,7 +79,8 @@ int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
 	btScalar r2 = r * r;
 	btScalar q3 = q * q * q;
 	btScalar A, B;
-    if (r2 <= (q3 + eps)) { //<<-- FIXED!
+	if (r2 <= (q3 + eps))
+	{  //<<-- FIXED!
 		btScalar t = r / sqrt(q3);
 		if (t < -1)
 			t = -1;
@@ -90,7 +94,8 @@ int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
 		x[2] = q * cos((t - TwoPi) / 3) - a;
 		return (3);
 	}
-    else {
+	else
+	{
 		//A =-pow(fabs(r)+sqrt(r2-q3),1./3);
 		A = -root3(fabs(r) + sqrt(r2 - q3));
 		if (r < 0)
@@ -101,7 +106,8 @@ int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
 		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) {
+		if (fabs(x[2]) < eps)
+		{
 			x[2] = x[1];
 			return (2);
 		}
@@ -113,18 +119,22 @@ int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
 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) {
+	if (y == 0)
+	{
 		r = sqrt(r);
-        if (x >= 0) {
+		if (x >= 0)
+		{
 			a = r;
 			b = 0;
 		}
-        else {
+		else
+		{
 			a = 0;
 			b = r;
 		}
 	}
-    else { // y != 0
+	else
+	{  // y != 0
 		a = sqrt(0.5 * (x + r));
 		b = 0.5 * y / a;
 	}
@@ -133,7 +143,8 @@ void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns:  a+i*s
 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) {
+	if (D >= 0)
+	{
 		btScalar sD = sqrt(D);
 		btScalar x1 = (-b + sD) / 2;
 		btScalar x2 = (-b - sD) / 2;  // x2 <= x1
@@ -166,7 +177,8 @@ int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2
 		x[3] = sx2;
 		return 2;
 	}
-    else { // if( D < 0 ), two pair of compex roots
+	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]);
@@ -185,7 +197,8 @@ 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) {
+	if (c < b)
+	{
 		SWAP(b, c);  // now a<=b, b<=c
 		if (a > b)
 			SWAP(a, b);  // now a<=b
@@ -210,7 +223,8 @@ int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation
 			btScalar sz2 = sqrt(x[1]);
 			btScalar sz3 = sqrt(x[2]);
 			// Note: sz1*sz2*sz3= -c (and not equal to 0)
-            if (c > 0) {
+			if (c > 0)
+			{
 				x[0] = (-sz1 - sz2 - sz3) / 2;
 				x[1] = (-sz1 + sz2 + sz3) / 2;
 				x[2] = (+sz1 - sz2 + sz3) / 2;
@@ -290,27 +304,32 @@ int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d)
 	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) {
+	if (res == 4)
+	{
 		x[0] -= a / 4;
 		x[1] -= a / 4;
 		x[2] -= a / 4;
 		x[3] -= a / 4;
 	}
-    else if (res == 2) {
+	else if (res == 2)
+	{
 		x[0] -= a / 4;
 		x[1] -= a / 4;
 		x[2] -= a / 4;
 	}
-    else {
+	else
+	{
 		x[0] -= a / 4;
 		x[2] -= a / 4;
 	}
 	// one Newton step for each real root:
-    if (res > 0) {
+	if (res > 0)
+	{
 		x[0] = N4Step(x[0], a, b, c, d);
 		x[1] = N4Step(x[1], a, b, c, d);
 	}
-    if (res > 2) {
+	if (res > 2)
+	{
 		x[2] = N4Step(x[2], a, b, c, d);
 		x[3] = N4Step(x[3], a, b, c, d);
 	}
@@ -341,14 +360,16 @@ btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) /
 	btScalar x2, f2, f2s;  // next values, f(x2), f'(x2)
 	btScalar dx = 0;
 
-    if (e < 0) {
+	if (e < 0)
+	{
 		x0 = 0;
 		x1 = brd;
 		f0 = e;
 		f1 = F5(x1);
 		x2 = 0.01 * brd;
 	}  // positive root
-    else {
+	else
+	{
 		x0 = -brd;
 		x1 = 0;
 		f0 = F5(x0);
@@ -363,17 +384,20 @@ btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) /
 
 	// now x0<x1, f(x0)<0, f(x1)>0
 	// Firstly 10 bisections
-    for (cnt = 0; cnt < 10; cnt++) {
+	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) {
+		if (f2 > 0)
+		{
 			x1 = x2;
 			f1 = f2;
 		}
-        else {
+		else
+		{
 			x0 = x2;
 			f0 = f2;
 		}
@@ -383,7 +407,8 @@ btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) /
 	// x0<x1, f(x0)<0, f(x1)>0.
 	// x2 - next value
 	// we hope that x0 < x2 < x1, but not necessarily
-    do {
+	do
+	{
 		if (cnt++ > 50)
 			break;
 		if (x2 <= x0 || x2 >= x1)
@@ -391,16 +416,19 @@ btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) /
 		f2 = F5(x2);             // f(x2)
 		if (fabs(f2) < eps)
 			return x2;
-        if (f2 > 0) {
+		if (f2 > 0)
+		{
 			x1 = x2;
 			f1 = f2;
 		}
-        else {
+		else
+		{
 			x0 = x2;
 			f0 = f2;
 		}
 		f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d;  // f'(x2)
-        if (fabs(f2s) < eps) {
+		if (fabs(f2s) < eps)
+		{
 			x2 = 1e99;
 			continue;
 		}

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/btReducedVector.h

@@ -267,7 +267,7 @@ public:
         std::sort(tuples.begin(), tuples.end());
         btAlignedObjectArray<int> new_indices;
         btAlignedObjectArray<btVector3> new_vecs;
-        for (int i = 0; i < tuples.size(); ++i)
+        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]);

thirdparty/bullet/LinearMath/btScalar.h

@@ -25,7 +25,7 @@ subject to the following restrictions:
 #include <float.h>
 
 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
-#define BT_BULLET_VERSION 289
+#define BT_BULLET_VERSION 307
 
 inline int btGetVersion()
 {

thirdparty/bullet/LinearMath/btSerializer.h

@@ -479,9 +479,9 @@ public:
 			buffer[8] = 'V';
 		}
 
-		buffer[9] = '2';
-		buffer[10] = '8';
-		buffer[11] = '9';
+		buffer[9] = '3';
+		buffer[10] = '0';
+		buffer[11] = '7';
 	}
 
 	virtual void startSerialization()